/****************************************************************************/
/* Copyright 2003-2012 MBARI						    */
/****************************************************************************/
/* Summary  : Event Cycle Routines for BEDS on Persistor CF2		    */
/* Filename : event.c                                                       */
/* Author   : Robert Herlien (rah)					    */
/* Project  : BEDS (Benthic Event Detection System)			    */	
/* Revision : 1.0							    */
/* Created  : 11/28/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:						    */
/* 28nov2012 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 <event.h>		/* BEDS event cycle			*/
#include <watch.h>		/* BEDS watch cycle			*/
#include <io.h>			/* BEDS I/O definitions			*/
#include <file.h>		/* BEDS file I/O definitions		*/
#include <variable.h>		/* Variable function definitions        */
#include <dmpSetup.h>		/* BEDS DMP setup routines		*/
#include <clock.h>		/* BEDS Clock module			*/
#include <syslog.h>		/* System logger			*/
#include <adc.h>		/* BEDS Analog Module			*/

#include <stddef.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.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"

typedef struct				/********************************/
{					/* Struct to accumulate pressure*/
    Nat32	accum;			/* accumulator			*/
    Nat16	nSamples;		/* number of samples accumulated*/
    Nat16	pressTime;		/* Time in seconds		*/
    MBool	warmedUp;		/* TRUE if sensor warmed up	*/
} PressureStruct;			/********************************/


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

Extern Int32	evtFreq;		/* FIFO rate during event	*/
Extern Int32	maxEvent;		/* Maximum time for event (secs)    */
Extern Int32	sensorWarmup;		/* Warmup time for turning on system sensors*/
Extern Int32	evtPressureSecs;	/* Period (secs) to get press in event*/


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

MLocal DataFileStruct eventStruct = 
    {"EVT", "EVENT.IDX", 1, 3000, NULL, NULL, {0}};


/************************************************************************/
/* Function    : eventInit						*/
/* Purpose     : Initialize this module					*/
/* Inputs      : None							*/
/* Outputs     : OK or ERROR						*/
/************************************************************************/
Errno eventInit(Void)
{
    eventStruct.fileIndex = 1;
    eventStruct.ppbBuf = calloc(PPB_SIZE, 1);
    return(eventStruct.ppbBuf == NULL ? ERROR : OK);
}


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

    if ((eventStruct.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 > eventStruct.maxVal)
		eventStruct.maxVal = absVal;
	}
	p = idata.quat;
	for (i = 0; i < 4; i++, p+=QUAT_SIZE)
	    LoopModeMemCopyBytes(p, (Byte *)(&quat[i]), QUAT_SIZE);
    }

    PPBWrite(eventStruct.ppb, &idata, sizeof(idata));

    return(INV_SUCCESS);

} /* recordEventDataPoint() */


/************************************************************************/
/* Function    : setupEventCycle					*/
/* Purpose     : Set up to start an event cycle				*/
/* Inputs      : None							*/
/* Outputs     : None							*/
/************************************************************************/
MLocal Void setupEventCycle(Void)
{
    sysLog("Entering Event Cycle");

    setDMP(evtFreq);
    eventStruct.dataRate = 60*evtFreq;
    eventStruct.maxVal = 0;
    openNewDataFile(&eventStruct);

    /* 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(recordEventDataPoint, INV_PRIORITY_EVENT);
}


/************************************************************************/
/* Function    : stopEventCycle						*/
/* Purpose     : Tear down an event cycle				*/
/* Inputs      : None							*/
/* Outputs     : None							*/
/************************************************************************/
MLocal Void stopEventCycle(Void)
{
    inv_unregister_fifo_rate_process(recordEventDataPoint);
    closeDataFile(&eventStruct);
    sysLog("Exiting Event Cycle");
}


/************************************************************************/
/* Function    : writePressure						*/
/* Purpose     : Write a pressure record to event stream		*/
/* Inputs      : None							*/
/* Outputs     : None							*/
/************************************************************************/
MLocal Void writePressure(Void)
{
    PressureRcd	rcd;

    if (eventStruct.ppb == NULL)
	return;

    rcd.recType = PressureType;
    rcd.rsvd = 0;
    rcd.pressure = adcSampleAvg(EXT_PRESS_CHAN, 10);
    PPBWrite(eventStruct.ppb, &rcd, sizeof(rcd));
}


/************************************************************************/
/* Function    : eventCycle						*/
/* Purpose     : Run the event cycle for the BEDS application		*/
/* Inputs      : None							*/
/* Outputs     : OK, or TMOUT if exceeded maxEvent			*/
/************************************************************************/
Errno	eventCycle(Void)
{
    time_t	tod, startTime;
    Nat16	pressTime=0;

    sensorsOn(SENSORS_SAMPLING);
    setupEventCycle();
    startTime = clkGetAccurateTime();

    while (inv_get_motion_state() == INV_MOTION)
    {
	if (newSecond())
	{
            checkVars();                /* Make sure variables are within limits */

	    tod = clkGetAccurateTime();
	    writeSecondMarker(eventStruct.ppb, tod);
	    if ((tod - startTime) >= sensorWarmup)
		if (++pressTime >= evtPressureSecs)
		{
		    writePressure();
		    pressTime = 0;
		}

	    checkDataFile(&eventStruct); /* See if it's time to write data*/

	    if ((tod - startTime) > maxEvent)
	    {					/* Exceeded max event time*/
		sysLog("Exceeded max event time");
		sensorsOff(SENSORS_SAMPLING);
		stopEventCycle();		/* Stop event		*/
		return(TMOUT);
	    }
	}
	else
	    ioSleep(loopDelay(evtFreq));

        /* MPU-6000 Interrupt
         * In this sample app, we check the interrupt in the main loop.
         * However, you can configure your microcontroller to trigger
         * this interrupt asynchronously.
         */
        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
    }

    sensorsOff(SENSORS_SAMPLING);
    stopEventCycle();
    return(OK);
}