// %flair:license{
// This file is part of the Flair framework distributed under the
// CECILL-C License, Version 1.0.
// %flair:license}
//  created:    2020/11/20
//  filename:   TwoWheelRobot.cpp
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    classe definissant un TwoWheelRobot
//
/*********************************************************************/

#include "TwoWheelRobot.h"
#include <TabWidget.h>
#include <Tab.h>
#include <DoubleSpinBox.h>
#include <SpinBox.h>
#include <GroupBox.h>
#include <math.h>
#include <SimuUgvControls.h>
#ifdef GL
#include <ISceneManager.h>
#include <IMeshManipulator.h>
#include "MeshSceneNode.h"
#include "Gui.h"
#include <Mutex.h>
#endif

#define G (float)9.81 // gravity ( N/(m/s²) )

#ifdef GL
using namespace irr::video;
using namespace irr::scene;
using namespace irr::core;
#endif
using namespace flair::core;
using namespace flair::gui;
using namespace flair::actuator;

namespace flair {
namespace simulator {

TwoWheelRobot::TwoWheelRobot(std::string name, uint32_t modelId)
    : Model(name,modelId) {
  Tab *setup_tab = new Tab(GetTabWidget(), "model");
  m = new DoubleSpinBox(setup_tab->NewRow(), "mass (kg):", 0, 20, 0.1,1,0.2);
  size = new DoubleSpinBox(setup_tab->NewRow(), "size (m):", 0, 20, 0.1,1,0.1);
  t_speed = new DoubleSpinBox(setup_tab->NewRow(), "translational speed (m/s):",
                              0, 5, 0.1);
  r_speed = new DoubleSpinBox(setup_tab->NewRow(), "rotational speed (deg/s):",
                              0, 180, 10);
  
  
  Tab *visual_tab = new Tab(GetTabWidget(), "visual");
  bodyColorR = new SpinBox(visual_tab->NewRow(), "arm color (R):", 0, 255, 1,255);
  bodyColorG = new SpinBox(visual_tab->LastRowLastCol(), "arm color (G):", 0, 255, 1,0);
  bodyColorB = new SpinBox(visual_tab->LastRowLastCol(), "arm color (B):", 0, 255, 1,0);
  
 
  
  controls = new SimuUgvControls(this, name, modelId,0);
  
  SetIsReady(true);
}

TwoWheelRobot::~TwoWheelRobot() {
  // les objets irrlicht seront automatiquement detruits par parenté
}

#ifdef GL

void TwoWheelRobot::Draw(void) {
  // create unite (1m=100cm) robot; scale will be adapted according to settings in gcs
  // parameter
  // note that the frame used is irrlicht one:
  // left handed, North East Up
  const IGeometryCreator *geo;
  geo = getGui()->getSceneManager()->getGeometryCreator();

  colored_body = geo->createCubeMesh(vector3df(100,100,100));
  colored_body->setBoundingBox(aabbox3df(0,0,0,1,1,1));//bug with bounding box? workaround is to reduce it... we use only wheel box
  MeshSceneNode* mesh= new MeshSceneNode(this, colored_body);
  
  IMesh *wheel = geo->createCylinderMesh(35, 10, 64, SColor(0, 0, 0, 0));
  MeshSceneNode *l_wheel = new MeshSceneNode(this, wheel, vector3df(0, 50, -30),vector3df(0, 0, 0));
  MeshSceneNode *r_wheel = new MeshSceneNode(this, wheel, vector3df(0, -50-10, -30),vector3df(0, 0, 0));
  
 ExtraDraw();
}

void TwoWheelRobot::AnimateModel(void) {
  if (bodyColorR->ValueChanged() == true || bodyColorG->ValueChanged() == true || bodyColorB->ValueChanged() == true) {
    getGui()->getSceneManager()->getMeshManipulator()->setVertexColors(colored_body, SColor(0,bodyColorR->Value(), bodyColorG->Value(), bodyColorB->Value()));
  }

  // adapt robot size
  if (size->ValueChanged() == true) {
    setScale(size->Value());
  }
   
}

size_t TwoWheelRobot::dbtSize(void) const {
  return 3 * sizeof(float) + 2 * sizeof(float); // 3ddl+2motors
}

void TwoWheelRobot::WritedbtBuf(
    char *dbtbuf) { /*
                       float *buf=(float*)dbtbuf;
                       vector3df vect=getPosition();
                       memcpy(buf,&vect.X,sizeof(float));
                       buf++;
                       memcpy(buf,&vect.Y,sizeof(float));
                       buf++;
                       memcpy(buf,&vect.Z,sizeof(float));
                       buf++;
                       vect=getRotation();
                       memcpy(buf,&vect.X,sizeof(float));
                       buf++;
                       memcpy(buf,&vect.Y,sizeof(float));
                       buf++;
                       memcpy(buf,&vect.Z,sizeof(float));
                       buf++;
                       memcpy(buf,&motors,sizeof(rtsimu_motors));*/
}

void TwoWheelRobot::ReaddbtBuf(
    char *dbtbuf) { /*
                       float *buf=(float*)dbtbuf;
                       vector3df vect;
                       memcpy(&vect.X,buf,sizeof(float));
                       buf++;
                       memcpy(&vect.Y,buf,sizeof(float));
                       buf++;
                       memcpy(&vect.Z,buf,sizeof(float));
                       buf++;
                       setPosition(vect);
                       memcpy(&vect.X,buf,sizeof(float));
                       buf++;
                       memcpy(&vect.Y,buf,sizeof(float));
                       buf++;
                       memcpy(&vect.Z,buf,sizeof(float));
                       buf++;
                       ((ISceneNode*)(this))->setRotation(vect);
                       memcpy(&motors,buf,sizeof(rtsimu_motors));
                       AnimateModele();*/
}
#endif // GL

// states are computed on fixed frame NED
// x north
// y east
// z down
void TwoWheelRobot::CalcModel(void) {
  float speed,turn;
  Time motorTime;

  controls->GetControls(&speed,&turn,&motorTime);
	 
  // compute quaternion from W
  // Quaternion derivative: dQ = 0.5*(  Q*Qw)
  state[0].W.x=0;
  state[0].W.y=0;
  state[0].W.z=turn*r_speed->Value();
  Quaternion dQ = state[-1].Quat.GetDerivative(state[0].W);

  // Quaternion integration
  state[0].Quat = state[-1].Quat + dQ * dT();
  state[0].Quat.Normalize();

  Vector3D<double> dir = Vector3D<double>(speed*t_speed->Value(),0,0);
  dir.Rotate(state[0].Quat);
  state[0].Pos = state[-1].Pos + dT() * dir;
  
 
  /*
  ** ===================================================================
  **     z double integrator
  **
  ** ===================================================================
  */
  state[0].Pos.z = (dT() * dT() / m->Value()) * ( m->Value() * G) + 2 * state[-1].Pos.z - state[-2].Pos.z;
  state[0].Vel.z = (state[0].Pos.z - state[-1].Pos.z) / dT();
}

} // end namespace simulator
} // end namespace flair