// $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/src/motion_ctrl/multiDeviceT.h 3765 2021-11-11 10:47:14Z depa $

#pragma once

// TAN
#include <config.h>
#include <Basda/Mocca/Exception.h>
// #include <Basda/Mocca/WheelServiceWorkerInterface.h>
#include <Basda/ServiceStateMachine.h>
#include <Basda/ServiceState.h>
#include <Nice/App/Endpoint.h>
#include <Nice/Property/BridgeMemFn.hh>
#include <Nice/App/ConfigClient.h>
#include <Nice/Time.h>
#include <Nice/Time/Timer.h>
//#include <Nice/Log.h>


// SHINS
#include "../common_sharknir.h"
#include "../multiDevice.h"
#include "../niceCallback.h"

template <typename T>
class multiDeviceT;

template <typename T>
class  multiDeviceT : public Nice::Shared, public multiDevice
{
private:
  Nice::SyslogFunction *logger;
  int m_counter;
  std::size_t m_nDevices;

  boost::shared_ptr<niceCallback> m_ptrCallbackObj;
  boost::shared_ptr<niceCallback> m_ptrCallbackObjState;

  boost::function<void(const Nice::Completion&)> m_callbackFn;
  boost::function<void(const Nice::Completion&)> m_callbackFnCount;


  std::map<std::string,
           boost::function<void(const Nice::Completion&)> > m_mapCallbackFnReState;

  boost::function<void(const Basda::ServiceStateMachine&)> m_callbackFnState;

  std::string m_strCallbackName;
  std::string m_strCallbackNameState;
  std::string m_strCallbackNameCount;
  std::string m_strCallbackNameHome;

  void install_callback_on_completion(const std::string& _strCallbackName,
                                      const std::string& _strPropName);
  void install_callback_on_state(const std::string& _strCallbackName,
                                 const std::string& _strPropName);

  void install_callback_on_reference(const std::string& _strCallbackName,
                                     const std::string& _strPropName);
  // void apply_callback2(std::map<std::string, T>::iterator _it);

protected:
  Nice::JointPtr m_config;
  // Nice::JointPtr m_configDummy;

  std::map<std::string, T> m_mapDevices; //<WHEELALIAS, MoccaService>
  std::vector<T> m_vecDevices;
  std::vector<std::string> m_vecAliasName;

  // there should be one method each accepting as parameter
  // the wheel idx in optical path or the wheel name.
  std::string getEndPointName(const std::size_t _deviceIdx);
  std::string getBaseName(const std::size_t _deviceIdx);



public:
  multiDeviceT(const Nice::JointPtr& _config,
             // const Nice::JointPtr& _configDummy,
             const std::vector<std::string>& _vecBaseName);
  virtual ~multiDeviceT();
  virtual void connect();
  virtual void online();
  virtual void standby();

  virtual void moveToReference();
  virtual void moveToReferenceStart();
  virtual void moveToReferenceWait(const Nice::Time& _time = Nice::Time());

  virtual void moveDeviceToReference(const std::vector<std::string>& _vecDevName);
  virtual void moveDeviceToReferenceStart(const std::vector<std::string>& _vecDevAlias);
  virtual void moveDeviceToReferenceWait(const std::vector<std::string>& _vecDevAlias,
                                         const Nice::Time& _time = Nice::Time());

  virtual void moveToNamedPosition(const std::vector<int> &_vecPos);
  virtual void moveToNamedPositionStart(const std::vector<int>& _vecPos);
  virtual void moveToNamedPositionWait(const std::vector<int>& _vecPos,
                                       const Nice::Time& _time = Nice::Time());

  virtual void moveToNamedPosition(const std::map<std::string, int> &_mapNamedPos);
  virtual void moveToNamedPositionStart(const std::map<std::string, int>& _mapPos);
  virtual void moveToNamedPositionWait(const std::map<std::string, int>& _mapNamedPos,
                                       const Nice::Time& _time = Nice::Time());

  virtual void moveToPosition(std::map<std::string, int>& _mapPos);
  virtual void moveToPositionStart(std::map<std::string, int>& _mapPos);
  virtual void moveToPositionWait(std::map<std::string, int>& _mapPos,
                                  const Nice::Time& _time = Nice::Time());

  virtual void emergencyAbort();
  virtual int stop();


  virtual bool isDevice(const std::string& _strDevAlias);
  // callback installation
  virtual void set_callback_fn(boost::function<void(const Nice::Completion&)>&,
                               const std::string& _strFnName);
  virtual void set_callback_fn_state(boost::function<void(const Basda::ServiceStateMachine&)>&,
                                     const std::string& _strFnName);
  virtual void set_callback_fn_state(boost::function<void(const Basda::ServiceStateMachine&,
                                                          const std::string&)>&,
                                     const std::string& _strDevAlias);
  virtual void set_callback_fn_count(boost::function<void(const Nice::Completion&)>&,
                                     const std::string& _strFnName);

  // GETTERS
  // the number of wheel is constant in SHARK-NIR; thus it is an
  // hard coded value
  inline virtual std::size_t getNdevices() { return m_nDevices; }
  virtual std::vector<std::string> getNamedPosition(const std::size_t _deviceIdx);
  virtual std::vector<std::string> getNamedPosition(const std::vector<std::string>& _vecAlias);
  T getDevicePtr(const std::string& _strDeviceName);
  // SETTERS

