#class auto
#memmap xmem

#use "gps.lib"

#define CINBUFSIZE 127
#define COUTBUFSIZE 127

int prepYaw();
int getYawRate();

void AccelInit();
int prepAccel();
void getAccel();

int prepOBD();
float getRPM();
float getSpeed();
int getThrot();
float hex2dec(char[]);

int prepGPS();
void getGPS();

int prepCam();
void camClick();

void bootSD();
void SDreset();

void giveTime();
void giveMTime();
void giveSTime();

void lightsOff();
void lightsOn();
void flashLED(int light);
void flashLED1();
void flashLED2();
void flashLED3();
void flashLED4();
void flashLED5();

void testLEDs();

struct
{
	float x;            // in units of (g)
   float y;
}Accelleration;
char capture_status;
unsigned long pulse_widthX;
unsigned long pulse_widthY;

int gps_x;
int gps_y;
unsigned long gps_time;
char GPSdata[100];
int GPSdataPointer;
//int GPScounter;
GPSPosition current_pos;


int resetShadow;

main() {

   /*Yaw Rate Specific*/
	int yawGTG;					//is yaw rate gyro good to go?
   int yawRate;				//value returned from yawRate grab

   /*Accel Specific*/
   int accelGTG;				//is accel good to go?
   								//Accel data is global so not initalized here

   int GPSGTG;					//is GPS good to go?
   								//GPS data is global so not initalized here

   int OBDGTG;					//is OBD goot to go?
   int vehRPM;					//vehicle RPM
   int vehSpeed;           //vehicle Speed
   int vehThrot;           //vehicle Throttal position

  //START OF CAMERA BULLSHIT
   int camGTG;					//is camera good to go?
  //END OF CAMERA BULLSHIT



	yawGTG=0;
   yawRate=0;
   accelGTG=0;
   GPSGTG=0;
   OBDGTG=0;
   vehRPM=0;
   vehSpeed=0;
   vehThrot=0;
   camGTG=0;

   printf("\nMain Function Starting");
   printf("\nChecking Devices...");


   OBDGTG = prepOBD();
   yawGTG = prepYaw();
   accelGTG = prepAccel();
   GPSGTG = prepGPS();
   camGTG = prepCam();

   flashLED1();
   flashLED2();
   flashLED3();
   flashLED4();
   flashLED5();


   if(accelGTG==0)
   	WrPortI(PEB3R, NULL, 0xFF);
   if(OBDGTG==0)
   	WrPortI(PEB4R, NULL, 0xFF);
   if(yawGTG==0)
   	WrPortI(PEB5R, NULL, 0xFF);
   if(camGTG==0)
   	WrPortI(PEB6R, NULL, 0xFF);
   if(GPSGTG==0)
		WrPortI(PEB7R, NULL, 0xFF);


   // Display boot report for all devices
   printf("\n\nBoot Report-\nYaw Rate Gyro:%d\nAccelerometer:%d\nOBD:%d\nGPS:%d\nCamera:%d\n\n\n",yawGTG,accelGTG,OBDGTG,GPSGTG,camGTG);
   giveTime();


  	while (1)
   {
      if(yawGTG)
      {
      	yawRate=getYawRate();
      	printf("Yaw- %d\n",yawRate);
      }
      if(accelGTG)
      {
      	WrPortI(PEB3R, NULL, 0xFF);
         getAccel();
   		printf("Accel- x:%f, y:%f\n",Accelleration.x,Accelleration.y);
      }
      if(OBDGTG)
      {
      	vehRPM=getRPM();
         printf("%d\n",vehRPM);
      }
      if(GPSGTG)
      {
      	getGPS();
         printf("Latitude: %d %f\n", current_pos.lat_degrees, current_pos.lat_minutes);
			printf("Longitude: %d %f\n", current_pos.lon_degrees, current_pos.lon_minutes);
      }
      if(camGTG)
      {
         prepCam();
      }
      printf("\nCycle Ended\n");
	}
}

