/*
 * VehicleSimulator.cpp
 *
 *  Created on: Aug 15, 2010
 *      Author: Dula Nad
 *
 *  This is the implementation of the VehicleSimulator class.
 */
#include "NullCommsInterface.h"
#include "VehicleSimulator.h"
#include <string>
#include <iostream>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>

using LABUST::SIMULATION::VehicleSimulator;
using boost::numeric::ublas::matrix_row;


VehicleSimulator::VehicleSimulator():
  Driver(*(new LABUST::COMMUNICATION::NullCommsInterface()),false),
  addNoise(false),
  noise(boost::numeric::ublas::zero_matrix<double>(1,12)){};

VehicleSimulator::~VehicleSimulator(){};

int VehicleSimulator::configure()
{
  using namespace boost::numeric::ublas;
  //Get inertial parameters
  double m,g;
  c_matrix<double,3,3> Io = zero_matrix<double>(3);
  c_matrix<double,1,3> rg = zero_matrix<double>(1,3);

  bool flag = getValue("mass",m) == 0;
  flag = flag && (getValue("gravity",g) == 0);
  flag = flag && (getMatrix("inertia-matrix",Io) == 0);
  flag = flag && (getMatrix("rg",rg) == 0);
  model.setInertiaParameters(m,Io,matrix_row<c_matrix<double,1,3> >(rg,0),g);

  //Get added mass
  c_matrix<double,6,6> Ma = zero_matrix<double>(6);
  flag = flag && (getMatrix("added-mass",Ma) == 0);
  model.setAddedMass(Ma);

  //Get damping
  c_matrix<double,6,6> Dlin = zero_matrix<double>(6);
  c_matrix<double,6,6> Dquad = zero_matrix<double>(6);
  flag = flag && (getMatrix("linear-damping",Dlin) == 0);
  flag = flag && (getMatrix("quadratic-damping",Dquad) == 0);
  model.setDampingMatrix(Dlin,Dquad);

  //Get buoyancy
  double rho,B;
  c_matrix<double,1,3> rb = zero_matrix<double>(1,3);
  flag = flag && (getValue("rho",rho) == 0);
  flag = flag && (getValue("lift-force",B) == 0);
  flag = flag && (getMatrix("rb",rb) == 0);
  model.setBuoyancyInfo(rho,B,matrix_row<c_matrix<double,1,3> >(rb,0));

  //Get initial
  c_matrix<double,1,6> nu0 = zero_matrix<double>(1,6);
  c_matrix<double,1,6> eta0 = zero_matrix<double>(1,6);
  flag = flag && (getMatrix("nu0",nu0) == 0);
  flag = flag && (getMatrix("eta0",eta0) == 0);
  model.setNuAndEta(matrix_row<c_matrix<double,1,6> >(nu0,0),matrix_row<c_matrix<double,1,6> >(eta0,0));

  double Ts;
  flag = flag && (getValue("sampling-time",Ts) == 0);
  model.setTs(Ts);

  addNoise = getMatrix("noise",noise) == 0;

  std::cout<<model;
  std::cout<<"Noise vector:"<<noise;
  return flag?0:1;
}

int VehicleSimulator::setTAU(const LABUST::VEHICLES::COREDATASET::tauMap& tau)
{
  using namespace boost::numeric::ublas;
  c_vector<double,6> cmd = zero_vector<double>(6);
  using namespace LABUST::VEHICLES::COREDATASET;
  tauMap::const_iterator element;
  element = tau.find(X);
    cmd(0) = element!=tau.end() ? element->second : 0;
  element = tau.find(Y);
    cmd(1) = element!=tau.end() ? element->second : 0;
  element = tau.find(Z);
    cmd(2) = element!=tau.end() ? element->second : 0;
  element = tau.find(K);
    cmd(3) = element!=tau.end() ? element->second : 0;
  element = tau.find(M);
    cmd(4) = element!=tau.end() ? element->second : 0;
  element = tau.find(N);
    cmd(5) = element!=tau.end() ? element->second : 0;
  model.step(cmd);
  return 0;
}

int VehicleSimulator::getState(LABUST::VEHICLES::COREDATASET::stateMap& state)
{
  using namespace boost::numeric::ublas;
  using namespace LABUST::VEHICLES::COREDATASET;
  c_vector<double,6> nu = model.getNu();
  c_vector<double,6> eta = model.getEta();

  for (int i=0;i<6;++i)
  {
    if (addNoise)
    {
      eta(i) += noise(0,i)*getNoise();
      nu(i) += noise(0,i+6)*getNoise();
    }

    state[(stateElement)(x+i)]=eta(i);
    state[(stateElement)(u+i)]=nu(i);
  }

  state[yaw] = wrapRad(state[yaw]);
  state[roll] = wrapRad(state[roll]);
  state[pitch] = wrapRad(state[pitch]);

  return 0;
}

double VehicleSimulator::getNoise()
{
 return ((double)rand() - rand.max()/2.)/rand.max();
}

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

