/****************************************************************************/
/* Copyright 2011 MBARI                                                     */
/* MBARI Proprietary Information. All rights reserved.               		*/
/* Author: EJM																*/
/* PIC 24F Turns Compass Interface	Micro				               		*/
/* 																			*/
/* 	This program serves to communicate via 2 UARTs, polling a Sparton 3002D	*/
/*	compass for heading data and a console port intended to be 				*/
/*	communicating with an ROV via Benthos 885 modems. 						*/ 
/****************************************************************************/
#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: External, 
	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 1843200
#define SECSPERCLOCK 1/CLOCKSPERSEC

int getCommand(int, unsigned char*, int);
int elapisodd(double);
int getBytes(int, unsigned char*,int*,int);
int compute_nmea_checksum(unsigned char *);

int turnsperiod __attribute__ ((persistent)); /* period of message transmission on the console uart*/
	

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 RP12 U2RX */
	RPOR6bits.RP13R = 5;     /* Make Pin RP13 U2TX */

	
	serInit(); /* initialize uarts */
	initBoard();


	serPutString(UCONSOLE,"\n\rCompass Counter V1.0\n\r");
	serPutString(UCONSOLE,"MBARI 2011 EJM\n\r");
	
	ledOn(1);ledOn(2); /* Turn on LEDs at startup */

	/* String Initialization */
	unsigned char compraw[MAX_CHARS];
	int  comprawlen =0;
	unsigned char concmd[MAX_CHARS];
	unsigned char out[MAX_CHARS];
	
	/* Sparton Commands */
	unsigned char compreq[] 		= {0xA4,0x09,0xA0}; /* mag heading request */
	unsigned char mountcfg[] 		= {0xA4,0x4A,0x01,0xA0}; /* compass orientation set to vertical */
	int  compreqlen = 3;
	int  mountcfglen = 4;

	/* Flags */ 
	short goodcompass 	= FALSE;
	short pauseoutput 	= FALSE;
	short vertcompass 	= FALSE;
	
	/* Other Variables */ 
	unsigned int hdg  = 0;
	unsigned int bit1,bit2;
	
	
	double turnshead	= 0.0;
	double turns 		= 0.0;
	double heading 		= -1.0;
	double lasthead 	= -1.0;
	double delta 		= 0.0;

	int firstloop 		= TRUE;
	int turnstemp 		= 30;
	int checksum, inmenu, i;
	int goodreads 		= 0;
	unsigned int ping   = 0;
	
	if ((RCONbits.POR == 0 ) &&	(RCONbits.BOR == 0)) { 
		/* turnsperiod is already set */	
	}
	else if (turnsperiod >= 60 || turnsperiod <= 0) turnsperiod = 30; /*initialize persistent data */
		
	//Startup Menu
	time_t  start, end;
	time(&start);
	sprintf(out,"MESSAGE PERIOD IS %i SECONDS.\r\n",turnsperiod);
	serPutString(UCONSOLE, out);
	serPutString(UCONSOLE,"YOU HAVE  5 SECONDS TO ENTER MENU\n\rHIT ENTER NOW\n\r");
	double elap = 0;
	double lastelap = 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
		
		serRxFlush(UCOMPASS);/* clear unused buffers */

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

	/* Change Interval */ 
	if (inmenu == 1) {
		serPutString(UCONSOLE,"ENTER NEW PERIOD (0-65535):");
		while (inmenu == 1) {
			if(getCommand(UCONSOLE, concmd,1)) {
				turnstemp = atoi(concmd);
				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;
				}
			}
			
			serRxFlush(UCOMPASS);/* clear unused buffers */

		}
	}
	
	ledOff(1); ledOff(2); /* end of startup, turn off all LED's */
	
	/* Main Operating Loop */
	time(&start); /* Reset timer for turns reporting */
	while (1)  {
	
		time(&end);
		elap = difftime(end, start); 
		elap = elap*SECSPERCLOCK; /* calculate time in seconds since last reset */
		if (lastelap == 0) {
			lastelap = floor(lastelap);
		}
		

		/* POLLING FOR THE SPARTON SP3002D */
		if (vertcompass == FALSE && comprawlen == 0) { /* first message to the compass should be to verify orientation */
			for (i=0;i<mountcfglen;i++)
			serPutByte(UCOMPASS,mountcfg[i]);
		}
		
		if (floor(elap)-lastelap >= 1) { /* 1 Hz Compass Polling */ 
			if (vertcompass == TRUE && comprawlen == 0) { /* request a magnetic heading */
				for (i=0;i<compreqlen;i++)
				serPutByte(UCOMPASS,compreq[i]);
			}
			lastelap = floor(elap);
			/* serPutString(UCONSOLE,"1HZ POLL\r\n");*/
					
		}

		if (getBytes(UCOMPASS, compraw, &comprawlen,FALSE) == TRUE) {
#ifdef DEBUG
			for (i=0;i<comprawlen;i++) {
				sprintf(out,"%02X,",compraw[i]);
				serPutString(UCONSOLE,out);
			}
#endif
			
			if (comprawlen > 3) { /* 3 is the error message length */

				if (compraw[1] == 0x09) { /* message is a magnetic heading value */
					bit1=(unsigned int)compraw[2];
					bit2=(unsigned int)compraw[3];
					hdg     = (bit1<<8) + bit2; /* byte shift for a 16-bit value */
					heading = (double)hdg;
					heading = heading*360/4096; /* scale for actual heading value */
#ifdef DEBUG
					sprintf(out,"%i,%i,%0.2f\r\n",comprawlen,hdg,heading);
					serPutString(UCONSOLE,out);
#endif
					/* TURNS CALCULATION */
					if (heading > 360.0 || heading < 0.0) { /* heading validity check */
						goodcompass = FALSE;
						continue; //bad value
					}
					if (lasthead == -1)	lasthead = heading; /* first time in loop */
					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.2f\r\n",turns);
					serPutString(UCONSOLE,out);
#endif
					lasthead = heading; /* for the next loop */
					goodcompass = TRUE; /* Data is valid */
					/* Increment goodreads, if a good read */
					if (goodcompass == TRUE) goodreads++;
				}
				else if (compraw[1] == 0x4A) {/* mounting configuration message */
					if (compraw[2] == 0x01) vertcompass = TRUE;
					else vertcompass = FALSE;
				}	
				
			}
			else { 
#ifdef DEBUG
				serPutString(UCONSOLE,"BadRead\r\n>");
#endif
				goodcompass = FALSE; /* compass data state value */
			}
			comprawlen = 0;

		}
		/* FIRST LOOP ONE-TIME MESSAGE */
		if (elap >= 10 && firstloop == TRUE && goodreads > 0){
			serPutString(UCONSOLE,"$MBTRN,GOOD TO GO\r\n"); 
			if (firstloop == TRUE) { firstloop = FALSE; }/* done with first loop */
			
		}

		/* TEST FOR TURNS OUTPUT */
		if ((elap >= turnsperiod) && (firstloop == FALSE)) {
			sprintf(out, "$MBTRN,%0.2f,%i,%i\r\n",turns,ping,goodreads); /* Form console output, without checksum */
			checksum = compute_nmea_checksum(out); /* generate checksum value */
			sprintf(out, "$MBTRN,%0.2f,%i,%i*%2X\r\n",turns,ping, goodreads,checksum); /* Form console output string again, with checksum */
			if (lasthead != -1 && pauseoutput != TRUE) {
				serPutString(UCONSOLE,out); /* transmit. */
				ping++;
			}
			time(&start); /* reset timer */
			lastelap = 0; /* reset 1Hz pacer time */
			goodcompass = FALSE; /* always reset data condition state */
			goodreads = 0; /*reset the number of goodreads */
		}
		
			
		// 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;
			}

		}
		
		/* 1 Second LED On/Off Function for "in loop" visual status */
		if (elapisodd(elap) == 1) ledOn(1);
		else ledOff(1);

		/* User response button light, REMOVE */
		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,  unsigned char* cmd, int echo_mode)
{
	static int char_count = 0;
	static  unsigned char line[MAX_CHARS] = "";
	const  unsigned char terminator = '\r';
	static 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 getBytes(int srcuart, unsigned char*cmd, int* len, int echo_mode) {
	static int 				char_count 		=	0;
	static unsigned char 	line[MAX_CHARS] = "";
	const  unsigned char 	terminator 		= 0xA0;
	static int 				got_line;
	unsigned char 			b;
	int 					i;

	got_line = FALSE;

	if ( serGetByte(srcuart, &b) )
	{
		if ( echo_mode == TRUE ){

			serPutByte(UCONSOLE, b);
		}

		if ( b != terminator ){
			if (char_count < MAX_CHARS)
			line[char_count++] = b;
			cmd[0] = '\0';
			*len = char_count;
		}
		else {
			line[char_count++] = b;
			for (i = 0; i < char_count; i++) {
				cmd[i] = line[i];
			}
			*len = char_count;
			char_count = 0;
			got_line = TRUE;
		}
	}
	return got_line;
}

int compute_nmea_checksum(unsigned char *msg)
{
	// INITIALIZE 
	int checksum, slen, charplace;
	checksum = 0;
	slen = strlen(msg);
	
	// CALCULATE CHECKSUM BETWEEN $ AND *, RETURN AS INT
	for (charplace = 0; charplace < slen; charplace++)	{
		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 XOR INT
	return checksum;
}