#include <system.h>
#pragma DATA _CONFIG1H, _OSC_HS_1H 
#pragma DATA _CONFIG2H, _WDT_OFF_2H 
#pragma DATA _CONFIG4L, _LVP_OFF_4L & _XINST_OFF_4L
#pragma DATA _CONFIG3H, _MCLRE_ON_3H
#pragma CLOCK_FREQ 20000000


/****************************************************************************/
/***********************Input, Outputs, and Variables************************/
/****************************************************************************/


/*Analog inputs*/
unsigned char ucap_v = 0;					// on AN0/RA0 - PORTA.0
unsigned char starter_motor_current = 0;	// on AN1/RA1 - PORTA.1
unsigned char ucap_stack_current = 0;		// on AN2/RA2 - PORTA.2
unsigned char driving_motor_current = 0;	// on AN3/RA3 - PORTA.3
unsigned char temp_sensor = 0;				// on AN4/RA5 - PORTA.5
unsigned char auxillary = 0;				// on RB2 - PORTB.2

/*Outputs to generator*/
volatile bit on_off@PORTD.0;				// turn on/off generator: on RD0
volatile bit starter@PORTD.2;				// turn on starter motor: on RD1
volatile bit glow_plug@PORTD.1;				// turn on glow plug: on RD2
volatile bit aux_out@PORTD.3;				// auxillary output: on RD3

/*A/D converter parameters*/
volatile bit go_done@ADCON0.1;				// A/D conversion status bit
volatile bit chs3@ADCON0.5;					// channel select bit 3
volatile bit chs2@ADCON0.4;					// channel select bit 2
volatile bit chs1@ADCON0.3;					// channel select bit 1
volatile bit chs0@ADCON0.2;					// channel select bit 0

/*LCD parameters*/
unsigned char tx_packet[20];
unsigned char ucap_v_pos = 0x0B;			// position of ucap_v on LCD
unsigned char st_curr_pos = 0x49;			// position of st_curr on LCD

/*GPS (UART) parameters*/
volatile bit txif@PIR1.4;					// transmit register interrupt flag bit
volatile bit rcif@PIR1.5;					// receive register interrupt enable bit
volatile bit rcie@PIE1.5;					// EUSART receive interrupt enable bit
char GPS_receive[34];
int k = 34;

/*SPI parameters - for SD and LCD*/
volatile bit smp@SSPSTAT.7;
volatile bit cke@SSPSTAT.6;
volatile bit bf@SSPSTAT.0;
volatile bit wcol@SSPCON1.7;
volatile bit sspov@SSPCON1.6;
volatile bit sspen@SSPCON1.5;
volatile bit ckp@SSPCON1.4;
volatile bit sspif@PIR1.3;
volatile bit sspie@PIE1.3;

/*Digital outputs*/
volatile bit LCD_cs@PORTC.0;				// LCD chip select: on RC0
volatile bit SD_cs@PORTC.1;					// SD chip select: on RC1
volatile bit SD_wp@PORTD.4;					// SD write protect: on RD4

/*Digital inputs*/
volatile bit GPS_pc@PORTB.1;				// GPS power check: on RB1
volatile bit SD_cd@PORTB.3;					// SD card defect: on RB3
volatile bit GPS_tm@PORTB.4;				// GPS time mark: on RB4


/****************************************************************************/
/******************** Microcontroller and I/O routines **********************/
/****************************************************************************/


