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

#include <boost/exception/diagnostic_information.hpp>


#include "shins_util.h"
#include "multiStageX.hpp"

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


void StageXDevice::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 StageXDevice::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<double>()));
        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") << " - " << (int)nodechild->get<double>());
    }
    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 << "]");
    }
}


multiStageX::multiStageX(const std::string & _service):
    multiDevice(std::vector<std::string>(), typeid(this).name())
{
    U6_LLOG(__FUNCTION__ << " - constructor");
    m_service = _service;

//    Basda::MoccaXServiceWorkerInterfacePtr tmpStageX;

//     for(std::size_t i = 0; i < m_nDevices; i++)
//     {
//         m_vecStageX.push_back(tmpStageX);
//     }
//     std::stringstream log_message;
//     log_message << " - Alias Names: ";
//     std::vector<std::string>::const_iterator it;
//     for(it = _vecAliasName.begin(); it < _vecAliasName.end(); ++it)
//     {
//         std::string to_upper_it = su::str_toupper(*it);
//         log_message << *it << "; ";
//         m_mapStageX.insert(std::pair<std::string, Basda::MoccaXServiceWorkerInterfacePtr>(boost::to_upper_copy(*it), tmpStageX));
//         std::string cfgFile = "";
//         std::map<std::string, std::string>::const_iterator itFind = _mapCfgFiles.find(to_upper_it);
//         if(itFind != _mapCfgFiles.end())cfgFile = itFind->second;
//         m_MapStageX.insert(std::pair<std::string, StageXDevice>(to_upper_it, StageXDevice(cfgFile)));
//         A_LLOG(__FUNCTION__ << " ~ " << to_upper_it << " ~ " << m_MapStageX.find(to_upper_it)->second.alias);
//         std::map<std::string, StageXDevice>::iterator itFind2 = m_MapStageX.find(to_upper_it);
//         assert(itFind2 != m_MapStageX.end() && itFind2->second.alias == to_upper_it);
//    }

    U8_LLOG("");
}


multiStageX::~multiStageX()
{
    U6_LLOG(__FUNCTION__);
    U8_LLOG("");
}


void multiStageX::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, StageXDevice>::iterator itFind;
    itFind = m_MapStageX.find(alias);
    if(itFind != m_MapStageX.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("");
}


void multiStageX::install_callback(const std::string &_strCallbackName, const std::string &_strPropName)
{
    U6_LLOG("multiStageX::"  << __FUNCTION__ << " ~ " << _strCallbackName << " ~  " << _strPropName);
    Nice::PropertyPtr prop;
    std::string bridgeName;
    std::map<std::string, Basda::MoccaXServiceWorkerInterfacePtr>::const_iterator it;
    void (StageXDevice::* Bridge)(const Nice::Completion &)  = NULL;
    if(_strPropName == "CMD.MOVETOHOME.EXEC")
    {
        Bridge = &StageXDevice::MoveToHomeBridge;
    }
    else if(_strPropName == "CMD.MOVETONAMEDPOSITION.EXEC")
    {
        Bridge = &StageXDevice::MoveToNamedBridge;
    }
    else if(_strPropName == "CMD.MOVEABSOLUTE.EXEC")
    {
        Bridge = &StageXDevice::MoveAbsoluteBridge;
    }
    else if(_strPropName == "CMD.MOVERELATIVE.EXEC")
    {
        Bridge = &StageXDevice::MoveRelativeBridge;
    }
    else if(_strPropName == "CMD.MOVETOLIMIT.EXEC")
    {
        Bridge = &StageXDevice::MoveToLimitBridge;
    }
    for(it = m_mapStageX.begin(); it != m_mapStageX.end(); ++it)
    {
//        bridgePropPtr = m_drot->config()->leaf(strPropertyName);
        if(it->second)
        {
            bridgeName = _strCallbackName + "-" + it->first + "-" + m_service + "-";
            prop = it->second->config()->leaf(_strPropName);
            if(prop->getBridge(bridgeName + "count"))
            {
                U9_LLOG( it->first << " -- Bridge Removed for" << _strPropName);
                prop->removeBridge(bridgeName + "count");
            }
            std::map<std::string, StageXDevice>::iterator itFind = m_MapStageX.find(it->first);
            prop->applyBridge(
    //            new Nice::PropertyBridgeMemFn<Nice::Completion, multiStageX, boost::function<void(const Nice::Completion&)> multiStageX::*>(
    //                this, &multiStageX::m_callbackFnCount, prop->id()),
    //            new Nice::PropertyBridgeMemFn<Nice::Completion, multiStageX>(this, &multiStageX::callbackCompletion2, prop->id()),
    //                new Nice::PropertyBridgeFn<Nice::Completion >(m_callbackFnCount),
    //            bridgeName + "count");
    //        prop->applyBridge(

                new Nice::PropertyBridgeMemFn<Nice::Completion, StageXDevice, void (StageXDevice::*)(const Nice::Completion &)>(
                // new Nice::PropertyBridgeDispatchedMemFn<Nice::Completion, StageXDevice, void (StageXDevice::*)(const Nice::Completion &)>(
                    &itFind->second, Bridge, prop->id()),
                    bridgeName + "count");
            U9_LLOG( it->first << " -- " << bridgeName << "count ~ " << Bridge);
        }
        else
        {
            std::stringstream log_msg;
            log_msg << "Cannot install callback " << _strPropName << " to " << it->first << " device is unavailable";
            W_LLOG(log_msg.str());
//            U8_LLOG("");
//            throw(std::runtime_error(log_msg.str()));
        }
    }
    U8_LLOG("");
}


std::string multiStageX::getEndPointName(const std::size_t _stageIdx)
{
    U6_LLOG(__FUNCTION__);

    if(_stageIdx > m_vecStageX.size())
    {
        E_LLOG(" range error");
        U8_LLOG("");
        throw std::range_error("range error");
    }
    U8_LLOG("");
    return (getBaseName(_stageIdx) + ".CON");
}


std::string multiStageX::getBaseName(const std::size_t _stageIdx)
{
    U6_LLOG(__FUNCTION__);
    if(_stageIdx > m_vecStageX.size())
    {
        E_LLOG(" out of range");
        U8_LLOG("");
        throw std::out_of_range("out of range");
    }
    else
    {
        U8_LLOG("");
        return *(m_vecAliasName.begin() + _stageIdx);
    }
}


