/*
 *   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 "controllers/FlyingController.h"
#include "core/XMLPort.h"
#include "worldentities/pawns/SpaceShip.h"
#include "util/Math.h"
#include <OgreMatrix3.h>

namespace orxonox
{    
    RegisterClass (FlyingController);
    
    FlyingController::FlyingController(Context* context): CommonController(context)
    {
        RegisterObject(FlyingController);        
        this->rotationProgress_ = 0;
        this->spread_ = 200;
        this->tolerance_ = 80;
    }
    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(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 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;
    }
    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_ )
        {
            Vector2 coord = get2DViewCoordinates
                ( entity->getPosition() , 
                entity->getOrientation()  * WorldEntity::FRONT, 
                entity->getOrientation()  * WorldEntity::UP, 
                targetPosition );
            float rotateX = -clamp( coord.x * 10, -1.0f, 1.0f );
            float rotateY = clamp( coord.y * 10, -1.0f, 1.0f );
            entity->rotateYaw( ROTATEFACTOR * rotateX * dt );
            entity->rotatePitch( ROTATEFACTOR * rotateY * dt );
          
            if (distance > this->tolerance_*1.5f || (rotateX > -0.03 && rotateX < 0.03 && rotateY > -0.03 && rotateY < 0.03))
                entity->moveFrontBack( SPEED * dt );
                copyTargetOrientation(dt);
        }
        else
        {      
            bHasTargetPosition_ = false;
        }
    }
    
    void FlyingController::moveToTargetPosition(float dt)
    {
        this->moveToPosition (this->targetPosition_, dt);
    }
    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);
        }
    }

    void FlyingController::copyTargetOrientation(float dt)
    {
        if (bHasTargetOrientation_)
        {   
            this->copyOrientation(targetOrientation_, dt);
        }
    }
    
    void FlyingController::setTargetPosition(const Vector3& target)
    {
        this->targetPosition_ = target;
        this->bHasTargetPosition_ = true;
    }

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

    void FlyingController::setTargetOrientation(ControllableEntity* target)
    {
        if (target)
            this->setTargetOrientation(target->getOrientation());
    }
    void FlyingController::boostControl()
    {
        if (!this->getControllableEntity())
            return;
        SpaceShip* ship = orxonox_cast<SpaceShip*>(this->getControllableEntity());
        if(ship == NULL) 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);
        }
    }
    void FlyingController::keepFormation(const ControllableEntity* leaderEntity, Vector3& targetRelativePosition)
    {
        ControllableEntity* myEntity = this->getControllableEntity();
        Vector3 myPosition = myEntity->getWorldPosition();

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

        if (!leaderEntity)
        {
            return;
        }
        Vector3 targetAbsolutePosition = 
            (leaderPosition + (orient*WorldEntity::FRONT) * (leaderEntity->getVelocity().length()/5)
             + (orient* (targetRelativePosition)));
        //let ship finish rotating. also don't call copyOrientation to often as it is a slow function.
        if (static_cast<int>(rnd(1.0f) * 100) % 3 == 0)
            this->setTargetOrientation (orient);
        this->setTargetPosition (targetAbsolutePosition);
        if ((targetAbsolutePosition - myPosition).length() > this->tolerance_ * 1.5f)
        {
            this->boostControl();
        }
        else
        {
           this->getControllableEntity()->boost(false);
        }
    }
}