/***************************************************************************

    file                 : driver.cpp
    created              : Sun Jul 28 11:04:45 CET 2003
    copyright            : (C) 2003 Paul Bain

 ***************************************************************************/

/***************************************************************************
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 ***************************************************************************/

#include "driver.h"

#define FUEL_CONSUMPTION "Fuel Consumption"
#define FUEL_PER_LAP "fuelperlap"
#define TRACTION_CONTROL "Traction Control"
#define TCL_SLIP_OPTION "Traction Control Slip"
#define ANTILOCK_BRAKES "Anti-Lock Braking"
#define ABS_SLIP_OPTION "ABS Slip"

const float Driver::MAX_UNSTUCK_ANGLE = 30.0/180.0*PI;  /* [radians] */
const float Driver::UNSTUCK_TIME_LIMIT = 2.0;           /* [s] */
const float Driver::G = 9.81;                           /* [m/(s*s)] */
const float Driver::FULL_ACCEL_MARGIN = 1.0;            /* [m/s] */
const float Driver::BRAKE_INCREMENT = 0.1;              /* [-] */
const float Driver::ACCEL_INCREMENT = 0.1;              /* [-] */
const float Driver::SHIFT = 0.95;                       /* [-] (% of rpmredline) */
const float Driver::SHIFT_MARGIN = 4.0;                 /* [m/s] */
const float Driver::ABS_MINSPEED = 0.1;                 /* [m/s] */
const float Driver::TCL_MINSPEED = 0.1;                 /* [m/s] */
const float Driver::LOOKAHEAD_CONST = 17.0;             /* [m] */
const float Driver::LOOKAHEAD_FACTOR = 0.33;            /* [-] */
const float Driver::MAX_UNSTUCK_SPEED = 5.0;            /* [m/s] */
const float Driver::MIN_UNSTUCK_DIST = 3.0;             /* [m] */
const float Driver::SIDECOLL_MARGIN = 1.0;              /* [m] */
const float Driver::BORDER_OVERTAKE_MARGIN = 0.5;       /* [m] */
const float Driver::OVERTAKE_OFFSET_INC = 0.1;          /* [m/timestep] */
const float Driver::WIDTHDIV = 3.0;                     /* [-] */
const float Driver::PIT_LOOKAHEAD = 6.0;                /* [m] */
const float Driver::PIT_BRAKE_AHEAD = 200.0;            /* [m] */
const float Driver::PIT_MU = 0.4;                       /* [-] */
const float Driver::MIN_SIDE_CLEARANCE = 0.0;           /* [m] */

Driver::Driver(int index)
{
    INDEX = index;
}

Driver::~Driver()
{
  /*float average = 0;
  for(int i = 1; i <= lapno; i++)
    {
    average += fuelused[i];
    }
  average = average / (lapno - 1);
  printf("Average fuel used: %f\n", average);*/
  delete opponents;
  delete pit;
}

/* Called for every track change or new race. */
void Driver::initTrack(tTrack* t, void *carHandle, void **carParmHandle, tSituation *s)
{
    track = t;

    char buffer[256];
    /* get a pointer to the first char of the track filename */
    char* trackname = strrchr(track->filename, '/') + 1;

    switch (s->_raceType)
      {
      case RM_TYPE_PRACTICE:
	sprintf(buffer, "drivers/astigot/%d/practice/%s", INDEX, trackname);
	break;
      case RM_TYPE_QUALIF:
	sprintf(buffer, "drivers/astigot/%d/qualifying/%s", INDEX, trackname);
	break;
      case RM_TYPE_RACE:
	sprintf(buffer, "drivers/astigot/%d/race/%s", INDEX, trackname);       
	break;
      default:
	break;
      }

    *carParmHandle = GfParmReadFile(buffer, GFPARM_RMODE_STD);
    if (*carParmHandle == NULL)
      {
      sprintf(buffer, "drivers/astigot/%d/default.xml", INDEX);
      *carParmHandle = GfParmReadFile(buffer, GFPARM_RMODE_STD);
      }
    else
      {
      /* Calculate how much fuel is needed for the race */
      fuelperlap = GfParmGetNum(*carParmHandle, FUEL_CONSUMPTION, FUEL_PER_LAP, (char*)NULL, 100.0);
      float fuel = fuelperlap * (s->_totLaps + 1.0);
      
      /* If we HAVE to stop at least once in the race divide the race into equal stints */
      float i = 2.0;
      float startfuel = fuel;
      while(startfuel > 100.0)
	{
	startfuel = fuel / i;
	i++;
	}
      GfParmSetNum(*carParmHandle, SECT_CAR, PRM_FUEL, (char*)NULL, MIN(startfuel, 100.0));
      stops = i-1;
      
      /* Get allowed traction control slip */
      TCL_SLIP_VALUE = GfParmGetNum(*carParmHandle, TRACTION_CONTROL, TCL_SLIP_OPTION, (char*)NULL, 0.9);
      
      /* Get allowed ABS slip */
      ABS_SLIP_VALUE = GfParmGetNum(*carParmHandle, ANTILOCK_BRAKES, ABS_SLIP_OPTION, (char*)NULL, 0.9);
      }
}

