// $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/src/tiptilt_ctrl/tiptiltCtrl.hpp 3964 2022-05-28 15:36:51Z depa $

#pragma once

#include <config.h>
#include <Nice/App/Application.h>
#include <Nice/App/ConfigClient.h>
#include "tiptilt_ctrl/bcu/Interface/Sharknir/Bcu/ServiceWorker.h"
#include "tiptilt_ctrl/bcu/Interface/Sharknir/Bcu/BcuInterface.h"
#include "tiptilt_ctrl/bcu/Interface/Sharknir/Bcu/BcuException.h"
#include "tiptilt_ctrl/bcu/Interface/Sharknir/Bcu/Const.h"
#include "tiptilt_ctrl/bcu/Interface/Sharknir/Bcu/OverallDiagnosticRecordRingBuffer.h"
#include "tiptilt_ctrl/bcu/util/ClockPtr.h"

#include "tiptilt_ctrl/bcu/util/NiceClock.h"
#include "tiptilt_ctrl/bcu/util/rtcDetector.hpp"
#include "tiptilt_ctrl/bcu/util/rtcDM.hpp"
#include "tiptilt_ctrl/bcu/util/rtcSlopeComputer.hpp"

#include "tiptilt_ctrl/bcu/util/bcuMemoryMap.h"
#include "tiptilt_ctrl/tiptiltCtrl.hpp"

//#include "tiptilt_ctrl/util/Eigen/Core"

#include <boost/algorithm/string.hpp>
#include <boost/algorithm/string/predicate.hpp>
#include <boost/bind.hpp>
#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
#ifndef BOOST_NO_CXX11_HDR_CHRONO
//
//#include <chrono>
//#include <thread>
//
#else
//
//#include <boost/thread.hpp>
//#include <boost/chrono.hpp>
//
#endif

#include <pthread.h>

#include <CCfits>
//#include <Eigen/Core>
#include <algorithm>
#include <bitset>
#include <cmath>
#include <fstream>
#include <functional>
#include <inttypes.h>
#include <locale>
#include <sstream>
#include <stdint.h>
#include <vector>
#include <unistd.h>

#include <Ice/Initialize.h>
#include <Ice/Logger.h>
#include <IceUtil/Handle.h>
#include <IceUtil/Shared.h>

#include "util/shins_util.h"

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

enum EAbortTiptiltOp
{
    TIPTILT_ABORT_TECCAM_ACQ = 1
};


struct STeccamExpose
{
    bool bAcquiringPixcube;
    std::map<std::string, std::string> PixCubeSetup;
    std::string fitfile_name;
};

class tiptiltCtrl: public IceUtil::Shared
{
public:
    shins::util::rtcDetectorPtr m_ptrDetector;
    shins::util::rtcDMPtr m_ptrDm;
    shins::util::rtcSlopeComputerPtr m_ptrSlopeComputer;
    static const std::string & getRtcStatus(const std::string & key);
    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);
    int getJsonStatus(std::string & jsonStatus);
    su::EServiceStatus Connect();
    bool CheckConnection(bool bThrow = true);
    bool CheckOnline(bool throw_if_notonline = true);

private:

    Nice::AppConfigClientPtr m_ConfigClient;
    su::EServiceStatus m_service_status;
    std::string m_service;
    // void connectToService();
    Sharknir::BcuServiceWorkerInterfacePtr m_bcuService;
    std::vector<int> m_diagData;
    std::vector<std::vector<int> > m_slope_frames;
    std::vector<std::vector<short int> > m_pix_frames;

    static tiptiltCtrl * m_pTipTilt;

    std::map<std::string, std::string> m_currentSetup;
    bool m_bSaveRawDiagData;
    bool m_bCheckBcuWrite;

    bool m_isInit;
    float m_writeBcuSleep;

    int m_abort;
//    pthread_mutex_t m_TiptiltMutex;
//    pthread_mutex_t m_BcuWriteMutex;
//    pthread_mutex_t m_BcuReadMutex;
//    static tiptiltCtrl * m_pTT;
//    static void * acquire_pixcube(void *);
    IceUtil::Mutex m_BcuWriteMutex;
    IceUtil::Mutex m_BcuReadMutex;
    IceUtil::Mutex m_AcquiringCubeMutex;
    IceUtil::Mutex m_UpdateAcquiringCubeMutex;
    bool m_bAcquiringPixCube;
    Ice::Int abortTeccamAcquisition();