/* BEGIN: YAW RATE GYRO FUNCTIONS */
int prepYaw(){

   int testValue;
   int testCounter;
   int passed;

   testCounter=0;
   passed=0;
	testValue = 3000;

   while(testCounter<15)
   {
      giveSTime();
	   WrPortI(PFDDR, NULL, 0xC1);
	   BitWrPortI(PFDR, &resetShadow, 1, 6);
	   getYawRate();                             // Initialize yaw rate gyro
	   getYawRate();                             // Clears bad data
	   testValue = getYawRate();                 // Test if yaw rate working properly
      if(testValue>=1500 && testValue<=2500)
      {
      	passed=1;
         break;
      }
      else
      	++testCounter;
   }
   if(passed)
   	return 1;
   else
   	return 0;
}

int getYawRate()
{
	int i;
	int DOUT[17];
	int decimalSum;

   DOUT[0]=0;

   //Turn on Yaw LED
   WrPortI(PEB5R, NULL, 0xFF);

   BitWrPortI(PFDR, PFDRShadow,  0, 7); 	//CS low
   BitWrPortI(PGDR, PGDRShadow,  1, 2);	//WRITE
   for(i=0;i<15;i++);

   BitWrPortI(PFDR, PFDRShadow,  0, 0);	//SCLK low
   BitWrPortI(PFDR, PFDRShadow,  1, 0);	//SCLK high
   DOUT[1]=BitRdPortI(PGDR, 3);				//DOUT low

	BitWrPortI(PGDR, PGDRShadow,  0, 2);	//LOW

   BitWrPortI(PFDR, PFDRShadow,  0, 0);	//SCLK low
   BitWrPortI(PFDR, PFDRShadow,  1, 0);	//SCLK high
   DOUT[2]=BitRdPortI(PGDR, 3);				//DOUT low

   BitWrPortI(PGDR, PGDRShadow,  0, 2);	//DC

   BitWrPortI(PFDR, PFDRShadow,  0, 0);	//SCLK low
   BitWrPortI(PFDR, PFDRShadow,  1, 0);	//SCLK high
   DOUT[3]=BitRdPortI(PGDR, 3);				//DOUT ADD1

   BitWrPortI(PGDR, PGDRShadow,  0, 2);	//DC

   BitWrPortI(PFDR, PFDRShadow,  0, 0);	//SCLK low
   BitWrPortI(PFDR, PFDRShadow,  1, 0);	//SCLK high
   DOUT[4]=BitRdPortI(PGDR, 3);				//DOUT ADD0

   BitWrPortI(PGDR, PGDRShadow,  0, 2); 	//ADD!

   BitWrPortI(PFDR, PFDRShadow,  0, 0);	//SCLK low
   BitWrPortI(PFDR, PFDRShadow,  1, 0);	//SCLK high
   DOUT[5]=BitRdPortI(PGDR, 3);				//DOUT DB11

   BitWrPortI(PGDR, PGDRShadow,  0, 2); 	//ADD0

   BitWrPortI(PFDR, PFDRShadow,  0, 0);	//SCLK low
   BitWrPortI(PFDR, PFDRShadow,  1, 0);	//SCLK high
   DOUT[6]=BitRdPortI(PGDR, 3);				//DOUT DB10

   BitWrPortI(PGDR, PGDRShadow,  1, 2); 	//HIGH

   BitWrPortI(PFDR, PFDRShadow,  0, 0);	//SCLK low
   BitWrPortI(PFDR, PFDRShadow,  1, 0);	//SCLK high
   DOUT[7]=BitRdPortI(PGDR, 3);				//DOUT DB9

   BitWrPortI(PGDR, PGDRShadow,  1, 2); 	//HIGH

   BitWrPortI(PFDR, PFDRShadow,  0, 0);	//SCLK low
   BitWrPortI(PFDR, PFDRShadow,  1, 0);	//SCLK high
   DOUT[8]=BitRdPortI(PGDR, 3);				//DOUT DB8

   BitWrPortI(PGDR, PGDRShadow,  0, 2); 	//DC

   BitWrPortI(PFDR, PFDRShadow,  0, 0);	//SCLK low
   BitWrPortI(PFDR, PFDRShadow,  1, 0);	//SCLK high
   DOUT[9]=BitRdPortI(PGDR, 3);				//DOUT DB7

   BitWrPortI(PGDR, PGDRShadow,  0, 2); 	//DC

   BitWrPortI(PFDR, PFDRShadow,  0, 0);	//SCLK low
   BitWrPortI(PFDR, PFDRShadow,  1, 0);	//SCLK high
   DOUT[10]=BitRdPortI(PGDR, 3);				//DOUT DB6

   BitWrPortI(PGDR, PGDRShadow,  0, 2); 	//LOW

   BitWrPortI(PFDR, PFDRShadow,  0, 0);	//SCLK low
   BitWrPortI(PFDR, PFDRShadow,  1, 0);	//SCLK high
   DOUT[11]=BitRdPortI(PGDR, 3);				//DOUT DB5

   BitWrPortI(PGDR, PGDRShadow,  1, 2); 	//CODING

   BitWrPortI(PFDR, PFDRShadow,  0, 0);	//SCLK low
   BitWrPortI(PFDR, PFDRShadow,  1, 0);	//SCLK high
   DOUT[12]=BitRdPortI(PGDR, 3);				//DOUT DB4

   BitWrPortI(PGDR, PGDRShadow,  0, 2); 	//DC

   BitWrPortI(PFDR, PFDRShadow,  0, 0);	//SCLK low
   BitWrPortI(PFDR, PFDRShadow,  1, 0);	//SCLK high
   DOUT[13]=BitRdPortI(PGDR, 3);				//DOUT DB3

   BitWrPortI(PGDR, PGDRShadow,  0, 2); 	//DC

   BitWrPortI(PFDR, PFDRShadow,  0, 0);	//SCLK low
   BitWrPortI(PFDR, PFDRShadow,  1, 0);	//SCLK high
   DOUT[14]=BitRdPortI(PGDR, 3);				//DOUT DB2

   BitWrPortI(PGDR, PGDRShadow,  0, 2); 	//DC

   BitWrPortI(PFDR, PFDRShadow,  0, 0);	//SCLK low
   BitWrPortI(PFDR, PFDRShadow,  1, 0);	//SCLK high
   DOUT[15]=BitRdPortI(PGDR, 3);				//DOUT DB1

   BitWrPortI(PGDR, PGDRShadow,  0, 2); 	//DC

   BitWrPortI(PFDR, PFDRShadow,  0, 0);	//SCLK low
   BitWrPortI(PFDR, PFDRShadow,  1, 0);	//SCLK high

   BitWrPortI(PFDR, PFDRShadow,  1, 7); 	//CS high

   decimalSum = DOUT[15];
   decimalSum += DOUT[14]*2;
   decimalSum += DOUT[13]*4;
   decimalSum += DOUT[12]*8;
   decimalSum += DOUT[11]*16;
   decimalSum += DOUT[10]*32;
   decimalSum += DOUT[9]*64;
   decimalSum += DOUT[8]*128;
   decimalSum += DOUT[7]*256;
   decimalSum += DOUT[6]*512;
   decimalSum += DOUT[5]*1024;
   decimalSum += DOUT[4]*2048;

   //Turn off Yaw LED
   WrPortI(PEB5R, NULL, 0x00);

   return decimalSum;
}
/* END: YAW RATE GYRO FUNCTIONS */





