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

#include "shins_util.h"
#include "calunitCtrl.h"

namespace su = shins::util;

void CalunitLogBoostDiagnosticInformation(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());
}


// ----------------------
// OVERLOADED CONSTRUCTOR
// ----------------------
calunitCtrl::calunitCtrl(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::vector<std::string> vecAlias;

    m_ptrStateMachine = _ptrStateMachine;

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

    m_callbackFnSM = boost::bind(&niceCallback::callback_motion_complete_SM, _1, boost::ref(m_ptrStateMachine), boost::ref(m_mtxStateMachine));

    m_ptrMonSeq = boost::make_shared<IceUtil::Monitor<IceUtil::Mutex> >();

    m_callbackFnCount = boost::bind(&niceCallback::callback_motion_complete_count, _1, boost::ref(m_moving_devs), boost::ref(m_mtxCount), boost::ref(m_ptrMonSeq));

    U8_LLOG("");
}

calunitCtrl::~calunitCtrl()
{
    U6_LLOG(__FUNCTION__);
//    delete m_app;
    U8_LLOG("");
}


su::EServiceStatus calunitCtrl::Connect()
{
    Nice::JointPtr nodecalunit = Nice::Application::config()->node(m_service);
    if(m_service_status == su::SERVICE_NOTCONNECTED)
    {
        try
        {
            U1_LLOG("Creating AppConfigClient for " << m_service);
            m_ConfigClient = Nice::AppConfigClient::create(nodecalunit->getAtNode<Nice::Endpoint>("ALIAS"), nodecalunit);
            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::SeqString children;
        nodestagex->children(children);
        m_ptrMultiStageX = boost::make_shared<multiStageX>(m_service);
        if(m_ptrMultiStageX)
        {
            m_service_status = su::SERVICE_ALLOCATED;
        }
    }
    if(m_service_status == su::SERVICE_ALLOCATED)
    {
        try
        {
            m_ptrMultiStageX->connect();
            m_service_status = su::SERVICE_CONNECTED;
        }
        catch(...)
        {
            E_LLOG("Erorr connecting to " << m_service << " motors");
        }
    }
    if(m_service_status == su::SERVICE_CONNECTED)
    {
        try
        {
            m_ptrMultiStageX->standby();
            m_ptrMultiStageX->online();
            m_service_status = su::SERVICE_TOBEINITED;
        }
        catch(...)
        {
            E_LLOG("Erorr changing state to MultiStageX");
        }
    }
    if(m_service_status == su::SERVICE_TOBEINITED)
    {
        m_ptrMultiStageX->set_callback_fn(m_callbackFnSM, "calib_dev-motion_complete_SM");
        m_ptrMultiStageX->set_callback_fn_count(m_callbackFnCount, "motion_complete_count");
        U1_LLOG("Connection established with service " << m_service);
        m_service_status = su::SERVICE_ONLINE;
    }
    return m_service_status;
}

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

bool calunitCtrl::isDevice(const std::string& _strDevAlias)
{
    return m_ptrMultiStageX->isDevice(_strDevAlias);
}

double calunitCtrl::ConvertToDefaultUnit(const std::string & alias, double destination, const std::string & unit)
{
    std::map<std::string, StageXDevice>::const_iterator itDevice = GetMultiStageX().find(alias);
    static std::string log_msg;
    log_msg.clear();
    if(itDevice != GetMultiStageX().end())
    {
        std::string final_unit = unit;
        if(unit == suc::default_unit)final_unit = suc::steps;//TBD unify default unit
        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
        {   log_msg = std::string(__FUNCTION__) + ": No " + unit + " unit in " + alias + ".";
            E_LLOG(log_msg);
            throw std::invalid_argument(log_msg);
        }
    }
    else
    {
        log_msg = std::string(__FUNCTION__) + ": No " + alias + " device in CALUNIT_CTRL. Throwing!";
        E_LLOG(log_msg);
        throw std::invalid_argument(log_msg);
    }
}


void calunitCtrl::moveCalFiberDepToSafe()
{
    std::vector<std::string> vecDevAlias(1,suc::CAL_FIBER_DEP);
    m_ptrMultiStageX->moveToPositiveLimit(vecDevAlias);
}


void calunitCtrl::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))
{
    m_ptrMultiStageX->SetSecondaryCallbacks(alias, MoveToHomeCB, MoveToNamedCB, MoveAbsoluteCB, MoveRelativeCB, MoveToLimitCB);
}


