/*
 * MarineVehicleModel.h
 *
 *  Created on: Aug 15, 2010
 *      Author: Dula Nad
 *
 *  This class implements a 6DOF marine vehicle model. This model is based on
 *  the equations of motion derived in chapter 2 of Guidance and control of
 *  Ocean Vehicles by Fossen (1994).
 */

#ifndef MARINEVEHICLEMODEL_H_
#define MARINEVEHICLEMODEL_H_

#include <string>
#include <sstream>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>

using boost::numeric::ublas::c_matrix;
using boost::numeric::ublas::c_vector;

#ifndef NDEBUG
   #define NDEBUG
#endif

namespace LABUST
{
  namespace SIMULATION
  {
    /**
     *  This class implements a 6DOF marine vehicle model. This model is based on
     *  the equations of motion derived in chapter 2 of Guidance and control of
     *  Ocean Vehicles by Fossen (1994).
     *
     *  This class parameters can be specified with an XML configuration file.
     *  Matrix manipulation is done by use of the uBlas library. The model is
     *  executed with a sampling time defined in the configuration file.
     *
     *  Enable #define NDEBUG for release code. Increases uBlas performance.
     *
     *  \see Boost uBlas library: http://www.boost.org/doc/libs/1_43_0/libs/numeric/ublas/doc/index.htm
     */
    class MarineVehicleModel
    {
    public:
      /**
       * The default no-parameter constructor
       */
      MarineVehicleModel();
      /**
       * This constructor allows for XML configuration of model parameters
       */
      MarineVehicleModel(const std::string& configPath, const std::string& modelID);
      /**
       * Default destructor.
       */
      virtual ~MarineVehicleModel();

      /**
       * Set the sampling time
       *
       * \param dT The sampling time
       */
      void setTs(double dT);
      /**
       * Set mass, inertia tensor and center of gravity vector. Mrb matrix is directly calculated from these.
       *
       * \param m Vehicle mass
       * \param Io Rigid-body inertia matrix
       * \param rg Center of gravity vector
       */
      void setInertiaParameters(double m, const c_matrix<double,3,3>& Io,const c_vector<double,3>& rg, double g = 9.81);
      /**
       * Set Ma matrix.
       *
       * \param Ma Added mass parameters
       */
      void setAddedMass(const c_matrix<double,6,6>& Ma);
      /**
       * Set Dlin and Dquad matrices.
       *
       * \param Dlin Linear part of the damping matrix
       * \param Dquad Quadratic part of the damping matrix
       */
      void setDampingMatrix(const c_matrix<double,6,6>& Dlin,const c_matrix<double,6,6>& Dquad);
      /**
       * Set buoyancy data.
       *
       * \param rho Water density
       * \param B Lift force
       * \param rb Center of buoyancy
       */
      void setBuoyancyInfo(double rho, double B, const c_vector<double,3>& rb);
      /**
       * Set initial states.
       *
       * \param nu Inital speeds in the body-fixed reference frame
       * \param eta Inital positions in the earth-fixed reference frame
       */
       void setNuAndEta(const c_vector<double,6>& nu,const c_vector<double,6>& eta);

      /**
       * Returns the NU velocity vector reference
       */
      const c_vector<double,6>& getNu(void) const;
      /**
       * Returns the ETA velocity vector reference
       */
      const c_vector<double,6>& getEta(void) const;

      /**
       * Perform one step of simulation
       *
       * \param tau Input forces and moment vector
       */
      void step(const c_vector<double,6>& tau);
      /**
       * This friend function outputs the model matrices.
       */
      friend std::ostream& operator<<(std::ostream& s,const MarineVehicleModel& model);

    protected:
      /**
       * This methods performs the general setup from the configuration file
       */
      void setup();
      /**
       * This methods performs the matrix setup from the configuration file
       */
      void setupMatrix();

      /**
       * Assemble the Crb and Ca matrices
       */
      void asmCoriolis();
      /**
       * Assemble the Jacobian matrix
       *
       * \param J1 Matrix reference for the upper part of Jacobian matrix
       * \param J2 Matrix reference for the lower part of Jacobian matrix
       */
      void asmJacobian(c_matrix<double,3,3>& J1, c_matrix<double,3,3>& J2);
      /**
       * Assemble the restoring forces vector.
       *
       * \param invJ1 Matrix inverse of the upper part of Jacobian matrix
       */
      void asmRestoringForces(const c_matrix<double,3,3>& invJ1);

      /**
       * This is the sample time used for calculation.
       */
      double dT;

      /**
       * Speeds in the body-fixed reference system.
       * The value is initialized to nu0.
       */
      c_vector<double,6> nu;
      /**
       * Positions in the earth-fixed reference system
       * The value is initialized to eta0.
       */
      c_vector<double,6> eta;
      /**
       * Center of gravity vector. Used to construct the Mrb matrix.
       */
      c_vector<double,3> rg;
      /**
       * Center of buoyancy vector. Used to construct the g vector.
       */
      c_vector<double,3> rb;
      /**
       * Vehicle mass. Used to construct the Mrb matrix.
       */
      double m;
      /**
       * Vehicle lift force. Used to construct the g vector.
       */
      double B;
      /**
       * Water density. Used to construct the g vector.
       */
      double rho;
      /**
       * Gravity acceleration. Used to construct the g vector.
       */
      double g_acc;
      /**
       * Forces and moments vector
       */
      c_vector<double,6> tau;
      /**
       * Body inertia tensor. Used to construct the Mrb matrix.
       */
      c_matrix<double,3,3> Io;
      /**
       * Rigid-body inertia matrix
       */
      c_matrix<double,6,6> Mrb;
      /**
       * Coriolis and centripetal matrix Crb is calculated from this
       * and state vector.
       */
      c_matrix<double,6,6>Crb;
      /**
       * Rigid-body added inertia matrix
       */
      c_matrix<double,6,6> Ma;
      /**
       * Coriolis and centripetal added forces matrix Ca is calculated
       * from this and state vector.
       */
      c_matrix<double,6,6> Ca;
      /**
       * Hydrodynamic damping matrix - linear part
       */
      c_matrix<double,6,6> Dlin;
      /**
       * Hydrodynamic damping matrix - quadratic part
       */
      c_matrix<double,6,6> Dquad;
      /**
       * Hydrodynamic damping matrix
       * Calculated interactively if needed.
       */
      c_matrix<double,6,6> D;
      /**
       * Restoring forces matrix
       * If it depends on the state vector is calculated interactively
       * otherwise it is constant.
       */
      c_vector<double,6> g;

      //XML VARIABLES SHOULD GO BELOW HERE
    };

    //Friend function definition so that we don't put it into a separate file
    std::ostream& operator<<(std::ostream& s,const MarineVehicleModel& model);
  };
};

#endif /* MARINEVEHICLEMODEL_H_ */