  // void setActualPosLabel(const std::vector<std::string>& _vecLabel);
};

// #endif // _MULTI_WHEEL_

template <typename T>
multiDeviceT::multiDeviceT(const Nice::JointPtr& _config,
                           const std::vector<std::string>& _vecAliasName)
  : m_config(_config)
{
  m_ptrLogger = boost::make_shared<Nice::SyslogFunction>("SHINS");
  logger = new Nice::SyslogFunction("shins");
  m_counter = 0;
  T tmpDevice;

  m_vecAliasName = _vecAliasName;
  m_nDevices = _vecAliasName.size();
  for (std::size_t i = 0; i < m_nDevices; i++)
    {
      m_vecDevices.push_back(tmpDevice);
    }

  // Build map containg T objects
  U5_LOG("Creation of wheels' map");
  for (BOOST_AUTO(it, _vecAliasName.begin()); it < _vecAliasName.end(); ++it)
    {
#if ! defined BOOST_NO_UNIFIED_INITIALIZATION_SYNTAX
      m_mapDevices.insert({boost::to_upper_copy(*it), tmpDevice});
#else
      m_mapDevices.insert(std::pair<std::string, T>
                         (boost::to_upper_copy(*it), tmpDevice));
#endif
      U5_LOG(__FUNCTION__ << " " << *it);
    }
  // initialize callback object
  m_ptrCallbackObj = boost::make_shared<niceCallback>();
}

template <typename T>
multiDeviceT::~multiDeviceT()
{
  //
  D_LOG("multiDeviceT destructor")
}

// --------------------
// CALLBACKS begin
// --------------------
template <typename T>
void multiDeviceT::install_callback_on_completion(const std::string& _strCallbackName,
                                                const std::string& _strPropName)
{
  std::cout << "IN -- " << __FUNCTION__ << std::endl;
  Nice::PropertyPtr prop;
  std::string bridgeName = "";
  int c = 0;
  // Install callback for each wheel
  for (BOOST_AUTO(it, m_mapDevices.begin()); it != m_mapDevices.end(); ++it)
    {
      bridgeName = _strCallbackName + "-" + it->first + "-motion";
      prop = it->second->config()->leaf(_strPropName);
      prop->applyBridge
        (
         new Nice::PropertyBridgeMemFn
         <Nice::Completion, multiDeviceT<T>,
         boost::function<void(const Nice::Completion&)> multiDeviceT<T>::* >
         (
          this,
          &multiDeviceT<T>::m_callbackFnCount, prop->id()),
         bridgeName+"-count"
         );

      c++;
      std::cout << it->first << " -- " << __FUNCTION__ << std::endl;
    }

  std::cout << "OUT -- " <<__FUNCTION__ << " " << c << std::endl;
}


// Install callback on property
template <typename T>
void multiDeviceT::install_callback_on_reference(const std::string& _strCallbackName,
                                               const std::string& _strPropName)
{
  Nice::PropertyPtr prop;
  std::string bridgeName = "";
  boost::shared_ptr<niceCallback> ptrCallbackObjReference;
  boost::function<void(const Nice::Completion&)> callbackFnReference;
  for (BOOST_AUTO(it, m_mapDevices.begin()); it != m_mapDevices.end(); ++it)
    {
      // bridgeName = _strCallbackName + "-" + it->first + "-motion";
      // prop = it->second->config()->leaf(_strPropName);
      // callbackFnReference = boost::bind(&niceCallback::callback_device_move_to_reference,
      //                                   ptrCallbackObjReference,
      //                                   _1, it->second);
      // prop->applyBridge
      //   (
      //    new Nice::PropertyBridgeMemFn
      //    <Nice::Completion, multiDeviceT,
      //    boost::function<void(const Nice::Completion&)> multiDeviceT::* >
      //    (
      //     this,
      //     &callbackFnReference, prop->id()),
      //    bridgeName+"-reference"
      //    );
    }
}

template <typename T>
void multiDeviceT::install_callback_on_state(const std::string& _strCallbackName,
                                           const std::string& _strPropName)
{
  std::cout << "IN -- " << __FUNCTION__ << std::endl;
  Nice::PropertyPtr prop;
  std::string bridgeName = "";
#if defined BOOST_HAS_TR1_SHARED_PTR
  auto it = m_mapDevices.begin();
#else
  BOOST_AUTO(it, m_mapDevices.begin());
#endif
  for (it = m_mapDevices.begin(); it != m_mapDevices.end(); ++it)
    {
      // set_callback_fn_state(m_callbackFnState, it->first, "merda");
      // bridgeName = _strCallbackName + "-" + it->first + "-state"; //
      // prop = it->second->config()->leaf(_strPropName);
      // prop->applyBridge
      //   (
      //    new Nice::PropertyBridgeMemFn
      //    <Basda::ServiceStateMachine, multiDeviceT,
      //    boost::function<void(const Basda::ServiceStateMachine&)> multiDeviceT::* >
      //    (
      //     this,
      //     &multiDeviceT::m_callbackFnState, prop->id()),
      //    bridgeName
      //    );

      std::cout << it->first << " -- " << __FUNCTION__ << std::endl;

      // if (m_mapCallbackFnState[it->first])
      //   {
      //     bridgeName = _strCallbackName + "-" + it->first + "-state2";
      //     prop = it->second->config()->leaf(_strPropName);
      //     prop->applyBridge
      //       (
      //        new Nice::PropertyBridgeMemFn
      //        <Basda::ServiceStateMachine, multiDeviceT,
      //        boost::function<void(const Basda::ServiceStateMachine&)> multiDeviceT::* >
      //        (
      //         this,
      //         &multiDeviceT::m_mapCallbackFnState[it->first], prop->id()),
      //        bridgeName
      //        );
      //   }
    }
}

