// %flair:license{
// This file is part of the Flair framework distributed under the
// CECILL-C License, Version 1.0.
// %flair:license}
//  created:    2022/01/05
//  filename:   PlaneStateMachine.cpp
//
//  author:     Gildas Bayard, Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    state machine class for plane
//
//
/*********************************************************************/

#include "PlaneStateMachine.h"
#include "Plane.h"
#include <GridLayout.h>
#include <DoubleSpinBox.h>
#include <Tab.h>
#include <TabWidget.h>
#include <PushButton.h>
#include <UavMultiplex.h>
#include <Bldc.h>
#include <Servos.h>
#include <Ahrs.h>
#include <ControlLaw.h>
#include <Pid.h>
#include <PidThrust.h>
#include <NestedSat.h>
#include <MetaDualShock3.h>
#include <AhrsData.h>
#include <BatteryMonitor.h>
#include <FrameworkManager.h>
#include <math.h>

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

PlaneStateMachine::PlaneStateMachine(TargetController *controller):
  Thread(getFrameworkManager(),"PlaneStateMachine",50),
  uav(GetPlane()),controller(controller),failSafeMode(true),flagConnectionLost(false),flagBatteryLow(false),flagCriticalSensorLost(false),safeToFly(true){

  uav->UseDefaultPlot();

  Tab *uavTab = new Tab(getFrameworkManager()->GetTabWidget(), "uav", 0);
  buttonslayout = new GridLayout(uavTab->NewRow(), "buttons");
  button_kill = new PushButton(buttonslayout->NewRow(), "kill");
  button_start_log = new PushButton(buttonslayout->NewRow(), "start_log");
  button_stop_log = new PushButton(buttonslayout->LastRowLastCol(), "stop_log");
  button_start = new PushButton(buttonslayout->NewRow(), "start");
  button_stop = new PushButton(buttonslayout->LastRowLastCol(), "stop");
  thrust = new DoubleSpinBox(buttonslayout->NewRow(), "fixed thrust",0, 50, 0.1, 2);
                        
 
  Tab *lawTab = new Tab(getFrameworkManager()->GetTabWidget(), "control laws");
  TabWidget *tabWidget = new TabWidget(lawTab->NewRow(), "laws");
  setupLawTab = new Tab(tabWidget, "Setup");
  graphLawTab = new Tab(tabWidget, "Graphes");

  uRoll = new NestedSat(setupLawTab->At(0, 0), "u_roll");
  uRoll->ConvertSatFromDegToRad();
  uRoll->UseDefaultPlot(graphLawTab->NewRow());

  uPitch = new NestedSat(setupLawTab->At(0, 1), "u_pitch");
  uPitch->ConvertSatFromDegToRad();
  uPitch->UseDefaultPlot(graphLawTab->LastRowLastCol());

  uYaw = new Pid(setupLawTab->At(0, 2), "u_yaw");
  uYaw->UseDefaultPlot(graphLawTab->LastRowLastCol());

  getFrameworkManager()->AddDeviceToLog(uRoll);
  uRoll->AddDeviceToLog(uPitch);
  uRoll->AddDeviceToLog(uYaw);

  joy=new MetaDualShock3("uav high level controller",controller);
  uav->GetAhrs()->AddPlot(joy->GetReferenceOrientation(),DataPlot::Blue);

  orientationMode = OrientationMode_t::Manual;
  torqueMode = TorqueMode_t::Default;
}

PlaneStateMachine::~PlaneStateMachine() {}

void PlaneStateMachine::AddDeviceToControlLawLog(const IODevice *device) {
  uRoll->AddDeviceToLog(device);
}

void PlaneStateMachine::AddDataToControlLawLog(const core::io_data *data) {
  uRoll->AddDataToLog(data);
}

const TargetController *PlaneStateMachine::GetTargetController(void) const {
    return controller;
}

MetaDualShock3 *PlaneStateMachine::GetJoystick(void) const {
    return joy;
}

const Quaternion &PlaneStateMachine::GetCurrentQuaternion(void) const {
  return currentQuaternion;
}

const Vector3Df &PlaneStateMachine::GetCurrentAngularSpeed(void) const {
  return currentAngularSpeed;
}


