cofunc int SDwrites(char[], int, int);
cofunc int SDwriteUTC(char[], int, char[], int, int, int, int, int);
cofunc int SDwriteInt(int, int);
cofunc int SDwriteFloat(float, int);
cofunc int SDwriteAccelYaw(int);
cofunc int SDwritePos(int, float, char[], int);
cofunc int SDreset();
cofunc int SDsetBaudRate(long);
int getYawRate();
void AccelInit();
//void YawRateInit();
#class auto
#use "gps.lib"
//#define SPI_SER_B
//#define SPI_CLK_DIVISOR		5
//#use "spi.lib"  				// make spi defines above this line
#define CINBUFSIZE 127
#define COUTBUFSIZE 127
#define DINBUFSIZE 127
#define DOUTBUFSIZE 127
#define MAX_SENTENCE 100
#define DELAY_LONG 500
#define DELAY_SHORT 0
#define YawAverage 2072  		// This value can be found with the average raw yaw rate value
struct
{
	float x;            // in units of (g)
   float y;
}Accelleration;
const char dayname[7][4] = {"Sun","Mon","Tue","Wed","Thu","Fri","Sat"};
const char monthname[12][4] = {"Jan", "Feb", "Mar", "Apr", "May", "Jun",
	"Jul", "Aug", "Sep", "Oct", "Nov", "Dec"};
