// $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/src/obs_ctrl/SeqMotion.hpp 3680 2021-11-03 11:19:35Z laud $
/* * seqISetup.cpp ** Created on: Nov 3, 2021 * Author: shinsmgr */

#include <boost/exception/diagnostic_information.hpp>

#include <Nice/Property/BitSet.h>

#include "seqISetup.hpp"


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

std::string & get_position_string(double pos)
{
    static std::string result;
    std::stringstream pos_stream;
    int int_pos = (int)pos;
    int decimals = (int)abs((pos - int_pos)*10000);
    pos_stream << int_pos;
    if(decimals > 0)
    {
        pos_stream << ".";
        pos_stream << std::setfill('0') << std::setw(4) << decimals;
    }
    result = pos_stream.str();
    return result;
}

    SeqSetup::SeqSetup(Ice::PropertiesPtr props,
            boost::shared_ptr<motionCtrl> & pmotion,
             boost::shared_ptr<calunitCtrl> & pcalunit,
             boost::shared_ptr<trackingCtrl> & ptrack,
             boost::shared_ptr<tiptiltCtrl> & ptiptilt,
             boost::shared_ptr<SeqTcs> & ptcs,
             boost::shared_ptr<SeqDataMgr> & pseqDataMgr,
             boost::shared_ptr<SeqSnmp> & pseqSnmp):      m_motion(pmotion),
                                                          m_calibration(pcalunit),
                                                          m_track(ptrack),
                                                          m_tiptilt(ptiptilt),
                                                          m_tcs(ptcs),
                                                          m_seqDataMgr(pseqDataMgr),
                                                          m_seqSnmp(pseqSnmp),
                                                          m_props(props),
                                                          m_calunit_initialized(false),
                                                          m_motion_initialized(false),
                                                          m_bExecutingSetup(false),
                                                          m_bAbortSetup(false)
    {
//        pthread_mutexattr_t mutex_attributes;
//        pthread_mutexattr_init(&mutex_attributes);
//        pthread_mutex_init(&m_SetupMutex, &mutex_attributes);
//        pthread_mutexattr_destroy(&mutex_attributes);

        m_pSeq = this;
//        m_pResetCalibration = NULL;

        std::map<std::string, std::string> lamps;
        std::string lamp_status;
        m_seqSnmp->GetPdu("CAL_FIBER_FOCUS_LAMP", lamp_status);
        lamps["CAL_FIBER_FOCUS_LAMP"] = lamp_status;
        m_seqSnmp->GetPdu("CAL_FF_LAMP", lamp_status);
        lamps["CAL_FF_LAMP"] = lamp_status;
        m_seqSnmp->GetPdu("CAL_FIBER_DEFOCUS_LAMP", lamp_status);
        lamps["CAL_FIBER_DEFOCUS_LAMP"] = lamp_status;
        m_seqDataMgr->setLampsStatus(lamps);

//        init_motion_control(this);
//        init_calibration_control(this);

        for(int i = 0; i < 3; i++)
        {
            for(int j = 1; j <= 8; j++)
            {
                std::stringstream property;
                std::stringstream J;
                J << j;
                property << "Shins.MoCon" << i + 1 << ".Module" << j;
                std::string property_value = props->getProperty(property.str());
                if(property_value.empty())
                {
                    W_LLOG("Could not find property \"" << property.str() << "\"");
                }
                else
                {
                    m_Motors.push_back(property_value);
                }
            }
        }

        pthread_t tid1;
        pthread_create(&tid1, NULL, connect_to_services, this);

    }

SeqSetup * SeqSetup::m_pSeq = NULL;


void * SeqSetup::connect_to_services(void * arg)
{
    SeqSetup & Me = *((SeqSetup *)arg);
    bool bTrackNotConnected = true, bMotionNotConnected = true, bCalunitNotConnected = true;
    su::EServiceStatus CalunitStatus = su::SERVICE_NOTCONNECTED, MotionStatus = su::SERVICE_NOTCONNECTED, TrackStatus = su::SERVICE_NOTCONNECTED;
    su::EServiceStatus TipTiltStatus = su::SERVICE_NOTCONNECTED, TcsStatus = su::SERVICE_NOTCONNECTED;
    while(MotionStatus != su::SERVICE_ONLINE ||
          CalunitStatus != su::SERVICE_ONLINE ||
          TrackStatus != su::SERVICE_ONLINE ||
          TcsStatus != su::SERVICE_ONLINE ||
          TipTiltStatus != su::SERVICE_ONLINE)
    {
        if(TcsStatus != su::SERVICE_ONLINE)
        {
            TcsStatus = Me.m_tcs->Connect();
            if(TcsStatus == su::SERVICE_ONLINE)
            {
                if(Me.m_track)
                {
                    Me.m_track->UpdateTcsInterface(Me.m_tcs->getIifInterface());
                }
                Me.m_seqDataMgr->UpdateTcsInterface(Me.m_tcs->getIifInterface());
            }
        }
        if(TipTiltStatus != su::SERVICE_ONLINE)
        {
            TipTiltStatus = Me.m_tiptilt->Connect();
            if(TipTiltStatus == su::SERVICE_ONLINE)
            {
                Me.m_tiptilt->default_initialization(Me.m_props);
            }
        }
        if(MotionStatus != su::SERVICE_ONLINE)
        {
            MotionStatus = Me.m_motion->Connect();
            if(MotionStatus == su::SERVICE_ONLINE)
            {
                init_motion_control(&Me);
            }
        }
        if(CalunitStatus != su::SERVICE_ONLINE)
        {
            CalunitStatus = Me.m_calibration->Connect();
            if(CalunitStatus == su::SERVICE_ONLINE)
            {
                init_calibration_control(&Me);
            }
        }
        if(TrackStatus != su::SERVICE_ONLINE)
        {
            TrackStatus = Me.m_track->Connect();
            if(TrackStatus == su::SERVICE_ONLINE)
            {
                if(Me.m_tcs)
                {
                    Me.m_track->UpdateTcsInterface(Me.m_tcs->getIifInterface());
                }
                U1_LLOG("Connection established with service track_ctrl-svr");
            }
        }
        if(MotionStatus != su::SERVICE_ONLINE || CalunitStatus != su::SERVICE_ONLINE|| TrackStatus != su::SERVICE_ONLINE || 
           TipTiltStatus != su::SERVICE_ONLINE || TcsStatus != su::SERVICE_ONLINE)
        {
            usleep(120000000);
        }
    }
    N_LLOG("Connection to Services completed");
    Me.m_seqDataMgr->updateInstrumentHeader();
    return NULL;
}


void * SeqSetup::init_motion_control(void * arg)
{
    usleep(1000000);
    U6_LLOG(__FUNCTION__);

    SeqSetup & me = *((SeqSetup *)arg);

    me.m_motion->SetWheelInterdictionCallbacks(suc::CORO_SLIT_W, SeqSetup::s_coro_interdiction, SeqSetup::put_cal_fiber_in_safe_area2);

    me.m_motion->SetSecondaryCallbacks(suc::INBEAM_DEP, MotionRestartCatcher, MotionRestartCatcher, MotionRestartCatcher, MotionRestartCatcher, MotionRestartCatcher);
    me.m_motion->SetSecondaryCallbacks(suc::SHUTTER, MotionRestartCatcher, MotionRestartCatcher, MotionRestartCatcher, MotionRestartCatcher, MotionRestartCatcher);
    me.m_motion->SetSecondaryCallbacks(suc::PUPIL_LENS_DEP, MotionRestartCatcher, MotionRestartCatcher, MotionRestartCatcher, MotionRestartCatcher, MotionRestartCatcher);

    me.m_motion->SetSecondaryCallbacks(suc::CORO_SLIT_W, coro_callback, coro_callback, coro_callback, coro_callback, coro_callback);
    //    me.m_motion->SetSecondaryCallbacks(suc::coroSlit, me.m_motion->GetMultiWheel


//    std::vector<std::string> inbeam_tt;
//    inbeam_tt.push_back(suc::INBEAM_TT);
    try
    {
//        me.m_motion->moveDeviceHome(inbeam_tt);
        me.m_motion_initialized = true;
    }
    catch(...)
    {
        E_LLOG("Could not move INBEAM_TT to HOME");
        SeqISetupLogBoostDiagnosticInformation(__LINE__);
    }


    //    me.m_motion->moveToReferenceAll();
//    me.m_motion_initialized = true;
    U8_LLOG(__FUNCTION__ << " ~ m_motion_initialized = " << (me.m_motion_initialized ? "TRUE" : "FALSE"));
    return NULL;
}

