// $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/src/tiptilt_ctrl/bcu/util/rtcSlopeComputer.hpp 3983 2022-07-04 08:43:50Z depa $

#pragma once

#include <Nice/BuiltinSequences.h>

#include <boost/shared_ptr.hpp>

#include "tiptilt_ctrl/bcu/util/bcuMemoryMap.h"
#include "util/shins_util.h"
#include "util/logger.h"

#include "rtcDM.hpp"
#include "rtcDetector.hpp"

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

enum rtcGainMode
{
    RTC_UNDEFINED_GAIN,
    RTC_FLAT_GAIN,
    RTC_POINTLIKE_GAIN,
    RTC_GAUSS_GAIN,
    RTC_FILE_GAIN
};

namespace shins
{//namespace shins
  namespace util
  {  //namespace util
    class rtcSlopeComputer
    {
    private:
        std::string m_diagIpv4;
        std::string m_diagMac;
        unsigned short int m_diagPort;
        unsigned int m_diagEnabled;
        unsigned int m_diagDecimation;
        unsigned int m_pixelSaveEnabled;
        unsigned int m_pixelSaveDecimation;
        unsigned int m_masterDecimation;
        unsigned int m_jumboEnabled;
        unsigned int m_masterEnabled;
        float m_Kp;
        float m_Ki;
        float m_Kd;
        float m_Tf;
        float m_Ts;
        float m_tiptilt_threshold;
        Nice::SeqFloat m_PidPars;
        Nice::SeqFloat m_centroidGain;
        Nice::SeqFloat m_centroidOrigin;
        Nice::SeqFloat m_centroidFact;
        Nice::SeqFloat m_modeVector;
        rtcGainMode m_gainMode;
        Nice::SeqFloat m_pixelGain;
        std::string m_strPixelGainFile;      // must correspond with the window to be monitored

        Nice::SeqInt m_FramesCounter;
      Nice::SeqInt m_FramesCounterIrq;
      Nice::SeqInt m_ParamSelector;
      Nice::SeqInt m_ParamSelectorIrq;
      Nice::SeqInt m_ResetLoop;
      Nice::SeqInt m_TTControlLoopEnabled;
      Nice::SeqInt m_enableRTCommands;

      /* DM related */

      // Nice::SeqInt m_DMInuseNumAct;  retrieved from rtcDM object
      // Nice::SeqInt m_DMNumActuators; retrieved from rtcDM object
      Nice::SeqFloat m_DMBitOffset;
      Nice::SeqInt m_DMActuatorMapPtr;
      Nice::SeqInt m_DMActuatorSignPtr;
      Nice::SeqInt m_DMActuatorsIrq;
      Nice::SeqInt m_DMBiasVectorPtr;
      Nice::SeqInt m_DMCmdSkippedCnt;
      Nice::SeqInt m_DMCommandVectPtr;
      Nice::SeqInt m_DMInfluenceMatrix0Ptr;
      Nice::SeqInt m_DMInfluenceMatrix1Ptr;
      Nice::SeqInt m_DMNumActuatorsIrq;
      Nice::SeqInt m_DMOutputEnable;
      Nice::SeqFloat m_DMOutputVector;
      Nice::SeqInt m_DMTmpOutputVector;
      Nice::SeqInt m_DMTmpOutputVectorPtr;
      Nice::SeqFloat m_DMTotalPower;
      Nice::SeqInt m_DMUpdateStart;
      Nice::SeqFloat m_DMm2bitConversion;       // DM command conversion factor from meters to bit
      Nice::SeqFloat m_MaxAbsDMCommand;
      Nice::SeqInt m_MaxAbsDMCommandSkip;
      Nice::SeqFloat m_MaxTTCommandValue;
      Nice::SeqFloat m_MinTTCommandValue;
      Nice::SeqInt m_ModeVector0Ptr;
      Nice::SeqInt m_ModeVector1Ptr;
      Nice::SeqInt m_NewModeVectorPtr;

