// %flair:license{
// This file is part of the Flair framework distributed under the
// CECILL-C License, Version 1.0.
// %flair:license}
//  created:    2013/12/10
//  filename:   ButterworthLowPass_impl.cpp
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    objet permettant le calcul d'un filtre passe bas de Butterworth
//
//
/*********************************************************************/

#include "ButterworthLowPass_impl.h"
#include "ButterworthLowPass.h"
#include <Matrix.h>
#include <Layout.h>
#include <GroupBox.h>
#include <SpinBox.h>
#include <DoubleSpinBox.h>

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

ButterworthLowPass_impl::ButterworthLowPass_impl(const ButterworthLowPass *self,
                                                 const LayoutPosition *position,
                                                 string name, uint32_t order,uint32_t nbRow,uint32_t nbCol) {
  // init UI
  GroupBox *reglages_groupbox = new GroupBox(position, name);
  T = new DoubleSpinBox(reglages_groupbox->NewRow(), "period, 0 for auto", " s",
                        0, 10, 0.01);
  cutoff = new DoubleSpinBox(reglages_groupbox->NewRow(), "cutoff frequency",
                             " Hz", 0, 10000, 0.1, 2, 1);

  MatrixDescriptor *desc = new MatrixDescriptor(nbRow, nbCol);
  //desc->SetElementName(0, 0, "output");
  output = new Matrix(self, desc, floatType, name);
  delete desc;

  f=(PoleFilter**)malloc(sizeof(PoleFilter*)*nbRow*nbCol);
  
  for(uint32_t i=0;i<nbRow;i++) {
    for(uint32_t j=0;j<nbCol;j++) {
      f[i*nbCol+j] = new PoleFilter(order);
      if (T->Value() != 0) {
        f[i*nbCol+j]->setup(1. / T->Value(), cutoff->Value());
      }
      f[i*nbCol+j]->reset();
    }
  }

  this->order=order;
  this->nbRow=nbRow;
  this->nbCol=nbCol;
  this->self=self;
}

ButterworthLowPass_impl::~ButterworthLowPass_impl() { 
  for(uint32_t i=0;i<nbRow;i++) {
    for(uint32_t j=0;j<nbCol;j++) {
     delete f[i*nbCol+j];
    }
  }
  free(f);
}

void ButterworthLowPass_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;
  }
  
  if (T->ValueChanged() && T->Value() != 0) {
    for(uint32_t i=0;i<nbRow;i++) {
      for(uint32_t j=0;j<nbCol;j++) {
        f[i*nbCol+j]->setup(1. / T->Value(), cutoff->Value());
        //settingsChanged(f[i*nbCol+j],input->Value(i, j));
      }
    }
  }

  if (T->Value() == 0) {
    delta_t = (float)(data->DataDeltaTime()) / 1000000000.;
    for(uint32_t i=0;i<nbRow;i++) {
      for(uint32_t j=0;j<nbCol;j++) {
        f[i*nbCol+j]->setup(1. / delta_t, cutoff->Value());
      }
    }
  } else {
    delta_t=T->Value();
  }
  
  // on prend une fois pour toute les mutex et on fait des accès directs
  output->GetMutex();
  input->GetMutex();

  if (cutoff->ValueChanged()) {
    for(uint32_t i=0;i<nbRow;i++) {
      for(uint32_t j=0;j<nbCol;j++) {
        f[i*nbCol+j]->setup(1. / delta_t, cutoff->Value());
        //settingsChanged(f[i*nbCol+j],input->ValueNoMutex(i, j));
      }
    }
  }

  if (delta_t!=0) {
    for(uint32_t i=0;i<nbRow;i++) {
      for(uint32_t j=0;j<nbCol;j++) {
        float result = f[i*nbCol+j]->filter(input->ValueNoMutex(i, j));
        output->SetValueNoMutex(i, j, result);
      }
    }
  }

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

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

//ne gere pas les oscillations (s'arrete des qu'une valeure est bonne a 5%)
void ButterworthLowPass_impl::settingsChanged(PoleFilter*f,float inputValue) {
  float result=f->filter(inputValue);
  
  while(result<inputValue*0.95 || result>inputValue*1.05) {
      result=f->filter(inputValue);
  }
}