Ice::Int calunitCtrl::getPosition(std::map<std::string, Ice::Double> &_mapPosition, const std::string &_unit)
{
//    U6_LLOG(__FUNCTION__);
    Ice::Int status = EXIT_SUCCESS;
    try
    {
        status = m_ptrMultiStageX->getPosition(_mapPosition, _unit);
    }
    catch(...)
    {
        U6_LLOG(__FUNCTION__);
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
//    U8_LLOG("");
    return status;
}


Ice::Int calunitCtrl::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;
    if(!(m_service_status == su::SERVICE_ONLINE)){ return EXIT_FAILURE;}

    int counter = 0;
    std::stringstream catch_msg;
    double deviceEncoderPosition;

    const std::map<std::string, StageXDevice> & MapStageX = m_ptrMultiStageX->GetMultiStageX();

    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; CalunitLogBoostDiagnosticInformation(__LINE__, catch_msg.str());}
            _mapPosition[itStageX->first] = deviceEncoderPosition;
        }
    }
//    U8_LLOG("");
    return status;
}



int calunitCtrl::getNdevices()
{
    U6_LLOG(__FUNCTION__);
    U8_LLOG("");
    return (m_ptrMultiStageX->getNdevices());
}


void calunitCtrl::moveHome()
{
    U6_LLOG(__FUNCTION__);
    if(m_ptrMultiStageX->getNdevices() > 0)
    {
        m_ptrStateMachine->process_event(osfsm::ready_busy(m_ptrMultiStageX->getNdevices()));
        m_ptrMultiStageX->moveToReference();
    }
    else
    {
        E_LLOG(std::string(__FUNCTION__) << ":No devices in calunit_ctlr! Throwing");
        U8_LLOG("");
        throw std::runtime_error("No devices in calunit_ctlr! ");
    }
    U8_LLOG("");
}


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

    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;
        }
    }
    try
    {
        m_ptrStateMachine->process_event(osfsm::ready_busy(tmpVecStageX.size()));
        m_ptrMultiStageX->moveDeviceToReference(tmpVecStageX);
        m_ptrMultiStageX->moveDeviceToReferenceWait(tmpVecStageX, Nice::Time::seconds(80));
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
}