void multiStageX::connect()
{
    U6_LLOG(__FUNCTION__);

    Nice::JointPtr nodestagexs = Nice::Application::config()-> node(m_service + "." + "STAGEX");
    Nice::SeqString StageXChildren;
    nodestagexs->children(StageXChildren);

    Nice::SeqString::iterator itSeq;
    Basda::MoccaXServiceWorkerInterfacePtr iPtr;
    m_vecAliasName = StageXChildren;
    for(itSeq = StageXChildren.begin(); itSeq < StageXChildren.end(); itSeq++)
    {
        try
        {
            Nice::JointPtr nodestagex = nodestagexs->node(*itSeq + ".SVC");
            iPtr = Basda::MoccaXServiceWorkerInterface::create(nodestagex);
        }
        catch (const IceUtil::NullHandleException & e){ A_LLOG(e.what()); }
        catch(...){E_LLOG(__FUNCTION__)};
        m_mapStageX.insert(std::pair<std::string, Basda::MoccaXServiceWorkerInterfacePtr>(*itSeq, iPtr));
        m_MapStageX.insert(std::pair<std::string, StageXDevice>(*itSeq, StageXDevice(iPtr)));
        m_nDevices++;
    }

    U8_LLOG("");

    // std::map<std::string, Basda::MoccaXServiceWorkerInterfacePtr>::iterator it;
    // for(it = m_mapStageX.begin(); it != m_mapStageX.end(); ++it)
    // {
    //     it->second = NULL;
    //     try
    //     {
    //         Nice::JointPtr nodebeam = Nice::Application::config()->node(m_service + ".STAGEX." + it->first + ".SVC");
    //         it->second = Basda::MoccaXServiceWorkerInterface::create(nodebeam);
    //     }
    //     catch (const IceUtil::NullHandleException & e)
    //     {
    //         A_LLOG(e.what());
    //     }
    //     m_MapStageX.find(it->first)->second.SetInterfacePointer(it->second);
    // }
}


void multiStageX::standby(const std::vector<std::string> &_vecDevAlias)
{
    U6_LLOG(__FUNCTION__);
    Nice::Time timeout = Nice::Time::seconds(2);
    int change_state_result;
    if(!_vecDevAlias.empty())
    {
        std::vector<std::string>::const_iterator it;
        for(it = _vecDevAlias.begin(); it != _vecDevAlias.end(); ++it)
        {
            std::map<std::string, Basda::MoccaXServiceWorkerInterfacePtr>::const_iterator itFind;
            itFind = m_mapStageX.find(*it);
            if(itFind != m_mapStageX.end() && itFind->second)
            {
                try
                {
                    if(itFind->second->state() == Basda::WORKING)
                        itFind->second->stop();
                    if(itFind->second->state() != Basda::STANDBY)
                    {
                        change_state_result = itFind->second->changeState(Basda::STANDBY, timeout);
                        if(change_state_result == 0)// timed out
                        {
                            W_LLOG(itFind->first << " change state request timed out.")
                        }
                        else { I_LLOG(itFind->first + " STANDBY"); }
                    }
                }
                catch (Basda::ServiceInitException &e)
                {
                    E_LLOG(e.what());
                }
            }
            else
            {
                std::stringstream log_msg;
                if(itFind == m_mapStageX.end())
                {
                    log_msg << __FUNCTION__  << " ~ Cannot find device " << *it;
                }
                else
                {
                    log_msg << __FUNCTION__ << " ~ Device " << *it << " is unavailable";
                }
                W_LLOG(log_msg.str());
            }
        }
    }
    else
    {
        std::map<std::string, Basda::MoccaXServiceWorkerInterfacePtr>::const_iterator it;
        for(it = m_mapStageX.begin(); it != m_mapStageX.end(); ++it)
        {
            // U6_LLOG(it->first << " state = " << it->second->state());
            if(it->second)
            {
                try
                {
                    if(it->second->state() == Basda::WORKING)
                        it->second->stop();
                    if(it->second->state() != Basda::STANDBY)
                    {
                        change_state_result = it->second->changeState(Basda::STANDBY, timeout);
                        if(change_state_result == 0)// timed out
                        {
                            W_LLOG(it->first << " change state request timed out.")
                        }
                        else { I_LLOG(it->first + " STANDBY"); }
                    }
                }
                catch (std::exception e)
                {
                    E_LLOG(e.what());
                    U8_LLOG("");
                    throw(e);
                    // E_LLOG(err.what());
                }
            }
            else
            {
                std::stringstream log_msg;
                log_msg << __FUNCTION__ << " ~ Device " << it->first << " is unavailable";
                W_LLOG(log_msg.str());
            }
        }
    }
    // U6_LLOG("State changed");
    U8_LLOG("");
}


void multiStageX::moveToPositiveLimit(const std::vector<std::string> &_vecDevAlias)
{
    U6_LLOG(__FUNCTION__);
    std::vector<std::string>::const_iterator it;
    for(it = _vecDevAlias.begin(); it != _vecDevAlias.end(); ++it)
    {
        std::map<std::string, Basda::MoccaXServiceWorkerInterfacePtr>::const_iterator itFind;
        itFind = m_mapStageX.find(*it);
        if(itFind != m_mapStageX.end() && itFind->second)
        {
            try
            {
                itFind->second->moveToLimit(1.);
            }
            catch (Basda::ServiceInitException &e)
            {
                E_LLOG(e.what());
                throw(e);
            }
        }
        else
        {
            std::stringstream log_msg;
            if(itFind == m_mapStageX.end())
            {
                log_msg << __FUNCTION__ << suc::tilde << itFind->first << " : Invalid device name";
            }
            else
            {
                log_msg << __FUNCTION__ << suc::tilde << itFind->first << " : Device is unavailable";
            }
            E_LLOG(log_msg.str());
            U8_LLOG("");
            throw std::invalid_argument(log_msg.str());
        }
    }
    U8_LLOG("");
}


