/****************************************************************************/
/* Copyright 2010 MBARI.                                                    */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
#include <p24fxxxx.h>

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "sys_defs.h"

#include "actions.h"
#include "errors.h"
#include "parser.h"
#include "serial.h"
#include "dig_io.h"
#include "rtcc.h"
#include "spi_io.h"
#include "analog.h"

#define MAX_CHARS   80  /* maximum command line entry length */
static char msg_buff[64];
char cmd_buff[MAX_CHARS];
#define ChipSelect OUT_BIT_13

/****************************************************************************/
/*                               Root commands                              */
/****************************************************************************/
int actGetCmd(char* s)
{
    return prsParse(s, cmdsGet, ERR_NO_SUB_COMMAND);
}

int actSetCmd(char* s)
{
    return prsParse(s, cmdsSet, ERR_NO_SUB_COMMAND);
}

int actClrCmd(char* s)
{
    return prsParse(s, cmdsClr, ERR_NO_SUB_COMMAND);
}

int actHelloKittyCmd(char* s)
{
    serPutString(SER_CONSOLE, "MEOW\r\n");
    return ERR_SUCCESS;
}

/****************************************************************************/
/*                               Get commands                               */
/****************************************************************************/

int actGetDinCmd(char* s)
{
    int bit = atoi(s);
    
    if ( digGetInBit(bit) )
        sprintf(msg_buff, "BIT %d IS HI\r\n", bit);
    else
        sprintf(msg_buff, "BIT %d IS LO\r\n", bit);

    serPutString(SER_CONSOLE, msg_buff);


    return ERR_SUCCESS;
}

int actGetDoutCmd(char* s)
{
    int bit = atoi(s);
    
    if ( digGetOutBit(bit) )
        sprintf(msg_buff, "BIT %d IS HI\r\n", bit);
    else
        sprintf(msg_buff, "BIT %d IS LO\r\n", bit);

    serPutString(SER_CONSOLE, msg_buff);


    return ERR_SUCCESS;
}

int actGetTimeCmd(char* s)
{
    DateTime dt;

    rtcRead(&dt);
    rtcDateToString(&dt, msg_buff);

    serPutString(SER_CONSOLE, "DATE: ");
    serPutString(SER_CONSOLE, msg_buff);
    serPutString(SER_CONSOLE, "\r\n");

    rtcTimeToString(&dt, msg_buff);

    serPutString(SER_CONSOLE, "TIME: ");
    serPutString(SER_CONSOLE, msg_buff);
    serPutString(SER_CONSOLE, "\r\n");
    
    return ERR_SUCCESS;
}

int actGetADCCmd(char* s)
{
	int bit = atoi(s);
	unsigned int analog1 = 0;
	unsigned int analog2 = 0;
	unsigned int analog3 = 0;
	unsigned int analog4 = 0;
	unsigned int analog5 = 0;
	unsigned int analog6 = 0;
	int conword = 0;
    //int i;
	double value = 0;

	switch (bit) // Establish Control Word for each bit
	{
	case 0: conword = 134; break; 
	case 1: conword = 198; break; 
	case 2: conword = 150; break;
	case 3: conword = 214; break;
	case 4: conword = 166; break;
	case 5: conword = 230; break;
	case 6: conword = 182; break;
	case 7: conword = 246; break;
	default: return;
	}

	
	digSetOutBit(3, 0); // deasserts chip select	
	SPI2STATbits.SPIROV = 0;
	spiReadWrite(2); // setup up clock type in ADC chip (internal)
	Nop();
	
	spiReadWrite(conword); // Send ADC control word
	//for(i = 0; i < 10; ++i)
    //	Nop();
	analog1 = spiReadWrite(0); // MSB
	analog2 = spiReadWrite(0); // LSB
	analog3 = spiReadWrite(0); // Extra Bit + Trailing zeroes?

   	digSetOutBit(3,1);	// asserts chip select

	/* Combine 2 8-bit analog words to 1 16-bit word */	
	analog4 = analog1;
    analog4 <<= 8;
    analog4 |= (0x00FF & analog2);

	analog4 <<=1; // shift resulting word 1 bit left

	analog3 >>= 7; // shift Extra bit to '1' position
	analog5 = 0x0000;
	analog5 <<=8;
	analog5 |= (0x00FF & analog3); // Mask Extra bit into 16-bit word

	analog6 = (analog4 | analog5); // Add Extra bit onto 16-bit word

	//sprintf(cmd_buff, "\r\nanalog6 = %d\r\n", analog6);
	//serPutString(SER_CONSOLE, cmd_buff);

	value = anaConvSample(bit, analog6,2);
	
	sprintf(cmd_buff, "\r\nReturned Value = %f\r\n", value);
	serPutString(SER_CONSOLE, cmd_buff);
	
	return ERR_SUCCESS;
}


