//ai.cpp
//the game's AI system for computer-controlled opponents
#include <math.h>
#include "entities.h"
#include "controls.h"
#include "carphysics.h"
#include "gameframe.h"
#include "roads.h"

#define kAIMaxThottleSlip 0.05
#define kAIMinSpinAngularVelo (6*PI)
#define kAIThrottleTime 0.2
#define kAIThrottleReleaseTime 0.4
#define kAIMaxBrakeSlip 0.05
#define kAIBrakeTime 0.4
#define kAIBrakeReleaseTime 0.2
#define kAIWaypointAheadDistance (gGameInfo->arcade==kGameModeTurbo?25:15)
#define kAIMaxSteering 2

float AISteering(tCarDefinition *car,tGameEntity *entity,tVector3 wayPoint,float *overSteer)
{
	// Will adjust the cars steering in order to make the car head to wayPoint.
	
	tVector3 carDir=!Vector(MatrixGetZVector(entity->dir)->x,0,MatrixGetZVector(entity->dir)->z);
	tVector3 wayDir=!Vector(entity->pos.x-wayPoint.x,0,entity->pos.z-wayPoint.z);
	tVector3 veloDir=(VectorZero(entity->velo)?carDir:!entity->velo);
	*overSteer=(carDir%veloDir).y;
	float angleSin=(carDir%wayDir).y;
	if(fabs(angleSin)>1.0)
		angleSin=sign(angleSin);
	float angle=-asin(angleSin);
	float steering=angle/car->wheels[0].maxAngle;
	if(steering>kAIMaxSteering)steering=kAIMaxSteering;
	if(steering<-kAIMaxSteering)steering=-kAIMaxSteering;
	return steering;
}

#define kAIMinCarDistance 4
#define kAICollisionPredictPrecission 20
#define kAIMaxCollisionDelay 5.0
#define kAICarLength 2
#define kAIVeloAdd 2.0

float AIGetCollisionDelay(tGameEntity *entity,tGameEntity **obstacle)
{
	tVector2 basePos1=Vector(entity->pos.x,entity->pos.z);
	tVector2 velo1=Vector(entity->velo.x,entity->velo.z);
	tVector2 dir1=Vector(MatrixGetZVector(entity->dir)->x,MatrixGetZVector(entity->dir)->z);
	float fVelo1=~velo1;
	if(fVelo1>0.1)
		velo1=velo1*((1/fVelo1)*(fVelo1+kAIVeloAdd));
	else
		velo1=dir1*kAIVeloAdd;
	int minDelay=kAICollisionPredictPrecission;
	for(int i=0;i<gGameInfo->numPlayers;i++)
		if(gCarEntities[i]!=entity)
		{
			tVector2 pos2=Vector(gCarEntities[i]->pos.x,gCarEntities[i]->pos.z);
			tVector2 diff=pos2-basePos1;
			if(sqr(diff)<sqr(120)&&diff*dir1>=0)
			//is the other car close in front of us?
			{
				tVector2 pos1=basePos1;
				tVector2 velo2=Vector(gCarEntities[i]->velo.x,gCarEntities[i]->velo.z);
				tVector2 dir2=Vector(MatrixGetZVector(gCarEntities[i]->dir)->x,MatrixGetZVector(gCarEntities[i]->dir)->z);
				for(int j=0;j<kAICollisionPredictPrecission&&j<minDelay;j++)
				{
					pos1=pos1+velo1*(kAIMaxCollisionDelay/kAICollisionPredictPrecission);
					pos2=pos2+velo2*(kAIMaxCollisionDelay/kAICollisionPredictPrecission);
					if(sqr(pos1+dir1*kAICarLength-pos2+dir2*kAICarLength)<sqr(kAIMinCarDistance))
						{minDelay=j;*obstacle=gCarEntities[i];}
					else if(sqr(pos1-dir1*kAICarLength-pos2+dir2*kAICarLength)<sqr(kAIMinCarDistance))
						{minDelay=j;*obstacle=gCarEntities[i];}
					else if(sqr(pos1+dir1*kAICarLength-pos2-dir2*kAICarLength)<sqr(kAIMinCarDistance))
						{minDelay=j;*obstacle=gCarEntities[i];}
					else if(sqr(pos1-dir1*kAICarLength-pos2-dir2*kAICarLength)<sqr(kAIMinCarDistance))
						{minDelay=j;*obstacle=gCarEntities[i];}
				}
			}
		}
	return (minDelay+1)*(kAIMaxCollisionDelay/kAICollisionPredictPrecission);
}

