// $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/src/motion_ctrl/wheels/multiWheel.hpp 3806 2022-01-23 17:58:37Z depa $

#pragma once

#include <sys/types.h>
#include <math.h>

// TAN
#include <config.h>
#include <Basda/Mocca/Exception.h>
#include <Basda/Mocca/WheelServiceWorkerInterface.h>
//#include <Basda/Mocca/WheelServiceWorkerInternalData.h>
#include <Basda/ServiceStateMachine.h>
#include <Basda/ServiceState.h>
#include <Nice/App/Endpoint.h>
#include <Nice/Property/BridgeMemFn.hh>
#include <Nice/App/ConfigClient.h>
#include <Nice/Time.h>
#include <Nice/Time/Timer.h>
//#include <Nice/Log.h>
#include <Nice/Map/Point.h>

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

class multiWheel;
typedef Nice::Handle<multiWheel> multiWheelPtr;

struct WheelDevice
{
    Basda::MoccaWheelServiceWorkerInterfacePtr iPtr;
    std::string alias;
    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;
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 (* ScanAllReferenceSwitchesCB)(const Nice::Completion & c);
    bool (* checkInterdiction)(void);
    bool (* interdictionReaction)(void);
    static bool interdiction_false(){ return false; }
    static bool no_reaction(){return true;};
    int moving;
    void CheckState(Nice::CompletionState state)
    {
        switch(state)
        {
        case Nice::EXEC:
            if(moving == 0)moving++;
            else
            {
                W_LLOG("Wheel Device - CheckState - Completion state error");
            }
            break;
        case Nice::OK :
            if(moving > 0)moving--;
            break;
        case Nice::ERROR :
            if(moving > 0)moving--;
            break;
        default :
            break;
        }
    };
public:
    bool CheckInterdiction() const { return checkInterdiction();}
    bool InterdictionReaction() const { return interdictionReaction();}
    int GetMoving() const {return moving;};
    void ErrorBridge(const Nice::Completion & c)
    {
        U6_LLOG(__FUNCTION__ << " ~ " << alias << " ~ " << "c.state() = " << c.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());
        U8_LLOG("");
    }
    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 ScanAllReferenceSwitchesBridge(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(ScanAllReferenceSwitchesCB)
            ScanAllReferenceSwitchesCB(c);
        U8_LLOG("");
    }
    WheelDevice(Basda::MoccaWheelServiceWorkerInterfacePtr iptr,const std::string & alias_name):
        iPtr(iptr),
        alias(alias_name),
        checkInterdiction(interdiction_false),
        interdictionReaction(no_reaction)
    {
        MoveToHomeCB = NULL;
        MoveToNamedCB = NULL;
        MoveAbsoluteCB = NULL;
        MoveRelativeCB = NULL;
        ScanAllReferenceSwitchesCB = NULL;
        moving = 0;
    }
    WheelDevice(const std::string & cfg_file):
        iPtr(NULL),
        cfgFile(cfg_file),
        checkInterdiction(interdiction_false),
        interdictionReaction(no_reaction)
    {
        GetDataFromConfigFile();
        MoveToHomeCB = NULL;
        MoveToNamedCB = NULL;
        MoveAbsoluteCB = NULL;
        MoveRelativeCB = NULL;
        ScanAllReferenceSwitchesCB = NULL;
        moving = 0;
    }
    WheelDevice(Basda::MoccaWheelServiceWorkerInterfacePtr iptr): // const Nice::JointPtr & dev):
        iPtr(iptr),
        checkInterdiction(interdiction_false),
        interdictionReaction(no_reaction)
    {
        GetDataFromConfig(iptr->config());
        MoveToHomeCB = NULL;
        MoveToNamedCB = NULL;
        MoveAbsoluteCB = NULL;
        MoveRelativeCB = NULL;
        ScanAllReferenceSwitchesCB = NULL;
        moving = 0;
    }
    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 (* scanAllReferenceSwitchesCB)(const Nice::Completion & c))
    {
        MoveToHomeCB = moveToHomeCB;
        MoveToNamedCB = moveToNamedCB;
        MoveAbsoluteCB = moveAbsoluteCB;
        MoveRelativeCB = moveRelativeCB;
        ScanAllReferenceSwitchesCB = scanAllReferenceSwitchesCB;
    }
    void SetInterdictionCallbacks(bool (* check_interdiction)(void), bool (* interdiction_reaction)(void))
    {
        checkInterdiction = check_interdiction;
        interdictionReaction = interdiction_reaction;
    }

    void GetDataFromConfigFile();
    void GetDataFromConfig(const Nice::JointPtr dev);
    void SetInterfacePointer(Basda::MoccaWheelServiceWorkerInterfacePtr iptr){ iPtr = iptr;};
};


