bool Stuck::seemsStuck(CarState &cs) {
    cs.getSpeedX()<STUCK_SPEED?slowSpeedTicks++:slowSpeedTicks = 0;
    if(notStuckAnymore(cs.getTrackPos(), cs.getAngle())){
        slowSpeedTicks=0;
    }
    return (slowSpeedTicks>MAX_SLOW_SPEED_TICKS?1:0);
}

float
SimpleDriver::filterABS(CarState &cs,float brake)
{
	// convert speed to m/s
	float speed = cs.getSpeedX() / 3.6;
	// when spedd lower than min speed for abs do nothing
    if (speed < absMinSpeed)
        return brake;
    
    // compute the speed of wheels in m/s
    float slip = 0.0f;
    for (int i = 0; i < 4; i++)
    {
        slip += cs.getWheelSpinVel(i) * wheelRadius[i];
    }
    // slip is the difference between actual speed of car and average speed of wheels
    slip = speed - slip/4.0f;
    // when slip too high applu ABS
    if (slip > absSlip)
    {
        brake = brake - (slip - absSlip)/absRange;
    }
    
    // check brake is not negative, otherwise set it to zero
    if (brake<0)
    	return 0;
    else
    	return brake;
}

/** The transition choose the most fitted state at the moment of the race. */
void
FSMDriver3::transition(CarState &cs) {
    DrivingState *state = current_state;

    if(gameTicks <= 10000){
        distRaced = cs.getDistRaced();
        ticks_on_inside_track = inside_track->get_ticks_in_state();
        ticks_on_stuck = stuck->get_ticks_in_state();
        ticks_on_out_of_track = out_of_track->get_ticks_in_state();
        damage = cs.getDamage();
        gameTicks += 1;
    }/*
    else if(gameTicks == 10000){
        distRaced = cs.getDistRaced();
        gameTicks+=1;
        ticks_on_inside_track = inside_track->get_ticks_in_state();
        ticks_on_stuck = stuck->get_ticks_in_state();
        ticks_on_out_of_track = out_of_track->get_ticks_in_state();
        damage = cs.getDamage();
    }*/

    setTrackType();

    if(stuck->isStuck(cs)) {
        state = stuck;
    } else {
        if (cs.getTrack(1) > 0)
            state = inside_track;
        else {
            state = out_of_track;
        }
    }

    if (current_state != state) changeTo(state);
}

int
getGear(CarState & cs) {
	int current_gear = cs.getGear();
	if(!current_gear) return 1;

	if(cs.getRpm() > 8000) ++current_gear;
	else if(current_gear > 1 && cs.getRpm() < 5000) --current_gear;
	return current_gear;
}

float ApproachingCurve::getBrake(CarState &cs) {
    float brake = 0;
    float brakeFactor = 0.02;
    float diff = cs.getSpeedX() - targetSpeed;

    //if (fabs(cs.getSpeedX()) < 2) return 1;
    if (cs.getSpeedX() < 0) return 1;
    if (diff > 0) brake = brakeFactor * diff;

    return brake;
}

int
InsideTrackA::get_gear(CarState &cs) {
    int gear = cs.getGear();
    if(gear <= 0) return start_gear;

    int rpm = cs.getRpm();

    if(shouldIncreaseGear(gear, rpm)) ++gear;
    else if(shouldDecreaseGear(gear, rpm)) --gear;

    return gear;
}

float
SimpleDriver::getSteer(CarState &cs)
{
	// steering angle is compute by correcting the actual car angle w.r.t. to track 
	// axis [cs.getAngle()] and to adjust car position w.r.t to middle of track [cs.getTrackPos()*0.5]
    float targetAngle=(cs.getAngle()-cs.getTrackPos()*0.5);
    // at high speed reduce the steering command to avoid loosing the control
    if (cs.getSpeedX() > steerSensitivityOffset)
        return targetAngle/(steerLock*(cs.getSpeedX()-steerSensitivityOffset)*wheelSensitivityCoeff);
    else
        return (targetAngle)/steerLock;

}

