/***************************************************************************
 *   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 "car.h"

namespace gillo {

Car::Car(Context& c)
 : Entity(c)
{
	sgVec3 center = { 0,0,0 };
	sgVec3 dim = { ODE_CAR_DIMX, ODE_CAR_DIMY, ODE_CAR_DIMZ };
	dMass mass;

	type = CAR;
	
	dSpaceID sid = dSimpleSpaceCreate(c.getSid());
	gid = (dGeomID) sid;
	
	
	// creating car core
	carbid = dBodyCreate(c.getWid());
	bid = carbid;
	Entity::Init();
	
	dGeomID cargid = dCreateBox (sid, dim[0], dim[1], dim[2]);
	dGeomSetBody (cargid, carbid);
	dGeomSetData (cargid, this);
	
	
	ssgEntity* e = c.searchInPool("CarBody1");
	if (e == NULL) {
		e = ssgLoadAC( "car_1.ac" ) ;
		ssgFlatten ( e ) ;
		ssgStripify( e ) ;
		e -> setName          ("CarBody1") ;
		c.addToPool(e);
	}
	trans.addKid( e );

	e = NULL;
	e = c.searchInPool("CarWheel1");
	if (e == NULL) {	
		e = ssgLoadAC( "wheel.ac" );
		ssgFlatten(e);
		ssgStripify(e);
		e -> setName          ("CarWheel1") ;
		c.addToPool(e);
	}
	ssgEntity* wheel_obj = e;
	
	dGeomSetPosition (cargid, (dReal) center[0], (dReal) center[1], (dReal) center[2]);
	dBodySetPosition(carbid, center[0], center[1], center[2]);
	
	dMassSetBoxTotal (&mass, ODE_CAR_MASS, dim[0], dim[1], dim[2]);
	dBodySetMass(carbid, &mass);
	
	
	//creating car wheels
	//@fixme make a separate WHEEL entity?
	//dReal offset[nwheels] = { -0.69, 0.69 };
	dReal offset[nwheels] = { -(ODE_CAR_DIMX+ODE_WHEEL_LENGTH)/2, (ODE_CAR_DIMX+ODE_WHEEL_LENGTH)/2 };
	dMatrix3 R;
	dRFromAxisAndAngle (R, 0.0, 1.0, 0.0, M_PI/2);

	jid = dJointGroupCreate(0);

	//--------------------------------------------------------------------------------------------------------
/*	ssgaSphere * e_obj =  new ssgaSphere     () ;
	e_obj -> setSize          ( 2*ODE_WHEEL_RADIUS*0.8 ) ;
	e_obj -> setCenter        ( center ) ;
	e_obj -> regenerate       () ;
	//--------------------------------------------------------------------------------------------------------
	tamtrans = new ssgTransform();
	c.getScenePtr()->addKid(tamtrans);
	tamtrans->addKid( e_obj );
	dGeomID tampax = dCreateSphere(sid, ODE_WHEEL_RADIUS*0.9);
	tambody = dBodyCreate(c.getWid());
	dMassSetSphereTotal(&mass, ODE_WHEEL_MASS*0.1, ODE_WHEEL_RADIUS*0.9);
	dBodySetMass(tambody, &mass);
	dGeomSetBody(tampax, tambody);
	dBodySetPosition (tambody, (dReal) center[0], (dReal) center[1]+ODE_CAR_DIMY/2, (dReal) center[2]);
	dGeomSetPosition (tampax , (dReal) center[0], (dReal) center[1]+ODE_CAR_DIMY/2, (dReal) center[2]);
	dGeomSetData(tampax, this);
	dJointID tamjoe = dJointCreateBall (c.getWid(), jid);
	dJointAttach(tamjoe, carbid, tambody);
	dJointSetBallAnchor (tamjoe, (dReal) center[0], (dReal) center[1]+ODE_CAR_DIMY/2, (dReal) center[2]);
	//--------------------------------------------------------------------------------------------------------
	tamtrans2 = new ssgTransform();
	c.getScenePtr()->addKid(tamtrans2);
	tamtrans2->addKid( e_obj );
	dGeomID tampax2 = dCreateSphere(sid, ODE_WHEEL_RADIUS*0.9);
	tambody2 = dBodyCreate(c.getWid());
	dMassSetSphereTotal(&mass, ODE_WHEEL_MASS*0.1, ODE_WHEEL_RADIUS*0.9);
	dBodySetMass(tambody2, &mass);
	dGeomSetBody(tampax2, tambody2);
	dBodySetPosition (tambody2, (dReal) center[0], (dReal) center[1]-ODE_CAR_DIMY/2, (dReal) center[2]);
	dGeomSetPosition (tampax2 , (dReal) center[0], (dReal) center[1]-ODE_CAR_DIMY/2, (dReal) center[2]);
	dGeomSetData(tampax2, this);
	dJointID tamjoe2 = dJointCreateBall (c.getWid(), jid);
	dJointAttach(tamjoe2, carbid, tambody2);
	dJointSetBallAnchor (tamjoe2, (dReal) center[0], (dReal) center[1]-ODE_CAR_DIMY/2, (dReal) center[2]);*/
	//--------------------------------------------------------------------------------------------------------

