//----------------------------------------------------------------------------------
// <copyright file="Power.c" company="LiquidRobotics">
//	Copyright (c) Liquid Robotics Corporation.  All rights reserved.
// </copyright>
//
// <summary>
// 	This is the source file for managing a power on/off sensor.
// </summary>
//
// <owner>Arash Ushani</owner>
//----------------------------------------------------------------------------------

#include "includes.h"
#include <avr/io.h>
#include <avr/pgmspace.h>
#include <string.h>
#include <avr/wdt.h>
#include <avr/power.h>
#include <avr/pgmspace.h>
#include "uart.h"
#include "pib.h"
#include "timer.h"
#include "lrstat.h"
#include "spidebug.h"
#include "wdtrig.h"
#include "pib_apps.h"
#include "User_app_init.h"
#include "fltPyldComms.h"
#include "UserData.h"
#include "Empty.h"

#include "SerialDebug.h"


#define NUM_POWER_COMMANDS 7
#define LEN_POWER_COMMAND  10

//Autorun State stored at offset 0x0040 from PM base -- 1 byte
#define POWER_AUTORUN_STATE_OFFSET		0x0040

extern const prog_char tokenListConsole[];
prog_char powerCommands[NUM_POWER_COMMANDS][LEN_POWER_COMMAND] = {"on", "off", "autorun", "run", "stop", "id", "status"};
prog_char powerParserID[] = "POWER";
prog_char powerCmdList[] = "ofarxisOFARXIS";

typedef struct _POWER_BUFFER_
{
	BOOL					powerOn;
	uint8_t					initState;
	char					tokenSeparatorList[MAX_TOKEN_SEPARATORS];
	char					consoleResponse[256];
	CONSOLE_RESPONSE_INFO	responseInfo;
}POWER_BUFFER, *PPOWER_BUFFER;


typedef struct _POWER_TASK_
{
	uint8_t		taskID;
	uint8_t 	portNum;
	uint16_t	eepromAddress;
	PPOWER_BUFFER 	pBuf;
}POWER_TASK, *PPOWER_TASK;

uint8_t Power_Init(PPOWER_TASK pTaskInfo);
void Power(void* pParam);

void Power_ParseConsole(PPOWER_TASK pTaskInfo);
uint8_t Power_ParseCommand(PPOWER_TASK pTaskInfo, char command, char* cmdParams);
void Power_QueueConsoleResponse(PPOWER_TASK pTaskInfo);

void SerialPortPassThroughPower(uint8_t port);

uint8_t Power_Id(PPOWER_TASK pTaskInfo);

extern CONSOLE_CMD_FORMAT consoleIn[NUM_TASK_FILES] __attribute__((section(".xdata")));
extern BOOL consoleAvailable[NUM_TASK_FILES];

extern BOOL kill[NUM_TASK_FILES];

extern BOOL serialPortPassThrough[NUM_TASK_FILES];
extern BOOL serialPortPassThroughOK;
extern BOOL readyForSerialPassThru;

extern OS_EVENT* pMutexFloatQueue;
extern FLOAT_TX_BUFFER txFloatData[MAX_FLOAT_BUFFERS] __attribute__((section(".xdata")));
extern uint8_t	headFloatTx;
extern uint8_t tailFloatTx;

extern FLOAT_TX_BUFFER txFloatAck[MAX_ACK_BUFFERS] __attribute__((section(".xdata")));
extern uint8_t	headFloatAckTx;
extern uint8_t tailFloatAckTx;

extern uint8_t	DotToPad(char* pad, char* dot);
extern uint16_t mainTaskAddress;

extern int32_t latitude;		//JGK Added 06-11-10
extern int32_t longitude;		//JGK Added 06-11-10
extern char	fixValid;			//JGK Added 06-11-10


uint8_t Power_Init(PPOWER_TASK pTaskInfo)
{
	uint8_t retval = SUCCESS;

	if(pTaskInfo->portNum == USER_PORT_1)
		pTaskInfo->eepromAddress = EEPROM_PM1_START;
	if(pTaskInfo->portNum == USER_PORT_2)
		pTaskInfo->eepromAddress = EEPROM_PM2_START;
	if(pTaskInfo->portNum == USER_PORT_3)
		pTaskInfo->eepromAddress = EEPROM_PM3_START;

	if(pTaskInfo->pBuf != NULL)
		memset(pTaskInfo->pBuf, 0, sizeof(POWER_BUFFER));


	//Get default power state
	pTaskInfo->pBuf->initState = 0xFF;
	EepromReadByte((void*)(pTaskInfo->eepromAddress + POWER_AUTORUN_STATE_OFFSET), (void*)&pTaskInfo->pBuf->initState);
	if((pTaskInfo->pBuf->initState != AUTORUN_ON) && (pTaskInfo->pBuf->initState != AUTORUN_OFF))
	{
		pTaskInfo->pBuf->initState = AUTORUN_OFF;	//Default to 1 sample per minute if not defined
		EepromWriteByteUnSigned((void*)(pTaskInfo->eepromAddress + POWER_AUTORUN_STATE_OFFSET), pTaskInfo->pBuf->initState);
	}

	OSuartInitPort(pTaskInfo->portNum, 9600, UART_MODE_EIGHT_DATA_BITS, UART_MODE_ONE_STOP_BIT, UART_MODE_NO_PARITY);

	pTaskInfo->pBuf->powerOn = FALSE;

	if(pTaskInfo->pBuf->initState == AUTORUN_ON)
	{
		char tempBuf[2] = " ";
		Power_ParseCommand(pTaskInfo, 'r', tempBuf);
	}
			
	return retval;
}


