#include <ode/ode.h>
#include "car.h"

void CCar::init(dWorldID w, dSpaceID s, double x, double y, double z)
{
	if(alive)
		destroy();
	int i;
	
	world_citzen=w;
	space_citzen=s;
	
	dMass m;
	chassis = dBodyCreate(w);
	dBodySetPosition(chassis, x, y, z);
	dMassSetBox(&m, 1, 2, 3, 1.5);
	dBodySetMass(chassis, &m);
	chassisbox = dCreateBox(s, 2, 3, 1.5);
	dGeomSetBody(chassisbox, chassis);
	
	for(i=0; i<4; i++)
	{
		wheel[i] = dBodyCreate(w);
		dMassSetSphere(&m, 5, 0.8);
		dBodySetMass(wheel[i], &m);
		wheelsphere[i] = dCreateSphere(s, 0.8);
		dGeomSetBody(wheelsphere[i], wheel[i]);
	}
	dBodySetPosition(wheel[0], x-1.9, y-1.5, z-1.3);
	dBodySetPosition(wheel[1], x+1.9, y-1.5, z-1.3);
	dBodySetPosition(wheel[2], x-1.9, y+1.5, z-1.3);
	dBodySetPosition(wheel[3], x+1.9, y+1.5, z-1.3);
	
	for(i=0; i<4; i++)
	{
		wheeljoint[i] = dJointCreateHinge2(w, 0);
		dJointAttach(wheeljoint[i], chassis, wheel[i]);
		const dReal *a = dBodyGetPosition(wheel[i]);
		dJointSetHinge2Anchor(wheeljoint[i], a[0], a[1], a[2]);
		dJointSetHinge2Axis1(wheeljoint[i], 0, 0, 1);
		dJointSetHinge2Axis2(wheeljoint[i], 1, 0, 0);
		
		dJointSetHinge2Param(wheeljoint[i], dParamSuspensionERP, 0.6);
		dJointSetHinge2Param(wheeljoint[i], dParamSuspensionCFM, 0.08);
	}
	for(i=0; i<2; i++)
	{
		dJointSetHinge2Param(wheeljoint[i], dParamLoStop, 0);
		dJointSetHinge2Param(wheeljoint[i], dParamHiStop, 0);
	}
	steer=0;
	for(i=2; i<4; i++)
	{
		dJointSetHinge2Param(wheeljoint[i], dParamLoStop, steer);
		dJointSetHinge2Param(wheeljoint[i], dParamHiStop, steer);
		dJointSetHinge2Param(wheeljoint[i], dParamSuspensionERP, 0);
		dJointSetHinge2Param(wheeljoint[i], dParamSuspensionCFM, 0);
	}
	
	alive=1;
}

void CCar::destroy(void)
{
	int i;
	for(i=0; i<4; i++)
		dJointDestroy(wheeljoint[i]);
	
	dGeomDestroy(chassisbox);
	for(i=0; i<4; i++)
		dGeomDestroy(wheelsphere[i]);
		
	dBodyDestroy(chassis);
	for(i=0; i<4; i++)
		dBodyDestroy(wheel[i]);

	alive=0;
}

void CCar::fly(void)
{
	dBodyAddForce(chassis, 0, 0, 2000);
}

void CCar::set_engine(int mode)
{
	int i;
	if(mode==CARENGINE_COAST)
	{
		for(i=0; i<4; i++)
			dJointSetHinge2Param(wheeljoint[i], dParamFMax2, 0);
	}
	if(mode==CARENGINE_BRAKE)
	{
		for(i=0; i<4; i++)
		{
			dJointSetHinge2Param(wheeljoint[i], dParamVel2, 0);
			dJointSetHinge2Param(wheeljoint[i], dParamFMax2, 200);
		}
	}
	if(mode==CARENGINE_FORWARD)
	{
		for(i=0; i<4; i++)
		{
      		        dJointSetHinge2Param(wheeljoint[i], dParamVel2, 13);
			dJointSetHinge2Param(wheeljoint[i], dParamFMax2, 90);
		}
	}
	if(mode==CARENGINE_REVERSE)
	{
		for(i=0; i<4; i++)
		{
			dJointSetHinge2Param(wheeljoint[i], dParamVel2, -13);
			dJointSetHinge2Param(wheeljoint[i], dParamFMax2, 90);
		}
	}
}

void CCar::set_steer(double s)
{
	steer=s;
	if(steer>0.5)
		steer=0.5;
	if(steer<-0.5)
		steer=-0.5;
	adjust_steer();
}

void CCar::add_steer(double s)
{
	steer+=s;
	if(steer>0.5)
		steer=0.5;
	if(steer<-0.5)
		steer=-0.5;
	adjust_steer();
}

void CCar::adjust_steer(void)
{
		if(steer==0)
		{
			dJointSetHinge2Param(wheeljoint[2], dParamLoStop, 0);
			dJointSetHinge2Param(wheeljoint[2], dParamHiStop, 0);
			dJointSetHinge2Param(wheeljoint[3], dParamLoStop, 0);
			dJointSetHinge2Param(wheeljoint[3], dParamHiStop, 0);
		}
		else if(steer>0)
		{
			dJointSetHinge2Param(wheeljoint[2], dParamLoStop, M_PI/2-atan((tan(M_PI/2-steer)*3+1.9)/3));
			dJointSetHinge2Param(wheeljoint[2], dParamHiStop, M_PI/2-atan((tan(M_PI/2-steer)*3+1.9)/3)+0.02);
			dJointSetHinge2Param(wheeljoint[3], dParamLoStop, M_PI/2-atan((tan(M_PI/2-steer)*3-1.9)/3));
			dJointSetHinge2Param(wheeljoint[3], dParamHiStop, M_PI/2-atan((tan(M_PI/2-steer)*3-1.9)/3)+0.02);
		}
		else
		{
			dJointSetHinge2Param(wheeljoint[2], dParamLoStop, atan((tan(M_PI/2+steer)*3-1.9)/3)-M_PI/2);
			dJointSetHinge2Param(wheeljoint[2], dParamHiStop, atan((tan(M_PI/2+steer)*3-1.9)/3)-M_PI/2+0.02);
			dJointSetHinge2Param(wheeljoint[3], dParamLoStop, atan((tan(M_PI/2+steer)*3+1.9)/3)-M_PI/2);
			dJointSetHinge2Param(wheeljoint[3], dParamHiStop, atan((tan(M_PI/2+steer)*3+1.9)/3)-M_PI/2+0.02);
		}
}