// CALLBACKS end
// --------------------


template <typename T>
std::string multiDeviceT::getEndPointName(const std::size_t _deviceIdx)
{
  if (_deviceIdx > m_vecDevices.size())
    throw std::range_error("range error");
  else
    {
      m_logObjet.date = Nice::Date::now();
      m_logObjet.type = Nice::LWARNING;
      m_logObjet.host = "vultus";
      m_logObjet.pid = -99;
      m_logObjet.tid = -99;
      m_logObjet.user = "user";
      m_logObjet.file = __BASE_FILE__;
      m_logObjet.line = __LINE__;
      m_logObjet.function = __PRETTY_FUNCTION__;
      m_logObjet.name = LOG_NAME;
      m_logObjet.log = "test";
      m_ptrLogger->log(m_logObjet);
      U6_LOG(getBaseName(_deviceIdx) << ".CON");
      U5_LOG(__FUNCTION__ << " --- " << getBaseName(_deviceIdx) << ".CON");
      return (getBaseName(_deviceIdx) + ".CON");
    }
}

template <typename T>
std::string multiDeviceT::getBaseName(const std::size_t _deviceIdx)
{
  if (_deviceIdx > m_vecDevices.size())
    throw std::out_of_range("out of range");
  else
    {
      BOOST_AUTO(it, m_vecAliasName.begin() + _deviceIdx);
      // auto it = m_vecBaseName.begin() + _deviceIdx;
      U5_LOG(__FUNCTION__ << " " << *it);
      return (*it);
    }
}

template <typename T>
void multiDeviceT::connect()
{
  std::vector<std::string> namedPos;
  // std::cout << __FUNCTION__ << "Connecting" << std::endl;
  U5_LOG(" Connecting ...");
  m_logObjet.date = Nice::Date::now();
  m_logObjet.type = Nice::LWARNING;
  m_logObjet.host = "vultus";
  m_logObjet.pid = -99;
  m_logObjet.tid = -99;
  m_logObjet.user = "user";
  m_logObjet.file = __BASE_FILE__;
  m_logObjet.line = __LINE__;
  m_logObjet.function = __PRETTY_FUNCTION__;
  m_logObjet.name = m_logObjet.file + " " + m_logObjet.function; //LOG_NAME;
  for (BOOST_AUTO(it, m_mapDevices.begin()); it != m_mapDevices.end(); ++it)
    {
      try
        {
          std::cout << it->first << "---" << std::endl;
          U5_LOG("config node name "<< it->first << ".CON");
          Nice::AppConfigClient::create(m_config->getAtNode<Nice::Endpoint>(it->first + ".CON"), m_config->node(it->first));
          U5_LOG("created node");
          it->second = Basda::MoccaWheelServiceWorkerInterface::create(m_config->node(it->first));
          T_LOG("Connected to node " << it->first << ".CON");
          std::cout << it->first << "---" << std::endl;
          std::cout << std::endl;
        }
      catch(Basda::ServiceInitException &e)
        {
          W_LOG("ERROR COUGHT " << e.what());
          m_logObjet.log = e.what();
          m_ptrLogger->log(m_logObjet);
          // break;
        }
    }
  for (BOOST_AUTO(it, namedPos.begin()); it != namedPos.end(); ++it)
    {
      std::cout << *it << std::endl;
    }
  // logger->log(lo);
  // std::cout << __FUNCTION__ << std::endl;
}

template <typename T>
void multiDeviceT::standby()
{
  Nice::LoggerObject lo;
  m_logObjet.host = "vultus";
  m_logObjet.pid = -99;
  m_logObjet.tid = -99;
  m_logObjet.user = "user";
  m_logObjet.file = __BASE_FILE__;
  m_logObjet.line = __LINE__;
  m_logObjet.function = __PRETTY_FUNCTION__;
  m_logObjet.name = m_logObjet.file + " " + m_logObjet.function;
  for (BOOST_AUTO(it, m_mapDevices.begin()); it != m_mapDevices.end(); ++it)
  {
    try{
      it->second->changeState(Basda::STANDBY);
      m_logObjet.type = Nice::LINFO;
      m_logObjet.date = Nice::Date::now();
      m_logObjet.log = "Changed state to STANDBY for wheel";
      m_ptrLogger->log(m_logObjet);
    }catch(Basda::ServiceInitException &e)
    {
      W_LOG("ERROR CAUGHT " << e.what());
      m_logObjet.log = e.what();
      m_logObjet.type = Nice::LWARNING;
      m_logObjet.date = Nice::Date::now();
      m_ptrLogger->log(m_logObjet);
      it->second->init();
      it->second->changeState(Basda::STANDBY);
    }
  }
  std::cout << __FUNCTION__ << std::endl;
}