int actGetCurrCmd(char* s)
{
	double value = 0;
	int bit = atoi(s);

	switch (bit)
	{
		case 24:
		{
			value = storeADC(0,999);
			sprintf(cmd_buff, "24V Current is %f amps\r\n", value);
			serPutString(SER_CONSOLE, cmd_buff);
			break;
		}
		case 3:
		{
			value = storeADC(2,999);
			sprintf(cmd_buff, "3.3V Current is %f amps\r\n", value);
			serPutString(SER_CONSOLE, cmd_buff);
			break;
		}
		case 5:
		{	
			value = storeADC(4,999);
			sprintf(cmd_buff, "5V Current is %f amps\r\n", value);
			serPutString(SER_CONSOLE, cmd_buff);
			break;
		}
		case 12:
		{
			value = storeADC(6,999);
			sprintf(cmd_buff, "12V Current is %f amps\r\n", value);
			serPutString(SER_CONSOLE, cmd_buff);
			break;
		}
		default:break;
	
	}
	return ERR_SUCCESS;
}
  
int actGetVoltCmd(char* s)
{
	double value = 0;
	int bit = atoi(s);

	switch(bit)
	{
		case 24:
		{
			value = storeADC(1,999);
			sprintf(cmd_buff, "24V Voltage is %f volts\r\n", value);
			serPutString(SER_CONSOLE, cmd_buff);
			break;
		}
		case 3:
		{
			value = storeADC(3,999);
			sprintf(cmd_buff, "3.3V Voltage is %f volts\r\n", value);
			serPutString(SER_CONSOLE, cmd_buff);
			break;
		}
		case 5:
		{	
			value = storeADC(5,999);
			sprintf(cmd_buff, "5V Voltage is %f volts\r\n", value);
			serPutString(SER_CONSOLE, cmd_buff);
			break;
		}
		case 12:
		{
			value = storeADC(7,999);
			sprintf(cmd_buff, "12V Voltage is %f volts\r\n", value);
			serPutString(SER_CONSOLE, cmd_buff);
			break;
		}
		default:break;
	
	}
	return ERR_SUCCESS;
}	

int actGetTempCmd(char* s)
{
	int bit = atoi(s);
	double value = 0;   

	switch(bit)
	{
		case 0:	
		{
			value = storeTemp(0,999);
			sprintf(cmd_buff, "3.3V DCDC Converter is %f degrees\r\n", value);
			serPutString(SER_CONSOLE, cmd_buff);
			break;
		}
		case 1:
		{
			value = storeTemp(1,999);
			sprintf(cmd_buff, "5V DCDC Converter is %f degrees\r\n", value);
			serPutString(SER_CONSOLE, cmd_buff);
			break;
		}
		case 2:
		{
			value = storeTemp(2,999);
			sprintf(cmd_buff, "12V DCDC Converter is %f degrees\r\n", value);
			serPutString(SER_CONSOLE, cmd_buff);
			break;
		}
		default: break;
	}	

	return ERR_SUCCESS;
}

int actGetFanCmd(char* s)
{
	int bit = atoi(s);
	int fanState = 0;

	fanState = digGetInBit(0);

	if (digGetInBit(fanState))
	{
        sprintf(msg_buff, "Fan is On\r\n");
		//fanState = 1;
	}
    else
	{
        sprintf(msg_buff, "Fan is Off\r\n");
		//fanState = 0;
	}
	
	storeFan(fanState, 0);
    serPutString(SER_CONSOLE, msg_buff);

	return ERR_SUCCESS;
}


   

/****************************************************************************/
/*                               Set commands                               */
/****************************************************************************/
int actSetDoutCmd(char* s)
{
    int bit = atoi(s);
    
    digSetOutBit(bit, 1);

    return ERR_SUCCESS;
}

