// $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/src/track_ctrl/trackingCtrl.cpp 3969 2022-06-03 15:00:46Z depa $

#include <pthread.h>

#include "trackingCtrl.h"



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

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


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


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


bool trackingCtrl::AdcCallback(double & pressure, double & temperature, double & elevation, double & drot_pos)
{
    trackingCtrl & me = *trackingCtrl::m_pTrackingCtrl;

    double smt_pressure, smt_temperature;
    const double OneAtm = 101500.;

    lbto::DDstruct dd;
    lbto::SeqDD ddentries;
    dd.DDname = "ELPosition";
    ddentries.push_back(dd);
    dd.DDname = "LBTPressure";
    ddentries.push_back(dd);
    dd.DDname = "LBTTemp";
    ddentries.push_back(dd);
    dd.DDname = "SMTPressure";
    ddentries.push_back(dd);
    dd.DDname = "SMTTemp";
    ddentries.push_back(dd);

    lbto::result res;
    bool get_parameters = true;
    int retry_counter = 0;
    while(get_parameters)
    {
        get_parameters = false;
        try
        {
            if(me.m_iif)
            {
                res = me.m_iif->iifGetParameter(ddentries);
            }
            else
            {
                 res.rescode = EXIT_FAILURE;
            }
        }
        catch(Basda::ServiceIsBusyException)
        {
            if(retry_counter && retry_counter % 10 == 0)
            {
                W_LLOG("Busy : retrying " << retry_counter);
            }
            retry_counter++;
            get_parameters = true;
            usleep(1000000);
            continue;
        }
        catch(const std::exception& e)
        {
            W_LLOG(e.what());
            return false;
        }
        catch(...)
        {
            W_LLOG(__FUNCTION__);
            return false;
        }
    }
    if(res.rescode == EXIT_SUCCESS)
    {
        su::getValue<double>(std::string(res.resmsg[1].c_str()), elevation);
        su::getValue<double>(std::string(res.resmsg[2].c_str()), pressure);
        su::getValue<double>(std::string(res.resmsg[3].c_str()), temperature);
        su::getValue<double>(std::string(res.resmsg[4].c_str()), smt_pressure);
        su::getValue<double>(std::string(res.resmsg[5].c_str()), smt_temperature);
        // U7_LLOG(pressure << " ~ " << temperature << " ~ " << elevation << " ~ " <<  smt_pressure << " ~ " << smt_temperature );
    }
    else
    {
        U7_LLOG("Error getting parameters from TCS");
        for(unsigned int i = 1; i < res.resmsg.size(); i++)
        {
            U7_LLOG("resmsg[" << i << "] = " << res.resmsg[i]);
        }
        return false;
    }
    double elevation2 = (elevation/3600)*3.14159265358979/180.;
    double ZA = 90*(3.14159265358979/180.) - elevation2;
    double P = 1000*100*(pressure/OneAtm);
    double P2 = 1000*100*smt_pressure/OneAtm; // pressure in hPa

    double T = temperature; // m_iif->getDouble("LBTTemp", "env.weather.lbt.temperature");// temperature in C
    double T2 = smt_temperature;// temperature in C
    // U7_LLOG(P << " ~ " << T << " ~ " << elevation2 << " ~ " <<  P2 << " ~ " << T2 );

    bool bGotPosition = false;
    while(!bGotPosition)
    {
        try
        {
            drot_pos = me.getPosition(suc::DROT, "DEGREE");
            bGotPosition = true;
        }
        catch(Basda::DevException & e)
        {
            W_LLOG("Got DevException while asking for drot position : " << e.what());
            usleep(5000000);
        }
        catch(...)
        {
            TrackLogBoostDiagnosticInformation(__LINE__);
            usleep(5000000);
        }
    }

    return true;
}


trackingCtrl * trackingCtrl::m_pTrackingCtrl = NULL;

trackingCtrl::trackingCtrl(boost::shared_ptr<osfsm::OSFsm>& _ptrStateMachine, Ice::PropertiesPtr props)
:
    __OBJ_NAME__(typeid(this).name()),
    m_drot(NULL),
    m_adc1(NULL),
    m_adc2(NULL),
    m_props(props),
    m_service_status(su::SERVICE_NOTCONNECTED)
{
    U6_LLOG(__FUNCTION__ << " - Constructor");
    m_pTrackingCtrl = NULL;

    m_ptrAdcWorkerThread = NULL;
    m_ConfigClient = NULL;

    m_service = props->getProperty("OcsTrackService.Name");
    m_ptrStateMachine = _ptrStateMachine;

    m_pTrackingCtrl = this;

    std::string jsonStatus;
    getJsonStatus(jsonStatus);

    U8_LLOG(__FUNCTION__ << " ~ DONE contructor");
}


trackingCtrl::~trackingCtrl()
{
    I_LLOG(__FUNCTION__);
    if(m_ptrAdcWorkerThread)
        delete m_ptrAdcWorkerThread;
}


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


su::EServiceStatus trackingCtrl::Connect()
{
    static bool bDrotOnline = false;
    static bool bAdcOnline = false;
    static bool bAdc1Online = false;
    static bool bAdc2Online = false;

    Nice::JointPtr nodetrack = 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(nodetrack->getAtNode<Nice::Endpoint>("ALIAS"), nodetrack);
            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::Time timeout = Nice::Time::seconds(4);
        Nice::JointPtr NodeDerotator = Nice::Application::config()->node(m_service + ".DEROTATOR");
        Nice::JointPtr NodeAdc = Nice::Application::config()->node(m_service + ".ADC");
        Nice::SeqString DerotatorChildren, AdcChildren;
        NodeDerotator->children(DerotatorChildren);
        NodeAdc->children(AdcChildren);
        Nice::SeqString::iterator itString;
        for(itString = DerotatorChildren.begin(); itString < DerotatorChildren.end(); itString++)
        {
            U9_LLOG("DEROTATOR." << *itString << " : ");
            try
            {
                U9_LLOG("Creating interface to remote node " << "DEROTATOR." << *itString << ".SVC");
                Sharknir::DrotServiceWorkerInterfacePtr iptr = m_drot = Sharknir::DrotServiceWorkerInterface::create(NodeDerotator->node(*itString + ".SVC"));
                m_MapDrots.insert(std::pair<std::string, DrotDevice>(*itString, DrotDevice(iptr)));
            }
            catch (const IceUtil::NullHandleException & e){ A_LLOG(e.what()); }
            catch(...){ TrackLogBoostDiagnosticInformation(__LINE__); }
        }
        for(itString = AdcChildren.begin(); itString < AdcChildren.end(); itString++)
        {
            U9_LLOG("ADC." << *itString << " : ");
            try
            {
                U9_LLOG("Creating interface to remote node " << "ADC." << *itString << ".SVC");
                Basda::MoccaWheelServiceWorkerInterfacePtr iptr = Basda::MoccaWheelServiceWorkerInterface::create(NodeAdc->node(*itString + ".SVC"));
                m_MapAdcs.insert(std::pair<std::string, WheelDevice>(iptr->config()->getAtNode<std::string>("NAME"), WheelDevice(iptr)));
            }
            catch (const IceUtil::NullHandleException & e){ A_LLOG(e.what()); }
            catch(...){ TrackLogBoostDiagnosticInformation(__LINE__); }
        }

        // Temporary
        m_adc1 = m_MapAdcs.find("ADC1")->second.iPtr;
        m_adc2 = m_MapAdcs.find("ADC2")->second.iPtr;

    if(false)
    {
        Nice::JointPtr NodeDrot = Nice::Application::config()->node(m_service + ".DEROTATOR.DROT.SVC");
        Nice::JointPtr NodeAdc1 = Nice::Application::config()->node(m_service + ".ADC.ADC1.SVC");
        Nice::JointPtr NodeAdc2 = Nice::Application::config()->node(m_service + ".ADC.ADC2.SVC");

        Nice::SeqString DrotChildren, Adc1Children, Adc2Children;
        NodeDrot->children(DrotChildren);
        NodeAdc1->children(Adc1Children);
        NodeAdc2->children(Adc2Children);

        if(!m_drot)
        {
            try
            {
                U9_LLOG("Creating interface to DROT remote node " << "DEROTATOR.DROT.SVC");
                m_drot = Sharknir::DrotServiceWorkerInterface::create(NodeDrot);
                m_MapDrots.insert(std::pair<std::string, DrotDevice>(suc::DROT, DrotDevice(m_drot)));
            }
            catch (const IceUtil::NullHandleException & e){ A_LLOG(e.what()); }
            catch(...){ TrackLogBoostDiagnosticInformation(__LINE__); }
        }
        if(!m_adc1)
        {
            try
            {
                U9_LLOG("Creating interface to ADC 1 remote node " << "ADC.ADC1.SVC");
                m_adc1 = Basda::MoccaWheelServiceWorkerInterface::create(NodeAdc1);
                m_MapAdcs.insert(std::pair<std::string, WheelDevice>("ADC1", WheelDevice(m_adc1)));
            }
            catch (const IceUtil::NullHandleException & e){ A_LLOG(e.what()); }
            catch(...){ TrackLogBoostDiagnosticInformation(__LINE__); }
        }
        if(!m_adc2)
        {
            try
            {
                U9_LLOG("Creating interface to ADC 2 remote node " << "ADC.ADC2.SVC");
                m_adc2 = Basda::MoccaWheelServiceWorkerInterface::create(NodeAdc2);
                m_MapAdcs.insert(std::pair<std::string, WheelDevice>("ADC2", WheelDevice(m_adc2)));
            }
            catch (const IceUtil::NullHandleException & e){ A_LLOG(e.what()); }
            catch(...){ TrackLogBoostDiagnosticInformation(__LINE__); }
        }
    }
        if(m_drot && m_adc1 && m_adc2)
        {
            // m_service_status = su::SERVICE_ALLOCATED;
            m_service_status = su::SERVICE_CONNECTED;
        }
    }
    if(m_service_status == su::SERVICE_CONNECTED)
    {
        Nice::Time timeout = Nice::Time::seconds(4);
        setDerotationParameters1(1200 /*numPolynomials*/, 1 /*samplingRate*/, 1 /*frequency*/);
        setDerotationParameters2(2 /*polynomialOrder*/, 350 /*faultTolerance*/, 100 /*samplesPerSegment*/);
        setDerotationParameters3(1, 1, 1);

        std::map<std::string, DrotDevice>::iterator itDerotator;
        if(!bDrotOnline)
        {
            int DrotsOnline = 0;
            for(itDerotator = m_MapDrots.begin(); itDerotator != m_MapDrots.end(); itDerotator++)
            {
                itDerotator->second.SetSecondaryCallbacks(NULL,  // void (* moveToHomeCB)(const Nice::Completion & c),
                                                          NULL,  // void (* moveToNamedCB)(const Nice::Completion & c),
                                                          NULL,  // void (* moveAbsoluteCB)(const Nice::Completion & c),
                                                          NULL,  // void (* moveRelativeCB)(const Nice::Completion & c),
                                                          NULL,  // void (* moveToLimitCB)(const Nice::Completion & c),
                                                          NULL,  // void (* startDerotationCB)(const Nice::Completion &c),
                                                          NULL,  // void (* startDerotationHereCB)(const Nice::Completion & c),
                                                          NULL); // void (* varAbsoluteEncoderCB)(const double & val));
                try
                {
                    int change_state_result = 0;
                    change_state_result = itDerotator->second.iPtr->changeState(Basda::STANDBY, timeout);
                    if(change_state_result == 0){W_LLOG("Derotator " << itDerotator->second.alias << "change state to STANDBY timed out");}
                    usleep(200000);
                    change_state_result = itDerotator->second.iPtr->changeState(Basda::ONLINE, timeout);
                    if(change_state_result == 0){W_LLOG("Derotator " << itDerotator->second.alias << "change state to ONLINE timed out");}
                    U9_LLOG("DROT->state = " << itDerotator->second.iPtr->state());
                    DrotsOnline++;
                }
                catch(...)
                {
                    E_LLOG("Error changing state of derotator");
                }
            }
            if(DrotsOnline == m_MapDrots.size())bDrotOnline = true;
        }
        std::map<std::string, WheelDevice>::iterator itAdc;
        if(!bAdcOnline)
        {
            int AdcsOnline = 0;
            for(itAdc = m_MapAdcs.begin(); itAdc != m_MapAdcs.end(); itAdc++)
            {
                itAdc->second.SetSecondaryCallbacks(NULL,  // void (* moveToHomeCB)(const Nice::Completion & c),
                                                    NULL,  // void (* moveToNamedCB)(const Nice::Completion & c),
                                                    NULL,  // void (* moveAbsoluteCB)(const Nice::Completion & c),
                                                    NULL,  // void (* moveRelativeCB)(const Nice::Completion & c),
                                                    NULL); // void (* scanAllReferenceSwitchesCB)(const Nice::Completion & c));
                try
                {
                    int change_state_result = 0;
                    change_state_result = itAdc->second.iPtr->changeState(Basda::STANDBY, timeout);
                    if(change_state_result == 0){W_LLOG("ADC " << itAdc->second.alias << "change state to STANDBY timed out");}
                    usleep(200000);
                    change_state_result = itAdc->second.iPtr->changeState(Basda::ONLINE, timeout);
                    if(change_state_result == 0){W_LLOG("ADC " << itAdc->second.alias << "change state to ONLINE timed out");}
                    AdcsOnline++;
                }
                catch(...)
                {
                    E_LLOG("Error changing state of derotator");
                }
            }
            if(AdcsOnline == m_MapAdcs.size())bAdcOnline = true;
        }
                // ADC worker thread
        if(bAdcOnline)
        {
            if(m_ptrAdcWorkerThread == NULL)
            {
                m_ptrAdcWorkerThread = new AdcWorkerThread(m_adc1, m_adc2, AdcCallback, m_props);
                U9_LLOG("ADC1->state = " << m_adc1->state() << "; ADC2->state = " << m_adc2->state());
            }
        }
        if(bDrotOnline && bAdcOnline)
        {
            m_service_status = su::SERVICE_ONLINE;
        }
    }
    return m_service_status;
}