      /* detector and centroid calculation raleted */
      Nice::SeqFloat m_PIDDelayLine;
      Nice::SeqFloat m_PIDFilterCoeff;
      Nice::SeqFloat m_centroidOut;
      Nice::SeqFloat m_pixelMinThreshold;
      Nice::SeqInt m_CCDInterfaceEnable;
      Nice::SeqInt m_CCDSubFrameCnt;
      Nice::SeqFloat m_CCDTotalFlux;
      Nice::SeqInt m_CCDWaitForReset;
      Nice::SeqInt m_CentroidFactPtr;
      Nice::SeqInt m_CentroidPixelAreaPtr;
      Nice::SeqFloat m_MaxCentroidValue;
      Nice::SeqFloat m_MinCentroidValue;
      Nice::SeqInt m_NumPixelsToDoPtr;
      Nice::SeqInt m_NumPixelsToDo_A;
      Nice::SeqInt m_NumPixelsToDo_B;
      Nice::SeqInt m_PixelGainPtr;
      Nice::SeqInt m_PixelOffsetPtr;
      Nice::SeqInt m_PreviousPixelAreaPtr;
      Nice::SeqFloat m_SubapIntensityThres;

      int m_MaxTimeHistoryLen;
      int m_LoadedHistoryLen;
      std::string m_TimeHistoryFile;

      Nice::SeqInt m_TimeHistoryAct;
      Nice::SeqInt m_TimeHistoryEndPtr;
      Nice::SeqInt m_TimeHistoryPtr;
      Nice::SeqFloat m_TimeHistoryGain;

      /* Diagnostic buffer */
      Nice::SeqInt m_DiagBuffDataPtr;
      Nice::SeqInt m_DiagBuffFooterPtr;
      Nice::SeqInt m_DiagBuffHeaderPtr;

      /* HSDL are defined but not used in SHARK-NIR */
      Nice::SeqInt m_HSDLVectorLen;
      Nice::SeqInt m_HSDLCmdStart;
      Nice::SeqInt m_HSDLCmdNotReadyCnt;
      Nice::SeqInt m_HSDLToSkipFrameCnt;
      Nice::SeqInt m_HSDLInputVectorPtr;
      Nice::SeqInt m_HSDLReplyVectorPtr;
      Nice::SeqFloat m_HSDLCommandScale;

      Nice::SeqInt m_HSDLInputVector;

    private:
      /* ------------------------------------------ */

      template <typename T>
      T get_value_from_map(const std::map<std::string, std::string>& _mapSetup,
                           const std::string& _key);
      const rtcDetector & m_det;
      const rtcDM & m_dm;
    public:

      rtcSlopeComputer(const std::map<std::string, std::string>& _mapSetup, const rtcDetector & _detector, const rtcDM & _dm);
      ~rtcSlopeComputer();

      Nice::SeqInt getDiagnosticRemoteAddress(const std::string MAC, const std::string & IP, unsigned short int Port);
      int setupGain(const std::map<std::string, std::string> & _gainSetup);
      int setup_pid(const std::map<std::string, std::string> & pid_setup);
      void setup_mode_vector(){m_modeVector.resize(m_dm.get_length_of_modes_vector(), 0);};

      void init(const int& _dmOutputEnabled, const int& _ttLoopEnabled,
                const int& _enableRTCmds, const float& _pixelMinThreshold,
                const unsigned int&   _diagEnabled,
                const unsigned int&   _diagFrameDec,
                const float& _minCentroidValue, const float& _maxCentroidValue,
                const float& _minTTCommandValue, const float& _maxTTCommandValue,
                const float& _centroidGainX,
                const float& _centroidGainY,
                const float& _centroidOriginX,
                const float& _centroidOriginY);
      int init_gain(const std::string & file_name);
      void generate_pointlike_gain(int sizex, int sizey, float cx, float cy, float radius, float amplitude = 1.0f);
      void generate_gauss_gain(int sizex, int sizey, float cx, float cy, float radius);
      int setup_diagnostic(std::map<std::string, std::string> _diagSetup);
      void set_PID_filter_coeff (const int& _frameRate);
      void set_PID_filter_coeff(float Kp, float Ki, float Kd, float Tf, float Ts);
      void set_time_history_gain(const std::vector<float>& _timeHistGain);
      void set_diagnostic_address(const std::string & diagIpv4, const std::string & diagMac, int diagPort)
      {
          m_diagIpv4 = diagIpv4;
          m_diagMac = diagMac;
          m_diagPort = (unsigned short int)diagPort;
      };
      const std::string & get_diag_ipv4() const { return m_diagIpv4;};
      const std::string & get_diag_mac() const { return m_diagMac;};
      unsigned short int get_diag_port() const { return m_diagPort;};
      void gen_centroid_params(Nice::SeqFloat & centroid_fact) const;
      void update_n_slopes();
      void set_tiptilt_threshold(float new_threshold){ m_tiptilt_threshold = new_threshold;};

