/****************************************************************************/
/* Copyright 2012-2018 MBARI												*/
/****************************************************************************/
/* Summary	: Watch Cycle Routines for BEDS2 on PIC32MX470					*/
/* Filename : watch.c														*/
/* Author	: Robert Herlien (rah)											*/
/* Project	: BEDS (Benthic Event Detection System)							*/	
/* Revision : 1.0															*/
/* Created	: 10/15/2012													*/
/*																			*/
/* MBARI provides this documentation and code "as is", with no warranty,	*/
/* express or implied, of its quality or consistency. It is provided without*/
/* support and without obligation on the part of the Monterey Bay Aquarium	*/
/* Research Institute to assist in its use, correction, modification, or	*/
/* enhancement. This information should not be published or distributed to	*/
/* third parties without specific written permission from MBARI.			*/
/*																			*/
/****************************************************************************/
/* Modification History:													*/
/* 15oct2012 rah - created for BEDS											*/
/* 23feb2018 rah - Ported to BEDS2 on PIC32MX470							*/
/****************************************************************************/

#include <mbariTypes.h>			/* MBARI type definitions				*/
#include <beds.h>				/* BEDS general definitions				*/		
#include <watch.h>				/* BEDS watch cycle						*/
#include <event.h>				/* BEDS event cycle						*/
#include <dig_io.h>				/* BEDS digital I/O definitions			*/
#include <serial.h>				/* BEDS serial I/O definitions			*/
#include <file.h>				/* BEDS file I/O definitions			*/
#include <config.h>				/* BEDS configuration					*/
#include <clock.h>				/* BEDS Clock module					*/
#include <variable.h>			/* Variable function definitions		*/
#include <imu.h>				/* BEDS IMU routines					*/
#include <bedsUI.h>				/* BEDS Command line processor			*/
#include <modem.h>				/* BEDS acoustic modem software			*/
#include <adc.h>				/* BEDS Analog Module					*/
#include <fatFs/ff.h>			/* FatFs definitions					*/
#include <syslog.h>				/* System logger						*/

#include <stddef.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <string.h>


/********************************/
/*		External Data			*/
/********************************/

Extern Int32	evtFreq;				/* FIFO rate during event		*/
Extern Int32	watchFreq;				/* FIFO rate during watch cycle */
Extern Flt32	lowBattSampleShdn;		/* Battery level to stop sampling */
Extern Flt32	lowBattOff;				/* Battery level to shut down	*/
Extern Int32	evtPressureMs;			/* Period (ms) to get press in event*/
Extern Int32	sysRecSecs;				/* Period (secs) to get sys parms in watch*/
Extern Int32	modemPort;				/* Modem port					*/
Extern Int32	modemPeriod;			/* Period (secs) to turn on modem	*/
Extern Int32	modemOffset;			/* Offset from top of modemPeriod	*/
Extern Int32	modemOnTime;			/* "On" time for modem				*/
Extern Int32	modemWarmup;			/* Warmup time for turning on ac. modem*/
Extern Int32	modemWait;				/* Wait time for turning off ac. modem*/
Extern MBool	modemDebug;				/* Look for chars even when modem off*/
Extern Int32	sensorWarmup;			/* Warmup time for turning on system sensors*/
Extern Int32	sensorMinSecs;			/* If sysRecSecs < this, leave sensors on*/

Extern Int32	deploymentModemPeriod;	/* Modem Period during deployment mode*/
Extern Int32	deploymentModeSecs;		/* Time to stay in deployment mode*/

Extern volatile Nat32 accurateClkSec;	 /* Num ints from DS3234		*/
Extern volatile time_t	accurateTod;	 /* DS3234 time of day			*/

#define WATCH_ADC_AVG	10


/********************************/
/*		External Data			*/
/********************************/

Extern MBool	wdtOn;					/* Use watchdog timer			*/


/********************************/
/*		Global Data				*/
/********************************/

Global Nat16	modemMinBatt = NAT16_MAX;


/********************************/
/*		Module Local Data		*/
/********************************/

MLocal DataFileStruct watchStruct = 
	{"WAT", "WATCH.IDX", 1, 600, 0, 0, 0};

MLocal RunMode	runMode = WATCH_MODE;
MLocal MBool	deploymentMode = TRUE;
MLocal time_t	deploymentTime;
MLocal time_t	lastSec = 0L;
MLocal Nat32	lastTick = 0L;
MLocal time_t	lastSensorCycle = 0L;
MLocal time_t	lastModemCycle = 0L;

