/***************************************************************************
 *   Copyright (C) 2004-2005 by                                            *
 *     Paolo Sacconier   <axa1981@tin.it>                                  *
 *     Francesco Tamagni <minchiahead@hacari.org>                          *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#include "omnicar.h"

namespace gillo {
sgVec3 Omnicar::dirDim = { 0.04, 0.35, 0.04 };
// sgVec3 Omnicar::carDim  = { 0.04, 0.12, 0.02 };
sgVec3 Omnicar::carDim  = { 0.05, 0.12, 0.05 };
Omnicar::Omnicar(Context& c, SimpleBall& b, float pi, int plid, int nw)
 : Entity(c), ball(b), speed(0), steering(0), backwards(false), jumping(false), steerAngle(0), ballJoint(0), jumpForce(Omnicar::maxJumpForce), fire(false), fireTime(1.0), IBallPicker(pi, plid), nWheels(nw)
{
	carBid = new dBodyID[nw];
	wheelBid = new dBodyID[nw];
	wheelTrans = new ssgTransform* [nw];
	carTrans = new ssgTransform* [nw];

	charge = savedCharge = 3;
	
	const double wheelRadius = 0.1;
	sgVec3 zero = { 0.0, 0.0, 0.0 };
	
	
	const double wheelDistance = carDim[1] + wheelRadius;
	
	sgVec3 dirCenter = { 0.0, dirDim[1]/2, 0.0 };
	
// 	sgVec3 carCenter[3]   = { {    1*carDim[1]/2,                        0, 0 },
// 							  { -0.5*carDim[1]/2,  1.7320508/2*carDim[1]/2, 0 },
// 							  { -0.5*carDim[1]/2, -1.7320508/2*carDim[1]/2, 0 } };
// 	
// 	sgVec3 wheelCenter[3] = { {    1*wheelDistance,                          0, 0 },
// 							  { -0.5*wheelDistance,  1.7320508/2*wheelDistance, 0 },
// 							  { -0.5*wheelDistance, -1.7320508/2*wheelDistance, 0 } };
	
	dMass carMass, wheelMass, dirMass;
	dGeomID * carGid = new dGeomID[nw];
	dGeomID * wheelGid = new dGeomID[nw];
	
	dJointID joe;
	
	type = CAR;
	
	jid = dJointGroupCreate(0);
	dSpaceID sid = dSimpleSpaceCreate(c.getSid());
	gid = (dGeomID) sid;
	c.getScenePtr()->addKid(&trans);

	ssgEntity* e;
	if (plid <= 1) {
		e = c.searchInPool("OmniCarBody1");
		if (e == NULL) {
			e = ssgLoadAC( "omnicar_1.ac" ) ;
			e -> setName          ("OmniCarBody1") ;
		}
	} else {
		e = c.searchInPool("OmniCarBody2");
		if (e == NULL) {
			e = ssgLoadAC( "omnicar_2.ac" ) ;
			e -> setName          ("OmniCarBody2") ;
		}
	}
	
	if (e != NULL) {
		ssgFlatten ( e ) ;
		ssgStripify( e ) ;
		c.addToPool(e);
	}
	
	ssgTransform * carObj = new ssgTransform();
	sgMat4 scalm;
	sgMakeIdentMat4(scalm);
	scalm[0][0] = carDim[0];
	scalm[1][1] = carDim[1];
	scalm[2][2] = carDim[2];
	carObj->setTransform(scalm);
	carObj->addKid(e);
	
	
	ssgEntity* wheelObj = c.searchInPool("Wheel");
	if (wheelObj == NULL) {
		ssgaSphere* w =  new ssgaSphere     () ;
		w -> setSize          ( 2*wheelRadius ) ;
		w -> setCenter        ( zero ) ;
		w -> setKidState      ( c.getState(STA_CAR_WHEEL) ) ;
		w -> regenerate       () ;
		w -> setName          ("Wheel") ;
		wheelObj = (ssgEntity*) w;
		c.addToPool(wheelObj);
	}
	
	dMassSetBoxTotal    ( &carMass,   ODE_CAR_MASS, carDim[0], carDim[1], carDim[2]);
	dMassSetSphereTotal ( &wheelMass, ODE_WHEEL_MASS, wheelRadius);
	
	jam = new dJointID[nw];
	
	for (int i = 0; i < nw; i++) {
		dMatrix3 R;
		double angle = ((double) i)*2.0/((double) nw)*M_PI;
		carTrans[i] = new ssgTransform();
		c.getScenePtr()->addKid(carTrans[i]);
		carTrans[i]->addKid(carObj);
		carBid[i] = dBodyCreate(c.getWid());
		carGid[i] = dCreateBox (sid, carDim[0], carDim[1], carDim[2]);
		dGeomSetBody (carGid[i], carBid[i]);
		dGeomSetData (carGid[i], this);
		dRFromAxisAndAngle (R, 0.0, 0.0, 1.0, angle+M_PI/2);
		dBodySetRotation(carBid[i], R);
// 		dGeomSetPosition (carGid[i], (dReal) carCenter[i][0], (dReal) carCenter[i][1], (dReal) carCenter[i][2]);
		dGeomSetPosition (carGid[i], (dReal) cos(angle)*carDim[1]/2.0, (dReal) sin(angle)*carDim[1]/2.0, 0.0);

		dBodySetMass(carBid[i], &carMass);
		
		wheelTrans[i] = new ssgTransform();	
		c.getScenePtr()->addKid(wheelTrans[i]);
		wheelTrans[i]->addKid(wheelObj);
		wheelBid[i] = dBodyCreate(c.getWid());
		wheelGid[i] = dCreateSphere (sid, wheelRadius);
		dGeomSetBody (wheelGid[i], wheelBid[i]);
		dGeomSetData (wheelGid[i], this);
		dGeomSetPosition (wheelGid[i], (dReal) cos(angle)*wheelDistance, (dReal) sin(angle)*wheelDistance, 0);
		dBodySetMass(wheelBid[i], &wheelMass);
		joe = dJointCreateBall (c.getWid(), jid);
		dJointAttach (joe, carBid[i], wheelBid[i]);
		dJointSetBallAnchor (joe, (dReal) cos(angle)*wheelDistance, (dReal) sin(angle)*wheelDistance, 0);
		
		jam[i] = dJointCreateAMotor (c.getWid(), jid);
		dJointAttach (jam[i], carBid[i], wheelBid[i]);
		dJointSetAMotorMode (jam[i], dAMotorEuler);
		dJointSetAMotorNumAxes (jam[i], 3);
		dJointSetAMotorAxis (jam[i], 0, 1, 1.0, 0.0, 0.0);
		dJointSetAMotorAxis (jam[i], 2, 2, 0.0, 0.0, 1.0);
	}
	
	for (int i = 1; i < nw; i++) {
		joe = dJointCreateFixed (c.getWid(), jid);
		dJointAttach (joe, carBid[i], carBid[i-1]);
		dJointSetFixed(joe);
	}
// 	joe = dJointCreateFixed (c.getWid(), jid);
// 	dJointAttach (joe, carBid[2], carBid[1]);
// 	dJointSetFixed(joe);
	
// 	dMassSetBoxTotal    ( &dirMass, 0.1, carDim[0], carDim[1], carDim[2]);
	dMassSetBoxTotal    ( &dirMass, 0.1, dirDim[0], dirDim[1], dirDim[2]);
	
	e = c.searchInPool("Picker");
	if (e == NULL) {
		e = ssgLoadAC( "picker.ac" ) ;
		ssgFlatten ( e ) ;
		ssgStripify( e ) ;
		e -> setName          ("Picker") ;
		c.addToPool(e);
	}
	
	ssgTransform * dirObj = new ssgTransform();
	sgMakeIdentMat4(scalm);
	scalm[0][0] = dirDim[0];
	scalm[1][1] = dirDim[1];
	scalm[2][2] = dirDim[2];
	dirObj->setTransform(scalm);
	dirObj->addKid(e);
	
	dirTrans = new ssgTransform();	
	c.getScenePtr()->addKid(dirTrans);
	dirTrans->addKid(dirObj);
	
	dirBid = dBodyCreate(c.getWid());
	dBodySetMass(dirBid, &dirMass);
	dBodySetPosition (dirBid, (dReal) dirCenter[0], (dReal) dirCenter[1], (dReal) dirCenter[2]);
	
	dirGid = dCreateBox (sid, dirDim[0], dirDim[1], dirDim[2]);
	dGeomSetBody (dirGid, dirBid);
	dGeomSetData (dirGid, this);
	
	joe = dJointCreateBall (c.getWid(), jid);
	dJointAttach (joe, carBid[0], dirBid);
	dJointSetBallAnchor (joe, (dReal) zero[0], (dReal) zero[1], (dReal) zero[2]);
	
	diram = dJointCreateAMotor (c.getWid(), jid);
	dJointAttach (diram, carBid[0], dirBid);
	dJointSetAMotorMode (diram, dAMotorEuler);
	dJointSetAMotorNumAxes (diram, 3);
	dJointSetAMotorAxis (diram, 0, 2, 0.0, 1.0, 0.0);
	dJointSetAMotorAxis (diram, 2, 1, 0.0, 0.0, 1.0);
	
	dJointSetAMotorParam (diram, dParamLoStop, 0);
	dJointSetAMotorParam (diram, dParamHiStop, 0);
	dJointSetAMotorParam (diram, dParamLoStop2, 0);
	dJointSetAMotorParam (diram, dParamHiStop2, 0);

}

bool Omnicar::isPickerGeom( dGeomID o ) {
	if ( o == dirGid)
		return true;
	return false;
}

Omnicar::~Omnicar()
{
	if (this->stickyBall) 
		this->context.releaseBall((IBallPicker*) this);
	dJointGroupEmpty (jid);
	for (int i=0; i<nWheels; i++) {
		dBodyDestroy(wheelBid[i]);
		dBodyDestroy(carBid[i]);
		context.getScenePtr()->removeKid(wheelTrans[i]);
		context.getScenePtr()->removeKid(carTrans[i]);
	}
	dBodyDestroy(dirBid);
	context.getScenePtr()->removeKid(dirTrans);
}
	
void Omnicar::releaseBall() {
	this->fireTime = 0;
	this->stickyBall = false;
	if (ballJoint != 0) {
		dJointDestroy(ballJoint);
		ballJoint = 0;
	}
	this->charge = this->savedCharge;

	sgVec3 f;
	sgNormalizeVec3(f, this->bodyDir);
	sgScaleVec3(f, 100.0);
	this->ball.addForce(f);
}

void Omnicar::update(float dt) {

	sgVec3 normal;	// direction normal to the ideal car plane
	sgMat4 rot;
	// getting the normal from the ssg node transform
// 	this->carTrans[0]->getTransform(rot);
// 	sgCopyVec3(normal, rot[2]);
	
	

	dVector3 rotDir;
	dJointGetAMotorAxis(diram, 2, rotDir);
	normal[0] = (float) rotDir[0];
	normal[1] = (float) rotDir[1];
	normal[2] = (float) rotDir[2];
	sgNormalizeVec3(normal);
	
// 	if (steering != 0) {
// 		sgMakeRotMat4(rot, steering*dt, normal);
// 		sgXformVec3(direction, rot);
// 	}
	
	float steer = steering;
	// since the car is double face we want the normal always point upward
	if(sgScalarProductVec3(this->context.getGravity(), normal) > 0) {
// 		sgNegateVec3(normal);
// 		printf("Dio %f\n",dt);
		steer = -steering;
	}
// 	sgVec3 axis;
// 	sgVectorProductVec3(axis, normal, direction);
// 	sgNormalizeVec3(axis);
// 	
// 	sgVec3 normalDir;
// 	sgNormalizeVec3(normalDir, direction);
// 	
// 	float angle = 90 -180/M_PI*acos(sgScalarProductVec3(normalDir, normal));
// 	sgMakeRotMat4(rot, angle, axis);
// 	sgXformVec3(direction, rot);
	
	dVector3 currentDir;
	dJointGetAMotorAxis(diram, 0, currentDir);
	bodyDir[0] = (float) currentDir[0];
	bodyDir[1] = (float) currentDir[1];
	bodyDir[2] = (float) currentDir[2];
	sgNormalizeVec3(bodyDir);
	
// 	float bodyAngle = M_PI/2.0 - acos(sgScalarProductVec3(bodyDir, normalDir));
// 	float bodyAngularVel = bodyAngle/dt;
// 	
// 	printf("ANG %f\n", bodyAngle*180/M_PI);
// 	
// 	if (steering!=0) {
// 		bodyAngularVel -= steer;
// 	}
// 	dJointSetAMotorParam (diram, dParamVel3 ,   bodyAngularVel);
// 	dJointSetAMotorParam (diram, dParamFMax3,   20);
	
	sgMat4 trans, res;
	sgVec3 dirHpr;
	sgVec3 dirPos;
	const dReal* carPos;
	sgVec3 ballDistance, temp;
	const dReal* ballPos = dBodyGetPosition(this->ball.getBid());
	this->getPos(dirPos);
	sgScaleVec3(temp, bodyDir, dirDim[1] + this->ball.getSize()/2.0*1.1);
	
	sgAddVec3(dirPos, temp);
	ballDistance[0] = ballPos[0] - dirPos[0];
	ballDistance[1] = ballPos[1] - dirPos[1];
	ballDistance[2] = ballPos[2] - dirPos[2];
	
	
	if (stickyBall && possessionIncrement!=0)
		this->ball.setPossessionIncrement(possessionIncrement);
	
	if (!stickyBall)
		this->fireTime += dt;
	if (sgLengthVec3(ballDistance) < this->ball.getSize()/1.7 && !stickyBall && (this->fireTime > 1.0)) {
		this->context.releaseBall((IBallPicker*) this);
		this->savedCharge = this->charge;
		this->charge = 0;
		stickyBall = true;
		if (ballJoint != 0)
			dJointDestroy(ballJoint);
		ballJoint = dJointCreateBall (this->context.getWid(), 0);
// 		dJointSetFeedback (ballJoint, &jf);
		dJointAttach (ballJoint, dirBid, this->ball.getBid());
		dBodySetPosition(this->ball.getBid(), (double) dirPos[0], (double) dirPos[1], (double) dirPos[2]);
		dJointSetBallAnchor (ballJoint, (double) dirPos[0], (double) dirPos[1], (double) dirPos[2]);
	}
	if (fire) {
		this->fire = false;
		if (stickyBall)
			this->releaseBall();
		this->charge = -this->charge;
	}
	if (speed != 0) {
// 		sgNormalizeVec3(direction);
// 		sgScaleVec3(direction, speed);
		sgScaleVec3(bodyDir, speed);
	} 
	if (steering != 0) {
// 		sgMakeRotMat4(rot, steering*dt, normal);
// 		sgXformVec3(direction, rot);
		dJointSetAMotorParam (diram, dParamVel3 ,   -steer);
		dJointSetAMotorParam (diram, dParamFMax3,   5);
	} 
	else {
		dJointSetAMotorParam (diram, dParamVel3 ,   0);
		dJointSetAMotorParam (diram, dParamFMax3,   5);
	}
	
	sgVec3 jumpDir;
	if (jumping && (jumpForce > 0)) {
		sgNormalizeVec3(jumpDir, this->context.getGravity());
		sgVec3 jumpDirs;
// 		jumpForce -= 100*dt;
		for(int i = 0; i<nWheels; i++){
			sgScaleVec3(jumpDirs, jumpDir, -jumpForce*ODE_CAR_MASS);
			dBodyAddForce(this->carBid[i], jumpDirs[0], jumpDirs[1], jumpDirs[2]);
			sgScaleVec3(jumpDirs, jumpDir, -jumpForce*ODE_WHEEL_MASS);
			dBodyAddForce(this->wheelBid[i], jumpDirs[0], jumpDirs[1], jumpDirs[2]);
		}
		if (stickyBall){
			sgScaleVec3(jumpDirs, jumpDir, -jumpForce*ODE_BALL_MASS);
			dBodyAddForce(this->ball.getBid(), jumpDirs[0], jumpDirs[1], jumpDirs[2]);
		}
		sgScaleVec3(jumpDirs, jumpDir, -jumpForce*0.1);
		dBodyAddForce(this->dirBid, jumpDirs[0], jumpDirs[1], jumpDirs[2]);	
	} 
	else if (jumpForce < Omnicar::maxJumpForce) {
// 		jumpForce += 10*dt;
	}
	Entity::odeTrans2ssgTrans(this->dirBid, *(this->dirTrans));
	sgMat4 mtx;
	for (int i = 0; i < nWheels; i++) {
		Entity::odeTrans2ssgTrans(this->carBid[i],   *(this->carTrans[i]));
		Entity::odeTrans2ssgTrans(this->wheelBid[i], *(this->wheelTrans[i]));
		if (speed!=0){
			if (!backwards) 
				dBodyAddForce(this->carBid[i], bodyDir[0], bodyDir[1], bodyDir[2]);
			else
				dBodyAddForce(this->carBid[i], -bodyDir[0], -bodyDir[1], -bodyDir[2]);
// 			if (!backwards) 
// 				dBodyAddForce(this->carBid[i], direction[0], direction[1], direction[2]);
// 			else
// 				dBodyAddForce(this->carBid[i], -direction[0], -direction[1], -direction[2]);
			dJointSetAMotorParam (jam[i], dParamFMax, 0);
			dJointSetAMotorParam (jam[i], dParamFMax2, 0);	
			dJointSetAMotorParam (jam[i], dParamFMax3, 0);
		}
		else {
			dJointSetAMotorParam (jam[i], dParamVel,    0.01);
			dJointSetAMotorParam (jam[i], dParamFMax, 5);
			dJointSetAMotorParam (jam[i], dParamVel2,   0.01);
			dJointSetAMotorParam (jam[i], dParamFMax2, 5);
			dJointSetAMotorParam (jam[i], dParamVel3,   0.01);
			dJointSetAMotorParam (jam[i], dParamFMax3, 5);
		}
		
	}
	
/*	printf("angle:      %f\n", angle);
	printf("normal:    (%f) %f %f %f\n", sgLengthVec3(normal), normal[0], normal[1], normal[2]);
	printf("axis:      (%f) %f %f %f\n", sgLengthVec3(axis), axis[0], axis[1], axis[2]);
	printf("direction: (%f) %f %f %f\n", sgLengthVec3(direction), direction[0], direction[1], direction[2]);*/
}

