/*
 *   ORXONOX - the hottest 3D action shooter ever to exist
 *                    > www.orxonox.net <
 *
 *
 *   License notice:
 *
 *   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
 *   of the License, or ( at your option )any later version.
 *
 *   This program is distributed in the hope that it will be useful,
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *   GNU General Public License for more details.
 *
 *   You should have received a copy of the GNU General Public License
 *   along with this program; if not, write to the Free Software
 *   Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 *
 *   Author:
 *      Fabian 'x3n' Landau
 *   Co-authors:
 *      Dominik Solenicki
 *
 */
#include "controllers/CommonController.h"
#include "core/XMLPort.h"

//stuff for sameTeam function
#include "worldentities/pawns/TeamBaseMatchBase.h"
#include "gametypes/TeamDeathmatch.h"
#include "gametypes/Dynamicmatch.h"
#include "gametypes/Mission.h"
#include "gametypes/Gametype.h"
#include "controllers/WaypointPatrolController.h"
#include "controllers/NewHumanController.h"
#include "controllers/DroneController.h"


namespace orxonox
{

    RegisterClass( CommonController );
    const float SPEED = 0.9f/0.02f;
    const float ROTATEFACTOR = 1.0f/0.02f;

    //Table of content:
    //Constructor, Destructor & tick
    //XML methods
    //World interaction
    //Helper methods
    //Flying methods
    //Fighting methods
    //Actionpoint methods


    //------------------------------------------------------------------------------
    //------------------------Constructor, Destructor & tick------------------------
    //------------------------------------------------------------------------------
    
    CommonController::CommonController( Context* context ): Controller( context )
    {
        this->squaredaccuracy_ = 10000;
        this->bFirstTick_ = true;
        this->tolerance_ = 50;
        this->action_ = Action::NONE;
        this->stopLookingAtTarget();
        this->attackRange_ = 2500;
        RegisterObject( CommonController );
    }
    CommonController::~CommonController() 
    {
       
    }
    void CommonController::tick(float dt)
    {
        if (this->bHasTargetPosition_)
        {
            this->moveToTargetPosition(dt);
        }
        else if (this->bLookAtTarget_)
        {
            this->lookAtTarget(dt);
        }
        if (bShooting_)
        {
            this->doFire();
        }
        if (this->bFirstTick_)
        {
            std::reverse(parsedActionpoints_.begin(), parsedActionpoints_.end());
            std::reverse(actionpoints_.begin(), actionpoints_.end());
        }
        if (this->bFirstTick_)
            this->bFirstTick_ = false;
        SUPER(CommonController, tick, dt);
    }

    //------------------------------------------------------------------------------
    //----------------------------------XML methods---------------------------------
    //------------------------------------------------------------------------------
    
