// %flair:license{
// This file is part of the Flair framework distributed under the
// CECILL-C License, Version 1.0.
// %flair:license}
//  created:    2018/12/11
//  filename:   UavVrpnObject_impl.cpp
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    uav vrpn object, can display a real vrpn object in a simulated environment
//
/*********************************************************************/
#ifdef GL

#include "UavVrpnObject_impl.h"
#include "UavVrpnObject.h"
#include <TabWidget.h>
#include <Tab.h>
#include <DoubleSpinBox.h>
#include "Simulator.h"
#include <ISceneManager.h>
#include <IVideoDriver.h>
#include "Blade.h"
#include "MeshSceneNode.h"
#include "Gui.h"
#include "FollowMeCamera.h"
#include <Matrix.h>
#include <Quaternion.h>
#include <Euler.h>

#define MOTOR_SPEED 100

using namespace irr::video;
using namespace irr::scene;
using namespace irr::core;
using namespace flair::core;
using namespace flair::simulator;
using namespace flair::gui;
  
UavVrpnObject_impl::UavVrpnObject_impl(UavVrpnObject *self,std::string name) 
  : IODevice(self,name),
    ISceneNode(getGui()->getSceneManager()->getRootSceneNode(), getGui()->getSceneManager(), -1) {
  this->self=self;
  
  // init user interface
  Tab *tab = new Tab(getSimulator()->GetTabWidget(), ObjectName());
  arm_length = new DoubleSpinBox(tab->LastRowLastCol(), "arm length (m):",0, 2, 0.1);

  // camera
  FollowMeCamera* camera = new FollowMeCamera(this,name);
  
  Draw();
}

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

void UavVrpnObject_impl::UpdateFrom(const io_data *data) {
  const Matrix* input = dynamic_cast<const Matrix*>(data);
  if (!input) {
      self->Warn("casting %s to Matrix failed\n",data->ObjectName().c_str());
      return;
  }
  
  Vector3Df vrpnPosition;
  Quaternion vrpnQuaternion;
  input->GetMutex();
  vrpnQuaternion.q0=input->ValueNoMutex(0, 0);
  vrpnQuaternion.q1=input->ValueNoMutex(1, 0);
  vrpnQuaternion.q2=input->ValueNoMutex(2, 0);
  vrpnQuaternion.q3=input->ValueNoMutex(3, 0);
  vrpnPosition.x=input->ValueNoMutex(4, 0);
  vrpnPosition.y=input->ValueNoMutex(5, 0);
  vrpnPosition.z=input->ValueNoMutex(6, 0);
  input->ReleaseMutex();
  
  //use yaw rotation from earth to vrpn
  Quaternion yaw_rot_quat;
  Euler yaw_rot_euler(0, 0, getSimulator()->Yaw()); // yaw_rad is vrpn rotation in earth reference
  yaw_rot_euler.ToQuaternion(yaw_rot_quat);
  vrpnPosition.Rotate(yaw_rot_quat);
  vrpnQuaternion= yaw_rot_quat * vrpnQuaternion;
  
  vector3df nodePosition;
  Quaternion nodeQuaternion;
  Euler nodeEuler;

  //transform form earth to irrlicht
  nodePosition = ToIrrlichtCoordinates(vrpnPosition);
  setPosition(nodePosition);
  nodeQuaternion = ToIrrlichtOrientation(vrpnQuaternion);
  nodeQuaternion.ToEuler(nodeEuler);
  setRotation(Euler::ToDegree(1) * vector3df(nodeEuler.roll,nodeEuler.pitch, nodeEuler.yaw));
  
  if (arm_length->ValueChanged() == true) {
    setScale(vector3df(arm_length->Value(), arm_length->Value(), arm_length->Value()));
  }
}

void UavVrpnObject_impl::render(void) {
  IVideoDriver *driver = SceneManager->getVideoDriver();
  driver->setTransform(ETS_WORLD, AbsoluteTransformation);
}

void UavVrpnObject_impl::OnRegisterSceneNode(void) {
  if (IsVisible)
    SceneManager->registerNodeForRendering(this);

  ISceneNode::OnRegisterSceneNode();
}

void UavVrpnObject_impl::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 *red_arm = geo->createCylinderMesh(2.5, 100, 16, SColor(0, 255, 0, 0));
  IMesh *black_arm = geo->createCylinderMesh(2.5, 100, 16, SColor(0, 128, 128, 128));
  IMesh *motor = geo->createCylinderMesh(7.5, 15, 16); //,SColor(0, 128, 128, 128));
  // geo->drop();

  ITexture *texture = getGui()->getTexture("carbone.jpg");
  MeshSceneNode *fl_arm = new MeshSceneNode(this, red_arm, vector3df(0, 0, 0),
                             vector3df(0, 0, -135));
  MeshSceneNode *fr_arm = new MeshSceneNode(this, red_arm, vector3df(0, 0, 0),
                             vector3df(0, 0, -45));
  MeshSceneNode *rl_arm = new MeshSceneNode(this, black_arm, vector3df(0, 0, 0),
                             vector3df(0, 0, 135), texture);
  MeshSceneNode *rr_arm = new MeshSceneNode(this, black_arm, vector3df(0, 0, 0),
                             vector3df(0, 0, 45), texture);

  texture = getGui()->getTexture("metal047.jpg");
  MeshSceneNode *fl_motor = new MeshSceneNode(this, motor, vector3df(70.71, -70.71, 2.5),
                               vector3df(90, 0, 0), texture);
  MeshSceneNode *fr_motor = new MeshSceneNode(this, motor, vector3df(70.71, 70.71, 2.5),
                               vector3df(90, 0, 0), texture);
  MeshSceneNode *rl_motor = new MeshSceneNode(this, motor, vector3df(-70.71, -70.71, 2.5),
                               vector3df(90, 0, 0), texture);
  MeshSceneNode *rr_motor = new MeshSceneNode(this, motor, vector3df(-70.71, 70.71, 2.5),
                               vector3df(90, 0, 0), texture);

  fl_blade = new Blade(this, vector3df(70.71, -70.71, 17.5));
  fr_blade = new Blade(this, vector3df(70.71, 70.71, 17.5),  vector3df(0, 0, 0),true);
  rl_blade = new Blade(this, vector3df(-70.71, -70.71, 17.5),  vector3df(0, 0, 0),true);
  rr_blade = new Blade(this, vector3df(-70.71, 70.71, 17.5));
  
  fl_blade->SetRotationSpeed(vector3df(0, 0,MOTOR_SPEED));
  fr_blade->SetRotationSpeed(vector3df(0, 0,-MOTOR_SPEED));
  rl_blade->SetRotationSpeed(vector3df(0, 0,-MOTOR_SPEED));
  rr_blade->SetRotationSpeed(vector3df(0, 0,MOTOR_SPEED));
  
  setScale(vector3df(arm_length->Value(), arm_length->Value(), arm_length->Value()));
}
#endif // GL