MLocal DeviceState modemState = OffState, sensorState = OffState;
MLocal Int32	modemTimer = 0, sensorTimer = 0;

MLocal MBool	eventTmout = FALSE;

/* Who's using the sensors power		*/
MLocal Nat16	sensorUsers = 0;

// Forward Declaration
MLocal MBool watchSecond(void);


/************************************************************************/
/* Function	   : watchInit												*/
/* Purpose	   : Initialize this module									*/
/* Inputs	   : None													*/
/* Outputs	   : OK or ERROR											*/
/************************************************************************/
Errno watchInit(void)
{
	watchStruct.fileIndex = 1;

	setDeploymentMode(TRUE);
	lastSec = clkTime();				/* Init sec counter				*/
	lastTick = clkGetTick();			/* Init tick counter			*/
	return(OK);
}


/************************************************************************/
/* Function	   : getDeploymentMode										*/
/* Purpose	   : Get status of deployment mode							*/
/* Inputs	   : None													*/
/* Outputs	   : TRUE if ON, else FALSE									*/
/************************************************************************/
MBool getDeploymentMode(void)
{
	return(deploymentMode);
}


/************************************************************************/
/* Function	   : getRunMode												*/
/* Purpose	   : Get current running mode								*/
/* Inputs	   : None													*/
/* Outputs	   : COMMAND_MODE, WATCH_MODE, or EVENT_MODE				*/
/************************************************************************/
RunMode getRunMode(void)
{
	return(runMode);
}


/************************************************************************/
/* Function	   : setDeploymentMode										*/
/* Purpose	   : Turn on/off deployment mode							*/
/* Inputs	   : MBool, TRUE for ON										*/
/* Outputs	   : OK or ERROR											*/
/************************************************************************/
void setDeploymentMode(MBool onoff)
{
	sysLogPrintf("Turning %s deployment mode", onoff ? "ON" : "OFF");

	deploymentMode = onoff;
	if (onoff)
		deploymentTime = clkTime();
}


/************************************************************************/
/* Function	   : recordDataPoint										*/
/* Purpose	   : Record one data point									*/
/* Inputs	   : None													*/
/* Outputs	   : OK or ERROR											*/
/************************************************************************/
Errno recordDataPoint(DataFileStruct *dfp)
{
	InertialRcd idata;
	Nat32		i;
	Byte		*p, *valp;
	Nat32		accel[3];
	Nat32		quat[4];
	Int32		val, absVal;

	if ((imuGetAccel(accel, 3) == OK) &&
		(imuGetQuat(quat, 4) == OK))
	{
		idata.recType = InertialDataType;
		p = &idata.accel[0][0];
		for (i = 0; i < 3; i++, p+=3)
		{
			val = accel[i];
			valp = (Byte *)(&accel[i]) + 2;
			*p++ = *valp--;
			*p++ = *valp--;
			*p++ = *valp--;
			absVal = (val < 0) ? -val : val;
			if (absVal > watchStruct.maxVal)
				watchStruct.maxVal = absVal;
		}
		p = &idata.quat[0][0];
		for (i = 0; i < 4; i++, p+=QUAT_SIZE)
		{
			valp = (Byte *)(&quat[i]) + 3;
			*p++ = *valp--;
			*p++ = *valp--;
			*p++ = *valp--;
			*p++ = *valp--;
		}
	}

	writeDataFile(dfp, &idata, sizeof(idata));
	return(OK);

} /* recordDataPoint() */


/************************************************************************/
/* Function	   : setupWatchCycle										*/
/* Purpose	   : Set up to start a watch cycle							*/
/* Inputs	   : None													*/
/* Outputs	   : None													*/
/************************************************************************/
MLocal void setupWatchCycle(void)
{
	runMode = WATCH_MODE;
	serRxFlush(1);
	serRxFlush(2);
	sysLog("Entering Watch Cycle");

	imuSetup(watchFreq);
	watchStruct.dataRate = 60*watchFreq;
	watchStruct.maxVal = 0;
	sensorsOff(SENSOR_USERS_ALL);
	sensorState = OffState;
	openNewDataFile(&watchStruct);

	/* Read and throw away any samples still in queue.	Old samples		*/
	/* could be interpreted with old multipliers (before setDMP())		*/
//	umplOnTimer();
}


