// $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/src/motion_ctrl/stagesXY/multiStageXY.cpp 3857 2022-03-24 17:11:52Z depa $

#include <Nice/Map/WMatrix.h>
#include "shins_util.h"
#include "multiStageXY.hpp"

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


void StageXYDevice::GetDataFromConfigFile()
{
    W_LLOG(__FUNCTION__ << " ~ TBCh");
    units = su::get_main_units_two_axes(cfgFile);
//    namedPositions = su::get_named_positions(cfgFile);
    alias = su::get_alias_from_config(cfgFile);
}


void StageXYDevice::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, Nice::Point>(nodechild->leaf()->getAttribute<std::string>("NAME"), nodechild->get<Nice::Point>()));
        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<Nice::Point>().xp << ", " << (int)nodechild->get<Nice::Point>().yp << "]");
    }
    dev_parent->node("DEV.1.CFG.TRANS")->children(trans_children);
    for(itString = trans_children.begin(); itString < trans_children.end(); itString++)
    {
    //    Nice::WMatrix trans = dev_parent->node("DEV.1.CFG.TRANS." + *itString)->get<Nice::WMatrix>();
        Nice::WMatrix trans = dev_parent->getAtNode<Nice::WMatrix>("DEV.1.CFG.TRANS." + *itString);
        units.insert(std::pair<std::string, std::tr1::tuple<double, double, double, double, double, double> >(
            *itString, std::tr1::tuple<double, double, double, double, double, double>(trans.m11(), trans.m12(), trans.m21(), trans.m22(), trans.dx(), trans.dy())));
        // U9_LLOG(__FUNCTION__ << " ~ Transformation : " << *itString << "[[" << trans.m11() << ", " << trans.m12() << "], [" << trans.m21() << ", " << trans.m22() << "], [" << trans.dx() << ", " << trans.dy() << "]]");
    }
}


multiStageXY::multiStageXY(const std::string & _service)
: multiDevice(std::vector<std::string>(), typeid(this).name())
    // multiDevice(_vecAliasName, _ptrIceLogger, typeid(this).name()),
// m_info(_ptrIceLogger, "INFO"),
// m_debug(_ptrIceLogger, "DEBUG"),
// m_error(_ptrIceLogger),
// m_warning(_ptrIceLogger)
{
    U6_LLOG(__FUNCTION__);
    m_service = _service;
    // m_counter = 0;
//    Basda::MoccaXYServiceWorkerInterfacePtr tmpStageXY;
    // m_ptrLogger = boost::make_shared<Nice::SyslogFunction>("SHINS");

    // m_nWheels = NUM_WHEELS;
    // m_vecBaseName = _vecBaseName;
    // m_nDevices = _vecBaseName.size();
    // for(std::size_t i = 0; i < m_nDevices; i++)
    // {
    //     m_vecStageXY.push_back(tmpStageXY);
    // }

    //shins::itString_t it;
    // 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_mapStageXY.insert(std::pair<std::string, Basda::MoccaXYServiceWorkerInterfacePtr>(to_upper_it, tmpStageXY));
    //     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_MapStageXY.insert(std::pair<std::string, StageXYDevice>(to_upper_it, StageXYDevice(cfgFile)));
    //     std::map<std::string, StageXYDevice>::iterator itFind2 = m_MapStageXY.find(to_upper_it);
    //     assert(itFind2 != m_MapStageXY.end() && itFind2->second.alias == to_upper_it);
    // }

    m_ptrCallbackObj = boost::make_shared<niceCallback>();
    // m_callbackFn = boost::bind(&multiWheel::callback_motion_complete_count, this, _1, boost::ref(m_counter));
    // m_callbackFn = boost::bind(&niceCallback::callback_motion_complete_count, m_ptrCallbackObj, _1, boost::ref(m_counter));
    // m_strCallbackName = "motion_complete_count";
    // // m_callbackFn = boost::bind(niceCallback::callback_motion_complete_SM, this, _1, boost::ref(m_ptrSM));
    // if(m_callbackFn)
    //   {
    //     //m_callbackFn(Nice::Completion());
    //     std::cout << "Callback function created  --- " << m_counter << std::endl;
    //   }
    U8_LLOG("");
}


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

// void multiStageXY::apply_callback2(std::map<std::string, Basda::MoccaXYServiceWorkerInterfacePtr>::iterator _it)
// {
//   Nice::PropertyPtr prop;
//   std::string bridgeName = "";

//   bridgeName = _it->first + "-motion";
//   prop = _it->second->config()->leaf("CMD.MOVETONAMEDPOSITION.EXEC");
//   prop->applyBridge
//     (
//      new Nice::PropertyBridgeMemFn
//      <Nice::Completion, multiStageXY,
//      boost::function<void(const Nice::Completion&)> multiStageXY::* >
//      (
//       this,
//       &multiStageXY::m_callbackFn, prop->id()),
//      bridgeName
//      );

// }

