/** \file
 *
 *  Contains the Simulator class declaration.
 *
 *  Copyright (c) 2007,2008,2009 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 */

#ifndef SIMULATOR_H_
#define SIMULATOR_H_

#include "SimInitStruct.h"
#include "SimResultStruct.h"
#include "SimRunStruct.h"

#include "data/Location.h"
#include "data/Matrix6x6.h"
#include "data/Point3D.h"
#include "data/Point6D.h"
#include "ActuatorModel.h"
#include "ThrusterModel.h"
#include "utils/AuvMath.h"

/**
 *  A simulator "evolved" from the Altex and Odyssey sims.
 *
 *  The Simulator starts with the Odyssey and Altex simulation code but
 *  takes greater liberties to make the code easier to use (i.e. using
 *  matrix math libraries...)
 *
 *  All three simulators are very similar.  All use an iterative
 *  fourth-order Runge-Kutta method (see Simulator::motion for
 *  more details) to solve (F = ma) where F are the various hydrodynamic forces
 *  on the vehicle (body drag, fins, thrust, etc). m is the "mass" of the vehicle
 *  including its actual mass as well as added mass terms to account for
 *  its shape.  The state variable "a" (or more appropriately vdot) includes
 *  all three linear accelerations
 *  and all three angular accelerations.
 *
 *  The core state of the vehicle then (called "rate" and "pos" for most of the code) is
 *  the three linear velocities and three angular rates, plus the three
 *  cardinal axes (x,y,z) and three angles.
 *
 *  The function Simulator::invertMass() is responsible for generating
 *  the inverse mass matrix and is called once at initialization.
 *
 *  The functions Simulator::motion() performs one "step" of the
 *  simulation, running the R-K method once.  It calls Simulator::calcForce()
 *  to calculate the forces on the vehicle, which in turn calls Simulator::calcFinForces()
 *  to calculate forces and moments from the fins.  Small encapsulation
 *  classes (rudderModel_, elevatorModel_, thrusterModel_)are used to abstract away some of the simulated qualities (backlash,
 *  delay) of the fins and thruster.
 *
 *  \ingroup modules_simulator
 */
class Simulator
{

public:
    /// Constructor.
    Simulator( bool simulateSensors, int nSteps, bool debug = false );

    /// Destructor.
    virtual ~Simulator();

    /// Initialize the simulator
    void initialize( const SimInitStruct& initParams, SimResultStruct& results );

    /// Run the simulation
    void run( const SimRunStruct& runParams, SimResultStruct& results );

    /// Uninitialize the simulation
    virtual void uninitialize() {}

    /// The "take one timestep" function for the simulator.
    void motion( float propOmegaAction, float rudderAngleAction,
                 float elevatorAngleAction, float massPositionAction,
                 float buoyancyAction, int dropWeightState,
                 float density, float dt,
                 SimResultStruct& results );

    /// Set the current location to the indicated values in radians
    /// Returns error code conversion to UTM is unsuccessful. Otherwise returns 0.
    /// Use Wgs84::UtmErrorToString( error ) to decode the error (from Datum.h).
    long setLocation( const double& latitude, const double& longitude );

    /// Set the current depth to the indicated value
    void setDepth( float depth );

    /// Set the current heading to the indicated value
    void setHeading( float heading );

    /// Set the current pitch to the indicated value
    void setPitch( float pitch );

    /// Set the current roll to the indicated value
    void setRoll( float roll );

    /// Set the current prop omega to the indicated value
    void setPropOmega( float propOmega );

    /// Set the current rudder angle to the indicated value
    void setRudderAngle( float rudderAngle );

    /// Set the current elevator angle to the indicated value
    void setElevatorAngle( float elevatorAngle );

    /// Set the current mass position to the indicated value
    void setMassPosition( float massPosition );

    /// Set the current mass position to the indicated value
    void setBuoyancyPosition( float buoyancyPosition );

