/*
   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()
{
	this->process(AIEngine::getInstance()->dtS);
}


void MovementModule::process(float dt)
{
	collectInformation(dt);
	
	if(myNPC == NULL)return;
	
	Vector myPosition = myNPC->getAbsCoor();
	float myRadius = getRadius(myNPC);
	int mySwarm = myNPC->swarm;
	int myTeam = myNPC->team;
	
	Vector vectorToPlayer = playerPosition - myPosition;
	
	Vector tmpVector;
	float tmpFloat;
	
	Vector npcCollision;
	Vector swarmVector;
	Vector playerCollision;
	


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


	//Anti Player Collision
	tmpFloat=vectorToPlayer.len()-playerRadius-myRadius-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-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);
}