/*initializes AD conversion, bits, and ports*/
void initialize_microcontroller(void)
{	
	// bit 7-6 set as 00 - unimplemented
	// bit 5-2 set as 0000 - initialize Analog Channel at AN0
	// bit 1 set as 0 - initialize A/D conversion status bit as idle
	// bit 0 set as 1 - enable A/D converter module
	adcon0 = 0b00000001;
	
	// bit 7-6 set as 00 - unimplemented
	// bit 5-4 set as 00 - want Vdd and Vss as Voltage reference
	// bit 3-0 set as 0110 - set only AN0 - AN8 as analog (9 analog inputs)
	// though we do not use AN5, AN6, and AN7
	adcon1 = 0b00000110;
	
	// bit 7 set as 0 - left justify the result
	// bit 6 set as 0 - unimplemented
	// bit 5-3 set as 010 - select time as 4 TAD (default)
	// bit 2-0 set as 010 - set clock as FOSC/32 (default)
	adcon2 = 0b00010010;

	// RA7 - unused
	// RA6 - unused
	// RA5 - analog input: temperature sensor
	// RA4 - unused
	// RA3 - analog input: driving wheel current
	// RA2 - analog input: ultra-capacitor current
	// RA1 - analog input: starter motor current
	// RA0 - analog input: ultra-capacitor voltage
	// make them all inputs!
	trisa = 0b11111111;
	
	// port b has 4 inputs (analog/digital)
	// RB0 is not used
	// RB5, RB6, and RB7 are used for the programmer
	//
	trisb.4 = 1; // RB4 - digital input: GPS timemark
	trisb.3 = 1; // RB3 - digital input: SD card defect
	trisb.2 = 1; // RB2 - analog input: auxillary input
	trisb.1 = 1; // RB1 - digital input: GPS power check
	
	// RC2 is not used
	trisc.7 = 1; // RC7 - digital input: UART receive (for GPS)
	trisc.6 = 0; // RC6 - digital output: UART transmit (for GPS)
	trisc.5 = 0; // RC5 - digital output: SPI out (for LCD & SD)
	trisc.4 = 1; // RC4 - digital input: SPI in (for LCD & SD)
	trisc.3 = 0; // RC3 - digital output: SPI clock (for LCD & SD)
	trisc.1 = 0; // RC1 - digital output: SD card chip select
	trisc.0 = 0; // RC0 - digital output: LCD chip select
	
	// RD7 - unused
	// RD6 - unused
	// RD5 - unused
	// RD4 - digital output: SD write protect
	// RD3 - digital output: auxillary output
	// RD2 - digital output: glow plug
	// RD1 - digital output: starter motor
	// RD0 - digital output: fuel valve on/off
	trisd = 0b00000000;
	
	// enable global interrupts
	intcon = 0b11000000; //ensure that GIE and PEIE are set
	rcie = 1;
	
	LCD_cs = 1;		// not select LCD
	SD_cs = 1;		// not select SD
	SD_wp = 0;		// turn off write protect for SD card
	
	// initialize all outputs to 0
	starter = 0;
	on_off = 0;
	glow_plug = 0;
	aux_out = 0;
	
	return;
}

/*performs A/D conversion on analog input - ultra-capacitor voltage*/
void ucap_v_AD_conv(void)
{
	// set input as AN0 (CHS3:CHS0 = 0000)
	chs3 = 0;
	chs2 = 0;
	chs1 = 0;
	chs0 = 0;
	
	// set GO/DONE bit to start conversion
	go_done = 1;
	
	while(go_done == 1)
	{
		// wait for A/D converter to stop writing to ADRESH and ADRESL
	}
	ucap_v = adresh;
	return;
}

/*performs A/D conversion for analog input - starter motor current*/
void starter_motor_current_AD_conv(void)
{
	// set input as AN1 (CHS3:CHS0 = 0001)
	chs3 = 0;
	chs2 = 0;
	chs1 = 0;
	chs0 = 1;
	
	// set GO/DONE bit to start conversion
	go_done = 1;
		
	while(go_done == 1)
	{
		// wait for A/D converter to stop writing to ADRESH and ADRESL
	}
	starter_motor_current = adresh;
	return;
}

/*performs A/D conversion for analog input - ultra-capacitor current*/
void ucap_stack_current_AD_conv(void)
{
	// set input as AN2 (CHS3:CHS0 = 0010)
	chs3 = 0;
	chs2 = 0;
	chs1 = 1;
	chs0 = 0;
	
	// set GO/DONE bit to start conversion
	go_done = 1;
	
	while(go_done == 1)
	{
		// wait for A/D converter to stop writing to ADRESH and ADRESL
	}
	ucap_stack_current = adresh;
	return;
}

/*performs A/D conversion for analog input - driving wheel current*/
void driving_motor_current_AD_conv(void)
{
	// set input as AN3 (CHS3:CHS0 = 0011)
	chs3 = 0;
	chs2 = 0;
	chs1 = 1;
	chs0 = 1;
	
	// set GO/DONE bit to start conversion
	go_done = 1;
	
	while(go_done == 1)
	{
		// wait for A/D converter to stop writing to ADRESH and ADRESL
	}
	driving_motor_current = adresh;
	return;
}

