/*
 * MarineVehicleModel.cpp
 *
 *  Created on: Aug 15, 2010
 *      Author: dnad
 */

#include "MarineVehicleModel.h"
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/banded.hpp>
#include "MatrixOperations.h"
#include <algorithm>

using LABUST::SIMULATION::MarineVehicleModel;
using boost::numeric::ublas::zero_matrix;
using boost::numeric::ublas::zero_vector;
using boost::numeric::ublas::identity_matrix;
using boost::numeric::ublas::unit_vector;
using boost::numeric::ublas::diagonal_matrix;


//Constructors and destructors
MarineVehicleModel::MarineVehicleModel():
    dT(0.1),
    nu(zero_vector<double>(6)),
    eta(zero_vector<double>(6)),
    rg(zero_vector<double>(3)),
    rb(zero_vector<double>(3)),
    m(1),
    B(9.81),
    rho(1000),
    g_acc(9.81),
    tau(zero_vector<double>(6)),
    Io(identity_matrix<double>(3)),
    Mrb(identity_matrix<double>(6)),
    Crb(zero_matrix<double>(6)),
    Ma(identity_matrix<double>(6)),
    Ca(zero_matrix<double>(6)),
    Dlin(zero_matrix<double>(6)),
    Dquad(zero_matrix<double>(6)),
    D(zero_matrix<double>(6)),
    g(zero_vector<double>(6))
{};

MarineVehicleModel::MarineVehicleModel(const std::string& configPath, const std::string& modelID):
    dT(0.1),
    nu(zero_vector<double>(6)),
    eta(zero_vector<double>(6)),
    rg(zero_vector<double>(3)),
    rb(zero_vector<double>(3)),
    m(1),
    B(9.81),
    rho(1000),
    g_acc(9.81),
    tau(zero_vector<double>(6)),
    Io(identity_matrix<double>(3)),
    Mrb(identity_matrix<double>(6)),
    Crb(zero_matrix<double>(6)),
    Ma(identity_matrix<double>(6)),
    Ca(zero_matrix<double>(6)),
    Dlin(zero_matrix<double>(6)),
    Dquad(zero_matrix<double>(6)),
    D(zero_matrix<double>(6)),
    g(zero_vector<double>(6))
{
  //SETUP XML READING AND CALL UPON SETUP METHODS
};

MarineVehicleModel::~MarineVehicleModel(){};

//Setter methods
void MarineVehicleModel::setTs(double dT)
{
  this->dT = dT;
}

void MarineVehicleModel::setInertiaParameters
  (double m, const c_matrix<double,3,3>& Io,const c_vector<double,3>& rg, double g)
{
  this->m = m;
  this->Io = Io;
  this->rg = rg;
  this->g_acc = g;

  //calculate Mrb
  subrange(Mrb, 0,3, 0,3) = m*identity_matrix<double>(3,3);
  subrange(Mrb, 0,3, 3,6) = -m*skewSymm3(rg);
  subrange(Mrb, 3,6, 0,3) = -m*skewSymm3(rg);
  subrange(Mrb, 3,6, 3,6) = Io;
}

void MarineVehicleModel::setAddedMass(const c_matrix<double,6,6>& Ma)
{
  this->Ma = Ma;
}

void MarineVehicleModel::setDampingMatrix(const c_matrix<double,6,6>& Dlin,const c_matrix<double,6,6>& Dquad)
{
  this->Dlin = Dlin;
  this->Dquad = Dquad;
}

void MarineVehicleModel::setBuoyancyInfo(double rho, double B,const c_vector<double,3>& rb)
{
  this->rho = rho;
  this->rb = rb;
  this->B = B;
}

void MarineVehicleModel::setNuAndEta(const c_vector<double,6>& nu,const c_vector<double,6>& eta)
{
  this->nu = nu;
  this->eta = eta;
}

//Getters

const c_vector<double,6>& MarineVehicleModel::getNu(void) const
{
  return this->nu;
}

const c_vector<double,6>& MarineVehicleModel::getEta(void) const
{
  return this->eta;
}

//Main methods

void MarineVehicleModel::step(const c_vector<double,6>& tau)
{
  //Set forces and moments
  this->tau = tau;
  //In the future external forces and moments due to rudder deflection,
  //thruster rotation, etc. can be simulated as well

  c_matrix<double,3,3> J1(3,3),J2(3,3),invJ1(3,3);

  asmJacobian(J1,J2);
  inv<double>(J1,invJ1);
  asmCoriolis();
  asmRestoringForces(invJ1);

  vector<double> nu_abs(6);
  std::transform(nu.begin(),nu.end(),nu_abs.begin(),static_cast<double (*)(double)>(fabs));

  c_matrix<double,6,6> invM;

  if (!inv<double>(Ma + Mrb,invM))
  {
    std::cout<<"Inertia matrix is singular."<<std::endl;
  }

  c_matrix<double,6,6> D = Dlin + prod(Dquad,diagonal_matrix<double>(nu_abs.size(),nu_abs.data()));
  c_vector<double,6> CDv = prod(Ca + Crb + D,nu);

  nu += dT*prod(invM,this->tau - CDv - g);

  subrange(eta, 0,3) += dT*prod(J1,subrange(nu, 0,3));
  subrange(eta, 3,6) += dT*prod(J2,subrange(nu, 3,6));
}