template <typename T>
void multiDeviceT::online()
{
  m_logObjet.host = "vultus";
  m_logObjet.pid = -99;
  m_logObjet.tid = -99;
  m_logObjet.user = "user";
  m_logObjet.file = __BASE_FILE__;
  m_logObjet.line = __LINE__;
  m_logObjet.function = __PRETTY_FUNCTION__;
  m_logObjet.name = m_logObjet.file + " " + m_logObjet.function;
  for (BOOST_AUTO(it, m_mapDevices.begin()); it != m_mapDevices.end(); ++it)
    {
      try
      {
        it->second->changeState(Basda::ONLINE);
        m_logObjet.type = Nice::LINFO;
        m_logObjet.date = Nice::Date::now();
        m_logObjet.log = "Changed state to ONLINE for wheel";
        m_ptrLogger->log(m_logObjet);
      }catch (Basda::ServiceInitException & e)
      {
        W_LOG("ERROR CAUGHT " << e.what());
        m_logObjet.log = e.what();
        m_logObjet.type = Nice::LWARNING;
        m_logObjet.date = Nice::Date::now();
        m_ptrLogger->log(m_logObjet);
        it->second->init();
        it->second->changeState(Basda::ONLINE);
      }
    }
  std::cout << __FUNCTION__ << std::endl;
}

// move all the wheels to reference position and waits for them.
template <typename T>
void multiDeviceT::moveToReference()
{
  std::string strPropName = "CMD.MOVETOHOME.EXEC";
  // install_callback_on_completion(m_strCallbackName, strPropName);
  U8_LOG(__FUNCTION__ << " Moving Home");
  moveToReferenceStart();
  moveToReferenceWait();
  // try{
  //   for (BOOST_AUTO(it, m_mapDevices.begin()); it != m_mapDevices.end(); ++it)
  //     {
  //       it->second->moveToHomeStart();
  //     }
  //   U8_LOG(__FUNCTION__ << " Moving Home Start");

  //   for (BOOST_AUTO(it, m_mapDevices.begin()); it != m_mapDevices.end(); ++it)
  //     {
  //       it->second->moveToHomeWait();
  //     }
  //   U8_LOG(__FUNCTION__ << " Moving Home Wait");

  // }catch (Basda::DevException e)
  //   {
  //     E_LOG("Caught exception in wheels.moveHome. Maybe a dev is DUMMY?");
  //   }
}

// Moves all the wheels to reference position and DOES NOT waits for them.
template <typename T>
void multiDeviceT::moveToReferenceStart()
{
  std::string strPropName = "CMD.MOVETOHOME.EXEC";
  std::string strCallbackName = "motion_complete_reference";
  install_callback_on_completion(m_strCallbackName, strPropName);
  install_callback_on_reference(strCallbackName, strPropName);
  U8_LOG(__FUNCTION__ << " Moving Home");
  try{
    for (BOOST_AUTO(it, m_mapDevices.begin()); it != m_mapDevices.end(); ++it)
      {
        it->second->moveToHomeStart();
      }
    U8_LOG(__FUNCTION__ << " Moving Home Start");
  }catch (Basda::DevException e)
    {
      E_LOG("Caught exception in wheels.moveHome. Maybe a dev is DUMMY?");
    }
}

// WAITS all the wheels to reach reference position.
template <typename T>
void multiDeviceT::moveToReferenceWait(const Nice::Time& _time)
{
  std::string strPropName = "CMD.MOVETOHOME.EXEC";
  //  install_callback_on_completion(m_strCallbackName, strPropName);
  U8_LOG(__FUNCTION__ << " Moving Home");
  try{
    for (BOOST_AUTO(it, m_mapDevices.begin()); it != m_mapDevices.end(); ++it)
      {
        it->second->moveToHomeWait(_time);
      }
    U8_LOG(__FUNCTION__ << " Moving Home Wait");
  }catch (Basda::DevException e)
    {
      E_LOG("Caught exception in wheels.moveHome. Maybe a dev is DUMMY?");
    }
}

template <typename T>
void multiDeviceT::moveDeviceToReference(const std::vector<std::string>& _vecDevName)
{
  std::string strPropName = "CMD.MOVETOHOME.EXEC";
  //  install_callback_on_completion(m_strCallbackName, strPropName);
  moveDeviceToReferenceStart(_vecDevName);
  moveDeviceToReferenceWait(_vecDevName);
  // for (BOOST_AUTO(it, _vecDevName.begin()); it < _vecDevName.end(); ++it)
  //   {
  //     if (m_mapDevices.find(boost::to_upper_copy(*it)) != m_mapDevices.end())
  //       m_mapDevices[*it]->moveToHomeStart();
  //     else
  //       throw std::invalid_argument("Invalid device name.");
  //   }
  // for (BOOST_AUTO(it, _vecDevName.begin()); it < _vecDevName.end(); ++it)
  //   m_mapDevices[boost::to_upper_copy(*it)]->moveToHomeWait();
}