    void CommonController::XMLPort( Element& xmlelement, XMLPort::Mode mode )
    {
        SUPER( CommonController, XMLPort, xmlelement, mode );
        XMLPortParam( CommonController, "formationMode", setFormationModeXML, getFormationModeXML,  xmlelement, mode );
        XMLPortObject(CommonController, WorldEntity, "actionpoints", addActionpoint, getActionpoint,  xmlelement, mode);
    }
    void CommonController::setFormationModeXML( std::string val )
    {
        const std::string valUpper = getUppercase( val );
        FormationMode::Value value;
        
        if ( valUpper == "WALL" )
            value = FormationMode::WALL;
        else if ( valUpper == "FINGER4" )
            value = FormationMode::FINGER4;
        else if ( valUpper == "DIAMOND" )
            value = FormationMode::DIAMOND;
        else
            ThrowException( ParseError, std::string( "Attempting to set an unknown FormationMode: '" )+ val + "'." );
        this->setFormationMode( value );
    }
    std::string CommonController::getFormationModeXML() 
    {
        switch ( this->formationMode_ )
        {
            case FormationMode::WALL:
            { return "WALL"; break; }
            case FormationMode::FINGER4:
            { return "FINGER4"; break; }
            case FormationMode::DIAMOND:
            { return "DIAMOND"; break; }
            default:
                return "DIAMOND"; break;
        }
    }
    void CommonController::setFormationMode(FormationMode::Value val)
    { 
        this->formationMode_ = val; 
    }
    FormationMode::Value CommonController::getFormationMode() const
    { 
        return this->formationMode_; 
    }
    void CommonController::setRank(Rank::Value val)
    { 
        this->rank_ = val; 
    }
    Rank::Value CommonController::getRank() const
    { 
        return this->rank_; 
    }
    void CommonController::addActionpoint(WorldEntity* actionpoint)
    {
        std::string actionName;
        Vector3 position;
        std::string targetName;
        Point p;
        if (static_cast<Actionpoint*> (actionpoint))
        {
            Actionpoint* ap = static_cast<Actionpoint*> (actionpoint);
            actionName = ap->getActionXML();
            targetName = ap->getName();
            position = ap->getWorldPosition();

            Action::Value value;
            
            if ( actionName == "FIGHT" )
            { value = Action::FIGHT; }
            else if ( actionName == "FLY" )
            { value = Action::FLY; }
            else if ( actionName == "PROTECT" )
            { value = Action::PROTECT; }
            else if ( actionName == "NONE" )
            { value = Action::NONE; }
            else if ( actionName == "FIGHTALL" )
            { value = Action::FIGHTALL; }
            else if ( actionName == "ATTACK" )
            { value = Action::ATTACK; }
            else
                ThrowException( ParseError, std::string( "Attempting to set an unknown Action: '" )+ actionName + "'." );
            p.action = value; p.name = targetName; p.position = position;
            parsedActionpoints_.push_back(p);
        }
        else
        {
            p.action = Action::FLY; p.name = ""; p.position = actionpoint->getWorldPosition();
        }
            parsedActionpoints_.push_back(p);
            this->actionpoints_.push_back(actionpoint);
    }
    WorldEntity* CommonController::getActionpoint(unsigned int index) const
    {
        if (index < this->actionpoints_.size())
            return this->actionpoints_[index];
        else
            return 0;
    }

    //------------------------------------------------------------------------------
    //-------------------------------World interaction------------------------------
    //------------------------------------------------------------------------------

    //"Virtual" methods
    bool CommonController::setWingman ( CommonController* wingman )
    { return false; }
    bool CommonController::hasWingman() 
    { return true; }
    bool CommonController::hasTarget() 
    {
        if ( this->target_ )
            return true;
        return false;
    }
    ControllableEntity* CommonController::getTarget()
    {
        return this->target_;
    }
    Action::Value CommonController::getAction ()
    {
        return this->action_;
    }
    std::string CommonController::getActionName()
    {
        switch ( this->action_ )
        {
            case Action::FIGHT:
            { return "FIGHT"; break; }
            case Action::FLY:
            { return "FLY"; break; }
            case Action::PROTECT:
            { return "PROTECT"; break; }
            case Action::NONE:
            { return "NONE"; break; }
            case Action::FIGHTALL:
            { return "FIGHTALL"; break; }
            case Action::ATTACK:
            { return "ATTACK"; break; }
            default:
                return "NONE";
                break;
        }
    }
    void CommonController::setAction (Action::Value action)
    {
        this->action_ = action;
    }
    void CommonController::setAction (Action::Value action, ControllableEntity* target)
    {
        this->action_ = action;
        if (action == Action::FIGHT || action == Action::FIGHTALL || action == Action::ATTACK)
        {   
            if (target)
                this->setTarget (target);
        }
        else if (action == Action::PROTECT)
        {
            if (target)
                this->setProtect (target);
        }
    }
    void CommonController::setAction (Action::Value action, const Vector3& target)
    {
        this->action_ = action;
        if (action == Action::FLY)
        {
            this->setTargetPosition (target);
        }
    }
    void CommonController::setAction (Action::Value action, const Vector3& target,  const Quaternion& orient )
    {
        this->action_ = action;
        if (action == Action::FLY)
        {
            this->setTargetPosition (target);
            this->setTargetOrientation (orient);
        }  
    }

    //------------------------------------------------------------------------------
    //--------------------------------Helper methods--------------------------------
    //------------------------------------------------------------------------------