CarControl Stuck::drive(FSMDriver5 *fsmdriver5, CarState &cs) {
    ++elapsedTicks;
    trackInitialPos = getInitialPos(cs);
    if(notStuckAnymore(cs.getTrackPos(), cs.getAngle()) || hasBeenStuckLongEnough()){
        elapsedTicks = 0;
        slowSpeedTicks = 0;
        trackInitialPos = 0;
    }
    const float accel = 1, brake = 0, clutch = 0;
    const int gear = -1, focus = 0, meta = 0;
    float steer = getSteer(trackInitialPos, cs);

    return CarControl(accel, brake, gear, steer, clutch, focus, meta);
}

/**************************************************************************
 * Modularization*/
float 
Stuck::get_steer(CarState &cs) {
    if(abs(cs.getAngle()) > M_PI) // around 180 graus
    return (getInitialPos(cs) > 0 ? -1 : 1);

    return (getInitialPos(cs) > 0 ? 1 : -1);
}

float Stuck::getSteer(float trackInitialPos, CarState &cs){
    //return (trackInitialPos > 0 ? 1 : -1);
    if(abs(cs.getAngle()) > 1.557){// around 180 graus
        return (trackInitialPos > 0 ? -1 : 1);
    }else{
    	return (trackInitialPos > 0 ? 1 : -1);
    }
}

float
getSteering(CarState & cs) {
	// based on Loiacono's SimpleDriver

	const float
	  steerLock = 0.366519;
	float
	  targetAngle = (cs.getAngle() - cs.getTrackPos() * 0.5) / steerLock;

	// normalize steering
	if(targetAngle < -1)
		targetAngle = -1;
	else if(targetAngle > 1)
		targetAngle = 1;

	return targetAngle;
}

float
ApproachingCurve::getSteering(CarState &cs) {
    if(r_sensor == l_sensor) return 0;

    float angle = cs.getAngle();
    // If the controller is not in a pre-defined region amongst the inside limits of the track (between 0.7 and 0.9 with the current
    // set of values, normalized), than it will be adjusted to do so
    bool adjustedToCurve = ((fabs(cs.getTrackPos()) - target_pos >= 0) && (fabs(cs.getTrackPos()) - target_pos < 0.2));

    if(!adjustedToCurve) {
        if(approachingRightTurn())
            angle = max_steering - angle;
        else
            angle -= max_steering;
    }

    return angle;
}

void
ApproachingCurve::updateSensors(CarState &cs) {
    float speedFactor = 5000;                       // The target speed is obtained through a constant factor

    if (cs.getFocus(2) == -1) {                     // Focus sensors are available only once per second
        r_sensor = cs.getTrack(10);                  // Use track sensors
        c_sensor = cs.getTrack(9);
        l_sensor = cs.getTrack(8);
    }
    else {
        r_sensor = cs.getFocus(3);                   // Use focus sensors
        c_sensor = cs.getFocus(2);
        l_sensor = cs.getFocus(1);
    }
    target_speed = base_speed + speedFactor / fabs(l_sensor - r_sensor);

    sensors_are_updated = true;
}

float
InsideTrackA::get_accel(CarState &cs) {
    setTargetSpeed(cs);
    float Front, max10, max20;

    Front = cs.getTrack(10);
    max10 = max(cs.getTrack(9), cs.getTrack(11));
    max20 = max(cs.getTrack(8), cs.getTrack(12));

    float accel = (cs.getSpeedX() > target_speed ? 0 : (Front+max10+max20)/(3*200));

    if(Front >= 70) accel = 1;

    if(Front <= 20 && cs.getSpeedX() <= 30) accel = 1;  /*Resolve o caso em que o carro está preso com a frente voltada para a borda da pista*/

    //printf("%.0f, %.0f, %.0f --> accel: %.2f\n", Front, max10, max20, accel);

    return accel ;
}

