//----------------------------------------------------------------------------------
// <copyright file="app_init.c" company="LiquidRobotics">
//	Copyright (c) Liquid Robotics Corporation.  All rights reserved.
// </copyright>
//
// <summary>
// 	Application initialization module, which contains methods called by main();
//  these functions are intended to be overridden by the client app.
// </summary>
//
// <owner>Mike Cookson</owner>
//---------------------------------------------------------------------------------

#include "includes.h"
#include <avr/io.h>
#include <avr/wdt.h>
#include <avr/power.h>
#include "user_app_init.h"
#include "uart.h"
#include "pib.h"
#include "timer.h"
#include "lrstat.h"
#include "spidebug.h"
#include "wdtrig.h"
//#include "boot.h"

#include "pib_apps.h"
#include "User_Health.h"
#include "FltPyldComms.h"
#include "UserData.h"
#include "Turner_C3.h"
#include "Turner_C3_Orig.h"
#include "SeabirdCTD_37SI.h"
#include "Seabird37SI.h"
#include "SeabirdGPCTD.h"
#include "Empty.h"
#include "Power.h"
#include "TeledyneATM-88X.h"
#include "MBARI_CO2.h"

#include "boot.h"

#include "SerialDebug.h"

BOOLEAN restartCause_Watchdog;			// true of watchdog caused reset, false if not

OS_EVENT* pMutexFloatQueue;

char taskBuffers[TASK_BUF_SIZE * NUM_TASK_BUFS] __attribute__((section(".xdata")));

void AppInit(void)
{
	MCUSR = 0;
	wdt_disable();

	//Port A -- If External Memory Enabled, It Overrides Settings Here
	PORTA = 0x00;	//Disable Pull-Ups
	DDRA = 0x00;	//Set All Pin to Input


	//Port B -- If SPI enabled, It Overrides Pins 3:0, Pins 7:0 are PCINT 7:0
	PORTB = 0x00;	//Disable Pull-Ups
	DDRB = 0x00;	//Set All Pin to Input

	//Port C -- If External Memory Enabled, It Overrides Settings Here
	PORTC = 0x00;	//Disable Pull-Ups
	DDRC = 0x00;	//Set All Pin to Input

	//Port D -- If USART1 is Enabled, it overrides Pins 3:2, If TWI is enabled, it
	//Overrides Pins 1:0, Pins 3:0 are also INT 3:0
	PORTD = 0x00;	//Disable Pull-Ups
	DDRD = 0x00;	//Set All Pin to Input

	//Port E -- If USART0 is Enabled, it overrides Pins 1:0, Pins 7:4 are INT 7:4, Pin0 is PCINT8
	PORTE = 0x00;	//Disable Pull-Ups
	DDRE = 0x00;	//Set All Pin to Input

	//Port F -- If JTAG is enabled, it overrides Pins 7:4, Pins 7:0 are ADC inputs 7:0
	PORTF = 0x00;	//Disable Pull-Ups
	DDRF = 0x00;	//Set All Pin to Input

	//Port G -- Only 6 Pins on Port G, 32KHz Oscillator Enabled overrides Pins 4:3
	// If External Memory is enabled, it overrides Pins 2:0
	PORTG = 0x00;	//Disable Pull-Ups
	DDRG = 0x00;	//Set All Pin to Input

	//Port H -- If USART2 is Enabled, it overrides Pins 1:0
	PORTH = 0x00;	//Disable Pull-Ups
//		DDRH = 0x00;	//Set All Pin to Input

	//Port J -- If USART3 is Enabled, it overrides Pins 1:0, Pins 6:0 are PCINT 15:9
	PORTJ = 0x00;	//Disable Pull-Ups
	DDRJ = 0x00;	//Set All Pin to Input

	//Port K -- Pins 7:0 are ADC INputs 7:0, Pins 7:0 are PCINT 23:16
	PORTK = 0x00;	//Disable Pull-Ups
	DDRK = 0x00;	//Set All Pin to Input

	//Port L 
	PORTL = 0x00;	//Disable Pull-Ups
	DDRL = 0x00;	//Set All Pin to Input

	power_all_disable();
	OStmrDefault();
	OSuartDefaultState();
	BootCheckProgramImage();
	restartCause_Watchdog = BootCausedByWatchdog();
//#if WDTRIG_ENABLED != 0
//	OSwatchdogInit(WDTRIG_CONFIG);
//#endif
#if SPIDEBUG_ENABLED != 0
	OSspiDebugInit();
#endif
}

extern uint8_t InitializeFileTransferManager();
extern PYLD_COMMS_INIT commPortState __attribute__((section(".xdata")));
extern BOOL fileAllowed[NUM_TASK_FILES] __attribute__((section(".xdata")));
//TASK_INIT_STRUCT taskInit1;
//TASK_INIT_STRUCT taskInit2;
//TASK_INIT_STRUCT taskInit3;

TASK_INIT_STRUCT taskInit[3] __attribute__((section(".xdata")));

extern void Float_Comm_Init(void);