/* Start a new race. */
void Driver::newRace(tCarElt* car, tSituation *s)
{
    MAX_UNSTUCK_COUNT = int(UNSTUCK_TIME_LIMIT/s->deltaTime);
    stuck = 0;
    myoffset = 0.0;
    brake = 0.0;
    accel = 1.0;   // Full acceleration off start line
    this->car = car;
    CARMASS = GfParmGetNum(car->_carHandle, SECT_CAR, PRM_MASS, NULL, 1000.0);
    initCa();
    initCw();
    initTCLfilter();

    /* initialize the list of opponents */
    opponents = new Opponents(s, this);
    opponent = opponents->getOpponentPtr();

    /* create the pit object */
    pit = new Pit(s, this);

    /*lapno = car->_laps;
      fuellevel = car->_fuel;*/
}

/* Drive during race. */
void Driver::drive(tSituation *s)
{
    memset(&car->ctrl, 0, sizeof(tCarCtrl));

    update(s);

    /*if(car->_laps > lapno)
      {
      printf("Lap %i used %f fuel\n", lapno, fuellevel - car->_fuel);
      fuelused[lapno] = fuellevel - car->_fuel;
      lapno = car->_laps;
      fuellevel = car->_fuel;
      }*/

    if (isStuck())
      {
      car->ctrl.steer = -angle / car->_steerLock;
      car->ctrl.gear = -1; // reverse gear
      car->ctrl.accelCmd = 0.5; // 50% accelerator pedal
      car->ctrl.brakeCmd = 0.0; // no brakes
      accel = 0.0;
      brake = 0.0;
      }
    else
      {
      car->ctrl.steer = SideCollisionFilter(getSteer());
      car->ctrl.gear = getGear();
      brake += filterABS(FrontCollisionFilter(filterBPit(getBrake())));
      if(brake > 1.0)
	brake = 1.0;
      else if(brake < 0.0)
	brake = 0.0;	
	
      if (brake == 0.0)
	{
	car->ctrl.accelCmd = filterTCL(filterTrk(getAccel()));
        }
      else
	{
	car->ctrl.accelCmd = 0.0;
	accel = 0.0;
	car->ctrl.brakeCmd = brake;
        }
      }
}

/* Set pitstop commands. */
int Driver::pitCommand(tSituation *s)
{
    car->_pitRepair = pit->getRepair();
    car->_pitFuel = pit->getFuel();
    pit->setPitstop(false);
    return ROB_PIT_IM; /* return immediately */
}

/* End of the current race */
void Driver::endRace(tSituation *s)
{
}

/* Update my private data every timestep */
void Driver::update(tSituation *s)
{
    trackangle = RtTrackSideTgAngleL(&(car->_trkPos));
    angle = trackangle - car->_yaw;
    NORM_PI_PI(angle);
    mass = CARMASS + car->_fuel;
    speed = Opponent::getSpeed(car);
    opponents->update(s, this);
    currentspeedsqr = car->_speed_x*car->_speed_x;
    pit->update();
}

