// This robot driver is by Randy Saint, February, 1995
// adapted to ver. 0.39 3/6/95 by M. Timin

#include <string.h>
#include <stdlib.h>
#include <math.h>
#include "car.h"

#define max(x,y) (((x)<(y))?(y):(x))

#define SPEED_CORN 0.5
#define SHORT_CORN (PI/2.0)
#define ACCEL_CORN (PI/3.0)
#define ALPHA_CORN (PI/16.0)

extern const double CARWID;
extern char* glob_name;

con_vec Ramdu2(situation s)
{
   const char name[] = "Ramdu2";
   static int init_flag = 1;
   con_vec result;
   double alpha, vc;
   double Accel_vc;
   double width;                      // track width, feet
   double cur_rad;                    // current radius
   double nex_rad;                    // next radius
   double nex_alpha;                  // alpha we'll need on the next turn
   double dist_from_inside;           // distance from inside of turn
   double nex_corn_spd;               // speed for next corner
   double steer_gain = .18;
   const double steer_damp = .48;
   const double corn_con = 6.0;
   static double corn_spd = 65.0;
   static double acc_const = 85.0;
   static int max_accel = 0;

   if(init_flag)  {
	  strcpy(glob_name, name);
	  init_flag = 0;
	  result.alpha = result.vc = 0;
	  return result;
   }

   if(stuck(s.backward, s.v,s.vn, s.to_lft,s.to_rgt, &result.alpha,&result.vc))
      return result;


//  if(s.to_lft < 1e-5 && s.to_lft>-1e-5)   // PREVENTS A DIV BY ZERO
  //   s.to_lft = .01;


   width = s.to_lft + s.to_rgt;                        // find width of track
   cur_rad = s.cur_rad;
   nex_rad = s.nex_rad;

   if (nex_rad < 0.0) {
	nex_rad += width;
   }
   if(cur_rad == 0.0 && nex_rad == 0.0)
	  corn_spd = 200.0;
   else if(cur_rad == 0.0 && nex_rad != 0.0) {
	  corn_spd = max(40.0, corn_con * sqrt(nex_rad > 0.0 ? nex_rad : -nex_rad));
	  if (s.nex_len < SHORT_CORN)
		  corn_spd += corn_spd * SPEED_CORN * sqrt(SHORT_CORN-s.nex_len);
   }
   else if (cur_rad != 0.0) {
	  corn_spd = max(40.0, corn_con * sqrt(cur_rad > 0.0 ? cur_rad : -cur_rad));
	  if (s.cur_len < SHORT_CORN)
		  corn_spd += corn_spd * SPEED_CORN * sqrt(SHORT_CORN-s.cur_len);

	  if (nex_rad != 0.0)
		nex_corn_spd = max(40.0, corn_con * sqrt(nex_rad > 0.0 ? nex_rad : -nex_rad));
	  else
		nex_corn_spd = 200.0;
   }

   // prevent division by zero in code to follow:
   if(s.to_lft < 1e-5 && s.to_lft > -1e-5)
      s.to_lft = .01;
   if(s.to_rgt < 1e-5 && s.to_rgt > -1e-5)
      s.to_rgt = .01;

   nex_alpha = 0.0;
   if(nex_rad > 0.0)   {
	  nex_alpha = steer_gain * (4 * (s.to_lft -
						 (s.after_rad < 0.0 ? width/3 : 1.6*CARWID)
										) / width + .2 * width/s.to_rgt);
	  dist_from_inside = s.to_lft;
   }
   else if (nex_rad < 0.0) {
	  nex_alpha = -steer_gain * (4 * (s.to_rgt -
							   (s.after_rad > 0.0 ? width/3 : 1.6*CARWID)
											 ) / width  + .2 * width/s.to_lft);
	  dist_from_inside = s.to_rgt;
   }
   if(cur_rad == 0) {
	  alpha = 0.0;
	  if (s.to_end < (width * (dist_from_inside/width)))
		  alpha = nex_alpha * (width-s.to_end)/width;

	}
   else if(cur_rad > 0.0)   {
	  alpha = steer_gain * (4 * (s.to_lft -
						 (nex_rad < 0.0 ? width/3 : 1.6*CARWID)
										) / width + .2 * width/s.to_rgt);
	  if ((s.to_end) < ALPHA_CORN)  // we're coming out of a turn
		 alpha = alpha*(s.to_end/ALPHA_CORN) + nex_alpha*((ALPHA_CORN-s.to_end)/ALPHA_CORN);  // Scale alpha down to nex_alpha
   }
   else  {
	  alpha = -steer_gain * (4 * (s.to_rgt -
							   (nex_rad > 0.0 ? width/3 : 1.6*CARWID)
											 ) / width  + .2 * width/s.to_lft);
	  if ((s.to_end) < ALPHA_CORN)  // we're coming out of a turn
		 alpha = alpha*(s.to_end/ALPHA_CORN) + nex_alpha*((ALPHA_CORN-s.to_end)/ALPHA_CORN);  // Scale alpha down to nex_alpha
   }

   alpha -= steer_damp * s.vn / s.v;  // This is damping, to prevent oscillation

   if (((s.power_req < 0.85) && (max_accel)) || ((s.power_req > 1.0) && (max_accel))) {
      if(s.power_req < 1e-5 && s.power_req > -1e-5)   // prevent div by 0
         s.power_req = .001;
		acc_const /= s.power_req;
		acc_const *= 0.9;
   }
   Accel_vc = s.v + acc_const/s.v;
   if(cur_rad == 0)         // If we are on a straightaway,
	  if((1.25*s.to_end/(s.v*0.05)) > (s.v - corn_spd))
		 vc = Accel_vc;        // keep accellerating near full power
	  else  {                 // otherwise,
		 vc = ((Accel_vc<corn_spd)? (Accel_vc) : corn_spd);
	  }
   else {
	  if ((s.to_end) > ACCEL_CORN)
		 vc = corn_spd;
	  else
		 vc = corn_spd*(s.to_end/ACCEL_CORN) + nex_corn_spd*((ACCEL_CORN-s.to_end)/ACCEL_CORN);

	  vc = ((Accel_vc<vc)? (Accel_vc) : vc);           // maintain cornering speed, braking at first

   }
   if (vc == Accel_vc) max_accel = 1;
   else max_accel = 0;

   result.vc = vc;   result.alpha = alpha;
   return result;
}                                                              // v;