/****************************************************************************/
/* Copyright 2003-2012 MBARI						    */
/****************************************************************************/
/* Summary  : Watch Cycle Routines for BEDS on Persistor CF2		    */
/* 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						    */
/* $Log$
 */
/****************************************************************************/

#include <mbariTypes.h>		/* MBARI type definitions		*/
#include <mbariConst.h>		/* MBARI constants			*/
#include <cfxpico.h>		/* Persistor PicoDOS Definitions	*/
#include <beds.h>		/* BEDS general definitions		*/	
#include <watch.h>		/* BEDS watch cycle			*/
#include <event.h>		/* BEDS event cycle			*/
#include <io.h>			/* BEDS 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 <dmpSetup.h>		/* BEDS DMP setup routines		*/
#include <bedsUI.h>		/* BEDS Command line processor		*/
#include <modem.h>		/* BEDS acoustic modem software		*/
#include <adc.h>		/* BEDS Analog Module			*/
#include <syslog.h>		/* System logger			*/

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

/* From Invensense MotionApps Stack */
#include "log.h"
#undef MPL_LOG_TAG
#define MPL_LOG_TAG "uMPL-main"

// MPL interface
#include "ml.h"
#include "mldl.h"
#include "mlFIFO.h"
#include "umpl-states.h"


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

Extern Int32	evtFreq;		/* FIFO rate during event	*/
Extern Int32	watchFreq;		/* FIFO rate during watch cycle */
Extern Int32	modemBaud;		/* Baud rate on SCI port	*/
Extern Int32	debugBaud;		/* Baud rate on TPU port	*/
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	clkDriftPeriod;		/* Period (secs) to Write clock drift file*/
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 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


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

Global Nat16	modemMinBatt = NAT16_MAX;


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

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

MLocal MBool	deploymentMode = TRUE;
MLocal time_t	deploymentTime;
MLocal Nat32	lastAccSec = 0L;
MLocal Nat32	lastAccTick = 0L;
MLocal time_t	lastClockDrift = 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;
    watchStruct.ppbBuf = calloc(PPB_SIZE, 1);

    setDeploymentMode(TRUE);
    lastAccSec = accurateClkSec;	/* Init sec counter		*/
    lastAccTick = ioGetTick();

    return(watchStruct.ppbBuf == NULL ? ERROR : OK);
}


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


/************************************************************************/
/* 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 = clkGetAccurateTime();
}


/************************************************************************/
/* Function    : recordWatchDataPoint					*/
/* Purpose     : Record one data point during watch cycle		*/
/* Inputs      : None							*/
/* Outputs     : INV_SUCCESS (for MPU stack)				*/
/************************************************************************/
MLocal inv_error_t recordWatchDataPoint(Void)
{
    InertialRcd	idata;
    Nat16	i;
    Byte	*p;
    Nat32	accel[3];
    Nat32	quat[4];
    Int32	val, absVal;

    if ((watchStruct.ppb != NULL) &&
	(inv_get_linear_accel_in_world(accel) == INV_SUCCESS) &&
	(inv_get_quaternion(quat) == INV_SUCCESS))
    {
	idata.recType = InertialDataType;
	p = idata.accel;
	for (i = 0; i < 3; i++, p+=3)
	{
	    val = accel[i];
	    LoopModeMemCopyBytes(p, (Byte *)(&val) + 1, 3);
	    absVal = (val < 0) ? -val : val;
	    if (absVal > watchStruct.maxVal)
		watchStruct.maxVal = absVal;
	}
	p = idata.quat;
	for (i = 0; i < 4; i++, p+=QUAT_SIZE)
	    LoopModeMemCopyBytes(p, (Byte *)(&quat[i]), QUAT_SIZE);
    }

    PPBWrite(watchStruct.ppb, &idata, sizeof(idata));
    return(INV_SUCCESS);

} /* recordWatchDataPoint() */