int trackingCtrl::setDerotationParameters1(int numPolynomials, double samplingRate, double frequency)
{
    U6_LLOG(__FUNCTION__ << " ~ Num Polynomials " << numPolynomials << " ~ Sampling Rate " << samplingRate << " ~ Frequency " << frequency);
    if(m_drot)
    {
        m_drot->setDerotationParameters1(numPolynomials, samplingRate, frequency);
    }
    else
    {
        std::string log_msg("Could not set Derotation Parameters1 to DROT : module is not active");
        E_LLOG(log_msg);
        U8_LLOG("");
        throw(std::runtime_error(log_msg));
    }
    U8_LLOG("");
    return EXIT_SUCCESS;
}

int trackingCtrl::setDerotationParameters2(int polynomialOrder, int faultTolerance, int samplesPerSegment)
{
    U6_LLOG(__FUNCTION__ << " ~ Polynomial Order " << polynomialOrder << " ~ fault Tolerance " << faultTolerance << " ~ Samples per segment " << samplesPerSegment);
    if(m_drot)
    {
        m_drot->setDerotationParameters2(polynomialOrder, faultTolerance, samplesPerSegment);
    }
    else
    {
        std::string log_msg("Could not set Derotation Parameters2 to DROT : module is not active");
        E_LLOG(log_msg);
        U8_LLOG("");
        throw(std::runtime_error(log_msg));
    }
    U8_LLOG("");
    return EXIT_SUCCESS;
}
int trackingCtrl::setDerotationParameters3(double debug1, double debug2, double debug3)
{
    U6_LLOG(__FUNCTION__);
    if(m_drot)
    {
        m_drot->setDerotationParameters3(debug1, debug2, debug3);

        if(false)
        try
        {
            Nice::Date start_date = Nice::Date::now() + Nice::Time::seconds(60);
            U9_LLOG(Nice::Date::now());
            U9_LLOG("Get Rotator Polynomials first request");
            usleep(2000000);
            Nice::SeqDouble result1 = m_drot->OsGetDerotationProfile(start_date, 36, 300);
            if(result1.size() == 36)
            {
                U9_LLOG("result1 : " << su::seq_double_to_str(result1, 20));
            }
            else
            {
                U9_LLOG("result1.size() = " << result1.size());
            }
        }
        catch(const Ltcs::RotatorPolynomialException & e)
        {
            E_LLOG(e.what() << " : cought")
        }
        catch(...)
        {
            TrackLogBoostDiagnosticInformation(__LINE__);
        }

    }
    else
    {
        std::string log_msg("Could not set Derotation Parameters3 to DROT : module is not active");
        E_LLOG(log_msg);
        U8_LLOG("");
        throw(std::runtime_error(log_msg));
    }
    U8_LLOG("");
    return EXIT_SUCCESS;
}

int trackingCtrl::setDerotationParameters4(double debug4, double debug5, double debug6)
{
    U6_LLOG(__FUNCTION__);
    if(m_drot)
    {
        m_drot->setDerotationParameters4(debug4, debug5, debug6);
    }
    else
    {
        std::string log_msg("Could not set Derotation Parameters4 to DROT : module is not active");
        E_LLOG(log_msg);
        U8_LLOG("");
        throw(std::runtime_error(log_msg));
    }
    U8_LLOG("");
    return EXIT_SUCCESS;
}

int trackingCtrl::setAdcCorrectionParameters(double co, double p0, double p1, double t0, double t1)
{
    if(m_ptrAdcWorkerThread)
    {
        m_ptrAdcWorkerThread->SetCorrectionParameters(co, p0, p1, t0, t1);
        return EXIT_SUCCESS;
    }
    else
    {
        W_LLOG("Could not set Adc Correction Parameters : module is not active");
        return EXIT_FAILURE;
    }
}

int trackingCtrl::sharkDerotatorBacklashProcedure()
{
    int result = EXIT_FAILURE;
    try
    {
        result = m_drot->sharkDerotatorBacklashProcedure();
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        throw;
    }
    return result;
}

    int trackingCtrl::setAdcCoRotationOffset(double corotation)
{
    if(m_ptrAdcWorkerThread)
    {
        m_ptrAdcWorkerThread->setAdcCoRotationOffset(corotation);
        return EXIT_SUCCESS;
    }
    else
    {
        W_LLOG("Could not set Adc Correction Parameters : module is not active");
        return EXIT_FAILURE;
    }
}

int trackingCtrl::setAdcCounterRotationOffset(double counterrotation)
{
    if(m_ptrAdcWorkerThread)
    {
        m_ptrAdcWorkerThread->setAdcCounterRotationOffset(counterrotation);
        return EXIT_SUCCESS;
    }
    else
    {
        W_LLOG("Could not set Adc Correction Parameters : module is not active");
        return EXIT_FAILURE;
    }
}


void trackingCtrl::changeState(const Basda::ServiceState& _state, const std::vector<std::string>& _vecDevAlias)
{
    U6_LLOG(__FUNCTION__);
    switch (_state)
    {
    case Basda::ONLINE:
    case Basda::STANDBY:
    {
        if(std::find(_vecDevAlias.begin(), _vecDevAlias.end(), suc::DROT) < _vecDevAlias.end())
        {
            if(m_drot)
            {
                m_drot->changeState(_state);
            }
            else
            {
                W_LLOG("Could not change state to DROT : module is not active");
            }
        }

        if(std::find(_vecDevAlias.begin(), _vecDevAlias.end(), suc::ADC1) < _vecDevAlias.end())
        {
            U9_LLOG("calling changeState on ADC.PRISM.1")
            if(m_adc1)
            {
                m_adc1->changeState(_state);
            }
            else
            {
                std::string log_msg("Could not change state to ADC1 : module is not active");
                E_LLOG(log_msg);
                U8_LLOG("");
                throw(std::runtime_error(log_msg));
            }
        }
        if(std::find(_vecDevAlias.begin(), _vecDevAlias.end(), suc::ADC2) < _vecDevAlias.end())
        {
            U9_LLOG("calling changeState on ADC.PRISM.2")
            if(m_adc2)
            {
                m_adc2->changeState(_state);
            }
            else
            {
                std::string log_msg("Could not change state to ADC2 : module is not active");
                E_LLOG(log_msg);
                U8_LLOG("");
                throw(std::runtime_error(log_msg));
            }
        }
        break;
    }
    case Basda::SHUTDOWN:
    case Basda::OFFLINE:
    case Basda::WORKING:
    case Basda::ERROR:
    default:
    {
        W_LLOG("State " << _state << " not handled");
    }
    }
    U8_LLOG("");
}


