/*
build notes:
data fragmentation problem after really long periods for pictures,


*/
#class auto
#memmap xmem

#use "gps.lib"

#define CINBUFSIZE 127
#define COUTBUFSIZE 127

#define BINBUFSIZE 511
#define BOUTBUFSIZE 127

#define DINBUFSIZE 127
#define DOUTBUFSIZE 1023

#define EINBUFSIZE 127
#define EOUTBUFSIZE 127

#define YawAverage 2090  		// This value can be found with the average raw yaw rate value



#define OBDdelay 10


#define DEBUGSYS 0			//if 1 use printf statements
									//if 0 printf statements disabled

#define PrintSDData 0		//Print each SD data line before saving to SD

#define PrintSDCall 0		//Print indicators of when SD is being accessed

#define EnableSleepy 1


// Cap the amount of attempts each device will be tested
#define OBDTESTLIMIT 5
#define GPSTESTLIMIT 120
#define TIMTESTLIMIT 120
#define ACLTESTLIMIT	50
#define YAWTESTLIMIT 30
#define CAMTESTLIMIT 220
#define camBaudRate 57600
#define camDelay 250

#define SD_FLAG_DELAY 15

//master initilization function
void bootSystem();
int systemGTG;          //is entire system functioning?
int resetShadow;


//YAW RATE GYRO routines & variables
int prepYaw();
void getYawRate();
char yawSign;
int yawRateG;
int yawGTG;             //is yaw rate gyro good to go?
int yawRate;            //value returned from yawRate grab
int yawJarCounter;





//ACCELEROMETER routines & variables
void AccelInit();
int prepAccel();
void getAccel();
struct
{
	float x;            // in units of (g)
   float y;
   float x1;
   char signx1;
   float y1;
   char signy1;
   unsigned int PollCounter;
}Accelleration;
char capture_status;
unsigned long pulse_widthX;
unsigned long pulse_widthY;
int accJarCounter;
float AccXOffSet;
float AccYOffSet;
 int accelGTG;           //is accel good to go?
                        //Accel data is global so not initalized here





//OBD routines & variables
int prepOBD();
float getRPM();
cofunc int getRPMMT();
cofunc int getSpeedMT();
cofunc int getThrotMT();
float hex2dec(char[]);
int globalRPM;
int globalSpeed;
int globalThrot;

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







//GPS routines & variables
int prepGPS();
void getGPS();
cofunc int getGPSMT();
int GPSchar;
char GPSdata[100];
int GPSdataPointer;
GPSPosition current_pos;
struct tm current_time;
int degreesLat;
int degreesLon;
float minutesLat;
float minutesLon;
char directionLat;
char directionLon;
int timeStamp[6];

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

int timeOffset;
 int myTime;




//CAMERA routines & variables
int prepCam();
//void picString(int);
cofunc int getPic();

// Camera picture arrays, problems when declared in function.
char picture[4095];
char picture2[4095];
char picture3[4095];
char picPacket[508];

int camGTG;             //is camera good to go?
char camSync[6];
char camSyncR[6];
char camReceive[6];
char camInit[6];
char camPkgSize[6];
char camSnapShot[6];
char camGetPic[6];
char camAck[6];
char camReset[6];
char camSleep[6];
char endPic[6];
char tempString[15];
   char tempString2[25];


//SD MODULE routines & variables
static const long baud_rate = 460800L;
int prepSD();
void resetSD();
int initSD();
void writeSDcBc(char[]);
cofunc int writeSD3(char[]);
cofunc int writeSDcam(int);
cofunc int writeSDcamTime();
cofunc int SDwrites(char[], int, int);

int SDGTG;              //is Camera good to go?
int usingSD;
char currentFileName[19];
char currentFileNamePic[19];
char SDdata[101];

//Non multitasking delays
void giveTime();
void giveMTime();
void giveSTime();


//LED controls
void lightsOff();
void lightsOn();
void flashLED1();
void flashLED2();
void flashLED3();
void flashLED4();
void flashLED5();
void flashLED6();
void flashLED7();
void testLEDs();
void signalMaster();


















void main() {

   int x;

   bootSystem();



  	while (1)
   {
      costate
      {

         waitfor(serCrdUsed()>70);
         if(GPSpollCounterA==0 || GPSpollCounterB==0)
         {

         	wfd getGPSMT();
         	if(directionLon!='N' && directionLon!='W' && directionLon!='S' && directionLon!='E')
      		{
	            myTime = (SEC_TIMER+timeOffset);
	            timeStamp[0]=0;
	            timeStamp[1]=0;
	            timeStamp[2]=0;
	            timeStamp[3]=myTime/3600;
	            timeStamp[4]=(myTime-(timeStamp[3]*3600))/60;
	            timeStamp[5]=myTime%60;
      		}

         }
         else
         	waitfor(DelayMs(150));
      }

      costate
      {
         if(yawGTG)
         {
            waitfor(DelayMs(100));
            getYawRate();
         }
      }

      costate
      {
         if(accelGTG)
         //if(1)
         {
            getAccel();
         }
      }

      costate
      {
         if(OBDGTG)
         {
            wfd getRPMMT();
            wfd getThrotMT();
            wfd getSpeedMT();
         }
      }

      costate
      {
         if(camGTG)
         {
				wfd getPic();
            //waitfor(DelayMs(50));
         }
      }

      costate
      {

               //removed \r from before \n
         		sprintf(SDdata,"%02d-%02d-%04d %02d:%02d:%02d $$ %02d %09f %c %03d %09f %c $$ %c%02d $$ %5.2f %5.2f $$ %04d %03d %03d\r\n\0",
	               timeStamp[0], timeStamp[1], timeStamp[2], timeStamp[3], timeStamp[4], timeStamp[5],
	               degreesLat, minutesLat, directionLat,
	               degreesLon, minutesLon, directionLon,
	               yawSign, yawRateG,
	               Accelleration.x1,
	               Accelleration.y1,
	               globalRPM, globalSpeed, globalThrot);

               GPSpollCounterA=0; 		//reset GPS location poll counter, allowing it to get data again
               GPSpollCounterB=0;      //reset GPS time poll counter, allowing it to get data again


            	waitfor(usingSD==0); 	//Waitfor SD module to be free
               usingSD=1; //Lock down SD module preventing any other device to write to it
               wfd writeSD3(SDdata);  	//write current batch of data to SD
               usingSD=0;              //release SD, allows other devices to use for writing
               waitfor(DelayMs(60));	//give time for devices to poll new data
      }

      costate
      {
      	waitfor(DelaySec(30));
         if(1)
         //if(EnableSleepy && OBDGTG && accelGTG && globalRPM<100)
         {

            waitfor(usingSD==0); 	//Waitfor SD module to be free
            usingSD=1; //Lock down SD module preventing any other device to write to it
            waitfor(DelaySec(10));
	         lightsOff();
	         BitWrPortI(PADR, &PADRShadow, 0, 0);
	         giveTime();
            for(x=0;x<100;++x)
            	getAccel();
	         while(1)
	         {
	            getAccel();
               x=(x+100)*1000;
               x=0;
	            if(Accelleration.x1 >= 0.3 || Accelleration.x1 <= -0.3)
	            {
	               ++accJarCounter;
	               if(accJarCounter>6)
	               {
	                  giveTime();
	                  bootSystem();
	                  break;
	               }
	            }
	         }
         }

      }





   }
}



