// $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/src/motion_ctrl/motionCtrl.cpp 3987 2022-07-06 16:38:50Z depa $

#include "shins_util.h"
#include "motionCtrl.hpp"


void MotionLogBoostDiagnosticInformation(int line, std::string device)
{
    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());
    std::stringstream log_msg;
    log_msg << "Line: " << line << " ~ " << device << " ~ " << what;
    E_LLOG(log_msg.str());
}

// std::map<std::string, std::string> m_mapCfgFiles;

// this constructor
motionCtrl::motionCtrl(boost::shared_ptr<osfsm::OSFsm> &_ptrStateMachine,
                       Ice::PropertiesPtr prop)
:
    __OBJ_NAME__(typeid(this).name()),
    m_service_status(su::SERVICE_NOTCONNECTED)
{
    U6_LLOG(__FUNCTION__ << " Constructor");
    m_ConfigClient = NULL;
    std::string strEP;
    std::string strDevAlias;
    std::string logMsg;

    m_service = prop->getProperty("OcsMotionService.Name");

    m_ptrStateMachine = _ptrStateMachine;
    // U8_LLOG("State Machine state: " << *(m_ptrStateMachine->current_state()));

    // +++++++
    // CREATING CALLBACK FUNCTIONS
    // +++++++

//    m_callbackFnSM = boost::bind(&niceCallback::callback_motion_complete_SM, m_ptrCallbackObjSM, _1,
//        boost::ref(m_ptrStateMachine), boost::ref(m_mtxStateMachine));
    m_callbackFnSM = boost::bind(&niceCallback::callback_motion_complete_SM, _1, boost::ref(m_ptrStateMachine), boost::ref(m_mtxStateMachine));
    //std::cout << __FUNCTION__ << &m_mtxStateMachine << std::endl;

    // bind to function, parameter of callback used by multiDevice
    // m_callbackFnIndi = boost::bind(&niceCallback::callback_motion_complete_indi,
    //                                m_ptrCallbackObjIndi,
    //                                _1, _2,
    //                                boost::ref(m_ptrLbtoIF));

    m_ptrMonSeq = boost::make_shared<IceUtil::Monitor<IceUtil::Mutex> >();
//    m_callbackFnCount = boost::bind(&niceCallback::callback_motion_complete_count, m_ptrCallbackObjCount, _1,
//        boost::ref(m_moving_devs), boost::ref(m_mtxCount), boost::ref(m_ptrMonSeq));
    m_callbackFnCount = boost::bind(&niceCallback::callback_motion_complete_count, _1, boost::ref(m_moving_devs), boost::ref(m_mtxCount), boost::ref(m_ptrMonSeq));

    //void callback_device_move_to_reference(const Nice::Completion& _c, const Basda::MoccaWheelServiceWorkerInterfacePtr& _device);

    // m_callbackFnIndi = boost::bind(&niceCallback::callback_motion_complete_indi,
    //                                m_ptrCallbackObjIndi,
    //                                _1,
    //                                boost::ref(m_ptrLbtoIF));
    // U2_LLOG(__FUNCTION__ << " Connecting to remote configuration tree ...")

    U8_LLOG(__FUNCTION__ << " : Done");
}


motionCtrl::~motionCtrl()
{
}


bool motionCtrl::CheckOnline(bool throw_if_notonline)
{
    if(throw_if_notonline && !(m_service_status == su::SERVICE_ONLINE))
    {
        throw(std::runtime_error("MOTION_CTRL Service is not ONLINE"));
    }
    return m_service_status == su::SERVICE_ONLINE;
}


su::EServiceStatus motionCtrl::Connect()
{
    static bool bStageXConnected = false;
    static bool bStageXyConnected = false;
    static bool bWheelsConnected = false;
    static bool bStageXOnline = false;
    static bool bStageXyOnline = false;
    static bool bWheelsOnline = false;
    static bool bStageXInited = false;
    static bool bStageXyInited = false;
    static bool bWheelsInited = false;

    Nice::JointPtr nodemotion = Nice::Application::config()->node(m_service);
    if(m_service_status == su::SERVICE_NOTCONNECTED)
    {
        try
        {
            U1_LLOG("Creating AppConfigClient for " << m_service << " ~ " << nodemotion);
            Nice::JointPtrVector leafs;
            Nice::JointPtrVector::iterator it;
            nodemotion->leafs(leafs);
            m_ConfigClient = Nice::AppConfigClient::create(nodemotion->getAtNode<Nice::Endpoint>("ALIAS"), nodemotion);
            m_service_status = su::SERVICE_ALLOCATING;
        }
        catch (const Nice::Exception & e){ A_LLOG(e.what()); }
        catch(...){ A_LLOG(__FUNCTION__); }
    }
    if(m_service_status == su::SERVICE_ALLOCATING)
    {
        Nice::JointPtr nodestagex = Nice::Application::config()-> node(m_service + "." + "STAGEX");
        Nice::JointPtr nodestagexy = Nice::Application::config()->node(m_service + "." + "STAGEXY");
        Nice::SeqString WheelChildren, StageXChildren, StageXyChildren;
        nodestagex->children(StageXChildren);
        nodestagexy->children(StageXyChildren);
        if(!m_ptrMultiWheel)
        {  
            m_ptrMultiWheel = boost::make_shared<multiWheel>(m_service);
        }
        if(!m_ptrMultiStageX)
        {  
            m_ptrMultiStageX = boost::make_shared<multiStageX>(m_service);
        }
        if(!m_ptrMultiStageXY)
        {  
            m_ptrMultiStageXY = boost::make_shared<multiStageXY>(m_service);
        }

        if(m_ptrMultiWheel && m_ptrMultiStageX && m_ptrMultiStageXY)
        {
            m_service_status = su::SERVICE_ALLOCATED;
        }
    }
    if(m_service_status == su::SERVICE_ALLOCATED)
    {
        try
        {
            if(!bStageXConnected)
            {
                m_ptrMultiStageX->connect();
                bStageXConnected = true;
            }
            if(!bWheelsConnected)
            {
                m_ptrMultiWheel->connect();
                bWheelsConnected = true;
            }
            if(!bStageXyConnected)
            {
                m_ptrMultiStageXY->connect();
                bStageXyConnected = true;
            }
            if(bStageXyConnected && bWheelsConnected && bStageXConnected)
            {
                m_service_status = su::SERVICE_CONNECTED;
            }
        }
        catch(...)
        {
            if(!bStageXConnected)
            {
                E_LLOG("Erorr connecting to motion_ctrl-svr stagex");
            }
            else if(!bWheelsConnected)
            {
                E_LLOG("Erorr connecting to motion_ctrl-svr wheels");
            }
            else if(!bStageXyConnected)
            {
                E_LLOG("Erorr connecting to motion_ctrl-svr stagexy");
            }
        }
    }
    if(m_service_status == su::SERVICE_CONNECTED)
    {
        if(!bStageXOnline)
        {
            try
            {
                m_ptrMultiStageX->standby();
                m_ptrMultiStageX->online();
                bStageXOnline = true;
            }
            catch(...){}
        }
        if(!bWheelsOnline)
        {
            try
            {
                m_ptrMultiWheel->standby();
                m_ptrMultiWheel->online();
                bWheelsOnline = true;
            }
            catch(...){}
        }
        if(!bStageXyOnline)
        {
            try
            {
                m_ptrMultiStageXY->standby();
                m_ptrMultiStageXY->online();
                bStageXyOnline = true;
            }
            catch(...){}
        }
        if(bStageXOnline && bStageXyOnline && bWheelsOnline)
        {
            m_service_status = su::SERVICE_TOBEINITED;
        }
    }
    if(m_service_status == su::SERVICE_TOBEINITED)
    {
        if(!bStageXInited)
        {
            m_ptrMultiStageX->set_callback_fn(m_callbackFnSM, "motion_dev-motion_complete_SM");
            m_ptrMultiStageX->set_callback_fn_count(m_callbackFnCount, "motion_complete_count");
            U1_LLOG("Connection established with service " << m_service);
            bStageXInited = true;
        }
        if(!bStageXyInited)
        {
            // Setting callback function for multiWheel object
            m_ptrMultiStageXY->set_callback_fn(m_callbackFnCount, "motion_complete_count");
            U2_LLOG("callbacks set on stage xy");
            bStageXyInited = true;
        }
        if(!bWheelsInited)
        {
            // Setting callback function for multiWheel object
            m_ptrMultiWheel->set_callback_fn(m_callbackFnSM, "motion_complete_SM");
            m_ptrMultiWheel->set_callback_fn_state(m_callbackFnIndi, "motion_state_change");
            m_ptrMultiWheel->set_callback_fn_count(m_callbackFnCount, "motion_complete_count");
            U2_LLOG(":callbacks set on rotation stages");
            bWheelsInited = true;
        }
        if(bWheelsInited && bStageXyInited && bStageXInited)
        {
            m_service_status = su::SERVICE_ONLINE;
        }
    }
    return m_service_status;
}


