/*
 *   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/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 )
    {
        this->rotationProgress_ = 0;
        this->spread_ = 200;
        this->tolerance_ = 60;
        RegisterObject( FlyingController );
    }
    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() 
    {
        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 FlyingController::stopMoving()
    {
        this->bHasTargetPosition_ = false;
    }

    void FlyingController::moveToPosition( const Vector3& target, float dt )
    {
        ControllableEntity* entity = this->getControllableEntity();

        float distance = ( target - this->getControllableEntity() ->getPosition() ).length();


        if ( distance >= this->tolerance_ )
        {
            Vector2 coord = get2DViewCoordinates
                ( entity->getPosition() , 
                entity->getOrientation()  * WorldEntity::FRONT, 
                entity->getOrientation()  * WorldEntity::UP, 
                target );
            float rotateX = -clamp( coord.x * 10, -1.0f, 1.0f );
            float rotateY = clamp( coord.y * 10, -1.0f, 1.0f );
            this->getControllableEntity() ->rotateYaw( ROTATEFACTOR * rotateX * dt );
            this->getControllableEntity() ->rotatePitch( ROTATEFACTOR * rotateY * 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;
        }
        copyTargetOrientation(dt);
    }
    
    void FlyingController::moveToTargetPosition(float dt)
    {
        this->moveToPosition (this->targetPosition_, dt);
    }
    void FlyingController::copyOrientation( const Quaternion& orient, float dt )
    {
        //inspired by
        //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?
       
        Quaternion myOrient = this->getControllableEntity()->getOrientation();
        this->rotationProgress_ += dt;

        if (this->rotationProgress_ > 1)
        {
            this->rotationProgress_ = 0;
            this->bHasTargetOrientation_ = false;
        }
        else
        {

            Quaternion delta = Quaternion::Slerp(rotationProgress_, myOrient, orient, true);
            
            //rotate roll builds a Quaternion in roll method of WorldEntity, then it sets orientation.
            //it is faster just to set orientation, plus that way there is no need in calculating the roll angle.
            //On the downside, however, ship might also yaw and pitch, but this effect is neglectable, as we only call 
            //copyOrientation after we move our ship, thus it doesn't affect ships's flying direction too much.
            //If you don't like the code style, you are welcomed to uncomment the code below
            //and comment out setOrientation part, it will work just fine, but it will also be a tiny bit slower.
            //P.S. apperantly it did affect ship's direction and did so way too much.
            Matrix3 orientMatrix, myMatrix;

            delta.ToRotationMatrix(orientMatrix);
            myOrient.ToRotationMatrix (myMatrix);

            Radian yRad, pRad, rRad, yMy, pMy, rMy;
            orientMatrix.ToEulerAnglesYXZ(yRad, pRad, rRad);
            myMatrix.ToEulerAnglesYXZ (yMy, pMy, rMy);

            this->getControllableEntity()->rotateRoll ((rRad.valueRadians() - rMy.valueRadians())*ROTATEFACTOR*dt);
            // this->getControllableEntity()->setOrientation(delta);
        }
        
        
    }
    //change log: increased precision, increased rotation speed
    void FlyingController::copyTargetOrientation( float dt )
    {
        if ( bHasTargetOrientation_ )
        {   
            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 )
            setTargetOrientation( target->getOrientation() );
    }
    void FlyingController::boostControl()
    {
        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);
        }
    }
    
}