//  created:    2011/05/01
//  filename:   DemoOpticalFlow.cpp
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    demo optical flow
//
//
/*********************************************************************/

#include "DemoOpticalFlow.h"
#include <Uav.h>
#include <Camera.h>
#include <CvtColor.h>
#include <OpticalFlow.h>
#include <OpticalFlowSpeed.h>
#include <LowPassFilter.h>
#include <cvmatrix.h>
#include <GridLayout.h>
#include <DataPlot1D.h>
#include <Tab.h>
#include <TabWidget.h>
#include <GroupBox.h>
#include <DoubleSpinBox.h>
#include <PushButton.h>
#include <FrameworkManager.h>
#include <MetaDualShock3.h>
#include <Vector2D.h>
#include <AhrsData.h>
#include <Ahrs.h>
#include <Pid.h>

#include <stdio.h>

using namespace std;
using namespace flair::core;
using namespace flair::gui;
using namespace flair::filter;
using namespace flair::meta;
using namespace flair::sensor;

DemoOpticalFlow::DemoOpticalFlow(TargetController *controller): UavStateMachine(controller) {
  Uav* uav=GetUav();
	if (uav->GetVerticalCamera() == NULL) {
        exit(1);
    }
uav->GetVerticalCamera()->SetLogFormat(Camera::LogFormat::JPG);
getFrameworkManager()->AddDeviceToLog(uav->GetVerticalCamera());
	
	startOpticalflow=new PushButton(GetButtonsLayout()->NewRow(),"start optical flow");

	greyCameraImage=new CvtColor(uav->GetVerticalCamera(),"gray",CvtColor::Conversion_t::ToGray);

	uav->GetVerticalCamera()->UseDefaultPlot(greyCameraImage->Output()); // Le defaultPlot de la caméra peut afficher n'importe quoi?

	//optical flow stack
	opticalFlow=new OpticalFlow(greyCameraImage,uav->GetVerticalCamera()->GetLayout()->NewRow(),"flux optique");
	opticalFlowSpeed=new OpticalFlowSpeed(opticalFlow,"vitesse du flux optique");

	cvmatrix_descriptor* desc=new cvmatrix_descriptor(2,1);
	for(int i=0;i<2;i++) {
			desc->SetElementName(i,0,opticalFlowSpeed->Output()->Name(i,0));
	}
	cvmatrix* prev_value=new cvmatrix((const Thread*)this,desc,floatType,uav->ObjectName()); // diamond inheritance
	for(int i=0;i<2;i++) {
			prev_value->SetValue(i,0,0);
	}

	opticalFlowSpeedFiltered=new LowPassFilter(opticalFlowSpeed,uav->GetVerticalCamera()->GetLayout()->NewRow(),"passe bas",prev_value);
// delete prev_value?

	getFrameworkManager()->AddDeviceToLog(opticalFlowSpeed);

	Tab* opticalFlowTab=new Tab(getFrameworkManager()->GetTabWidget(),"flux optique");
	DataPlot1D* xVelocityPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x_velocity",-5,5);
	DataPlot1D* yVelocityPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y_velocity",-5,5);

	xVelocityPlot->AddCurve(opticalFlowSpeed->Output()->Element(0,0));
	xVelocityPlot->AddCurve(opticalFlowSpeedFiltered->Matrix()->Element(0,0),DataPlot::Blue);
	yVelocityPlot->AddCurve(opticalFlowSpeed->Output()->Element(1,0));
	yVelocityPlot->AddCurve(opticalFlowSpeedFiltered->Matrix()->Element(1,0),DataPlot::Blue);

	u_x=new Pid(setupLawTab->At(1,0),"u_x");
	u_x->UseDefaultPlot(graphLawTab->NewRow());
	u_y=new Pid(setupLawTab->At(1,1),"u_y");
	u_y->UseDefaultPlot(graphLawTab->LastRowLastCol());

	opticalFlowGroupBox=new GroupBox(GetJoystick()->GetTab()->NewRow(),"consignes fo");
	maxXSpeed=new DoubleSpinBox(opticalFlowGroupBox->NewRow(),"debattement x"," m/s",-5,5,0.1,1);
	maxYSpeed=new DoubleSpinBox(opticalFlowGroupBox->LastRowLastCol(),"debattement y"," m/s",-5,5,0.1,1);

	opticalFlowReference=new cvmatrix((const Thread*)this,2,1,floatType);
	xVelocityPlot->AddCurve(opticalFlowReference->Element(0,0),DataPlot::Green,"consigne");
	yVelocityPlot->AddCurve(opticalFlowReference->Element(1,0),DataPlot::Green,"consigne");

	customReferenceOrientation= new AhrsData(this,"reference");
	uav->GetAhrs()->AddPlot(customReferenceOrientation,DataPlot::Yellow);
	AddDataToControlLawLog(customReferenceOrientation);
}

