/****************************************************************************/
/* Copyright 2010 MBARI                                                     */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>

#include <time.h>

#include "mbariPic.h"
#include "serial.h"
#include "sys_defs.h"

#define UCONSOLE 1
#define UCOMPASS 2

#ifndef TRUE
	#define TRUE 1
#endif

#ifndef FALSE
	#define FALSE 0
#endif


//#define DEBUG 0



/* JTAG/Code Protect/Write Protect/Clip-on Emulation mode
Watchdog Timer/ICD pins select */
_CONFIG1(JTAGEN_OFF & GCP_OFF & GWRP_OFF & COE_OFF & FWDTEN_OFF & ICS_PGx2) 
/* Enable CLK switch, Disable CLK monitor, OSCO or Fosc/2, 
Primary Oscillator Mode: Disabled, Internal Fast RC oscillator */
_CONFIG2(FCKSM_CSDCMD & OSCIOFNC_OFF & POSCMOD_XT & FNOSC_PRI)
/* Disable CLK switch and CLK monitor, OSCO or Fosc/2, HS oscillator,
Primary oscillator */

#define MAX_CHARS   80  /* maximum command line entry length */
#define CLOCKSPERSEC 3650000
#define SECSPERCLOCK 1/CLOCKSPERSEC

int getCommand(int,char*, int);
int compute_nmea_checksum(char*);
int checkCompassChecksum(char*);
float parseCompassHeading(char*);
int elapisodd(double);



int main(void)
{
	AD1PCFG = 0xffff;
	//Setup UARTs
	RPINR18bits.U1RXR = 10;  /* Make Pin RP10 U1RX */
	RPOR5bits.RP11R = 3;     /* Make Pin RP11 U1TX */

	RPINR19bits.U2RXR = 12;  /* Make Pin RP3 U2RX */
 	RPOR6bits.RP13R = 5;     /* Make Pin RP2 U2TX */

	serInit();	
	initBoard();



	serPutString(UCONSOLE,"\n\rCompass Counter V0.1\n\r");
	serPutString(UCONSOLE,"MBARI 2010\n\r");
	ledOn(1);ledOn(2);
	char cmd[MAX_CHARS];
	char concmd[MAX_CHARS];
	
	int checksum, inmenu;
	short goodcompass 	= FALSE;
	short pauseoutput 	= FALSE;
	double turnshead	= 0.0;
	double turns 		= 0.0;
	char out[MAX_CHARS];
	double heading 		= -1.0;
	double lasthead 	= -1.0;
	double delta 		= 0.0;
	int firstloop 		= 1;
	unsigned int turnsperiod    = 30;
	unsigned int turnstemp = 30;
	
	//Startup Menu
	time_t  start, end;
	time(&start);
	serPutString(UCONSOLE,"YOU HAVE  5 SECONDS TO ENTER MENU\n\rHIT ENTER NOW\n\r");
	double elap = 0;

	while (elap<=5) {
		time(&end);
		elap = difftime(end,start);
		elap = elap*SECSPERCLOCK;
#ifdef DEBUG
			sprintf(out, "TM: %5.1f\r\n",elap);
			serPutString(UCONSOLE,out);
#endif
		//This data will fill up the buffer, let's be sure to clear it often.
		serRxFlush(UCOMPASS);

		if(getCommand(UCONSOLE, cmd,1)) {
				inmenu = 1; //Flag for menu response
				elap = 10;  //Escape Start Timer Loop
		}		
	}


	if (inmenu == 1) {
		serPutString(UCONSOLE,"ENTER NEW PERIOD (0-65535):");
		while (inmenu == 1) {
			if(getCommand(UCONSOLE, cmd,1)) {
				turnstemp = atoi(cmd);
				if (turnstemp > 0) {
					turnsperiod = turnstemp;
					sprintf(out, "PERIOD IS NOW %i SECONDS.\r\n",turnsperiod);
					serPutString(UCONSOLE,out);
					inmenu = 0;
				}
				else {
					serPutString(UCONSOLE,"INVALID INPUT, STARTING UP\r\n");
					inmenu = 0;
				}
			}
			//This data will fill up the buffer, let's be sure to clear it often.
			serRxFlush(UCOMPASS);
			
		}
	}

	ledOff(1);
	
	//Reset timer for turns reporting
  	time(&start);

	while (1)  {

		
		//serial echo function
		if (getCommand(UCOMPASS, cmd,0)) {
#ifdef DEBUG
				serPutString(UCONSOLE,"GOT:");
				serPutString(UCONSOLE,cmd);
				serPutString(UCONSOLE,"\r\n");	
#endif
			checksum = checkCompassChecksum(cmd);
			if (checksum) {
				heading = parseCompassHeading(cmd);
				
				//conduct turns calcuation;
				if (heading > 360.0 || heading < 0.0) {
					goodcompass = FALSE;
					continue; //bad value
				}
				if (lasthead == -1) {//first time in loop
					lasthead = heading;
				}
				delta = heading - lasthead;
				if (delta >  180.0) delta -= 360.0;
				if (delta < -180.0) delta += 360.0;
				turnshead = turnshead + delta;
				turns = turnshead/360;
				
#ifdef DEBUG
					sprintf(out, "TURNS: %4.1f\r\n",turns);
					serPutString(UCONSOLE,out);
#endif
				lasthead = heading;	
				goodcompass = TRUE;			
				
			}
			else {
#ifdef DEBUG
				serPutString(UCONSOLE,"BadRead\r\n>");		
#endif
				goodcompass = FALSE; 
			}
				
			
		}
		
		// TEST FOR TURNS OUTPUT
		time(&end);
		elap = difftime(end, start);
		elap = elap*SECSPERCLOCK;
		if (elap >= turnsperiod || firstloop == 1) {
			sprintf(out, "$MBTRN,%5.2f,%1i\r\n",turns,goodcompass);
			//let's add a checksum
			checksum = compute_nmea_checksum(out);
			sprintf(out, "$MBTRN,%5.2f,%1i*%2X\r\n",turns,goodcompass,checksum);	
			//if (lasthead != -1 && pauseoutput != TRUE) {// we haven't gotten any compass values, so don't transmit.
				serPutString(UCONSOLE,out); 
			//}	
			time(&start);
			if (firstloop ==1) firstloop=0;
			goodcompass = FALSE; // this way if the compass data lapses we'll know
		}
		
		// IN RUN MENU FUNCTIONS // RESPONSE FROM MODEM
		if (getCommand(UCONSOLE, concmd,0)) {
			if (strncmp(concmd,"P",1) == 0) {
				serPutString(UCONSOLE,"PAUSING OUTPUT\r\n"); 
				pauseoutput = TRUE;					
			}
			else if (strncmp(concmd,"R",1) == 0) {
				serPutString(UCONSOLE,"RESUMING OUTPUT\r\n"); 
				pauseoutput = FALSE;					
			}

		}

		if (elapisodd(elap) == 1) ledOn(1); 
		else ledOff(1);
		
			
		
		
		//I Am Alive Test Button
		if (getSwitch(2) == 1) {
			ledOn(2);
		}
		else {
			ledOff(2);
		}
	}
	return 0;
}