void multiStageXY::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, StageXYDevice>::iterator itFind;
    itFind = m_MapStageXY.find(alias);
    if(itFind != m_MapStageXY.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 multiStageXY::install_callback(const std::string &_strCallbackName, const std::string &_strPropName)
{
    U6_LLOG("multiStageXY::" << __FUNCTION__ << " ~ " << _strCallbackName << " ~  " << _strPropName);
    Nice::PropertyPtr prop;
    std::string bridgeName = "";

    //int counter = 0;
    std::map<std::string, Basda::MoccaXYServiceWorkerInterfacePtr>::const_iterator it;
    for(it = m_mapStageXY.begin(); it != m_mapStageXY.end(); ++it)
    {
        bridgeName = _strCallbackName + "-" + it->first + "-motion";
        // prop = it->second->config()->leaf("CMD.MOVETONAMEDPOSITION.EXEC");
        prop = it->second->config()->leaf(_strPropName);
        // U2_LLOG(it->second->config()->path());
        // std::cin.ignore();
        // prop->applyBridge
        //   (
        //    new Nice::PropertyBridgeMemFn
        //    <Nice::Completion, multiWheel>(
        //                                   this,
        //                                   &multiWheel::callback_motion_complete, prop->id()),
        //    bridgeName
        //    );
        // prop->applyBridge
        //   (
        //    new Nice::PropertyBridgeMemFn
        //    <Nice::Completion, multiWheel,
        //    boost::function<void(const Nice::Completion&)> multiWheel::* >
        //    (
        //     this,
        //     &multiWheel::m_callbackFn, prop->id()),
        //    bridgeName
        //    );

        prop->applyBridge(
            new Nice::PropertyBridgeMemFn<Nice::Completion, multiStageXY,
                boost::function<void(const Nice::Completion&)> multiStageXY::*>(this, &multiStageXY::m_callbackFn,
            // new Nice::PropertyBridgeDispatchedMemFn<Nice::Completion, multiStageXY,
                prop->id()), bridgeName);

        // std::cout << it->first << " -- " << __FUNCTION__ << std::endl;
    }
    // std::cout << "OUT -- " <<__FUNCTION__ << std::endl;
    U8_LLOG("");
}


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

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


std::string multiStageXY::getBaseName(const std::size_t _stageIdx)
{
    U6_LLOG(__FUNCTION__);

    if(_stageIdx > m_vecStageXY.size())
    {
        E_LLOG(std::string(__FUNCTION__) + ":range error. Throwing");
        U8_LLOG("");
        throw std::range_error("range error");
    }
    else
    {
        U8_LLOG("");
        return *(m_vecAliasName.begin() + _stageIdx);
    }
}


void multiStageXY::connect()
{
    U6_LLOG(__FUNCTION__);
    Nice::JointPtr nodestagexys = Nice::Application::config()->node(m_service + "." + "STAGEXY");
    Nice::SeqString StageXyChildren;
    nodestagexys->children(StageXyChildren);

    Nice::SeqString::iterator itSeq;
    Basda::MoccaXYServiceWorkerInterfacePtr iPtr;
    m_vecAliasName = StageXyChildren;
    for(itSeq = StageXyChildren.begin(); itSeq < StageXyChildren.end(); itSeq++)
    {
        try
        {
            Nice::JointPtr nodestagexy = nodestagexys->node(*itSeq + ".SVC");
            iPtr = Basda::MoccaXYServiceWorkerInterface::create(nodestagexy);
        }
        catch (const IceUtil::NullHandleException & e){ A_LLOG(e.what()); }
        catch(...){E_LLOG(__FUNCTION__)};
        m_mapStageXY.insert(std::pair<std::string, Basda::MoccaXYServiceWorkerInterfacePtr>(*itSeq, iPtr));
        m_MapStageXY.insert(std::pair<std::string, StageXYDevice>(*itSeq, StageXYDevice(iPtr)));
        m_vecStageXY.push_back(iPtr);
        m_nDevices++;
    }
    U8_LLOG("");

    // U6_LLOG(__FUNCTION__);

    // std::map<std::string, Basda::MoccaXYServiceWorkerInterfacePtr>::iterator it;
    // for(it = m_mapStageXY.begin(); it != m_mapStageXY.end(); ++it)
    // {
    //     Nice::JointPtr nodett = Nice::Application::config()->node("motion_ctrl-svr.STAGEXY." + it->first + ".SVC");
    //     it->second = Basda::MoccaXYServiceWorkerInterface::create(nodett);
    //     m_MapStageXY.find(it->first)->second.SetInterfacePointer(it->second);
    // }
    // U8_LLOG("");
}


void multiStageXY::standby(const std::vector<std::string> &_vecDevAlias)
{
    U6_LLOG(__FUNCTION__);
    if(!_vecDevAlias.empty())
    {
        std::vector<std::string>::const_iterator it;
        for(it = _vecDevAlias.begin(); it != _vecDevAlias.end(); ++it)
        {
            std::map<std::string, Basda::MoccaXYServiceWorkerInterfacePtr>::iterator itFind;
            itFind = m_mapStageXY.find(boost::to_upper_copy(*it));
            if(itFind != m_mapStageXY.end())
            {
                try
                {
                    if(itFind->second->state() == Basda::WORKING)
                        itFind->second->stop();
                    if(itFind->second->state() != Basda::STANDBY)
                    {
                        itFind->second->changeState(Basda::STANDBY);
                        I_LLOG(itFind->first << " STANDBY");
                    }
                }
                catch(Basda::ServiceInitException &e)
                {
                    E_LLOG(e.what());
                    itFind->second->changeState(Basda::STANDBY);
                }
            }
        }
    }
    else
    {
        std::map<std::string, Basda::MoccaXYServiceWorkerInterfacePtr>::iterator it;
        try
        {
            for(it = m_mapStageXY.begin(); it != m_mapStageXY.end(); ++it)
                it->second->changeState(Basda::STANDBY);
        }
        catch(Basda::ServiceInitException &e)
        {
            E_LLOG(e.what());
            it->second->changeState(Basda::STANDBY);
        }
        catch(std::exception e)
        {
            E_LLOG(e.what());
            U8_LLOG("");
            throw(e);
            // E_LLOG(err.what());
        }
    }
    // std::cout << __FUNCTION__ << std::endl;
    U8_LLOG("");
}


void multiStageXY::online(const std::vector<std::string> &_vecDevAlias)
{
    U6_LLOG(__FUNCTION__);
    if(!_vecDevAlias.empty())
    {
        std::vector<std::string>::const_iterator it;
        for(it = _vecDevAlias.begin(); it != _vecDevAlias.end(); ++it)
        {
            std::map<std::string, Basda::MoccaXYServiceWorkerInterfacePtr>::iterator itFind;
            itFind = m_mapStageXY.find(boost::to_upper_copy(*it));
            if(itFind != m_mapStageXY.end())
            {
                try
                {
                }
                catch(Basda::ServiceInitException &e)
                {
                    E_LLOG(e.what());
                    // itFind->second->init();
                    itFind->second->changeState(Basda::ONLINE);
                }
            }
        }
    }
    else
    {
        try
        {
            int change_state_result = 0;
            Nice::Time timeout = Nice::Time::seconds(2);
            std::map<std::string, Basda::MoccaXYServiceWorkerInterfacePtr>::const_iterator it;
            for(it = m_mapStageXY.begin(); it != m_mapStageXY.end(); ++it)
            {
                if(it->second)
                {
                    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"); }
                }
            }
        }
        catch(std::exception e)
        {
            E_LLOG(e.what());
            U8_LLOG("");
            throw e;
        }
    }
    // std::cout << __FUNCTION__ << std::endl;
    U8_LLOG("");
}


void multiStageXY::changeState(const Basda::ServiceState &_state, const std::vector<std::string> &_vecDevAlias)
{
    U6_LLOG(__FUNCTION__);
    Nice::Time timeout = Nice::Time::seconds(2);
    bool wait_result;
    if(!_vecDevAlias.empty())
    {
        std::vector<std::string>::const_iterator it;
        for(it = _vecDevAlias.begin(); it != _vecDevAlias.end(); ++it)
        {
            std::map<std::string, Basda::MoccaXYServiceWorkerInterfacePtr>::iterator itFind;
            itFind = m_mapStageXY.find(boost::to_upper_copy(*it));
            if(itFind != m_mapStageXY.end())
            {
                try
                {
                    wait_result = false;
                    if(itFind->second)
                        wait_result = itFind->second->changeState(_state, timeout);
                    I_LLOG(itFind->first << _state << (wait_result ? "success" :  (itFind->second ? "timed out" : "failed")) << suc::tilde);
                }
                catch(Basda::ServiceInitException &e)
                {
                    E_LLOG(e.what());
                    // itFind->second->init();
                    itFind->second->changeState(Basda::STANDBY);
                }
            }
        }
    }
    else
    {
        std::map<std::string, Basda::MoccaXYServiceWorkerInterfacePtr>::const_iterator it;
        for(it = m_mapStageXY.begin(); it != m_mapStageXY.end(); ++it)
        {
            try
            {
                wait_result = false;
                if(it->second)
                    wait_result = it->second->changeState(_state, timeout);
                I_LLOG(it->first << _state << (wait_result ? "success" :  (it->second ? "timed out" : "failed")) << suc::tilde);
            }
            catch(Basda::ServiceInitException &e)
            {
                E_LLOG(e.what());
                // it->second->init();
                it->second->changeState(Basda::STANDBY);
            }
        }
    }
    U8_LLOG("");
}


void multiStageXY::moveToReference()
{
    U6_LLOG(__FUNCTION__);
    try
    {
        moveToReferenceStart();
        moveToReferenceWait(Nice::Time::seconds(60));
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
}


// Moves all the devices to reference position and DOES NOT waits for them.
void multiStageXY::moveToReferenceStart()
{
    U6_LLOG(__FUNCTION__);
    std::string strPropName = "CMD.MOVETOHOME.EXEC";
    install_callback(m_strCallbackName, strPropName);
    // U8_LLOG(__FUNCTION__ << " Moving Home");
    try
    {
        std::map<std::string, Basda::MoccaXYServiceWorkerInterfacePtr>::const_iterator it;
        for(it = m_mapStageXY.begin(); it != m_mapStageXY.end(); ++it)
        {
            it->second->moveToHomeStart();
        }
        // U8_LLOG(__FUNCTION__ << " Moving Home Start");
    }
    catch(Basda::DevException e)
    {
        E_LLOG(std::string(__FUNCTION__) + ":Caught exception. Maybe a dev is DUMMY configuration? Throwing");
        U8_LLOG("");
        throw e;
        // E_LLOG("Caught exception in wheels.moveHome. Maybe a dev is DUMMY?");
    }
    U8_LLOG("");
}


// WAITS all the devices to reach reference position.
void multiStageXY::moveToReferenceWait(const Nice::Time &_time)
{
    U6_LLOG(__FUNCTION__);
//    std::string strPropName = "CMD.MOVETOHOME.EXEC";
//    install_callback(m_strCallbackName, strPropName);
    // U8_LLOG(__FUNCTION__ << " Moving Home");
    std::map<std::string, Basda::MoccaXYServiceWorkerInterfacePtr>::const_iterator it;
    su::Timer timer(_time);
    for(it = m_mapStageXY.begin(); it != m_mapStageXY.end(); ++it)
    {
        try
        {
            int WAIT_RESULT_TIMED_OUT = 0;
            if(it->second->moveToHomeWait(timer.residual()) == WAIT_RESULT_TIMED_OUT)
            {
                std::stringstream log_msg;
                log_msg << __FUNCTION__ << suc::tilde << it->first << suc::tilde << "Error : wait timed out";
                throw(std::runtime_error(log_msg.str()));
            }
        }
        catch(Basda::DevException e)
        {
            E_LLOG(__FUNCTION__ << suc::tilde <<  it->first << suc::tilde <<  ":Caught exception. Maybe a dev is DUMMY configuration? Throwing");
            U8_LLOG("");
            throw e;
        }
        catch(...)
        {
            E_LLOG(__FUNCTION__ << suc::tilde << it->first);
            U8_LLOG("");
            throw;
        }
    }
    U8_LLOG("");
}


void multiStageXY::moveDeviceToReference(const std::vector<std::string> &_vecDevName)
{
    U6_LLOG(__FUNCTION__);
    try
    {
        moveDeviceToReferenceStart(_vecDevName);
        moveDeviceToReferenceWait(_vecDevName, Nice::Time::seconds(60));
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
}


void multiStageXY::moveDeviceToReferenceStart(const std::vector<std::string> &_vecDevName)
{
    U6_LLOG(__FUNCTION__);
//    std::string strPropName = "CMD.MOVETOHOME.EXEC";
//    install_callback(m_strCallbackName, strPropName);
    std::vector<std::string>::const_iterator it;
    for(it = _vecDevName.begin(); it < _vecDevName.end(); ++it)
    {
        if(m_mapStageXY.find(boost::to_upper_copy(*it)) != m_mapStageXY.end())
        {
            U9_LLOG(__FUNCTION__ << " ~ " << *it);
            m_mapStageXY[*it]->moveToHomeStart();
        }
        else
        {
            E_LLOG(std::string(__FUNCTION__) + ":Invalid device name.");
            U8_LLOG("");
            throw std::invalid_argument("Wrong alias name for device");
        }
    }
    // for(BOOST_AUTO(it, _vecDevName.begin()); it < _vecDevName.end(); ++it)
    //   m_mapStageXY[boost::to_upper_copy(*it)]->moveToHomeWait();
    U8_LLOG("");
}


void multiStageXY::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)
    {
        if(m_mapStageXY.find(*it) != m_mapStageXY.end())
        {
            try
            {
                int WAIT_RESULT_TIMED_OUT = 0;
                if(m_mapStageXY[*it]->moveToHomeWait(timer.residual()) == WAIT_RESULT_TIMED_OUT)
                {
                    std::stringstream log_msg;
                    log_msg << __FUNCTION__ << suc::tilde << *it << suc::tilde << "Error : wait timed out";
                    throw(std::runtime_error(log_msg.str()));
                }
            }
            catch(...)
            {
                E_LLOG(__FUNCTION__ << suc::tilde << *it);
                U8_LLOG("");
                throw;
            }
        }
        else
        {
            E_LLOG(__FUNCTION__ <<  ":Invalid device name : "  << *it);
            U8_LLOG("");
            throw std::invalid_argument("Wrong alias name for device");
        }
    }
    U8_LLOG("");
}


// Moves stage to named position. The input map must have as keys the device names (not divided in X and Y
// coordinates), and as value the named position number.
void multiStageXY::moveToNamedPosition(const std::map<std::string, int> &_mapNamedPos)
{
    U6_LLOG(__FUNCTION__);
    try
    {
        moveToNamedPositionStart(_mapNamedPos);
        moveToNamedPositionWait(_mapNamedPos, Nice::Time::seconds(60));
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
}


void multiStageXY::moveToNamedPositionStart(const std::map<std::string, int> &_mapNamedPos)
{
//    U6_LLOG(__FUNCTION__);
//    std::string strPropName = "CMD.MOVETONAMEDPOSITION.EXEC";
//    install_callback(m_strCallbackName, strPropName);
    std::map<std::string, int>::const_iterator it;
    for(it = _mapNamedPos.begin(); it != _mapNamedPos.end(); ++it)
    {
        if(m_mapStageXY.find(it->first) != m_mapStageXY.end())
        {
            m_mapStageXY[it->first]->moveToNamedPositionStart(it->second);
        }
        else
        {
            E_LLOG(std::string(__FUNCTION__) + ":Wrong alias name for device. Throwing");
//            U8_LLOG("");
            throw std::invalid_argument("Wrong alias name for device.");
        }
    }
//    U8_LLOG("");
}


void multiStageXY::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)
    {
        if(m_mapStageXY.find(it->first) != m_mapStageXY.end())
        {
            try
            {
                int WAIT_RESULT_TIMED_OUT = 0;
                if(m_mapStageXY[it->first]->moveToNamedPositionWait(timer.residual()) == WAIT_RESULT_TIMED_OUT)
                {
                    std::stringstream log_msg;
                    log_msg << __FUNCTION__ << suc::tilde << it->first << suc::tilde << "Error : wait timed out";
                    throw(std::runtime_error(log_msg.str()));
                }
            }
            catch(...)
            {
                E_LLOG(__FUNCTION__ << suc::tilde << it->first);
//                U8_LLOG("");
                throw;
            }
        }
        else
        {
            E_LLOG(std::string(__FUNCTION__) + ":Wrong alias name for device. Throwing");
//            U8_LLOG("");
            throw std::invalid_argument("Wrong alias name for device.");
        }
    }
//    U8_LLOG("");
}