/* BEGIN: ACCELEROMETER FUNCTIONS */
void AccelInit()
{
	WrPortI(ICS1R, NULL, 0x88); 	//PF1 is pulse capture pin for channel 1 - X
   WrPortI(ICS2R, NULL, 0x99); 	//PF3 is pulse capture pin for channel 2 - Y
	WrPortI(TAT8R, NULL, 50 - 1); //(TA8 prescaler)
	WrPortI(ICCSR, NULL, 0x0c); 	//zero out counters
	WrPortI(ICCR, NULL, 0x00); 	//no interrupts

	//run counter start to stop
	//start is falling edge, stop is rising edge
	//latch counter on stop
	WrPortI(ICT1R, NULL, 0x59);
   WrPortI(ICT2R, NULL, 0x59);
	pulse_widthX = 0;
   pulse_widthY = 0;
   getAccel();
   giveTime();
   getAccel();
}

int prepAccel()
{
	int testCounter;
   int passed;

   testCounter=0;
   passed=0;

   while(testCounter<15)
   {
	   ++testCounter;
	   AccelInit();
	   getAccel();
	   if(Accelleration.x>=-10 && Accelleration.x<=10 && Accelleration.y>=-10 && Accelleration.y<=10)
	   {
	      passed=1;
	      break;
	   }
   }

   if(passed)
	   return 1;
	else
	   return 0;
}