int elapisodd(double inp) {
	int ret = (int)inp;
	ret = ret % 2;
	return ret;
}

int getCommand(int srcuart, char* cmd, int echo_mode)
{
    static int char_count = 0;
    static char line[MAX_CHARS] = "";
    const unsigned char terminator = '\r';
    int got_line;
    unsigned char b;
    int i;
    
    got_line = FALSE;

    if ( serGetByte(srcuart, &b) )
    {
        if ( echo_mode == 1 ){
            serPutByte(UCONSOLE, b);
            if (b == '\r') serPutByte(UCONSOLE, '\n');
        }

        if ( b != terminator ){
            if ( (b != '\b') && (b != '\n')&& (char_count < MAX_CHARS) )
                line[char_count++] = b;
            else if ( char_count > 0 && b == '\b' )
                --char_count;

            cmd[0] = '\0';
        } 
        else {
            got_line = TRUE;
            line[char_count] = '\0';
            for (i = 0; i <= char_count; i++)
                cmd[i] = line[i];
            char_count = 0;
        }
    }
    return got_line;
}

int compute_nmea_checksum(char *msg)
{
    int checksum = 0;
    int	slen = strlen(msg);
	int charplace = 0;
    for (charplace = 0; charplace < slen; charplace = charplace +1)	{
        if(msg[charplace] == '*'){
            break;
        }
        switch (msg[charplace]){
            case '$':
			   break;
            case '*':
	            continue;
            default:{
                if(checksum == 0){
                   checksum = (int)msg[charplace];
                }
                else{
                   checksum = checksum ^ (int)msg[charplace];
                }
                break;
           }
        }
    }
    return checksum;
}

int checkCompassChecksum(char* msg){
	int checksum = compute_nmea_checksum(msg);
	char compcs[5];
	sprintf(compcs, "%X",checksum);

	//assuming on
	char *tok;
	tok = strtok(msg,"*");
	tok = strtok(NULL,"\r\n");
	
	if (strcmp(tok, compcs) == 0) //MATCH
	{
		return TRUE;
	}
	else {
		return FALSE;
	}
 
	//tok = strtok(

	return 0;
}
float parseCompassHeading(char* msg) {
	//assuming on
	float heading = -1;
	char *tok;
	tok = strtok(msg,",");
   	tok = strtok(NULL,",");
	heading = atof(tok);
	return heading;
}