// MISC.CPP - miscellaneous support functions - M. Timin, May, 1995
// ver. 0.6b 5/8/95 b for beta

#include <string.h>
#include "car.h"
#include "os.h"
#include "track.h"

// call repeatedly to change direction by 180 degrees:
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.
}

// 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 = 20.0;    
   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;
}

// 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 = 3.2;  // 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;
}

// This routine is so that the KB is polled much less frequently when
// we are trying to speed up the simulation, as indicated by the no_display
// and real_speed global variables.
int do_kb(void)      // Returns non_zero when kb should be polled.
{
   static int kb_count = 0;

   ++kb_count;
   if(no_display) 
      if(kb_count >= 50) {
         kb_count = 0;
         return 1;
      }
      else
         return 0;
   else if(!real_speed)
      if(kb_count >= 10) {
         kb_count = 0;
         return 1;
      }
      else
         return 0;
   // This is when there is a display and the speed is realistic
   kb_count = 0;
   return 1;
}