/*
   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(aiacc, MovementModule, setAccleration);


//class AIEngine;

MovementModule::MovementModule(){
	//MovementModule::aa=100.0f;
	//a=100.0f;
	//std::cout << "MovementModule created...\n";
}

MovementModule::~MovementModule(){
}


void MovementModule::setAccleration(float newValue)
{
	std::cout << "Setting a to: "<< newValue << "\n";
	//MovementModule::aa=newValue;
}




float MovementModule::getSize(WorldEntity* object)
{
	//does not work...
	AABB* aabb = object->getModelAABB();
	Vector a = aabb->getAxisX();
	Vector b = aabb->getAxisY();
	Vector c = aabb->getAxisZ();

	float da=a.len();
	float db=b.len();
	float dc=c.len();
	float size;

	if(da>db){
		size=(dc>da)?dc:da;
	}else{
		size=(dc>db)?dc:db;
	}

	return size;
}




void MovementModule::process()
{
	float dt=AIEngine::getInstance()->dtS;
	if( owner == NULL) return;
	Vector myAbsPos = owner->getAbsCoor();

	//PRINTF(0)(" NPC abs coor: %f, %f, %f\n", myAbsPos.x, myAbsPos.y, myAbsPos.z);

	Player* pl = State::getPlayer();
	if( pl == NULL) return;
	Vector playerAbsPos = pl->getPlayable()->getAbsCoor();

	//PRINTF(0)(" Player abs coor: %f, %f, %f\n", playerAbsPos.x, playerAbsPos.y, playerAbsPos.z);

	////////////

	//if(aa<0)aa=100.0f;
	float a=200.0f;
	float vMax=200.0f;
	
	//float keepDist= this->getSize(owner);//+ this->getSize(pl->getPlayable());
	//std::cout << "Distance: " << keepDist << "\n";


	Vector vectorToPlayer = playerAbsPos - myAbsPos;
	float dist=vectorToPlayer.len();
	//Vector nVectorToPlayer=vectorToPlayer/dist*(dist-keepDist);


	// std::vector<Vector> collisonCorrection;
	// get all npcs

	Vector tmpNPCpos;
	float tmpDist;
	Vector tmpCorrectionVect;
	Vector antiNPCCollision;

	for (ObjectList<NPC2>::const_iterator it = NPC2::objectList().begin();
			 it != NPC2::objectList().end();
			 ++it)
	{
		if(*it==owner)continue;
		tmpNPCpos=(*it)->getAbsCoor();
		tmpCorrectionVect = myAbsPos-tmpNPCpos;
		
		tmpDist=tmpCorrectionVect.len()-16;
		if(tmpDist<0.1)tmpDist=0.1;
		tmpCorrectionVect=tmpCorrectionVect/(tmpDist*tmpDist);
		
		antiNPCCollision=antiNPCCollision+tmpCorrectionVect;
	}



	Vector antiPlayerCollision=vectorToPlayer*(-1);
	tmpDist=antiPlayerCollision.len()-45;
	if(tmpDist<0.1)tmpDist=0.1;
	antiPlayerCollision=antiPlayerCollision/(tmpDist*tmpDist);
	
	Vector correction=antiPlayerCollision*50+antiNPCCollision*50+vectorToPlayer-v;
	//Vector nCorrection=
	correction.y=0;
	float correctionLen=correction.len();
	if(correctionLen>a*dt)correction=correction/correctionLen*a*dt;
	v+=correction;

	float vLen=v.len();
	if(vLen>vMax)v/vLen*vMax;

	//Move NPC...
	owner->shiftCoor(v*dt);

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