int actSetTimeCmd(char* s)
{
    DateTime dt;

    if ( strlen(s) == 0 )
    {
        serPutString(SER_CONSOLE, "TIME SET FORMAT: hh:mm:ss:wd:DD:MM:YY\r\n");
        serPutString(SER_CONSOLE, "wd(week day): 0 = Sunday, 1 = Monday, etc.\r\n");
        return ERR_SUCCESS; 
    }
    
    sscanf(s, "%hhd:%hhd:%hhd:%hhd:%hhd:%hhd:%hhd", &dt.hours, &dt.minutes, 
           &dt.seconds, &dt.day_of_week, &dt.day, &dt.month, &dt.year);

    if ( !rtcSet(&dt) )
        serPutString(SER_CONSOLE, "ERR: FAILED TO SET RTC\r\n");

    return ERR_SUCCESS;
}

int actSetFanCmd(char* s)
{
	int bit = atoi(s);
		
	if (bit)
	{
		digSetOutBit(5, 1);
		storeFan(1, 0);
	}
	else
	{
		digSetOutBit(5, 0);
		storeFan(0, 0);
	}

	return ERR_SUCCESS;

}


/****************************************************************************/
/*                              Clear commands                              */
/****************************************************************************/
int actClrDoutCmd(char* s)
{
    int bit = atoi(s);
    
    digSetOutBit(bit, 0);

    return ERR_SUCCESS;
}



/****************************************************************************/
/*                    Power Supply Board commands                           */
/****************************************************************************/

double readADC(int chan, int type)
{
	unsigned int analog1 = 0;
	unsigned int analog2 = 0;
	unsigned int analog3 = 0;
	unsigned int analog4 = 0;
	unsigned int analog5 = 0;
	unsigned int analog6 = 0;
	int conword = 0;
    double value = 0;

	switch (chan) // Establish Control Word for each bit
	{
		case 0: conword = 134; break; 
		case 1: conword = 198; break; 
		case 2: conword = 150; break;
		case 3: conword = 214; break;
		case 4: conword = 166; break;
		case 5: conword = 230; break;
		case 6: conword = 182; break;
		case 7: conword = 246; break;
		default: return;
	}

	digSetOutBit(3, 0); // deasserts chip select	
	SPI2STATbits.SPIROV = 0;
	spiReadWrite(2); // setup up clock type in ADC chip (internal)
	Nop();
	
	spiReadWrite(conword); // Send ADC control word
	analog1 = spiReadWrite(0); // MSB
	analog2 = spiReadWrite(0); // LSB
	analog3 = spiReadWrite(0); // Extra Bit + Trailing zeroes?

   	digSetOutBit(3,1);	// asserts chip select

	/* Combine 2 8-bit analog words to 1 16-bit word */	
	analog4 = analog1;
    analog4 <<= 8;
    analog4 |= (0x00FF & analog2);

	analog4 <<=1; // shift resulting word 1 bit left

	analog3 >>= 7; // shift Extra bit to '1' position
	analog5 = 0x0000;
	analog5 <<=8;
	analog5 |= (0x00FF & analog3); // Mask Extra bit into 16-bit word

	analog6 = (analog4 | analog5); // Add Extra bit onto 16-bit word
	
	storeADCword(chan, 0, analog6);	

	value = anaConvSample(chan, analog6,type);
	filterADC(chan, value);

//	sprintf(cmd_buff, "\rADC Channel %d = %f", chan, value);
//	serPutString(SER_CONSOLE, cmd_buff);
/*
	if (chan == 2)
	{
		sprintf(cmd_buff, "\r%d", analog6);
		serPutString(SER_CONSOLE, cmd_buff);
	}
*/
	return value;
}


void filterADC(int chan, double value)
{
	int numsamp = 100;
	static double currSamp[8][100] = {0};
	double avgValue[8] = {0};
	double temp = 0;
	double runTot[8] = {0};
	int i = 0;
	int j = 0;
	static int count[8] = {0};


	if (count[chan] >= numsamp)
	{
		for (j=0; j<numsamp; j++)
			{
				currSamp[chan][j] = currSamp[chan][j+1];
			}
		count[chan] = numsamp-1;
		currSamp[chan][count[chan]] = value;
	}
	else
	{
		currSamp[chan][count[chan]] = value;
	}

	for (i=0; i<=count[chan]; i++)
	{
		runTot[chan] = runTot[chan] + currSamp[chan][i];
	}

	avgValue[chan] = runTot[chan]/(count[chan]+1);
/*
	sprintf(cmd_buff, "\ravgValue = %f", avgValue[chan]);
	serPutString(SER_CONSOLE, cmd_buff);
	sprintf(cmd_buff, "\rrunTot = %f", runTot[chan]);
	serPutString(SER_CONSOLE, cmd_buff);
	sprintf(cmd_buff, "\rcount(%d) = %d", chan, count[chan]);
	serPutString(SER_CONSOLE, cmd_buff);
*/
	count[chan]++;	
	
	temp = avgValue[chan];
	storeADC(chan, temp);

	return;
}


