/*
* Robotic Car Traction Control
* Andrew Giftt
* Rob Miceli
*/

#include <avr/io.h>
#include <util/delay.h>
#include <avr/pgmspace.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <inttypes.h>
#include <avr/interrupt.h>
#include "lcd_lib.h"

//set up the debugging utility ASSERT
#define __ASSERT_USE_STDERR
#include <assert.h>

// serial communication library
#include "uart.h"

//Definitions
#define slipThresh 1.5
#define Vthresh 1.3
#define deBtol 10
#define printTime 100
#define changeThresh 3
#define Athresh 1.15
#define delPWM 1
#define VDthresh 1.03

//LCD constants
const int8_t LCD_L0[] PROGMEM = "w1:     w3:    \0";
const int8_t LCD_L1[] PROGMEM = "w2:     w4:    \0";


//declarations
int8_t lcd_buffer[17];	// LCD display buffer
uint16_t count;			// a number to display on the LCD  
uint8_t anipos, dir;	// move a character around  


//Tdiff stores time in phase
//Tct is increment 5kHz
volatile uint16_t Tdiff1w1, Tdiff2w1, Tavw1;
volatile uint16_t T2w1, T3w1, Tct1;	

volatile uint16_t Tdiff1w2, Tdiff2w2, Tavw2;
volatile uint16_t T2w2, T3w2, Tct2;	

volatile uint16_t Tdiff1w3, Tdiff2w3, Tavw3;
volatile uint16_t T2w3, T3w3, Tct3;	
	
volatile uint16_t Tdiff1w4, Tdiff2w4, Tavw4;
volatile uint16_t T2w4, T3w4,Tct4;	

uint16_t Tdes;

int v1, v2, v3, v4;


volatile unsigned char timeFlag, timeDisp, finDisp, lcdDisp;
unsigned char deBw1, deBw2, deBw3, deBw4;
unsigned char lastRead, newRead, lastGood;

unsigned char speedUpw1w2;
unsigned char speedUpw3w4;

unsigned char slowDnw1w2;
unsigned char slowDnw3w4;
unsigned char changeFlag;

// UART file descriptor
// putchar and getchar are in uart.c
FILE uart_str = FDEV_SETUP_STREAM(uart_putchar, uart_getchar, _FDEV_SETUP_RW);


//timer 0 compare ISR
ISR (TIMER0_COMPA_vect){

	Tct1++; Tct2++, Tct3++; Tct4++;
	timeFlag = 1;
	if(timeDisp > 0){ timeDisp--; }

}

void checkWheels(void){
	//increase timers
	newRead = PINB;

	
	//If last value equals new values increase deB counter
	//if it is different, clear it
	if((newRead & 0x01) == (lastRead & 0x01)){
		deBw1++;
	}else{ deBw1 = 0; }
	if((newRead & 0x02) == (lastRead & 0x02)){
		deBw2++;
	}else{ deBw2 = 0; }
	if((newRead & 0x04) == (lastRead & 0x04)){
		deBw3++;
	}else{ deBw3 = 0; }
	if((newRead & 0x08) == (lastRead & 0x08)){
		deBw4++;
	}else{ deBw4 = 0; }
	
	//move last Tdiff down, update new Tdiff
	if((deBw1 > deBtol) && ((newRead & 0x01) != (lastGood & 0x01)) && (Tct1>220)){
		Tdiff1w1 = Tdiff2w1;
		Tdiff2w1 = Tct1;
		Tct1 &= 0x00;
		lastGood = lastGood ^ 0x01;
	}
	if((deBw2 > deBtol) && ((newRead & 0x02) != (lastGood & 0x02)) && (Tct2>220)){
		Tdiff1w2 = Tdiff2w2;
		Tdiff2w2 = Tct2;
		Tct2 &= 0x00;
		lastGood = lastGood ^ 0x02;
	}
	if((deBw3 > deBtol) && ((newRead & 0x04) != (lastGood & 0x04)) && (Tct3>220)){
		Tdiff1w3 = Tdiff2w3;
		Tdiff2w3 = Tct3;
		Tct3 &= 0x00;
		lastGood = lastGood ^ 0x04;
	}
	if((deBw4 > deBtol) && ((newRead & 0x08) != (lastGood & 0x08)) && (Tct4>220)){
		Tdiff1w4 = Tdiff2w4;
		Tdiff2w4 = Tct4;
		Tct4 &= 0x00;
		lastGood = lastGood ^ 0x08;
	}	//prevents overflow by assigning wheel speed to slowest speed (greatest + value)
	if(Tct1 == 32767){
		Tdiff1w1 = Tdiff2w1;
		Tdiff2w1 = Tct1;
		Tct1 &= 0x00;

	}
	if(Tct2 == 32767){
		Tdiff1w2 = Tdiff2w2;
		Tdiff2w2 = Tct2;
		Tct2 &= 0x00;
	}
	if(Tct3 == 32767){
		Tdiff1w3 = Tdiff2w3;
		Tdiff2w3 = Tct3;
		Tct3 &= 0x00;

	}
	if(Tct4 == 32767){
		Tdiff1w4 = Tdiff2w4;
		Tdiff2w4 = Tct4;
		Tct4 &= 0x00;
	}
	
		
	lastRead = newRead;


}