/* Check if I'm stuck */
bool Driver::isStuck()
{
    if (fabs(angle) > MAX_UNSTUCK_ANGLE &&
	car->_speed_x < MAX_UNSTUCK_SPEED &&
        fabs(car->_trkPos.toMiddle) > MIN_UNSTUCK_DIST)
      {
      if (stuck > MAX_UNSTUCK_COUNT && car->_trkPos.toMiddle*angle < 0.0)
	{
	  return true;
        }
      else
	{
	  stuck++;
	  return false;
        }
      }
    else
      {
      stuck = 0;
      return false;
      }
}

/* Compute the allowed speed on a segment */
float Driver::getAllowedSpeed(tTrackSeg *segment)
{
    if (segment->type == TR_STR)
      {
      return FLT_MAX;
      }
    else
      {
      float arc = 0.0;
      tTrackSeg *s = segment;
        
      while (s->type == segment->type && arc < PI/2.0)
	{
	arc += s->arc;
	s = s->next;
        }
      arc /= PI/2.0;
      float mu = segment->surface->kFriction;
      float r = (segment->radius + segment->width/2.0)/sqrt(arc);
      return sqrt((mu*G*r)/(1.0 - MIN(1.0, r*CA*mu/mass)));
      }
}



/* Compute the length to the end of the segment */
float Driver::getDistToSegEnd()
{
    if (car->_trkPos.seg->type == TR_STR)
      {
      return car->_trkPos.seg->length - car->_trkPos.toStart;
      }
    else
      {
      return (car->_trkPos.seg->arc - car->_trkPos.toStart)*car->_trkPos.seg->radius;
      }
}

/* Compute fitting acceleration */
float Driver::getAccel()
{
    float allowedspeed = getAllowedSpeed(car->_trkPos.seg);
    float gr = car->_gearRatio[car->_gear + car->_gearOffset];
    float rm = car->_enginerpmRedLine;
    if (allowedspeed > car->_speed_x + FULL_ACCEL_MARGIN)
      {
      return 1.0;
      }
    else
      {
      return allowedspeed/car->_wheelRadius(REAR_RGT)*gr /rm;
      }
}

/* Calculate required braking distance */
float Driver::critDistance(float higherspeed, float lowerspeed, float friction)
{
    float c = friction*G;
    float d = (CA*friction + CW)/mass;
    float higherspeedsqr = higherspeed*higherspeed;
    float lowerspeedsqr = lowerspeed*lowerspeed;
    float v1sqr = higherspeedsqr;
    float v2sqr = lowerspeedsqr;
    float brakedist = -log((c + v2sqr*d)/(c + v1sqr*d))/(2.0*d);

    return brakedist;
}

/* Brake command */
float Driver::getBrake()
{
    tTrackSeg *segptr = car->_trkPos.seg;
    float currentspeedsqr = car->_speed_x*car->_speed_x;
    float mu = segptr->surface->kFriction;
    float maxlookaheaddist = currentspeedsqr/(2.0*mu*G);
    float lookaheaddist = getDistToSegEnd();
    float allowedspeed = getAllowedSpeed(segptr);

    if (allowedspeed < car->_speed_x)
      return BRAKE_INCREMENT;

    segptr = segptr->next;
    while (lookaheaddist < maxlookaheaddist)
      {
      allowedspeed = getAllowedSpeed(segptr);
      if (allowedspeed < car->_speed_x)
	{
	float brakedist = critDistance(car->_speed_x, allowedspeed, mu);
	if (brakedist > lookaheaddist)
	  {
          return BRAKE_INCREMENT;
	  }
        }
      lookaheaddist += segptr->length;
      segptr = segptr->next;
      }
    return -1.0; // So brake can go below 0, lets drive() know to turn off braking
}