// moveToAbsolutePosition
// INPUT map with device alias as key and position as value, expressed in steps
void multiStageXY::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(60));
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
}


void multiStageXY::moveToAbsolutePositionStart(const std::map<std::string, double> &_mapPos, const std::map<std::string, std::string> &_mapUnit)
{
//    U6_LLOG(__FUNCTION__);
    // does nothing in this object, could be templatized over value type
    // U8_LLOG(__FUNCTION__);
    Nice::Point p;
    // m_vecStageXY contains MoccaXYServiceWorkerInterfacePtr objects
    // _mapPos contains coordinate in steps for mirror position. They do not have
    // to double the number of XY serivces.
    if(_mapPos.size() > (2 * m_vecStageXY.size()))
    {
        E_LLOG(":Number of coodinates cannot be higher then double the number of stages. Throwing");
//        U8_LLOG("");
        throw std::invalid_argument("Number of coodinates cannot be higher then double the number of stages.");
    }
    else
    {
        std::map<std::string, double>::const_iterator itFindX, itFindY;
        itFindX = _mapPos.find("INBEAM_TT_X");
        itFindY = _mapPos.find("INBEAM_TT_Y");
        if(itFindX != _mapPos.end() && itFindY != _mapPos.end())
        {
            // Looking for units
            std::string unit = "STEPS";
            {
                std::map<std::string, std::string>::const_iterator itFindUnitX, itFindUnitY;
                itFindUnitX = _mapUnit.find("INBEAM_TT_X");
                itFindUnitY = _mapUnit.find("INBEAM_TT_Y");
                if( itFindUnitX != _mapUnit.end() && itFindUnitY != _mapUnit.end())
                {
                    std::string unitx = itFindUnitX->second, unity = itFindUnitY->second;
                    if(unitx == "DEFAULT")unitx = "STEPS";
                    if(unity == "DEFAULT")unity = "STEPS";
                    if(unitx == unity)
                    {
                        unit = unitx;
                    }
                    else
                    {
                        E_LLOG(":Specified different units to move INBEAM_TT. Throwing");
//                        U8_LLOG("");
                        throw std::invalid_argument("Specified different units to move INBEAM_TT");
                    }
                }
                else if(itFindUnitX != _mapUnit.end() || itFindUnitY != _mapUnit.end())
                {
                    E_LLOG(":Missing one UNIT to move INBEAM_TT. Throwing");
//                    U8_LLOG("");
                    throw std::invalid_argument("Missing one UNIT to move INBEAM_TT");
                }
            }
            double x = itFindX->second;
            double y = itFindY->second;
            p.setX(x);
            p.setY(y);
            U9_LLOG(":Moving INBEAM_TT to " << p);
            m_mapStageXY["INBEAM_TT"]->moveAbsoluteStart(p, unit);
        }
        else if(itFindX != _mapPos.end() || itFindY != _mapPos.end())
        {
            E_LLOG(":Missing one coordinate to move INBEAM_TT. Throwing");
//            U8_LLOG("");
            throw std::invalid_argument("Missing one coordinate to move INBEAM_TT");
        }
    }
//    U8_LLOG("");
}