void multiStageX::moveToPositiveLimitStart(const std::vector<std::string> &_vecDevAlias)
{
    U6_LLOG(__FUNCTION__);
    std::vector<std::string>::const_iterator it;
    for(it = _vecDevAlias.begin(); it != _vecDevAlias.end(); ++it)
    {
        std::map<std::string, Basda::MoccaXServiceWorkerInterfacePtr>::const_iterator itFind;
        itFind = m_mapStageX.find(*it);
        if(itFind != m_mapStageX.end() && itFind->second)
        {
            try
            {
                itFind->second->moveToLimitStart(1.);
            }
            catch (Basda::ServiceInitException &e)
            {
                E_LLOG(e.what());
                throw(e);
            }
        }
        else
        {
            std::stringstream log_msg;
            if(itFind == m_mapStageX.end())
            {
                log_msg << __FUNCTION__ << suc::tilde << itFind->first << " : Invalid device name";
            }
            else
            {
                log_msg << __FUNCTION__ << suc::tilde << itFind->first << " : Device is unavailable";
            }
            E_LLOG(log_msg.str());
            U8_LLOG("");
            throw std::invalid_argument(log_msg.str());
        }
    }
    U8_LLOG("");
}


void multiStageX::moveToPositiveLimitWait(const std::vector<std::string> &_vecDevAlias,const Nice::Time & timeout)
{
    U6_LLOG(__FUNCTION__);
    std::vector<std::string>::const_iterator it;
    su::Timer timer(timeout);
    for(it = _vecDevAlias.begin(); it != _vecDevAlias.end(); ++it)
    {
        std::map<std::string, Basda::MoccaXServiceWorkerInterfacePtr>::const_iterator itFind;
        itFind = m_mapStageX.find(*it);
        if(itFind != m_mapStageX.end() && itFind->second)
        {
            try
            {
                int WAIT_RESULT_TIMED_OUT = 0;
                if(itFind->second->moveToLimitWait(timer.residual()) == WAIT_RESULT_TIMED_OUT)
                {
                    std::stringstream log_msg;
                    log_msg << __FUNCTION__ << suc::tilde << itFind->first << suc::tilde << "Error : wait timed out";
                    throw(std::runtime_error(log_msg.str()));
                }
            }
            catch (Basda::ServiceInitException &e)
            {
                E_LLOG(e.what());
                U8_LLOG("");
                throw(e);
            }
            catch(...)
            {
                E_LLOG(__FUNCTION__);
                U8_LLOG("");
                throw;
            }
        }
        else
        {
            std::stringstream log_msg;
            if(itFind == m_mapStageX.end())
            {
                log_msg << __FUNCTION__ << suc::tilde << itFind->first << " : Invalid device name";
            }
            else
            {
                log_msg << __FUNCTION__ << suc::tilde << itFind->first << " : Device is unavailable";
            }
            E_LLOG(log_msg.str());
            U8_LLOG("");
            throw std::invalid_argument(log_msg.str());
        }
    }
    U8_LLOG("");
}


void multiStageX::moveToNegativeLimitStart(const std::vector<std::string> &_vecDevAlias)
{
    U6_LLOG(__FUNCTION__);
    std::vector<std::string>::const_iterator it;
    for(it = _vecDevAlias.begin(); it != _vecDevAlias.end(); ++it)
    {
        std::map<std::string, Basda::MoccaXServiceWorkerInterfacePtr>::const_iterator itFind;
        itFind = m_mapStageX.find(*it);
        if(itFind != m_mapStageX.end() && itFind->second)
        {
            try
            {
                itFind->second->moveToLimitStart(-1.);
            }
            catch (Basda::ServiceInitException &e)
            {
                E_LLOG(e.what());
                throw(e);
            }
        }
        else
        {
            std::stringstream log_msg;
            if(itFind == m_mapStageX.end())
            {
                log_msg << __FUNCTION__ << suc::tilde << itFind->first << " : Invalid device name";
            }
            else
            {
                log_msg << __FUNCTION__ << suc::tilde << itFind->first << " : Device is unavailable";
            }
            E_LLOG(log_msg.str());
            U8_LLOG("");
            throw std::invalid_argument(log_msg.str());
        }
    }
    U8_LLOG("");
}

void multiStageX::moveToNegativeLimitWait(const std::vector<std::string> &_vecDevAlias,const Nice::Time & timeout)
{
    U6_LLOG(__FUNCTION__);
    std::vector<std::string>::const_iterator it;
    su::Timer timer(timeout);
    for(it = _vecDevAlias.begin(); it != _vecDevAlias.end(); ++it)
    {
        std::map<std::string, Basda::MoccaXServiceWorkerInterfacePtr>::const_iterator itFind;
        itFind = m_mapStageX.find(*it);
        if(itFind != m_mapStageX.end() && itFind->second)
        {
            try
            {
                int WAIT_RESULT_TIMED_OUT = 0;
                if(itFind->second->moveToLimitWait(timer.residual()) == WAIT_RESULT_TIMED_OUT)
                {
                    std::stringstream log_msg;
                    log_msg << __FUNCTION__ << suc::tilde << itFind->first << suc::tilde << "Error : wait timed out";
                    throw(std::runtime_error(log_msg.str()));
                }
            }
            catch (Basda::ServiceInitException &e)
            {
                E_LLOG(e.what());
                throw(e);
            }
            catch(...)
            {
                E_LLOG(__FUNCTION__);
                U8_LLOG("");
                throw;
            }
        }
        else
        {
            std::stringstream log_msg;
            if(itFind == m_mapStageX.end())
            {
                log_msg << __FUNCTION__ << suc::tilde << itFind->first << " : Invalid device name";
            }
            else
            {
                log_msg << __FUNCTION__ << suc::tilde << itFind->first << " : Device is unavailable";
            }
            E_LLOG(log_msg.str());
            U8_LLOG("");
            throw std::invalid_argument(log_msg.str());
        }
    }
    U8_LLOG("");
}


