// %flair:license{
// This file is part of the Flair framework distributed under the
// CECILL-C License, Version 1.0.
// %flair:license}
//  created:    2014/02/07
//  filename:   SimuUsGL.cpp
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    Class for a simulation us
//
//
/*********************************************************************/
#ifdef GL

#include "SimuLaserGL.h"
#include "Model.h"
#include "Gui.h"
#include <cvmatrix.h>
#include <SharedMem.h>
#include <TabWidget.h>
#include <Tab.h>
#include <DoubleSpinBox.h>
#include <Vector3DSpinBox.h>
#include <ISceneNode.h>
#include <ISceneCollisionManager.h>

using namespace irr;
using namespace irr::scene;
using namespace irr::core;
using namespace flair::core;
using namespace flair::gui;
using namespace flair::simulator;

namespace flair {
namespace sensor {
SimuLaserGL::SimuLaserGL(const Model *parent, std::string name, int dev_id)
    : SimuLaser(parent, name, dev_id), SensorGL(parent) {
  Tab *setup_tab = new Tab(parent->GetTabWidget(), name);
  position = new Vector3DSpinBox(setup_tab->NewRow(), "position", -2, 2, .01);
  direction = new Vector3DSpinBox(setup_tab->NewRow(), "direction", -2, 2, .01);
  range = new DoubleSpinBox(setup_tab->NewRow(), "range:", 0, 30, 1);
}

SimuLaserGL::~SimuLaserGL() {}

void SimuLaserGL::UpdateFrom(const io_data *data) {
  float value[360];

  if (noGui() == false && data == NULL) {
    for (int i = 0; i < 360; i++) {
      line3d<f32> ray_laser;         // rayon provenant de l'ultra son
      vector3df intersection_laser;  // point intersection us avec le sol
      triangle3df hitTriangle_laser; // triangle intersection us avec le sol

      // get rotation matrix of node - Zeuss must be getRotation not
      // getRelativeTransformation
      matrix4 m;
      matrix4 M;
      m.setRotationDegrees(Node()->getRotation());

      // Matrice de rotation pour balayage du laser, angle i
      M.setRotationDegrees(vector3df(0, 0, i));
      // transform forward vector of us
      vector3df frv = ToIrrlichtCoordinates(direction->Value());
      M.transformVect(frv);
      m.transformVect(frv);
      frv.normalize();

      // transform pos vector of us
      vector3df pos = ToIrrlichtCoordinates(position->Value());
      m.transformVect(pos);

      ray_laser.start = Node()->getPosition() + pos;
      ray_laser.end = ray_laser.start + ToIrrlichtScale(range->Value()) * frv;

      scene::ISceneNode *selectedSceneNode =
          CollMan()->getSceneNodeAndCollisionPointFromRay(
              ray_laser, intersection_laser, hitTriangle_laser);
      // //////////////////////////////////////////
      if (selectedSceneNode) //
      {
        value[i] = ToSimulatorScale(
            ray_laser.start.getDistanceFrom(intersection_laser));
      } else {
        value[i] = -1;
      }
    }
    shmem->Write((char *)value, 360 * sizeof(float));
  }
}

} // end namespace sensor
} // end namespace flair
#endif