/************************************************************************/
/* Function	   : stopWatchCycle											*/
/* Purpose	   : Tear down a watch cycle								*/
/* Inputs	   : None													*/
/* Outputs	   : None													*/
/************************************************************************/
MLocal void stopWatchCycle(void)
{
	closeDataFile(&watchStruct);
	sensorsOff(SENSOR_USERS_ALL);
	sensorState = OffState;
	sysLog("Exiting Watch Cycle");
}


/************************************************************************/
/* Function	   : newSecond												*/
/* Purpose	   : See if clock ticked to a new second					*/
/* Inputs	   : None													*/
/* Outputs	   : TRUE if new second, else FALSE							*/
/************************************************************************/
MBool newSecond(void)
{
	/* Check for new second		*/
	if ((lastSec == clkTime()) &&
		((clkGetTick() - lastTick) < 2*TICKS_PER_SECOND))
		return(FALSE);

	/* Start of a new second	*/

	RTCack();
	lastSec = clkTime();
	lastTick = clkGetTick();

	if (wdtOn)
		wdtPing();					/* Keep watchdog running */

	return(TRUE);

} /* newSecond() */


/************************************************************************/
/* Function	   : watchCycle												*/
/* Purpose	   : Run the watch cycle for the BEDS application			*/
/* Inputs	   : None													*/
/* Outputs	   : None													*/
/************************************************************************/
void watchCycle(void)
{
	Nat32	delay;
	SerPort	auxPort;
	
	modemState = OffState;				/* Start with modem off			*/
	modemOff();					
	auxPort = (modemPort == 1) ? 2 : 1;

	setupWatchCycle();

	while(TRUE)
	{
		while(imuReadFifo() > 0)
			recordDataPoint(&watchStruct);

		/* Check for new second */
		if (newSecond())
		{
			if (deploymentMode && (clkTime() >= (deploymentTime + deploymentModeSecs)))
				setDeploymentMode(FALSE);

			checkVars();				/* Make sure variables are within limits */

			if (watchSecond())			/* Do 1 second periodic tasks	*/
			{							/* If modem connect				*/
				stopWatchCycle();		/* stop cycle & run UI			*/
				eventTmout = FALSE;		/* Running UI cycle, so clear event tmout*/

				runMode = COMMAND_MODE;
				runUI(modemPort);
				modemState = WaitOff;	/* Mark modem for turning off, but*/
				modemTimer = 0;			/* wait for all chars to be xmitted*/
										/* Prevent another immed modem chk*/
				setupWatchCycle();		/* When UI done, run next watch cycle*/
			}
		}
		else if (modemState == OnState) /* If checking for modem char,	*/
			clkDelayUs(10000);			/* don't sleep but just delay	*/
		else							 /* Else sleep to allow FIFO to fill*/
		{
			delay = loopDelay(watchFreq);
			if (delay >= TICKS_PER_SECOND)
				clkSleep(1);
			else
				clkIdle(delay);
		}

		if (serRxCount(auxPort))
		{								/* If key hit on aux port,		*/
			stopWatchCycle();			/* stop cycle, and run UI		*/
			modemState = OffState;
			modemOff();

			runMode = COMMAND_MODE;
			runUI(auxPort);

			setupWatchCycle();			/* When done, run next watch cycle*/
			clkIntsOn();				/* Start up clk ints again		*/
		}

		if ((imuGetMotionState() == IMU_MOTION) && !eventTmout && !deploymentMode)
		{
			stopWatchCycle();
			modemState = OffState;
			modemOff();
			runMode = EVENT_MODE;
			if (eventCycle() == TMOUT)			/* Run event cycle		*/
				eventTmout = TRUE;				/* If tmout, force modem check*/
			setupWatchCycle();
		}
	}
}


/************************************************************************/
/* Function	   : sensorsOn												*/
/* Purpose	   : Turn on power to the internal/external sensors			*/
/* Inputs	   : Who needs the sensors on								*/
/* Outputs	   : None													*/
/************************************************************************/
void sensorsOn(Nat16 who)
{
	digSensorsEnbl();
	sensorUsers |= who;
}


/************************************************************************/
/* Function	   : sensorsOff												*/
/* Purpose	   : Turn off power to the internal/external sensors		*/
/* Inputs	   : Who's releasing sensor power							*/
/* Outputs	   : None													*/
/************************************************************************/
void sensorsOff(Nat16 who)
{
	sensorUsers &= ~who;
	if (sensorUsers == 0)
	{
		digSensorsDisbl();
		sensorState = OffState;
	}
}