      void set_diagnostic_decimation(int decimation){ m_diagDecimation = decimation;};
      void set_diagnostic_enabled(bool enabled){m_diagEnabled = enabled;};
      unsigned int get_diagnostic_enabled(){ return m_diagEnabled;};
      unsigned int get_decimation_factor(){ return m_diagDecimation;};
      inline Nice::SeqInt get_diagnostic_frame_dword () const {
          return su::uint_to_seqint((m_diagEnabled << 31) + m_diagDecimation);
      }
      void set_pixel_decimation(int decimation){m_pixelSaveDecimation = decimation;};
      void set_pixel_enabled(bool enabled){ m_pixelSaveEnabled = enabled;};
      unsigned int get_pixel_save_enabled() { return m_pixelSaveEnabled;};
      unsigned int get_pixel_save_decimation() { return m_pixelSaveDecimation;};
      inline Nice::SeqInt get_pixel_frame_dword () const {
          return su::uint_to_seqint((m_pixelSaveEnabled << 31) + m_pixelSaveDecimation);
      }
      void set_master_decimation(int decimation){m_masterDecimation = decimation;};
      void set_master_enabled(bool enabled){m_masterEnabled = enabled;};
      void set_jumbo_enabled(bool enabled);
      unsigned int get_master_decimation() { return m_masterDecimation;};
      unsigned int get_jumbo_enabled() { return m_jumboEnabled;};
      unsigned int get_master_enabled() { return m_masterEnabled;};
      inline Nice::SeqInt get_master_diagnostic_dword () const {
          return su::uint_to_seqint((m_masterDecimation << 16) + (m_jumboEnabled << 1) + m_masterEnabled);
      }
      int update_time_history_gain(const std::map<std::string, std::string> & _execSetup);
      void apply_mode_vector(const Nice::SeqFloat & modes, bool absolute);
      inline const Nice::SeqFloat & get_pid_pars() const               {
          return m_PidPars;
      }
      inline const Nice::SeqFloat & get_centroid_gain() const          {
          return m_centroidGain;
      }
      inline const Nice::SeqFloat & get_centroid_origin() const        {
          return m_centroidOrigin;
      }
      inline const Nice::SeqFloat & get_centroid_fact     () const     {
          return m_centroidFact;
      }
      const Nice::SeqFloat & get_pixel_gain() const {
          return m_pixelGain;
      }
      void set_centroid_origin(const Nice::SeqFloat& _centroidOrigin);

      /* dm related setters */
//      inline void set_dm_actuators_irq          (const int& _setValue) {
//        m_DMActuatorsIrq[0] = (_setValue > 0) ? 1 : 0;
//      }
//      inline void set_dm_num_actuators_irq      (const int& _setValue) {
//        m_DMNumActuatorsIrq[0] = (_setValue > 0) ? 1 : 0;
//      }
//      inline void set_dm_output_enable          (const int& _setValue) {
//        m_DMOutputEnable[0] = (_setValue > 0) ? 1 : 0;
//      }
//      inline void set_dm_update_start           (const int& _setValue) {
//        m_DMUpdateStart[0] = (_setValue > 0) ? 1 : 0;
//      }
//      inline void set_enable_rt_commands        (const int& _setValue) {
//        m_enableRTCommands[0] = (_setValue > 0) ? 1 : 0;
//      }
//      inline void set_frames_counter            (const int& _setValue) {
//        m_FramesCounter[0] = (_setValue > 0) ? 1 : 0;
//      }
//      inline void set_frames_counter_irq        (const int& _setValue) {
//        m_FramesCounterIrq[0] = (_setValue > 0) ? 1 : 0;
//      }
//      inline void set_param_selector            (const int& _setValue) {
//        m_ParamSelector[0] = (_setValue > 0) ? _setValue : 0;
//      }
//      inline void set_param_selector_irq        (const int& _setValue) {
//        m_ParamSelectorIrq[0] = (_setValue > 0) ? _setValue : 0;
//      }
//      inline void set_pixel_min_threshold       (const float& _setValue) {
//        m_pixelMinThreshold[0] = (_setValue > 0) ? _setValue : 0;
//      }
//      inline void set_reset_loop                (const int& _setValue) {
//        m_ResetLoop[0] = (_setValue > 0) ? 1 : 0;
//      }
//      inline void set_subap_intensity_threshold (const float& _setValue) {
//        m_SubapIntensityThres[0] = (_setValue > 0) ? _setValue : 0;
//      }
//      inline void set_tt_control_loop_enabled   (const int& _setValue) {
//        m_TTControlLoopEnabled[0] = (_setValue > 0) ? 1 : 0;
//      }