template <typename T>
void multiDeviceT::moveDeviceToReferenceStart(const std::vector<std::string>& _vecDevName)
{
  std::string strPropName = "CMD.MOVETOHOME.EXEC";
  install_callback_on_completion(m_strCallbackName, strPropName);
  for (BOOST_AUTO(it, _vecDevName.begin()); it < _vecDevName.end(); ++it)
    {
      if (m_mapDevices.find(boost::to_upper_copy(*it)) != m_mapDevices.end())
        m_mapDevices[*it]->moveToHomeStart();
      else
        throw std::invalid_argument("Invalid device name.");
    }
  // for (BOOST_AUTO(it, _vecDevName.begin()); it < _vecDevName.end(); ++it)
  //   m_mapDevices[boost::to_upper_copy(*it)]->moveToHomeWait();
}
template <typename T>
void multiDeviceT::moveDeviceToReferenceWait(const std::vector<std::string>& _vecDevName,
                                           const Nice::Time& _time)
{
  for (BOOST_AUTO(it, _vecDevName.begin()); it < _vecDevName.end(); ++it)
    m_mapDevices[boost::to_upper_copy(*it)]->moveToHomeWait(_time);
}


// moveToNamedPosition
// INPUT vector of named positions, expressed as integer index
template <typename T>
void multiDeviceT::moveToNamedPosition(const std::vector<int> &_vecPos)
{
  BOOST_AUTO(itPos, _vecPos.begin());
  std::string strPropName = "CMD.MOVETONAMEDPOSITION.EXEC";
  // install_callback_on_completion(m_strCallbackName, strPropName);
  moveToNamedPositionStart(_vecPos);
  moveToNamedPositionWait(_vecPos);

  // std::cout << m_mapDevices.size() << std::endl;
  // for (BOOST_AUTO(it, m_mapDevices.begin()); it != m_mapDevices.end(); ++it)
  //   {
  //     std::cout << it->first << std::endl
  //               << it->second->readServiceName() << std::endl;
  //     it->second->moveToNamedPositionStart(*itPos);
  //     std::advance(itPos, 1);
  //   }
  // itPos = _vecPos.begin();
  // for (BOOST_AUTO(it, m_mapDevices.begin()); it != m_mapDevices.end(); ++it)
  //   {
  //     it->second->moveToNamedPositionWait();
  //     std::advance(itPos, 1);
  //   }
}

template <typename T>
void multiDeviceT::moveToNamedPositionStart(const std::vector<int> &_vecPos)
{
  BOOST_AUTO(itPos, _vecPos.begin());
  std::string strPropName = "CMD.MOVETONAMEDPOSITION.EXEC";
  install_callback_on_completion(m_strCallbackName, strPropName);

  std::cout << m_mapDevices.size() << std::endl;
  for (BOOST_AUTO(it, m_mapDevices.begin()); it != m_mapDevices.end(); ++it)
    {
      std::cout << it->first << std::endl
                << it->second->readServiceName() << std::endl;
      try{
        it->second->moveToNamedPositionStart(*itPos);
        std::cout << __FUNCTION__ << std::endl;
        std::advance(itPos, 1);
      // }catch (Basda::InitException & e){
      //   it->second->init();
      //   it->second->moveToNamedPositionStart(*itPos);
      //   std::advance(itPos, 1);
      }catch (Basda::ServiceInitException & e){
        it->second->init();
        it->second->moveToNamedPositionStart(*itPos);
        std::advance(itPos, 1);
      }
    }
}

template <typename T>
void multiDeviceT::moveToNamedPositionWait(const std::vector<int>& _vecPos,
                                         const Nice::Time& _time)
{
  m_logObjet.host = "vultus";
  m_logObjet.pid = -99;
  m_logObjet.tid = -99;
  m_logObjet.user = "user";
  m_logObjet.file = __BASE_FILE__;
  m_logObjet.line = __LINE__;
  m_logObjet.function = __PRETTY_FUNCTION__;
  m_logObjet.name = m_logObjet.file + " " + m_logObjet.function;
  for (BOOST_AUTO(it, m_mapDevices.begin()); it != m_mapDevices.end(); ++it)
    {
      try
      {
        it->second->moveToNamedPositionWait(_time);
        std::cout << __FUNCTION__ << std::endl;
      }catch(Basda::ServiceIsNotOnlineException &e)
      {
        E_LOG(e.what());
        m_logObjet.log = e.what();
        m_logObjet.type = Nice::LWARNING;
        m_logObjet.date = Nice::Date::now();
        m_ptrLogger->log(m_logObjet);
      }catch(Basda::ServiceInitException &e)
      {
        E_LOG(e.what());
        m_logObjet.log = e.what();
        m_logObjet.type = Nice::LWARNING;
        m_logObjet.date = Nice::Date::now();
        m_ptrLogger->log(m_logObjet);
      }catch(std::exception &e)
      {
        E_LOG(e.what());
        m_logObjet.log = e.what();
        m_logObjet.type = Nice::LWARNING;
        m_logObjet.date = Nice::Date::now();
        m_ptrLogger->log(m_logObjet);
      }
    }
}


