/*****************************
    ECE 476 FINAL PROJECT
    The Autonomous Peanut Bot
    Israni Angie (ai45)
    Spiel Seth (sbs43)
    Chawda Hemanshu (hkc2)
*****************************/ 

//include files             
#include <mega32.h>         //include mega32 hardware specific profile
#include <stdio.h>          //include c-standard i/o functions
#include <string.h>         //include c-string manipulation functions      
#include <delay.h>          //include delay functions
#include <stdlib.h>         //include atoi function
#include "angle_array.h"    //include array usedd for triangulation

#define TRUE 1
#define FALSE 0
#define array_size 1000
#define limit 900 //1000 - 10%*1000
#define buffer_length 16
#define DEBUG TRUE
#define ms_delay 20
#define HyperTerm

// *********************************************************
// VARIABLE DECLARATIONS 
// *********************************************************

//time variables
signed int timeUpdate, timePWM;	

//RXC ISR variables															
unsigned char r_index;                  	//current string index
unsigned char r_buffer[buffer_length];	    //input string 
unsigned char r_ready;                  	//flag for receive done
unsigned char r_char;                     	//current character   

//TX empth ISR variables								
unsigned char t_index;                  	//current string index
unsigned char t_buffer[buffer_length];	    //output string 
unsigned char t_ready;                  	//flag for transmit done
unsigned char t_char;                   	//current character  

//rotate & move car controls 
unsigned char servo1, servo2, servo3;   	//time to set PWM duty cycle
unsigned int rotateTime, moveTime, waitTime;//set up counters for each movement
int scaledRotate, scaledMove;           	//find how much to relate angle/distance to time
unsigned char moveForeward;
eeprom int romScaledRotate = 6;
eeprom int romScaledMove   = 35;   

                                        
unsigned char autoTurnCount;            //count to see how much car has rotated in search mode   
//record of the last several microphone readings
unsigned char ADCbuffer[array_size], sampleNum;
unsigned int micIndex1[4], micIndex2[4], micIndex3[4];
//record of the last several car movements, used for determining when the pulse generator is found
signed int distance_record[buffer_length], angle_record[buffer_length], sampleAvgMic1, sampleAvgMic2, sampleAvgMic3;

//servo control variables
enum ServoState{Idle = 0, Waiting, Rotating, Foreward} servoState;
//valid states the user can input
enum UserState{UsrControl = 0, UsrListen, UsrEEPROMUpdate};
//autonomous control variables
enum AutoState{Controlled = 0, Stabilize, Listen, Locate, Move, Done} autoState;                
                                        //0 if not in autonomous mode
                                        //1 if waiting for silence
                                        //2 if listening for a pulse
                                        //3 if done listening and anaylizing data to find azimuth
                                        //4 if analysis done, rotate then move
                                        //5 if there is negligible change in last several movements

//timeout values for each task
flash unsigned char tUpdate = 50; 
flash unsigned char tPWM = 40; 

// *********************************************************
// FUNCTION DECLARATIONS 
// *********************************************************

//the  subroutines
void error();                               //display error status on led
void done();                                //display done status on the led
void initialize();                          //set up MCU 
void updateSystem();                        //check system 
void recordMove(signed int, signed int);   //records the angle, distance of the current move
void rotateMoveCar(int angle, int distance);//set up rotation - call first when moving
void moveCar(int distance);                 //set up move
void updateServos();		                //update the servo state machine 
void resetServors();                        //reset the servo states
void autoControl();                         //set up autonomous mode                            
void enable_receive_interrupt();            //set up receive interrupt 
void enable_transmit_interrupt();           //set up transmit interrupt
void transmit_strf(unsigned char flash *);  //transmit a string from flash over the serial (interrupt driven)
void parseCommandASCII(unsigned char[], unsigned char*, signed int*, signed int*, unsigned char);

//macro for absolute value   
inline int intAbs(int a)
{
    ((a<0)? (a=(-a)):(a=(a)));
    return a; 
}

inline unsigned char charAbs(signed char a)
{
    ((a<0)? (a=(-a)):(a=(a)));
    return a; 
}

void error()
{
    #ifdef HyperTerm
        transmit_strf("Error AutoState = ");
        delay_ms(ms_delay);
        sprintf(t_buffer,"%i",autoState);
        enable_transmit_interrupt();
        transmit_strf(", ServoState = ");
        delay_ms(ms_delay);
        sprintf(t_buffer,"%i",servoState);
        enable_transmit_interrupt();
        delay_ms(ms_delay);
        transmit_strf("\r\n");
    #endif
    while(TRUE)
    {
        PORTD.7 = !PORTD.7;
        delay_ms(100);
    }
}

void done() {
	transmit_strf("PeanutBot Done!!!\r\n");
    PORTD.7 = !PORTD.7;
}