/*
   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 "ai_swarm.h"
#include "debug.h"
#include <stdio.h>

void AISwarm::process(float dt)
{
	std::set<AIModule*>::iterator it;
	Vector swarmPosition=this->getPosition();
	//std::cout << swarmPosition.x << " " << swarmPosition.z << "\n";
	
	float aMax=70.0f;
	float vMax=1000.0f;
	
	Vector correction=(destination-swarmPosition)-movement;
	correction.y=0;
	
	float correctionLen=correction.len();
	if(correctionLen>aMax*dt)correction=correction/correctionLen*aMax*dt;
	
	//std::cout << angleRad(correction,movement) << "\n";
	movement+=correction;
	
	float movementLen=movement.len();
	if(movementLen>vMax)movement=movement/movementLen*vMax;
	
	
	for (it= swarmMembers.begin(); it!= swarmMembers.end(); it++ ){
		(*it)->setDestination(swarmPosition);
		(*it)->setDestinationMovement(movement);
		(*it)->setEnemyList(enemyList);
		(*it)->process(dt);
	}
}

void AISwarm::addAI(AIModule* aiModule)
{
	swarmMembers.insert(aiModule);
}

void AISwarm::removeAI(AIModule* aiModule)
{
	std::set<AIModule*>::iterator it=swarmMembers.find(aiModule);
	if(it==swarmMembers.end())return;
	delete (*it);
}

Vector AISwarm::getPosition()
{
	Vector center=Vector(0,0,0);
	std::set<AIModule*>::iterator it;
	for (it= swarmMembers.begin(); it!= swarmMembers.end(); it++ )
		center=center+(*it)->getPosition();

	return center/swarmMembers.size();
}