double motionCtrl::ConvertToDefaultUnit(const std::string & alias, double destination, const std::string & unit)
{
    if(m_ptrMultiWheel->isDevice(alias))
    {
        std::map<std::string, StageXDevice>::const_iterator itDevice = GetMultiStageX().find(alias);
        if(itDevice != GetMultiStageX().end())
        {
            std::string final_unit = unit;
            if(unit == suc::default_unit)final_unit = suc::steps;
            std::map<std::string, std::tr1::tuple<double, double> >::const_iterator itUnit = itDevice->second.units.find(final_unit);
            if(itUnit != itDevice->second.units.end())
            {
                return destination/std::tr1::get<0>(itUnit->second) + std::tr1::get<1>(itUnit->second);
            }
            else
            {
                E_LLOG(std::string(__FUNCTION__) << ":No " << unit << " unit in " << alias << ". Throwing!");
                throw std::invalid_argument(std::string(__FUNCTION__) + ": No " + unit + " unit in " + alias + ".");
            }
        }
        else
        {
            E_LLOG(std::string(__FUNCTION__) << ":No " << alias << " device in CALUNIT_CTRL. Throwing!");
            throw std::invalid_argument(std::string(__FUNCTION__) + ": No " + alias + " device in CALUNIT_CTRL.");
        }
    }
    else if(m_ptrMultiStageXY->isDevice(alias))
    {
        E_LLOG("ERROR: " << __FUNCTION__ << " TBD");
        throw std::invalid_argument(std::string("ERROR: ") + __FUNCTION__ + " TBD");
        std::map<std::string, StageXYDevice>::const_iterator itDevice = GetMultiStageXY().find(alias);
        if(itDevice != GetMultiStageXY().end())
        {
            std::string final_unit = unit;
            if(unit == suc::default_unit)final_unit = suc::steps;
//            std::map<std::string, std::tr1::tuple<double, double> >::iterator itUnit = itDevice->second.units.find(final_unit);
//            if(itUnit != itDevice->second.units.end())
            {
                return destination;///std::tr1::get<0>(itUnit->second) + std::tr1::get<1>(itUnit->second);
            }
//            else
            {
                E_LLOG(std::string(__FUNCTION__) << ":No " << unit << " unit in " << alias << ". Throwing!");
                throw std::invalid_argument(std::string(__FUNCTION__) + ": No " + unit + " unit in " + alias + ".");
            }
        }
        else
        {
            E_LLOG(std::string(__FUNCTION__) << ":No " << alias << " device in CALUNIT_CTRL. Throwing!");
            throw std::invalid_argument(std::string(__FUNCTION__) + ": No " + alias + " device in CALUNIT_CTRL.");
        }
    }
    if(m_ptrMultiWheel->isDevice(alias))
    {
        std::map<std::string, WheelDevice>::const_iterator itDevice = GetMultiWheel().find(alias);
        if(itDevice != GetMultiWheel().end())
        {
            std::string final_unit = unit;
            if(unit == suc::default_unit)final_unit = suc::steps;
            std::map<std::string, std::tr1::tuple<double, double> >::const_iterator itUnit = itDevice->second.units.find(final_unit);
            if(itUnit != itDevice->second.units.end())
            {
                return destination/std::tr1::get<0>(itUnit->second) + std::tr1::get<1>(itUnit->second);
            }
            else
            {
                E_LLOG(std::string(__FUNCTION__) << ":No " << unit << " unit in " << alias << ". Throwing!");
                throw std::invalid_argument(std::string(__FUNCTION__) + ": No " + unit + " unit in " + alias + ".");
            }
        }
        else
        {
            E_LLOG(std::string(__FUNCTION__) << ":No " << alias << " device in CALUNIT_CTRL. Throwing!");
            throw std::invalid_argument(std::string(__FUNCTION__) + ": No " + alias + " device in CALUNIT_CTRL.");
        }
    }
    else
    {
        E_LLOG(std::string(__FUNCTION__) << ":No " << alias << " device in MOTION_CTRL. Throwing!");
        throw std::invalid_argument(std::string(__FUNCTION__) + ": No " + alias + " device in MOTION_CTRL.");
    }
}


void motionCtrl::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))
{
    if(m_ptrMultiWheel->isDevice(alias))
        m_ptrMultiWheel->SetSecondaryCallbacks(alias, MoveToHomeCB, MoveToNamedCB, MoveAbsoluteCB, MoveRelativeCB, MoveToLimitCB);
    else if(m_ptrMultiStageX->isDevice(alias))
        m_ptrMultiStageX->SetSecondaryCallbacks(alias, MoveToHomeCB, MoveToNamedCB, MoveAbsoluteCB, MoveRelativeCB, MoveToLimitCB);
    else if(m_ptrMultiStageXY->isDevice(alias))
        m_ptrMultiStageXY->SetSecondaryCallbacks(alias, MoveToHomeCB, MoveToNamedCB, MoveAbsoluteCB, MoveRelativeCB, MoveToLimitCB);
}


bool motionCtrl::isDevice(const std::string& _strDevAlias)
{
    return (m_ptrMultiWheel->isDevice(_strDevAlias) ||
        m_ptrMultiStageX->isDevice(_strDevAlias) ||
        m_ptrMultiStageXY->isDevice(_strDevAlias));
}


Ice::Int motionCtrl::getPosition(std::map<std::string, Ice::Double> &_mapPosition, const std::string &_unit)
{
//    U6_LLOG(__FUNCTION__);
    int status = EXIT_SUCCESS;
//        U9_LLOG("BREAK");
    status = m_ptrMultiWheel->getPosition(_mapPosition, _unit);
//        U9_LLOG("BREAK");
    status = m_ptrMultiStageX->getPosition(_mapPosition, _unit);
//        U9_LLOG("BREAK");
    status = m_ptrMultiStageXY->getPosition(_mapPosition, _unit);
//        U9_LLOG("BREAK");
//    U8_LLOG("");
    return status;
}

Ice::Int motionCtrl::getConfigPosition(std::map<std::string, Ice::Double> &_mapPosition, const std::string &_unit)
{
//    U6_LLOG(__FUNCTION__);
    int status = EXIT_SUCCESS;
    std::map<std::string, Ice::Double>::iterator itMap;
    std::map<std::string, StageXDevice>::const_iterator itStageX;
    std::map<std::string, StageXYDevice>::const_iterator itStageXY;
    std::map<std::string, WheelDevice>::const_iterator itWheel;
    if(!(m_service_status == su::SERVICE_ONLINE)){ return EXIT_FAILURE;}

    int counter = 0;
    std::stringstream catch_msg;

    const std::map<std::string, StageXDevice> & MapStageX = m_ptrMultiStageX->GetMultiStageX();
    const std::map<std::string, StageXYDevice> & MapStageXY = m_ptrMultiStageXY->GetMultiStageXY();
    const std::map<std::string, WheelDevice> & MapWheels = m_ptrMultiWheel->GetMultiWheel();

    double deviceEncoderPosition;
    Nice::Point deviceEncoderPoint;

    for(itMap = _mapPosition.begin(); itMap != _mapPosition.end(); itMap++)
    {
        for(itStageX = MapStageX.begin(); itStageX != MapStageX.end(); itStageX++)
        {
            if(itStageX->first != itMap->first)continue;
            try{deviceEncoderPosition = itStageX->second.iPtr->config()->leaf("VAR.POSITION.DEVICE_ENCODER." + _unit)->get<double>();}
            catch(...){catch_msg.str(""); catch_msg << m_service << " ~ getDeviceEncoderPosition ~ " << itStageX->first; MotionLogBoostDiagnosticInformation(__LINE__, catch_msg.str());}
            _mapPosition[itStageX->first] = deviceEncoderPosition;
        }

        for(itStageXY = MapStageXY.begin(); itStageXY != MapStageXY.end(); itStageXY++)
        {
            if(itMap->first.size() >= itStageXY->first.size())
            {
                if(su::left(itMap->first, itStageXY->first.size()) == itStageXY->first)
                {
                    try{deviceEncoderPoint = itStageXY->second.iPtr->config()->leaf("VAR.POSITION.DEVICE_ENCODER.STEPS")->get<Nice::Point>();}
                    catch(...){catch_msg.str(""); catch_msg << m_service << " ~ getDeviceEncoderPosition ~ " << itStageXY->first; MotionLogBoostDiagnosticInformation(__LINE__, catch_msg.str());}
                    if(su::right(itMap->first, 1) == "X")itMap->second = deviceEncoderPoint.xp;
                    else if(su::right(itMap->first, 1) == "Y")itMap->second = deviceEncoderPoint.yp;
                }
            }
        }

        for(itWheel = MapWheels.begin(); itWheel != MapWheels.end(); itWheel++)
        {
            if(itWheel->first != itMap->first)continue;
            try{deviceEncoderPosition = itWheel->second.iPtr->config()->leaf("VAR.POSITION.DEVICE_ENCODER." + _unit)->get<int>();}
            catch(...){catch_msg.str(""); catch_msg << m_service << " ~ getDeviceEncoderPosition ~ " << itWheel->first; MotionLogBoostDiagnosticInformation(__LINE__, catch_msg.str());}
            _mapPosition[itWheel->first] = deviceEncoderPosition;
        }
    }
//    U8_LLOG("");
    return status;
}

std::map<std::string, std::string> motionCtrl::getNamedPosition(const std::vector<std::string>& _vecAlias)
{
//    U6_LLOG(__FUNCTION__);
    std::map<std::string, std::string> mapWheels, mapStageX, mapStageXY, result;
//    std::vector<std::string>::const_iterator it;
//    for(it = _vecAlias.begin(); it < _vecAlias.end(); it++){ result[*it] = "";};
    std::map<std::string, std::string>::iterator it;
    U9_LLOG(__FUNCTION__ << " ~ MultiWheel");
    mapWheels = m_ptrMultiWheel->getNamedPosition2(_vecAlias);
    for(it = mapWheels.begin(); it != mapWheels.end(); it++)
    {
        U9_LLOG(it->first << " ~ " << it->second);
    }
    U9_LLOG(__FUNCTION__ << " ~ MultiStageX");
    mapStageX = m_ptrMultiStageX->getNamedPosition2(_vecAlias);
    for(it = mapStageX.begin(); it != mapStageX.end(); it++)
    {
        U9_LLOG(it->first << " ~ " << it->second);
    }
    U9_LLOG(__FUNCTION__ << " ~ MultiStageXY");
    mapStageXY = m_ptrMultiStageXY->getNamedPosition2(_vecAlias);
    for(it = mapStageXY.begin(); it != mapStageXY.end(); it++)
    {
        U9_LLOG(it->first << " ~ " << it->second);
    }
    result.insert(mapWheels.begin(), mapWheels.end());
    result.insert(mapStageX.begin(), mapStageX.end());
    result.insert(mapStageXY.begin(), mapStageXY.end());
//    U8_LLOG("");
    return result;
}


// return number of devices controlled
int motionCtrl::getNdevices()
{
    U6_LLOG(__FUNCTION__);
    int result = (m_ptrMultiWheel->getNdevices() + m_ptrMultiStageX->getNdevices() + m_ptrMultiStageXY->getNdevices());
    U8_LLOG("");
    return result;
}


