/****************************************************************************/
/* Copyright 2003-2012 MBARI						    */
/****************************************************************************/
/* Summary  : Variation on Watch Cycle to send teapot packets		    */
/* 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 <dmpSetup.h>		/* BEDS DMP setup routines		*/
#include <watch.h>		/* BEDS watch 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 <bedsUI.h>		/* BEDS Command line processor		*/
#include <syslog.h>		/* System logger			*/
#include <packet.h>

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


inv_error_t	angleAndAccelSend(void);
int		sendPacket(unsigned char type, unsigned char * payload);
TUPort	*tuport;
MBool	gotTPUchar = FALSE;
uint	packetCount = 0;


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

Extern Int32	watchFreq;		/* FIFO rate during watch cycle	*/
Extern Int32	modemBaud;		/* Baud rate on SCI port	*/
Extern volatile Nat32 accurateClkSec;	/* Num clk ints from DS3234	*/


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

MLocal Nat32 lastAccSec = 0L;
MLocal Nat32 lastAccTick = 0L;

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

/* Prototypes for dummy commands put here to satisfy linker	*/
Errno eventInit(void);
Errno watchInit(void);
Void modemInit(Void);
Void checkModemBatt(Void);
char *DecodeCmd(CmdInfoPtr cip);
char *ModemCmd(CmdInfoPtr cip);


/************************************************************************/
/* Function    : watchInit						*/
/* Purpose     : Placed here to satisfy linker for BEDS Teapot app	*/
/* Inputs      : None							*/
/* Outputs     : OK or ERROR						*/
/************************************************************************/
Errno watchInit(void)
{
    return(OK);
}


/************************************************************************/
/* Function    : eventInit						*/
/* Purpose     : Placed here to satisfy linker for BEDS Teapot app	*/
/* Inputs      : None							*/
/* Outputs     : OK or ERROR						*/
/************************************************************************/
Errno eventInit(void)
{
    return(OK);
}


/************************************************************************/
/* Function    : modemInit						*/
/* Purpose     : Placed here to satisfy linker for BEDS Teapot app	*/
/* Inputs      : None							*/
/* Outputs     : None							*/
/************************************************************************/
Void modemInit(Void)
{
}


/************************************************************************/
/* Function    : checkModemBatt						*/
/* Purpose     : Placed here to satisfy linker for BEDS Teapot app	*/
/* Inputs      : None							*/
/* Outputs     : None							*/
/************************************************************************/
Void checkModemBatt(Void)
{
}


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

    setDMP(watchFreq);
    inv_register_fifo_rate_process(angleAndAccelSend, INV_PRIORITY_WATCH);
}


/************************************************************************/
/* Function    : stopWatchCycle						*/
/* Purpose     : Tear down a watch cycle				*/
/* Inputs      : None							*/
/* Outputs     : None							*/
/************************************************************************/
MLocal Void stopWatchCycle(Void)
{
    inv_unregister_fifo_rate_process(angleAndAccelSend);
    sysLog("Exiting Watch Cycle");
}


/************************************************************************/
/* Function    : watchCycle						*/
/* Purpose     : Run the watch cycle for the BEDS application		*/
/* Inputs      : None							*/
/* Outputs     : None							*/
/************************************************************************/
Void	watchCycle(Void)
{
    tuport = setTpuBaud(115200L);		//Set Teapot baud
    PIOSet(AUX_TX_OFF);				//Enable aux Tx
    setupWatchCycle();

    while(TRUE)
    {
	ioSleep(loopDelay(watchFreq));

	if ((lastAccSec != accurateClkSec) ||	/* New second?		*/
	    ((ioGetTick() - lastAccTick) > 2*TICKS_PER_SECOND))
	{
	    clkCalcTimeOffset();	/* Read accurate RTC and calc offset*/
	    enableClkInt();		/* Re-enable clock interrupts	*/
	    lastAccSec = accurateClkSec;
	    lastAccTick = ioGetTick();
	}

	if (kbhit())
	{
	    stopWatchCycle();
	    setConsoleBaud(modemBaud);
	    setConsole(SER_CONSOLE, FALSE);
	    runUI(DBG_PRIV);
	    setupWatchCycle();
	}
	
        /* 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.
         */
	PIOSet(UMPL_ON_PIN);
        umplOnTimer();
	PIOClear(UMPL_ON_PIN);
    }

} /* watchCycle() */


