// $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/src/motion_ctrl/motionCtrl.hpp 3965 2022-05-30 18:43:35Z depa $

#pragma once
#include <map>
#include <typeinfo>
//#include <stblib>

// SHINS
#include "common_sharknir.h"
#include "wheels/multiWheel.hpp"
#include "stagesX/multiStageX.hpp"
// #include "multiDeviceT.h"
#include "stagesXY/multiStageXY.hpp"
#include "niceCallback.h"
#include "util/logger.h"

#include "util/shins_util.h"

// TaN
// #include <Nice/Log.h>
#include <Nice/App/Endpoint.h>
#include <Nice/App/ConfigClient.h>
#include <Basda/Mocca/WheelServiceWorkerInterface.h>
#include <Basda/Mocca/XServiceWorkerInterface.h>
#include <Basda/Mocca/XYServiceWorkerInterface.h>
#include <Basda/ServiceState.h>
#include <Basda/ServiceStateMachine.h>

#include <Ice/LoggerUtil.h>
#include <Ice/Ice.h>
#include <boost/chrono.hpp>


namespace suc = shins::util::constants;
namespace su = shins::util;

struct motionSetup
{
    std::map<std::string, int> NamedMapWheel;
    std::map<std::string, int> NamedMapStageX;
    std::map<std::string, int> NamedMapStageXY;
    std::map<std::string, double> AbsMapWheel;
    std::map<std::string, double> AbsMapStageX;
    std::map<std::string, double> AbsMapStageXY;
    std::map<std::string, double> RelMapWheel;
    std::map<std::string, double> RelMapStageX;
    std::map<std::string, double> RelMapStageXY;
    std::map<std::string, std::string> AbsMapWheelUnits;
    std::map<std::string, std::string> AbsMapStageXUnits;
    std::map<std::string, std::string> AbsMapStageXYUnits;
    std::map<std::string, std::string> RelMapWheelUnits;
    std::map<std::string, std::string> RelMapStageXUnits;
    std::map<std::string, std::string> RelMapStageXYUnits;
    void clear()
    {
        NamedMapWheel.clear();
        NamedMapStageX.clear();
        NamedMapStageXY.clear();
        AbsMapWheel.clear();
        AbsMapStageX.clear();
        AbsMapStageXY.clear();
        RelMapWheel.clear();
        RelMapStageX.clear();
        RelMapStageXY.clear();
        AbsMapWheelUnits.clear();
        AbsMapStageXUnits.clear();
        AbsMapStageXYUnits.clear();
        RelMapWheelUnits.clear();
        RelMapStageXUnits.clear();
        RelMapStageXYUnits.clear();
    }
};

struct motionInit
{
    std::vector<std::string> InitStageX;
    std::vector<std::string> InitWheels;
    std::vector<std::string> InitStageXY;
    void clear()
    {
        InitStageX.clear();
        InitWheels.clear();
        InitStageXY.clear();
    }
};

class motionCtrl
{
private:
  // typedef  multiDeviceT<Basda::MoccaXServiceWorkerInterfacePtr> multiStageX;
  motionSetup m_setup;
  motionSetup m_setupWait;
  motionInit m_init;
  //boost::shared_ptr<indiIF> m_ptrLbtoIF;
  /* Ice::Trace   m_info   ; */
  /* Ice::Trace   m_debug  ; */
  /* Ice::Error   m_error  ; */
  /* Ice::Warning m_warning; */

  /*
   * USING BOOST LIBRARY
   */
  // devices
  boost::shared_ptr<multiWheel>   m_ptrMultiWheel;
  boost::shared_ptr<multiStageX>  m_ptrMultiStageX;
  boost::shared_ptr<multiStageXY> m_ptrMultiStageXY;

  // ptr to callback objects, on which to call bound functions
  boost::shared_ptr<niceCallback> m_ptrCallbackObjSM;
  boost::shared_ptr<niceCallback> m_ptrCallbackObjCount;
  boost::shared_ptr<niceCallback> m_ptrCallbackObjReference;
  boost::shared_ptr<niceCallback> m_ptrCallbackObjIndi;

  // ptr to state machine
  boost::shared_ptr<osfsm::OSFsm> m_ptrStateMachine;

  // cb functions
  boost::function<void(const Nice::Completion&)> m_callbackFnSM;
  boost::function<void(const Nice::Completion&)> m_callbackFnCount;
  boost::function<void(const Basda::ServiceStateMachine&, const std::string&)> m_callbackFnIndi;
  std::map<std::string, boost::function<void(const Nice::Completion&)> > m_mapCallbackFnReference;


  // mutex
  IceUtil::Mutex m_mtxStateMachine;
  IceUtil::Mutex m_mtxCount;
  IceUtil::Mutex m_mtxSeq;
  boost::shared_ptr<IceUtil::Monitor<IceUtil::Mutex> > m_ptrMonSeq;
  // boost::shared_ptr<boost::mutex> m_ptrMut;
  int m_moving_devs;
  const std::string __OBJ_NAME__;
  Nice::AppConfigClientPtr m_ConfigClient;
  su::EServiceStatus m_service_status;
  std::string m_service;

  bool isActive;

