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

namespace gillo {

Entity::Entity(Context& c)
: context(c), shifting(false), trans(*(new ssgTransform())), alphaTrans(*(new ssgTransform())),charge(Entity::defaultCharge), timeOfRemoval(0.0), stickyBall(false)
{
	type = DEFAULT;
	bid = 0;
	gid = 0;
}

void Entity::Init()
{
	if (!dGeomIsSpace(gid))
		dGeomSetBody (gid, bid);
	//@todo set data in body if useful
	dGeomSetData(gid, this);
	context.getScenePtr()->addKid(&trans);
	context.getAlphaScenePtr()->addKid(&alphaTrans);
}

Entity::~Entity()
{
	printf("Deleted Entity: %s\n", this->toString());
	dGeomDestroy (gid);
	if (bid)
		dBodyDestroy (bid);
	//@todo ssg destroy code - not needed if in pool
	context.getScenePtr()->removeKid(&trans);
	context.getAlphaScenePtr()->removeKid(&alphaTrans);
}

void Entity::update(float dt)
{
	if (shifting)
		dBodyAddForce(this->bid, shiftForce[0], shiftForce[1], shiftForce[2]);
}

void Entity::move(Move m)
{
	switch (m) {
		case GO_FWRD:
			this->startShifting(0, 100, 0);
			break;
		case GO_BWRD:
			this->startShifting(0, -100, 0);
			break;
		case TURN_LEFT:
			this->startShifting(100, 0, 0);
			break;
		case TURN_RIGHT:
			this->startShifting(-100, 0, 0);
			break;
		case FIRE:
			this->charge = -this->charge;
			printf("bang!\n");
			break;
		case JUMP:
			this->startShifting(0, 0, 1000);
			break;
		case GO_STOP:
		case TURN_STOP:
		case JUMP_STOP:
			this->stopShifting();
			break;
		default:
			printf("Warning: unimplemented movement in %s.", this->toString());
			break;
	}

}

void Entity::getPos(sgVec3 out)
{
	const dReal* position = dBodyGetPosition(this->bid);
	out[0] = (float) position[0];
	out[1] = (float) position[1];
	out[2] = (float) position[2];
}


void Entity::setPos(double x, double y, double z)
{
	dBodySetLinearVel  (this->bid, 0, 0, 0);
	dBodySetAngularVel (this->bid, 0, 0, 0);
	dBodySetPosition(this->bid, x, y, z);
}

void Entity::setVel(double x, double y, double z)
{
	dBodySetLinearVel(this->bid, x, y, z);
}

void Entity::getMagnetPos(sgVec3 out)
{
	getPos(out);
}

void Entity::getMagnetDir(sgVec3 out)
{
	// omnidirectional by default
	out[0] = 0;
	out[1] = 0;
	out[2] = 0;
}

void Entity::addForce(sgVec3 force)
{
	if (bid)
		dBodyAddForce(bid, force[0], force[1], force[2]);
}

void Entity::startShifting(double x, double y, double z)
{
	this->shiftForce[0] = x;
	this->shiftForce[1] = y;
	this->shiftForce[2] = z;
	this->shifting = true;
}

void Entity::stopShifting()
{
	this->shifting = false;
}

char* Entity::toString() {
	snprintf(string, 255, "type %d, ptr %p", type, this); //@fixme
	return this->string;
}

void Entity::getUpVector(sgVec3 out)
{
	sgNegateVec3(out, this->context.getGravity());
	sgNormalizeVec3(out);
}

void Entity::odeTrans2ssgTrans(const dBodyID bid, ssgTransform& sgt) {
	sgMat4 rotm, trasm, totm;
	const dReal* odepos;
	const dReal* oderot;
	
	/* read ode transformation */
	odepos   = dBodyGetPosition(bid); 
	oderot = dBodyGetRotation(bid);
	
	/* write sg transformation */
	sgMakeTransMat4 (trasm, (float) odepos[0], (float)  odepos[1], (float) odepos[2]) ; 
	sgMakeIdentMat4(rotm);
	for (int i = 0; i < 3; i++) {
		for (int j = 0; j < 3; j++) {
			rotm[j][i] = (float) oderot[i*4+j]; // it's a kind of magic! oh oh!
		}
	}
	sgMultMat4 (totm, trasm, rotm);
	
	/* apply transformation */
	sgt.setTransform ( totm ) ;

}

void Entity::ssgRot2odeRot(const dGeomID gid, ssgTransform& sgt) {
	sgMat4 trans;
	dMatrix3 rot;

	sgt.getTransform(trans);
	for (int i=0; i<3; i++)
		for (int j=0; j<3; j++) {
			rot[i*4+j] = (double) trans[j][i];
		}
		
	dGeomSetRotation(gid, rot);
}

void Entity::transform(ssgTransform* t, float angle, float scale)
{
	sgMat4 rotm, scalm;
	scalm[0][1] = 0;
	scalm[0][2] = 0;
	scalm[0][3] = 0;
	scalm[1][0] = 0;
	scalm[1][2] = 0;
	scalm[1][3] = 0;
	scalm[2][0] = 0;
	scalm[2][1] = 0;
	scalm[2][3] = 0;
	scalm[3][0] = 0;
	scalm[3][1] = 0;
	scalm[3][2] = 0;
	scalm[0][0] = scale;
	scalm[1][1] = scale;
	scalm[2][2] = scale;
	scalm[3][3] = 1;
	if (angle != 0) {
		const sgVec3 axis = { 0, 1, 0 };
		sgMakeRotMat4(rotm, angle, axis);
		sgPreMultMat4 (scalm, rotm);
	}
	t->setTransform(scalm);
}
};