GPSPosition current_pos;
struct tm current_time;
static const int cr = 0xD;					// for DOSonCHIP baud rate set function
static const long baud_rate = 460800L;
int writeToken;
int resetShadow;
//int DOUTShadow;
//int DINShadow;
int i;
int SDinitialized;
int temp;
char SPI_Reading[2];
char LSB;
char MSB;
char iccsr;
char capture_status;
unsigned long pulse_widthX;
unsigned long pulse_widthY;
char adc_reading[2];					// start yawrate variables
int adc_sample;
int currentYawRate[5];
char outputchar[2];              // end   yawrate variables
int main()
{
   char sentence[MAX_SENTENCE];	// incoming GPS sentence
	int input_char;
	int string_pos;
	char dir_string[2];
   brdInit();
   SDinitialized = 0;
   string_pos = 0;
	dir_string[1] = 0;
   writeToken = 1;
   serCopen(4800);					// GPS NMEA 0183
   WrPortI(PBDDR, NULL, 0xC1);
   BitWrPortI(PBDR, &resetShadow, 1, 6);
//   BitWrPortI(PCDR, &DINShadow, 0, 4);
   AccelInit();
//   YawRateInit();
	getYawRate();						// Initialize yaw rate gyro
   getYawRate();						// First yaw rate value is not valid
   loopinit();
	while(1)
   {
   	loophead();
   	costate
      {
         if(SDinitialized == 0)
         {
         	SDinitialized = 0;

          	wfd SDreset();
            serDopen(9600);
            wfd SDwrites("B 05A9\r", 1, DELAY_LONG);
            waitfor(DelayMs(1000));
            serDclose();
            waitfor(DelayMs(1000));
            serDopen(baud_rate);

            waitfor(DelayMs(1000));
            wfd SDwrites("# 1\r", 1, DELAY_LONG);
            wfd SDwrites("I\r", 1, DELAY_LONG);
   			wfd SDwrites("O 1W>UTEST.TXT\r", 1, DELAY_LONG);
   			wfd SDwrites("W 1>52\r", 1, DELAY_LONG); // 82 bytes = 32+50
            wfd SDwrites("UTC Time\t\t\t\tLatitude\tLongitude\r\n", 1, DELAY_LONG); //32 bytes
         	wfd SDwrites("------------------------------------------------\r\n", 1, DELAY_LONG); //50 bytes
            wfd SDwrites("F 1\r", 1, DELAY_LONG);
            printf("\nSD Initialized\n");
				SDinitialized = 1;
         }
     	}

      costate
      {
         wfd input_char = cof_serCgetc();
	  		if(input_char == '\r' || input_char == '\n')
			{
				sentence[string_pos] = 0; //add null
				//printf("%s\n", sentence);
            if(gps_get_utc(&current_time, sentence) == 0)
				{
              	if(writeToken==1)
               {
               	wfd SDwriteUTC(
               		dayname[current_time.tm_wday],
					  		current_time.tm_mday,
							monthname[current_time.tm_mon - 1],
					  		1900 + current_time.tm_year,
							current_time.tm_hour,
							current_time.tm_min,
							current_time.tm_sec,
                  	DELAY_SHORT);
                  printf("UTC: %s %d-%s-%d %02d:%02d:%02d\n",
						dayname[current_time.tm_wday],
						current_time.tm_mday,
						monthname[current_time.tm_mon - 1],
						1900 + current_time.tm_year,
						current_time.tm_hour,
						current_time.tm_min,
						current_time.tm_sec );

               }
            }
            if(gps_get_position(&current_pos, sentence) == 0)
				{
					dir_string[0] = current_pos.lat_direction;
               if(writeToken==2)
               {
               	wfd SDwritePos(current_pos.lat_degrees, current_pos.lat_minutes, dir_string, DELAY_SHORT);
               	/*printf("Latitude: %d %f' %s\n",
						current_pos.lat_degrees, current_pos.lat_minutes,
						dir_string);
                  */
               	dir_string[0] = current_pos.lon_direction;
						wfd SDwritePos(current_pos.lon_degrees, current_pos.lon_minutes, dir_string, DELAY_SHORT);
               	writeToken=3;
               	//wfd SDwrites("\n\r", 0, DELAY);
               	/*printf("Longitude: %d %f' %s\n",
						current_pos.lon_degrees, current_pos.lon_minutes,
						dir_string);
                  */
  					}
				}

				string_pos = 0;
			}
			else if(input_char > 0)
			{
        		//printf("%c,", input_char);
		  		sentence[string_pos] = input_char;
				string_pos++;
				if(string_pos == MAX_SENTENCE)
					string_pos = 0;	//reset string if too large
			}
      }
      costate
      {	//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;
			}
      }

      costate
      {
         if(writeToken==3)
         {
            waitfor(DelayMs(10));
            wfd SDwriteAccelYaw(DELAY_SHORT);
            waitfor(DelayMs(100));
            wfd SDwriteAccelYaw(DELAY_SHORT);
            waitfor(DelayMs(100));
            wfd SDwriteAccelYaw(DELAY_SHORT);
            waitfor(DelayMs(100));
            wfd SDwriteAccelYaw(DELAY_SHORT);
            waitfor(DelayMs(100));
            wfd SDwriteAccelYaw(DELAY_SHORT);
            waitfor(DelayMs(100));
            wfd SDwriteAccelYaw(DELAY_SHORT);
            waitfor(DelayMs(100));
            wfd SDwriteAccelYaw(DELAY_SHORT);
            waitfor(DelayMs(100));
            wfd SDwriteAccelYaw(DELAY_SHORT);
            waitfor(DelayMs(100));
            wfd SDwriteAccelYaw(DELAY_SHORT);
            /*waitfor(DelayMs(100));
            wfd SDwriteAccelYaw(DELAY_SHORT);
            */
         	writeToken=1;
         }
      }
      costate
      { ;
         /*BitWrPortI(PBDR, &PBDRShadow, 0, 7);	// chip select low
      	waitfor(DelayMs(1));
			//SPIRead(adc_reading, 2);
      	SPIWrRd(outputchar, adc_reading, 2);
      	BitWrPortI(PBDR, &PBDRShadow, 1, 7);	// chip select high
			//printf("SPI bytes = 0x%x 0x%x\n", adc_reading[0], adc_reading[1]);
			adc_sample = (adc_reading[1] & 0xFF);
      	adc_sample += ((adc_reading[0] & 0x0F)*256);
			//printf("ADC value:                     %d\n", adc_sample);
         //waitfor(DelayMs(1));
         if(adc_sample > 1)
         {
         	currentYawRate[4]=currentYawRate[3];
				currentYawRate[3]=currentYawRate[2];
            currentYawRate[2]=currentYawRate[1];
            currentYawRate[1]=adc_sample;
            currentYawRate[0]=((currentYawRate[1]+currentYawRate[2]+currentYawRate[3]+currentYawRate[4])/4);
           // printf("curr yaw rt %d", currentYawRate[0]);
         }
         */
      }
	}
}

