// $Heamder: https://oberon.oapd.inaf.it/shins/branches/tiptilt/shins/src/tiptilt_ctrl/bcu/util/rtcDM.cpp 3357 2020-07-14 13:43:26Z depa $

#include "rtcDM.hpp"

namespace shins {
namespace util {

rtcDM::rtcDM():
    m_zonalMode(false),
    m_maxPower(1)
{
    U6_LLOG(__FUNCTION__);
    U8_LLOG("");
}
/**
 *  \brief object constructor
 *
 *  accept a C++ map as input, extracts the pertinent values and calls a different contructor
 *
 *  \param _mapSetup C++ map with values from XML file, parsed by Python template script
 *  \return None
 */
rtcDM::rtcDM(const std::map<std::string, std::string>& _mapSetup)
{
    U6_LLOG(__FUNCTION__);
    int DMMaxPower;
    std::string strM2C;
    std::string strActuatorMap;
    std::string strDMFlat;
    std::string strActuatorSingMap;

    bool modalMode = true;
    bool zonalMode = false;
    // check if modalMode or zonalMode
    int dm_steps = 0;
    try
    {
        dm_steps = 1;
    try
    {
        strM2C = get_value_from_map<std::string>(_mapSetup, "TTM2CFILE");
        U3_LLOG("DM set to MODAL MODE mode with mode to command file: " << strM2C);
        if(strM2C == "ZONAL" || strM2C == "IDENTITY")
        {
            modalMode = false;
            zonalMode = true;
        }
    }
    catch(std::invalid_argument& e)
    {
        // parameters identifying modalMode are missing, then we should be zonalMode
        W_LLOG(e.what());
        modalMode = false;
        zonalMode = true;
    }

    dm_steps = 2;

    // getting other parameters, if missing rethrow error
    try
    {
        strActuatorMap = get_value_from_map<std::string>(_mapSetup, "TTACTUATORMAPFILE");
        strDMFlat = get_value_from_map<std::string>(_mapSetup, "TTDMFLATFILE");
        strActuatorSingMap = get_value_from_map<std::string>(_mapSetup, "TTACTUATORSIGNMAPFILE");
    }
    catch(std::invalid_argument& e)
    {
        E_LLOG(e.what());
        U8_LLOG("");
        throw e;
    }

    dm_steps = 3;
    /* DM maximum power had a default value of 40 lowered then to 15*/
    try
    {
        DMMaxPower = get_value_from_map<int>(_mapSetup, "TTDMMAXPOWER");
    }
    catch(std::invalid_argument& e)
    {
        W_LLOG("Missing value for TTDMMAXPOWER. Setting to default " << sud::DMMaxPower)
        DMMaxPower = sud::DMMaxPower;
    }
    m_maxPower = DMMaxPower;
    /* call mode to commands contructor */
    dm_steps = 4;
    std::string TIPTILT_CONFIG = su::get_env("INSROOT")+"/etc/tiptilt_ctrl/";
    // load actuators map
    dm_steps = 5;
    load_one_col_map<int>(std::string(TIPTILT_CONFIG + "rtc/" + strActuatorMap), m_actuatorMap);
    // load DM flat
    dm_steps = 6;
    load_one_col_map<float>(std::string(TIPTILT_CONFIG + "rtc/dm_flats/" + strDMFlat), m_flatMap);
    // load actuators sign
    dm_steps = 7;
    load_one_col_map<float>(std::string(TIPTILT_CONFIG + "rtc/" +  strActuatorSingMap), m_actuatorSignMap);

    if(modalMode)
    {
        dm_steps = 8;
        init_modalMode(strM2C);
    }
    if(zonalMode)
    {
        dm_steps = 9;
        init_zonalMode();
    }
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__ << " ~ At step "<< dm_steps);
        U8_LLOG("");
        throw;
    }