void calunitCtrl::changeState(const Basda::ServiceState &_state, const std::vector<std::string> &_vecDevAlias)
{
    U6_LLOG(__FUNCTION__);
    switch (_state)
    {
    case Basda::ONLINE:
    {
        m_ptrMultiStageX->online(_vecDevAlias);
        break;
    }
    case Basda::STANDBY:
    {
        m_ptrMultiStageX->standby(_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("");
}



void calunitCtrl::moveToReferenceAll()
{
    U6_LLOG(__FUNCTION__);
    static Nice::Time nice_time = Nice::Time::seconds(80);
    try
    {
        moveToReferenceAllStart();
        moveToReferenceAllWait(nice_time);
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
}


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

    if(m_ptrMultiStageX->getNdevices() != 0)
    {
        try
        {
            m_ptrMultiStageX->moveToReferenceStart();
        }
        catch(...)
        {
            E_LLOG(__FUNCTION__);
            U8_LLOG("");
            throw;
        }
    }

    U8_LLOG("");
}


void calunitCtrl::moveToReferenceAllWait(const Nice::Time &_time)
{
    // m_ptrStateMachine->process_event(osfsm::ready_busy(m_ptrMultiStageX->getNdevices()));
    // moving to "home" reference switch all devices
    // the number of moving wheels is multiplied yby 2 since they move to reference twice
    // m_moving_devs = m_ptrMultiStageX->getNdevices();

    U6_LLOG(__FUNCTION__);
    if(m_ptrMultiStageX->getNdevices() != 0)
    {
        try
        {
            m_ptrMultiStageX->moveToReferenceWait(_time);
        }
        catch(...)
        {
            E_LLOG(__FUNCTION__);
            U8_LLOG("");
            throw;
        }
    }
//    m_ptrStateMachine->decrease_running_devs();
//    m_ptrStateMachine->decrease_running_devs();
//    m_ptrStateMachine->decrease_running_devs();
//    m_ptrStateMachine->process_event(osfsm::busy_ready);

    U8_LLOG("");
}
void calunitCtrl::moveToReference(const std::vector<std::string> &_vecDevAlias)
{
    U6_LLOG(__FUNCTION__);
    CheckOnline();
    try
    {
        moveToReferenceStart(_vecDevAlias);
        moveToReferenceWait(_vecDevAlias, Nice::Time::seconds(60));
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
}


void calunitCtrl::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_ptrMultiStageX->isDevice(*it))
        {
            m_init.InitStageX.push_back(*it);
            continue;
        }
    }
}


void calunitCtrl::moveToReferenceStart(const std::vector<std::string> &_vecDevAlias)
{
    U6_LLOG(__FUNCTION__);
    CheckOnline();
    initPrepare(_vecDevAlias);
    m_ptrStateMachine->process_event(osfsm::ready_busy(m_ptrMultiStageX->getNdevices()));
    // moving to "home" reference switch all devices
    // the number of moving wheels is multiplied by 2 since they move to reference twice
    m_moving_devs = m_ptrMultiStageX->getNdevices();

//    if(m_ptrMultiStageX->getNdevices() != 0)
    {
        try
        {
            m_ptrMultiStageX->moveDeviceToReferenceStart(_vecDevAlias);
        }
        catch(...)
        {
            E_LLOG(__FUNCTION__);
            U8_LLOG("");
            throw;
        }
    }

    U8_LLOG("");
}


void calunitCtrl::moveToReferenceWait(const std::vector<std::string> &_vecDevAlias, const Nice::Time &_time)
{
    // m_ptrStateMachine->process_event(osfsm::ready_busy(m_ptrMultiStageX->getNdevices()));
    // moving to "home" reference switch all devices
    // the number of moving wheels is multiplied yby 2 since they move to reference twice
    // m_moving_devs = m_ptrMultiStageX->getNdevices();

    U6_LLOG(__FUNCTION__);
    if(m_ptrMultiStageX->getNdevices() != 0)
    {
        try
        {
            m_ptrMultiStageX->moveDeviceToReferenceWait(_vecDevAlias, _time);
        }
        catch(...)
        {
            E_LLOG(__FUNCTION__);
            U8_LLOG("");
            throw;
        }
    }

    U8_LLOG("");
}


void calunitCtrl::moveToNamedPosition(const std::map<std::string, std::string> &_mapNamedPos)
{
    U6_LLOG(__FUNCTION__);
    try
    {
        moveToNamedPositionStart(_mapNamedPos);
        moveToNamedPositionWait(_mapNamedPos, Nice::Time::seconds(80));
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
}

void calunitCtrl::setupPrepare(const std::map<std::string, std::string>& _mapNamedPos)
{
    U6_LLOG(__FUNCTION__);
    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;
    m_setup.clear();
    for(it = _mapNamedPos.begin(); it != _mapNamedPos.end(); ++it)
    {
        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());
                        throw(std::invalid_argument(msg.str()));
                    }
                    m_setup.NamedMapStageX.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);
                m_setup.AbsMapStageX.insert(std::pair<std::string, double>(it->first, absolute));
                m_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 << "; unit = " << unit);
                m_setup.RelMapStageX.insert(std::pair<std::string, double>(it->first, relative));
                m_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());
                    throw(std::invalid_argument(msg.str()));
                }
            }
            continue;
        }
    }
    U8_LLOG("");
}

void calunitCtrl::moveToNamedPositionStart(const std::map<std::string, std::string> &_mapNamedPos)
{
    U6_LLOG(__FUNCTION__);
    CheckOnline();
    setupPrepare(_mapNamedPos);

    // swich state machine to "busy"
    // U8_LLOG(tmpMapStageX.size());
    // I_LLOG("State Machine state: " << *(m_ptrStateMachine->current_state()));
    m_moving_devs = m_setup.NamedMapStageX.size() + m_setup.AbsMapStageX.size() + m_setup.RelMapStageX.size();
//    m_ptrStateMachine->process_event(osfsm::ready_busy(m_moving_devs));

    // I_LLOG("State Machine state: " << *(m_ptrStateMachine->current_state()));
    // I_LLOG(*(m_ptrStateMachine->event_(osfsm::readyDone)));
    // IceUtil::Monitor<IceUtil::Mutex>::Lock lock(*(m_ptrMonSeq));
    if(m_moving_devs > 0)
    {
        try
        {
            m_ptrMultiStageX->moveToNamedPositionStart(m_setup.NamedMapStageX);
            m_ptrMultiStageX->moveToAbsolutePositionStart(m_setup.AbsMapStageX, m_setup.AbsMapStageXUnits);
            m_ptrMultiStageX->moveToRelativePositionStart(m_setup.RelMapStageX, m_setup.RelMapStageXUnits);
        }
        catch(Basda::Svc::MoccaMotionException exMotion)
        {
            E_LLOG(__FUNCTION__ << " ~ Error : " << exMotion.what());
            U8_LLOG("");
            throw exMotion;
        }
        catch(Basda::ServiceException exService)
        {
            E_LLOG(__FUNCTION__ << " ~ Error : " <<  exService.what());
            // A_LLOG(__FUNCTION__ << Nice::Application::config()->node(m_service));
            U8_LLOG("");
            throw exService;
        }
        catch(...)
        {
            E_LLOG(__FUNCTION__);
            U8_LLOG("");
            throw;
        }
    }
    U8_LLOG("");
}