/*performs A/D conversion for analog input - temperature sensor*/
void temp_sensor_AD_conv(void)
{
	// set input as AN4 (CHS3:CHS0 = 0100)
	chs3 = 0;
	chs2 = 1;
	chs1 = 0;
	chs0 = 0;
	
	// set GO/DONE bit to start conversion
	go_done = 1;
	
	while(go_done == 1)
	{
		// wait for A/D converter to stop writing to ADRESH and ADRESL
	}
	temp_sensor = adresh;
	return;
}

/*checks temperature to determine whether or not to turn on the glow plug*/
void temp_check(void)
{
	temp_sensor_AD_conv(); // first get temp_sensor value
	while (temp_sensor <= 22)
	{
		on_off = 0;
		starter = 0;
		glow_plug = 1;
		temp_sensor_AD_conv();
		ucap_v_AD_conv();
		starter_motor_current_AD_conv();
		ucap_stack_current_AD_conv();
		driving_motor_current_AD_conv();
		temp_sensor_AD_conv();
	}
	glow_plug = 0;
	return;
}


/****************************************************************************/
/***************************** LCD Routines *********************************/
/****************************************************************************/


/*initialize SPI LCD*/
void LCD_init(void)
{
	sspen = 0;		// first disable SPI,
	
	// then configure SPI
	trisc.0 = 0;
	trisc.3 = 0;
	trisc.5 = 0;
	sspcon1 = 0x12;
	sspstat = 0;
	
	// then re-enable SPI
	sspen = 1;
	return;
}

/*send 'x' byte of the tx_packet[] to the serial port*/
void send_packet(unsigned int x)
{
	unsigned int ix;
	LCD_cs = 0;					// select LCD
	for (ix = 0; ix < x; ix++)
	{
		sspbuf = tx_packet[ix];
		while(!bf);
		delay_us(5);			// reduce effective clock rate
	}
	LCD_cs = 1;					// unselect LCD
	return;
}

/*set LCD cursor (position)*/
/*@param x: send 8 bit number that tells the position of LCD display*/
void LCD_cursor(unsigned int x)
{
	tx_packet[0] = 0xFE;
	tx_packet[1] = 0x45;
	tx_packet[2] = x;
	send_packet(3);
	delay_ms(10);
	return;
}

/*clear one line of display*/
/*@param x: the line in which we want to clear*/
void clear_line(unsigned int x)
{
	unsigned int ij;
	for (ij = 0; ij < x; ij++)
	{
		tx_packet[ij] = 0x20;
	}
	send_packet(x);
	return;
}

/*clear the LCD display*/
void LCD_clear(void)
{
	tx_packet[0] = 0xFE;
	tx_packet[1] = 0x51;
	send_packet(2);
	delay_ms(10);
	return;
}

/*outputs ASCII character val to the LCD*/
void LCD_char(unsigned char val)
{
	tx_packet[0] = val;
	send_packet(1);
	delay_ms(10);
	return;
}

/*outputs decimal data to the LCD*/
void LCD_dec(char data)
{
	LCD_char(((data /100) & 255) + 0x30);
	data = data % 100;
	LCD_char(((data / 10) & 255) + 0x30);
	LCD_char(((data % 10) & 255) + 0x30);
	delay_ms(10);
	return;
}

void LCD_ucap(char data)
{
	unsigned short data_conv1;
	data_conv1 = (data*2)/5;
	data_conv1 = data_conv1 + 1;
	unsigned char data_conv2 = data_conv1;
	LCD_char(((data_conv2 / 10) & 255) + 0x30);	// display value in 10
	LCD_char(((data_conv2 % 10) & 255) + 0x30);	// display value in 1
	LCD_char(0x20);								// display a space
	LCD_char(0x56);								// display a V
	delay_ms(10);
	return;
}


/****************************************************************************/
/***************************** GPS Routines *********************************/
/****************************************************************************/


/*initialize the clock and UART*/
void GPS_init(void)
{
	// set Fosc as 8 MHz
	osctune = 0b10000000; // use 31.25 kHz device clock
	osccon = 0b11111111;
	
	// set UART registers
	txsta = 0b00100100;
	rcsta = 0b10110000;
	trisc.7 = 1;
	trisc.6 = 1;
	baudcon |= 0b00001000;
	
	// SPBRGH:SPBRG = 416 corresponds to baud rate of 4.8 k
	spbrg = 416 & 0xFF;
	spbrgh = (416 >> 8) & 0xFF;
	
	return;
}