void motionCtrl::moveHome()
{
    U6_LLOG(__FUNCTION__);

    m_ptrStateMachine->process_event(
        osfsm::ready_busy(
            m_ptrMultiWheel->getNdevices() + m_ptrMultiStageX->getNdevices() + m_ptrMultiStageXY->getNdevices()));
    // U8_LLOG("State Machine state: " << *(m_ptrStateMachine->current_state()));

    if(m_ptrMultiWheel->getNdevices() != 0)
    {
        m_ptrMultiWheel->moveToReference();
    }
    if(m_ptrMultiStageX->getNdevices() != 0)
    {
        m_ptrMultiStageX->moveToReference();
    }
    if(m_ptrMultiStageXY->getNdevices() != 0)
    {
        m_ptrMultiStageXY->moveToReference();
    }
    U8_LLOG(__FUNCTION__);
}


void motionCtrl::changeState(const Basda::ServiceState &_state, const std::vector<std::string> &_vecDevAlias)
{
    U6_LLOG(__FUNCTION__);
    switch (_state)
    {
    case Basda::ONLINE:
    {
        m_ptrMultiWheel->online(_vecDevAlias);
        m_ptrMultiStageX->online(_vecDevAlias);
        m_ptrMultiStageXY->online(_vecDevAlias);
        break;
    }
    case Basda::STANDBY:
    {
        m_ptrMultiWheel->online(_vecDevAlias);
        m_ptrMultiStageX->online(_vecDevAlias);
        m_ptrMultiStageXY->online(_vecDevAlias);
        break;
    }
    case Basda::SHUTDOWN:
    {
        W_LLOG("State " << _state << " not handled");
        break;
    }
    case Basda::OFFLINE:
    {
        W_LLOG("State " << _state << " not handled");
        break;
    }
    case Basda::WORKING:
    {
        W_LLOG("State " << _state << " not handled");
        break;
    }
    case Basda::ERROR:
    {
        W_LLOG("State " << _state << " not handled");
        break;
    }
    default:
        W_LLOG("State " << _state << " not handled")
        ;
    }
    U8_LLOG("");
}


// Performs initialization of motion stages
// move wheels to reference
// move stages X to referece
// move stage XY to referece
// wait wheels to reference
// wait stages X to referece
// wait stage XY to referece
// move wheels to reference
// wait wheels to reference