DEBUG_OUTPUT_INFO	descTaskInfo __attribute__((section(".xdata")));
char 				descTaskMsg[80] __attribute__((section(".xdata")));

AppStartContinuationTask_t AppStartTasks(void)
{
//static OS_STK stax[3][256];
	void AppContinuation(void* pdata);
	void BusyTask(void* pdata);
	INT8U oserr;
	uint8_t task;
	uint8_t flagOutput = 0;

	DisableAllPIBModules();
	OStmrInit();
#if OS_TASK_STAT_EN
	OSStatInit();
#endif


	

	pMutexFloatQueue = OSMutexCreate(29, &oserr);	


	DEFINE_TASK_STACK_AND_LAUNCH(HealthStatus, 256, UserHealthStatus, (void*) 0, 60, 1001, (void*) 0, OS_TASK_OPT_STK_CHK, oserr);

	
	commPortState.pmState[0] = PM_STANDARD_COMM;	//Float - Always set to standard comm at this point -- have it hear for ease of indexing
	//commPortState.pmState[1] = PM_COMM_NOT_USED;	//PM 1 may have a dumb sensor so not used for payload Comms
	//commPortState.pmState[2] = PM_COMM_NOT_USED;			//SMC on PM2
	//commPortState.pmState[3] = PM_COMM_NOT_USED;	//PM 3 may have a dumb sensor so not used for payload Comms
	


	// Load  Tasks
	uint8_t tasks[3];
	for(int i=0; i<3; i++)
	{
		uint16_t eepromAddress;

		eepromAddress = EEPROM_SYSTEM_PARAMS_START + EEPROM_TASK1_OFFSET + (i * EEPROM_TASK_SIZE);

		EepromReadByte((void*)eepromAddress, (void*)&task);
		if(task == 0xFF)
		{
			task = 0;
			EepromWriteByteUnSigned((void*)eepromAddress, task);
		}

		tasks[i] = task;

		switch(task)
		{
			case PYLDCOMM:
				commPortState.pmState[i+1] = PM_STANDARD_COMM;
				break;

			case SMC:
				commPortState.pmState[i+1] = PM_SMC_COMM;
				break;

			default:
				commPortState.pmState[i+1] = PM_COMM_NOT_USED;				
				break;

		}
	}


	fileAllowed[0] = FALSE;

	Float_Comm_Init();

	DEFINE_TASK_STACK_AND_LAUNCH(FloatComm, 512, FloatComms, (void*) 0, 20, 1002, (void*) 0, OS_TASK_OPT_STK_CHK, oserr);

	for(int i=0; i<3; i++)
	{
		task = tasks[i];

		sprintf_P(dbgMsg, PSTR("i=%d, tsk=%d"), i, task);
		SEND_MSG;		

		memset(descTaskMsg, 0, 80);

		void *taskToLaunch = NULL;


		switch(task)
		{
			case PYLDCOMM:				
				fileAllowed[i+1] = FALSE;

				flagOutput = 1;
				sprintf_P(descTaskMsg, PSTR("\r\nBoard ID=%02d, Task ID=%02d, Task Running is the Payload Comm Support\r\n\r\n"), mainBoardID, i+1);

				break;

			case SMC:				
				fileAllowed[i+1] = FALSE;

				flagOutput = 1;
				sprintf_P(descTaskMsg, PSTR("\r\nBoard ID=%02d, Task ID=%02d, Task Running is the SMC Comm Support\r\n\r\n"), mainBoardID, i+1);
				break;

			case CTD37SI:				
				fileAllowed[i+1] = TRUE;

				flagOutput = 1;
				sprintf_P(descTaskMsg, PSTR("\r\nBoard ID=%02d, Task ID=%02d, Task Running is the Seabird 37SI CTD\r\n\r\n"), mainBoardID, i+1);
				//taskToLaunch = Sbrd37SI;
				taskToLaunch = SB37SI;
				break;

			case GPCTD:
				fileAllowed[i+1] = TRUE;

				flagOutput = 1;
				sprintf_P(descTaskMsg, PSTR("\r\nBoard ID=%02d, Task ID=%02d, Task Running is the Seabird GPCTD\r\n\r\n"), mainBoardID, i+1);
				taskToLaunch = SbrdGPCTD;
				break;

			case C3:
				fileAllowed[i+1] = TRUE;

				flagOutput = 1;
				sprintf_P(descTaskMsg, PSTR("\r\nBoard ID=%02d, Task ID=%02d, Task Running is the Turner C3 Flourometer\r\n\r\n"), mainBoardID, i+1);

				taskToLaunch = TurnerC3_Orig;

				break;

			case EMPTY:
				fileAllowed[i+1] = FALSE;

				flagOutput = 1;
				sprintf_P(descTaskMsg, PSTR("\r\nBoard ID=%02d, Task ID=%02d, Task Running is an Empty PM Sensor\r\n\r\n"), mainBoardID, i+1);

				taskToLaunch = Empty;

				break;

			case POWER:
				fileAllowed[i+1] = FALSE;

				flagOutput = 1;
				sprintf_P(descTaskMsg, PSTR("\r\nBoard ID=%02d, Task ID=%02d, Task Running is a Power On/Off Sensor\r\n\r\n"), mainBoardID, i+1);

				taskToLaunch = Power;

				break;

			case ATM:
				fileAllowed[i+1] = TRUE;

				flagOutput = 1;
				sprintf_P(descTaskMsg, PSTR("\r\nBoard ID=%02d, Task ID=%02d, Task Running is Teledyne ATM\r\n\r\n"), mainBoardID, i+1);

				taskToLaunch = TeledyneATM88X;

				break;

			case CO2:
				fileAllowed[i+1] = TRUE;

				flagOutput = 1;
				sprintf_P(descTaskMsg, PSTR("\r\nBoard ID=%02d, Task ID=%02d, Task Running is MBARI CO2 Sensor\r\n\r\n"), mainBoardID, i+1);

				taskToLaunch = MBARI_CO2;

				break;

			default:
				fileAllowed[i+1] = FALSE;

				flagOutput = 1;
				sprintf_P(descTaskMsg, PSTR("\r\nBoard ID=%02d, Task ID=%02d, Task is not used/unknkown\r\n\r\n"), mainBoardID, i+1);
				break;
		}
								
								
		memcpy(dbgMsg, descTaskMsg, 80);
		SEND_MSG;
			
		if(flagOutput != 0)
		{
			descTaskInfo.pBuffer = descTaskMsg;
			descTaskInfo.lenBuffer = strlen(descTaskMsg);
			descTaskInfo.fmtBuffer = DEBUG_ASCII;
			descTaskInfo.dbgLevel = 5;

			SendDebugMsg(&descTaskInfo);
			flagOutput = 0;
		}

		if(taskToLaunch!=NULL)
		{
			taskInit[i].taskID = i+1;
			taskInit[i].portNum = i+1;
			taskInit[i].pBuf = &taskBuffers[i * TASK_BUF_SIZE];
		
				
			switch(i)
			{
				case 0:
					DEFINE_TASK_STACK_AND_LAUNCH(PM1, 512, taskToLaunch, (void*) &taskInit[i], 30 + (10 * i), 1003 + i, (void*) 0, OS_TASK_OPT_STK_CHK, oserr);
					break;

				case 1:
					DEFINE_TASK_STACK_AND_LAUNCH(PM2, 512, taskToLaunch, (void*) &taskInit[i], 30 + (10 * i), 1003 + i, (void*) 0, OS_TASK_OPT_STK_CHK, oserr);
					break;

				case 2:
					DEFINE_TASK_STACK_AND_LAUNCH(PM3, 512, taskToLaunch, (void*) &taskInit[i], 30 + (10 * i), 1003 + i, (void*) 0, OS_TASK_OPT_STK_CHK, oserr);
					break;

			}				
			

//			switch(i)
//			{
//				case 0:
//					DEFINE_TASK_STACK_AND_LAUNCH(PM1, 512, SbrdGPCTD, (void*) &taskInit[i], 30 + (10 * i), 1003 + i, (void*) 0, OS_TASK_OPT_STK_CHK, oserr);
//					break;
//				case 1:
//					DEFINE_TASK_STACK_AND_LAUNCH(PM2, 512, SbrdGPCTD, (void*) &taskInit[i], 40, 1004, (void*) 0, OS_TASK_OPT_STK_CHK, oserr);
//					break;
//				case 2:
//					DEFINE_TASK_STACK_AND_LAUNCH(PM3, 512, SbrdGPCTD, (void*) &taskInit[i], 50, 1005, (void*) 0, OS_TASK_OPT_STK_CHK, oserr);
//					break;
//			}

		}
	}	

	// I'm going to wait for 5 minutes and then clear the "tentative" flag
	OSTimeDlyHMSM(0,5,0,0);

	extern BOOL handledStatusRequest;

	if (handledStatusRequest)
		BootClearTentativeImage();
	else if (BootImageIsTentative())
		BootRestart(FALSE);

	return (AppStartContinuationTask_t)0;
}

#if 0
	#define MON_PORT (0)
	#define MON_PUTS(putstr) OSuartPutStrWait(MON_PORT, putstr, 0, &err)
#endif

void AppContinuation(void* pdata)
{
#if 0
	extern void LRPS_Command(uint8_t portID);
	uint8_t err;

	OSuartInitPort(MON_PORT, 9600, UART_MODE_EIGHT_DATA_BITS, UART_MODE_TWO_STOP_BITS, UART_MODE_NO_PARITY);
	OSTimeDlyHMSM(0,0,1,0);
#endif

	while(OS_TRUE)
	{
		OSTimeDlyHMSM(0,0,10,0);
#if 0
		LRPS_Command(MON_PORT);
		MON_PUTS("\n\n");
#endif
	}
}


void BusyTask(void* pdata)
{
	while(OS_TRUE)
	{
		uint32_t cdown = 10000;
		while(--cdown);
		OSTimeDlyHMSM(0,0,0,100);
	}
}