void bootSystem()
{
   giveMTime();
	if(DEBUGSYS)printf("Boot System Function Entered\n");
   if(DEBUGSYS)printf("Checking Devices...\n");



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

	globalRPM=0;
	globalSpeed=0;
	globalThrot=0;

   //Block Excess GPS Accesses
   GPSpollCounterA=0;
   GPSpollCounterB=0;

   yawJarCounter=0;
   accJarCounter=0;

   //Prevents SD from being accessed during operation
	usingSD=0;

   brdInit();
   lightsOff();

   if(DEBUGSYS)printf("\n Turning on 5V regulator.");
   WrPortI(SPCR, NULL, 0x084);  	// Set Parallel Port A as Output to control the 5V Voltage reg
   BitWrPortI(PADR, &PADRShadow, 1, 0);   // 5V Regulator ON
   if(DEBUGSYS)printf("\n 5V regulator on.");

   if(DEBUGSYS)
   {
   	printf("\nTESTING LEDS\n");
	   giveTime();
	   flashLED1();
	   flashLED2();
	   flashLED3();
	   flashLED4();
	   flashLED5();
	   flashLED6();
	   flashLED7();
	   giveTime();
   }


   //Notify User of Power Up
	signalMaster();
	signalMaster();

   if(DEBUGSYS)
   {
   	giveMTime();
   	printf("Performing OBD Test\n");
   }
   OBDGTG = prepOBD();
	signalMaster();

   if(DEBUGSYS)
   {
   	giveMTime();
   	printf("Performing YAW Test\n");
   }
   yawGTG = prepYaw();
   signalMaster();

   if(DEBUGSYS)
   {
   	giveMTime();
   	printf("Performing ACL Test\n");
   }
   accelGTG = prepAccel();
   signalMaster();

   if(DEBUGSYS)
   {
   	giveMTime();
   	printf("Performing GPS Test\n");
   }
   GPSGTG = prepGPS();
   signalMaster();

   if(DEBUGSYS)
   {
   	giveMTime();
   	printf("Performing CAM Test\n");
   }
   camGTG = prepCam();
	signalMaster();

   if(DEBUGSYS)
   {
   	giveMTime();
		printf("Performing SD Test\n");
   }
   SDGTG = prepSD();
   signalMaster();

   if(DEBUGSYS)printf("Signaling ALL Test Complete on ALL LED\n");
   flashLED1();
   flashLED2();
   flashLED3();
   flashLED4();
   flashLED5();
   flashLED6();
   flashLED7();

   lightsOff();

   if(accelGTG==0)
   	WrPortI(PEB3R, NULL, 0xFF);
   else
   {
   	Accelleration.x1=0.0;
   	Accelleration.y1=0.0;
   }

   if(OBDGTG==0)
   	WrPortI(PEB4R, NULL, 0xFF);
   if(yawGTG==0)
   	WrPortI(PEB5R, NULL, 0xFF);
   if(camGTG==0)
   	WrPortI(PEB6R, NULL, 0xFF);
   if(GPSGTG==0)
   {
      if(SEC_TIMER<0)
      	timeOffset = SEC_TIMER - (2*SEC_TIMER);
      else
      	timeOffset = -1*SEC_TIMER;
   	WrPortI(PEB7R, NULL, 0xFF);
   }
   if(SDGTG==0)
   {
   	while(1)
      {
	      giveSTime();
         WrPortI(PEB1R, NULL, 0xFF);
	      WrPortI(PEB0R, NULL, 0xFF);
	      giveMTime();
         WrPortI(PEB1R, NULL, 0x00);
	      WrPortI(PEB0R, NULL, 0x00);
	      giveSTime();
      }
   }

	if(SDGTG && GPSGTG && camGTG && yawGTG && accelGTG && OBDGTG)
  		systemGTG=1;

   if(systemGTG==0)
      WrPortI(PEB1R, NULL, 0xFF);


printf("\n\nBoot Report-\nYaw Rate Gyro:%d\nAccelerometer:%d\nOBD:%d\nGPS:%d\nCamera:%d\nSD:%d\nSystem:%d\n\n",yawGTG,accelGTG,OBDGTG,GPSGTG,camGTG,SDGTG,systemGTG);
   //if(DEBUGSYS)printf("\n\nBoot Report-\nYaw Rate Gyro:%d\nAccelerometer:%d\nOBD:%d\nGPS:%d\nCamera:%d\nSD:%d\n\n\n",yawGTG,accelGTG,OBDGTG,GPSGTG,camGTG,SDGTG);
}

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

   int testValue;
   int testCounter;
   int passed;

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

   while(testCounter<YAWTESTLIMIT)
   {
   	if(DEBUGSYS)printf("Yaw Test: %d\n",testCounter);
      giveSTime();

      // Setting up the Yaw rate Data Direction Register PFDDR
      BitWrPortI(PFDDR, &PFDDRShadow, 1, 0);          	// PF0 (Output) CS
      BitWrPortI(PFDDR, &PFDDRShadow, 1, 2);				// PF2 (Output) Yaw rate DIN
      BitWrPortI(PFDDR, &PFDDRShadow, 0, 5);				// PF5 (Input)  Yaw rate DOUT
      BitWrPortI(PFDDR, &PFDDRShadow, 1, 7);				// PF7 (Output) SCLK

	   //BitWrPortI(PFDR, &resetShadow, 1, 6);
	   getYawRate();                             // Initialize yaw rate gyro
	   getYawRate();
      getYawRate();
      getYawRate();                             // Clears bad data
	   //testValue = getYawRate();                 // Test if yaw rate working properly
      if(yawRateG>=-5 && yawRateG<=5)
      {
      	passed=1;
         break;
      }
      else
      	++testCounter;
   }
   if(passed)
   	return 1;
   else
   {
      yawSign = '+';
   	yawRateG = 0;
   	return 0;
   }
}