    U8_LLOG("");
}


/**
 *  \brief object contructor for modalMode mode
 *
 *  In modalMode mode, a mode to command matrix have to be provided
 *
 *  \param string _strMode2commands : file name for mode to commands matrix. A FITS file is expected
 *  \param string _strMap           : file name for actuators map. It is expected to be a 1 column txt file of integers
 *  \param string _strFlat          : file name for DM flat. It is expected to be a 1 column txt file of float
 *  \param string _strSign          : file name for sign of DM actuators. It is expected to be a 1 column txt file of integers
 *  \param int    _maxPower         : maximum power that can be sent to actuators.
 *  \return None
 */
void rtcDM::init_modalMode(const std::string & _strMode2commands)
{
    U6_LLOG(__FUNCTION__);
    // rtcDM::rtcDM
    // load mode to command matrix
    if(_strMode2commands == "IDENTITY" || _strMode2commands == "ZONAL")
    {
        init_zonalMode();
        return;
    }
    int loadedActNum = 0;
    int loadedNModes = 0;
    std::string strExtension = "";
    std::stringstream log_string;
    std::string TIPTILT_CONFIG = su::get_env("INSROOT")+"/etc/tiptilt_ctrl/";

    std::vector<std::vector<double> > m2c;
    std::vector<std::vector<double> > m2c2;
    // _strMode2commands is expected to be a FITS file. check this
    // strExtension = boost::filesystem::path(_strMode2commands).extension().string();
    // if(boost::algorithm::to_upper_copy(strExtension) == "FITS")
    //   {

    try
    {
//         load_m2c_fits_file(std::string(TIPTILT_CONFIG + "rtc/m2cs/" + _strMode2commands), m2c2);
//         loadedActNum = m2c2.size();
//         if(loadedActNum > 0)loadedNModes = m2c2[0].size();
//         U9_LLOG("Actuators Number from M2C2 : " << loadedActNum << "; Modes Number from M2C : " << loadedNModes);
//         for(int i = 0; i < 10; i++)
//         {
//             int index = i + (i/5)*87;
//             log_string.str("");
//             log_string << index + 1 << "\t";
//             for(int j = 0; j < 10; j++)
//             {
// //              log_string << vecM2C[j + i*loadedActNum] << "\t";
//                 log_string << m2c2[index][j] << "\t";
//             }
//             U9_LLOG(log_string.str());
//         }
        U9_LLOG(" %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Z2C %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%")
        int load_result = EXIT_FAILURE;
        if(su::str_tolower(su::right(_strMode2commands, 4)) == ".dat")load_result = load_m2c_dat_file(_strMode2commands, m2c);
        else if(su::str_tolower(su::right(_strMode2commands, 4)) == "fits")load_result = load_m2c_fits_file(_strMode2commands, m2c);
        if(load_result == EXIT_SUCCESS)
        {
            loadedActNum = m2c.size();
            if(loadedActNum > 0)loadedNModes = m2c[0].size();
            U9_LLOG("Actuators Number from M2C : " << loadedActNum << "; Modes Number from M2C : " << loadedNModes);
            for(int i = 0; i < 10; i++)// actuator
            {
                int index = i + (i/5)*(loadedActNum - 10);
                log_string.str("");
                log_string << std::setw(3) << std::setfill('_') << index + 1 << ")\t";
                log_string << su::seq_double_to_str(m2c[index]);
                U9_LLOG(log_string.str());
            }
        }
    }
    catch(CCfits::FITS::CantOpen& e)
    {
        A_LLOG("Unable to open file " << std::string(TIPTILT_CONFIG + "rtc/m2cs" +  _strMode2commands) << " message:");
        A_LLOG(e.message());
        U8_LLOG("");
        throw (e);
    }
    m_influenceMatrix.resize(loadedNModes, loadedActNum);
    m_influenceMatrix2.resize(loadedActNum, Nice::SeqFloat(loadedNModes, 0.f));
    U9_LLOG("m_influenceMatrix (rows, columns) : (" << m_influenceMatrix.rows() << ", " << m_influenceMatrix.cols() << ")");
    m_influenceMatrix.setZero();
//    for(unsigned int i = 0; i < vecM2C.size(); i++)
//    {
//        // turn vecM2C vector to matrix
//        // this matrix must have dimension number_of_actuators x number_of_modes.
//        // m_influenceMatrix(i%loadedActNum,i/loadedActNum)=vecM2C[i];
//
//        m_influenceMatrix(i/loadedActNum, i%loadedActNum) = vecM2C[i];
//    }
    // transposing
//    int discrepancies = 0;
    log_string.str("");
    m_modeWeights.resize(get_length_of_modes_vector(), 0);
    for(int i = 0; i < loadedNModes; i++)
    {
        float modeWeight = 0.f;
        for(int j = 0; j < loadedActNum; j++)
        {
            double val = m2c[j][i];
            m_influenceMatrix(i, j) = val; // the one used
            m_influenceMatrix2[j][i] = val;
            modeWeight += val*val;
            // if(fabs(m2c[j][i] - m2c2[j][i]) > 0.00001)
            // {
            //     if(discrepancies <= 300)
            //         log_string << "(" << j + 1 << ", " << i + 1 << ");";
            //     discrepancies++;
            // }
        }
        m_modeWeights[i + 2] = sqrtf(modeWeight);
    }
    m_modeWeights[0] = m_modeWeights[2];
    m_modeWeights[1] = m_modeWeights[3];
    U9_LLOG("m_modeWeights : " << su::seq_float_to_str(m_modeWeights));
//    U9_LLOG("Discrepancies : " << discrepancies << " -> " << log_string.str());
    m_zonalMode = false;

    U8_LLOG("");
}


void rtcDM::update_flat(const std::string & _strFlat)
{
    std::string flat_dir = su::get_env("INSROOT")+"/etc/tiptilt_ctrl/rtc/dm_flats/";
    std::vector<float> new_flat;
    load_one_col_map<float>(flat_dir + _strFlat, new_flat);
    if(new_flat.size() == m_flatMap.size())
    {
        m_flatMap = new_flat;
    }
    else
    {
        std::stringstream err;
        err << "DM Bias file " << _strFlat << " not right length : is " << new_flat.size() << " expected " << m_flatMap.size();
        E_LLOG(err.str());
        throw std::invalid_argument(err.str());
    }
}
/**
 * @brief      object contructor for zonalMode mode
 *
 * @details    in zonalMode mode the number of actuators and modes have to be provided
 *
 * @param      int _actuatorsNumber : the number of actuators for the influence matrix
 * @param      int _nModes          : the number of modes for the influence matrix
 * @param      string _strMap       : file name for actuators map. It is expected to be a 1 column txt file of integers
 * @param      string _strFlat      : file name for DM flat. It is expected to be a 1 column txt file of float
 * @param      string _strSign      : file name for sign of DM actuators. It is expected to be a 1 column txt file of integers
 * @param      int    _maxPower     : maximum power that can be sent to actuators.
 *
 * @return     None
 */
void rtcDM::init_zonalMode()
{
    U6_LLOG(__FUNCTION__ << " ~ " << this );
    int _actuatorsNumber = m_flatMap.size();
    m_zonalMode = true;
    // generate mode to command matrix as Identity matrix
    m_influenceMatrix = Eigen::MatrixXf::Identity(_actuatorsNumber, _actuatorsNumber);
    m_influenceMatrix2.clear();
    m_influenceMatrix2.resize(_actuatorsNumber, Nice::SeqFloat(_actuatorsNumber, 0.f));
    m_modeWeights.resize(get_length_of_modes_vector());
    for( int i = 0; i < _actuatorsNumber; i++)
    {
        m_influenceMatrix2[i][i] = 1.f;
        m_modeWeights[i + 2] = 1.f;
    }

    int loadedActNum = 0, loadedNModes = 0;
    loadedActNum = m_influenceMatrix2.size();
    if(loadedActNum > 0)loadedNModes = m_influenceMatrix2[0].size();
    std::stringstream log_string;
    for(int i = 0; i < 10; i++)// actuator
    {
        log_string.str("");
        int index = i + (i/5)*(loadedActNum - 10);
        log_string << std::setw(3) << std::setfill('_') << index + 1 << ")\t";
        log_string << su::seq_float_to_str(m_influenceMatrix2[index]);
        U9_LLOG(log_string.str());
    }
    U8_LLOG("");
}

/**
 *  \brief destructor
 *
 *  Detailed description
 *
 */
rtcDM::~rtcDM()
{
    U6_LLOG(__FUNCTION__ << " ~ " << this);
    U8_LLOG("");
}

Nice::Int rtcDM::update_m2c(std::string _filename)
{
    std::vector<std::vector<double> > m2c;
    int loadedActNum = 0, loadedNModes = 0;
    load_m2c_dat_file(_filename, m2c);
    std::stringstream log_string;
    loadedActNum = m2c.size();
    if(loadedActNum > 0)loadedNModes = m2c[0].size();
    U9_LLOG("Actuators Number from M2C : " << loadedActNum << "; Modes Number from M2C : " << loadedNModes);
    for(int i = 0; i < 5; i++)// actuator
    {
        log_string.str("");
        log_string << i + 1 << "\t";
        for(int j = 0; j < 10; j++) //modeweight
        {
            log_string << m2c[i][j] << "\t";
        }
        U9_LLOG(log_string.str());
    }
    for(int i = loadedActNum - 5; i < loadedActNum; i++)// actuator
    {
        log_string.str("");
        log_string << i + 1 << "\t";
        for(int j = 0; j < 10; j++) //modeweight
        {
            log_string << m2c[i][j] << "\t";
        }
        U9_LLOG(log_string.str());
    }

    m_influenceMatrix.resize(loadedNModes, loadedActNum);
    m_influenceMatrix2.resize(loadedActNum, Nice::SeqFloat(loadedNModes, 0.f));
    U9_LLOG("m_influenceMatrix (rows, columns) : (" << m_influenceMatrix.rows() << ", " << m_influenceMatrix.cols() << ")");
    m_influenceMatrix.setZero();
    m_modeWeights.resize(loadedNModes + 2);
    // transposing
    log_string.str("");
    for(int i = 0; i < loadedNModes; i++)
    {
        float modeWeight = 0.f;
        for(int j = 0; j < loadedActNum; j++)
        {
            double val = m2c[j][i];
            m_influenceMatrix(i, j) = val; // the one used
            m_influenceMatrix2[j][i] = val;
            modeWeight += val*val;
        }
        m_modeWeights[i + 2] = sqrtf(modeWeight);
    }
    m_modeWeights[0] = m_modeWeights[2];
    m_modeWeights[1] = m_modeWeights[3];
    m_zonalMode = false;

    return EXIT_SUCCESS;
}

/**
 *  \brief function to load fits file as C++ standard valarray
 *
 *  Detailed description
 *
 *  \param _filename (input)
 *  \param _loadedActNum (output)
 *  \param _loadedNModes (output)
 *  \return valarray of doubles containing coefficients to convert modes to DM commands
 */
Nice::Int rtcDM::load_m2c_fits_file(std::string _filename, std::vector<std::vector<double> > & _m2c)
{
    U6_LLOG(__FUNCTION__ << " ~ Loading file: " << _filename);
    std::string TIPTILT_CONFIG = su::get_env("INSROOT")+"/etc/tiptilt_ctrl/rtc/m2cs/";
    std::valarray<double> vecM2C;
    std::string strExtension = std::string();
    try
    {
        std::auto_ptr<CCfits::FITS> pInFile(new CCfits::FITS(TIPTILT_CONFIG + _filename, CCfits::Read, true));
        CCfits::PHDU& phduM2C = pInFile->pHDU();

        phduM2C.read(vecM2C);
        int _loadedActNum = phduM2C.axis(0);   // actuators number in DM. Number of columns
        int _loadedNModes = phduM2C.axis(1);   // number of modes. Number of rows
        _m2c.resize(_loadedActNum);
        for(int i = 0; i < _loadedActNum; i++)
        {
            _m2c[i].resize(_loadedNModes);
            for(int j = 0; j < _loadedNModes; j++)
            {
                _m2c[i][j] = vecM2C[i + j*_loadedActNum];
            }
        }
        T_LLOG("loading FITS file ... DONE");
    }
    catch(CCfits::FITS::CantOpen& e)
    {
        E_LLOG("Cought exception " << e.message());
        strExtension = boost::filesystem::path(_filename).extension().string();
        if(boost::algorithm::to_upper_copy(strExtension) != "FITS")
        {
            E_LLOG("Mode to commands file should be a FITS files");
            U8_LLOG("");
            throw e;
        }
    }
    catch(CCfits::FitsException& e)
    {
        strExtension = boost::filesystem::path(_filename).extension().string();
        if(boost::algorithm::to_upper_copy(strExtension) != "FITS")
        {
            E_LLOG(e.message());
            E_LLOG("Mode to commands file MUST be a FITS files");
            U8_LLOG("");
            throw e;
        }
    }
    catch(std::exception e)
    {
        strExtension = boost::filesystem::path(_filename).extension().string();
        if(boost::algorithm::to_upper_copy(strExtension) != "FITS")
        {
            E_LLOG(e.what());
            E_LLOG("Mode to commands file MUST be a FITS files");
            U8_LLOG("");
            throw e;
        }
        else
        {
            E_LLOG(__FUNCTION__);
            U8_LLOG("");
            throw e;
        }
    }
    U8_LLOG("");
    return EXIT_SUCCESS;
}


Nice::Int rtcDM::load_m2c_dat_file(const std::string & _datFile, std::vector<std::vector<double> > & _m2c)
{
//    int loadedActNum = _m2c.size();
//    int loadedNModes = 0;
    std::string strExtension = "";
    std::string TIPTILT_CONFIG = su::get_env("INSROOT")+"/etc/tiptilt_ctrl/";
    std::string line;
    int i = 0, xindex = 0, last_xindex = 0;
    std::ifstream ifstrm;
    std::istringstream isstream;
    int read_data = 0;
    float read_value;
    if(boost::filesystem::exists(boost::filesystem::path(TIPTILT_CONFIG + "rtc/m2cs/" + _datFile)))
    {
        ifstrm.open((TIPTILT_CONFIG + "rtc/m2cs/" + _datFile).c_str());
        while(std::getline(ifstrm, line))
        {
            isstream.clear();
            isstream.str(line);
            if(i > 1 && xindex != last_xindex)
            {
                E_LLOG("Irregular M2C matrix at line " << i);
                _m2c.clear();
                return EXIT_FAILURE;
            }
            _m2c.push_back(std::vector<double>());
            last_xindex = xindex;
            xindex = 0;
            // if(i == loadedActNum)
            // {
            //     E_LLOG("Number of lines exeeds expected value.");
            //     break;
            // }
//            for(int j = 0; j < loadedNModes; j++)
            while(!isstream.fail())
            {
                isstream >> read_value;
                if(!isstream.fail())
                {
                    _m2c[i].push_back(read_value);
                    xindex++;
                    read_data++;
                }
            }
            i++;
            line = "";
        }
        ifstrm.close();
    }
    else
    {
        std::stringstream log_msg;
        log_msg << "File " << TIPTILT_CONFIG + "rtc/m2cs/" + _datFile << " does not exists";
        E_LLOG(log_msg.str());
        throw (std::invalid_argument(log_msg.str()));
    }

    return EXIT_SUCCESS;
}
/**
 *  \brief get DM flat as const sequence of integer to be loaded in DSP
 *
 *  Detailed description
 *
 *  \param param
 *  \return SeqInt of Nice library
 */
Nice::SeqFloat rtcDM::get_flat() const
{
//    U6_LLOG(__FUNCTION__);
    Nice::SeqFloat resizedDMFlat = m_flatMap;
    // increasing size and adding 0 to additional elements.
    resizedDMFlat.resize(get_in_use_actuator_number(), 0);
//    U8_LLOG("");
    return resizedDMFlat;
}

/**
 *  \brief function description
 *
 *  Detailed description
 *
 *  \param param
 *  \return return type
 */
Nice::SeqFloat rtcDM::get_signmap_for_bcu() const
{
    U6_LLOG(__FUNCTION__);
    Nice::SeqFloat resizedSignMap = m_actuatorSignMap;
    resizedSignMap.resize(get_in_use_actuator_number(), 0);
    U8_LLOG("");
    return resizedSignMap;
}

/**
 *  \brief function description
 *
 *  Detailed description
 *
 *  \param param
 *  \return return type
 */
Nice::SeqInt rtcDM::get_in_use_actuator_number_for_bcu() const
{
    U6_LLOG(__FUNCTION__);
    Nice::SeqInt retSeq(1,0);
//    retSeq[0] = static_cast<unsigned int>(std::ceil(m_influenceMatrix.cols()/8.)*8);
    retSeq[0] = ((m_influenceMatrix.cols() + 7)/8)*8;
//    retSeq[0] = ((m_influenceMatrix2.size() + 7)/8)*8;
    U8_LLOG("");
    return retSeq;
}


Nice::SeqInt rtcDM::get_number_of_actuators_for_bcu() const
{
    U6_LLOG(__FUNCTION__);
    // Nice::SeqInt retSeq(1,0);
    // retSeq[0] = static_cast<unsigned int>(std::ceil(m_influenceMatrix.rows()/64.)*64);
    // return retSeq;
    U8_LLOG("");
//    return Nice::SeqInt(1, static_cast<unsigned int>(std::ceil(m_influenceMatrix.cols()/64.)*64));
    return Nice::SeqInt(1, ((m_influenceMatrix.cols() + 63)/64)*64);
//    return Nice::SeqInt(1, ((m_influenceMatrix2.size() + 63)/64)*64);
}


// remapping M2C matrix before sending to BCU
Nice::SeqSeqFloat rtcDM::get_influence_matrix() const
{
    U6_LLOG(__FUNCTION__);
    Nice::SeqSeqFloat result;
    Nice::SeqFloat DM0;
    Nice::SeqFloat DM1;
    result.push_back(DM0);
    result.push_back(DM1);

    int nCols = get_length_of_modes_vector_const();
    int nRows = get_in_use_actuator_number();
    int i, j;
    U3_LLOG("NROWS = " << nRows); // 104
    U3_LLOG("NCOLS = " << nCols); // 80
    int dm_size = nRows*nCols/2;
    result[0].resize(dm_size);
    result[1].resize(dm_size);

    std::vector<std::vector< double> > DMInfluenceMatrix(nRows);
    for(i = 0; i < nRows; i++)DMInfluenceMatrix[i].resize(nCols);
    int numActuators = m_influenceMatrix.cols(); // 97
    int numModes = m_influenceMatrix.rows();     // 78
    for(i = 0; i < numActuators; i++)
    {
        for(j = 0; j < numModes; j++)
        {
            if(m_zonalMode)
            {
                DMInfluenceMatrix[i][j + 2] = m_influenceMatrix(j,i);
            }
            else
            {
                if(j < 2)
                {
                    // First two modes are switched and replicated twice
                    DMInfluenceMatrix[i][1 - j] = m_influenceMatrix(j,i);
                    DMInfluenceMatrix[i][1 - j + 2] = m_influenceMatrix(j,i);
                }
                else
                    DMInfluenceMatrix[i][j + 2] = m_influenceMatrix(j,i);
            }

        }
    }

    uint32_t pattern[4] = {0, 2, 1, 3};
    for(i = 0; i < nRows; i++)
    {
        for(j = 0; j < nCols; j++)
        {
            int index = i*nCols + j;
            result[index/dm_size][index % dm_size] = DMInfluenceMatrix[(i/4)*4 + pattern[j % 4]][j / 4 + (i*(nCols/4)) % nCols];
        }
    }

    // Nice::SeqSeqFloat result2;
    // result2.push_back(DM0);
    // result2.push_back(DM1);
    // result2[0].resize(dm_size);
    // result2[1].resize(dm_size);
    // std::vector<std::vector< double> > InfluenceMatrixFittizia(nRows);
    // nCols = 104;
    // for(i = 0; i < nRows; i++)InfluenceMatrixFittizia[i].resize(nCols);
    // for(i = 0; i < nRows; i++)
    // {
    //     for(j = 0; j < nCols; j++)
    //     {
    //         InfluenceMatrixFittizia[i][j] = 1000*(i + 1) + (j + 1);
    //     }
    // }
    //     U9_LLOG("BREAK");
    // for(i = 0; i < nRows; i++)
    // {
    //     for(j = 0; j < nCols; j++)
    //     {
    //         int index = i*nCols + j;
    //         result2[index/dm_size][index % dm_size] = InfluenceMatrixFittizia[(i/4)*4 + pattern[j % 4]][j/4 + (i*(nCols/4)) % nCols];
    //     }
    // }
    //     U9_LLOG("BREAK");
    // std::stringstream log_string;
    // for(int i = 0; i < nRows/2; i++)// actuator
    // {
    //     log_string.str("");
    //     log_string << std::setw(3) << std::setfill('_') << i + 1 << ")\t";
    //     for(j = 0; j < nCols; j++)
    //     {
    //         log_string << std::setw(6) << std::setfill('0') << result2[0][i*nCols + j] << " ~ ";
    //     }
    //     U9_LLOG(log_string.str());
    // }
    //     U9_LLOG("BREAK");
    // for(int i = 0; i < nRows/2; i++)// actuator
    // {
    //     log_string.str("");
    //     log_string << std::setw(6) << std::setfill('_') << i + 1 + nRows/2 << ")\t";
    //     for(j = 0; j < nCols; j++)
    //     {
    //         log_string << std::setw(6) << std::setfill('0') << result2[1][i*nCols + j] << " ~ ";
    //     }
    //     U9_LLOG("BREAK");
    // }
    //     U9_LLOG(log_string.str());
    // for(int i = 0; i < nRows/2; i++)// actuator
    // {
    //     log_string.str("");
    //     log_string << std::setw(3) << std::setfill('_') << i + 1 << ")\t";
    //     for(j = 0; j < nCols; j++)
    //     {
    //         log_string << result[0][i*nCols + j] << " ~ ";
    //     }
    //     U9_LLOG(log_string.str());
    // }
    // for(int i = 0; i < nRows/2; i++)// actuator
    // {
    //     log_string.str("");
    //     log_string << std::setw(3) << std::setfill('_') << i + 1 + nRows/2 << ")\t";
    //     for(j = 0; j < nCols; j++)
    //     {
    //         log_string << result[1][i*nCols + j] << " ~ ";
    //     }
    //     U9_LLOG(log_string.str());
    // }

    U8_LLOG("");
    return result;
}

// remapping M2C matrix before sending to BCU
Nice::SeqSeqFloat rtcDM::get_influence_matrix2() const
{
    U6_LLOG(__FUNCTION__);
    Nice::SeqSeqFloat result;
    Nice::SeqFloat DM0;
    Nice::SeqFloat DM1;
    result.push_back(DM0);
    result.push_back(DM1);

    int nCols = get_length_of_modes_vector_const();
    int nRows = get_in_use_actuator_number();
    int i, j;
    U3_LLOG("NROWS = " << nRows); // 104
    U3_LLOG("NCOLS = " << nCols); // 80
    int dm_size = nRows*nCols/2;
    result[0].resize(dm_size);
    result[1].resize(dm_size);

    std::vector<std::vector< float> > DMInfluenceMatrix(nRows);
    for(i = 0; i < nRows; i++)DMInfluenceMatrix[i].resize(nCols);
    int numActuators = m_influenceMatrix2.size();    // 97
    int numModes = m_influenceMatrix2[0].size();     // 78
    for(i = 0; i < numActuators; i++)
    {
        for(j = 0; j < numModes; j++)
        {
            if(j < 2) {
                // First two modes are switched and replicated twice
                DMInfluenceMatrix[i][1 - j] = m_influenceMatrix2[i][j];
                DMInfluenceMatrix[i][1 - j + 2] = m_influenceMatrix2[i][j]; }
            else
                DMInfluenceMatrix[i][j + 2] = m_influenceMatrix2[i][j];

        }
    }

    uint32_t pattern[4] = {0, 2, 1, 3};
    for(i = 0; i < nRows; i++)
    {
        for(j = 0; j < nCols; j++)
        {
            int index = i*nCols + j;
            result[index/dm_size][index % dm_size] = DMInfluenceMatrix[(i/4)*4 + pattern[j % 4]][j / 4 + (i*(nCols/4)) % nCols];
        }
    }

    U8_LLOG("");
    return result;
}

float rtcDM::get_mode_weight(int index) const
{
    if(index >= 0 && index < m_modeWeights.size())
        return m_modeWeights[index];
    else
    {
        std::stringstream log_msg;
        log_msg << "Error asked weight " << index + 1 << "; Currently loaded modes are " << m_modeWeights.size();
        E_LLOG(log_msg.str());
        throw(std::invalid_argument(log_msg.str()));
    }
}
}// util namespace
}// shins namespace
