/* Name       : BTT Flight THINGY   */
 /* By         : Ben Showman      */
 /* Modified   : 2-11-2002        */
 /* Version    : 0.56             */


#include <dos.h>
#include <stdio.h>
#include <conio.h>
#include <math.h>
#include <bios.h>
#include <stdlib.h>
#include "driver.h"
#include "GPSI.h"




void main(void)
{

int gps();
int gps_process();
int c, ppp, ddd;
int outdate[2];
int i;
int delay_time;
int opt_x, opt_y;
outdate[0] = -1;
outdate[1] = -1;

x[2] = 0.0;
y[2] = 0.0;
z[2] = 0.0;
dx_dt[1] = 0.0;
dy_dt[1] = 0.0;
dz_dt[1] = 0.0;

 /*\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*/
 /*//////////////////////////////////////////////////////////////////////*/

#include "INTERI.h"

 /*----------------------------------------------------------------------*/
 /*----------------------------------------------------------------------*/

 printf("\nBTT CONTROLLER INTERFACE v-0.58. Press ESC to quit.\n\n");
 printf("G = GPS data.\n");
 printf("T = toggle between receiver and controller.\n");
 printf("L = toggle LED.\n");
 printf("D = read A/D pins.\n");
 printf("S = control servo positions.\n");
 ddd = 0;
 servo[0] = 0;
 servo[1] = 0;
 servo[2] = 0;
 servo[3] = 0;
 driver[0] = 0;
 driver[1] = 1;
 driver[2] = 0;
 driver[3] = 0;
 driver[4] = 0;
 driver[5] = 0;
 driver[6] = 0;
 driver[7] = 0;
 PutDriverChannel( 3 );
 PutDriver( driver[3] ); // Set capacitor drain to off
 PutDriverChannel( 0 );
 PutDriver( driver[0] ); // Set receiver to off
 PutDriverChannel( 1 );
 PutDriver( driver[1] ); // Set servo controller to on

 do {
	  c = 0;

	  if (kbhit()){c = getch();
		  }
	  if ((c == 71) || (c == 103)) {
			if (gps()) {
				printf("\nGood GPS String. \n");
				if ((gps_process() == 1) && (gps_zero == 1)) {
					printf("\nCurrent Altitude: \n");
					printf("%f", z[2]);
					printf("\nOld Altitude: \n");
					printf("%f", z[1]);

					printf("\nCurrent Time: \n");
					printf("%f", t[2]);
					printf("\nOld Time: \n");
					printf("%f", t[1]);

					printf("\nCurrent Y: \n");
					printf("%f", y[2]);
					printf("\nOld Y: \n");
					printf("%f", y[1]);

					printf("\nCurrent X: \n");
					printf("%f", x[2]);
					printf("\nOld X: \n");
					printf("%f", x[1]);

					if (t[2] != t[1]) {
						printf("\nY-speed: \n");
						printf("%f", (y[2]-y[1])/(t[2]-t[1]));
						printf("\nX-speed: \n");
						printf("%f", (x[2]-x[1])/(t[2]-t[1]));
					}

					printf("\ndy_dt-speed: \n");
					printf("%f", dy_dt[1]);
					printf("\ndx_dt-speed: \n");
					printf("%f", dx_dt[1]);
					printf("\ndz_dt-speed: \n");
					printf("%f", dz_dt[1]);

					outdate[0] = outdate[1];
					outdate[1] = t[2];
					if (outdate[0] == outdate[1]) {
						printf("\nGPS data May be outdated.\n");
						printf("Check GPS connection.\n");
					}
				}
			}
			else {
				printf("\nNonexistent/Bad GPS String. \n");
			}

	  }

	  if ((c == 115) || (c == 83)) {
				printf ("\nEnter servo number: ");
				scanf ("%d", &ppp);
				printf ("\nEnter position for servo %d: ", ppp);
				if ((ppp >= 0) && (ppp < 4)) {
				scanf ("%4d", &servo[ppp]);
					if ((servo[ppp] >= 0) && (servo[ppp] < 4096)) {
						set_servo( ppp, servo[ppp] ); // Set servo position
					}
					else {
						printf ("\nInvalid servo position.\n");
					}
				}

				else {
					printf ("\nInvalid servo number.\n");
				}
	  }

		if ((c == 68) || (c == 100)) {
				printf ( "\nEnter delay in milliseconds: " );
				scanf ("%d", &delay_time );
				do {
					PutA2DChannel( 2 );
					opt_x = GetA2D();
					PutA2DChannel( 3 );
					opt_y = GetA2D();
					printf ("\nopt_x = %d, opt_y = %d", opt_x, opt_y );
					delay( delay_time );
				} while( !( kbhit() ) );
		}

		if ((c == 84) || (c == 116)) {
			toggle_control(); // Switch between receiver and computer control
		}

		if ((c == 76) || (c == 108)) {
			toggle_light(); // Toggle power to LED
		}




	 } while (c !=27);

 /*----------------------------------------------------------------------*/
 /*----------------------------------------------------------------------*/

 #include "POSTINI.h"

  /*\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*/
 /*//////////////////////////////////////////////////////////////////////*/

}