cofunc int SDwriteAccelYaw(int delay)
{
   int places;
   int a;
   long int intNumber;
   char b[3];
   int count;
   float x;
   float y;
   int yawDegrees;
   places = 2;
   waitfor(SDinitialized);
	count=0;

   yawDegrees = getYawRate() - YawAverage;
   //printf("adc - %d\n",adc_sample);
   //printf("%d\n", currentYawRate[0]);
   yawDegrees /= (YawAverage/300);



   x=Accelleration.x;
   y=Accelleration.y;

   if(x < 0)
   	count++;
   if(y < 0)
   	count++;
   if(yawDegrees<0)
   	count++;
   if(abs(yawDegrees)>=10)
   	count++;
   if(abs(yawDegrees)>=100)
   	count++;


   wfd SDwrites("F 1\r", 0, delay);
   wfd SDwrites("O 1A>UTEST.TXT\r", 0, delay);
   if(count==0)
   	wfd SDwrites("W 1>13\r", 0, delay);
   if(count==1)
   	wfd SDwrites("W 1>14\r", 0, delay);
   if(count==2)
   	wfd SDwrites("W 1>15\r", 0, delay);
   if(count==3)
   	wfd SDwrites("W 1>16\r", 0, delay);
   if(count==4)
   	wfd SDwrites("W 1>17\r", 0, delay);
   if(count==5)
   	wfd SDwrites("W 1>18\r", 0, delay);

	wfd SDwrites("\r\n\t", 0, delay);
   if(x < 0)
   {
      wfd SDwrites("-", 0, delay);
      x *= -1;
   }
   intNumber=x*100;
   for(a=0; a<places; a++)
   {
   	b[(places - a) - 1]=(intNumber%10)+48;
      intNumber=intNumber/10;
   }
   b[2]='\0';

   wfd SDwriteInt(intNumber, delay);
   wfd SDwrites(".", 0, delay);
   wfd SDwrites(b, 0, delay);

   wfd SDwrites("  ", 0, delay);

   if(y < 0)
   {
      wfd SDwrites("-", 0, delay);
      y *= -1;
   }
   intNumber=y*100;
   for(a=0; a<places; a++)
   {
   	b[(places - a) - 1]=(intNumber%10)+48;
      intNumber=intNumber/10;
   }
   b[2]='\0';
   wfd SDwriteInt(intNumber, delay);
   wfd SDwrites(".", 0, delay);
   wfd SDwrites(b, 0, delay);


   wfd SDwrites("  ", 0, delay);

   if(yawDegrees < 0)
   {
      wfd SDwrites("-", 0, delay);
      yawDegrees *= -1;
   }
   intNumber=yawDegrees*100;
   for(a=0; a<places; a++)
   {
   	b[(places - a) - 1]=(intNumber%10)+48;
      intNumber=intNumber/10;
   }
   b[2]='\0';
   wfd SDwriteInt(intNumber, delay);
   wfd SDwrites(".", 0, delay);
   wfd SDwrites(b, 0, delay);
   //printf("%d.%d\n", intNumber, b[0]);

   return 1;
}

cofunc int SDwriteFloat(float floatNumber, int delay)
{
   int places;
   int a;
   long int intNumber;
   char b[7];
   places = 6;
   waitfor(SDinitialized);

   intNumber=floatNumber*1000000;
   for(a=0; a<places; a++)
   {
   	b[(places - a) - 1]=(intNumber%10)+48;
      intNumber=intNumber/10;
   }
   b[6]='\0';
   wfd SDwriteInt(intNumber, delay);
   wfd SDwrites(".", 0, delay);
   wfd SDwrites(b, 0, delay);
   return 1;
}

cofunc int SDwritePos(int degrees, float minutes, char direction[], int delay)
{
   waitfor(SDinitialized);
   waitfor(writeToken==2);
   wfd SDwrites("\t", 0, delay);
   wfd SDwrites(direction, 0, delay);
   wfd SDwrites("' ", 0, delay);
   wfd SDwriteInt(degrees, delay);
   wfd SDwrites(" ", 0, delay);
   wfd SDwriteFloat(minutes, delay);
   return 1;
}

cofunc int SDwriteInt(int intToBeSent, int delay)
{
	int i;
   int j;
   char reversedInts[12];
   char forwardInts[13];
   waitfor(SDinitialized);
   reversedInts[0] = (intToBeSent%10)+48;
   intToBeSent=intToBeSent/10;
   for(i=1; intToBeSent; i++)
   {
      reversedInts[i] = (intToBeSent%10)+48;
      intToBeSent=intToBeSent/10;
	}
   i--;
   for(j=0; i>=0; i--)
   {
   	forwardInts[j]=reversedInts[i];
      j++;
   }
   forwardInts[j]='\0';
   wfd SDwrites(forwardInts, 0, delay);
   return 1;
}
cofunc int SDwriteUTC(char day[], int date, char month[], int year, int hour, int minute, int second, int delay)
{	// UTC
   waitfor(SDinitialized);
   waitfor(writeToken==1);
   wfd SDwrites("F 1\r", 0, delay);
   wfd SDwrites("O 1A>UTEST.TXT\r", 0, delay);
   wfd SDwrites("W 1>3A\r", 0, delay);  // 58 decimal bytes
   wfd SDwrites("\r\n", 0, delay);
   wfd SDwrites(day, 0, delay);
   wfd SDwrites(" ", 0, delay);
   if(date<10)
   	wfd SDwriteInt(0,delay);
   wfd SDwriteInt(date, delay);
   wfd SDwrites(" ", 0, delay);
   wfd SDwrites(month, 0, delay);
   wfd SDwrites(" ", 0, delay);
   wfd SDwriteInt(year, delay);
   wfd SDwrites(" ", 0, delay);
   if(hour<10)
   	wfd SDwriteInt(0,delay);
   wfd SDwriteInt(hour, delay);
   wfd SDwrites(":", 0, delay);
   if(minute<10)
   	wfd SDwriteInt(0,delay);
   wfd SDwriteInt(minute, delay);
   wfd SDwrites(":", 0, delay);
   if(second<10)
   	wfd SDwriteInt(0,delay);
   wfd SDwriteInt(second, delay);
   writeToken=2;
   return 1;
}