void PlaneStateMachine::Run() {
  WarnUponSwitches(true);
  uav->StartSensors();
//uav->GetBldc()->SetEnabled(true);
//uav->GetServos()->SetEnabled(true);
  if (getFrameworkManager()->ErrorOccured() == true) {
    SafeStop();
  }

  while (!ToBeStopped()) {
    SecurityCheck();

    // get controller inputs
    CheckJoystick();
    CheckPushButton();

    if (IsPeriodSet()) {
      WaitPeriod();
    } else {
      WaitUpdate(uav->GetAhrs());
    }
    needToComputeDefaultTorques = true;
    
    SignalEvent(Event_t::EnteringControlLoop);

    ComputeOrientation();
   
    // compute torques to apply
    ComputeTorques();
    
    //check nan/inf problems
    bool isValuePossible=true;
    //test it separately to warn for all problems
    if(!IsValuePossible(currentTorques.roll,"roll torque")) isValuePossible=false;
    if(!IsValuePossible(currentTorques.pitch,"pitch torque")) isValuePossible=false;
    if(!IsValuePossible(currentTorques.yaw,"yaw torque")) isValuePossible=false;

    if(!isValuePossible) {
      if(failSafeMode) {
        Warn("We are already in safe mode, the uav is going to crash!\n");
      } else {
        Thread::Warn("switching back to safe mode\n");
        EnterFailSafeMode();
        needToComputeDefaultTorques = true;//should not be necessary, but put it to be sure to compute default thrust/torques
       
        ComputeTorques();
      }
        
    }

    // Set torques for roll, pitch and yaw angles (value between -1 and 1). Set
    // thrust (value between 0 and 1)
    uav->GetUavMultiplex()->SetRoll(currentTorques.roll);
    uav->GetUavMultiplex()->SetPitch(-currentTorques.pitch);
    uav->GetUavMultiplex()->SetYaw(-currentTorques.yaw);
    uav->GetUavMultiplex()->SetThrust(thrust->Value());
   
    uav->GetUavMultiplex()->SetRollTrim(joy->RollTrim());
    uav->GetUavMultiplex()->SetPitchTrim(joy->PitchTrim());
    uav->GetUavMultiplex()->SetYawTrim(0);
    uav->GetUavMultiplex()->Update(GetTime());
  }

  WarnUponSwitches(false);
}

bool PlaneStateMachine::IsValuePossible(float value,std::string desc) {
  if(isnan(value)) {
    Warn("%s is not an number\n",desc.c_str());
    return false;
  } else if(isinf(value)) {
    Warn("%s is infinite\n",desc.c_str());
    return false;
  } else {
    return true;
  }
}


void PlaneStateMachine::ComputeOrientation(void) {
  if (failSafeMode) {
    GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
                                                          currentAngularSpeed);
  } else {
    GetOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
                                                   currentAngularSpeed);
  }
}

const AhrsData *PlaneStateMachine::GetOrientation(void) const {
  return GetDefaultOrientation();
}

const AhrsData *PlaneStateMachine::GetDefaultOrientation(void) const {
  return uav->GetAhrs()->GetDatas();
}


const AhrsData *PlaneStateMachine::ComputeReferenceOrientation(void) {
  if (orientationMode == OrientationMode_t::Manual) {
    return GetDefaultReferenceOrientation();
  } else {
    return GetReferenceOrientation();
  }
}

const AhrsData *PlaneStateMachine::GetDefaultReferenceOrientation(void) const {
  // We directly control yaw, pitch, roll angles
  return joy->GetReferenceOrientation();
}

const AhrsData *PlaneStateMachine::GetReferenceOrientation(void) {
  Thread::Warn("Default GetReferenceOrientation method is not overloaded => "
               "switching back to safe mode\n");
  EnterFailSafeMode();
  return GetDefaultReferenceOrientation();
}

void PlaneStateMachine::ComputeTorques(void) {
  if (torqueMode == TorqueMode_t::Default) {
    ComputeDefaultTorques(currentTorques);
  } else {
    ComputeCustomTorques(currentTorques);
  }
}

