// $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/src/motion_ctrl/stagesXY/multiStageXY.hpp 3779 2021-11-24 19:19:43Z depa $

#pragma once

#include <ctime>

// TAN
#include <Basda/Mocca/Exception.h>
#include <Basda/Mocca/XYServiceWorkerInterface.h>
#include <Nice/App/ConfigClient.h>
#include <Nice/App/Endpoint.h>
//#include <Nice/Log.h>
#include <Nice/Map/NPoint.h> // not sure what it is needed for
#include <Nice/Map/Point.h>
#include <Nice/Property/BridgeMemFn.hh>
#include <Nice/Time.h>
#include <Nice/Time/Timer.h>
#include <config.h>

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

// BOOST
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/variate_generator.hpp>

class multiStageXY;
typedef Nice::Handle<multiStageXY> multiStageXYPtr;

struct StageXYDevice
{
    Basda::MoccaXYServiceWorkerInterfacePtr iPtr;
    // UNITS = {ScaleX, ShearX, ShearY, ScaleY, OffsetX, OffsetY}
    std::map<std::string, std::tr1::tuple<double, double, double, double, double, double> > units;
    // std::map<std::string, std::tr1::tuple<double, double> > namedPositions;
    std::map<std::string, Nice::Point > namedPositions;
    std::map<std::string, int> namedIds;
    std::string cfgFile;
    std::string alias;
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);
public:
    void MoveToHomeBridge(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());
//        U7_LLOG(__FUNCTION__ << " ~ " << alias << " ~ " << "c.command() = " << c.command());
        if(MoveToHomeCB)
            MoveToHomeCB(c);
        U8_LLOG("");
    }
    void MoveToNamedBridge(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());
        if(MoveToNamedCB)
            MoveToNamedCB(c);
        U8_LLOG("");
    }
    void MoveAbsoluteBridge(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());
        if(MoveAbsoluteCB)
            MoveAbsoluteCB(c);
        U8_LLOG("");
    }
    void MoveRelativeBridge(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());
        if(MoveRelativeCB)
            MoveRelativeCB(c);
        U8_LLOG("");
    }
    void MoveToLimitBridge(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());
        if(MoveToLimitCB)
            MoveToLimitCB(c);
        U8_LLOG("");
    }
    StageXYDevice(Basda::MoccaXYServiceWorkerInterfacePtr iptr,const std::string & alias_name):
        iPtr(iptr),
        alias(alias_name)
    {
        MoveToHomeCB = NULL;
        MoveToNamedCB = NULL;
        MoveAbsoluteCB = NULL;
        MoveRelativeCB = NULL;
        MoveToLimitCB = NULL;
    }
    StageXYDevice(const std::string & cfg_file):
        iPtr(NULL),
        cfgFile(cfg_file)
    {
        GetDataFromConfigFile();
        MoveToHomeCB = NULL;
        MoveToNamedCB = NULL;
        MoveAbsoluteCB = NULL;
        MoveRelativeCB = NULL;
        MoveToLimitCB = NULL;
    }
    StageXYDevice(Basda::MoccaXYServiceWorkerInterfacePtr iptr):
        iPtr(iptr)
    {
        GetDataFromConfig(iptr->config());
        MoveToHomeCB = NULL;
        MoveToNamedCB = NULL;
        MoveAbsoluteCB = NULL;
        MoveRelativeCB = NULL;
        MoveToLimitCB = NULL;
    }
    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;
    }
    void GetDataFromConfigFile();
    void GetDataFromConfig(Nice::JointPtr dev);
    void SetInterfacePointer(Basda::MoccaXYServiceWorkerInterfacePtr iptr){ iPtr = iptr;};
};

// Devices models as XY stages are:
// inbeam_TT

class multiStageXY : 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;
   // static const std::size_t m_nDevices = 3;
   std::size_t m_nDevices;
   boost::shared_ptr<niceCallback> m_ptrCallbackObj;
   boost::function<void(const Nice::Completion&)> m_callbackFn;
   std::string m_strCallbackName;
 */
void install_callback(const std::string&, const std::string&);
//void apply_callback2(std::map<std::string, Basda::MoccaXYServiceWorkerInterfacePtr>::iterator _it);
Nice::Point genRandomPosition(const Basda::MoccaXYServiceWorkerInterfacePtr _svc);

protected:
Nice::JointPtr m_config;

std::map<std::string, Basda::MoccaXYServiceWorkerInterfacePtr> m_mapStageXY;
std::vector<Basda::MoccaXYServiceWorkerInterfacePtr> m_vecStageXY;
std::map<std::string, StageXYDevice> m_MapStageXY;
// std::vector<std::string> m_vecBaseName;

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

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

virtual ~multiStageXY();

const std::map<std::string, StageXYDevice> & GetMultiStageXY(){ return m_MapStageXY;};
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 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>());

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());

void moveToPositionPoint(std::map<std::string, Nice::Point>& _mapPos);
void moveToPositionPointStart(std::map<std::string, Nice::Point>& _mapPos);
void moveToPositionPointWait(std::map<std::string, Nice::Point>& _mapPos, const Nice::Time& _time = Nice::Time());

virtual bool isDevice(const std::string& _strDevAlias);
virtual void set_callback_fn(boost::function<void(const Nice::Completion&)> &, const std::string &);
// 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");

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

//class multiStageXY;