void ApproachingCurve::updateSensors(CarState &cs) {
    float speedFactor = 5000;                       //The target speed is obtained through a constant factor

    if (cs.getFocus(2) == -1) {                     //Focus sensors are available only once per second
        // cout << "FOCUS MISS!" << endl;
        rSensor = cs.getTrack(10);                  //Use track sensors
        cSensor = cs.getTrack(9);
        lSensor = cs.getTrack(8);
    }
    else {
        // cout << "FOCUS HIT!" << endl;
        rSensor = cs.getFocus(3);                   //Use focus sensors
        cSensor = cs.getFocus(2);
        lSensor = cs.getFocus(1);
    }
    targetSpeed = BASE_SPEED + speedFactor / fabs(lSensor - rSensor);

    sensorsAreUpdated = true;
}

float ApproachingCurve::getSteering(CarState &cs) {
    if(rSensor == lSensor) return 0;
    
    float angle = cs.getAngle();
    //If the controller is not in a pre-defined region amongst the inside limits of the track (between 0.7 and 0.9 with the current
    //set of values, normalized), than it will be adjusted to do so
    bool adjustedToCurve = ((fabs(cs.getTrackPos()) - TARGET_POS >= 0) && (fabs(cs.getTrackPos()) - TARGET_POS < 0.2));
    //Previous conditions:																// 0.2 is an arbitrary margin
    //bool adjustedToCurve = (cs.getTrackPos() <= TARGET_POS);

    if(!adjustedToCurve) {   
        if(approachingRightTurn())
            angle = MAX_STEERING - angle;
        else
            angle -= MAX_STEERING;
    }
    
    return angle;
}

CarControl ApproachingCurve::drive(FSMDriver5 *FSMDriver5, CarState &cs) {
    if(!sensorsAreUpdated) /*@todo Só atualiza na 1a vez mesmo? */
        updateSensors(cs);

    const int focus = 0, meta = 0;
    const float clutch = 0;
    
    return CarControl(getAccel(cs), getBrake(cs), getGear(cs), cs.getAngle(), clutch, focus, meta);
    //Use the line below if the behavior of adjusting the car to the curve ahead is desired (not fully functional):
    //return CarControl(getAccel(cs), getBrake(cs), getGear(cs), getSteering(cs), clutch, focus, meta);
}

float Curve::findFarthestDirection(CarState &cs) {
    float farthestSensor = -INFINITY;
    float farthestDirection = 0;
    for (int i = 0; i < 19; i++) {
        if (farthestSensor < cs.getTrack(i)) {
            farthestSensor = cs.getTrack(i);
            farthestDirection = i;
        }
    }
    farthestDirection = -M_PI/2 + farthestDirection*M_PI/18;
    return normalizeSteer(-farthestDirection);
}

int
SimpleDriver::getGear(CarState &cs)
{

    int gear = cs.getGear();
    int rpm  = cs.getRpm();

    // if gear is 0 (N) or -1 (R) just return 1 
    if (gear<1)
        return 1;
    // check if the RPM value of car is greater than the one suggested 
    // to shift up the gear from the current one     
    if (gear <6 && rpm >= gearUp[gear-1])
        return gear + 1;
    else
    	// check if the RPM value of car is lower than the one suggested 
    	// to shift down the gear from the current one
        if (gear > 1 && rpm <= gearDown[gear-1])
            return gear - 1;
        else // otherwhise keep current gear
            return gear;
}

void
SimpleDriver::clutching(CarState &cs, float &clutch)
{
  double maxClutch = clutchMax;

  // Check if the current situation is the race start
  if (cs.getCurLapTime()<clutchDeltaTime  && stage==RACE && cs.getDistRaced()<clutchDeltaRaced)
    clutch = maxClutch;

  // Adjust the current value of the clutch
  if(clutch > 0)
  {
    double delta = clutchDelta;
    if (cs.getGear() < 2)
	{
      // Apply a stronger clutch output when the gear is one and the race is just started
	  delta /= 2;
      maxClutch *= clutchMaxModifier;
      if (cs.getCurLapTime() < clutchMaxTime)
        clutch = maxClutch;
	}

    // check clutch is not bigger than maximum values
	clutch = min(maxClutch,double(clutch));

	// if clutch is not at max value decrease it quite quickly
	if (clutch!=maxClutch)
	{
	  clutch -= delta;
	  clutch = max(0.0,double(clutch));
	}
	// if clutch is at max value decrease it very slowly
	else
		clutch -= clutchDec;
  }
}