/*transmit a command to the GPS*/
void GPS_transmit(unsigned char gps_command)
{
	while(!txif);
	txreg = gps_command;
	return;
}

/*send command to GPS to tell speed of vehicle*/
/*only need to call once, then sends data once a second*/
void tell_speed(void)
{
	unsigned char cmd[22];
	
	// want cmd to be ASCII '$PSRF103,05,00,01,01*25'
	cmd[22] = '$';
	cmd[21] = 'P';
	cmd[20] = 'S';
	cmd[19] = 'R';
	cmd[18] = 'F';
	cmd[17] = '1';
	cmd[16] = '0';
	cmd[15] = '3';
	cmd[14] = ',';
	cmd[13] = '0';
	cmd[12] = '5';
	cmd[11] = ',';
	cmd[10] = '0';
	cmd[9] = '0';
	cmd[8] = ',';
	cmd[7] = '0';
	cmd[6] = '1';
	cmd[5] = ',';
	cmd[4] = '0';
	cmd[3] = '1';
	cmd[2] = '*';
	cmd[1] = '2';
	cmd[0] = '5';
	
	int i;
	for (i=22;i>=0;i--)
	{
		GPS_transmit(cmd[i]);
	}
	return;
}

/*use this interrupt to receive data*/
void interrupt(void)
{
	// interrupt routine for UART receive
	char dat;
	dat = rcreg;
	
	// if first command char is received
	if (dat != '$' && k == 34)
	{
		// if first char isn't $, set k = 34
		k = 34;
	}
	else if (dat == '$' && k == 34)
	{
		// if first char is $, subtract one from k
		// and see the second char
		GPS_receive[k] = dat;
		k = k - 1;
	}
	else if (dat != 'G' && k == 33)
	{
		// if second char isn't G, reset k to 34
		k = 34;
	}
	else if (dat == 'G' && k == 33)
	{
		// if second char is G, subtract one from k
		// and see the third char
		GPS_receive[k] = dat;
		k = k - 1;
	}
	else if (dat != 'P' && k == 32)
	{
		// if third char isn't P, reset k to 34
		k = 34;
	}
	else if (dat == 'P' && k == 32)
	{
		// if third char isn P, subtract one from k
		// and see the fourth char
		GPS_receive[k] = dat;
		k = k - 1;
	}
	else if (dat != 'V' && k == 31)
	{
		// if fourth char isn't V, reset k to 34
		k = 34;
	}
	else if (dat == 'V' && k == 31)
	{
		// if fourth char is V, subtract one from k
		// and see the fifth char
		GPS_receive[k] = dat;
		k = k - 1;
	}
	else if (dat != 'T' && k == 30)
	{
		// if fifth char isn't T, reset k to 34
		k = 34;
	}
	else if (dat == 'T' && k == 30)
	{
		// if fifth char is T, subtract one from k
		// and see the sixth char
		GPS_receive[k] = dat;
		k = k - 1;
	}
	else if (dat != 'G' && k == 29)
	{
		// if sixth char isn't G, reset k to 34
		k = 34;
	}
	else if (dat == 'G' && k == 29)
	{
		// if sixth char is G, subtract one from k
		// this indicates that $GPVTG is received as the first six chars
		// so that means we've received the velocity message
		GPS_receive[k] = dat;
		k = k - 1;
	}
	// wait for k to reach 9, that's when speed starts
	// between k is 9 to 7, the three ASCII chars shows the speed
	else if (k == 9)
	{
		GPS_receive[k] = dat;
		k = k - 1;
	}
	else if (k == 8)
	{
		GPS_receive[k] = dat;
		k = k - 1;
	}
	else if (k == 7)
	{
		GPS_receive[k] = dat;
		k = k - 1;
	}
	else if (k == 0)
	{
		// once we've reached k = 0, reset k to 34
		// and start over in looking for $
		k = 34;
	}
	else
	{
		k = k - 1;
	}
}


/****************************************************************************/
/*************************** SD Card Routines *******************************/
/****************************************************************************/


