//  created:    2012/04/12
//  filename:   OpticalFlow.cpp
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    calcul flux optique lk
//
//
/*********************************************************************/

#include "OpticalFlow.h"
#include "OpticalFlowData.h"
#include "VisionFilter.h"
#include <Image.h>
#include <Layout.h>
#include <GroupBox.h>
#include <SpinBox.h>
#include <OneAxisRotation.h>
#include <typeinfo>

#define LEVEL_PYR 2

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

class OpticalFlow_impl {
public:
    OpticalFlow_impl(flair::filter::OpticalFlow *self,const LayoutPosition* position,string name) {
        this->self=self;
        is_init=false;

        GroupBox* reglages_groupbox=new GroupBox(position,name);
        rotation=new OneAxisRotation(reglages_groupbox->NewRow(),"post rotation",OneAxisRotation::PostRotation);
        max_features=new SpinBox(reglages_groupbox->NewRow(),"max features:",1,65535,1,1);
        
        try{
            Image::Type const &imageType=dynamic_cast<Image::Type const &>(((IODevice*)(self->Parent()))->GetOutputDataType());
            pyr=new Image(self,imageType.GetWidth(),imageType.GetHeight(),Image::Type::Format::Gray);
            pyr_old=new Image(self,imageType.GetWidth(),imageType.GetHeight(),Image::Type::Format::Gray);
            output=new flair::filter::OpticalFlowData(self->Parent(),max_features->Value());
/*
            pointsA=(CvPoint*)AllocFunction(max_features->Value()*sizeof(CvPoint));
            pointsB=(CvPoint*)AllocFunction(max_features->Value()*sizeof(CvPoint));

            found_feature=(char*)AllocFunction(max_features->Value()*sizeof(char));
            feature_error=(unsigned int*)AllocFunction(max_features->Value()*sizeof(unsigned int));
            
            iplgimg = (IplImage*)AllocFunction(sizeof(IplImage));
            iplgimg_old = (IplImage*)AllocFunction(sizeof(IplImage));
            iplpyr = (IplImage*)AllocFunction(sizeof(IplImage));
            iplpyr_old = (IplImage*)AllocFunction(sizeof(IplImage));
            
            iplpyr->width=imageType.GetWidth();
            iplpyr->height=imageType.GetHeight();
            iplpyr_old->width=imageType.GetWidth();
            iplpyr_old->height=imageType.GetHeight();
*/  
      } catch(std::bad_cast& bc) {
            self->Err("io type mismatch\n");
            pyr=NULL;
            pyr_old=NULL;
        }
      
    }

    ~OpticalFlow_impl() {
        /*
        FreeFunction((char*)pointsA);
        FreeFunction((char*)pointsB);
        FreeFunction((char*)found_feature);
        FreeFunction((char*)feature_error);
        FreeFunction((char*)iplgimg);
        FreeFunction((char*)iplgimg_old);
        FreeFunction((char*)iplpyr);
        FreeFunction((char*)iplpyr_old);*/
    }
    
