// %flair:license{
// This file is part of the Flair framework distributed under the
// CECILL-C License, Version 1.0.
// %flair:license}
//  created:    2021/12/22
//  filename:   Plane.cpp
//
//  author:     Armando Alatorre Sevilla, Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    classe definissant un avion
//
/*********************************************************************/

#include "Plane.h"
#include <SimuBldc.h>
#include <SimuServos.h>
#include <TabWidget.h>
#include <Tab.h>
#include <DoubleSpinBox.h>
#include <SpinBox.h>
#include <GroupBox.h>
#include <math.h>
#ifdef GL
#include <ISceneManager.h>
#include <IMeshManipulator.h>
#include "Blade.h"
#include "MeshSceneNode.h"
#include "Gui.h"
#include <Mutex.h>
#endif

#define K_MOT 0.4f    // blade animation
#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 {

Plane::Plane(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);
  
  motorTimeout = new SpinBox(setup_tab->NewRow(), "motor timeout:","ms", 0, 1000, 100,100);
  
  motor = new SimuBldc(this, name, 1, modelId,0);
  servos = new SimuServos(this, name, 2, modelId,0);
  
  SetIsReady(true);
}

Plane::~Plane() {
  // les objets irrlicht seront automatiquement detruits (moteurs, helices,
  // pales) par parenté
}

#ifdef GL

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

  // cylinders are aligned with y axis
  IMesh *motor_mesh = geo->createCylinderMesh(7.5, 5, 16);
  ITexture *texture = getGui()->getTexture("metal047.jpg");
  MeshSceneNode *f_motor = new MeshSceneNode(this, motor_mesh, vector3df(150, 0, 20),
                               vector3df(0, 0, -90), texture);
  f_blade = new Blade(this, vector3df(155, 0,20), vector3df(90, 0, 90));
  
  IMesh *body_mesh = geo->createCylinderMesh(20, 150, 32,SColor(0, 255, 255, 255));
  MeshSceneNode *body = new MeshSceneNode(this, body_mesh, vector3df(0, 0, 20),
                             vector3df(0, 0, -90));
  
  IMesh *wing= geo->createCubeMesh (vector3df(35, 100, 2));
  IMesh *aileron= geo->createCubeMesh (vector3df(10, 90, 2));
  IMesh *axe = geo->createCylinderMesh(.5, 100, 16);//axe pour definir l'axe de rotation en bout d'aileron (sinon l'axe est au milieu d'aileron)
  
  //origine de l'aile en son centre
  MeshSceneNode *l_wing = new MeshSceneNode(this, wing, vector3df(75, -20-50, 20),vector3df(0, 0, 0));
  MeshSceneNode *r_wing = new MeshSceneNode(this, wing, vector3df(75, 20+50, 20),vector3df(0, 0, 0));
                      
       
  l_axe_aileron = new MeshSceneNode(this, axe, vector3df(75-35/2, -20-100, 20),vector3df(0, 0, 0));
  MeshSceneNode *l_aileron = new MeshSceneNode(l_axe_aileron, aileron, vector3df(-5, 50, 0),vector3df(0, 0, 0),texture);
 
                        
  r_axe_aileron = new MeshSceneNode(this, axe, vector3df(75-35/2, 20, 20),vector3df(0, 0, 0));
  MeshSceneNode *r_aileron = new MeshSceneNode(r_axe_aileron, aileron, vector3df(-5, 50,0),vector3df(0, 0, 0),texture);
                               
  actuators_mutex = new Mutex(this);
  motor_speed = 0;
  for (int i = 0; i < 2; i++) servos_pos[i] = 0;
  ExtraDraw();
}