#define PKT_SIZE	42		/* We extended the protocol to 42 bytes total*/
#define SND_TMOUT	((short)10)	/* 10 ms timeout on send		*/
#define RCV_TMOUT	((short)100)	/* 100 ms timeout on receive		*/

/* --------------- */
/* -  Functions. - */
/* --------------- */

/**
 *  @brief sendPacket sends a 42-byte packet in a specific format on the udi_cdc port.
 *		   It Checks for timeout on a per-byte basis. Out of the 42 bytes, 38 bytes are 
 *		   for payload and the rest are for control.
 *		   The format of each packet is as follows:
 *			<center>packet[0] = $</center>
 *			<center>packet[1] = DATA_TYPE</center>
 *			<center>packet[2] to packet[39] = DATA</center>
 *			<center>packet[40] = '\\r'</center>
 *			<center>packet[41] = '\\n'</center>
 *
 *
 *  @return 0 if successful; non-zero if timed out
 */
int sendPacket(unsigned char type, unsigned char * payload)
{
    short	ii;
    unsigned char out[PKT_SIZE];

    out[0] = '$';
    out[1] = type;
    for (ii = 2; ii < PKT_SIZE-2; ii++)
        out[ii] = payload[ii-2];
    out[PKT_SIZE-2] = '\r';
    out[PKT_SIZE-1] = '\n';

    return((TUTxPutBlock(tuport, out, (long)PKT_SIZE, SND_TMOUT) >= PKT_SIZE) ? SUCCESS : ERROR);
}


