// CNTRL1.CPP - "driver" function for RARS - M. Timin, Dec. 1994
// (see CNTRL0.CPP for better comments)
// adapted to ver. 0.39 3/6/95 by M. Timin

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

extern char* glob_name;       // The name string, below, will be copied here
extern const double CARWID;   // car width, feet

// The task of this function is to compute vc and alpha.  vc is the speed
// of the bottom of the wheel relative to the car. (Velocity Commanded)
// If the car is coasting at zero power then vc == s.v, the speed of the
// car.  If vc is held constant for a while the cars speed will approach
// vc, unless it is cornering, in which case vc may greatly exceed the
// speed.  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.)
con_vec cntrl1(situation s)
{
   const char name[] = "Knuth";      // This is the robot driver's name!
   static int init_flag = 1;           // cleared by first call
   // A "lane" is just a preferred distance from the wall of the track.
   static double lane = 75;    // determines right-left position on straightaway
   static int lane_time = 0;           // counts time when not in normal lane
   con_vec result;                     // This is what is returned.
   double width;                      // track width, feet
   const double normalane = 75.0;      // usual position down the straight
   const double steer_gain = .18;      // servo parameters on the straight:
   const double steer_damp = .3;
   double corn_speed;     // target speed for cornering, ft/sec
   double alpha, vc;           // components of result
   static double power_control = 1.0;
   static double slip_var = 90.0;
   const double init_slip_var = 90.0;
   double temp;

   if(init_flag)  {            // first time through, only copy name:
      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;

   // power_control is recent past average of power_req
   power_control += .175 * (s.power_req - power_control);
   slip_var -= 4.0 * (power_control - 1.0);

   if(s.cur_rad > 0.0)
      corn_speed = 61.3 * sqrt(s.cur_rad/100.0);
   else if(s.cur_rad < 0.0)
      corn_speed = 61.3 * sqrt((-s.cur_rad)/100.0);
   else corn_speed = 61.3;

   // maybe choose a different lane: (to help in passing)
   if(!s.dead_ahead) {
      if(lane_time <= 0) {
         lane = normalane;
         lane_time = 0;
      }
   }
   else if(!lane_time)   {
      // pick a different lane:
      if(lane >= 80)               // pick a new lane somehow:
         lane = random(60) - 20;
      else if(lane <= -80)
         lane = 20 - random(60);
      else
         lane = random(180) - 90;
      lane_time = 160;              // we will stay in the new lane this long
   }
   if(lane_time > 0)
      --lane_time;

   // set alpha based on a servo-mechanism approach, trying to maintain
   // a certain distance from the track wall:
   width = s.to_lft + s.to_rgt;                        // find width of track
   if(s.cur_rad == 0)  {     // If we are on the straightaway:
      // alpha is proportional to lane error:
      alpha = .35 * steer_gain * (s.to_lft - s.to_rgt - lane) / width;
      if(s.dead_ahead)       // change lanes quicker if someone in front
          alpha *= 2.0;
   }
   else if(s.cur_rad > 0) {  // in the turn, stick close to the inside:
      alpha = steer_gain * (4 * (s.to_lft-2.5*CARWID) / width
                                                       + .2 * width/s.to_rgt);
      if(s.dead_ahead)    // if someone ahead, reduce alpha to pass on outside
          alpha *= .8;
   }
   else {                   // this is for right turns, similar to above
      alpha = -steer_gain * (4 * (s.to_rgt-2.5*CARWID) / width
                                                        + .2 * width/s.to_lft);
      if(s.dead_ahead)
          alpha *= .8;
   }
   alpha -= steer_damp * s.vn / s.v;  // This is damping, to prevent oscillation

   if(s.cur_rad == 0)         // If we are on a straightaway,
      if(s.to_end/s.cur_len > .32)      // if we are far from the end:
         vc = s.v + slip_var/s.v;        // keep accellerating near full power
      else  {                 // otherwise,
         // decelerate, approaching cornering speed:
         vc = .1 * (9*s.v + corn_speed);
         // when near end of straight, steer toward the turn (assumed left!)
         temp = .55 * (s.v/corn_speed) *
                                (s.cur_len/(s.to_end + s.cur_len) - (1/1.32));
         alpha += temp * (lane + 90.0) / 180.0;
         if(s.dead_ahead)    // cut to left if someone's dead ahead!
             alpha *= 2.0;
      }
   else     {       // if we're in the curve, maintain speed, +
      vc = .5*(s.v+corn_speed) + .9 / (s.to_end + .4);
      slip_var = init_slip_var;
   }

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