void calunitCtrl::moveToNamedPositionWait(const std::map<std::string, std::string> &_mapNamedPos, const Nice::Time &_time)
{
    U6_LLOG(__FUNCTION__);
    su::Timer timer(_time);
    try
    {
        m_ptrMultiStageX->moveToNamedPositionWait(m_setup.NamedMapStageX, timer.residual());
        m_ptrMultiStageX->moveToAbsolutePositionWait(m_setup.AbsMapStageX, timer.residual());
        m_ptrMultiStageX->moveToRelativePositionWait(m_setup.RelMapStageX, timer.residual());
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
}


void calunitCtrl::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 calunitCtrl::moveToAbsolutePositionStart(const std::map<std::string, double> &_mapPos,
    const std::map<std::string, std::string> &_mapUnit)
{
    U6_LLOG(__FUNCTION__);
    CheckOnline();
    std::map<std::string, double> tmpMapStageX;
    int movingDevices = 0;

    std::map<std::string, double>::const_iterator it;
    for(it = _mapPos.begin(); it != _mapPos.end(); ++it)
    {
        if(m_ptrMultiStageX->isDevice(it->first))
        {
            // std::cout << "stageX" << std::endl;
            tmpMapStageX.insert(*it);
            continue;
        }

    }
    movingDevices = tmpMapStageX.size();
    m_ptrStateMachine->process_event(osfsm::ready_busy(movingDevices));

    if(tmpMapStageX.size() > 0)
    {
        try
        {
            m_ptrMultiStageX->moveToAbsolutePositionStart(tmpMapStageX, _mapUnit);
        }
        catch(...)
        {
            E_LLOG(__FUNCTION__);
            U8_LLOG("");
            throw;
        }
    }
    U8_LLOG("");
}


void calunitCtrl::moveToAbsolutePositionWait(const std::map<std::string, double> &_mapPos, const Nice::Time &_time)
{
    U6_LLOG(__FUNCTION__);
    std::map<std::string, double> tmpMapStageX;
//    int movingDevices = 0;

    std::map<std::string, double>::const_iterator it;
    for(it = _mapPos.begin(); it != _mapPos.end(); ++it)
    {
        if(m_ptrMultiStageX->isDevice(it->first))
        {
            // std::cout << "stageX" << std::endl;
            tmpMapStageX.insert(*it);
            continue;
        }
    }

    if(tmpMapStageX.size() > 0)
    {
        try
        {
            m_ptrMultiStageX->moveToAbsolutePositionWait(tmpMapStageX, _time);
        }
        catch(...)
        {
            U8_LLOG("Error Waiting");
            throw;
        }

    }
    U8_LLOG("");
}


void calunitCtrl::moveToRelativePosition(const std::map<std::string, double> &_mapPos, const std::map<std::string, std::string> &_mapUnit)
{
    U6_LLOG(__FUNCTION__);
    E_LLOG(__FUNCTION__ << " not implemented");
    U8_LLOG("");
    throw(std::runtime_error("Function not implemented yet"));
}


void calunitCtrl::moveToRelativePositionStart(const std::map<std::string, double> &_mapPos, const std::map<std::string, std::string> &_mapUnit)
{
    U6_LLOG(__FUNCTION__);
    E_LLOG(__FUNCTION__ << " not implemented");
    U8_LLOG("");
    throw(std::runtime_error("Function not implemented yet"));
}


void calunitCtrl::moveToRelativePositionWait(const std::map<std::string, double> &_mapPos, const Nice::Time &_time)
{
    U6_LLOG(__FUNCTION__);
    E_LLOG(__FUNCTION__ << " not implemented");
    U8_LLOG("");
    throw(std::runtime_error("Function not implemented yet"));
}


void calunitCtrl::setupLamps(const std::map<std::string, std::string> &_mapSetup)
{
    U6_LLOG(__FUNCTION__);
    E_LLOG(__FUNCTION__ << " not implemented");
    U8_LLOG("");
    throw(std::runtime_error("Function not implemented yet"));
}


void calunitCtrl::emergencyAbort()
{
    U6_LLOG(__FUNCTION__);
    try
    {
        m_ptrMultiStageX->emergencyAbort();
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
    }
    U8_LLOG("");
}


int calunitCtrl::stop(const std::vector<std::string> &_vecDevAlias)
{
    int status = EXIT_SUCCESS;
    U6_LLOG(__FUNCTION__);
    try
    {
        m_ptrMultiStageX->stop(_vecDevAlias);
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
    }
    U8_LLOG("");
    return status;
}


int calunitCtrl::abort(const std::vector<std::string> &_vecDevAlias)
{
    int status = EXIT_SUCCESS;
    U6_LLOG(__FUNCTION__);
    try
    {
        m_ptrMultiStageX->abort(_vecDevAlias);
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
    }
    U8_LLOG("");
    return status;
}

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

    std::vector<std::string> vecAlias;
    vecAlias.push_back("CAL_FIBER_DEP");
    vecAlias.push_back("CAL_MIRROR_DEP");
    vecAlias.push_back("CAL_FILTER_DEP");

    std::map<std::string, std::string> status_motion;
    if(m_service_status != su::SERVICE_ONLINE){return EXIT_FAILURE;};
    status_motion = m_ptrMultiStageX->getNamedPosition2(vecAlias);

    //    struct MotorStatus{
    //               int steps;
    //               string namedPos;
    //               bool connected;
    //               bool moving;
    //               bool atHome;
    //               bool inSwNegativeLimit;
    //               bool inSwPositiveLimit;
    //               bool inHwNegativeLimit;
    //               bool inWhPositiveLimit;
    //    };
    std::map<std::string, StageXDevice>::const_iterator itStageX;
    const std::map<std::string, StageXDevice> & MapStageX = m_ptrMultiStageX->GetMultiStageX();
    int counter = 0;
    for(itStageX = MapStageX.begin(); itStageX != MapStageX.end(); itStageX++)
    {
        counter++;
        // Nice::JointPtr nodecalunit = Nice::Application::config()->node(m_service + ".CALUNIT_CTRL." + service_name);
//        Nice::SeqBitSet switch_status;
        std::string switch_status;
        Nice::JointPtr itStageConfig = itStageX->second.iPtr->config();
        bool isSimulated = false;
        std::string service_name;
        try{service_name = itStageConfig->leaf("VAR.SERVICE_NAME")->get<std::string>(); /*itStageX->second.iPtr->readServiceName();*/}catch(...){E_LLOG("readServiceName");}
//        try{switch_status = itStageX->second.iPtr->getPositionSwitchStatus(); isSimulated = switch_status[0].getDepth() < 32;}catch(...){}
        try
        {
            switch_status = itStageConfig->leaf("VAR.SWITCH_STATUS")->get<std::string>(); // itStageX->second.iPtr->getPositionSwitchStatus();
//            isSimulated = switch_status[0].getDepth() < 32;
              isSimulated = switch_status.size() < 32;
        }
        // catch(Nice::ChatException err)
        // {
        //     usleep(1000000);
        //     continue;
        // }
        catch(...)
        {
            E_LLOG(service_name << " ~ getPositionSwitchStatus");
            usleep(1000000);
            continue;
        }
        std::string named_pos = itStageConfig->leaf("VAR.NAMED_POSITION")->get<std::string>();
//        try{U9_LLOG("getPositionSwitchStatus : " << switch_status << " ~ " << switch_status[0].getDepth());}catch(...){E_LLOG("getPositionSwitchStatus");}
        bool isAtHome = false;
        try{isAtHome = itStageConfig->leaf("VAR.ATHOME")->get<bool>(); /*itStageX->second.iPtr->isAtHome();*/}catch(...){E_LLOG(service_name << " ~ isAtHome");}
        bool inLimit = false;
        try{inLimit = itStageConfig->leaf("VAR.LIMIT")->get<bool>(); /*itStageX->second.iPtr->isAtLimit();*/}catch(...){E_LLOG(service_name << " ~ isAtLimit");}
//         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);
//         }
        bool isMoving = false;
        try{isMoving = itStageConfig->leaf("VAR.MOVING")->get<bool>(); /*itStageX->second.iPtr->isMoving();*/}catch(...){E_LLOG(service_name << " ~ isMoving");}
//        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 = itStageConfig->leaf("CMD.ISREACHABLE.RET")->get<bool>(); /*itStageX->second.iPtr->isReachable();*/}catch(...){E_LLOG(service_name << " ~ isReachable");}
//        try{U9_LLOG("getPosition : " << itStageX->second.iPtr->getPosition());}catch(...){E_LLOG("getPosition");}
        int deviceEncoderPosition;
        try{deviceEncoderPosition = (int)(itStageConfig->leaf("VAR.POSITION.DEVICE_ENCODER.STEPS")->get<double>()); /*itStageX->second.iPtr->getDeviceEncoderPosition();*/}catch(...){E_LLOG(service_name << " ~ getDeviceEncoderPosition");}
//        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;
        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 += ", ";
    }

    return status;
}