void * SeqSetup::at_coro_wheel_exec(void * arg)
{
    U6_LLOG(__FUNCTION__);
    SeqSetup & me = *((SeqSetup *)arg);

    const std::map<std::string, WheelDevice> & multi_wheel = me.m_motion->GetMultiWheel();
    if(multi_wheel.find(suc::CORO_SLIT_W) != multi_wheel.end())
    {
        const WheelDevice & coroSlitWheel = multi_wheel.find(suc::CORO_SLIT_W)->second;
        bool ContinueLoop = true;
        while(ContinueLoop && coroSlitWheel.GetMoving())
        {
            if(coroSlitWheel.GetMoving() < 0)
            {
                E_LLOG(__FUNCTION__ << " ~ Error: Internal counter out of range");
                usleep(100000);
            }
            if(SeqSetup::s_coro_interdiction())
            {
                try
                {
                    A_LLOG(__FUNCTION__ << " ~ " << "Attempted moving CORO Wheel when CAL_FIBER_DEP was in dangerous area. Interdicted");
                    std::vector<std::string> vecAlias;
                    vecAlias.push_back(suc::CORO_SLIT_W);
                    me.m_motion->abort(vecAlias);
                    ContinueLoop = false;
                }
                catch(...)
                {
                    SeqISetupLogBoostDiagnosticInformation(__LINE__);
                }
                try
                {
                    W_LLOG("Interdiction Condition Detected. Aborting CORO Wheel Movement and putting FIBER deployer in safe!");
                    SeqSetup::put_cal_fiber_in_safe_area2();
                }
                catch (Basda::ServiceIsBusyException &e)
                {
                    E_LLOG(e.what());
                    W_LLOG("Could not put FIBER deployer in safe position");
                }
                catch (Basda::ServiceIsNotOnlineException &e)
                {
                    E_LLOG(e.what());
                }
                catch(...)
                {
                    SeqISetupLogBoostDiagnosticInformation(__LINE__);
                }
            }
            usleep(20000);
        }
    }
    U8_LLOG(__FUNCTION__ << " ~ Completed");
    return NULL;
}


void SeqSetup::coro_callback(const Nice::Completion & _c)
{
    pthread_t tid1;
    switch (_c.state())
    {
    case Nice::OK:
        break;
    case Nice::EXEC:
        {
            const std::map<std::string, WheelDevice> & multi_wheel = SeqSetup::m_pSeq->m_motion->GetMultiWheel();
            if(multi_wheel.find(suc::CORO_SLIT_W) != multi_wheel.end())
            {
                const WheelDevice & coroSlitWheel = multi_wheel.find(suc::CORO_SLIT_W)->second;
                // coroSlitWheel.GetMoving()) == 1 -> CheckState already called: first EXEC
                if(coroSlitWheel.GetMoving() == 1)
                {
                    pthread_create(&tid1, NULL, at_coro_wheel_exec, SeqSetup::m_pSeq);
                }
            }
        }
        break;
    default:
        break;
    }
    U6_LLOG(__FUNCTION__ << " ~ CORO_SLIT_W ~ " << _c.state());
    U8_LLOG("");
}


void * SeqSetup::init_calibration_control(void * arg)
{
//    usleep(1000000);
    U6_LLOG(__FUNCTION__);

    SeqSetup & me = *((SeqSetup *)arg);

    me.m_calibration->SetSecondaryCallbacks(suc::CAL_FIBER_DEP, CalunitRestartCatcher, CalunitRestartCatcher, CalunitRestartCatcher, CalunitRestartCatcher, CalunitRestartCatcher);
    me.m_calibration->SetSecondaryCallbacks(suc::CAL_MIRROR_DEP, CalunitRestartCatcher, CalunitRestartCatcher, CalunitRestartCatcher, CalunitRestartCatcher, CalunitRestartCatcher);
    me.m_calibration->SetSecondaryCallbacks(suc::CAL_FILTER_DEP, CalunitRestartCatcher, CalunitRestartCatcher, CalunitRestartCatcher, CalunitRestartCatcher, CalunitRestartCatcher);

    std::vector<std::string> cal_fiber;
    cal_fiber.push_back(suc::CAL_FIBER_DEP);
    try
    {
        me.m_calibration->moveToReference(cal_fiber);
        me.m_calunit_initialized = true;
    }
    catch(...)
    {
        E_LLOG("Could not move CAL_FIBER_DEPT to HOME");
        SeqISetupLogBoostDiagnosticInformation(__LINE__);
    }
    U8_LLOG(__FUNCTION__ << " ~ m_calunit_initialized = " << me.m_calunit_initialized);
    return NULL;
}


void * SeqSetup::test_exec(void * arg)
{
#ifdef UNDEFIND
    const std::string & coroSlit = suc::CORO_SLIT_W;
    multiWheel & me = *((multiWheel *)arg);
    Basda::MoccaWheelServiceWorkerInterfacePtr interfacePtr;
    WheelDevice & coro = me.m_MapWheels.find(coroSlit)->second;
    if(coro.CheckInterdiction())
    {
        try
        {
            A_LLOG(__FUNCTION__ << " ~ " << "Attempted moving CORO Wheel when CAL_FIBER_DEP was in dangerous area. Interdicted");
            coro.iPtr->abort();
        }
        catch (...)
        {
            SeqISetupLogBoostDiagnosticInformation(__LINE__);
            throw;
        }
                    try
                    {
                        if(coro.iPtr->isMoving())
                        {
                            U9_LLOG("Ancor si muove a velocità : " << coro.iPtr->getVelocity());
                        }
                        else
                        {
                            U9_LLOG("Non si muove più");
                        }
                    }
                    catch (...)
                    {
                        U9_LLOG("Cought");
                    }
        try
        {
            coro.InterdictionReaction();
            U9_LLOG("Interdiction Condition Detected. Aborting CORO Wheel Movement and putting FIBER deployer in safe!");
        }
        catch (Basda::ServiceIsBusyException &e)
        {
            E_LLOG(e.what());
            U9_LLOG("Could not put FIBER deployer in safe position");
        }
        catch (Basda::ServiceIsNotOnlineException &e)
        {
            E_LLOG(e.what());
            throw e;
        }
                        try
                        {
                            coro.iPtr->dumpStatusInformation();
                            Nice::Completion c = coro.iPtr->config()->getAtNode<Nice::Completion>("CMD.MOVETOHOME.EXEC");
                            U9_LLOG("Home Completion for " << coroSlit << " is : " << c.state());
                            c = interfacePtr->moveToHomeCompletion();
                            U9_LLOG("Home Completion for " << coroSlit << " is : " << c.state());
                        }
                        catch (...)
                        {
                            W_LLOG("Unknown Error");
                        }

    }
#endif
    return NULL;
}

void SeqSetup::CalunitRestartCatcher(const Nice::Completion & c)
{
    static int call_counter = 0;
    static pthread_t tid1;
    static time_t last_call;

    return;

    if(c.state() == Nice::OK)
    {
        time_t current_call;
        time(&current_call);
        if(current_call - last_call > 1)call_counter = 0;
        call_counter++;
        if(call_counter == 5)
        {
            SeqSetup::m_pSeq->m_calunit_initialized = false;
            U6_LLOG(__FUNCTION__ << " calunitCtrl Restart Caught");
            usleep(10000000);
            pthread_create(&tid1, NULL, init_calibration_control, SeqSetup::m_pSeq);
            call_counter = 0;
            U8_LLOG("");
        }
        last_call = current_call;
    }
    else
    {
        call_counter = 0;
    }
}


void SeqSetup::MotionRestartCatcher(const Nice::Completion & c)
{
    static int call_counter = 0;
    static pthread_t tid1;
    static time_t last_call;

    return;

    if(c.state() == Nice::OK)
    {
        time_t current_call;
        time(&current_call);
        int old_call = call_counter;
        if(current_call - last_call > 1)call_counter = 0;
        call_counter++;
        U7_LLOG(old_call << " ~ " << call_counter);
        if(call_counter == 5)
        {
            SeqSetup::m_pSeq->m_motion_initialized = false;
            U6_LLOG(__FUNCTION__ << " motionCtrl Restart Caught");
            pthread_create(&tid1, NULL, init_motion_control, SeqSetup::m_pSeq);
            call_counter = 0;
            U8_LLOG("");
        }
        last_call = current_call;
    }
    else
    {
        call_counter = 0;
    }
}


inline bool SeqSetup::cal_fiber_dangerous_area_test(double steps)
{
    return steps < CAL_FIBER_DEP_CORO_WHEEL_INTERDICTION_STEPS;
}