// moveToNamedPosition
// INPUT map of device as key and named positions as value, expressed as integer index
template <typename T>
void multiDeviceT::moveToNamedPosition(const std::map<std::string, int> &_mapNamedPos)
{
  U8_LOG(__PRETTY_FUNCTION__);
  m_logObjet.host = "vultus";
  m_logObjet.pid = -99;
  m_logObjet.tid = -99;
  m_logObjet.user = "user";
  m_logObjet.file = __BASE_FILE__;
  m_logObjet.line = __LINE__;
  m_logObjet.function = __PRETTY_FUNCTION__;
  m_logObjet.name = m_logObjet.file + " " + m_logObjet.function;
  std::string strPropName = "CMD.MOVETONAMEDPOSITION.EXEC";
  // install_callback_on_completion(m_strCallbackName, strPropName);
  try
  {
    moveToNamedPositionStart(_mapNamedPos);
    moveToNamedPositionWait(_mapNamedPos);
  }
  catch (Basda::ServiceIsNotOnlineException &e)
    {
        // E_LOG(e.what());
        m_logObjet.log = e.what();
        m_logObjet.type = Nice::LERROR;
        m_logObjet.date = Nice::Date::now();
        m_ptrLogger->log(m_logObjet);
        throw e;
    }
  catch(std::exception &e)
  {
    E_LOG(e.what());
    m_logObjet.log = e.what();
    m_logObjet.type = Nice::LWARNING;
    m_logObjet.date = Nice::Date::now();
    m_ptrLogger->log(m_logObjet);
  }
  // for (BOOST_AUTO(it, _mapNamedPos.begin()); it != _mapNamedPos.end(); ++it)
  //   {
  //     if (m_mapDevices.find(boost::to_upper_copy(it->first)) != m_mapDevices.end())
  //       {
  //         try
  //           {
  //             I_LOG(it->first << " Moving to named position " << it->second);
  //             m_mapDevices[boost::to_upper_copy(it->first)]->moveToNamedPositionStart(it->second);
  //           }
  //         catch (std::exception err)
  //           {
  //             E_LOG(it->first << " Service Exception!" << err.what());
  //             throw err;
  //           }
  //       }
  //     else
  //       throw std::invalid_argument("Wrong alias name for device.");
  //   }

  // for (BOOST_AUTO(it, _mapNamedPos.begin()); it != _mapNamedPos.end(); ++it)
  //   {
  //     if (m_mapDevices.find(boost::to_upper_copy(it->first)) != m_mapDevices.end())
  //       {
  //         try
  //           {
  //             m_mapDevices[boost::to_upper_copy(it->first)]->moveToNamedPositionWait();
  //           }
  //         catch (std::exception err)
  //           {
  //             E_LOG(it->first << " Service Exception!" << err.what());
  //             throw err;
  //           }
  //       }
  //     else
  //       throw std::invalid_argument("Wrong alias name for device.");
  //   }
}


template <typename T>
void multiDeviceT::moveToNamedPositionStart(const std::map<std::string, int> &_mapNamedPos)
{
  m_logObjet.host = "vultus";
  m_logObjet.pid = -99;
  m_logObjet.tid = -99;
  m_logObjet.user = "user";
  m_logObjet.file = __BASE_FILE__;
  m_logObjet.line = __LINE__;
  m_logObjet.function = __PRETTY_FUNCTION__;
  m_logObjet.name = m_logObjet.file + " " + m_logObjet.function;
  U8_LOG(__PRETTY_FUNCTION__);
  // if (_mapNamedPos.size() != m_vecDevices.size())
  //   throw std::invalid_argument("Number of positions must be the same as number of wheels.");
  // else
  //   {
  // boost::function<void(std::map<std::string, T>::const_iterator)> fn;
  // fn = boost::bind(&multiDeviceT::apply_callback2, this, _1);
  // std::for_each(m_mapDevices.begin(), m_mapDevices.end(),
  //               boost::bind(std::mem_fun(&multiDeviceT::apply_callback2), this, _1));
  std::string strPropName = "CMD.MOVETONAMEDPOSITION.EXEC";
  install_callback_on_completion(m_strCallbackName, strPropName);

  // strPropName = "STATE";
  // install_callback_on_state(m_strCallbackNameState, strPropName);

  for (BOOST_AUTO(it, _mapNamedPos.begin()); it != _mapNamedPos.end(); ++it)
    {
      if (m_mapDevices.find(boost::to_upper_copy(it->first)) != m_mapDevices.end())
        {
          try
            {
              I_LOG(it->first << " Moving to named position " << it->second);
              m_mapDevices[boost::to_upper_copy(it->first)]->moveToNamedPositionStart(it->second);
            }
          catch (Basda::ServiceIsNotOnlineException &e)
            {
                E_LOG(e.what());
                m_logObjet.log = e.what();
                m_logObjet.type = Nice::LWARNING;
                m_logObjet.date = Nice::Date::now();
                m_ptrLogger->log(m_logObjet);
                throw e;
            }
          catch (std::exception err)
            {
              E_LOG(it->first << " Service Exception!" << err.what());
              throw err;
            }
        }
      else
        throw std::invalid_argument("Wrong alias name for device.");
    }
  // for (BOOST_AUTO(it, _mapNamedPos.begin()); it != _mapNamedPos.end(); ++it)
  //   m_mapDevices[boost::to_upper_copy(it->first)]->moveToNamedPositionWait();
  // }
}