double storeADC(int chan, double value)
{
	static double ADCValue[8] = {0};
	double output = 0;

	if (value == 999)
	{
		output = ADCValue[chan];
	}
	else
	{
		ADCValue[chan] = value;
	}

	//sprintf(cmd_buff, "\rstoreADC output value = %f", output);
	//serPutString(SER_CONSOLE, cmd_buff);
	return output;
}

unsigned int storeADCword(int chan, int sendrec, unsigned int word)
{
	static unsigned int ADCword[8] = {0};
	unsigned int outputword = 0;

	if (sendrec == 1) // read
	{
		outputword = ADCword[chan];
	}
	else // write
	{
		ADCword[chan] = word;
	}

	return outputword;
}



double readTemp(int chan)
{
	unsigned int readbackMSB = 0;
	unsigned int readbackLSB = 0;	
	unsigned int readback = 0;
	double value = 0;

	if (chan >2) chan  = 2; // prevent out of range cs 

	digSetOutBit(chan, 0); // deasserts chip select	
	SPI2STATbits.SPIROV = 0;

	readbackMSB = spiReadWrite(0);
	readbackLSB = spiReadWrite(0);
	Nop();
	digSetOutBit(chan,1); // reasserts chip select

	readback = readbackMSB; // copy MSB
    readback <<= 5; // shift MSB
    readbackLSB &=0b11111000;
	readbackLSB >>= 3;
	readback |= (0x00FF & readbackLSB);

	//sprintf(cmd_buff, "\rWord = %d", readback);
	//serPutString(SER_CONSOLE, cmd_buff);

	value = readback * 0.25;
	filterTemp(chan, value); 

	return value;
}

void filterTemp(int chan, double value)
{
	static double currTempSamp[3][20] = {0};
	double avgValue[3] = {0};
	double temp = 0;
	double runTot[3] = {0};
	int i = 0;
	int j = 0;
	static int countTemp[3] = {0};

	if (countTemp[chan] >= 20)
	{
		for (j=0; j<20; j++)
			{
				currTempSamp[chan][j] = currTempSamp[chan][j+1];
			}
		countTemp[chan] = 19;
		currTempSamp[chan][countTemp[chan]] = value;
	}
	else
	{
		currTempSamp[chan][countTemp[chan]] = value;
	}

	for (i=0; i<=countTemp[chan]; i++)
	{
		runTot[chan] = runTot[chan] + currTempSamp[chan][i];
	}

	avgValue[chan] = runTot[chan]/(countTemp[chan]+1);
/*
	sprintf(cmd_buff, "\ravgValue = %f", avgValue[chan]);
	serPutString(SER_CONSOLE, cmd_buff);
	sprintf(cmd_buff, "\rrunTot = %f", runTot[chan]);
	serPutString(SER_CONSOLE, cmd_buff);
	sprintf(cmd_buff, "\rcount(%d) = %d", chan, countTemp[chan]);
	serPutString(SER_CONSOLE, cmd_buff);
*/
	countTemp[chan]++;	
	
	temp = avgValue[chan];
	storeTemp(chan, temp);

	return;;

}

double storeTemp(int chan, double value)
{
	static double TempValue[3] = {0};
	double output = 0;

	if (value == 999)
	{
		output = TempValue[chan];
	}
	else
	{
		TempValue[chan] = value;
	}

	//	sprintf(cmd_buff, "\rstoreADC output value = %f", output);
	//	serPutString(SER_CONSOLE, cmd_buff);
	return output;
}


int storeFan(int state, int value)
{
 	static int fanState = 0;
	int output = 0;

	if (value == 999)
	{
		output = fanState;
	}
	else
	{
		fanState = state;
	}

	return output;
}