// $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/src/tiptilt_ctrl/bcu/util/rtcDetector.cpp 3953 2022-05-19 08:34:04Z depa $
#include "rtcDetector.hpp"

namespace shins { // shins namespace
namespace util {   // util namespace



rtcDetector::rtcDetector(const std::map<std::string, std::string> & _mapSetup)
{
    U6_LLOG(__FUNCTION__);

    m_pixelInUseMask.resize(Sharknir::Bcu::OcamInUsePixelMapSize, 0);
    m_nSlopesToDoMap.resize(Sharknir::Bcu::OcamNumSlopeToDoSize, 0);
    m_pixelToSaveMap.resize(Sharknir::Bcu::OcamToSavePixelMapSize, 0);
    m_SlopePixelsMap.resize(Sharknir::Bcu::OcamSlopePixelsMapSize, 0);

    m_lastSnakeTemperature = -500.f;
    m_winCoordX = 0;
    m_winCoordY = 0;
    m_winCols = sud::cRedCols;
    m_winRows = sud::cRedRows;
    init(_mapSetup);

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

void rtcDetector::init(const std::map<std::string, std::string> & _mapSetup)
{
    U6_LLOG(__FUNCTION__);
    int nSlopePixels = sud::nSlopePixels;
    std::string strPixelBiasFile /*=""*/;
    std::string strPixelGainFile;
    std::map<std::string, std::string>::const_iterator itFind;
    std::stringstream log_msg;

    std::fill(m_pixelInUseMask.begin(), m_pixelInUseMask.end(), 0);
    std::fill(m_nSlopesToDoMap.begin(), m_nSlopesToDoMap.end(), 0);
    std::fill(m_pixelToSaveMap.begin(), m_pixelToSaveMap.end(), 0);
    std::fill(m_SlopePixelsMap.begin(), m_SlopePixelsMap.end(), 0);

    //////////////////////////
    // MANDATORY PARAMETERS //
    //////////////////////////

    int frame_rate = 0, n_slope_pixels = 0;
    float cam_integration_time = 0.f;
    std::string bias_file;

    bool b_cam_integration_time, b_bias_file;
    bool b_frame_rate, b_n_slope_pixels;

    std::map<std::string, std::string> ExecSetup = _mapSetup;
    std::map<std::string, std::string> extractSetup;

    std::vector<std::string> cropping;
    cropping.push_back("TTWINCOORDX");
    cropping.push_back("TTWINCOORDY");
    cropping.push_back("TTWINROWS");
    cropping.push_back("TTWINCOLS");
    extractSetup = shins::util::extract_setup(cropping, ExecSetup);
    set_cropping(extractSetup);

    itFind = _mapSetup.find("TTNSLOPEPIXELS");
    b_n_slope_pixels = itFind != _mapSetup.end();
    if(b_n_slope_pixels)n_slope_pixels = su::convertStringToType<int>(itFind->second);

    itFind = _mapSetup.find("TTBIASFILE");
    b_bias_file = itFind != _mapSetup.end();
    if(b_bias_file)bias_file = itFind->second;

    itFind = _mapSetup.find("TTFRAMERATE");
    b_frame_rate = itFind != _mapSetup.end();
    if(b_frame_rate)frame_rate = su::convertStringToType<int>(itFind->second);

    itFind = _mapSetup.find("TTCAMTINT");
    b_cam_integration_time = itFind != _mapSetup.end();
    if(b_cam_integration_time)cam_integration_time = su::convertStringToType<float>(itFind->second);

    if(!(b_cam_integration_time && b_frame_rate && b_bias_file && b_n_slope_pixels))
    {
        log_msg << __FUNCTION__ << " ~ Error : Missing mandatory parameter [TTNSLOPEPIXELS, TTBIASFILE, TTFRAMERATE, TTCAMTINT]";
        E_LLOG(log_msg.str());
        U8_LLOG("");
        throw(std::invalid_argument(log_msg.str()));
    }

    m_winSize = m_winRows*m_winCols;
    m_detectorFrameRate = frame_rate;
    m_camIntegrationTime = cam_integration_time;

    {
        if(m_winSize < Sharknir::Bcu::OcamSlopePixelsMapSize)
        {
            nSlopePixels = m_winSize;
        }
        if(b_n_slope_pixels)
        {
            if(n_slope_pixels <= nSlopePixels)
            {
                nSlopePixels = n_slope_pixels;
            }
            else
            {
                log_msg.str("Error: value of TTNSLOPEPIXELS cannot be greater than TTWINROWS*TTWINCOLS");
                E_LLOG(log_msg.str());
                U8_LLOG("");
                throw(std::invalid_argument(log_msg.str()));
            }
        }
        update_n_slopes(nSlopePixels);
    }

    if(b_bias_file)
    {
        init_bias(bias_file);// alias mttib_PixelOffset
    }

    set_irq_nios_delay();

    m_simulation = false;

    U9_LLOG(__FUNCTION__ << " ~ top_left_x = " << m_winCoordX << "; top_left_y = " << m_winCoordY << "; winRows = " << m_winRows << "; winCols = " << m_winCols);
    U9_LLOG(__FUNCTION__ << " ~ frameRate = " << m_detectorFrameRate << "; integration time = " << m_camIntegrationTime << "; nSlopePixels = " << nSlopePixels);

    U8_LLOG("");
}


int rtcDetector::set_irq_nios_delay()
{
    U6_LLOG(__FUNCTION__);
    int result = EXIT_SUCCESS;
    int _winRows = m_winRows;
    int _winCols = m_winCols;
    int _frameRate = m_detectorFrameRate;
    int _nSlopePixels = m_nSlopePixels;
    /* setting IRQ NIOS delay */
    // ttLoopModes 6
    if((_winRows == 512) &&
        (_winCols == 640) &&
        (_nSlopePixels == 16) &&
        (_frameRate == 200))
    {
        m_IrqNiosDelay = 2760;
    }

    // ttLoopModes 7
    if((_winRows == 84) &&
        (_winCols == 96) &&
        (_nSlopePixels == 8064) &&
        (_frameRate == 1000))
    {
        m_IrqNiosDelay = 0;
    }

    // ttLoopModes 8
    if((_winRows == 256) &&
        (_winCols == 320) &&
        (_nSlopePixels == 16) &&
        (_frameRate == 800))
    {
        m_IrqNiosDelay = 700;
    }

    // ttLoopModes 9
    if((_winRows == 64) &&
        (_winCols == 64) &&
        (_nSlopePixels == 4096) &&
        (_frameRate == 1000))
    {
        m_IrqNiosDelay = 0;
    }

    U8_LLOG("");
    return result;
}

int rtcDetector::update_n_slopes(int _nSlopePixels)
{
    U6_LLOG(__FUNCTION__);
    int result = EXIT_SUCCESS;

    m_nSlopePixels = _nSlopePixels;
    m_matPixelBias.resize(_nSlopePixels,0);

    // OcamInUsePixelMap = (2048×32)bits but max 8192 can be 1
    // fill bit mask with pixels to be copied in DSP memory
    std::fill(m_pixelInUseMask.begin(), m_pixelInUseMask.end(), 0);
    for(int i = 0; i < _nSlopePixels; i++)
    {
        m_pixelInUseMask[i/32] += (1 << (i % 32));
    }


    std::fill(m_nSlopesToDoMap.begin(), m_nSlopesToDoMap.end(), 0);
    // number of slopes to do every 16 pixels
    for(int i = 0; i <= (_nSlopePixels/16); i++)
    {
        m_nSlopesToDoMap[i] = i*2;
    }


    // Specify max 65535 pixels to be saved or all (65536)
    std::fill(m_pixelToSaveMap.begin(), m_pixelToSaveMap.end(), 0);
    int pixel_to_save_map_size = m_winSize;
    if(m_winSize > Sharknir::Bcu::OcamToSavePixelMapSize*32)
        pixel_to_save_map_size = Sharknir::Bcu::OcamToSavePixelMapSize*32;
    for(int i = 0; i < pixel_to_save_map_size; i++)
    {
        m_pixelToSaveMap[i/32] += (1 << (i % 32));
    }


    std::fill(m_SlopePixelsMap.begin(), m_SlopePixelsMap.end(), 0);
    for(int i = 0; i < _nSlopePixels; i++)
    {
        m_SlopePixelsMap[i] = i + 4;
    }
    for(int i = 5; i < _nSlopePixels; i += 4)
    {
        int old = m_SlopePixelsMap[i];
        m_SlopePixelsMap[i] = m_SlopePixelsMap[i + 1];
        m_SlopePixelsMap[i + 1] = old;
    }
    // Setting 15th bit of last pointer to 1 to indicate last value
    m_SlopePixelsMap[_nSlopePixels - 1] += 0x8000;
    U9_LLOG("m_SlopePixelsMap : " << su::seq_int_to_str(get_slope_pixel_map_for_bcu()));

    U8_LLOG("");
    return result;
}

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


int rtcDetector::set_cropping(const std::map<std::string, std::string> & cropSetup)
{
    // throws if error return SUCCESS only if at least 1 parameter changed
    U6_LLOG(__FUNCTION__);
    int result = EXIT_FAILURE;
    try
    {
        std::map<std::string, std::string>::const_iterator it;
        int top_left_x = m_winCoordX, top_left_y = m_winCoordY;
        int winrows = m_winRows, wincols = m_winCols;
        bool testx = (it = cropSetup.find("TTWINCOORDX")) != cropSetup.end();
        if(testx)
        {
            top_left_x = su::convertStringToType<int>(it->second);
            testx = top_left_x != m_winCoordX;
        }
        bool testy = (it = cropSetup.find("TTWINCOORDY")) != cropSetup.end();
        if(testy)
        {
            top_left_y = su::convertStringToType<int>(it->second);
            testy = top_left_y != m_winCoordY;
        }
        bool testcols = (it = cropSetup.find("TTWINCOLS")) != cropSetup.end();
        if(testcols)
        {
            wincols = su::convertStringToType<int>(it->second);
            testcols = wincols != m_winCols;
        }
        bool testrows = (it = cropSetup.find("TTWINROWS")) != cropSetup.end();
        if(testrows)
        {
            winrows = su::convertStringToType<int>(it->second);
            testrows = winrows != m_winRows;
        }
        if(testx || testy || testrows || testcols)
        {
            std::stringstream log_msg;
            if(testrows)
            {
                if(winrows % 4)
                {
                    log_msg << "Error : Number of rows must be a multiple of 4";
                    E_LLOG(log_msg.str());
                    U8_LLOG("");
                    throw(std::invalid_argument(log_msg.str()));
                }
                if(winrows < 0 ||  winrows > sud::cRedRows)
                {
                    log_msg << __FUNCTION__ << " ~ Error : Invalid mandatory parameter TTWINROWS";
                    E_LLOG(log_msg.str());
                    U8_LLOG("");
                    throw(std::invalid_argument(log_msg.str()));
                }
            }
            if(testcols)
            {
                if(wincols % 32)
                {
                    log_msg << "Error : Number of columns must be a multiple of 32";
                    E_LLOG(log_msg.str());
                    U8_LLOG("");
                    throw(std::invalid_argument(log_msg.str()));
                }
                if(wincols < 0 ||  wincols > sud::cRedCols)
                {
                    log_msg << __FUNCTION__ << " ~ Error : Invalid mandatory parameter TTWINCOLS";
                    E_LLOG(log_msg.str());
                    U8_LLOG("");
                    throw(std::invalid_argument(log_msg.str()));
                }
            }
            if(testx)
            {
                if(top_left_x % 32)
                {
                    log_msg << "Error : OffsetX (WINCOORDX) must be a multiple of 32";
                    E_LLOG(log_msg.str());
                    U8_LLOG("");
                    throw(std::invalid_argument(log_msg.str()));
                }
                if(top_left_x < 0 || top_left_x + wincols > sud::cRedCols)
                {
                    log_msg << __FUNCTION__ << " ~ Error : Parameters TTWINCOLS & TTWINCOORDX outside bounds";
                    E_LLOG(log_msg.str());
                    U8_LLOG("");
                    throw(std::invalid_argument(log_msg.str()));
                }
            }
            if(testy)
            {
                if(top_left_y % 4)
                {
                    log_msg << "Error : OffsetY (WINCOORDY) must be a multiple of 4";
                    E_LLOG(log_msg.str());
                    U8_LLOG("");
                    throw(std::invalid_argument(log_msg.str()));
                }
                if(top_left_y < 0 ||  top_left_y + winrows > sud::cRedRows)
                {
                    log_msg << __FUNCTION__ << " ~ Error : Parameters TTWINROWS & TTWINCOORDY outside bounds";
                    E_LLOG(log_msg.str());
                    U8_LLOG("");
                    throw(std::invalid_argument(log_msg.str()));
                }
            }
            m_winCoordX = top_left_x;
            m_winCoordY = top_left_y;
            m_winCols = wincols;
            m_winRows = winrows;
            result = EXIT_SUCCESS;
        }
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
    return result;
}


int rtcDetector::init_bias(const std::string & file_name)
{
    U6_LLOG(__FUNCTION__);
    int result = EXIT_SUCCESS;
    std::string line;
    int i = 0;
    std::ifstream ifstrm;
    std::istringstream isstream;
    int read_data = 0;
    std::string TIPTILT_CONFIG = su::get_env("INSROOT")+"/etc/tiptilt_ctrl/rtc/";
    m_strPixelBiasFile  = TIPTILT_CONFIG + "darks/" + file_name;
    if(boost::filesystem::exists(boost::filesystem::path(m_strPixelBiasFile)))
    {
        ifstrm.open(m_strPixelBiasFile.c_str());
        while (std::getline(ifstrm, line))
        {
            isstream.clear();
            isstream.str(line);
            for(int j = 0; j < m_winCols; j++)
            {
                float read_value = 0;
                isstream >> read_value;
                if(isstream.fail())
                {
                    E_LLOG("Failed reading line " << i + 1 << " column " << j + 1 << "/" << m_winCols << " from " << m_strPixelBiasFile << "; read_data = " << read_data);
                    break;
                }
                else
                {
                    if(read_data < m_nSlopePixels)
                    {
                        m_matPixelBias[i*m_winCols + j] = (int)(read_value + 0.5f);//read_value always greater than 0
                    }
                    read_data++;
                }
            }
            i++;
            line = "";
        }
        std::fill(m_matPixelBias.begin(), m_matPixelBias.begin() + 4, 0);
        ifstrm.close();
        if(read_data > m_nSlopePixels)
        {
            std::stringstream log_msg;
            log_msg << "File \"" << m_strPixelBiasFile << "\" (" << read_data << ") not conformant to image size" << m_nSlopePixels;
            E_LLOG(log_msg.str());
            U8_LLOG("");
            throw (std::invalid_argument(log_msg.str()));
        }
    }
    else if(!file_name.empty())
    {
        E_LLOG("File " << m_strPixelBiasFile << " does not exists");
        U8_LLOG("");
        throw (std::invalid_argument("File " + m_strPixelBiasFile + " does not exists"));
    }
    U8_LLOG("");
    return result;
}


Nice::SeqInt rtcDetector::get_irq_nios_delay_for_bcu() const
{
    U6_LLOG(__FUNCTION__);
    Nice::SeqInt retSeq(1,0);
    if(m_IrqNiosDelay != 0)
        retSeq[0] = static_cast<int>(22.1000*m_IrqNiosDelay+3.3298);

    U8_LLOG("");
    return retSeq;
}


// winCoordX is coordinate of upper left corner
std::vector<int> rtcDetector::get_window_size_cols()
{
//    U6_LLOG(__FUNCTION__);
    std::vector<int> retVec(2,0);

    retVec[0] = m_winCoordX;
    retVec[1] = m_winCols;

//    U8_LLOG("");
    return retVec;
}


// winCoordY is coordinate of upper left corner
std::vector<int> rtcDetector::get_window_size_rows()
{
//    U6_LLOG(__FUNCTION__);
    std::vector<int> retVec(2,0);
    retVec[0] = m_winCoordY;
    retVec[1] = m_winRows;

//    U8_LLOG("");
    return retVec;
}


Nice::SeqInt rtcDetector::get_in_use_pixel_map_for_bcu   () const
{
    U6_LLOG(__FUNCTION__);
    Nice::SeqInt retVec(m_pixelInUseMask.size(), 0);
    std::memcpy(&*retVec.begin(), &*m_pixelInUseMask.begin(), m_pixelInUseMask.size()*sizeof(int));
    U8_LLOG("");
    return retVec;
}


Nice::SeqInt rtcDetector::get_slopes_todo_map_for_bcu() const
{
    U6_LLOG(__FUNCTION__);
    Nice::SeqInt retVec(m_nSlopesToDoMap.size(), 0);
    std::memcpy(&*retVec.begin(), &*m_nSlopesToDoMap.begin(), m_nSlopesToDoMap.size()*sizeof(int));
    U8_LLOG("");
    return retVec;
}


Nice::SeqInt rtcDetector::get_pixel_to_save_map_for_bcu   () const
{
    U6_LLOG(__FUNCTION__);
    Nice::SeqInt retVec(m_pixelToSaveMap.size(), 0);
    std::memcpy(&*retVec.begin(), &*m_pixelToSaveMap.begin(), m_pixelToSaveMap.size()*sizeof(int));
    U8_LLOG("");
    return retVec;
}


Nice::SeqInt rtcDetector::get_slope_pixel_map_for_bcu     () const
{
    U6_LLOG(__FUNCTION__);
    Nice::SeqInt retVec(m_SlopePixelsMap.size(), 0);
    std::memcpy(&*retVec.begin(), &*m_SlopePixelsMap.begin(), m_SlopePixelsMap.size()*sizeof(int));
    U8_LLOG("");
    return retVec;
}

int rtcDetector::set_cam_integration_time(float cit)
{
    int result = EXIT_FAILURE;
    if(cit != m_camIntegrationTime)
    {
        if(cit > 0.000001 && cit < 10000)
        {
            m_camIntegrationTime = cit;
        }
        else
        {
            std::stringstream log_msg;
            log_msg << "Error : camera integration time value " << cit << " not allowed";
            E_LLOG(log_msg.str());
            throw (std::invalid_argument(log_msg.str()));
        }
    }
    return result;
}

int rtcDetector::set_frame_rate(float fr)
{
    int result = EXIT_FAILURE;
    if(m_detectorFrameRate != fr)
    {
        if(fr >= 0.0001 && fr <= 4000)
        {
            m_detectorFrameRate = fr;
            result = EXIT_SUCCESS;
        }
        else
        {
            std::stringstream log_msg;
            log_msg << "Error : frame rate value " << fr << " not allowed";
            E_LLOG(log_msg.str());
            throw (std::invalid_argument(log_msg.str()));
        }
    }
    return result;
}


}// util namespace
}// Sharknir namespace
