/*
   orxonox - the future of 3D-vertical-scrollers

   Copyright (C) 2004 orx

   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, or (at your option)
   any later version.

   ### File Specific:
   main-programmer: Thomas Fahrni
   co-programmer:
*/
#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_AI

#include "movement_module.h"
#include "ai_engine.h"
#include "state.h"
#include "debug.h"
#include "player.h"
#include "playable.h"
#include "aabb.h"
#include "npcs/npc_test.h"

#include "shell_command.h"
// SHELL_COMMAND(setDistanceToPlayer, MovementModule, setDistanceToPlayer);
// SHELL_COMMAND(setDistanceToNPC, MovementModule, setDistanceToNPC);
// SHELL_COMMAND(setMaxAccleartion, MovementModule, setMaxAccleartion);
//
// float MovementModule::distanceToPlayer=15;
// float MovementModule::distanceToNPC=2;
// float MovementModule::maxAccleration=200.0f;
//
// void MovementModule::setDistanceToPlayer(float newValue){ distanceToPlayer=newValue; }
// void MovementModule::setDistanceToNPC(float newValue){ distanceToNPC=newValue; }
// void MovementModule::setMaxAccleartion(float newValue){ maxAccleration=newValue; }
//
//
//
//
//
//
// std::vector<Vector>	MovementModule::hidingPoint;
// std::vector<float>	MovementModule::hidingPointSize;
//
// std::vector<NPC2*> MovementModule::npcList;
// std::vector<Vector> MovementModule::npcPosition;
// std::vector<float> MovementModule::npcRadius;
// std::vector<int> MovementModule::npcSwarm;
// std::vector<int> MovementModule::npcTeam;
//
// Vector MovementModule::playerPosition;
// Vector MovementModule::playerMovement;
// float MovementModule::playerRadius;
//
// std::vector<Vector> MovementModule::swarmCenter;
// std::vector<int> MovementModule::swarmMemberCount;
//
//
//
//
//
MovementModule::MovementModule(NPC2* object){ this->myNPC=object; }
MovementModule::~MovementModule(){}




float MovementModule::getRadius(WorldEntity* object)
{
	AABB* aabb = object->getModelAABB();
	if( aabb == NULL)return -1;

	float a = aabb->halfLength[0];
	float b = aabb->halfLength[1];
	float c = aabb->halfLength[2];

	if(a>b){
		return (c>a)?c:a;
	}else{
		return (c>b)?c:b;
	}
}




// void MovementModule::collectInformation(float dt)
// {
// 	//return if already processed..
// 	if(dt==this->oldDT)return;
// 	this->oldDT=dt;
//
//
// 	//clear old Information..
// 	hidingPoint.clear();
// 	hidingPointSize.clear();
//
// 	npcList.clear();
// 	npcPosition.clear();
// 	npcRadius.clear();
// 	npcSwarm.clear();
// 	npcTeam.clear();
//
// 	swarmCenter.clear();
// 	swarmMemberCount.clear();
//
//
// 	//get all NPCs..
// 	for(ObjectList<NPC2>::const_iterator it = NPC2::objectList().begin(); it != NPC2::objectList().end(); ++it)
// 	{
// 		npcList.push_back(*it);
// 		npcRadius.push_back(getRadius(*it));
// 		npcPosition.push_back((*it)->getAbsCoor());
// 		npcSwarm.push_back((*it)->swarm);
// 		npcTeam.push_back((*it)->team);
// 	}
//
//
// 	//Swarm Information
// 	unsigned int tmpSwarm;
// 	for(unsigned int i=0;i<npcList.size();i++){
// 		tmpSwarm = npcSwarm.at(i);
// 		if(tmpSwarm > swarmMemberCount.size()){
// 			//swarmMemberCount.insert(swarmMemberCount.size(), tmpSwarm - swarmMemberCount.size(), 0);
// 			//swarmCenter.insert(swarmCenter.size(), tmpSwarm - swarmCenter.size(), Vector(0,0,0));
// 		}
// 		//swarmMemberCount.at(tmpSwarm)++;
// 		//swarmCenter.at(tmpSwarm)=swarmCenter.at(tmpSwarm)+npcPosition.at(i);
// 	}
// 	for(unsigned int i=0;i<swarmCenter.size();i++){
// 		//swarmCenter.at(i)=swarmCenter.at(i)/swarmMemberCount.at(i);
// 	}
//
//
// 	//get information about Player
// 	Player* pl = State::getPlayer();
// 	if( pl != NULL){
// 		playerPosition = pl->getPlayable()->getAbsCoor();
// 		playerRadius=getRadius( pl->getPlayable() );
// 		//PRINTF(0)("Player Radius: %f\n",playerRadius);
// 	}
//
//
// 	//calculate hiding Points..
//
// }


void MovementModule::process(float dt)
{
	if(myNPC == NULL)return;

	//get information about Player
	Player* pl = State::getPlayer();
	if( pl == NULL)return;

	Vector playerPosition = pl->getPlayable()->getAbsCoor();
	float playerRadius=getRadius( pl->getPlayable() );


// collectInformation(dt);

	Vector myPosition = myNPC->getAbsCoor();
	float myRadius = getRadius(myNPC);


	Vector vectorToPlayer = playerPosition - myPosition;

	Vector tmpVector;
	float tmpFloat;

// 	Vector npcCollision;
// 	Vector swarmVector;
	Vector playerCollision;



	//float a=200.0f;
	float vMax=200.0f;
	float maxAccleration=300.0f;
	//float safetyDistance=2.0f;


	//Anti Player Collision
	tmpFloat=vectorToPlayer.len()-playerRadius-myRadius-30;//distanceToPlayer;
	if(tmpFloat<0.1)tmpFloat=0.1;
	playerCollision=vectorToPlayer/(tmpFloat*tmpFloat)*(-1);



// 	for (unsigned int i=0;i<npcList.size();i++)
// 	{
// 		if(npcList.at(i)==myNPC)continue;
//
// 		//Anti NPC Collision
// 		tmpVector=myPosition-npcPosition.at(i);
// 		tmpFloat=tmpVector.len()-myRadius-npcRadius.at(i)-distanceToNPC;
//
// 		if(tmpFloat<0.1)tmpFloat=0.1;
// 		tmpVector=tmpVector/(tmpFloat*tmpFloat);
//
// 		npcCollision=npcCollision+tmpVector;
//
// 		//Schwarmverhalten
// 		if(npcSwarm.at(i) == mySwarm){
//
//
// 		}
// 	}





	//Vector correction=playerCollision*50+npcCollision*50+vectorToPlayer+Vector(70,0,0)-myMovement;
	Vector correction=playerCollision*50+vectorToPlayer+Vector(50,0,0)-myMovement;

	correction.y=0;
	float correctionLen=correction.len();
	if(correctionLen>maxAccleration*dt)correction=correction/correctionLen*maxAccleration*dt;
	myMovement+=correction;

	float movementLen=myMovement.len();
	if(movementLen>vMax)myMovement/movementLen*vMax;

	//Move NPC...
	myNPC->shiftCoor(myMovement * dt);

	//Rotate NPC
	Vector view = myMovement;
	//if(vectorToPlayer.dot(v)<0){
	//	view = v.cross( Vector(0,1,0) ).getNormalized();
	//}else{
	view = myMovement.cross( Vector(0,-1,0) ).getNormalized();
	//}
	//if(dist<keepDist)view=view*-1;
	//myNPC->setAbsDir( Quaternion( view, Vector(0,1,0)));
	myNPC->setAbsDirSoft( Quaternion( view, Vector(0,1,0)),3);
}