//    int async_tech_cam_expose();
//    STeccamExpose m_TeccamExpose;

    std::valarray<double> load_fits(std::string _filename, int &_loadedActNum, int &_loadedNModes);
    Nice::SeqFloat set_PID_filter_coeff(const int &_frameRate, const int &_CCDMode);
//    template<typename Derived>
//    Nice::SeqInt convert_to_seqint(const Eigen::MatrixBase<Derived> &_emat);
//    template<typename Derived>
//    Nice::SeqFloat convert_to_seqfloat(const Eigen::MatrixBase<Derived> &_emat);
    const std::string __OBJ_NAME__;
    void sleep(int n);
    int read_bcu_preallocated(const int &_address, const std::string &_mem, Nice::SeqInt & result);
    int setup_tech_cam_expose(const std::map<std::string, std::string> & _dTechCamSetup);
    int tech_cam_process_data(bool save_as_bias);
    int save_pix_cube(const std::vector< std::vector<short int> > & pixel_frames,
                      const std::vector<short int> & bias,
                      const std::vector<float> & gain,
                      const std::vector< std::vector<int> > & slope_frames,
                      std::string & fit_file);
    void LogCfitsioStack(int line, int n, int & status);
    int save_pixel_bias(const std::vector< std::vector<short int> > pixel_frames, std::string & dat_file);
    unsigned int m_CubePixelDecimation;
    unsigned int m_CubeDiagDecimation;
public:
    tiptiltCtrl(Ice::PropertiesPtr props);
    ~tiptiltCtrl();

    void emergencyAbortAll(){ abortTeccamAcquisition();};
//    bool isAcquiringPixCube(){ return m_TeccamExpose.bAcquiringPixcube; };
    Ice::Int SequencerIConfigCamera(const std::map<std::string, std::string> &_dictTipTiltSetup);
    void interruptCallback(int _sig);
    void default_initialization(Ice::PropertiesPtr props);
    int init_bcu(const std::map<std::string, std::string> &_mapSetup);
    int setup_bcu();
    int start_bcu();

    int set_centroid_pos(const float &centroidX, const float &centroidY);
    int write_bcu_single(const int &_address, const Nice::SeqFloat &_buffer, const std::string &_mem = "dsp");
    int write_bcu(const int &_address, const Nice::SeqInt &_buffer, const std::string &_mem = "dsp");
    // int write_cred(const int& _address, const Nice::SeqInt & _buffer);
    // int write_bcu_float(const int& _address, const Nice::SeqInt & _buffer);
    Nice::SeqFloat read_bcu_single(const int & _address, const int & _lengthInDWords, const std::string & _mem);
    Nice::SeqInt read_bcu(const int &_address, const int &_lengthInDWords, const std::string &_mem = "dsp");
    // Nice::SeqInt read_ocam(const int& _address, const int& _lengthInDWords);
    int reset_bcu();
    int switch_ccd_mode(const std::map<std::string, std::string>& _mapSetup);

    int enable_loop(const bool &_enableLoop);
    Nice::SeqInt check_loop();

    int write_time_history_to_bcu(const std::string & file_name);
    int write_time_history_to_bcu();
    int enable_time_history(const bool &_enable);

    int apply_mode_vector(const Nice::SeqFloat &_modeVector, bool absolute);

    int filter_known_commands(const std::string & command, std::string & reply);
    int send_command_to_camera(const std::string &_command, std::string &_reply);
    int config_camera(const std::map<std::string, std::string> &_mapSetup);
    int config_camera();
    int config_camera_cropping();
    int config_camera_framerate();
    int config_camera_tint();

    int enable_diagnostic(const std::map<std::string, std::string> & _diagSetup);
    int enable_diagnostic(unsigned int diagEnabled,
                          unsigned int decimationFactor,
                          unsigned int pixelEnabled,
                          unsigned int pixelDecimation,
                          const std::string& diagIpv4,
                          const std::string& diagMac,
                          int& diagPort,
                          unsigned int masterDecimation,
                          unsigned int jumboEnabled,
                          unsigned int masterEnabled);

    std::map<std::string, bool> exec_setup(const std::map<std::string, std::string> & _execSetup, bool force = false);

    int enable_diagnostic(int &_decimationFactor, const int &_saveAllFrames, const int &_transfPXtoDSM, const int &enableJumboFrames = false);

    int enable_px_to_DSM(const bool &_enableFlag);

    int tech_cam_expose(const std::map<std::string, std::string> &_dTechCamSetup, std::string & _fit_file);

    Sharknir::Bcu::SeqOverallDiagnosticRecord getPastOverallDiagnosticRecordsInOnePass(const int &_nMaxRecords);
    Sharknir::Bcu::SeqOverallDiagnosticRecord getPastCcdOverallDiagnosticRecordsInOnePass(const int &_nMaxRecords);
    Sharknir::Bcu::SeqOverallDiagnosticRecord getPastSlopeOverallDiagnosticRecordsInOnePass(const int &_nMaxRecords);

    Sharknir::Bcu::SeqOverallDiagnosticRecord getFutureOverallDiagnosticRecordsInOnePass(const int &_nMaxRecords,
        const Nice::Time &_timeout);
    Sharknir::Bcu::SeqOverallDiagnosticRecord getFutureCcdOverallDiagnosticRecordsInOnePass(const int &_nMaxRecords,
        const Nice::Time &_timeout);
    Sharknir::Bcu::SeqOverallDiagnosticRecord getFutureSlopeOverallDiagnosticRecordsInOnePass(const int &_nMaxRecords,
        const Nice::Time &_timeout);
    void createPastOverallDiagnosticRecordSnapshot(const std::string &_cookie, const int &_nRecords);
    Sharknir::Bcu::SeqOverallDiagnosticRecord getPastOverallDiagnosticRecordsFromSnapshot(const std::string &COOKIE,
        int INCLUDING_BEGIN, int N_RECORDS);
    void freePastOverallDiagnosticRecordSnapshot(const std::string &COOKIE);

    void createPastSlopeOverallDiagnosticRecordSnapshot(const std::string &COOKIE, int N_RECORDS);
    Sharknir::Bcu::SeqOverallDiagnosticRecord getPastSlopeOverallDiagnosticRecordsFromSnapshot(
        const std::string &COOKIE, int INCLUDING_BEGIN, int N_RECORDS);
    void freePastSlopeOverallDiagnosticRecordSnapshot(const std::string &COOKIE);

    void createFutureOverallDiagnosticRecordSnapshot(const std::string &COOKIE, int N_RECORDS, Nice::Time TIMEOUT);
    Sharknir::Bcu::SeqOverallDiagnosticRecord getFutureOverallDiagnosticRecordsFromSnapshot(const std::string &COOKIE,
        int INCLUDING_BEGIN, int N_RECORDS);
    void freeFutureOverallDiagnosticRecordSnapshot(const std::string &COOKIE);

    void createFutureSlopeOverallDiagnosticRecordSnapshot(const std::string &COOKIE, int N_RECORDS, Nice::Time TIMEOUT);
    Sharknir::Bcu::SeqOverallDiagnosticRecord getFutureSlopeOverallDiagnosticRecordsFromSnapshot(
        const std::string &COOKIE, int INCLUDING_BEGIN, int N_RECORDS);
    void freeFutureSlopeOverallDiagnosticRecordSnapshot(const std::string &COOKIE);

    bool is_init(bool except = false);
    inline bool is_dm_zonal_mode()
    {
        return m_ptrDm->is_zonal_mode();
    }

