//  created:    2020/12/09
//  filename:   CircleFollower.cpp
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    demo cercle avec optitrack
//
//
/*********************************************************************/

#include "CircleFollower.h"
#include <TargetController.h>
#include <GridLayout.h>
#include <PushButton.h>
#include <DataPlot1D.h>
#include <DataPlot2D.h>
#include <FrameworkManager.h>
#include <VrpnClient.h>
#include <MetaVrpnObject.h>
#include <TrajectoryGenerator2DCircle.h>
#include <Matrix.h>
#include <Tab.h>
#include <Pid.h>
#include <Ugv.h>
#include <UgvControls.h>

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

CircleFollower::CircleFollower(string name,TargetController *controller): Thread(getFrameworkManager(),"CircleFollower",50), behaviourMode(BehaviourMode_t::Default), vrpnLost(false) {
   this->controller=controller;
    controller->Start();
    
    Ugv* ugv=GetUgv();
    ugv->UseDefaultPlot();
    
    VrpnClient* vrpnclient=new VrpnClient("vrpn", ugv->GetDefaultVrpnAddress(),80);
    
    if(vrpnclient->ConnectionType()==VrpnClient::Xbee) {
        ugvVrpn = new MetaVrpnObject(name,(uint8_t)0);
        targetVrpn=new MetaVrpnObject("target",1);
    } else if (vrpnclient->ConnectionType()==VrpnClient::Vrpn) {
        ugvVrpn = new MetaVrpnObject(name);
        targetVrpn=new MetaVrpnObject("target");
    }
    
    getFrameworkManager()->AddDeviceToLog(ugvVrpn);
    getFrameworkManager()->AddDeviceToLog(targetVrpn);
    vrpnclient->Start();
	
	Tab *ugvTab = new Tab(getFrameworkManager()->GetTabWidget(), "ugv", 0);
    GridLayout* buttonslayout = new GridLayout(ugvTab->NewRow(), "buttons");
	button_kill = new PushButton(buttonslayout->NewRow(), "kill");
    startCircle=new PushButton(buttonslayout->NewRow(),"start_circle");
    stopCircle=new PushButton(buttonslayout->LastRowLastCol(),"stop_circle");
    
    circle=new TrajectoryGenerator2DCircle(vrpnclient->GetLayout()->NewRow(),"circle");
    ugvVrpn->xPlot()->AddCurve(circle->GetMatrix()->Element(0,0),DataPlot::Blue);
    ugvVrpn->yPlot()->AddCurve(circle->GetMatrix()->Element(0,1),DataPlot::Blue);
    ugvVrpn->VxPlot()->AddCurve(circle->GetMatrix()->Element(1,0),DataPlot::Blue);
    ugvVrpn->VyPlot()->AddCurve(circle->GetMatrix()->Element(1,1),DataPlot::Blue);
    ugvVrpn->XyPlot()->AddCurve(circle->GetMatrix()->Element(0,1),circle->GetMatrix()->Element(0,0),DataPlot::Blue,"circle");
/*
    uX=new Pid(setupLawTab->At(1,0),"u_x");
    uX->UseDefaultPlot(graphLawTab->NewRow());
    uY=new Pid(setupLawTab->At(1,1),"u_y");
    uY->UseDefaultPlot(graphLawTab->LastRowLastCol());*/
}

CircleFollower::~CircleFollower() {
}

void CircleFollower::Run(void) {
    WarnUponSwitches(true);
    SetPeriodMS(100);
    
    if (getFrameworkManager()->ErrorOccured() == true) {
        SafeStop();
    }

    while (!ToBeStopped()) {
        float speed=-controller->GetAxisValue(3);
        float turn=controller->GetAxisValue(0);
       
        //sumoControl->move(x, rot);
        GetUgv()->GetUgvControls()->SetControls(speed,turn);
        if (button_kill->Clicked() == true)
            SafeStop();
        
        WaitPeriod();
    }
}

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

    targetVrpn->GetPosition(target_pos);
    target_pos.To2Dxy(target_2Dpos);
    circle->SetCenter(target_2Dpos);

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

    uX->Reset();
    uY->Reset();
    behaviourMode=BehaviourMode_t::Circle;*/
}

void CircleFollower::StopCircle(void) {
	/*
    circle->FinishTraj();
    //GetJoystick()->Rumble(0x70);
    Thread::Info("CircleFollower: finishing circle\n");*/
}