// %flair:license{
// This file is part of the Flair framework distributed under the
// CECILL-C License, Version 1.0.
// %flair:license}
//  created:    2016/02/08
//  filename:   SimuX4.cpp
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    Class defining a simulation x4 uav
//
//
/*********************************************************************/
#ifdef CORE2_64

#include "SimuX4.h"
#include <FrameworkManager.h>
#include <X4X8Multiplex.h>
#include <SimulatedImu.h>
#include <SimulatedAhrs.h>
#include <SimulatedBldc.h>
#include <SimulatedUs.h>
#include <SimulatedCamera.h>
#include <SimulatedPressureSensor.h>
#include <BatteryMonitor.h>
#include <SimulatedGps.h>
#include <Tab.h>
#include <FindArgument.h>

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

namespace flair {
namespace meta {

SimuX4::SimuX4(string name, uint32_t simu_id,string options,
               filter::UavMultiplex *multiplex)
    : Uav(name, multiplex) {

  if (multiplex == NULL)
    SetMultiplex(new X4X8Multiplex("motors", X4X8Multiplex::X4));

  SetBldc(new SimulatedBldc(GetUavMultiplex(), GetUavMultiplex()->GetLayout(),
                       "motors", GetUavMultiplex()->MotorsCount(), simu_id,0));
  SetUsRangeFinder(new SimulatedUs("us", simu_id,0, 60));
  SetAhrs(new SimulatedAhrs("ahrs", simu_id, 0,70));
  Tab *bat_tab = new Tab(getFrameworkManager()->GetTabWidget(), "battery");
  SetBatteryMonitor(new BatteryMonitor(bat_tab->NewRow(), "battery"));
  GetBatteryMonitor()->SetBatteryValue(12);
  
  uint16_t camvWidth=320,camvHeight=240;
	ReadCameraResolutionOption(options,"camv",camvWidth,camvHeight);
  Info("using vertical camera resolution: %ix%i\n",camvWidth, camvHeight);
  SetVerticalCamera(new SimulatedCamera("simu_cam_v", camvWidth, camvHeight, 3, simu_id,0, 10));
  
  uint16_t camhWidth=320,camhHeight=240;
  ReadCameraResolutionOption(options,"camh",camhWidth,camhHeight);
  Info("using horizontal camera resolution: %ix%i\n",camhWidth, camhHeight);
  SetHorizontalCamera(new SimulatedCamera("simu_cam_h", camhWidth, camhHeight, 3, simu_id,1, 10));
  
  string useGps=FindArgument(options,"use_gps=",false);
  if(useGps=="true") {
    SetGps(new SimulatedGps("gps", (NmeaGps::NMEAFlags_t)(NmeaGps::GGA | NmeaGps::VTG), 0,0, 40));
  }
  
  string usePressureSensor=FindArgument(options,"use_pressure_sensor=",false);
  if(usePressureSensor=="true") {
    SetPressureSensor(new SimulatedPressureSensor("pressuresensor", 0,0, 10));
  } 
}

SimuX4::~SimuX4() {}

void SimuX4::StartSensors(void) {
  ((SimulatedImu *)(GetAhrs()->GetImu()))->Start();
  ((SimulatedUs *)GetUsRangeFinder())->Start();
  ((SimulatedCamera *)GetVerticalCamera())->Start();
  ((SimulatedCamera *)GetHorizontalCamera())->Start();
  if(GetGps()) ((SimulatedGps *)GetGps())->Start();
  if(GetPressureSensor()) ((SimulatedPressureSensor *)GetPressureSensor())->Start();
}

void SimuX4::ReadCameraResolutionOption(string options,string cameraName,uint16_t &camWidth,uint16_t &camHeight) const {
  string camOpts=FindArgument(options,cameraName +"=",false);
	if(camOpts!="") {
    size_t position=camOpts.find("x");
    if(position!=std::string::npos) {
        camWidth=std::stoi(camOpts.substr(0,position));
        camHeight=std::stoi(camOpts.substr(position+1,std::string::npos));
    } else {
      Warn("bad camera resolution parameter (%s) should be WIDTHxHEIGHT format\n",camOpts.c_str());
    }
  }
}

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

#endif