/* Compute gear */
int Driver::getGear()
{
    if (car->_gear <= 0)
      return 1;

    float gr_up = car->_gearRatio[car->_gear + car->_gearOffset];
    float omega = car->_enginerpmRedLine/gr_up;
    float wr = car->_wheelRadius(2);

    if (omega*wr*SHIFT < car->_speed_x)
      {
      return car->_gear + 1;
      }
    else
      {
      float gr_down = car->_gearRatio[car->_gear + car->_gearOffset - 1];
      omega = car->_enginerpmRedLine/gr_down;
      if (car->_gear > 1 && omega*wr*SHIFT > car->_speed_x + SHIFT_MARGIN)
	{
	return car->_gear - 1;
	}
      }
    return car->_gear;
}

/* Compute aerodynamic downforce coefficient CA */
void Driver::initCa()
{
    char *WheelSect[4] = {SECT_FRNTRGTWHEEL, SECT_FRNTLFTWHEEL, SECT_REARRGTWHEEL, SECT_REARLFTWHEEL};
    float rearwingarea = GfParmGetNum(car->_carHandle, SECT_REARWING, PRM_WINGAREA, (char*) NULL, 0.0);
    float rearwingangle = GfParmGetNum(car->_carHandle, SECT_REARWING, PRM_WINGANGLE, (char*) NULL, 0.0);
    float wingca = 1.23*rearwingarea*sin(rearwingangle);
    float cl = GfParmGetNum(car->_carHandle, SECT_AERODYNAMICS, PRM_FCL, (char*) NULL, 0.0) + GfParmGetNum(car->_carHandle, SECT_AERODYNAMICS, PRM_RCL, (char*) NULL, 0.0);
    float h = 0.0;
    int i;
    for (i = 0; i < 4; i++)
        h += GfParmGetNum(car->_carHandle, WheelSect[i], PRM_RIDEHEIGHT, (char*) NULL, 0.20);
    h*= 1.5; h = h*h; h = h*h; h = 2.0 * exp(-3.0*h);
    CA = h*cl + 4.0*wingca;
}

/* Compute aerodynamic drag coefficient CW */
void Driver::initCw()
{
    float cx = GfParmGetNum(car->_carHandle, SECT_AERODYNAMICS, PRM_CX, (char*) NULL, 0.0);
    float frontarea = GfParmGetNum(car->_carHandle, SECT_AERODYNAMICS, PRM_FRNTAREA, (char*) NULL, 0.0);
    CW = 0.645*cx*frontarea;
}

/* Antilocking filter for brakes */
float Driver::filterABS(float brake)
{
    if (car->_speed_x < ABS_MINSPEED)
      return brake;

    int i;
    float slip[4] = { 0.0, 0.0, 0.0, 0.0 };
    for (i = 0; i < 4; i++)
      {
      slip[i] = car->_wheelSpinVel(i) * car->_wheelRadius(i) / car->_speed_x;
      }
    if(((slip[0] + slip[1]) / 2.0) < ABS_SLIP_VALUE || ((slip[2] + slip[3]) / 2.0) < ABS_SLIP_VALUE)
      brake = -0.1;
    return brake;
}

/* TCL filter for accelerator pedal */
float Driver::filterTCL(float accel)
{
    if (car->_speed_x < TCL_MINSPEED) return accel;
    float slip = car->_speed_x/(this->*GET_DRIVEN_WHEEL_SPEED)();
    if (slip < TCL_SLIP_VALUE)
      {
      return 0.0;
      }
    return accel;
}

/* Traction Control (TCL) setup */
void Driver::initTCLfilter()
{
    char *traintype = GfParmGetStr(car->_carHandle, SECT_DRIVETRAIN, PRM_TYPE, VAL_TRANS_RWD);
    if (strcmp(traintype, VAL_TRANS_RWD) == 0)
      {
      GET_DRIVEN_WHEEL_SPEED = &Driver::filterTCL_RWD;
      }
    else if (strcmp(traintype, VAL_TRANS_FWD) == 0)
      {
      GET_DRIVEN_WHEEL_SPEED = &Driver::filterTCL_FWD;
      }
    else if (strcmp(traintype, VAL_TRANS_4WD) == 0)
      {
      GET_DRIVEN_WHEEL_SPEED = &Driver::filterTCL_4WD;
      }
}