bool SeqSetup::put_cal_fiber_in_safe_area2(void)
{
    U6_LLOG(__FUNCTION__);
//    vecSetupNamedAbsPos  result;
    bool result = false;
    if(SeqSetup::m_pSeq)
    {
        if(SeqSetup::m_pSeq->m_calibration)
        {
            vecSetupNamedAbsPos safe_cal_pos;
            Ice::Double calPosition = -3000000;
            try
            {
                calPosition = m_pSeq->get_cal_fiber_pos_steps();
            }
            catch(...)
            {
                W_LLOG("Could not get CAL_FIBER_DEP position");
            }
            safe_cal_pos.push_back(std::tr1::make_tuple(suc::CAL_FIBER_DEP, "", CAL_FIBER_DEP_CORO_WHEEL_INTERDICTION_STEPS, suc::steps));
            try
            {
                if(cal_fiber_dangerous_area_test(calPosition))
                {
                    m_pSeq->move_cal_fiber_dep(safe_cal_pos);
                    result = true;
                }
                else
                {
                    result = true;
                }
            }
            catch(Basda::ServiceIsBusyException ex)
            {
                W_LLOG("CAL_FIBER_DEP Service is busy");
            }
            catch(...)
            {
                SeqISetupLogBoostDiagnosticInformation(__LINE__);
                U8_LLOG("");
                throw;
            }

        }
    }
    U8_LLOG(__FUNCTION__);
    return result;
}


vecSetupNamedAbsPos SeqSetup::put_cal_fiber_in_safe_area()
{
    U6_LLOG(__FUNCTION__);
    vecSetupNamedAbsPos  result;
    if(SeqSetup::m_pSeq)
    {
        if(SeqSetup::m_pSeq->m_calibration)
        {
            vecSetupNamedAbsPos safe_cal_pos;
            if(s_coro_interdiction())
            {
                Ice::Double calPosition = m_pSeq->get_cal_fiber_pos_steps();
                result.push_back(std::tr1::make_tuple(suc::CAL_FIBER_DEP, "", calPosition, suc::steps));
                safe_cal_pos.push_back(std::tr1::make_tuple(suc::CAL_FIBER_DEP, "", CAL_FIBER_DEP_CORO_WHEEL_INTERDICTION_STEPS, suc::steps));
                try
                {
                    m_pSeq->move_cal_fiber_dep(safe_cal_pos);
//                m_pSeq->m_calibration->moveCalFiberDepToSafe();
                }
                catch(...)
                {
                    SeqISetupLogBoostDiagnosticInformation(__LINE__);
                    U8_LLOG("");
                    throw;
                }
            }
        }
        else
        {
            U9_LLOG("Error putting CAL_FIBER_DEP in safe area 2");
            U8_LLOG("");
        }
    }
    else
    {
        U9_LLOG("Error putting CAL_FIBER_DEP in safe area 1");
        U8_LLOG("");
    }
    return result;
}


Ice::Double SeqSetup::get_cal_fiber_pos_steps()
{
    std::map<std::string, Ice::Double> mapPositions;
    mapPositions[suc::CAL_FIBER_DEP] = -1234567890;
    std::string unit = suc::steps;
    if(m_calibration)
    {
//        calunitCtrl & cal = **m_ppCalibration;
        try
        {
            m_calibration->getPosition(mapPositions, unit);
        }
        catch(...)
        {
            SeqISetupLogBoostDiagnosticInformation(__LINE__);
            throw;
        }
    }
    return mapPositions[suc::CAL_FIBER_DEP];
}

bool SeqSetup::s_coro_interdiction()
{
//   U6_LLOG(__FUNCTION__);
    Ice::Double calPositions = -3000000.;
    if(SeqSetup::m_pSeq)
    {
        if(SeqSetup::m_pSeq->m_calibration)
        {
            try
            {
                calPositions = SeqSetup::m_pSeq->get_cal_fiber_pos_steps();
            }
            catch(std::runtime_error & e)
            {
                E_LLOG(e.what());
                return true;
            }
            catch(...)
            {
                E_LLOG(__FUNCTION__);
                return true;
            }
            if(cal_fiber_dangerous_area_test(calPositions))
            {
                W_LLOG("Interdiction 1 : pos = " << ((int)calPositions));
//               U8_LLOG("");
                return true;
            }
            else
            {
//               U8_LLOG(__FUNCTION__ << " ~ Free ~ pos = " << (int)calPositions);
                return false;
            }
        }
        else
        {
            E_LLOG("Interdiction 2");
//           U8_LLOG("");
            return true;
        }
    }
    else
    {
        E_LLOG("Interdiction 3");
//       U8_LLOG("");
        return true;
    }
}


Ice::Int SeqSetup::move_cal_fiber_dep(vecSetupNamedAbsPos movement)
{
    U6_LLOG(__FUNCTION__);
    if(movement.empty())
    {
        U9_LLOG(__FUNCTION__ << " ~ Empty Argument");
    }
    else if(std::tr1::get<0>(*movement.begin()) == suc::CAL_FIBER_DEP)
    {
//        calunitCtrl & cal = **m_ppCalibration;
        try
        {
            // if named position is empty
            if(std::tr1::get<1>(*movement.begin()).empty())
            {
                U9_LLOG("Moving CAL_FIBER_DEP to absolute position " << (int)std::tr1::get<2>(*movement.begin()));
                dictSetupPos setupSafe;
                setupSafe[suc::CAL_FIBER_DEP] = std::tr1::get<2>(*movement.begin());
                dictDevUnit unit;
                unit[suc::CAL_FIBER_DEP] = suc::steps;
                if(m_calunit_initialized)
                {
                    m_calibration->moveToAbsolutePositionStart(setupSafe, unit);
                    m_calibration->moveToAbsolutePositionWait(setupSafe, Nice::Time::seconds(90));
                }
                else
                {
                    m_calibration->moveCalFiberDepToSafe();
                }
            }
            else
            {
                U9_LLOG("Moving CAL_FIBER_DEP to Named position " << std::tr1::get<1>(*movement.begin()));
                dictSetupNamedPos setupSafe;
                setupSafe[suc::CAL_FIBER_DEP] = std::tr1::get<1>(*movement.begin());
                if(m_calunit_initialized)
                {
                    m_calibration->moveToNamedPositionStart(setupSafe);
                    m_calibration->moveToNamedPositionWait(setupSafe, Nice::Time::seconds(90));
                }
                else
                {
                    m_calibration->moveCalFiberDepToSafe();
                }
            }
        }
        catch(...)
        {
            SeqISetupLogBoostDiagnosticInformation(__LINE__);
            U8_LLOG("");
            throw;
        }

    }
    else
    {
        std::stringstream log_message;
        log_message << "Error Invalid Argument " << std::tr1::get<0>(*movement.begin());
        E_LLOG(log_message.str());
        U8_LLOG("");
        throw (std::invalid_argument(log_message.str()));
    }

    U8_LLOG("");
    return EXIT_SUCCESS;
}


//Ice::Int SeqSetup::setupInstrument(const dictSetupNamedPos &_dSetupNamedPos, const Ice::Current &_c)
//{
//    U6_LLOG(__FUNCTION__);
//    int status = EXIT_SUCCESS;
//    pthread_t tid1;
//    if(m_bExecutingSetup == false)
//    {
//        m_bExecutingSetup = true;
//        m_executingSetup = _dSetupNamedPos;
//        pthread_create(&tid1, NULL, setup_instrument, SeqSetup::m_pSeq);
//    }
//    else
//    {
//        std::stringstream log_message;
//        log_message << "Error : Instrument is busy.";
//        E_LLOG(log_message.str());
//        U8_LLOG("");
//        throw (std::invalid_argument(log_message.str()));
//    }
//    U8_LLOG("");
//    return status;
//}
//
//
//void * SeqSetup::setup_instrument(void * arg)
//{
//   U6_LLOG(__FUNCTION__);
//   SeqSetup & me = *((SeqSetup *)arg);
//   me.setupInstrument(me.m_executingSetup);
//   pthread_mutex_lock(&me.m_SetupMutex);
//       me.m_bExecutingSetup = false;
//       me.m_bAbortSetup = false;
//   pthread_mutex_unlock(&me.m_SetupMutex);
//   U8_LLOG(__FUNCTION__ << " ~  Completed");
//   return NULL;
//}

Ice::Int SeqSetup::abortSetup()
{
//    pthread_mutex_lock(&m_SetupMutex);
    m_SetupUpdateMutex.lock();
        if(m_bExecutingSetup)
            m_bAbortSetup = true;
    m_SetupUpdateMutex.unlock();
//    pthread_mutex_unlock(&m_SetupMutex);
    return EXIT_SUCCESS;
}