//Calculate the Maximal Throttle an AI player may get, which may be more than
//100% to help the AI a little.
float AIMaxThrottle(tCarPhysics *phys)
{
	if(gFrameCount*kFrameTime<kStartGameDelaySeconds)
		return 1;
	//"power cycle" for more random AI behavior
	float base=1.0+0.25*sin((gFrameCount*kFrameTime)*0.1+phys->aiPowerCycle);
	
	//help for players in the back of the field.
	float aid=(-phys->lead-8)/22;
	if(aid<0)aid=0;
	if(aid>1)aid=1;
	
	return base+aid*0.6;
}

void AIBrakeThrottle(tCarDefinition *car,tCarPhysics *phys,float velo,float optimalVelo)
{
	float driveSlip=0,slip=0,angularVelo=0;
	float maxThrottle=AIMaxThrottle(phys);
	
	for(int i=0;i<car->numWheels;i++)
	{
		driveSlip+=phys->wheels[i].slip*car->wheels[i].powered;
		slip+=phys->wheels[i].slip;
		angularVelo+=fabs(phys->wheels[i].angularVelo*car->wheels[i].powered);
	}
	slip/=car->numWheels;
	float maxSlip=velo<20?0.2-velo/20*(0.2-kAIMaxThottleSlip):kAIMaxThottleSlip;

	if((driveSlip*sign(phys->gear)<maxSlip||angularVelo<kAIMinSpinAngularVelo||(phys->clutch<=1&&phys->gear<=1)) //traction control
		&&velo<optimalVelo)		//we don't want to drive too fast!
		phys->throttle+=kFrameTime*(1/kAIThrottleTime);
	else
		phys->throttle-=kFrameTime*(1/kAIThrottleReleaseTime);
			
	if(phys->throttle>maxThrottle)
		phys->throttle=maxThrottle;
	else if(phys->throttle<phys->idleThrottle)
		phys->throttle=phys->idleThrottle;	
	
	if((velo>optimalVelo+1||optimalVelo==0))
	{
		if((-slip*sign(angularVelo)<kAIMaxBrakeSlip)||(phys->brake<0.1))
			phys->brake+=kFrameTime*(1/kAIBrakeTime);
		else
			phys->brake-=kFrameTime*(1/kAIBrakeTime);		
		phys->arcadeBrake+=kFrameTime*(1/kAIBrakeTime);
	}
	else{
		phys->arcadeBrake-=kFrameTime*(1/kAIBrakeReleaseTime);
		phys->brake-=kFrameTime*(1/kAIBrakeReleaseTime);
	}
	
	if(phys->arcadeBrake>1)
		phys->arcadeBrake=1;
	else if(phys->arcadeBrake<0)
		phys->arcadeBrake=0;
	if(phys->brake>1)
		phys->brake=1;
	else if(phys->brake<0)
		phys->brake=0;
}

#define kAIOvertakeRoadAnalyseDistance 50 //length of approaching road section to be evaluated for overtaking
#define kAIOvertakeMinVeloDiffPossible 10 
#define kAIOvertakeAbortDistance 60
#define kAIOvertakeFinishDistance 20
#define kAIOvertakeLaneChangeTime 3.0

void AIEvaluateOvertaking(tGameEntity *overtaker,tGameEntity *obstacle,float track)
{
	tCarPhysics *phys=(tCarPhysics*)overtaker->physics;
	if(phys->overtaking)//are we already overtaking?
		return;
	if(overtaker->velo**MatrixGetZVector(overtaker->dir)<=obstacle->velo**MatrixGetZVector(overtaker->dir)) //are we faster?
		return;
	float minTrack,maxTrack,minSpeed;
	RoadGetWayPointData(overtaker->lastRoadIndex,kAIOvertakeRoadAnalyseDistance,&minTrack,&maxTrack,&minSpeed);
	if(minSpeed*0.75<~obstacle->velo+kAIOvertakeMinVeloDiffPossible) //is it possible to drive faster than obstacle all the time?
		return;
//	if(fabs(minTrack-maxTrack)>1)
//		return;
	phys->overtaking=obstacle;
	phys->overtakeSide=fabs(1+minTrack)>fabs(1-maxTrack)?-1:1;
}

