// CARZ.CPP - simulated robot car racing - begin development Dec. '94
// by Mitchell E. Timin, State College, PA
// see CAR.H & TRACK.H for class and structure declarations
// This version is for Borland C++, version 3.1, and is for DOS
// This is part of version 0.39 of RARS (Robot Auto Racing Simulation)
// ver. 0.1 release January 12, 1995
// ver. 0.2 1/23/95
// ver. 0.3 2/7/95
// ver. 0.39 3/6/95

#include <math.h>
#include <ctype.h>
#include <stdlib.h>
#include <string.h>
#include <iostream.h>
#include "car.h"
#include "track.h"
#include "os.h"

static const double PM = 1e5;    // Power, Maximum, 100,000 ft. lb. per sec
static const double g = 32.2;    // acceleration due to gravity, ft./sec^2
static const double DRAG_CON = .0065;  // air drag, lb. per (ft/sec)^2
static const double M = 80;  // mass, slugs   (2600 lb.)
static const double STARTING_SPEED = 40.0;  // cars start at this speed, ft/sec
static const double REVERSE_GEAR_LIMIT = 20; // ft/sec max to allow reverse

// global variables:
// These are set by the command line arguments: (see get_args())
int lap_count;       // length of the race in laps
int car_count;       // how many cars in the race
int race_count;      // how many races?
int real_speed;      // if non-zero, PC clock will control speed of race
int no_display;      // if non-zero, race will be invisible
int keep_order;      // if this is 0, then starting order will be re-arranged

char *glob_name;     // will hold name of each robot driver (one at a time)
double length;       // total lenth of track (average of inner and outer rails)
int done_count;  // incremented by each car that finishes the race
int out_count;   // incremented by each car that crashes
double time_count;  // elapsed time, seconds

// These are the control or "driver" programs which compete in the race:
con_vec cntrl0(situation);
con_vec cntrl1(situation);  // See drivers[], below.
con_vec cntrl2(situation);
con_vec cntrl3(situation);
con_vec cntrl4(situation);
con_vec cntrl5(situation);
con_vec cntrlb0(situation);
con_vec cntrlb(situation);
con_vec cntrlR(situation);
con_vec bill(situation);
con_vec dynamic(situation);
con_vec arelys(situation);
con_vec Ramdu2(situation);

Car* pcar[MAXCARS];                // array of pointers to the various cars
char* nam_ptr[MAXCARS];            // array of pointers to their name strings
char* namptr[MAXCARS];             // re-arrangement of nam_ptr[]
int* order;                        // will point to array of positions
int* new_data;                     // points to array of new data indicators
int lap[MAXCARS];                  // set, then cleared as each lap completes
colors car_colors[MAXCARS];  // There should be MAXCARS of these color pairs:

// This is the permanent array of available drivers.
// Their order here determines their car colors.
con_vec(*drovers[])(situation) = {   // pointers to their control programs
   cntrlR, Ramdu2, cntrl1, cntrl2, cntrlb0, arelys,
   cntrl3, cntrlb, cntrl0, cntrl4, cntrl5,  bill,
   dynamic, dynamic  };

// This is the array used during the race.  (It will be re-arranged.)
// The order here determines their starting positions.
con_vec(*drivers[])(situation) = {
   cntrlR, Ramdu2, cntrl1, cntrl2, cntrlb0, arelys,
   cntrl3, cntrlb, cntrl0, cntrl4, cntrl5,  bill,
   dynamic, dynamic  };

void report_overall(int, int, char**);       // in REPORT.CPP
void report_results(int, int*, char**, Car**);      // in REPORT.CPP
void RAM_report(void);                       // in REPORT.CPP

// these things are defined in DRAW.CPP:
extern int xcon(double x);        // convert x coordinate to pixels
extern int ycon(double y);        // convert y coordinate to pixels
extern int round(double given);   // convert double to int by rounding
extern void draw_arc(double,double,double,double,double);
extern void make_dec_string(char* out, double input);
extern void get_avg_spd(int i, char* out);
extern void get_max_spd(int i, char* out);
extern double drawpath(double, double, double, segment*);
extern int drawcar(double, double, double, int, int);
extern void lapper(int which, int lap);  // shows lap count on scoreboard,
extern void resume_normal_display(void);
extern void leaders(int);
extern void scoreboard(void);
extern void graph_setup(void);
extern void refresh_finish_line(void);

// call repeatedly to change direction by 180 degrees:
inline void reverse(double v, double* alpha_ptr, double* vc_ptr)
{
   if(v > 10.0)         // This is algorithm to reverse velocity vector:
       *vc_ptr = 0.0;      // if not going very slow, brake hard
    else
       *vc_ptr = -15.0;    // when going slow enough, put 'er in reverse!
    *alpha_ptr = 0.0;      // don't turn.
}

/* This routine analyses the parameters to determine if the car is in an
   abnormal situation.  If so, it returns non-zero & sets the result
   vector so as to free the car.  If all is normal it returns zero.
   This is a service for the robot drivers; it is only called by them. */
