#include <math.h>

#include <AmbrosiaTools/stdtypes.h>

#include "vectors.h"
#include "carphysics.h"
#include "gameframe.h"
#include "text.h"
#include "roads.h"

#define CAMBER 0

/*
Race Car Vehicle Dynamices Ferrari Tire:
float a[15]={1.799,0,1688,4140,6.026,0,-0.3589,1,0,-6.111/1000,-3.244/100,0,0,0,0};
float b[11]={1.65,0,1688,0,229,0,0,0,-10,0,0};

http://dynamic2.gamespy.com/~hg/forums/showthread.php?s=&threadid=49801
???
float a[15]={1.025,5,850,1200,46,0.5,-0.5,-0.5,0.5,-0.04,-0.08,0,0,0,0};
float b[11]={1.5,5,1190,0.5,60,-0.05,-0.5,-0.05,0.25,0.05,-0.025};

205/45ZR16
float a[15]={1,-106,1629,1843,9.4,0.013,-0.336,1.14,0.019,-0.019,-0.18,-20.7,-0.021,0.48,12.2};
float b[11]={1.7,-80,1571,23.3,300,0,0.0069,0.055,-0.024,0.014,0.26};


my testing values:
float a[15]={1.0,-120,1688,4140,6.026,0,-0.3589,1,0,-6.111/1000,-3.244/100,0,0,0,0};
float b[11]={1.0,-120,1588,0,229,0,0,0,-10,0,0};

*/

float a[15]={1.0,-60,1688,4140,6.026,0,-0.3589,1,0,-6.111/1000,-3.244/100,0,0,0,0};
float b[11]={1.0,-60,1588,0,229,0,0,0,-10,0,0};

float CalcLongitudinalForce(float Fz,float slip)
{
	Fz*=0.001;//convert to kN
	slip*=100; //covert to %
	float uP=b[1]*Fz+b[2];
	float D=uP*Fz;	
	float B=((b[3]*Fz+b[4])*exp(-b[5]*Fz))/(b[0]*uP);
	float S=slip+b[9]*Fz+b[10];
	float E=b[6]*sqr(Fz)+b[7]*Fz+b[8];
	float Fx=D*sin(b[0]*atan(S*B+E*(atan(S*B)-S*B)));
	return Fx;
}

/*
float CalcLateralForce(float Fz,float slipAngle)
{
	Fz*=0.001;//convert to kN
	slipAngle*=(360/(2*PI)); //convert angle to deg
	float uP=a[1]*Fz+a[2];
	float D=uP*Fz;
	float B=(a[3]*sin(2*atan(Fz/a[4]))*(1-a[5]*fabs(CAMBER)))/(a[0]*uP*Fz);
	float S=slipAngle+a[8]*CAMBER+a[9]*Fz+a[10];
	float E=a[6]*Fz+a[7];
	float Sv=((a[11]*Fz+a[11])*CAMBER+a[12])*Fz+a[13];
	float Fy=D*sin(a[0]*atan(S*B+E*(atan(S*B)-S*B)))+Sv;
	return Fy;
}
*/
float CalcLateralForce(float Fz,float slipAngle)
{
	Fz*=0.001;//convert to kN
	slipAngle*=(360/(2*PI)); //convert angle to deg
	float uP=a[1]*Fz+a[2];
	float D=uP*Fz;
	float B=(a[3]*sin(2*atan(Fz/a[4])))/(a[0]*uP*Fz);
	float S=slipAngle+a[9]*Fz+a[10];
	float E=a[6]*Fz+a[7];
	float Sv=a[12]*Fz+a[13];
	float Fy=D*sin(a[0]*atan(S*B+E*(atan(S*B)-S*B)))+Sv;
	return Fy;
}

float CalcLongitudinalForceUnit(float Fz,float slip,tCarPhysics *phys)
{
	return CalcLongitudinalForce(Fz,slip*phys->maxSlip);
}

