// $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/src/motion_ctrl/stagesX/multiStageX.hpp 3856 2022-03-18 09:57:21Z depa $

# pragma once

// TAN
#include <config.h>
#include <Basda/Mocca/Exception.h>
#include <Basda/Mocca/XServiceWorkerInterface.h>
#include <Nice/App/Endpoint.h>
#include <Nice/Property/BridgeMemFn.hh>
//#include <Nice/Property/BridgeFn.hh>
#include <Nice/App/ConfigClient.h>
#include <Nice/Time.h>
#include <Nice/Time/Timer.h>
//#include <Nice/Log.h>

// SHINS
// #include "../multiDeviceT.h"
#include "motion_ctrl/common_sharknir.h"
#include "motion_ctrl/multiDevice.hpp"
#include "motion_ctrl/niceCallback.h"




struct StageXDevice
{
private:
    void (* MoveToHomeCB)(const Nice::Completion & c);
    void (* MoveToNamedCB)(const Nice::Completion & c);
    void (* MoveAbsoluteCB)(const Nice::Completion & c);
    void (* MoveRelativeCB)(const Nice::Completion & c);
    void (* MoveToLimitCB)(const Nice::Completion & c);
    int moving;
    void CheckState(Nice::CompletionState state)
    {
        switch(state)
        {
        case Nice::EXEC:
            moving++;
            break;
        case Nice::OK :
            moving--;
            break;
        case Nice::ERROR :
            moving--;
            break;
        default :
            break;
        }
    };
public:
    Basda::MoccaXServiceWorkerInterfacePtr iPtr;
    std::map<std::string, std::tr1::tuple<double, double> > units;
    std::map<std::string, double> namedPositions;
    std::map<std::string, int> namedIds;
    std::string cfgFile;
    std::string alias;
    int GetMoving() const {return moving;};
    void MoveToHomeBridge(const Nice::Completion & c)
    {
        Nice::CompletionState state = c.state();
        U6_LLOG(__FUNCTION__ << " ~ " << alias << " ~ " << "c.state() = " << state << " ~ " << "c.percent() = " << c.percent() << " ~ " << "c.action() = " << c.action() << " ~ " << "c.exceptionString() = " << c.exceptionString() << " ~ " << "c.stack() = " << c.stack() << " ~ " << "c.timestamp() = " << c.timestamp() << " ~ " << "c.id() = " << c.id());
        CheckState(state);
        if(MoveToHomeCB)
            MoveToHomeCB(c);
        U8_LLOG("");
    }
    void MoveToNamedBridge(const Nice::Completion & c)
    {
        Nice::CompletionState state = c.state();
        U6_LLOG(__FUNCTION__ << " ~ " << alias << " ~ " << "c.state() = " << state << " ~ " << "c.percent() = " << c.percent() << " ~ " << "c.action() = " << c.action() << " ~ " << "c.exceptionString() = " << c.exceptionString() << " ~ " << "c.stack() = " << c.stack() << " ~ " << "c.timestamp() = " << c.timestamp() << " ~ " << "c.id() = " << c.id());
        CheckState(state);
        if(MoveToNamedCB)
            MoveToNamedCB(c);
        U8_LLOG("");
    }
    void MoveAbsoluteBridge(const Nice::Completion & c)
    {
        Nice::CompletionState state = c.state();
        U6_LLOG(__FUNCTION__ << " ~ " << alias << " ~ " << "c.state() = " << state << " ~ " << "c.percent() = " << c.percent() << " ~ " << "c.action() = " << c.action() << " ~ " << "c.exceptionString() = " << c.exceptionString() << " ~ " << "c.stack() = " << c.stack() << " ~ " << "c.timestamp() = " << c.timestamp() << " ~ " << "c.id() = " << c.id());
        CheckState(state);
        if(MoveAbsoluteCB)
            MoveAbsoluteCB(c);
        U8_LLOG("");
    }
    void MoveRelativeBridge(const Nice::Completion & c)
    {
        Nice::CompletionState state = c.state();
        U6_LLOG(__FUNCTION__ << " ~ " << alias << " ~ " << "c.state() = " << state << " ~ " << "c.percent() = " << c.percent() << " ~ " << "c.action() = " << c.action() << " ~ " << "c.exceptionString() = " << c.exceptionString() << " ~ " << "c.stack() = " << c.stack() << " ~ " << "c.timestamp() = " << c.timestamp() << " ~ " << "c.id() = " << c.id());
        CheckState(state);
        if(MoveRelativeCB)
            MoveRelativeCB(c);
        U8_LLOG("");
    }
    void MoveToLimitBridge(const Nice::Completion & c)
    {
        Nice::CompletionState state = c.state();
        U6_LLOG(__FUNCTION__ << " ~ " << alias << " ~ " << "c.state() = " << state << " ~ " << "c.percent() = " << c.percent() << " ~ " << "c.action() = " << c.action() << " ~ " << "c.exceptionString() = " << c.exceptionString() << " ~ " << "c.stack() = " << c.stack() << " ~ " << "c.timestamp() = " << c.timestamp() << " ~ " << "c.id() = " << c.id());
        CheckState(state);
        if(MoveToLimitCB)
            MoveToLimitCB(c);
        CheckState(state);
        U8_LLOG("");
    }
    void ChangeStateBridge(const Nice::Completion & c)
    {
        Nice::CompletionState state = c.state();
        U6_LLOG(__FUNCTION__ << " ~ " << alias << " ~ " << "c.state() = " << state << " ~ " << "c.percent() = " << c.percent() << " ~ " << "c.action() = " << c.action() << " ~ " << "c.exceptionString() = " << c.exceptionString() << " ~ " << "c.stack() = " << c.stack() << " ~ " << "c.timestamp() = " << c.timestamp() << " ~ " << "c.id() = " << c.id());
//        CheckState(state);
//        if(MoveToLimitCB)
//            MoveToLimitCB(c);
//        CheckState(state);
        U8_LLOG("");
    }
    StageXDevice(Basda::MoccaXServiceWorkerInterfacePtr iptr,const std::string & alias_name):
        iPtr(iptr),
        alias(alias_name)
    {
        MoveToHomeCB = NULL;
        MoveToNamedCB = NULL;
        MoveAbsoluteCB = NULL;
        MoveRelativeCB = NULL;
        MoveToLimitCB = NULL;
        moving = 0;
    }
    StageXDevice(const std::string & cfg_file):
        iPtr(NULL),
        cfgFile(cfg_file)
    {
        GetDataFromConfigFile();
        MoveToHomeCB = NULL;
        MoveToNamedCB = NULL;
        MoveAbsoluteCB = NULL;
        MoveRelativeCB = NULL;
        MoveToLimitCB = NULL;
        moving = 0;
    }
    StageXDevice(Basda::MoccaXServiceWorkerInterfacePtr iptr):
        iPtr(iptr)
    {
        GetDataFromConfig(iptr->config());
        MoveToHomeCB = NULL;
        MoveToNamedCB = NULL;
        MoveAbsoluteCB = NULL;
        MoveRelativeCB = NULL;
        MoveToLimitCB = NULL;
        moving = 0;
    }
    void GetDataFromConfigFile();
    void GetDataFromConfig(const Nice::JointPtr dev);
    void SetInterfacePointer(Basda::MoccaXServiceWorkerInterfacePtr iptr){ iPtr = iptr;};
    void SetSecondaryCallbacks(void (* moveToHomeCB)(const Nice::Completion & c),
                            void (* moveToNamedCB)(const Nice::Completion & c),
                            void (* moveAbsoluteCB)(const Nice::Completion & c),
                            void (* moveRelativeCB)(const Nice::Completion & c),
                            void (* moveToLimitCB)(const Nice::Completion & c))
    {
        MoveToHomeCB = moveToHomeCB;
        MoveToNamedCB = moveToNamedCB;
        MoveAbsoluteCB = moveAbsoluteCB;
        MoveRelativeCB = moveRelativeCB;
        MoveToLimitCB = moveToLimitCB;
    }

};



