#include "ai.h"
#include "vamosworld.h"

#define GRAVITY 9.8

//used to calculate brake value
#define MAX_SPEED_DIFF 6.0
#define MIN_SPEED_DIFF 1.0

//used to find the point the car should steer towards to
#define LOOKAHEAD_FACTOR1 2.25
#define LOOKAHEAD_FACTOR2 0.33
//used to detect very sharp corners like Circuit de Pau
#define LOOKAHEAD_MIN_RADIUS 8.0

//used to calculate friction coefficient
#define FRICTION_FACTOR_LONG 0.68
#define FRICTION_FACTOR_LAT 0.62

void AI::update(float timefactor, float fps, TRACK* track_p)
{
        std::vector<AI_Car>::iterator it;

        for( it = AI_Cars.begin (); it != AI_Cars.end (); it++ )
        {
		updateGasBrake(&(*it), timefactor, fps, track_p);
		updateGear(&(*it), timefactor, fps);
		updateClutch(&(*it), timefactor, fps);
		updateSteer(&(*it), timefactor, fps);
        }
}


void AI::updateGasBrake(AI_Car *c, float timefactor, float fps, TRACK* track_p)
{
	float brake_value = 0.0;
	float gas_value = 0.5;

	if (c->car->engine()->rotational_speed() < 1.0) c->car->engine()->start();

	if( state.GetGameState() == STATE_STAGING )
	{
		//have to calculate friction coefficient after the simulation start,
		//since it's dependent on the normal force (car weight)
		calcMu(c, track_p);
		c->car->brake(1.0,0.0);
		return;
	}

        BEZIER *curr_patch = c->car->GetColPatch(0);
	//if car is not on track, just let it roll
        if (!curr_patch)
	{
		curr_patch = c->car->GetColPatch(1); //let's try the other wheel
		if (!curr_patch)
		{
			c->car->brake(0.0,0.0);
			c->car->gas(0.8,0.0);
			return;
		}
	}

	//maximum lookahead distance
	VERTEX patch_direction = ((curr_patch->points[0][0] + curr_patch->points[0][3]) -
				 (curr_patch->points[3][0] + curr_patch->points[3][3])).ScaleR(0.5);
	Vamos_Geometry::Three_Vector direction_vector;
	direction_vector[0] = patch_direction.x;
	direction_vector[2] = patch_direction.y;
	direction_vector[1] = -patch_direction.z;
	
	//this version uses the velocity along tangent vector. it should calculate a lower current speed,
	//hence higher gas value or lower brake value
	float currentspeed = c->car->chassis().cm_velocity().component(direction_vector);
	//this version just uses the velocity, do not care about the direction
	//float currentspeed = c->car->chassis().cm_velocity().magnitude();

	float maxlookahead = calcBrakeDist(c, currentspeed, 0.0, c->longitude_mu);

	//check speed against speed limit of current patch
	float speed_limit = calcSpeedLimit(c, curr_patch, c->lateral_mu);
	float speed_diff = speed_limit - currentspeed;

	if (speed_diff < 0.0)
	{
		if (-speed_diff < MIN_SPEED_DIFF) //no need to brake is diff is small
		{
			brake_value = 0.0;
		}
		else
		{
			brake_value = -speed_diff / MAX_SPEED_DIFF;
			if (brake_value > 1.0) brake_value = 1.0;
		}
		gas_value = 0.0;
	}
	else if (isnan(speed_diff) || speed_diff > MAX_SPEED_DIFF)
	{
		gas_value = 1.0;
		brake_value = 0.0;
	}
	else
	{
		gas_value = speed_diff / MAX_SPEED_DIFF;
		brake_value = 0.0;
	}

	//check upto maxlookahead distance
	float dist_checked = 0.0;
	float brake_dist = 0.0;
	BEZIER *patch_to_check = curr_patch;

	while (dist_checked < maxlookahead)
	{
		patch_to_check = patch_to_check->next_patch;
		//if there is no next patch(probably a non-closed track, just let it roll
		if (!patch_to_check)
		{
			brake_value = 0.0;
			dist_checked = maxlookahead;
			break;
		}
		speed_limit = calcSpeedLimit(c, patch_to_check, c->lateral_mu);
		VERTEX center1 = (patch_to_check->points[0][0] + patch_to_check->points[0][3]);
		VERTEX center2 = (patch_to_check->points[3][0] + patch_to_check->points[3][3]);
		dist_checked += (center1 - center2).len() / 2.0;
		brake_dist = calcBrakeDist(c, currentspeed, speed_limit, c->longitude_mu);
		if (brake_dist > dist_checked)
		{
			brake_value = 1.0;
			gas_value = 0.0;
		}
	}

	if (c->car->gear() == 0) gas_value = 0.2;
	c->car->brake(brake_value,0.0);
	c->car->gas(gas_value,0.0);
	
}

