// %flair:license{
// This file is part of the Flair framework distributed under the
// CECILL-C License, Version 1.0.
// %flair:license}
//  created:    2021/05/20
//  filename:   NEDPosition.cpp
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    Class for a NED position sensor
//
//
/*********************************************************************/

#include "NEDPosition.h"
#include <FrameworkManager.h>
#include <Tab.h>
#include <DataPlot1D.h>
#include <Matrix.h>

using std::string;
using namespace flair::core;
using namespace flair::gui;


class NEDPosition_impl {
public:
  NEDPosition_impl(flair::sensor::NEDPosition* self,string name) {
    
    MatrixDescriptor *desc = new MatrixDescriptor(6, 1);
    desc->SetElementName(0, 0, "x");
    desc->SetElementName(1, 0, "y");
    desc->SetElementName(2, 0, "z");
    desc->SetElementName(3, 0, "vx");
    desc->SetElementName(4, 0, "vy");
    desc->SetElementName(5, 0, "vz");
    output = new Matrix(self, desc, floatType);
    delete desc;
    
     // ui
    plot_tab = new Tab(getFrameworkManager()->GetTabWidget(), name);
    x_plot = new DataPlot1D(plot_tab->NewRow(), "x", -10, 10);
    x_plot->AddCurve(output->Element(0));
    y_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "y", -10, 10);
    y_plot->AddCurve(output->Element(1));
    z_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "z", -2, 0);
    z_plot->AddCurve(output->Element(2));
    vx_plot = new DataPlot1D(plot_tab->NewRow(), "vx", -5, 5);
    vx_plot->AddCurve(output->Element(3));
    vy_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "vy", -5, 5);
    vy_plot->AddCurve(output->Element(4));
    vz_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "vz", -2, 2);
    vz_plot->AddCurve(output->Element(5));
  }

  ~NEDPosition_impl() {}

  void GetPosition(Vector3Df &point) {
    output->GetMutex();
    point.x = output->ValueNoMutex(0, 0);
    point.y = output->ValueNoMutex(1, 0);
    point.z = output->ValueNoMutex(2, 0);
    output->ReleaseMutex();
  }
  void GetVelocity(Vector3Df &point) {
    output->GetMutex();
    point.x = output->ValueNoMutex(3, 0);
    point.y = output->ValueNoMutex(4, 0);
    point.z = output->ValueNoMutex(5, 0);
    output->ReleaseMutex();
  }
  Matrix *output;
  Tab *plot_tab;
  DataPlot1D *x_plot,*y_plot,*z_plot,*vx_plot,*vy_plot,*vz_plot;
  
private:

};
  

namespace flair {
namespace sensor {

NEDPosition::NEDPosition(string name)
    : IODevice(getFrameworkManager(), name) {
  pimpl_ = new ::NEDPosition_impl(this, name);
  AddDataToLog(pimpl_->output);
  
  SetIsReady(true);
}

NEDPosition::~NEDPosition(void) { 
  delete pimpl_;
}

Matrix *NEDPosition::Output(void) const { return pimpl_->output; }

Tab *NEDPosition::GetPlotTab(void) const { return pimpl_->plot_tab; }

DataPlot1D *NEDPosition::xPlot(void) const { return pimpl_->x_plot; }

DataPlot1D *NEDPosition::yPlot(void) const { return pimpl_->y_plot; }

DataPlot1D *NEDPosition::zPlot(void) const { return pimpl_->z_plot; }

DataPlot1D *NEDPosition::vxPlot(void) const { return pimpl_->vx_plot; }

DataPlot1D *NEDPosition::vyPlot(void) const { return pimpl_->vy_plot; }

DataPlot1D *NEDPosition::vzPlot(void) const { return pimpl_->vz_plot; }

void NEDPosition::GetPosition(Vector3Df &point) const {
  pimpl_->GetPosition(point);
}

void NEDPosition::GetVelocity(Vector3Df &point) const {
  pimpl_->GetVelocity(point);
}

void NEDPosition::SetDatas(const Vector3Df &position,const Vector3Df &velocity,const Time time) {
  pimpl_->output->GetMutex();
  pimpl_->output->SetValueNoMutex(0,0,position.x);
  pimpl_->output->SetValueNoMutex(1,0,position.y);
  pimpl_->output->SetValueNoMutex(2,0,position.z);
  pimpl_->output->SetValueNoMutex(3,0,velocity.x);
  pimpl_->output->SetValueNoMutex(4,0,velocity.y);
  pimpl_->output->SetValueNoMutex(5,0,velocity.z);
  pimpl_->output->ReleaseMutex();
  pimpl_->output->SetDataTime(time);
  ProcessUpdate(pimpl_->output);
}


} // end namespace sensor
} // end namespace flair