// $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/src/motion_ctrl/wheels/multiWheel.cpp 3908 2022-04-19 11:07:50Z depa $

#include "shins_util.h"
#include "multiWheel.hpp"
//#include <Basda/BridgeDispatchedCmdMemFn.hh>

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


void WheelDevice::GetDataFromConfigFile()
{
    units = su::get_main_units_one_axe(cfgFile);
    namedPositions = su::get_named_positions_one_axe(cfgFile);
    alias = su::get_alias_from_config(cfgFile);
}


void WheelDevice::GetDataFromConfig(const Nice::JointPtr dev)
{
    Nice::JointPtr dev_parent = dev->parent();
    Nice::SeqString abs_children, trans_children;
    Nice::SeqString::iterator itString;
    dev_parent->node("DEV.1.CFG.NAMED_POS.ABS")->children(abs_children);
    alias = dev->node("NAME")->get<std::string>();
    // U9_LLOG(__FUNCTION__ << " ~ Device : " << alias);
    for(itString = abs_children.begin(); itString < abs_children.end(); itString++)
    {
        Nice::JointPtr nodechild = dev_parent->node("DEV.1.CFG.NAMED_POS.ABS." + *itString);
        namedPositions.insert(std::pair<std::string, double>(nodechild->leaf()->getAttribute<std::string>("NAME"), nodechild->get<int>()));
        int idxPos;
        su::getValue<int>(*itString, idxPos);
        namedIds.insert(std::pair<std::string, int>(nodechild->leaf()->getAttribute<std::string>("NAME"), idxPos));
        // U9_LLOG(__FUNCTION__ << " ~ " << "Named Position " << idxPos << " : " << nodechild->leaf()->getAttribute<std::string>("NAME") << " - " << nodechild->get<int>());
    }
    dev_parent->node("DEV.1.CFG.TRANS")->children(trans_children);
    for(itString = trans_children.begin(); itString < trans_children.end(); itString++)
    {
        Nice::Point trans = dev_parent->node("DEV.1.CFG.TRANS." + *itString)->get<Nice::Point>();
        units.insert(std::pair<std::string, std::tr1::tuple<double, double> >(*itString, std::tr1::tuple<double, double>(trans.xp, trans.yp)));
        // U9_LLOG(__FUNCTION__ << " ~ Transformation : " << *itString << "[" << trans.xp << ", " << trans.yp << "]");
    }
}


/** @brief      Constructs the object.
 *  @param[in]  _config        The configuration
 *  @param[in]  _vecAliasName  The vector alias name
 *  @param[in]  _ptrIceLogger  The pointer ice logger */
multiWheel::multiWheel(const std::string & _service)
: multiDevice(std::vector<std::string>(), typeid(this).name())
{
    U6_LLOG("Function : " << __FUNCTION__ << "; ObjName : " << __OBJ_NAME__ << "; objname = " << m_objname);
    // m_counter = 0;
    Basda::MoccaWheelServiceWorkerInterfacePtr tmpWheel;
    m_service = _service;
    // m_ptrIceLogger = boost::make_shared<Nice::SyslogFunction>("SHINS");
    // __OBJ_NAME__ = typeid(this).name();
    // m_ptrIceLogger = _ptrIceLogger;
    // m_vecAliasName = _vecAliasName;
    // m_nDevices = _vecAliasName.size();
    // for(std::size_t i = 0; i < m_nDevices; i++)
    // {
    //     m_vecWheels.push_back(tmpWheel);
    // }

    // //shins::itString_t it;
    // // Build map containg Basda::MoccaWheelServiceWorkerInterfacePtr objects
    // // U3_LLOG("Creation of wheels' map");
    // std::stringstream log_message;
    // log_message << " - Alias Names: ";
    // std::vector<std::string>::const_iterator it;
    // for(it = _vecAliasName.begin(); it < _vecAliasName.end(); ++it)
    // {
    //     log_message << *it << "; ";
    //     std::string upper_it = shins::util::str_toupper(*it);
    //     m_mapWheels.insert(std::pair<std::string, Basda::MoccaWheelServiceWorkerInterfacePtr>(upper_it, tmpWheel));
    //     // MoccaWheelServiceWorkerInterfacePtr will be initialized by calling multiWheel::connect
    //     m_mapWheels[upper_it] = tmpWheel;

    //     std::string cfgFile = "";
    //     std::map<std::string, std::string>::const_iterator itFind = _mapCfgFiles.find(upper_it);
    //     if(itFind != _mapCfgFiles.end())cfgFile = itFind->second;
    //     else
    //     {
    //         E_LLOG("Missing Config File Declaration for : " << upper_it);
    //     }
    //     m_MapWheels.insert(std::pair<std::string, WheelDevice>(upper_it, WheelDevice(cfgFile)));

    //     std::map<std::string, WheelDevice>::iterator itFind2 = m_MapWheels.find(upper_it);
    //     assert(itFind2 != m_MapWheels.end() && itFind2->second.alias == upper_it);
    // }

    U8_LLOG(__FUNCTION__);
}


/** @brief      Destroys the object. */
multiWheel::~multiWheel()
{
    U6_LLOG(__FUNCTION__ << " multiWheel destructor");
    U8_LLOG("");
}


int multiWheel::setSimulatedDevice(const std::string & _dev)
{
    int result = EXIT_FAILURE;
    return result;
}

// --------------------
// CALLBACKS begin
// --------------------

/**
 * @brief      { function_description }
 *
 * @param[in]  _strCallbackName  The string callback name
 * @param[in]  _strPropName      The string property name
 */
void multiWheel::install_callback_on_completion(const std::string &_strCallbackName, const std::string &_strPropName)
{
    U6_LLOG("multiWheel::" << __FUNCTION__ << suc::tilde << _strCallbackName << suc::tilde << _strPropName);
    Nice::PropertyPtr prop;
    std::string bridgeName = "", logMsg = "";

    void (WheelDevice::* Bridge)(const Nice::Completion &)  = &WheelDevice::ErrorBridge;
    if(_strPropName == "CMD.MOVETOHOME.EXEC")
    {
        Bridge = &WheelDevice::MoveToHomeBridge;
    }
    else if(_strPropName == "CMD.MOVETONAMEDPOSITION.EXEC")
    {
        Bridge = &WheelDevice::MoveToNamedBridge;
    }
    else if(_strPropName == "CMD.MOVEABSOLUTE.EXEC")
    {
        Bridge = &WheelDevice::MoveAbsoluteBridge;
    }
    else if(_strPropName == "CMD.MOVERELATIVE.EXEC")
    {
        Bridge = &WheelDevice::MoveRelativeBridge;
    }
    else if(_strPropName == "CMD.SCANALLREFERENCESWITCHES.EXEC")
    {
        Bridge = &WheelDevice::ScanAllReferenceSwitchesBridge;
    }
    // Install callback for each wheel
    std::map<std::string, Basda::MoccaWheelServiceWorkerInterfacePtr>::const_iterator it;
    for(it = m_mapWheels.begin(); it != m_mapWheels.end(); ++it)
    {
        bridgeName = _strCallbackName + "-" + it->first + "-motion";
        prop = it->second->config()->leaf(_strPropName);
//        if(it->first == "CORO_SLIT_W")
//        {
//            prop->applyBridge(
//                new Nice::PropertyBridgeMemFn<Nice::Completion, multiWheel,
//                    void (multiWheel::*)(const Nice::Completion &) >(this, &multiWheel::coro_callback, prop->id()),
//                    bridgeName + "-count");
//        }
//        else
//        {
//            prop->applyBridge(
//                new Nice::PropertyBridgeMemFn<Nice::Completion, multiWheel,
//                    boost::function<void(const Nice::Completion&)> multiWheel::*>(this, &multiWheel::m_callbackFnCount, prop->id()),
//                    bridgeName + "-count");
//        }
        std::map<std::string, WheelDevice>::iterator itFind = m_MapWheels.find(it->first);
        if(itFind->second.iPtr)
        {
            prop->applyBridge( new Nice::PropertyBridgeMemFn<Nice::Completion, WheelDevice, void (WheelDevice::*)(const Nice::Completion &)>(
            //prop->applyBridge( new Nice::PropertyBridgeDispatchedMemFn<Nice::Completion, WheelDevice, void (WheelDevice::*)(const Nice::Completion &)>(
                &itFind->second, Bridge, prop->id()),
                bridgeName + "count");
            U9_LLOG( it->first << " -- " << bridgeName << "-count");
        }
    }
    U8_LLOG("");
}


