// %flair:license{
// This file is part of the Flair framework distributed under the
// CECILL-C License, Version 1.0.
// %flair:license}
//  created:    2011/05/01
//  filename:   TrajectoryGenerator1D_impl.cpp
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    Class generating a trajectory in 1D
//
//
/*********************************************************************/

#include "TrajectoryGenerator1D.h"
#include "TrajectoryGenerator1D_impl.h"
#include <cvmatrix.h>
#include <Layout.h>
#include <GroupBox.h>
#include <DoubleSpinBox.h>
#include <cmath>

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

TrajectoryGenerator1D_impl::TrajectoryGenerator1D_impl(
    TrajectoryGenerator1D *self, const LayoutPosition *position, string name,
    string unit) {
  first_update = true;
  is_started = false;

  // init UI
  GroupBox *reglages_groupbox = new GroupBox(position, name);
  T = new DoubleSpinBox(reglages_groupbox->NewRow(), "period, 0 for auto:",
                        " s", 0, 1, 0.01);
  if (unit == "") {
    max_veloctity =
        new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),
                          "velocity max (absolute):", 0., 200000, 1);
  } else {
    max_veloctity = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),
                                      "velocity max (absolute):",
                                      " " + unit + "/s", 0., 200000, 1);
  }
  if (unit == "") {
    acceleration = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),
                                     "acceleration (absolute):", 0., 10, 1, 3);
  } else {
    acceleration = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),
                                     "acceleration (absolute):",
                                     " " + unit + "/s²", 0., 200000, 1);
  }

  Reset();

  // init matrix
  cvmatrix_descriptor *desc = new cvmatrix_descriptor(2, 1);
  desc->SetElementName(0, 0, "pos");
  desc->SetElementName(1, 0, "vel");
  output = new cvmatrix(self, desc, floatType, name);
  delete desc;

  output->SetValue(0, 0, pos);
  output->SetValue(1, 0, v);
}

TrajectoryGenerator1D_impl::~TrajectoryGenerator1D_impl() {}

void TrajectoryGenerator1D_impl::Reset(void) {
  pos = 0;
  v = 0;
  pos_off = 0;
  vel_off = 0;
}

void TrajectoryGenerator1D_impl::StartTraj(float start_pos, float end_pos) {
  is_started = true;
  is_finished = false;
  first_update = true;

  // configure trajectory
  end_position = end_pos;
  pos = start_pos;
  acc = acceleration->Value();
  v = 0;
  if (end_position < start_pos) {
    acc = -acc;
    // max_veloctity=-max_veloctity;
  }
}

// revoir l'interet du stop?
void TrajectoryGenerator1D_impl::StopTraj(void) {
  is_started = false;
  v = 0;
  // output->SetValue(1,0,v);
}

void TrajectoryGenerator1D_impl::Update(Time time) {
  float delta_t;

  if (T->Value() == 0) {
    if (first_update == true) {
      first_update = false;
      previous_time = time;
      output->GetMutex();
      output->SetValueNoMutex(0, 0, pos + pos_off);
      output->SetValueNoMutex(1, 0, v + vel_off);
      output->ReleaseMutex();

      output->SetDataTime(time);
      return;
    } else {
      delta_t = (float)(time - previous_time) / 1000000000.;
    }
  } else {
    delta_t = T->Value();
  }
  previous_time = time;

  if (is_started == true) {
    if (is_finished == false) {
      v += acc * delta_t;
      if (fabs(v) > fabs(max_veloctity->Value())) {
        if (v > 0)
          v = max_veloctity->Value();
        else
          v = -max_veloctity->Value();
      }
      pos += v * delta_t;
      if (end_position - v * v / (2 * acc) <= pos && v >= 0)
        acc = -acc;
      if (end_position - v * v / (2 * acc) >= pos && v < 0)
        acc = -acc;
      if (pos >= end_position && v >= 0)
        is_finished = true;
      if (pos <= end_position && v < 0)
        is_finished = true;
    }
    // else
    if (is_finished == true) {
      v = 0;
      pos = end_position;
    }
  }

  // on prend une fois pour toute les mutex et on fait des accès directs
  output->GetMutex();
  output->SetValueNoMutex(0, 0, pos + pos_off);
  output->SetValueNoMutex(1, 0, v + vel_off);
  output->ReleaseMutex();

  output->SetDataTime(time);
}