/****************************************************************************/
/* 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 "rtcc.h"
#include "actions.h"
#include "errors.h"
#include "parser.h"
#include "serial.h"
#include "dig_io.h"
#include "spi_io.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 actGetBdAddCmd(char* s)
{
	int bit = atoi(s);
	int BoardAdd = 0;

	BoardAdd = GetBdAdd(1);

	sprintf(msg_buff, "Board Address is %d \r\n", BoardAdd);
    serPutString(SER_CONSOLE, msg_buff);

	return ERR_SUCCESS;
}

int actGetBdTypeCmd(char* s)
{
	int bit = atoi(s);
	int type = 0;

	type = GetBdType(1);

	sprintf(msg_buff, "Board Type is %d \r\n", type);
    serPutString(SER_CONSOLE, msg_buff);
	
	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, "Temp Sensor [J6] is %f degrees\r\n", value);
			serPutString(SER_CONSOLE, cmd_buff);
			break;
		}
		case 1:
		{
			value = storeTemp(1,999);
			sprintf(cmd_buff, "Temp Sensor [J7] is %f degrees\r\n", value);
			serPutString(SER_CONSOLE, cmd_buff);
			break;
		}
		case 2:
		{
			value = storeTemp(2,999);
			sprintf(cmd_buff, "Temp Sensor [J8] is %f degrees\r\n", value);
			serPutString(SER_CONSOLE, cmd_buff);
			break;
		}
		case 3:
		{
			value = storeTemp(3,999);
			sprintf(cmd_buff, "Temp Sensor [J9] is %f degrees\r\n", value);
			serPutString(SER_CONSOLE, cmd_buff);
			break;
		}
		case 4:
		{
			value = storeTemp(4,999);
			sprintf(cmd_buff, "Temp Sensor [J13] is %f degrees\r\n", value);
			serPutString(SER_CONSOLE, cmd_buff);
			break;
		}
		case 5:
		{
			value = storeTemp(5,999);
			sprintf(cmd_buff, "Temp Sensor [J15] is %f degrees\r\n", value);
			serPutString(SER_CONSOLE, cmd_buff);
			break;
		}
		case 6:
		{
			value = storeTemp(6,999);
			sprintf(cmd_buff, "Temp Sensor [J16] is %f degrees\r\n", value);
			serPutString(SER_CONSOLE, cmd_buff);
			break;
		}
		case 7:
		{
			value = storeTemp(7,999);
			sprintf(cmd_buff, "Temp Sensor [J17] is %f degrees\r\n", value);
			serPutString(SER_CONSOLE, cmd_buff);
			break;
		}

	
		default: break;
	}	

	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;
  	double value = 0;
	int cs_add = 0;

	if (bit < 8)
	{
		cs_add = 8;
		switch (bit) // Establish Control Word for each bit
		{
			case 0: conword = 134; break; // 8344 ch 0
			case 1: conword = 198; break; // 8344 ch 1
			case 2: conword = 150; break; // 8344 ch 2
			case 3: conword = 214; break; // 8344 ch 3
			case 4: conword = 166; break; // 8344 ch 4
			case 5: conword = 230; break; // 8344 ch 5
			case 6: conword = 182; break; // 8344 ch 6
			case 7: conword = 246; break; // 8344 ch 7
			default: return;
		}
	}
	else
	{
		cs_add = 9;
		switch (bit) // Establish Control Word for each bit
		{	
			case 8: conword = 150; break; // 8341 ch 0
			case 9: conword = 166; break; // 8341 ch 1
			case 10: conword =  86; break; // 8341 ch 2
			case 11: conword = 214; break; // 8341 ch 3
			default: return;
		}
	}


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

   	digSetOutBit(cs_add,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;
}



/****************************************************************************/
/*                               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 actSetCSAddCmd(char* s)
{
	int bit = atoi(s);

	CSAddSet(bit);	

	return ERR_SUCCESS;
}

int actSetBusIntCmd(char* s)
{
	int bit = atoi(s);

	if(bit == 0)
	{
		digSetOutBit(5,0);
	}
	else
	{
		digSetOutBit(5,1);
	}

	return ERR_SUCCESS;
}

int actSetPoten0RESCmd(char* s)
{
	int bit = atoi(s);

	DigPotSet(0,bit);

	return ERR_SUCCESS;
}

int actSetPoten1RESCmd(char* s)
{
	int bit = atoi(s);

	DigPotSet(1,bit);

	return ERR_SUCCESS;
}


int actSetPotenRSTCmd(char* s)
{
	int bit = atoi(s);

	DigPotRST(1);

	return ERR_SUCCESS;
}



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

    return ERR_SUCCESS;
}


/****************************************************************************/
/*                    Environmental Board commands                          */
/****************************************************************************/