void getAccel()
{
   WrPortI(PEB3R, NULL, 0xFF);
   //listen for capture states
   capture_status = RdPortI(ICCSR);
   if(capture_status & 0x10)
   {
      //channel 1 stop occured
      //read capture value, LSB first
      pulse_widthX += RdPortI(ICL1R);
      pulse_widthX += RdPortI(ICM1R)*256;

      Accelleration.x = (((pulse_widthX/29420.0)-0.5)/0.3);
      //printf("channel 1 accel: %.2f\n", Accelleration.x);
      pulse_widthX = 0;
      WrPortI(ICCSR, NULL, 0x04); //zero out counter
   }
   else if(capture_status & 0x04)
   {
      printf("Input capture counter rolled over.\n");
      pulse_widthX += 0x10000;
   }

   if(capture_status & 0x40)
   {
      //channel 2 stop occured
      //read capture value, LSB first
      pulse_widthY += RdPortI(ICL2R);
      pulse_widthY += RdPortI(ICM2R)*256;

      Accelleration.y = (((pulse_widthY/29420.0)-0.5)/0.3);
      //printf("channel 2 accel: %.2f\n", Accelleration.y);
      pulse_widthY = 0;
      WrPortI(ICCSR, NULL, 0x08); //zero out counter
   }
   else if(capture_status & 0x08)
   {
      printf("Input capture counter rolled over.\n");
      pulse_widthY += 0x10000;
   }
   WrPortI(PEB3R, NULL, 0x00);
}
/* END: ACCELEROMETER FUNCTIONS */


/* BEGIN: OBD FUNCTIONS */
int prepOBD()
{
   int fetchedRPM;
   int fetchedSpeed;
   int fetchedThrot;

   int testCounter;
   int passed;

   fetchedRPM=0;
   fetchedSpeed=0;
   fetchedThrot=0;
   testCounter=0;
   passed=0;

   while(testCounter<5)
   {
		++testCounter;
      if(fetchedRPM>5)
      {
      	passed=1;
         break;
      }
      else
      {
      	serEclose();
         giveSTime();
      	brdInit();
         lightsOff();
         WrPortI(PGDDR, &PGDDRShadow, 0x40);
	   	serEopen(38400);
	   	giveTime();
	   	fetchedRPM=getRPM();
      }
   }
   if(passed)
   	return 1;
   else
   	return 0;
}

float getRPM()
{
   char readSentence[25];
   char OBDresponse[5];
   float carRPM;
   int x;
   int loopExit;
   int loopPass;
   int RTSvar;
   int RTRvar;

   carRPM=1;
   loopExit=1;
   loopPass=1;
   RTSvar=0;
   RTRvar=0;

   WrPortI(PEB4R, NULL, 0xFF);
   while(loopExit)
   {
   	if(0==BitRdPortI(PGDR, 4))
      {
      	loopExit=0;
         RTSvar=1;
      }
      if(loopPass>=5000)
      {
      	loopExit=0;
   	}
    	loopPass++;
   }

   loopPass=1;
   loopExit=1;
 	//waitfor(0==BitRdPortI(PGDR, 4));
	//waitfor(DelayMs(10));
   if(RTSvar)
   {
	   serErdFlush();
	   serEwrFlush();
	   serEputs("01 0C\r");
      while(loopExit)
      {
        if(21==serErdUsed())
        {
        		loopExit=0;
            RTRvar=1;
        }
        if(loopPass>=5000)
        {
        		loopExit=0;
        }
        loopPass++;
      }
      if(RTRvar)
      {
	      //waitfor(21==serErdUsed());
	      //waitfor(DelayMs(10));
	      serEread(readSentence, 21, 10);
	      for(x=0;x<15;x++)
	      {
	         if(readSentence[x]=='4' && readSentence[x+1]=='1' && readSentence[x+2]==' ')
	         {
	            OBDresponse[0]=readSentence[x+6];
	            OBDresponse[1]=readSentence[x+7];
	            OBDresponse[2]=readSentence[x+9];
	            OBDresponse[3]=readSentence[x+10];
	            OBDresponse[4]='\0';
	            carRPM = hex2dec(OBDresponse);
	            carRPM = carRPM/4.0;
	         }
	      }
      }

   }
   WrPortI(PEB4R, NULL, 0x00);
   return carRPM;
}