void Omnicar::getPos(sgVec3 out)
{
	const dReal* carPos;
	sgZeroVec3(out);
	for (int i = 0; i<nWheels; i++){
		carPos = dBodyGetPosition(carBid[i]);
		out[0] += carPos[0];
		out[1] += carPos[1];
		out[2] += carPos[2];
	}
	sgScaleVec3(out, 1.0/((float) nWheels));
}

void Omnicar::traslateBody(dBodyID b, dVector3 diff) {
	const dReal * bodyPos = dBodyGetPosition(b);
	dBodySetLinearVel  (b, 0, 0, 0);
	dBodySetAngularVel (b, 0, 0, 0);
	dBodySetPosition(b, bodyPos[0]+diff[0], bodyPos[1]+diff[1], bodyPos[2]+diff[2]);
}

void Omnicar::setPos(double x, double y, double z)
{
	sgVec3 currPos;
	this->getPos(currPos);
	dVector3 diff;
	
	diff[0] = (dReal) x - currPos[0]; 
	diff[1] = (dReal) y - currPos[1];
	diff[2] = (dReal) z - currPos[2];
	
	for (int i = 0; i < nWheels; i++){
		Omnicar::traslateBody(carBid[i], diff);
		Omnicar::traslateBody(wheelBid[i], diff);
		
	}
	Omnicar::traslateBody(dirBid, diff);
// 	Omnicar::traslateBody(rejBid, diff);
	
// 	for (int i=0; i<2; i++)
// 		Omnicar::traslateBody(borBid[i], diff);
	
}