/************************************************************************/
/* Function	   : forceModemCycle										*/
/* Purpose	   : Force a Modem session									*/
/* Inputs	   : None													*/
/* Outputs	   : None													*/
/************************************************************************/
void forceModemCycle(void)
{
	eventTmout = TRUE;
}


/************************************************************************/
/* Function	   : writeSysRec											*/
/* Purpose	   : Write a system record into the Watch data file			*/
/* Inputs	   : DataFileStruct											*/
/* Outputs	   : OK or ERROR											*/
/************************************************************************/
Errno writeSysRec(DataFileStruct *dfp)
{
	SystemRcd	sr;		

	if (dfp == NULL)
		return(ERROR);

	memset(&sr, 0, sizeof(sr));
	sr.recType = SysRecType;
	time(&sr.rcdTime);

	sr.battVoltage = adcSampleAvg(BATT_CHAN, WATCH_ADC_AVG);
	sr.minModemBatt = modemMinBatt;
	sr.extPressure = adcSampleAvg(EXT_PRESS_CHAN, WATCH_ADC_AVG);
	sr.intPressure = adcSampleAvg(INT_PRESS_CHAN, WATCH_ADC_AVG);
	sr.intTemp = adcSampleAvg(TEMP_CHAN, WATCH_ADC_AVG);
	sr.intHumidity = adcSampleAvg(HUMIDITY_CHAN, WATCH_ADC_AVG);

	writeDataFile(dfp, &sr, sizeof(sr));
	modemMinBatt = adcSample(MODEM_MIN_CHAN);
	return(OK);

} /* writeSysRec() */


/************************************************************************/
/* Function	   : watchSecond											*/
/* Purpose	   : Do the once/second watch cycle activity				*/
/* Inputs	   : None													*/
/* Outputs	   : TRUE if found modem connection, else FALSE				*/
/************************************************************************/
MLocal MBool watchSecond(void)
{
	time_t		tod;
	Int32		todMod, thisModemPeriod;

	tod = clkTime();					/* Get and remember current time */
	writeSecondMarker(&watchStruct, tod);

	if (sensorState == OffState)		/* Check for time to sample sensors*/
	{
		todMod = tod % sysRecSecs;
		if ((todMod < sensorWarmup) || ((tod - lastSensorCycle) > sysRecSecs))
		{
			sensorsOn(SENSORS_SAMPLING);
			sensorState = Warmup;
			sensorTimer = 0;
		}
	}
	else if ((sensorState == Warmup) && (++sensorTimer >= sensorWarmup))
	{
		writeSysRec(&watchStruct);
		lastSensorCycle = tod;
		if (sysRecSecs > sensorMinSecs)
		{
			sensorsOff(SENSORS_SAMPLING);
			sensorState = OffState;
		}
	}

	if (modemState == OffState)
	{
		thisModemPeriod = deploymentMode ? deploymentModemPeriod : modemPeriod;

		todMod = (tod - modemOffset) % thisModemPeriod;
		if ((todMod < modemWarmup) || ((tod - lastModemCycle) > thisModemPeriod)
			|| eventTmout)
		{
			modemOn();
			modemState = Warmup;
			modemTimer = 0;
		}
	}
	else if ((modemState == Warmup) && (++modemTimer >= modemWarmup))
	{
		modemState = OnState;
		modemTimer = 0;
		serRxFlush(modemPort);				/* Toss away modem signon msg	*/
	}
	else if ((modemState == WaitOff) && (++modemTimer >= modemWait))
	{
		modemState = OffState;
		modemOff();
		lastModemCycle = tod;
	}

	if (modemState == OnState)
	{
		//check for modem connection
		if (modemConnectCheck())
		{
			sysLog("Got modem connection");
			return(TRUE);				/* Return TRUE to go to User I/F		*/
		}

		if (++modemTimer >= modemOnTime)
		{								/* If timeout checking for modem connect,*/
			modemState = OffState;		/* go back to quiescent state			*/
			sysLog("Timed out waiting for modem connection");
			modemOff();
			lastModemCycle = tod;
			eventTmout = FALSE;			/* Did modem cycle, so clear event tmout*/
		}
	}

	/* 3/5/2018 rah, Debug code -- Always check modem port so we don't	*/
	/* have to wait	for modem to cycle.  For debugging with direct		*/
	/* serial line on what's normally the modem port.					*/
	if (modemDebug && serRxCount(modemPort))
		return(TRUE);
	
	return(FALSE);

} /* watchSecond() */