#include <stdio.h>
#include <stdlib.h>

int main(int argc, char *argv[])
{
  FILE *fone;
  fone = fopen("Results2.txt","w+");
  printf("glider simulation\n");
  float heading ;
  float deltaheading;
  float angular_acceleration;
  float gps;
  float integral;
  float derivative;
  float damping_constant;
  float rudder_constant;
  float P_constant;
  float I_constant;
  float D_constant;
  float rudder;
  float gpsdelta;
  int iterations;
  float pseudogps;
  float pseudogpsdelta;
  int n;
  int k;
  printf("input rudder constant, damping constant, P, I, D, iterations, and heading offset\n");
  scanf("%f",&rudder_constant);
  scanf("%f",&damping_constant);
  scanf("%f",&P_constant);
  scanf("%f",&I_constant);
  scanf("%f",&D_constant);
  scanf("%d",&iterations);
  scanf("%f",&heading);
  integral=0;
  deltaheading=0;
  rudder=0;
  fprintf(fone,"%f\t%f\t%f\t%f\t%f\n",rudder_constant,damping_constant,P_constant,I_constant,D_constant);
  printf("and we're off!\n");
  for (n=1;n<=iterations;n++)
   {
   gpsdelta=heading-gps; 
   gps=heading ;                                                                      /*a GPS measurement*/
   pseudogpsdelta=heading+gpsdelta-pseudogps;
   pseudogps=heading+gpsdelta;
   pseudogpsdelta=gpsdelta;                                                            /*this just undoes the approximation*/
   pseudogps=gps;
   for (k=1;k<=50;k++)                                                                 /*one second of flight*/
    {angular_acceleration=(rudder_constant*rudder)-(deltaheading*damping_constant);
     deltaheading+=(angular_acceleration/50);
     heading+=(deltaheading/50);}
   integral+=pseudogps;                                                                      /*gps is processed*/
   rudder=(P_constant*pseudogps)+(D_constant*pseudogpsdelta)+(I_constant*integral);
   fprintf(fone,"%f\t%f\n",heading,rudder);
   }
  system("PAUSE");	
  return 0;
}