//  created:    2015/11/05
//  filename:   SimpleFleet.cpp
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    demo fleet
//
//
/*********************************************************************/

#include "SimpleFleet.h"
#include <TargetController.h>
#include <Uav.h>
#include <GridLayout.h>
#include <PushButton.h>
#include <DataPlot1D.h>
#include <Ahrs.h>
#include <MetaUsRangeFinder.h>
#include <MetaDualShock3.h>
#include <FrameworkManager.h>
#include <VrpnClient.h>
#include <MetaVrpnObject.h>
#include <TrajectoryGenerator2DCircle.h>
#include <Vector3D.h>
#include <Vector2D.h>
#include <PidThrust.h>
#include <Euler.h>
#include <cvmatrix.h>
#include <AhrsData.h>
#include <Ahrs.h>
#include <DoubleSpinBox.h>
#include <stdio.h>
#include <cmath>
#include <Tab.h>
#include <Pid.h>
#include <UdpSocket.h>
#include <string.h>

#define PI ((float)3.14159265358979323846)

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


SimpleFleet::SimpleFleet(string broadcast,TargetController *controller): UavStateMachine(controller), behaviourMode(BehaviourMode_t::Default), vrpnLost(false) {
    Uav* uav=GetUav();
		
	VrpnClient* vrpnclient=new VrpnClient("vrpn", uav->GetDefaultVrpnAddress(),80);
	uavVrpn = new MetaVrpnObject(uav->ObjectName());
	getFrameworkManager()->AddDeviceToLog(uavVrpn);
	uav->GetAhrs()->YawPlot()->AddCurve(uavVrpn->State()->Element(2),DataPlot::Green);
	vrpnclient->Start();

    circle=new TrajectoryGenerator2DCircle(vrpnclient->GetLayout()->NewRow(),"circle");
    uavVrpn->xPlot()->AddCurve(circle->Matrix()->Element(0,0),0,0,255);
    uavVrpn->yPlot()->AddCurve(circle->Matrix()->Element(0,1),0,0,255);
    uavVrpn->VxPlot()->AddCurve(circle->Matrix()->Element(1,0),0,0,255);
    uavVrpn->VyPlot()->AddCurve(circle->Matrix()->Element(1,1),0,0,255);

    xCircleCenter=new DoubleSpinBox(vrpnclient->GetLayout()->NewRow(),"x circle center"," m",-5,5,0.1,1,0);
    yCircleCenter=new DoubleSpinBox(vrpnclient->GetLayout()->NewRow(),"y circle center"," m",-5,5,0.1,1,0);
    yDisplacement=new DoubleSpinBox(vrpnclient->GetLayout()->NewRow(),"y displacement"," m",0,2,0.1,1,0);

    //parent->AddDeviceToLog(Uz());

    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());

    message=new UdpSocket(uav,"Message",broadcast,true);

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

    customOrientation=new AhrsData(this,"orientation");
/*
    //check init conditions
    Vector3D uav_pos;
    Euler vrpn_euler;
    GetVrpnObject()->GetPosition(uav_pos);
    GetVrpnObject()->GetEuler(vrpn_euler);

    if(name=="x8_0") {
        //x8_0 should be on the left, with 0 yaw
        if(uav_pos.y>yCircleCenter->Value() || vrpn_euler.yaw>20 || vrpn_euler.yaw<-20) Thread::Err("wrong init position\n");
    }
    if(name=="x8_1") {
        //x8_1 should be on the right, with 180 yaw
        if(uav_pos.y<yCircleCenter->Value() || (vrpn_euler.yaw<160 && vrpn_euler.yaw>-160)) Thread::Err("wrong init position %f %f\n",yCircleCenter->Value(),vrpn_euler.yaw);
    }
    */
}

SimpleFleet::~SimpleFleet() {
}

const AhrsData *SimpleFleet::GetOrientation(void) const {
    //get yaw from vrpn
		Quaternion vrpnQuaternion;
    uavVrpn->GetQuaternion(vrpnQuaternion);

    //get roll, pitch and w from imu
    Quaternion ahrsQuaternion;
    Vector3Df ahrsAngularSpeed;
    GetDefaultOrientation()->GetQuaternionAndAngularRates(ahrsQuaternion, ahrsAngularSpeed);

    Euler ahrsEuler=ahrsQuaternion.ToEuler();
    ahrsEuler.yaw=vrpnQuaternion.ToEuler().yaw;
    Quaternion mixQuaternion=ahrsEuler.ToQuaternion();

    customOrientation->SetQuaternionAndAngularRates(mixQuaternion,ahrsAngularSpeed);

    return customOrientation;
}