float getSpeed()
{

   char readSentence[25];
   char OBDresponse[5];
   float carSpeed;
   int x;

   carSpeed = 1;

   WrPortI(PEB4R, NULL, 0xFF);
   serErdFlush();
   serEwrFlush();
   //waitfor(0==BitRdPortI(PGDR, 4));
   //waitfor(DelayMs(10));
   serEputs("01 0D\r");
   //waitfor(20==serErdUsed());
   //waitfor(DelayMs(10));
   serEread(readSentence, 20, 10);
   //printf("\n%s",readSentence);
   for(x=0;x<14;x++)
   {
	   if(readSentence[x]=='4' && readSentence[x+1]=='1' && readSentence[x+2]==' ')
      {
	      OBDresponse[0]='0';
	      OBDresponse[1]='0';
	      OBDresponse[2]=readSentence[x+6];
	      OBDresponse[3]=readSentence[x+7];
	      OBDresponse[4]='\0';
	      carSpeed = hex2dec(OBDresponse);
         carSpeed = carSpeed/1.609;
      }
   }
   WrPortI(PEB4R, NULL, 0x00);
   return carSpeed;
}

int getThrot()
{
   char readSentence[25];
   char OBDresponse[5];
   int carThrot;
   int x;

   carThrot=1;

   WrPortI(PEB4R, NULL, 0xFF);
   serErdFlush();
   serEwrFlush();
   //waitfor(DelayMs(10));
   //waitfor(0==BitRdPortI(PGDR, 4));
   serEputs("01 11\r");
   //waitfor(20==serErdUsed());
   //waitfor(DelayMs(10));
   serEread(readSentence, 20, 10);

   for(x=0;x<14;x++)
   {
	   if(readSentence[x]=='4' && readSentence[x+1]=='1' && readSentence[x+2]==' ')
      {
	      OBDresponse[0]='0';
	      OBDresponse[1]='0';
	      OBDresponse[2]=readSentence[x+6];
	      OBDresponse[3]=readSentence[x+7];
	      OBDresponse[4]='\0';
	      carThrot = hex2dec(OBDresponse);
      	carThrot = carThrot*100;
      	carThrot = carThrot/255;
      }
   }
   WrPortI(PEB4R, NULL, 0x00);
   return carThrot;
}

float hex2dec(char hex[])
{
	float decValue;
   int x;
   int multip;
   int errorFlag;

   errorFlag=0;
   decValue = 0;
   x=0;
   multip=0;

   while(x<4){
	   if(x==0)
	      multip=4096;
	   else if(x==1)
	      multip=256;
	   else if(x==2)
	      multip=16;
	   else if(x==3)
	      multip=1;
	   else
	      errorFlag=1;

      if(hex[x] >= 48 && hex[x] <= 57){
	      decValue = decValue + ((hex[x]-48)*multip);
         //printf("\n0-9\n");
         }
	   else if(hex[x] == 'a' || hex[x] == 'A') {
	      decValue = decValue + (10*multip);
          //printf("\nA\n");
         }
	   else if(hex[x] == 'b' || hex[x] == 'B'){
	      decValue = decValue + (11*multip);
         //printf("\nB\n");
         }
	   else if(hex[x] == 'c' || hex[x] == 'C') {
	      decValue = decValue + (12*multip);
         //printf("\nC\n");
         }
	   else if(hex[x] == 'd' || hex[x] == 'D')  {
	      decValue = decValue + (13*multip);
         //printf("\nD\n");
         }
	   else if(hex[x] == 'e' || hex[x] == 'E') {
	      decValue = decValue + (14*multip);
         //printf("\nE\n");
         }
	   else if(hex[x] == 'f' || hex[x] == 'F'){
	      decValue = decValue + (15*multip);
         //printf("\nF\n");
         }
      else
      	errorFlag=1;
		x++;
   }
   //decValue = 44986;
   if(errorFlag)
   	decValue = 0;
	return decValue;
}
/* END: OBD FUNCTIONS */



int prepGPS()
{
	int testCount;
   int passed;

   testCount=0;
	passed=0;
	serCopen(4800);
   giveTime();

   while(testCount<75)
   {
      ++testCount;
      getGPS();
      if(current_pos.lat_degrees>0)
      {
      	passed=1;
      	break;
      }
      else
      {
      	serCrdFlush();
         serCwrFlush();
         serCclose();
         serCopen(4800);
      	giveSTime();
      }
   }

   if(passed)
  		return 1;
   else
		return 0;
}