/**
 * @brief       Install callback on property
 *
 * @param[in]  _strCallbackName  The string callback name
 * @param[in]  _strPropName      The string property name
 */
void multiWheel::install_callback_on_reference(const std::string& _strCallbackName, const std::string& _strPropName)
{
    U6_LLOG("multiWheel::" << __FUNCTION__ << suc::tilde << _strCallbackName << suc::tilde << _strPropName);
    Nice::PropertyPtr prop;
    boost::shared_ptr<niceCallback> ptrCallbackObjReference;
    boost::function<void(const Nice::Completion&)> callbackFnReference;
    std::string bridgeName = "", logMsg="";

    std::map<std::string, Basda::MoccaWheelServiceWorkerInterfacePtr>::const_iterator it;
    for(it = m_mapWheels.begin(); it != m_mapWheels.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, multiWheel,
      //    boost::function<void(const Nice::Completion&)> multiWheel::* >
      //    (
      //     this,
      //     &callbackFnReference, prop->id()),
      //    bridgeName+"-reference"
      //    );
    }
    U8_LLOG("");
}


/**
 * @brief      Install callback on service state
 *
 * @param[in]  _strCallbackName  The string callback name
 * @param[in]  _strPropName      The string property name
 */
void multiWheel::install_callback_on_state(const std::string& _strCallbackName, const std::string& _strPropName)
{
    U6_LLOG("multiWheel::" << __FUNCTION__ << suc::tilde << _strCallbackName << suc::tilde << _strPropName);
    Nice::PropertyPtr prop;
    std::string bridgeName = "", logMsg="";

    std::map<std::string, Basda::MoccaWheelServiceWorkerInterfacePtr>::const_iterator it;
    for(it = m_mapWheels.begin(); it != m_mapWheels.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, multiWheel,
      //    boost::function<void(const Basda::ServiceStateMachine&)> multiWheel::* >
      //    (
      //     this,
      //     &multiWheel::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, multiWheel,
      //        boost::function<void(const Basda::ServiceStateMachine&)> multiWheel::* >
      //        (
      //         this,
      //         &multiWheel::m_mapCallbackFnState[it->first], prop->id()),
      //        bridgeName
      //        );
      //   }
    }
    U8_LLOG("");
}


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

void multiWheel::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))
{
    U6_LLOG(__FUNCTION__);
    std::map<std::string, WheelDevice>::iterator itFind;
    itFind = m_MapWheels.find(alias);
    if(itFind != m_MapWheels.end())
    {
        itFind->second.SetSecondaryCallbacks(moveToHomeCB, moveToNamedCB, moveAbsoluteCB, moveRelativeCB, moveToLimitCB);
    }
    else
    {
        std::stringstream msg;
        msg << "Error invalid argument : " << alias;
        E_LLOG(msg.str());
        U8_LLOG("");
        throw(std::invalid_argument(msg.str()));
    }
    U8_LLOG("");
}


/**
 * @brief      Gets the end point name.
 *
 * @param[in]  _wheelIdx  The wheel index
 *
 * @return     The end point name.
 */
std::string multiWheel::getEndPointName(const std::size_t _wheelIdx)
{
    U6_LLOG(__FUNCTION__);

    if(_wheelIdx > m_vecWheels.size())
    {
        E_LLOG("Range error");
        U8_LLOG("");
        throw std::range_error("range error");
    }
    else
    {
        // m_ptrIceLogger->trace("INFO", __PRETTY_FUNCTION__);
        // U3_LLOG(getBaseName(_wheelIdx) << ".CON");
        // U3_LLOG(__FUNCTION__ << " --- " << getBaseName(_wheelIdx) << ".CON");
        U8_LLOG("");
        return (getBaseName(_wheelIdx) + ".CON");
    }
}


std::string multiWheel::getBaseName(const std::size_t _wheelIdx)
{
    U6_LLOG(__FUNCTION__);

    if(_wheelIdx > m_vecWheels.size())
    {
        E_LLOG("out of range");
        U8_LLOG("");
        throw std::out_of_range("out of range");
    }
    else
    {
        std::vector<std::string>::const_iterator it;
        it = m_vecAliasName.begin() + _wheelIdx;
        // auto it = m_vecBaseName.begin() + _wheelIdx;
        // U3_LLOG(__FUNCTION__ << " " << *it);
        U8_LLOG("");
        return (*it);
    }
}


/** @brief      Connect to the basdard server tree */
void multiWheel::connect()
{
    U6_LLOG(__FUNCTION__);
    Nice::JointPtr nodewheels = Nice::Application::config()->  node(m_service + "." + "WHEELS");
    Nice::SeqString WheelChildren;
    nodewheels->children(WheelChildren);
    Nice::SeqString::iterator itSeq;
    Basda::MoccaWheelServiceWorkerInterfacePtr iPtr;
    m_vecAliasName = WheelChildren;
    for(itSeq = WheelChildren.begin(); itSeq < WheelChildren.end(); itSeq++)
    {
        try
        {
            Nice::JointPtr nodewheel = nodewheels->node(*itSeq + ".SVC");
            iPtr = Basda::MoccaWheelServiceWorkerInterface::create(nodewheel);
        }
        catch (const IceUtil::NullHandleException & e){ A_LLOG(e.what()); }
        catch(...){E_LLOG(__FUNCTION__)};
        m_mapWheels.insert(std::pair<std::string, Basda::MoccaWheelServiceWorkerInterfacePtr>(*itSeq, iPtr));
        m_MapWheels.insert(std::pair<std::string, WheelDevice>(*itSeq, WheelDevice(iPtr)));
        m_nDevices++;
    }

    // std::map<std::string, Basda::MoccaWheelServiceWorkerInterfacePtr>::iterator it;

    // for(it = m_mapWheels.begin(); it != m_mapWheels.end(); ++it)
    // {
    //     it->second = NULL;
    //     try
    //     {
    //         Nice::JointPtr nodeWheel = Nice::Application::config()->node("motion_ctrl-svr.WHEELS." + it->first + ".SVC");
    //         it->second = Basda::MoccaWheelServiceWorkerInterface::create(nodeWheel);
    //     }
    //     catch (const IceUtil::NullHandleException & e){ A_LLOG(e.what()); }
    //     catch(...){E_LLOG(__FUNCTION__)};
    //     m_MapWheels.find(it->first)->second.SetInterfacePointer(it->second);
    // }
    U8_LLOG("");
}