void 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(PFDR, PFDRShadow,  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(PFDR, 5);				//DOUT low

	BitWrPortI(PFDR, PFDRShadow,  0, 2);	//LOW

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

   BitWrPortI(PFDR, PFDRShadow,  0, 2);	//DC

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

   BitWrPortI(PFDR, PFDRShadow,  0, 2);	//DC

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

   BitWrPortI(PFDR, PFDRShadow,  0, 2); 	//ADD!

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

   BitWrPortI(PFDR, PFDRShadow,  0, 2); 	//ADD0

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

   BitWrPortI(PFDR, PFDRShadow,  1, 2); 	//HIGH

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

   BitWrPortI(PFDR, PFDRShadow,  1, 2); 	//HIGH

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

   BitWrPortI(PFDR, PFDRShadow,  0, 2); 	//DC

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

   BitWrPortI(PFDR, PFDRShadow,  0, 2); 	//DC

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

   BitWrPortI(PFDR, PFDRShadow,  0, 2); 	//LOW

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

   BitWrPortI(PFDR, PFDRShadow,  1, 2); 	//CODING

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

   BitWrPortI(PFDR, PFDRShadow,  0, 2); 	//DC

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

   BitWrPortI(PFDR, PFDRShadow,  0, 2); 	//DC

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

   BitWrPortI(PFDR, PFDRShadow,  0, 2); 	//DC

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

   BitWrPortI(PFDR, PFDRShadow,  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);

   decimalSum = decimalSum - YawAverage;
   decimalSum /= (YawAverage/300);

   //printf("\nYAW: %d\n",decimalSum);

   if(decimalSum >= 0)
   {
      yawSign = '+';
      if(decimalSum<=99)
   		yawRateG = decimalSum;
      else
      	yawRateG = 99;
   }
   else
   {
   	yawSign = '-';
      if(decimalSum>=-99)
			yawRateG = decimalSum - (2*decimalSum);
		else
      	yawRateG = 99;
   }
}
/* 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;

   //Accel Offsets
   AccXOffSet=0.0;
   AccXOffSet=0.0;

   getAccel();
   giveTime();
   getAccel();
}

int prepAccel()
{
	int testCounter;
   int passed;

   testCounter=0;
   passed=0;

   AccelInit();



   while(testCounter<ACLTESTLIMIT)
   {
   	if(DEBUGSYS)printf("ACL Test: %d\n",testCounter);
	   ++testCounter;

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

   if(passed)
	   return 1;
	else
   {
   	Accelleration.signx1 = '+';
   	Accelleration.x1 = 0;
   	Accelleration.signy1 = '+';
   	Accelleration.y1 = 0;
	   return 0;
	}
}

void getAccel()
{
	int AccelDelta;

   float AccXOffSet;
   float AccYOffSet;

   AccelDelta=0;



   WrPortI(PEB3R, NULL, 0xFF);      	// LED
   //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)
   {
      if(DEBUGSYS)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)
   {
      if(DEBUGSYS)printf("Input capture counter rolled over.\n");
      pulse_widthY += 0x10000;
   }
   WrPortI(PEB3R, NULL, 0x00);

   //printf("\nX: %9.2f Y: %9.2f", Accelleration.x+0.21, Accelleration.y+0.183);
 //  Accelleration.x=Accelleration.x+0.21;
  // Accelleration.y=Accelleration.y+0.183;
  // printf("\nX: %9.3f Y: %9.3f", Accelleration.x, Accelleration.y);
   //Accelleration.x=Accelleration.x+AccXOffSet;
   //Accelleration.y=Accelleration.y+AccYOffSet;
   //printf("\nX: %18f Y: %18f", Accelleration.x, Accelleration.y);


   if((Accelleration.x+0.21) >= -1.2 && (Accelleration.x+0.21) <= 1.2)
   {
      Accelleration.x1=Accelleration.x+0.21;
   	/*
      if(Accelleration.x >= 0)
      {
         Accelleration.signx1 = '+';
         Accelleration.x1 = Accelleration.x;
      }
      else
      {
         Accelleration.signx1 = '-';
         Accelleration.x1 = Accelleration.x - (2*Accelleration.x);
      }
      */
   }

   if((Accelleration.y+0.183) >= -1.2 && (Accelleration.y+0.183) <= 1.2)
   {
      Accelleration.y1=Accelleration.y+0.183;
   	/*
      if(Accelleration.y >= 0)
      {
         Accelleration.signy1 = '+';
         Accelleration.y1 = Accelleration.y;
      }
      else
      {
         Accelleration.signy1 = '-';
         Accelleration.y1 = Accelleration.y - (2*Accelleration.y);
      }
      */
   }
}
/* 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<OBDTESTLIMIT)
   {
   	if(DEBUGSYS)printf("OBD Test: %d\n",testCounter);
		++testCounter;
      if(fetchedRPM>5)
      {
      	passed=1;
         break;
      }
      else
      {
      	serEclose();
         giveSTime();
         WrPortI(PGDDR, &PGDDRShadow, 0x40);
	   	serEopen(38400);
	   	giveMTime();
         serErdFlush();
	   	serEwrFlush();
         serEputs("AT Z\r");
         giveTime();
         giveTime();
	   	fetchedRPM=getRPM();
      }
   }
   if(passed)
   	return 1;
   else
   {
	   globalRPM=0;
	   globalSpeed=0;
	   globalThrot=0;
   	return 0;
   }
}

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

   int p;

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

   	WrPortI(PEB4R, NULL, 0xFF);		//LED CONTROL
      //Clear Buffer
	   serErdFlush();
	   serEwrFlush();

      p=0;
      while(BitRdPortI(PGDR,4)==1 && p<15)
      {
      	if(DEBUGSYS)printf("waiting OBD...\n");
      	giveSTime();
      	++p;
      }
	   serEputs("01 0C\r");
      //WrPortI(PEB4R, NULL, 0x00);		//LED CONTROL
      giveMTime();
      //WrPortI(PEB4R, NULL, 0xFF);		//LED CONTROL
      giveMTime();
      WrPortI(PEB4R, NULL, 0x00);		//LED CONTROL
      p=0;
      while(BitRdPortI(PGDR,4)==1 && p<15)
      {
      	if(DEBUGSYS)printf("waiting OBD...\n");
      	giveSTime();
      	++p;
      }
		WrPortI(PEB4R, NULL, 0xFF); 		//LED CONTROL
      serEread(readSentence, serErdUsed(), 100);

      if(DEBUGSYS)
      {
	      printf("\n");
	      for(p=0;p<40;++p)
	         printf(" %c",readSentence[p]);
	      printf("\n");
      }

      for(x=0;x<35;x++)
      {
         if(readSentence[x]=='4' && readSentence[x+1]=='1' && readSentence[x+2]==' ')
         {
            if(DEBUGSYS)printf("OBD ACK Received\n");
            OBDresponse[0]=readSentence[x+6];
            OBDresponse[1]=readSentence[x+7];
            OBDresponse[2]=readSentence[x+9];
            OBDresponse[3]=readSentence[x+10];
            OBDresponse[4]='\0';
            if(DEBUGSYS)printf("%c%c%c%c\n",OBDresponse[0],OBDresponse[1],OBDresponse[2],OBDresponse[3]);
            carRPM = hex2dec(OBDresponse);
            carRPM = carRPM/4.0;
         	globalRPM = carRPM;
         }
      }



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


cofunc int getRPMMT()
{
   char readSentence[40];
   char OBDresponse[5];
   float carRPM;
   int x;
   int loopExit;
   int loopPass;
   int RTSvar;
   int RTRvar;

   int p;

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

   	WrPortI(PEB4R, NULL, 0xFF);  			//LED CONTROL
	   serErdFlush();
	   serEwrFlush();
      waitfor(0==BitRdPortI(PGDR, 4));
	   serEputs("01 0C\r");
      waitfor(DelayMs(OBDdelay));
      waitfor(0==BitRdPortI(PGDR, 4));
      serEread(readSentence, serErdUsed(), 100);


      WrPortI(PEB4R, NULL, 0x00); 				//LED CONTROL
      for(x=0;x<35;x++)
      {
         if(readSentence[x]=='4' && readSentence[x+1]=='1' && readSentence[x+2]==' ')
         {

            printf("OBD ACK Received\n");
            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;
         	globalRPM = carRPM;
         }
      }


   return 1;
}



cofunc int getSpeedMT()
{

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

   carSpeed = 0;

   WrPortI(PEB4R, NULL, 0xFF);		//LED CONTROLS
   serErdFlush();
   serEwrFlush();
   waitfor(0==BitRdPortI(PGDR, 4));
   serEputs("01 0D\r");
   waitfor(DelayMs(OBDdelay));
   waitfor(0==BitRdPortI(PGDR, 4));
   serEread(readSentence, 20, 10);
   WrPortI(PEB4R, NULL, 0x00);	//LED CONTROLS
   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;
         globalSpeed = carSpeed;
      }
   }

   return 1;
}

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

   carThrot=0;

   //WrPortI(PEB4R, NULL, 0xFF);			//LED CONTROLS
   serErdFlush();
   serEwrFlush();
   waitfor(0==BitRdPortI(PGDR, 4));
   serEputs("01 11\r");
   waitfor(DelayMs(OBDdelay));
   waitfor(0==BitRdPortI(PGDR, 4));
   serEread(readSentence, 20, 10);
   //WrPortI(PEB4R, NULL, 0x00);	//LED CONTROLS
   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;
         globalThrot = carThrot;
      }
   }
   return 1;
}

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<GPSTESTLIMIT)
   {
   	if(DEBUGSYS)printf("GPS Test: %d\n",testCount);
      ++testCount;
      getGPS();
      //directionLon='0';

      if(directionLon=='N' || directionLon=='W' || directionLon=='S' || directionLon=='E' /*&& current_time.tm_mon >= 0 && current_time.tm_mon <= 12*/)
      {
      	passed=1;
      	break;
      }
      else
      {
      	serCrdFlush();
         serCwrFlush();
         serCclose();
       	serCopen(4800);
         giveSTime();
   	}
   }

   if(passed)
  		return 1;
   else
	{
	   degreesLat = 0;
	   degreesLon = 0;
	   minutesLat = 0;
	   minutesLon = 0;
	   directionLat = '0';
	   directionLon = '0';
	   timeStamp[0]= 0;
	   timeStamp[1]= 0;
	   timeStamp[2]= 0;
	   timeStamp[3]= 0;
	   timeStamp[4]= 0;
	   timeStamp[5]= 0;
		return 0;
	}
}




