/*
 *   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:
 *      Gani Aliguzhinov
 *   Co-authors:
 *      Fabian 'x3n' Landau, Dominik Solenicki
 *
 */
#include "controllers/FightingController.h"
#include "core/XMLPort.h"
#include "util/Math.h"


#include "worldentities/pawns/SpaceShip.h"

#include "weaponsystem/WeaponMode.h"
#include "weaponsystem/WeaponPack.h"
#include "weaponsystem/Weapon.h"
#include "weaponsystem/WeaponSlot.h"
#include "weaponsystem/WeaponSystem.h"
#include "weaponsystem/Munition.h"

namespace orxonox
{

    RegisterClass (FightingController);
    
    FightingController::FightingController( Context* context ): FlyingController( context )
    {
    	this->attackRange_ = 2500;
        this->stopLookingAtTarget();
        this->bSetupWorked = false;
        this->timeout_ = 0;
        RegisterObject( FightingController );
    }
    FightingController::~FightingController() 
    {
        
    }
    void FightingController::XMLPort( Element& xmlelement, XMLPort::Mode mode )
    {
        SUPER( FightingController, XMLPort, xmlelement, mode );
    }
    void FightingController::lookAtTarget(float dt)
    {
        if (!this || !this->getControllableEntity())
            return;
        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 FightingController::stopLookingAtTarget()
    {
        this->bLookAtTarget_ = false;
    }
    void FightingController::startLookingAtTarget()
    {
        this->bLookAtTarget_ = true;
    }
    bool FightingController::hasTarget() const
    {
        if ( this->target_ )
            return true;
        return false;
    }

    void FightingController::setTarget( ControllableEntity* target )
    {
        this->target_ = target;        
        if ( this->target_ )
        {
            this->setPositionOfTarget( target_->getWorldPosition() );
        }
    }
    void FightingController::setPositionOfTarget( const Vector3& target )
    {
        this->positionOfTarget_ = target;
        this->bHasPositionOfTarget_ = true;
    }
    void FightingController::setOrientationOfTarget( const Quaternion& orient )
    {
        this->orientationOfTarget_=orient;
        this->bHasOrientationOfTarget_=true;
    }
    
    void FightingController::maneuver() 
    {
        if ( !this->target_ || !this->getControllableEntity())
            return;


        Vector3 thisPosition = this->getControllableEntity()->getWorldPosition();
        this->setPositionOfTarget(this->target_->getWorldPosition());
        //this->setOrientationOfTarget(this->target_->getOrientation());
        Vector3 diffVector = this->positionOfTarget_ - thisPosition;
        float diffLength = diffVector.length();
        Vector3 diffUnit = diffVector/diffLength;

        if (!this || !this->getControllableEntity())
            return;

        //too far? well, come closer then
        if (diffLength > this->attackRange_)
        {
            this->spread_ = 400;
            this->formationMode_ = FormationMode::DIAMOND;
            this->bKeepFormation_ = true;
            
            this->setTargetPosition(this->positionOfTarget_ - diffUnit * 100.0f);
        }
        else
        {   
            bool bTargetIsLookingAtThis = CommonController::isLooking (this->target_, this->getControllableEntity(), math::pi/10.0f)
                || this->deltaHp < 0;
            this->bKeepFormation_ = false;

            if (!this || !this->getControllableEntity())
                return;
            // if (this->actionCounter_ % 3 == 0)
            if (!this->bDodge_)
            {
                // orxout(internal_error) << "attacking" << endl;
                this->bStartedDodging_ = false;

                this->setTargetPosition(this->positionOfTarget_ - diffUnit * 50.0f);   
                return;
            }
            else if (bTargetIsLookingAtThis || diffLength < 700.0f)
            {
                // orxout(internal_error) << "dodging" << endl;
                if (!this->bStartedDodging_)
                {
                    this->bStartedDodging_ = true;
                    dodge(thisPosition, diffLength, diffUnit);        
                }
            }
            else
            {
                if (diffLength < 1000)
                {
                // orxout(internal_error) << "looking" << endl;                    
                    this->stopMoving();
                    this->startLookingAtTarget();

                }
                else
                {
                // orxout(internal_error) << "closing up" << endl;                    
                    this->setTargetPosition(this->positionOfTarget_ - diffUnit * 300.0f);
                }
            }
        }
    }
    
    void FightingController::dodge(const Vector3& thisPosition, float diffLength, Vector3& diffUnit)
    {
        //d.x*x + d.y*y + d.z*z == 0
        //z = 1/d.z * (-d.y*y - d.x * x)
        float x = CommonController::randomInRange (300, 800) * (CommonController::randomInRange(0,1) <= 0.5 ? 1 : -1);
        float y = CommonController::randomInRange (300, 800) * (CommonController::randomInRange(0,1) <= 0.5 ? 1 : -1);
        float z = diffUnit.z == 0 ? 0 : (1/diffUnit.z) * (-x * diffUnit.x - y * diffUnit.y);
        if (diffLength < 150.0f)
        {
            this->setTargetPosition(this->positionOfTarget_ + Vector3(z,x,y));
        }
        else
        {
        this->setTargetPosition(thisPosition + Vector3(x,y,z) + (this->deltaHp < 0 ? -diffUnit * 450.0f : 
            (diffLength < 700.0f ? -diffUnit*700.0f : diffUnit * 50.0f)));

        }
        this->boostControl();

    }
    bool FightingController::canFire() 
    {
        //no target? why fire?
        if (!this->target_ || !this->getControllableEntity())
            return false;
        Vector3 newPositionOfTarget = getPredictedPosition(this->getControllableEntity()->getWorldPosition(), 
                                                           hardcoded_projectile_speed, this->target_->getWorldPosition(), 
                                                           this->target_->getVelocity());
        if (!this->target_ || !this->getControllableEntity())
            return false;
        //Vector3.isNaN() is what I used on my machine and it worked...
        if (!(std::isnan(newPositionOfTarget.x) || std::isnan(newPositionOfTarget.y) || std::isnan(newPositionOfTarget.z)))
        {
            this->setPositionOfTarget(newPositionOfTarget);
        }

        return squaredDistanceToTarget() < this->attackRange_*this->attackRange_ && this->isLookingAtTarget(math::pi / 10.0f);
    }


    float FightingController::squaredDistanceToTarget()  const
    {
        if (!this || !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 FightingController::isLookingAtTarget( float angle ) const
    {
        if ( !this->getControllableEntity()  || !this->target_ )
            return false;
        return CommonController::isLooking(this->getControllableEntity(), this->getTarget(), angle);
    }
    void FightingController::setClosestTarget()
    {
        this->setTarget (static_cast<ControllableEntity*>( closestTarget() ) ); 
    }
    
    Pawn* FightingController::closestTarget() const
    {
        if (!this || !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;  
    }
    //I checked it out, rockets DO NOT cause any problems! this->getControllableEntity() is always a SpaceShip
    void FightingController::doFire()
    {
        if (!this->bSetupWorked)
        {
            this->setupWeapons();
        }
        if (!this->target_ || !this->getControllableEntity())
        {
            return;
        }

        Pawn* pawn = orxonox_cast<Pawn*> (this->getControllableEntity());
        if (pawn)
            pawn->setAimPosition (this->positionOfTarget_);
        // if (pawn->getHealth() < 100)
        //     orxout(internal_error) << "not full, hp = " << pawn->getHealth() << endl;
        int firemode;
        float distance = CommonController::distance (this->getControllableEntity(), this->target_);

        // firemode = distance < 1500 ? (distance > 800 && distance < 1200 ? 
        //                             (this->rocketsLeft_ > 0 && !this->bFiredRocket_ ? getFiremode("RocketFire") : getFiremode ("HsW01"))
        //                                                      : getFiremode("HsW01")) : 
        //                                                      (distance < 2500 ? getFiremode("LightningGun") : getFiremode("HsW01"));
        if (distance < 1500)
        {
            if (this->rocketsLeft_ > 0 && !this->bFiredRocket_)
            {
                firemode = getFiremode ("RocketFire");
            }
            else
            {
                if (distance > 800)
                    firemode = getFiremode ("HsW01");
                else
                    firemode = getFiremode ("LightningGun");
            }

        } 
        // else if (distance < 1000)
        // {
        //     if (this->rocketsLeft_ > 0 && !this->bFiredRocket_)
        //     {
        //         firemode = getFiremode ("RocketFire");
        //     }
        //     else
        //     {
        //         firemode = getFiremode ("HsW01");
        //     }
        // }
        else if (distance < 2000)
        {
            firemode = getFiremode ("HsW01");
        }
        else
        {
            firemode = getFiremode ("LightningGun");
        }
        if (firemode < 0)
        {
            //assuming there is always some weapon with index 0
            firemode = 0;
        }
        if (firemode == getFiremode("RocketFire"))
        {
            this->timeout_ = 5;
            this->rocketsLeft_--;
            this->bFiredRocket_ = true;
        }
        if (firemode == getFiremode("SimpleRocketFire"))
        {
            this->rocketsLeft_--;
        }
              
        this->getControllableEntity()->fire(firemode);
        
    }
    void FightingController::setupWeapons() //TODO: Make this function generic!! (at the moment is is based on conventions)
    {
        this->bSetupWorked = false;
        if(this->getControllableEntity())
        {
            Pawn* pawn = orxonox_cast<Pawn*>(this->getControllableEntity());
            if(pawn && pawn->isA(Class(SpaceShip))) //fix for First Person Mode: check for SpaceShip
            {
                this->weaponModes_.clear(); // reset previous weapon information
                WeaponSlot* wSlot = 0;
                for(int l=0; (wSlot = pawn->getWeaponSlot(l)) ; l++)
                {
                    WeaponMode* wMode = 0;
                    for(int i=0; (wMode = wSlot->getWeapon()->getWeaponmode(i)) ; i++)
                    {
                        std::string wName = wMode->getIdentifier()->getName();
                        // SubclassIdentifier<Munition> munition =  ClassByString(wName);
                        if (wName == "RocketFire")
                            this->rocketsLeft_ = 10;
                            // this->rocketsLeft_ = orxonox_cast<Pawn*>(this->getControllableEntity())->getWeaponSystem()->getMunition(&munition)->getNumMunitionInCurrentMagazine(wMode);
                        if(this->getFiremode(wName) == -1) //only add a weapon, if it is "new"
                            weaponModes_[wName] = wMode->getMode();
                    }
                }
                if(weaponModes_.size())//at least one weapon detected
                    this->bSetupWorked = true;
            }//pawn->weaponSystem_->getMunition(SubclassIdentifier< Munition > *identifier)->getNumMunition (WeaponMode *user);
        }

        //orxout (internal_error) << this->rocketsLeft_ << " rockets left" << endl;
    }

    int FightingController::getFiremode(std::string name)
    {
        for (std::map< std::string, int >::iterator it = this->weaponModes_.begin(); it != this->weaponModes_.end(); ++it)
        {
            if (it->first == name)
                return it->second;
        }
        return -1;
    }
}