void Power(void* pParam)
{
	POWER_TASK taskInfo;
	PPOWER_TASK pTaskInit = (PPOWER_TASK)pParam;
	uint8_t retval = FAIL;

	taskInfo.taskID = pTaskInit->taskID;
	taskInfo.portNum = pTaskInit->portNum;
	taskInfo.pBuf = pTaskInit->pBuf;

	retval = Power_Init(&taskInfo);

	if(retval == SUCCESS)
	{
		while(1)
		{
			OSTimeDlyHMSM(0,0,1,0);

			//sprintf_P(dbgMsg, PSTR("Power loop for PM%d"), taskInfo.portNum);
			//SEND_MSG;

			if(serialPortPassThrough[taskInfo.portNum] == TRUE)
			{
				//Set Flag so the float COMM task will proceed
				serialPortPassThroughOK = TRUE;
				
				EnablePIBModule(taskInfo.portNum, TRUE, FALSE);
				OSTimeDlyHMSM(0,0,0,500);

				SerialPortPassThroughPower(taskInfo.portNum);
			}

			if(consoleAvailable[taskInfo.portNum] == TRUE)
			{
				taskInfo.pBuf->responseInfo.source = consoleIn[taskInfo.taskID].cmdSrcAddress;
				taskInfo.pBuf->responseInfo.status = 0;
				taskInfo.pBuf->responseInfo.length = consoleIn[taskInfo.taskID].cmdSize;
				taskInfo.pBuf->responseInfo.id = consoleIn[taskInfo.taskID].id;

				Power_ParseConsole(&taskInfo);
				consoleAvailable[taskInfo.portNum] = FALSE;
			}	
			
			// Check to see if we need to end this task and release resources
			if (kill[taskInfo.portNum] == TRUE) 
			{
				//sprintf_P(dbgMsg, PSTR("Ending task...."));
				//SEND_MSG;

				uint8_t val = 0xFF;
				uint16_t startAddr = 0;

				if(taskInfo.portNum == USER_PORT_1)
					startAddr = EEPROM_PM1_START;
				if(taskInfo.portNum == USER_PORT_2)
					startAddr = EEPROM_PM2_START;
				if(taskInfo.portNum == USER_PORT_3)
					startAddr = EEPROM_PM3_START;

				if (startAddr == 0)
				{
					//sprintf_P(dbgMsg, PSTR("No start addr"));
					//SEND_MSG;
					kill[taskInfo.portNum] = FALSE;
				}
				else
				{
					// Write all F's to EEPROM
					for (uint16_t addr = startAddr; addr < startAddr + EEPROM_PM_SIZE; addr++)
						EepromWriteBlock((void*)addr, (void*)&val, 1);

					memset(taskInfo.pBuf, 0, sizeof(POWER_BUFFER));

					// Now end task (ie, just sleep for a really long time)
					while (1) OSTimeDlyHMSM(1,0,0,0);
				}
            }			
		}
	}

	return;
}