  static motionCtrl * m_pMotionCtrl;
  static void * connect_to_service(void * arg);
  bool m_connectionEstablished;
  void connectToService();
public:
  motionCtrl(boost::shared_ptr<osfsm::OSFsm>& _ptrStateMachine, Ice::PropertiesPtr prop);
  ~motionCtrl();

  bool CheckOnline(bool throw_if_notonline = true);
  su::EServiceStatus Connect();

  bool isDevice(const std::string& _strDevAlias);

  double ConvertToDefaultUnit(const std::string & alias, double destination, const std::string & unit);
  void SetSecondaryCallbacks(const std::string & alias,
                          void (* MoveToHomeCB)(const Nice::Completion & c),
                          void (* MoveToNamedCB)(const Nice::Completion & c),
                          void (* MoveAbsoluteCB)(const Nice::Completion & c),
                          void (* MoveRelativeCB)(const Nice::Completion & c),
                          void (* MoveToLimitCB)(const Nice::Completion & c));
  Ice::Int SetWheelInterdictionCallbacks(const std::string & alias,
                                     bool (* checkInterdiction)(void),
                                     bool (* interdictionReaction)(void))
  {
      if(m_ptrMultiWheel)
      {
          Ice::Int status = m_ptrMultiWheel->SetWheelInterdictionCallbacks(alias, checkInterdiction, interdictionReaction);
          return status;
      }
      else
          return EXIT_FAILURE;
  }
  const std::map<std::string, StageXDevice> & GetMultiStageX(){ return m_ptrMultiStageX->GetMultiStageX();};
  const std::map<std::string, StageXYDevice> & GetMultiStageXY(){ return m_ptrMultiStageXY->GetMultiStageXY();};
  const std::map<std::string, WheelDevice> & GetMultiWheel(){ return m_ptrMultiWheel->GetMultiWheel();};

  Nice::SeqBitSet isAtLimit(const std::string &_devAlias){ return m_ptrMultiStageX->isAtLimit(_devAlias);}

  int getJsonStatus(std::string & jsonStatus);


  // TAN Application related methods
  int run(int argc, char* argv []);
  // !! Have to be a bit more complex then this:
  // a control on wheelId. Maybe is better wheel name then the Id, but this a
  // a structure to map name to id
  //std::string getSvcPath(const int&) const;
  std::vector<std::string> getAliasesFromFile(const std::string& _strFileName, const std::string& _which);
  int getNdevices();
  Ice::Int getPosition(std::map<std::string, Ice::Double>& _mapPosition, const std::string& _unit="STEPS");
  Ice::Int getConfigPosition(std::map<std::string, Ice::Double>& _mapPosition, const std::string& _unit="STEPS");
  std::map<std::string, std::string> getNamedPosition(const std::vector<std::string>& _vecAlias);
  void setMonitor(boost::shared_ptr<IceUtil::Monitor<IceUtil::Mutex> >& _ptrMonSeq)
  {
    m_ptrMonSeq = _ptrMonSeq;
  }
  void moveHome();
  void changeState(const Basda::ServiceState& _state, const std::vector<std::string>& _vecDevAlias=std::vector<std::string>());

  void initializeAll();
  void initializeAllStart();
  void initializeAllWait(const Nice::Time& _time = Nice::Time());

  void initPrepare(const std::vector<std::string>& _vecDevAlias);
  void initialize(const std::vector<std::string>& _vecDevAlias);
  void initializeStart(const std::vector<std::string>& _vecDevAlias);
  void initializeWait(const Nice::Time& _time = Nice::Time());

  void moveDeviceHome(const std::vector<std::string>& _vecDevName);
  int moveDeviceToNegativeLimit(const std::vector<std::string>& _vecDevName);
  int moveDeviceToPositiveLimit(const std::vector<std::string>& _vecDevName);

  void setup(const std::map<std::string, std::string>& _mapNamedPos);
  void setupStart(const std::map<std::string, std::string>& _mapNamedPos);
  void setupPrepare(const std::map<std::string, std::string>& _mapNamedPos, motionSetup & setup);
  void setupWait(const std::map<std::string, std::string> &_mapNamedPos, const Nice::Time& _time = Nice::Time());

  void moveToAbsolutePosition(const std::map<std::string, double>& _mapPos,      const std::map<std::string, std::string>& _mapUnit=std::map<std::string, std::string>());
  void moveToAbsolutePositionStart(const std::map<std::string, double>& _mapPos, const std::map<std::string, std::string>& _mapUnit=std::map<std::string, std::string>());
  void moveToAbsolutePositionWait(const std::map<std::string, double>& _mapPos, const Nice::Time& _time = Nice::Time());

  void moveToRelativePosition(const std::map<std::string, double>& _mapPos,      const std::map<std::string, std::string>& _mapUnit=std::map<std::string, std::string>());
  void moveToRelativePositionStart(const std::map<std::string, double>& _mapPos, const std::map<std::string, std::string>& _mapUnit=std::map<std::string, std::string>());
  void moveToRelativePositionWait(const std::map<std::string, double>& _mapPos, const Nice::Time& _time = Nice::Time());

  void emergencyAbort();
  int stop (const std::vector<std::string>& _vecDevAlias);
  int abort(const std::vector<std::string>& _vecDevAlias);
};