    void getLocation( double& latitude, double& longitude )
    {
        latitude = latitude_;
        longitude = longitude_;
    }

private:
    // Note that the copy constructor below is private and not given a body.
    // Any attempt to call it will return a compiler error.
    Simulator( const Simulator& old ); // disallow copy constructor

protected:

    virtual void publishState( const double time, SimResultStruct& results );

    virtual void configureSensors() {}

    virtual void simulateSensors( double time, double latDeg, double lonDeg, SimResultStruct& results ) {}

    /// Load the hydrodynamic properties from the slate and
    /// initialize some variables.
    bool loadParams( const SimInitStruct& initParams, SimResultStruct& results );

    /// Calculate the LHS of the equation (the "1/mass" for solving a in ma = F).
    //ReturnMatrix invertMass( void );
    void invertMass( SimResultStruct& results );

    /// Calculate the RHS of the equation (the "force" in ma = F).
    void calcForce( Point6D &rate, Point6D &pos, float propOmegaAction,
                    float rudderAngleAction, float elevatorAngleAction,
                    float massPositionAction, float buoyancyAction,
                    int dropWeightState, float density, float dt, float smallDt );

    //------------------ Helper functions --------------------------------

    /// Given the vehicle state (rate) and fin angles, computes the forces and
    /// moments from the fins in body coords.
    void calcFinForces( Point6D &rate, float ruddAngleProj, float elevAngleProj,
                        Point3D finForces[], Point3D finMoments[], float density );

    //*------------------- static constant variables ----------------------------*/

    /// Defines fins
    enum
    {
        LO_RUDD,   // Lower Rudder
        PO_ELEV,   // Port Elevator
        UP_RUDD,   // Upper Rudder
        ST_ELEV,   // Starboard Elevator
    };

    ///< "Oswald" efficiency factor
#define EFF_FACTOR 0.90

    //*------------------- other member variables ----------------------------*/

    /// indicates whether to simulate sensors (salinity, temp, u, v)
    bool simulateSensors_;

    /// filename of file that contains ocean model run
    Str oceanModelData_;

    /// number of extra variables in ocean model data
    int oceanModelVarCount_;

    /// names of extra variables in ocean model data
    Str* oceanModelVarNames_;

    /// units of extra variables in ocean model data
    Str* oceanModelVarUnits_;

    /// The simulator actually runs minNSteps_ times per "timestep"
    /// (i.e. if the control thread runs at 5Hz, the sim runs at 50Hz if nSteps = 10
    int minNSteps_;

    /// Largest allowable timestep
    float maxDt_;

    /// indicates whether things are ok to run the simulation
    bool ok_;

    /// Actuator and thruster models
    ActuatorModel elevatorModel_;
    ActuatorModel rudderModel_;
    ActuatorModel movableMassModel_;
    ActuatorModel buoyancyModel_;
    ThrusterModel thrusterModel_;

    /// Inverse of "mass" matrix.  Calculated just once in invertMass() called from initialize()
    Matrix6x6 massInvert_;

    /// buoyancy force
    float netBuoy_;

    /// 6-dimensional force vector
    Point6D force_;

    /// Thruster outputs
    float propThrust_, propTorque_;

    /// 6-dimensional position vector, body coordinates
    Point6D pos_;
    Point6D posDot_;

    /// 6-dimensional rate vector, earth coordinates
    Point6D rate_;
    Point6D rateDot_;

    /// UTM zone gathered from initial position
    //unsigned int utmZone_;
    //bool northernHemi_;

    // WGS84 position
    double latitude_;
    double longitude_;

    /// Ocean current (m/s north, east,down)
    Point3D current_;

    float northCurrent_; /// Static northward current.
    float eastCurrent_; /// Static eastward current.
    float vertCurrent_; /// Static vertical current (positive downward)
    float magneticVariation_; // Overrides value calculated from lat & lon
    float soundSpeed_;        // Overrides value calculated from density, pressure
    float density_;           // Overrides sea water density from ocean model.
    float sst_; // Overrides sea surface temperature from ocean model
    float tMixed_; // Overrides mixed layer depth temperature from ocean model
    float t300_; // Overrides temperature at 300 meters from ocean model
    float sss_; // Overrides sea surface salinity from ocean model
    float sMixed_; // Overrides mixed layer depth salinity from ocean model
    float s300_; // Overrides salinity at 300 meters from ocean model
    float mixedLayerDepth_; // Overrides mixed layer depth from ocean model

