/*
   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 "swarm_gorel.h"
#include "ai_module.h"
#include "movement_module.h"

void SwarmGoRel::initialize()
{
	std::map<WorldEntity*,AIModule*>::iterator it;

	//calculate  movement parameters..
	float averageRadius;
	for (it=members.begin(); it!= members.end(); it++ ){
		averageRadius+=it->second->getNPCRadius();
	}
	averageRadius=averageRadius/members.size();
	aMax=80;//400/averageRadius;
	vMax=maxSpeed;//1000/averageRadius;//300/averageRadius;
	viewChangeMax=90/averageRadius;

	//load correct ai-module..
	for (it= members.begin(); it!= members.end(); it++ ){
		changeAIModule(it, new MovementModule());
	}

	//get swarm position..
	//position=this->getPosition();
}

void SwarmGoRel::process(float dt)
{
	std::map<WorldEntity*,AIModule*>::iterator it;

	//Vector swarmPosition=this->getPosition();
	Vector correction=Vector(0,0,0);
	Vector destination;
        
        WorldEntity* taskRelObject = NULL;
        if ( taskRelObjectName != "" )
          taskRelObject = dynamic_cast<WorldEntity*>(WorldEntity::objectList().getBaseObject( taskRelObjectName ));

	if(taskRelObject!=NULL && taskMaxTime>0){
		destination=taskRelObject->getAbsCoor()+taskRelPos;
		correction=(destination-position)-view*speed;
		correction.y=0;
		taskMaxTime-=dt;
	}else{
		taskComplete=true;
	}

	float correctionLen=correction.len();
	if(correctionLen>aMax*dt)correction=correction/correctionLen*aMax*dt;

	//std::cout << angleRad(correction,movement) << "\n";
	Vector movement=view*speed+correction;

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

	float newSpeed=movement.len();
	Vector newView=movement.getNormalized();

	/*if((newSpeed<=taskSpeed && speed>=taskSpeed)||(newSpeed>=taskSpeed && speed<=taskSpeed)){
		newSpeed=taskSpeed;
	}else if((newSpeed>speed && speed<taskSpeed) || (newSpeed < speed && speed < taskSpeed)){
		newSpeed=taskSpeed;
	}*/

	/*if(angleDeg(view,newView)>viewChangeMax){
		std::cout << "alarm\n";
	}*/
	if(newSpeed<40)newSpeed=40;
	if(newSpeed>vMax)newSpeed=vMax;

	speed=newSpeed;
	view=newView;
	position=position+view*speed*dt;

	//tell orders to swarm-members...
	for (it= members.begin(); it!= members.end(); it++ ){
		it->second->setDestination(position);
		it->second->setDestinationMovement(view*speed);
		it->second->process(dt);
	}

	//check if destination reached
	if(!taskComplete){
		//swarmPosition=this->getPosition();
		if((destination-position).len()<10)taskComplete=true;
	}
}