int stuck(int backward, double v, double vn, double to_lft,
                             double to_rgt, double* alpha_ptr, double* vc_ptr)
{
   if(to_lft < 0.0)        // If over the left wall, 
      if(vn > .5 * v)  {  // test for more than 30 degrees off course
         reverse(v, alpha_ptr, vc_ptr);
         return 1;
      }
      else if(vn > -.5 * v && backward) {  // or going well backward
         reverse(v, alpha_ptr, vc_ptr);
         return 1;
      }
      else if(vn < -.5 * v) {    // heading away from wall,
         *alpha_ptr = .03;       // turn to left
         *vc_ptr = (.66667 * v + 10.0);  // accelerate toward 30 fps
         return 1;
      }
      else {
         *alpha_ptr = -.03;       // turn to right
         *vc_ptr = (.66667 * v + 10.0);  // accelerate toward 30 fps
         return 1;
      }
   else if(to_rgt < 0.0)  // if over the right wall:
      if(vn < -.5 * v)  {  // test for more than 30 degrees off course
         reverse(v, alpha_ptr, vc_ptr);
         return 1;
      }
      else if(vn < .5 * v && backward) {  // or going well backward
         reverse(v, alpha_ptr, vc_ptr);
         return 1;
      }
      else if(vn > .5 * v) {    // heading away from wall,
         *alpha_ptr = -.03;       // turn to right
         *vc_ptr = .66667 * v + 10.0;  // accelerate toward 30 fps
         return 1;
      }
      else {
         *alpha_ptr = .03;       // turn to left
         *vc_ptr = .66667 * v + 10.0;  // accelerate toward 30 fps
         return 1;
      }
   else if(backward)
     if(vn > .866 * v)  { // you are going more-or-less sideways left
        *alpha_ptr = -.03;
        *vc_ptr = .66667 * v + 10;
         return 1;
     }
     else if(vn < -.866 * v) {  // you are going more-or-less sideways rt.
        *alpha_ptr = .03;
        *vc_ptr = .66667 * v + 10;
         return 1;
     }
     else {
      reverse(v, alpha_ptr, vc_ptr);
      return 1;
     }
   else if(v < 15) {  // nothing wrong except you are going very slow:
      if(to_rgt > to_lft)     // you are on left side of track
         if(vn < -.7 * v)     // and you are not heading very much to right
            *alpha_ptr = -.03;
         else
            *alpha_ptr = .03;
      else                   // you are on the right side,
         if(vn > .7 * v)     // and you are not heading very much to left
            *alpha_ptr = .03;
         else
            *alpha_ptr = -.03;
     *vc_ptr = .66667 * v + 10;    // acellerate moderately
      return 1;
   }
   return 0;   // We get here only if all is normal.
}

// a sorting routine that's fast when things are already in order:
// This routine updates the order[] array which has the position of each
// car.  order[0] is the ID of the leader, order[1] is in second place, etc.
// Also, the new_data[] array is set to show which positions have changed.
void sortem(int num_disp)
{
   int i, temp;
   int old_order[MAXCARS];

   for(i=0; i<num_disp; i++)     // copy first part of order[] array
      old_order[i] = order[i];

   for(i=0; i<car_count-1; i++) // the while loop below does not usually repeat:
      while(farther(pcar[order[i]], pcar[order[i+1]]))  {
         temp = order[i];       // When a car passes another, we swap positions
         order[i] = order[i+1];
         order[i+1] = temp;
         if(i > 0)              // now we have to see if it passed another car
            --i;
      }

   for(i=0; i<num_disp; i++)     // see what has changed and flag it:
      new_data[i] = (old_order[i] != order[i]);
}

inline double vec_mag(double x, double y)  // sqrt of sum of squares
{                                          // (used for vector magnitude)
   return sqrt(x * x + y * y);
}

// Comparison routine for determining who's ahead: (i.e., sorting)
// Returns 0 iff car0 has gone farther than car1, 1 otherwise.
int farther(Car* car0, Car* car1)
{
   int blok0, blok1;  /* A block is the same as a track segment except for
                         segment 0.  For segment 0, the block is 0 when the
                         car is past the finish line.  When the car is heading
                         toward the finish line the block is NSEG.  */
   blok0 = car0->seg_id;
   if(!blok0)
      if(car0->to_end > from_start_to_seg1)
         blok0 = NSEG;
   blok1 = car1->seg_id;
   if(!blok1)
      if(car1->to_end > from_start_to_seg1)
         blok1 = NSEG;
   if(car0->laps > car1->laps)
      return 0;
   else if(car0->laps < car1->laps)
      return 1;                              // else laps are equal
   else if(blok0 > blok1)             
      return 0;
   else if(blok1 > blok0)
      return 1;                            // else they are in the same blok:
   else if(car0->to_end <= car1->to_end)
      return 0;
   else return 1;        // returns 1 in case of total equality
}

inline int incseg(int seg)       // returns the next segment of the track
{                                // (i.e. 0 yields 1, 1 yields 2, but
   if(++seg == NSEG)             // NSEG-1 yields 0)
      seg = 0;
   return seg;
}

// This is the model of the track friction force on the tire.  This force
// provides propulsion, cornering, and braking.  Force is assumed to depend
// only on the slip velocity, rising very rapidly with small slip velocity,
// and then asymtotically approaching an upper limit.  (This is similar to
// tires on unpaved surfaces.)  ALSO: There is randomness in the force at
// small slip velocities!
const double MYU_MAX = 1.0;  // maximum coeficient of friction, tire vs. track:
inline double rfriction(double slip)     // returns the coef. of friction,
{                                       // given the slip speed, ft. per sec.
   double slipping;     // slip speed at which half maximum force is reached

   slipping = 1.5 + (double)rand()/RAND_MAX;    // range is 1.5 to 2.5 fps
   return (MYU_MAX * slip)/(slipping + slip);
}