    /// Default density to use in calculating buoyancy if none is measured
    float defaultDensity_;

    // Variable buoyancy setting
    float buoyancyNeutral_;     // cc    // volume in buoyancy "bag" at neutral tri,.

    /// Last measured density
    float lastDensity_;

    ///*------------------- vehicle parameters --------------------------*/
    //
    //   x is fwd, y stbd, z down.
    //

    //
    ///* translational and rotational body mass */
    //
    float mass_;
    float movableMass_;
    float volume_;
    Point3D cOfMass_;
    Point3D cOfMovableMass_;
    Point3D cOfBuoy_;
    Point6D moment_;

    // Dropweight1 parameters
    float dropWt1Volume_;    /* volume of drop weight #1 */
    float dropWt1Mass_; /* mass of drop weight #1 */
    float dropWt1X_; /* location of drop weight #1 */
    float dropWt1Y_;
    float dropWt1Z_;

    // Simple geometry model of vehicle
    float cylinderLength_;
    float cylinderRadius_;

    //
    ///* hydrodynamic parameters ----------------------------------------------*/
    //
    //
    /* added mass */
    float kpDot_;
    float mqDot_;
    float nrDot_;
    float xuDot_;
    float yvDot_;
    float zwDot_;

    /* added mass cross terms */
    float kvDot_;
    float mwDot_;
    float nvDot_;
    float yrDot_;
    float ypDot_;
    float zqDot_;

    //                /* quadratic drag */
    float kpabp_;  // Kp|p| , kg-m^2
    float mqabq_;  // Mq|q| , kg-m^2
    float nrabr_;  // Nr|r| , kg-m^2
    float xuabu_;
    float yvabv_;  // Yv|v| , kg/m
    float zwabw_;  // Zw|w| , kg/m
    //
    //                /* quadratic drag cross terms */
    float mwabw_;  // Mw|w| , kg
    float nvabv_;  // Nv|v| , kg
    float yrabr_;  // Yr|r| , ?
    float zqabq_;  // Zq|q| , ?
    //
    //                /* in-line lift and drag */
    float muq_;  // Muq   , kg-m
    float muw_;  // Muw   , kg
    float mpr_;  // Mpr   , kg-m^2
    //
    float nur_;  // Nur   , kg-m
    float nuv_;  // Nuv   , kg
    float npq_;  // Npq   , kg-m^2
    //
    float xvv_;  // Xvv   , kg/m
    float xww_;  // Xww   , kg/m
    float xvr_;  // Xvr   , kg
    float xwq_;  // Xwq   , kg
    float xrr_;  // Xrr   , kg-m
    float xqq_;  // Xqq   , kg-m

    float yur_;  // Yur   , kg
    float yuv_;  // Yuv   , kg/m
    float ywp_;  // Ywp   , kg-m

    float zuq_;  // Zuq   , kg
    float zuw_;  // Zuw   , kg/m
    float zvp_;  // Zvp   , kg

    float kvt2_;  // ?     , ?

    float aspectRatio_;  //   , Unitless
    float stallAngle_;  //   , Deg.

    float finArea_;  //   , m^2
    float coeffDrag_;  // Coef. of Drag  , Unitless
    float coeffLift_;  // Coef. of Lift  , Unitless

    //
    ///* FINS.  Note fin #1 is lower rudder, #2 is port elevator, #3 is
    //   upper rudder, #4 is stbd elevator.  This fin model is taken
    //   directly from PNA, which cites Whicker and Fehlner, 1958.  */
    //
    Point3D fins_[4];

    // Set to true for debugging output
    bool debug_;

};

#endif /*SIMULATOR_H_*/