/* TCL filter plugin for rear wheel driven cars */
float Driver::filterTCL_RWD()
{
    return (car->_wheelSpinVel(REAR_RGT) + car->_wheelSpinVel(REAR_LFT)) * car->_wheelRadius(REAR_LFT) / 2.0;
}


/* TCL filter plugin for front wheel driven cars */
float Driver::filterTCL_FWD()
{
    return (car->_wheelSpinVel(FRNT_RGT) + car->_wheelSpinVel(FRNT_LFT)) * car->_wheelRadius(FRNT_LFT) / 2.0;
}


/* TCL filter plugin for all wheel driven cars */
float Driver::filterTCL_4WD()
{
    return (car->_wheelSpinVel(FRNT_RGT) + car->_wheelSpinVel(FRNT_LFT)) * car->_wheelRadius(FRNT_LFT) / 4.0 +
           (car->_wheelSpinVel(REAR_RGT) + car->_wheelSpinVel(REAR_LFT)) * car->_wheelRadius(REAR_LFT) / 4.0;
}

/* Work out where I want to go */
v2d Driver::getTargetPoint()
{
    tTrackSeg *seg = car->_trkPos.seg;
    float lookahead = LOOKAHEAD_CONST + car->_speed_x*LOOKAHEAD_FACTOR;
    float length = getDistToSegEnd();
    float offset = getOvertakeOffset();  

    if (pit->getInPit()) {
        if (currentspeedsqr > pit->getSpeedlimitSqr()) {
            lookahead = PIT_LOOKAHEAD + car->_speed_x*LOOKAHEAD_FACTOR;
        } else {
            lookahead = PIT_LOOKAHEAD;
        }
    }

    while (length < lookahead)
      {
      seg = seg->next;
      length += seg->length;
      }

    length = lookahead - length + seg->length;
    float fromstart = seg->lgfromstart;
    fromstart += length;
    offset = pit->getPitOffset(offset, fromstart);
    v2d s;
    s.x = (seg->vertex[TR_SL].x + seg->vertex[TR_SR].x)/2.0;
    s.y = (seg->vertex[TR_SL].y + seg->vertex[TR_SR].y)/2.0;

    if(seg->type == TR_STR)
      {
      v2d d, n;
      n.x = (seg->vertex[TR_EL].x - seg->vertex[TR_ER].x)/seg->length;
      n.y = (seg->vertex[TR_EL].y - seg->vertex[TR_ER].y)/seg->length;
      n.normalize();
      d.x = (seg->vertex[TR_EL].x - seg->vertex[TR_SL].x)/seg->length;
      d.y = (seg->vertex[TR_EL].y - seg->vertex[TR_SL].y)/seg->length;
      return s + d*length + offset*n;
      }
    else
      {
      v2d c, n;
      c.x = seg->center.x;
      c.y = seg->center.y;
      float arc = length/seg->radius;
      float arcsign = (seg->type == TR_RGT) ? -1.0 : 1.0;
      arc = arc*arcsign;
      s = s.rotate(c, arc);
      n = c - s;
      n.normalize();
      return s + arcsign*offset*n;
      }
}

/* Steer the car towards where I want to go */
float Driver::getSteer()
{
    float targetAngle;
    v2d target = getTargetPoint();

    targetAngle = atan2(target.y - car->_pos_Y, target.x - car->_pos_X);
    targetAngle -= car->_yaw;
    NORM_PI_PI(targetAngle);
    return targetAngle / car->_steerLock;
}

/* Hold car on the track */
float Driver::filterTrk(float accel)
{
    tTrackSeg* seg = car->_trkPos.seg;

    if (car->_speed_x < MAX_UNSTUCK_SPEED || pit->getInPit())
      return accel;
    if (seg->type == TR_STR)
      {
      float tm = fabs(car->_trkPos.toMiddle);
      float w = seg->width/WIDTHDIV;
      if (tm > w)
	return 0.0;
      else
	return accel;
      }
    else
      {
      float sign = (seg->type == TR_RGT) ? -1 : 1;
      if (car->_trkPos.toMiddle*sign > 0.0)
	{
	return accel;
        }
      else
	{
        float tm = fabs(car->_trkPos.toMiddle);
	float w = seg->width/WIDTHDIV;
	if (tm > w)
	  return 0.0;
	else
	  return accel;
        }
      }
}