private:
    void writeImage(std::string &_fitsFilename, Eigen::Matrix<int16_t, Eigen::Dynamic, 0> &_eVec, const uint &_nrows,
        const uint &_ncols, const uint &_nFrames);

    void tech_cam_download_data(void);
    bool m_bTechCamDownload;

    template<typename T>
    T get_value_from_map(const std::map<std::string, std::string> &_mapSetup, const std::string &_key);

    template<typename T>
    T get_value_from_map_with_default(const std::map<std::string, std::string> &_mapSetup, const std::string &_key,
        const T &defaultValue);

    template<typename T>
    T handle_arguments(const Ice::StringSeq &_stringSeq, const std::string &_argname);

    template<typename T>
    T handle_opt_arguments(const Ice::StringSeq &_stringSeq, const std::string &_argname, const T &_defValue);

    std::vector<char> reformat_from_sram(const Nice::SeqInt &_inVector);

    Nice::SeqInt string_to_seqint(const std::string & _cmd);
    std::string string_from_seqint(const Nice::SeqInt &_inVector, int begin, int end);
};

typedef IceUtil::Handle<tiptiltCtrl> tiptiltCtrlPtr;

/*
 * @method handle_arguments
 * @param _stringSeq
 * @param _argname
 */
template<typename T>
T tiptiltCtrl::handle_arguments(const Ice::StringSeq &_stringSeq, const std::string &_argname)
{
    D_LLOG(_argname);
    T retValue;
    Ice::StringSeq lowcaseStringSeq;
    std::transform(_stringSeq.begin(), _stringSeq.end(), std::back_inserter(lowcaseStringSeq),
        boost::bind(boost::to_lower_copy<std::string>, _1, std::locale()));
    T_LLOG("[std::transform]");
    BOOST_AUTO(itFind,
        std::find(lowcaseStringSeq.begin(), lowcaseStringSeq.end(), boost::to_lower_copy<std::string>(_argname)));
    if(itFind != lowcaseStringSeq.end())
    {
        if(itFind + 1 != lowcaseStringSeq.end() || !(boost::algorithm::starts_with((*(itFind + 1)), "--")))
        {
            try
            {
                T_LLOG("[try]");
                retValue = shins::util::convertStringToType<T>(*(itFind + 1));
                T_LLOG("[shins::util::convertStringToType]");
            }
            catch (shins::util::BadConversion e)
            {
                // m_error << "ERROR: " << __PRETTY_FUNCTION__ << ":" << __LINE__ << ":"
                //         << "Value for " << (*itFind) << " cannot be converted to numeric";
                // m_error.flush();
                throw e;
            }
        }
        else
        {
            // m_error << "ERROR: " << __PRETTY_FUNCTION__ << ":" << __LINE__ << ":"
            //         << "Command line argument " << (*itFind) << " has no value";
            // m_error.flush();
            throw shins::util::NotFound(_argname + " not found");
            // throw std::runtime_error("Command line argument " + (*itFind) + " has no value");
        }
    }
    else
    {
        // m_error << "ERROR: " << __PRETTY_FUNCTION__ << ":" << __LINE__ << ":"
        //         << "Command line argument " << (*itFind) << " is missing";
        // m_error.flush();
        throw shins::util::NotFound(_argname + " not found");
        // throw std::runtime_error("Command line argument " + (*itFind) + "  is missing");
    }
    return retValue;
}


