//  created:    2014/06/10
//  filename:   Uav.cpp
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    Base class to construct sensors/actuators depending on uav type
//
//
/*********************************************************************/

#include "Uav.h"
#include <FrameworkManager.h>
#include <Tab.h>
#include <GridLayout.h>
#include <DataPlot1D.h>
#include <VrpnClient.h>
#include <Ahrs.h>
#include <Imu.h>
#include <UavMultiplex.h>
#include <UsRangeFinder.h>
#include <Bldc.h>
#include <cvmatrix.h>
#include "MetaUsRangeFinder.h"
#include "MetaVrpnObject.h"

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

namespace flair { namespace meta {

Uav::Uav(FrameworkManager* parent,string name,UavMultiplex *multiplex): Object(parent,name) {
    vrpnclient=NULL;
    uav_vrpn=NULL;
    verticalCamera=NULL;
    this->multiplex=multiplex;
}

Uav::~Uav() {
}

void Uav::SetUsRangeFinder(const UsRangeFinder *inUs) {
    us=(UsRangeFinder*)inUs;
    meta_us=new MetaUsRangeFinder(us);
    getFrameworkManager()->AddDeviceToLog(us);
}

void Uav::SetAhrs(const Ahrs *inAhrs) {
    ahrs=(Ahrs*)inAhrs;
    imu=(Imu*)ahrs->GetImu();
    getFrameworkManager()->AddDeviceToLog(imu);
}

void Uav::SetBldc(const Bldc* inBldc) {
    bldc=(Bldc*)inBldc;
}

void Uav::SetBatteryMonitor(const BatteryMonitor* inBattery) {
    battery=(BatteryMonitor*)inBattery;
}

void Uav::SetMultiplex(const UavMultiplex *inMultiplex) {
    multiplex=(UavMultiplex*)inMultiplex;
    getFrameworkManager()->AddDeviceToLog(multiplex);
}

void Uav::SetVerticalCamera(const Camera* inVerticalCamera) {
    verticalCamera=(Camera*)inVerticalCamera;
}
/*
void Uav::SetupVRPNSerial(SerialPort *vrpn_port,string name,int VRPNSerialObjectId) {
    vrpnclient=new VrpnClient(getFrameworkManager(),"vrpn",vrpn_port,10000,80);
    uav_vrpn=new MetaVrpnObject(vrpnclient,name,VRPNSerialObjectId);

    getFrameworkManager()->AddDeviceToLog(uav_vrpn);
}
*/
void Uav::SetupVRPNAutoIP(string name) {
    SetupVRPN("192.168.147.197:3883",name);
}

void Uav::SetupVRPN(string optitrack_address,string name) {
    vrpnclient=new VrpnClient(getFrameworkManager(),"vrpn",optitrack_address,10000,80);
    uav_vrpn=new MetaVrpnObject(vrpnclient,name);

    getFrameworkManager()->AddDeviceToLog(uav_vrpn);

    GetAhrs()->YawPlot()->AddCurve(uav_vrpn->State()->Element(2),DataPlot::Green);
    //GetAhrs()->RollPlot()->AddCurve(uav_vrpn->State()->Element(0),DataPlot::Green);
    //GetAhrs()->PitchPlot()->AddCurve(uav_vrpn->State()->Element(1),DataPlot::Green);
}

void Uav::StartSensors(void)  {
    if(vrpnclient!=NULL) {
        vrpnclient->Start();
    }
}
void Uav::UseDefaultPlot(void) {
    multiplex->UseDefaultPlot();

    if(bldc->HasSpeedMeasurement()) {
        Tab* plot_tab=new Tab(multiplex->GetTabWidget(),"Speeds");
        DataPlot1D* plots[4];
        plots[0]=new DataPlot1D(plot_tab->NewRow(),"front left",0,7000);
        plots[1]=new DataPlot1D(plot_tab->LastRowLastCol(),"front right",0,7000);
        plots[2]=new DataPlot1D(plot_tab->NewRow(),"rear left",0,7000);
        plots[3]=new DataPlot1D(plot_tab->LastRowLastCol(),"rear right",0,7000);

        if(bldc->MotorsCount()==8) {
            for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i),0),DataPlot::Red,"top");
            for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i+4),0),DataPlot::Blue,"bottom");
        } else {
            for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i),0));
        }
    }

    if(bldc->HasCurrentMeasurement()) {
        Tab* plot_tab=new Tab(multiplex->GetTabWidget(),"Currents");
        DataPlot1D* plots[4];
        plots[0]=new DataPlot1D(plot_tab->NewRow(),"front left",0,10);
        plots[1]=new DataPlot1D(plot_tab->LastRowLastCol(),"front right",0,10);
        plots[2]=new DataPlot1D(plot_tab->NewRow(),"rear left",0,10);
        plots[3]=new DataPlot1D(plot_tab->LastRowLastCol(),"rear right",0,10);

        if(bldc->MotorsCount()==8) {
            for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i),1),DataPlot::Red,"top");
            for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i+4),1),DataPlot::Blue,"bottom");
        } else {
            for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i),1));
        }
    }

    meta_us->UseDefaultPlot();
    ahrs->UseDefaultPlot();
}

UavMultiplex* Uav::GetUavMultiplex(void) const {
    return multiplex;
}

Bldc* Uav::GetBldc(void) const {
    return bldc;
}

Ahrs* Uav::GetAhrs(void) const {
    return ahrs;
}

Imu* Uav::GetImu(void) const {
    return imu;
}

MetaUsRangeFinder* Uav::GetMetaUsRangeFinder(void) const {
    return meta_us;
}

UsRangeFinder* Uav::GetUsRangeFinder(void) const {
    return us;
}

BatteryMonitor* Uav::GetBatteryMonitor(void) const {
    return battery;
}

VrpnClient* Uav::GetVrpnClient(void) const {
    if(vrpnclient==NULL) Err("vrpn is not setup, call SetupVRPN before\n");
    return vrpnclient;
}

MetaVrpnObject* Uav::GetVrpnObject(void) const {
    return uav_vrpn;
}

Camera* Uav::GetVerticalCamera(void) const {
    return verticalCamera;
}

} // end namespace meta
} // end namespace flair