/* Apply brakes to avoid hitting people in front of me */
float Driver::FrontCollisionFilter(float brake)
{
    int i;

    for (i = 0; i < opponents->getNOpponents(); i++)
      {
      if (opponent[i].getState() & OPP_COLL)
	{
	float brakedist = critDistance(GetSpeedOnTrack(), opponent[i].getSpeed(), car->_trkPos.seg->surface->kFriction);

	if (brakedist >= opponent[i].getDistance())
	  {
	  if(brake < 0.0)
	    brake = 0.0;
          return brake + 0.1;
          }
        }
      }
    return brake;
}

/* Filter to stop me driving into the side of other cars */
float Driver::SideCollisionFilter(float steer)
{
    int i;
    float sidedist = 0.0, fsidedist = 0.0, minsidedist = FLT_MAX;
    Opponent *o = NULL;

    /* get the index of the nearest car (o) */
    for (i = 0; i < opponents->getNOpponents(); i++)
      {
      if (opponent[i].getState() & OPP_SIDE)
	{
        sidedist = opponent[i].getSideDist();
	fsidedist = fabs(sidedist);
	if (fsidedist < minsidedist)
	  {
	  minsidedist = fsidedist;
	  o = &opponent[i];
	  }
        }
      }
    /* if there is another car handle the situation */
    if (o != NULL)
      {
      float d = fsidedist - o->getWidth();
      /* near enough */
      if (d < SIDECOLL_MARGIN)
	{
        /* compute angle between cars */
	tCarElt *ocar = o->getCarPtr();
	float diffangle = ocar->_yaw - car->_yaw;
	NORM_PI_PI(diffangle);
	const float c = SIDECOLL_MARGIN/2.0;
	d = d - c;
	if (d < 0.0) d = 0.0;
	float psteer = diffangle/car->_steerLock;
	psteer = steer*(d/c) + 2.0*psteer*(1.0-d/c);
	
	if(car->_trkPos.toMiddle - ocar->_trkPos.toMiddle > 0)
	  {
	  /* Check if I'm about to drive off the road */
	  if(car->_trkPos.toLeft - (car->_dimension_x / 2.0) <= MIN_SIDE_CLEARANCE)
	    {
	    float angle = RtTrackSideTgAngleL(&(car->_trkPos)) - car->_yaw;
	    NORM_PI_PI(angle);
	    angle -= car->_trkPos.toLeft/car->_trkPos.seg->width;
	    return angle;  // Follow curve of track
	    }
	  /* Other car is to the right of me */
	  else
	    {
	    if(steer < psteer)
	      {
	      return psteer;
	      }
	    else
	      {
	      return steer;
	      }
	    }
	  }
	else if(car->_trkPos.toMiddle - ocar->_trkPos.toMiddle < 0)
	  {
	  if(car->_trkPos.toRight - (car->_dimension_x / 2.0) <= MIN_SIDE_CLEARANCE)
	    {
	    float angle = RtTrackSideTgAngleL(&(car->_trkPos)) - car->_yaw;
	    NORM_PI_PI(angle);
	    angle -= car->_trkPos.toRight/car->_trkPos.seg->width;
	    return angle;  // Follow curve of track
	    }
	  else
	    {
	    /* Other car is to the left of me */
	    if(steer > psteer)
	      {
	      return psteer;
	      }
	    else
	      {
	      return steer;
	      }
	    }
	  }
	}
      }
    return steer;
}

/* Work out speed in direction of track */
float Driver::GetSpeedOnTrack()
{
    v2d speed, dir;
    float trackangle = RtTrackSideTgAngleL(&(car->_trkPos));

    speed.x = car->_speed_X;
    speed.y = car->_speed_Y;
    dir.x = cos(trackangle);
    dir.y = sin(trackangle);
    return speed*dir;
}