void motionCtrl::initializeAll()
{
    U6_LLOG(__FUNCTION__);
    try
    {
        initializeAllStart();
        initializeAllWait(Nice::Time::seconds(80));
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
}


void motionCtrl::initializeAllStart()
{
    U6_LLOG(__FUNCTION__);
    // moving to "home" reference switch all devices
    // the number of moving wheels is multiplied yby 2 since they move to reference twice
//    m_ptrStateMachine->process_event(osfsm::ready_busy(m_ptrMultiStageX->getNdevices()));

    m_moving_devs = 2 * m_ptrMultiWheel->getNdevices() + m_ptrMultiStageX->getNdevices() + m_ptrMultiStageXY->getNdevices();

    try
    {
        if(m_ptrMultiWheel->getNdevices() != 0)
        {
            // wheels must be moved to home reference switch
            // twice to reach in spech repeatability. The second initialization
            // is implemented in initializeWait
            m_ptrMultiWheel->moveToReferenceStart();
            U9_LLOG("Moving rotation stages to reference ...");

        }
        if(m_ptrMultiStageX->getNdevices() != 0)
        {
            m_ptrMultiStageX->moveToReferenceStart();
            U9_LLOG("Moving X stages to reference ...");

        }
        if(m_ptrMultiStageXY->getNdevices() != 0)
        {
            m_ptrMultiStageXY->moveToReferenceStart();
            U9_LLOG("Moving rotation XY to reference ...");

        }
    }
    catch(std::exception e)
    {
        E_LLOG(e.what());
        U8_LLOG("");
        throw e;
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
}


void motionCtrl::initializeAllWait(const Nice::Time& _time)
{
    U6_LLOG(__FUNCTION__);
    su::Timer timer(_time);
    try
    {
        if(m_ptrMultiWheel->getNdevices() != 0)
        {
            m_ptrMultiWheel->moveToReferenceWait(timer.residual());
        }
        if(m_ptrMultiStageX->getNdevices() != 0)
        {
            m_ptrMultiStageX->moveToReferenceWait(timer.residual());
        }
        if(m_ptrMultiStageXY->getNdevices() != 0)
        {
            m_ptrMultiStageXY->moveToReferenceWait(timer.residual());
        }
    }
    catch(std::exception e)
    {
        E_LLOG(e.what());
        U8_LLOG("");
        throw e;
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
}


void motionCtrl::initialize(const std::vector<std::string> &_vecDevAlias)
{
    U6_LLOG(__FUNCTION__);
    try
    {
        initializeStart(_vecDevAlias);
        initializeWait(Nice::Time::seconds(80));
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
}


void motionCtrl::initPrepare(const std::vector<std::string> & _vecDevAlias)
{
    m_init.clear();
    std::vector<std::string>::const_iterator it;
    for(it = _vecDevAlias.begin(); it != _vecDevAlias.end(); ++it)
    {
        if(m_ptrMultiWheel->isDevice(*it))
        {
            m_init.InitWheels.push_back(*it);
            continue;
        }
        if(m_ptrMultiStageX->isDevice(*it))
        {
            m_init.InitStageX.push_back(*it);
            continue;
        }
        if(m_ptrMultiStageXY->isDevice(*it))
        {
            m_init.InitStageXY.push_back(*it);
            continue;
        }
    }
}

    // Moves all or specified devices to start positions
void motionCtrl::initializeStart(const std::vector<std::string> &_vecDevAlias)
{
    U6_LLOG(__FUNCTION__ << " Initialiazing motion devices");
    initPrepare(_vecDevAlias);
    std::string logMsg;

    m_ptrStateMachine->process_event(
        osfsm::ready_busy(
            m_ptrMultiWheel->getNdevices() + m_ptrMultiStageX->getNdevices() + m_ptrMultiStageXY->getNdevices()));

    try
    {
        if(/*( m_ptrMultiStageX->getNdevices() > 0) &&  */!(m_init.InitStageX.empty()))
            m_ptrMultiStageX->moveDeviceToReferenceStart(m_init.InitStageX);
    }
    catch(Nice::Exception e)
    {
        E_LLOG(e.what());
        U8_LLOG("");
        throw(e);
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    try
    {
        if(/*( m_ptrMultiWheel->getNdevices() > 0) &&  */!(m_init.InitWheels.empty()))
            m_ptrMultiWheel->moveDeviceToReferenceStart(m_init.InitWheels);
    }
    catch(Nice::Exception e)
    {
        E_LLOG(e.what());
        U8_LLOG("");
        throw(e);
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    try
    {
        if(/*( m_ptrMultiStageXY->getNdevices() > 0) &&  */!(m_init.InitStageXY.empty()))
            m_ptrMultiStageXY->moveDeviceToReferenceStart(m_init.InitStageXY);
    }
    catch(Nice::Exception e)
    {
        E_LLOG(e.what());
        U8_LLOG("");
        throw(e);
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
}


void motionCtrl::initializeWait(const Nice::Time &_time)
{
    U6_LLOG(__FUNCTION__ << " - Waiting device initialization");
    su::Timer timer(_time);

    // filter input vector
    std::vector<std::string>::const_iterator it;

    try
    {
        if(/*( m_ptrMultiWheel->getNdevices() > 0) &&  */!(m_init.InitWheels.empty()))
        {
            m_ptrMultiWheel->moveDeviceToReferenceWait(m_init.InitWheels, timer.residual());
        }

        if(/*( m_ptrMultiStageX->getNdevices() > 0) &&  */!(m_init.InitStageX.empty()))
        {
            m_ptrMultiStageX->moveDeviceToReferenceWait(m_init.InitStageX, timer.residual());
        }
        if(/*( m_ptrMultiStageXY->getNdevices() > 0) &&  */!(m_init.InitStageXY.empty()))
        {
            m_ptrMultiStageXY->moveDeviceToReferenceWait(m_init.InitStageXY, timer.residual());
        }
//        if((m_ptrMultiWheel->getNdevices() > 0) && !(m_init.InitWheels.empty()))
//        {
//            m_ptrMultiWheel->moveDeviceToReference(m_init.InitWheels);
//        }
    }
    catch(std::exception e)
    {
        E_LLOG(e.what());
        U8_LLOG("");
        throw e;
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
}


void motionCtrl::moveDeviceHome(const std::vector<std::string> &_vecDevAlias)
{
    U6_LLOG(__FUNCTION__);

    std::vector<std::string> tmpVecWheel;
    std::vector<std::string> tmpVecStageX;
    std::vector<std::string> tmpVecStageXY;
    std::string logMsg = "";

    std::vector<std::string>::const_iterator it;
    for(it = _vecDevAlias.begin(); it != _vecDevAlias.end(); ++it)
    {
        if(m_ptrMultiWheel->isDevice(*it))
        {
            tmpVecWheel.push_back(*it);
            continue;
        }
        if(m_ptrMultiStageX->isDevice(*it))
        {
            tmpVecStageX.push_back(*it);
            continue;
        }
        if(m_ptrMultiStageXY->isDevice(*it))
        {
            tmpVecStageXY.push_back(*it);
            continue;
        }
    }
    // swich state machine to "busy"
    m_ptrStateMachine->process_event(osfsm::ready_busy(tmpVecWheel.size()));
    su::Timer timer(Nice::Time::seconds(80));
    try
    {
        m_ptrMultiWheel->moveDeviceToReference(tmpVecWheel);
        m_ptrMultiStageX->moveDeviceToReference(tmpVecStageX);
        m_ptrMultiStageXY->moveDeviceToReference(tmpVecStageXY);

        m_ptrMultiWheel->moveDeviceToReferenceWait(tmpVecWheel, timer.residual());
        m_ptrMultiStageX->moveDeviceToReferenceWait(tmpVecStageX, timer.residual());
        m_ptrMultiStageXY->moveDeviceToReferenceWait(tmpVecStageXY, timer.residual());
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    if(timer.elapsed())
    {
        E_LLOG(__FUNCTION__ << " timed out!");
        U8_LLOG("");
        throw std::runtime_error("motion_ctrl TIMED OUT while moving to home position.  Elapsed time = " + timer.get_duration().toString());
    }
    U8_LLOG("");
}


int motionCtrl::moveDeviceToNegativeLimit(const std::vector<std::string> &_vecDevAlias)
{
    U6_LLOG(__FUNCTION__);
    std::vector<std::string> tmpVecStageX;
    std::string logMsg = "";

    std::vector<std::string>::const_iterator it;
    for(it = _vecDevAlias.begin(); it != _vecDevAlias.end(); ++it)
    {
        if(m_ptrMultiStageX->isDevice(*it))
        {
            tmpVecStageX.push_back(*it);
            continue;
        }
        else
        {
            E_LLOG("Error: tryed to move " << *it << " to negative limit. Only Motion Stage X allowed for this operation");
            continue;
        }
    }
    // swich state machine to "busy"
//    m_ptrStateMachine->process_event(osfsm::ready_busy(tmpVecWheel.size()));


    try
    {
        m_ptrMultiStageX->moveToNegativeLimitStart(tmpVecStageX);
        m_ptrMultiStageX->moveToNegativeLimitWait(tmpVecStageX, Nice::Time::seconds(80));
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }


    U8_LLOG("");
    return EXIT_SUCCESS;
}


int motionCtrl::moveDeviceToPositiveLimit(const std::vector<std::string> &_vecDevAlias)
{
    U6_LLOG(__FUNCTION__);
    std::vector<std::string> tmpVecStageX;
    std::string logMsg = "";

    std::vector<std::string>::const_iterator it;
    for(it = _vecDevAlias.begin(); it != _vecDevAlias.end(); ++it)
    {
        if(m_ptrMultiStageX->isDevice(*it))
        {
            tmpVecStageX.push_back(*it);
            continue;
        }
        else
        {
            E_LLOG("Error: tryed to move " << *it << " to positive limit. Only Motion Stage X allowed for this operation");
            continue;
        }
    }
    // swich state machine to "busy"
//    m_ptrStateMachine->process_event(osfsm::ready_busy(tmpVecWheel.size()));

    try
    {
        m_ptrMultiStageX->moveToPositiveLimitStart(tmpVecStageX);
        m_ptrMultiStageX->moveToPositiveLimitWait(tmpVecStageX, Nice::Time::seconds(80));
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }

    U8_LLOG("");
    return EXIT_SUCCESS;
}


void motionCtrl::setup(const std::map<std::string, std::string> &_mapNamedPos)
{
    U6_LLOG(__FUNCTION__);
    try
    {
        setupStart(_mapNamedPos);
        setupWait(_mapNamedPos, Nice::Time::seconds(180));
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
}


void motionCtrl::setupPrepare(const std::map<std::string, std::string>& _mapNamedPos, motionSetup & setup)
{
    U6_LLOG(__FUNCTION__);
//    std::map<std::string, std::tr1::tuple<int, int> > NamedPosMap;
    std::string svcNameXY = "";
    int idxPos;
    std::map<std::string, std::string>::const_iterator it;
    shins::util::EShinsMovements detected;
    double absolute, relative = 0;
    std::string unit;
    setup.clear();
    for(it = _mapNamedPos.begin(); it != _mapNamedPos.end(); ++it)
    {
        if(m_ptrMultiWheel->isDevice(it->first))
        {
            detected = shins::util::detect_command_type(it->second, absolute, relative, unit);
            U9_LLOG("Element : " << std::setw(20) << it->first << "  <> Value : " << it->second);
            switch(detected)
            {
            case shins::util::SHINS_MOV_NAMED:
            {
                const std::map<std::string, int> & nps = m_ptrMultiWheel->GetMultiWheel().find(it->first)->second.namedIds;
                if(nps.find(it->second) != nps.end())
                {
                    idxPos = nps.find(it->second)->second;
                }
                else
                {
                    std::stringstream msg;
                    msg << "Invalid Argument : [" << it->first << ", " << it->second << "]";
                    E_LLOG(msg.str());
                    U8_LLOG("");
                    throw(std::invalid_argument(msg.str()));
                }
                setup.NamedMapWheel.insert(std::pair<std::string, int>(it->first, idxPos));
            }
                break;
            case shins::util::SHINS_MOV_ABSOLUTE:
                U9_LLOG("Detected Absolute Position : " << it->first << " -> " << it->second << "; value = " << absolute << "; unit = " << unit);
                setup.AbsMapWheel.insert(std::pair<std::string, double>(it->first, absolute));
                setup.AbsMapWheelUnits.insert(std::pair<std::string, std::string>(it->first, unit));
                break;
            case shins::util::SHINS_MOV_RELATIVE:
                U9_LLOG("Detected Relative Position : " << it->first << " -> " << it->second << "; value = " << relative << "; unit = " << unit);
                setup.RelMapWheel.insert(std::pair<std::string, double>(it->first, relative));
                setup.RelMapWheelUnits.insert(std::pair<std::string, std::string>(it->first, unit));
                break;
            case shins::util::SHINS_MOV_UNDEFINED:
                {
                    std::stringstream msg;
                    msg << "Invalid Argument : [" << it->first << ", " << it->second << "]";
                    U9_LLOG(msg.str());
                    U8_LLOG("");
                    throw(std::invalid_argument(msg.str()));
                }
            }
            continue;
        }
        if(m_ptrMultiStageX->isDevice(it->first))
        {
            detected = shins::util::detect_command_type(it->second, absolute, relative, unit);
            U9_LLOG("Element : " << std::setw(20) << it->first << "  <> Value : " << it->second);
            switch(detected)
            {
            case shins::util::SHINS_MOV_NAMED:
                {
                const std::map<std::string, int> & nps = m_ptrMultiStageX->GetMultiStageX().find(it->first)->second.namedIds;
                if(nps.find(it->second) != nps.end())
                {
                    idxPos = nps.find(it->second)->second;
                    U9_LLOG("Element : " << std::setw(20) << it->first << "  <> Value : " << it->second);
                }
                else
                {
                    std::stringstream msg;
                    msg << "Invalid Argument : [" << it->first << ", " << it->second << "]";
                    E_LLOG(msg.str());
                    U8_LLOG("");
                    throw(std::invalid_argument(msg.str()));
                }
                }
                setup.NamedMapStageX.insert(std::pair<std::string, int>(it->first, idxPos));
//                NamedPosMap.clear();
                break;
            case shins::util::SHINS_MOV_ABSOLUTE:
                U9_LLOG("Detected Absolute Position : " << it->first << "-> " << it->second << "; value = " << absolute);
                setup.AbsMapStageX.insert(std::pair<std::string, double>(it->first, absolute));
                setup.AbsMapStageXUnits.insert(std::pair<std::string, std::string>(it->first, unit));
                break;
            case shins::util::SHINS_MOV_RELATIVE:
                U9_LLOG("Detected Relative Position : " << it->first << "-> " << it->second << "; value = " << relative);
                setup.RelMapStageX.insert(std::pair<std::string, double>(it->first, relative));
                setup.RelMapStageXUnits.insert(std::pair<std::string, std::string>(it->first, unit));
                break;
            case shins::util::SHINS_MOV_UNDEFINED:
                {
                    std::stringstream msg;
                    msg << "Invalid Argument : [" << it->first << ", " << it->second << "]";
                    E_LLOG(msg.str());
                    U8_LLOG("");
                    throw(std::invalid_argument(msg.str()));
                }
            }
            continue;
        }
        if(boost::algorithm::starts_with(it->first, "INBEAM_TT"))
        {
            U9_LLOG("Element : " << std::setw(20) << it->first << "  <> Value : " << it->second);
            std::string first_end = shins::util::right(it->first, 2);
            if(first_end == "_X" || first_end == "_Y")
            {
                detected = shins::util::detect_command_type(it->second, absolute, relative, unit);
                switch(detected)
                {
                case shins::util::SHINS_MOV_NAMED:
                case shins::util::SHINS_MOV_UNDEFINED:
                    {
                        std::stringstream msg;
                        msg << "Invalid Argument : [" << it->first << ", " << it->second << "]";
                        E_LLOG(msg.str());
                        U8_LLOG("");
                        throw(std::invalid_argument(msg.str()));
                    }
                    break;
                case shins::util::SHINS_MOV_ABSOLUTE:
                    setup.AbsMapStageXY.insert(std::pair<std::string, double>(it->first, absolute));
                    setup.AbsMapStageXYUnits.insert(std::pair<std::string, std::string>(it->first, unit));
                    break;
                case shins::util::SHINS_MOV_RELATIVE:
                    setup.AbsMapStageXY.insert(std::pair<std::string, double>(it->first, absolute));
                    setup.AbsMapStageXYUnits.insert(std::pair<std::string, std::string>(it->first, unit));
                    break;
                }
            }
            else
            {
                if(m_ptrMultiStageXY->isDevice(it->first))
                {
                    bool bError = false;
                    switch(detected)
                    {
                    case shins::util::SHINS_MOV_NAMED:
                    {
                        int idxPos;
                        std::map<std::string, StageXYDevice>::const_iterator itStageXy = m_ptrMultiStageXY->GetMultiStageXY().find(it->first);
                        if(itStageXy != m_ptrMultiStageXY->GetMultiStageXY().end())
                        {
                            const std::map<std::string, int> & nps = itStageXy->second.namedIds;
                            if(nps.find(it->second) != nps.end())
                            {
                                idxPos = nps.find(it->second)->second;
                            }
                            else
                                bError = true;
                        }
                        else
                            bError = true;
                        if(bError)
                        {
                            std::stringstream msg;
                            msg << "Invalid Argument : [" << it->first << ", " << it->second << "]";
                            E_LLOG(msg.str());
                            U8_LLOG("");
                            throw(std::invalid_argument(msg.str()));
                        }
                        setup.NamedMapStageXY.insert(std::pair<std::string, int>(it->first, idxPos));
                    }
                    break;
                    }
                }
                else
                {
                    std::stringstream msg;
                    msg << "Invalid Argument : [" << it->first << ", " << it->second << "]";
                    E_LLOG(msg.str());
                    U8_LLOG("");
                    throw(std::invalid_argument(msg.str()));
                }
            }
        }
    }
    U8_LLOG("");
}


void motionCtrl::setupStart(const std::map<std::string, std::string> &_mapNamedPos)
{
    U6_LLOG(__FUNCTION__);
    setupPrepare(_mapNamedPos, m_setup);
    //m_mtxSeq.lock();
    // swich state machine to "busy"
    // U2_LLOG("State Machine state: " << *(m_ptrStateMachine->current_state()));
    m_moving_devs = m_setup.NamedMapWheel.size() + m_setup.NamedMapStageX.size();
    U9_LLOG(__FUNCTION__ << " ~ calling process event to SM");
    m_ptrStateMachine->process_event(osfsm::ready_busy(m_setup.NamedMapWheel.size() + m_setup.NamedMapStageX.size()));    //+
    U9_LLOG(__FUNCTION__ << " ~ Finished calling process event to SM");
    // U2_LLOG("State Machine state: " << *(m_ptrStateMachine->current_state()));
    // U2_LLOG(*(m_ptrStateMachine->event_(osfsm::readyDone)));
    // IceUtil::Monitor<IceUtil::Mutex>::Lock lock(*(m_ptrMonSeq));
    try
    {
        m_ptrMultiWheel->moveToNamedPositionStart(m_setup.NamedMapWheel);
        m_ptrMultiWheel->moveToAbsolutePositionStart(m_setup.AbsMapWheel, m_setup.AbsMapWheelUnits);
        m_ptrMultiWheel->moveToRelativePositionStart(m_setup.RelMapWheel, m_setup.RelMapWheelUnits);
        m_ptrMultiStageX->moveToNamedPositionStart(m_setup.NamedMapStageX);
        m_ptrMultiStageX->moveToAbsolutePositionStart(m_setup.AbsMapStageX, m_setup.AbsMapStageXUnits);
        m_ptrMultiStageX->moveToRelativePositionStart(m_setup.RelMapStageX, m_setup.RelMapStageXUnits);
        m_ptrMultiStageXY->moveToNamedPositionStart(m_setup.NamedMapStageXY);
        m_ptrMultiStageXY->moveToAbsolutePositionStart(m_setup.AbsMapStageXY, m_setup.AbsMapStageXYUnits);
        m_ptrMultiStageXY->moveToRelativePositionStart(m_setup.RelMapStageXY, m_setup.RelMapStageXYUnits);
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    
    U8_LLOG("");
}


void motionCtrl::setupWait(const std::map<std::string, std::string> &_mapNamedPos, const Nice::Time &_time)
{
    U6_LLOG(__FUNCTION__);
    setupPrepare(_mapNamedPos, m_setupWait);
    su::Timer timer(_time);
//    boost::chrono::steady_clock::duration delay = boost::chrono::seconds(120);
//    boost::chrono::steady_clock::duration deltat;

    try
    {
        m_ptrMultiWheel->moveToNamedPositionWait(m_setupWait.NamedMapWheel, timer.residual());
        m_ptrMultiWheel->moveToAbsolutePositionWait(m_setupWait.AbsMapWheel, timer.residual());
        m_ptrMultiWheel->moveToRelativePositionWait(m_setupWait.RelMapWheel, timer.residual());
        m_ptrMultiStageX->moveToNamedPositionWait(m_setupWait.NamedMapStageX, timer.residual());
        m_ptrMultiStageX->moveToAbsolutePositionWait(m_setupWait.AbsMapStageX, timer.residual());
        m_ptrMultiStageX->moveToRelativePositionWait(m_setupWait.RelMapStageX, timer.residual());
        m_ptrMultiStageXY->moveToNamedPositionWait(m_setupWait.NamedMapStageXY, timer.residual());
        m_ptrMultiStageXY->moveToAbsolutePositionWait(m_setupWait.AbsMapStageXY, timer.residual());
        m_ptrMultiStageXY->moveToRelativePositionWait(m_setupWait.RelMapStageXY, timer.residual());
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }

    U8_LLOG("motionCtrl::" << __FUNCTION__);
}


// moving to arbitrary position in steps. NOTE that here the input map can
// contain the position of INBEAT_TT expressed with X and Y coordinates
void motionCtrl::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 motionCtrl::moveToAbsolutePositionStart(const std::map<std::string, double> &_mapPos,
    const std::map<std::string, std::string> &_mapUnit)
{
    U6_LLOG(__FUNCTION__);
    std::map<std::string, double> AbsMapWheel;
    std::map<std::string, double> AbsMapStageX;
    std::map<std::string, double> AbsMapStageXY;
    std::string strAlias = "";
    int movingDevices = 0;
    // boost::chrono::steady_clock::duration delay = boost::chrono::seconds(120);
//    boost::chrono::steady_clock::duration deltat;
    std::map<std::string, double>::const_iterator it;
    for(it = _mapPos.begin(); it != _mapPos.end(); ++it)
    {
        D_LLOG(__FUNCTION__ << ":" << __LINE__ << ":" << it->first);

        if(m_ptrMultiWheel->isDevice(it->first))
        {
            AbsMapWheel.insert(*it);
            continue;
        }
        if(m_ptrMultiStageX->isDevice(it->first))
        {
            AbsMapStageX.insert(*it);
            continue;
        }
        if(boost::algorithm::starts_with(it->first, "INBEAM_TT_"))
        {
            strAlias = "INBEAM_TT";
            // if the service INBEAM_TT is active then add the two coodinates to
            // the temporary map for motion.
            if(m_ptrMultiStageXY->isDevice(strAlias))
            {
                // std::cout << "stageXY" << std::endl;
                // to move XY stage we need X and Y positions
                AbsMapStageXY.insert(*it);
                continue;
            }
        }
    }
    // switch state machine to "busy"
    movingDevices = AbsMapWheel.size() + AbsMapStageX.size() + (AbsMapStageXY.size() / 2); // the XY stage equals to 1 device
    m_ptrStateMachine->process_event(osfsm::ready_busy(movingDevices));
    m_moving_devs = AbsMapWheel.size() + AbsMapStageX.size() + (AbsMapStageXY.size() / 2);
    try
    {
        if(AbsMapWheel.size() > 0)
        {
            m_ptrMultiWheel->moveToAbsolutePositionStart(AbsMapWheel, _mapUnit);
        }
        if(AbsMapStageX.size() > 0)
        {
            m_ptrMultiStageX->moveToAbsolutePositionStart(AbsMapStageX, _mapUnit);
        }
        if(AbsMapStageXY.size() > 0)
        {
            m_ptrMultiStageXY->moveToAbsolutePositionStart(AbsMapStageXY, _mapUnit);
        }
    }
    catch(Basda::Svc::MoccaMotionException basdaExecp)
    {
        E_LLOG(basdaExecp.what());
        U8_LLOG("");
        throw(basdaExecp);
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }

    U8_LLOG("");
}


void motionCtrl::moveToAbsolutePositionWait(const std::map<std::string, double> &_mapPos, const Nice::Time &_time)
{
    U6_LLOG(__FUNCTION__);
    std::map<std::string, double> AbsMapWheel;
    std::map<std::string, double> AbsMapStageX;
    std::map<std::string, double> AbsMapStageXY;
    std::string strAlias = "";
    su::Timer timer(_time);
    // int movingDevices = 0;
    // boost::chrono::steady_clock::duration delay = boost::chrono::seconds(120);
//    boost::chrono::steady_clock::duration deltat;

    std::map<std::string, double>::const_iterator it;
    for(it = _mapPos.begin(); it != _mapPos.end(); ++it)
    {
        D_LLOG("_mapPos.first : " << it->first);

        if(m_ptrMultiWheel->isDevice(it->first))
        {
            // std::cout << "wheel" << std::endl;
            AbsMapWheel.insert(*it);
            continue;
        }
        if(m_ptrMultiStageX->isDevice(it->first))
        {
            // std::cout << "stageX" << std::endl;
            AbsMapStageX.insert(*it);
            continue;
        }
        if(boost::algorithm::starts_with(it->first, "INBEAM_TT_"))
        {
            strAlias = "INBEAM_TT";
            // if the service INBEAM_TT is active then add the two coodinates to
            // the temporary map for motion.
            if(m_ptrMultiStageXY->isDevice(strAlias))
            {
                // std::cout << "stageXY" << std::endl;
                // to move XY stage we need X and Y positions
                AbsMapStageXY.insert(*it);
                continue;
            }
        }
    }
    try
    {
        if(AbsMapWheel.size() > 0)
            m_ptrMultiWheel->moveToAbsolutePositionWait(AbsMapWheel, timer.residual());
        if(AbsMapStageX.size() > 0)
            m_ptrMultiStageX->moveToAbsolutePositionWait(AbsMapStageX, timer.residual());
        if(AbsMapStageXY.size() > 0)
            m_ptrMultiStageXY->moveToAbsolutePositionWait(AbsMapStageXY, timer.residual());
    }
    catch(Basda::Svc::MoccaMotionException basdaExecp)
    {
        E_LLOG(basdaExecp.what());
        U8_LLOG("");
        throw(basdaExecp);
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG(__FUNCTION__ << " Done");
}


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

    std::map<std::string, double> RelMapWheel;
    std::map<std::string, double> RelMapStageX;
    std::map<std::string, double> RelMapStageXY;
    std::string strAlias = "";
    int movingDevices = 0;
    // boost::chrono::steady_clock::duration delay = boost::chrono::seconds(120);
//  boost::chrono::steady_clock::duration deltat;

    std::map<std::string, double>::const_iterator it;
    for(it = _mapPos.begin(); it != _mapPos.end(); ++it)
    {
        D_LLOG("_mapPos.first :" << it->first);

        if(m_ptrMultiWheel->isDevice(it->first))
        {
            RelMapWheel.insert(*it);
            continue;
        }
        if(m_ptrMultiStageX->isDevice(it->first))
        {
            RelMapStageX.insert(*it);
            continue;
        }
        if(boost::algorithm::starts_with(it->first, "INBEAM_TT_"))
        {
            strAlias = "INBEAM_TT";
            // if the service INBEAM_TT is active then add the two coordinates to
            // the temporary map for motion.
            if(m_ptrMultiStageXY->isDevice(strAlias))
            {
                // std::cout << "stageXY" << std::endl;
                // to move XY stage we need X and Y positions
                RelMapStageXY.insert(*it);
                continue;
            }
        }
    }
    // switch state machine to "busy"
    movingDevices = RelMapWheel.size() + RelMapStageX.size() + (RelMapStageXY.size() / 2); // the XY stage equals to 1 device
    m_ptrStateMachine->process_event(osfsm::ready_busy(movingDevices));
    m_moving_devs = RelMapWheel.size() + RelMapStageX.size() + (RelMapStageXY.size() / 2);
    try
    {
        if(RelMapWheel.size() > 0)
        {
            m_ptrMultiWheel->moveToRelativePositionStart(RelMapWheel, _mapUnit);
        }
        if(RelMapStageX.size() > 0)
        {
            m_ptrMultiStageX->moveToRelativePositionStart(RelMapStageX, _mapUnit);
        }
        if(RelMapStageXY.size() > 0)
        {
            m_ptrMultiStageXY->moveToRelativePositionStart(RelMapStageXY, _mapUnit);
        }
    }
    catch(Basda::Svc::MoccaMotionException basdaExecp)
    {
        E_LLOG(basdaExecp.what());
        U8_LLOG("");
        throw(basdaExecp);
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
}


void motionCtrl::moveToRelativePositionWait(const std::map<std::string, double> &_mapPos, const Nice::Time &_time)
{
    U6_LLOG(__FUNCTION__ << " : Implements moveToRelativePositionWait");
    std::map<std::string, double> RelMapWheel;
    std::map<std::string, double> RelMapStageX;
    std::map<std::string, double> RelMapStageXY;
    std::string strAlias = "";
    su::Timer timer(_time);
    // int movingDevices = 0;
    // boost::chrono::steady_clock::duration delay = boost::chrono::seconds(120);
//  boost::chrono::steady_clock::duration deltat;

    std::map<std::string, double>::const_iterator it;
    for(it = _mapPos.begin(); it != _mapPos.end(); ++it)
    {
        D_LLOG("_mapPos.first : " << it->first);

        if(m_ptrMultiWheel->isDevice(it->first))
        {
            // std::cout << "wheel" << std::endl;
            RelMapWheel.insert(*it);
            continue;
        }
        if(m_ptrMultiStageX->isDevice(it->first))
        {
            // std::cout << "stageX" << std::endl;
            RelMapStageX.insert(*it);
            continue;
        }
        if(boost::algorithm::starts_with(it->first, "INBEAM_TT_"))
        {
            strAlias = "INBEAM_TT";
            // if the service INBEAM_TT is active then add the two coodinates to
            // the temporary map for motion.
            if(m_ptrMultiStageXY->isDevice(strAlias))
            {
                // std::cout << "stageXY" << std::endl;
                // to move XY stage we need X and Y positions
                RelMapStageXY.insert(*it);
                continue;
            }
        }
    }
    try
    {
        if(RelMapWheel.size() > 0)
            m_ptrMultiWheel->moveToRelativePositionWait(RelMapWheel, timer.residual());
        if(RelMapStageX.size() > 0)
            m_ptrMultiStageX->moveToRelativePositionWait(RelMapStageX, timer.residual());
        if(RelMapStageXY.size() > 0)
            m_ptrMultiStageXY->moveToRelativePositionWait(RelMapStageXY, timer.residual());
    }
    catch(Basda::Svc::MoccaMotionException basdaExecp)
    {
        E_LLOG(basdaExecp.what());
        U8_LLOG("");
        throw(basdaExecp);
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG(__FUNCTION__);
}


void motionCtrl::emergencyAbort()
{
    U6_LLOG(__FUNCTION__);
    if(m_ptrMultiStageX->getNdevices() > 0)
        m_ptrMultiStageX->emergencyAbort();
    if(m_ptrMultiWheel->getNdevices() > 0)
        m_ptrMultiWheel->emergencyAbort();
    if(m_ptrMultiStageXY->getNdevices() > 0)
        m_ptrMultiStageXY->emergencyAbort();
    U8_LLOG(__FUNCTION__ << " Emergency Abort!");
}


int motionCtrl::stop(const std::vector<std::string> &_vecDevAlias)
{
    U6_LLOG(__FUNCTION__ << " Stopping selected devices");
    std::vector<std::string> tmpVecWheel;
    std::vector<std::string> tmpVecStageX;
    std::vector<std::string> tmpVecStageXY;
    int status = EXIT_SUCCESS;

    std::vector<std::string>::const_iterator it;
    for(it = _vecDevAlias.begin(); it != _vecDevAlias.end(); ++it)
    {
        if(m_ptrMultiWheel->isDevice(*it))
        {
            tmpVecWheel.push_back(*it);
            continue;
        }
        if(m_ptrMultiStageX->isDevice(*it))
        {
            tmpVecStageX.push_back(*it);
            continue;
        }
        if(m_ptrMultiStageXY->isDevice(*it))
        {
            tmpVecStageXY.push_back(*it);
            continue;
        }
    }

    try
    {
        if(tmpVecStageX.size())
            m_ptrMultiStageX->stop(tmpVecStageX);
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
    }

    try
    {
        if(tmpVecWheel.size())
            m_ptrMultiWheel->stop(tmpVecWheel);
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
    }

    try
    {
        if(tmpVecStageXY.size())
            m_ptrMultiStageXY->stop(tmpVecStageXY);
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
    }
    U8_LLOG("");
    return status;
}


int motionCtrl::abort(const std::vector<std::string> &_vecDevAlias)
{
    U6_LLOG(__FUNCTION__);
    std::vector<std::string> tmpVecWheel;
    std::vector<std::string> tmpVecStageX;
    std::vector<std::string> tmpVecStageXY;
    int status = EXIT_SUCCESS;

    std::vector<std::string>::const_iterator it;
    for(it = _vecDevAlias.begin(); it != _vecDevAlias.end(); ++it)
    {
        if(m_ptrMultiWheel->isDevice(*it))
        {
            tmpVecWheel.push_back(*it);
            continue;
        }
        if(m_ptrMultiStageX->isDevice(*it))
        {
            tmpVecStageX.push_back(*it);
            continue;
        }
        if(m_ptrMultiStageXY->isDevice(*it))
        {
            tmpVecStageXY.push_back(*it);
            continue;
        }
    }

    try
    {
        if(!tmpVecStageX.empty())
            m_ptrMultiStageX->abort(tmpVecStageX);
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }

    try
    {
        if(!tmpVecWheel.empty())
            m_ptrMultiWheel->abort(tmpVecWheel);
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }

    try
    {
        if(!tmpVecStageXY.empty())
            m_ptrMultiStageXY->abort(tmpVecStageXY);
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    A_LLOG(__FUNCTION__ << " Called abort method on selected devices");

    U8_LLOG("");
    return status;
}


// The method returns a STL vector containing endpoint names as [ALIAS=aliasName].
// It scans the alias file looking for the [ALIAS= substring and extracts
// the substring between the '=' and the first ','
// an example of alias string is
// motion_ctrl-svr.nd_w.ALIAS:Endpoint=[ALIAS=nd_w, NAME=motion_ctrl, PATH=MOTION_CTRL.ND_W.SVC]
// string _strFileName: path to aliases file
// string _component:   sw componet, which can assume specific values: motion_ctrl, calunit_ctrl, track_ctrl
std::vector<std::string> motionCtrl::getAliasesFromFile(const std::string &_strFileName, const std::string &_component)
{
    U6_LLOG(__FUNCTION__ << " ~ " << _strFileName);
    std::ifstream ifsAlias(_strFileName.c_str(), std::ifstream::in);

    std::vector<std::string> vecAlias;
    std::vector<std::string> tmpVecSplit;
    std::vector<std::string>::const_iterator itFind;

    std::string strLine = "";
    std::string strAlias = "";
    std::string strTokens = "[+#,=:\"";
    std::string logMsg;

    while (std::getline(ifsAlias, strLine))
    {
        if(boost::algorithm::starts_with(strLine, _component) &&              //
            !(boost::algorithm::contains(strLine, "LABTEST")) &&              //
            (boost::algorithm::contains(strLine, "ALIAS="))) // excluding alias endpoint for server
        {
            tmpVecSplit = shins::util::split_with_boost(strLine, strTokens);
            itFind = std::find(tmpVecSplit.begin(), tmpVecSplit.end(), "ALIAS");
            if(itFind < tmpVecSplit.end())
            {
                ++itFind;
                vecAlias.push_back(*itFind);
                tmpVecSplit.clear();
                // U2_LLOG(vecAlias.back());
                continue;
            }
            else
            {
                logMsg = "Did not find alias in line: " + strLine;
                E_LLOG(logMsg);
                U8_LLOG("");
                throw std::runtime_error(logMsg);
            }
        }
    }
    ifsAlias.close();
    U8_LLOG(__FUNCTION__);
    return vecAlias;
}

void LogBoostDiagnosticInformationMotion(int line)
{
    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());
    std::stringstream log_msg;
    log_msg << "Line: " << line << " ~ " << what;
    E_LLOG(log_msg.str());
}


int motionCtrl::getJsonStatus(std::string & jsonStatus)
{
    int status = EXIT_SUCCESS;

    std::map<std::string, StageXDevice>::const_iterator itStageX;
    if(!(m_service_status == su::SERVICE_ONLINE)){ return EXIT_FAILURE;}
    const std::map<std::string, StageXDevice> & MapStageX = m_ptrMultiStageX->GetMultiStageX();
    int counter = 0;
    std::stringstream catch_msg;
    for(itStageX = MapStageX.begin(); itStageX != MapStageX.end(); itStageX++)
    {
        counter++;
        bool inLimit = false;
        bool isMoving = false;
        std::string service_name;
        try{service_name = itStageX->second.iPtr->config()->leaf("VAR.SERVICE_NAME")->get<std::string>(); /*itStageX->second.iPtr->readServiceName();*/}catch(...){catch_msg.str(""); catch_msg << service_name << " ~ readServiceName"; MotionLogBoostDiagnosticInformation(__LINE__, catch_msg.str());}
        try
        {
            isMoving = itStageX->second.iPtr->config()->leaf("VAR.MOVING")->get<bool>(); /*itStageX->second.iPtr->isMoving();*/
        }
        // catch(Nice::ChatException err)
        // {
        //     usleep(1000000);
        //     continue;
        // }
        catch(...)
        {
            catch_msg.str("");
            catch_msg << service_name << " ~ isMoving";
            MotionLogBoostDiagnosticInformation(__LINE__, catch_msg.str());
            usleep(1000000);
            continue;
        }
        try{inLimit =itStageX->second.iPtr->config()->leaf("VAR.LIMIT")->get<bool>(); /*itStageX->second.iPtr->isAtLimit();*/}catch(...){catch_msg.str(""); catch_msg << service_name << " ~ isAtLimit"; MotionLogBoostDiagnosticInformation(__LINE__, catch_msg.str());}
        std::string named_pos = itStageX->second.iPtr->config()->leaf("VAR.NAMED_POSITION")->get<std::string>();
        bool isSimulated = false;
        Nice::BitSet bs;
        bs.depth = 8;
        bs.value = Nice::U32(pow(2, bs.depth) - 1);
        Nice::SeqBitSet switch_status = Nice::SeqBitSet(1, bs);
        try{switch_status = itStageX->second.iPtr->config()->leaf("VAR.SWITCH_STATUS")->get<Nice::SeqBitSet>(); isSimulated = switch_status[0].getDepth() < 32;}catch(...){E_LLOG(service_name << " ~ getPositionSwitchStatus"); }
        bool isAtHome;
        try{isAtHome = itStageX->second.iPtr->config()->leaf("VAR.ATHOME")->get<bool>(); /*itStageX->second.iPtr->isAtHome();*/}catch(...){catch_msg.str(""); catch_msg << service_name << " ~ isAtHome"; MotionLogBoostDiagnosticInformation(__LINE__, catch_msg.str());}
//         if(itStageX->second.iPtr->isAtLimit())
//         {
//             try{U9_LLOG("isAtLimit2 : " << itStageX->second.iPtr->get<int>("VAR.LIMIT"));}catch(...){E_LLOG("isAtLimit2");}
//             std::string limit_result = itStageX->second.iPtr->config()->getAtNode<std::string>("VAR.LIMIT");
//             U9_LLOG("PropertyName : " << "VAR.LIMIT" << "; limit_result : " << limit_result);
//             itStageX->second.iPtr->config()->setAtNode<std::string>("VAR.LIMIT", "-2");
//             limit_result = itStageX->second.iPtr->config()->getAtNode<std::string>("VAR.LIMIT");
//             U9_LLOG("PropertyName : " << "VAR.LIMIT" << "; limit_result : " << limit_result);
//             limit_result = itStageX->second.iPtr->config()->leaf("VAR.LIMIT")->get<std::string>();
//             U9_LLOG("PropertyName : " << "VAR.LIMIT" << "; limit_result : " << limit_result);
// //                val.limit = m_node->getAtNode<double> ("LIMIT")
//             double limit_double = itStageX->second.iPtr->config()->getAtNode<double>("LIMIT");
//             U9_LLOG("PropertyName : " << "LIMIT" << "; limit_double : " << limit_double);
//         }
//        try{U9_LLOG("NamedPosition : " << named_pos);}catch(...){E_LLOG("Named Position");}
//        try{U9_LLOG("getNamedPosition : " << itStageX->second.iPtr->getNamedPosition(1));}catch(...){E_LLOG("getNamedPosition");}
//        try{U9_LLOG("readPosition : " << itStageX->second.iPtr->readPosition());}catch(...){E_LLOG("readPosition");}
//        try{U9_LLOG("readDeviceEncoderPosition : " << itStageX->second.iPtr->readDeviceEncoderPosition());}catch(...){E_LLOG("readDeviceEncoderPosition");}
//        try{U9_LLOG("readAbsoluteEncoderPosition : " << itStageX->second.iPtr->readAbsoluteEncoderPosition());}catch(...){E_LLOG("readAbsoluteEncoderPosition");}
//        try{U9_LLOG("readIncrementalEncoderPosition : " << itStageX->second.iPtr->readIncrementalEncoderPosition());}catch(...){E_LLOG("readIncrementalEncoderPosition");}
        bool isReachable = false;
        try{isReachable = itStageX->second.iPtr->config()->leaf("CMD.ISREACHABLE.RET")->get<bool>(); /*itStageX->second.iPtr->isReachable();*/}catch(...){catch_msg.str(""); catch_msg << service_name << " ~ isReachable"; MotionLogBoostDiagnosticInformation(__LINE__, catch_msg.str());}
//        try{U9_LLOG("getPosition : " << itStageX->second.iPtr->getPosition());}catch(...){E_LLOG("getPosition");}
        int deviceEncoderPosition = -100000000;
        try{deviceEncoderPosition = itStageX->second.iPtr->config()->leaf("VAR.POSITION.DEVICE_ENCODER.STEPS")->get<double>(); /*itStageX->second.iPtr->getDeviceEncoderPosition();*/}catch(...){catch_msg.str(""); catch_msg << service_name << " ~ getDeviceEncoderPosition"; MotionLogBoostDiagnosticInformation(__LINE__, catch_msg.str());}
//        try{U9_LLOG("getAbsoluteEncoderPosition : " << itStageX->second.iPtr->getAbsoluteEncoderPosition());}catch(...){E_LLOG("getAbsoluteEncoderPosition");}
//        try{U9_LLOG("getIncrementalEncoderPosition : " << itStageX->second.iPtr->getIncrementalEncoderPosition());}catch(...){E_LLOG("getIncrementalEncoderPosition");}
//        try{U9_LLOG("getVelocity : " << itStageX->second.iPtr->getVelocity());}catch(...){E_LLOG("getVelocity");}

        std::stringstream json_alias;
        try{
            json_alias << "\"" << itStageX->first << "\": { \"name\": \"" << service_name << "\", \"named\": \"" << named_pos << "\", \"encoder\": " << deviceEncoderPosition << ", ";
            json_alias << "\"is_moving\": " << isMoving << ", \"in_limit\": " << inLimit << ", \"available\": " << isReachable << ", \"simulated\": " << isSimulated << "}";
            jsonStatus += json_alias.str();
            if(counter < MapStageX.size())jsonStatus += ", ";
           }
        catch(...){
            U9_LLOG("Not found : " << itStageX->first);
        }
    }

    std::map<std::string, WheelDevice>::const_iterator itWheel;
    const std::map<std::string, WheelDevice> & MapWheel = m_ptrMultiWheel->GetMultiWheel();
    if(!MapWheel.empty() && counter > 0)
    {
        jsonStatus += ", ";
        counter = 0;
    }
    for(itWheel = MapWheel.begin(); itWheel != MapWheel.end(); itWheel++)
    {
        counter++;
        std::string named_pos = itWheel->second.iPtr->config()->leaf("VAR.NAMED_POSITION")->get<std::string>();
        std::string service_name;
        try{service_name = itWheel->second.iPtr->config()->leaf("VAR.SERVICE_NAME")->get<std::string>(); /*itWheel->second.iPtr->readServiceName();*/}catch(...){catch_msg.str(""); catch_msg << service_name << " ~ readServiceName"; MotionLogBoostDiagnosticInformation(__LINE__, catch_msg.str());}
        bool isSimulated = false;
        Nice::SeqBitSet switch_status;
        // try{switch_status = itWheel->second.iPtr->getPositionSwitchStatus(); isSimulated = switch_status.size() < 32;}catch(...){E_LLOG(service_name << " ~ getPositionSwitchStatus"); LogBoostDiagnosticInformationMotion(__LINE__);}
//        try{U9_LLOG("getPositionSwitchStatus : " << switch_status);}catch(...){E_LLOG("getPositionSwitchStatus");}
        bool isAtHome;
        try{isAtHome = itWheel->second.iPtr->config()->leaf("VAR.ATHOME")->get<bool>(); /*itWheel->second.iPtr->isAtHome();*/}catch(...){catch_msg.str(""); catch_msg << service_name << " ~ isAtHome"; MotionLogBoostDiagnosticInformation(__LINE__, catch_msg.str());}
        bool inLimit = false;
//         try{U9_LLOG("isAtLimit : " << itWheel->second.iPtr->isAtLimit());}catch(...){E_LLOG("isAtLimit");}
//         if(itWheel->second.iPtr->isAtLimit())
//         {
//             std::string limit_result = itWheel->second.iPtr->config()->getAtNode<std::string>("VAR.LIMIT");
//             U9_LLOG("PropertyName : " << "VAR.LIMIT" << "; limit_result : " << limit_result);
//             itWheel->second.iPtr->config()->setAtNode<std::string>("VAR.LIMIT", "-2");
//             limit_result = itWheel->second.iPtr->config()->getAtNode<std::string>("VAR.LIMIT");
//             U9_LLOG("PropertyName : " << "VAR.LIMIT" << "; limit_result : " << limit_result);
//             limit_result = itWheel->second.iPtr->config()->leaf("VAR.LIMIT")->get<std::string>();
//             U9_LLOG("PropertyName : " << "VAR.LIMIT" << "; limit_result : " << limit_result);
// //                val.limit = m_node->getAtNode<double> ("LIMIT")
//             double limit_double = itWheel->second.iPtr->config()->getAtNode<double>("LIMIT");
//             U9_LLOG("PropertyName : " << "LIMIT" << "; limit_double : " << limit_double);
//         }
        bool isMoving = false;
        try{isMoving = itWheel->second.iPtr->config()->leaf("VAR.MOVING")->get<bool>(); /*itWheel->second.iPtr->isMoving();*/}catch(...){catch_msg.str(""); catch_msg << service_name << " ~ isMoving"; MotionLogBoostDiagnosticInformation(__LINE__, catch_msg.str());}
//        try{U9_LLOG("NamedPosition : " << named_pos);}catch(...){E_LLOG("Named Position");}
//        try{U9_LLOG("getNamedPosition : " << itWheel->second.iPtr->getNamedPosition(1));}catch(...){E_LLOG("getNamedPosition");}
//        try{U9_LLOG("readPosition : " << itWheel->second.iPtr->readPosition());}catch(...){E_LLOG("readPosition");} // signals cast error
//        try{U9_LLOG("readDeviceEncoderPosition : " << itWheel->second.iPtr->readDeviceEncoderPosition());}catch(...){E_LLOG("readDeviceEncoderPosition");}
//        try{U9_LLOG("readAbsoluteEncoderPosition : " << itWheel->second.iPtr->readAbsoluteEncoderPosition());}catch(...){E_LLOG("readAbsoluteEncoderPosition");}
//        try{U9_LLOG("readIncrementalEncoderPosition : " << itWheel->second.iPtr->readIncrementalEncoderPosition());}catch(...){E_LLOG("readIncrementalEncoderPosition");}
        bool isReachable = false;
        try{isReachable = itWheel->second.iPtr->config()->leaf("CMD.ISREACHABLE.RET")->get<bool>(); /*itWheel->second.iPtr->isReachable();*/}catch(...){catch_msg.str(""); catch_msg << service_name << " ~ isReachable"; MotionLogBoostDiagnosticInformation(__LINE__, catch_msg.str());}
//        try{U9_LLOG("getPosition : " << itWheel->second.iPtr->getPosition());}catch(...){E_LLOG("getPosition");}
        int deviceEncoderPosition = -100000000;
        try{deviceEncoderPosition = itWheel->second.iPtr->config()->leaf("VAR.POSITION.DEVICE_ENCODER.STEPS")->get<int>(); /*itWheel->second.iPtr->getDeviceEncoderPosition();*/}catch(...){catch_msg.str(""); catch_msg << service_name << " ~ getDeviceEncoderPosition"; MotionLogBoostDiagnosticInformation(__LINE__, catch_msg.str());}
//        try{U9_LLOG("getAbsoluteEncoderPosition : " << itWheel->second.iPtr->getAbsoluteEncoderPosition());}catch(...){E_LLOG("getAbsoluteEncoderPosition");}
//        try{U9_LLOG("getIncrementalEncoderPosition : " << itWheel->second.iPtr->getIncrementalEncoderPosition());}catch(...){E_LLOG("getIncrementalEncoderPosition");}
//        try{U9_LLOG("getVelocity : " << itWheel->second.iPtr->getVelocity());}catch(...){E_LLOG("getVelocity");}

        std::stringstream json_alias;
        json_alias << "\"" << itWheel->first << "\": { \"name\": \"" << service_name << "\", \"named\": \"" << named_pos << "\", \"encoder\": " << deviceEncoderPosition << ", ";
        json_alias << "\"is_moving\": " << isMoving << ", \"in_limit\": " << inLimit << ", \"available\": " << isReachable << ", \"simulated\": " << isSimulated << "}";
        jsonStatus += json_alias.str();
        if(counter < MapWheel.size())jsonStatus += ", ";
    }
   
    std::map<std::string, StageXYDevice>::const_iterator itStageXY;
    const std::map<std::string, StageXYDevice> & MapStageXY = m_ptrMultiStageXY->GetMultiStageXY();
    if(!MapStageXY.empty() && counter > 0)
    {
        jsonStatus += ", ";
        counter = 0;
    }
    for(itStageXY = MapStageXY.begin(); itStageXY != MapStageXY.end(); itStageXY++)
    {
        counter++;
        std::string named_pos = itStageXY->second.iPtr->config()->leaf("VAR.NAMED_POSITION")->get<std::string>();
        std::string service_name;
        try{service_name = itStageXY->second.iPtr->config()->leaf("VAR.SERVICE_NAME")->get<std::string>(); /*itStageXY->second.iPtr->readServiceName();*/}catch(...){catch_msg.str(""); catch_msg << service_name << " ~ readServiceName"; MotionLogBoostDiagnosticInformation(__LINE__, catch_msg.str());}
        bool isSimulated = false;
        Nice::SeqBitSet switch_status;
        // try{switch_status = itStageXY->second.iPtr->getPositionSwitchStatus(); isSimulated = switch_status.size() < 32;}catch(...){E_LLOG(service_name << " ~ getPositionSwitchStatus");}
//        try{U9_LLOG("getPositionSwitchStatus : " << switch_status);}catch(...){E_LLOG("getPositionSwitchStatus");}
        bool isAtHome;
        try{isAtHome = itStageXY->second.iPtr->config()->leaf("VAR.ATHOME")->get<bool>(); /*itStageXY->second.iPtr->isAtHome();*/}catch(...){catch_msg.str(""); catch_msg << service_name << " ~ isAtHome"; MotionLogBoostDiagnosticInformation(__LINE__, catch_msg.str());}
        bool inLimit = false;
        try{inLimit = itStageXY->second.iPtr->config()->leaf("VAR.LIMIT")->get<bool>(); /*itStageXY->second.iPtr->isAtLimit();*/}catch(...){catch_msg.str(""); catch_msg << service_name << " ~ isAtLimit"; MotionLogBoostDiagnosticInformation(__LINE__, catch_msg.str());}
        if(inLimit)
        {
//             std::string limit_result = itStageXY->second.iPtr->config()->getAtNode<std::string>("VAR.LIMIT");
//             U9_LLOG("PropertyName : " << "VAR.LIMIT" << "; limit_result : " << limit_result);
//             itStageXY->second.iPtr->config()->setAtNode<std::string>("VAR.LIMIT", "-2");
//             limit_result = itStageXY->second.iPtr->config()->getAtNode<std::string>("VAR.LIMIT");
//             U9_LLOG("PropertyName : " << "VAR.LIMIT" << "; limit_result : " << limit_result);
//             limit_result = itStageXY->second.iPtr->config()->leaf("VAR.LIMIT")->get<std::string>();
//             U9_LLOG("PropertyName : " << "VAR.LIMIT" << "; limit_result : " << limit_result);
// //                val.limit = m_node->getAtNode<double> ("LIMIT")
//             double limit_double = itStageXY->second.iPtr->config()->getAtNode<double>("LIMIT");
//             U9_LLOG("PropertyName : " << "LIMIT" << "; limit_double : " << limit_double);
        }
        bool isMoving = false;
        try{isMoving = itStageXY->second.iPtr->config()->leaf("VAR.MOVING")->get<bool>(); /*itStageXY->second.iPtr->isMoving();*/}catch(...){catch_msg.str(""); catch_msg << service_name << " ~ isMoving"; MotionLogBoostDiagnosticInformation(__LINE__, catch_msg.str());}
//        try{U9_LLOG("NamedPosition : " << named_pos);}catch(...){E_LLOG("Named Position");}
//        try{U9_LLOG("getNamedPosition : " << itStageXY->second.iPtr->getNamedPosition(1));}catch(...){E_LLOG("getNamedPosition");}
//        try{U9_LLOG("readPosition : " << itStageXY->second.iPtr->readPosition());}catch(...){E_LLOG("readPosition");}
//        try{U9_LLOG("readDeviceEncoderPosition : " << itStageXY->second.iPtr->readDeviceEncoderPosition());}catch(...){E_LLOG("readDeviceEncoderPosition");}
//        try{U9_LLOG("readAbsoluteEncoderPosition : " << itStageXY->second.iPtr->readAbsoluteEncoderPosition());}catch(...){E_LLOG("readAbsoluteEncoderPosition");}
//        try{U9_LLOG("readIncrementalEncoderPosition : " << itStageXY->second.iPtr->readIncrementalEncoderPosition());}catch(...){E_LLOG("readIncrementalEncoderPosition");}
        bool isReachable = false;
        try{isReachable = itStageXY->second.iPtr->config()->leaf("CMD.ISREACHABLE.RET")->get<bool>(); /*itStageXY->second.iPtr->isReachable();*/}catch(...){catch_msg.str(""); catch_msg << service_name << " ~ isReachable"; MotionLogBoostDiagnosticInformation(__LINE__, catch_msg.str());}
//        try{U9_LLOG("getPosition : " << itStageXY->second.iPtr->getPosition());}catch(...){E_LLOG("getPosition");}
        Nice::Point deviceEncoderPosition;
        try{deviceEncoderPosition = itStageXY->second.iPtr->config()->leaf("VAR.POSITION.DEVICE_ENCODER.STEPS")->get<Nice::Point>(); /*itStageXY->second.iPtr->getDeviceEncoderPosition();*/}catch(...){catch_msg.str(""); catch_msg << service_name << " ~ getDeviceEncoderPosition"; MotionLogBoostDiagnosticInformation(__LINE__, catch_msg.str());}
//        try{U9_LLOG("getAbsoluteEncoderPosition : " << itStageXY->second.iPtr->getAbsoluteEncoderPosition());}catch(...){E_LLOG("getAbsoluteEncoderPosition");}
//        try{U9_LLOG("getIncrementalEncoderPosition : " << itStageXY->second.iPtr->getIncrementalEncoderPosition());}catch(...){E_LLOG("getIncrementalEncoderPosition");}
//        try{U9_LLOG("getVelocity : " << itStageXY->second.iPtr->getVelocity());}catch(...){E_LLOG("getVelocity");}

        std::stringstream json_alias;
        json_alias << "\"" << itStageXY->first << "\": { \"name\": \"" << service_name << "\", \"named\": \"" << named_pos << "\", \"encoder\": " << deviceEncoderPosition << ", ";
        json_alias << "\"is_moving\": " << isMoving << ", \"in_limit\": " << inLimit << ", \"available\": " << isReachable << ", \"simulated\": " << isSimulated << "}";
        jsonStatus += json_alias.str();
        if(counter < MapStageXY.size())jsonStatus += ", ";
    }

//    std::map<std::string, std::string> status_motion;
//    status_motion = getNamedPosition(vecAlias);


    //    struct MotorStatus{
    //               int steps;
    //               string namedPos;
    //               bool connected;
    //               bool moving;
    //               bool atHome;
    //               bool inSwNegativeLimit;
    //               bool inSwPositiveLimit;
    //               bool inHwNegativeLimit;
    //               bool inWhPositiveLimit;
    //    };

    return status;
}