char getCarAcc(void){
	char AccIn;
   //get the sample  
   AccIn = 0;//ADCH;
   //start another conversion
   ADCSRA |= (1<<ADSC) ; 
   //results to hyperterm

//	fprintf(stdout,"*** IN ACCEL%d\n\r",AccIn);
   return AccIn;

}

int calcAccel(int t1, int t2){
	return ((2*(t1-t2)/t1)/t2)/(t1+t2);
}

void checkSlip(){

	//Find min and max times that correspond to min/max velocity
	//to remove wheel 1 switch these declarations to w2, (for vmin, vmax acc min, acc max)
	int Tmin = Tdiff2w1;
	int Tmax = Tdiff2w1;
	if(Tdiff2w2 < Tmin){ Tmin = Tdiff2w2; }
	else if(Tdiff2w2 > Tmax){ Tmax = Tdiff2w2; }
	if(Tdiff2w3 < Tmin){ Tmin = Tdiff2w3; }
	else if(Tdiff2w3 > Tmax){ Tmax = Tdiff2w3; }
	if(Tdiff2w4 < Tmin){ Tmin = Tdiff2w4; }
	else if(Tdiff2w4 > Tmax){ Tmax = Tdiff2w4; }
	
	if(Tct1>Tmax){ Tmax = Tct1; }
	if(Tct2>Tmax){ Tmax = Tct2; }
	if(Tct3>Tmax){ Tmax = Tct3; }
	if(Tct4>Tmax){ Tmax = Tct4; }
	
	//find min,max accel
	int Amin = calcAccel(Tdiff1w1,Tdiff2w1);
	int Amax = Amin;
	int A2 = calcAccel(Tdiff1w2,Tdiff2w2);
	int A3 = calcAccel(Tdiff1w3,Tdiff2w3);
	int A4 = calcAccel(Tdiff1w4,Tdiff2w4);

	
	if(A2 > Amax){ Amax = A2; }
	else if(A2 < Amin){Amin = A2; }
    if(A3 > Amax){ Amax = A3; }
	else if(A3 < Amin){Amin = A3; }
	if(A4 > Amax){ Amax = A4; }
	else if(A4 < Amin){Amin = A4; }



	
	//***************** Wheel 1*************************
	//if wheel speed is faster than desired or slowest wheel, slow down
	if((Tdiff2w1 < Tmax/Vthresh) || (Tdiff2w1 < Tdes/VDthresh)){
		//slow down
		slowDnw1w2 += 0x10;
		speedUpw1w2 = speedUpw1w2 & 0x0F;
	}	//if wheel speed is slower than desired, speed up
	else if((Tdiff2w1 >= Tmax/Vthresh)){
		//speed up
		speedUpw1w2 += 0x10;
		slowDnw1w2 = slowDnw1w2 & 0x0F;
	}
	else{
		//clear all pointers
		speedUpw1w2 = speedUpw1w2 & 0x0F;
		slowDnw1w2 = slowDnw1w2 & 0x0F;
	}
	
	//***************** Wheel 2*************************
	if((Tdiff2w2 < Tmax/Vthresh) || (Tdiff2w2 < Tdes/VDthresh)){
		//slow down
		slowDnw1w2 += 0x01;
		speedUpw1w2 = speedUpw1w2 & 0xF0;
	}
	else if((Tdiff2w2 >= Tmax/Vthresh)){
	    //speed up
		speedUpw1w2 += 0x01;
		slowDnw1w2 = slowDnw1w2 & 0xF0;
		
	}
	else{
		//clear all pointers
		speedUpw1w2 = speedUpw1w2 & 0xF0;
		slowDnw1w2 = slowDnw1w2 & 0xF0;
	}	
	
	//***************** Wheel 3*************************
	if((Tdiff2w3 < Tmax/Vthresh) || (Tdiff2w3 < Tdes/VDthresh)){
		//slow down
		slowDnw3w4 += 0x10;
		speedUpw3w4 = speedUpw3w4 & 0x0F;
	}
	else if((Tdiff2w3 >= Tmax/Vthresh)){
		//speed up
		speedUpw3w4 += 0x10;
		slowDnw3w4 = slowDnw3w4 & 0x0F;
		
	}
	else{
		//clear all pointers
		speedUpw3w4 = speedUpw3w4 & 0x0F;
		slowDnw3w4 = slowDnw3w4 & 0x0F;
	}
	
	//***************** Wheel 4*************************
	if((Tdiff2w4 < Tmax/Vthresh) || (Tdiff2w4 < Tdes/VDthresh)){
		//slow down
		slowDnw3w4 += 0x01;
		speedUpw3w4 = speedUpw3w4 & 0xF0;
	}
	else if((Tdiff2w4 >= Tmax/Vthresh)){
		//speed up
		speedUpw3w4 += 0x01;
		slowDnw3w4 = slowDnw3w4 & 0xF0;
		
	}
	else{
		//clear all pointers
		speedUpw3w4 = speedUpw3w4 & 0xF0;
		slowDnw3w4 = slowDnw3w4 & 0xF0;
	}		



/************************************************************/
	//check if speed change is consistent (debounce speed change) w1
	if((speedUpw1w2 >>4) >= changeThresh){
		speedUpw1w2 = speedUpw1w2 & 0x0F;
		changeFlag = changeFlag & 0xFC | 0x01;
		if (OCR1B>10) OCR1B = OCR1B - delPWM;
		else OCR1B =1;
	}
	else if((slowDnw1w2 >>4) >= changeThresh){
		slowDnw1w2 = slowDnw1w2 & 0x0F;
		changeFlag = changeFlag & 0xFC | 0x02;
		if(OCR1B<240) OCR1B = OCR1B + delPWM;
		else OCR1B = 240;
	}
	//check if speed change is consistent (debounce speed change) w2
	if((speedUpw1w2 <<4) >= changeThresh<<4){
		speedUpw1w2 = speedUpw1w2 & 0xF0;
		changeFlag = changeFlag & 0xF3 | 0x04;
		if (OCR1A>10) OCR1A = OCR1A - delPWM;
		else OCR1A =1;
	}
	else if((slowDnw1w2 <<4) >= changeThresh<<4){
		slowDnw1w2 = slowDnw1w2 & 0xF0;
		changeFlag = changeFlag & 0xF3 | 0x08;
		if(OCR1A<240) OCR1A = OCR1A + delPWM;
		else OCR1A = 240;

	}
	//check if speed change is consistent (debounce speed change) w3
	if((speedUpw3w4 >>4) >= changeThresh){
		speedUpw3w4 = speedUpw3w4 & 0x0F;
		changeFlag = changeFlag & 0xCF | 0x10;
		if (OCR2B>10) OCR2B = OCR2B - delPWM;
		else OCR2B =1;
	}
	else if((slowDnw3w4 >>4) >= changeThresh){
		slowDnw3w4 = slowDnw3w4 & 0x0F;
		changeFlag = changeFlag & 0xCF | 0x20;
		if(OCR2B<240) OCR2B = OCR2B + delPWM;
		else OCR2B = 240;
	}
	//check if speed change is consistent (debounce speed change) w4
	if((speedUpw3w4 <<4) >= changeThresh<<4){
		speedUpw3w4 = speedUpw3w4 & 0xF0;
		changeFlag = changeFlag & 0x3F | 0x40;
		if (OCR2A>10) OCR2A = OCR2A - delPWM;
		else OCR2A =1;
	}
	else if((slowDnw3w4 <<4) >= changeThresh<<4){
		slowDnw3w4 = slowDnw3w4 & 0xF0;
		changeFlag = changeFlag & 0x3F | 0x80;

		if(OCR2A<240) OCR2A = OCR2A + delPWM;
		else OCR2A = 240;
	}	
}
	
	
void printLCD(){
	CopyStringtoLCD(LCD_L0,0,0);
	CopyStringtoLCD(LCD_L1,0,1);
	LCDGotoXY(3,0);
	sprintf(lcd_buffer,"%-i",OCR1B);
	LCDstring(lcd_buffer,strlen(lcd_buffer));
	LCDGotoXY(11,0);
	sprintf(lcd_buffer,"%-i",Tdiff2w2);
	LCDstring(lcd_buffer,strlen(lcd_buffer));
	LCDGotoXY(3,1);
	sprintf(lcd_buffer,"%-i",Tdiff2w3);
	LCDstring(lcd_buffer,strlen(lcd_buffer));
	LCDGotoXY(11,1);
	sprintf(lcd_buffer,"%-i",OCR2A);
	LCDstring(lcd_buffer,strlen(lcd_buffer));
}	

