/*******************************************************************************
 * Portions Copyright (c) 2008 InvenSense Corporation, All Rights Reserved.
 * Modified by Bob Herlien, 8 Feb 2012
 *******************************************************************************/

#include <windows.h>
#include <stdio.h>
#include <stdlib.h>
#include "math.h"
#include "conio.h"
#include "main.h"
#include "mlMouse.h"
#include "sio.h"
#include "packet.h"
#include "log.h"

#define VERSION "MBARIv1.0"

/* ------------ */
/* - Globals. - */
/* ------------ */

tQuatPacket	qData;
AngleAccelPacket aaData;

/* -------------- */
/* - Functions. - */
/* -------------- */
int getSerialData(tQuatPacket *qData);


/**********************************************************************/
/*                                                                    */
/* Function Name: getSerialData                                       */
/*                                                                    */
/*   Description: Parse rx data buffer to get angles and accelerations*/
/*                                                                    */
/*   Returns: Return code.                                            */
/*                                                                    */
/**********************************************************************/
int getSerialData(tQuatPacket *qData)
{
    long  lval;
    int i, retCode = 1;
    unsigned int len;
    unsigned char rxBuffer[SIO_RBUFF_SIZE];

    retCode = SIOReadBuffer((unsigned char) 1, rxBuffer);

    if (rxBuffer[0] != '$')
	return retCode;

    len = (unsigned int)rxBuffer[1];

#ifdef DEBUG
    for(int i=0; i<len; i++)
        printf("0x%02X ", (unsigned char)rxBuffer[i]);
    printf("\n");
#endif

    switch(rxBuffer[2])
    {
      case PACKET_TYPE_QUATERNION:
	  //Parse MPU Data
	  qData->mlQuaternion[0] = (long) ((((unsigned long) rxBuffer[2]) << 8)
					   + ((unsigned long) rxBuffer[3]));

	  qData->mlQuaternion[1] = (long) ((((unsigned long) rxBuffer[4]) << 8)
					   + ((unsigned long) rxBuffer[5]));

	  qData->mlQuaternion[2] = (long) ((((unsigned long) rxBuffer[6]) << 8)
					   + ((unsigned long) rxBuffer[7]));

	  qData->mlQuaternion[3] = (long) ((((unsigned long) rxBuffer[8]) << 8)
					   + ((unsigned long) rxBuffer[9]));
	  for( i = 0; i < 4; i++ )
	  {
	      if( qData->mlQuaternion[i] > 32767 )
		  qData->mlQuaternion[i] -= 65536;
	      qData->quat[i] = ((float) qData->mlQuaternion[i]) / 16384.0f;
	  }

	  qData->button = rxBuffer[10];
	  qData->packetCnt = rxBuffer[11];

	  logQuat(qData);

	  if( qData->mlQuaternion[1] != 0 )
	  {
	      IMUgetQuaternion(qData);
	      logEulerFromQuat(qData);
	  }
	  break;

      case PACKET_TYPE_ACCEL:
	  lval = (long)((((unsigned long)rxBuffer[2]) << 16)
			+ (((unsigned long)rxBuffer[3]) << 8)
			+ ((unsigned long)rxBuffer[4]));
	  if (lval & 0x800000)
	      lval |= 0xff000000;
	  qData->rcvdAccel[0] = (float)lval / 65536.0f;

	  lval = (long)((((unsigned long)rxBuffer[5]) << 16)
			+ (((unsigned long)rxBuffer[6]) << 8)
			+ ((unsigned long)rxBuffer[7]));
	  if (lval & 0x800000)
	      lval |= 0xff000000;
	  qData->rcvdAccel[1] = (float)lval / 65536.0f;

	  lval = (long)((((unsigned long)rxBuffer[8]) << 16)
			+ (((unsigned long)rxBuffer[9]) << 8)
			+ ((unsigned long)rxBuffer[10]));
	  if (lval & 0x800000)
	      lval |= 0xff000000;
	  qData->rcvdAccel[2] = (float)lval / 65536.0f;

	  qData->packetCnt = rxBuffer[11];
	  logAccel(qData);
	  break;

      case PACKET_TYPE_ACCEL_LOCAL:
	  lval = (long)((((unsigned long)rxBuffer[2]) << 16)
			+ (((unsigned long)rxBuffer[3]) << 8)
			+ ((unsigned long)rxBuffer[4]));
	  if (lval & 0x800000)
	      lval |= 0xff000000;
	  qData->rcvdAccelLocal[0] = (float)lval / 65536.0f;

	  lval = (long)((((unsigned long)rxBuffer[5]) << 16)
			+ (((unsigned long)rxBuffer[6]) << 8)
			+ ((unsigned long)rxBuffer[7]));
	  if (lval & 0x800000)
	      lval |= 0xff000000;
	  qData->rcvdAccelLocal[1] = (float)lval / 65536.0f;

	  lval = (long)((((unsigned long)rxBuffer[8]) << 16)
			+ (((unsigned long)rxBuffer[9]) << 8)
			+ ((unsigned long)rxBuffer[10]));
	  if (lval & 0x800000)
	      lval |= 0xff000000;
	  qData->rcvdAccelLocal[2] = (float)lval / 65536.0f;

	  qData->packetCnt = rxBuffer[11];
	  logAccelLocal(qData);
	  break;

      case PACKET_TYPE_EULER:
	  lval = (long)((((unsigned long)rxBuffer[2]) << 24)
			+ (((unsigned long)rxBuffer[3]) << 16)
			+ (((unsigned long)rxBuffer[4]) << 8));
	  qData->rcvdEuler[0] = (float)lval / 65536.0f;

	  lval = (long)((((unsigned long)rxBuffer[5]) << 24)
			+ (((unsigned long)rxBuffer[6]) << 16)
			+ (((unsigned long)rxBuffer[7]) << 8));
	  qData->rcvdEuler[1] = (float)lval / 65536.0f;

	  lval = (long)((((unsigned long)rxBuffer[8]) << 24)
			+ (((unsigned long)rxBuffer[9]) << 16)
			+ (((unsigned long)rxBuffer[10]) << 8));
	  qData->rcvdEuler[2] = (float)lval / 65536.0f;

	  qData->packetCnt = rxBuffer[11];
	  logEuler(qData);
	  break;

      case PACKET_TYPE_ANGLE_ACCEL:
	  //Parse MPU Data
	  if (len < 40)
	      printf("Error - AngleAccel packet len %u, should be 42\n", len);
	  
	  aaData.pktCount = (unsigned short)(rxBuffer[3] << 8)
			     + (unsigned short)(rxBuffer[4]);
	  
	  lval = (long)((((unsigned long)rxBuffer[5]) << 24)
			+ (((unsigned long)rxBuffer[6]) << 16)
			+ (((unsigned long)rxBuffer[7]) << 8));
	  aaData.rcvdEuler[0] = (float)lval / 65536.0f;

	  lval = (long)((((unsigned long)rxBuffer[8]) << 24)
			+ (((unsigned long)rxBuffer[9]) << 16)
			+ (((unsigned long)rxBuffer[10]) << 8));
	  aaData.rcvdEuler[1] = (float)lval / 65536.0f;

	  lval = (long)((((unsigned long)rxBuffer[11]) << 24)
			+ (((unsigned long)rxBuffer[12]) << 16)
			+ (((unsigned long)rxBuffer[13]) << 8));
	  aaData.rcvdEuler[2] = (float)lval / 65536.0f;


	  lval = (long)((((unsigned long)rxBuffer[14]) << 16)
			+ (((unsigned long)rxBuffer[15]) << 8)
			+ ((unsigned long)rxBuffer[16]));
	  if (lval & 0x800000)
	      lval |= 0xff000000;
	  aaData.rcvdAccel[0] = (float)lval / 65536.0f;

	  lval = (long)((((unsigned long)rxBuffer[17]) << 16)
			+ (((unsigned long)rxBuffer[18]) << 8)
			+ ((unsigned long)rxBuffer[19]));
	  if (lval & 0x800000)
	      lval |= 0xff000000;
	  aaData.rcvdAccel[1] = (float)lval / 65536.0f;

	  lval = (long)((((unsigned long)rxBuffer[20]) << 16)
			+ (((unsigned long)rxBuffer[21]) << 8)
			+ ((unsigned long)rxBuffer[22]));
	  if (lval & 0x800000)
	      lval |= 0xff000000;
	  aaData.rcvdAccel[2] = (float)lval / 65536.0f;


	  lval = (long)((((unsigned long)rxBuffer[23]) << 16)
			+ (((unsigned long)rxBuffer[24]) << 8)
			+ ((unsigned long)rxBuffer[25]));
	  if (lval & 0x800000)
	      lval |= 0xff000000;
	  aaData.rcvdAccelWorld[0] = (float)lval / 65536.0f;

	  lval = (long)((((unsigned long)rxBuffer[26]) << 16)
			+ (((unsigned long)rxBuffer[27]) << 8)
			+ ((unsigned long)rxBuffer[28]));
	  if (lval & 0x800000)
	      lval |= 0xff000000;
	  aaData.rcvdAccelWorld[1] = (float)lval / 65536.0f;

	  lval = (long)((((unsigned long)rxBuffer[29]) << 16)
			+ (((unsigned long)rxBuffer[30]) << 8)
			+ ((unsigned long)rxBuffer[31]));
	  if (lval & 0x800000)
	      lval |= 0xff000000;
	  aaData.rcvdAccelWorld[2] = (float)lval / 65536.0f;


	  aaData.longQuat[0] = (long) ((((unsigned long) rxBuffer[32]) << 8)
					   + ((unsigned long) rxBuffer[33]));

	  aaData.longQuat[1] = (long) ((((unsigned long) rxBuffer[34]) << 8)
					   + ((unsigned long) rxBuffer[35]));

	  aaData.longQuat[2] = (long) ((((unsigned long) rxBuffer[36]) << 8)
					   + ((unsigned long) rxBuffer[37]));

	  aaData.longQuat[3] = (long) ((((unsigned long) rxBuffer[38]) << 8)
					   + ((unsigned long) rxBuffer[39]));
	  for( i = 0; i < 4; i++ )
	  {
	      if( aaData.longQuat[i] > 32767 )
		  aaData.longQuat[i] -= 65536;
	      aaData.rcvdQuat[i] = ((float) aaData.longQuat[i]) / 16384.0f;
	  }

	  logAnglesAndAccel(&aaData);

	  break;

      default:
	  logMisc(rxBuffer, 14);
	  break;
    }

    return retCode;
}