float CalcLateralForceUnit(float Fz,float slipAngle,tCarPhysics *phys)
{
	return CalcLongitudinalForce(Fz,slipAngle*phys->maxAngle);
}

void CalcCombinedForce(float Fz,tCarPhysics *phys,float *lateral,float *longitunal,float slip,float slipAngle)
{
	float unitSlip=slip/phys->maxSlip;
	float unitAngle=slipAngle/phys->maxAngle;
	float p=sqrt(sqr(unitSlip)+sqr(unitAngle));
	if(p){
		*longitunal=unitSlip/p*CalcLongitudinalForceUnit(Fz,p,phys);
		*lateral=unitAngle/p*CalcLateralForceUnit(Fz,p,phys);
	}
	else
	{
		*longitunal=0;
		*lateral=0;
	}
	/*
	*longitunal=CalcLongitudinalForce(Fz,slip);
	*lateral=CalcLateralForce(Fz,slipAngle);
	if(*longitunal>Fz)*longitunal=Fz;
	if(sqr(*lateral)+sqr(*longitunal)>sqr(Fz))*lateral=sqrt(sqr(Fz)-sqr(*longitunal))*sign(*lateral);*/
}

#define kStepSize 0.001
#define kTestNormalForce 4000

void InitSlipMaxima(tCarDefinition *car,tCarPhysics *phys)
{
	float force=0;
	for(float slip=kStepSize;;slip+=kStepSize)
	{
		float newForce=CalcLongitudinalForce(kTestNormalForce,slip);
		if(force<newForce)
			force=newForce;
		else {
			phys->maxSlip=slip-kStepSize;
			break;
		}
	}
	force=0;
	for(float slipAngle=kStepSize;;slipAngle+=kStepSize)
	{
		float newForce=CalcLateralForce(kTestNormalForce,slipAngle);
		if(force<newForce)
			force=newForce;
		else {
			phys->maxAngle=slipAngle-kStepSize;
			break;
		}
	}
}

#define kFullSlipVelo 4.0
#define kFullAngleVelo 2.0

float CalcSlipRatio(tVector3 wheelVelo,tVector3 wheelDir,float angularVelo,float radius)
{
	float wheelRoadVelo=wheelVelo*wheelDir; 
	if(wheelRoadVelo==0)
		return 0;
	float slipDamp=fabs(wheelRoadVelo)<kFullSlipVelo?fabs(wheelRoadVelo)/kFullSlipVelo:1;
	return ((angularVelo*radius-wheelRoadVelo)/fabs(wheelRoadVelo))*slipDamp;	
}

float CalcSlipAngle(tVector3 wheelVelo,tVector3 wheelDir,tVector3 groundNormal)
{
	tVector3 cross=groundNormal%wheelVelo;
	if(VectorZero(cross))
		return 0;
	tVector3 wheelMotionDir=!((groundNormal%wheelVelo)%groundNormal);
	float angleDamp=sqr(wheelVelo)<sqr(kFullAngleVelo)?~wheelVelo/kFullAngleVelo:1;
	float sinSlipAngle=(wheelDir%wheelMotionDir)*groundNormal;
	if(fabs(sinSlipAngle)>1) // to avoid precision errors causing nan's
		sinSlipAngle=sign(sinSlipAngle)*1;
	return VectorZero(wheelMotionDir)?0:asin(sinSlipAngle)*angleDamp*angleDamp;
}


#define kMaxSlipResAngularVelo 100.0
#define kMaxSlipRes 10.0