void PlaneStateMachine::ComputeDefaultTorques(Euler &torques) {
  if (needToComputeDefaultTorques) {
    const AhrsData *refOrientation = ComputeReferenceOrientation();
    Quaternion refQuaternion;
    Vector3Df refAngularRates;
    refOrientation->GetQuaternionAndAngularRates(refQuaternion,
                                                 refAngularRates);
    Euler refAngles = refQuaternion.ToEuler();
    Euler currentAngles = currentQuaternion.ToEuler();

    uYaw->SetValues(currentAngles.YawDistanceFrom(refAngles.yaw),
                    currentAngularSpeed.z - refAngularRates.z);
    uYaw->Update(GetTime());
    torques.yaw = uYaw->Output();

    uPitch->SetValues(refAngles.pitch, currentAngles.pitch,
                      currentAngularSpeed.y);
    uPitch->Update(GetTime());
    torques.pitch = uPitch->Output();
//Printf("%f %f %f %f\n",refAngles.pitch, currentAngles.pitch, currentAngularSpeed.y,torques.pitch);
    uRoll->SetValues(refAngles.roll, currentAngles.roll, currentAngularSpeed.x);
    uRoll->Update(GetTime());
    torques.roll = uRoll->Output();

    savedDefaultTorques = torques;
    needToComputeDefaultTorques = false;
  } else {
    torques = savedDefaultTorques;
  }
}

void PlaneStateMachine::ComputeCustomTorques(Euler &torques) {
  Thread::Warn("Default ComputeCustomTorques method is not overloaded => "
               "switching back to safe mode\n");
  EnterFailSafeMode();
  ComputeDefaultTorques(torques);
}


void PlaneStateMachine::SignalEvent(Event_t event) {
  switch (event) {
  case Event_t::Started:
    Thread::Info("entering 'Started' state\n");
    break;
  case Event_t::Stopped:
    Thread::Info("entering 'Stopped' state\n");
    break;
  case Event_t::EmergencyStop:
    Thread::Info("Emergency stop!\n");
    break;
  case Event_t::EnteringFailSafeMode:
    Thread::Info("Entering fail safe mode\n");
    break;
  }
}

void PlaneStateMachine::EmergencyStop(void) {
    
    StopActuators();
    EnterFailSafeMode();
    joy->Rumble(0x70);
    SignalEvent(Event_t::EmergencyStop);
    
    //safeToFly=false;
    //Warn("Emergency stop, UAV will not take off again until program is rerunned\n");
}

void PlaneStateMachine::StartActuators(void) {
  //The uav always takes off in fail safe mode
  flagBatteryLow=false;
  EnterFailSafeMode();
  joy->SetLedOFF(4);//DualShock3::led4
  joy->SetLedOFF(1);//DualShock3::led1
  joy->Rumble(0x70);
  joy->SetZRef(0);

  uRoll->Reset();
  uPitch->Reset();
  uYaw->Reset();

  uav->GetUavMultiplex()->LockUserInterface();
  // Active les moteurs. Pouvoir les désactiver permet de pouvoir observer les
  // consignes moteurs
  // sans les faire tourner effectivement (en déplaçant à la main le drone)
  uav->GetBldc()->SetEnabled(true);
  uav->GetServos()->SetEnabled(true);
  SignalEvent(Event_t::Started);
}

void PlaneStateMachine::StopActuators(void) {
  joy->FlashLed(1, 10, 10); // DualShock3::led1
  uav->GetBldc()->SetEnabled(false);
  uav->GetServos()->SetEnabled(false);
  uav->GetUavMultiplex()->UnlockUserInterface();
  SignalEvent(Event_t::Stopped);
  uav->GetAhrs()->UnlockUserInterface();

  uRoll->Reset();
  uPitch->Reset();
  uYaw->Reset();
}

GridLayout *PlaneStateMachine::GetButtonsLayout(void) const {
  return buttonslayout;
}

void PlaneStateMachine::SecurityCheck(void) {
  MandatorySecurityCheck();
  ExtraSecurityCheck();
}

void PlaneStateMachine::MandatorySecurityCheck(void) {
  if (getFrameworkManager()->ConnectionLost() && !flagConnectionLost) {
    flagConnectionLost = true;
    
    EnterFailSafeMode();
  }
    if(uav->GetBatteryMonitor()->IsBatteryLow() && !flagBatteryLow) {
        flagBatteryLow=true;
        Thread::Err("Low Battery\n");
        EnterFailSafeMode();
    }
}

void PlaneStateMachine::CheckJoystick(void) {
  GenericCheckJoystick();
  ExtraCheckJoystick();
}

