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

namespace gillo {

const double Goalkeeper::weight = 10;
const double Goalkeeper::radius = 0.5;
const double Goalkeeper::operativeRadius = ODE_GOAL_DISTANCE/2*1.5;
const double Goalkeeper::aimingRadius = operativeRadius * 4;
const double Goalkeeper::posCorrIntensity = 1000;
const double Goalkeeper::rotCorrIntensity = 300;
const float  Goalkeeper::maxMagnetIntensity = 500;


Goalkeeper::Goalkeeper(Context& c, Goal& g, SimpleBall& b)
 : Entity(c), goal(g), ball(b)
{
	dMass	mass;
	sgVec3 dim = { 2.2*radius, 0, 2.2*radius };
	sgVec3 center = { 0, 0, 0};

	type = GOALKEEPER;
	bid = dBodyCreate(c.getWid());
	gid = dCreateSphere (c.getSid(), radius);
	Entity::Init();
	
	ssgEntity* e = c.searchInPool("Goalkeeper");
	if (e == NULL) {
		ssgaSphere* e_obj =  new ssgaSphere     () ;
		e_obj -> setSize          ( 2*radius ) ;
		e_obj -> setCenter        ( center ) ;
		e_obj -> setKidState      ( c.getState(STA_GOALKEEPER) ) ;
		e_obj -> regenerate       () ;
		e_obj -> setName          ("Goalkeeper") ;
		e = (ssgEntity*) e_obj;
		c.addToPool(e);
	}
	trans.addKid( e );
	
// 	e = c.searchInPool("GoalkeeperFur");
// 	if (e == NULL) {
// 		ssgaCube * e_obj =  new ssgaCube     () ;
// 		e_obj -> setSize          ( dim ) ;
// 		e_obj -> setCenter        ( center ) ;
// 		e_obj -> setKidState      ( c.getState(STA_GOALKEEPERFUR) ) ;
// 		e_obj -> regenerate       () ;
// 		e_obj->setName("GoalkeeperFur");
// 		e = (ssgEntity*) e_obj;
// 		c.addToPool(e);
// 	}
// 	ssgCutout* co = new ssgCutout(true);
// 	ssgTransform* tr0 = new ssgTransform();
// 	ssgTransform* tr1 = new ssgTransform();
// 	tr0->addKid(e);
// 	tr1->addKid(e);
// 	co->addKid(tr0);
// 	co->addKid(tr1);
// 	alphaTrans.addKid( co );

	dMassSetSphereTotal(&mass, weight, radius);
// 	dMassTranslate (&mass, 0, 0, 2*radius);
	dBodySetMass(bid, &mass);
	dBodySetPosition(bid, 0, 0, 0);
	
	jid = dJointGroupCreate(0);	
	jam = dJointCreateAMotor (c.getWid(), jid);
	dJointAttach (jam, this->bid, 0);
	dJointSetAMotorMode (jam, dAMotorEuler);
	dJointSetAMotorNumAxes (jam, 3);
	dJointSetAMotorAxis (jam, 0, 2, 0.0, 1.0, 0.0);
	dJointSetAMotorAxis (jam, 2, 1, 0.0, 0.0, 1.0);
	dJointSetAMotorParam (jam, dParamLoStop, 0);
	dJointSetAMotorParam (jam, dParamHiStop, 0);
	dJointSetAMotorParam (jam, dParamLoStop2, 0);
	dJointSetAMotorParam (jam, dParamHiStop2, 0);
}


Goalkeeper::~Goalkeeper()
{
}

void Goalkeeper::update(float dt) {
	sgVec3 myPos, ballPos, diff, normGravity;
	float currentDistance, distance, ballDistance;
	sgMat4 rot;
	
	// roll control
// 	this->trans.getTransform(rot);
// 	sgNegateVec3(rot[2]);
// 	sgNormalizeVec3(normGravity, context.getGravity());
// 	sgSubVec3(diff, normGravity, rot[2]);
// 	distance = sgLengthVec3(diff);
// 	if (distance > 0) {
// 		sgScaleVec3(diff, rotCorrIntensity);
// 		dBodyAddForceAtRelPos(this->bid, diff[0], diff[1], diff[2], 0,0,-radius);
// 	}

	
	
	this->getPos(myPos);
	currentDistance = -dGeomCCylinderPointDepth (goal.getAreaGeom(), (dReal) myPos[0], (dReal) myPos[1], (dReal) myPos[2]) + goal.getAreaRadius();

	ball.getPos(ballPos);
	sgSubVec3(diff, ballPos, myPos);
	
	// scope limits
	ballDistance = -dGeomCCylinderPointDepth (goal.getAreaGeom(), (dReal) ballPos[0], (dReal) ballPos[1], (dReal) ballPos[2]) + goal.getAreaRadius();
	
	if (currentDistance > operativeRadius || ballDistance > operativeRadius ){
		if (ballDistance < aimingRadius) {
			// track Z position...
			myPos[2] = -diff[2];
		}
		sgScaleVec3(myPos, posCorrIntensity/operativeRadius);
		dBodyAddForce(this->bid, -myPos[0], -myPos[1], -myPos[2]);
		if (ballDistance > operativeRadius) {
			const dReal * myVel = dBodyGetLinearVel(this->bid);
			dBodySetLinearVel(this->bid, myVel[0]*0.8, myVel[1]*0.8, myVel[2]*0.8);
		}
		this->charge = 0;
// 		printf("CD: %f BD: %f (%f)\n", currentDistance, ballDistance, currentDistance*(posCorrIntensity/operativeRadius));
	}
	else {
		this->charge = 10000;
	}
		
	// catch the ball
	float l = sgLengthVec3(diff);
	sgNormalizeVec3(diff);
	float intensity = this->charge*10/l/l;
	if (sgAbs(intensity) > maxMagnetIntensity)
		intensity = intensity > 0 ? maxMagnetIntensity : -maxMagnetIntensity;
	sgScaleVec3(diff, intensity);
	dBodyAddForce(this->bid, diff[0]*weight, diff[1]*weight, diff[2]*weight);
	
// 	if (ballDistance < goal.getAreaRadius()*0.8) {
// 		sgNegateVec3(diff);
// 		sgScaleVec3(diff, ODE_BALL_MASS);
// 		ball.addForce(diff);
// 	}
	
	// update roto-translations
// 	static float i = 0, angle = 0;
// 	angle += 2*dt;
// 	i += dt;
// 	Entity::transform(((ssgTransform*) ((ssgCutout*) alphaTrans.getKid(0))->getKid(0)),      angle, 0.05*cos(i/4)+0.95);
// 	Entity::transform(((ssgTransform*) ((ssgCutout*) alphaTrans.getKid(0))->getKid(1)), -0.2*angle, 0.15*cos(i/8)+0.9);

	Entity::odeTrans2ssgTrans(this->bid, this->trans);
	sgMat4 xform;
	trans.getTransform(xform);
	alphaTrans.setTransform(xform);
}

void Goalkeeper::move(Move m) {
	// should not be controlled
	return;
}


};
