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

#ifdef __cplusplus
extern "C" {
#endif


#define MAXDATA 10000
#define TIMESTEP .01

float startspeed=0;
float maxspeed=0;
float mass=0;
float data[MAXDATA][3];
int ndata = 0;
float g = 32;	/* feet/sec/sec */

/* Guess the drag at 30mph */
float loguess = 0;
float lospeed = 0;
float higuess = 100;
float hispeed = 1000000;
float newguess, newspeed;

float CalcSpeed(float drag30);


void main(int argc, char *argv[])
{
  FILE *profile;
  int i;
  
  if (argc != 2) {
    fprintf(stderr, "Usage:  %s <hillProfileFile>\n", argv[0]);
    fprintf(stderr,
	    "\nhillProfileFile should have the form:\n"
	    "<distance (in miles)>	<elevation (in feet)>\n"
	    "e.g.\n"
	    "0.0	350\n"
	    "0.09	380\n"
	    "0.18	400\n");
    exit(-1);
  }

  /* Read profile */
  profile = fopen(argv[1], "r");
  while(fscanf(profile, "%f %f", &(data[ndata][0]), &(data[ndata][1])) == 2) {
    ndata++;
    if (ndata >= MAXDATA) {
      fprintf(stderr, "Error, too many profile data points! \n");
      exit(-1);
    }
  }
  fclose(profile);

  /* Print profile */
  for (i=0; i < ndata; i++) {
    data[i][2] = (i==ndata-1)? 0 :
      (data[i+1][1]-data[i][1])/
	(5280*(data[i+1][0]-data[i][0]));
    printf("%.2f %.0f (%.4f%%)\n", data[i][0], data[i][1], data[i][2]);
  }
  
  /* Read stats */
  printf("Enter starting speed -->");
  fscanf(stdin, "%f", &startspeed);
  printf("Enter max speed -->");
  fscanf(stdin, "%f", &maxspeed);
  printf("Enter total mass -->");
  fscanf(stdin, "%f", &mass);

  /* print stats */
  printf("start %f max %f mass %f\n", startspeed, maxspeed, mass);

  /* calculate final speeds at different drags */
  lospeed = CalcSpeed(loguess);
  hispeed = CalcSpeed(higuess);

  /* Now try different speeds */
  for (i=0; i < 20; i++) {
    newguess = 0.5 * (loguess + higuess);
    newspeed = CalcSpeed(newguess);
    if (newspeed < maxspeed) {
      hispeed = newspeed;
      higuess = newguess;
    } else {
      lospeed = newspeed;
      loguess = newguess;
    }
  }
}

#define DRAGEXP 1.6

float CalcSpeed(float drag30)
{
  float force;
  float drag1  = drag30 / powf(44.0, DRAGEXP); /* drag at 1 ft/sec */
  float pos = data[ndata-1][0];
  int i = ndata-2;
  float speed = startspeed * 5280.0 / 3600.0;  /* feet per second */
  float maxspeed = startspeed;
  
  while (pos > data[0][0]) {
    /* Figure out which slope we're in */
    while (pos < data[i][0]) i--;

    /* Now calculate the force */
    force = data[i][2] * mass;
    force -= drag1 * powf(speed, DRAGEXP);
    
    /* now adjust speed */
    speed += TIMESTEP * force * g / mass;

    /* Figure out max? */
    if (speed > maxspeed) {
      maxspeed = speed;
    }
    
    /* update position */
    pos -= speed * TIMESTEP/5280.0;
  }

  /* Final stat */
  maxspeed *= 3600.0/5280;
  printf("Run:  drag30 %.3f, final speed %.2f\n", drag30, maxspeed);
  
  return(maxspeed);
}


#ifdef __cplusplus
}
#endif