void ControlEntityAIInput(tGameEntity *entity)
//The game's AI - adjusts all the controls of the car passed in entity.
{
	if(entity->physicsType!=kPhysicsTypeCar)//is entity really a car?
		return;
	tCarPhysics *phys=(tCarPhysics*)entity->physics;
	tCarDefinition *car=&(phys->car);
	float velo=~entity->velo;

	
	//determine course		
	float optimalVelo;
	float track=phys->overTakeProgress+0.4*sin((gFrameCount*kFrameTime)*0.16+phys->aiRouteCycle);
	if(track<-1)track=-1;
	else if(track>1)track=1;
	tVector3 wayPoint=RoadGetNextWaypoint(entity->pos,&entity->lastRoadIndex,&track,&optimalVelo,kAIWaypointAheadDistance);
	
	if(gGameInfo->arcade==kGameModeArcade||gGameInfo->arcade==kGameModeTurbo)
		optimalVelo*=1.3;
	
	if(phys->lapCount>gGameInfo->numLaps)
		optimalVelo=0;
		
	//see if we are about to crash into another car if we are to remain on our course.
	tGameEntity *obstacle;
	float del=AIGetCollisionDelay(entity,&obstacle);

	//if we will crash into another car within the next 5 seconds, see if we can overtake
	if(del<=5)
		AIEvaluateOvertaking(entity,obstacle,track);

	if(del>5&&phys->stuckTime>10*kFPS)
	//if we are stuck, recover car.
	{
		RoadCenterCar(entity);
		phys->overtaking=NULL;
		phys->stuckTime=0;
	}
		
	//if we are overtaking 	
	//phys->lightFlags&=~(kLightFlagLIndLight|kLightFlagRIndLight);
	if(phys->overtaking)
	{
		if(fabs(phys->overTakeProgress)<1)
		{
			phys->overTakeProgress+=(1/kAIOvertakeLaneChangeTime)*kFrameTime*sign(phys->overtakeSide);
			if(fabs(phys->overTakeProgress)>1)
				phys->overTakeProgress=sign(phys->overTakeProgress);
			/*if(phys->lapCount<=gGameInfo->numLaps)
				phys->lightFlags|=(phys->overtakeSide==(gGameInfo->reverse?-1:1)?kLightFlagLIndLight:kLightFlagRIndLight);*/
		}
		float dist=-(phys->overtaking->pos-entity->pos)**MatrixGetZVector(entity->dir);
		if(dist>kAIOvertakeFinishDistance)//are we finished overtaking?
			phys->overtaking=NULL;
		else if(dist<-kAIOvertakeAbortDistance)//or are we too slow to overtake?
			phys->overtaking=NULL;
		if(fabs(phys->overtakeSide-track)<0.5)
			phys->overtaking=NULL;
	}
	else if(fabs(phys->overTakeProgress)>0)
	{
		int s=sign(phys->overTakeProgress);
		phys->overTakeProgress-=(1/kAIOvertakeLaneChangeTime)*kFrameTime*s;
		//if(phys->lapCount<=gGameInfo->numLaps)
			//phys->lightFlags|=(s==-1?kLightFlagLIndLight:kLightFlagRIndLight);
		if(s!=sign(phys->overTakeProgress))
			phys->overTakeProgress=0;
	}
	
	//steering
	float overSteer;
	phys->steering=AISteering(car,entity,wayPoint,&overSteer);
	
	//brake / throttle	
	AIBrakeThrottle(car,phys,velo,optimalVelo);
	
	if(gFrameCount*kFrameTime>=kStartGameDelaySeconds)
	{
		//gear shifting / clutch control		
		if(phys->gear==0)
		{
			phys->gear=1;
			phys->lastGearSwitch=gFrameCount*kFrameTime;
		}
		if(phys->gear>=1&&(gFrameCount-1)*kFrameTime>phys->lastGearSwitch+car->gearSwitchTime&&gFrameCount*kFrameTime>=kStartGameDelaySeconds+1.5)
		{
			if(phys->rpm>car->maxRPM&&phys->gear<car->numGears-2)
			{
				phys->gear++;
				phys->lastGearSwitch=gFrameCount*kFrameTime;
			}	
			if(phys->rpm<car->shiftDownRPM&&phys->gear>1)
			{
				phys->gear--;
				phys->lastGearSwitch=gFrameCount*kFrameTime;
			}	
		}
		
		if(gFrameCount*kFrameTime<phys->lastGearSwitch+car->gearSwitchTime)
		{
			float switchTime=(gFrameCount*kFrameTime-phys->lastGearSwitch)/car->gearSwitchTime;
			phys->clutch=switchTime;
			if(phys->rpm<2000)
				phys->clutch=(phys->rpm-car->idleRPM)/(2000-car->idleRPM);
			if(switchTime<0.5)
				phys->throttle=phys->idleThrottle;
		}
		else
			if(phys->rpm<2000)
				phys->clutch=(phys->rpm-car->idleRPM)/(2000-car->idleRPM);
			else 
				phys->clutch=1;
	}	
	
	if(overSteer*velo>20)
		if(car->wheels[2].powered)
		{
			phys->clutch=0;
			phys->throttle=0;
		}	
}