// This is a version of the above without any randomness.  They produce the
// same result when the random variable in rfriction() has its mean value.
const double SLIPPING = 2.0;
inline double friction(double slip)     // returns the coef. of friction,
{                                       // given the slip speed, ft. per sec.
   return (MYU_MAX * slip)/(SLIPPING + slip);
}

// Member Functions of the Car Class:

// returns laps and passes seg_id and distance to next segment via pointers:
inline int Car::where_is_it(int* seg_id_addr, double* to_end_addr)
{
   *seg_id_addr = seg_id;
   *to_end_addr = to_end;
   return laps;
}

// limits the rate of change and maximum value of the angle of attack:
double alpha_limit(double was,       // This is what alpha was
                   double request)   // The robot wants this alpha
{
   const double MAX_RATE = 4.6;  // maximum radians per second possible
   const double MAX_ALPHA= 1.0;   // maximum radians possible

   double alpha;                 // the result to return

   if(request - was > MAX_RATE * delta_time)   // want more positive alpha
      alpha = was + MAX_RATE * delta_time;
   else if(was - request > MAX_RATE * delta_time)  // want more negative alpha
      alpha = was - MAX_RATE * delta_time;
   else
      alpha = request;

   if(alpha > MAX_ALPHA)
      alpha = MAX_ALPHA;
   else if(alpha < -MAX_ALPHA)
      alpha = -MAX_ALPHA;

   return alpha;
}

// the purpose of control() is to call the car's driver function, and
// then to return the steering and throttle settings produced by
// that routine.
void Car::control(situation s)  // calls the control function via the pointer
{                               // that was installed in the car object by
   con_vec output;              // by the constructor.
   con_vec (*func_ptr)(situation);

   if(out)
      output.alpha = output.vc = 0.0;  // no action if out of race
   else {
      s.data_ptr = data_ptr;
      s.starting = starting;  // startup signal for the robot
      starting = 0;
      func_ptr = cntrl;
      output = (*func_ptr)(s);   // call the robot driver to set alpha and vc
   }
   alpha = output.alpha;   vc = output.vc;  // set private vars in car object
}

double power_excess(double vc, double sine, double cosine, double v)
{
   double Ln, Lt;      // normal and tangential components of slip vector
   double l;           // magnitude of slip vector, ft. per sec.
   double F;           // force on car from track, lb.
   double Fn, Ft;      // tangential and normal (to car's path) force components

   Ln = -vc * sine;   Lt = v - vc * cosine; // vector sum to compute slip vector
   l = vec_mag(Lt, Ln);                     // compute slip speed
   F = M * g * friction(l);                 // compute friction force from track
   if(l < .0001)              // to prevent possible division by zero
      Fn = Ft = 0.0;
   else  {
      Fn = -F * Ln/l;      // compute components of force vector
      Ft = -F * Lt/l;
   }
   // compute power delivered:
   return (vc < 0.0 ? -vc : vc) * (Ft * cosine + Fn * sine) - .9975*PM;
}

inline double abs(double x)
{
   if(x < 0.0)
      return -x;
   else
      return x;
}

inline double SIGN(double a, double b)
{
   return b >= 0.0 ? abs(a) : -abs(a);
}

// This routine was adapted from the book "Numerical Recipes in C" by
// Press, et. al.  It searches for a value of vc which causes the
// power to be very close to the maximum available.
// (This is Brent's method of root finding.)  x1 and x2 are values of
// vc which bracket the root.  b represents the variable vc.
double zbrent(double sine, double cosine, double v, double x1, double x2, double tol)
{
   const int ITMAX = 20;
   const double EPS = 1.0e-8;
	int iter;
	double a=x1, b=x2, c=x2, d,e,min1,min2;
	double fa=power_excess(a, sine, cosine, v);
	double fb=power_excess(b, sine, cosine, v);
   double fc,p,q,r,s,tol1,xm;
   double Ln, Lt;      // normal and tangential components of slip vector
   double l;           // magnitude of slip vector, ft. per sec.
   double F;           // force on car from track, lb.
   double Fn, Ft;      // tangential and normal (to car's path) force components

	if ((fa > 0.0 && fb > 0.0) || (fa < 0.0 && fb < 0.0))
		return b;              // This should never happen.
	fc=fb;
	for (iter=1;iter<=ITMAX;iter++) {
		if ((fb > 0.0 && fc > 0.0) || (fb < 0.0 && fc < 0.0)) {
			c=a;
			fc=fa;
			e=d=b-a;
		}
		if (abs(fc) < abs(fb)) {
			a=b;
			b=c;
			c=a;
			fa=fb;
			fb=fc;
			fc=fa;
		}
		tol1=2.0*EPS*abs(b)+0.5*tol;
		xm=0.5*(c-b);
		if (abs(xm) <= tol1 || fb == 0.0)
         return b;
		if (abs(e) >= tol1 && abs(fa) > abs(fb)) {
			s=fb/fa;
			if (a == c) {
				p=2.0*xm*s;
				q=1.0-s;
			} else {
				q=fa/fc;
				r=fb/fc;
				p=s*(2.0*xm*q*(q-r)-(b-a)*(r-1.0));
				q=(q-1.0)*(r-1.0)*(s-1.0);
			}
			if (p > 0.0) q = -q;
			p=abs(p);
			min1=3.0*xm*q-abs(tol1*q);
			min2=abs(e*q);
			if (2.0*p < (min1 < min2 ? min1 : min2)) {
				e=d;
				d=p/q;
			} else {
				d=xm;
				e=d;
			}
		} else {
			d=xm;
			e=d;
		}
		a=b;
		fa=fb;
		if (abs(d) > tol1)
			b += d;
		else
			b += SIGN(tol1,xm);
      Ln = -b * sine;   Lt = v - b * cosine; // vector sum to compute slip vector
      l = vec_mag(Lt, Ln);                     // compute slip speed
      F = M * g * friction(l);                 // compute friction force from track
      if(l < .0001)              // to prevent possible division by zero
         Fn = Ft = 0.0;
      else  {
         Fn = -F * Ln/l;      // compute components of force vector
         Ft = -F * Lt/l;
      }
      // compute power delivered:
      fb = (b < 0.0 ? -b : b) * (Ft * cosine + Fn * sine) - .9975*PM;
	}
	return b;
}