void AI::calcMu(AI_Car *c, TRACK* track_p)
{
	int i;
	float long_friction = 0.0;
	float lat_friction = 0.0;

	float friction1, friction2;
	track_p->GetFrictionParams(friction1,friction2);

	for (i=0; i<4; i++)
	{
		Vamos_Body::Wheel* wheel = c->car->wheel(i);
		double sp = wheel->GetTire()->GetSigmaHat(); //ideal slip ratio given tire loading 
		double maxfx = wheel->GetTire()->GetFx(sp); //maximum tire friction given ideal slip ratio
		maxfx *= (1.0 - wheel->GetTire()->tread()) * friction1 + wheel->GetTire()->tread() * friction2;
		long_friction += maxfx;
		double sa = wheel->GetTire()->GetAlphaHat(); //ideal slip angle given tire loading 
		double maxfy = wheel->GetTire()->GetFy(sa); //maximum tire friction given ideal slip angle
		maxfy *= (1.0 - wheel->GetTire()->tread()) * friction1 + wheel->GetTire()->tread() * friction2;
		lat_friction += maxfy;
	}

	float long_mu = FRICTION_FACTOR_LONG * long_friction / c->car->chassis().mass() / GRAVITY;
	float lat_mu = FRICTION_FACTOR_LAT * lat_friction / c->car->chassis().mass() / GRAVITY;
	if (!isnan(long_mu)) c->longitude_mu = long_mu;
	if (!isnan(lat_mu)) c->lateral_mu = lat_mu;
}

float AI::calcSpeedLimit(AI_Car *c, BEZIER* patch, float friction)
{
	//adjust the radius at corner exit to allow a higher speed.
	//this will get the car to accellerate out of corner
	VERTEX center1 = (patch->points[0][0] + patch->points[3][0]);
	VERTEX center2 = (patch->points[0][3] + patch->points[3][3]);
	float track_width = (center1 - center2).len() / 2.0;
	float adjusted_radius = patch->track_radius;
	BEZIER* next_patch = patch->next_patch;
	
	if (next_patch && next_patch->track_radius > patch->track_radius &&
	    patch->track_radius > LOOKAHEAD_MIN_RADIUS)
	{
		adjusted_radius = patch->track_radius + track_width; 
	}
	
	//no downforce
	//float v1 = sqrt(friction * GRAVITY * adjusted_radius);

	//take into account downforce
	float v2 = sqrt((friction * GRAVITY * adjusted_radius) / 
		(1.0 - min(1.0, adjusted_radius * -(c->car->CA) * friction / c->car->chassis().mass())));
	
	return v2;
}

float AI::calcBrakeDist(AI_Car *ai_car, float current_speed, float allowed_speed, float friction)
{
	float c = friction * GRAVITY;
	float d = (-(ai_car->car->CA) * friction + ai_car->car->CW) / ai_car->car->chassis().mass();
	float v1sqr = current_speed * current_speed;
	float v2sqr = allowed_speed * allowed_speed;
	return -log((c + v2sqr*d)/(c + v1sqr*d))/(2.0*d);
}

void AI::updateGear(AI_Car *c, float timefactor, float fps)
{
	if ((c->car->ShiftPending()) && (c->car->gear() == 0))
        {
        	c->car->disengage_clutch (0.2);
                c->car->shift(1);
                c->shift_time = AUTO_CLUTCH_ENGAGE_TIME;
        }

	int current_gear = c->car->gear();
	// only autoshift when no shift pending and we are not in neutral or reverse gear
	if( !c->car->ShiftPending() && ( current_gear > 0 ) )
	{
		double peak_engine_speed = c->car->engine()->peak_engine_speed();
		// shift up when engine speed exceeds peak speed
		if( ( c->car->engine()->rotational_speed() > peak_engine_speed ) &&
		    ( c->car->engine()->last_rotational_speed() <= peak_engine_speed ) &&
		    ( current_gear < c->car->transmission()->forward_gears() ) )
		{
        		c->car->disengage_clutch (0.2);
	                c->car->shift_up();
        	        c->shift_time = AUTO_CLUTCH_ENGAGE_TIME;
		}

		// shift down when engine speed below shift_down_point
		// however, we do not auto shift down from 1st gear to neutral
		if( ( current_gear > 1 ) && 
		    c->car->engine()->rotational_speed() < 
		    DownshiftPoint( c, current_gear ) )
		{
        		c->car->disengage_clutch (0.2);
	                c->car->shift_down();
        	        c->shift_time = AUTO_CLUTCH_ENGAGE_TIME;
		}
	}
}

float AI::DownshiftPoint(AI_Car *c, int gear)
{
        float shift_down_point = 200.0;
        if( gear > 2 )
        {
                double current_gear_ratio = c->car->transmission()->gear_ratio( gear );
                double lower_gear_ratio = c->car->transmission()->gear_ratio( gear - 1 );
                float peak_engine_speed = c->car->engine()->peak_engine_speed();
                shift_down_point = peak_engine_speed / lower_gear_ratio * current_gear_ratio;
                shift_down_point -= ( 200.0 / gear );
        }
        return shift_down_point;
}