int SeqSetup::storeSetup(const dictSetupNamedPos& _dSetupNamedPos)
{
    m_seqDataMgr->setLampsStatus(_dSetupNamedPos);

    dictSetupNamedPos::const_iterator itSetup;

    itSetup = _dSetupNamedPos.find(suc::MaskComb);
    if(itSetup != _dSetupNamedPos.end())
    {
        IS.mask_comb = itSetup->second;
    }
    else
    {
        IS.mask_comb = "NONE";
    }

    itSetup = _dSetupNamedPos.find(suc::InstrumentMode);
    if(itSetup != _dSetupNamedPos.end())
    {
        IS.instrument_mode = itSetup->second;
    }

    itSetup = _dSetupNamedPos.find(suc::ADC_Mode);
    if(itSetup != _dSetupNamedPos.end())
    {
        IS.adc_mode = itSetup->second;
    }

    itSetup = _dSetupNamedPos.find(suc::DROT_Mode);
    if(itSetup != _dSetupNamedPos.end())
    {
        IS.drot_mode = itSetup->second;
    }
    return 0;
}


Ice::Int SeqSetup::setupInstrument(const dictSetupNamedPos &_dSetupNamedPos, const Ice::Current &_c)
{
    U6_LLOG(__FUNCTION__);
    m_SetupMutex.lock();
    U9_LLOG(__FUNCTION__ << " ~ Entering");
    m_SetupUpdateMutex.lock();
    m_bExecutingSetup = true;
    m_SetupUpdateMutex.unlock();
    Ice::Int status = EXIT_SUCCESS;
    dictSetupNamedPos SetupNamedPos;
    std::vector<setupStep> result;
//    calunitCtrl & cal = **m_ppCalibration;
    storeSetup(_dSetupNamedPos);

    try
    {
    if(!m_calunit_initialized)
    {
        std::vector<std::string> cal_fiber;
        cal_fiber.push_back(suc::CAL_FIBER_DEP);
        try
        {
            m_calibration->moveToReference(cal_fiber);
            m_calunit_initialized = true;
        }
        catch(...)
        {
            E_LLOG("Error could not initialize CAL_FIBER_DEP");
            SeqISetupLogBoostDiagnosticInformation(__LINE__);
        }
    }
    if(m_motion_initialized)
    {
    }
    if(true) // m_calunit_initialized)
    {
        try
        {
            result = manageCoroFiberClash(_dSetupNamedPos);
        }
        catch(...)
        {
            SeqISetupLogBoostDiagnosticInformation(__LINE__);
            U8_LLOG("");
            throw;
        }
        std::vector<setupStep>::iterator itStep;
        int counter = 0;
        int wait_result = 0;
        for(itStep = result.begin(); itStep < result.end(); itStep++)
        {
            counter++;
            m_SetupUpdateMutex.lock();
            if(m_bAbortSetup)
            {
                U9_LLOG("Aborting setup at step " << counter);
                m_SetupUpdateMutex.unlock();
                break;
            }
            else
            {
                U9_LLOG("Starting setup STEP : " << counter);
            }
            try
            {
                setupInstrumentStart(itStep->action);
            }
            catch(...)
            {
                SeqISetupLogBoostDiagnosticInformation(__LINE__);
                m_SetupUpdateMutex.unlock();
                U8_LLOG("");
                throw;
            }
            m_SetupUpdateMutex.unlock();
            try
            {
                wait_result = setupInstrumentWait(itStep->wait, Nice::Time::seconds(100));
            }
            catch(...)
            {
                SeqISetupLogBoostDiagnosticInformation(__LINE__);
                U8_LLOG("");
                throw;
            }
            if(wait_result == EXIT_FAILURE)
            {
                status = EXIT_FAILURE;
                break;
            }
        }
        SetupNamedPos = _dSetupNamedPos;
        dictSetupPos actualPositions;
        dictSetupNamedPos::const_iterator itSetup;
        {
            if(m_Motors.size() == 24)
            {
                m_Motors.push_back("DROT_PosAng");
                m_Motors.push_back("ADC1_PosAng");
                m_Motors.push_back("ADC2_PosAng");
            }
            for(itSetup = _dSetupNamedPos.begin(); itSetup != _dSetupNamedPos.end(); itSetup++)
            {
                if(std::find(m_Motors.begin(), m_Motors.end(), itSetup->first) != m_Motors.end())
                {
                    actualPositions.insert(std::pair<std::string, double>(itSetup->first, -1234567890.));
                }
                if(std::find(m_seqSnmp->m_Outlets.begin(), m_seqSnmp->m_Outlets.end(), itSetup->first) != m_seqSnmp->m_Outlets.end())
                {
                    std::string outlet_status;
                    m_seqSnmp->GetPdu(itSetup->first, outlet_status);
                    U9_LLOG(__FUNCTION__ << " ~ " << itSetup->first << " status = " << outlet_status);
                }
                else if(itSetup->first == suc::InstrumentMode)
                {
                    U9_LLOG(__FUNCTION__ << " ~ " << itSetup->first << " :  " << IS.instrument_mode);
                }
                else if(itSetup->first == suc::MaskComb)
                {
                    U9_LLOG(__FUNCTION__ << " ~ " << itSetup->first << " :  " << IS.mask_comb);
                }
                else if(itSetup->first == suc::DROT_Mode)
                {
                    U9_LLOG(__FUNCTION__ << " ~ " << itSetup->first << " :  " << m_track->getMode(suc::DROT_Mode));
                }
                else if(itSetup->first == suc::ADC_Mode)
                {
                    U9_LLOG(__FUNCTION__ << " ~ " << itSetup->first << " :  " << m_track->getMode(suc::ADC_Mode));
                }
            }
            m_motion->getPosition(actualPositions, suc::steps);
            m_calibration->getPosition(actualPositions, suc::steps);
            m_track->getPositionDict(actualPositions, suc::degree);
            dictSetupPos::iterator itPos;
            for(itPos = actualPositions.begin(); itPos != actualPositions.end(); itPos++)
            {
                if(itPos->second != -1234567890.)
                {
                    U9_LLOG(__FUNCTION__ << " ~ " << itPos->first << " position = " << get_position_string(itPos->second));
                }
                else
                {
                    U9_LLOG(__FUNCTION__ << " ~ " << itPos->first << " position could not be read");
                }
            }
        }
    }
    else
    {
        std::stringstream log_msg;
        log_msg << "Error CAL_FIBER_DEP not initialized";
        E_LLOG(log_msg.str());
        U8_LLOG("");
        throw std::runtime_error(log_msg.str());
    }
    }
    catch(...)
    {
        m_SetupUpdateMutex.lock();
        m_bExecutingSetup = false;
        m_bAbortSetup = false;
        m_SetupUpdateMutex.unlock();
        m_SetupMutex.unlock();
        U8_LLOG("");
        throw;
    }
    m_SetupUpdateMutex.lock();
    m_bExecutingSetup = false;
    m_bAbortSetup = false;
    m_SetupUpdateMutex.unlock();
    m_SetupMutex.unlock();
    U8_LLOG(__FUNCTION__ << " Returned : " << (status == EXIT_SUCCESS ? "EXIT_SUCCESS" : "EXIT_FAILURE"));
    return status;
}


Ice::Int SeqSetup::setupInstrumentStart(dictSetupNamedPos &_dSetupNamedPos)
{
    U6_LLOG(__FUNCTION__);
    Ice::Int status = EXIT_SUCCESS;
 //   calunitCtrl & cal = **m_ppCalibration;
    U9_LLOG("Setting up Motions Control");
    try
    {
        m_motion->setupStart(_dSetupNamedPos);
    }
    catch (WheelInterdiction &e)
    {
        E_LLOG(e.what());
        U8_LLOG("");
        throw e;
    }
    catch(...)
    {
        SeqISetupLogBoostDiagnosticInformation(__LINE__);
        U8_LLOG("");
        throw;
    }
    if(status == EXIT_SUCCESS)
    {
        U9_LLOG("Setting up Calibration Control");
        try
        {
            int result = m_seqSnmp->Setup(_dSetupNamedPos);
            if(result == EXIT_FAILURE)
            {
                E_LLOG("Error executing snmp_setup" );
            }
            std::string status;
            // m_calibration->setupLamps(_dSetupNamedPos);
        }
        catch(const std::invalid_argument &e)
        {
            W_LLOG("Missing lamp related keywords : " << e.what())
        }
        catch(const std::runtime_error & e)
        {
            W_LLOG(e.what());
        }
        try
        {
//        m_calibration->moveToNamedPositionStart(_dSetupNamedPos);
            m_calibration->moveToNamedPositionStart(_dSetupNamedPos);
        }
        catch(...)
        {
            SeqISetupLogBoostDiagnosticInformation(__LINE__);
            U8_LLOG("");
            throw;
        }
        try
        {
        // m_track->moveToPositionStart(_dSetupNamedPos);
            m_track->setupInstrumentStart(_dSetupNamedPos);
        }
        catch(...)
        {
            SeqISetupLogBoostDiagnosticInformation(__LINE__);
            U8_LLOG("");
            throw;
        }
    }
    U8_LLOG(__FUNCTION__);
    return status;
}