template <typename T>
void multiDeviceT::moveToNamedPositionWait(const std::map<std::string, int>& _mapNamedPos,
                                         const Nice::Time& _time)
{
  m_logObjet.host = "vultus";
  m_logObjet.pid = -99;
  m_logObjet.tid = -99;
  m_logObjet.user = "user";
  m_logObjet.file = __BASE_FILE__;
  m_logObjet.line = __LINE__;
  m_logObjet.function = __PRETTY_FUNCTION__;
  m_logObjet.name = m_logObjet.file + " " + m_logObjet.function;
  for (BOOST_AUTO(it, _mapNamedPos.begin()); it != _mapNamedPos.end(); ++it)
    {
      try
        {
          m_mapDevices[boost::to_upper_copy(it->first)]->moveToNamedPositionWait(_time);
        }
      catch (Basda::ServiceIsNotOnlineException &e)
        {
          E_LOG(e.what());
          m_logObjet.log = e.what();
          m_logObjet.type = Nice::LWARNING;
          m_logObjet.date = Nice::Date::now();
          m_ptrLogger->log(m_logObjet);
          throw e;
        }
      catch (std::exception err)
        {
          E_LOG(it->first << " Exception " << err.what());
          //
          // throw err;
        }
    }
}

// moveToPosition
// INPUT map with device alias as key and position as value, expressed in steps
template <typename T>
void multiDeviceT::moveToPosition(std::map<std::string, int>& _mapPos)
{
  U8_LOG(__PRETTY_FUNCTION__);
  // if (_mapPos.size() != m_vecDevices.size())
  //   throw std::invalid_argument("Number of positions must be the same as number of wheels.");
  // else
  //   {
  std::string strPropName = "CMD.MOVEABSOLUTE.EXEC";
  install_callback_on_completion(m_strCallbackName, strPropName);
  moveToPositionStart(_mapPos);
  moveToPositionWait(_mapPos);

  // for (BOOST_AUTO(it, _mapPos.begin()); it != _mapPos.end(); ++it)
  //   {
  //     if (m_mapDevices.find(it->first) != m_mapDevices.end())
  //       {
  //         m_mapDevices[it->first]->moveAbsoluteStart(it->second);
  //       }
  //     else
  //       throw std::invalid_argument("Wrong alias name for device.");
  //   }
  // for (BOOST_AUTO(it, _mapPos.begin()); it != _mapPos.end(); ++it)
  //   {
  //     if (m_mapDevices.find(it->first) != m_mapDevices.end())
  //       {
  //         m_mapDevices[it->first]->moveAbsoluteWait();
  //       }
  //     else
  //       throw std::invalid_argument("Wrong alias name for device.");
  //   }

  // for (BOOST_AUTO(it, _mapPos.begin()); it != _mapPos.end(); ++it)
  //   m_mapDevices[it->first]->moveAbsoluteWait();
  // }
}

template <typename T>
void multiDeviceT::moveToPositionStart(std::map<std::string, int>& _mapPos)
{
  U8_LOG(__PRETTY_FUNCTION__);
  // if (_mapPos.size() != m_vecDevices.size())
  //   throw std::invalid_argument("Number of positions must be the same as number of wheels.");
  // else
  //   {
  std::string strPropName = "CMD.MOVEABSOLUTE.EXEC";
  install_callback_on_completion(m_strCallbackName, strPropName);

  for (BOOST_AUTO(it, _mapPos.begin()); it != _mapPos.end(); ++it)
    {
      if (m_mapDevices.find(it->first) != m_mapDevices.end())
        {
          m_mapDevices[it->first]->moveAbsoluteStart(it->second);
        }
      else
        throw std::invalid_argument("Wrong alias name for device.");
    }
  // for (BOOST_AUTO(it, _mapPos.begin()); it != _mapPos.end(); ++it)
  //   m_mapDevices[it->first]->moveAbsoluteWait();
  // }
}

template <typename T>
void multiDeviceT::moveToPositionWait(std::map<std::string, int>& _mapPos,
                                    const Nice::Time& _time)
{
  for (BOOST_AUTO(it, _mapPos.begin()); it != _mapPos.end(); ++it)
    m_mapDevices[it->first]->moveAbsoluteWait(_time);
}


template <typename T>
bool multiDeviceT::isDevice(const std::string& _strDevAlias)
{
  bool boolRet;
  if (m_mapDevices.find(boost::to_upper_copy(_strDevAlias)) != m_mapDevices.end())
    boolRet = true;
  else
    boolRet = false;
  return boolRet;
}


// This function is used by superobject motionCtrl
template <typename T>
void multiDeviceT::set_callback_fn(boost::function<void(const Nice::Completion&)>& _cbfn,
                                 const std::string& _strFnName)
{
  m_callbackFn = _cbfn;
  m_strCallbackName = _strFnName;
}

template <typename T>
void multiDeviceT::set_callback_fn_state(boost::function<void(const Basda::ServiceStateMachine&)>& _cbfn,
                                       const std::string& _strFnName)
{
  // m_callbackFnState = _cbfn;
  // m_strCallbackNameState = _strFnName;
}