std::ostream& LABUST::SIMULATION::operator<<(std::ostream& s,const MarineVehicleModel& model)
{
  using boost::numeric::ublas::operator<<;
  s<<"Sample time:"<<model.dT<<std::endl;
  s<<"Current state:\n\t Nu:"<<model.nu<<"\n\tEta:"<<model.eta<<std::endl;
  s<<"Inertia matrices:\n\tMrb:"<<model.Mrb<<"\n\t Ma:"<<model.Ma<<std::endl;
  s<<"Coriolis matrices:\n\tCrb:"<<model.Crb<<"\n\t Ca:"<<model.Ca<<std::endl;
  s<<"Damping matrices:\n\t Dlin:"<<model.Dlin<<"\n\tDquad:"<<model.Dquad<<std::endl;
  s<<"Gravity and buoyancy:\n\trg:"<<model.rg<<"\n\trb:"<<model.rb<<std::endl;
  s<<"Inertia tensor:\n\tIo:"<<model.Io<<std::endl;
  s<<"Restoring forces:\n\tg:"<<model.g<<std::endl;
  s<<"Tau:"<<model.tau<<std::endl;
  s<<"Other:\n\tg_acc:"<<model.g_acc<<"\n\t  rho:"<<model.rho<<"\n\t    B:"<<model.B<<"\n\t    m:"<<model.m<<std::endl;

  return s;
}

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

void MarineVehicleModel::asmJacobian(c_matrix<double,3,3>& J1,c_matrix<double,3,3>& J2)
{
  double c1 = cos(eta(3)), s1 = sin(eta(3));
  double c2 = cos(eta(4)), s2 = sin(eta(4)), t2 = tan(eta(4));
  double c3 = cos(eta(5)), s3 = sin(eta(5));

  J1(0,0)=c3*c2;
  J1(0,1)=c3*s2*s1-s3*c1;
  J1(0,2)=s3*s1+c3*c1*s2;
  J1(1,0)=s3*c2;
  J1(1,1)=c1*c3+s1*s2*s3;
  J1(1,2)=c1*s2*s3-c3*s1;
  J1(2,0)=-s2;
  J1(2,1)=c2*s1;
  J1(2,2)=c1*c2;

  J2(0,0)=1;
  J2(0,1)=s1*t2;
  J2(0,2)=c1*t2;
  J2(1,0)=J2(2,0)= 0;
  J2(1,1)=c1;
  J2(1,2)=-s1;
  J2(2,1)=s1/c2;
  J2(2,2)=c1/c2;
}

void MarineVehicleModel::asmCoriolis()
{
  using namespace LABUST::SIMULATION;
  using namespace boost::numeric::ublas;

  //We assume that Mrb and Ma matrices are set
  c_vector<double,3> nu1 = subrange(nu, 0, 3);
  c_vector<double,3> nu2 = subrange(nu, 3, 6);

  subrange(Crb, 0,3, 0,3) = zero_matrix<double>(3,3);
  subrange(Crb, 0,3, 3,6) = -m*skewSymm3(nu1) - m*prod(skewSymm3(nu2),skewSymm3(rg));
  subrange(Crb, 3,6, 0,3) = -m*skewSymm3(nu1) + m*prod(skewSymm3(rg),skewSymm3(nu2));
  subrange(Crb, 3,6, 3,6) = -skewSymm3(prod(Io,nu2));

  c_matrix<double,3,3> M11=subrange(Ma, 0,3, 0,3);
  c_matrix<double,3,3> M12=subrange(Ma, 0,3, 3,6);
  c_matrix<double,3,3> M21=subrange(Ma, 3,6, 0,3);
  c_matrix<double,3,3> M22=subrange(Ma, 3,6, 3,6);

  subrange(Ca, 0,3, 0,3) = zero_matrix<double>(3,3);
  subrange(Ca, 0,3, 3,6) = -skewSymm3(prod(M11,nu1) + prod(M12,nu2));
  subrange(Ca, 3,6, 0,3) = -skewSymm3(prod(M11,nu1) + prod(M21,nu2));
  subrange(Ca, 3,6, 3,6) = -skewSymm3(prod(M21,nu1) + prod(M22,nu2));
}

void MarineVehicleModel::asmRestoringForces(const c_matrix<double,3,3>& invJ1)
{
  unit_vector<double> id(3,2);
  c_vector<double,3> fg = prod(invJ1,m*g_acc*id);
  c_vector<double,3> fb = -prod(invJ1,B*id);
  subrange(g, 0,3) = -fg-fb;
  subrange(g, 3,6) = -prod(skewSymm3(rg),fg)-prod(skewSymm3(rb),fb);
}