void getGPS()
{
	int GPSdataSearcher;
   char GPSchar;
   int testRun;
   int loopBreaker;

   GPSdataSearcher = 0;
   GPSdataPointer=0;
	testRun = 0;
   loopBreaker = 1;

   WrPortI(PEB7R, NULL, 0xFF);
   while(loopBreaker)
   {
	   GPSchar = serCgetc();
      if(GPSchar == '\r' || GPSchar == '\n')
      {
         loopBreaker=0;
         GPSdata[GPSdataPointer]=0;
         gps_get_position(&current_pos, GPSdata);
         GPSdataPointer=0;
      }
      else if(GPSchar>0)
	   {
      	GPSdata[GPSdataPointer] = GPSchar;
         GPSdataPointer++;
         if(GPSdataPointer == 100)
         	GPSdataPointer = 0;
	   }
   }
   WrPortI(PEB7R, NULL, 0x00);
}




int prepCam()
{

	int i;
   int passed;
	char camSync[6];
   char camSyncR[6];
   char camReceive[6];
   char camInit[6];
   char camPkgSize[6];
   char camSnapShot[6];

   i=0;
   passed=0;

	camSync[0] = 0xAA;
	camSync[1] = 0x0D;
	camSync[2] = 0x00;
	camSync[3] = 0x00;
	camSync[4] = 0x00;
	camSync[5] = 0x00;

   camSyncR[0] = 0xAA;
   camSyncR[1] = 0x0E;
   camSyncR[2] = 0x0D;
   camSyncR[3] = 0x00;
   camSyncR[4] = 0x00;
   camSyncR[5] = 0x00;

   camReceive[0] = 0x00;
   camReceive[1] = 0x00;
   camReceive[2] = 0x00;
   camReceive[3] = 0x00;
   camReceive[4] = 0x00;
   camReceive[5] = 0x00;

   camInit[0] = 0xAA;
   camInit[1] = 0x01;
   camInit[2] = 0x00;
   camInit[3] = 0x07;
   camInit[4] = 0x03;
   camInit[5] = 0x01;

   camPkgSize[0] = 0xAA;
   camPkgSize[1] = 0x06;
   camPkgSize[2] = 0x08;
   camPkgSize[3] = 0x00;
   camPkgSize[4] = 0x02;
   camPkgSize[5] = 0x00;

   camSnapShot[0] = 0xAA;
   camSnapShot[1] = 0x05;
   camSnapShot[2] = 0x00;
   camSnapShot[3] = 0x00;
   camSnapShot[4] = 0x00;
   camSnapShot[5] = 0x00;

   serBclose();
   serBopen(14400);
	serBwrFlush();
   serBrdFlush();
   printf("\n");
   while(i<75)
   {
   	WrPortI(PEB6R, NULL, 0xFF);
   	printf("Trying to connect Cam\n");
      serBwrite(camSync, 6);
      serBread(camReceive, 6, 200);
      WrPortI(PEB6R, NULL, 0x00);
      if(camReceive[0]==0xAA && camReceive[1]==0x0E && camReceive[2]==0x0D)
      {
	    //  giveSTime();
       	WrPortI(PEB6R, NULL, 0xFF);
	      camReceive[0]=0x00;
	      camReceive[1]=0x00;
	      camReceive[2]=0x00;
	      printf("ACK Received!\n");
         //Flushing B breaks flow
	      //printf("Flushing Serial Port B\n");
	      //serBrdFlush();
	      serBread(camReceive, 6, 200);
         WrPortI(PEB6R, NULL, 0x00);
	      if(camReceive[0]==0xAA && camReceive[1]==0x0D && camReceive[2]==0x00 && camReceive[3]==0x00 && camReceive[4]==0x00 && camReceive[5]==0x00)
	      {
            WrPortI(PEB6R, NULL, 0xFF);
            //giveSTime();
	         camReceive[0]=0x00;
	         camReceive[1]=0x00;
	         camReceive[2]=0x00;
         	printf("SYNC Received!\n");
            serBwrite(camSyncR,6);
            //giveSTime();
            giveSTime();
            serBwrite(camInit,6);
            giveSTime();
            serBread(camReceive, 6, 200);
            WrPortI(PEB6R, NULL, 0x00);
            if(camReceive[1]== 0x0E)
            {
            	WrPortI(PEB6R, NULL, 0xFF);
            	camReceive[0]=0x00;
	         	camReceive[1]=0x00;
	         	camReceive[2]=0x00;
         		printf("ACK Received - Initial\n");
               serBwrite(camPkgSize,6);
               //giveSTime();
               giveSTime();
               serBread(camReceive, 6, 200);
               if(camReceive[1]== 0x0E && camReceive[2]== 0x06)
               {
               	WrPortI(PEB6R, NULL, 0x00);
            		camReceive[0]=0x00;
	         		camReceive[1]=0x00;
	         		camReceive[2]=0x00;
               	printf("ACK Received - PkgSize\n");
                  serBwrite(camSnapShot, 6);
                  //giveSTime();
                  giveSTime();
                  serBread(camReceive, 6, 200);
                  if(camReceive[1]== 0x0E && camReceive[2]== 0x05)
               	{
                  	WrPortI(PEB6R, NULL, 0xFF);
               		camReceive[0]=0x00;
	         			camReceive[1]=0x00;
	         			camReceive[2]=0x00;
               		printf("ACK Received - SnapShot\n");
                     i=150;
	                  passed=1;
                     WrPortI(PEB6R, NULL, 0x00);
                  }
               }
            }
	      }
   	}
      giveSTime();
   	++i;
      WrPortI(PEB6R, NULL, 0x00);
   }

   if(passed)
   	return 1;
   else
   	return 0;
}