Nice::SeqBitSet multiStageX::isAtLimit(const std::string &_devAlias)
{
    U6_LLOG(__FUNCTION__);
    std::map<std::string, Basda::MoccaXServiceWorkerInterfacePtr>::const_iterator itFind;
    itFind = m_mapStageX.find(_devAlias);
    Nice::SeqBitSet result;
    if(itFind != m_mapStageX.end() && itFind->second)
    {
        try
        {
            result = itFind->second->getPositionSwitchStatus();
        }
        catch (Basda::ServiceInitException &e)
        {
            E_LLOG(e.what());
            U8_LLOG("");
            throw(e);
        }
        catch(...)
        {
            E_LLOG(__FUNCTION__);
            U8_LLOG("");
            throw;
        }
    }
    else
    {
        std::stringstream log_msg;
        if(itFind == m_mapStageX.end())
        {
            log_msg << __FUNCTION__ << suc::tilde << itFind->first << " : Invalid device name";
        }
        else
        {
            log_msg << __FUNCTION__ << suc::tilde << itFind->first << " : Device is unavailable";
        }
        E_LLOG(log_msg.str());
        U8_LLOG("");
        throw std::invalid_argument(log_msg.str());
    }
    U8_LLOG("");
    return result;
}


void multiStageX::online(const std::vector<std::string> &_vecDevAlias)
{
    U6_LLOG(__FUNCTION__);
    Nice::Time timeout = Nice::Time::seconds(2);
    int change_state_result;
    if(!_vecDevAlias.empty())
    {
        std::vector<std::string>::const_iterator it;
        std::map<std::string, Basda::MoccaXServiceWorkerInterfacePtr>::const_iterator itFind;
        for(it = _vecDevAlias.begin(); it != _vecDevAlias.end(); ++it)
        {
            itFind = m_mapStageX.find(*it);
            if(itFind != m_mapStageX.end() && itFind->second)
            {
                try
                {
                    change_state_result = itFind->second->changeState(Basda::ONLINE, timeout);
                    if(change_state_result == 0)// timed out
                    {
                        W_LLOG(itFind->first << " change state request timed out.")
                    }
                    else { I_LLOG(itFind->first + " ONLINE"); }
                }
                catch (Basda::ServiceInitException &e)
                {
                    E_LLOG(e.what());
                    // itFind->second->init();
                    // itFind->second->changeState(Basda::ONLINE);
                }
            }
            else
            {
                std::stringstream log_msg;
                if(itFind == m_mapStageX.end())
                {
                    log_msg << __FUNCTION__ << suc::tilde << *it << " : Invalid device name";
                }
                else
                {
                    log_msg << __FUNCTION__ << suc::tilde << *it << " : Device is unavailable";
                }
                W_LLOG(log_msg.str());
            }
        }
    }
    else
    {
        try
        {
            std::map<std::string, Basda::MoccaXServiceWorkerInterfacePtr>::const_iterator it;
            for(it = m_mapStageX.begin(); it != m_mapStageX.end(); ++it)
            {
                if(it->second)
                {
                    int change_state_result = it->second->changeState(Basda::ONLINE, timeout);
                    if(change_state_result == 0)// timed out
                    {
                        W_LLOG(it->first << " change state request timed out.")
                    }
                    else { I_LLOG(it->first + " ONLINE"); }
                }
                else
                {
                    std::stringstream log_msg;
                    log_msg << __FUNCTION__ << suc::tilde << it->first << " : Device is unavailable";
                    W_LLOG(log_msg.str());
                }
            }
        }
        catch (std::exception e)
        {
            E_LLOG(e.what());
            U8_LLOG("");
            throw e;
        }
    }
    // U6_LLOG("State changed");
    U8_LLOG("");
}


void multiStageX::changeState(const Basda::ServiceState &_state, const std::vector<std::string> &_vecDevAlias)
{
    U6_LLOG(__FUNCTION__);
    Nice::Time timeout = Nice::Time::seconds(2);
    int change_state_result;
    if(!_vecDevAlias.empty())
    {
        std::vector<std::string>::const_iterator it;
        for(it = _vecDevAlias.begin(); it != _vecDevAlias.end(); ++it)
        {

            std::map<std::string, Basda::MoccaXServiceWorkerInterfacePtr>::const_iterator itFind;
            itFind = m_mapStageX.find(*it);
            if(itFind != m_mapStageX.end() && itFind->second)
            {
                try
                {
                    change_state_result = itFind->second->changeState(_state, timeout);
                    if(change_state_result == 0)// timed out
                    {
                        W_LLOG(itFind->first << " change state request timed out.")
                    }
                    else { I_LLOG(itFind->first << " " << _state); }
                }
                catch (Basda::ServiceInitException &e)
                {
                    E_LLOG(e.what());
                }
            }
            else
            {
                std::stringstream log_msg;
                if(itFind == m_mapStageX.end())
                {
                    log_msg << __FUNCTION__ << suc::tilde << *it << " : Invalid device name";
                }
                else
                {
                    log_msg << __FUNCTION__ << suc::tilde << *it << " : Device is unavailable";
                }
                W_LLOG(log_msg.str());
            }
        }
    }
    else
    {
        std::map<std::string, Basda::MoccaXServiceWorkerInterfacePtr>::const_iterator it;
        for(it = m_mapStageX.begin(); it != m_mapStageX.end(); ++it)
        {
            if(it->second)
            {
                try
                {
                    change_state_result = it->second->changeState(_state, timeout);
                    if(change_state_result == 0)// timed out
                    {
                        W_LLOG(it->first << " change state request timed out.")
                    }
                    else { I_LLOG(it->first << " " << _state); }
                }
                catch (Basda::ServiceInitException &e)
                {
                    E_LLOG(e.what());
                }
            }
            else
            {
                std::stringstream log_msg;
                log_msg << __FUNCTION__ << suc::tilde << it->first << " : Device is unavailable";
                W_LLOG(log_msg.str());
            }
        }
    }
    U8_LLOG("");
}


// void multiStageX::standby()
// {
//   // U6_LLOG("Change state to STANDBY")
//   I_LLOG(__PRETTY_FUNCTION__);

//   try
//     {
//       // U6_LLOG(m_mapStageX.size());
//       for(BOOST_AUTO(it, m_mapStageX.begin()); it != m_mapStageX.end(); ++it)
//         {
//           // U6_LLOG(it->first << " state = " << it->second->state());
//           if(it->second->state() == Basda::WORKING)
//             it->second->stop();
//           if(it->second->state() != Basda::STANDBY)
//             {
//               it->second->changeState(Basda::STANDBY);
//               // U6_LLOG(it->first << " state changed to STANDBY");
//             }
//         }
//     }
//   catch (std::exception e)
//     {
//       E_LLOG(e.what());
//       throw(e);
//       // E_LLOG(err.what());
//     }
//   // U6_LLOG("State changed");
// }