void getGPS()
{
   int loopBreaker;
   int i;

   GPSdataPointer=0;
   loopBreaker = 1;
   i=0;

   while(loopBreaker && i<4000)
   {
   	++i;
   	WrPortI(PEB7R, NULL, 0xFF);  						//led control
      GPSchar = serCgetc();
      if(GPSchar == '\r' || GPSchar == '\n')
      {
      	WrPortI(PEB7R, NULL, 0x00);  					//led control
         loopBreaker=0;
         GPSdata[GPSdataPointer]=0;
         GPSdataPointer=0;
         if(gps_get_position(&current_pos, GPSdata) == 0)
         {
         	degreesLat = current_pos.lat_degrees;
				degreesLon = current_pos.lon_degrees;
				minutesLat = current_pos.lat_minutes;
				minutesLon = current_pos.lon_minutes;
         	directionLat = current_pos.lat_direction;
            directionLon = current_pos.lon_direction;
         }
         if(gps_get_utc(&current_time, GPSdata) == 0)
         {
	         timeStamp[0]=current_time.tm_mon;
	         timeStamp[1]=current_time.tm_mday;
	         timeStamp[2]=current_time.tm_year+1900;
	         timeStamp[3]=current_time.tm_hour;
	         timeStamp[4]=current_time.tm_min;
	         timeStamp[5]=current_time.tm_sec;
         }
      }
      else if(GPSchar>0)
	   {
      	WrPortI(PEB7R, NULL, 0x00);  					//led control
      	GPSdata[GPSdataPointer] = GPSchar;
         GPSdataPointer++;
         if(GPSdataPointer == 100)
         	GPSdataPointer = 0;
	   }
   }
   WrPortI(PEB7R, NULL, 0x00);  					//led control

}

cofunc int getGPSMT()
{
   int loopBreaker;

   GPSdataPointer=0;
   loopBreaker = 1;

   //printf("\n%d",serCrdUsed());  			//used to test when GPS is being entered

   WrPortI(PEB7R, NULL, 0xFF);
   while(loopBreaker)
   {
	   wfd GPSchar = cof_serCgetc();
      if(GPSchar == '\r' || GPSchar == '\n')
      {
      	loopBreaker=0;
         GPSdata[GPSdataPointer]=0;
         GPSdataPointer=0;
      	if(gps_get_position(&current_pos, GPSdata) == 0)
         {
         	degreesLat = current_pos.lat_degrees;
				degreesLon = current_pos.lon_degrees;
				minutesLat = current_pos.lat_minutes;
				minutesLon = current_pos.lon_minutes;
         	directionLat = current_pos.lat_direction;
            directionLon = current_pos.lon_direction;
            GPSpollCounterA=1;
         }
         if(gps_get_utc(&current_time, GPSdata) == 0)
         {
	         timeStamp[0]=current_time.tm_mon;
	         timeStamp[1]=current_time.tm_mday;
	         timeStamp[2]=current_time.tm_year+1900;
	         timeStamp[3]=current_time.tm_hour;
	         timeStamp[4]=current_time.tm_min;
	         timeStamp[5]=current_time.tm_sec;
            GPSpollCounterB=1;
         }
      }
      else if(GPSchar>0)
	   {
      	GPSdata[GPSdataPointer] = GPSchar;
         GPSdataPointer++;
         if(GPSdataPointer == 100)
         	GPSdataPointer = 0;
	   }
   }
   WrPortI(PEB7R, NULL, 0x00);

   if(timeStamp[0] <= 0 || timeStamp[0] > 12)
	{
      timeStamp[0]=0;
      timeStamp[1]=0;
      timeStamp[2]=0;
      timeStamp[3]=0;
      timeStamp[4]=0;
      timeStamp[5]=0;
   }
   return 1;
}

