#define PORT1 0x3F8  /* Port Address */
#define INTVECT 0x0C /* Com Port's IRQ here (Must also change PIC setting) */

  /* Defines Serial Ports Base Address */
  /* COM1 0x3F8                        */
  /* COM2 0x2F8			       */
  /* COM3 0x3E8			       */
  /* COM4 0x2E8			       */

int bufferin = 0;
int gps_flag = 0;
int gps_zero = 0;
int i = 0;
int driver[8];
int servo[4];
char buffer[56], gps_string[56];
double x[3], y[3], z[3], t[3], dx_dt[2], dy_dt[2], dz_dt[2], h[3];
const double pi = 3.1415926535898;
const double r = 20926677.; /*radius of the earth in feet*/
double phi = 0.0; /* Latitude in radians*/
double theta = 0.0; /*Longitude in radians*/
double phi_0 = 0.0; /*Regional longitude*/


void interrupt (*oldport1isr)();

void interrupt PORT1INT()  /* Interrupt Service Routine (ISR) for PORT1 */
{
 int c;
 do { c = inportb(PORT1 + 5);
		if (c & 1) {buffer[bufferin] = inportb(PORT1);
		  if (buffer[bufferin] == 64) {
		  bufferin = 0;
		  }
		  else {
				bufferin++;
		  }
		  if (bufferin == 55) {
				for (i = 0; i <= 55; i++)
					gps_string[i] = buffer[i]; /* Update entire GPS string */
				gps_flag = 1; /* GPS string is available to main program */
				bufferin = 0;}}
	 }while (c & 1);
 outportb(0x20,0x20);
}


int gps()
{
	if ( (gps_flag) &&
	((gps_string[12] == 78) || (gps_string[12] == 83)) &&
	((gps_string[20] == 87) || (gps_string[20] == 69)) &&
	((gps_string[29] == 71) || (gps_string[29] == 83) || (gps_string[29] == 103))
	&&
	((gps_string[33] == 43) || (gps_string[33] == 45)) &&
	((gps_string[39] == 69) || (gps_string[39] == 87)) &&
	((gps_string[44] == 78) || (gps_string[44] == 83)) &&
	((gps_string[49] == 85) || (gps_string[49] == 68)) &&
	(gps_string[54] == 13) ) {
		for (i = 0; i <=55; i++) {
			printf("%c",gps_string[i]);
		}
		printf("\n");
		return(1);
		}
	else {
		for (i = 0; i <=55; i++) {
			printf("%c",gps_string[i]);
		}
		return(0);
		}

}