void Plane::AnimateModel(void) {
  actuators_mutex->GetMutex();
  f_blade->SetRotationSpeed(K_MOT *vector3df( 0,motor_speed,0));
  l_axe_aileron->setRotation(vector3df(0,servos_pos[1]*180./3.14,0));
  r_axe_aileron->setRotation(vector3df(0,servos_pos[0]*180./3.14,0));

  actuators_mutex->ReleaseMutex();

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

size_t Plane::dbtSize(void) const {
  return 6 * sizeof(float) + 1 * sizeof(float); // 6ddl+1moteur
}

void Plane::WritedbtBuf(char *dbtbuf) {
}

void Plane::ReaddbtBuf(char *dbtbuf) {
}
#endif // GL

// states are computed on fixed frame NED
// x north
// y east
// z down
void Plane::CalcModel(void) {
  float u_roll, u_pitch, u_yaw, u_thrust;
  Time motorTime,servoTime;
  
#ifdef GL
  actuators_mutex->GetMutex();
#endif // GL
  motor->GetSpeeds(&motor_speed,&motorTime);
  if((GetTime()-motorTime)/1000000>motorTimeout->Value()) {
      if(motor_speed!=0) {
        motor_speed=0;
      }
  }
  servos->GetPositions(servos_pos,&servoTime);
  
#ifdef GL
  actuators_mutex->ReleaseMutex();
#endif // GL
  
 
  /*
  ** ===================================================================
  **    u roll: roll torque
  **
  ** ===================================================================
  */
  /*
  u_roll = arm_length->Value() * k_mot->Value() *
           (fl_speed * fl_speed + rl_speed * rl_speed - fr_speed * fr_speed -
            rr_speed * rr_speed) *
           sqrtf(2) / 2;

  /// Classical Nonlinear model of a quadrotor ( This is the w_x angular speed
  /// of the quadri in the body frame). It is a discrete integrator
  state[0].W.x =
      (dT() / j_roll->Value()) *
          ((j_yaw->Value() - j_pitch->Value()) * state[-1].W.y * state[-1].W.z +
           u_roll) +
      state[-1].W.x;
*/
 
  /*
  ** ===================================================================
  **   u pitch : pitch torque
  **
  ** ===================================================================
  */
  /*
  u_pitch = arm_length->Value() * k_mot->Value() *
            (fl_speed * fl_speed + fr_speed * fr_speed - rl_speed * rl_speed -
             rr_speed * rr_speed) *
            sqrtf(2) / 2;

  /// Classical Nonlinear model of a quadrotor ( This is the w_y angular speed
  /// of the quadri in the body frame). It is a discrete integrator
  state[0].W.y =
      (dT() / j_pitch->Value()) *
          ((j_roll->Value() - j_yaw->Value()) * state[-1].W.x * state[-1].W.z +
           u_pitch) +
      state[-1].W.y;
*/
  
  /*
  ** ===================================================================
  **    u yaw : yaw torque
  **
  ** ===================================================================
  */
  /*
  u_yaw = c_mot->Value() * (fl_speed * fl_speed + rr_speed * rr_speed -
                            fr_speed * fr_speed - rl_speed * rl_speed);

  /// Classical Nonlinear model of a quadrotor ( This is the w_z angular speed
  /// of the quadri in the body frame). It is a discrete integrator
  state[0].W.z = (dT() / j_yaw->Value()) * u_yaw + state[-1].W.z;
*/
 
  // compute quaternion from W
  // Quaternion derivative: dQ = 0.5*(Q*Qw)
  Quaternion dQ = state[-1].Quat.GetDerivative(state[0].W);

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

  // Calculation of the thrust from the reference speed of motors
  /*
  u_thrust = k_mot->Value() * (fl_speed * fl_speed + fr_speed * fr_speed +
                               rl_speed * rl_speed + rr_speed * rr_speed);
  Vector3D<double> vect(0, 0, -u_thrust);
  vect.Rotate(state[0].Quat);
*/
  /*
      ** ===================================================================
      **     x double integrator
      **
      ** ===================================================================
      */
   /*   
  state[0].Pos.x =
      (dT() * dT() / m->Value()) *
          (vect.x - f_air_lat->Value() * (state[-1].Pos.x - state[-2].Pos.x) / dT()) +
      2 * state[-1].Pos.x - state[-2].Pos.x;
  state[0].Vel.x = (state[0].Pos.x - state[-1].Pos.x) / dT();
 */
  /*
  ** ===================================================================
  **     y double integrator
  **
  ** ===================================================================
  */
  /*
  state[0].Pos.y =
      (dT() * dT() / m->Value()) *
          (vect.y -
           f_air_lat->Value() * (state[-1].Pos.y - state[-2].Pos.y) / dT()) +
      2 * state[-1].Pos.y - state[-2].Pos.y;
  state[0].Vel.y = (state[0].Pos.y - state[-1].Pos.y) / dT();
*/
  /*
  ** ===================================================================
  **     z double integrator
  **
  ** ===================================================================
  */
  /*
  state[0].Pos.z =
      (dT() * dT() / m->Value()) *
          (vect.z +
           f_air_vert->Value() * (state[-1].Pos.z - state[-2].Pos.z) / dT() +
           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();
*/

#ifndef GL
  if (state[0].Pos.z < 0)
    state[0].Pos.z = 0;
#endif
}

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