void PlaneStateMachine::GenericCheckJoystick(void) {
  static bool isEmergencyStopButtonPressed = false;
  static bool isTakeOffButtonPressed = false;
  static bool isSafeModeButtonPressed = false;

  if (controller->IsButtonPressed(1)) { // select
    if (!isEmergencyStopButtonPressed) {
      isEmergencyStopButtonPressed = true;
      Thread::Info("Emergency stop from joystick\n");
      EmergencyStop();
    }
  } else
    isEmergencyStopButtonPressed = false;

  if (controller->IsButtonPressed(0)) { // start
    if (!isTakeOffButtonPressed) {
      isTakeOffButtonPressed = true;
      StartActuators();
    }
  } else
    isTakeOffButtonPressed = false;
    
  // cross
  // gsanahuj:conflict with Majd programs.
  // check if l1,l2,r1 and r2 are not pressed
  // to allow a combination in user program
  if (controller->IsButtonPressed(5) && !controller->IsButtonPressed(6) &&
      !controller->IsButtonPressed(7) && !controller->IsButtonPressed(9) &&
      !controller->IsButtonPressed(10)) {
    if (!isSafeModeButtonPressed) {
      isSafeModeButtonPressed = true;
      EnterFailSafeMode();
    }
  } else
    isSafeModeButtonPressed = false;
}

void PlaneStateMachine::CheckPushButton(void) {
  GenericCheckPushButton();
  ExtraCheckPushButton();
}

void PlaneStateMachine::GenericCheckPushButton(void) {
  if (button_kill->Clicked() == true)
    SafeStop();
  if (button_start_log->Clicked() == true)
    getFrameworkManager()->StartLog();
  if (button_stop_log->Clicked() == true)
    getFrameworkManager()->StopLog();
  if (button_start->Clicked() == true)
    StartActuators();
  if (button_stop->Clicked() == true)
    StopActuators();
}

void PlaneStateMachine::EnterFailSafeMode(void) {
  SetOrientationMode(OrientationMode_t::Manual);;
  SetTorqueMode(TorqueMode_t::Default);

  GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
                                                        currentAngularSpeed);
  joy->SetYawRef(currentQuaternion);
  uYaw->Reset();
  uPitch->Reset();
  uRoll->Reset();

  failSafeMode = true;
  SignalEvent(Event_t::EnteringFailSafeMode);
}

bool PlaneStateMachine::ExitFailSafeMode(void) {
  // only exit fail safe mode if in Stabilized altitude state
  // gsanahuj: pour la demo inaugurale on ne peut pas etre en failsafe
  // le ruban perturbe l'us
  /*
    if (altitudeState!=AltitudeState_t::Stabilized) {
        return false;
    } else*/ {
    failSafeMode = false;
    return true;
  }
}

bool PlaneStateMachine::SetTorqueMode(TorqueMode_t const &newTorqueMode) {
  if ((newTorqueMode == TorqueMode_t::Custom) && (failSafeMode)) {
    if (!ExitFailSafeMode())
      return false;
  }
  // When transitionning from Custom to Default torque mode, we should reset the
  // default control laws
  if ((torqueMode == TorqueMode_t::Custom) &&
      (newTorqueMode == TorqueMode_t::Default)) {
    uYaw->Reset();
    uPitch->Reset();
    uRoll->Reset();
  }
  torqueMode = newTorqueMode;
  return true;
}

bool PlaneStateMachine::SetOrientationMode(
    OrientationMode_t const &newOrientationMode) {
  if ((newOrientationMode == OrientationMode_t::Custom) && (failSafeMode)) {
    if (!ExitFailSafeMode())
      return false;
  }
  // When transitionning from Custom to Manual mode we must reset to yaw
  // reference to the current absolute yaw angle,
  // overwise the Uav will abruptly change orientation
  if ((orientationMode == OrientationMode_t::Custom) &&
      (newOrientationMode == OrientationMode_t::Manual)) {
    joy->SetYawRef(currentQuaternion);
  }
  orientationMode = newOrientationMode;
  return true;
}


NestedSat* PlaneStateMachine::GetURoll(void) { return uRoll;};
NestedSat* PlaneStateMachine::GetUPitch(void) { return uPitch;};
Pid* PlaneStateMachine::GetUYaw(void) { return uYaw;};