// Simulates the physics, moves the car by changing the state variables.
// The angle "alpha" is the angle between the car's orientation angle and
// its velocity vector.  (like angle of attack of an aircraft)
// Cornering force depends on alpha.  (It is also thought of as a slip angle.)
// "Slip" refers to wheel vs. track motion.
// "vc" is the speed of the bottom of the wheel relative to the car.
// This model is like a four-wheel drive car, since the forces are not
// computed separately for front and rear wheels.
void Car::move_car()
{
   double D;        // force on car from air, lb.
   double Fn, Ft;      // normal & tangential components of track force vector
   double P;           // power delivered to track, ft. lb. per sec.
   double v;           // car's speed
   double Ln, Lt;      // normal and tangential components of slip vector
   double l;           // magnitude of slip vector, ft. per sec.
   double F;           // force on car from track, lb.
   double x_a, y_a;    // accelleration components in x & y directions
   double sine, cosine, separation, dx, dy;
   double pvec_x, pvec_y;   // pointing vector of car (velocity vec + alpha)
   int i, other_seg;
   double dot, mag_prod, temp;
   double rel_xdot, rel_ydot;   // velocity relative to other car being hit

   if(out)            // This gets set if the car is stuck and off the track
      return;
   v = vec_mag(xdot, ydot);                     // the car's speed, feet/sec
   if(v > speed_max)                            // keep track of max speed
        speed_max = v;
   // limit how fast alpha can change, and its maximum value
   alpha = alpha_limit(prev_alpha, alpha);
   prev_alpha = alpha;
   sine = sin(alpha);   cosine = cos(alpha);    // alpha is angle of attack
   // don't allow reverse gear
   if(vc < 0.0)    // This would be a reverse gear request
      if(v > REVERSE_GEAR_LIMIT)   // if going too fast
         vc = 0.0;                      // make it maximum braking

   dead_ahead = 0;   // will be set if there is a car more-or-less dead ahead
   for(i=0; i<car_count; i++) {     // check for cars nearby or bumping
      if(i == which) continue;         // ignore oneself
      other_seg = pcar[i]->seg_id;     // only consider present and next segment
      if(!(seg_id == other_seg || incseg(seg_id) == other_seg))
          continue;
      dx = pcar[i]->x - x;      // components of vector to other car
      dy = pcar[i]->y - y;
      separation = vec_mag(dx, dy);      // distance to the other car
      if(separation > 2.5 * CARLEN)      // ignore cars farther away than this
         continue;
      // Is there a car dead ahead?  "dead ahead" means within 2.5 car lengths
      // AND within a few degrees of pointing vector  (arcos(.94) == 20 deg.)
      // First compute pointing vector by rotating velocity vector by alpha:
      pvec_x = xdot * cosine - ydot * sine;  // components of pointing vector:
      pvec_y = xdot * sine + ydot * cosine;
      // compute dot product, rel. position & pointing vectors:
      dot = pvec_x * dx + pvec_y * dy;
      mag_prod = separation * v;    // p_vec and v vectors are same length
      if(dot > .94 * mag_prod)      // test based on properties of dot product
         dead_ahead = 1;            // there is a car more-or-less dead ahead
      if(separation > CARLEN)    // If separation is greater than length,
           continue;             // cars cannot be bumping.
      // cars might be bumping, test for that.  Simplified test, assumes
      // cars are parallel.  Also, only test for a car in front, as the
      // other car will test also, and find this car in front.
      if(v < .01)              // to prevent division error just below
         continue;
      temp = dot / v;          // dot/v is longitudinal component of separation
      if(temp > CARLEN || dot < 0.0)
         continue;
      if(separation - temp/separation > CARWID)  // test for lateral separation
         continue;
      //The cars are bumping, reduce speed of rear car, and damage both;
      rel_xdot = xdot - pcar[i]->xdot;
      rel_ydot = ydot - pcar[i]->ydot;
      temp = rel_xdot * rel_xdot + rel_ydot * rel_ydot;  //impact energy
      damage += .35 * temp;
      pcar[i]->damage += .2 * temp;   // less damage to car in front
      xdot *= .8;
      ydot *= .8;
   }

   int it = 0;
   VC:    // maybe loop to control power (we don't permit P > PM)

   Ln = -vc * sine;   Lt = v - vc * cosine; // vector sum to compute slip vector
   l = vec_mag(Lt, Ln);                     // compute slip speed
   F = M * g * friction(l);                 // compute friction force from track
   D = DRAG_CON * v * v;                    // air drag force
   if(offroad)  {                       // if the car is off the track,
      D = (0.6 + .008 * v) * M * g;     // add a lot more resistance
      if(veryoffroad)
         D += 1.7 * M * g;
   }
   if(l < .0001)              // to prevent possible division by zero
      Fn = Ft = 0.0;
   else  {
      Fn = -F * Ln/l;      // compute components of force vector
      Ft = -F * Lt/l;
   }
   // compute power delivered:
   P =  (vc < 0.0 ? -vc : vc) * (Ft * cosine + Fn * sine);

   if(!it)  { 
      power_req = P/PM;  // Tell the driver how much power it requested.
      if(P > PM) {       // If the request was too high, reduce it to 100% pwr.
          ++it;
          vc = zbrent(sine, cosine, v, v * cosine, vc, .006);
          goto VC;
      }
   }

   // put some randomness in the magnitude of the traction force, F:
   if(Ft != 0.0) // This might be set to 0.0 above, in which case F will be zero
      temp = rfriction(l) * M * g / F;  // ratio of new to original force
   else
      temp = 1.0;

   // compute centripetal and tangential acceleration components:
   cen_a = Fn * temp / M;
   tan_a = (Ft * temp - D) / M;
   if(offroad)  {        // this makes it stop when offroad and reversed
      damage += int(tan_a * tan_a + cen_a * cen_a) / 10;
      if(damage > 30000) {
         ++out_count;
         out = 1;
         return;
      }
   }
   if(v < .0001)                         // prevent division by zero
      adot = sine = cosine = 0.0;
   else  {
     adot = cen_a / v;                         // angular velocity
     sine = ydot/v;   cosine = xdot/v;         // direction of motion
   }
   x_a = tan_a * cosine - cen_a * sine;      // x & y components of acceleration
   y_a = cen_a * cosine + tan_a * sine;
   xdot += x_a * delta_time;                 // update the state vector:
   ydot += y_a * delta_time;
   x += (xdot + .5 * x_a * delta_time) * delta_time;
   y += (ydot + .5 * y_a * delta_time) * delta_time;
   if(v >= .0001)
      ang = atan2(ydot,xdot);                   // new orientation angle
}