cofunc int SDsetBaudRate(long baud_rate)    // for DOSonCHIP only, not for uALFAT
{
	serDopen(baud_rate);
   while(BitRdPortI(PBDR, 5)){printf("wait1 ");}
	serDwrite(&cr, 1);
	waitfor(DelayMs(10));
   while(BitRdPortI(PBDR, 5)){printf("wait2 ");}
 	serDwrite(&cr, 1);
   waitfor(DelayMs(100));
   return 1;
}


cofunc int SDreset()
{
   BitWrPortI(PBDR, &resetShadow, 0, 6);
   waitfor(DelayMs(500));
   BitWrPortI(PBDR, &resetShadow, 1, 6);
   waitfor(DelayMs(500));
   return 1;
}

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

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;
}

/*
void YawRateInit()
{
   SPIinit();
   SPIxor = 0x00;                         // yawrate spi bits non-inverted
	SPITXxor = 0x00;                       // yawrate spi bits non-inverted
   BitWrPortI(PBDR, &PBDRShadow, 1, 7);	// yawrate spi chip select high
   adc_reading[0]=0;
   adc_reading[1]=0;
   adc_sample=0;
   outputchar[0]=0xC1;
   outputchar[1]=0x08;
   currentYawRate[4]=YawAverage;
   currentYawRate[3]=YawAverage;
   currentYawRate[2]=YawAverage;
   currentYawRate[1]=YawAverage;
   currentYawRate[0]=YawAverage;

   for(i = 0; i < 10; i++)
		BitWrPortI(PBDR, &PBDRShadow, 0, 7);	// chip select low
   SPIWrite(outputchar, 2);
	//SPIWrRd(outputchar, adc_reading, 2);
	BitWrPortI(PBDR, &PBDRShadow, 1, 7);	// chip select high
	printf("SPI bytes = 0x%x 0x%x\n", adc_reading[0], adc_reading[1]);
	adc_sample = (adc_reading[1] & 0xFF);
   adc_sample += ((adc_reading[0] & 0x0F)*256);
	printf("ADC value:                     %d\n", adc_sample);

}
*/


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

   DOUT[0]=0;


   BitWrPortI(PBDR, PBDRShadow,  0, 7); 	//CS low
   BitWrPortI(PCDR, PCDRShadow,  1, 4);	//WRITE
   for(i=0;i<15;i++);

   BitWrPortI(PBDR, PBDRShadow,  0, 0);	//SCLK low
   BitWrPortI(PBDR, PBDRShadow,  1, 0);	//SCLK high
   DOUT[1]=BitRdPortI(PCDR, 5);				//DOUT low

	BitWrPortI(PCDR, PCDRShadow,  0, 4);	//LOW

   BitWrPortI(PBDR, PBDRShadow,  0, 0);	//SCLK low
   BitWrPortI(PBDR, PBDRShadow,  1, 0);	//SCLK high
   DOUT[2]=BitRdPortI(PCDR, 5);				//DOUT low

   BitWrPortI(PCDR, PCDRShadow,  0, 4);	//DC

   BitWrPortI(PBDR, PBDRShadow,  0, 0);	//SCLK low
   BitWrPortI(PBDR, PBDRShadow,  1, 0);	//SCLK high
   DOUT[3]=BitRdPortI(PCDR, 5);				//DOUT ADD1

   BitWrPortI(PCDR, PCDRShadow,  0, 4);	//DC

   BitWrPortI(PBDR, PBDRShadow,  0, 0);	//SCLK low
   BitWrPortI(PBDR, PBDRShadow,  1, 0);	//SCLK high
   DOUT[4]=BitRdPortI(PCDR, 5);				//DOUT ADD0

   BitWrPortI(PCDR, PCDRShadow,  0, 4); 	//ADD!

   BitWrPortI(PBDR, PBDRShadow,  0, 0);	//SCLK low
   BitWrPortI(PBDR, PBDRShadow,  1, 0);	//SCLK high
   DOUT[5]=BitRdPortI(PCDR, 5);				//DOUT DB11

   BitWrPortI(PCDR, PCDRShadow,  0, 4); 	//ADD0

   BitWrPortI(PBDR, PBDRShadow,  0, 0);	//SCLK low
   BitWrPortI(PBDR, PBDRShadow,  1, 0);	//SCLK high
   DOUT[6]=BitRdPortI(PCDR, 5);				//DOUT DB10

   BitWrPortI(PCDR, PCDRShadow,  1, 4); 	//HIGH

   BitWrPortI(PBDR, PBDRShadow,  0, 0);	//SCLK low
   BitWrPortI(PBDR, PBDRShadow,  1, 0);	//SCLK high
   DOUT[7]=BitRdPortI(PCDR, 5);				//DOUT DB9

   BitWrPortI(PCDR, PCDRShadow,  1, 4); 	//HIGH

   BitWrPortI(PBDR, PBDRShadow,  0, 0);	//SCLK low
   BitWrPortI(PBDR, PBDRShadow,  1, 0);	//SCLK high
   DOUT[8]=BitRdPortI(PCDR, 5);				//DOUT DB8

   BitWrPortI(PCDR, PCDRShadow,  0, 4); 	//DC

   BitWrPortI(PBDR, PBDRShadow,  0, 0);	//SCLK low
   BitWrPortI(PBDR, PBDRShadow,  1, 0);	//SCLK high
   DOUT[9]=BitRdPortI(PCDR, 5);				//DOUT DB7

   BitWrPortI(PCDR, PCDRShadow,  0, 4); 	//DC

   BitWrPortI(PBDR, PBDRShadow,  0, 0);	//SCLK low
   BitWrPortI(PBDR, PBDRShadow,  1, 0);	//SCLK high
   DOUT[10]=BitRdPortI(PCDR, 5);				//DOUT DB6

   BitWrPortI(PCDR, PCDRShadow,  0, 4); 	//LOW

   BitWrPortI(PBDR, PBDRShadow,  0, 0);	//SCLK low
   BitWrPortI(PBDR, PBDRShadow,  1, 0);	//SCLK high
   DOUT[11]=BitRdPortI(PCDR, 5);				//DOUT DB5

   BitWrPortI(PCDR, PCDRShadow,  1, 4); 	//CODING

   BitWrPortI(PBDR, PBDRShadow,  0, 0);	//SCLK low
   BitWrPortI(PBDR, PBDRShadow,  1, 0);	//SCLK high
   DOUT[12]=BitRdPortI(PCDR, 5);				//DOUT DB4

   BitWrPortI(PCDR, PCDRShadow,  0, 4); 	//DC

   BitWrPortI(PBDR, PBDRShadow,  0, 0);	//SCLK low
   BitWrPortI(PBDR, PBDRShadow,  1, 0);	//SCLK high
   DOUT[13]=BitRdPortI(PCDR, 5);				//DOUT DB3

   BitWrPortI(PCDR, PCDRShadow,  0, 4); 	//DC

   BitWrPortI(PBDR, PBDRShadow,  0, 0);	//SCLK low
   BitWrPortI(PBDR, PBDRShadow,  1, 0);	//SCLK high
   DOUT[14]=BitRdPortI(PCDR, 5);				//DOUT DB2

   BitWrPortI(PCDR, PCDRShadow,  0, 4); 	//DC

   BitWrPortI(PBDR, PBDRShadow,  0, 0);	//SCLK low
   BitWrPortI(PBDR, PBDRShadow,  1, 0);	//SCLK high
   DOUT[15]=BitRdPortI(PCDR, 5);				//DOUT DB1

   BitWrPortI(PCDR, PCDRShadow,  0, 4); 	//DC

   BitWrPortI(PBDR, PBDRShadow,  0, 0);	//SCLK low
   BitWrPortI(PBDR, PBDRShadow,  1, 0);	//SCLK high
   //DOUT[15]=BitRdPortI(PCDR, 5);				//DOUT DB0


   BitWrPortI(PBDR, PBDRShadow,  1, 7); 	//CS high


   //printf("\n\nDOUT - %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d", DOUT[0], DOUT[1], DOUT[2], DOUT[3], DOUT[4], DOUT[5], DOUT[6], DOUT[7], DOUT[8], DOUT[9], DOUT[10], DOUT[11], DOUT[12], DOUT[13], DOUT[14], DOUT[15]);

   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;

   //printf(" = %d", decimalSum);
   return decimalSum;
}