Ice::Int multiWheel::SetWheelInterdictionCallbacks(const std::string & alias,
                                   bool (* checkInterdiction)(void),
                                   bool (* interdictionReaction)(void))
{
    Ice::Int status = EXIT_FAILURE;
    std::map<std::string, WheelDevice>::iterator itWheel = m_MapWheels.find(alias);
    if(itWheel != m_MapWheels.end())
    {
        itWheel->second.SetInterdictionCallbacks(checkInterdiction, interdictionReaction);
        return EXIT_SUCCESS;
    }
    else
        return status;
}


int multiWheel::setWheelsInterdictionCallbacks(const std::map<std::string, boost::function<bool()> > callbacks_map)
{
    U6_LLOG(__FUNCTION__);
//    boost::function<bool()> f;
//    std::map<std::string, boost::function<bool()> >::const_iterator it;
//    for(it = m_mapInterdictions.begin(); it != m_mapInterdictions.end(); it++)
//    {
////        U9_LLOG(it->first << " - is_interdict returns : " << ((it->second)() ? "TRUE" : "FALSE"));
//        if(callbacks_map.find(it->first) != callbacks_map.end())
//        {
//            f = callbacks_map.find(it->first)->second;
//            m_mapInterdictions[it->first] = f;
////            U9_LLOG(it->first << " - is_interdict returns : " << ((it->second)() ? "TRUE" : "FALSE"));
//        }
//        else
//        {
//            E_LLOG("Error");
//            U8_LLOG("");
//            throw std::invalid_argument("Missing Wheel " + it->first);
//        }
//    }
    U8_LLOG("");
    return EXIT_SUCCESS;
}


int multiWheel::setWheelsInterdictionReactionCallback(const std::map<std::string, boost::function<void()> > callbacks_map)
{
    U6_LLOG(__FUNCTION__);
//    boost::function<void()> f;
//    std::map<std::string, boost::function<void()> >::const_iterator it;
//    for(it = m_mapReactions.begin(); it != m_mapReactions.end(); it++)
//    {
//        if(callbacks_map.find(it->first) != callbacks_map.end())
//        {
//            f = callbacks_map.find(it->first)->second;
//            m_mapReactions[it->first] = f;
//        }
//        else
//        {
//            E_LLOG("Error");
//            U8_LLOG("");
//            throw std::invalid_argument("Missing Wheel " + it->first);
//        }
//    }
    U8_LLOG("");
    return EXIT_SUCCESS;}

/**
 * @brief      Set state to standby
 */
// void multiWheel::standby()
// {
//   U3_LLOG(__PRETTY_FUNCTION__);

//   for(BOOST_AUTO(it, m_mapWheels.begin()); it != m_mapWheels.end(); ++it)
//     {
//       try{
//         it->second->changeState(Basda::STANDBY);
//         U3_LLOG(it->first << " STANDBY");
//       }catch(Basda::ServiceInitException &e)
//         {
//           E_LLOG(e.what());
//           it->second->init();
//           it->second->changeState(Basda::STANDBY);
//         }
//     }
// }



// /**
//  * @brief      Set state to online
//  */
// void multiWheel::online()
// {
//   for(BOOST_AUTO(it, m_mapWheels.begin()); it != m_mapWheels.end(); ++it)
//     {
//       try
//         {
//           it->second->changeState(Basda::ONLINE);
//           U3_LLOG(it->first << " ONLINE");
//         }catch(Basda::ServiceInitException & e)
//         {
//           E_LLOG("ERROR CAUGHT " << e.what());
//           it->second->init();
//           it->second->changeState(Basda::ONLINE);
//         }
//     }
//   // std::cout << __FUNCTION__ << std::endl;
// }

/**
 * @brief      Set state to standby
 */
void multiWheel::standby(const std::vector<std::string>& _vecDevAlias)
{
    U6_LLOG(__FUNCTION__);
    std::stringstream log_message;
    bool wait_result;
    if( _vecDevAlias.size() == 1)
        log_message << "Wheel : ";
    else
        log_message << "Wheels : ";
    if(!_vecDevAlias.empty())
    {
        std::vector<std::string>::const_iterator it;
        std::map<std::string, Basda::MoccaWheelServiceWorkerInterfacePtr>::iterator itFind;
        for(it = _vecDevAlias.begin(); it != _vecDevAlias.end(); ++it)
        {
            itFind = m_mapWheels.find(boost::to_upper_copy(*it));
            if(itFind != m_mapWheels.end())
            {
                try
                {
                    wait_result = false;
                    if(itFind->second)
                        wait_result = itFind->second->changeState(Basda::STANDBY);
                    log_message << itFind->first << " STANDBY -> " << (wait_result ? "success" : (itFind->second ? "timed out" : "failed")) << suc::tilde;
                }
                catch(Basda::ServiceInitException &e)
                {
                    E_LLOG(e.what());
                    // itFind->second->init();
                    itFind->second->changeState(Basda::STANDBY);
                }
            }
        }
        U8_LLOG(log_message.str());
    }
    else
    {
         std::map<std::string, Basda::MoccaWheelServiceWorkerInterfacePtr>::const_iterator it;
        for(it = m_mapWheels.begin(); it != m_mapWheels.end(); ++it)
        {
            try
            {
                wait_result = false;
                U9_LLOG(__FUNCTION__ << suc::tilde << it->first << suc::tilde << it->second);
                if(it->second)
                    wait_result = it->second->changeState(Basda::STANDBY);
                log_message << it->first << " STANDBY -> " << (wait_result ? "success" : (it->second ? "timed out" : "failed")) << suc::tilde;
            }
            catch(Basda::ServiceInitException &e)
            {
                E_LLOG(e.what());
                // it->second->init();
                it->second->changeState(Basda::STANDBY);
            }
        }
        U8_LLOG(log_message.str());
    }
}