    float CommonController::randomInRange( float a, float b )
    {
        float random = rnd( 1.0f );
        float diff = b - a;
        float r = random * diff;
        return a + r;
    }
    float CommonController::distance (ControllableEntity* entity1, ControllableEntity* entity2)
    {
        if (!entity1 || !entity2)
            return std::numeric_limits<float>::infinity();
        return ( entity1->getPosition() - entity2->getPosition() ).length();
    }
    bool CommonController::sameTeam (ControllableEntity* entity1, ControllableEntity* entity2, Gametype* gametype)
    {
        /*if (!entity1 || !entity2)
            return false;
        return entity1->getTeam() == entity2->getTeam();*/
        if (entity1 == entity2)
            return true;

        int team1 = entity1->getTeam();
        int team2 = entity2->getTeam();

        Controller* controller = 0;
        if (entity1->getController())
            controller = entity1->getController();
        else
            controller = entity1->getXMLController();
        if (controller)
        {
            CommonController* ac = orxonox_cast<CommonController*>(controller);
            if (ac)
                team1 = ac->getTeam();
        }

        if (entity2->getController())
            controller = entity2->getController();
        else
            controller = entity2->getXMLController();
        if (controller)
        {
            CommonController* ac = orxonox_cast<CommonController*>(controller);
            if (ac)
                team2 = ac->getTeam();
        }

        TeamGametype* tdm = orxonox_cast<TeamGametype*>(gametype);
        if (tdm)
        {
            if (entity1->getPlayer())
                team1 = tdm->getTeam(entity1->getPlayer());

            if (entity2->getPlayer())
                team2 = tdm->getTeam(entity2->getPlayer());
        }

        TeamBaseMatchBase* base = 0;
        base = orxonox_cast<TeamBaseMatchBase*>(entity1);
        if (base)
        {
            switch (base->getState())
            {
                case BaseState::ControlTeam1:
                    team1 = 0;
                    break;
                case BaseState::ControlTeam2:
                    team1 = 1;
                    break;
                case BaseState::Uncontrolled:
                default:
                    team1 = -1;
            }
        }
        base = orxonox_cast<TeamBaseMatchBase*>(entity2);
        if (base)
        {
            switch (base->getState())
            {
                case BaseState::ControlTeam1:
                    team2 = 0;
                    break;
                case BaseState::ControlTeam2:
                    team2 = 1;
                    break;
                case BaseState::Uncontrolled:
                default:
                    team2 = -1;
            }
        }

        DroneController* droneController = 0;
        droneController = orxonox_cast<DroneController*>(entity1->getController());
        if (droneController && static_cast<ControllableEntity*>(droneController->getOwner()) == entity2)
            return true;
        droneController = orxonox_cast<DroneController*>(entity2->getController());
        if (droneController && static_cast<ControllableEntity*>(droneController->getOwner()) == entity1)
            return true;
        DroneController* droneController1 = orxonox_cast<DroneController*>(entity1->getController());
        DroneController* droneController2 = orxonox_cast<DroneController*>(entity2->getController());
        if (droneController1 && droneController2 && droneController1->getOwner() == droneController2->getOwner())
            return true;

        Dynamicmatch* dynamic = orxonox_cast<Dynamicmatch*>(gametype);
        if (dynamic)
        {
            if (dynamic->notEnoughPigs||dynamic->notEnoughKillers||dynamic->notEnoughChasers) {return false;}

            if (entity1->getPlayer())
                team1 = dynamic->getParty(entity1->getPlayer());

            if (entity2->getPlayer())
                team2 = dynamic->getParty(entity2->getPlayer());

            if (team1 ==-1 ||team2 ==-1 ) {return false;}
            else if (team1 == dynamic->chaser && team2 != dynamic->chaser) {return false;}
            else if (team1 == dynamic->piggy && team2 == dynamic->chaser) {return false;}
            else if (team1 == dynamic->killer && team2 == dynamic->chaser) {return false;}
            else return true;
        }

        return (team1 == team2 && team1 != -1);
    }
    bool CommonController::isLooking( ControllableEntity* entityThatLooks, ControllableEntity* entityBeingLookedAt, float angle )
    {
        if ( !entityThatLooks || !entityBeingLookedAt )
            return false;
        return ( getAngle( entityThatLooks ->getPosition() , 
            entityThatLooks->getOrientation()  * WorldEntity::FRONT, 
            entityBeingLookedAt->getWorldPosition() ) < angle );
    }
    std::string CommonController::getName(Pawn* entity)
    {
        std::string name = entity->getName();
        if (name == "")
        {
            const void * address = static_cast<const void*>(entity);
            std::stringstream ss;
            ss << address;  
            name = ss.str();            
        }
        return name;
    }
    float CommonController::squaredDistanceToTarget()  const
    {
        if ( !this->getControllableEntity()  )
            return 0;
        if ( !this->target_ || !this->getControllableEntity() )
            return ( this->getControllableEntity() ->getPosition() .squaredDistance( this->targetPosition_ ) );
        else
            return ( this->getControllableEntity() ->getPosition() .squaredDistance( this->positionOfTarget_ ) );
    }
    bool CommonController::isLookingAtTarget( float angle )
    {
        if ( !this->getControllableEntity()  || !this->target_ )
            return false;
        return this->isLooking(this->getControllableEntity(), this->getTarget(), angle);
    }