void camClick()
{
	int i;
   int passed;
	char camSync[6];
   char camSyncR[6];
   char camReceive[6];
   char camGetPic[6];
   char camInit[6];
   char camPkgSize[6];
   char camSnapShot[6];

   //Turn on Cam LED
   WrPortI(PEB6R, NULL, 0xFF);

   i=0;
   passed=0;

	camSync[0] = 0xAA;
	camSync[1] = 0x0D;
	camSync[2] = 0x00;
	camSync[3] = 0x00;
	camSync[4] = 0x00;
	camSync[5] = 0x00;

   camSyncR[0] = 0xAA;
   camSyncR[1] = 0x0E;
   camSyncR[2] = 0x0D;
   camSyncR[3] = 0x00;
   camSyncR[4] = 0x00;
   camSyncR[5] = 0x00;

   camReceive[0] = 0x00;
   camReceive[1] = 0x00;
   camReceive[2] = 0x00;
   camReceive[3] = 0x00;
   camReceive[4] = 0x00;
   camReceive[5] = 0x00;

   camGetPic[0] = 0xAA;
   camGetPic[1] = 0x04;
   camGetPic[2] = 0x01;
   camGetPic[3] = 0x00;
   camGetPic[4] = 0x00;
   camGetPic[5] = 0x00;

   camInit[0] = 0xAA;
   camInit[1] = 0x01;
   camInit[2] = 0x00;
   camInit[3] = 0x07;
   camInit[4] = 0x03;
   camInit[5] = 0x01;

   camPkgSize[0] = 0xAA;
   camPkgSize[1] = 0x06;
   camPkgSize[2] = 0x08;
   camPkgSize[3] = 0x00;
   camPkgSize[4] = 0x02;
   camPkgSize[5] = 0x00;

   camSnapShot[0] = 0xAA;
   camSnapShot[1] = 0x05;
   camSnapShot[2] = 0x00;
   camSnapShot[3] = 0x00;
   camSnapShot[4] = 0x00;
   camSnapShot[5] = 0x00;

	serBwrFlush();
   serBrdFlush();
   printf("\nClick!\n");
   WrPortI(PEB6R, NULL, 0x00);
   while(i<30)
   {
   	printf("Checking Camera\n");
      serBwrite(camSync, 6);
      serBread(camReceive, 6, 200);
      if(camReceive[0]==0xAA && camReceive[1]==0x0E && camReceive[2]==0x0D)
      {
   		WrPortI(PEB6R, NULL, 0xFF);
      	camReceive[0]=0x00;
	   	camReceive[1]=0x00;
	   	camReceive[2]=0x00;
      	printf("Camera Says Sup?\n");
         serBwrite(camInit,6);
         giveSTime();
         serBread(camReceive,6,200);
         if(!(camReceive[0]==0xAA && camReceive[1]==0x0E && camReceive[2]==0x01))
         {
            printf("BREAKING!!!\n");
            break;
         }
      	camReceive[0]=0x00;
	   	camReceive[1]=0x00;
	   	camReceive[2]=0x00;
         serBwrite(camPkgSize,6);
         serBread(camReceive,6,200);
         if(!(camReceive[0]==0xAA && camReceive[1]==0x0E && camReceive[2]==0x06))
         {
            printf("BREAKING2!!!\n");
            break;
         }
      	camReceive[0]=0x00;
	   	camReceive[1]=0x00;
	   	camReceive[2]=0x00;
         serBwrite(camSnapShot,6);
         serBread(camReceive,6,200);
         if(!(camReceive[0]==0xAA && camReceive[1]==0x0E && camReceive[2]==0x05))
         {
            printf("BREAKING3!!!\n");
            break;
         }
         WrPortI(PEB6R, NULL, 0x00);

   	}
      giveSTime();
   	++i;
   }
   //Turn off Cam LED
   WrPortI(PEB6R, NULL, 0x00);
}