void multiStageXY::moveToAbsolutePositionWait(const std::map<std::string, double> &_mapPos, const Nice::Time &_time)
{
//    U6_LLOG(__FUNCTION__);

    if(_mapPos.size() > (2 * m_vecStageXY.size()))
    {
        E_LLOG(std::string(__FUNCTION__) + ":Number of coodinates cannot double the number of stages. Throwing");
//        U8_LLOG("");
        throw std::invalid_argument("Number of coodinates cannot double the number of stages.");
    }
    std::map<std::string, double>::const_iterator itFindX, itFindY;
    itFindX = _mapPos.find("INBEAM_TT_X");
    itFindY = _mapPos.find("INBEAM_TT_Y");
    su::Timer timer(_time);
    if(itFindX != _mapPos.end() && itFindY != _mapPos.end())
    {
        try
        {
            int WAIT_RESULT_TIMED_OUT = 0;
            if(m_mapStageXY["INBEAM_TT"]->moveAbsoluteWait(timer.residual()) == WAIT_RESULT_TIMED_OUT)
            {
                std::stringstream log_msg;
                log_msg << __FUNCTION__ << suc::tilde << "INBEAM_TT" << suc::tilde << "Error : wait timed out";
                throw(std::runtime_error(log_msg.str()));
            }
        }
        // catch(const Basda::MoCon::MotorInNegLimitException & err)
        // {
        //     // Ignore park error
        //     if(itFindX->second == 0. && itFindY->second == 0.)
        //     {
        //         I_LLOG("Catched park Error" );
        //     }
        //     else
        //     {
        //         throw(err);
        //     }
        // }
        catch(const Basda::ServiceWorkerInterfaceCommandFailedException & err)
        {
            // Ignore park error
            if(itFindX->second == 0. && itFindY->second == 0.)
            {
                I_LLOG("Catched park Error" );
                
            }
            else
            {
                throw(err);
            }
        }
        catch(...)
        {
            E_LLOG(__FUNCTION__ << suc::tilde << itFindX->first);
//            U8_LLOG("");
            throw;
        }
    }
//    U8_LLOG("");
}