// void multiStageX::online()
// {
//   // std::ostringstream streamLog;
//   // U6_LLOG("Change state to ONLINE");
//   I_LLOG(__PRETTY_FUNCTION__);

//   try
//     {
//       for(BOOST_AUTO(it, m_mapStageX.begin()); it != m_mapStageX.end(); ++it)
//         {
//           it->second->changeState(Basda::ONLINE);
//           I_LLOG(it->first + " ONLINE");
//           if(it->second->isAtLimit())
//             {
//               E_LLOG(it->first + " Is at limit!");

//               I_LLOG(it->first + " Moving to referece ...");
//               it->second->moveToHome();

//               I_LLOG(it->first + " At referece.");
//             }
//         }
//     }
//   catch (std::exception e)
//     {
//       E_LLOG(e.what());
//       throw e;
//     }
//   // U6_LLOG("State changed");
// }



void multiStageX::moveToReference()
{
    U6_LLOG(__FUNCTION__);
    static Nice::Time nice_time = Nice::Time::seconds(80);
    try
    {
        moveToReferenceStart();
        moveToReferenceWait(nice_time);
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
}


// Moves all the devices to reference position and DOES NOT waits forthem.
void multiStageX::moveToReferenceStart()
{
    U6_LLOG(__FUNCTION__);
    std::map<std::string, Basda::MoccaXServiceWorkerInterfacePtr>::const_iterator it;
    for(it = m_mapStageX.begin(); it != m_mapStageX.end(); ++it)
    {
        if(it->second)
        {
            try
            {
                it->second->moveToHomeStart();
            }
            catch (Basda::DevException e)
            {
                E_LLOG("Caught exception in wheels.moveHome. Maybe a dev is DUMMY?");
                U8_LLOG("");
                throw(e);
            }
        }
        else
        {
            std::stringstream log_msg;
            log_msg << __FUNCTION__ << suc::tilde << it->first << " : Device is unavailable";
            E_LLOG(log_msg.str());
            U8_LLOG("");
            throw(std::runtime_error(log_msg.str()));
        }
    }
    U8_LLOG("");
}


// WAITS all the devices to reach reference position.
void multiStageX::moveToReferenceWait(const Nice::Time &_time)
{
    U6_LLOG(__FUNCTION__);
    std::map<std::string, Basda::MoccaXServiceWorkerInterfacePtr>::const_iterator it;
    su::Timer timer(_time);
    for(it = m_mapStageX.begin(); it != m_mapStageX.end(); ++it)
    {
        if(it->second)
        {
            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(":Caught exception in device.moveHome. Maybe a dev is DUMMY?" << e.what());
            }
            catch(...)
            {
                std::string what = boost::current_exception_diagnostic_information();
                std::string toErase = "std::exception::what:";
                size_t pos = what.find(toErase);
                if(pos != std::string::npos)what.erase(pos, toErase.length());
                what.erase(std::remove(what.begin(), what.end(), '\n'), what.end());
                E_LLOG(it->first << suc::tilde << what);
                throw;
            }
        }
        else
        {
            std::stringstream log_msg;
            log_msg << __FUNCTION__ << suc::tilde << it->first << " : Device is unavailable";
            E_LLOG(log_msg.str());
            U8_LLOG("");
            throw(std::runtime_error(log_msg.str()));
        }
    }
    U8_LLOG("");
}


void multiStageX::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("");
}


void multiStageX::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, Basda::MoccaXServiceWorkerInterfacePtr>::const_iterator itFind;
        itFind = m_mapStageX.find(*it);
        if(itFind != m_mapStageX.end() && itFind->second)
        {
            itFind->second->moveToHomeStart();
        }
        else
        {
            std::stringstream log_msg;
            if(itFind == m_mapStageX.end())
            {
                log_msg << __FUNCTION__ << suc::tilde << *it << " : Invalid device name";
            }
            else
            {
                log_msg << __FUNCTION__ << suc::tilde << *it << " : Device is unavailable";
            }
            E_LLOG(log_msg.str());
            U8_LLOG("");
            throw(std::runtime_error(log_msg.str()));
        }
    }
    U8_LLOG("");
}


void multiStageX::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::MoccaXServiceWorkerInterfacePtr>::iterator itFind = m_mapStageX.find(*it);
        if(itFind != m_mapStageX.end() && itFind->second)
        {
            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 << itFind->first << 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 log_msg;
            if(itFind == m_mapStageX.end())
            {
                log_msg << __FUNCTION__ << suc::tilde << *it << " : Invalid device name";
            }
            else
            {
                log_msg << __FUNCTION__ << suc::tilde << *it << " : Device is unavailable";
            }
            E_LLOG(log_msg.str());
            U8_LLOG("");
            throw(std::runtime_error(log_msg.str()));
        }
    }
    U8_LLOG("");
}