    //------------------------------------------------------------------------------
    //--------------------------------Flying methods--------------------------------
    //------------------------------------------------------------------------------

    void CommonController::stopMoving()
    {
        this->bHasTargetPosition_ = false;
    }
    void CommonController::stopLookingAtTarget()
    {
        this->bLookAtTarget_ = false;
    }
    void CommonController::startLookingAtTarget()
    {
        this->bLookAtTarget_ = true;
    }
    void CommonController::moveToPosition( const Vector3& target, float dt )
    {
        ControllableEntity* entity = this->getControllableEntity();
        Vector2 coord = get2DViewCoordinates
            ( entity->getPosition() , 
            entity->getOrientation()  * WorldEntity::FRONT, 
            entity->getOrientation()  * WorldEntity::UP, 
            target );

        float distance = ( target - this->getControllableEntity() ->getPosition() ).length();
        float rotateX = -clamp( coord.x * 10, -1.0f, 1.0f );
        float rotateY = clamp( coord.y * 10, -1.0f, 1.0f );

        if ( distance > this->tolerance_ )
        {
            this->getControllableEntity() ->rotateYaw( ROTATEFACTOR * rotateX * dt );
            this->getControllableEntity() ->rotatePitch( ROTATEFACTOR * rotateY * dt );

            if ( distance < 300 )
            {
                if ( bHasTargetOrientation_ )
                {
                    copyTargetOrientation( dt );
                }
            }
            if (distance > this->tolerance_*1.5f || (rotateX > -0.01 && rotateX < 0.01 && rotateY > -0.01 && rotateY < 0.01))
                this->getControllableEntity() ->moveFrontBack( SPEED * dt );
        }
        else
        {      
            bHasTargetPosition_ = false;
            bHasTargetOrientation_ = false;
        }
    }
    void CommonController::moveToTargetPosition(float dt)
    {
        this->moveToPosition (this->targetPosition_, dt);
    }
    void CommonController::copyOrientation( const Quaternion& orient, float dt )
    {
        //roll angle difference in radian
        float diff=orient.getRoll(false).valueRadians() -
                        ( this->getControllableEntity() ->getOrientation() .getRoll( false ).valueRadians() );
        while( diff>math::twoPi )diff-=math::twoPi;
        while( diff<-math::twoPi )diff+=math::twoPi;
        this->getControllableEntity() ->rotateRoll( diff*ROTATEFACTOR * dt );
    }
    void CommonController::copyTargetOrientation( float dt )
    {
        if ( bHasTargetOrientation_ )
        {   
            copyOrientation( targetOrientation_, dt );
        }
    }
    void CommonController::lookAtTarget(float dt)
    {
        ControllableEntity* entity = this->getControllableEntity();
        if ( !entity )
            return;
        Vector2 coord = get2DViewCoordinates
            ( entity->getPosition() , 
            entity->getOrientation()  * WorldEntity::FRONT, 
            entity->getOrientation()  * WorldEntity::UP, 
            positionOfTarget_ );

        //rotates should be in range [-1,+1], clamp cuts off all that is not
        float rotateX = -clamp( coord.x * 10, -1.0f, 1.0f );
        float rotateY = clamp( coord.y * 10, -1.0f, 1.0f ); 
   
        //Yaw and Pitch are enough to start facing the target
        this->getControllableEntity() ->rotateYaw( ROTATEFACTOR * rotateX * dt );
        this->getControllableEntity() ->rotatePitch( ROTATEFACTOR * rotateY * dt );
    }
    void CommonController::setTargetPosition( const Vector3& target )
    {
        this->targetPosition_ = target;
        this->bHasTargetPosition_ = true;
    }