void Car::draw_car(void)  // Calls drawcar() twice, once to erase, once to draw
{
   if(out)       // Don't redraw a car that is out of the race.
      return;
   drawcar(prex, prey, prang, TRACK_COLOR, TRACK_COLOR);     // erase old one
   drawcar(x, y, prang = ang+alpha, nose_color, tail_color); // draw new one
   prex = x;   prey = y;     // save these to erase car next time
}

// The Car constructor:
Car::Car(colors use_these, double x_pos, double y_pos, double alf_ang, int i)
{
   which = i;
   starting = 1;
   cntrl = drivers[i];
   out = lap_flag = seg_id = dead_ahead = 0;
   init_flag = damage = 0;
   laps = -1;    // to become 0 crossing the finish line at start of race
   to_end = trackin[0].length;
   alpha = ydot = prang = ang = prev_alpha = 0.0;
   speed_avg = speed_max = 0;
   nose_color = use_these.nose;
   tail_color = use_these.tail;
   prex = x = x_pos;
   prey = y = y_pos;
   xdot = vc = STARTING_SPEED;
   ang = alf_ang;
   power_req = .9;
   data_ptr = (void*)new char[512];
   if(!no_display)
      draw_car();    // draw the new car on the screen
}

Car::~Car(void)
{
   if(!no_display)
      drawcar(prex, prey, prang, TRACK_COLOR, TRACK_COLOR);  // erase old car
   delete [] (char *)data_ptr;
}