void trackingCtrl::initialize(const std::vector<std::string>& _vecDevAlias)
{
    U6_LLOG(__FUNCTION__);
    try
    {
        initializeStart(_vecDevAlias);
        initializeWait(_vecDevAlias, Nice::Time::milliSeconds(suc::DROT_TIMEOUT*1000));  // time estimated to move by 190 deg
    }
    catch(...)
    {
        TrackLogBoostDiagnosticInformation(__LINE__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
}

void trackingCtrl::resetPosition(const std::string _devAlias)
{
    U6_LLOG(__FUNCTION__);
    CheckOnline();
    try
    {
        if(m_adc1 && _devAlias == "ADC1")
        {
        //    m_adc1->setPosition(0);
        }
    }
    catch(...)
    {
        TrackLogBoostDiagnosticInformation(__LINE__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
}

void trackingCtrl::initializeStart(const std::vector<std::string>& _vecDevAlias)
{
    U6_LLOG(__FUNCTION__);
    double position;
    try
    {
        // DROT has absolute encoder and is not initialized
        if(m_adc1)
        {
            m_adc1->moveToHomeStart();
        }
        else
        {
            std::string log_msg("Could not initialize ADC1 : module is not active");
            E_LLOG(log_msg);
            U8_LLOG("");
            throw(std::runtime_error(log_msg));
        }
        if(m_adc2)
        {
             m_adc2->moveToHomeStart();
        }
        else
        {
            std::string log_msg("Could not initialize ADC2 : module is not active");
            E_LLOG(log_msg);
            U8_LLOG("");
            throw(std::runtime_error(log_msg));
        }
    }
    catch(std::exception e)
    {
        E_LLOG(__FUNCTION__ << ":" << e.what());
        U8_LLOG("");
        throw e;
    }
    catch(...)
    {
        TrackLogBoostDiagnosticInformation(__LINE__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
}

void trackingCtrl::initializeWait(const std::vector<std::string>& _vecDevAlias, const Nice::Time& _time)
{
    U6_LLOG(__FUNCTION__);
    su::Timer timer(_time);
    try
    {
        if(m_adc1)
        {
            m_adc1->moveToHomeWait(timer.residual());
        }
        if(m_adc2)
        {
            m_adc2->moveToHomeWait(timer.residual());
        }
    }
    catch(...)
    {
        TrackLogBoostDiagnosticInformation(__LINE__);
        U8_LLOG("");
        throw;
    }

}


Ice::Int trackingCtrl::setupInstrumentStart(const sharknir::dictSetupNamedPos &_dSetupPos)
{
    U6_LLOG(__FUNCTION__);
    Ice::Int status = EXIT_SUCCESS;
    setMode(_dSetupPos);
    
    setupPrepare(_dSetupPos, m_track_setup);

    try
    {
        if(!m_track_setup.AbsMapDrot.empty())
            m_drot->moveAbsoluteStart(m_track_setup.AbsMapDrot.begin()->second, m_track_setup.AbsMapDrotUnits.begin()->second);
        else if(!m_track_setup.RelMapDrot.empty())
            m_drot->moveRelativeStart(m_track_setup.RelMapDrot.begin()->second, m_track_setup.RelMapDrotUnits.begin()->second);
        else if(!m_track_setup.NamedMapDrot.empty())
            m_drot->moveToNamedPositionStart(m_track_setup.NamedMapDrot.begin()->second);
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }

    try
    {
        std::map< std::string, std::string>::const_iterator itUnit;
        std::map< std::string, double>::const_iterator itWheel;
        std::map< std::string, int>::const_iterator itNamed;
        if(!m_track_setup.AbsMapWheel.empty())
        {
            for(itWheel = m_track_setup.AbsMapWheel.begin(); itWheel != m_track_setup.AbsMapWheel.end(); itWheel++)
            {
                itUnit = m_track_setup.AbsMapWheelUnits.find(itWheel->first);
                std::string unit = "STEPS";
                if(itUnit != m_track_setup.AbsMapWheelUnits.end())unit = itUnit->second;
                m_MapAdcs.find(itWheel->first)->second.iPtr->moveAbsoluteStart(itWheel->second, unit);
            }
        }
        if(!m_track_setup.RelMapWheel.empty())
        {
            for(itWheel = m_track_setup.RelMapWheel.begin(); itWheel != m_track_setup.RelMapWheel.end(); itWheel++)
            {
                itUnit = m_track_setup.RelMapWheelUnits.find(itWheel->first);
                std::string unit = "STEPS";
                if(itUnit != m_track_setup.RelMapWheelUnits.end())unit = itUnit->second;
                m_MapAdcs.find(itWheel->first)->second.iPtr->moveRelativeStart(itWheel->second, unit);
            }
        }
        if(!m_track_setup.NamedMapWheel.empty())
        {
            for(itNamed = m_track_setup.NamedMapWheel.begin(); itNamed != m_track_setup.NamedMapWheel.end(); itNamed++)
            {
                m_MapAdcs.find(itNamed->first)->second.iPtr->moveToNamedPositionStart(itNamed->second);
            }
        }
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }

    U8_LLOG(__FUNCTION__);
    return status;
}

void trackingCtrl::setupPrepare(const std::map<std::string, std::string>& _mapNamedPos, trackSetup & setup)
{
    U6_LLOG(__FUNCTION__);
    std::map<std::string, std::tr1::tuple<int, int> > NamedPosMap;
    int idxPos;
    std::map<std::string, std::string>::const_iterator it;
    shins::util::EShinsMovements detected;
    double absolute, relative = 0;
    std::string unit;
    setup.clear();
    // Nice::JointPtr nodetrack = Nice::Application::config()->node(m_service);
    // Nice::JointPtr NodeDeviceDrot = nodetrack->node("DEROTATOR.DROT.DEV.1.CFG.NAMED_POS.ABS");
    // Nice::JointPtr NodeDeviceAdc1 = nodetrack->node("ADC.ADC1.DEV.1.CFG.NAMED_POS.ABS");
    // Nice::JointPtr NodeDeviceAdc2 = nodetrack->node("ADC.ADC2.DEV.1.CFG.NAMED_POS.ABS");
    for(it = _mapNamedPos.begin(); it != _mapNamedPos.end(); ++it)
    {
        std::string alias;
        std::map<std::string, WheelDevice>::iterator itAdc;
        std::map<std::string, DrotDevice>::iterator itDrot;
        bool bIsAdc = false, bIsDrot = false;
        for(itAdc = m_MapAdcs.begin(); itAdc != m_MapAdcs.end(); itAdc++)
        {
            if(it->first.size() >= itAdc->first.size() && su::left(it->first, itAdc->first.size()) == itAdc->first)
            {
                alias = itAdc->first;
                bIsAdc = true;
                break;
            }
        }
        if(!bIsAdc)
        {
            for(itDrot = m_MapDrots.begin(); itDrot != m_MapDrots.end(); itDrot++)
            {
                if(it->first.size() >= itDrot->first.size() && (su::left(it->first, itDrot->first.size()) == itDrot->first && su::right(it->first, it->first.size() - itDrot->first.size()) == "_PosAng"))
                {
                    alias = itDrot->first;
                    bIsDrot = true;
                    break;
                }
            }
        }

        if(bIsAdc)
        {
            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:
                if(m_MapAdcs.find(alias) != m_MapAdcs.end())
                {
                    const std::map<std::string, int> & nps = m_MapAdcs.find(alias)->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>(alias, idxPos));
                }
                break;
            case shins::util::SHINS_MOV_ABSOLUTE:
                U9_LLOG("Detected Absolute Position : " << it->first << "/" << alias << " -> " << it->second << "; value = " << absolute << "; unit = " << unit);
                setup.AbsMapWheel.insert(std::pair<std::string, double>(alias, absolute));
                if(unit == "DEFAULT" && (it->first == alias + "_PosAng"))unit = "DEGREE";
                else if(unit == "DEFAULT" && (it->first == alias))unit = "STEPS";
                setup.AbsMapWheelUnits.insert(std::pair<std::string, std::string>(alias, unit));
                break;
            case shins::util::SHINS_MOV_RELATIVE:
                U9_LLOG("Detected Relative Position : " << it->first << "/" << alias << " -> " << it->second << "; value = " << relative << "; unit = " << unit);
                setup.RelMapWheel.insert(std::pair<std::string, double>(alias, relative));
                setup.RelMapWheelUnits.insert(std::pair<std::string, std::string>(alias, 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;
        }
        else if(bIsDrot)
//        if(it->first == suc::DROT_PosAng || m_MapDrots.find(it->first) != m_MapDrots.end())
        {
            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:
                if(m_MapDrots.find(alias) != m_MapDrots.end())
                {
                    const std::map<std::string, int> & nps = m_MapDrots.find(alias)->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.NamedMapDrot.insert(std::pair<std::string, int>(alias, idxPos));
                }
                else
                {
                    U9_LLOG("Element : " << std::setw(20) << it->first << "  <> Value : " << it->second);
                    const std::map<std::string, int> & nps = m_MapDrots.find("DROT")->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.NamedMapDrot.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.AbsMapDrot.insert(std::pair<std::string, double>(it->first, absolute));
                if(unit == "DEFAULT" && it->first == "DROT_PosAng")unit = "DEGREE";
                else if(unit == "DEFAULT" && it->first == "DROT")unit = "STEPS";
                setup.AbsMapDrotUnits.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.RelMapDrot.insert(std::pair<std::string, double>(it->first, relative));
                setup.RelMapDrotUnits.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;
        }
    }
    U8_LLOG("");
}


Ice::Int trackingCtrl::setupInstrumentWait(const sharknir::dictSetupNamedPos &_dSetupPos, const Nice::Time& _time)
{
    U6_LLOG(__FUNCTION__);
    Ice::Int status = EXIT_SUCCESS;

    su::Timer timer(_time);
    std::map<std::string, WheelDevice>::iterator itWheel;
    std::map<std::string, DrotDevice>::iterator itDrot;
    std::map<std::string, double>::iterator itMove;
    std::map<std::string, int>::iterator itNamed;

    try
    {
        for(itMove = m_track_setup.AbsMapWheel.begin(); itMove != m_track_setup.AbsMapWheel.end(); itMove++)
        {
            for(itWheel = m_MapAdcs.begin(); itWheel != m_MapAdcs.end(); itWheel++)
            {
                if(itWheel->first == itMove->first)
                {
                    itWheel->second.iPtr->moveAbsoluteWait(timer.residual());
                }
            }
        }
        for(itMove = m_track_setup.RelMapWheel.begin(); itMove != m_track_setup.RelMapWheel.end(); itMove++)
        {
            for(itWheel = m_MapAdcs.begin(); itWheel != m_MapAdcs.end(); itWheel++)
            {
                if(itWheel->first == itMove->first)
                {
                    itWheel->second.iPtr->moveRelativeWait(timer.residual());
                }
            }
        }
        for(itNamed = m_track_setup.NamedMapWheel.begin(); itNamed != m_track_setup.NamedMapWheel.end(); itNamed++)
        {
            for(itWheel = m_MapAdcs.begin(); itWheel != m_MapAdcs.end(); itWheel++)
            {
                if(itWheel->first == itNamed->first)
                {
                    itWheel->second.iPtr->moveToNamedPositionWait(timer.residual());
                }
            }
        }

        for(itMove = m_track_setup.AbsMapDrot.begin(); itMove != m_track_setup.AbsMapDrot.end(); itMove++)
        {
            for(itDrot = m_MapDrots.begin(); itDrot != m_MapDrots.end(); itDrot++)
            {
                if(itDrot->first == itMove->first)
                {
                    itDrot->second.iPtr->moveAbsoluteWait(timer.residual());
                }
            }
        }
        for(itMove = m_track_setup.RelMapDrot.begin(); itMove != m_track_setup.RelMapDrot.end(); itMove++)
        {
            for(itDrot = m_MapDrots.begin(); itDrot != m_MapDrots.end(); itDrot++)
            {
                if(itDrot->first == itMove->first)
                {
                    itDrot->second.iPtr->moveRelativeWait(timer.residual());
                }
            }
        }
        for(itNamed = m_track_setup.NamedMapDrot.begin(); itNamed != m_track_setup.NamedMapDrot.end(); itNamed++)
        {
            for(itDrot = m_MapDrots.begin(); itDrot != m_MapDrots.end(); itDrot++)
            {
                if(itDrot->first == itNamed->first)
                {
                    itDrot->second.iPtr->moveToNamedPositionWait(timer.residual());
                }
            }
        }
    }
    catch(...)
    {
        TrackLogBoostDiagnosticInformation(__LINE__);
        U8_LLOG("");
        throw;
    }

    U8_LLOG(__FUNCTION__);
    return status;
}


void trackingCtrl::moveToNamedPosition(const std::map<std::string, std::string>& _mapNamedPos)
{
    U6_LLOG(__FUNCTION__ << " ~ entering");
    throw(std::runtime_error(std::string(std::string("trackingCtrl::") + __FUNCTION__)));
    try
    {
        moveToNamedPositionStart(_mapNamedPos);
        moveToNamedPositionWait(_mapNamedPos, Nice::Time::seconds(100));
    }
    catch(...)
    {
        TrackLogBoostDiagnosticInformation(__LINE__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG(__FUNCTION__ << " ~ exiting");
}


void trackingCtrl::moveToNamedPositionStart(const std::map<std::string, std::string>& _mapNamedPos)
{
    U6_LLOG(__FUNCTION__ << " ~ entering");
    U8_LLOG("");
    throw(std::runtime_error(std::string(std::string("trackingCtrl::") + __FUNCTION__)));
#ifdef UNDEFINED
    int idxPos;
    std::map<std::string, std::string>::const_iterator itPos = _mapNamedPos.find("DROT_PosAng");
    if(itPos != _mapNamedPos.end())
    {
        U9_LLOG("DROT_PosAng :" << itPos->second);
        if(m_drot)
        {
            try
            {
                std::map<std::string, int> nps;
                nps = m_MapDrots.find("DROT")->second.namedIds;
                I_LLOG("Moving to named position " << itPos->second);
                const std::map<std::string, std::tr1::tuple<int, int> > & NamedPosMap = m_mainMapNamedPos.find(suc::DROT)->second;
                if(NamedPosMap.find(itPos->second) != NamedPosMap.end())
                {
                    m_drot->moveToNamedPositionStart(std::tr1::get<0>(NamedPosMap.find(itPos->second)->second));
                }
                else
                {
                    std::stringstream log_msg;
                    log_msg << "Error :cannot find named position " << itPos->second;
                    E_LLOG(log_msg.str());
                    throw(std::runtime_error(log_msg.str()));
                }
            }
            catch(...)
            {
                TrackLogBoostDiagnosticInformation(__LINE__);
                U8_LLOG("");
                throw;
            }
        }
        else
        {
            std::string log_msg("Requested derotation to position: derotator is not connected");
            E_LLOG(log_msg);
            U8_LLOG("");
            throw(std::runtime_error(log_msg));
        }
    }

    itPos = _mapNamedPos.find("ADC1_PosAng");
    if(itPos != _mapNamedPos.end())
    {
        if(m_ptrAdcWorkerThread && m_ptrAdcWorkerThread->GetMode() == "OFF")
        {
            if(m_adc1)
            {
                try
                {
                    const std::map<std::string, std::tr1::tuple<int, int> > & NamedPosMap = m_mainMapNamedPos.find(suc::ADC1)->second;
                    if(NamedPosMap.find(itPos->second) != NamedPosMap.end())
                    {
                        m_adc1->moveToNamedPositionStart(std::tr1::get<0>(NamedPosMap.find(itPos->second)->second));
                    }
                    else
                    {
                        std::stringstream log_msg;
                        log_msg << "Error :cannot find named position " << itPos->second;
                        E_LLOG(log_msg.str());
                        throw(std::runtime_error(log_msg.str()));
                    }
                }
                catch(...)
                {
                    TrackLogBoostDiagnosticInformation(__LINE__);
                    U8_LLOG(__FUNCTION__);
                    throw;
                }
            }
            else
            {
                std::string log_msg("Error : ADC1 is not connected");
                E_LLOG(log_msg);
                U8_LLOG("");
                throw(std::runtime_error(log_msg));
            }
        }
        else
        {
            std::stringstream log_msg;
            if(m_ptrAdcWorkerThread)
            {
                log_msg << __FUNCTION__ << " ~ Error : ADC automatic correction mode is ON";
            }
            else
            {
                log_msg << __FUNCTION__ << " ~ Error : ADC is not available";
            }
            E_LLOG(log_msg.str());
            U8_LLOG("");
            throw(std::runtime_error(log_msg.str()));
        }
    }
    itPos = _mapNamedPos.find("ADC2_PosAng");
    if(itPos != _mapNamedPos.end())
    {
        if(m_ptrAdcWorkerThread && m_ptrAdcWorkerThread->GetMode() == "OFF")
        {
            if(m_adc2)
            {
                try
                {
                    const std::map<std::string, std::tr1::tuple<int, int> > & NamedPosMap = m_mainMapNamedPos.find(suc::ADC2)->second;
                    if(NamedPosMap.find(itPos->second) != NamedPosMap.end())
                    {
                        m_adc2->moveToNamedPositionStart(std::tr1::get<0>(NamedPosMap.find(itPos->second)->second));
                    }
                    else
                    {
                        std::stringstream log_msg;
                        log_msg << "Error :cannot find named position " << itPos->second;
                        E_LLOG(log_msg.str());
                        throw(std::runtime_error(log_msg.str()));
                    }
                }
                catch(...)
                {
                    TrackLogBoostDiagnosticInformation(__LINE__);
                    U8_LLOG(__FUNCTION__);
                    throw;
                }
            }
            else
            {
                std::string log_msg("Requested positioning to angle: ADC2 is not connected");
                E_LLOG(log_msg);
                U8_LLOG("");
                throw(std::runtime_error(log_msg));
            }
        }
        else
        {
            std::stringstream log_msg;
            if(m_ptrAdcWorkerThread)
            {
                log_msg << __FUNCTION__ << " ~ Error : ADC automatic correction mode is ON";
            }
            else
            {
                log_msg << __FUNCTION__ << " ~ Error : ADC is not available";
            }
            E_LLOG(log_msg.str());
            U8_LLOG("");
            throw(std::runtime_error(log_msg.str()));
        }
    }
    U8_LLOG(__FUNCTION__ << " ~ exiting");
#endif
}


void trackingCtrl::moveToNamedPositionWait(const std::map<std::string, std::string>& _mapNamedPos, const Nice::Time & _time)
{
    U6_LLOG(__FUNCTION__ << " ~ entering");
    U8_LLOG("");
    throw(std::runtime_error(std::string(std::string("trackingCtrl::") + __FUNCTION__)));
#ifdef UNDEFINED
    ////// DROT //////
    std::map<std::string, std::string>::const_iterator itPos = _mapNamedPos.find("DROT_PosAng");
    if(itPos != _mapNamedPos.end())
    {
        if(m_drot)
        {
            try
            {
                U9_LLOG(__FUNCTION__ << " ~ Start Waiting " << _time << " seconds");
                m_drot->moveToNamedPositionWait(_time);
                U9_LLOG(__FUNCTION__ << " ~ Waiting finished");
            }
            catch(...)
            {
                TrackLogBoostDiagnosticInformation(__LINE__);
                U8_LLOG("");
                throw;
            }
        }
        else
        {
            std::string log_msg("Requested derotation to position: derotator is not connected");
            E_LLOG(log_msg);
            U8_LLOG("");
            throw(std::runtime_error(log_msg));
        }
    }

    ////// ADC //////
    itPos = _mapNamedPos.find("ADC1_PosAng");
    if(itPos != _mapNamedPos.end())
    {
        if(m_adc1)
        {
            try
            {
                int result = m_adc1->moveToNamedPositionWait(_time);
                U9_LLOG("m_adc1->moveAbsoluteWait returned -> " << result);
            }
            catch(...)
            {
                TrackLogBoostDiagnosticInformation(__LINE__);
                U8_LLOG("");
                throw;
            }
        }
        else
        {
            std::string log_msg("Requested positioning to angle: ADC is not connected");
            E_LLOG(log_msg);
            U8_LLOG("");
            throw(std::runtime_error(log_msg));
        }
    }
    itPos = _mapNamedPos.find("ADC2_PosAng");
    if(itPos != _mapNamedPos.end())
    {
        if(m_adc2)
        {
            try
            {
                m_adc2->moveToNamedPositionWait(_time);
            }
            catch(...)
            {
                TrackLogBoostDiagnosticInformation(__LINE__);
                U8_LLOG("");
                throw;
            }
        }
        else
        {
            std::string log_msg("Requested positioning to angle: ADC is not connected");
            E_LLOG(log_msg);
            U8_LLOG("");
            throw(std::runtime_error(log_msg));
        }
    }
    U8_LLOG("");
#endif
}


void trackingCtrl::install_callbacks()
{
    U6_LLOG("trackingCtrl::"  << __FUNCTION__ );
    Nice::PropertyPtr prop;
    std::string bridgeName;
    std::vector<std::string> vecCommands;
    vecCommands.push_back("MOVETOHOME");
    vecCommands.push_back("MOVETONAMEDPOSITION");
    vecCommands.push_back("MOVEABSOLUTE");
    vecCommands.push_back("MOVERELATIVE");
    vecCommands.push_back("MOVETOLIMIT");
    vecCommands.push_back("STARTDEROTATION");
    vecCommands.push_back("STARTDEROTATIONHERE");
    std::vector<std::string>::iterator itCmd;
    if(m_drot)
    {
    for(itCmd = vecCommands.begin(); itCmd < vecCommands.end(); itCmd++)
    {
        std::string strPropName = "CMD." + *itCmd + ".EXEC";
        bridgeName = "derotator-" + su::str_tolower(*itCmd);
        prop = m_drot->config()->leaf(strPropName);
        if(prop->getBridge(bridgeName))
        {
            U9_LLOG("Deleting bridge : " << bridgeName);
            prop->removeBridge(bridgeName);
        }
        void (DrotDevice::* Bridge)(const Nice::Completion &)  = NULL;
        if(strPropName == "CMD.MOVETOHOME.EXEC")
        {
            Bridge = &DrotDevice::MoveToHomeBridge;
        }
        else if(strPropName == "CMD.MOVETONAMEDPOSITION.EXEC")
        {
            Bridge = &DrotDevice::MoveToNamedBridge;
        }
        else if(strPropName == "CMD.MOVEABSOLUTE.EXEC")
        {
            Bridge = &DrotDevice::MoveAbsoluteBridge;
        }
        else if(strPropName == "CMD.MOVERELATIVE.EXEC")
        {
            Bridge = &DrotDevice::MoveRelativeBridge;
        }
        else if(strPropName == "CMD.MOVETOLIMIT.EXEC")
        {
            Bridge = &DrotDevice::MoveToLimitBridge;
        }
        else if(strPropName == "CMD.STARTDEROTATION.EXEC")
        {
            Bridge = &DrotDevice::StartDerotationBridge;
        }
        else if(strPropName == "CMD.STARTDEROTATIONHERE.EXEC")
        {
            Bridge = &DrotDevice::StartDerotationHereBridge;
        }
        if(m_MapDrots.begin()->second.iPtr)
        {
            prop->applyBridge(
                new Nice::PropertyBridgeMemFn<Nice::Completion, DrotDevice, void (DrotDevice::*)(const Nice::Completion &)>(&m_MapDrots.begin()->second, Bridge, prop->id()), bridgeName);
            U9_LLOG( m_drot << " -- " << bridgeName << " ~ " << Bridge);
        }
        else
        {
            U9_LLOG("Could  not create Bridge due to NULL Object");
        }
    }
    }

    // std::string strPropName = "VAR.POSITION.ABSOLUTE_ENCODER";
    // void (DrotDevice::* Bridge)(const double & val)  = NULL;
    // bridgeName = "derotator-absolute-encoder";
    // prop = m_drot->config()->leaf(strPropName);
    // Bridge = &DrotDevice::VarAbsoluteEncoderBridge;

    // // Non funzionano
    // // prop->applyBridge(
    // //     new Nice::PropertyBridgeMemFn<double, DrotDevice, void (DrotDevice::*)(double)>(&m_MapDrots.begin()->second, Bridge, prop->id()), bridgeName);
    // //prop->applyBridge(
    // //    new Nice::PropertyBridgeMemFn<double, DrotDevice>(&m_MapDrots.begin()->second, &DrotDevice::VarAbsoluteEncoderBridge), strPropName);

    // // Funziona ma non prosegue OS
    // prop->applyBridge(
    //    new Nice::PropertyBridgeMemFn<double, niceCallback>(&m_callbackObject, &niceCallback::callback_absolute_encoder_position), strPropName);
    // U9_LLOG( m_drot << " -- " << bridgeName << " ~ " << Bridge);

    U8_LLOG("");
}

std::string trackingCtrl::getMode(const std::string & _device)
{
//    U6_LLOG(__FUNCTION__);
    std::string dev = su::left(_device, 3);
    std::string result;
    if(dev == "DRO")
    {
        if(m_drot)
            result = m_drot->getMode();
        else {W_LLOG("Asked DROT mode but DROT is unavailable");}
    }
    else if(dev == "ADC")
    {
        if(m_ptrAdcWorkerThread)
        {
            result = m_ptrAdcWorkerThread->GetMode();
        }
    }
    else
    {
        std::stringstream log_msg;
        log_msg << "Error : " << _device << " non recognized";
        U6_LLOG(__FUNCTION__);
        E_LLOG(log_msg.str());
        U8_LLOG("");
        throw(std::invalid_argument(log_msg.str()));
    }
//    U8_LLOG("");
    return result;
}


void trackingCtrl::moveAbsolute(const std::map<std::string, double> & _mapPos, const std::map<std::string, std::string> & _mapUnit)
{
    U6_LLOG(__FUNCTION__ << " ~ entering");
    try
    {
        moveAbsoluteStart(_mapPos, _mapUnit);
        moveAbsoluteWait(_mapPos, Nice::Time::seconds(100));
    }
    catch(...)
    {
        TrackLogBoostDiagnosticInformation(__LINE__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG(__FUNCTION__ << " ~ exiting");
}


void trackingCtrl::moveAbsoluteStart(const std::map<std::string, double> & _mapPos, const std::map<std::string, std::string> & _mapUnit)
{
    U6_LLOG(__FUNCTION__ << " ~ entering");

    std::map<std::string, double>::const_iterator itPos = _mapPos.find("DROT_PosAng");
    bool bDrotInvolved = false;
    std::map<std::string, DrotDevice>::iterator itDrot;
    for(itDrot = m_MapDrots.begin(); itDrot != m_MapDrots.end(); itDrot++)
    {
        if(_mapPos.find(itDrot->first) != _mapPos.end())
        {
            bDrotInvolved = true;
            break;
        }
    }
    if(itPos != _mapPos.end() || bDrotInvolved)
    {
        U9_LLOG("DROT_PosAng :" << itPos->second);
        if(m_drot)
        {
            if(itPos != _mapPos.end())
            {
                try
                {
                    std::string unit = "DEGREE";
                    if(_mapUnit.find("DROT_PosAng") != _mapUnit.end() && _mapUnit.find("DROT_PosAng")->second != "DEFAULT")
                        unit = _mapUnit.find("DROT_PosAng")->second;
                    m_drot->moveAbsoluteStart(itPos->second, unit);
                }
                catch(...){ TrackLogBoostDiagnosticInformation(__LINE__); U8_LLOG(__FUNCTION__); throw; }
            }
            else
            {
                try
                {
                    for(itDrot = m_MapDrots.begin(); itDrot != m_MapDrots.end(); itDrot++)
                    {
                        std::string unit = "STEPS";
                        if(_mapUnit.find(itDrot->first) != _mapUnit.end())
                            unit = _mapUnit.find(itDrot->first)->second;
                        itDrot->second.iPtr->moveAbsoluteStart(_mapPos.find(itDrot->first)->second, unit);
                    }
                }
                catch(...){ TrackLogBoostDiagnosticInformation(__LINE__); U8_LLOG(__FUNCTION__); throw; }
            }
        }
        else
        {
            std::string log_msg("Requested derotation to position: derotator is not connected");
            E_LLOG(log_msg);
            U8_LLOG("");
            throw(std::runtime_error(log_msg));
        }
    }

    std::map<std::string, double>::const_iterator itPos1 = _mapPos.find("ADC1_PosAng");
    std::map<std::string, double>::const_iterator itPos2 = _mapPos.find("ADC2_PosAng");
    itPos1 = _mapPos.find("ADC1_PosAng");
    bool bAdcInvolved = false;
    std::map<std::string, WheelDevice>::iterator itWheel;
    for(itWheel = m_MapAdcs.begin(); itWheel != m_MapAdcs.end(); itWheel++)
    {
        if(_mapPos.find(itWheel->first) != _mapPos.end())
        {
            bAdcInvolved = true;
            break;
        }
    }
    if(m_MapAdcs.size() == 0)
    {
        std::string log_msg("Error : ADC is not connected");
        E_LLOG(log_msg);
        U8_LLOG("");
        throw(std::runtime_error(log_msg));
    }
    if(itPos1 != _mapPos.end() || itPos2 != _mapPos.end() || bAdcInvolved)
    {
        if(m_ptrAdcWorkerThread && m_ptrAdcWorkerThread->GetMode() == "OFF")
        {
            if(itPos1 != _mapPos.end() && m_adc1)
            {
                try
                {
                    std::string unit = "DEGREE";
                    if(_mapUnit.find("ADC1_PosAng") != _mapUnit.end() && _mapUnit.find("ADC1_PosAng")->second != "DEFAULT")
                        unit = _mapUnit.find("ADC1_PosAng")->second;
                    m_adc1->moveAbsoluteStart(itPos1->second, unit);
                }
                catch(...){ TrackLogBoostDiagnosticInformation(__LINE__); U8_LLOG(__FUNCTION__); throw; }
            }
            else if(itPos1 != _mapPos.end() && !m_adc1)
            {
                std::string log_msg("Error : ADC1 is not connected");
                E_LLOG(log_msg);
                U8_LLOG("");
                throw(std::runtime_error(log_msg));
            }
            if(itPos2 != _mapPos.end() && m_adc2)
            {
                try
                {
                    std::string unit = "DEGREE";
                    if(_mapUnit.find("ADC2_PosAng") != _mapUnit.end() && _mapUnit.find("ADC2_PosAng")->second != "DEFAULT")
                        unit = _mapUnit.find("ADC2_PosAng")->second;
                    m_adc2->moveAbsoluteStart(itPos2->second, unit);
                }
                catch(...){ TrackLogBoostDiagnosticInformation(__LINE__); U8_LLOG(__FUNCTION__); throw; }
            }
            else if(itPos2 != _mapPos.end() && !m_adc2)
            {
                std::string log_msg("Error : ADC2 is not connected");
                E_LLOG(log_msg);
                U8_LLOG("");
                throw(std::runtime_error(log_msg));
            }
            if(bAdcInvolved)
            {
                try
                {
                    for(itWheel = m_MapAdcs.begin(); itWheel != m_MapAdcs.end(); itWheel++)
                    {
                        std::string unit = "STEPS";
                        if(_mapUnit.find(itWheel->first) != _mapUnit.end())
                        {
                            if(itWheel->second.iPtr)
                            {
                                unit = _mapUnit.find(itWheel->first)->second;
                                itWheel->second.iPtr->moveAbsoluteStart(_mapPos.find(itWheel->first)->second, unit);
                            }
                        }
                    }
                }
                catch(...){ TrackLogBoostDiagnosticInformation(__LINE__); U8_LLOG(__FUNCTION__); throw; }
            }
        }
        else
        {
            std::stringstream log_msg;
            if(m_ptrAdcWorkerThread)
            {
                log_msg << __FUNCTION__ << " ~ Error : ADC automatic correction mode is ON";
            }
            else
            {
                log_msg << __FUNCTION__ << " ~ Error : ADC is not available";
            }
            E_LLOG(log_msg.str());
            U8_LLOG("");
            throw(std::runtime_error(log_msg.str()));
        }
    }
    U8_LLOG(__FUNCTION__ << " ~ exiting");
}

/** Method to wait end of  motion to absolute position for tracking devices */
void trackingCtrl::moveAbsoluteWait(const std::map<std::string, double>& _mapPos, const Nice::Time& _time)
{
    U6_LLOG(__FUNCTION__ << " ~ entering");
    su::Timer timer(_time);

    const int WAIT_RESULT_TIMED_OUT = 0;
    ////// DROT //////
    std::map<std::string, double>::const_iterator itPos = _mapPos.find("DROT_PosAng");
    if(itPos != _mapPos.end())
    {
        if(m_drot)
        {
            try
            {
                U9_LLOG(__FUNCTION__ << " ~ Start Waiting for " << timer.residual());
                if(m_drot->moveAbsoluteWait(timer.residual()) == WAIT_RESULT_TIMED_OUT)
                {
                    std::stringstream log_msg;
                    log_msg << __FUNCTION__ << suc::tilde << "DROT_PosAng" << suc::tilde << "Error : wait timed out";
                    throw(std::runtime_error(log_msg.str()));
                }
            }
            catch(...)
            {
                TrackLogBoostDiagnosticInformation(__LINE__);
                U8_LLOG("");
                throw;
            }
        }
        else
        {
            std::string log_msg("Requested derotation to position: derotator is not connected");
            E_LLOG(log_msg);
            U8_LLOG("");
            throw(std::runtime_error(log_msg));
        }
    }

    ////// ADC //////
    itPos = _mapPos.find("ADC1_PosAng");
    if(itPos != _mapPos.end())
    {
        if(m_adc1)
        {
            try
            {
                if(m_adc1->moveAbsoluteWait(timer.residual()) == WAIT_RESULT_TIMED_OUT)
                {
                    std::stringstream log_msg;
                    log_msg << __FUNCTION__ << suc::tilde << "ADC1_PosAng" << suc::tilde << "Error : wait timed out";
                    throw(std::runtime_error(log_msg.str()));
                }
            }
            catch(...)
            {
                TrackLogBoostDiagnosticInformation(__LINE__);
                U8_LLOG("");
                throw;
            }
        }
        else
        {
            std::string log_msg("Requested positioning to angle: ADC is not connected");
            E_LLOG(log_msg);
            U8_LLOG("");
            throw(std::runtime_error(log_msg));
        }
    }
    itPos = _mapPos.find("ADC2_PosAng");
    if(itPos != _mapPos.end())
    {
        if(m_adc2)
        {
            try
            {
                if(m_adc2->moveAbsoluteWait(timer.residual()) == WAIT_RESULT_TIMED_OUT)
                {
                    std::stringstream log_msg;
                    log_msg << __FUNCTION__ << suc::tilde << "ADC2_PosAng" << suc::tilde << "Error : wait timed out";
                    throw(std::runtime_error(log_msg.str()));
                }
            }
            catch(...)
            {
                TrackLogBoostDiagnosticInformation(__LINE__);
                U8_LLOG("");
                throw;
            }
        }
        else
        {
            std::string log_msg("Requested positioning to angle: ADC is not connected");
            E_LLOG(log_msg);
            U8_LLOG("");
            throw(std::runtime_error(log_msg));
        }
    }
    U8_LLOG("");
}


void trackingCtrl::moveRelative(const std::map<std::string, double>& _mapPos, const std::map<std::string, std::string> & _mapUnit)
{
    std::stringstream log_msg;
    log_msg << __FUNCTION__ << " ~ Not yet implemented";
    W_LLOG(log_msg.str());
    throw(std::runtime_error(log_msg.str()));
}


void trackingCtrl::moveRelativeStart(const std::map<std::string, double>& _mapPos, const std::map<std::string, std::string> & _mapUnit)
{
    std::stringstream log_msg;
    log_msg << __FUNCTION__ << " ~ Not yet implemented";
    W_LLOG(log_msg.str());
    throw(std::runtime_error(log_msg.str()));
}


void trackingCtrl::moveRelativeWait(const std::map<std::string, double>& _mapPos, const Nice::Time& _time)
{
    std::stringstream log_msg;
    log_msg << __FUNCTION__ << " ~ Not yet implemented";
    W_LLOG(log_msg.str());
    throw(std::runtime_error(log_msg.str()));
}

/** @brief      Starts tracking test. Starts derotation for DROT at constant speed for the specified range. OsStartDerotationTest is blocking (synchronous)
  * @param[in] startPosDegree  The start position of derotation
  * @param[in] endPosDegree  End position of derotation
  * @param[in] speedDegreeSec Desired speed for derotation. Absolute value is taken into account */
int trackingCtrl::startTrackingTest(double startPosDegree, double endPosDegree, double speedDegreeSec)
{
    U6_LLOG(__FUNCTION__);
    int result = EXIT_FAILURE;
    if(m_drotMode == DROT_MODE_FIELD_FREE || m_drotMode == DROT_MODE_FIELD_FIXED)
    {
        try
        {
            I_LLOG("Starting Tracking test ... ");
            result = m_drot->OsStartDerotationTest(startPosDegree, endPosDegree, speedDegreeSec);
            I_LLOG("tracking test Completed");
        }
        catch(Basda::Dev::MoccaUploadExternalProfileException & err)
        {
            U7_LLOG(err.what())
            U8_LLOG("");
            throw err;
        }
        catch(Basda::DevException & err)
        {
            U7_LLOG(err.what())
            U8_LLOG("");
            throw err;
        }
        catch(...)
        {
            TrackLogBoostDiagnosticInformation(__LINE__);
            U8_LLOG("");
            throw;
        }
    }
    else
    {
        std::string log_msg = "Cannot start tracking when DROT is in PUPIL mode";
        E_LLOG(log_msg);
        throw(std::runtime_error(log_msg));
    }
    U8_LLOG(__FUNCTION__ << " ~ exiting");
    return result;
}



/** @brief      Starts tracking. Starts derotation and tracking for DROT. OsStartDerotation is blocking (synchronous)
  * @param[in] _startDate  The start date
  * @param[in] _position   The "Position Angle" of the derotator in degree. It will be the angle of the true north from y direction of scicam measuring in clock-wise direction */
int trackingCtrl::startTracking(const Nice::Date & _startDate, double _position)
{
    U6_LLOG(__FUNCTION__);
    int result = EXIT_FAILURE;
    if(m_drotMode == DROT_MODE_FIELD_FREE || m_drotMode == DROT_MODE_FIELD_FIXED)
    {
#define SLITNARROW 1
#define SLITWIDE   2
        if(m_drot)
        {
            bool autoStart = m_drotMode == DROT_MODE_FIELD_FREE;
            try
            {
                I_LLOG("Starting tracking ... ");
                result = m_drot->OsStartDerotation(_startDate, _position, SLITNARROW, autoStart);
                I_LLOG("tracking Completed");
            }
            catch(...)
            {
                TrackLogBoostDiagnosticInformation(__LINE__);
                U8_LLOG("");
                throw;
            }
        }
        else
        {
            std::stringstream log_msg;
            log_msg << "Cannot start Derotation: DROT is unavailable";
            W_LLOG(log_msg.str());
            U8_LLOG("");
            throw(std::runtime_error(log_msg.str()));
        }
    }
    else
    {
        std::string log_msg = "Cannot start tracking when DROT is in PUPIL mode";
        E_LLOG(log_msg);
        throw(std::runtime_error(log_msg));
    }
    U8_LLOG(__FUNCTION__ << " ~ exiting");
    return result;
}

int trackingCtrl::startTracking()
{
    U6_LLOG(__FUNCTION__);
    int result = EXIT_FAILURE;
    if(m_drotMode == DROT_MODE_FIELD_FREE || m_drotMode == DROT_MODE_FIELD_FIXED)
    {
        if(m_drot)
        {
            try
            {
                // result = m_drot->OsStartDerotation(m_drotMode == DROT_MODE_FIELD_FREE);
                // I_LLOG("tracking started");
                E_LLOG("Error , deprecated function.");
            }
            catch(...)
            {
                TrackLogBoostDiagnosticInformation(__LINE__);
                U8_LLOG("");
                throw;
            }
        }
        else
        {
            std::stringstream log_msg;
            log_msg << "Cannot start Derotation: DROT is unavailable";
            W_LLOG(log_msg.str());
            U8_LLOG("");
            throw(std::runtime_error(log_msg.str()));
        }
    }
    else
    {
        std::string log_msg = "Cannot start tracking when DROT is in PUPIL mode";
        E_LLOG(log_msg);
        U8_LLOG("");
        throw(std::runtime_error(log_msg));
    }
    U8_LLOG(__FUNCTION__ << " ~ exiting");
    return result;
}

int trackingCtrl::waitTracking(Ice::Long seconds)
{
    U6_LLOG(__FUNCTION__);
    int status = EXIT_SUCCESS;
    if(m_drot)
    {
        try
        {
            status = m_drot->OsWaitDerotation(seconds);
        }
        catch(...)
        {
            TrackLogBoostDiagnosticInformation(__LINE__);
            U8_LLOG("");
            throw;
        }
    }
    else
    {
        std::stringstream log_msg;
        log_msg << "Cannot start Derotation: DROT is unavailable";
        W_LLOG(log_msg.str());
        U8_LLOG("");
        throw(std::runtime_error(log_msg.str()));
    }
    U8_LLOG(__FUNCTION__ << " ~ exiting");
    return status;
}


/** @brief set devices mode, will be used as discriminant when starting tracking
  * @param[in] _mapSetup: the map as read from the OB */
int trackingCtrl::setMode(const std::map<std::string, std::string> & _mapSetup)
{
    U6_LLOG(__FUNCTION__);
    int status = EXIT_SUCCESS;
    std::map<std::string, std::string>::const_iterator itFindMode;

    itFindMode = _mapSetup.find("DROT_Mode");
    if(itFindMode != _mapSetup.end())
    {
        EDrotModes previousMode = m_drotMode;
        if(itFindMode->second == "PUPIL")
        {
            m_drotMode = DROT_MODE_PUPIL;
        }
        else if(itFindMode->second == "FIELD_FIXED")
        {
            m_drotMode = DROT_MODE_FIELD_FIXED;
        }
        else if(itFindMode->second == "FIELD_FREE")
        {
            m_drotMode = DROT_MODE_FIELD_FREE;
        }
        else
        {
            std::stringstream log_msg;
            log_msg << "Error : admitted modes for DROT are {\"PUPIL\", \"FIELD_FIXED\", \"FIELD_FREE\"}";
            E_LLOG(log_msg.str());
            throw(std::invalid_argument(log_msg.str()));
        }
        if(m_drot)
        {
            if(itFindMode->second == "PUPIL" && m_drot->isMoving())
            {
                m_drot->stop();
                I_LLOG("DROT moving when switching to PUPIL mode: motion stopped ");
            }
            else
            {
    //            startTracking
            }
        }
        else
        {
            std::stringstream log_msg;
            log_msg << "Cannot change mode : DROT is unavailable";
            W_LLOG(log_msg.str());
            U8_LLOG("");
            throw(std::runtime_error(log_msg.str()));
        }
    }

    itFindMode = _mapSetup.find("ADC_Mode");
    if(itFindMode != _mapSetup.end())
    {
        if(m_ptrAdcWorkerThread)
            m_ptrAdcWorkerThread->SetMode(itFindMode->second);
        else
        {
            std::stringstream log_msg;
            log_msg << __FUNCTION__ << " ~ Error : ADC is not available";
            E_LLOG(log_msg.str());
            U8_LLOG("");
            throw(std::runtime_error(log_msg.str()));
        }
    }

    U8_LLOG("");
    return status;
}


// get the number of devices connected
int trackingCtrl::getNdevices()
{
    int nDevices = 0;
    U6_LLOG(__FUNCTION__ << " ~ Getting number of tracking devices");
    if(m_adc1)
    {
        nDevices++;
    }
    if(m_adc2)
    {
        nDevices++;
    }
    if(m_drot)
    {
        nDevices++;
    }
    U8_LLOG("");
    return nDevices;
}


Ice::Int trackingCtrl::getPositionDict(std::map<std::string, double>& _mapPosition, const std::string& _unit)
{
//    U6_LLOG(__FUNCTION__);
    Ice::Int status = EXIT_SUCCESS;

    std::map<std::string, double>::const_iterator itFindDerot2 = _mapPosition.find(suc::DROT_PosAng);
    std::map<std::string, double>::const_iterator itFindADC12 = _mapPosition.find(suc::ADC1_PosAng);
    std::map<std::string, double>::const_iterator itFindADC22 = _mapPosition.find(suc::ADC2_PosAng);
    std::map<std::string, double>::const_iterator itFindDerot = _mapPosition.find(suc::DROT);
    std::map<std::string, double>::const_iterator itFindADC1 = _mapPosition.find(suc::ADC1);
    std::map<std::string, double>::const_iterator itFindADC2 = _mapPosition.find(suc::ADC2);

    if(itFindDerot != _mapPosition.end())
    {
        _mapPosition[itFindDerot->first] = getPosition(itFindDerot->first, _unit);
    }
    if(itFindDerot2 != _mapPosition.end())
    {
        _mapPosition[itFindDerot2->first] = getPosition(itFindDerot2->first, _unit);
    }
    if(itFindADC1 != _mapPosition.end())
    {
        _mapPosition[itFindADC1->first] = getPosition(itFindADC1->first, _unit);
    }
    if(itFindADC12 != _mapPosition.end())
    {
        _mapPosition[itFindADC12->first] = getPosition(itFindADC12->first, _unit);
    }
    if(itFindADC2 != _mapPosition.end())
    {
        _mapPosition[itFindADC2->first] = getPosition(itFindADC2->first, _unit);
    }
    if(itFindADC22 != _mapPosition.end())
    {
        _mapPosition[itFindADC22->first] = getPosition(itFindADC22->first, _unit);
    }
//    U8_LLOG("");
    return status;
}

/** input
 *  @param[in] _deviceName : equal the TAN service alias name
 *  @param[in] _unit       :  */
double trackingCtrl::getPosition(const std::string & _deviceName, const std::string & _unit)
{
//    U6_LLOG(__FUNCTION__);

    double position = -9e12;
    if(_deviceName == suc::DROT || _deviceName == suc::DROT_PosAng)
    {
        if(m_drot)
        {
            position = m_drot->getPosition(_unit);
            try{position = m_drot->config()->leaf("VAR.POSITION.DEVICE_ENCODER." + _unit)->get<double>();}catch(...){TrackLogBoostDiagnosticInformation(__LINE__);} 
        }
        else
        {
            W_LLOG("Could not get DROT position : module is not active");
        }
    }

    if(_deviceName == suc::ADC1 || _deviceName == suc::ADC1_PosAng)
    {
        if(m_adc1)
        {
            position = m_adc1->getPosition(_unit);
        }
        else
        {
            W_LLOG("Could not get ADC1 position : module is not active");
        }
    }

    if(_deviceName == suc::ADC2 || _deviceName == suc::ADC2_PosAng)
    {
        if(m_adc2)
        {
            position = m_adc2->getPosition(_unit);
        }
        else
        {
            W_LLOG("Could not get ADC2 position : module is not active");
        }
    }
//    U8_LLOG("");
    return position;
}


double trackingCtrl::getConfigPosition(const std::string & _deviceName, const std::string & _unit)
{
//    U6_LLOG(__FUNCTION__);
    int status = EXIT_SUCCESS;
    std::map<std::string, DrotDevice>::const_iterator itDrot;
    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;
    double deviceEncoderPosition;

    for(itDrot = m_MapDrots.begin(); itDrot != m_MapDrots.end(); itDrot++)
    {
        if(itDrot->first != _deviceName)continue;
        try{deviceEncoderPosition = itDrot->second.iPtr->config()->leaf("VAR.POSITION.DEVICE_ENCODER." + _unit)->get<double>();}
        catch(...){catch_msg.str(""); catch_msg << m_service << " ~ getDeviceEncoderPosition ~ " << itDrot->first; TrackLogBoostDiagnosticInformation(__LINE__, catch_msg.str());}
        return deviceEncoderPosition;
    }

    for(itWheel = m_MapAdcs.begin(); itWheel != m_MapAdcs.end(); itWheel++)
    {
        if(itWheel->first != _deviceName)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; TrackLogBoostDiagnosticInformation(__LINE__, catch_msg.str());}
        return deviceEncoderPosition;
    }
//    U8_LLOG("");
    return -9.0e-9;
}



// only derotator is equipped with absolute encoder
double trackingCtrl::getAbsEncPosition()
{
    double absEncPos = 0;
    U6_LLOG(__FUNCTION__ << " ~ Getting drot absolute encoder postion ...")
    if(m_drot)
    {
        absEncPos = m_drot->getAbsoluteEncoderPosition();
    }
    else
    {
        std::stringstream log_msg;
        log_msg << "Cannot get absolute encoder position : DROT is unavailable";
        W_LLOG(log_msg.str());
        U8_LLOG("");
        throw(std::runtime_error(log_msg.str()));
    }
    U8_LLOG("");
    return absEncPos;
}

int trackingCtrl::isMoving(std::map<std::string, bool>& _mapDev)
{
    U6_LLOG(__FUNCTION__ << " ~ Checking ifDROT is moving ... ");
    W_LLOG(__FUNCTION__ << " ~ NOT YET IMPLEMENTED")
    U8_LLOG("");
    throw(std::runtime_error("isMoving : function not yet implemented"));
    return EXIT_FAILURE;
}

int trackingCtrl::emergencyAbort()
{
    U6_LLOG(__FUNCTION__ << " ~ ABORT motion for derotator");
    int status = EXIT_SUCCESS;
    try
    {
        if(m_drot)
        {
            m_drot->abort();
        }
        else
        {
            W_LLOG("Could not abort DROT : module is not active");
        }
    }
    catch(...)
    {
        TrackLogBoostDiagnosticInformation(__LINE__);
        W_LLOG("Caught exception aborting DROT, not retrown to assure emergency abort");
        status = EXIT_FAILURE;
    }

    try
    {
        if(m_adc1)
        {
            m_adc1->abort();
        }
        else
        {
            W_LLOG("Could not abort ADC1 : module is not active");
        }
    }
    catch(...)
    {
        TrackLogBoostDiagnosticInformation(__LINE__);
        W_LLOG("Caught exception aborting ADC1, not retrown to assure emergency abort");
        status = EXIT_FAILURE;
    }
    try
    {
        if(m_adc2)
        {
            m_adc2->abort();
        }
        else
        {
            W_LLOG("Could not stop ADC2 : module is not active");
        }
    }
    catch(...)
    {
        TrackLogBoostDiagnosticInformation(__LINE__);
        W_LLOG("Caught exception aborting ADC2, not retrown to assure emergency abort");
        status = EXIT_FAILURE;
    }
    U8_LLOG("");
    return status;
}


int trackingCtrl::stopAll()
{
    U6_LLOG(__FUNCTION__);
    W_LLOG("Stopping all tracking devices");
    int status = EXIT_SUCCESS;
    if(m_drot)
    {
        try
        {
            I_LLOG("Stopping derotator ...");
            m_drot->stop();
        }
        catch(...)
        {
            TrackLogBoostDiagnosticInformation(__LINE__);
            W_LLOG("Caught exception stopping DROT, not retrown to assure emergency stop");
            status = EXIT_FAILURE;
        }
    }
    else
    {
        W_LLOG("Could not stop DROT : module is not active");
    }
    if(m_adc1)
    {
        try
        {
            m_adc1->stop();
        }
        catch(...)
        {
            TrackLogBoostDiagnosticInformation(__LINE__);
            W_LLOG("Caught exception stopping ADC1, not retrown to assure emergency stop");
            status = EXIT_FAILURE;
        }
    }
    else
    {
        W_LLOG("Could not stop ADC1 : module is not active");
    }
    if(m_adc2)
    {
        try
        {
            m_adc2->stop();
            I_LLOG("Stopped");
        }
        catch(...)
        {
            TrackLogBoostDiagnosticInformation(__LINE__);
            W_LLOG("Caught exception stopping ADC2, not retrown to assure emergency stop");
            status = EXIT_FAILURE;
        }
    }
    else
    {
        W_LLOG("Could not stop ADC2 : module is not active");
    }
    U8_LLOG("");
    return status;
}


int trackingCtrl::stop(const std::vector<std::string>& _vecDevAlias)
{
    U6_LLOG(__FUNCTION__);
    int status = EXIT_SUCCESS;
    std::vector<std::string>::const_iterator itFind;
    itFind = std::find(_vecDevAlias.begin(), _vecDevAlias.end(), suc::DROT_PosAng);
    if(itFind != _vecDevAlias.end())
    {
        if(m_drot)
        {
            try
            {
                m_drot->stop();
            }
            catch(...)
            {
                TrackLogBoostDiagnosticInformation(__LINE__);
                W_LLOG("Caught exception stopping DROT, not retrown to assure emergency stop");
                status = EXIT_FAILURE;
            }
        }
        else
        {
            W_LLOG("Could not stop DROT : module is not active");
        }
    }

    itFind = std::find(_vecDevAlias.begin(), _vecDevAlias.end(), suc::ADC1_PosAng);
    if(itFind != _vecDevAlias.end())
    {
        if(m_adc1)
        {
            try
            {
                m_adc1->stop();
            }
            catch(...)
            {
                TrackLogBoostDiagnosticInformation(__LINE__);
                W_LLOG("Caught exception stopping ADC1, not retrown to assure emergency stop");
                status = EXIT_FAILURE;
            }
        }
        else
        {
            W_LLOG("Could not stop ADC1 : module is not active");
        }
    }

    itFind = std::find(_vecDevAlias.begin(), _vecDevAlias.end(), suc::ADC2_PosAng);
    if(itFind != _vecDevAlias.end())
    {
        if(m_adc2)
        {
            try
            {
                m_adc2->stop();
            }
            catch(...)
            {
                TrackLogBoostDiagnosticInformation(__LINE__);
                W_LLOG("Caught exception stopping ADC2, not retrown to assure emergency stop");
                status = EXIT_FAILURE;
            }
        }
        else
        {
            W_LLOG("Could not stop ADC2 : module is not active");
        }
    }
    U8_LLOG("");
    return status;
}

int trackingCtrl::abort(const std::vector<std::string>& _vecDevAlias)
{
    U6_LLOG(__FUNCTION__);
    int status = EXIT_SUCCESS;
    std::vector<std::string>::const_iterator itFind;
    itFind = std::find(_vecDevAlias.begin(), _vecDevAlias.end(), suc::DROT_PosAng);
    if(itFind != _vecDevAlias.end())
    {
        if(m_drot)
        {
            try
            {
                m_drot->abort();
            }
            catch(...)
            {
                TrackLogBoostDiagnosticInformation(__LINE__);
                W_LLOG("Caught exception aborting DROT, not retrown to assure emergency abort");
                status = EXIT_FAILURE;
            }
        }
        else
        {
            W_LLOG("Could not abort DROT : module is not active");
        }
    }
    itFind = std::find(_vecDevAlias.begin(), _vecDevAlias.end(), suc::ADC1_PosAng);
    if(itFind != _vecDevAlias.end())
    {
        if(m_adc1)
        {
            try
            {
                m_adc1->abort();
            }
            catch(...)
            {
                TrackLogBoostDiagnosticInformation(__LINE__);
                W_LLOG("Caught exception aborting ADC2, not retrown to assure emergency abort");
                status = EXIT_FAILURE;
            }
        }
        else
        {
            W_LLOG("Could not abort ADC1 : module is not active");
        }
    }

    itFind = std::find(_vecDevAlias.begin(), _vecDevAlias.end(), suc::ADC2_PosAng);
    if(itFind != _vecDevAlias.end())
    {
        if(m_adc2)
        {
            try
            {
                m_adc2->abort();
            }
            catch(...)
            {
                TrackLogBoostDiagnosticInformation(__LINE__);
                W_LLOG("Caught exception aborting ADC2, not retrown to assure emergency abort");
                status = EXIT_FAILURE;
            }
        }
        else
        {
            W_LLOG("Could not abort ADC2 : module is not active");
        }
    }
    U8_LLOG("");
    return status;
}


// Nice::SeqDouble trackingCtrl::getSinusCurve(Nice::Time _duration,
//                                             Nice::Time _period,
//                                             Nice::Time _timeResolution,
//                                             double _amplitude,
//                                             double _phase,
//                                             double _startPoint)
// {
//     U6_LLOG(__FUNCTION__);
//     Nice::SeqDouble positionSequence;
//     int numberOfSamples = (int)(_duration.toSecondsDouble() / _timeResolution.toSecondsDouble());
//     for(int i = 0; i < numberOfSamples; i++)
//     {
//         positionSequence.push_back(_amplitude * sin(2 * M_PI * i * _timeResolution.toSecondsDouble() / _period.toSecondsDouble() + _phase) + _startPoint);
//     }
//     U8_LLOG("");
//     return positionSequence;
// }


int trackingCtrl::getJsonStatus(std::string & jsonStatus)
{
//    U6_LLOG(__FUNCTION__);
    int status = EXIT_SUCCESS;
    bool bJsonEmpty = jsonStatus.empty();
    bool bDrotReachable = true;

    std::vector<std::string>::iterator itAlias;
    static int bFirstTime = 0;
    Nice::JointPtr nodetrack = Nice::Application::config()->node(m_service);
    if(m_service_status != su::SERVICE_ONLINE)// !nodetrack->exist("NAME"))// node("NAME", false))
    {
        return EXIT_FAILURE;
    }
    std::string service_name = ""; // nodetrack->node("NAME")->get<std::string>();
    Nice::JointPtr nodeadc1 = nodetrack->node(su::str_toupper(service_name) + "ADC.ADC1.SVC");
    Nice::JointPtr nodeadc2 = nodetrack->node(su::str_toupper(service_name) + "ADC.ADC2.SVC");
    Nice::JointPtr nodedrot = nodetrack->node(su::str_toupper(service_name) + "DEROTATOR.DROT.SVC");

    bFirstTime++;
    if(m_drot)
    {
        bool isMoving = false;
//        try{isMoving = m_drot->isMoving();}catch(...){E_LLOG("isMoving");}
        static int DrotCounter = 0;
//        if((DrotCounter++ % 60) == 0){U9_LLOG("Drot Reachable -> " << m_drot->isReachable());}
        try
        {
            isMoving = m_drot->config()->leaf("VAR.MOVING")->get<bool>(); // m_drot->isMoving();
        }
        // catch(Nice::ChatException err)
        // {
        //     usleep(1000000);
        //     bDrotReachable = false;
        // }
        catch(...)
        {
            bDrotReachable = false;
            E_LLOG("isMoving failed");
            usleep(1000000);
        }
        if(bDrotReachable)
        {
            std::string named_pos = m_drot->config()->leaf("VAR.NAMED_POSITION")->get<std::string>();
            std::string service_name;
            try{service_name = m_drot->config()->leaf("VAR.SERVICE_NAME")->get<std::string>();/*m_drot->readServiceName();*/}catch(...){E_LLOG("readServiceName");}// service_name = "derotator"
            bool isSimulated = false;
            Nice::SeqBitSet switch_status;
        //    try{switch_status = m_drot->getPositionSwitchStatus(); isSimulated = switch_status[0].getDepth() < 32;}catch(...){E_LLOG("getPositionSwitchStatus");}
        //    try{U9_LLOG("getPositionSwitchStatus : " << switch_status << " ~ " << switch_status[0].getDepth());}catch(...){E_LLOG("getPositionSwitchStatus");}
            bool isAtHome = false;
            try{isAtHome = m_drot->config()->leaf("VAR.ATHOME")->get<bool>();/*m_drot->isAtHome();*/}catch(...){E_LLOG("isAtHome");}
            bool inLimit = false;
            try{inLimit = m_drot->config()->leaf("VAR.LIMIT")->get<bool>(); /*m_drot->isAtLimit();*/}catch(...){E_LLOG("isAtLimit");}
            if(inLimit)
            {
        //         try{U9_LLOG("isAtLimit2 : " << m_drot->get<int>("VAR.LIMIT"));}catch(...){E_LLOG("isAtLimit2");}
        //         std::string limit_result = m_drot->config()->getAtNode<std::string>("VAR.LIMIT");
        //         U9_LLOG("PropertyName : " << "VAR.LIMIT" << "; limit_result : " << limit_result);
        //         m_drot->config()->setAtNode<std::string>("VAR.LIMIT", "-2");
        //         limit_result = m_drot->config()->getAtNode<std::string>("VAR.LIMIT");
        //         U9_LLOG("PropertyName : " << "VAR.LIMIT" << "; limit_result : " << limit_result);
        //         limit_result = m_drot->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 = m_drot->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 : " << m_drot->getNamedPosition(1));}catch(...){E_LLOG("getNamedPosition");}
        //    try{U9_LLOG("readPosition : " << m_drot->readPosition());}catch(...){E_LLOG("readPosition");}
        //    try{U9_LLOG("readDeviceEncoderPosition : " << m_drot->readDeviceEncoderPosition());}catch(...){E_LLOG("readDeviceEncoderPosition");}
        //    try{U9_LLOG("readAbsoluteEncoderPosition : " << m_drot->readAbsoluteEncoderPosition());}catch(...){E_LLOG("readAbsoluteEncoderPosition");}
        //    try{U9_LLOG("readIncrementalEncoderPosition : " << m_drot->readIncrementalEncoderPosition());}catch(...){E_LLOG("readIncrementalEncoderPosition");}
            bool isReachable = false;
            try{isReachable = m_drot->config()->leaf("CMD.ISREACHABLE.RET")->get<bool>(); /*m_drot->isReachable();*/}catch(...){E_LLOG("isReachable");}
        //    try{U9_LLOG("getPosition : " << m_drot->getPosition());}catch(...){E_LLOG("getPosition");}
            int deviceEncoderPosition;
            try{ deviceEncoderPosition = (int)(m_drot->config()->leaf("VAR.POSITION.DEVICE_ENCODER.STEPS")->get<double>()); /*m_drot->getDeviceEncoderPosition();*/}catch(...){E_LLOG("getDeviceEncoderPosition");}
        //    try{U9_LLOG("getDeviceEncoderPosition : " << m_drot->getDeviceEncoderPosition());}catch(...){E_LLOG("getDeviceEncoderPosition");}
        //    try{U9_LLOG("getAbsoluteEncoderPosition : " << m_drot->getAbsoluteEncoderPosition());}catch(...){E_LLOG("getAbsoluteEncoderPosition");}
        //    try{U9_LLOG("getIncrementalEncoderPosition : " << m_drot->getIncrementalEncoderPosition());}catch(...){E_LLOG("getIncrementalEncoderPosition");}
        //    try{U9_LLOG("getVelocity : " << m_drot->getVelocity());}catch(...){E_LLOG("getVelocity");}

            std::stringstream json_alias;
            json_alias << "\"" << "DROT_PosAng" << "\": { \"name\": \"" << service_name << "\", \"named\": \"" << named_pos << "\", \"encoder\": " << deviceEncoderPosition << ", ";
            json_alias << "\"is_moving\": " << isMoving << ", \"in_limit\": " << inLimit << ", \"available\": " << isReachable << ", \"simulated\": " << isSimulated << "}";
            jsonStatus += json_alias.str();
            jsonStatus += ", ";
        }
    }
    if(!m_drot || !bDrotReachable)
    {
        std::stringstream json_alias;
        std::string drot_status = "UNAVAILABLE";
        std::string unavailable = drot_status;
        bDrotReachable = false;
        bool isMoving = false, inLimit = false, isSimulated = false;
        json_alias << "\"" << "DROT_PosAng" << "\": { \"name\": \"" << "derotator" << "\", \"named\": \"" << drot_status << "\", \"encoder\": " << 0 << ", ";
        json_alias << "\"is_moving\": " << isMoving << ", \"in_limit\": " << inLimit << ", \"available\": " << bDrotReachable << ", \"simulated\": " << isSimulated << "}";
        jsonStatus += json_alias.str();
        jsonStatus += ", ";
    }
    // DROT_Mode
    {
        std::stringstream json_alias;
        std::string drot_status = "UNAVAILABLE";
        std::string unavailable = drot_status;
        bool isReachable = false;
        bool isMoving = false, inLimit = false, isSimulated = false;
        if(m_drot)
        {
            drot_status = m_drot->getMode();
            isReachable = true;
        }
        json_alias << "\"" << "DROT_Mode" << "\": { \"name\": \"" << "DROT_Mode" << "\", \"named\": \"" << drot_status << "\", \"encoder\": " << 0 << ", ";
        json_alias << "\"is_moving\": " << isMoving << ", \"in_limit\": " << inLimit << ", \"available\": " << isReachable << ", \"simulated\": " << isSimulated << "}";
        jsonStatus += json_alias.str();
        jsonStatus += ", ";
    }

    Basda::MoccaWheelServiceWorkerInterfacePtr adcs[2] = {m_adc1, m_adc2};
    for(int i = 0; i < 2; i++)
    {
        if(m_adc1 && m_adc2)
        {
            bool isMoving = false;
            std::string service_name;
            try{service_name = adcs[i]->config()->leaf("VAR.SERVICE_NAME")->get<std::string>();/*adcs[i]->readServiceName();*/}catch(...){E_LLOG("readServiceName");}
//            try{isMoving = adcs[i]->isMoving();}catch(...){E_LLOG(service_name << " ~ isMoving");}
            try
            {
                isMoving = adcs[i]->config()->leaf("VAR.MOVING")->get<bool>(); /*adcs[i]->isMoving();*/
            }
            // catch(const Nice::ChatException & err)
            // {
            //     continue;
            //     usleep(1000000);
            // }
            catch(...)
            {
                E_LLOG(service_name << " ~ isMoving");
                usleep(1000000);
                continue;
            }
            std::string named_pos = adcs[i]->config()->leaf("VAR.NAMED_POSITION")->get<std::string>();
            bool isSimulated = false;
            Nice::SeqBitSet switch_status;
    //        try{switch_status = adcs[i]->getPositionSwitchStatus(); isSimulated = switch_status.size() < 32;}catch(...){E_LLOG(service_name << " ~ getPositionSwitchStatus");} lots of errors
    //        try{U9_LLOG("getPositionSwitchStatus : " << switch_status);}catch(...){E_LLOG("getPositionSwitchStatus");}
            bool isAtHome = false;
            try{isAtHome = adcs[i]->config()->leaf("VAR.ATHOME")->get<bool>(); /*adcs[i]->isAtHome();*/}catch(...){E_LLOG(service_name << " ~ isAtHome");}
            bool inLimit = false;
    //        try{U9_LLOG("NamedPosition : " << named_pos);}catch(...){E_LLOG("Named Position");}
    //        try{U9_LLOG("readDeviceEncoderPosition : " << adcs[i]->readDeviceEncoderPosition());}catch(...){E_LLOG("readDeviceEncoderPosition");}
            bool isReachable = false;
            try{isReachable = adcs[i]->config()->leaf("CMD.ISREACHABLE.RET")->get<bool>(); /*adcs[i]->isReachable();*/}catch(...){E_LLOG(service_name << " ~ isReachable");}
    //        try{U9_LLOG("getPosition : " << adcs[i]->getPosition());}catch(...){E_LLOG("getPosition");}
            int deviceEncoderPosition;
            try{deviceEncoderPosition = adcs[i]->config()->leaf("VAR.POSITION_REMAINDER")->get<int>(); /*adcs[i]->getDeviceEncoderPosition();*/}catch(...){E_LLOG(service_name << " ~ getDeviceEncoderPosition");}
    //        try{U9_LLOG("getVelocity : " << adcs[i]->getVelocity());}catch(...){E_LLOG("getVelocity");}
            std::stringstream json_alias;
            json_alias << "\"" << "ADC" << i + 1 << "_PosAng\": { \"name\": \"" << service_name << "\", \"named\": \"" << named_pos << "\", \"encoder\": " << deviceEncoderPosition << ", ";
            json_alias << "\"is_moving\": " << isMoving << ", \"in_limit\": " << inLimit << ", \"available\": " << isReachable << ", \"simulated\": " << isSimulated << "}";
            jsonStatus += json_alias.str();
            jsonStatus += ", ";
        }
        else
        {
            std::stringstream json_alias;
            std::string adc_status = "UNAVAILABLE";
            std::string unavailable = adc_status;
            bool isReachable = false;
            bool isMoving = false, inLimit = false, isSimulated = false;
            json_alias << "\"" << "ADC" << i + 1 << "_PosAng\": { \"name\": \"" << "PRISM" << i + 1 << "\", \"named\": \"" << unavailable << "\", \"encoder\": " << 0 << ", ";
            json_alias << "\"is_moving\": " << isMoving << ", \"in_limit\": " << inLimit << ", \"available\": " << isReachable << ", \"simulated\": " << isSimulated << "}";
            jsonStatus += json_alias.str();
            jsonStatus += ", ";
        }
    }
    // ADC_Mode
    {
        std::stringstream json_alias;
        std::string adc_status = "UNAVAILABLE";
        std::string unavailable = adc_status;
        bool isReachable = false;
        bool isMoving = false, inLimit = false, isSimulated = false;
        if(m_ptrAdcWorkerThread)
        {
            adc_status = m_ptrAdcWorkerThread->GetMode();
            isReachable = true;
        }
        json_alias << "\"" << "ADC_Mode" << "\": { \"name\": \"" << "ADC_Mode" << "\", \"named\": \"" << adc_status << "\", \"encoder\": " << 0 << ", ";
        json_alias << "\"is_moving\": " << isMoving << ", \"in_limit\": " << inLimit << ", \"available\": " << isReachable << ", \"simulated\": " << isSimulated << "}";
        jsonStatus += json_alias.str();
    }
    if(bJsonEmpty)
    {
//        U9_LLOG("jsonStatus : " << jsonStatus);
    }
//    U8_LLOG("");
    return status;
}


void trackingCtrl::setFlags(uint64_t flags, uint64_t more_flags)
{
    if(flags & shins::flags::TT_SAVE_RAW_DIAG_DATA)
    {
//        m_bSaveRawDiagData = true;
//        U9_LLOG("Save raw diag data : ON");
    }
    if(m_ptrAdcWorkerThread)
        m_ptrAdcWorkerThread->setFlags(flags, more_flags);
};
void trackingCtrl::unsetFlags(uint64_t flags, uint64_t more_flags)
{
    if(flags & shins::flags::TT_SAVE_RAW_DIAG_DATA)
    {
//        m_bSaveRawDiagData = false;
//        U9_LLOG("Save raw diag data : OFF");
    }
    if(m_ptrAdcWorkerThread)
        m_ptrAdcWorkerThread->unsetFlags(flags, more_flags);
};
void trackingCtrl::toggleFlags(uint64_t flags, uint64_t more_flags)
{
    if(flags & shins::flags::TT_SAVE_RAW_DIAG_DATA)
    {
//        m_bSaveRawDiagData = !m_bSaveRawDiagData;
//        U9_LLOG("Save raw diag data : " << (m_bSaveRawDiagData ? "ON" : "OFF"));
    }
    if(m_ptrAdcWorkerThread)
        m_ptrAdcWorkerThread->toggleFlags(flags, more_flags);
};