/************************************************************************/
/* Function    : setupWatchCycle					*/
/* Purpose     : Set up to start a watch cycle				*/
/* Inputs      : None							*/
/* Outputs     : None							*/
/************************************************************************/
MLocal Void setupWatchCycle(Void)
{
    CIOiflush();
    TIOiflush();
    sysLog("Entering Watch Cycle");

    setDMP(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();

    inv_register_fifo_rate_process(recordWatchDataPoint, INV_PRIORITY_WATCH);
}


/************************************************************************/
/* Function    : stopWatchCycle						*/
/* Purpose     : Tear down a watch cycle				*/
/* Inputs      : None							*/
/* Outputs     : None							*/
/************************************************************************/
MLocal Void stopWatchCycle(Void)
{
    inv_unregister_fifo_rate_process(recordWatchDataPoint);
    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 ((lastAccSec == accurateClkSec) &&
	((ioGetTick() - lastAccTick) < 2*TICKS_PER_SECOND))
	return(FALSE);

    /* Start of a new second	*/
    
    clkCalcTimeOffset();	/* Read accurate RTC and calc offset*/
				/* Fills in accurateTod		*/
    enableClkInt();		/* Re-enable clock interrupts	*/
    lastAccSec = accurateClkSec;
    lastAccTick = ioGetTick();
#ifdef WATCHDOG
    TickleSWSR();		/* Keep watchdog alive		*/
#endif
    return(TRUE);

} /* newSecond() */


/************************************************************************/
/* Function    : watchCycle						*/
/* Purpose     : Run the watch cycle for the BEDS application		*/
/* Inputs      : None							*/
/* Outputs     : None							*/
/************************************************************************/
Void watchCycle(Void)
{
    modemState = OffState;		/* Start with modem off		*/
    modemOff();			

    // MPU6000 powers on in motion state
    // Go directly to event state to avoid extra watch files and
    //  power-cycling modem.
    // Deleted 26sept2013, rah, when adding deployment mode
    //    if (inv_get_motion_state() == INV_MOTION)
    //		eventCycle();

    setupWatchCycle();

    while(TRUE)
    {
	/* Check for new second	*/
	if (newSecond())
	{
	    if (deploymentMode && (accurateTod >= (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		*/
		disableClkInt();	/* Can't maintain clk diff during UI*/
		eventTmout = FALSE;	/* Running UI cycle, so clear event tmout*/
#ifndef MODEM_ON_TPU			//Normal case
		setConsole(SER_CONSOLE, TRUE);
		setConsoleBaud(modemBaud);
		runUI(MODEM_PRIV);
#else					//Modem on TPU port
		setConsole(SER_TPU, TRUE);
		setTpuBaud(modemBaud);
		runUI(MODEM_PRIV);
		setConsole(SER_CONSOLE, FALSE);
#endif
		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*/
		enableClkInt();		/* Start up clk ints again	*/
	    }
	}
	else if (modemState == OnState)	/* If checking for modem char,	*/
	    RTCDelayMicroSeconds(10000); /* don't sleep but just delay	*/
	else				 /* Else sleep to allow FIFO to fill*/
	    ioSleep(loopDelay(watchFreq));


	if (auxPortInCheck())
	{				/* If key hit on aux port,	*/
	    stopWatchCycle();		/* stop cycle, and run UI	*/
	    modemState = OffState;
	    modemOff();
	    disableClkInt();		/* Can't maintain clk diff during UI*/
#ifndef MODEM_ON_TPU			//Normal case
	    setConsole(SER_TPU, FALSE);
	    setTpuBaud(debugBaud);
	    runUI(DBG_PRIV);
	    setConsole(SER_CONSOLE, TRUE);
#else					//Modem on TPU port, debug on console
	    setConsole(SER_CONSOLE, FALSE);
	    setConsoleBaud(debugBaud);
	    runUI(DBG_PRIV);
#endif
	    setupWatchCycle();		/* When done, run next watch cycle*/
	    enableClkInt();		/* Start up clk ints again	*/
	}

        /* MPU-6000 Interrupt
         * Taken from MPU sample app, we check the interrupt in the main loop.
         */
        if (ioCheckMPUInt()) {
            umplNotifyInterrupt(INTSRC_MPU);
        }

        /* umplOnTimer:
         * This should be run either at a fixed rate corresponding to
         * the FIFO Rate (see umpl-setup / inv_set_fifo_rate) or each time
         * an MPU interrupt has occurred.
         * It is a time-consuming call, so it is advised not to run it from
         * an ISR.
         */
#ifdef DEBUG_UMPL
	PIOSet(UMPL_ON_PIN);
        umplOnTimer();
	PIOClear(UMPL_ON_PIN);
#else
        umplOnTimer();
#endif
	if ((inv_get_motion_state() == INV_MOTION) && !eventTmout && !deploymentMode)
	{
	    stopWatchCycle();
	    modemState = OffState;
	    modemOff();
	    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)
{
    PIOSet(SENSOR_PWR_EN);
    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)
	PIOClear(SENSOR_PWR_EN);
}


/************************************************************************/
/* 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      : Ping-pong buf to write record into			*/
/* Outputs     : OK or ERROR						*/
/************************************************************************/
Errno writeSysRec(Void *ppb)
{
    SystemRcd	sr;	

    if (ppb == 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);

    PPBWrite(ppb, &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 = clkGetAccurateTime();		/* Get and remember current time */
    writeSecondMarker(watchStruct.ppb, tod);

    if ((tod - lastClockDrift) >= clkDriftPeriod)
    {
	writeClkDrift();
	lastClockDrift = tod;
    }

    checkDataFile(&watchStruct);	/* See if PPB needs write to disk*/

    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.ppb);
	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;
	modemiFlush();			/* 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*/
	}
    }

    // Makes for easier debugging in the lab
//    if (kbhit())
//	return(TRUE);

    return(FALSE);

} /* watchSecond() */