// %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:   LowPassFilter_impl.cpp
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    objet permettant le calcul d'un filtre passe bas
//
//
/*********************************************************************/

#include "LowPassFilter_impl.h"
#include "LowPassFilter.h"
#include <cvmatrix.h>
#include <Layout.h>
#include <GroupBox.h>
#include <SpinBox.h>
#include <DoubleSpinBox.h>

#define PI ((float)3.14159265358979323846)

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

LowPassFilter_impl::LowPassFilter_impl(const LowPassFilter *self,
                                       const LayoutPosition *position,
                                       string name,
                                       const cvmatrix *init_value) {
  first_update = true;

  if (init_value != NULL) {
    prev_value = (cvmatrix *)init_value;
  } else {
    // if NULL, assume dimension 1, and init=0
    cvmatrix_descriptor *desc = new cvmatrix_descriptor(1, 1);
    desc->SetElementName(0, 0, "output");
    prev_value = new cvmatrix(self, desc, floatType, name);
    prev_value->SetValue(0, 0, 0);
  }

  // init UI
  GroupBox *reglages_groupbox = new GroupBox(position, name);
  T = new DoubleSpinBox(reglages_groupbox->NewRow(), "period, 0 for auto", " s",
                        0, 10, 0.01);
  freq = new DoubleSpinBox(reglages_groupbox->NewRow(), "cutoff frequency",
                           " Hz", 0, 10000, 0.1, 2, 1);

  // init output matrix of same size as init
  cvmatrix_descriptor *desc =
      new cvmatrix_descriptor(prev_value->Rows(), prev_value->Cols());

  for (int i = 0; i < prev_value->Rows(); i++) {
    for (int j = 0; j < prev_value->Cols(); j++) {
      desc->SetElementName(i, j, prev_value->Name(i, j));
    }
  }

  output = new cvmatrix(self, desc,
                        prev_value->GetDataType().GetElementDataType(), name);

  output->SetValue(0, 0, 0);
}

LowPassFilter_impl::~LowPassFilter_impl() {}

void LowPassFilter_impl::UpdateFrom(const io_data *data) {
  float delta_t;
  float result;
  cvmatrix *input = (cvmatrix *)data;

  // 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++) {
        output->SetValueNoMutex(i, j, prev_value->ValueNoMutex(i, j));
        prev_value->SetValueNoMutex(i, j, input->ValueNoMutex(i, j));
      }
    }
    first_update = false;
  } else {
    if (T->Value() == 0) {
      delta_t = (float)(data->DataTime() - previous_time) / 1000000000.;
    } else {
      delta_t = T->Value();
    }
    for (int i = 0; i < input->Rows(); i++) {
      for (int j = 0; j < input->Cols(); j++) {

        if (freq->Value() != 0) {
          output->SetValueNoMutex(i, j, (1 - 2 * PI * freq->Value() * delta_t) *
                                                prev_value->ValueNoMutex(i, j) +
                                            2 * PI * freq->Value() * delta_t *
                                                input->ValueNoMutex(i, j));
        } else {
          output->SetValueNoMutex(i, j, input->ValueNoMutex(i, j));
        }
        prev_value->SetValueNoMutex(i, j, output->ValueNoMutex(i, j));
      }
    }
  }
  input->ReleaseMutex();
  output->ReleaseMutex();

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