void multiStageXY::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(60));
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
}


void multiStageXY::moveToRelativePositionStart(const std::map<std::string, double> &_mapPos, const std::map<std::string, std::string> &_mapUnit)
{
//    U6_LLOG(__FUNCTION__);
    std::string unit = shins::util::constants::steps;//"TBD";
    Nice::Point p;

    std::map<std::string, double>::const_iterator itFindX, itFindY;
    itFindX = _mapPos.find("INBEAM_TT_X");
    itFindY = _mapPos.find("INBEAM_TT_Y");
    if(itFindX != _mapPos.end() && itFindY != _mapPos.end())
    {
        double x = itFindX->second;
        double y = itFindY->second;
        p.setX(x);
        p.setY(y);
        U9_LLOG(":Moving INBEAM_TT of (" << x << ", " << y << "]");
        m_mapStageXY["INBEAM_TT"]->moveRelativeStart(p, unit);
    }
    else if(itFindX != _mapPos.end() || itFindY != _mapPos.end())
    {
        E_LLOG(":Missing one coordinate to move INBEAM_TT. Throwing");
//        U8_LLOG("");
        throw std::invalid_argument("Missing one coordinate to move INBEAM_TT");
    }
//    U8_LLOG("");
}


void multiStageXY::moveToRelativePositionWait(const std::map<std::string, double> &_mapPos, const Nice::Time &_time)
{
//    U6_LLOG(__FUNCTION__);
    if(_mapPos.size() == 2)
    {
        try
        {
            int WAIT_RESULT_TIMED_OUT = 0;
            if(m_mapStageXY["INBEAM_TT"]->moveRelativeWait(_time) == WAIT_RESULT_TIMED_OUT)
            {
                std::stringstream log_msg;
                log_msg << __FUNCTION__ << suc::tilde << "INBEAM_TT" << suc::tilde << "Error : wait timed out";
                throw(std::runtime_error(log_msg.str()));
            }
        }
        catch(...)
        {
            E_LLOG(__FUNCTION__ << suc::tilde << "INBEAM_TT");
//            U8_LLOG("");
            throw;
        }
    }
//    U8_LLOG("");
}


