#include <sys/types.h>
#include <sys/errno.h>
#include <math.h>
#include <stdio.h>
#include "pi.h"

#define DIAG 0
#define DATAFILE "transmission.dat"
#define BUFSIZE 132
#define ERROR -1
#define OK 0

main()
{
  float w1,w2,wt;
  float a1,a2;
  float t,dt,endt;
  float Ts,A,B,C,w0,Iw,Im,It;
  float R, Power, effy;
  float Ti, wi;
  float rloops;
  float tspd, wdiff, wdifp, score;
  int loops,nloops;
  FILE *fp;
  unsigned char line[BUFSIZE];
  float diag;
  diag = DIAG;
  if((fp=fopen(DATAFILE,"r")) == NULL)
    {
      printf("Error Opening File: %s \n", DATAFILE);
      exit(ERROR);
    }
  else
    {
      if(diag >= 8) printf("Success Opening File: %s \n", DATAFILE);
    }
  fgets(line, BUFSIZE, fp);  sscanf(line,"%f", &diag );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%f", &dt );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%f", &endt );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%f", &rloops );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%f", &R );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%f", &wt );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%g", &A );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%g", &B );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%g", &C );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%f", &Iw );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%g", &Im );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%f", &Ts );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%f", &w0 );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%f", &w1 );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%f", &effy );
  if(fclose(fp) != OK)
    {
      printf("Error Closing file: %s \n", DATAFILE);
      exit(ERROR);
    }
      printf("# %7.2f Transmission Ratio (ratio of in to out speed) \n", R);
      printf("# %7.2f Wheel speed [rpm] at time measurement \n",  wt);
      printf("# %7.5g w cubed drag coefficient \n",  A );
      printf("# %7.5g w squared drag coefficient \n",  B);
      printf("# %7.5g w 1st power drag coefficient \n", C);
      printf("# %7.2f Wheel Inertia        [Kg-meters-squared] \n", Iw);
      printf("# %7.5g Motor Inertia        [Kg-meters-squared] \n", Im);
      printf("# %7.2f Motor Stall Torque   [Newton-meters] \n", Ts);
      printf("# %7.2f Motor no-load Speed  [rpm] (at 20.0 volts) \n", w0);
      printf("# %7.2f Motor starting speed [rpm] \n", w1);
      printf("# %7.2f Transmission efficiency [ratio] \n", effy);
      printf("#  \n");
  nloops = rloops;
  w0 = w0 * PIT2/60.0;   /* Convert from rpm to radians/second    */
  w1 = w1 * PIT2/60.0;   /* Convert from rpm to radians/second    */
  Ts = Ts * effy;        /* Reduce effective torque by trans efficiency */
  Ts = Ts * R;           /* Transmission Speed Ratio multiplier   */
  w0 = w0 / R;           /* Transmission Speed Ratio divider      */
  t = 0.0;               /* Start time                            */
  It = Iw + (Im * R * R); /* Total Inertia = Wheel I + Motor I * R^2 */
  a1 = Ts / It;
  Power = (Ts - ((Ts/w0)*w1)) * w1;
  printf("#Time[s] Speed[rpm] Accel[1/s] Power[cW] \n");
  /*  printf(" %f, %f, %f, %f \n", t, (w1*60.0/PIT2), a1, (Power*100.0)); */
  printf(" %f, %f \n", t, (w1*60.0/PIT2));
  for ( t = 0.0; t < endt; t = t + dt )
  {
      a1 = (Ts - ( (A*w1*w1*w1)+(B*w1*w1)+(C*w1)+((Ts/w0)*w1) ) ) / It;
      w2 = w1 + (a1 * dt);
      for ( loops = 0; loops < nloops; loops=loops+1 )
      {
	  a2 = (Ts - ( (A*w2*w2*w2)+(B*w2*w2)+(C*w2)+((Ts/w0)*w2) ) ) / It;
	  w2 = w1 + (((a1 + a2)/2.0) * dt);
      }
      Power = (Ts - ((Ts/w0)*w2)) * w2;
      w1 = w2;
      printf(" %f, %f \n",
	     (t + dt), (w2*60.0/PIT2));
/*      printf(" %f, %f, %f, %f \n",
	(t + dt), (w2*60.0/PIT2), a2, (Power*100.0)); */
/*      printf("%f, %f, %f \n", (t + dt), w2, a2);  */
  }
}