    void UpdateFrom(const io_data *data){
        //Time tStart=GetTime();
        Image *img=(Image*)data;
        Image *img_old=((Image*)data->Prev(1));
/*
        iplgimg->width=img->GetDataType().GetWidth();
        iplgimg->height=img->GetDataType().GetHeight();
        iplgimg->imageData=img->buffer;
        
        iplgimg_old->width=img_old->GetDataType().GetWidth();
        iplgimg_old->height=img_old->GetDataType().GetHeight();
        iplgimg_old->imageData=img_old->buffer;
        
        iplpyr->imageData=pyr->buffer;
        iplpyr_old->imageData=pyr_old->buffer;

        unsigned int count;
        CvSize window = {3,3};
        CvTermCriteria termination_criteria ={CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, .3 };
        unsigned int i;

        if(max_features->ValueChanged()==true) {
            FreeFunction((char*)pointsA);
            FreeFunction((char*)pointsB);
            FreeFunction((char*)found_feature);
            FreeFunction((char*)feature_error);
            pointsA=(CvPoint*)AllocFunction(max_features->Value()*sizeof(CvPoint));
            pointsB=(CvPoint*)AllocFunction(max_features->Value()*sizeof(CvPoint));
            found_feature=(char*)AllocFunction(max_features->Value()*sizeof(char));
            feature_error=(unsigned int*)AllocFunction(max_features->Value()*sizeof(unsigned int));

            output->Resize(max_features->Value());
        }

        if(is_init==false) {
            data->GetMutex();
            //init image old
            dspPyrDown(iplgimg,iplpyr_old,LEVEL_PYR);
            data->ReleaseMutex();
            is_init=true;
            printf("ajouter mise a 0 des points\n");
            return;
        }

        data->GetMutex();
        data->Prev(1)->GetMutex();

        //select good features
        count=max_features->Value();
        dspGoodFeaturesToTrack(iplgimg_old,pointsA,&count,0.08,5);
        //pyramide
        dspPyrDown(iplgimg,iplpyr,LEVEL_PYR);
        //lk
        dspCalcOpticalFlowPyrLK(iplgimg_old,iplgimg,iplpyr_old,iplpyr,pointsA,pointsB,count,window,LEVEL_PYR,found_feature,feature_error,termination_criteria,0) ;

        data->Prev(1)->ReleaseMutex();
        data->ReleaseMutex();

        //apply rotation
        for(i=0;i<count;i++) {
            Vector3Df tmp;
            tmp.x=pointsA[i].x;
            tmp.y=pointsA[i].y;
            tmp.z=0;
            rotation->ComputeRotation(tmp);
            pointsA[i].x=tmp.x;
            pointsA[i].y=tmp.y;

            tmp.x=pointsB[i].x;
            tmp.y=pointsB[i].y;
            tmp.z=0;
            rotation->ComputeRotation(tmp);
            pointsB[i].x=tmp.x;
            pointsB[i].y=tmp.y;
        }

        output->GetMutex();
        CvPoint2D32f* pointsBf= output->PointsB();
        for(i=0;i<count;i++) {
            pointsBf[i].x=pointsA[i].x+((float)pointsB[i].x)/256;
            pointsBf[i].y=pointsA[i].y+((float)pointsB[i].y)/256;
        }
        output->ReleaseMutex();

        output->SetPointsA(pointsA);
        output->SetFoundFeature(found_feature);
        output->SetFeatureError(feature_error);
        output->SetNbFeatures(count);

        //rotation
        swap(pyr,pyr_old);
*/
        output->SetDataTime(data->DataTime());
        //Printf("Optical flow computation time=%f\n",(GetTime()-tStart)/(1000.*1000));
    };
    
    flair::filter::OpticalFlowData *output;

private:
    flair::filter::OpticalFlow *self;
    SpinBox *max_features;
    OneAxisRotation* rotation;

    //CvPoint* pointsA;
    //CvPoint* pointsB;
    char *found_feature;
    unsigned int *feature_error;
    Image *pyr,*pyr_old;
    //IplImage *iplgimg,*iplgimg_old;
    //IplImage *iplpyr,*iplpyr_old;

    bool is_init;
};


namespace flair
{
namespace filter
{

OpticalFlow::OpticalFlow(const IODevice* parent,const LayoutPosition* position,string name) : IODevice(parent,name) {
    Printf("optical flow: voir pour faire du multiple output\n");//pour pts A et B, found et error
    pimpl_=new OpticalFlow_impl(this,position,name);
}

OpticalFlow::~OpticalFlow(void) {
    delete pimpl_;
}

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

OpticalFlowData* OpticalFlow::Output(void) {
    return pimpl_->output;

}

DataType const &OpticalFlow::GetOutputDataType() const {
    if(pimpl_->output!=NULL) {
        return pimpl_->output->GetDataType();
    } else {
        return dummyType;
    }
}

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