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

#define PI 3.141592654  /* (4.0*atan(1.0)) */
#define PIT2 (PI*2.0)

#define DIAG 0
#define DATAFILE "mclimb.dat"
#define PARFILE "p.par"
#define BUFSIZE 132
#define ERROR -1
#define OK 0

main()
{
  float w,sp,Tm;
  float l,ld,ldd;
  float x,y;
  float lx,ly,nmesh;
  int pmesh;
  float theta;
  float t,dt,endt;
  int ipt, ipc;
  float g;
  float Ts, w0, vt, Mass;
  float Ts12, w012;
  int Nmotors;
  float I, Ist, Inl, Vn, Ish;
  float Va, Vb, Vc, Vm;
  float Rp, Rm;
  float Powerm, Powere;
  float Re, teffy;
  float meffy;
  FILE *fp;
  /*  unsigned char line[BUFSIZE]; */
  char line[BUFSIZE];
  int diag, par;
  diag = DIAG;
  if((fp=fopen(DATAFILE,"r")) == NULL)
    {
      printf("Error Opening File: %s \n", DATAFILE);
      exit(ERROR);
    }
  else
    {
      if(diag >= 1) printf("Success Opening File: %s \n", DATAFILE);
    }
  fgets(line, BUFSIZE, fp);  sscanf(line,"%i", &diag );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%i", &par );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%f", &dt );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%i", &ipt );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%f", &endt );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%f", &lx );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%f", &ly );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%i", &pmesh );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%f", &nmesh );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%f", &g );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%f", &Ts12 );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%f", &w012 );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%f", &Ist );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%f", &Inl );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%f", &Vn );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%f", &Ish );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%f", &Rp );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%f", &vt );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%i", &Nmotors );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%f", &Re );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%f", &Mass );
  fgets(line, BUFSIZE, fp);  sscanf(line,"%f", &teffy );
  if(fclose(fp) != OK)
    {
      printf("Error Closing file: %s \n", DATAFILE);
      exit(ERROR);
    }
  if ( diag >= 1 )
    {
      printf(" %i, %f, %f, %f, %f \n",
	     diag, dt, endt, g);
    }
  w012 = w012 * PIT2/60.0; /* Convert from rpm to radians/second    */
  Ts12 = Ts12 / 1000.0;  /* Convert from mN-m to N-m              */

  Ts = Ts12;
  w0 = w012;
  printf("\
#Speed[krpm]Torque[cNm] M.Power[W] Current[A] Voltage[V] E.Power[W] MotorEffy*10\n");
  for ( w = (1.0*w0); w > 0; w = w - (w0/200.0) )
  {
    Va = Vn;
    Vb = 0.9*Vn;
    Vc = (Va-Vb);
    Rm = Vn/Ist;
    /*  printf("						\
	#       Vn         Vb         Va         Vc          I \n"); */
    while (Vc > (Vn/10000.0))
      {
	if (diag > 0) printf(" %9.4f, %9.4f, %9.4f, %9.4f, %9.4f \n", w, Vb, Va, Vc, I);
	Va = ((9.0*Va)+Vb)/10.0;
	Vm = Va * (Rm/(Rm+Rp));
	Tm = (Ts/Vn)*Vm*(1.0-(w/(w0*Vm/Vn)));
	/*	Tm = (Ts/Vn)*Va*(1.0-(w/(w0*Va/Vn))); */
	I = Nmotors * ((((Ist - Inl)/Ts) * Tm) + Inl);
	if ( I > Ish ) I = Ish;
	if ( I < 0.0 ) I = 0.0;
	Vb = Vn - (Vn * I/Ish);
	Vc = (Va-Vb);
	if ( Vc < 0.0 ) Vc = -1.0 * Vc;
      }
    if ( Tm >= 0.0 )
      {
	sp = w * 60/(PIT2);
	Powerm = Nmotors * Tm * w;
	Powere = I * Va;
	meffy = Powerm/Powere;
	printf(" %11.4f, %9.4f, %9.4f, %9.4f, %9.4f, %9.4f, %9.4f \n",	\
	       sp, (Nmotors*Tm*100.0), Powerm, I, Va, Powere, meffy*10.0);
      }
  }
  exit(OK);
}