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

#include "card.h"
#include "sys_defs.h"
#include "actions.h"
#include "errors.h"
#include "parser.h"
#include "serial.h"
#include "spi1.h"
#include "spi2.h"
#include "analog.h"
#include "dig_io.h"

static char msg_buff[64];

double readTemp(int chan);
void filterTemp(int chan, double value);
double storeTemp(int chan, double value);

/****************************************************************************/
/*                               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 actResetCmd(char* s)
{
    asm("reset");
    return ERR_SUCCESS;
}

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

int actSpi2TestCmd(char* s)
{
    unsigned char in1 = 0, in2 = 0, in3 = 0;
    unsigned int i;
    
    while (1)
    {
        CSAddSet(8);
        spiReadWrite2(0b10000111);
        Nop();
        
        in1 = spiReadWrite2(0);
        in2 = spiReadWrite2(0);
        in3 = spiReadWrite2(0);
        
        //CSAddSet(99);
        sprintf(msg_buff, "in1 = %d, in2 = %d, in3 = %d\r\n", in1, in2, in3);
        serPutString(SER_CONSOLE, msg_buff);
    }
    
    return ERR_SUCCESS;
}

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

int actGetCardInfoCmd(char* s)
{
    cardInfoDump(&cardInfo);
    return ERR_SUCCESS;
}

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

    serPutString(SER_CONSOLE, spiOutBuff);

    return ERR_SUCCESS;
}

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

    serPutString(SER_CONSOLE, spiOutBuff);

    return ERR_SUCCESS;
}

int actGetSpiStatsCmd(char* s)
{
    unsigned int stat;
    
    stat = spiGetIrqCnt();
    sprintf(msg_buff, "SPI IRQ COUNT = %u\r\n", stat);
    serPutString(SER_CONSOLE, msg_buff);
    
    stat = spiGetCmdCnt();
    sprintf(msg_buff, "SPI CMD COUNT = %u\r\n", stat);
    serPutString(SER_CONSOLE, msg_buff);
    
    stat = spiGetInfoCnt();
    sprintf(msg_buff, "SPI INFO COUNT = %u\r\n", stat);
    serPutString(SER_CONSOLE, msg_buff);
    
    stat = spiGetReadCnt();
    sprintf(msg_buff, "SPI READ COUNT = %u\r\n", stat);
    serPutString(SER_CONSOLE, msg_buff);
    
    stat = spiGetWriteCnt();
    sprintf(msg_buff, "SPI WRITE COUNT = %u\r\n", stat);
    serPutString(SER_CONSOLE, msg_buff);
    
    if ( SPI1STATbits.SPIROV )
        serPutString(SER_CONSOLE, "SPI OVERFLOW BIT  = 1\r\n");
    else
        serPutString(SER_CONSOLE, "SPI OVERFLOW BIT  = 0\r\n");


    sprintf(msg_buff, "SPI1STAT = 0x%04X\r\n", SPI1STAT);
    serPutString(SER_CONSOLE, msg_buff);


    if ( _SPI1IF )
        serPutString(SER_CONSOLE, "_SPI1IF = 1\r\n");
    else
        serPutString(SER_CONSOLE, "_SPI1IF = 0\r\n");

    if ( _SPI1IE )
        serPutString(SER_CONSOLE, "_SPI1IE = 1\r\n");
    else
        serPutString(SER_CONSOLE, "_SPI1IE = 0\r\n");

    return ERR_SUCCESS;
}

int actGetCompassCmd(char* s) {
    int bit = atoi(s);
    unsigned int conword = 0;
    double value = 0.0;
    
    unsigned int analog1 = 0;
	unsigned int analog2 = 0;
	unsigned int analog3 = 0;
	unsigned int analog4 = 0;
	unsigned int analog5 = 0;
	unsigned int analog6 = 0;
    
    switch (bit) {
        case 0: conword = 150; break;
        case 1: conword = 214; break;
        case 2: conword = 166; break;
        case 3: conword = 230; break;
    }
    
    SPI2STATbits.SPIROV = 1;
	// spiReadWrite2(2); // setup up clock type in ADC chip (internal)
	Nop();
    
    CSAddSet(9);
	
	spiReadWrite2(conword); // Send ADC control word
	Nop();
	analog1 = spiReadWrite2(0x00); // MSB
	analog2 = spiReadWrite2(0x00); // LSB
	analog3 = spiReadWrite2(0x00); // Extra Bit + Trailing zeroes?

	CSAddSet(99);
 
	/* 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

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

    sprintf(spiOutBuff, "analog1 = %d\r\n", analog1);
	serPutString(SER_CONSOLE, spiOutBuff);
    
    sprintf(spiOutBuff, "analog2 = %d\r\n", analog2);
	serPutString(SER_CONSOLE, spiOutBuff);

    sprintf(spiOutBuff, "analog3 = %d\r\n", analog3);
	serPutString(SER_CONSOLE, spiOutBuff);

    sprintf(spiOutBuff, "analog4 = %d\r\n", analog4);
	serPutString(SER_CONSOLE, spiOutBuff);

    sprintf(spiOutBuff, "analog5 = %d\r\n", analog5);
	serPutString(SER_CONSOLE, spiOutBuff);

    sprintf(spiOutBuff, "analog6 = %d\r\n", analog6);
	serPutString(SER_CONSOLE, spiOutBuff);
	
	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 < 9)
		cs_add = 8;
	else
		cs_add = 9;

		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
			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: break;
		}



	CSAddSet(cs_add);

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

	CSAddSet(99);
 
	/* 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

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

    sprintf(spiOutBuff, "analog1 = %d\r\n", analog1);
	serPutString(SER_CONSOLE, spiOutBuff);
    
    sprintf(spiOutBuff, "analog2 = %d\r\n", analog2);
	serPutString(SER_CONSOLE, spiOutBuff);

    sprintf(spiOutBuff, "analog3 = %d\r\n", analog3);
	serPutString(SER_CONSOLE, spiOutBuff);

    sprintf(spiOutBuff, "analog4 = %d\r\n", analog4);
	serPutString(SER_CONSOLE, spiOutBuff);

    sprintf(spiOutBuff, "analog5 = %d\r\n", analog5);
	serPutString(SER_CONSOLE, spiOutBuff);

    sprintf(spiOutBuff, "analog6 = %d\r\n", analog6);
	serPutString(SER_CONSOLE, spiOutBuff);
	
	return ERR_SUCCESS;
}

int actGetTempCmd(char* s)
{
	int bit = atoi(s);
	int value = 0;   
    
	switch(bit)
	{
		case 0:	
		{
			value = storeTemp(0,999);
			sprintf(spiOutBuff, "Temp Sensor [J6] is %d degrees\r\n", value);
			serPutString(SER_CONSOLE, spiOutBuff);
			break;
		}
		case 1:
		{
			value = storeTemp(1,999);
			sprintf(spiOutBuff, "Temp Sensor [J7] is %d degrees\r\n", value);
			serPutString(SER_CONSOLE, spiOutBuff);
			break;
		}
		case 2:
		{
			value = storeTemp(2,999);
			sprintf(spiOutBuff, "Temp Sensor [J8] is %d degrees\r\n", value);
			serPutString(SER_CONSOLE, spiOutBuff);
			break;
		}
		case 3:
		{
			value = storeTemp(3,999);
			sprintf(spiOutBuff, "Temp Sensor [J9] is %d degrees\r\n", value);
			serPutString(SER_CONSOLE, spiOutBuff);
			break;
		}
		case 4:
		{
			value = storeTemp(4,999);
			sprintf(spiOutBuff, "Temp Sensor [J13] is %d degrees\r\n", value);
			serPutString(SER_CONSOLE, spiOutBuff);
			break;
		}
		case 5:
		{
			value = storeTemp(5,999);
			sprintf(spiOutBuff, "Temp Sensor [J15] is %d degrees\r\n", value);
			serPutString(SER_CONSOLE, spiOutBuff);
			break;
		}
		case 6:
		{
			value = storeTemp(6,999);
			sprintf(spiOutBuff, "Temp Sensor [J16] is %d degrees\r\n", value);
			serPutString(SER_CONSOLE, spiOutBuff);
			break;
		}
		case 7:
		{
			value = storeTemp(7,999);
			sprintf(spiOutBuff, "Temp Sensor [J17] is %d degrees\r\n", value);
			serPutString(SER_CONSOLE, spiOutBuff);
			break;
		}

	
		default: break;
	}
    
	return ERR_SUCCESS;
}

int actGetAdcBusyCmd(char* s)
{
    int bits[4];
    bits[0] = digGetInBit(0);
    bits[1] = digGetInBit(1);
    bits[2] = digGetInBit(2);
    bits[3] = digGetInBit(3);
    
    sprintf(msg_buff, "%u%u%u%u\r\n", bits[0], bits[1], bits[2], bits[3]);
    serPutString(SER_CONSOLE, msg_buff);
    
    return ERR_SUCCESS;
}


/****************************************************************************/
/*                               Set commands                               */
/****************************************************************************/
int actSetStubCmd(char* s)
{
    int num = atoi(s);
    
    sprintf(spiOutBuff, "actSetStubCmd() = %d\r\n", num);
    serPutString(SER_CONSOLE, msg_buff);

    return ERR_SUCCESS;
}

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

    return ERR_SUCCESS;
}

int actSetCSAddCmd(char* s)
{
	int cs = atoi(s);

	CSAddSet(cs);

	return ERR_SUCCESS;
}

/****************************************************************************/
/*                              Clear commands                              */
/****************************************************************************/
int actClrSpiStatsCmd(char* s)
{
    spiClrIrqCnt();
    spiClrCmdCnt();
    spiClrInfoCnt();
    spiClrReadCnt();
    spiClrWriteCnt();

    /* clear SPI overflow */
    SPI1STATbits.SPIROV = 0;
    
    return ERR_SUCCESS;
}

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

    return ERR_SUCCESS;
}

/***************************************************************************/
/*                         Environ Bd 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;
		}	
		case 99:
		{
			digSetOutBit(0,1);  // Add A
			digSetOutBit(1,1);  // Add B
			digSetOutBit(2,1);  // 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;
		}
	}
	 
	//sprintf(spiOutBuff, "\r\nIn CSAdd, CS = %d\r\n", address);
	//serPutString(SER_CONSOLE, spiOutBuff);

	return;
}

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;
}