// %flair:license{
// This file is part of the Flair framework distributed under the
// CECILL-C License, Version 1.0.
// %flair:license}
//  created:    2022/01/05
//  filename:   PlaneMultiplex.cpp
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    Class defining plane multiplexing
//
//
/*********************************************************************/

#include "PlaneMultiplex.h"
#include <Matrix.h>
#include <Tab.h>
#include <GridLayout.h>
#include <GroupBox.h>
#include <ComboBox.h>
#include <DoubleSpinBox.h>
#include <DataPlot1D.h>
#include <math.h>


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

namespace flair {
namespace filter {
class PlaneMultiplexBldc : public IODevice {
 
public:
  PlaneMultiplexBldc(PlaneMultiplex *parent, string name):IODevice(parent,name) {
    output = new Matrix(this, 1,1, floatType);
  }
  ~PlaneMultiplexBldc(){};


private:
  Matrix *output;
  void UpdateFrom(const core::io_data *data) {
    const Matrix* input = dynamic_cast<const Matrix*>(data);
  
    if (!input) {
        Warn("casting %s to Matrix failed\n",data->ObjectName().c_str());
        return;
    }
    output->SetValue(0, 0, input->Value(0,0));

    output->SetDataTime(data->DataTime());

    ProcessUpdate(output);
  }

};

class PlaneMultiplexServos : public IODevice {
 
public:

  
  PlaneMultiplexServos(PlaneMultiplex *parent, string name):IODevice(parent,name) {
      output = new Matrix(this, 2,1, floatType);
  }
  ~PlaneMultiplexServos(){};


private:
  Matrix *output;
  void UpdateFrom(const core::io_data *data) {
    const Matrix* input = dynamic_cast<const Matrix*>(data);
  
    if (!input) {
        Warn("casting %s to Matrix failed\n",data->ObjectName().c_str());
        return;
    }
    input->GetMutex();
    output->GetMutex();
    for (int i = 0; i < 2; i++) output->SetValueNoMutex(i, 0, input->ValueNoMutex(1+i,0));
    output->ReleaseMutex();
    input->ReleaseMutex();

    output->SetDataTime(data->DataTime());

    ProcessUpdate(output);
  }


};

} // end namespace filter
} // end namespace flair

PlaneMultiplex::PlaneMultiplex(string name) :UavMultiplex(name){
  
  bldc=new PlaneMultiplexBldc(this,"bldc_multiplex");
  servos=new PlaneMultiplexServos(this,"servos_multiplex");
 
  MatrixDescriptor *desc = new MatrixDescriptor(6, 1);
  for (int i = 0; i < 3; i++) {
    desc->SetElementName(i, 0, ActuatorName(i));
  }

  output = new Matrix(this, desc, floatType);
  delete desc;
  AddDataToLog(output);
}

PlaneMultiplex::~PlaneMultiplex(void) {}

IODevice* PlaneMultiplex::GetBldcMultiplex(void) const {
  return bldc;
}

IODevice* PlaneMultiplex::GetServosMultiplex(void) const {
  return servos;
}


void PlaneMultiplex::UseDefaultPlot(void) {
  Tab *plot_tab = new Tab(GetTabWidget(), "Values");
  plots[0] = new DataPlot1D(plot_tab->NewRow(), ActuatorName(0), 0, 1);
  plots[1] = new DataPlot1D(plot_tab->LastRowLastCol(), ActuatorName(1), 0, 1);
  plots[2] = new DataPlot1D(plot_tab->NewRow(), ActuatorName(2), 0, 1);

  for (int i = 0; i < 3; i++) {
    plots[i]->AddCurve(output->Element(i));
  }
}

void PlaneMultiplex::UpdateFrom(const io_data *data) {
  float u_roll, u_pitch, u_yaw, u_thrust;
  float trim_roll, trim_pitch, trim_yaw;
  float value[3];

  const Matrix* input = dynamic_cast<const Matrix*>(data);
  
  if (!input) {
      Warn("casting %s to Matrix failed\n",data->ObjectName().c_str());
      return;
  }

  // on prend une fois pour toute le mutex et on fait des accès directs
  input->GetMutex();

  u_roll = input->ValueNoMutex(0, 0);
  u_pitch = input->ValueNoMutex(1, 0);
  u_yaw = input->ValueNoMutex(2, 0);
  u_thrust = input->ValueNoMutex(3, 0);
  trim_roll = input->ValueNoMutex(4, 0);
  trim_pitch = input->ValueNoMutex(5, 0);
  trim_yaw = input->ValueNoMutex(6, 0);

  input->ReleaseMutex();
  
  //font motor
  value[0]=0;
  
  //aileron right
  value[1]=0;
 
  //aileron left
  value[2]=0;
 
  // on prend une fois pour toute le mutex et on fait des accès directs
  output->GetMutex();
  for (int i = 0; i < 3; i++)
    output->SetValueNoMutex(i, 0, value[i]);
  output->ReleaseMutex();

  output->SetDataTime(data->DataTime());

  ProcessUpdate(output);
}

string PlaneMultiplex::ActuatorName(int index) {
 
    switch (index) {
    case 0:
      return "front motor";
    case 1:
      return "right aileron";
    case 2:
      return "left aileron";
    default:
      return "unammed actuator";
    }
}

uint8_t PlaneMultiplex::MotorsCount(void) const { return 3; }