// %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.h
//
//  author:     Gildas Bayard, Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    state machine for plane
//
//
/*********************************************************************/

#ifndef PLANESTATEMACHINE_H
#define PLANESTATEMACHINE_H

#include <Thread.h>
#include <Vector2D.h>
#include <Vector3D.h>
#include <Euler.h>
#include <Quaternion.h>

namespace flair {
namespace core {
class FrameworkManager;
class AhrsData;
class io_data;
}
namespace gui {
class PushButton;
class GridLayout;
class Tab;
class DoubleSpinBox;
}
namespace filter {
class ControlLaw;
class NestedSat;
class Pid;
}
namespace sensor {
class TargetController;
}
namespace meta {
class MetaDualShock3;
class Plane;
}
}

namespace flair {
namespace meta {

/*! \class PlaneStateMachine
*
* \brief State machine for UAV
*  The Thread is created with 
*  the FrameworkManager as parent. FrameworkManager must be created before.
* The Thread is synchronized with Ahrs, unless a period is set with SetPeriodUS or
* SetPeriodMS
*/

class PlaneStateMachine : public core::Thread {
protected:

  enum class OrientationMode_t { Manual, Custom };
  const OrientationMode_t &GetOrientationMode(void) const {
    return orientationMode;
  }
  bool SetOrientationMode(const OrientationMode_t &orientationMode);

  enum class TorqueMode_t { Default, Custom };
  const TorqueMode_t &GetTorqueMode(void) const { return torqueMode; }
  bool SetTorqueMode(const TorqueMode_t &torqueMode);

  enum class Event_t {
    EnteringFailSafeMode,
    EnteringControlLoop,
    Stopped,
    EmergencyStop,
    Started,
  };

        PlaneStateMachine(sensor::TargetController* controller);
        ~PlaneStateMachine();

  const core::Quaternion &GetCurrentQuaternion(void) const;

  const core::Vector3Df &GetCurrentAngularSpeed(void) const;

      
        void EmergencyStop(void);
        //! Used to signal an event
        /*!
            \param event the event which occured
        */
        virtual void SignalEvent(Event_t event);

  virtual const core::AhrsData *GetOrientation(void) const;
  const core::AhrsData *GetDefaultOrientation(void) const;

  void EnterFailSafeMode(void);
  bool ExitFailSafeMode(void);
  
  gui::GridLayout *GetButtonsLayout(void) const;
  virtual void ExtraSecurityCheck(void){};
  virtual void ExtraCheckJoystick(void){};
  virtual void ExtraCheckPushButton(void){};

  // float GetDefaultThrustOffset(void);
  const core::AhrsData *GetDefaultReferenceOrientation(void) const;
  virtual const core::AhrsData *GetReferenceOrientation(void);

  /*!
  * \brief Compute Custom Torques
  *
  * Reimplement this method to use TorqueMode_t::Custom. \n
  * This method is called internally by PlaneStateMachine, do not call it by
  *yourself. \n
  * See GetTorques if you need torques values.
  *
  * \param torques custom torques
  */
  virtual void ComputeCustomTorques(core::Euler &torques);

  /*!
  * \brief Compute Default Torques
  *
  * This method is called internally by PlaneStateMachine when using
  *TorqueMode_t::Default. \n
  * Torques are only computed once by loop, other calls to this method will use
  *previously computed torques.
  *
  * \param torques default torques
  */
  void ComputeDefaultTorques(core::Euler &torques);

  /*!
  * \brief Get Torques
  *
  * \return torques current torques
  */
  // const core::Euler &GetTorques() const;


  /*!
  * \brief Add an IODevice to the control law logs
  *
  * The IODevice will be automatically logged among the Uz logs,
  * if logging is enabled (see IODevice::SetDataToLog,
  *FrameworkManager::StartLog
  * and FrameworkManager::AddDeviceToLog). \n
  *
  * \param device IODevice to log
  */
  void AddDeviceToControlLawLog(const core::IODevice *device);

  /*!
  * \brief Add an io_data to the control law logs
  *
  * The io_data will be automatically logged among the Uz logs,
  * if logging is enabled (see IODevice::SetDataToLog,
  *FrameworkManager::StartLog
  * and FrameworkManager::AddDeviceToLog). \n
  *
  * \param data io_data to log
  */
  void AddDataToControlLawLog(const core::io_data *data);

  const sensor::TargetController *GetTargetController(void) const;
  MetaDualShock3 *GetJoystick(void) const;
  
  filter::NestedSat *GetURoll(void);
  filter::NestedSat *GetUPitch(void);
  filter::Pid *GetUYaw(void);
  
  gui::Tab *setupLawTab, *graphLawTab;

private:
  bool failSafeMode;
  void SecurityCheck(void);
  void MandatorySecurityCheck(void);
  void CheckJoystick();
  void GenericCheckJoystick();
  void CheckPushButton(void);
  void GenericCheckPushButton(void);
  void Run(void);
  void StartActuators(void);
  void StopActuators(void);
  bool IsValuePossible(float value,std::string desc);

  Plane *uav;
  MetaDualShock3 *joy;
  sensor::TargetController *controller;

  core::Quaternion currentQuaternion;
  core::Vector3Df currentAngularSpeed;

  const core::AhrsData *ComputeReferenceOrientation(void);

  void ComputeOrientation(void);
  
  void ComputeTorques(void);
  core::Euler currentTorques, savedDefaultTorques;
  bool needToComputeDefaultTorques;

  gui::PushButton *button_kill,*button_start_log, *button_stop_log;
  gui::PushButton *button_start,*button_stop;
  gui::GridLayout *buttonslayout;
  gui::DoubleSpinBox *thrust;
  OrientationMode_t orientationMode;
  TorqueMode_t torqueMode;
  bool flagBatteryLow;
  bool flagConnectionLost;
  bool flagCriticalSensorLost;
  bool safeToFly;
  filter::NestedSat *uRoll, *uPitch;
  filter::Pid *uYaw;
};
};
};
#endif // PLANESTATEMACHINE_H