void Power_ParseConsole(PPOWER_TASK pTaskInfo)
{
	char* myCommand = consoleIn[pTaskInfo->taskID].cmd; 
	char* result = NULL;
	char command = 0;
	BOOL goodCommand = FALSE;
	uint8_t resultCode = 0;

	myCommand[MAX_FILE_SIZE_IN - 1] = 0;			//Make sure command string is NULL terminated

	memset(pTaskInfo->pBuf->tokenSeparatorList, 0, MAX_TOKEN_SEPARATORS);
	strcat_P(pTaskInfo->pBuf->tokenSeparatorList, tokenListConsole);

	result = strtok_r(myCommand, pTaskInfo->pBuf->tokenSeparatorList, &myCommand);
	resultCode = 1;				//No Command Received
	if(result != NULL)
	{
		resultCode = 2;			//Unknown Parser Code
		if(strncasecmp_P(result, powerParserID, strlen_P(powerParserID)) == 0)
		{
			resultCode = 3;		//Unknown command
			command = getCommand(&myCommand, powerCommands[0], LEN_POWER_COMMAND, NUM_POWER_COMMANDS, powerCmdList, pTaskInfo->pBuf->tokenSeparatorList);

			if(command != 0)
				goodCommand = TRUE;
		}
	}

	if(goodCommand == TRUE)
		resultCode = Power_ParseCommand(pTaskInfo, command, myCommand);

	if(resultCode != SUCCESS)
	{
		switch(resultCode)
		{
			case 1:
				pTaskInfo->pBuf->responseInfo.status = CONSOLE_RESPONSE_EMPTY_CMD;
				break;
			case 2:
				pTaskInfo->pBuf->responseInfo.status = CONSOLE_RESPONSE_WRONG_DEVICE;
				break;
			case 3:
				pTaskInfo->pBuf->responseInfo.status = CONSOLE_RESPONSE_UNKNOWN_CMD;
				break;
			default:
				pTaskInfo->pBuf->responseInfo.status = CONSOLE_RESPONSE_CMD_FAILED;
				break;
		}
	}

	Power_QueueConsoleResponse(pTaskInfo);

	memset(consoleIn[pTaskInfo->taskID].cmd, 0, MAX_FILE_SIZE_IN);
	consoleIn[pTaskInfo->taskID].cmdSrcAddress = 0;
	consoleIn[pTaskInfo->taskID].functionCode = 0;
	consoleIn[pTaskInfo->taskID].cmdSize = 0;
	consoleIn[pTaskInfo->taskID].id = 0;

	return;

}

uint8_t Power_ParseCommand(PPOWER_TASK pTaskInfo, char command, char* cmdParams)
{
	uint8_t retval = FAIL;

	switch(command)
	{
		case 'i':
		case 'I':
		case 's':
		case 'S':
			Power_Id(pTaskInfo);
			pTaskInfo->pBuf->responseInfo.status = CONSOLE_RESPONSE_OK;
			retval = SUCCESS;
			break;

		case 'o':
		case 'O':
		case 'r':
		case 'R':
			retval = EnablePIBModule(pTaskInfo->portNum, TRUE, FALSE);
			if (retval)
				pTaskInfo->pBuf->powerOn = TRUE;
			sprintf_P(pTaskInfo->pBuf->consoleResponse, PSTR("Power is %s"), pTaskInfo->pBuf->powerOn==TRUE ? "ON":"OFF");
			pTaskInfo->pBuf->responseInfo.length = strlen(pTaskInfo->pBuf->consoleResponse);
			pTaskInfo->pBuf->responseInfo.status = CONSOLE_RESPONSE_OK;
			retval = SUCCESS;
			break;

		case 'f':
		case 'F':
		case 'x':
		case 'X':
			retval = EnablePIBModule(pTaskInfo->portNum, FALSE, FALSE);
			if (retval)
				pTaskInfo->pBuf->powerOn = FALSE;
			sprintf_P(pTaskInfo->pBuf->consoleResponse, PSTR("Power is %s"), pTaskInfo->pBuf->powerOn==TRUE ? "ON":"OFF");
			pTaskInfo->pBuf->responseInfo.length = strlen(pTaskInfo->pBuf->consoleResponse);
			pTaskInfo->pBuf->responseInfo.status = CONSOLE_RESPONSE_OK;
			retval = SUCCESS;
			break;

		case 'a':
		case 'A':
		{
			char* result = NULL;

			memset(pTaskInfo->pBuf->tokenSeparatorList, 0, MAX_TOKEN_SEPARATORS);
			strcat_P(pTaskInfo->pBuf->tokenSeparatorList, tokenListConsole);

			result = strtok_r(cmdParams, pTaskInfo->pBuf->tokenSeparatorList, &cmdParams);

			if(result != NULL)
			{
				char* pTest = result;
				if((*pTest == 'o') || (*pTest == 'O'))
				{
					pTest++;
				
					if((*pTest == 'f') || (*pTest == 'F'))
						*result = *pTest;
				}

				switch(*result)
				{
					case 'y':
					case 'Y':
					case 'o':
					case 'O':
					case '1':
						pTaskInfo->pBuf->initState = AUTORUN_ON;
						retval = SUCCESS;
						break;
					case 'n':
					case 'N':
					case 'f':
					case 'F':
					case '0':
						pTaskInfo->pBuf->initState = AUTORUN_OFF;
						retval = SUCCESS;
						break;
					default:
						retval = FAIL;
				}


				if(retval == SUCCESS)
				{
					EepromWriteByteUnSigned((void*)(pTaskInfo->eepromAddress + POWER_AUTORUN_STATE_OFFSET), pTaskInfo->pBuf->initState);

					sprintf_P(pTaskInfo->pBuf->consoleResponse, PSTR("AutoRun is %s"), pTaskInfo->pBuf->initState==AUTORUN_ON ? "ON":"OFF");
					pTaskInfo->pBuf->responseInfo.length = strlen(pTaskInfo->pBuf->consoleResponse);
					pTaskInfo->pBuf->responseInfo.status = CONSOLE_RESPONSE_OK;
				}

			}
			else
			{
				retval = FAIL;
			}
			break;
		}
		default:
			retval = FAIL;
			break;
	}

	return retval;

}