template <typename T>
void multiDeviceT::set_callback_fn_state(boost::function<void(const Basda::ServiceStateMachine&,
                                                            const std::string&)>& _cbfn,
                                       const std::string& _strDevAlias)
{
  // m_callbackFnState = boost::bind(&niceCallback::callback_state_change, m_ptrCallbackObjState, _1, boost::ref(_strDevAlias));
  //  m_callbackFnState = boost::bind(&niceCallback::callback_motion_complete_indi)
  // m_mapCallbackFnState[boost::to_upper_copy(_strDevAlias)] = _cbfn;
  // _strFnName not used
}

// This function is used by superobject motionCtrl
template <typename T>
void multiDeviceT::set_callback_fn_count(boost::function<void(const Nice::Completion&)>& _cbfn,
                                       const std::string& _strFnName)
{
  m_callbackFnCount = _cbfn;
  m_strCallbackNameCount = _strFnName;
}


// --------------------------
// GETTERS
// --------------------------
// Get last occupied named position for each wheel
template <typename T>
std::vector<std::string> multiDeviceT::getNamedPosition(const std::size_t _deviceIdx)
{
  std::vector<std::string> res;
  if (_deviceIdx >= m_nDevices)
    {
      // return position for all wheels
      for (BOOST_AUTO(it, m_mapDevices.begin()); it != m_mapDevices.end(); ++it)
        {
          res.push_back(it->second->config()->leaf("VAR.NAMED_POSITION")->get<std::string>());
        }
    }
  else
    {
      BOOST_AUTO(it, m_mapDevices.begin());
      std::advance(it, _deviceIdx);
      res.push_back(it->second->config()->leaf("VAR.NAMED_POSITION")->get<std::string>());
      // BOOST_AUTO(it, m_vecDevices.begin()+_deviceIdx);
      // res.push_back((*it)->config()->leaf("VAR.NAMED_POSITION")->get<std::string>());
    }
  return res;
}

// Get last occupied named position for each wheel
template <typename T>
std::vector<std::string> multiDeviceT::getNamedPosition(const std::vector<std::string>& _vecAlias)
{
  std::vector<std::string> vecRes;
  // int idx = 0;
  BOOST_AUTO(itPos, _vecAlias.begin());
  for (BOOST_AUTO(it, m_mapDevices.begin()); it != m_mapDevices.end(); ++it)
    {
      // idx = std::distance(it, m_mapDevices.begin());
      // itPos = _vecAlias.begin() + idx;
      if (m_mapDevices.find(*itPos) != m_mapDevices.end())
        {
          vecRes.push_back(it->second->config()->leaf("VAR.NAMED_POSITION")->get<std::string>());
          std::advance(itPos, 1);
        }
      else
        throw std::invalid_argument("Wrong alias name for device");
    }
  return vecRes;
}

template <typename T>
T multiDeviceT::getDevicePtr(const std::string& _strDeviceName)
{
  BOOST_AUTO(itFind, m_mapDevices.find(_strDeviceName));
  if (itFind == m_mapDevices.end())
    {
      E_LOG("Wrong alias name for device");
      throw std::invalid_argument("Wrong alias name for device");
    }
  else
    return itFind->second;
}

template <typename T>
void multiDeviceT::emergencyAbort()
{
  std::string strPropName = "CMD.MOVEABSOLUTE.EXEC";
  install_callback_on_completion(m_strCallbackName, strPropName);
  for (BOOST_AUTO(it, m_mapDevices.begin()); it != m_mapDevices.end(); ++it)
    it->second->abort();
}

template <typename T>
int multiDeviceT::stop()
{
  int status = EXIT_SUCCESS;
  try
    {
      for (BOOST_AUTO(it, m_mapDevices.begin()); it != m_mapDevices.end(); ++it)
        {
          it->second->stop();
          W_LOG(it->first << " Stopped");
        }
    }
  catch(Nice::Exception err)
    {
      E_LOG(err);
      status = EXIT_FAILURE;
    }
  return status;
}

void inAppConfig_DontDoThat()
{
    /* a config file whould look like

         WHEEL.SCI1_W.CON=[ALIAS=shark.coro_slit_u]
         WHEEL.SCI2_W.CON=[ALIAS=shark.coro_slit_W]

       or 2 configs:

       main.cfg:

         WHEEL.CONFIG=sub.cfg

       sub.cfg:

         SCI1_W.CON=[ALIAS=shark.coro_slit_u]
         SCI2_W.CON=[ALIAS=shark.coro_slit_W]

       instead of alias endpoints, you can also use:

         SCI1_W.CON=[NAME=shark.motion_ctrl,TCP=:23005,PATH=MOTION_CTRL.CORO_SLIT_U.SVC]
         V.CON=[NAME=shark.motion_ctrl,TCP=:23005,PATH=MOTION_CTRL.CORO_SLIT_V.SVC]

    */
    Nice::JointPtr node = Nice::Application::config()->node("WHEEL");
    Nice::Endpoint ep;

    ep = Nice::Endpoint::fromString(std::string("[ALIAS=sci1_w]"));
    node->setAtNode<Nice::Endpoint>("SCI1_W.CON", ep);

    ep = Nice::Endpoint::fromString(std::string("[ALIAS=sci2_w]"));
    node->setAtNode<Nice::Endpoint>("SCI2_W.CON", ep);
}