void DemoOpticalFlow::SignalEvent(Event_t event) {
    switch(event) {
    case Event_t::EnteringControlLoop:
        opticalFlowReference->SetValue(0,0,GetJoystick()->GetAxisValue(1)*maxXSpeed->Value());//joy axis 0 maps to x displacement
        opticalFlowReference->SetValue(1,0,GetJoystick()->GetAxisValue(0)*maxYSpeed->Value());//joy axis 1 maps to y displacement
        break;
    }
}

void DemoOpticalFlow::StartOpticalFlow(void) {
	if (SetOrientationMode(OrientationMode_t::Custom)) {
			Thread::Info("(Re)entering optical flow mode\n");
			u_x->Reset();
			u_y->Reset();
	} else {
			Thread::Warn("Could not enter optical flow mode\n");
	}
}

void DemoOpticalFlow::ExtraCheckPushButton(void) {
	if(startOpticalflow->Clicked()) {
        //StartOpticalFlow();
				GetUav()->GetVerticalCamera()->SavePictureToFile("./toto.jpg");
				
	}
}

void DemoOpticalFlow::ExtraCheckJoystick(void) {
    static bool wasOpticalFlowModeButtonPressed=false;
    // controller button R1 enters optical flow mode
    if(GetJoystick()->IsButtonPressed(9)) { // R1
        if (!wasOpticalFlowModeButtonPressed) {
            wasOpticalFlowModeButtonPressed=true;
            StartOpticalFlow();
        }
    } else {
        wasOpticalFlowModeButtonPressed=false;
    }
}

const AhrsData *DemoOpticalFlow::GetReferenceOrientation(void) {
    Euler refAngles=GetDefaultReferenceOrientation()->GetQuaternion().ToEuler();//to keep default yaw reference

    // /!\ in this demo, the target value is a speed (in pixel/s). As a consequence the error is the difference between the current speed and the target speed
    Vector2D error, errorVariation; // in Uav coordinate system

    //opticalFlow= matrice de déplacements en pixels entre 2 images consécutives
    //opticalFlowSpeed=vitesse de déplacement en pixel par seconde (moyenne sur tous les points et division par le delta T)
    error.x=opticalFlowSpeedFiltered->Output(0,0)-opticalFlowReference->Value(0,0);
    error.y=opticalFlowSpeedFiltered->Output(1,0)-opticalFlowReference->Value(1,0);

    //la dérivée est à la fréquence de la loi de commande ("rapide") alors que le flux optique est à la fréquence de la caméra
    // fréquemment la dérivée car le signal n'a pas bougé -> dérivée super crade
    //gsanahuj: brancher un eulerderivative derriere le opticalFlowSpeedFiltered pour avoir la derivee
    //opticalFlowSpeed doit etre renomme car finalement ce n'est pas une vitesse mais un deplacement
    errorVariation.x=0;
    errorVariation.y=0;

    u_x->SetValues(error.x, errorVariation.x);
    u_x->Update(GetTime());
    refAngles.pitch=u_x->Output();

    u_y->SetValues(error.y, errorVariation.y);
    u_y->Update(GetTime());
    refAngles.roll=-u_y->Output();

    customReferenceOrientation->SetQuaternionAndAngularRates(refAngles.ToQuaternion(),Vector3D(0,0,0));

    return customReferenceOrientation;
}

DemoOpticalFlow::~DemoOpticalFlow() {
}