      /* getters */
      inline Nice::SeqInt get_frames_counter         () const {
        return m_FramesCounter;
      }
      inline Nice::SeqInt get_frames_counter_irq     () const {
        return m_FramesCounterIrq;
      }
      inline Nice::SeqInt get_param_selector         () const {
        return m_ParamSelector;
      }
      inline Nice::SeqInt get_param_selector_irq     () const {
        return m_ParamSelectorIrq;
      }
      inline Nice::SeqInt get_reset_loop             () const {
        return m_ResetLoop;
      }
      inline Nice::SeqInt get_tt_control_loop_enable () const {
        return m_TTControlLoopEnabled;
      }
      inline void set_tt_control_loop_enable(bool enable) {
        m_TTControlLoopEnabled = Nice::SeqInt(1, enable);
      }
      inline Nice::SeqInt get_enable_rt_commands     () const {
        return m_enableRTCommands;
      }

      /* dm related getters*/
      inline Nice::SeqInt get_dm_actuator_map_ptr        () const {
        return m_DMActuatorMapPtr;
      }
      inline Nice::SeqInt get_dm_actuator_sign_map_ptr   () const {
        return m_DMActuatorSignPtr;
      }
      inline Nice::SeqInt get_dm_actuators_irq           () const {
        return m_DMActuatorsIrq;
      }
      inline Nice::SeqInt get_dm_bias_vector_ptr         () const {
        return m_DMBiasVectorPtr;
      }
      inline Nice::SeqInt get_dm_commands_skipped_count  () const {
        return m_DMCmdSkippedCnt;
      }
      inline Nice::SeqInt get_dm_command_vector_ptr      () const {
        return m_DMCommandVectPtr;
      }
      inline Nice::SeqInt get_dm_influence_matrix0_ptr   () const {
        return m_DMInfluenceMatrix0Ptr;
      }
      inline Nice::SeqInt get_dm_influence_matrix1_ptr   () const {
        return m_DMInfluenceMatrix1Ptr;
      }
      inline Nice::SeqInt get_dm_num_actuators_irq       () const {
        return m_DMNumActuatorsIrq;
      }
      inline Nice::SeqInt get_dm_tmp_output_vector       () const {
        return m_DMTmpOutputVector;
      }
      inline Nice::SeqInt get_dm_tmp_output_vector_ptr   () const {
        return m_DMTmpOutputVectorPtr;
      }
      inline Nice::SeqFloat get_dm_total_power           () const {
        return m_DMTotalPower;
      }
      inline Nice::SeqInt get_dm_update_start            () const {
        return m_DMUpdateStart;
      }
      inline Nice::SeqFloat get_dm_meters2bit_conversion () const {
        return m_DMm2bitConversion;
      }
      inline Nice::SeqFloat get_dm_max_abs_command       () const {
        return m_MaxAbsDMCommand;
      }
      inline Nice::SeqInt get_dm_max_abs_command_skip    () const {
        return m_MaxAbsDMCommandSkip;
      }
      inline Nice::SeqFloat get_dm_max_tt_command_value  () const {
        return m_MaxTTCommandValue;
      }
      inline Nice::SeqFloat get_dm_min_tt_command_value  () const {
        return m_MinTTCommandValue;
      }
      inline Nice::SeqInt get_dm_mode_vector0_ptr        () const {
        return m_ModeVector0Ptr;
      }
      inline Nice::SeqInt get_dm_mode_vector1_ptr        () const {
        return m_ModeVector1Ptr;
      }
      inline Nice::SeqInt get_dm_new_mode_vector_ptr     () const {
        return m_NewModeVectorPtr;
      }
      inline Nice::SeqFloat get_dm_bit_offset            () const {
        return m_DMBitOffset;
      }
      inline Nice::SeqInt get_dm_output_enabled          () const {
        return m_DMOutputEnable;
      }
      inline Nice::SeqFloat get_dm_output_vector         () const {
        return m_DMOutputVector;
      }
      inline Nice::SeqFloat get_mode_vector              () const {
        return m_modeVector;
      }
      /* detector related getters */
      inline Nice::SeqFloat get_pixel_min_threshold      () const {
        return m_pixelMinThreshold; // ML -> PixelMinValue
      }
      inline Nice::SeqFloat get_centroid_out             () const {
        return m_centroidOut;
      }
      inline Nice::SeqFloat get_PID_delay_line           () const {
        return m_PIDDelayLine;
      }
      inline Nice::SeqFloat get_PID_filter_coeff         () const {
        return m_PIDFilterCoeff;
      }
      inline Nice::SeqInt get_ccd_interface_enable       () const {
        return m_CCDInterfaceEnable;
      }
      inline Nice::SeqInt get_ccd_sub_frame_cnt          () const {
        return m_CCDSubFrameCnt;
      }
      inline Nice::SeqFloat get_ccd_total_flux           () const {
        return m_CCDTotalFlux;
      }
      inline Nice::SeqInt get_ccd_wait_for_reset        () const {
        return m_CCDWaitForReset;
      }
      inline Nice::SeqInt get_centroid_fact_ptr         () const {
        return m_CentroidFactPtr;
      }
      inline Nice::SeqInt get_centroid_pixel_area_ptr   () const {
        return m_CentroidPixelAreaPtr;
      }
      inline Nice::SeqFloat get_max_centroid_value      () const {
        return m_MaxCentroidValue;
      }
      inline Nice::SeqFloat get_min_centroid_value      () const {
        return m_MinCentroidValue;
      }
      inline Nice::SeqInt get_num_pixels_to_do_ptr      () const {
        return m_NumPixelsToDoPtr;
      }
      inline Nice::SeqInt get_num_pixels_to_do_a        () const {
        return m_NumPixelsToDo_A;
      }
      inline Nice::SeqInt get_num_pixels_to_do_b        () const {
        return m_NumPixelsToDo_B;
      }
      inline Nice::SeqInt get_pixel_gain_ptr            () const {
        return m_PixelGainPtr;
      }
      inline Nice::SeqInt get_pixel_offset_ptr          () const {
        return m_PixelOffsetPtr;
      }
      inline Nice::SeqInt get_previous_pixel_area_ptr   () const {
        return m_PreviousPixelAreaPtr;
      }
      inline Nice::SeqFloat get_subap_intensity_thres   () const {
        return m_SubapIntensityThres;
      }
      inline Nice::SeqInt get_time_history_act          () const {
        return m_TimeHistoryAct;
      }
      inline Nice::SeqInt get_time_history_end_ptr      () const {
        return m_TimeHistoryEndPtr;
      }
      inline Nice::SeqInt get_time_history_ptr          () const {
        return m_TimeHistoryPtr;
      }
      inline const std::string & get_time_history_file  () const {
        return m_TimeHistoryFile;
      }
      inline int get_max_time_history_len               () const {
        return m_MaxTimeHistoryLen;
      }
      inline void set_loaded_time_history_len    (int len)       {
          m_LoadedHistoryLen = len;
      }
      inline void set_time_history_file(const std::string & file_name){
          m_TimeHistoryFile = file_name;
      }
      inline Nice::SeqFloat get_time_history_gain       () const {
        return m_TimeHistoryGain;
      }

      /* diagnostic buffer related getters */
      inline Nice::SeqInt get_diagnostic_header_ptr() const {
        return m_DiagBuffHeaderPtr;
      }
      inline Nice::SeqInt get_diagnostic_footer_ptr() const {
        return m_DiagBuffFooterPtr;
      }
      inline Nice::SeqInt get_diagnostic_data_ptr  () const {
        return m_DiagBuffDataPtr;
      }
      /* generators */
      inline Nice::SeqInt get_diag_record_len() const {
          return Nice::SeqInt(1, 4 + (m_DiagBuffFooterPtr[0] - m_DiagBuffHeaderPtr[0]));
      }

    };

    template <typename T>
    T rtcSlopeComputer::get_value_from_map(const std::map<std::string, std::string>& _mapSetup, const std::string& _key)
    {
        T value = T();
        // shins::util::get_value_from_map throws std::invalid_argument if _key not present
        value = shins::util::get_value_from_map<T, std::map<std::string, std::string>, std::string>(_mapSetup, _key);
        return value;
    }

    typedef boost::shared_ptr<rtcSlopeComputer> rtcSlopeComputerPtr;
  }
}