Ice::Int SeqSetup::setupInstrumentWait(const dictSetupNamedPos &_dSetupNamedPos, const IceUtil::Optional<Nice::Time> &_time)
{
    U6_LLOG(__FUNCTION__ <<  " - ENTER");
    Ice::Int status = EXIT_SUCCESS;
//    calunitCtrl & cal = **m_ppCalibration;
    Nice::Time time = Nice::Time::seconds(100);
    if(_time)time = _time.get();
    su::Timer timer(time);
    try
    {
        if(m_motion)
        {
            m_motion->setupWait(_dSetupNamedPos, timer.residual());
            U9_LLOG("Motion wait completed");
        }
    }
    catch(const Basda::ServiceWorkerInterfaceCommandFailedException &e)
    {
        E_LLOG("Basda::ServiceWorkerInterfaceCommandFailedException : " << e.what());
        status = EXIT_FAILURE;
        U8_LLOG("");
        throw e;
    }
    catch(...)
    {
        SeqISetupLogBoostDiagnosticInformation(__LINE__);
        U8_LLOG("");
        throw;
    }
    try
    {
        U9_LLOG("Waiting for Calibration Control");
        m_calibration->moveToNamedPositionWait(_dSetupNamedPos, timer.residual());
    }
    catch (const Basda::ServiceWorkerInterfaceCommandFailedException &e)
    {
        E_LLOG("Basda::ServiceWorkerInterfaceCommandFailedException : " << e.what());
        status = EXIT_FAILURE;
        U8_LLOG("");
        throw e;
    }
    catch(...)
    {
        SeqISetupLogBoostDiagnosticInformation(__LINE__);
        U8_LLOG("");
        throw;
    }
    try
    {
        U9_LLOG("Waiting for Track Control");
        m_track->setupInstrumentWait(_dSetupNamedPos, timer.residual());
    }
    catch (const Basda::ServiceWorkerInterfaceCommandFailedException &e)
    {
        E_LLOG("Basda::ServiceWorkerInterfaceCommandFailedException : " << e.what());
        status = EXIT_FAILURE;
        U8_LLOG("");
        throw e;
    }
    catch(...)
    {
        SeqISetupLogBoostDiagnosticInformation(__LINE__);
        U8_LLOG("");
        throw;
    }

    U8_LLOG(__FUNCTION__ << " - EXIT : " << status << " (" << (status == EXIT_FAILURE ? "EXIT_FAILURE" : "EXIT_SUCCESS") << ")");
    return status;
}


// setups the instrument using positions in steps, stored in a C++ map
// type of(dictSetupPos) is dictionary<string, double>
Ice::Int SeqSetup::setupInstrumentAbsolutePosition(const dictSetupPos &_dSetupPos,
                                                   const IceUtil::Optional<dictDevUnit> &_dUnit,
                                                   const Ice::Current &_c)
{
    U6_LLOG(__FUNCTION__ << "ENTER");
    Ice::Int status = EXIT_SUCCESS;
    dictSetupNamedPos NamedPos;
    dictSetupPos::const_iterator itSetup;
    dictDevUnit units;
    dictDevUnit::iterator itUnit;
    if(_dUnit)units = _dUnit.get();
    for(itSetup = _dSetupPos.begin(); itSetup != _dSetupPos.end(); itSetup++)
    {
        std::string unit = "DEFAULT";
        itUnit = units.find(itSetup->first);
        if(itUnit != units.end())unit = itUnit->second;
        NamedPos.insert(std::pair<std::string, std::string>(itSetup->first, su::NamedStylePos(itSetup->second, unit, su::SHINS_MOV_ABSOLUTE)));
    }
    status = setupInstrument(NamedPos, _c);

    U8_LLOG("");
    return status;
}


Ice::Int SeqSetup::setupInstrumentRelativePosition(const dictSetupPos &_dSetupPos,
                                                   const IceUtil::Optional<dictDevUnit> &_dUnit,
                                                   const Ice::Current &_c)
{
    U6_LLOG(__FUNCTION__ << "ENTER");
    Ice::Int status = EXIT_SUCCESS;
    dictSetupNamedPos NamedPos;
    dictSetupPos::const_iterator itSetup;
    dictDevUnit units;
    dictDevUnit::iterator itUnit;
    if(_dUnit)units = _dUnit.get();
    for(itSetup = _dSetupPos.begin(); itSetup != _dSetupPos.end(); itSetup++)
    {
        std::string unit = "DEFAULT";
        itUnit = units.find(itSetup->first);
        if(itUnit != units.end())unit = itUnit->second;
        NamedPos.insert(std::pair<std::string, std::string>(itSetup->first, su::NamedStylePos(itSetup->second, unit, su::SHINS_MOV_RELATIVE)));
    }
    status = setupInstrument(NamedPos, _c);

    U8_LLOG("");
    return status;
}