    void CommonController::setTargetOrientation( const Quaternion& orient )
    {
        this->targetOrientation_=orient;
        this->bHasTargetOrientation_=true;
    }

    void CommonController::setTargetOrientation( ControllableEntity* target )
    {
        if ( target )
            setTargetOrientation( target->getOrientation() );
    }

    //------------------------------------------------------------------------------
    //-------------------------------Fighting methods-------------------------------
    //------------------------------------------------------------------------------

    void CommonController::setTarget( ControllableEntity* target )
    {
        this->target_ = target;        
        if ( this->target_ )
        {
            this->setPositionOfTarget( target_->getWorldPosition() );
        }
    }
    void CommonController::setPositionOfTarget( const Vector3& target )
    {
        this->positionOfTarget_ = target;
        this->bHasPositionOfTarget_ = true;
    }
    void CommonController::setOrientationOfTarget( const Quaternion& orient )
    {
        this->orientationOfTarget_=orient;
        this->bHasOrientationOfTarget_=true;
    }
    void CommonController::setProtect (ControllableEntity* protect)
    {
        this->protect_ = protect;
    }
    ControllableEntity* CommonController::getProtect ()
    {
        return this->protect_;
    }
    void CommonController::maneuver() 
    {
        maneuverCounter_++;
        if (maneuverCounter_ > 5)
            maneuverCounter_ = 0;

        if ( !this->target_ || !this->getControllableEntity())
            return;
        
        Vector3 thisPosition = this->getControllableEntity()->getWorldPosition();
        this->setPositionOfTarget( getPredictedPosition( 
            thisPosition, 
            hardcoded_projectile_speed, 
            this->target_->getWorldPosition() , 
            this->target_->getVelocity()  
            )  );
        this->setOrientationOfTarget( this->target_->getOrientation() );

        Vector3 diffVector = this->positionOfTarget_ - thisPosition;
        float diffLength = diffVector.length();
        Vector3 diffUnit = diffVector/diffLength;

        bool bTargetIsLookingAtThis = this->isLooking ( this->target_, getControllableEntity(), math::pi/10.0f );
        
        //too far? well, come closer then
        if ( diffLength > this->attackRange_ )
        {
            this->setTargetPosition( this->positionOfTarget_ );
        }
        //too close? How do u expect to dodge anything? Just attack!
        else if ( diffLength < 500 )
        {   
            //at this point, just look and shoot
            if ( diffLength < 250 )
            {
                this->stopMoving();
                this->startLookingAtTarget();
            }
            else
            {
                this->setTargetPosition( this->positionOfTarget_ );
            }
        }
        //Good distance? Check if target looks at us. It doesn't? Go hunt!
        else if ( !bTargetIsLookingAtThis )
        {
            this->setTargetPosition( this->positionOfTarget_ );
        }
        //That's unfortunate, he is looking and probably shooting... try to dodge what we can...  
        else 
        {   
            if (maneuverCounter_ == 0)
            {
                this->setTargetPosition( this->positionOfTarget_ );   
                return;
            }
            dodge( thisPosition, diffUnit );
        }
    }
    