// moveToNamedPosition
// INPUT map of device as key and named positions as value, expressed as integer index
void multiStageX::moveToNamedPosition(const std::map<std::string, int> &_mapNamedPos)
{
    U6_LLOG(__FUNCTION__);
    try
    {
        moveToNamedPositionStart(_mapNamedPos);
        moveToNamedPositionWait(_mapNamedPos, Nice::Time::seconds(80));
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
}


void multiStageX::moveToNamedPositionStart(const std::map<std::string, int> &_mapNamedPos)
{
//    U6_LLOG(__FUNCTION__);
    std::map<std::string, int>::const_iterator it;
    for(it = _mapNamedPos.begin(); it != _mapNamedPos.end(); ++it)
    {
        std::map<std::string, Basda::MoccaXServiceWorkerInterfacePtr>::iterator itFind = m_mapStageX.find(it->first);
        if(itFind != m_mapStageX.end() && itFind->second)
        {
            try
            {
                I_LLOG(it->first << " Moving to position : " << it->second);
                itFind->second->moveToNamedPositionStart(it->second);
            }
            catch (Basda::Svc::MoccaMotionException exMotion)
            {
                E_LLOG(it->first << " " << exMotion.what());
//                U8_LLOG("");
                throw(exMotion);
            }
            catch (Basda::ServiceException exService)
            {
                E_LLOG(it->first << ":" << exService.what());
//                U8_LLOG("");
                throw(exService);
            }
            catch (std::exception err)
            {
                E_LLOG(it->first + " Service Exception!" + err.what());
//                U8_LLOG("");
                throw err;
            }
            catch(...)
            {
                E_LLOG(__FUNCTION__);
//                U8_LLOG("");
                throw;
            }
        }
        else
        {
            std::stringstream log_msg;
            if(itFind == m_mapStageX.end())
            {
                log_msg << __FUNCTION__ << suc::tilde << it->first << " : Invalid device name";
            }
            else
            {
                log_msg << __FUNCTION__ << suc::tilde << it->first << " : Device is unavailable";
            }
            E_LLOG(log_msg.str());
//            U8_LLOG("");
            throw(std::runtime_error(log_msg.str()));
        }
    }
//    U8_LLOG("");
}


void multiStageX::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)
    {
        std::map<std::string, Basda::MoccaXServiceWorkerInterfacePtr>::iterator itFind = m_mapStageX.find(it->first);
        if(itFind != m_mapStageX.end() && itFind->second)
        {
            try
            {
                int WAIT_RESULT_TIMED_OUT = 0;
                if(itFind->second->moveToNamedPositionWait(timer.residual()) == WAIT_RESULT_TIMED_OUT)
                {
                    std::stringstream log_msg;
                    log_msg << __FUNCTION__ << suc::tilde << itFind->first << suc::tilde << "Error : wait timed out";
                    throw(std::runtime_error(log_msg.str()));
                }
            }
            catch (Basda::Svc::MoccaMotionException limEx)
            {
                W_LLOG(it->first << " " << limEx.what());
//                U8_LLOG("");
                throw(limEx);
            }
            catch (std::exception err)
            {
                E_LLOG(it->first + ": Exception " + err.what());
//                U8_LLOG("");
                throw err;
            }
            catch(...)
            {
                E_LLOG(__FUNCTION__ << suc::tilde << it->first);
//                U8_LLOG("");
                throw;
            }
        }
        else
        {
            std::stringstream log_msg;
            if(itFind == m_mapStageX.end())
            {
                log_msg << __FUNCTION__ << suc::tilde << it->first << " : Invalid device name";
            }
            else
            {
                log_msg << __FUNCTION__ << suc::tilde << it->first << " : Device is unavailable";
            }
            E_LLOG(log_msg.str());
//            U8_LLOG("");
            throw(std::runtime_error(log_msg.str()));
        }
    }
//    U8_LLOG("");
}


// moveToPosition
// INPUT map with device alias as key and position as value, expressed in steps
void multiStageX::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("");
}


void multiStageX::moveToAbsolutePositionStart(const std::map<std::string, double> &_mapPos, const std::map<std::string, std::string> &_mapUnit)
{
//    U6_LLOG(__FUNCTION__);
    std::string unit = "";

    std::map<std::string, double>::const_iterator it;
    for(it = _mapPos.begin(); it != _mapPos.end(); ++it)
    {
        std::map<std::string, Basda::MoccaXServiceWorkerInterfacePtr>::iterator itFind = m_mapStageX.find(it->first);
        if(itFind != m_mapStageX.end() && itFind->second)
        {
            if(_mapUnit.find(it->first) != _mapUnit.end())
            {
                std::map<std::string, std::string>::const_iterator itPair;
                itPair = _mapUnit.find(it->first);
                unit = itPair->second;
                if(unit == "DEFAULT")unit = "STEPS";
            }
            else
            {
                unit = "STEPS";
            }
            try
            {
                if(unit == "STEPS")
                    itFind->second->moveAbsoluteStart(static_cast<int>(it->second), unit);
                else
                    itFind->second->moveAbsoluteStart(it->second, unit);
            }
            catch (Basda::Svc::MoccaMotionException limEx)
            {
                E_LLOG(it->first + ": Exception " + limEx.what());
//                U8_LLOG("");
                throw limEx;
            }
            catch(...)
            {
                E_LLOG(__FUNCTION__);
//                U8_LLOG("");
                throw;
            }
        }
        else
        {
            std::stringstream log_msg;
            if(itFind == m_mapStageX.end())
            {
                log_msg << __FUNCTION__ << suc::tilde << it->first << " : Invalid device name";
            }
            else
            {
                log_msg << __FUNCTION__ << suc::tilde << it->first << " : Device is unavailable";
            }
            E_LLOG(log_msg.str());
//            U8_LLOG("");
            throw(std::runtime_error(log_msg.str()));
        }
    }
//    U8_LLOG("");
}


void multiStageX::moveToAbsolutePositionWait(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)
    {
        std::map<std::string, Basda::MoccaXServiceWorkerInterfacePtr>::iterator itFind = m_mapStageX.find(it->first);
        if(itFind != m_mapStageX.end() && itFind->second)
        {
            try
            {
                int WAIT_RESULT_TIMED_OUT = 0;
                if(itFind->second->moveAbsoluteWait(timer.residual()) == WAIT_RESULT_TIMED_OUT)
                {
                    std::stringstream log_msg;
                    log_msg << __FUNCTION__ << suc::tilde << itFind->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;
            }
        }
        else
        {
            std::stringstream log_msg;
            if(itFind == m_mapStageX.end())
            {
                log_msg << __FUNCTION__ << suc::tilde << it->first << " : Invalid device name";
            }
            else
            {
                log_msg << __FUNCTION__ << suc::tilde << it->first << " : Device is unavailable";
            }
            E_LLOG(log_msg.str());
//            U8_LLOG("");
            throw(std::runtime_error(log_msg.str()));
        }
    }
//    U8_LLOG("");
}