void Omnicar::getMagnetDir(sgVec3 out) {
	sgNormalizeVec3(out, bodyDir);
}

void Omnicar::addForce(sgVec3 force) {
	for (int i = 0; i < nWheels; i++)
		dBodyAddForce(this->carBid[i], force[0], force[1], force[2]);
}

void Omnicar::getMagnetPos(sgVec3 out)
{
	sgVec3 ndir;
	this->getPos(out);
// 	sgNormalizeVec3(ndir, direction);
	sgNormalizeVec3(ndir, bodyDir);
	sgScaleVec3(ndir, dirDim[1] + this->ball.getSize()/2.0);
	sgAddVec3(out, ndir);
}

void Omnicar::move(Entity::Move m)
{
	switch (m) {
		case GO_FWRD:
			speed = 100;
			backwards = false;
			break;
		case GO_BWRD:
			speed = 120;
			backwards = true;
			break;
		case TURN_LEFT:
			steering = 5;
			break;
		case TURN_RIGHT:
			steering = -5;
			break;
		case FIRE:
			this->fire = true;
			this->context.sound.play(Sound::FIRE);
			break;
		case JUMP:
			printf("boing!\n");
// 			this->context.sound.play(Sound::FIRE);
			jumping = true;
			break;
		case GO_STOP:
			backwards = false;
			speed = 0;
			break;
		case TURN_STOP:
			steering = 0;
			break;
		case JUMP_STOP:
			printf("being!\n");
			jumping = false;
			break;
		default:
			printf("Warning: unimplemented movement in %s.", this->toString());
			break;
	}
}


};
