// $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/src/obs_ctrl/SeqMotion.hpp 3680 2021-07-15 11:19:35Z laud $

// IceLite (from MPIA) 3.6 does not support C++ 11

/* SeqMotion.hpp *  Created on: Oct 7, 2021 * Author: shinsmgr */

#ifndef OBS_CTRL_SEQSETUP_HPP_
#define OBS_CTRL_SEQSETUP_HPP_

#include <pthread.h>

#include "util/shins_util.h"
#include "util/constants.h"
//#include "util/logger.h"
#include "motion_ctrl/motionCtrl.hpp"
#include "calunit_ctrl/calunitCtrl.h"
#include "track_ctrl/trackingCtrl.h"
#include "tiptilt_ctrl/tiptiltCtrl.hpp"
#include "seqIDataMgr.hpp"
#include "seqITcs.hpp"
#include "seqISnmp.hpp"

//#include "sharknir.h"

//#include "obsControl.h"
//using namespace sharknir;


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

struct setupStep
{
    dictSetupNamedPos action;
    dictSetupNamedPos wait;
};

#define CAL_FIBER_DEP_CORO_WHEEL_INTERDICTION_STEPS -1600000 // Luca c-red-2 08-10 10:50

typedef std::vector<std::tr1::tuple<std::string, std::string, double, std::string> > vecSetupNamedAbsPos;



enum EClashCases
{
    CLASH_CASE_0,//  UNDEFINED
    CLASH_CASE_1,//  Coro Wheel not involved in setup
    CLASH_CASE_2,//  Coro Wheel is not changing position during setup, Calibration fiber in safe position
    CLASH_CASE_3,//  Coro Wheel is not changing position during setup, Calibration fiber in dangerous position
    CLASH_CASE_4,//  Calibration Fiber is in safe position, and is not going to change position
    CLASH_CASE_5,//  Calibration Fiber is in safe position, and is moving to safe position
    CLASH_CASE_6,//  Calibration Fiber is in safe position moving to dangerous position. Its movement will be split
    CLASH_CASE_7,//  Calibration Fiber is in dangerous position. Will be put in safe position and then its position restored
    CLASH_CASE_8,//  Calibration Fiber is in dangerous position, moving toward safe position. Its movement will be split
    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
};
//boost::shared_ptr<osfsm::OSFsm> &_ptrSM

struct SInstrumentSetup
{
    std::string instrument_mode;
    std::string drot_mode;
    std::string adc_mode;
    std::string mask_comb;
};


class SeqSetup //: public obsControl::Sequencer
{
    boost::shared_ptr<motionCtrl> m_motion;
    boost::shared_ptr<calunitCtrl> m_calibration;
    boost::shared_ptr<trackingCtrl> m_track;
    boost::shared_ptr<tiptiltCtrl> & m_tiptilt;
    boost::shared_ptr<SeqDataMgr> & m_seqDataMgr;
    boost::shared_ptr<SeqSnmp> & m_seqSnmp;
    boost::shared_ptr<SeqTcs> & m_tcs;


    std::vector<setupStep> manageCoroFiberClash(const dictSetupNamedPos &dSetupNamedPos);

    dictSetupNamedPos m_executingSetup;
    bool m_bExecutingSetup;
    bool m_bAbortSetup;
    IceUtil::Mutex m_SetupMutex;
    IceUtil::Mutex m_SetupUpdateMutex;
    IceUtil::Mutex m_JsonMutex;
    SInstrumentSetup IS;
    int storeSetup(const dictSetupNamedPos& _dSetupNamedPos);
    std::vector<std::string> m_Motors;

    static void * connect_to_services(void * arg);
    Ice::PropertiesPtr m_props;
public:
    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);


//    void SetResetCalibrationCallback(void (* pReset)(void)){m_pResetCalibration = pReset;};

    static void getMotorStatus(su::tagElementHeader & el);

    static SeqSetup * m_pSeq;
    static bool no_wheel_interdiction(){ return false;};
    static void no_reaction(){};
    static bool s_coro_interdiction();
    static inline bool cal_fiber_dangerous_area_test(double steps);
    static bool put_cal_fiber_in_safe_area2(void);
    static vecSetupNamedAbsPos put_cal_fiber_in_safe_area();
    static void * init_calibration_control(void * arg);
    static void * init_motion_control(void * arg);
    static void coro_callback(const Nice::Completion & _c);
    static void * at_coro_wheel_exec(void * arg);
    static void * test_exec(void * arg);
//    static void * setup_instrument(void * arg);
    Ice::Double get_cal_fiber_pos_steps();
    Ice::Int move_cal_fiber_dep(vecSetupNamedAbsPos destination = vecSetupNamedAbsPos());
    EClashCases check_coro_fiber_clash_3(vecSetupNamedAbsPos &_dSetupNamedPos);
    vecSetupNamedAbsPos m_CalFiberInSafeResult;

    void SetGetObContextCallback(const su::SObContext & (* pGetObContext)(void)){ m_pGetObContext = pGetObContext;};
    const su::SObContext & (*m_pGetObContext)(void);


    bool m_calunit_initialized;
    bool m_motion_initialized;
    static void CalunitRestartCatcher(const Nice::Completion & c);
    static void MotionRestartCatcher(const Nice::Completion & c);

    void extractClashParam(dictSetupPos & _dSetupPos, const IceUtil::Optional<dictDevUnit> & _dUnit, dictSetupNamedPos setupNamedPos, su::EShinsMovements type);

    Ice::Int moveDevices(const dictSetupNamedPos& _dSetupNamedPos);
    Ice::Int moveDevicesStart(const dictSetupNamedPos& _dSetupNamedPos);
    Ice::Int moveDevicesWait();

    Ice::Int setupInstrument(const dictSetupNamedPos& _dSetupNamedPos, const Ice::Current&);

//    Ice::Int setupInstrument(dictSetupNamedPos& _dSetupNamedPos);

    Ice::Int setupInstrumentStart(dictSetupNamedPos& _dSetupNamedPos);

    Ice::Int setupInstrumentWait(const dictSetupNamedPos& _dSetupNamedPos, const IceUtil::Optional<Nice::Time>& _time);

    Ice::Int abortSetup();

    Ice::Int isBusy(){ return m_bExecutingSetup;};

    Ice::Int setupInstrumentAbsolutePosition(const dictSetupPos& _dSetupPos,
                                             const IceUtil::Optional<dictDevUnit>& _dUnit,
                                             const Ice::Current&);


    Ice::Int setupInstrumentRelativePosition(const dictSetupPos& _dSetupPos,
                                             const IceUtil::Optional<dictDevUnit>& _dUnit,
                                             const Ice::Current&);

    Ice::Int getJsonStatus(std::string & jsonStatus, bool throw_on_failure = true);

};

#endif /* OBS_CTRL_SEQSETUP_HPP_ */

