// ARELYS.CPP - A robot "driver" for RARS, by M. Timin, February, 1995
// for ver. 0.39 3/6/95
// for ver. 0.60

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

const double CORN_SPD_CON = 5.7;   // determines how fast to take corners
const double BIAS = 0.05;          // estimate of alpha when cornering
const double STEER_GAIN = 0.7;    // servo gain, for staying in "lane"
const double STEER_DAMP = 2.0;     // servo damping, to prevent "weaving"
const double SLIP_LIM = 4.0;     // maximum wheel slip, ft/sec, in wheel_slip()
const double SLIP_CON = 199.0;    //
const double SHARP_TURN = .1;    // change in alpha when attempting to pass
const int PASSING_TIME = 70;     // time to stay in passing maneuver, counts
const double CUSHION = 10.0;     // feet from inside rail to shoot for
const double CRIT_WIDTHS = 2.9;  // length in widths meaning nearing corner
const double BRAK_ACCEL = 23.0;  // braking acceleration, ft/sec sq.

double sdc(double radius)
{
   if(radius < 0.0)         // change sign of negative radius
      radius = -radius;
   else if(radius == 0.0)   // This is just insurance, this funtion doesn't
      return(200.0);        // make sense when the radius is zero.
   return CORN_SPD_CON * sqrt(radius);
}

double sdw(double goal, double present)
{
   double ws;

   if(present > goal + 2 * SLIP_LIM)  // if too fast,
      ws = present - SLIP_LIM;      // slow down.
   else if(present < goal - 2 * SLIP_LIM)  // if too slow,
      ws = present + SLIP_LIM;             // accelerate.
   else                           // if quite close,
      ws = (goal + present) / 2;      // approach desired speed gently.

   if(ws < present)          // can slip more when braking
     ws -= (present - ws);

   return ws;
}

double widths(double len, double rad, double wide)
{
   if(rad == 0.0)
      return len/wide;
   else if(rad > 0.0)
      return (rad+.5*wide)*len/wide;
   else
      return (.5*wide - rad)*len/wide;
}

con_vec Arelys(situation &s)
{
   const char name[] = "Arelys";      // This is the robot driver's name!
   static int init_flag = 1;          // cleared by first call
   double speed;                      // target speed for cornering, ft/sec
   double speed_next;                 // target speed for next corner
   con_vec result;                    // This is what is returned.
   double width;                      // track width, feet
   double alpha, vc;           // components of result
   static double alpha_inc = 0.0;  // alpha increment during passing maneuver
   static int counting = 0;    // will be set and counting down when passing
   double dest;                // target distance from left wall
   double arg;                 // for temporary storage of calculation
   double widths_cur;          // length remaining of current segment, widths
   static int started = 0;     // set after the "dragout"
   double redline;             // speed at which to begin braking on straight
   double bias;                // estimated alpha value for a curve
 
   if(init_flag)  {            // first time through, only copy name:
      my_name_is(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;

   width = s.to_lft + s.to_rgt;
   widths_cur = widths(s.to_end, s.cur_rad, width);
 //  widths_nex = widths(s.nex_len, s.nex_rad, width);

   if(s.cur_rad == 0.0)
      if(s.nex_rad > 0.0)
         dest = 2.0 * CUSHION;
      else
         dest = width - 2.0 * CUSHION;
   else if(s.cur_rad > 0.0)
      if(s.nex_rad >= 0.0)
         dest = CUSHION;
      else if(widths_cur > CRIT_WIDTHS)
         dest = CUSHION;
      else
         dest = .5 * width;
   else
      if(s.nex_rad <= 0.0)
         dest = width - CUSHION;
      else if(widths_cur > CRIT_WIDTHS)
         dest = width - CUSHION;
      else
         dest = .5 * width;

   // calculate target speeds for current corner and the next:
   if(s.cur_rad == 0.0)
      arg = 0.0;
   else if(s.cur_rad > 0.0)
      arg = s.cur_rad + .5 * width;
   else
      arg = s.cur_rad - .5 * width;
   speed = sdc(arg);      // speed is based on radius

   if(s.cur_rad == 0.0)     // calculate a bias for alpha in a turn:
      bias = 0.0;
   else if(s.cur_rad > 0.0)
      bias = 100 * BIAS / speed;
   else
      bias = -100.0 * BIAS / speed;

   if(s.nex_rad == 0.0)     // calculate speed for next turn
      arg = 0.0;
   else if(s.nex_rad > 0.0)
      arg = s.nex_rad + .5 * width;
   else
      arg = s.nex_rad - .5 * width;
   speed_next = sdc(arg); // of center line of track.

   alpha = bias + STEER_GAIN * (s.to_lft - dest) / width;
   alpha -= STEER_DAMP * s.vn / s.v;  // This is damping, to prevent oscillation

   // now set the tire speed, vc:
   if(s.cur_rad == 0.0) {                // If we are on a straightaway,
      redline = sqrt(speed_next * speed_next + BRAK_ACCEL * 2.0 * s.to_end);
      if(s.v > .85 * redline)
          started = 1;
      if(s.v < redline)
         vc = s.v + SLIP_CON / s.v;          // keep accellerating near full power
      else                     // otherwise,
         vc = sdw(speed_next, s.v);    // brake for next corner
   }
   else                              // If we're in the curve, maintain speed.
      if(widths_cur > CRIT_WIDTHS)
                  // if we are far from the next corner, stay at "speed".
         vc = sdw(speed, s.v);
      else        // but when we near the next corner, adjust to "speed_next"
         vc = sdw(speed_next, s.v);
   if(!started)   // don't steer during dragout
      if(s.vn > .02 * s.v || s.vn < -.02 * s.v)    // except if astray
         started = 1;
      else
         alpha = 0;
   // The passing maneuver:
   if(s.dead_ahead & !counting)  {  // When first encountering the car ahead:
      counting = PASSING_TIME;         // setup the timer,
      if(rand() < RAND_MAX/2)        // choose a right or left maneuver:
         alpha_inc = SHARP_TURN;
      else
         alpha_inc = -SHARP_TURN;
   }
   if(counting)  {                // If we are still in the passing maneuver,
      alpha += alpha_inc;              // change alpha
      --counting;                      // count down to zero
   }

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