// BINGO.CPP
// This is just a fairly simple modification of my first robot Bingo so
// that the car will stay on the circuit fourmile.trk.

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

const double BREAK_ANGLE   = 0.06;
const double NBA           = 0.1;
const double DAMPING       = 4.2;
const double GAIN          = 0.01;
const double FC            = 1;      /*Friction Coefficient*/
// const double g             = 32.2;
const double V_FACTOR      = 1.3;
const double VF2           = 1.06;
const double ZFACT         = 0;

double cnr_speed(double rho)
{
  return sqrt(abs(rho)*g)*VF2;
}

con_vec Bingo1(situation& s)
{
   const char name[] = "Bingo1";
   static int init_flag = 1;
   con_vec result;
   static double alpha, vc, width, a, b, v1, v2, u, c;
   static int start,off;

   if(init_flag)  {
      my_name_is(name);
      init_flag = 0;
      result.alpha = result.vc = 0; start = 1;
      return result;
   }

   if(start){
     start = 0; width = s.to_lft + s.to_rgt;
     a = s.cur_rad; b = s.cur_len; v1 = cnr_speed(s.nex_rad);
   }

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

  if((s.cur_rad!=a) || (s.cur_len!=b)) {
    a = s.cur_rad; b = s.cur_len; c = v1; off = 0;
    if((s.after_rad!=0) && (s.nex_rad!=0)){
      v1 = cnr_speed(s.nex_rad);
      v2 = cnr_speed(s.after_rad+width*ZFACT);
      if (v2<v1) {
	u = (v1*v1-v2*v2)/(2*g*FC);
	if ((s.nex_len<0.1 ? s.nex_len : BREAK_ANGLE) < u/abs(s.nex_rad))
	  v1 = sqrt(v2*v2 + 2*((s.nex_len<0.1 ? s.nex_len : BREAK_ANGLE)*abs(s.nex_rad)*g*FC));
      }
    }
    else {
      if (s.nex_rad == 0) {
	v2 = cnr_speed(abs(s.after_rad)+width/2);
	v1 = sqrt(v2*v2 + 2*s.nex_len*g*FC);
      }
      else {
	v1 = cnr_speed(s.nex_rad);
      }
    }
  }

  if (s.cur_rad==0) {
    vc = ( s.to_end<(s.v*s.v-v1*v1)/(2*g*FC) ? 0 : 5*(s.v+10));
    u = (s.nex_rad>0 ? -1 : 1)*(width/2 - 20)+width/2;
    alpha = GAIN*(s.to_lft-u);
    if (s.v!=0)
      alpha -= DAMPING*asin(s.vn/s.v);
  }
  else {
    vc = c*V_FACTOR;
    if (s.to_end<BREAK_ANGLE){
      alpha = 0;
      if (v1>s.v)
	vc = 2*s.v;
      else
	vc = s.v/5;
    }
    if (s.to_end*abs(s.cur_rad)>350){
      vc = 2*s.v;
    }
    if (s.nex_rad == 0){
      alpha = GAIN*(s.cur_rad>0 ? (s.to_lft-15) : (s.to_lft-width+15));}
    else
      alpha = GAIN*(s.nex_rad>0 ? (s.to_lft-15) : (s.to_lft-width+15));
    alpha -= 2*DAMPING*asin(s.vn/s.v);
  }
  result.vc = vc; result.alpha = alpha;
  return result;
}