/**********************************************************************/
/*                                                                    */
/* Function Name: main                                                */
/*                                                                    */
/*   Description:                                                     */
/*                                                                    */
/*    Parameters:                                                     */
/*                                                                    */
/*       Returns:                                                     */
/*                                                                    */
/**********************************************************************/
int main(int argc, char *argv[])
{
    int i;
    unsigned short pktsInBuffer;

    printf("MBARI-InvenSense Data Log App %s %s %s\r\n", VERSION, __DATE__, __TIME__);

    if (argc == 2)
    {
        SIOSetCom(atoi(argv[1]),(MLU32)115200);
    }
    else
    {
        printf("Please assign the com port number!!\r\n");
        while(!_kbhit())
        {
	    Sleep(5000);
        }//while
    }

    if (SIOOpen((unsigned char)AUTO_PROCESS_BUFFER)==ML_ERROR)
    {
	printf("ERROR opening serial port.  Exiting\r\n");
	exit(1);
    }

    if (logOpen() != ML_SUCCESS)
    {
	printf("Error opening log file(s).  Exiting.\r\n");
	exit(1);
    }

    IMUquaternionInit(&qData);

    while(TRUE)
    {
        SIOHandler();

	//Process data in buffer
	SIOPktsInBuffer(&pktsInBuffer);

	for( i = 0; i < pktsInBuffer; i++ )
	    getSerialData(&qData);

	Sleep(2);

	if (_kbhit())
	    if (getchar() == 3)
	    {
		logClose();
		exit(0);
	    }
	 }

    return(ML_SUCCESS);

}   // end of main()