void AI::updateClutch(AI_Car *c, float timefactor, float fps)
{
	float autoclutch = 0.0;
        if (c->car->engine()->rotational_speed() < AUTO_CLUTCH_THRESH)
        {
        	float rotspeed = c->car->engine()->rotational_speed();
        	float stall = c->car->engine()->stall_speed() + AUTO_CLUTCH_MARGIN;
        	float clutch = (rotspeed-stall) / (AUTO_CLUTCH_THRESH-stall);
                if (clutch < 0)
                	clutch = 0;
                clutch = 1.0f - clutch;
                if (clutch > autoclutch)
                autoclutch = clutch;
        }

        if (c->shift_time > 0)
        {
                float clutch = c->shift_time / AUTO_CLUTCH_ENGAGE_TIME;
                if (clutch > autoclutch)
                        autoclutch = clutch;

                c->shift_time -= timefactor/fps;
        }

        if (!c->car->ShiftPending())
        {
                c->car->clutch (1.0f-autoclutch, 0.0);
        }
}

void AI::updateSteer(AI_Car *c, float timefactor, float fps)
{
	BEZIER *curr_patch = c->car->GetColPatch(0);
	//if car has no contact with track, just let it roll
	if (!curr_patch)
	{
		curr_patch = c->car->GetColPatch(1); //let's try the other wheel
		if (!curr_patch)
		{
			if (!c->last_patch) return;
			//if car is off track, steer the car towards the last patch it was on
			//this should get the car back on track
			else curr_patch = c->last_patch;
		}
	}

	c->last_patch = curr_patch; //store the last patch car was on

	BEZIER *next_patch = curr_patch->next_patch;
	//if there is no next patch (probably a non-closed track), let it roll
	if (!next_patch) return;

	//find the point to steer towards to
	VERTEX center1 = (curr_patch->points[0][0] + curr_patch->points[3][0]);
	VERTEX center2 = (curr_patch->points[0][3] + curr_patch->points[3][3]);
	float track_width = (center1 - center2).len() / 2.0;
	float lookahead = track_width * LOOKAHEAD_FACTOR1 + 
			  c->car->chassis().cm_velocity().magnitude() * LOOKAHEAD_FACTOR2;
	float length = 0.0;
	VERTEX dest_point;
	while (length < lookahead)
	{
        	VERTEX center1 = (next_patch->points[0][0] + next_patch->points[0][3]);
	        VERTEX center2 = (next_patch->points[3][0] + next_patch->points[3][3]);
		length += (center1 - center2).len();
		dest_point = center1.ScaleR(0.5);
		next_patch = next_patch->next_patch;
		//if there is no next patch for whatever reason, stop lookahead
		//if next patch is a very sharp corner, stop lookahead
		if ((!next_patch) || next_patch->track_radius < LOOKAHEAD_MIN_RADIUS)
		{
			length = lookahead;
			break;
		}
	}	

	//introduce some bias depends on whether it's left or right turn and the radius
	//float bias = 0.0;
	//if (next_patch->track_radius > 300.0) bias = 0.0;
	//else bias = 0.3 * (1 - next_patch->track_radius/300.0) * next_patch->turn;
	//VERTEX next_patch_center = next_patch->SurfCoord(0.5 + bias,0.5);

	Vamos_Geometry::Three_Vector next_position;
	next_position[0] = dest_point.x;
	next_position[2] = dest_point.y;
	next_position[1] = -dest_point.z;

	Vamos_Geometry::Three_Vector car_position = c->car->chassis().position();
	Vamos_Geometry::Three_Vector car_orientation = c->car->chassis().orientation().unit(0);

	Vamos_Geometry::Three_Vector desire_orientation = next_position - car_position;

	//car's direction on the horizontal plane
	car_orientation[2] = 0;
	//desired direction on the horizontal plane
	desire_orientation[2] = 0;

	//the angle between car's direction and unit y vector (forward direction)
	Vamos_Geometry::Three_Vector unit_y(0,1,0);
	double alpha = car_orientation.angle(unit_y);
	if (car_orientation[0] < 0) alpha = -alpha;
	alpha = alpha * 180.0 / M_PI;

	//the angle between desired direction and unit y vector (forward direction)
        double beta = desire_orientation.angle(unit_y);
        if (desire_orientation[0] < 0) beta = -beta;
	beta = beta * 180.0 / M_PI;

	//calculate steering angle and direction
	double angle = beta - alpha;
	if (angle > -360.0 && angle <= -180.0)
		angle = -(360.0 + angle);
	else if (angle > -180.0 && angle <= 0.0)
		angle = - angle;
	else if (angle > 0.0 && angle <= 180.0)
		angle = - angle;
	else if (angle > 180.0 && angle <= 360.0)
		angle = 360.0 - angle;

	float steer_value = angle / c->car->max_steer_angle();
	if (steer_value > 1.0) steer_value = 1.0;
	else if (steer_value < -1.0) steer_value = -1.0;

	c->car->steer(steer_value);
}

