/*
 *   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:
 *   ...
 *
 */
#include <OgreMatrix3.h>                        //for Quaternion manipulations

#include "util/Math.h"                          
#include "core/XMLPort.h"
#include "controllers/FlyingController.h"

#include "worldentities/pawns/SpaceShip.h"      //for boosting

namespace orxonox
{    
    const float FlyingController::SPEED = 0.9f/0.02f;
    const float FlyingController::ROTATEFACTOR = 0.6f/0.02f;

    RegisterClass (FlyingController);
    
    FlyingController::FlyingController(Context* context): CommonController(context)
    {
        RegisterObject(FlyingController);        
        this->rotationProgress_ = 0;
        this->spread_ = 200;
        this->tolerance_ = 80;
        this->bCopyOrientation_ = true;
    }
    FlyingController::~FlyingController() 
    {
    }

    void FlyingController::XMLPort(Element& xmlelement, XMLPort::Mode mode)
    {
        XMLPortParam(FlyingController, "spread", setSpread, getSpread,  xmlelement, mode);
        XMLPortParam(FlyingController, "formationMode", setFormationModeXML, getFormationModeXML,  xmlelement, mode);
        SUPER(FlyingController, XMLPort, xmlelement, mode);
    }
   
    void FlyingController::setFormationModeXML(const std::string& val)
    {
        const std::string valUpper = getUppercase(val);
        FormationMode 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 FlyingController::getFormationModeXML() const
    {
        switch (this->formationMode_)
        {
            case FormationMode::WALL:
            { return "WALL"; }
            case FormationMode::FINGER4:
            { return "FINGER4"; }
            case FormationMode::DIAMOND:
            { return "DIAMOND"; }
            default:
                return "DIAMOND";
        }
    }
    void FlyingController::stopMoving()
    {
        this->bHasTargetPosition_ = false;
    }
    /**
    @brief
      if distance to targetPosition is smaller than this->tolerance_, no moving should be made, otherwise
      find amount of yaw and pitch that have to be applied, so that ship looks at targetPosition, then
      ship is moved forward towards targetPosition. Also target orientation is being applied.
    */
    void FlyingController::moveToPosition(const Vector3& targetPosition, float dt)
    {
        if (!this->getControllableEntity())
            return;
        ControllableEntity* entity = this->getControllableEntity();

        float distance = (targetPosition - entity->getPosition()).length();

        if (distance >= this->tolerance_)
        {
            //function that calculates how much yaw and pitch are to be applied
            Vector2 coord = get2DViewCoordinates
                (entity->getPosition() , 
                entity->getOrientation()  * WorldEntity::FRONT, 
                entity->getOrientation()  * WorldEntity::UP, 
                targetPosition);
            //limit yaw and pitch by [-1,1]
            float rotateX = -clamp(coord.x * 10, -1.0f, 1.0f);
            float rotateY = clamp(coord.y * 10, -1.0f, 1.0f);

            if (!entity)
                return;

            //apply yaw and pitch
            entity->rotateYaw(ROTATEFACTOR * rotateX * dt);
            entity->rotatePitch(ROTATEFACTOR * rotateY * dt);
            
            if (!entity)
                return;
           
            //only move either if ship looks at target with a certain tolerance, or if ship is far enough for it to be ok to move in a curve.
            if (distance > this->tolerance_*1.5f || (rotateX > -0.03 && rotateX < 0.03 && rotateY > -0.03 && rotateY < 0.03))
                entity->moveFrontBack(SPEED * dt);
            //roll
            copyTargetOrientation(dt);
        }
        else
        {      
            bHasTargetPosition_ = false;
        }
    }
    /**
    @brief
      fly towards a preset targetPosition_
    */
    void FlyingController::moveToTargetPosition(float dt)
    {
        this->moveToPosition (this->targetPosition_, dt);
    }
    /**
    @brief
      roll ship so that it has same roll as orient
    */
    void FlyingController::copyOrientation(const Quaternion& orient, float dt)
    {
        //copied from
        //http://www.ogre3d.org/tikiwiki/tiki-index.php?page=Quaternion+and+Rotation+Primer&structure=Tutorials#Q._How_can_I_make_my_objects_rotate_smoothly_You_mentioned_slerp_etc_
        //how can I make my objects rotate smoothly?
        if (!this->getControllableEntity())
            return;
        Quaternion myOrient = this->getControllableEntity()->getOrientation();
        this->rotationProgress_ += dt;

        if (this->rotationProgress_ > 1)
        {
            this->rotationProgress_ = 0;
            this->bHasTargetOrientation_ = false;
        }
        else
        {
            Quaternion deltaOrientation = Quaternion::Slerp(rotationProgress_, myOrient, orient, true);
            
            Matrix3 deltaMatrix, myMatrix;

            deltaOrientation.ToRotationMatrix(deltaMatrix);
            myOrient.ToRotationMatrix (myMatrix);

            Radian yawDelta, pitchDelta, rollDelta, yawMy, pitchMy, rollMy;
            deltaMatrix.ToEulerAnglesYXZ(yawDelta, pitchDelta, rollDelta);
            myMatrix.ToEulerAnglesYXZ (yawMy, pitchMy, rollMy);

            if (!this->getControllableEntity())
                return;
            this->getControllableEntity()->rotateRoll ((rollDelta.valueRadians() - rollMy.valueRadians())*ROTATEFACTOR*dt);
        }
    }
    /**
    @brief
      roll ship so that it has same roll as a preset targetOrientation_
    */
    void FlyingController::copyTargetOrientation(float dt)
    {
        if (bHasTargetOrientation_)
        {   
            this->copyOrientation(targetOrientation_, dt);
        }
    }
    /**
    @brief
      set Vector to fly to
    */
    void FlyingController::setTargetPosition(const Vector3& target)
    {
        this->targetPosition_ = target;
        this->bHasTargetPosition_ = true;
    }
    /**
    @brief
      set orientation to apply
    */
    void FlyingController::setTargetOrientation(const Quaternion& orient)
    {
        this->targetOrientation_=orient;
        this->bHasTargetOrientation_=true;
    }
    /**
    @brief
      set orientation to apply
    */
    void FlyingController::setTargetOrientation(ControllableEntity* target)
    {
        if (target)
            this->setTargetOrientation(target->getOrientation());
    }
    /**
    @brief
      boost if you can
    */
    void FlyingController::boostControl()
    {
        if (!this->getControllableEntity())
            return;
        SpaceShip* ship = orxonox_cast<SpaceShip*>(this->getControllableEntity());
        if(ship == nullptr) return;
        if(ship->getBoostPower()*1.5f > ship->getInitialBoostPower()) //upper limit ->boost
        {
            this->getControllableEntity()->boost(true);
        }
        else if(ship->getBoostPower()*4.0f < ship->getInitialBoostPower()) //lower limit ->do not boost
        {
           this->getControllableEntity()->boost(false);
        }
    }
    /**
    @brief
      keep this ship in a formation with its division
    */
    void FlyingController::keepFormation(const ControllableEntity* leaderEntity, Vector3& targetRelativePosition)
    {
        if (!this->getControllableEntity())
            return;
        ControllableEntity* myEntity = this->getControllableEntity();
        Vector3 myPosition = myEntity->getWorldPosition();

        if (!leaderEntity)
        {
            return;
        }
        Quaternion orient = leaderEntity->getWorldOrientation();
        Vector3 leaderPosition = leaderEntity->getWorldPosition();

        if (!leaderEntity)
        {
            return;
        }
        //calculate where in world coordinates this ship should fly
        Vector3 targetAbsolutePosition = 
            (leaderPosition + (orient*WorldEntity::FRONT) * (leaderEntity->getVelocity().length()/5)
             + (orient* (targetRelativePosition)));
        //let ship finish rotating. also don't call copyOrientation too often as it is a slow function. Don't know how to do it different
        if (this->bCopyOrientation_)
            this->setTargetOrientation (orient);
        //set a position to fly to
        this->setTargetPosition (targetAbsolutePosition);

        //boost if too far
        if ((targetAbsolutePosition - myPosition).length() > this->tolerance_ * 1.5f)
        {
            this->boostControl();
        }
        else
        {
            if (!this->getControllableEntity())
                return;
            this->getControllableEntity()->boost(false);
        }
    }
}