void multiStageX::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 multiStageX::moveToRelativePositionStart(const std::map<std::string, double> &_mapPos, const std::map<std::string, std::string> &_mapUnit)
{
//    U6_LLOG(__FUNCTION__);
    std::string unit = "";

    std::map<std::string, double>::const_iterator it;
    for(it = _mapPos.begin(); it != _mapPos.end(); ++it)
    {
        std::map<std::string, Basda::MoccaXServiceWorkerInterfacePtr>::iterator itFind = m_mapStageX.find(it->first);
        if(itFind != m_mapStageX.end() && itFind->second)
        {
            try
            {
                if(_mapUnit.find(it->first) != _mapUnit.end())
                {
                    std::map<std::string, std::string>::const_iterator itPair;
                    itPair = _mapUnit.find(it->first);
                    unit = itPair->second;
                    U3_LLOG("unit is " << itPair->second);
                }
                else
                {
                    unit = "STEPS";
                }
                U9_LLOG("Moving " << it->first << " of " << it->second << " " << unit);
                itFind->second->moveRelativeStart(it->second, "STEPS");
            }
            catch(...)
            {
                E_LLOG(__FUNCTION__);
//                U8_LLOG("");
                throw;
            }
        }
        else
        {
            std::stringstream log_msg;
            if(itFind == m_mapStageX.end())
            {
                log_msg << __FUNCTION__ << suc::tilde << it->first << " : Invalid device name";
            }
            else
            {
                log_msg << __FUNCTION__ << suc::tilde << it->first << " : Device is unavailable";
            }
            E_LLOG(log_msg.str());
//            U8_LLOG("");
            throw(std::runtime_error(log_msg.str()));
        }
    }
//    U8_LLOG("");
}


void multiStageX::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)
    {
        std::map<std::string, Basda::MoccaXServiceWorkerInterfacePtr>::iterator itFind = m_mapStageX.find(it->first);
        if(itFind != m_mapStageX.end() && itFind->second)
        {
            try
            {
                int WAIT_RESULT_TIMED_OUT = 0;
                if(m_mapStageX[it->first]->moveRelativeWait(timer.residual()) == WAIT_RESULT_TIMED_OUT)
                {
                    std::stringstream log_msg;
                    log_msg << __FUNCTION__ << suc::tilde << itFind->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;
            }
        }
        else
        {
            std::stringstream log_msg;
            if(itFind == m_mapStageX.end())
            {
                log_msg << __FUNCTION__ << suc::tilde << it->first << " : Invalid device name";
            }
            else
            {
                log_msg << __FUNCTION__ << suc::tilde << it->first << " : Device is unavailable";
            }
            E_LLOG(log_msg.str());
//            U8_LLOG("");
            throw(std::runtime_error(log_msg.str()));
        }
    }
//    U8_LLOG("");
}


bool multiStageX::isDevice(const std::string &_strDevAlias)
{
    bool boolRet;
    if(m_mapStageX.find(_strDevAlias) != m_mapStageX.end())
        boolRet = true;
    else
        boolRet = false;
//    U6_LLOG("multiStageX::" << __FUNCTION__ << " ~ " << _strDevAlias << " returned : " << boolRet);
//    U8_LLOG("");
    return boolRet;
}


void multiStageX::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(m_strCallbackName, strPropName);
    strPropName = "CMD.MOVEABSOLUTE.EXEC";
    install_callback(m_strCallbackName, strPropName);
    strPropName = "CMD.MOVERELATIVE.EXEC";
    install_callback(m_strCallbackName, strPropName);
    strPropName = "CMD.MOVETOHOME.EXEC";
    install_callback(m_strCallbackName, strPropName);
    strPropName = "CMD.MOVETOLIMIT.EXEC";
    install_callback(m_strCallbackName, strPropName);
    U8_LLOG("");
}


void multiStageX::set_callback_fn_count(boost::function<void(const Nice::Completion&)> &_cbfn, const std::string &_strFnName)
{
    U6_LLOG(__FUNCTION__);
    m_callbackFnCount = _cbfn;
    m_strCallbackNameCount = _strFnName;
    U8_LLOG("");
}


// --------------------------
// GETTERS
// --------------------------
std::vector<std::string> multiStageX::getNamedPosition(const std::size_t _stageIdx)
{
    U6_LLOG(__FUNCTION__);
    std::vector<std::string> res;

    if(_stageIdx >= m_nDevices)
    {
        // return position forall stages
        std::vector<Basda::MoccaXServiceWorkerInterfacePtr>::const_iterator it;
        for(it = m_vecStageX.begin(); it < m_vecStageX.end(); ++it)
        {
            if(*it)
                res.push_back((*it)->config()->leaf("VAR.NAMED_POSITION")->get<std::string>());
            // U8_LLOG((*it)->config()->leaf("VAR.NAMED_POSITION")->get<std::string>());
        }
    }
    else
    {
        std::map<std::string, Basda::MoccaXServiceWorkerInterfacePtr>::const_iterator it = m_mapStageX.begin();
        std::advance(it, _stageIdx);
        if(it->second)
            res.push_back(it->second->config()->leaf("VAR.NAMED_POSITION")->get<std::string>());
    }
    U8_LLOG("");
    return res;
}


std::vector<std::string> multiStageX::getNamedPosition(const std::vector<std::string> &_vecAlias)
{
    U6_LLOG(__FUNCTION__);
    std::vector<std::string> vecRes;
    // int idx = 0;
    std::vector<std::string>::const_iterator itPos = _vecAlias.begin();

    std::map<std::string, Basda::MoccaXServiceWorkerInterfacePtr>::const_iterator it;
    for(it = m_mapStageX.begin(); it != m_mapStageX.end(); ++it)
    {
        // idx = std::distance(it, m_mapWheels.begin());
        // itPos = _vecAlias.begin() + idx;
        if(m_mapStageX.find(*itPos) != m_mapStageX.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 fordevice");
            U8_LLOG("");
            throw std::invalid_argument("Wrong alias name fordevice");
        }
    }
    U8_LLOG("");
    return vecRes;
}


std::map<std::string, std::string> multiStageX::getNamedPosition2(const std::vector<std::string> &_vecAlias)
{
//    U6_LLOG(__FUNCTION__);
    std::map<std::string, std::string> mapRes;
    std::vector<std::string>::const_iterator itPos;// = _vecAlias.begin();

    std::map<std::string, Basda::MoccaXServiceWorkerInterfacePtr>::const_iterator it;
    for(itPos = _vecAlias.begin(); itPos != _vecAlias.end(); ++itPos)
    {
        it = m_mapStageX.find(*itPos);
        if(it != m_mapStageX.end() && it->second)
        {
            mapRes[*itPos] = it->second->config()->leaf("VAR.NAMED_POSITION")->get<std::string>();
        }
        else
        {
            std::stringstream log_msg;
            if(it == m_mapStageX.end())
            {
                log_msg << __FUNCTION__ << suc::tilde << it->first << " : Invalid device name";
            }
            else
            {
                log_msg << __FUNCTION__ << suc::tilde << it->first << " : Device is unavailable";
            }
            W_LLOG(log_msg.str());
        }
    }
//    U8_LLOG("");
    return mapRes;
}