class multiStageX;
typedef Nice::Handle<multiStageX> multiStageXPtr;


// class multiStageX : public multiDeviceT<Basda::MoccaXServiceWorkerInterfacePtr>
// {
// public:
//   virtual void connect();
// };

// Devices models as X stages are:
// inbeam_dep
// shutter
// pupil_lens_dep

class multiStageX : public Nice::Shared, public multiDevice
{
private:
// Ice::Trace   m_info   ;
// Ice::Trace   m_debug  ;
// Ice::Error   m_error  ;
// Ice::Warning m_warning;
/*
 * moved to parent class
   int m_counter;
   std::size_t m_nDevices;

   boost::shared_ptr<niceCallback> m_ptrCallbackObj;

   boost::function<void(const Nice::Completion&)> m_callbackFn;
   boost::function<void(const Nice::Completion&)> m_callbackFnCount;

   std::string m_strCallbackName;
   std::string m_strCallbackNameCount;
 */
void install_callback(const std::string & _strCallbackName, const std::string & _strPropName);

void install_callback_on_completion(const std::string& _strCallbackName,
                                    const std::string& _strPropName);
void install_callback_on_state(const std::string& _strCallbackName,
                               const std::string& _strPropName);

//void apply_callback2(std::map<std::string, Basda::MoccaXServiceWorkerInterfacePtr>::iterator _it);


//boost::function<void(const Nice::Completion&)> m_callbackFnCount;
void callbackCompletion2(const Nice::Completion & completion)
{
    m_callbackFnCount(completion);
}

protected:
Nice::JointPtr m_config;

std::map<std::string, Basda::MoccaXServiceWorkerInterfacePtr> m_mapStageX;
std::vector<Basda::MoccaXServiceWorkerInterfacePtr> m_vecStageX;
// std::vector<std::string> m_vecAliasName;

// these should be one method each accepring as parameter
// the stageX idx in optical path or the stageX name.
std::string getEndPointName(const std::size_t _stageXIdx);
std::string getBaseName(const std::size_t _stageXIdx);

std::map<std::string, StageXDevice> m_MapStageX;
public:
multiStageX(const std::string & _service);

virtual ~multiStageX();

const std::map<std::string, StageXDevice> & GetMultiStageX(){ return m_MapStageX;};

virtual void connect();
virtual void online (const std::vector<std::string>& _vecDevAlias=std::vector<std::string>());
virtual void standby(const std::vector<std::string>& _vecDevAlias=std::vector<std::string>());
virtual void changeState(const Basda::ServiceState& _state,
                         const std::vector<std::string>& _vecDevAlias=std::vector<std::string>());

void moveToPositiveLimit(const std::vector<std::string> &_vecDevAlias);
void moveToPositiveLimitStart(const std::vector<std::string> &_vecDevAlias);
void moveToPositiveLimitWait(const std::vector<std::string> &_vecDevAlias, const Nice::Time & timeout);
void moveToNegativeLimitStart(const std::vector<std::string> &_vecDevAlias);
void moveToNegativeLimitWait(const std::vector<std::string> &_vecDevAlias, const Nice::Time & timeout);

Nice::SeqBitSet isAtLimit(const std::string &_DevAlias);
void SetSecondaryCallbacks(const std::string & alias,
                        void (* moveToHomeCB)(const Nice::Completion & c),
                        void (* moveToNamedCB)(const Nice::Completion & c),
                        void (* moveAbsoluteCB)(const Nice::Completion & c),
                        void (* moveRelativeCB)(const Nice::Completion & c),
                        void (* moveToLimitCB)(const Nice::Completion & c));


virtual void moveToReference();
virtual void moveToReferenceStart();
virtual void moveToReferenceWait(const Nice::Time& _time = Nice::Time());

virtual void moveDeviceToReference(const std::vector<std::string>& _vecDevName);
virtual void moveDeviceToReferenceStart(const std::vector<std::string>& _vecDevAlias);
virtual void moveDeviceToReferenceWait(const std::vector<std::string>& _vecDevAlias, const Nice::Time& _time = Nice::Time());

virtual void moveToNamedPosition(const std::map<std::string, int> &_mapNamedPos);
virtual void moveToNamedPositionStart(const std::map<std::string, int>& _mapPos);
virtual void moveToNamedPositionWait(const std::map<std::string, int>& _mapNamedPos, const Nice::Time& _time = Nice::Time());

virtual void moveToAbsolutePosition(const std::map<std::string, double>& _mapPos,      const std::map<std::string, std::string>& _mapUnit=std::map<std::string, std::string>());
virtual void moveToAbsolutePositionStart(const std::map<std::string, double>& _mapPos, const std::map<std::string, std::string>& _mapUnit=std::map<std::string, std::string>());
virtual void moveToAbsolutePositionWait(const std::map<std::string, double>& _mapPos,  const Nice::Time& _time = Nice::Time());

virtual void moveToRelativePosition(const std::map<std::string, double>& _mapPos,      const std::map<std::string, std::string>& _mapUnit=std::map<std::string, std::string>());
virtual void moveToRelativePositionStart(const std::map<std::string, double>& _mapPos, const std::map<std::string, std::string>& _mapUnit=std::map<std::string, std::string>());
virtual void moveToRelativePositionWait(const std::map<std::string, double>& _mapPos,  const Nice::Time& _time = Nice::Time());

virtual void emergencyAbort();
virtual int stopAll();
virtual int stop(const std::vector<std::string>& _vecDevAlias);
virtual int abort(const std::vector<std::string>& _vecDevAlias);
// virtual void emergencyAbort();

virtual bool isDevice(const std::string& _strDevAlias);
// callback installation
virtual void set_callback_fn(boost::function<void(const Nice::Completion&)> & _cbfn, const std::string & _strFnName);
virtual void set_callback_fn_count(boost::function<void(const Nice::Completion&)> & , const std::string& _strFnName);

// GETTERS
// the number of wheel is constant in SHARK-NIR; thus it is an
// hard coded value
inline virtual std::size_t getNdevices() {
    return m_nDevices;
}
virtual std::vector<std::string> getNamedPosition(const std::size_t _stageIdx = 3);
virtual std::vector<std::string> getNamedPosition(const std::vector<std::string>& _vecAlias);
virtual std::map<std::string, std::string> getNamedPosition2(const std::vector<std::string>& _vecAlias);
virtual Ice::Int getPosition(std::map<std::string, Ice::Double>& _mapPosition, const std::string& _unit="STEPS");
};