/*SD card command function*/
void sd_command(unsigned char command[], unsigned char response[])
{
    int i, limit; 
    if (command[0] == 0x40 || command[0] == 0x58 || command[0] == 0x51)
		limit = 1;
    if (command[0] == 0x48 || command[0] == 0x7A)
		limit = 5;
       
	sspbuf=0xFF;
	while(!sspif);
	sspif=0;
	for (i=0;i<6;i++)
	{ 
		sspbuf=command[i];
		while(!sspif){};
		sspif=0;
	}

	do
	{
		sspbuf=0xFF;
		while(!sspif&&!bf);
		sspif=0;
		response[0] = sspbuf;
	} 
	while (response[0]==0xFF);

	if (response[0] & 0x04) // if illegal command 
	{
		limit=1;
	}
	
	for (i=1;i<limit;i++)
	{
		while(!bf&&!sspif) {};
        response[i] = sspbuf;
        sspif=0;
	}
    return;
}

/*initialize SD card*/
int sd_init(void)
{
	SD_cs = 0;		// select SD
	
	// wcol=0, sspov=0, sspen=1, ckp=0 for idle clock is low
	// spi master mode with clock=Fosc/64
	sspcon1 = 0b00100010;
	
	// sample at middle of input, transition on rising clock
	sspstat &= 0b00111111;
	sspie = 1;
	
	unsigned char command[]= {0x40,0x0,0x0,0x0,0x0,0x95}; // CMD0
	unsigned char response[5] = {0x00};
	
	// waiting for SD card to reach operating point
	int i;
	for (i=0;i<20;i++)
	{
		sspbuf=0xFF;
		while(!sspif){};
		sspif=0;
	}

	sd_command(command,response); //CMD0
	command[0]=0x48; 
	command[1]=0x0;
	command[2]= 0x0;
	command[3]= 0x1;
	command[4]= 0xAA; 
	command[5]=0x95; // CMD8
	sd_command(command,response);
	
	do
	{
		command[0]=0x77; 
		command[1]=0x0;
		command[2]= 0x0;
		command[3]= 0x0;
		command[4]= 0x0; 
		command[5]=0x95; // CMD55
		sd_command(command,response);
		command[0]=0x69; 
		command[1]=0x0;
		command[2]= 0x0;
		command[3]= 0x0;
		command[4]= 0x0; 
		command[5]=0x95; // ACMD41
		sd_command(command,response);
	}
	while (response[0]!= 0x00);
	
	command[0]=0x50; 
	command[1]=0x0;
	command[2]= 0x0;
	command[3]= 0x02;
	command[4]= 0x0; 
	command[5]=0x95; // CMD16 setting 512b block length
	sd_command(command,response);
	
	SD_cs = 1;
	return 1;
}


/****************************************************************************/
/****************************************************************************/
/***************************** Main CODE!!! *********************************/
/****************************************************************************/
/****************************************************************************/


void main()
{
	// initialize the microcontroller and SD card
	initialize_microcontroller();
	
	// check temperature
	temp_check();
	
	while(1)
	{
		// calls functions to do all A/D conversion routines
		ucap_v_AD_conv();
		starter_motor_current_AD_conv();
		ucap_stack_current_AD_conv();
		driving_motor_current_AD_conv();
		temp_sensor_AD_conv();
		
		// a ucap_v of greater than 154 (corresponding to 62 V) should send nothing
		// 49 corresponds to a voltage of 20
		if (ucap_v > 154)
		{
			// leave the generator alone
			starter = 0;
			on_off = 0;
		}
		
		else if (ucap_v <= 154)
		{
			// start the starter motor to initialize generator
			starter = 1;
			on_off = 1;
			delay_ms(50);
			starter_motor_current_AD_conv();
			
			// this while loop waits for starter motor current to go down
			while (starter_motor_current > 150)
			{
				// keep polling for starter_motor_current
				// once the current drops, will jump out of loop
				// call all A/D conversion functions
				ucap_v_AD_conv();
				starter_motor_current_AD_conv();
				ucap_stack_current_AD_conv();
				driving_motor_current_AD_conv();
				temp_sensor_AD_conv();
			}
			starter = 0; // turn off starter once current goes down
			
			// 198 corresponds to a stack voltage of 80 V
			// we would like about 86 V across the stack before switching off though
			// 212 corresponds to a voltage of 86 V
			// jumps out of loop once the ultra-cap voltage gets too high
			// 62 corresponds to 25 V
			while (ucap_v < 212)
			{
				// call all A/D conversion functions
				ucap_v_AD_conv();
				starter_motor_current_AD_conv();
				ucap_stack_current_AD_conv();
				driving_motor_current_AD_conv();
				temp_sensor_AD_conv();
			}
			on_off = 0;
		}
		else
		{
			// do nothing
		}
	}
}