// $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/src/track_ctrl/trackingCtrl.h 3965 2022-05-30 18:43:35Z depa $

#pragma once

#include <fstream>
#include <functional>
// SHINS
#include "util/common_sharknir.h"
#include "util/shins_util.h"
#include "util/logger.h"
#include "motion_ctrl/niceCallback.h"
//#include "motion_ctrl/indiIF.h"
#include "obs_ctrl/sharknir.h"

// #include "motion_ctrl/wheels/multiWheel.h"
#include "motion_ctrl/niceCallback.h"
#include "AdcWorkerThread.h"
#include <Sharknir/Drot/SharknirDrotServiceWorkerInterface.h>

// TaN
// #include <Nice/Log.h>
#include <Nice/App/Endpoint.h>
#include <Nice/App/ConfigClient.h>
#include <Basda/ServiceState.h>
#include <Basda/ServiceStateMachine.h>
#include <Nice/Map/NPoint.h>
// #include <Basda/Mocca/TrajServiceWorkerInterface.h>
#include <Basda/Mocca/WheelServiceWorkerInterface.h>
#include <Basda/Mocca/Constant.h>
#include <Basda/Mocca/Exception.h>
#include <Nice/Property/BridgeMemFn.hh>
#include <Nice/App/ConfigClient.h>
#include <Nice/Time.h>
#include <Nice/Time/Timer.h>
#include <Nice/Util/Thread.h>
#include <LbtoIIFInterface2.h>
#include <Ltcs/Iif/ServiceWorkerInterface.h>
#include <Ltcs/Iif/PropertyName.h>

#include "motion_ctrl/wheels/multiWheel.hpp"

// #include <Ice/LoggerUtil.h>

struct DrotDevice
{
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);
    void (* StartDerotationCB)(const Nice::Completion & c);
    void (* StartDerotationHereCB)(const Nice::Completion & c);
    void (* VarAbsoluteEncoderCB)(const double & val);
    int moving;
    void CheckState(Nice::CompletionState state)
    {
        switch(state)
        {
        case Nice::EXEC:
            if(moving == 0)moving++;
            else
            {
                U6_LLOG(__FUNCTION__);
                W_LLOG("Called with moving = " << moving);
                U8_LLOG("");
            }
            break;
        case Nice::OK :
            if(moving == 1)moving--;
            break;
        case Nice::ERROR :
            {
                if(moving == 1)
                    moving--;
            }
            break;
        default :
            break;
        }
    };
public:
    Sharknir::DrotServiceWorkerInterfacePtr 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(iPtr && state == Nice::OK)
        {
//            U9_LLOG("Drot position : " << iPtr->getPosition());
        }
        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(iPtr && state == Nice::OK)
        {
//            U9_LLOG("Drot position : " << iPtr->getPosition());
        }
        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 StartDerotationBridge(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(StartDerotationCB)
            StartDerotationCB(c);
//        CheckState(state);
        U8_LLOG("");
    }
    void StartDerotationHereBridge(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(StartDerotationHereCB)
            StartDerotationHereCB(c);
//        CheckState(state);
        U8_LLOG("");
    }
    void VarAbsoluteEncoderBridge(const double & val)
    {
        U6_LLOG(__FUNCTION__ << " ~ " << (int)val);
        if(VarAbsoluteEncoderCB)
            VarAbsoluteEncoderCB(val);
        U8_LLOG("");
    }
    DrotDevice(Sharknir::DrotServiceWorkerInterfacePtr iptr,const std::string & alias_name):
        iPtr(iptr),
        alias(alias_name)
    {
        MoveToHomeCB = NULL;
        MoveToNamedCB = NULL;
        MoveAbsoluteCB = NULL;
        MoveRelativeCB = NULL;
        MoveToLimitCB = NULL;
        StartDerotationCB = NULL;
        StartDerotationHereCB = NULL;
        VarAbsoluteEncoderCB = NULL;
        moving = 0;
    }
    DrotDevice(const std::string & cfg_file):
        iPtr(NULL),
        cfgFile(cfg_file)
    {
        GetDataFromConfigFile();
        MoveToHomeCB = NULL;
        MoveToNamedCB = NULL;
        MoveAbsoluteCB = NULL;
        MoveRelativeCB = NULL;
        MoveToLimitCB = NULL;
        StartDerotationCB = NULL;
        StartDerotationHereCB = NULL;
        VarAbsoluteEncoderCB = NULL;
        moving = 0;
    }
    DrotDevice(Sharknir::DrotServiceWorkerInterfacePtr iptr):
        iPtr(iptr)
    {
        GetDataFromConfig(iptr->config());
        MoveToHomeCB = NULL;
        MoveToNamedCB = NULL;
        MoveAbsoluteCB = NULL;
        MoveRelativeCB = NULL;
        MoveToLimitCB = NULL;
        StartDerotationCB = NULL;
        StartDerotationHereCB = NULL;
        VarAbsoluteEncoderCB = NULL;
        moving = 0;
    }
    void GetDataFromConfigFile();
    void GetDataFromConfig(const Nice::JointPtr dev);
    void SetInterfacePointer(Sharknir::DrotServiceWorkerInterfacePtr 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),
                               void (* startDerotationCB)(const Nice::Completion &c),
                               void (* startDerotationHereCB)(const Nice::Completion & c),
                               void (* varAbsoluteEncoderCB)(const double & val))
    {
        MoveToHomeCB = moveToHomeCB;
        MoveToNamedCB = moveToNamedCB;
        MoveAbsoluteCB = moveAbsoluteCB;
        MoveRelativeCB = moveRelativeCB;
        MoveToLimitCB = moveToLimitCB;
        StartDerotationCB = startDerotationCB;
        StartDerotationHereCB = startDerotationHereCB;
        VarAbsoluteEncoderCB = varAbsoluteEncoderCB;
    }
};

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