void SimpleFleet::AltitudeValues(float &z,float &dz) const {
    Vector3Df uav_pos,uav_vel;

    uavVrpn->GetPosition(uav_pos);
    uavVrpn->GetSpeed(uav_vel);
    //z and dz must be in uav's frame
    z=-uav_pos.z;
    dz=-uav_vel.z;
}

const AhrsData *SimpleFleet::GetReferenceOrientation(void) {
    Vector2Df pos_err, vel_err; // in uav coordinate system
    float yaw_ref;
    Euler refAngles;

    PositionValues(pos_err, vel_err, yaw_ref);

    refAngles.yaw=yaw_ref;

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

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

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

    return customReferenceOrientation;
}

void SimpleFleet::PositionValues(Vector2Df &pos_error,Vector2Df &vel_error,float &yaw_ref) {
    Vector3Df uav_pos,uav_vel; // in VRPN coordinate system
    Vector2Df uav_2Dpos,uav_2Dvel; // in VRPN coordinate system

    uavVrpn->GetPosition(uav_pos);
    uavVrpn->GetSpeed(uav_vel);

    uav_pos.To2Dxy(uav_2Dpos);
    uav_vel.To2Dxy(uav_2Dvel);

    if (behaviourMode==BehaviourMode_t::PositionHold1 || behaviourMode==BehaviourMode_t::PositionHold2
        || behaviourMode==BehaviourMode_t::PositionHold3 || behaviourMode==BehaviourMode_t::PositionHold4) {
        pos_error=uav_2Dpos-posHold;
        vel_error=uav_2Dvel;
        yaw_ref=yawHold;
    } else { //Circle
        Vector2Df circle_pos,circle_vel;
        Vector2Df target_2Dpos;

        //circle center
        target_2Dpos.x=xCircleCenter->Value();
        target_2Dpos.y=yCircleCenter->Value();
        circle->SetCenter(target_2Dpos);

        //circle reference
        circle->Update(GetTime());
        circle->GetPosition(circle_pos);
        circle->GetSpeed(circle_vel);

        //error in optitrack frame
        pos_error=uav_2Dpos-circle_pos;
        vel_error=uav_2Dvel-circle_vel;
        yaw_ref=PI/2+atan2(uav_pos.y-target_2Dpos.y,uav_pos.x-target_2Dpos.x);
    }
    //error in uav frame
    Quaternion currentQuaternion=GetCurrentQuaternion();
    Euler currentAngles;//in vrpn frame
    currentQuaternion.ToEuler(currentAngles);
    pos_error.Rotate(-currentAngles.yaw);
    vel_error.Rotate(-currentAngles.yaw);
}

void SimpleFleet::SignalEvent(Event_t event) {
    UavStateMachine::SignalEvent(event);

    switch(event) {
    case Event_t::EmergencyStop:
        message->SendMessage("EmergencyStop");
        break;
    case Event_t::TakingOff:
        //behaviourMode=BehaviourMode_t::Default;
        message->SendMessage("TakeOff");
        VrpnPositionHold();
        behaviourMode=BehaviourMode_t::PositionHold1;
        break;
    case Event_t::StartLanding:
        VrpnPositionHold();
        behaviourMode=BehaviourMode_t::PositionHold4;
        message->SendMessage("Landing");
        break;
    case Event_t::EnteringControlLoop:
        CheckMessages();
        if ((behaviourMode==BehaviourMode_t::Circle1) && (!circle->IsRunning())) {
            VrpnPositionHold();
            behaviourMode=BehaviourMode_t::PositionHold2;
            if(posHold.y<0) {
                posHold.y-=yDisplacement->Value();
            } else {
                posHold.y+=yDisplacement->Value();
            }
            posWait=GetTime();
            Printf("Circle1 -> PositionHold2\n");
        }
        if (behaviourMode==BehaviourMode_t::PositionHold2 && GetTime()>(posWait+3*(Time)1000000000)) {
            behaviourMode=BehaviourMode_t::PositionHold3;
            if(posHold.y<0) {
                posHold.y+=yDisplacement->Value();
            } else {
                posHold.y-=yDisplacement->Value();
            }
            posWait=GetTime();
            Printf("PositionHold2 -> PositionHold3\n");
        }
        if (behaviourMode==BehaviourMode_t::PositionHold3 && GetTime()>(posWait+3*(Time)1000000000)) {
            behaviourMode=BehaviourMode_t::Circle2;
            StartCircle();
            Printf("PositionHold3 -> Circle2\n");
        }
        if ((behaviourMode==BehaviourMode_t::Circle2) && (!circle->IsRunning())) {
            Printf("Circle2 -> Land\n");
            behaviourMode=BehaviourMode_t::PositionHold4;
            Land();
        }

        break;
    case Event_t::EnteringFailSafeMode:
        behaviourMode=BehaviourMode_t::Default;
        break;
    case Event_t::ZTrajectoryFinished:
        Printf("PositionHold1 -> Circle1\n");
        StartCircle();
        behaviourMode=BehaviourMode_t::Circle1;
        break;
    }
}