    void CommonController::dodge(Vector3& thisPosition, Vector3& diffUnit)
    {
        float factorX = 0, factorY = 0, factorZ = 0;
        float rand = randomInRange (0, 1);
    
        if (rand <= 0.5)
        { factorX = 1; }
        else
        { factorX = -1; }
        rand = randomInRange (0, 1);
        if (rand <= 0.5)
        { factorY = 1; }
        else
        { factorY = -1; }
        rand = randomInRange (0, 1);
        if (rand <= 0.5)
        { factorZ = 1; }
        else
        { factorZ = -1; }

        Vector3 target = ( diffUnit )* 8000.0f;
        Vector3* randVector = new Vector3( 
            factorX * randomInRange( 10000, 40000 ), 
            factorY * randomInRange( 10000, 40000 ), 
            factorZ * randomInRange( 10000, 40000 ) 
        );
        Vector3 projection = randVector->dotProduct( diffUnit )* diffUnit;
        *randVector -= projection;
        target += *randVector;
        this->setTargetPosition( thisPosition + target );
    }
    bool CommonController::canFire() 
    {
        //no target? why fire?
        if ( !this->target_ )
            return false;

        Vector3 newPositionOfTarget = getPredictedPosition( this->getControllableEntity() ->getWorldPosition() , 
            hardcoded_projectile_speed, this->target_->getWorldPosition() , this->target_->getVelocity() );
        if ( newPositionOfTarget != Vector3::ZERO )
        {
            this->setPositionOfTarget( newPositionOfTarget );
        }

        float squaredDistance = squaredDistanceToTarget();

        if ( squaredDistance < this->attackRange_*this->attackRange_ && this->isLookingAtTarget( math::pi / 20.0f)) 
        {
            return true;
        }
        else
        {
            return false;
        }
    }
    void CommonController::doFire() 
    {
        if ( !this->target_ || !this->getControllableEntity() )
        {
            return;
        }
      
        Pawn* pawn = orxonox_cast<Pawn*>( this->getControllableEntity() );

        if ( pawn )
            pawn->setAimPosition( this->positionOfTarget_ );
        this->getControllableEntity() ->fire( 0 );
    }
    void CommonController::setClosestTarget()
    {
        this->setTarget (static_cast<ControllableEntity*>( closestTarget() ) ); 
    }
    Pawn* CommonController::closestTarget()
    {
        if (!this->getControllableEntity())
            return 0;

        Pawn* closestTarget = 0;
        float minDistance =  std::numeric_limits<float>::infinity();
        Gametype* gt = this->getGametype();
        for (ObjectList<Pawn>::iterator itP = ObjectList<Pawn>::begin(); itP; ++itP)
        {
            if ( CommonController::sameTeam (this->getControllableEntity(), static_cast<ControllableEntity*>(*itP), gt) )
                continue;

            float distance = CommonController::distance (*itP, this->getControllableEntity());
            if (distance < minDistance)
            {
                closestTarget = *itP;
                minDistance = distance;
            }
        }
        if (closestTarget)
        {
           return closestTarget;
        } 
        return 0;  
    }
    void CommonController::startAttackingEnemiesThatAreClose()
    {
        if (this->action_ != Action::FIGHT && this->action_ != Action::FIGHTALL)
        {
            if ( (this->target_ && this->distance (this->getControllableEntity(), this->target_) > this->attackRange_) 
                || !this->target_ )
            {
                Pawn* newTarget = this->closestTarget();
                if ( newTarget && 
                    this->distance (this->getControllableEntity(), static_cast<ControllableEntity*>(newTarget))
                        <= this->attackRange_ )
                {
                    Point p = { Action::FIGHT, this->getName(newTarget), Vector3::ZERO };
                    this->parsedActionpoints_.push_back(p);
                    this->executeActionpoint();
                }
            }
        }
    }

    //------------------------------------------------------------------------------
    //------------------------------Actionpoint methods-----------------------------
    //------------------------------------------------------------------------------