enum EDrotModes
{
    DROT_MODE_PUPIL,
    DROT_MODE_FIELD_FIXED,
    DROT_MODE_FIELD_FREE
};

struct trackSetup
{
    std::map<std::string, int> NamedMapWheel;
    std::map<std::string, int> NamedMapDrot;
    std::map<std::string, double> AbsMapWheel;
    std::map<std::string, double> AbsMapDrot;
    std::map<std::string, double> RelMapWheel;
    std::map<std::string, double> RelMapDrot;
    std::map<std::string, std::string> AbsMapWheelUnits;
    std::map<std::string, std::string> AbsMapDrotUnits;
    std::map<std::string, std::string> RelMapWheelUnits;
    std::map<std::string, std::string> RelMapDrotUnits;
    std::string named_drot;
    std::string named_adc1;
    std::string named_adc2;
    void clear()
    {
        NamedMapWheel.clear();
        NamedMapDrot.clear();
        AbsMapWheel.clear();
        AbsMapDrot.clear();
        RelMapWheel.clear();
        RelMapDrot.clear();
        AbsMapWheelUnits.clear();
        AbsMapDrotUnits.clear();
        RelMapWheelUnits.clear();
        RelMapDrotUnits.clear();
    }
};

class trackingCtrl
{
private:
  /////////////
  // DEVICES //
  /////////////
  Sharknir::DrotServiceWorkerInterfacePtr m_drot;
  Basda::MoccaWheelServiceWorkerInterfacePtr m_adc1;
  Basda::MoccaWheelServiceWorkerInterfacePtr m_adc2;
  Ltcs::IifServiceWorkerInterfacePtr m_iif;
  // boost::shared_ptr<multiWheel> m_ptrADC;
//  boost::shared_ptr<indiIF> m_ptrLbtoIF;

  std::map<std::string, DrotDevice> m_MapDrots;
  std::map<std::string, WheelDevice> m_MapAdcs;
  void install_callbacks();

  AdcWorkerThread * m_ptrAdcWorkerThread;
  /////////////////////
  // UTILITY MEMBERS //
  /////////////////////
  niceCallback m_callbackObject;
  const std::string __OBJ_NAME__;
  std::vector<std::string> m_vecAliasDrot;
  std::vector<std::string> m_vecAliasADC;
  // ptr to state machine
  boost::shared_ptr<osfsm::OSFsm> m_ptrStateMachine;

  static trackingCtrl * m_pTrackingCtrl;
  EDrotModes m_drotMode;


    void setupPrepare(const std::map<std::string, std::string>& _mapNamedPos, trackSetup & setup);
    trackSetup m_track_setup;

  static bool AdcCallback(double & pressure, double & temperature, double & elevation, double & drot_pos);
  Ice::PropertiesPtr m_props;
  Nice::AppConfigClientPtr m_ConfigClient;
  su::EServiceStatus m_service_status;
  std::string m_service;

public:
  trackingCtrl(boost::shared_ptr<osfsm::OSFsm>& _ptrStateMachine, Ice::PropertiesPtr props);
  ~trackingCtrl();