// This function uses the current state of the car, and the current
// track segment data, to compute the car's local situation as seen by
// the driver.  (see struct situation)
situation Car::observe(rel_state* rel_vec_ptr)
{
   double rad;                // current radius
   double dx, dy, xp, yp;
   double sine, cosine, dot;  // dot used for dot product of two vectors
   double temp;
   situation s;
   int nex_seg;         // segment ID of the next segment
   double separation, dxdot, dydot;
   int i, k, m, other_seg;
   // These two arrays describe the three closest cars:
   // element 0 is for the closest, element 1 next, element two after that.
   int closest[] = { 999, 999, 999 };       // their ID's, initially too big
   double how_close[] = { 1e5, 1e5, 1e5 };  // initially very large distances
   int flag = 1;        // controls possible repetition of calculations due to
                        // completion of a lap.
   if(out)            // This gets set if the car is stuck and off the track
      return(s);

   s.nearby = rel_vec_ptr;
   s.v       = vec_mag(xdot, ydot);         // the actual speed
   s.dead_ahead = dead_ahead;         // copy the value set by move_car()
   s.power_req = power_req;           // copy the value set by move_car()
   // Computations are base on the right wall of track, which is described
   // by trackout[], except radii are based on the smaller, or inner, curve.

   while(flag)  {  // This loop repeats only when a segment boundary is crossed,
                   //   in which case it repeats once:

   sine = sin(temp = trackout[seg_id].beg_ang);     // track direction
   cosine = cos(temp);
   if((nex_seg = seg_id + 1) == NSEG)               // which segment is next
      nex_seg = 0;
   s.nex_len = trackout[nex_seg].length;            // length and radius
   s.nex_rad = trackin[nex_seg].radius;             // of next segment
   if(s.nex_rad < 0.0)                              // always use smaller radius
      s.nex_rad = trackout[nex_seg].radius;
   if(++nex_seg == NSEG)
      nex_seg = 0;
   s.after_rad = trackin[nex_seg].radius;         // and the one after that
   if(s.after_rad < 0.0)                          // always use smaller radius
      s.after_rad = trackout[nex_seg].radius;
   s.cur_len = trackout[seg_id].length;           // copy these two fields:
   s.cur_rad = rad = trackin[seg_id].radius;  // rt. turn will use trackout
   if(seg_id != 0)      // This is used to tell when the finish line is 
      lap_flag = 0;     // crossed, thus completing each lap.     

   if(rad == 0) {   // if current segment is straight,
      // xp and yp locate the car with respect to the beginning of the right
      // hand wall of the straight segment.  calculate them:
      dx = x - trackout[seg_id].beg_x;
      dy = y - trackout[seg_id].beg_y;
      xp = dx * cosine + dy * sine;
      yp = dy * cosine - dx * sine;
      s.to_rgt = yp;                   // fill in to_rgt and to_end:
      s.to_end = s.cur_len - xp;

      if(seg_id == 0)
         if(xp > FINISH * s.cur_len && !lap_flag) {
            lap_flag = 1;        // This means the line crossing was noted.
            if(++laps == lap_count)
               ++done_count;
            if(!no_display)
               lapper(which, laps);  // display the new lap count
            lap[which] = 1;
            if(laps == 0)       // record when the starting line is crossed:
                start_time = time_count;
            else      // update overall average speed:
                speed_avg = length * laps / (time_count-start_time);
         }

      // here we make sure we are still in the same segment:
      if(s.to_end <= 0.0)  {           // see if a lap has been completed,
         if(++seg_id == NSEG)
            seg_id = 0;
         continue;           // repeat the loop in context of next segment
      }
      s.to_lft = width - yp;           // fill in to_lft & cur_rad:
      s.cur_rad = 0.0;
      s.vn      = ydot * cosine - xdot * sine;  // compute cross-track speed
      s.backward = (xdot * cosine + ydot * sine < 0.0);
   }
   else if(rad > 0.0) {      // when current segment is a left turn:
      dx = x - trackout[seg_id].cen_x; // compute position relative to center
      dy = y - trackout[seg_id].cen_y;
      temp = atan2(dy, dx);           // this is the current angular position
      s.to_end = trackout[seg_id].end_ang - temp - PI/2.0;  // this is an angle
      if(s.to_end > 1.5 * PI)
         s.to_end -= 2.0 * PI;
      else if(s.to_end < -.5 * PI)
         s.to_end += 2.0 * PI;
      if(s.to_end <= 0.0)  {           // Handle segment crossing:
         if(++seg_id == NSEG)
            seg_id = 0;                // going from last segment to 1st
         continue;
      }
      s.to_lft = vec_mag(dx, dy) - rad;
      s.to_rgt = width - s.to_lft;
      s.vn = (-xdot * dx - ydot * dy)/vec_mag(dx, dy);   // a trig thing
      s.backward = (ydot * dx - xdot * dy < 0.0);
   }
   else {
      s.cur_rad = rad = trackout[seg_id].radius;  // rt. turn needs trackout
      dx = x - trackout[seg_id].cen_x; // compute position relative to center
      dy = y - trackout[seg_id].cen_y;
      temp = atan2(dy, dx);           // this is the current angular position
      s.to_end = -trackout[seg_id].end_ang + temp - PI/2.0;  // this is an angle
      if(s.to_end < -.5 * PI)
         s.to_end += 2.0 * PI;
      else if(s.to_end >= 1.5 * PI)
         s.to_end -= 2.0 * PI;
      if(s.to_end <= 0.0)  {           // Handle segment transistion:
         if(++seg_id == NSEG)
            seg_id = 0;
         continue;
      }
      s.to_rgt = vec_mag(dx, dy) + rad;
      s.to_lft = width - s.to_rgt;
      s.vn = (xdot * dx + ydot * dy)/vec_mag(dx, dy);   // a trig thing
      s.backward = (xdot * dy - ydot * dx < 0.0);
   }

   // If we get this far, we do not repeat the big loop.
   flag = 0;
   }  // end while(flag) loop
   if(s.backward)  {
      to_end = s.to_end;
      offroad = (s.to_lft < 0.0 || s.to_rgt < 0.0);   // maybe set offroad flag
      veryoffroad = (s.to_lft < -width || s.to_rgt < -width); // maybe veryoffroad
      return s;
   }

   offroad = (s.to_lft < 0.0 || s.to_rgt < 0.0);   // maybe set offroad flag
   veryoffroad = (s.to_lft < -width || s.to_rgt < -width); // maybe veryoffroad
   to_end = s.to_end;                       // fill in this field in Car object

   // find three closest cars in front:
   // "front", here, is direction of velocity vector, not pointing vector.
   for(i=0; i<car_count; i++) {     // check for cars nearby
      if(i == which) continue;         // ignore oneself
      other_seg = pcar[i]->seg_id;     // only consider present and next segment
      if(!(seg_id == other_seg || incseg(seg_id) == other_seg))
          continue;
      dx = pcar[i]->x - x;      // components of vector to other car
      dy = pcar[i]->y - y;
      separation = vec_mag(dx, dy);      // distance to the other car
      // compute dot product, rel. position & velocity vectors:
      dot = xdot * dx + ydot * dy;
      if(dot < 0.0)
         continue;              // Ignore cars behind you.
      for(k=0; k<3; k++)  {
         if(separation < how_close[k])  {
            for(m = 2; m > k; m--) {
               closest[m] = closest[m-1];
               how_close[m] = how_close[m-1];
            }
            closest[k] = i;
            how_close[k] = separation;
            break;
         }
      }
   }
     // now compute local relative state vector for those three cars:
   for(k=0; k<3; k++)  {
      if(closest[k] > car_count) {  // This is when there are less than 3
         for(; k<3; k++)
            s.nearby[k].who = 999;
         break;
      }
      i = closest[k];
      dx = pcar[i]->x - x;      // components of relative position vector
      dy = pcar[i]->y - y;
      dxdot = pcar[i]->xdot - xdot; // components of relative velocity
      dydot = pcar[i]->ydot - ydot;
      // the relative vectors must be found wrt to velocity vector frame
      // we need sine and cosine of rotation of axes:
      if(s.v > 1e-7)  {        // prevent division by 0
         sine = - xdot / s.v;                 // direction of velocity
         cosine = ydot / s.v;                  // wrt the x axis.
      }
      else  {
         sine = -1.0;
         cosine = 0.0;
      }
      s.nearby[k].who = i;
      s.nearby[k].rel_x = cosine * dx + sine * dy;
      s.nearby[k].rel_y = cosine * dy - sine * dx;
      s.nearby[k].rel_xdot = cosine * dxdot + sine * dydot;
      s.nearby[k].rel_ydot = cosine * dydot - sine * dxdot;
   }

   return s;
}