void SimpleFleet::CheckMessages(void) {
    char msg[64];
    char src[64];
    size_t src_size=sizeof(src);
    while(message->RecvMessage(msg,sizeof(msg),TIME_NONBLOCK,src,&src_size)>0) {
        //printf("%s %s\n",GetUav()->ObjectName().c_str(),src);
        if(strcmp(src,GetUav()->ObjectName().c_str())!=0) {
            /*
            if(strcmp(msg,"StopMotors")==0 && orientation_state!=OrientationState_t::Stopped)
            {
                joy->FlashLed(DualShock3::led1,10,10);
                joy->Rumble(0x70);
                GetBldc()->SetEnabled(false);
                GetUavMultiplex()->UnlockUserInterface();
                altitude_state=AltitudeState_t::Stopped;
                orientation_state=OrientationState_t::Stopped;
                GetAhrs()->UnlockUserInterface();
            }
*/
            if(strcmp(msg,"TakeOff")==0) {
                Printf("TakeOff fleet\n");
                TakeOff();
            }
            if(strcmp(msg,"Landing")==0) {
                Printf("Landing fleet\n");
                Land();
            }
            if(strcmp(msg,"EmergencyStop")==0) {
                Printf("EmergencyStop fleet\n");
                EmergencyStop();
            }
        }
    }
}

void SimpleFleet::ExtraSecurityCheck(void) {
    if (!vrpnLost && behaviourMode!=BehaviourMode_t::Default) {
        if (!uavVrpn->IsTracked(500)) {
            Thread::Err("Optitrack, uav lost\n");
            vrpnLost=true;
            EnterFailSafeMode();
            Land();
        }
    }
}

void SimpleFleet::ExtraCheckJoystick(void) {

}

void SimpleFleet::StartCircle(void) {
    if (SetOrientationMode(OrientationMode_t::Custom)) {
        Thread::Info("Demo flotte: start circle\n");
    } else {
        Thread::Warn("Demo flotte: could not start circle\n");
        return;
    }
    Vector3Df uav_pos;
    Vector2Df uav_2Dpos,target_2Dpos;

    //circle center
    target_2Dpos.x=xCircleCenter->Value();
    target_2Dpos.y=yCircleCenter->Value();
    circle->SetCenter(target_2Dpos);

    uavVrpn->GetPosition(uav_pos);
    uav_pos.To2Dxy(uav_2Dpos);
    circle->StartTraj(uav_2Dpos,1);

    u_x->Reset();
    u_y->Reset();
}

void SimpleFleet::StopCircle(void) {
    circle->FinishTraj();
    //joy->Rumble(0x70);
    Thread::Info("Demo flotte: finishing circle\n");
}

void SimpleFleet::VrpnPositionHold(void) {
  if (SetOrientationMode(OrientationMode_t::Custom)) {
        Thread::Info("Demo flotte: holding position\n");
    } else {
        Thread::Info("Demo flotte: could not hold position\n");
        //return;
    }  
		
		Quaternion vrpnQuaternion;
    uavVrpn->GetQuaternion(vrpnQuaternion);
		yawHold=vrpnQuaternion.ToEuler().yaw;

		Vector3Df vrpnPosition;
    uavVrpn->GetPosition(vrpnPosition);
    vrpnPosition.To2Dxy(posHold);

    u_x->Reset();
    u_y->Reset();
}