void CSAddSet(int address)
{
	switch(address)
	{
		case 0:
		{
			digSetOutBit(0,0);  // Add A
			digSetOutBit(1,0);  // Add B
			digSetOutBit(2,0);  // Add C
			digSetOutBit(3,0);	// Add D
			break;
		}
		case 1:
		{
			digSetOutBit(0,1);  // Add A
			digSetOutBit(1,0);  // Add B
			digSetOutBit(2,0);  // Add C
			digSetOutBit(3,0);	// Add D
			break;
		}

		case 2:
		{
			digSetOutBit(0,0);  // Add A
			digSetOutBit(1,1);  // Add B
			digSetOutBit(2,0);  // Add C
			digSetOutBit(3,0);	// Add D
			break;
		}

		case 3:
		{
			digSetOutBit(0,1);  // Add A
			digSetOutBit(1,1);  // Add B
			digSetOutBit(2,0);  // Add C
			digSetOutBit(3,0);	// Add D
			break;
		}
		case 4:
		{
			digSetOutBit(0,0);  // Add A
			digSetOutBit(1,0);  // Add B
			digSetOutBit(2,1);  // Add C
			digSetOutBit(3,0);	// Add D
			break;
		}
	
		case 5:
		{
			digSetOutBit(0,1);  // Add A
			digSetOutBit(1,0);  // Add B
			digSetOutBit(2,1);  // Add C
			digSetOutBit(3,0);	// Add D
			break;
		}
		case 6:
		{
			digSetOutBit(0,0);  // Add A
			digSetOutBit(1,1);  // Add B
			digSetOutBit(2,1);  // Add C
			digSetOutBit(3,0);	// Add D
			break;
		}
		case 7:
		{
			digSetOutBit(0,1);  // Add A
			digSetOutBit(1,1);  // Add B
			digSetOutBit(2,1);  // Add C
			digSetOutBit(3,0);	// Add D
			break;
		}
		case 8:
		{
			digSetOutBit(0,0);  // Add A
			digSetOutBit(1,0);  // Add B
			digSetOutBit(2,0);  // Add C
			digSetOutBit(3,1);	// Add D
			break;
		}
		case 9:
		{
			digSetOutBit(0,1);  // Add A
			digSetOutBit(1,0);  // Add B
			digSetOutBit(2,0);  // Add C
			digSetOutBit(3,1);	// Add D
			break;
		}		
		default: 
		{
			digSetOutBit(0,1);  // Add A
			digSetOutBit(1,1);  // Add B
			digSetOutBit(2,1);  // Add C
			digSetOutBit(3,1);	// Add D
			break;
		}
	}
	 
	return;
}


int GetBdAdd(int mult)
{
	int Add1 = 0;
	int Add2 = 0;
	int Add3 = 0;
	int AddInt1 = 0;
	int AddInt2 = 0;
	int AddInt3 = 0;
	int BoardAdd = 0;

	Add1 = digGetInBit(0); // LSB
	Add2 = digGetInBit(1);
	Add3 = digGetInBit(2); // MSB

	AddInt1 = Add1 * 1;
	AddInt2 = Add2 * 2;
	AddInt3 = Add3 * 4;	

	BoardAdd = (AddInt1 + AddInt2 + AddInt3) * mult;

	return BoardAdd;
}

int GetBdType(int mult)
{
	int BoardType = 5; // 5 = Environmental Board
	
	return BoardType;
}


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;
	int cs_add = 0;

	if (chan < 8)
	{
		cs_add = 8;
		switch (chan) // Establish Control Word for each bit
		{
			case 0: conword = 134; break; // 8344 ch 0
			case 1: conword = 198; break; // 8344 ch 1
			case 2: conword = 150; break; // 8344 ch 2
			case 3: conword = 214; break; // 8344 ch 3
			case 4: conword = 166; break; // 8344 ch 4
			case 5: conword = 230; break; // 8344 ch 5
			case 6: conword = 182; break; // 8344 ch 6
			case 7: conword = 246; break; // 8344 ch 7
			default: return;
		}
	}
	else
	{
		cs_add = 9;
		switch (chan) // Establish Control Word for each bit
		{	
			case 0: conword = 150; break; // 8341 ch 0 (chan 8)
			case 1: conword = 166; break; // 8341 ch 1 (chan 9)
			case 2: conword =  86; break; // 8341 ch 2 (chan 10)
			case 3: conword = 214; break; // 8341 ch 3 (chan 11)
			default: return;
		}
	}
	

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

   	digSetOutBit(cs_add,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[12][100] = {0};
	double avgValue[12] = {0};
	double temp = 0;
	double runTot[12] = {0};
	int i = 0;
	int j = 0;
	static int count[12] = {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[12] = {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[12] = {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 >7) chan  = 7; // prevent out of range cs 

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

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

	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[8][20] = {0};
	double avgValue[8] = {0};
	double temp = 0;
	double runTot[8] = {0};
	int i = 0;
	int j = 0;
	static int countTemp[8] = {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[8] = {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;
}


void DigPotSet(int chan, int value)
{
	int word = 0;
	
	if (chan == 0)
	{
		word = 17;
	}
	else
	{
		word = 18;
	}	

	digSetOutBit(4,0);
	spiReadWrite2(word);
	spiReadWrite2(value);
	digSetOutBit(4,1);

	return;
}

void DigPotRST(int mult)
{
	int i;
	
	digSetOutBit(9,0);
	for (i=0; i<100; i++);
	digSetOutBit(9,1);
	
	return;
}

void BdIdent(unsigned int type)
{
	SPI1STATbits.SPIROV = 0;
	spiReadWrite1(type);
	
	return;
}