// create a default situation vector to pass to control() during initializaton
situation fill_situation(rel_state* rel_state_vec_ptr)
{
   situation result;

   result.cur_rad = 0.0;
   result.cur_len = 1.0;
   result.to_lft =  1.0;    
   result.to_rgt =  1.0;  
   result.to_end =  1.0;    
   result.v = STARTING_SPEED;         
   result.vn = 0.0;        
   result.nex_len = 1.0;   
   result.nex_rad = 1.0;   
   result.after_rad = 0.0; 
   result.power_req = 1.0; 
   result.dead_ahead = 0;   
   result.backward = 0;     
   result.nearby = rel_state_vec_ptr;
   for(int k=0; k<3; k++)
      result.nearby[k].who = 999;

   return result;
}

// gets the names from the robot drivers and stores them in RAM.
// nam_ptr[] is the array of pointers to the names.
// some elements of nam_ptr[] may point to the same place; others may be 0;
void get_names(void)
{
   int i, j;
   situation s;                    // to pass data from observe() to control()
   rel_state rel_state_vec[3];     // needed by fill_situation() function
   con_vec (*func_ptr)(situation);

   for(i=0; i<MAXCARS; i++) {
      glob_name[0] = 0;        // to check later to see if driver filled it
      // fill in s to avoid div. by zero in driver.
      s = fill_situation(rel_state_vec);
      // The first call to a robot driver may put its name in glob_name.
      func_ptr = drovers[i];
      (*func_ptr)(s);
      if(glob_name[0] != 0)  {   // add new name to nam_ptr[] array:
         nam_ptr[i] = new char[strlen(glob_name) + 1];
         strcpy(nam_ptr[i], glob_name);
         *(nam_ptr[i] + 8) = '\0';   // chop any names greater than 8 chars.
      }
      else                       // if no name was given, see if there is
         for(j=i-1; j>=0; j--)   // already a name for that driver:
            if(drovers[i] == drovers[j]) {
               nam_ptr[i] = nam_ptr[j];
               break;
            }

   }
}

// Will put a random permutation of the robot drivers in "drovers[]" 
// into the "drivers[]" array.
void pick_random_order(void)
{
     int selected[MAXCARS];  // marks the points as they are picked
     int racers[MAXCARS];    // the random permutation goes here
     int i, j, k, count;

     for(i=0; i<car_count; i++) {     // initialize selected[] to all zeroes:
	     selected[i] = 0;    // (meaning no points are picked)
     }

     // for each element of rptr, j is no. of alternates
     for(j=car_count; j>0; j--) {  // remaining.  here we pick one of them:
        k = (j > 1) ? ((int)random(j)) : (0);       // k chooses among the remaining pts.
	     for(i=0, count=0; i<car_count; i++)
   	     if(!selected[i])            // if not already picked,
	           if(count++ == k)  {
		           selected[i] = 1;     // marks this point as taken
		           racers[j-1] = i;       // puts it into the genome
	           }
     }
     // now copy the selected members of drovers[] into drivers[],
     // and the same for their colors & names:
     for(i=0; i<car_count; i++) {
        drivers[i] = drovers[racers[i]];
        car_colors[i] = car_clrs[racers[i]];
        namptr[i] = nam_ptr[racers[i]];
     }

}

// copy drovers[] into drivers[], and the same for their colors & names:
void copy_drivers(void)
{
   int i;
   for(i=0; i<car_count; i++) {
      drivers[i] = drovers[i];
      car_colors[i] = car_clrs[i];
      namptr[i] = nam_ptr[i];
   }

}