/** @brief      Set state to online */
void multiWheel::online(const std::vector<std::string>& _vecDevAlias)
{
    U6_LLOG(__FUNCTION__);
    Nice::Time timeout = Nice::Time::seconds(2);
    bool wait_result;
    if(!_vecDevAlias.empty())
    {
        std::stringstream log_message("Devices: ");
        std::vector<std::string>::const_iterator it;
        for(it = _vecDevAlias.begin(); it != _vecDevAlias.end(); ++it)
        {
            std::map<std::string, Basda::MoccaWheelServiceWorkerInterfacePtr>::iterator itFind;
            itFind = m_mapWheels.find(boost::to_upper_copy(*it));
            if(itFind != m_mapWheels.end())
            {
                try
                {   wait_result = false;
                    if(itFind->second)
                        wait_result = itFind->second->changeState(Basda::ONLINE, timeout);
                    log_message << itFind->first << " ONLINE -> " << (wait_result ? "success" : (itFind->second ? "timed out" : "failed")) << suc::tilde;
                }
                catch(Basda::ServiceInitException & e)
                {
                    E_LLOG("ERROR CAUGHT " << e.what());
                    U8_LLOG("");
                    throw(e);
                }
                catch(...)
                {
                    E_LLOG(__FUNCTION__);
                    U8_LLOG("");
                    throw;
                }
            }
        }
        U8_LLOG(__FUNCTION__ << " " << log_message.str());
    }
    else
    {
        std::stringstream log_message("MapWheels");
        std::map<std::string, Basda::MoccaWheelServiceWorkerInterfacePtr>::iterator it;
        for(it = m_mapWheels.begin(); it != m_mapWheels.end(); ++it)
        {
            try
            {
                wait_result = false;
                if(it->second)
                    wait_result = it->second->changeState(Basda::ONLINE, timeout);
                log_message << it->first << " ONLINE -> " << (wait_result ? "success" : (it->second ? "timed out" : "failed")) << suc::tilde;
            }
            catch(Basda::ServiceInitException & e)
            {
                E_LLOG("ERROR CAUGHT " << e.what());
                U8_LLOG("");
                throw(e);
            }
            catch(...)
            {
                E_LLOG(__FUNCTION__);
                U8_LLOG("");
                throw;
            }
        }
        U8_LLOG(__FUNCTION__ << " " << log_message.str());
    }
}


/** @brief Generic method to change state **/
void multiWheel::changeState(const Basda::ServiceState& _state, const std::vector<std::string>& _vecDevAlias)
{
    U6_LLOG(__FUNCTION__);
    if(!_vecDevAlias.empty())
    {
        std::vector<std::string>::const_iterator it;
        for(it = _vecDevAlias.begin(); it != _vecDevAlias.end(); ++it)
        {
            std::map<std::string, Basda::MoccaWheelServiceWorkerInterfacePtr>::const_iterator itFind;
            itFind = m_mapWheels.find(boost::to_upper_copy(*it));
            if(itFind != m_mapWheels.end())
            {
                try
                {
                    itFind->second->changeState(_state);
                    U3_LLOG(itFind->first << _state);
                }
                catch(Basda::ServiceInitException &e)
                {
                    E_LLOG(e.what());
                    // itFind->second->init();
                    itFind->second->changeState(Basda::STANDBY);
                }
            }
        }
    }
    else
    {
        std::map<std::string, Basda::MoccaWheelServiceWorkerInterfacePtr>::const_iterator it;
        for(it = m_mapWheels.begin(); it != m_mapWheels.end(); ++it)
        {
            try
            {
                it->second->changeState(_state);
                U3_LLOG(it->first << _state);
            }
            catch(Basda::ServiceInitException &e)
            {
                E_LLOG(e.what());
                // it->second->init();
                it->second->changeState(Basda::STANDBY);
            }
        }
    }
    U8_LLOG("");
}