/* Compute an offset to the target point */
float Driver::getOvertakeOffset()
{
    int i;
    float catchdist, mincatchdist = FLT_MAX;
    Opponent *o = NULL;

    for (i = 0; i < opponents->getNOpponents(); i++)
      {
      if (opponent[i].getState() & OPP_FRONT)
	{
	catchdist = opponent[i].getCatchDist();
	if (catchdist < mincatchdist)
	  {
	  mincatchdist = catchdist;
	  o = &opponent[i];
	  }
        }
      }
    if (o != NULL)
      {
      float w = o->getCarPtr()->_trkPos.seg->width/WIDTHDIV-BORDER_OVERTAKE_MARGIN;
      float otm = o->getCarPtr()->_trkPos.toMiddle;
      if (otm > 0.0 && myoffset > -w)
	myoffset -= OVERTAKE_OFFSET_INC;
      else if (otm < 0.0 && myoffset < w)
	myoffset += OVERTAKE_OFFSET_INC;
      }
    else
      {
      if (myoffset > OVERTAKE_OFFSET_INC)
	myoffset -= OVERTAKE_OFFSET_INC;
      else if (myoffset < -OVERTAKE_OFFSET_INC)
	myoffset += OVERTAKE_OFFSET_INC;
      else myoffset = 0.0;
      }

      /* Check if I'm about to drive off the road */
      if(car->_trkPos.toLeft - (car->_dimension_x / 2.0) <= MIN_SIDE_CLEARANCE)
	myoffset = 0.0;
      else if(car->_trkPos.toRight - (car->_dimension_x / 2.0) <= MIN_SIDE_CLEARANCE)
	myoffset = 0.0;

      return myoffset;
}

float Driver::filterBPit(float brake)
{
    if (pit->getPitstop() && !pit->getInPit())
      {
      float dl, dw;
      RtDistToPit(car, track, &dl, &dw);
      if (dl < PIT_BRAKE_AHEAD)
	{
        float mu = car->_trkPos.seg->surface->kFriction*PIT_MU;
	if (critDistance(GetSpeedOnTrack(), 0.0, mu) > dl)
	  {
	  if(brake < 0.0)
	    brake = 0.0;
	  return brake + 0.1;
	  }
        }
      }
    if (pit->getInPit())
      {
      float s = pit->toSplineCoord(car->_distFromStartLine);
      /* pit entry */
      if (pit->getPitstop())
	{
	float mu = car->_trkPos.seg->surface->kFriction*PIT_MU;
	if (s < pit->getNPitStart())
	  {
	  /* brake to pit speed limit */
	  float dist = pit->getNPitStart() - s;
          if (critDistance(GetSpeedOnTrack(), pit->getSpeedlimit(), mu) > dist)
	    {
	    if(brake < 0.0)
	      brake = 0.0;
	    return brake + 0.1;
	    }
	  }
	else
	  {
	  /* hold speed limit */
	    if (currentspeedsqr > pit->getSpeedlimitSqr())
	      {
	      if(brake < 0.0)
		brake = 0.0;
	      return brake + 0.1;
	      }
	  }
	/* brake into pit (speed limit 0.0 to stop ) */
	float dist = pit->getNPitLoc() - s;
	if (critDistance(GetSpeedOnTrack(), 0.0, mu) >= dist)
	  {
	  if(brake < 0.0)
	    brake = 0.0;
	  return brake + 0.1;
	  }
	/* hold in the pit */
	if (s > pit->getNPitLoc())
	  {
	  if(brake < 0.0)
	    brake = 0.0;
	  return brake + 0.1;
	  }
        }
      else
	{
	/* pit exit */
	if (s < pit->getNPitEnd())
	  {
	  /* pit speed limit */
	  if (currentspeedsqr > pit->getSpeedlimitSqr())
	    {
	    if(brake < 0.0)
	      brake = 0.0; 
	    return brake + 0.1;
	    }
	  }
        }
      }
    return brake;
}