tVector3 PacejkaCalcWheelRoadForces(tGameEntity *carEntity,tCarDefinition *car,tCarPhysics *phys,tWheelCalcData *wheels,int i,float engineTorque,float frictionTorque)
{
	if(phys->maxSlip==0)
		InitSlipMaxima(car,phys);
		
	tVector3 wheelVelo=wheels[i].velo;
	tVector3 totalForce=Vector(0,0,0);

	if(phys->wheels[i].angularVelo<-50&&phys->gear>=0&&carEntity->controlType==kControlTypeAIInput)
	{
		phys->gear=0;
		phys->rpm=0;
		phys->wheels[i].angularVelo=10;
	}

	int slipRes=(kMaxSlipResAngularVelo-fabs(phys->wheels[i].angularVelo))/(kMaxSlipResAngularVelo/kMaxSlipRes);
	if(slipRes<1)slipRes=1;
	
	//adjust pajecka load sensitivity according to wheel width
	a[1]=-200*1/(car->wheels[i].width*5+0.5)+50;
	a[1]*=1+car->wheels[i].loadSensitivity;
	b[1]=a[1];

	float engineAngularDelta=engineTorque*kFrameTime/(slipRes*wheels[i].inertia);
	float frictionAngularDelta=frictionTorque*kFrameTime/(slipRes*wheels[i].inertia);
	float grip=(gSurfaceTypes->types+phys->wheels[i].surfaceType)->grip*(1+car->wheels[i].stickyness);
	
	tVector3 wheelDir,wheelNormal;
	float slipRatio,slipAngle;float lateral=0;
	static int damageDir=-1;
	if(phys->wheels[i].angle!=wheels[i].oldAngle)
		damageDir=sign(phys->wheels[i].angle-wheels[i].oldAngle);
	for(int j=0;j<slipRes;j++)
	{
		float angle=wheels[i].oldAngle+(phys->wheels[i].angle-wheels[i].oldAngle)*((float)j/slipRes);
		if(phys->damage>kSuspensionDamage&&i<2)
			angle-=0.02*damageDir;
		wheelDir=Vector(sin(angle),0,cos(angle));
		wheelDir=!((wheels[i].groundNormal%(wheelDir*carEntity->dir))%wheels[i].groundNormal);
		wheelNormal=Vector(cos(angle),0,-sin(angle));
		wheelNormal=!((wheels[i].groundNormal%(wheelNormal*carEntity->dir))%wheels[i].groundNormal);

		slipRatio=CalcSlipRatio(wheelVelo,wheelDir,phys->wheels[i].angularVelo,car->wheels[i].radius);
		slipAngle=CalcSlipAngle(wheelVelo,wheelDir,wheels[i].groundNormal);

		float longitudinalForce,lateralForce;
		CalcCombinedForce(wheels[i].normalForce,phys,&lateralForce,&longitudinalForce,slipRatio,slipAngle);
		lateralForce*=grip;
		longitudinalForce*=grip;	
		totalForce=totalForce+wheelDir*longitudinalForce-wheelNormal*lateralForce;
		
		lateral+=lateralForce/slipRes;
		phys->wheels[i].angularVelo-=(longitudinalForce*car->wheels[i].radius)*kFrameTime/(slipRes*wheels[i].inertia);
		phys->wheels[i].angularVelo+=engineAngularDelta;
		if(fabs(frictionAngularDelta)<fabs(phys->wheels[i].angularVelo))		
			phys->wheels[i].angularVelo-=frictionAngularDelta*sign(phys->wheels[i].angularVelo);		
		else 
			phys->wheels[i].angularVelo=0;
		wheelVelo=wheelVelo+(wheelDir*longitudinalForce-wheelNormal*lateralForce)*((1/car->mass)*(kFrameTime/(float)slipRes));
	}
	
	float wheelRoadVelo=VectorZero(wheelVelo)?0:wheelVelo*wheelDir; 
	float longitunalSlipVelo=fabs(phys->wheels[i].angularVelo*car->wheels[i].radius-wheelRoadVelo);	
	float lateralSlipVelo=wheelVelo*wheelNormal;
	phys->wheels[i].slipVelo=sqrt(sqr(longitunalSlipVelo)+sqr(lateralSlipVelo));//*wheels[i].normalForce*car->numWheels/(car->mass*9.81);
	phys->wheels[i].slip=slipRatio;
	phys->wheels[i].slipAngle=slipAngle;
			
	return totalForce*((float)1/slipRes);
}