Nice::Point multiStageXY::genRandomPosition(const Basda::MoccaXYServiceWorkerInterfacePtr _svc)
{
    U6_LLOG(__FUNCTION__);

    std::string strPath = "";
    std::string strPropName = "";
    std::string strTravelRangePath = "MOD.Y.CFG.DATASHEET.TRAVELRANGE";
    Nice::PropertyPtr prop;
    Nice::Point res, travelRange;
    struct timespec tm;
    clock_gettime(CLOCK_REALTIME, &tm);
    boost::random::mt19937 gen;
    // gen(tm.tv_nsec);
    strPath = _svc->config()->path();
    strPropName = strPath + "." + strTravelRangePath;

    prop = _svc->config()->leaf(strPropName);

    travelRange = prop->get<Nice::Point>();

    clock_gettime(CLOCK_REALTIME, &tm);

    boost::random::uniform_int_distribution<> dist(travelRange.x(), travelRange.y());
    res.setX(dist(gen));

    // clock_gettime(CLOCK_REALTIME, &tm);
    // gen(tm.tv_nsec);

    res.setY(dist(gen));

    U8_LLOG("");
    return res;
}


void multiStageXY::moveToPositionPoint(std::map<std::string, Nice::Point> &_mapPos)
{
    U6_LLOG(__FUNCTION__);
    try
    {
        moveToPositionPointStart(_mapPos);
        moveToPositionPointWait(_mapPos, Nice::Time::seconds(60));
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
}


void multiStageXY::moveToPositionPointStart(std::map<std::string, Nice::Point> &_mapPos)
{
    U6_LLOG(__FUNCTION__);

//    std::string strPropName = "CMD.MOVEABSOLUTE.EXEC";

    std::map<std::string, Nice::Point>::iterator it;
    for(it = _mapPos.begin(); it != _mapPos.end(); ++it)
    {
        if(m_mapStageXY.find(it->first) != m_mapStageXY.end())
        {
            m_mapStageXY[it->first]->moveAbsoluteStart(it->second);
        }
        else
        {
            E_LLOG(std::string(__FUNCTION__) + ":Wrong alias name for device. Throwing");
            U8_LLOG("");
            throw std::invalid_argument("Wrong alias name for device.");
        }
    }
    U8_LLOG("");
}


void multiStageXY::moveToPositionPointWait(std::map<std::string, Nice::Point> &_mapPos, const Nice::Time &_time)
{
    U6_LLOG(__FUNCTION__);

    std::map<std::string, Nice::Point>::iterator it;
    su::Timer timer(_time);
    for(it = _mapPos.begin(); it != _mapPos.end(); ++it)
    {
        if(m_mapStageXY.find(it->first) != m_mapStageXY.end())
        {
            try
            {
                int WAIT_RESULT_TIMED_OUT = 0;
                if(m_mapStageXY[it->first]->moveAbsoluteWait(timer.residual()) == WAIT_RESULT_TIMED_OUT)
                {
                    std::stringstream log_msg;
                    log_msg << __FUNCTION__ << suc::tilde << it->first << suc::tilde << "Error : wait timed out";
                    throw(std::runtime_error(log_msg.str()));
                }
            }
            catch(...)
            {
                E_LLOG(__FUNCTION__ << suc::tilde << it->first);
                U8_LLOG("");
                throw;
            }
        }
        else
        {
            E_LLOG(__FUNCTION__ << ":Wrong alias name for device. Throwing");
            U8_LLOG("");
            throw std::invalid_argument("Wrong alias name for device.");
        }
    }
    U8_LLOG("");
}

// void multiStageXY::moveToPosition(std::map<std::string, Nice::Point>& _mapPos)
// {
//   U8_LLOG(__FUNCTION__);
//   if(_mapPos.size() != m_vecStageXY.size())
//     throw std::invalid_argument("Number of positions must be the same as number of stages.");
//   else
//     {
//       std::string strPropName = "CMD.MOVEABSOLUTE.EXEC";
//       install_callback(m_strCallbackName, strPropName);
//       for(BOOST_AUTO(it, _mapPos.begin()); it != _mapPos.end(); ++it)
//         {
//           if(m_mapStageXY.find(it->first) != m_mapStageXY.end())
//             {
//               m_mapStageXY[it->first]->moveAbsoluteStart(it->second);
//             }
//           else
//             throw std::invalid_argument("Wrong alias name for device.");
//         }
//       for(BOOST_AUTO(it, _mapPos.begin()); it != _mapPos.end(); ++it)
//         m_mapStageXY[it->first]->moveAbsoluteWait();
//     }
// }


bool multiStageXY::isDevice(const std::string &_strDevAlias)
{
    bool boolRet = false;
//    if(boost::algorithm::contains(_strDevAlias, "INBEAM_TT"))
    {
//        N_LLOG(_strDevAlias);
        if(m_mapStageXY.find(_strDevAlias) != m_mapStageXY.end())
            boolRet = true;
        else
            boolRet = false;
    }
    // else
    //   {
    //     E_LLOG("The alias is not recognized for this XY device :" <<  _strDevAlias);
    //     throw std::invalid_argument("The alias is not recognized for this XY device: " + _strDevAlias);
    //   }
//    U6_LLOG("multiStageXY::" << __FUNCTION__ << " ~ " << _strDevAlias << " returned : " << boolRet);
//    U8_LLOG("");
    return boolRet;
}


void multiStageXY::set_callback_fn(boost::function<void(const Nice::Completion&)> &_cbfn, const std::string &_strFnName)
{
    U6_LLOG(__FUNCTION__);

    m_callbackFn = _cbfn;
    m_strCallbackName = _strFnName;
    U8_LLOG("");
}


std::vector<std::string> multiStageXY::getNamedPosition(const std::size_t _stageIdx)
{
    U6_LLOG(__FUNCTION__);
    std::vector<std::string> res;

    std::vector<Basda::MoccaXYServiceWorkerInterfacePtr>::const_iterator it;
    if(_stageIdx >= m_nDevices)
    {
        // return position for all stages
        for(it = m_vecStageXY.begin(); it < m_vecStageXY.end(); ++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
    {
        it = m_vecStageXY.begin() + _stageIdx;
        res.push_back((*it)->config()->leaf("VAR.NAMED_POSITION")->get<std::string>());
    }
    U8_LLOG("");
    return res;
}


std::vector<std::string> multiStageXY::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;
    itPos = _vecAlias.begin();
    std::map<std::string, Basda::MoccaXYServiceWorkerInterfacePtr>::const_iterator it;
    for(it = m_mapStageXY.begin(); it != m_mapStageXY.end(); ++it)
    {
        // idx = std::distance(it, m_mapWheels.begin());
        // itPos = _vecAlias.begin() + idx;
        if(m_mapStageXY.find(*itPos) != m_mapStageXY.end())
        {
            vecRes.push_back(it->second->config()->leaf("VAR.NAMED_POSITION")->get<std::string>());
            std::advance(itPos, 1);
        }
        else
        {
            E_LLOG(std::string(__FUNCTION__) + ":Wrong alias name for device");
            U8_LLOG("");
            throw std::invalid_argument("Wrong alias name for device");
        }
    }
    U8_LLOG("");
    return vecRes;
}


std::map<std::string, std::string> multiStageXY::getNamedPosition2(const std::vector<std::string> &_vecAlias)
{
    U6_LLOG(__FUNCTION__);

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


Ice::Int multiStageXY::getPosition(std::map<std::string, Ice::Double> &_mapPosition, const std::string &_unit)
{
//    U6_LLOG(__FUNCTION__);
    Ice::Int status = EXIT_SUCCESS;
    Nice::Point pos;

    std::map<std::string, Ice::Double>::iterator it;
    for(it = _mapPosition.begin(); it != _mapPosition.end(); ++it)
    {
        // the call is passing a dictionary containing INBEAM_TT_X and INBEAM_TT_Y, these have to be mapped to INBEAM_TT
        std::string inbeam_tt = su::left(it->first, 9);
        if(isDevice(inbeam_tt))
        {
            // as XY device, getPosition returns a Nice::Point
            pos = m_mapStageXY[inbeam_tt]->getPosition(_unit);
            if(it->first == "INBEAM_TT_X")
            {
//                U9_LLOG(__FUNCTION__ << " ~ " << it->first << " ~ " << _unit);
                _mapPosition[inbeam_tt + "_X"] = pos.x();
            }
            if(it->first == "INBEAM_TT_Y")
            {
//                U9_LLOG(__FUNCTION__ << " ~ " << it->first << " ~ " << _unit);
                _mapPosition[inbeam_tt + "_Y"] = pos.y();
            }
        }


    }
//    U8_LLOG("");
    return status;
}


void multiStageXY::emergencyAbort()
{
    // std::string strPropName = "CMD.MOVEABSOLUTE.EXEC";
    // install_callback_on_completion(m_strCallbackName, strPropName);
    U6_LLOG(__FUNCTION__);

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


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

    try
    {
        std::map<std::string, Basda::MoccaXYServiceWorkerInterfacePtr>::const_iterator it;
        for(it = m_mapStageXY.begin(); it != m_mapStageXY.end(); ++it)
            it->second->stop();
    }
    catch(Nice::Exception e)
    {
        E_LLOG(std::string(__FUNCTION__) + ":" + e.what());
        status = EXIT_FAILURE;
    }
    U8_LLOG("");
    return status;
}


int multiStageXY::stop(const std::vector<std::string> &_vecDevAlias)
{
    U6_LLOG(__FUNCTION__);
    W_LLOG("Provide implementation for multiStageXY::stop");
    // W_LLOG("Stopping motion of selected linear stages");
    int status = EXIT_SUCCESS;
    // try
    //   {
    //     // for(BOOST_AUTO(it, _vecDevAlias.begin()); it != _vecDevAlias.end(); ++it)
    //     for(int i=0; i<_vecDevAlias.size(); i++)
    //       {
    //         m_mapStageXY[_vecDevAlias[i]]->stop();
    //       }
    //   }
    // catch(Nice::Exception err)
    //   {
    //     E_LLOG(err.what());
    //     status = EXIT_FAILURE;
    //     throw err;
    //   }
    U8_LLOG("");
    return status;
}


int multiStageXY::abort(const std::vector<std::string> &_vecDevAlias)
{
    U6_LLOG(__FUNCTION__);
    W_LLOG("Provide implementation for multiStageXY::abort");
    // A_LLOG("Aborting motion of selected linear stages");
    int status = EXIT_SUCCESS;
    // try
    //   {
    //     for(BOOST_AUTO(it, _vecDevAlias.begin()); it != _vecDevAlias.end(); ++it)
    //       {
    //         m_mapStageX[(*it)]->abort();
    //       }
    //   }
    // catch(Nice::Exception err)
    //   {
    //     E_LLOG(err.what());
    //     status = EXIT_FAILURE;
    //     throw err;
    //   }
    U8_LLOG("");
    return status;
}