    //POST: this starts doing what was asked by the last element of parsedActionpoints_,
    //if last element was failed to be parsed, next element will be executed.
    void CommonController::executeActionpoint()
    {
        if (!this->parsedActionpoints_.empty())
        {
            this->action_ = this->parsedActionpoints_.back().action;

            switch ( this->action_ )
            {
                case Action::FIGHT:
                {
                    std::string targetName = this->parsedActionpoints_.back().name;
                    if (targetName == "")
                    {
                        break;
                    }
                    for (ObjectList<Pawn>::iterator itP = ObjectList<Pawn>::begin(); itP; ++itP)
                    {
                        if (CommonController::getName(*itP) == targetName)
                        {
                            this->setTarget (static_cast<ControllableEntity*>(*itP));
                        }
                    }          
                    break;
                }
                case Action::FLY:
                {
                    this->setTargetPosition( this->parsedActionpoints_.back().position );
                    if (this->squaredDistanceToTarget() <= this->squaredaccuracy_)
                    {
                        this->nextActionpoint();
                        this->executeActionpoint();
                    }
                    break;
                }
                case Action::PROTECT:
                {
                    std::string protectName = this->parsedActionpoints_.back().name;

                    for (ObjectList<Pawn>::iterator itP = ObjectList<Pawn>::begin(); itP; ++itP)
                    {
                        if (CommonController::getName(*itP) == protectName)
                        {
                            this->setProtect (static_cast<ControllableEntity*>(*itP));
                        }
                    }
                    if (!this->getProtect())
                    {
                        this->nextActionpoint();
                        this->executeActionpoint();
                    }
                    break;
                }
                case Action::NONE:
                {
                    break;
                }
                case Action::FIGHTALL:
                {
                    break;
                }
                case Action::ATTACK:
                {
                    std::string targetName = this->parsedActionpoints_.back().name;

                    for (ObjectList<Pawn>::iterator itP = ObjectList<Pawn>::begin(); itP; ++itP)
                    {
                        if (CommonController::getName(*itP) == targetName)
                        {
                            this->setTarget (static_cast<ControllableEntity*>(*itP));
                        }
                    }
                    if (!this->hasTarget())
                    {
                        this->nextActionpoint();
                        this->executeActionpoint();
                    }
                    break;
                }
                default:
                    break;
            }
        }
        else
        {
            this->setTarget(0);
            this->setTargetPosition(this->getControllableEntity()->getWorldPosition());
            this->action_ = Action::NONE;
        }
    }
    void CommonController::stayNearProtect()
    {
        Vector3* targetRelativePosition;
        targetRelativePosition = new Vector3 (0, 300, 300);  
        Vector3 targetAbsolutePosition = ((this->getProtect()->getWorldPosition()) + 
            (this->getProtect()->getWorldOrientation()* (*targetRelativePosition)));
        this->setTargetPosition(targetAbsolutePosition);
    }
    void CommonController::nextActionpoint()
    {
        if (!this->parsedActionpoints_.empty())
        {
            this->parsedActionpoints_.pop_back();
        }
        this->setAction(Action::NONE);
    }
    void CommonController::action()
    {
        this->startAttackingEnemiesThatAreClose();
        //No action -> pop one from stack
        if (this->action_ == Action::NONE)
        {
            this->executeActionpoint();
        }
        //Action fightall -> fight till nobody alive
        if (this->action_ == Action::FIGHTALL)
        {
            if (!this->hasTarget())
            {
                //----find a target----
                ControllableEntity* newTarget = this->closestTarget();    
                if (newTarget)
                {
                    this->setAction (Action::FIGHTALL, newTarget);
                }
                else
                {
                    this->nextActionpoint();
                    return;
                }
            }
            else if (this->hasTarget())
            {

            }
        }
        //Action fight -> fight as long as enemies in range
        else if (this->action_ == Action::FIGHT)
        {
            if (!this->hasTarget())
            {
                //----find a target----
                ControllableEntity* newTarget = this->closestTarget();    
                if (newTarget && 
                        CommonController::distance (this->getControllableEntity(), newTarget) < this->attackRange_)
                {
                    this->setAction (Action::FIGHT, newTarget);
                }
                else
                {
                    this->nextActionpoint();
                    return;
                }
            }
            else if (this->hasTarget())
            {
                //----fly in formation if far enough----
                Vector3 diffVector = this->positionOfTarget_ - this->getControllableEntity()->getWorldPosition();         
                    
                if (diffVector.length() > this->attackRange_)
                {
                    ControllableEntity* newTarget = this->closestTarget();
                    
                    if (newTarget && 
                        CommonController::distance (this->getControllableEntity(), newTarget) < this->attackRange_)
                    {
                        this->setAction (Action::FIGHT, newTarget);
                    }
                    else
                    {
                        this->nextActionpoint();
                        return;
                    }
                }
            }
        }
        else if (this->action_ == Action::FLY)
        {
            if (this->squaredDistanceToTarget() <= this->squaredaccuracy_)
            {
                this->nextActionpoint();   
                return;
            }
        }
        else if (this->action_ == Action::PROTECT)
        {
            if (!this->getProtect())
            {
                this->nextActionpoint(); 
                return;
            }
            this->stayNearProtect();
        }
        else if (this->action_ == Action::ATTACK)
        {   
            if (!this->hasTarget())
            {
                this->nextActionpoint();
                return;
            }
        }
        if (this->hasTarget())
        {
            //----choose where to go----
            this->maneuver();
            //----fire if you can----
            this->bShooting_ = this->canFire();                
        }
    }
}