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

namespace gillo {

CameraTrack::CameraTrack(Context& w, vector<Entity*> * es)
 : es(es), world(w)
{
}

void CameraTrack::getTrackEntities(vector<Entity*> *es) {
	vector<Entity*>::iterator i = es->begin();
	/* collect balls and cars */
	while (i != es->end()  ) {
		if (((*i)->getType() == Entity::BALL) || ((*i)->getType() == Entity::CAR) || ((*i)->getType() == Entity::CUBE) || ((*i)->getType() == Entity::GOALKEEPER))
			this->t.push_back(*i);
		i++;
	}
}

CameraTrack::~CameraTrack()
{
	delete variance;
	delete ctrl;
}

void CameraTrack::update(float dt)
{
	float control=0;
	static float distance=10;
	sgVec3 meanpos;
	sgVec3 ncampos, nballpos, pn, direction;
	static sgVec3 center;

	sgCopyVec3(pn, world.getGravity());
	sgVec3 axisY = {0, 1, 0};
	sgMat4 rot;
	sgMakeRotMat4(rot, CameraTrack::angleY, axisY);
	sgXformVec3(direction, pn, rot);
	sgVec3 axis;
	sgVectorProductVec3(axis, direction, axisY);
	sgMakeRotMat4(rot, CameraTrack::angleX, axis);
	sgXformVec3(direction, rot);
// 	printf("camera direction: %f, %f, %f\n", direction[0], direction[1], direction[2]); //@fixme
	sgNormalizeVec3(direction);
	sgCopyVec3(ncampos,direction);
	sgHPRfromVec3 ( hpr, direction );
	hpr[1] = -hpr[1]; // plib bug ???? my brain bug ????
	sgNegateVec3(direction);
	
	if (t.size() == 0) {
		getTrackEntities(es);
		if (!t.size()) return;
		ctrl = new float[t.size()];
		variance = new float[t.size()];
		sgZeroVec3(center);
		distance = 10;
	}
	
	float measure;
	
	sgZeroVec3(meanpos);
	
	for(int i=0; i<t.size(); i++) {
		t[i]->getPos(nballpos);
		if (world.isDebug()) {
			printf("NANPOS: %d %f\n", t[i]->getType(), nballpos[0]);
		}
		sgAddVec3(meanpos, nballpos);
		sgSubVec3(nballpos, this->pos.xyz);
		float mod = sgLengthVec3(nballpos);
		sgNormalizeVec3(nballpos);
		measure = CameraTrack::measureMultiplier*acos(sgScalarProductVec3(ncampos, nballpos))*180/M_PI;
		ctrl[i] = (CameraTrack::reference-measure) * CameraTrack::vel;
		control += ctrl[i];
		if (mod<1){
			mod = cos(mod/2*M_PI + M_PI*3/2);
			ctrl[i] *= mod;
		}
	}
	sgScaleVec3(meanpos, (float)1/t.size());
	
	float mean = control / t.size();
	float maxvariance = 0;
	int most_variant = 0;
	for(int i=0; i<t.size(); i++) {
		variance[i] = fabs(ctrl[i]-mean);
		if(variance[i]>maxvariance){
			maxvariance = variance[i];
			most_variant = i;
		}
	}

	control = 0;
	for(int i=0; i<t.size(); i++) {
		ctrl[i] *= variance[i]/variance[most_variant] * CameraTrack::emph;
		control += ctrl[i];
	}
	control /= t.size();
	
	sgVec3 amount, p;
	
	sgSubVec3(meanpos, center);
	sgScaleVec3(meanpos, 0.1);
	sgAddVec3(center, meanpos);
	
	float den = sgScalarProductVec3(pn, direction);
	assert(den < 0 );
	
	sgScaleVec3(amount, direction, sgScalarProductVec3(pn, center)/den);
	sgSubVec3(p, center, amount);
 	
	distance -= control;

	sgScaleVec3(amount, direction, distance);
	sgAddVec3(amount, p);
	sgSetCoord ( &this->pos, amount, hpr ) ;
}

};