inv_error_t angleAndAccelSend(void)
{
    inv_error_t result = INV_ERROR;
//    long longeuler[3];
    long longaccelw[3];
//    long longaccel[3];
    long longquat[4];
    unsigned char out[38];

    if ((umplGetState() != UMPL_RUN) && (umplGetState() != UMPL_ACCEL_ONLY)
	&& (umplGetState() != UMPL_LPACCEL_ONLY))
	return(INV_SUCCESS);

    memset(out, 0, 38);
    packetCount++;
    out[0] = (unsigned char)((packetCount >> 8) & 0xff);
    out[1] = (unsigned char)(packetCount & 0xff);
    out[2] = (unsigned char)(inv_get_motion_state() & 0xff);

#ifdef DEBUG_PKT_SEND
    PIOSet(PKT_SEND_PIN);
#endif
#if 0	//Removed because it takes too long, causing slow updates,
	// and we don't need it for Teapot
    /* longeuler: send a fixed point representation of the Euler angles
     * One degree = 2^16.  We send the 3 MSBs of each angle, which still
     * yields a precision of .004 degrees.
     */
    if (inv_get_euler_angles(longeuler) == INV_SUCCESS)
    {
	result = INV_SUCCESS;			/* If anything works, send packet */

        // Send the least significant three bytes of each angle
        out[3] = (unsigned char) ((uint16_t)(longeuler[0] >> 24) & 0xff);
        out[4] = (unsigned char) ((uint16_t)(longeuler[0] >> 16) & 0xff);
	out[5] = (unsigned char) ((uint16_t)(longeuler[0] >> 8) & 0xff);

        out[6] = (unsigned char) ((uint16_t)(longeuler[1] >> 24) & 0xff);
	out[7] = (unsigned char) ((uint16_t)(longeuler[1] >> 16) & 0xff);
	out[8] = (unsigned char) ((uint16_t)(longeuler[1] >> 8) & 0xff);

        out[9] = (unsigned char) ((uint16_t)(longeuler[2] >> 24) & 0xff);
        out[10] = (unsigned char) ((uint16_t)(longeuler[2] >> 16) & 0xff);
        out[11] = (unsigned char) ((uint16_t)(longeuler[2] >> 8) & 0xff);
    }

    /* longaccel: send a fixed point representation of the accelerations X,Y,Z
     * in world coordinates with gravity removed.  One g = 2^16.  By sending
     * the LSB 3 bytes for each coord, we can retain full precision while
     * representing up to 256 g, which is more than the chip can sense.
     */
    if (inv_get_linear_accel(longaccel) == INV_SUCCESS)
    {
	result = INV_SUCCESS;			/* If anything works, send packet */

        // Send the most significant three bytes of each acceleration component.
        out[12] = (unsigned char) ((uint16_t)(longaccel[0] >> 16) & 0xff);
        out[13] = (unsigned char) ((uint16_t)(longaccel[0] >> 8) & 0xff);
        out[14] = (unsigned char) ((uint16_t)(longaccel[0] & 0xff));

        out[15] = (unsigned char) ((uint16_t)(longaccel[1] >> 16) & 0xff);
        out[16] = (unsigned char) ((uint16_t)(longaccel[1] >> 8) & 0xff);
        out[17] = (unsigned char) ((uint16_t)(longaccel[1] & 0xff));

        out[18] = (unsigned char) ((uint16_t)(longaccel[2] >> 16) & 0xff);
        out[19] = (unsigned char) ((uint16_t)(longaccel[2] >> 8) & 0xff);
        out[20] = (unsigned char) ((uint16_t)(longaccel[2] & 0xff));
    }
#endif

    if (inv_get_linear_accel_in_world(longaccelw) == INV_SUCCESS)
    {
	result = INV_SUCCESS;			/* If anything works, send packet */

        // Send the most significant three bytes of each acceleration component.
        out[21] = (unsigned char) ((uint16_t)(longaccelw[0] >> 16) & 0xff);
        out[22] = (unsigned char) ((uint16_t)(longaccelw[0] >> 8) & 0xff);
        out[23] = (unsigned char) ((uint16_t)(longaccelw[0] & 0xff));

        out[24] = (unsigned char) ((uint16_t)(longaccelw[1] >> 16) & 0xff);
        out[25] = (unsigned char) ((uint16_t)(longaccelw[1] >> 8) & 0xff);
        out[26] = (unsigned char) ((uint16_t)(longaccelw[1] & 0xff));

        out[27] = (unsigned char) ((uint16_t)(longaccelw[2] >> 16) & 0xff);
        out[28] = (unsigned char) ((uint16_t)(longaccelw[2] >> 8) & 0xff);
        out[29] = (unsigned char) ((uint16_t)(longaccelw[2] & 0xff));
    }

    /* longquat: send a fixed point representation of the sensor fusion output.
     * The quaternion behaves well when less significant bits are truncated.
     * We get the 32 bit element representation with inv_get_quaternion,
     * and send the most significant 16 bits of each element in the packet.
     */
    if (inv_get_quaternion(longquat) == INV_SUCCESS)
    {
	result = INV_SUCCESS;			/* If anything works, send packet */

        // Send the most significant two bytes of each quaternion component.
        out[30] = (unsigned char) (longquat[0] >> 24);
        out[31] = (unsigned char) ((uint16_t)(longquat[0] >> 16) % 256);
        out[32] = (unsigned char) (longquat[1] >> 24);
        out[33] = (unsigned char) ((uint16_t)(longquat[1] >> 16) % 256);
        out[34] = (unsigned char) (longquat[2] >> 24);
        out[35] = (unsigned char) ((uint16_t)(longquat[2] >> 16) % 256);
        out[36] = (unsigned char) (longquat[3] >> 24);
        out[37] = (unsigned char) ((uint16_t)(longquat[3] >> 16) % 256);
    }

#ifdef DEBUG_PKT_SEND
    PIOClear(PKT_SEND_PIN);
#endif
    sendPacket(PACKET_TYPE_ANGLE_ACCEL, out);
    return(result);
}

/************************************************************************/
/* 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    : DecodeCmd						*/
/* Purpose     : Placed here to satisfy linker for BEDS Teapot app	*/
/* Inputs      : CmdInfoPtr						*/
/* Outputs     : NULL if OK, else error message				*/
/************************************************************************/
char *DecodeCmd(CmdInfoPtr cip)
{
    return(NULL);
}

/************************************************************************/
/* Function    : ModemCmd						*/
/* Purpose     : Placed here to satisfy linker for BEDS Teapot app	*/
/* Inputs      : CmdInfoPtr						*/
/* Outputs     : NULL if OK, else error message				*/
/************************************************************************/
char *ModemCmd(CmdInfoPtr cip)
{
    return(NULL);
}