int prepCam()
{

	int i;
   int j;
   int k;
   int m;
   int camResetSync;
   int jMul;	//multiplyer for j

   int packetPrinted;

   int bufferUsed;

   int bufferGrabed;

   char hexHolder;
   char hexChar[2];
   int passed;

   int byteTracker[35];
   int packetSize[35];
   int totalPic;
   int packFetchCalls;

   int lastPacket;


   int camI;
   int camPacketSize;
   int picUpperLimit;
   int picUpperLimit2;
   int picUpperLimit3;


   int packetReq;


   packetReq=0;

   camPacketSize=512;

   i=0;
   j=0;

   camResetSync = 100;
   totalPic = 0;
   lastPacket = 0;

   bufferGrabed = 0;


   jMul=0;
   bufferUsed = 0;
   hexHolder = 0x00;
   passed=0;

   hexChar[0] = "0";
   hexChar[1] = "1";

	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] = 0x03;

   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;

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

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

   camReset[0] = 0xAA;
   camReset[1] = 0x08;
   camReset[2] = 0x00;
   camReset[3] = 0x00;
   camReset[4] = 0x00;
   camReset[5] = 0xFF;

   camSleep[0] = 0xAA;
   camSleep[1] = 0x09;
   camSleep[2] = 0x00;
   camSleep[3] = 0x00;
   camSleep[4] = 0x00;
   camSleep[5] = 0x00;

   endPic[0] = 0xAA;
   endPic[1] = 0x0E;
   endPic[2] = 0x00;
   endPic[3] = 0x00;
   endPic[4] = 0xF0;
   endPic[5] = 0xF0;










   serBclose();
   serBopen(camBaudRate);
	serBwrFlush();
   serBrdFlush();
   printf("\n");
   while(i<CAMTESTLIMIT)
   {
   	WrPortI(PEB6R, NULL, 0xFF);				//LED Control
      if(DEBUGSYS)printf("CAM Test: %d\n",i);
      serBwrite(camSync, 6);
      serBread(camReceive, 6, 50);
      WrPortI(PEB6R, NULL, 0x00);				//LED Control
      if(camReceive[0]==0xAA && camReceive[1]==0x0E && camReceive[2]==0x0D)
      {
	    //  giveSTime();
       	WrPortI(PEB6R, NULL, 0xFF); 			//LED Control
	      camReceive[0]=0x00;
	      camReceive[1]=0x00;
	      camReceive[2]=0x00;
	      if(DEBUGSYS)printf("ACK Received!\n");
         //Flushing B breaks flow
	      //printf("Flushing Serial Port B\n");
	      //serBrdFlush();
	      serBread(camReceive, 6, 200);
         WrPortI(PEB6R, NULL, 0x00);			//LED Control
	      if(camReceive[0]==0xAA && camReceive[1]==0x0D && camReceive[2]==0x00 && camReceive[3]==0x00 && camReceive[4]==0x00 && camReceive[5]==0x00)
	      {
            WrPortI(PEB6R, NULL, 0xFF);		//LED Control
            //giveSTime();
	         camReceive[0]=0x00;
	         camReceive[1]=0x00;
	         camReceive[2]=0x00;
         	if(DEBUGSYS)printf("SYNC Received!\n");
            serBwrite(camSyncR,6);
            giveSTime();
            //giveSTime();
            serBwrite(camInit,6);
            giveSTime();
            serBread(camReceive, 6, 200);
            WrPortI(PEB6R, NULL, 0x00);		//LED Control
            if(camReceive[1]== 0x0E)
            {
            	//printf("!0E!%X!",camReceive[1]);
            	WrPortI(PEB6R, NULL, 0xFF);			//LED Control
            	camReceive[0]=0x00;
	         	camReceive[1]=0x00;
	         	camReceive[2]=0x00;
         		if(DEBUGSYS)printf("ACK Received - Initial\n");
               serBwrite(camPkgSize,6);
               //giveSTime();
               giveSTime();
               serBread(camReceive, 6, 200);
               if(camReceive[1]== 0x0E && camReceive[2]== 0x06)
               {
               	//printf("!06!%X!",camReceive[2]);
               	WrPortI(PEB6R, NULL, 0x00);		//LED Control
            		camReceive[0]=0x00;
	         		camReceive[1]=0x00;
	         		camReceive[2]=0x00;
               	if(DEBUGSYS)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);		//LED Control
               		camReceive[0]=0x00;
	         			camReceive[1]=0x00;
	         			camReceive[2]=0x00;
               		if(DEBUGSYS)printf("ACK Received - SnapShot\n");
                     serBwrite(camGetPic, 6);
                     giveSTime();
                     serBread(camReceive, 6, 200);
                     if(camReceive[1]== 0x0E && camReceive[2]== 0x04)
               		{
	                     WrPortI(PEB6R, NULL, 0x00);		//LED Control
	                     camReceive[0]=0x00;
	                     camReceive[1]=0x00;
	                     camReceive[2]=0x00;
	                     if(DEBUGSYS)printf("ACK Received - GetPic\n");
                        giveSTime();
                        serBread(camReceive, 6, 200);
	                     if(camReceive[1]== 0x0A && camReceive[2]== 0x01)
	                     {
	                        WrPortI(PEB6R, NULL, 0xFF);		//LED Control
	                        camReceive[0]=0x00;
	                        camReceive[1]=0x00;
	                        camReceive[2]=0x00;
	                        if(DEBUGSYS)printf("ACK Received - SSData\n\n");
                           serBrdFlush();

                           packFetchCalls=0;
                           camAck[4]=0x00;
                           for(k=0;k<15;++k)
                           	byteTracker[k]=0;
                           for(k=0;k<15;++k)
                           	packetSize[k]=0;

                        	byteTracker[packFetchCalls]=serBrdUsed();
                           ++packFetchCalls;
                           bufferGrabed=0;
                           packetReq = 0;

                           WrPortI(PEB6R, NULL, 0x00);		//LED Control
                           while(byteTracker[packFetchCalls-1] != byteTracker[packFetchCalls] || packFetchCalls==1)
                           {
                           	WrPortI(PEB6R, NULL, 0xFF);		//LED Control
                              byteTracker[packFetchCalls]=serBrdUsed();
                              ++packFetchCalls;
                              //printf("Sending Packet Request\n");
                              serBwrite(camAck, 6);
                              ++packetReq;
                              giveSTime();
	                           //giveSTime();
	                           byteTracker[packFetchCalls]=serBrdUsed();
                              if(DEBUGSYS)printf("serial B buffer Used: %d\n", serBrdUsed());

                              if(serBrdUsed() == 4095)
                              {
                                 ++bufferGrabed;
                                 if(DEBUGSYS)printf("buffer limit reached, dumping buffer\n");
                                 if(bufferGrabed==1)
                                 {
                                    if(DEBUGSYS)printf("Dumping into PICTURE\n");
                                 	serBread(picture,serBrdUsed(),400);
                                 }
                                 else if(bufferGrabed==2)
                                 {
                                 	if(DEBUGSYS)printf("Dumping into PICTURE2\n");
                                 	serBread(picture2,serBrdUsed(),400);
                              	}
                              }
	                           camAck[4]=camAck[4] + 0x01;
                           }
                           packFetchCalls = packFetchCalls - 2;

                           //for(k=1;k<=packFetchCalls;++k)
                           //	packetSize[k]=byteTracker[k+1]-byteTracker[k];


                           --packetReq;
                           totalPic=0;
                           lastPacket=0;
                           totalPic = (bufferGrabed)*4095 + serBrdUsed();
                           lastPacket=totalPic-(totalPic/512)*512;

                           WrPortI(PEB6R, NULL, 0x00);		//LED Control
                           //printf("suc packetReq: 	%d\n",packetReq);
                           //printf("bufferDump: 	%d\n",bufferGrabed);
                           //printf("totalPic:		%d\n",totalPic);
                           //printf("lastPacket:	%d\n",lastPacket);





                           //printf("packFetchCalls: %d\n",packFetchCalls);


                           //lastPacket = serBrdUsed();

                           //printf("Bytes Used for Picture: %d\n",totalPic);
                           //printf("Last Packet: %d\n", lastPacket);

                           if(bufferGrabed==0)
                           {
                           	if(DEBUGSYS)printf("Dumping into Picture\n");
                             	serBread(picture,serBrdUsed(),400);
                           }
                           else if(bufferGrabed==1)
                           {
                           	if(DEBUGSYS)printf("Dumping into Picture2\n");
                           	serBread(picture2,serBrdUsed(),400);
                           }
                           else if(bufferGrabed==2)
                           {
                           	if(DEBUGSYS)printf("Dumping into Picture3\n");
                           	serBread(picture3,serBrdUsed(),400);
                           }


                           picUpperLimit=0;
                        	picUpperLimit2=0;
                        	picUpperLimit3=0;

                           if(totalPic <= 4095)
                           {
                            	picUpperLimit=totalPic;
                            	picUpperLimit2=0;
                              picUpperLimit3=0;
                           }
                           else if(4095 < totalPic && totalPic <= 8190)
                           {
                            	picUpperLimit=4095;
                            	picUpperLimit2=totalPic-4095;
                              picUpperLimit3=0;
                           }
                           else if(8190 < totalPic && totalPic <= 12285)
                           {
                            	picUpperLimit=4095;
                            	picUpperLimit2=4095;
                              picUpperLimit3=totalPic-8190;
                           }
                           packetPrinted=0;
                           if(DEBUGSYS)printf("Got Picture\n");
                           serBwrite(endPic,6);
                           giveSTime();
	                        i=300;
	                        passed=1;
                        }
                     }
                  }
               }
            }
	      }
   	}
      giveSTime();
   	++i;
      WrPortI(PEB6R, NULL, 0x00);
      if(i%70==0)
      {
      	if(DEBUGSYS)printf("Attempting Camera Reset\n");
         //serBclose();
   		//serBopen(camBaudRate);
         giveTime();
			serBwrFlush();
   		serBrdFlush();
         if(DEBUGSYS)printf("Sending Camera Reset\n");
         serBwrite(camReset, 6);

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








cofunc int getPic()
{

	//NEEDED VARS
	int gettingPic;
   char camIDSize[4];
   int t;


   int god;

	int i;
   int j;
   int k;
   int kHex2DecHelper;
   int k2Hex2DecHelper;
   int m;
   int jMul;	//multiplyer for j

   int packetPrinted;

   int bufferUsed;

   int bufferGrabed;

   char hexHolder;
   char hexChar[2];
   int passed;

   int byteTracker[35];
   int packetSize[35];
   int totalPic;
   int packFetchCalls;

   int lastPacket;


   char lastPacketSizeHEX[3];

   int camI;
   int camPacketSize;



   int bufferPointer1;
   int bufferPointer2;
   int origBbuff;

   int packetReq;


   packetReq=0;

   camPacketSize=512;

   i=0;
   j=0;

   totalPic = 0;
   lastPacket = 0;

   bufferGrabed = 0;

   jMul=0;
   bufferUsed = 0;
   hexHolder = 0x00;
   passed=0;

   hexChar[0] = "0";
   hexChar[1] = "1";

   //CLEAR VARS
   gettingPic=0;
   t=0;

   while(i<CAMTESTLIMIT)
   {
   	if(DEBUGSYS)printf("Trying to connect Cam\n");
      serBwrite(camSync, 6);
      serBread(camReceive, 6, 200);
      if(camReceive[0]==0xAA && camReceive[1]==0x0E && camReceive[2]==0x0D)
      {
       	WrPortI(PEB6R, NULL, 0xFF); 			//LED Control



	      if(DEBUGSYS)printf("ACK Received!\n");
	      serBread(camReceive, 6, 200);
         WrPortI(PEB6R, NULL, 0x00);			//LED Control
	      if(camReceive[0]==0xAA && camReceive[1]==0x0D && camReceive[2]==0x00 && camReceive[3]==0x00 && camReceive[4]==0x00 && camReceive[5]==0x00)
	      {
            WrPortI(PEB6R, NULL, 0xFF);		//LED Control



         	if(DEBUGSYS)printf("SYNC Received!\n");
            serBwrite(camSyncR,6);
            waitfor(DelayMs(camDelay));
            serBwrite(camInit,6);
            waitfor(DelayMs(camDelay));
            serBread(camReceive, 6, 200);
            WrPortI(PEB6R, NULL, 0x00);		//LED Control
            if(camReceive[1]== 0x0E)
            {
            	WrPortI(PEB6R, NULL, 0xFF);			//LED Control
         		if(DEBUGSYS) printf("ACK Received - Initial\n");
               serBwrite(camPkgSize,6);
	            waitfor(DelayMs(camDelay));
               serBread(camReceive, 6, 200);
               if(camReceive[1]== 0x0E && camReceive[2]== 0x06)
               {
               	WrPortI(PEB6R, NULL, 0x00);		//LED Control



               	if(DEBUGSYS)printf("ACK Received - PkgSize\n");
                  serBwrite(camSnapShot, 6);
	               waitfor(DelayMs(camDelay));
                  serBread(camReceive, 6, 200);
                  if(camReceive[1]== 0x0E && camReceive[2]== 0x05)
               	{
                  	WrPortI(PEB6R, NULL, 0xFF);		//LED Control



               		if(DEBUGSYS)printf("ACK Received - SnapShot\n");
                     serBwrite(camGetPic, 6);
	                  waitfor(DelayMs(camDelay));
                     serBread(camReceive, 6, 200);
                     if(camReceive[1]== 0x0E && camReceive[2]== 0x04)
               		{
	                     WrPortI(PEB6R, NULL, 0x00);		//LED Control



	                     if(DEBUGSYS) printf("ACK Received - GetPic\n");
	                     waitfor(DelayMs(camDelay));
                        serBread(camReceive, 6, 200);
	                     if(camReceive[1]== 0x0A && camReceive[2]== 0x01)
	                     {
	                        WrPortI(PEB6R, NULL, 0xFF);		//LED Control



	                        if(DEBUGSYS)printf("ACK Received - SSData\n\n");
                           serBrdFlush();
                           packFetchCalls=0;
                           camAck[4]=0x00;
                           for(k=0;k<15;++k)
                           	byteTracker[k]=0;
                           for(k=0;k<15;++k)
                           	packetSize[k]=0;

                        	byteTracker[packFetchCalls]=serBrdUsed();
                           ++packFetchCalls;
                           bufferGrabed=0;
                           packetReq = 0;

                           WrPortI(PEB6R, NULL, 0x00);		//LED Control

                           gettingPic=1;
                           for(t=0;t<4;++t)
                           	camIDSize[t]=0x00;

                           waitfor(usingSD==0);
                           usingSD=1;
                           wfd writeSDcamTime();
                           usingSD=0;



                           while(gettingPic)
                           {
                           	serBrdFlush();
                              serBwrite(camAck, 6);
                              waitfor(serBrdUsed()>4);
                              for(t=0;t<4;++t)
                              	camIDSize[t]=serBgetc();
                              if(camIDSize[2]==0xFA && camIDSize[3]==0x01)
                              {
                                 waitfor(serBrdUsed()==508);
                                 wfd cof_serBread(picPacket,508,300);

                              	waitfor(usingSD==0);
	                              usingSD=1;
                                 wfd writeSDcam(506);
                                 usingSD=0;
                              }
                              else
                              {

                                 //READ LAST PACKET SIZE IN HEX CONVERT TO DEC
                                 lastPacketSizeHEX[0]=camIDSize[3];
                                 lastPacketSizeHEX[1]=camIDSize[2];

                                 if(lastPacketSizeHEX[0]>0)
                                 	kHex2DecHelper=lastPacketSizeHEX[0] + 255;
                                 else
                                 	kHex2DecHelper=0;

                                 k2Hex2DecHelper=lastPacketSizeHEX[1];
                                 kHex2DecHelper=kHex2DecHelper+k2Hex2DecHelper;

                                 waitfor(serBrdUsed()>=kHex2DecHelper+2);			//Plus 2 for check sum

                                 wfd cof_serBread(picPacket,kHex2DecHelper+2,300);

                                 waitfor(usingSD==0);
                                 usingSD=1;
                                 wfd writeSDcam(kHex2DecHelper);				//Check sum no longer needed
                                 usingSD=0;
                                 gettingPic=0;		//Picture Caught Break
                              }
                              camAck[4]=camAck[4] + 0x01;
                           }

                           WrPortI(PEB6R, NULL, 0x00);		//LED Control
                           serBwrite(endPic,6);
	                        waitfor(DelayMs(camDelay));
	                        i=300;
	                        passed=1;
                        }
                     }
                  }
               }
            }
	      }
   	}

	   //giveSTime();
	   waitfor(DelayMs(camDelay));


   	++i;
      WrPortI(PEB6R, NULL, 0x00);
      if(i%70==0)
      {
      	if(DEBUGSYS)printf("\n\n\n\nAttempting Camera Reset\n");
         serBclose();
         serBopen(camBaudRate);
			serBwrFlush();
   		serBrdFlush();
         if(DEBUGSYS)printf("Sending Camera Reset\n");
         serBwrite(camReset, 6);
         waitfor(DelayMs(camDelay));

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








int prepSD()
{
	int i;
   int result;

   i=0;
   result=0;
   while(i<15 && result==0)
   {
   	if(DEBUGSYS)printf("SD Test: %d\n",i);
   	WrPortI(PEB1R, NULL, 0xFF);		//LED Control
   	WrPortI(PEB0R, NULL, 0xFF); 		//LED Control
   	result = initSD();
      WrPortI(PEB1R, NULL, 0x00);		//LED Control
      WrPortI(PEB0R, NULL, 0x00); 		//LED Control
      giveSTime();
   }

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

int initSD()
{
   int j;
   char fileName[37];
   char SDresponse[100];
   int i;
   int passed;
   int k;
   int defaultNamer;
   int timeGrabbed;

   timeGrabbed=0;
   k=0;
	passed=0;

	resetSD();
   serDopen(9600);
   giveSTime();
   writeSDcBc("B 05A9\r");
   giveSTime();
   serDclose();
   giveSTime();
   serDopen(baud_rate);
   giveSTime();
   serDrdFlush();
   writeSDcBc("# 1\r");
   writeSDcBc("I\r");
   if(GPSGTG)
   {
      while(k<TIMTESTLIMIT && (timeStamp[0] <= 0 || timeStamp[0] > 12))
      {
      	if(DEBUGSYS)printf("pulling time: %d, abort @ 150\n", k);
      	giveSTime();
      	getGPS();
         ++k;
      }
      if(k<TIMTESTLIMIT)
   	{
      	timeGrabbed=1;
	      j=sprintf(currentFileName, "O 1W>DL%02d%02d%02d.TXT\r",timeStamp[1], timeStamp[3], timeStamp[4]);     //make currentFileName Write
	      //j=sprintf(currentFileName, "O 1W>%02d%02d%02d%02d.TXT\r",timeStamp[0], timeStamp[1], timeStamp[3], timeStamp[4]);     //make currentFileName Write
	      if(DEBUGSYS)printf("\nName for file: %s\n",currentFileName);
	      writeSDcBc(currentFileName);
	      j=sprintf(currentFileName, "O 1A>DL%02d%02d%02d.TXT\r",timeStamp[1], timeStamp[3], timeStamp[4]);     //make currentFileName Append
	       //j=sprintf(currentFileName, "O 1A>%02d%02d%02d%02d.TXT\r",timeStamp[0], timeStamp[1], timeStamp[3], timeStamp[4]);    //make currentFileName Append
	      if(camGTG)
	      {
	         j=sprintf(currentFileNamePic, "O 2W>PL%02d%02d%02d.TXT\r",timeStamp[1], timeStamp[3], timeStamp[4]);     //make currentFileName Write
	         if(DEBUGSYS)printf("\nName for pic file: %s\n",currentFileNamePic);
	         writeSDcBc(currentFileNamePic);
	         j=sprintf(currentFileNamePic, "O 2A>PL%02d%02d%02d.TXT\r",timeStamp[1], timeStamp[3], timeStamp[4]);
	      }
   	}
   }
   if(timeGrabbed==0)
   {
	   defaultNamer = SEC_TIMER;
	   if(DEBUGSYS)printf("\n%d\n",defaultNamer);
	   if(defaultNamer < 0)
	      defaultNamer = -1*defaultNamer;
	   while(defaultNamer>9999)
	   {
	      defaultNamer = defaultNamer/10;
	   }
	   if(DEBUGSYS)printf("\n%04d\n",defaultNamer);
      j=sprintf(currentFileName, "O 1W>DLOG%04d.TXT\r", defaultNamer);
      if(DEBUGSYS)printf("\n%s\n",currentFileName);
      //writeSDcBc("O 1W>DATALOGS.TXT\r");
      writeSDcBc(currentFileName);
      j=sprintf(currentFileName, "O 1A>DLOG%04d.TXT\r", defaultNamer);
      //j=sprintf(currentFileName, "O 1A>DATALOGS.TXT\r");
      if(camGTG)
      {
         j=sprintf(currentFileNamePic, "O 2W>PLOG%04d.TXT\r", defaultNamer);     //make currentFileNamePic Write
         if(DEBUGSYS)printf("\nName for pic file: %s\n",currentFileNamePic);
         writeSDcBc(currentFileNamePic);
         j=sprintf(currentFileNamePic, "O 2A>PLOG%04d.TXT\r", defaultNamer);
      }
   }
   writeSDcBc("W 1>D0\r");
   writeSDcBc("UTC Time            $$ UTC Position(Lat,Lon)          $$ Yaw $$ Accel(x,y)  $$ OBD(RPM,Speed,Throttle)\r\n");
   writeSDcBc("------------------------------------------------------------------------------------------------------\r\n");
   writeSDcBc("F 1\r");

   //writeSDcBc("W 2>E0\r");
   //writeSDcBc("JPEG PICTURES                                                                                                 \r\n");
   //writeSDcBc("--------------------------------------------------------------------------------------------------------------\r\n");
   //writeSDcBc("F 2\r");

   serDread(SDresponse,serDrdUsed(),400);
   if(DEBUGSYS)
   {
   	printf("%c\n",SDresponse[28]);
   	printf("%c\n",SDresponse[29]);
   	printf("%c\n",SDresponse[30]);
   }
   //writeSDcBc("F 1\r");
   //printf("File Name is: %s\n",currentFileName);

   if(SDresponse[28] == '!' && SDresponse[29] == '0' && SDresponse[30] == '0')
   	return 1;
   else
   	return 0;
}

void resetSD()
{
   BitWrPortI(PBDR, &resetShadow, 0, 6);
   giveSTime();
   BitWrPortI(PBDR, &resetShadow, 1, 6);
   giveSTime();
}

void writeSDcBc(char str2write[])
{
	int j;
   int loopBreaker;
   loopBreaker = 0;


   for(j=0; j<strlen(str2write);++j)
   {
   loopBreaker=0;
   	while(BitRdPortI(PBDR, 5) && loopBreaker < 5000)
      {
      	giveSTime();
         ++loopBreaker;
      }
      while(serDwrFree()<10 && loopBreaker < 5000)
      {
      	giveSTime();
         ++loopBreaker;
      }
      if(loopBreaker < 1500)
      	serDputc(str2write[j]);
   }
}


cofunc int SDwrites(char toBeWritten[], int forceWrite, int delay)
{
   int j;
   if(forceWrite == 0)
   	waitfor(1);
   waitfor(DelayMs(delay));
   for(j=0; j<strlen(toBeWritten); j++)
   {
      while(BitRdPortI(PBDR, 5))
      {
      	printf("wait ");
         waitfor(DelayMs(SD_FLAG_DELAY));
      }
      waitfor(serDwrFree()>10);
     	wfd cof_serDputc(toBeWritten[j]);
 	}
   return 1;
}



cofunc int writeSD3(char data[])
{
   WrPortI(PEB0R, NULL, 0xFF); 		//LED SIGNAL
   waitfor(serDwrUsed()==0);
   wfd cof_serDputs(currentFileName);
   while(BitRdPortI(PBDR, 5))
   {
      printf("wait ");
      waitfor(DelayMs(SD_FLAG_DELAY));
   }
   wfd cof_serDputs("W 1>5D\r");
   while(BitRdPortI(PBDR, 5))
   {
      printf("wait ");
      waitfor(DelayMs(SD_FLAG_DELAY));
   }
   wfd cof_serDputs(data);
   while(BitRdPortI(PBDR, 5))
   {
      printf("wait ");
      waitfor(DelayMs(SD_FLAG_DELAY));
   }
	wfd cof_serDputs("F 1\r");
   WrPortI(PEB0R, NULL, 0x00); 	//LED SIGNAL
}

cofunc int writeSDcam(int picPacketSize)
{
   char sizeDecleration[15];
   int totalpicPacketSize;
   int l;


   waitfor(serDwrUsed()==0);
   wfd cof_serDputs(currentFileNamePic);

   //Find total Size of Data to be written
   totalpicPacketSize = ((picPacketSize*2));

   //Create Size Decleration
   sprintf(sizeDecleration, "%s%X%s", "W 2>", totalpicPacketSize, "\r");

   while(BitRdPortI(PBDR, 5))
   {
      printf("wait ");
      waitfor(DelayMs(SD_FLAG_DELAY));
   }
   wfd cof_serDputs(sizeDecleration);

   //Send data to SD one HEX byte at a time
   waitfor(serDwrFree()>=1014);

   for(l=0;l<picPacketSize;++l)
   {
   	sprintf(tempString,"%02X\0", picPacket[l]);
       wfd SDwrites(tempString, 1, 0);
   }


   while(BitRdPortI(PBDR, 5))
   {
      printf("wait ");
      waitfor(DelayMs(SD_FLAG_DELAY));
   }
   //printf("\nClosing Picture File");
	wfd cof_serDputs("F 2\r");
}



cofunc int writeSDcamTime()
{
   char sizeDecleration[15];
   int totalpicPacketSize;
   int l;

   waitfor(serDwrUsed()==0);

   wfd cof_serDputs(currentFileNamePic);
   while(BitRdPortI(PBDR, 5))
   {
      printf("wait ");
      waitfor(DelayMs(SD_FLAG_DELAY));
   }

	//Create Time Stamp
	sprintf(tempString2,"*%02d-%02d-%04d %02d:%02d:%02d*\0", timeStamp[0], timeStamp[1], timeStamp[2], timeStamp[3], timeStamp[4], timeStamp[5]);
   sprintf(sizeDecleration, "%s%X%s", "W 2>", strlen(tempString2),"\r");
   wfd cof_serDputs(sizeDecleration);
   while(BitRdPortI(PBDR, 5))
   {
      printf("wait ");
      waitfor(DelayMs(SD_FLAG_DELAY));
   }
   wfd cof_serDputs(tempString2);
   while(BitRdPortI(PBDR, 5))
   {
      printf("wait ");
      waitfor(DelayMs(SD_FLAG_DELAY));
   }
	wfd cof_serDputs("F 2\r");
}








/* 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(PEB0R, NULL, 0x00);
	WrPortI(PEB1R, NULL, 0x00);
	WrPortI(PEB3R, NULL, 0x00);
   WrPortI(PEB4R, NULL, 0x00);
   WrPortI(PEB5R, NULL, 0x00);
   WrPortI(PEB6R, NULL, 0x00);
   WrPortI(PEB7R, NULL, 0x00);
}

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

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

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

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

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

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

void flashLED6()
{
	WrPortI(PEB0R, NULL, 0xFF);
	giveSTime();
	WrPortI(PEB0R, NULL, 0x00);
}

void flashLED7()
{
	WrPortI(PEB1R, NULL, 0xFF);
	giveSTime();
	WrPortI(PEB1R, NULL, 0x00);
}

void testLEDs()
{
		lightsOff();
	giveMTime();
		lightsOn();
	giveMTime();
		lightsOff();
	giveMTime();
		lightsOn();
	giveMTime();
		lightsOff();
	giveMTime();
		lightsOn();
	giveMTime();
   	lightsOff();
	giveMTime();
		lightsOn();
	giveMTime();
   	lightsOff();
}



void signalMaster()
{
   //Notify User of Power Up
   WrPortI(PEB1R, NULL, 0xFF);
   giveSTime();
   WrPortI(PEB1R, NULL, 0x00);
   giveSTime();
   WrPortI(PEB1R, NULL, 0xFF);
   giveSTime();
   WrPortI(PEB1R, NULL, 0x00);
   giveSTime();

}
/* END: OTHER FUNCTIONS */