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

#include "rtcSlopeComputer.hpp"

namespace shins {
namespace util {

rtcSlopeComputer::rtcSlopeComputer(const std::map<std::string, std::string>& _mapSetup,
                                   const rtcDetector & _detector,
                                   const rtcDM& _dm):
m_det(_detector),
m_dm(_dm)
{
    U6_LLOG(__FUNCTION__);

    m_gainMode = RTC_UNDEFINED_GAIN;
    int dmOutputEnabled = 1;
    int ttLoopEnabled = 0;
    int enableRTCmds = 0;
    float pixelMinThreshold;     //= get_value_from_map<float>(_mapSetup, "TTMINTHRESHOLD");
    unsigned int diagFrameDec;   //= get_value_from_map<unsigned int>(_mapSetup, "TTDIAGDECIMATION");
    unsigned int diagEnabled;    //= get_value_from_map<unsigned int>(_mapSetup, "TTDIAGENABLED");
    float minCentroidValue;      //= get_value_from_map<int>(_mapSetup, "TTMINCENTROIDVAL");
    float maxCentroidValue;      //= get_value_from_map<int>(_mapSetup, "TTMAXCENTROIDVAL");
    float minTTCommandValue;     //= get_value_from_map<int>(_mapSetup, "TTMINTTCOMMAND");
    float maxTTCommandValue;     //= get_value_from_map<int>(_mapSetup, "TTMAXTTCOMMAND");
    bool bdmOutputEnabled = true, bttLoopEnabled = false, benableRTCmds = false, bdiagEnabled = true;
    std::string strKey = "";

    m_LoadedHistoryLen = 0;
    m_TimeHistoryGain = Nice::SeqFloat(2,0.f);
    m_MaxTimeHistoryLen = sud::timeHistoryLen;

    m_diagEnabled = 1;
    m_diagDecimation = 0;
    m_pixelSaveEnabled = 1;
    m_pixelSaveDecimation = 0;
    m_masterDecimation = 0;
    m_jumboEnabled = 0;
    m_masterEnabled = 1;
    m_tiptilt_threshold = 4.3f;

    setup_mode_vector();

    std::map<std::string, std::string>::const_iterator itFind;
    if((itFind = _mapSetup.find("TTTIMEHISTORYFILE")) != _mapSetup.end())
        m_TimeHistoryFile = itFind->second;
    if((itFind = _mapSetup.find("TTTIMEHISTORYGAIN")) != _mapSetup.end())
        update_time_history_gain(_mapSetup);
    if((itFind = _mapSetup.find("TTTIMEHISTORYLEN")) != _mapSetup.end())
        m_MaxTimeHistoryLen = su::convertStringToType<int>(itFind->second);

    if(su::get_bool_from_setup(_mapSetup, "TTDMOUTPUTENABLED", bdmOutputEnabled))
        dmOutputEnabled = bdmOutputEnabled;
    if(su::get_bool_from_setup(_mapSetup, "TTLOOPENABLED", bttLoopEnabled))
        ttLoopEnabled = bttLoopEnabled;
    if(su::get_bool_from_setup(_mapSetup, "TTRTCMDENABLED", benableRTCmds))
        enableRTCmds = benableRTCmds;
    if(su::get_bool_from_setup(_mapSetup, "TTDIAGENABLED", bdiagEnabled))
        diagEnabled = bdiagEnabled;

    try
    {
        pixelMinThreshold = get_value_from_map<float>(_mapSetup, "TTMINTHRESHOLD");
    }
    catch(std::invalid_argument& e)
    {
        W_LLOG(e.what() << " Setting to default value " << sud::pixelMinThreshold);
        pixelMinThreshold = sud::pixelMinThreshold /*default value*/;
    }

    try
    {
        diagFrameDec = get_value_from_map<int>(_mapSetup, "TTDIAGDECIMATION");
    }
    catch(std::invalid_argument& e)
    {
        W_LLOG(e.what() << " Setting to default value " << sud::diagFrameDec);
        diagFrameDec = sud::diagFrameDec /*default value*/;
    }

    try
    {
        minCentroidValue = get_value_from_map<int>(_mapSetup, "TTMINCENTROIDVAL");
    }
    catch(std::invalid_argument& e)
    {
        W_LLOG(e.what() << " Setting to default value " << sud::minCentroidValue);
        minCentroidValue = sud::minCentroidValue;
    }

    try
    {
        maxCentroidValue = get_value_from_map<int>(_mapSetup, "TTMAXCENTROIDVAL");
    }
    catch(std::invalid_argument& e)
    {
        W_LLOG(e.what() << " Setting to default value " << sud::maxCentroidValue);
        maxCentroidValue = sud::maxCentroidValue;
    }

    try
    {
        minTTCommandValue = get_value_from_map<float>(_mapSetup, "TTMINTTCOMMAND");
    }
    catch(std::invalid_argument& e)
    {
        W_LLOG(e.what() << " Setting to default value " << sud::minTTCommandValue);
        minTTCommandValue = sud::minTTCommandValue;
    }

    try
    {
        maxTTCommandValue = get_value_from_map<float>(_mapSetup, "TTMAXTTCOMMAND");
    }
    catch(std::invalid_argument& e)
    {
        W_LLOG(e.what() << " Setting to default value " << sud::maxTTCommandValue);
        maxTTCommandValue = sud::maxTTCommandValue;
    }



    float centroidOriginY, centroidOriginX;
    float centroidGainX, centroidGainY;
    //////////////////////////////////////////////////
    // centroidGain value                           //
    // depends on the optical setup.                //
    //////////////////////////////////////////////////
    try
    {
        centroidGainX = get_value_from_map<float>(_mapSetup, "TTCENTROIDGAINX");
    }
    catch(const std::invalid_argument& ex)
    {
        W_LLOG(ex.what() << "missing TTCENTROIDGAINX, setting to default " << sud::centroidGainX);
        centroidGainX = sud::centroidGainX;
    }

    try
    {
        centroidGainY = get_value_from_map<float>(_mapSetup, "TTCENTROIDGAINY");
    }
    catch(const std::invalid_argument& ex)
    {
        W_LLOG(ex.what() << "missing TTCENTROIDGAINY, setting to default " << sud::centroidGainY);
        centroidGainY = sud::centroidGainY;
    }

    try
    {
        centroidOriginX  = get_value_from_map<float>(_mapSetup, "TTCENTROIDORIGX");
    }
    catch(const std::invalid_argument& ex)
    {
        centroidOriginX = sud::centroidOriginX[4];
        W_LLOG("setting default " << sud::centroidOriginX[4]);
    }

    try
    {
        centroidOriginY = get_value_from_map<float>(_mapSetup, "TTCENTROIDORIGY");
    }
    catch(const std::invalid_argument& ex)
    {
        centroidOriginY = sud::centroidOriginY[4];
        W_LLOG("setting default " << sud::centroidOriginY[4]);
    }

    setup_diagnostic(_mapSetup);

    std::map<std::string, std::string> pid_setup;
    pid_setup["TTPIDKP"] = "";
    pid_setup["TTPIDKI"] = "";
    pid_setup["TTPIDKD"] = "";
    pid_setup["TTPIDTF"] = "";
    pid_setup["TTPIDTS"] = "";

    su::extract_setup(pid_setup, _mapSetup);

    setup_pid(pid_setup);

    init(dmOutputEnabled,
         ttLoopEnabled,
         enableRTCmds,
         pixelMinThreshold,
         diagEnabled,
         diagFrameDec,
         minCentroidValue,
         maxCentroidValue,
         minTTCommandValue,
         maxTTCommandValue,
         centroidGainX,
         centroidGainY,
         centroidOriginX,
         centroidOriginY);

    setupGain(_mapSetup);

    update_n_slopes();

    U8_LLOG("");
}


int rtcSlopeComputer::setup_pid(const std::map<std::string, std::string> & pid_setup)
{
    U6_LLOG(__FUNCTION__);
    int status = EXIT_FAILURE;

    std::map<std::string, std::string>::const_iterator it;
    int counter = 0;
    for(it = pid_setup.begin(); it != pid_setup.end(); it++)
        if(it->second.empty())break;
        else counter++;
    if(counter == 5)
    {
        float Kp, Ki, Kd, Tf, Ts;
        try
        {
            Kp = get_value_from_map<float>(pid_setup, "TTPIDKP");
            Ki = get_value_from_map<float>(pid_setup, "TTPIDKI");
            Kd = get_value_from_map<float>(pid_setup, "TTPIDKD");
            Tf = get_value_from_map<float>(pid_setup, "TTPIDTF");
            Ts = get_value_from_map<float>(pid_setup, "TTPIDTS");

            m_Kp = Kp;
            m_Ki = Ki;
            m_Kd = Kd;
            m_Tf = Tf;
            m_Ts = Ts;
            m_PidPars.clear();
            m_PidPars.push_back(Kp);
            m_PidPars.push_back(Ki);
            m_PidPars.push_back(Kd);
            m_PidPars.push_back(Tf);
            m_PidPars.push_back(Ts);
            set_PID_filter_coeff(Kp, Ki, Kd, Tf, Ts);
            status = EXIT_SUCCESS;
        }
        catch(const std::invalid_argument & err)
        {
            W_LLOG(err.what());
        }
        catch(const su::BadConversion & err)
        {
            W_LLOG(err.what());
            throw err;
        }
    }
    U8_LLOG("");
    return status;
}


int rtcSlopeComputer::setupGain(const std::map<std::string, std::string> & _gainSetup)
{
    U6_LLOG(__FUNCTION__);
    int status = EXIT_SUCCESS;
    bool b_gain_file, b_gain_mode, b_gain_radius, b_gain_amplitude;
    std::map<std::string, std::string>::const_iterator itFind;
    itFind = _gainSetup.find("TTPIXELGAINMODE");
    b_gain_mode = itFind != _gainSetup.end();
    if(b_gain_mode)
    {
        if(itFind->second == "FLATGAIN")
        {
            m_gainMode = RTC_FLAT_GAIN;
        }
        else if(itFind->second == "POINTGAIN")
        {
            m_gainMode = RTC_POINTLIKE_GAIN;
        }
        else if(itFind->second == "GAUSSGAIN")
        {
            m_gainMode = RTC_GAUSS_GAIN;
        }
        else if(itFind->second == "FILEGAIN")
        {
            m_gainMode = RTC_FILE_GAIN;
        }
        else
        {
            m_gainMode = RTC_UNDEFINED_GAIN;
            std::stringstream log_msg;
            log_msg << "Error : undefined Pixel Gain received \"" << itFind->second << "\"";
            E_LLOG(log_msg.str());
            throw(std::invalid_argument(log_msg.str()));
        }
    }
    switch(m_gainMode)
    {
        case RTC_FLAT_GAIN:
        case RTC_POINTLIKE_GAIN:
        {
            int winrows = m_det.get_num_rows();
            int wincols = m_det.get_num_cols();
            float cx = -(m_centroidOrigin[0]*wincols/2 - wincols/2);
            float cy = m_centroidOrigin[1]*winrows/2 + winrows/2;
            float r = 10001.f;
            float amplitude = 1.0f;
            if(m_gainMode == RTC_POINTLIKE_GAIN)
            {
                itFind = _gainSetup.find("TTPIXELGAINRADIUS");
                b_gain_radius = itFind != _gainSetup.end();
                if(b_gain_radius)
                    r = su::convertStringToType<float>(itFind->second);
                else
                {
                    std::stringstream log_msg;
                    log_msg << "Error : missing pixel gain radius";
                    E_LLOG(log_msg.str());
                    throw(std::runtime_error(log_msg.str()));
                }
                itFind = _gainSetup.find("TTPIXELGAINAMPLITUDE");
                b_gain_amplitude = itFind != _gainSetup.end();
                if(b_gain_amplitude)
                {
                    amplitude = convertStringToType<float>(itFind->second);
                }
            }
            generate_pointlike_gain(wincols, winrows, cx, cy, r, amplitude);
            break;
        }
        case RTC_GAUSS_GAIN:
        {
            int winrows = m_det.get_num_rows();
            int wincols = m_det.get_num_cols();
            float cx = -(m_centroidOrigin[0]*wincols/2 - wincols/2);
            float cy = m_centroidOrigin[1]*winrows/2 + winrows/2;
            float r = 10001.f;
            itFind = _gainSetup.find("TTPIXELGAINRADIUS");
            b_gain_radius = itFind != _gainSetup.end();
            if(b_gain_radius)
                r = su::convertStringToType<float>(itFind->second);
            else
            {
                std::stringstream log_msg;
                log_msg << "Error : missing pixel gain radius";
                E_LLOG(log_msg.str());
                throw(std::runtime_error(log_msg.str()));
            }
            generate_gauss_gain(wincols, winrows, cx, cy, r);
            break;
        }
        case RTC_FILE_GAIN:
        {
            itFind = _gainSetup.find("TTPIXELGAINFILE");
            b_gain_file = itFind != _gainSetup.end();
            if(b_gain_file)
            {
                init_gain(itFind->second);
            }
            else
            {
                std::stringstream log_msg;
                log_msg << "Error : missing PIXEL GAIN file name";
                E_LLOG(log_msg.str());
                throw(std::runtime_error(log_msg.str()));
            }

        }
        break;
        case RTC_UNDEFINED_GAIN:
        {
            std::stringstream log_msg;
            log_msg << "Error : gain mode is undefined";
            E_LLOG(log_msg.str());
            throw(std::runtime_error(log_msg.str()));
        }
        break;
    }
    U8_LLOG("");
    return status;
}


int rtcSlopeComputer::setup_diagnostic(std::map<std::string, std::string> _diagSetup)
{
    U6_LLOG(__FUNCTION__);
    int result = EXIT_SUCCESS;
    unsigned int meno_uno = 0xFFFFFFFF;

    unsigned int decimationFactor = meno_uno;
    unsigned int pixelDecimation = meno_uno;

    std::string diagIpv4 = "";
    std::string diagMac = "";
    int diagPort = -1;
    unsigned int masterDecimation = meno_uno;
    bool bdiagEnabled = true, bpixelEnabled = true, bjumboEnabled = false, bmasterEnabled = true;

    std::map<std::string, std::string>::const_iterator itFind;

    try
    {
        if(su::get_bool_from_setup(_diagSetup, "TTDIAGENABLED", bdiagEnabled))
            m_diagEnabled = bdiagEnabled;
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }

    if((itFind = _diagSetup.find("TTDIAGDECIMATION")) != _diagSetup.end())
    {
        decimationFactor = su::convertStringToType<int>(itFind->second);
        if(!(decimationFactor >= 0 && decimationFactor < 65536))
        {
            E_LLOG(__FUNCTION__);
            U8_LLOG("");
            throw(std::invalid_argument("Invalid TTDIAGDECIMATION value"));
        }
        m_diagDecimation = decimationFactor;
    }

    try
    {
        if(su::get_bool_from_setup(_diagSetup, "TTPIXELENABLED", bpixelEnabled))
            m_pixelSaveEnabled = bpixelEnabled;
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }

    if((itFind = _diagSetup.find("TTPIXELDECIMATION")) != _diagSetup.end())
    {
        pixelDecimation = su::convertStringToType<int>(itFind->second);
        if(!(pixelDecimation >= 0 && pixelDecimation < 65536))
        {
            E_LLOG(__FUNCTION__);
            U8_LLOG("");
            throw(std::invalid_argument("Invalid TTPIXELDECIMATION value"));
        }
        m_pixelSaveDecimation = pixelDecimation;
    }

    if((itFind = _diagSetup.find("TTDIAGRECORDIPV4")) != _diagSetup.end())
    {
        diagIpv4 = itFind->second;
    }

    if((itFind = _diagSetup.find("TTDIAGRECORDMAC")) != _diagSetup.end())
    {
        diagMac = itFind->second;
    }

    if((itFind = _diagSetup.find("TTDIAGRECORDPORT")) != _diagSetup.end())
    {
        int diag_port = su::convertStringToType<int>(itFind->second);
        if(!(diag_port > 0 && diag_port < 65536))
        {
            E_LLOG(__FUNCTION__);
            U8_LLOG("");
            throw(std::invalid_argument("Invalid TTDIAGRECORDPORT value"));
        }
        else
            diagPort = diag_port;
    }
    if(!diagIpv4.empty() && !diagMac.empty() && diagPort != -1)
    {
        set_diagnostic_address(diagIpv4, diagMac, diagPort);
    }

    Nice::SeqInt diagAddress = getDiagnosticRemoteAddress(diagMac, diagIpv4, diagPort);

    if((itFind = _diagSetup.find("TTMASTERDECIMATION")) != _diagSetup.end())
    {
        masterDecimation = su::convertStringToType<int>(itFind->second);
        if(!(masterDecimation >= 0 && pixelDecimation < 65536))
        {
            E_LLOG(__FUNCTION__);
            U8_LLOG("");
            throw(std::invalid_argument("Invalid TTMASTERDECIMATION value"));
        }
        m_masterDecimation = masterDecimation;
    }

    try
    {
        if(su::get_bool_from_setup(_diagSetup, "TTJUMBOFRAMESENABLED", bjumboEnabled))
        {
            set_jumbo_enabled(bjumboEnabled);
        }
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }

    try
    {
        if(su::get_bool_from_setup(_diagSetup, "TTMASTERDECENABLED", bmasterEnabled))
            m_masterEnabled = bmasterEnabled;
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }

    U8_LLOG("");
    return result;
}


int rtcSlopeComputer::update_time_history_gain(const std::map<std::string, std::string> & _execSetup)
{
    float gain = su::convertStringToType<float>(_execSetup.find("TTTIMEHISTORYGAIN")->second);
    if(gain > 0.f && gain < 0.20f)
        m_TimeHistoryGain = Nice::SeqFloat(2, gain);
    else
    {
        std::stringstream log_msg;
        log_msg << "Error : Time History gain outside bounds (0, 0.2) ; TTTIMEHISTORYGAIN = " << gain;
        E_LLOG(log_msg.str());
        throw std::invalid_argument(log_msg.str());
    }
    return EXIT_SUCCESS;
}


void rtcSlopeComputer::set_jumbo_enabled(bool enabled)
{
    if(enabled)
    {
        W_LLOG("Warning jumbo enabled not supported! Defaulting to false");
    }
    m_jumboEnabled = false;
}

void rtcSlopeComputer::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)
{
    U6_LLOG(__FUNCTION__);
    std::vector<float> timeHistoryGain(2, 0.0255771428571428571);//3.73/21*1.2);

    m_centroidGain      = Nice::SeqFloat(2,0);
    m_centroidGain[0] = _centroidGainX;
    m_centroidGain[1] = _centroidGainY;

    m_centroidOrigin    = Nice::SeqFloat(2,0);
    m_centroidOrigin[0] = _centroidOriginX;
    m_centroidOrigin[1] = _centroidOriginY;

    m_FramesCounter        = Nice::SeqInt(1,0);
    m_FramesCounterIrq     = Nice::SeqInt(1,0);
    m_ParamSelector        = Nice::SeqInt(1,0);
    m_ParamSelectorIrq     = Nice::SeqInt(1,0);
    m_ResetLoop            = Nice::SeqInt(1,0);
    m_TTControlLoopEnabled = Nice::SeqInt(1,_ttLoopEnabled);
    m_enableRTCommands     = Nice::SeqInt(1,_enableRTCmds);

    /*  DM related */
    m_DMBitOffset           = std::vector<float>(1,1.f);
    m_DMActuatorMapPtr      = std::vector<int>(1,0);
    m_DMActuatorSignPtr     = std::vector<int>(1,0);
    m_DMActuatorsIrq        = std::vector<int>(1,0);
    m_DMBiasVectorPtr       = std::vector<int>(1,0);
    m_DMCmdSkippedCnt       = std::vector<int>(1,0);
    m_DMCommandVectPtr      = std::vector<int>(1,0);
    m_DMInfluenceMatrix0Ptr = std::vector<int>(1,0);
    m_DMInfluenceMatrix1Ptr = std::vector<int>(1,0);
    m_DMNumActuatorsIrq     = std::vector<int>(1,0);
    m_DMOutputEnable        = std::vector<int>(1,_dmOutputEnabled);
    m_DMOutputVector        = std::vector<float>(256,0.f);
    m_DMTmpOutputVectorPtr  = std::vector<int>(1,0);
    m_DMTotalPower          = std::vector<float>(1,0.f);
    m_DMUpdateStart         = std::vector<int>(1,0);
    m_DMm2bitConversion     = std::vector<float>(1, 8191.f);
    m_MaxAbsDMCommand       = std::vector<float>(1,1.f);
    m_MaxAbsDMCommandSkip   = std::vector<int>(1,0);
    m_MaxTTCommandValue     = std::vector<float>(1,_maxTTCommandValue);
    m_MinTTCommandValue     = std::vector<float>(1,_minTTCommandValue);
    m_ModeVector0Ptr        = std::vector<int>(1,0);
    m_ModeVector1Ptr        = std::vector<int>(1,0);
    m_NewModeVectorPtr      = std::vector<int>(1,0);

    m_HSDLVectorLen         = std::vector<int>(1, 0);
    m_HSDLCmdStart          = std::vector<int>(1, 0);
    m_HSDLCmdNotReadyCnt    = std::vector<int>(1, 0);
    m_HSDLCommandScale      = std::vector<float>(1, 0.f);
    m_HSDLToSkipFrameCnt    = std::vector<int>(1, 0);
    m_HSDLInputVectorPtr    = std::vector<int>(1, 61440);
    m_HSDLReplyVectorPtr    = std::vector<int>(1, 61440);
    m_HSDLInputVector       = std::vector<int>();

    /*  detector related   */
    m_PIDDelayLine         = Nice::SeqFloat(12,0);
    m_PIDFilterCoeff       = Nice::SeqFloat(0,0);
    m_centroidOut          = Nice::SeqFloat(2,0);
    m_pixelMinThreshold    = Nice::SeqFloat(1,_pixelMinThreshold);// ML -> PixelMinValue
    m_CCDInterfaceEnable   = Nice::SeqInt(1,0);
    m_CCDSubFrameCnt       = Nice::SeqInt(1,0);
    m_CCDTotalFlux         = Nice::SeqFloat(1,0.f);
    m_CCDWaitForReset      = Nice::SeqInt(1,0);
    m_CentroidFactPtr      = Nice::SeqInt(1,0);
    m_CentroidPixelAreaPtr = Nice::SeqInt(1,0);
    m_MaxCentroidValue     = Nice::SeqFloat(1,_maxCentroidValue);
    m_MinCentroidValue     = Nice::SeqFloat(1,_minCentroidValue);
    m_NumPixelsToDoPtr     = Nice::SeqInt(1,0);
    m_NumPixelsToDo_A      = Nice::SeqInt(4,0);
    m_NumPixelsToDo_B      = Nice::SeqInt(4,0);
    m_PixelGainPtr         = Nice::SeqInt(1,0);
    m_PixelOffsetPtr       = Nice::SeqInt(1,0);
    m_PreviousPixelAreaPtr = Nice::SeqInt(1,0);
    m_SubapIntensityThres  = Nice::SeqFloat(1,0);
    m_TimeHistoryAct       = Nice::SeqInt(1,0);
    m_TimeHistoryEndPtr    = Nice::SeqInt(1,0);
    m_TimeHistoryPtr       = Nice::SeqInt(1,0);
    m_TimeHistoryGain      = Nice::SeqFloat(2,0);

    /* Diagnostic buffer   */
    m_DiagBuffDataPtr      = Nice::SeqInt(1,0);
    m_DiagBuffFooterPtr    = Nice::SeqInt(1,0);
    m_DiagBuffHeaderPtr    = Nice::SeqInt(1,0);

    m_NewModeVectorPtr[0] = static_cast<int>(Sharknir::Bcu::BANK_0 + 0x0000F000);
    m_NumPixelsToDoPtr[0] = Sharknir::Bcu::BANK_1;
    m_PixelOffsetPtr[0] = Sharknir::Bcu::BANK_2;
    m_CentroidPixelAreaPtr[0] = m_NumPixelsToDoPtr[0] + 4;

    m_diagDecimation = _diagFrameDec;
    m_diagEnabled = _diagEnabled;

    m_TimeHistoryPtr[0]        = Sharknir::Bcu::TimeHistory; // Sharknir::Bcu::SaveRegs + (SaveRegsSize = 24);
    m_TimeHistoryEndPtr[0]     = m_TimeHistoryPtr[0] + m_MaxTimeHistoryLen;

    // setting time history gain
    m_TimeHistoryGain = timeHistoryGain;

    try
    {
       set_PID_filter_coeff(m_det.get_frame_rate());
    }
    catch(const std::invalid_argument& ex)
    {
        E_LLOG(ex.what());
    }

    U8_LLOG(__FUNCTION__ << " ~ DONE");
}

rtcSlopeComputer::~rtcSlopeComputer()
{
    U6_LLOG(__FUNCTION__);
    U8_LLOG("");
}


void rtcSlopeComputer::generate_pointlike_gain(int sizex, int sizey, float cx, float cy, float radius, float amplitude)
{
    U6_LLOG(__FUNCTION__);
    float point_gain = amplitude; // 1.f;
    if(radius > 10000.f)point_gain = 1.f;
    int i, j;
    float dx, dy;
    float rd = radius*radius + 0.001f;
    m_pixelGain.resize(m_det.get_slope_pixels_number());
    float CX = ((int)(cx*2))/2.f;
    float CY = ((int)(cy*2))/2.f;
    U9_LLOG("PointLike Gain : (" << sizex << ", " << sizey << ", " << CX << ", " << CY << ")");
    if(sizex*sizey == m_det.get_slope_pixels_number())
    {
        for(i = 0; i < sizey; i++)
        {
            for(j = 0; j < sizex; j++)
            {
                dx = CX - j - 0.5f;
                dy = CY - i - 0.5f;
                if(dx*dx + dy*dy < rd)
                {
                    m_pixelGain[i*sizex + j] = point_gain;
                //    U9_LLOG("{i, j} : {" << i << ", " << j << "}");
                }
                else
                    m_pixelGain[i*sizex + j] = 0;
            }
        }
        m_pixelGain[0] = m_pixelGain[1] = m_pixelGain[2] = m_pixelGain[3] = 0.f;
        m_pixelGain[sizex*sizey - 4] = m_pixelGain[sizex*sizey - 3] = m_pixelGain[sizex*sizey - 2] = m_pixelGain[sizex*sizey - 1] = 0.f;
        for(i = 0; i < 5; i++)
        {
            Nice::SeqFloat pixelGain;
            for(j = 0; j < 10; j++)
            {
               pixelGain.push_back(m_pixelGain[i*sizex + j]);
            }
        //    U9_LLOG("PixelGain : " << su::seq_float_to_str(pixelGain));
        }
    }
    else
    {
        std::stringstream log_msg;
        log_msg << "Error: size not conformant. Expected " << m_det.get_slope_pixels_number() << " but received " << sizex*sizey;
        E_LLOG(log_msg.str());
        U8_LLOG("");
        throw (std::invalid_argument(log_msg.str()));
    }
    U8_LLOG("");
}


void rtcSlopeComputer::generate_gauss_gain(int sizex, int sizey, float cx, float cy, float radius)
{
    U6_LLOG(__FUNCTION__);
    float point_gain = 5.f;
    int i, j;
    float dx, dy;
    float rd = radius*radius;
    double pi = 3.14159265358979;
    m_pixelGain.resize(m_det.get_slope_pixels_number());
    U9_LLOG("Gauss Gain : (" << sizex << ", " << sizey << ", " << cx << ", " << cy << ")");
    float max_gain = 0.f, gain;
    if(sizex*sizey == m_det.get_slope_pixels_number())
    {
        for(i = 0; i < sizey; i++)
        {
            for(j = 0; j < sizex; j++)
            {
                dx = cx - j - 0.5f;
                dy = cy - i - 0.5f;
                gain = point_gain*exp(-(dx*dx + dy*dy)/(2*radius*radius));
                if(gain > max_gain)max_gain = gain;
                m_pixelGain[i*sizex + j] = gain;
            }
        }
        m_pixelGain[0] = m_pixelGain[1] = m_pixelGain[2] = m_pixelGain[3] = 0.f;
        m_pixelGain[sizex*sizey - 4] = m_pixelGain[sizex*sizey - 3] = m_pixelGain[sizex*sizey - 2] = m_pixelGain[sizex*sizey - 1] = 0.f;
    }
    else
    {
        std::stringstream log_msg;
        log_msg << "Error: size not conformant. Expected " << m_det.get_slope_pixels_number() << "values but found " << sizex*sizey;
        E_LLOG(log_msg.str());
        U8_LLOG("");
        throw (std::invalid_argument(log_msg.str()));
    }
    U8_LLOG(__FUNCTION__ << " ~ Max Gain = " << max_gain);
}


int rtcSlopeComputer::init_gain(const std::string & file_name)
{
    U6_LLOG(__FUNCTION__);
    int result = EXIT_SUCCESS;
//    m_pixelGain.resize(_nSlopePixels, 0.f);
    std::string line;
    int i = 0;
    std::ifstream ifstrm;
    std::istringstream isstream;
    int read_data = 0;
    float read_value;
    std::string TIPTILT_CONFIG = su::get_env("INSROOT")+"/etc/tiptilt_ctrl/rtc/";
    m_strPixelGainFile  = TIPTILT_CONFIG + "gains/" + file_name;
    if(!file_name.empty() && boost::filesystem::exists(boost::filesystem::path(m_strPixelGainFile)))
    {
        ifstrm.open(m_strPixelGainFile.c_str());
        while (std::getline(ifstrm, line))
        {
            isstream.clear();
            isstream.str(line);
            isstream >> read_value;
            if(read_data < m_det.get_slope_pixels_number())
            {
                m_pixelGain[i] = read_value;
            }
            if(isstream.fail())
            {
                E_LLOG("Failed reading line " << i + 1 << " from " << m_strPixelGainFile << "; read_data = " << read_data);
                break;
            }
            else
                read_data++;
            i++;
            line = "";
        }
        ifstrm.close();
        if(read_data != m_det.get_slope_pixels_number())
        {
            std::stringstream log_msg;
            log_msg << "Error: Pixel Gain file \"" << file_name << "\" not conformant. Expected " << m_det.get_slope_pixels_number() << "values but found " << read_data;
            E_LLOG(log_msg.str());
            U8_LLOG("");
            throw (std::invalid_argument(log_msg.str()));
        }
        m_pixelGain[0] = m_pixelGain[1] = m_pixelGain[2] = m_pixelGain[3] = 0.f;
        m_pixelGain[read_data - 4] = m_pixelGain[read_data - 3] = m_pixelGain[read_data - 2] = m_pixelGain[read_data - 1] = 0.f;
    }
    else if(!file_name.empty())
    {
        E_LLOG("File " << m_strPixelGainFile << " does not exists");
        U8_LLOG("");
        throw (std::invalid_argument("File " + m_strPixelGainFile + " does not exists"));
    }
    U8_LLOG("");
    return result;
}


void rtcSlopeComputer::set_PID_filter_coeff(float Kp, float Ki, float Kd, float Tf, float Ts)
{
    U6_LLOG(__FUNCTION__ << " ~ Kp : " << Kp << "; Ki : " << Ki << "; Kd : " << Kd << "; Tf : " << Tf << "; Ta : "  << Ts);

    float Num2 = (Kd + Kp*Tf)/Tf;
    float Num1 = (-2*Kd -2*Kp*Tf + Kp*Ts + Ki*Tf*Ts)/Tf;
    float Num0 = (Kd + Kp*Tf - Kp*Ts - Ki*Tf*Ts + Ki*Ts*Ts)/Tf;
//  float Den2 = - Tf/Tf;
    float Den1 = - (-2*Tf + Ts)/Tf;
    float Den0 = - (Tf - Ts)/Tf;

    m_PIDFilterCoeff.clear();
    m_PIDFilterCoeff.push_back(Num2);
    m_PIDFilterCoeff.push_back(Num2);
    m_PIDFilterCoeff.push_back(Den0);
    m_PIDFilterCoeff.push_back(Den0);
    m_PIDFilterCoeff.push_back(Num0);
    m_PIDFilterCoeff.push_back(Num0);
    m_PIDFilterCoeff.push_back(Den1);
    m_PIDFilterCoeff.push_back(Den1);
    m_PIDFilterCoeff.push_back(Num1);
    m_PIDFilterCoeff.push_back(Num1);

    // pid_coeff([1 1 6 6 3 3 5 5 2 2])
    U8_LLOG(__FUNCTION__ << " : " << su::seq_float_to_str(m_PIDFilterCoeff));
}


void rtcSlopeComputer::set_PID_filter_coeff(const int& _frameRate)
{
    U6_LLOG(__FUNCTION__);
    m_PIDFilterCoeff.clear();
    int winSize = m_det.get_window_size();
    U3_LLOG("Frame rate is set to " << _frameRate << " Hz");
    if(_frameRate == 200)
    {
        m_PIDFilterCoeff.push_back(0.5);
        m_PIDFilterCoeff.push_back(0.5);
        m_PIDFilterCoeff.push_back(0.);
        m_PIDFilterCoeff.push_back(0.);
        m_PIDFilterCoeff.push_back(0.);
        m_PIDFilterCoeff.push_back(0.);
        m_PIDFilterCoeff.push_back(1.);
        m_PIDFilterCoeff.push_back(1.);
        m_PIDFilterCoeff.push_back(0.);
        m_PIDFilterCoeff.push_back(0.);
    }
    else if(_frameRate == 250)
    {
        m_PIDFilterCoeff.push_back(0.25);
        m_PIDFilterCoeff.push_back(0.25);
        m_PIDFilterCoeff.push_back(0.);
        m_PIDFilterCoeff.push_back(0.);
        m_PIDFilterCoeff.push_back(0.);
        m_PIDFilterCoeff.push_back(0.);
        m_PIDFilterCoeff.push_back(1.);
        m_PIDFilterCoeff.push_back(1.);
        m_PIDFilterCoeff.push_back(-0.15);
        m_PIDFilterCoeff.push_back(-0.15);
    }
    else if(_frameRate == 500)
    {
        m_PIDFilterCoeff.push_back(1);
        m_PIDFilterCoeff.push_back(1);
        m_PIDFilterCoeff.push_back(0);
        m_PIDFilterCoeff.push_back(0);
        m_PIDFilterCoeff.push_back(0);
        m_PIDFilterCoeff.push_back(0);
        m_PIDFilterCoeff.push_back(1);
        m_PIDFilterCoeff.push_back(1);
        m_PIDFilterCoeff.push_back(0);
        m_PIDFilterCoeff.push_back(0);
    }
    else if(_frameRate == 800)
    {
        m_PIDFilterCoeff.push_back(1.5);
        m_PIDFilterCoeff.push_back(1.5);
        m_PIDFilterCoeff.push_back(0.);
        m_PIDFilterCoeff.push_back(0.);
        m_PIDFilterCoeff.push_back(0.);
        m_PIDFilterCoeff.push_back(0.);
        m_PIDFilterCoeff.push_back(1.);
        m_PIDFilterCoeff.push_back(1.);
        m_PIDFilterCoeff.push_back(-0.25);
        m_PIDFilterCoeff.push_back(-0.25);
    }
    else if((_frameRate == 1000) && winSize == (86*94))
    {
        m_PIDFilterCoeff.push_back(1.4);
        m_PIDFilterCoeff.push_back(1.4);
        m_PIDFilterCoeff.push_back(0.);
        m_PIDFilterCoeff.push_back(0.);
        m_PIDFilterCoeff.push_back(0.);
        m_PIDFilterCoeff.push_back(0.);
        m_PIDFilterCoeff.push_back(1.);
        m_PIDFilterCoeff.push_back(1.);
        m_PIDFilterCoeff.push_back(0.);
        m_PIDFilterCoeff.push_back(0.);
    }
    else if((_frameRate == 1000) && winSize == (64*64))
    {
        m_PIDFilterCoeff.push_back(1.f);
        m_PIDFilterCoeff.push_back(1.f);
        m_PIDFilterCoeff.push_back(0.f);
        m_PIDFilterCoeff.push_back(0.f);
        m_PIDFilterCoeff.push_back(0.f);
        m_PIDFilterCoeff.push_back(0.f);
        m_PIDFilterCoeff.push_back(1.f);
        m_PIDFilterCoeff.push_back(1.f);
        m_PIDFilterCoeff.push_back(0.f);
        m_PIDFilterCoeff.push_back(0.f);
    }
    else if((_frameRate == 2000) && winSize == (86*94))
    {
        m_PIDFilterCoeff.push_back(1.6);
        m_PIDFilterCoeff.push_back(1.6);
        m_PIDFilterCoeff.push_back(0.);
        m_PIDFilterCoeff.push_back(0.);
        m_PIDFilterCoeff.push_back(0.);
        m_PIDFilterCoeff.push_back(0.);
        m_PIDFilterCoeff.push_back(1.);
        m_PIDFilterCoeff.push_back(1.);
        m_PIDFilterCoeff.push_back(-0.8);
        m_PIDFilterCoeff.push_back(-0.8);
    }
    else if((_frameRate == 2000) && winSize == (64*64))
    {
        m_PIDFilterCoeff.push_back(2.);
        m_PIDFilterCoeff.push_back(2.);
        m_PIDFilterCoeff.push_back(0.);
        m_PIDFilterCoeff.push_back(0.);
        m_PIDFilterCoeff.push_back(0.);
        m_PIDFilterCoeff.push_back(0.);
        m_PIDFilterCoeff.push_back(1.);
        m_PIDFilterCoeff.push_back(1.);
        m_PIDFilterCoeff.push_back(-1.);
        m_PIDFilterCoeff.push_back(-1.);
    }
    else
    {
        W_LLOG("Bad PID assignment");
        m_PIDFilterCoeff.push_back(2.);
        m_PIDFilterCoeff.push_back(2.);
        m_PIDFilterCoeff.push_back(0.);
        m_PIDFilterCoeff.push_back(0.);
        m_PIDFilterCoeff.push_back(0.);
        m_PIDFilterCoeff.push_back(0.);
        m_PIDFilterCoeff.push_back(1.);
        m_PIDFilterCoeff.push_back(1.);
        m_PIDFilterCoeff.push_back(-1.);
        m_PIDFilterCoeff.push_back(-1.);
    }
    U8_LLOG("");
}

void rtcSlopeComputer::set_time_history_gain(const std::vector<float>& _timeHistGain)
{
    U6_LLOG(__FUNCTION__);
    if(_timeHistGain.size() != 2)
    {
        U8_LLOG("");
        throw (std::invalid_argument("Wrong size for time history gain vector"));
    }
    m_TimeHistoryGain = _timeHistGain;
    U8_LLOG("");
}


void rtcSlopeComputer::update_n_slopes()
{
    U6_LLOG(__FUNCTION__);
    int DMInUseActNum     = m_dm.get_in_use_actuator_number();
    int ModesVectorLen = m_dm.get_length_of_modes_vector();

    int n_slopes = m_det.get_slope_pixels_number();
    /* Setting pointers value */
    m_DiagBuffHeaderPtr[0]     = m_CentroidPixelAreaPtr[0] + n_slopes;
    m_DMCommandVectPtr[0]      = m_DiagBuffHeaderPtr[0] + 4;
    m_DiagBuffDataPtr[0]       = m_DMCommandVectPtr[0] + DMInUseActNum;
    m_DiagBuffFooterPtr[0]     = m_DiagBuffDataPtr[0] + 8;
    m_ModeVector0Ptr[0]        = m_DiagBuffFooterPtr[0] + 4;
    m_DMInfluenceMatrix0Ptr[0] = m_ModeVector0Ptr[0] + ModesVectorLen;
    m_PixelGainPtr[0]          = m_PixelOffsetPtr[0] + n_slopes;
    m_CentroidFactPtr[0]       = m_PixelGainPtr[0] + n_slopes;
    m_PreviousPixelAreaPtr[0]  = m_CentroidFactPtr[0] + 2*n_slopes;
    m_ModeVector1Ptr[0]        = m_PreviousPixelAreaPtr[0] + n_slopes;
    m_DMInfluenceMatrix1Ptr[0] = m_ModeVector1Ptr[0] + ModesVectorLen;
    m_DMBiasVectorPtr[0]       = m_DMInfluenceMatrix1Ptr[0] +  ModesVectorLen*DMInUseActNum/2;
    m_DMActuatorSignPtr[0]     = m_DMBiasVectorPtr[0] + DMInUseActNum;
    m_DMTmpOutputVectorPtr[0]  = m_DMActuatorSignPtr[0] + DMInUseActNum;
    m_DMActuatorMapPtr[0]      = m_DMTmpOutputVectorPtr[0] + DMInUseActNum;

    m_DMTmpOutputVector.resize(m_DMTmpOutputVectorPtr[0] - m_DMActuatorSignPtr[0], 0);

    int subap = m_det.get_window_size();
    m_SubapIntensityThres[0] = 1.0f;

    m_pixelGain.resize(n_slopes, 0.f);

    gen_centroid_params(m_centroidFact);
    U8_LLOG("");
}


void rtcSlopeComputer::set_centroid_origin(const Nice::SeqFloat& _centroidOrigin)
{
    if(fabs(_centroidOrigin[0]) > 1.f || fabs(_centroidOrigin[1]) > 1.f)
    {
        std::stringstream log_msg;
        log_msg << "Error: Centroid command outside nominal bounds";
        E_LLOG(log_msg.str());
        throw(std::invalid_argument(log_msg.str()));
    }

    m_centroidOrigin = _centroidOrigin;
}


void rtcSlopeComputer::gen_centroid_params(Nice::SeqFloat & centroid_fact) const
{
    U6_LLOG(__FUNCTION__);
    int num_rows = m_det.get_num_rows();
    int num_cols = m_det.get_num_cols();
    int num_slopes = m_det.get_slope_pixels_number();
    U9_LLOG("m_winRows = " << num_rows << "; m_winCols = " << num_cols);
    centroid_fact.resize(2*num_slopes, 0);
    U9_LLOG(__FUNCTION__ << " ~ " << num_slopes << " ~ " << num_rows << " ~ " << num_cols);

    int hCols = num_cols/2;//m_winCols is even
    int hRows = num_rows/2;//m_winRows is even
    float rows_factor = 1.f/(hRows - 0.5f);
    float cols_factor = 1.f/(hCols - 0.5f);
    int set_counter = 0;

    for(int i = 0; i < num_rows; i++)
    {
        for(int j = 0; j < num_cols; j++)
        {
            centroid_fact[2*j + (i*num_cols)*2] = (j - hCols + 0.5f)*cols_factor; // x Weight
            centroid_fact[2*j + (i*num_cols)*2 + 1] = -(i - hRows + 0.5f)*rows_factor; // y Weight
            set_counter++;
            if(set_counter == 2*num_slopes)break;
        }
        if(set_counter == 2*num_slopes)break;
    }
    for(int i = 0; i < 8; i++)centroid_fact[i] = 0;
    U8_LLOG("");
}


Nice::SeqInt rtcSlopeComputer::getDiagnosticRemoteAddress(const std::string MAC, const std::string & IP, unsigned short int Port)
{
    Nice::SeqInt diagRemoteAddr(3,0);

    unsigned char remoteAddr[12];
    int octet[6];
    char c;
    std::istringstream ip(IP);
    for(int i = 0; i < 4; i++)
    {
        if(ip.fail() || ip.eof())
        {
            std::stringstream log_msg;
            log_msg << "Error: invalid IP string : " << IP << " ~ " << i;
            E_LLOG(log_msg.str());
            throw(std::invalid_argument(log_msg.str()));
        }
        ip >> octet[i] >> c;
        remoteAddr[i + 6] = (unsigned char)octet[i];
    }

    if(MAC.size() == 17)
    {
        std::istringstream mac(MAC);
        for(int i = 0; i < 6; i++)
        {
            if(mac.fail() || mac.eof())
            {
                std::stringstream log_msg;
                log_msg << "Error: invalid MAC string : " << MAC << " ~ " << i;
                E_LLOG(log_msg.str());
                throw(std::invalid_argument(log_msg.str()));
            }
            mac >> std::hex >> octet[i] >> c;
            remoteAddr[i] = (unsigned char)octet[i];
        }
    }
    else
    {
        E_LLOG(__FUNCTION__);
        throw(std::invalid_argument("Error: MAC address must be a string of 6 hexadecimal value separated by one character"));
    }

    remoteAddr[10] = (unsigned char)(Port & 0xff);
    remoteAddr[11] = (unsigned char)(Port >> 8);
    std::memcpy(&*diagRemoteAddr.begin(), remoteAddr, diagRemoteAddr.size()*sizeof(int));
    return diagRemoteAddr;
}

void rtcSlopeComputer::apply_mode_vector(const Nice::SeqFloat & modes, bool absolute)
{
    Nice::SeqFloat FullModeVector = m_modeVector;
    U9_LLOG("modes.size() : " << modes.size() << "  ~  m_modeVector.size() : " << m_modeVector.size());
    if(modes.size() <= m_modeVector.size())
    {
        //Check
        {
            if(absolute)
            {
                std::copy(modes.begin(), modes.end(), FullModeVector.begin());
            }
            else
            {
                Nice::SeqFloat::const_iterator itRelative;
                Nice::SeqFloat::iterator itModes = FullModeVector.begin();
                for(itRelative = modes.begin(); itRelative < modes.end(); itRelative++)
                {
                    *itModes = *itModes + *itRelative;
                    itModes++;
                }
            }
            if(FullModeVector[0] != 0.f || FullModeVector[1] != 0.f)
            {
                std::stringstream log_msg;
                log_msg << "Error: First two modes are reserved for fast Tip/Tilt correction and must be 0";
                E_LLOG(log_msg.str());
                throw(std::invalid_argument(log_msg.str()));
            }

            bool bError = false, bError2 = false;
            int StartMode = 4;
            float PowerThreshold = 0.5f;
            if(m_dm.is_zonal_mode())
            {
                StartMode = 2;
                PowerThreshold = 0.15f;
            }
            else
            {
                float val_tip = 0.f, val_tilt = 0.f;
                val_tilt = FullModeVector[2];
                val_tip = FullModeVector[3];
                float tip_tilt_threshold = m_tiptilt_threshold;
                if(val_tilt*val_tilt + val_tip*val_tip > tip_tilt_threshold*tip_tilt_threshold)
                {
                    std::stringstream log_msg;
                    log_msg << "Error: Tip/Tilt command exceeding value of " << tip_tilt_threshold;
                    E_LLOG(log_msg.str());
                    throw(std::invalid_argument(log_msg.str()));
                }
            }
            for(unsigned int i = StartMode; i < m_modeVector.size() ; i++)
            {
                float val = fabs(FullModeVector[i]);
                float final_val = val;
                float LocalPowerThreshold = PowerThreshold;
                if(m_dm.is_zonal_mode())
                {
                    Nice::SeqFloat resizedDMFlat = m_dm.get_flat();
                    if(fabs(resizedDMFlat[i - 2]) > PowerThreshold)LocalPowerThreshold = fabs(resizedDMFlat[i - 2]);
                    final_val = fabs(FullModeVector[i] + resizedDMFlat[i - 2]);
                }
                if(final_val*m_dm.get_mode_weight(i) > LocalPowerThreshold)
                {
                    bError2 = true;
                }
                if(bError || bError2)
                {
                    std::stringstream log_msg;
                    log_msg << "Error: value " << FullModeVector[i] << " not allowed for mode " << i + 1;
                    if(bError2)log_msg << "; mode weight is : " << m_dm.get_mode_weight(i);
                    E_LLOG(log_msg.str());
                    throw(std::invalid_argument(log_msg.str()));
                }
            }
        }

        if(absolute)
        {
            std::copy(modes.begin(), modes.end(), m_modeVector.begin());
        }
        else
        {
            Nice::SeqFloat::const_iterator itRelative;
            Nice::SeqFloat::iterator itModes = m_modeVector.begin();
            for(itRelative = modes.begin(); itRelative < modes.end(); itRelative++)
            {
                *itModes = *itModes + *itRelative;
                itModes++;
            }
        }
    }
    else
    {
        std::stringstream log_msg;
        log_msg << "Error mode vector greater than max number of modes";
        E_LLOG(log_msg.str());
        throw(std::invalid_argument(log_msg.str()));
    }

}

} // util namespace
} // shins namespace