/* BEGIN: OTHER FUNCTIONS */
void giveTime()
{
	int giveTimeX;
	int giveTimeY;

   for(giveTimeX=0;giveTimeX<=1000;giveTimeX++)
		for(giveTimeY=0;giveTimeY<=1000;giveTimeY++)
      	giveTimeY = giveTimeY * 1;
}

void giveMTime()
{
	int giveTimeX;
	int giveTimeY;

   for(giveTimeX=0;giveTimeX<=500;giveTimeX++)
		for(giveTimeY=0;giveTimeY<=500;giveTimeY++)
      	giveTimeY = giveTimeY * 1;
}

void giveSTime()
{
	int giveTimeX;
	int giveTimeY;

   for(giveTimeX=0;giveTimeX<=250;giveTimeX++)
		for(giveTimeY=0;giveTimeY<=250;giveTimeY++)
      	giveTimeY = giveTimeY * 1;
}

void lightsOff()
{
	WrPortI(PEB3R, NULL, 0x00);
   WrPortI(PEB4R, NULL, 0x00);
   WrPortI(PEB5R, NULL, 0x00);
   WrPortI(PEB6R, NULL, 0x00);
   WrPortI(PEB7R, NULL, 0x00);
}

void lightsOn()
{
	WrPortI(PEB3R, NULL, 0xFF);
   WrPortI(PEB4R, NULL, 0xFF);
   WrPortI(PEB5R, NULL, 0xFF);
   WrPortI(PEB6R, NULL, 0xFF);
   WrPortI(PEB7R, NULL, 0xFF);
}

void flashLED(int light)
{
	char port;

   port="xxxx";

	if(light==1)
	   port="PEB3R";
	else if(light==2)
	   port="PEB4R";
	else if(light==3)
	   port="PEB5R";
	else if(light==4)
	   port="PEB6R";
	else if(light==5)
	   port="PEB7R";
	WrPortI(port, NULL, 0xFF);
	giveMTime();
	WrPortI(port, NULL, 0x00);
	giveMTime();

}

void flashLED1()
{
	WrPortI(PEB3R, NULL, 0xFF);
	giveMTime();
	WrPortI(PEB3R, NULL, 0x00);
	giveSTime();
}

void flashLED2()
{
	WrPortI(PEB4R, NULL, 0xFF);
	giveMTime();
	WrPortI(PEB4R, NULL, 0x00);
	giveSTime();
}

void flashLED3()
{
	WrPortI(PEB5R, NULL, 0xFF);
	giveMTime();
	WrPortI(PEB5R, NULL, 0x00);
	giveSTime();
}

void flashLED4()
{
	WrPortI(PEB6R, NULL, 0xFF);
	giveMTime();
	WrPortI(PEB6R, NULL, 0x00);
	giveSTime();
}

void flashLED5()
{
	WrPortI(PEB7R, NULL, 0xFF);
	giveMTime();
	WrPortI(PEB7R, NULL, 0x00);
	giveSTime();
}

void testLEDs()
{
		lightsOff();
	giveMTime();
		lightsOn();
	giveMTime();
		lightsOff();
	giveMTime();
		lightsOn();
	giveMTime();
		lightsOff();
	giveMTime();
		lightsOn();
	giveMTime();
   	lightsOff();
	giveMTime();
		lightsOn();
	giveMTime();
   	lightsOff();
}
/* END: OTHER FUNCTIONS */