//calculates velocity in RPM
int velTorpm(int Tdiff){
	return (int)(18750/Tdiff)*2; //625*30 = 18650
}
int accTorpm(int Tdiff2,int Tdiff1){
	return (int)(Tdiff2-Tdiff1)/50;
}

	
void initStatus(){
	fprintf(stdout,"|  wheel 1 ||  wheel 2 ||  wheel 3 || wheel 4  ||Tmin|Tmax|Tdes|\n\r");
	fprintf(stdout,"|vel|Tdif|S||vel|Tdif|S||vel|Tdif|S||vel|Tdif|S||              \n\r");
}

void updateStatus(){
	v1 = 2*velTorpm(Tdiff2w1+Tdiff1w1);
	v2 =2* velTorpm(Tdiff2w2+Tdiff1w2);
	v3 = 2* velTorpm(Tdiff2w3+Tdiff1w3);
	v4 = 2* velTorpm(Tdiff2w4+Tdiff1w4);

	char s1 = changeFlag>>0 & 0x03;
	char s2 = changeFlag>>2 & 0x03;
	char s3 = changeFlag>>4 & 0x03;
	char s4 = changeFlag>>6 & 0x03;
	int  des = velTorpm(Tdes);

	int Tmin = Tdiff2w1;
	int Tmax = Tdiff2w1;
	if(Tdiff2w2 < Tmin){ Tmin = Tdiff2w2; }
	else if(Tdiff2w2 > Tmax){ Tmax = Tdiff2w2; }
	if(Tdiff2w3 < Tmin){ Tmin = Tdiff2w3; }
	else if(Tdiff2w3 > Tmax){ Tmax = Tdiff2w3; }
	if(Tdiff2w4 < Tmin){ Tmin = Tdiff2w4; }
	else if(Tdiff2w4 > Tmax){ Tmax = Tdiff2w4; }
	
	if(Tct1>Tmax){ Tmax = Tct1; }
	if(Tct2>Tmax){ Tmax = Tct2; }
	if(Tct3>Tmax){ Tmax = Tct3; }
	if(Tct4>Tmax){ Tmax = Tct4; }




//	fprintf(stdout,"|%3d|%4d|%1d|%3d|%4d|%1d|%3d|%4d|%1d|%3d|%4d|%1d|%4d| \n\r",v1,Tdiff2w1,s1,v2,Tdiff2w2,s2,v3,Tdiff2w3,s3,v4,Tdiff2w4,s4,des);

}
	
	
int main(void){
	initialize();
	initStatus();
	init_lcd();
	while(1){
		if(timeFlag == 1){  checkWheels(); timeFlag = 0;}	//debounce wheel state, check for differences
		if(timeDisp == 0){ finDisp--; timeDisp = printTime; checkSlip();}
		if(finDisp == 0){ updateStatus(); /*printLCD();*/ finDisp = 10; }	//~1/10 sec
	}
	
}