int gps_process()
{
	if (gps_zero == 0) {
		phi_0 = (((gps_string[13]- 48)*10 + (gps_string[14]-48))*60 +
		((gps_string[15]- 48)*10 + (gps_string[16]-48) + (gps_string[17]-48)*0.1 +
		(gps_string[18]-48)*0.01 +(gps_string[19]-48)*0.001))*pi/10800.0;
	}

	phi = (((gps_string[13]- 48)*10 + (gps_string[14]-48))*60 +
		((gps_string[15]- 48)*10 + (gps_string[16]-48) + (gps_string[17]-48)*0.1 +
		(gps_string[18]-48)*0.01 +(gps_string[19]-48)*0.001))*pi/10800.0;

	theta =  (((gps_string[21]- 48)*100 + (gps_string[22]-48)*10 +
		(gps_string[23]-48))*60 +
		((gps_string[24]- 48)*10 + (gps_string[25]-48) + (gps_string[26]-48)*0.1 +
		(gps_string[27]-48)*0.01 +(gps_string[28]-48)*0.001))*pi/10800.0;

	if (gps_zero == 0) {

		t[0] = ((gps_string[6]- 48)*10 + (gps_string[7]-48))*3600 +
		((gps_string[8]-48)*10+(gps_string[9]-48))*60 +
		((gps_string[10]-48)*10+(gps_string[11]-48));

		y[0] = r*phi;

		x[0] = r*sin(pi/2.0-phi_0)*theta;

		z[0] = ( (gps_string[38]- 48) + (gps_string[37]- 48)*10 +
		(gps_string[36]- 48)*100 + (gps_string[35]- 48)*1000 +
		(gps_string[34]- 48)*10000 )*3.281;

		gps_zero = 1;

	}

	t[1] = t[2];
	t[2] = ((gps_string[6]- 48)*10 + (gps_string[7]-48))*3600 +
	((gps_string[8]-48)*10+(gps_string[9]-48))*60 +
	((gps_string[10]-48)*10+(gps_string[11]-48)) - t[0];

	y[1] = y[2];
	y[2] = r*phi - y[0];

	x[1] = x[2];
	x[2] = -(r*sin(pi/2.0-phi_0)*theta - x[0]);

	z[1] = z[2];
	z[2] = ( (gps_string[38]- 48) + (gps_string[37]- 48)*10 +
	(gps_string[36]- 48)*100 + (gps_string[35]- 48)*1000 +
	(gps_string[34]- 48)*10000 )*3.281 - z[0];

	dx_dt[0] = dx_dt[1];
	if (gps_string[39] == 69) {
		dx_dt[1] = ( (gps_string[40]- 48)*100 + (gps_string[41]- 48)*10 +
		(gps_string[42]- 48) + (gps_string[43]- 48)*0.1)*3.281;
	}
	else {
		dx_dt[1] = -1.0*( (gps_string[40]- 48)*100 + (gps_string[41]- 48)*10 +
		(gps_string[42]- 48) + (gps_string[43]- 48)*0.1)*3.281;
	}

	dy_dt[0] = dy_dt[1];
	if (gps_string[44] == 78) {
		dy_dt[1] = ( (gps_string[45]- 48)*100 + (gps_string[46]- 48)*10 +
		(gps_string[47]- 48) + (gps_string[48]- 48)*0.1)*3.281;
	}
	else {
		dy_dt[1] = -1.0*( (gps_string[45]- 48)*100 + (gps_string[46]- 48)*10 +
		(gps_string[47]- 48) + (gps_string[48]- 48)*0.1)*3.281;
	}

	dz_dt[0] = dz_dt[1];
	if (gps_string[49] == 85) {
		dz_dt[1] = ( (gps_string[50]- 48)*10 + (gps_string[51]- 48) +
		(gps_string[52]- 48)*0.1 + (gps_string[53]- 48)*0.01)*3.281;
	}
	else {
		dz_dt[1] = -1.0*( (gps_string[50]- 48)*10 + (gps_string[51]- 48) +
		(gps_string[52]- 48)*0.1 + (gps_string[53]- 48)*0.01)*3.281;
	}

	return(1);
}


void toggle_control()
{
	if ((driver[0] != driver[1]) && ((driver[0] == 0) || (driver[1] ==0))) {
		driver[0] = (driver[0] == 0); // Toggle Receiver power
		driver[1] = (driver[1] == 0); // Toggle controller power
		PutDriverChannel( 0 );
		PutDriver( driver[0] );
		PutDriverChannel( 1 );
		PutDriver( driver[1] );

		if (driver[0]) {
			for (i = 0; i <4; i++) {
				PutD2AChannel(i);
				PutD2A(0); // Turn off servo voltage output
				}
			PutDriverChannel( 3 );
			PutDriver( 1 ); // Turn on capacitor drain
		}
		else {
			PutDriverChannel( 3 );
			PutDriver( 0 ); // Turn off capacitor drain
			for (i = 0; i <4; i++) {
				PutD2AChannel(i);
				PutD2A(servo[i]); // Revert to old servo positions
			}
		}

		printf ("\nReceiver mode %s.\n", driver[0] ? "on" : "off");
	}

	else {
		driver[0] = 0;
		driver[1] = 1;
		PutDriverChannel( 0 );
		PutDriver( driver[0] ); // Set receiver to off
		PutDriverChannel( 3 );
		PutDriver( 0 ); // Turn capacitor drain off
		PutDriverChannel( 1 );
		PutDriver( driver[1] ); // Set servo controller to on
		printf ("\nReceiver mode %s.\n", driver[0] ? "on" : "off");
	}
}

void toggle_light()
{
	driver[2] = (driver[2] == 0); // Toggle light power
	PutDriverChannel( 2 );
	PutDriver( driver[2] );
	printf ("\nLED is %s.\n", driver[2] ? "on" : "off");
}

void set_servo( int n, int p )
{
	PutD2AChannel (n);
	PutD2A (p);
}