#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)
{
FILE *out;
FILE *input_file;
int gps();
int gps_process();
int c, comp;
int driver[8];
int servo[4];
int outdate[2];
int i;
int old_gps, no_gps, comp_old;
double X_miss, Y_miss, Z_miss, Xdot_miss, Ydot_miss, Zdot_miss, X_tar[5], Y_tar[5], Z_tar[5];
double alpha_dot, theta_dot, range_sq, k_el, k_throt, k_ail, el_comm, throt_comm;
double ctrl_gain, ail_comm, h, hdot;

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"

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

 el_comm = 0;
 throt_comm = 0;
 ail_comm = 0;
 ctrl_gain = 0;

 k_throt = 1;
 k_ail = 1;
 k_el = 1;

 no_gps = 0;
 old_gps = 0;

 servo[0] = el_comm;
 servo[1] = throt_comm;
 servo[2] = ail_comm;
 servo[3] = ctrl_gain;
 driver[0] = 1;
 driver[1] = 0;
 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 on
 PutDriverChannel( 1 );
 PutDriver( driver[1] ); // Set servo controller to off
 i = 0;
 c = 0;		// counter for target number
 comp = 0;   	// 1 for onboard control

input_file = fopen( "target.dat", "r");
 if (input_file == NULL) abort();
 while(fscanf(input_file, "%f %f %f", X_tar[i], Y_tar[i], Z_tar[i]!= EOF)){
	 i++;
 }
 fclose(input_file);
 out = fopen("out.dat", "w+");
 do{
	comp_old = comp;
		if (gps()) {
			if ((gps_process() == 1) && (gps_zero == 1)) {
			driver[2] = (driver[2] == 0); // Toggle LED power
			PutDriverChannel( 2 );
			PutDriver( driver[2] );
				if (t[2] != t[1]) {
				X_miss = x[1];
				Y_miss = y[1];
				Z_miss = z[1];
					if(((Z_miss >= .8*Z_tar[c])&&(comp == 1)) || ((Z_miss >= Z_tar[c]) && (comp == 0))){
					comp = 1;
					Xdot_miss = dx_dt[0];
					Ydot_miss = dy_dt[0];
					Zdot_miss = dz_dt[0];
					range_sq = pow((X_miss - X_tar[c]),2)+pow((Y_miss - Y_tar[c]),2)+pow((Z_miss-Z_tar[c]),2);
						if (range_sq < 1000){
						c = c+1;
						range_sq = pow((X_miss - X_tar[c]),2)+pow((Y_miss - Y_tar[c]),2);
						}
					alpha_dot = ((X_tar[c]-X_miss)*Ydot_miss - (Y_tar[c]-Y_miss)*Xdot_miss)/range_sq;
					h = sqrt(pow(Y_tar[c]-Y_miss,2)+pow(X_tar[c]-X_miss,2));
					hdot = pow(pow(Ydot_miss,2)+pow(Xdot_miss,2),.5);
					theta_dot = (h*Zdot_miss-(Z_tar[c]-Z_miss)*hdot)/range_sq;
					el_comm = k_el*theta_dot;
					ail_comm = k_ail*alpha_dot;
					throt_comm = k_throt*theta_dot;
					}
					else{
					comp = 0;
					}
				}
			outdate[0] = outdate[1];
			outdate[1] = t[2];
				if (outdate[0] == outdate[1]) {
				old_gps = old_gps + 1;
				}
			}
			else {
			no_gps = no_gps+1;
			}
			if(2*no_gps+old_gps >=2){
			comp = 0;
			}
			if(comp != comp_old){
				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
						}
					}
				}
				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
				}
			}
		servo[0] = el_comm + 2048;
		servo[1] = ail_comm + 2048;
		servo[2] = throt_comm + 2048;
			for(i=0; i<=2; i++){
				if(servo[i] <= 0){
				servo[i] = 0;
				}
				if(servo[i] > 4096){
				servo[i] = 4096;
				}
			PutD2AChannel(i);
			PutD2A (servo[i]);
			fprintf(out, "%f %f %f %f %f %f %f %f %f %i %i\n", x[2], y[2], z[2], dx_dt[1], dy_dt[1], dz_dt[1], alpha_dot, theta_dot, range_sq, comp, c);
			}
		}
		if (kbhit()) c = getch();
		} while (c!=27);
	fclose(out);
 }