// CNTRL4.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 const double CARWID;
extern char* glob_name;

con_vec cntrl4(situation s)
{
   const char name[] = "Wirth";
   static int init_flag = 1;
   con_vec result;
   double alpha, vc;
   double width;                      // track width, feet
   double steer_gain = .18;
   const double steer_damp = .48;
   static int lane_time = 0;           // counts time when not in normal lane
   const double normalane = -80.0;
   static double lane = -80.0; // determines right-left position on straigtaway
   const double corn_con = 6.12;
   static double corn_spd = 61.2;

   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.cur_rad == 0.0 && s.nex_rad != 0.0)
      corn_spd = corn_con * sqrt(s.nex_rad > 0.0 ? s.nex_rad : -s.nex_rad);
   else
      corn_spd = corn_con * sqrt(s.cur_rad > 0.0 ? s.cur_rad : -s.cur_rad);

   // 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 = 175;              // we will stay in the new lane this long
   }
   if(lane_time > 0)
      --lane_time;

   width = s.to_lft + s.to_rgt;                        // find width of track
   if(s.cur_rad == 0)                                                  
      alpha = (s.dead_ahead ? .45 : .25) * steer_gain *
                                          (s.to_lft - s.to_rgt - lane) / width;   
   else if(s.cur_rad > 0)
      alpha = steer_gain * (3,5 * (s.to_lft-1.6*CARWID) / width
                                                       + .4 * width/s.to_rgt);
   else
         alpha = -steer_gain * (3.5 *
                         (s.to_rgt - (s.nex_rad > 0.0 ? width/3 : 1.6*CARWID))
                                                / width  + .4 * width/s.to_lft);
   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 > .2)      // if we are far from the end:
         vc = s.v +  90/s.v;        // keep accellerating near full power
      else  {                 // otherwise,
         vc = corn_spd;           // maintain cornering speed, braking at first
         // this next formala will not work if a rt. turn follows the straight:
         alpha += .2 * (s.cur_len/(s.to_end + s.cur_len) - (1/1.2));
      }
   else            // if we're in the curve, maintain speed, +
      vc = corn_spd + .9 / (s.to_end + .4);

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