Ice::Int multiStageX::getPosition(std::map<std::string, Ice::Double> &_mapPosition, const std::string &_unit)
{
//    U6_LLOG(__FUNCTION__);
    Ice::Int status = EXIT_SUCCESS;
    int position_counter = 0;
    std::map<std::string, Ice::Double>::const_iterator it;
    for(it = _mapPosition.begin(); it != _mapPosition.end(); ++it)
    {
        std::map<std::string, Basda::MoccaXServiceWorkerInterfacePtr>::iterator itFind = m_mapStageX.find(it->first);
        if(itFind != m_mapStageX.end() && itFind->second)
        {
            try
            {
                if(isDevice(it->first))
                {
                    position_counter++;
                    _mapPosition[it->first] = itFind->second->getPosition(_unit);
                }
            }
            catch (...)
            {
                std::string what = boost::current_exception_diagnostic_information();
                std::string toErase = "std::exception::what:";
                size_t pos = what.find(toErase);
                if(pos != std::string::npos)what.erase(pos, toErase.length());
                what.erase(std::remove(what.begin(), what.end(), '\n'), what.end());
                U6_LLOG(__FUNCTION__);
                E_LLOG(what);
                U8_LLOG("");
                throw;
            }
        }
        else
        {
            std::stringstream log_msg;
            if(itFind == m_mapStageX.end())
            {
//                log_msg << __FUNCTION__ << suc::tilde << it->first << " : Invalid device name";
            }
            else
            {
                log_msg << __FUNCTION__ << suc::tilde << it->first << " : Device is unavailable";
                U6_LLOG(__FUNCTION__);
                E_LLOG(log_msg.str());
                U8_LLOG("");
                throw(std::runtime_error(log_msg.str()));
            }
        }
    }
//    U8_LLOG("");
    return status;
}


void multiStageX::emergencyAbort()
{
    U6_LLOG(__FUNCTION__);
    std::map<std::string, Basda::MoccaXServiceWorkerInterfacePtr>::const_iterator it;
    for(it = m_mapStageX.begin(); it != m_mapStageX.end(); ++it)
    {
        if(it->second)
        {
            W_LLOG(it->first << ": ABORT");
            try
            {
                it->second->abort();
            }
            catch (Nice::Exception err)
            {
                E_LLOG(err.what());
            }
            catch(...)
            {
                E_LLOG("Error trying to ABORT " << it->first);
            }
        }
        else
        {
            E_LLOG("Could not issue ABORTO to : " << it->first);
        }
    }
    U8_LLOG("");
}


int multiStageX::stopAll()
{
    U6_LLOG(__FUNCTION__);
    int status = EXIT_SUCCESS;

    std::map<std::string, Basda::MoccaXServiceWorkerInterfacePtr>::const_iterator it;
    for(it = m_mapStageX.begin(); it != m_mapStageX.end(); ++it)
    {
        if(it->second)
        {
            W_LLOG(it->first << ": STOP");
            try
            {
                it->second->stop();
            }
            catch (Nice::Exception err)
            {
                E_LLOG(err.what());
                status = EXIT_FAILURE;
            }
            catch(...)
            {
                status = EXIT_FAILURE;
                E_LLOG("Error trying to STOP " << it->first);
            }
        }
        else
        {
            E_LLOG("Could not issue STOP to " << it->first  << " : device not connected");
        }
    }
    U8_LLOG("");
    return status;
}


int multiStageX::stop(const std::vector<std::string> &_vecDevAlias)
{
    std::stringstream log_msg;
    log_msg << "multiStageX::" << __FUNCTION__ << suc::tilde << "Stopping motion of selected linear stages";
    U6_LLOG(log_msg.str());

    int status = EXIT_SUCCESS;
    std::vector<std::string>::const_iterator it;
    for(it = _vecDevAlias.begin(); it != _vecDevAlias.end(); ++it)
    {
        std::map<std::string, Basda::MoccaXServiceWorkerInterfacePtr>::iterator itFind = m_mapStageX.find(*it);
        if(itFind != m_mapStageX.end() && itFind->second)
        {
            W_LLOG(itFind->first << ": STOP");
            try
            {
                itFind->second->stop();
            }
            catch (Nice::Exception err)
            {
                E_LLOG(err.what());
                status = EXIT_FAILURE;
            }
            catch(...)
            {
                E_LLOG("Error trying to STOP " << itFind->first);
            }
        }
        else
        {
            if(itFind == m_mapStageX.end())
            {
            }
            else
            {
                E_LLOG("Could not issue STOP to " << itFind->first  << " : device not connected");
            }
        }
    }
    U8_LLOG("");
    return status;
}


int multiStageX::abort(const std::vector<std::string> &_vecDevAlias)
{
    U6_LLOG("multiStageX::" << __FUNCTION__ << suc::tilde << "Aborting motion of selected motors");
    int status = EXIT_SUCCESS;
    std::vector<std::string>::const_iterator it;
    for(it = _vecDevAlias.begin(); it != _vecDevAlias.end(); ++it)
    {
        std::map<std::string, Basda::MoccaXServiceWorkerInterfacePtr>::iterator itFind = m_mapStageX.find(*it);
        if(itFind != m_mapStageX.end() && itFind->second)
        {
            W_LLOG(itFind->first << ": ABORT");
            try
            {
                itFind->second->abort();
            }
            catch (Nice::Exception err)
            {
                E_LLOG(err.what());
                status = EXIT_FAILURE;
            }
            catch(...)
            {
                E_LLOG("Error trying to ABORT " << itFind->first);
            }
        }
        else
        {
            if(itFind == m_mapStageX.end())
            {
                E_LLOG("Could not issue ABORT to " << itFind->first << " : invalid device");
            }
            else
            {
                E_LLOG("Could not issue ABORT to " << itFind->first  << " : device not connected");
            }
        }
    }
    U8_LLOG("");
    return status;
}