template<typename T>
T tiptiltCtrl::handle_opt_arguments(const Ice::StringSeq &_stringSeq, const std::string &_argname, const T &_defValue)
{
    D_LLOG(__PRETTY_FUNCTION__ + _argname);
    T retValue = _defValue;
    Ice::StringSeq lowerStringSeq;
    std::transform(_stringSeq.begin(), _stringSeq.end(), std::back_inserter(lowerStringSeq),
        boost::bind(boost::to_lower_copy<std::string>, _1, std::locale()));

    BOOST_AUTO(itFind, std::find(lowerStringSeq.begin(), lowerStringSeq.end(), _argname));
    if(itFind != lowerStringSeq.end())
    {
        if(itFind + 1 != lowerStringSeq.end() || !(boost::algorithm::starts_with((*(itFind + 1)), "--")))
        {
            try
            {
                retValue = shins::util::convertStringToType<T>(*(itFind + 1)); // this function converts to any type
            }
            catch (shins::util::BadConversion e)
            {
                // m_error << "ERROR: " << __PRETTY_FUNCTION__ << ":" << __LINE__ << ":"
                //         << "Value for " << (*itFind) << " cannot be converted to numeric";
                // m_error.flush();
                throw e;
            }
        }
        else
        {
            // m_error << "ERROR: " << __PRETTY_FUNCTION__ << ":" << __LINE__ << ":"
            //         << "Optional command line argument " << (*itFind) << " has no value";
            // m_error.flush();
            // throw shins::util::NotFound((*itFind) + " not found");
        }
    }
    else
    {
        // m_error << "ERROR: " << __PRETTY_FUNCTION__ << ":" << __LINE__ << ":"
        //         << "Optional command line argument " << (*itFind) << " is missing";
        // m_error.flush();
        // throw shins::util::NotFound((*itFind) + " not found");
    }
    return retValue;
}


template<typename T>
T tiptiltCtrl::get_value_from_map(const std::map<std::string, std::string> &_mapSetup, const std::string &_key)
{
    return shins::util::get_value_from_map<T, std::map<std::string, std::string>, std::string>(_mapSetup, _key);
}

template <typename T>
bool get_value_from_setup(const std::map<std::string, std::string> & map, const std::string & key, T & val)
{
    std::map<std::string, std::string>::const_iterator it = map.find(key);
    if(it == map.end())
    {
        return false;
    }
    else
    {
        val = su::convertStringToType<T>(it->second);
        return true;
    }
}

bool get_bool_from_setup(const std::map<std::string, std::string> & map, const std::string & key, bool & val);

template<typename T>
T tiptiltCtrl::get_value_from_map_with_default(const std::map<std::string, std::string> &_mapSetup,
                                               const std::string &_key,
                                               const T &defaultValue)
{
    T retValue = T();
    retValue = shins::util::get_value_from_map_with_default<T>(_mapSetup, _key, defaultValue);
    return retValue;
}