uint8_t Power_Id(PPOWER_TASK pTaskInfo)
{
	uint8_t retval = SUCCESS;

	char tempBuf[20];

	memset((char*)pTaskInfo->pBuf->consoleResponse, 0, 256);
	sprintf_P(pTaskInfo->pBuf->consoleResponse, PSTR("Power is %s: "), pTaskInfo->pBuf->powerOn==TRUE ? "ON":"OFF");
	memset(tempBuf, 0, 20);
	sprintf_P(tempBuf, PSTR("AutoRun is %s"), pTaskInfo->pBuf->initState==AUTORUN_ON ? "ON":"OFF");
	strcat(pTaskInfo->pBuf->consoleResponse, tempBuf);
	pTaskInfo->pBuf->responseInfo.length = strlen(pTaskInfo->pBuf->consoleResponse);

	return retval;

}

void Power_QueueConsoleResponse(PPOWER_TASK pTaskInfo)
{
	PCONSOLE_ACK_XMIT_2 pReport;
	uint8_t err = 0;
	char* pData;

	OSMutexPend(pMutexFloatQueue, INFINITE, &err);

	pReport = (PCONSOLE_ACK_XMIT_2)&(txFloatAck[headFloatAckTx]);
	pData = (char*)pReport;

	pReport->comm.headers.destination = FLOAT_TASK_FORWARD_MESSAGE;		//pTaskInfo->pBuf->responseInfo.source;

	pReport->comm.fwdInfo.msgFormat = MSG_FORMAT_SHIP_TO_SHORE;
	pReport->comm.fwdInfo.destination = pTaskInfo->pBuf->responseInfo.source;
	pReport->comm.fwdInfo.source = mainTaskAddress | pTaskInfo->taskID;

	if(pTaskInfo->pBuf->responseInfo.status < CONSOLE_RESPONSE_UNKNOWN_CMD)
	{
		pReport->comm.headers.length = sizeof(PIB_DATA_FORWARD_MESSAGE) + sizeof(CONSOLE_ACK) + pTaskInfo->pBuf->responseInfo.length + CRC16_SIZE;

		pReport->comm.iridiumInfo.headerICD.type = ACK_CONSOLE_DATA;

		pReport->consoleAck.len = pTaskInfo->pBuf->responseInfo.length;
		pReport->consoleAck.disposition = pTaskInfo->pBuf->responseInfo.status;
		pReport->consoleAck.format = pTaskInfo->pBuf->responseInfo.format;
		pData = pData + sizeof(PIB_DATA_FORWARD_MESSAGE) + sizeof(CONSOLE_ACK);
		memcpy(pData, pTaskInfo->pBuf->consoleResponse, pTaskInfo->pBuf->responseInfo.length);

		//memcpy(dbgMsg, pTaskInfo->pBuf->consoleResponse, pTaskInfo->pBuf->responseInfo.length);
		//dbgMsg[pTaskInfo->pBuf->responseInfo.length] = 0;
		//SEND_MSG;
	}
	else
	{
		pReport->comm.headers.length = sizeof(PIB_DATA_FORWARD_MESSAGE) + sizeof(CONSOLE_NACK) + CRC16_SIZE;
		pReport->comm.iridiumInfo.headerICD.type = NACK_CONSOLE_DATA;
		pReport->consoleNack.numDispositions = 1;
		pReport->consoleNack.disposition[0] = pTaskInfo->pBuf->responseInfo.status;
	}

	pReport->comm.iridiumInfo.headerICD.packetID = pTaskInfo->pBuf->responseInfo.id;
	pReport->comm.iridiumInfo.headerICD.size = pReport->comm.headers.length - sizeof(PIB_DATA_FORWARD_MESSAGE) - CRC16_SIZE;


	(headFloatAckTx < MAX_ACK_BUFFERS - 1) ? (headFloatAckTx++) : (headFloatAckTx = 0);

	if(headFloatAckTx == tailFloatAckTx)
	{
		(tailFloatAckTx < MAX_ACK_BUFFERS - 1) ? (tailFloatAckTx++) : (tailFloatAckTx = 0);
	}

	OSMutexPost(pMutexFloatQueue);

	return;
}

void SerialPortPassThroughPower(uint8_t port)
{
	readyForSerialPassThru = TRUE;

	while(1)
	{
		OSTimeDlyHMSM(1,0,0,0);
	}
}