// %flair:license{
// This file is part of the Flair framework distributed under the
// CECILL-C License, Version 1.0.
// %flair:license}
//  created:    2014/01/14
//  filename:   OpticalFlowCompensated.cpp
//
//  author:     Gildas Bayard
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    Compensate optical flow data for rotations
//
//
/*********************************************************************/
#include "OpticalFlowCompensated.h"
#include <io_data.h>
#include <Ahrs.h>
#include <AhrsData.h>
#include <OpticalFlow.h>
#include <OpticalFlowData.h>
#include <Euler.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;

class OpticalFlowCompensated_impl {
public:
    OpticalFlowCompensated_impl(flair::filter::OpticalFlowCompensated *self,const flair::filter::Ahrs *ahrs, const LayoutPosition* position, string name): ahrs(ahrs), output(NULL) {
        this->self=self;
        previousStepsAngularRates=new Vector3Df*[10];
        for (int i=0; i<10; i++) previousStepsAngularRates[i]=NULL;
        previousStepsAngularRatesIndex=0;

        GroupBox* reglages_groupbox=new GroupBox(position,name);
        //TODO: the gyroDelay is set to compensate for the time difference between image snapshot et gyro measurements
        //it is equal to the time between 2 images (because optical flow always lags 1 frame) + the time to compute optical flow
        //here it is approximated by 2 video frames
        gyroDelay=new SpinBox(reglages_groupbox->NewRow(),"gyro delay (in video frames):",0,10,1,2);
        gyroGain=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"gyro gain:",0.,500.,10.,2,300.);

       
    }

    ~OpticalFlowCompensated_impl() {
        delete output;
    }
    
    void UpdateFrom(const io_data *data){
        flair::filter::OpticalFlowData *input=(flair::filter::OpticalFlowData *)data;
        if (!output) { //first pass
            output=new flair::filter::OpticalFlowData(self,input->MaxFeatures(),input->ObjectName()+"_filtered");
            previousTime=input->DataTime();
            return;
        }
        //  float kX=320/Euler::ToRadian(70); //TEST: only for default simuCameraGL. fov=70° and image width=320 => k=320*180/(70*pi)
        //  float kY=240/Euler::ToRadian(70); //TEST: only for default simuCameraGL. fov=70° and image height=240
        float kX=gyroGain->Value();
        float kY=gyroGain->Value();
        float deltaT=(input->DataTime()-previousTime)/(1000*1000*1000.);
        previousTime=input->DataTime();

        int delayedIndex=previousStepsAngularRatesIndex-gyroDelay->Value(); // Ahhh décalage, esprit canal...
        if (delayedIndex<0) delayedIndex+=10;

        if (!previousStepsAngularRates[previousStepsAngularRatesIndex]) {
            previousStepsAngularRates[previousStepsAngularRatesIndex]=new Vector3Df();
        }
        *previousStepsAngularRates[previousStepsAngularRatesIndex++]=ahrs->GetDatas()->GetAngularRates();

        if (!previousStepsAngularRates[delayedIndex]) return;
        float rotationFlowX=previousStepsAngularRates[delayedIndex]->y*deltaT*kY;
        float rotationFlowY=-previousStepsAngularRates[delayedIndex]->x*deltaT*kX;
        if (previousStepsAngularRatesIndex==10) previousStepsAngularRatesIndex=0;
        input->GetMutex();
        output->GetMutex();

        for (int i=0; i<input->NbFeatures(); i++) {
            output->PointsA()[i].x=input->PointsA()[i].x;
            output->PointsA()[i].y=input->PointsA()[i].y;
            output->PointsB()[i].x=input->PointsB()[i].x-rotationFlowX;
            output->PointsB()[i].y=input->PointsB()[i].y-rotationFlowY;
        }
        output->SetNbFeatures(input->NbFeatures());
        output->SetFoundFeature(input->FoundFeature());
        output->SetFeatureError(input->FeatureError());

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

        output->SetDataTime(input->DataTime());
           
    };
    
    flair::filter::OpticalFlowData *output;

private:
    flair::filter::OpticalFlowCompensated *self;
    Time previousTime;
    const flair::filter::Ahrs *ahrs;
    Vector3Df **previousStepsAngularRates;
    unsigned int previousStepsAngularRatesIndex;
    SpinBox *gyroDelay;
    DoubleSpinBox *gyroGain;
};


namespace flair {
namespace filter {

OpticalFlowCompensated::OpticalFlowCompensated(const OpticalFlow *parent, const Ahrs *ahrs, const LayoutPosition* position, string name) : IODevice(parent, name) {
    pimpl_=new OpticalFlowCompensated_impl(this,ahrs,position,name);

}

OpticalFlowCompensated::~OpticalFlowCompensated() {
    delete pimpl_;
}

void OpticalFlowCompensated::UpdateFrom(const io_data *data) {
    pimpl_->UpdateFrom(data);
    ProcessUpdate(pimpl_->output);
}


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