  su::EServiceStatus Connect();
  su::EServiceStatus GetServiceStatus(){return m_service_status; };
  bool CheckOnline(bool throw_if_notonline = true);
  void UpdateTcsInterface(Ltcs::IifServiceWorkerInterfacePtr & iif){m_iif = iif;};
//  static void * connect_to_service(void * arg);
//  static void * connect_to_service(){connect_to_service(m_pTrackingCtrl);};

  void resetPosition(const std::string _devAlias);
  std::string getAdcMode(){if(m_ptrAdcWorkerThread){return m_ptrAdcWorkerThread->GetMode();}else{return std::string();}};
  std::string getMode(const std::string & _device);

//  bool isDevice(const std::string& _strDevAlias);
  void changeState(const Basda::ServiceState& _state, const std::vector<std::string>& _vecDevAlias=std::vector<std::string>());

  void initialize(const std::vector<std::string>& _vecDevAlias=std::vector<std::string>());
  void initializeStart(const std::vector<std::string>& _vecDevAlias=std::vector<std::string>());
  void initializeWait(const std::vector<std::string>& _vecDevAlias=std::vector<std::string>(), const Nice::Time& _time = Nice::Time());

    Ice::Int setupInstrumentStart(const sharknir::dictSetupNamedPos &_dSetupPos);
    Ice::Int setupInstrumentWait(const sharknir::dictSetupNamedPos &_dSetupPos, const Nice::Time& _time = Nice::Time());

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

  void moveAbsolute      (const std::map<std::string, double> & _mapPos, const std::map<std::string, std::string> & _mapUnit=std::map<std::string, std::string>());
  void moveAbsoluteStart (const std::map<std::string, double> & _mapPos, const std::map<std::string, std::string> & _mapUnit=std::map<std::string, std::string>());
  void moveAbsoluteWait  (const std::map<std::string, double> & _mapPos, const Nice::Time & _time = Nice::Time());

  void moveRelative      (const std::map<std::string, double> & _mapPos, const std::map<std::string, std::string> & _mapUnit=std::map<std::string, std::string>());
  void moveRelativeStart (const std::map<std::string, double> & _mapPos, const std::map<std::string, std::string> & _mapUnit=std::map<std::string, std::string>());
  void moveRelativeWait  (const std::map<std::string, double> & _mapPos, const Nice::Time & _time = Nice::Time());

  int startTrackingTest(double startPosDegree, double endPosDegree, double speedDegreeSec);
  int startTracking(const Nice::Date & _startDate, double _position);
  int startTracking();
  int waitTracking(Ice::Long seconds);

  int setMode(const std::map<std::string, std::string>& _mapSetup);

  int getNdevices();
    Ice::Int getPositionDict(std::map<std::string, double>& _mapPosition, const std::string &_unit="DEGREE");
  double getPosition(const std::string& _deviceName, const std::string &_unit="DEGREE");
  double getConfigPosition(const std::string& _deviceName, const std::string &_unit="DEGREE");
  double getAbsEncPosition();
  int getJsonStatus(std::string & jsonStatus);

  int setDerotationParameters1(int numPolynomials, double esamplingRate, double frequency);
  int setDerotationParameters2(int polynomialOrder, int faultTolerance, int samplesPerSegment);
  int setDerotationParameters3(double debug1, double debug2, double debug3);
  int setDerotationParameters4(double debug4, double debug5, double debug6);
    int sharkDerotatorBacklashProcedure();

    int setAdcCorrectionParameters(double co, double p0, double p1, double t0, double t1);
    int setAdcCoRotationOffset(double corotation);
    int setAdcCounterRotationOffset(double counterrotation);

  int isMoving(std::map<std::string, bool>& _mapDev);
  int emergencyAbort();
  int stopAll();
  int stop(const std::vector<std::string>& _vecDevAlias);
  int abort(const std::vector<std::string>& _vecDevAlias);
//   Nice::SeqDouble getSinusCurve(Nice::Time _duration,
//                                 Nice::Time _period,
//                                 Nice::Time _timeResolution,
//                                 double _amplitude,
//                                 double _phase,
//                                 double _startPoint);
    void setFlags(uint64_t flags, uint64_t more_flags);
    void unsetFlags(uint64_t flags, uint64_t more_flags);
    void toggleFlags(uint64_t flags, uint64_t more_flags);

};