class WheelInterdiction : public std::range_error
{
//    const char * info;
//    const char * what () const throw ()
//    {
//        return "Attempted move while interdiction condition is met";
//    }
public:
    WheelInterdiction(std::string msg): std::range_error(msg)
    {
    };
};


class multiWheel : public Nice::Shared, public multiDevice
{
private:
// Ice::Trace   m_info   ;
// Ice::Trace   m_debug  ;
// Ice::Error   m_error  ;
// Ice::Warning m_warning;

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 install_callback_on_reference(const std::string& _strCallbackName, const std::string& _strPropName);


protected:
Nice::JointPtr m_config;
// Nice::JointPtr m_configDummy;

std::map<std::string, Basda::MoccaWheelServiceWorkerInterfacePtr> m_mapWheels;   //<WHEELALIAS, MoccaService>
std::vector<Basda::MoccaWheelServiceWorkerInterfacePtr> m_vecWheels;
std::map<std::string, WheelDevice> m_MapWheels;
// std::vector<std::string> m_vecAliasName;

// there should be one method each accepting as parameter
// the wheel idx in optical path or the wheel name.
std::string getEndPointName(const std::size_t _wheelIdx);
std::string getBaseName(const std::size_t _wheelIdx);


public:
multiWheel(const std::string & _service);

virtual ~multiWheel();

const std::map<std::string, WheelDevice> & GetMultiWheel(){ return m_MapWheels;};
int setSimulatedDevice(const std::string & _dev);

virtual void connect();
int setWheelsInterdictionCallbacks(const std::map<std::string, boost::function<bool()> > callbacks_map);
int setWheelsInterdictionReactionCallback(const std::map<std::string, boost::function<void()> > callbacks_map);
Ice::Int SetWheelInterdictionCallbacks(const std::string & alias,
                                       bool (* checkInterdiction)(void),
                                       bool (* interdictionReaction)(void));
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 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 bool isDevice(const std::string& _strDevAlias);

// 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 _wheelIdx = 7);
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);
void getPositionPair(std::pair<std::string, Ice::Double>& _pairPosition, const std::string& _unit="STEPS");

virtual Ice::Int getPosition(std::map<std::string, Ice::Double>& _mapPosition, const std::string& _unit="STEPS");
Basda::MoccaWheelServiceWorkerInterfacePtr getDevicePtr(const std::string& _strDeviceName);
// SETTERS
// callback installation
virtual void set_callback_fn(boost::function<void(const Nice::Completion&)>&, const std::string& _strFnName);
virtual void set_callback_fn_state(boost::function<void(const Basda::ServiceStateMachine&)>&, const std::string& _strFnName);
virtual void set_callback_fn_state(boost::function<void(const Basda::ServiceStateMachine&, const std::string&)>&, const std::string& _strDevAlias);
virtual void set_callback_fn_count(boost::function<void(const Nice::Completion&)>&, const std::string& _strFnName);

// void setActualPosLabel(const std::vector<std::string>& _vecLabel);
};

// #endif // _MULTI_WHEEL_