void reverse_order(void)
{
   colors temp_color;
   con_vec(*temp_driver)(situation);
   char *temp_ptr;
   for(int i=0; i<car_count/2; i++) {
        temp_driver = drivers[i];
        drivers[i] = drivers[car_count - 1 - i];
        drivers[car_count - 1 - i] = temp_driver;
        temp_ptr = namptr[i];
        namptr[i] = namptr[car_count - 1 - i];
        namptr[car_count - 1 - i] = temp_ptr;
        temp_color = car_colors[i];
        car_colors[i] = car_colors[car_count - 1 - i];
        car_colors[car_count - 1 - i] = temp_color;
   }
}

main(int argc, char* argv[])       // 1st arg is no. of cars in race
{                                  // 2nd arg is no. of laps to race
   int i, j, ml;
   double x, y, dx, dy;            // used only for initial positions of cars
   situation s;                    // to pass data from observe() to control()
   int num_disp;   // how many cars to display in leaders area of scoreboard
   int starting;        // indicates the race is just starting
   rel_state rel_state_vec[3];    // contains up to 3 relative state vectors

   glob_name = new char[33];
   strcpy(glob_name, "Not yet filled in, 32 characters");

   get_names();  // call all the robot drivers to fill in the nam_ptr[] array.

   // get argument values into car_count and lap_count global variables
   get_args(argc, argv);       // sets car_count and lap_count
   num_disp = (car_count < 5) ? car_count : 5;  // show up to 5
   if(no_display)
      real_speed = 0;

   // setup the graphics display, also compute track data:
   graph_setup();

   randomizer();
   report_overall(car_count, lap_count, nam_ptr);
   // Main loop - Once through here for every complete race
 for(ml=0; ml<race_count; ++ml)  {
   done_count = 0;  // incremented by each car that finishes the race
   out_count = 0;   // incremented by each car that crashes
   time_count = 0.0;  // elapsed time, seconds
   starting = 1;

   if(keep_order)
      copy_drivers();
   else
      if(!(ml%2))
         pick_random_order();  // arrange the starting positions
      else
         reverse_order();      // reverse positions for 2nd race

   // create cars (instances of car class)
    dy = width/4;          // the dy and dx stuff is for arranging
   dx = 2.5 * CARLEN;     // the cars in their starting positions
   x = trackout[0].beg_x - dx;                // We assume that segment
   for(i=0; i<car_count; i++) {                    // 0 is straight, and
      if(!(i%4)) {                                 // parallel to the x axis.
         y = trackout[0].beg_y + dy/2;             // Cars start on seg. 0.
         x += dx;
      }
      else
         y += dy;
      pcar[i] = new Car(car_colors[i], x, y, 0, i);   // This is the key part!
   }
   if(!car_count) {  //When zero cars are requested, we only show the track.
      if(no_display)  {
         cout << "Zero cars were requested." << endl;
         exit(0);
      }
      get_ch();         // this executes only when 0 cars are requested
      resume_normal_display();
      exit(0);
   }

   // Put up the scoreboard and leader board:
   if(!no_display)
      scoreboard();

   done_count = 0;
   one_tick(1);
   order = new int[car_count];    //setup the order[] array:
   new_data = new int[num_disp];  //setup the new_data[] array:
   for(i=0, j=car_count-1; i<car_count; i++, j--) { // initialize order[] array
      order[i] = j;            // must correspond with initial track positions
      lap[i] = 1;
   }
   if(!no_display)
      if(get_ch() == ESC)  // Don't actually start the race until someone hits a key.
         break;               // Any key except ESC begins the race.
   // BEGIN THE RACE!
   // Note that the lap count on the scoreboard is updated by the
   // observe() function, which call lapper() to do that.
   if(no_display)
      cout << "Beginning race " << ml+1 << endl;
   while(1) {                                // main loop of the race:
      for(i=0; i<car_count; i++) {           // for each car:
         s = pcar[i]->observe(rel_state_vec); // compute its local situation
         pcar[i]->control(s);                // compute a control vector
      }
      for(i=0; i<car_count; i++) {     // for each car:
         pcar[i]->move_car();                // update state of car
      }
      if(!no_display)
         for(i=0; i<car_count; i++)       // for each car:
           pcar[i]->draw_car();             // update screen image of car
      time_count += delta_time;        // Advance the simulated time.
      sortem(num_disp);      // maintains the order[] and new_data[] arrays
      // For any line flagged by new_data[], update that line in "LEADERS" area:
      for(i=0; i<num_disp; i++)
         if(starting || new_data[i] || lap[order[i]]) {
            lap[order[i]] = 0;
            if(!no_display)
               leaders(i);
         }
      if(!no_display)
         refresh_finish_line();
      if(kb_hit())    // respond to the keyboard if its hit
         if(get_ch() == ESC)     // ESC key will end the race.
            break;
      // check for race over:
      if(done_count >= 2 || done_count >= car_count - out_count)
         break;
      if(real_speed)
         one_tick(0);      // wait here till the next clock tick
      starting = 0;       // the race is on!
   }
   if(no_display)
      cout << "End of race " << ml+1 << endl;

   report_results(ml+1, order, namptr, pcar);

   if(!no_display)
      if(get_ch() == ESC)  // Continue showing end of race until someone hits a key.
          break;

   for(i=0; i<car_count; i++) { // delete all the car objects
      delete pcar[i];
   }
 } // End of Main Loop.

   /* clean up, end graphics, back to normal */
   resume_normal_display();
   RAM_report();
   return 0;  // all done with execution, return to DOS.
}