/*	e.setPos(center[0], center[1]+ODE_CAR_DIMY/2, center[2]);
	dJointID tamjoe = dJointCreateBall (c.getWid(), jid);
	dJointAttach(tamjoe, carbid, e.getBid());
	dJointSetBallAnchor (tamjoe, (dReal) center[0], (dReal) center[1]+ODE_CAR_DIMY/2, (dReal) center[2]);*/
	
	dMassSetCappedCylinderTotal (&mass, ODE_WHEEL_MASS, 3, ODE_WHEEL_RADIUS, ODE_WHEEL_LENGTH);
	
	for (int i=0; i<nwheels; i++) {
		
		wheel_trans[i] = new ssgTransform();	
		c.getScenePtr()->addKid(wheel_trans[i]);
		wheel_trans[i]->addKid(wheel_obj);
		
		wheelbids[i] = dBodyCreate(c.getWid());
		wheelgids[i] = dCreateCCylinder (sid, ODE_WHEEL_RADIUS, ODE_WHEEL_LENGTH);
		
		
		dBodySetMass(wheelbids[i], &mass);
		
		dBodySetRotation(wheelbids[i], R);
		dBodySetPosition (wheelbids[i], (dReal) center[0] + offset[i], (dReal) center[1], (dReal) center[2]);
		dGeomSetRotation(wheelgids[i], R);
		dGeomSetPosition (wheelgids[i], (dReal) center[0] + offset[i], (dReal) center[1], (dReal) center[2]);
		dGeomSetBody (wheelgids[i], wheelbids[i]);
		dGeomSetData (wheelgids[i], this);
		
		//creating and attaching joints...
		dJointID joe = dJointCreateBall (c.getWid(), jid);
		jam[i] = dJointCreateAMotor (c.getWid(), jid);
		dJointAttach (joe, carbid, wheelbids[i]);
		dJointAttach (jam[i], carbid, wheelbids[i]);
		
		//setting up joints...
		//@todo save somewhere joint ids
		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);
		dJointSetAMotorAngle (jam[i], 1, 0);
		dJointSetAMotorAngle (jam[i], 2, 0);
		dJointSetAMotorParam (jam[i], dParamLoStop2, 0);
		dJointSetAMotorParam (jam[i], dParamHiStop2, 0);
		dJointSetAMotorParam (jam[i], dParamLoStop3, 0);
		dJointSetAMotorParam (jam[i], dParamHiStop3, 0);
		dJointSetBallAnchor (joe, (dReal) center[0] + offset[i], (dReal) center[1], (dReal) center[2]);
	}
	
	
	
}

Car::~Car()
{
	dJointGroupEmpty (jid);
	for (int i=0; i<nwheels; i++) {
		dBodyDestroy(wheelbids[i]);
		dGeomDestroy(wheelgids[i]);
	}
	dBodyDestroy(carbid);
}

void Car::update(float dt) {
// 	Entity::update(dt);
	//@todo control the pitching of car core
	Entity::odeTrans2ssgTrans(this->carbid,   this->trans);
// 	Entity::odeTrans2ssgTrans(this->tambody,  *(this->tamtrans));
// 	Entity::odeTrans2ssgTrans(this->tambody2, *(this->tamtrans2));
	for (int k = 0; k < nwheels; k++)
		Entity::odeTrans2ssgTrans(this->wheelbids[k], *(this->wheel_trans[k]));
}

void Car::move(Entity::Move m)
{
	static float   vel[2] = { 0.0, 0.0 };
	static float  gvel[2] = { 0.0, 0.0 };
	static float  tvel[2] = { 0.0, 0.0 };
	static float force[2] = { 100.0, 100.0 };
	
	switch (m) {
		case GO_FWRD:
			for (int i =0; i < nwheels; i++) {
				gvel[i]  =  10.0;
			}
			break;
		case GO_BWRD:
			for (int i =0; i < nwheels; i++) {
				gvel[i]  = -10.0;
			}
			break;
		case TURN_LEFT:
			tvel[0]  = -5.0;
			tvel[1]  =  5.0;
			break;
		case TURN_RIGHT:
			tvel[0]  =  5.0;
			tvel[1]  = -5.0;
			break;
		case FIRE:
// 			this->charge = -this->charge;
			printf("bang!\n");
			break;
		case JUMP:
// 			this->startShifting(0, 0, 1000);
			break;
		case GO_STOP:
			for (int i =0; i < nwheels; i++) {
				gvel[i] = 0.0;
			}
			break;
		case TURN_STOP:
			for (int i =0; i < nwheels; i++) {
				tvel[i] = 0.0;
			}
			break;
		case JUMP_STOP:
// 			this->stopShifting();
			break;
		default:
			printf("Warning: unimplemented movement in %s.", this->toString());
			break;
	}
		
	for (int i =0; i < nwheels; i++) {
		vel[i] = gvel[i] + tvel[i];
		dJointSetAMotorParam (jam[i], dParamVel,    vel[i]);
		dJointSetAMotorParam (jam[i], dParamFMax, force[i]);
	}

}

};
