// $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/src/calunit_ctrl/calunitCtrl.h 3965 2022-05-30 18:43:35Z depa $

#pragma once

// SHINS
#include "motion_ctrl/common_sharknir.h"
#include "motion_ctrl/stagesX/multiStageX.hpp"
#include "util/shins_util.h"

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

#include <Ice/LoggerUtil.h>
#include <Ice/Ice.h>

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

struct calunitSetup
{
    std::map<std::string, int> NamedMapStageX;
    std::map<std::string, double> AbsMapStageX;
    std::map<std::string, double> RelMapStageX;
    std::map<std::string, std::string> AbsMapStageXUnits;
    std::map<std::string, std::string> RelMapStageXUnits;
    void clear()
    {
        NamedMapStageX.clear();
        AbsMapStageX.clear();
        RelMapStageX.clear();
        AbsMapStageXUnits.clear();
        RelMapStageXUnits.clear();
    }
};

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

class calunitCtrl
{
private:
  // typedef  multiDeviceT<Basda::MoccaXServiceWorkerInterfacePtr> multiStageX;
  shins::util::mapPosType m_mainMapNamedPos;
//  boost::shared_ptr<indiIF> m_ptrLbtoIF;
  /* Ice::LoggerPtr m_ptrLogger; */
  /*
   * USING BOOST LIBRARY
   */
  // devices
  boost::shared_ptr<multiStageX> m_ptrMultiStageX;

  // 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_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;


//  boost::shared_ptr<calunitCtrl> & reset();

  // 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__;
  calunitSetup m_setup;
  calunitInit m_init;
//  static void * connect_to_service(void * arg);
  Nice::AppConfigClientPtr m_ConfigClient;
  su::EServiceStatus m_service_status;
  std::string m_service;

  static calunitCtrl * m_pCalunitCtrl;
  static void * connect_to_service(void * arg);
  bool m_connectionEstablished;
  void connectToService();
public:
  std::vector<std::string> m_vecAlias;
  std::map<std::string, std::string> m_mapCfgFiles;
  std::string m_strAliasFilename;
  // calunitCtrl(int argc, char* argv[], const std::string& _strAliasFileName,
  //            boost::shared_ptr<osfsm::OSFsm>& _ptrStateMachine);

  calunitCtrl(boost::shared_ptr<osfsm::OSFsm>& _ptrStateMachine, Ice::PropertiesPtr prop);
  ~calunitCtrl();

  su::EServiceStatus Connect();
  su::EServiceStatus GetServiceStatus(){return m_service_status; };
  bool CheckOnline(bool throw_if_notonline = true);

  bool isDevice(const std::string& _strDevAlias);
  
  const std::map<std::string, StageXDevice> & GetMultiStageX(){ return m_ptrMultiStageX->GetMultiStageX();};
  double ConvertToDefaultUnit(const std::string & alias, double destination, const std::string & unit);
  Nice::SeqBitSet isAtLimit(const std::string &_devAlias){ return m_ptrMultiStageX->isAtLimit(_devAlias);}

  void SetPrimaryBridges();
  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));
  void moveCalFiberDepToSafe();
  // TAN Application related methods
  int run(int argc, char* argv []);
  // std::vector<std::string> getAliasesFromFile(const std::string& _strFileName);
  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");
  int getJsonStatus(std::string & jsonStatus);

  int getNdevices();

  void changeState    (const Basda::ServiceState& _state, const std::vector<std::string>& _vecDevAlias=std::vector<std::string>());

  void moveHome();
  void moveDeviceHome(const std::vector<std::string>& _vecDevName);

  void moveToReferenceAll();
  void moveToReferenceAllStart();
  void moveToReferenceAllWait(const Nice::Time& _time = Nice::Time());

  void initPrepare(const std::vector<std::string>& _vecDevAlias);
  void moveToReference(const std::vector<std::string>& _vecDevAlias);
  void moveToReferenceStart(const std::vector<std::string>& _vecDevAlias);
  void moveToReferenceWait(const std::vector<std::string>& _vecDevAlias, const Nice::Time& _time = Nice::Time());

  void setupPrepare(const std::map<std::string, std::string>& _mapNamedPos);
  void moveToNamedPosition(const std::map<std::string, std::string> &_mapNamedPos);
  void moveToNamedPositionStart(const std::map<std::string, std::string>& _mapNamedPos);
  void moveToNamedPositionWait(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 setupLamps(const std::map<std::string, std::string> &_mapSetup);

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