/** @brief      Move all the wheels to reference position and waits for each wheel to have reached such position */
void multiWheel::moveToReference()
{
    U6_LLOG(__FUNCTION__);
    try
    {
        moveToReferenceStart();
        moveToReferenceWait(Nice::Time::seconds(80));
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
}

/** @brief      Start motion of all the wheels to reference position not waiting for them to reach the position. */
void multiWheel::moveToReferenceStart()
{
    U6_LLOG(__FUNCTION__);
    try
    {
        std::map<std::string, WheelDevice>::const_iterator it;
        for(it = m_MapWheels.begin(); it != m_MapWheels.end(); ++it)
        {
            if(!it->second.CheckInterdiction())
                it->second.iPtr->moveToHomeStart();
            else
            {
                E_LLOG(__FUNCTION__ << suc::tilde << it->first << suc::tilde << "Interdicted");
                U8_LLOG("")
                throw WheelInterdiction("Wheel " + it->first + "is interdicted");
            }
        }
    }
    catch(Basda::DevException e)
    {
        E_LLOG("Caught exception in wheels.moveHome. Maybe a dev is in DUMMY configuration?");
        // E_LLOG("Caught exception in wheels.moveHome. Maybe a dev is DUMMY?");
        U8_LLOG("");
        throw(e);
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
}


/** @brief      Waits all the wheels to reach reference position
  * @param[in]  _time  The time out  */
void multiWheel::moveToReferenceWait(const Nice::Time& _time)
{
    U6_LLOG(__FUNCTION__);
    std::map<std::string, Basda::MoccaWheelServiceWorkerInterfacePtr>::const_iterator it;
    su::Timer timer(_time);
    for(it = m_mapWheels.begin(); it != m_mapWheels.end(); ++it)
    {
        try
        {
            int WAIT_RESULT_TIMED_OUT = 0;
            if(it->second->moveToHomeWait(timer.residual()) == WAIT_RESULT_TIMED_OUT)
            {
                std::stringstream log_msg;
                log_msg << __FUNCTION__ << suc::tilde << it->first << suc::tilde << "Error : wait timed out";
                throw(std::runtime_error(log_msg.str()));
            }
        }
        catch(Basda::DevException e)
        {
            E_LLOG(__FUNCTION__ << suc::tilde << it->first << suc::tilde << "Caught exception in wheels.moveHome. Maybe a dev is in DUMMY configuration?");
            U8_LLOG("");
            throw(e);
        }
        catch(...)
        {
            E_LLOG(__FUNCTION__ << suc::tilde << it->first);
            U8_LLOG("");
            throw;
        }
    }
    U8_LLOG("");
}


/** @brief      Move a subset of devices to reference position
 *  Waits for each device to receive the reference position. No time out is foreseen
 *  @param[in]  _vecDevName  A std vector containing device names */
void multiWheel::moveDeviceToReference(const std::vector<std::string>& _vecDevName)
{
    U6_LLOG(__FUNCTION__);
    try
    {
        moveDeviceToReferenceStart(_vecDevName);
        moveDeviceToReferenceWait(_vecDevName, Nice::Time::seconds(80));
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
}


/** @brief      Start motion of a subset of devices to reference position
  * @param[in]  _vecDevName  A std vector containing device names */
void multiWheel::moveDeviceToReferenceStart(const std::vector<std::string>& _vecDevName)
{
    U6_LLOG(__FUNCTION__);
    std::vector<std::string>::const_iterator it;
    for(it = _vecDevName.begin(); it < _vecDevName.end(); ++it)
    {
        std::map<std::string, WheelDevice>::const_iterator itWheel;
        itWheel = m_MapWheels.find(*it);
        if(itWheel != m_MapWheels.end())
        {
            if(!itWheel->second.CheckInterdiction())
                m_mapWheels[*it]->moveToHomeStart();
            else
            {
                E_LLOG(__FUNCTION__ << suc::tilde << *it << suc::tilde << "Interdicted");
                U8_LLOG("")
                throw WheelInterdiction("Wheel " + *it + "is interdicted");
            }
        }
        else
        {
            E_LLOG(__FUNCTION__ << suc::tilde << *it << suc::tilde << "Not Found");
            U8_LLOG("")
            throw std::invalid_argument("Wheel " + *it +  "not found");
        }
    }
    U8_LLOG("");
}


/** @brief      Waits a subset of devices to reach reference position
  * @param[in]  _vecDevName  A std vector containing device names
  * @param[in]  _time        The time out */
void multiWheel::moveDeviceToReferenceWait(const std::vector<std::string>& _vecDevName, const Nice::Time& _time)
{
    U6_LLOG(__FUNCTION__);
    std::vector<std::string>::const_iterator it;
    su::Timer timer(_time);
    for(it = _vecDevName.begin(); it < _vecDevName.end(); ++it)
    {
        std::map<std::string, Basda::MoccaWheelServiceWorkerInterfacePtr>::const_iterator itFind = m_mapWheels.find(*it);
        if(itFind != m_mapWheels.end())
        {
            try
            {
                int WAIT_RESULT_TIMED_OUT = 0;
                if(itFind->second->moveToHomeWait(timer.residual()) == WAIT_RESULT_TIMED_OUT)
                {
                    std::stringstream log_msg;
                    log_msg << __FUNCTION__ << suc::tilde << *it << suc::tilde << "Error : wait timed out";
                    throw(std::runtime_error(log_msg.str()));
                }
            }
            catch(...)
            {
                E_LLOG(__FUNCTION__ << suc::tilde << itFind->first);
                U8_LLOG("");
                throw;
            }
        }
        else
        {
            std::stringstream msg;
            msg << "Error invalid argument : " << *it;
            E_LLOG(msg.str());
            throw(std::invalid_argument(msg.str()));
        }
        // print wheel position to FITS header
    }
    U8_LLOG("");
}


// moveToNamedPosition
// INPUT map of device as key and named positions as value, expressed as integer index

/** @brief      Move devices to named position.
  * Waits for each device to reach the named position
  * @param[in]  _mapNamedPos  The map associating device to named position  */
void multiWheel::moveToNamedPosition(const std::map<std::string, int> &_mapNamedPos)
{
    U6_LLOG(__FUNCTION__);
    try
    {
        moveToNamedPositionStart(_mapNamedPos);
        moveToNamedPositionWait(_mapNamedPos, Nice::Time::seconds(80));
    }
    catch(Basda::ServiceIsNotOnlineException &e)
    {
        // E_LLOG(e.what());
        E_LLOG(std::string(__FUNCTION__) + ":" + e.what());
        U8_LLOG("");
        throw e;
    }
    catch(WheelInterdiction &e)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw(e);
    }
    catch(std::exception &e)
    {
        // E_LLOG(e.what());
        E_LLOG(std::string(__FUNCTION__) + ":" + e.what());
        U8_LLOG("");
        throw(e);
    }
    U8_LLOG("");
}


/** @brief      Start motion of devices to named position
  * does not wait for devices to reach position
  * @param[in]  _mapNamedPos  The map associating device to named position */
void multiWheel::moveToNamedPositionStart(const std::map<std::string, int> &_mapNamedPos)
{
    U6_LLOG(__FUNCTION__ << suc::tilde << "_mapNamedPos");
    // strPropName = "STATE";
    // install_callback_on_state(m_strCallbackNameState, strPropName);
    std::map<std::string, int>::const_iterator it;
    for(it = _mapNamedPos.begin(); it != _mapNamedPos.end(); ++it)
    {
        std::map<std::string, WheelDevice>::const_iterator itWheel = m_MapWheels.find(it->first);
        if(itWheel != m_MapWheels.end())
        {
            try
            {
                if(!itWheel->second.CheckInterdiction())
                {
                    U3_LLOG(it->first << " Moving to named position " << it->second);
                    itWheel->second.iPtr->moveToNamedPositionStart(it->second);
                }
                else
                {
                    E_LLOG(__FUNCTION__ << " " << it->first << suc::tilde << "Interdicted");
                    U8_LLOG("")
                    throw WheelInterdiction("Wheel " + it->first + " is interdicted");
                }
            }
            catch(Basda::ServiceIsNotOnlineException &e)
            {
                E_LLOG(e.what());
                U8_LLOG("");
                throw e;
            }
        }
        else
        {
            E_LLOG(__FUNCTION__ << " " << it->first << suc::tilde << "Not found");
            U8_LLOG("")
            throw std::invalid_argument("Wheel " + it->first + " not found");
        }
    }
    U8_LLOG("");
}


/** @brief      Wait for each device to reach the named position
  * @param[in]  _mapNamedPos  The map associating device to named position
  * @param[in]  _time         The time out  */
void multiWheel::moveToNamedPositionWait(const std::map<std::string, int> &_mapNamedPos, const Nice::Time &_time)
{
//    U6_LLOG(__FUNCTION__);
    std::map<std::string, int>::const_iterator it;
    su::Timer timer(_time);
    for(it = _mapNamedPos.begin(); it != _mapNamedPos.end(); ++it)
    {
        try
        {
            int WAIT_RESULT_TIMED_OUT = 0;
            if(m_mapWheels[it->first]->moveToNamedPositionWait(timer.residual()) == WAIT_RESULT_TIMED_OUT)
            {
                std::stringstream log_msg;
                log_msg << __FUNCTION__ << suc::tilde << it->first << suc::tilde << "Error : wait timed out";
                throw(std::runtime_error(log_msg.str()));
            }
        }
        catch(Basda::ServiceIsNotOnlineException &e)
        {
            E_LLOG(it->first << suc::tilde << e.what());
//            U8_LLOG("");
            throw e;
        }
        catch(std::exception e)
        {
            E_LLOG(it->first << " Exception " << e.what());
//            U8_LLOG("");
            throw e;
        }
        catch(...)
        {
            E_LLOG(__FUNCTION__ << suc::tilde << it->first);
//            U8_LLOG("");
            throw;
        }
    }
//    U8_LLOG("");
}

/** @brief      Move devices to absolute position
  * Waits for each device to receive the position. The position is expressed in steps as default unit, it can be changed
  * @param      _mapPos  The map associating device to absolute position
  * @param[in]  _unit    The unit in which the absolute position is expressed. Must be supported by the BASDA device */
void multiWheel::moveToAbsolutePosition(const std::map<std::string, double> &_mapPos, const std::map<std::string, std::string> &_mapUnit)
{
    U6_LLOG(__FUNCTION__);
    try
    {
        moveToAbsolutePositionStart(_mapPos, _mapUnit);
        moveToAbsolutePositionWait(_mapPos, Nice::Time::seconds(80));
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
}


/** @brief      Start motion of devices to absolute position
  * Does not wait for motion completion. The position is expressed in steps as default unit, it can be changed via the _unit parameter
  * @param      _mapPos  The map associating device to absolute position
  * @param[in]  _unit    The unit */
void multiWheel::moveToAbsolutePositionStart(const std::map<std::string, double> &_mapPos, const std::map<std::string, std::string> &_mapUnit)
{
//    U6_LLOG(__FUNCTION__);
//    std::string strPropName = "CMD.MOVEABSOLUTE.EXEC";
    std::string unit = "";
    double newPosition = 0.;
//    double steps2Deg = 1.81316e-4;
//    int fullTurn = 1985482;
    // float limit = 180.;

//    install_callback_on_completion(m_strCallbackName, strPropName);

    std::map<std::string, boost::function<bool()> >::iterator it_interdict;
    std::map<std::string, std::string>::const_iterator itPair;
    std::map<std::string, double>::const_iterator it;
    for(it = _mapPos.begin(); it != _mapPos.end(); ++it)
    {  //for
        U3_LLOG("Device is " << it->first);
        newPosition = it->second;
        // setting the unit
        if(_mapUnit.find(it->first) != _mapUnit.end())
        {
            itPair = _mapUnit.find(it->first);
            unit = boost::to_upper_copy(itPair->second);
            if(unit == "DEFAULT")unit = "STEPS";
            U3_LLOG("unit is " << itPair->second);
        }
        else
        {
            unit = "STEPS";
        }

        // Position to move must not be >= 180 deg (178 deg for apodizer wheel).
        // If so a negative value must be used, to reach the position from the left.
        // setting the threshold according to device
#if 0
      if(boost::algorithm::contains(it->first, "APO"))
        {
          limit = 178.;
        }
      else
        {
          limit = 180.;
        }

      //checking if desired position is below or above the threshold
      if((unit == "STEPS") || unit == "")
        {//if
          try
            {//try
              if(it->second >= round(limit/steps2Deg))
                {
                  newPosition = fullTurn - round(limit/steps2Deg);
                  // limit = 178.;
                }
              else
                {
                  newPosition = it->second;
                }
            }
          catch(Nice::Exception e)
            {
              E_LLOG(e.toString());
            }
          catch(std::exception e)
            {
              E_LLOG("WRONG ACCESS TO PROPERTY TREE" << ":" << e.what());
//              U8_LLOG("");
              throw(e);
            }
        }
#endif
#if 0
      if(unit == "DEGREE")
        {
          // check and adjust if position > 178
          if(it->second >= limit)
            {
              newPosition = it->second - 360;
              U3_LLOG("Position set to " << newPosition << "deg");
            }
          else
            {
              newPosition = it->second; 
            }
        }
#endif
        // instruction for motion

        std::map<std::string, WheelDevice>::const_iterator itWheel = m_MapWheels.find(it->first);
        if(itWheel != m_MapWheels.end())
        {
            if(!itWheel->second.CheckInterdiction())
            {
                U3_LLOG("moving " << it->first << " to " << newPosition << " " << unit);
                itWheel->second.iPtr->moveAbsoluteStart(newPosition, unit);
            }
            else
            {
                E_LLOG(__FUNCTION__ << suc::tilde <<  it->first << suc::tilde << "Interdicted");
//                U8_LLOG("");
                throw WheelInterdiction("Wheel " + it->first + " is interdicted");
            }
        }
        else
        {
            E_LLOG(__FUNCTION__ << suc::tilde << it->first << suc::tilde << "Not found");
//            U8_LLOG("");
            throw std::invalid_argument("Wheel " + it->first + " not found");
        }
    }
//    U8_LLOG("");
}


/** @brief      Waits for devices to reach absolute position.
  * The position is expressed in wathever unit.
  * @param      _mapPos  The map associating device to absolute position
  * @param[in]  _time    The time out  */
void multiWheel::moveToAbsolutePositionWait(const std::map<std::string, double> &_mapPos, const Nice::Time &_time)
{
//    U6_LLOG(__FUNCTION__ << suc::tilde << _time << suc::tilde << _mapPos.size());
    std::map<std::string, double>::const_iterator it;
    su::Timer timer(_time);
    for(it = _mapPos.begin(); it != _mapPos.end(); ++it)
    {
        try
        {
            U9_LOG(it->first << suc::tilde << it->second << suc::tilde << m_mapWheels[it->first]);
//            U9_LOG(it->first << suc::tilde << it->second << suc::tilde << m_mapWheels[it->first] << suc::tilde << m_mapWheels[it->first]->isReachable());
            int WAIT_RESULT_TIMED_OUT = 0;
            if(m_mapWheels[it->first]->moveAbsoluteWait(timer.residual()) == WAIT_RESULT_TIMED_OUT)
            {
                std::stringstream log_msg;
                log_msg << __FUNCTION__ << suc::tilde << it->first << suc::tilde << "Error : wait timed out; Device isReachable =  " << m_mapWheels[it->first]->isReachable();
                throw(std::runtime_error(log_msg.str()));
            }
        }
        catch(...)
        {
            E_LLOG(__FUNCTION__ << suc::tilde << it->first);
//            U8_LLOG("");
            throw;
        }
    }
//    U8_LLOG(__FUNCTION__);
}


void multiWheel::moveToRelativePosition(const std::map<std::string, double>& _mapPos, const std::map<std::string, std::string>& _mapUnit)
{
    U6_LLOG(__FUNCTION__);
    try
    {
        moveToRelativePositionStart(_mapPos, _mapUnit);
        moveToRelativePositionWait(_mapPos, Nice::Time::seconds(80));
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
}


void multiWheel::moveToRelativePositionStart(const std::map<std::string, double> &_mapPos, const std::map<std::string, std::string> &_mapUnit)
{
//    U6_LLOG(__FUNCTION__);
//    std::string strPropName = "CMD.MOVERELATIVE.EXEC";
    std::string unit = "";
    double newPosition = 0.;
    double endPosition = 0.;
    int pos = 0;
    int equivPos = 0;
    double steps2Deg = 1.81316e-4;
    int fullTurn = 1985482;
//    float limit = 180.;

//    install_callback_on_completion(m_strCallbackName, strPropName);

    std::map<std::string, boost::function<bool()> >::iterator it_interdict;
    std::map<std::string, double>::const_iterator it;
    for(it = _mapPos.begin(); it != _mapPos.end(); ++it)
    {
        U3_LLOG("Device is " << it->first);

        // setting the unit
        if(_mapUnit.find(it->first) != _mapUnit.end())
        {
            std::map<std::string, std::string>::const_iterator itPair;
            itPair = _mapUnit.find(it->first);
            unit = boost::to_upper_copy(itPair->second);
            U3_LLOG(__FUNCTION__ << "unit for " << it->first << " is : " << itPair->second);
        }
        else
        {
            unit = "STEPS";
        }

        // check if final position will be > full turn and adjust to avoid wheel travel more then a full turn
        // since getPosition returns position in STEPS, even passing the `unit` parameter, position is converted to STEPS
        U3_LLOG("check if final position will be > full turn and adjust to avoid wheel travel more then a full turn TBD");
        pos = m_mapWheels[it->first]->getPosition(unit);
        if(unit == "DEGREE")
        {
            endPosition = pos + it->second / steps2Deg;
        }
        else
        {
            endPosition = pos + it->second;
        }

        if(endPosition > fullTurn)
        {
            U3_LLOG(
                "Final position " << endPosition*steps2Deg << " DEGREE would be greater then allowed travel range, correcting ... ");
            equivPos = endPosition - fullTurn;
            newPosition = equivPos - pos;
        }
        else
            newPosition = it->second;
        U3_LLOG("Final position will be " << newPosition*steps2Deg << " DEGREE");

        // instruction for motion
        std::map<std::string, WheelDevice>::const_iterator itWheel = m_MapWheels.find(it->first);
        if(itWheel != m_MapWheels.end())
        {
            if(!itWheel->second.CheckInterdiction())
            {
                U3_LLOG("moving " << it->first << " to " << newPosition*steps2Deg << " " << unit);
                itWheel->second.iPtr->moveRelativeStart(newPosition, unit);
            }
            else
            {
                E_LLOG(__FUNCTION__ << suc::tilde << it->first << suc::tilde << "Interdicted");
//                U8_LLOG("")
                throw WheelInterdiction("Wheel " + it->first + " is interdicted");
            }
        }
        else
        {
            E_LLOG(__FUNCTION__ << suc::tilde << it->first << suc::tilde << "Not found");
//            U8_LLOG("")
            throw std::invalid_argument("Wheel " + it->first + " not found");
        }
    }
//    U8_LLOG("");
}


void multiWheel::moveToRelativePositionWait(const std::map<std::string, double> &_mapPos, const Nice::Time &_time)
{
//    U6_LLOG(__FUNCTION__);
    std::map<std::string, double>::const_iterator it;
    su::Timer timer(_time);
    for(it = _mapPos.begin(); it != _mapPos.end(); ++it)
    {
        try
        {
            int WAIT_RESULT_TIMED_OUT = 0;
            if(m_mapWheels[it->first]->moveRelativeWait(timer.residual()) == WAIT_RESULT_TIMED_OUT)
            {
                std::stringstream log_msg;
                log_msg << __FUNCTION__ << suc::tilde << it->first << suc::tilde << "Error : wait timed out";
                throw(std::runtime_error(log_msg.str()));
            }
        }
        catch(...)
        {
            E_LLOG(__FUNCTION__ << suc::tilde << it->first);
//            U8_LLOG("");
            throw;
        }
    }
//    U8_LLOG("");
}


/** @brief      Determines if the input device is part of the object
  * @param[in]  _strDevAlias  The device alias
  * @return     True if device, False otherwise. */
bool multiWheel::isDevice(const std::string& _strDevAlias)
{
    bool boolRet;

    if(m_mapWheels.find(boost::to_upper_copy(_strDevAlias)) != m_mapWheels.end())
        boolRet = true;
    else
        boolRet = false;
//    U6_LLOG("multiWheel::" << __FUNCTION__ << suc::tilde << _strDevAlias << " returned : " << boolRet);
//    U8_LLOG("");
    return boolRet;
}


/**
 * @brief      Sets the callback function.
 *
 * This function is used by superobject motionCtrl
 *
 * @param      _cbfn       The callback boost function objecy
 * @param[in]  _strFnName  The string function name  */
void multiWheel::set_callback_fn(boost::function<void(const Nice::Completion&)> & _cbfn, const std::string & _strFnName)
{
    U6_LLOG(__FUNCTION__);
    m_callbackFn = _cbfn;
    m_strCallbackName = _strFnName;
    std::string strPropName;
    strPropName = "CMD.MOVETONAMEDPOSITION.EXEC";
    install_callback_on_completion(m_strCallbackName, strPropName);
    strPropName = "CMD.MOVEABSOLUTE.EXEC";
    install_callback_on_completion(m_strCallbackName, strPropName);
    strPropName = "CMD.MOVERELATIVE.EXEC";
    install_callback_on_completion(m_strCallbackName, strPropName);
    strPropName = "CMD.MOVETOHOME.EXEC";
    install_callback_on_completion(m_strCallbackName, strPropName);
//    strPropName = "CMD.MOVETOLIMIT.EXEC";
//    install_callback_on_completion(m_strCallbackName, strPropName);
    try
    {
        strPropName = "CMD.SCANALLREFERENCESWITCHES.EXEC";
        install_callback_on_completion(m_strCallbackName, strPropName);
    }
    catch(...)
    {
        W_LLOG("Could not register for SCANALLREFERENCESWITCHES!");
    }

    U8_LLOG("");
}


/**
 * @brief      Sets the callback function on the state.
 *
 * This function is used by superobject motionCtrl
 *
 * @param      _cbfn       The callback boost function object
 * @param[in]  _strFnName  The string function name
 */
void multiWheel::set_callback_fn_state(boost::function<void(const Basda::ServiceStateMachine&)>& _cbfn,
                                       const std::string& _strFnName)
{
    U6_LLOG(__FUNCTION__ << " Set the callback function state");

  // m_callbackFnState = _cbfn;
  // m_strCallbackNameState = _strFnName;
    U8_LLOG("");
}


/**
 * @brief      Sets the callback function state.
 *
 * @param      _cbfn         The cbfn
 * @param[in]  _strDevAlias  The string development alias
 */
void multiWheel::set_callback_fn_state(boost::function<void(const Basda::ServiceStateMachine&,
                                                            const std::string&)>& _cbfn,
                                       const std::string& _strDevAlias)
{
    U6_LLOG(__FUNCTION__ << " Set the callback function state 2");

  // 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
    U8_LLOG("");
}

// This function is used by superobject motionCtrl

/**
 * @brief      Sets the callback function on the number of moving devices
 *
 * @param      _cbfn       The callback boost function object
 * @param[in]  _strFnName  The string function name
 */
void multiWheel::set_callback_fn_count(boost::function<void(const Nice::Completion&)>& _cbfn,
                                       const std::string& _strFnName)
{
    U6_LLOG(__FUNCTION__ << " Set the callback function on the number of moving devices");

    m_callbackFnCount = _cbfn;
    m_strCallbackNameCount = _strFnName;
    U8_LLOG("");
}


// --------------------------
// GETTERS
// --------------------------
//

/**
 * @brief      Gets the named position for the specified device DEPRECATED
 *
 * Get last occupied named position for each wheel. If input parameter is bigger the the number of wheels,
 * return last occupied position for all wheels, otherwise input is interpreted as wheel index in the
 * optical path.
 *
 * @param[in]  _wheelIdx  The wheel index
 *
 * @return     The named position.
 */


std::vector<std::string> multiWheel::getNamedPosition(const std::size_t _wheelIdx)
{
    std::vector<std::string> res;

    U6_LLOG(__FUNCTION__);

    if(_wheelIdx >= m_nDevices)
    {
      // return position for all wheels
        std::map<std::string, Basda::MoccaWheelServiceWorkerInterfacePtr>::iterator it;
        for(it = m_mapWheels.begin(); it != m_mapWheels.end(); ++it)
        {
            res.push_back(it->second->config()->leaf("VAR.NAMED_POSITION")->get<std::string>());
        }
    }
    else
    {
        std::map<std::string, Basda::MoccaWheelServiceWorkerInterfacePtr>::iterator it = m_mapWheels.begin();
        std::advance(it, _wheelIdx);
        res.push_back(it->second->config()->leaf("VAR.NAMED_POSITION")->get<std::string>());
        // BOOST_AUTO(it, m_vecWheels.begin()+_wheelIdx);
        // res.push_back((*it)->config()->leaf("VAR.NAMED_POSITION")->get<std::string>());
    }
    U8_LLOG("");
    return res;
}


// Get last occupied named position for each wheel

/**
 * @brief      Gets the las occupied named position for each wheel specified in the input parameter.
 *
 * @param[in]  _vecAlias  The vector of alias names
 *
 * @return     The named position.
 */
std::vector<std::string> multiWheel::getNamedPosition(const std::vector<std::string> &_vecAlias)
{
    U6_LLOG(__FUNCTION__);
    std::vector<std::string> vecRes;
    std::vector<std::string>::const_iterator itPos = _vecAlias.begin();

    std::map<std::string, Basda::MoccaWheelServiceWorkerInterfacePtr>::iterator it;
    for(it = m_mapWheels.begin(); it != m_mapWheels.end(); ++it)
    {
        // idx = std::distance(it, m_mapWheels.begin());
        // itPos = _vecAlias.begin() + idx;
        if(m_mapWheels.find(*itPos) != m_mapWheels.end())
        {
            vecRes.push_back(it->second->config()->leaf("VAR.NAMED_POSITION")->get<std::string>());
            std::advance(itPos, 1);
        }
        else
        {
            E_LLOG(it->first << ":Wrong alias name for device");
            U8_LLOG("");
            throw std::invalid_argument("Wrong alias name for device");
        }
    }
    U8_LLOG("");
    return vecRes;
}


std::map<std::string, std::string> multiWheel::getNamedPosition2(const std::vector<std::string> &_vecAlias)
{
    U6_LLOG(__FUNCTION__);
    std::map<std::string, std::string> mapRes;
    std::vector<std::string>::const_iterator itPos;

    std::map<std::string, Basda::MoccaWheelServiceWorkerInterfacePtr>::const_iterator it;
    for(itPos = _vecAlias.begin(); itPos < _vecAlias.end(); ++itPos)
    {
        it = m_mapWheels.find(*itPos);
        if(it != m_mapWheels.end())
        {
            mapRes[*itPos] = it->second->config()->leaf("VAR.NAMED_POSITION")->get<std::string>();
        }
        else
        {
//            E_LLOG("Wrong alias name for device");
//            U8_LLOG("");
//            throw std::invalid_argument("Wrong alias name for device");
        }
    }
    U8_LLOG("");
    return mapRes;
}


void multiWheel::getPositionPair(std::pair<std::string, Ice::Double>& _pairPosition, const std::string& _unit)
{
    U6_LLOG(__FUNCTION__);
    if(isDevice(_pairPosition.first))
    {
        _pairPosition.second = m_mapWheels[_pairPosition.first]->getPosition();
    }
    U8_LLOG("");
}


Ice::Int multiWheel::getPosition(std::map<std::string, Ice::Double> &_mapPosition, const std::string &_unit)
{
//    U6_LLOG(__FUNCTION__);
    // W_LLOG("Provide implementation for getPosition");
    Ice::Int status = EXIT_SUCCESS;
    // ...
    // std::for_each(_mapPosition.begin(), _mapPosition.end(),
    //               boost::bind(&multiWheel::getPositionPair, _1, boost::cref(_unit)));
    std::map<std::string, Ice::Double>::iterator it;
    for(it = _mapPosition.begin(); it != _mapPosition.end(); ++it)
    {
        if(isDevice(it->first))
        {
//            if !(m_mapWheels[it->first]->isReachable())
            {
//                m_mapWheels[it->first]->connect(it->first + ".CON"), m_config->node(it->first);
                // or
                // m_mapWheels[it->first]->init();
            }
            _mapPosition[it->first] = m_mapWheels[it->first]->getPosition();
        }
    }
//    U8_LLOG(__FUNCTION__);
    return status;
}


/**
 * @brief      Gets the device pointer.
 *
 * @param[in]  _strDeviceName  The string device name
 *
 * @return     The device pointer.
 */
Basda::MoccaWheelServiceWorkerInterfacePtr multiWheel::getDevicePtr(const std::string& _strDeviceName)
{
    U6_LLOG(__FUNCTION__);
    std::map<std::string, Basda::MoccaWheelServiceWorkerInterfacePtr>::iterator itFind;
    itFind = m_mapWheels.find(_strDeviceName);

    if(itFind == m_mapWheels.end())
    {
        E_LLOG("Wrong alias name for device");
        U8_LLOG("");
        throw std::invalid_argument("Wrong alias name for device");
    }
    U8_LLOG("");
    return itFind->second;
}


/**
 * @brief      Abort motion of all devices
 *
 * emergency method.
 */
void multiWheel::emergencyAbort()
{
    U6_LLOG(__FUNCTION__);

    std::map<std::string, Basda::MoccaWheelServiceWorkerInterfacePtr>::iterator it;
    for(it = m_mapWheels.begin(); it != m_mapWheels.end(); ++it)
        it->second->abort();
    U8_LLOG("");
}


/**
 * @brief      Stop motion of all devices
 *
 * @return     { description_of_the_return_value }
 */
int multiWheel::stopAll()
{
    U6_LLOG(__FUNCTION__);
    int status = EXIT_SUCCESS;

    try
    {
        std::map<std::string, Basda::MoccaWheelServiceWorkerInterfacePtr>::iterator it;
        for(it = m_mapWheels.begin(); it != m_mapWheels.end(); ++it)
        {
            it->second->stop();
            W_LLOG(it->first + " Stopped");
        }
    }
    catch(Nice::Exception err)
    {
        E_LLOG(err.what());
        U8_LLOG("");
        status = EXIT_FAILURE;
        throw(err);
    }
    U8_LLOG("");
    return status;
}


int multiWheel::stop(const std::vector<std::string> &_vecDevAlias)
{
    U6_LLOG(__FUNCTION__);
    W_LLOG(__FUNCTION__ << " Stopping motion of selected wheels");
    int status = EXIT_SUCCESS;
    std::vector<std::string>::const_iterator it;
    try
    {
        for(it = _vecDevAlias.begin(); it != _vecDevAlias.end(); ++it)
        {
            m_mapWheels[(*it)]->stop();
        }
    }
    catch(Nice::Exception err)
    {
        E_LLOG(err.what());
        U8_LLOG("");
        status = EXIT_FAILURE;
        throw err;
    }
    U8_LLOG("");
    return status;
}



int multiWheel::abort(const std::vector<std::string>& _vecDevAlias)
{
    U6_LLOG(__FUNCTION__);
    int status = EXIT_SUCCESS;
    try
    {
        std::vector<std::string>::const_iterator it;
        for(it = _vecDevAlias.begin(); it != _vecDevAlias.end(); ++it)
        {
            m_mapWheels[(*it)]->abort();
        }
    }
    catch(Nice::Exception err)
    {
        E_LLOG(err.what());
        U8_LLOG("");
        status = EXIT_FAILURE;
        throw err;
    }
    U8_LLOG("");
    return status;
}

/**
 * @brief      ... Not clear, received from F. Briegel
 */
void inAppConfig_DontDoThat()
{
    U6_LLOG(__FUNCTION__);
  /* 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);
    U8_LLOG("");
}