std::vector<setupStep> SeqSetup::manageCoroFiberClash(const dictSetupNamedPos &dSetupNamedPos)
{
    U6_LLOG(__FUNCTION__);
    std::vector<setupStep> result;
    setupStep step1, step2, step3;
    vecSetupNamedAbsPos dSetupNamedAbsPos;
    bool CalFiberInvolved = false;
    bool CoroSlitInvolved = false;
    bool CalFiberSetupCommandIsRelative = false;
    std::string CalFiberAbsoluteSetupCommand = "";
    double absolute = 0, relative = 0;
    std::string unit;
    dictSetupNamedPos::const_iterator itFind;
    itFind = dSetupNamedPos.find(suc::CORO_SLIT_W);
    CoroSlitInvolved = itFind != dSetupNamedPos.end();
    std::map<std::string, Ice::Double> fiber_position;
//    calunitCtrl & cal = **m_ppCalibration;
    if(CoroSlitInvolved)
    {
        std::map<std::string, WheelDevice>::const_iterator itCoro = m_motion->GetMultiWheel().find(suc::CORO_SLIT_W);
        std::map<std::string, double> namedPositions;
        if(itCoro != m_motion->GetMultiWheel().end())namedPositions = itCoro->second.namedPositions;
        else
        {
            E_LLOG("Could not find " << suc::CORO_SLIT_W << ". Unrecoverable system change.");
            U8_LLOG("");
            throw(std::out_of_range("Could not find " + suc::CORO_SLIT_W + ". Unrecoverable system change."));
        }
        su::EShinsMovements movement = su::detect_command_type(itFind->second, absolute, relative, unit);
        if(movement == su::SHINS_MOV_NAMED || movement == su::SHINS_MOV_ABSOLUTE || movement == su::SHINS_MOV_RELATIVE)
        {
            if(movement == su::SHINS_MOV_NAMED)
            {
                if(namedPositions.find(itFind->second) != namedPositions.end())
                {
                    dSetupNamedAbsPos.push_back(std::tr1::make_tuple(suc::CORO_SLIT_W, itFind->second, namedPositions[itFind->second], unit));
                }
                else
                {
                    std::stringstream log_message;
                    log_message << "Invalid argument in setup : [" << itFind->first << ", " << itFind->second << "]";
                    E_LLOG(log_message.str());
                    U8_LLOG("");
                    throw std::invalid_argument(log_message.str());
                }
            }
            else if(movement == su::SHINS_MOV_ABSOLUTE)
            {
                dSetupNamedAbsPos.push_back(std::tr1::make_tuple(suc::CORO_SLIT_W, "", absolute, unit));
            }
            else if(movement == su::SHINS_MOV_RELATIVE)
            {
                std::map<std::string, Ice::Double> position;
                position[suc::CORO_SLIT_W] = 0.;
                m_motion->getPosition(position, suc::steps);
                dSetupNamedAbsPos.push_back(std::tr1::make_tuple(suc::CORO_SLIT_W, "", position[suc::CORO_SLIT_W] + relative, unit));
            }
            itFind = dSetupNamedPos.find(suc::CAL_FIBER_DEP);
            CalFiberInvolved = itFind != dSetupNamedPos.end();
            if(CalFiberInvolved)
            {
                std::map<std::string, StageXDevice>::const_iterator itFiber = m_calibration->GetMultiStageX().find(suc::CAL_FIBER_DEP);
                if(itFiber != m_calibration->GetMultiStageX().end())namedPositions = itFiber->second.namedPositions;
                else
                {
                    E_LLOG("Could not find " << suc::CAL_FIBER_DEP << ". Unrecoverable system change.");
                    U8_LLOG("");
                    throw(std::out_of_range("Could not find " + suc::CAL_FIBER_DEP + ". Unrecoverable system change."));
                }
                unit = "";
                movement = su::detect_command_type(itFind->second, absolute, relative, unit);
                if(movement == su::SHINS_MOV_NAMED)
                {
                    if(namedPositions.find(itFind->second) != namedPositions.end())
                    {
                        dSetupNamedAbsPos.push_back(std::tr1::make_tuple(suc::CAL_FIBER_DEP, itFind->second, namedPositions[itFind->second], unit));
                    }
                    else
                    {
                        std::stringstream log_message;
                        log_message << "Invalid argument in setup : [" << itFind->first << ", " << itFind->second << "]";
                        E_LLOG(log_message.str());
                        U8_LLOG("");
                        throw std::invalid_argument(log_message.str());
                    }
                }
                else if(movement == su::SHINS_MOV_ABSOLUTE)
                {
                    dSetupNamedAbsPos.push_back(std::tr1::make_tuple(suc::CAL_FIBER_DEP, "", absolute, unit));
                }
                else if(movement == su::SHINS_MOV_RELATIVE)
                {
                    CalFiberSetupCommandIsRelative = true;
                    fiber_position[suc::CAL_FIBER_DEP] = -1234567890.;
                    m_calibration->getPosition(fiber_position, unit);
                    A_LLOG("CAL_FIBER_DEP position is : " << fiber_position[suc::CAL_FIBER_DEP] << " " << unit);
                    CalFiberAbsoluteSetupCommand = su::NamedStylePos(fiber_position[suc::CAL_FIBER_DEP] + relative, unit, su::SHINS_MOV_ABSOLUTE);
                    dSetupNamedAbsPos.push_back(std::tr1::make_tuple(suc::CAL_FIBER_DEP, "", fiber_position[suc::CAL_FIBER_DEP] + relative, unit));
                }
                else
                {
                    std::stringstream log_message;
                    log_message << "Invalid argument in setup : [" << itFind->first << ", " << itFind->second << "]";
                    E_LLOG(log_message.str());
                    U8_LLOG("");
                    throw std::invalid_argument(log_message.str());
                }
            }
            else
            {
            }
        }
        else
        {
            std::stringstream log_message;
            log_message << "Invalid argument in setup : [" << itFind->first << ", " << itFind->second << "]";
            E_LLOG(log_message.str());
            U8_LLOG("");
            throw std::invalid_argument(log_message.str());
        }
    }
    EClashCases clash = check_coro_fiber_clash_3(dSetupNamedAbsPos);
    switch(clash)
    {
    case CLASH_CASE_1:// Coro Wheel not involved in setup
    case CLASH_CASE_2:// Coro Wheel is not changing position during setup, Calibration fiber in safe position
    case CLASH_CASE_4:// Calibration Fiber is in safe position, and is not going to change position
    case CLASH_CASE_5:// Calibration Fiber is in safe position, and is moving to safe position
        step1.action = dSetupNamedPos;
        step1.wait = dSetupNamedPos;
        result.push_back(step1);
        break;
    case CLASH_CASE_3:// Coro Wheel is not changing position during setup, Calibration fiber in dangerous position
    {
        step1.action = dSetupNamedPos;
        U9_LLOG("Removing CORO_SLIT_W command to avoid interdiction when attempting null movement");
        step1.action.erase(step1.action.find(suc::CORO_SLIT_W));
        step1.wait = step1.action;
        result.push_back(step1);
    }
        break;
    case CLASH_CASE_6:
        // Calibration Fiber is in safe position moving to dangerous position. Its movement will be split
        // Two phases: fiber to safe limit and coro in place, fiber to place; TEST 5
        {
            step1.action = dSetupNamedPos;
            step1.action[suc::CAL_FIBER_DEP] = su::NamedStylePos(CAL_FIBER_DEP_CORO_WHEEL_INTERDICTION_STEPS, suc::steps, su::SHINS_MOV_ABSOLUTE);
            step1.wait.insert(*dSetupNamedPos.find(suc::CAL_FIBER_DEP));
            step1.wait.insert(*dSetupNamedPos.find(suc::CORO_SLIT_W));
            result.push_back(step1);

            if(CalFiberSetupCommandIsRelative)
                step2.action.insert(std::pair<std::string, std::string>(suc::CAL_FIBER_DEP, CalFiberAbsoluteSetupCommand));
            else
                step2.action.insert(*dSetupNamedPos.find(suc::CAL_FIBER_DEP));
            step2.wait = dSetupNamedPos;
            result.push_back(step2);
        }
        break;
    case CLASH_CASE_7:
        // Calibration Fiber is in dangerous position. Will be put in safe position and then its position restored
        // Three phases: fiber in safe, coro in place, fiber in place; TEST 6
        {
            step2.action.insert(*dSetupNamedPos.find(suc::CORO_SLIT_W));
            step1.action = dSetupNamedPos;
            step1.action.erase(step1.action.find(suc::CORO_SLIT_W));
            step1.action[suc::CAL_FIBER_DEP] = su::NamedStylePos(CAL_FIBER_DEP_CORO_WHEEL_INTERDICTION_STEPS, suc::steps, su::SHINS_MOV_ABSOLUTE);
            step1.wait.insert(*step1.action.find(suc::CAL_FIBER_DEP));
            result.push_back(step1);

            step2.wait = step2.action;
            result.push_back(step2);

            std::string namedStyle = su::NamedStylePos(get_cal_fiber_pos_steps(), suc::steps, su::SHINS_MOV_ABSOLUTE);;
            step3.action.insert(std::pair<std::string, std::string>(suc::CAL_FIBER_DEP, namedStyle));
            step3.wait = dSetupNamedPos;
            step3.wait.insert(std::pair<std::string, std::string>(suc::CAL_FIBER_DEP, namedStyle));
            step3.wait.erase(step3.wait.find(suc::CORO_SLIT_W));
            result.push_back(step3);
        }
        break;
    case CLASH_CASE_8:
        // Calibration Fiber is in dangerous position, moving toward safe position. Its movement will be split.");
        // Two phases: fiber in safe, coro and fiber in place; TEST 7
        {
            step2.action.insert(*dSetupNamedPos.find(suc::CORO_SLIT_W));
            step2.action.insert(*dSetupNamedPos.find(suc::CAL_FIBER_DEP));
            step1.action = dSetupNamedPos;
            step1.action.erase(step1.action.find(suc::CORO_SLIT_W));
            step1.action[suc::CAL_FIBER_DEP] = su::NamedStylePos(CAL_FIBER_DEP_CORO_WHEEL_INTERDICTION_STEPS, suc::steps, su::SHINS_MOV_ABSOLUTE);
            step1.wait.insert(*dSetupNamedPos.find(suc::CAL_FIBER_DEP));
            result.push_back(step1);

            step2.wait = dSetupNamedPos;
            if(CalFiberSetupCommandIsRelative)
                step2.action[suc::CAL_FIBER_DEP] = CalFiberAbsoluteSetupCommand;
            result.push_back(step2);
        }
        break;
    case CLASH_CASE_9:
        // Calibration Fiber is in dangerous position moving to dangerous position. Will be put in safe position and its positioning will be postponed.");
        // Three phases: fiber in safe, coro in place, fiber in place; TEST 8
        {
            step2.action.insert(*dSetupNamedPos.find(suc::CORO_SLIT_W));
            step1.action = dSetupNamedPos;
            step1.action.erase(step1.action.find(suc::CORO_SLIT_W));
            step1.action[suc::CAL_FIBER_DEP] = su::NamedStylePos(CAL_FIBER_DEP_CORO_WHEEL_INTERDICTION_STEPS, suc::steps, su::SHINS_MOV_ABSOLUTE);
            step1.wait.insert(*step1.action.find(suc::CAL_FIBER_DEP));
            result.push_back(step1);

            step2.wait = step2.action;
            result.push_back(step2);

            if(CalFiberSetupCommandIsRelative)
                step3.action.insert(std::pair<std::string, std::string>(suc::CAL_FIBER_DEP, CalFiberAbsoluteSetupCommand));
            else
                step3.action.insert(*dSetupNamedPos.find(suc::CAL_FIBER_DEP));
            step3.wait = dSetupNamedPos;
            step3.wait.erase(step3.wait.find(suc::CORO_SLIT_W));
            result.push_back(step3);
        }
        break;
    default:
    {
        std::stringstream log_message;
        log_message << "Unknown error";
        E_LLOG(log_message.str());
        U8_LLOG("");
        throw std::invalid_argument(log_message.str());
    }
        break;
    };
    U8_LLOG("");
    return result;
}


