/*
 * VehicleSimulator.h
 *
 *  Created on: Aug 15, 2010
 *      Author: Dula Nad
 *
 *  This class implements a 6 DOF rigid-body dynamics/kinematics in a form
 *  of a vehicle driver. From a plugin perspective a control algorithm will
 *  not know the difference between the real vehicle and it's simulation.
 *
 */
#ifndef VEHICLESIMULATOR_H_
#define VEHICLESIMULATOR_H_

#include <VehicleDriver.h>
#include <MarineVehicleModel.h>
#include <boost/random/inversive_congruential.hpp>

namespace LABUST
{
  /**
   * The SIMULATION namespace contains classes and functions that allow for easy simulation
   * of real-life systems.
   */
  namespace SIMULATION
  {
    /**
     * This class implements a 6 DOF rigid-body dynamics/kinematics in a form
     * of a vehicle driver. From a plugin perspective a control algorithm will
     * not know the difference between the real vehicle and it's simulation.
     *
     * \see LABUST::VEHICLES::Driver
     */
    class VehicleSimulator : public LABUST::VEHICLES::Driver
    {
    public:
      /**
       * The vehicle simulator constructor.
       *
       * \see LABUST::SIMULATION::MatrixModel
       */
      VehicleSimulator();
      /*
       * Vehicle destructor frees the allocated memory.
       */
      virtual ~VehicleSimulator();

      /**
       * \overload LABUST::XMLConfigurable::configure
       */
      virtual int configure();

      /**
       * \overload LABUST::VEHICLES::Driver::setTAU
       */
      virtual int setTAU(const VEHICLES::COREDATASET::tauMap& tau) ;
      /**
       * \overload LABUST::VEHICLES::Driver::getState
       */
      virtual int getState(VEHICLES::COREDATASET::stateMap& state);

    public:
      MarineVehicleModel model;

    private:
      /**
       * Calculate one random element for noise
       */
      double getNoise();
      /**
       * Condition specifying whether to add noise
       */
      bool addNoise;
      /**
       * Additive noise coeficients.
       */
      c_matrix<double,1,12> noise;
      /**
       * Noise generator
       */
      boost::hellekalek1995 rand;
    };
  };

  namespace PLUGINS
  {
    /**
     * This is a plugin factory for LABUST::SIMULATION::VehicleSimulator.
     */
    class SimulatorFactory : public DriverFactory
    {
    public:
      SimulatorFactory(){};
      virtual ~SimulatorFactory(){};

      /**
       * This creates an instance of the simulator.
       */
      virtual VEHICLES::Driver* createInstance(LABUST::COMMUNICATION::CommsInterface& comms) const
            {
              return new LABUST::SIMULATION::VehicleSimulator();
            }
    };
  };

  extern "C"
  {
    LABUST::PLUGINS::SimulatorFactory* getDriverFactory();
  }
};

#endif /* VEHICLESIMULATOR_H_ */
