// %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:   EulerDerivative.cpp
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    objet permettant le calcul d'une derivee d'Euler
//
//
/*********************************************************************/

#include "EulerDerivative.h"
#include "EulerDerivative_impl.h"
#include <Matrix.h>
#include <Layout.h>
#include <GroupBox.h>
#include <DoubleSpinBox.h>

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

EulerDerivative_impl::EulerDerivative_impl(EulerDerivative *self,
                                           const LayoutPosition *position,
                                           string name,
                                           const Matrix *init_value) {
  this->self = self;
  first_update = true;
  
  if (init_value != NULL) {
    // init output matrix of same size as init
    cvmatrix_descriptor *desc =new cvmatrix_descriptor(init_value->Rows(), init_value->Cols());

    for (int i = 0; i < init_value->Rows(); i++) {
      for (int j = 0; j < init_value->Cols(); j++) {
        desc->SetElementName(i, j, init_value->Name(i, j));
      }
    }
    output = new Matrix(self, desc,init_value->GetDataType().GetElementDataType(), name);
    delete desc;
    for (int i = 0; i < init_value->Rows(); i++) {
      for (int j = 0; j < init_value->Cols(); j++) {
        output->SetValue(i, j, init_value->Value(i,j));
      }
    }
  } else {
    // if NULL, assume dimension 1, and init=0
    cvmatrix_descriptor *desc = new cvmatrix_descriptor(1, 1);
    desc->SetElementName(0, 0, "output");
    output = new Matrix(self, desc, floatType, name);
    delete desc;
  }
  
 
  cvmatrix_descriptor *desc = new cvmatrix_descriptor(output->Rows(), output->Cols());
  prev_input = new Matrix(self, desc, output->GetDataType().GetElementDataType(), name);
  prev_output = new Matrix(self, desc, output->GetDataType().GetElementDataType(), name);
  delete desc;


  // init UI
  GroupBox *reglages_groupbox = new GroupBox(position, name);
  T = new DoubleSpinBox(reglages_groupbox->NewRow(), "period, 0 for auto:"," s", 0, 1, 0.01);
  sat = new DoubleSpinBox(reglages_groupbox->NewRow(), "saturation, -1 to disable:",-1,100000,1,1,-1);
}

EulerDerivative_impl::~EulerDerivative_impl() {}

void EulerDerivative_impl::UpdateFrom(const io_data *data) {
  float delta_t;
  const Matrix* input = dynamic_cast<const Matrix*>(data);
  
  if (!input) {
      self->Warn("casting %s to Matrix failed\n",data->ObjectName().c_str());
      return;
  }

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

  if (first_update == true) {
    for (int i = 0; i < input->Rows(); i++) {
      for (int j = 0; j < input->Cols(); j++) {
        prev_input->SetValueNoMutex(i, j, input->ValueNoMutex(i, j));
      }
    }
    first_update = false;
  } else {
    if (T->Value() == 0) {
      delta_t = (float)(data->DataDeltaTime()) / 1000000000.;
    } else {
      delta_t = T->Value();
    }

    if(delta_t!=0) {
      for (int i = 0; i < input->Rows(); i++) {
        for (int j = 0; j < input->Cols(); j++) {
          float result=(input->ValueNoMutex(i, j) - prev_input->ValueNoMutex(i, j)) / delta_t;
          if(sat->Value()!=-1) {
            if(result>sat->Value() && result>0) result=sat->Value();
            if(result<-sat->Value() && result<0) result=-sat->Value();
          }/* filter by acc
          float acc=result-prev_output->ValueNoMutex(i, j)/delta_t;
          if(acc>20 || acc<-20)  {
            printf("%s %f\n",self->ObjectName().c_str(),acc);
          } else {*/
            output->SetValueNoMutex(i, j, result);
          //}
          prev_output->SetValueNoMutex(i, j, result);
          prev_input->SetValueNoMutex(i, j, input->ValueNoMutex(i, j));
          
        }
      }
    }
  }

  input->ReleaseMutex();
  output->ReleaseMutex();

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