#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 Tsi, w0i, w1i;
  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", &Tsi );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%f", &w0i );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%f", &w1i );
  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", Tsi);
      printf("# %7.2f Motor no-load Speed  [rpm] (at 20.0 volts) \n", w0i);
      printf("# %7.2f Motor starting speed [rpm] \n", w1i);
      printf("# %7.2f Transmission efficiency [ratio] \n", effy);
      printf("#  \n");
  nloops = rloops;
for ( effy = 1.0; effy > 0.0; effy = effy - 0.05 )
  {
  w0 = w0i * PIT2/60.0;   /* Convert from rpm to radians/second    */
  w1 = w1i * PIT2/60.0;   /* Convert from rpm to radians/second    */
  Ts = Tsi * effy;        /* Reduce effective torque by trans efficiency */
      printf("#Ratio  Time [s] to %3.0f rpm    effy=%f \n", wt, effy);
/*  printf("%f, %f, %g, %f \n", t, (w1*60.0/PIT2), a1, (Power*100.0));  */
  wdifp = wt;
  wi = w0;
  Ti = Ts;
  for ( R = 1.0; R < 10.0; R = R + 0.05 )
  {
      Ts = Ti * R;           /* Transmission Speed Ratio multiplier   */
      w0 = wi / R;           /* Transmission Speed Ratio divider      */
      w1 = 0.0;              /* Start speed                           */
      w2 = 1.0;              /* Start speed                           */
      t = 0.0;               /* Start time                            */
      It = Iw + (Im * R * R); /* Total Inertia = Wheel I + Motor I * R^2 */
      a1 = Ts / It;
      a2 = a1;
      Power = (Ts - ((Ts/w0)*w1)) * w1;
      for ( t = 0.0; w1 < w2; t = t + dt )
      {
	  w1 = w2;
	  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;
	  wdiff = abs((w2*60.0/PIT2)-wt);
	  if ( wdiff < wdifp ) tspd = t;
	  wdifp = wdiff;
      }
      score = ((w2*60.0/PIT2)/tspd);
      if ( tspd <= 0.0 ) score = 0.0;
      printf(" %4.2f, %7.2f \n",
	     R, (w2*60.0/PIT2) );
/*      printf(" %4.2f, %7.2f, %7.2f, %9.2f, %7.2f \n",
	      R, (t + dt), (w2*60.0/PIT2), tspd, score ); */
/*      printf(" %4.2f, %7.2f, %f, %e, %f \n",
	     R, (t + dt), (w2*60.0/PIT2), a2, (Power*100.0)); */
/*      printf("%f, %f, %f \n", (t + dt), w2, a2);  */
  }
  printf("&\n");
  }
}