EClashCases SeqSetup::check_coro_fiber_clash_3(vecSetupNamedAbsPos & dSetupNamedAbsPos)
{
    U6_LLOG(__FUNCTION__);
    std::map<std::string, std::string> mapNamedPos;
    vecSetupNamedAbsPos residual;
//    calunitCtrl & cal = **m_ppCalibration;

    EClashCases result = CLASH_CASE_0;

    vecSetupNamedAbsPos::iterator itFind = dSetupNamedAbsPos.begin();
    while(itFind != dSetupNamedAbsPos.end() && std::tr1::get<0>(*itFind) != suc::CORO_SLIT_W){ itFind++;}
    bool IsCoroInvolved = itFind != dSetupNamedAbsPos.end();

    if(IsCoroInvolved)
    {
        std::map<std::string, Ice::Double> coromap; // (std::initializer_list<std::pair<std::string, std::string> >(suc::CORO_SLIT_W, 0) >);
        coromap[suc::CORO_SLIT_W] = 0.;
        m_motion->getPosition(coromap, suc::steps);
        bool IsCoroGoingToMove = coromap[suc::CORO_SLIT_W] != std::tr1::get<2>(*itFind);
        bool IsCalFiberInDangerousPosition = s_coro_interdiction();
        if(IsCoroGoingToMove)
        {
            itFind = dSetupNamedAbsPos.begin();
            while(itFind != dSetupNamedAbsPos.end() && std::tr1::get<0>(*itFind) != suc::CAL_FIBER_DEP){ itFind++;}
            bool IsCalFiberMovingToDangerousPosition = false;
            bool IsCalFiberInvolved = itFind != dSetupNamedAbsPos.end();
            double destination;
            if(IsCalFiberInvolved)
            {
                destination = std::tr1::get<2>(*itFind);
                double final_destination = m_calibration->ConvertToDefaultUnit(suc::CAL_FIBER_DEP, destination, std::tr1::get<3>(*itFind));
                IsCalFiberMovingToDangerousPosition = cal_fiber_dangerous_area_test(final_destination);
            }
            if(IsCalFiberInDangerousPosition)
            {
                if(IsCalFiberInvolved)
                {
                    if(IsCalFiberMovingToDangerousPosition)
                    {
                        W_LLOG(__FUNCTION__ << " ~ CLASH_CASE_9 ~ Calibration Fiber is in dangerous position moving to dangerous position. Will be put in safe position and its positioning will be postponed.");
                        result = CLASH_CASE_9;
                        // Three phases: fiber in safe, coro in place, fiber in place; TEST 8
                    }
                    else
                    {
                        W_LLOG(__FUNCTION__ << " ~ CLASH_CASE_8 ~ Calibration Fiber is in dangerous position, moving toward safe position. Its movement will be split.");
                        result = CLASH_CASE_8;
                        // Two phases: fiber in safe, coro and fiber in place; TEST 7
                    }
                }
                else
                {
                    W_LLOG(__FUNCTION__ << " ~ CLASH_CASE_7 ~ Calibration Fiber is in dangerous position. Will be put in safe position and then its position restored.");
                    result = CLASH_CASE_7;
                    // Three phases: fiber in safe, coro in place, fiber in place; TEST 6
                }
            }
            else
            {
                if(IsCalFiberInvolved)
                {
                    if(IsCalFiberMovingToDangerousPosition)
                    {
                        W_LLOG(__FUNCTION__ << " ~ CLASH_CASE_6 ~ Calibration Fiber is in safe position moving to dangerous position. Its movement will be split!");
                        result = CLASH_CASE_6;
                        // Two phases: fiber to safe limit and coro in place, fiber to place; TEST 5
                    }
                    else
                    {
                        U9_LLOG(__FUNCTION__ << " ~ CLASH_CASE_5 ~ Calibration Fiber is in safe position, and is moving to safe position");
                        result = CLASH_CASE_5;
                        // One Phase; TEST 4
                    }
                }
                else
                {
                    U9_LLOG(__FUNCTION__ << " ~ CLASH_CASE_4 ~ Calibration Fiber is in safe position, and is not going to change position");
                    result = CLASH_CASE_4;
                    // One phase Test 3
                }
            }
        }
        else
        {
            if(IsCalFiberInDangerousPosition)
            {
                U9_LLOG(__FUNCTION__ << " ~ CLASH_CASE_3 ~ Coro Wheel is not changing position during setup, Calibration Fiber in dangerous Position");
                result = CLASH_CASE_3;
                // One phase; TEST 3
            }
            else
            {
                U9_LLOG(__FUNCTION__ << " ~ CLASH_CASE_2 ~ Coro Wheel is not changing position during setup, Calibration Fiber in safe Position");
                result = CLASH_CASE_2;
                // One phase; TEST 2
            }
        }
    }
    else
    {
        U9_LLOG(__FUNCTION__ << " ~ CLASH_CASE_1 ~ Coro Wheel not involved in setup");
        result = CLASH_CASE_1;
        // One Phase TEST 1
    }
    U8_LLOG("");
    return result;
}


