/*****************************************************************************
 * $LastChangedDate: 2011-04-23 21:07:07 -0400 (Sat, 23 Apr 2011) $
 * @file
 * @author  Jim E. Brooks  http://www.palomino3d.org
 * @brief   Commands to control an Aircraft.
 *//*
 * LEGAL:   COPYRIGHT (C) 2008 JIM E. BROOKS
 *          THIS SOURCE CODE IS RELEASED UNDER THE TERMS
 *          OF THE GNU GENERAL PUBLIC LICENSE VERSION 2 (GPL 2).
 *****************************************************************************/

#define CONTROL_COMMAND_CC 1
#include "base/module.hh"
#include "base/stream.hh"
using namespace base;
#include "math/module.hh"
#include "math/funcs_debug.hh"
#include "math/quaternion.hh"
using namespace math;
#include "world/module.hh"
using namespace world;
#include "control/module.hh"
#include "control/command.hh"

namespace control {

INTERN const string COMMAND_NAME_POSITION           = "pos";
INTERN const string COMMAND_NAME_QUATERNION         = "quat";
INTERN const string COMMAND_NAME_PHYSICAL_ROTATE    = "phyRot";
INTERN const string COMMAND_NAME_THROTTLE           = "throt";
INTERN const string COMMAND_NAME_SPEED              = "kph";
INTERN const string COMMAND_NAME_LANDING_GEAR       = "gear";
INTERN const string COMMAND_NAME_BRAKES             = "brake";

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////  functions  ///////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

/*****************************************************************************
 * Write Command (or derivative) to stream.
 * Uses the virtual Command::Write(stream).
 *
 * Asymmetrically, operator<<() (write) is defined, but operator>>() (read) is N/A,
 * since reading a derivative from stream into base is illogical.
 * Rather, use the factory method Command::Read(istream&).
 *****************************************************************************/
std::ostream& operator<<( std::ostream& strm, /*const*/ Command& command )
{
CHECK_TYPESIG(&command,TYPESIG_COMMAND);

    command.Write( strm );  // virtual method
    return strm;
}

////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////  Command  ///////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

/*****************************************************************************
 * Factory method.
 * Construct a Command derivative from input stream.
 *****************************************************************************/
shptr<Command>
Command::Read( std::istream& strm )
{
//CHECK_TYPESIG(this,TYPESIG_COMMAND);  // class method

    // Read name of command.
    // If stream is bad then it'll be empty and won't match anything.

    string cmdName;
    strm >> cmdName;

    if ( cmdName == COMMAND_NAME_POSITION )
    {
        fp x, y, z;
        strm >> x >> y >> z;
        return new CommandPosition( GeoVertex( Meter(x), Meter(y), Meter(z) ) );
    }
    else if ( cmdName == COMMAND_NAME_QUATERNION )
    {
        Quaternion quat;
        strm >> quat;
        return new CommandQuaternion( quat );
    }
    else if ( cmdName == COMMAND_NAME_PHYSICAL_ROTATE )
    {
        uint axis = 0;                      strm >> axis;
        fp controlFraction = 0;             strm >> controlFraction;
        Milliseconds::Int controlFreq = 0;  strm >> controlFreq;
        ASSERT_AXIS3(axis);
        return new CommandPhysicalRotate( axis, controlFraction, Milliseconds(controlFreq) );
    }
    else if ( cmdName == COMMAND_NAME_THROTTLE )
    {
        fp arg = 0; strm >> arg;
        return new CommandThrottle( arg );
    }
    else if ( cmdName == COMMAND_NAME_SPEED )
    {
        fp arg = 0; strm >> arg;
        return new CommandSpeed( physics::conv::KPH2Speed( SpeedKPH(arg) ) );
    }
    else if ( cmdName == COMMAND_NAME_LANDING_GEAR )
    {
        bool arg = 0; strm >> arg;
        return new CommandLandingGear( arg );
    }
    else if ( cmdName == COMMAND_NAME_BRAKES )
    {
        bool arg = 0; strm >> arg;
        return new CommandBrakes( arg );
    }
    else
    {
        throw ExceptionCommand( string("Command::Read: stream parse error ") + cmdName );
        return 0;  // shutup compiler
    }
}

////////////////////////////////////////////////////////////////////////////////
//////////////////////////////  CommandPosition  ///////////////////////////////
////////////////////////////////////////////////////////////////////////////////

/*****************************************************************************
 * Set position.
 *****************************************************************************/
CommandPosition::CommandPosition( const GeoVertex& geoPos )
:   mGeoPos(geoPos)
{
}

void
CommandPosition::Execute( shptr<Aircraft> aircraft, const ExecuteMode mode )
{
CHECK_TYPESIG(this,TYPESIG_COMMAND);

    if ( mode & EXECUTE_MODE_GRAPHICS )
    {
        aircraft->SetPosition( world::conv::GeoVertex2WorldVertex( mGeoPos ) );
    }
}

void
CommandPosition::Write( std::ostream& strm )
{
CHECK_TYPESIG(this,TYPESIG_COMMAND);

    // Write position as Meters (GeoVertex).
    strm << COMMAND_NAME_POSITION << ' '
         << mGeoPos[XX].FP() << ' '
         << mGeoPos[YY].FP() << ' '
         << mGeoPos[ZZ].FP() << ' ';  // omit std::endl
}

// specific:
WorldVertex
CommandPosition::GetWorldVertex( void )
{
CHECK_TYPESIG(this,TYPESIG_COMMAND);

    return world::conv::GeoVertex2WorldVertex( mGeoPos );
}

////////////////////////////////////////////////////////////////////////////////
/////////////////////////////  CommandQuaternion  //////////////////////////////
////////////////////////////////////////////////////////////////////////////////

/*****************************************************************************
 * Apply graphical rotation by quaternion.
 * Quaternions only define rotation.
 *****************************************************************************/
CommandQuaternion::CommandQuaternion( const Quaternion& quat )
:   mQuat(quat)
{
}

void
CommandQuaternion::Execute( shptr<Aircraft> aircraft, const ExecuteMode mode )
{
CHECK_TYPESIG(this,TYPESIG_COMMAND);

    if ( mode & EXECUTE_MODE_GRAPHICS )
    {
        aircraft->SetQuaternion( mQuat );
    }
}

void
CommandQuaternion::Write( std::ostream& strm )
{
CHECK_TYPESIG(this,TYPESIG_COMMAND);

    strm << COMMAND_NAME_QUATERNION << ' '
         << mQuat << ' ';  // omit std::endl
}

////////////////////////////////////////////////////////////////////////////////
///////////////////////////  CommandPhysicalRotate  ////////////////////////////
////////////////////////////////////////////////////////////////////////////////

/*****************************************************************************
 * Rotate according to physics model.
 *****************************************************************************/
CommandPhysicalRotate::CommandPhysicalRotate( uint axis, fp controlFraction, Milliseconds controlFreq )
:   mAxis(axis),
    mControlFraction(controlFraction),
    mControlFreq(controlFreq)
{
ASSERT_AXIS3(axis);
}

void
CommandPhysicalRotate::Execute( shptr<Aircraft> aircraft, const ExecuteMode mode )
{
CHECK_TYPESIG(this,TYPESIG_COMMAND);

    if ( mode & EXECUTE_MODE_PHYSICS )
        aircraft->PhysicalRotate( mAxis, mControlFraction, mControlFreq );
}

void
CommandPhysicalRotate::Write( std::ostream& strm )
{
CHECK_TYPESIG(this,TYPESIG_COMMAND);
ASSERT_AXIS3(mAxis);

    strm << COMMAND_NAME_PHYSICAL_ROTATE << ' '
         << mAxis << ' '
         << mControlFraction << ' '
         << mControlFreq << ' ';  // omit std::endl
}

////////////////////////////////////////////////////////////////////////////////
///////////////////////////////  CommandThrottle  //////////////////////////////
////////////////////////////////////////////////////////////////////////////////

/*****************************************************************************
 * Set throttle command.
 *****************************************************************************/
CommandThrottle::CommandThrottle( const fp throttle )
:   mThrottle(throttle)
{
}

void
CommandThrottle::Execute( shptr<Aircraft> aircraft, const ExecuteMode mode )
{
CHECK_TYPESIG(this,TYPESIG_COMMAND);

    if ( mode & EXECUTE_MODE_PHYSICS )
        aircraft->SetThrottle( mThrottle );
}

void
CommandThrottle::Write( std::ostream& strm )
{
CHECK_TYPESIG(this,TYPESIG_COMMAND);

    strm << COMMAND_NAME_THROTTLE << ' ' << mThrottle << ' ';  // omit std::endl
}

////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////  CommandSpeed  ///////////////////////////////
////////////////////////////////////////////////////////////////////////////////

/*****************************************************************************
 * Set speed command.
 *****************************************************************************/
CommandSpeed::CommandSpeed( const Speed speed )
:   mSpeed(speed)
{
}

void
CommandSpeed::Execute( shptr<Aircraft> aircraft, const ExecuteMode mode )
{
CHECK_TYPESIG(this,TYPESIG_COMMAND);

    if ( mode & EXECUTE_MODE_PHYSICS )
        aircraft->GetPhysics().SetSpeed( mSpeed );
}

void
CommandSpeed::Write( std::ostream& strm )
{
CHECK_TYPESIG(this,TYPESIG_COMMAND);

    // Record as KPH.
    const fp kph = physics::conv::Speed2KPH(mSpeed).FP();
    strm << COMMAND_NAME_SPEED << ' ' << kph << ' ';  // omit std::endl
}

////////////////////////////////////////////////////////////////////////////////
///////////////////////////  CommandLandingGear  ///////////////////////////////
////////////////////////////////////////////////////////////////////////////////

/*****************************************************************************
 * Set landing gear command.
 *****************************************************************************/
CommandLandingGear::CommandLandingGear( const bool down )
:   mDown(down)
{
}

void
CommandLandingGear::Execute( shptr<Aircraft> aircraft, const ExecuteMode mode )
{
CHECK_TYPESIG(this,TYPESIG_COMMAND);

    if ( mode & EXECUTE_MODE_PHYSICS )
        aircraft->SetLandingGear( mDown );
}

void
CommandLandingGear::Write( std::ostream& strm )
{
CHECK_TYPESIG(this,TYPESIG_COMMAND);

    strm << COMMAND_NAME_LANDING_GEAR << ' ' << mDown << ' ';  // omit std::endl
}

////////////////////////////////////////////////////////////////////////////////
/////////////////////////////  CommandBrakes  //////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

/*****************************************************************************
 * Set brakes command.
 *****************************************************************************/
CommandBrakes::CommandBrakes( const bool brakes )
:   mBrakes(brakes)
{
}

void
CommandBrakes::Execute( shptr<Aircraft> aircraft, const ExecuteMode mode )
{
CHECK_TYPESIG(this,TYPESIG_COMMAND);

    if ( mode & EXECUTE_MODE_PHYSICS )
        aircraft->EnableBrakes( mBrakes );
}

void
CommandBrakes::Write( std::ostream& strm )
{
CHECK_TYPESIG(this,TYPESIG_COMMAND);

    strm << COMMAND_NAME_BRAKES << ' ' << mBrakes << ' ';  // omit std::endl
}

} // namespace control