void init_lcd(void) {
	LCDinit();	//initialize the display
	LCDcursorOFF();
	LCDclr();				//clear the display

	LCDGotoXY(0,0);
//	CopyStringtoLCD(LCD_initialize, 0, 0);
}

void initialize(void){
	//set up timer 0 for 1 mSec ticks
	TIMSK0 = 2;		//turn on timer 0 cmp match ISR 
	OCR0A = 50;  	//set the compare reg to 50 time ticks
	TCCR0A = 0b00000010; // turn on clear-on-match
	TCCR0B = 0b00000011;	// clock prescalar to 64

    // timer 1 runs at full rate
    TCCR1B = 0x1 ;  

    // turn on pwm 
    // turn on fast PWM and OC1A and OC1B output
	// at full clock rate, toggle OC1A and OC1B (pin D5 and D4) 
   	// 16 microsec per PWM cycle sample time
	TCCR1A = (1<<COM1A0) | (1<<COM1A1) |(1<<COM1B0) | (1<<COM1B1) | (1<<WGM10) | (0<<WGM11) ;	//wgm11 changed to 0 (see p 127 table 14-5)

    // timer 2 runs at full rate
    TCCR2B = 0x1 ;  

    // turn on pwm 
    // turn on fast PWM and OC2A OC2B output
	// at full clock rate, toggle OC2A and OCA2B (pin D7 and D6) 
   	// 16 microsec per PWM cycle sample time
	TCCR2A = (1<<COM2A0) | (1<<COM2A1) |(1<<COM2B0) | (1<<COM2B1) | (1<<WGM20) | (0<<WGM21) ; //wgm21 changed to 0 (see p 127 table 14-5)
	
	//input on pins  C 3..0
	DDRB = 0x00;	//input
	PORTB = 0xFF;	//pull ups enabled

   //output on pins  D 7..0
	PORTD = 0x00;
	DDRD = 0xFF;	//output


   //init the A to D converter 
   //channel zero/ left adj /EXTERNAL Aref
   //!!!CONNECT Aref jumper!!!!
   ADMUX = (1<<ADLAR) ;  
    
   //enable ADC and set prescaler to 1/128*16MHz=125,000
   //and clear interupt enable
   //and start a conversion
   ADCSRA = (1<<ADEN) | (1<<ADSC) + 7 ; 

	//enable the uart
	uart_init();
	stdout = stdin = stderr = &uart_str;
	fprintf(stdout,"\n\nStarting...\n\r");
	
	//initialize variables
	Tdiff1w1 = 0;
	Tdiff2w1 = 0;
	T2w1 = 0;
	T3w1 = 0;
	Tdiff1w2 = 0;
	Tdiff2w2 = 0;
	T2w2 = 0;
	T3w2 = 0;
	Tdiff1w3 = 0;
	Tdiff2w3 = 0;
	T2w3 = 0;
	T3w3 = 0;
	Tdiff1w4 = 0;
	Tdiff2w4 = 0;
	T2w4 = 0;
	T3w4 = 0;
	timeFlag = 0;
	timeDisp = printTime;		
	finDisp = 10;
	lcdDisp = 
	lastGood = 0x00;
	Tdes = 550;
	changeFlag = 0;

    OCR1A = 128;
	OCR1B = 128;
	OCR2A = 128;
	OCR2B = 128;

	sei();
}