Ice::Int SeqSetup::moveDevices(const dictSetupNamedPos& _dSetupNamedPos)
{
    U6_LLOG(__FUNCTION__);
    int status = EXIT_FAILURE;
    try
    {
        moveDevicesStart(_dSetupNamedPos);
        moveDevicesWait();
    }
    catch(...)
    {
        SeqISetupLogBoostDiagnosticInformation(__LINE__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
    return status;
}


Ice::Int SeqSetup::moveDevicesStart(const dictSetupNamedPos& _dSetupNamedPos)
{
    U6_LLOG(__FUNCTION__);
    int status = EXIT_FAILURE;
    try
    {
        U8_LLOG("");
        throw(std::invalid_argument("Error function under development"));
    }
    catch(...)
    {
        SeqISetupLogBoostDiagnosticInformation(__LINE__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
    return status;
}


Ice::Int SeqSetup::moveDevicesWait()
{
    U6_LLOG(__FUNCTION__);
    int status = EXIT_FAILURE;
    try
    {
        U8_LLOG("");
        throw(std::invalid_argument("Error function under development"));
    }
    catch(...)
    {
        SeqISetupLogBoostDiagnosticInformation(__LINE__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
    return status;
}


Ice::Int SeqSetup::getJsonStatus(std::string & jsonStatus, bool throw_on_failure)
{
    int status = EXIT_SUCCESS;
    m_JsonMutex.lock();
    int status_motion, status_calib, status_track, status_tiptilt, status_pdu;

//    m_seqSnmp->GetPdu(1, "TESTPDU", status);
    try
    {
        jsonStatus = "{";
        if(m_motion->CheckOnline(false))
        {
            status_motion = m_motion->getJsonStatus(jsonStatus);
            jsonStatus += ", ";
        }
        if(m_calibration->CheckOnline(false))
        {
            status_calib = m_calibration->getJsonStatus(jsonStatus);
            jsonStatus += ", ";
        }
        if(m_track->CheckOnline(false))
        {

            status_track = m_track->getJsonStatus(jsonStatus);
            jsonStatus += ", ";
        }
        status_pdu = m_seqSnmp->getJsonStatus(jsonStatus);
        jsonStatus += ", ";
        if(m_tiptilt->CheckOnline(false))
        {
            status_tiptilt = m_tiptilt->getJsonStatus(jsonStatus);
            jsonStatus += ", ";
        }
        status_tiptilt = m_seqDataMgr->getJsonStatus(jsonStatus);
        jsonStatus += "}";
    }
    catch(...)
    {
        m_JsonMutex.unlock();
        if(throw_on_failure)
        {
            SeqISetupLogBoostDiagnosticInformation(__LINE__);
            throw;
        }
        else return EXIT_FAILURE;
    }

//    static int counter = 0;
//    if(counter % 60 == 0){ U9_LLOG(jsonStatus);}
//    counter++;
    m_JsonMutex.unlock();
    if(!(status_motion == EXIT_SUCCESS && status_calib == EXIT_SUCCESS && status_track == EXIT_SUCCESS))
        status = EXIT_FAILURE;
    return status;
}

void SeqSetup::getMotorStatus(su::tagElementHeader & el)
{
    // std::string type;
    // std::string nameid;
    // int mechanical_id;
    // float position1;
    // float position2;
    // int encoder1;
    // int encoder2;
    // bool status;
    if(m_pSeq)
    {
///        calunitCtrl & cal = **m_pSeq->m_ppCalibration;
        if(el.nameid == suc::APODIZER_W || el.nameid == suc::CORO_SLIT_W || el.nameid == suc::ND_FILT_W || el.nameid == suc::DB_FILT_W ||
           el.nameid == suc::LYOT_GRISM_W || el.nameid == suc::SCI_FILT_W1 || el.nameid == suc::SCI_FILT_W2)
        {
            std::map<std::string, double> posmap;
            posmap[el.nameid] = 0.;
            m_pSeq->m_motion->getConfigPosition(posmap, "DEGREE");
            el.position1 = posmap[el.nameid];
            m_pSeq->m_motion->getConfigPosition(posmap, "STEPS");
            el.encoder1 = (int)posmap[el.nameid];
            std::map<std::string, WheelDevice>::const_iterator itWheel = m_pSeq->m_motion->GetMultiWheel().find(el.nameid);
            std::stringstream new_type;
            new_type << el.encoder1 << ":" << "UNDEFINED";
            el.type = new_type.str();
            if(itWheel != m_pSeq->m_motion->GetMultiWheel().end())
            {
                const std::map<std::string, double> & namedPositions = itWheel->second.namedPositions;
                std::map<std::string, double>::const_iterator itNamedPos;
                for(itNamedPos = namedPositions.begin(); itNamedPos != namedPositions.end(); itNamedPos++)
                {
                    if(itNamedPos->second == el.encoder1)
                    {
                        new_type.str("");
                        new_type << el.encoder1 << ":" << itNamedPos->first;
                        el.type = new_type.str(); // itNamedPos->first;
                        break;
                    }
                }
            }
        }
        else if(el.nameid == suc::INBEAM_DEP || el.nameid == suc::PUPIL_LENS_DEP || el.nameid == suc::SHUTTER)
        {
            std::map<std::string, double> posmap;
            posmap[el.nameid] = 0.;
            m_pSeq->m_motion->getConfigPosition(posmap, "STEPS");
            el.encoder1 = (int)posmap[el.nameid];
            std::stringstream new_type;
            new_type << el.encoder1 << ":" << "UNDEFINED";
            el.type = new_type.str();
            std::map<std::string, StageXDevice>::const_iterator itStageX = m_pSeq->m_motion->GetMultiStageX().find(el.nameid);
            if(itStageX != m_pSeq->m_motion->GetMultiStageX().end())
            {
                const std::map<std::string, double> & namedPositions = itStageX->second.namedPositions;
                std::map<std::string, double>::const_iterator itNamedPos;
                for(itNamedPos = namedPositions.begin(); itNamedPos != namedPositions.end(); itNamedPos++)
                {
                    if(itNamedPos->second == el.encoder1)
                    {
                        new_type.str("");
                        new_type << el.encoder1 << ":" << itNamedPos->first;
                        el.type = new_type.str(); // itNamedPos->first;
                        break;
                    }
                }
            }
        }
        else if(el.nameid == suc::INBEAM_TT)
        {
            std::map<std::string, double> positions;
            positions["INBEAM_TT_X"] = 0.;
            positions["INBEAM_TT_Y"] = 0.;
            int result = m_pSeq->m_motion->getConfigPosition(positions, "MM");
            if(result == EXIT_SUCCESS)
            {
                std::map<std::string, StageXYDevice>::const_iterator itStageXY = m_pSeq->m_motion->GetMultiStageXY().find(el.nameid);
                const std::map<std::string, Nice::Point > & namedPositions = itStageXY->second.namedPositions;
                // U9_LLOG("INBEAM_TT named positions size is " << namedPositions.size());
                el.position1 = positions["INBEAM_TT_X"];
                el.position2 = positions["INBEAM_TT_Y"];
                m_pSeq->m_motion->getConfigPosition(positions, "STEPS");
                el.encoder1 = (int)positions["INBEAM_TT_X"];
                el.encoder2 = (int)positions["INBEAM_TT_Y"];
                std::string type = "UNDEFINED";
                std::stringstream new_type;
                new_type << "[" << el.encoder1 << ", " << el.encoder2 << "]"; // <<  ":" << type;
                el.type = new_type.str();
            }
            else
            {
                el.position1 = el.position2 = el.encoder1 = el.encoder2 = 0;
            }
        }
        else if(el.nameid == suc::CAL_FIBER_DEP || el.nameid == suc::CAL_FILTER_DEP || el.nameid == suc::CAL_MIRROR_DEP)
        {
#ifdef UNDEFINED
{
            const std::map<std::string, StageXDevice> & MapStageX = m_pSeq->m_calibration->GetMultiStageX();
            std::map<std::string, StageXDevice>::const_iterator itStageX;
            int counter = 0;
            itStageX = MapStageX.find(el.nameid);
            if(itStageX != MapStageX.end())
            {
                Nice::JointPtr itStageConfig = itStageX->second.iPtr->config();
                U1_LLOG("NodeConfig : " << itStageConfig);
                try{ el.position1 = itStageConfig->leaf("")->get<double>(); }catch(...){};
            }
            return;
}
#endif

            std::map<std::string, double> posmap;
            posmap[el.nameid] = 0.;
            m_pSeq->m_calibration->getConfigPosition(posmap, "MM");
            el.position1 = posmap[el.nameid];
            m_pSeq->m_calibration->getConfigPosition(posmap, "STEPS");
            el.encoder1 = (int)posmap[el.nameid];
            std::stringstream new_type;
            new_type << el.encoder1 << ":" << "UNDEFINED";
            el.type = new_type.str();
            std::map<std::string, StageXDevice>::const_iterator itStageX = m_pSeq->m_calibration->GetMultiStageX().find(el.nameid);
            if(itStageX != m_pSeq->m_motion->GetMultiStageX().end())
            {
                const std::map<std::string, double> & namedPositions = itStageX->second.namedPositions;
                std::map<std::string, double>::const_iterator itNamedPos;
                for(itNamedPos = namedPositions.begin(); itNamedPos != namedPositions.end(); itNamedPos++)
                {
                    if(itNamedPos->second == el.encoder1)
                    {
                        new_type.str("");
                        new_type << el.encoder1 << ":" << itNamedPos->first;
                        el.type = new_type.str(); // itNamedPos->first;
                        break;
                    }
                }
            }
        }
        else if(el.nameid == suc::DROT)
        {
            if(m_pSeq->m_track->GetServiceStatus() == su::SERVICE_ONLINE)
            {
                el.position1 = m_pSeq->m_track->getConfigPosition(el.nameid, "DEGREE");
                el.position2 = 0; // Position at start;
                el.encoder2 = m_pSeq->m_track->getConfigPosition(el.nameid, "ABSENC");
                el.encoder1 = m_pSeq->m_track->getConfigPosition(el.nameid, "STEPS");
                el.type = m_pSeq->m_track->getMode("DROT");
            }
            else
            {
                el.position1 = 0.0;
                el.position2 = 0.0;
                el.encoder1 = 0;
                el.encoder2 = 0;
            }
        }
        else if(su::left(el.nameid, 3) == "ADC")
        {
            if(el.nameid == suc::ADC_Mode)
            {
//                el.type = m_pSeq->IS.adc_mode;
                el.type = m_pSeq->m_track->getAdcMode();
            }
            else
            {
                if(m_pSeq->m_track->GetServiceStatus() == su::SERVICE_ONLINE)
                {
                    el.status = m_pSeq->m_track->getAdcMode() == "ON";
                    el.position1 = m_pSeq->m_track->getConfigPosition(suc::ADC1, "DEGREE");
                    el.encoder1 = m_pSeq->m_track->getConfigPosition(suc::ADC1, "STEPS");
                    el.position2 = m_pSeq->m_track->getConfigPosition(suc::ADC2, "DEGREE");
                    el.encoder2 = m_pSeq->m_track->getConfigPosition(suc::ADC2, "STEPS");
                }
                else
                {
                    el.status = false;
                    el.position1 = 0.0;
                    el.position2 = 0.0;
                    el.encoder1 = 0;
                    el.encoder2 = 0;
                }
            }
        }
        else if(el.nameid == suc::MaskComb)
        {
            el.type = m_pSeq->IS.mask_comb;
        }
        else if(el.nameid == suc::InstrumentMode)
        {
            el.type = m_pSeq->IS.instrument_mode;
        }
        else if(el.nameid == suc::ADC_Mode)
        {
        }
        else if(el.nameid == suc::DROT_Mode)
        {
//            el.type = m_pSeq->IS.drot_mode;
            el.type = m_pSeq->m_track->getMode(suc::DROT_Mode);
        }
        else
        {
            W_LLOG("Unrecognized motor : " << el.nameid);
            el.status = false;
            el.position1 = 0.0;
            el.position2 = 0.0;
            el.encoder1 = 0;
            el.encoder2 = 0;
        }
    }
}


