
// $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/src/tiptilt_ctrl/tiptiltCtrl.cpp 3980 2022-06-30 18:24:09Z depa $

#include "tiptiltCtrl.hpp"

#include <algorithm>

//#include <boost/exception/diagnostic_information.hpp>

void LogBoostDiagnosticInformationTipTilt(int line)
{
    std::string what = boost::current_exception_diagnostic_information();
    std::string toErase = "std::exception::what:";
    size_t pos = what.find(toErase);
    if(pos != std::string::npos)what.erase(pos, toErase.length());
    what.erase(std::remove(what.begin(), what.end(), '\n'), what.end());
    std::stringstream log_msg;
    log_msg << "Line: " << line << " ~ " << what;
    E_LLOG(log_msg.str());
}
// tiptiltCtrl::tiptiltCtrl()
// {
//   Ice::CommunicatorPtr icPtr = this->communicator();

// }

//tiptiltCtrl * tiptiltCtrl::m_pTT = NULL;

//! \brief tiptiltCtrl - class contructor
//! \param argc
//! \param argv
//! \param _strAliasFileName
//! \param _mapCfgFiles
tiptiltCtrl::tiptiltCtrl(Ice::PropertiesPtr props):
    __OBJ_NAME__(typeid(this).name()),
    m_bcuService(NULL),
    m_ConfigClient(NULL),
    m_service_status(su::SERVICE_NOTCONNECTED)
{
    U6_LLOG(__FUNCTION__ << " ~ Constructor");
    m_isInit = false;
    m_bTechCamDownload = false;
    m_abort = 0;
    m_bAcquiringPixCube = false;
    m_bSaveRawDiagData = false;
    m_bCheckBcuWrite = false;
//    m_TeccamExpose.bAcquiringPixcube = false;
//    m_pTT = this;

//    pthread_mutexattr_t mutex_attributes;
//    pthread_mutexattr_init(&mutex_attributes);
//    pthread_mutex_init(&m_TiptiltMutex, &mutex_attributes);
//    pthread_mutexattr_destroy(&mutex_attributes);
//    pthread_mutexattr_init(&mutex_attributes);
//    pthread_mutex_init(&m_BcuWriteMutex, &mutex_attributes);
//    pthread_mutexattr_destroy(&mutex_attributes);
//    pthread_mutexattr_init(&mutex_attributes);
//    pthread_mutex_init(&m_BcuReadMutex, &mutex_attributes);
//    pthread_mutexattr_destroy(&mutex_attributes);

    m_service = props->getProperty("OcsTipTiltService.Name");
    std::string strEP = "";
//    std::string strExpectedAlias = "tiptilt_ctrl";
    std::string strSvcNode = "";
    std::string strNodeNameClientSide = "TIPTILT";
    std::vector<std::string> vecAlias, vecAliasTipTilt;
    // Create device node at client side
    Nice::JointPtr nodeTipTilt = Nice::Application::config()->node(strNodeNameClientSide);
    Nice::JointPtr tmpNodeTipTilt;

    Nice::Endpoint ep;
    m_writeBcuSleep = 1000; // milliseconds

    // rtc_ctrl-svr.ALIAS:Endpoint = [NAME=rtc_ctrl-svr, PORT=50220]
//    vecAlias = shins::util::getAliasesFromFile(_strAliasFileName, strExpectedAlias);
    // if(vecAlias.size() == 0)
    // {
    //     E_LLOG(__FUNCTION__);
    //     U8_LLOG("");
    //     throw std::runtime_error("Issue with aliases: no alias found in " + _strAliasFileName);
    // }

    // Creating Nice endpoints from aliases
    // std::vector<std::string>::iterator it;
    // for(it = vecAlias.begin(); it < vecAlias.end(); ++it)
    // {
    //     strEP = "[ALIAS=" + (*it) + "]"; // endpoint name
    //     ep = Nice::Endpoint::fromString(strEP); // endpoint

    //     // Creating endpoints in local property tree using aliases
    //     if(boost::algorithm::starts_with(*it, strExpectedAlias))
    //     {
    //         nodeTipTilt->setAtNode<Nice::Endpoint>(boost::to_upper_copy(*it) + ".CON", ep);
    //         vecAliasTipTilt.push_back(boost::to_upper_copy(*it));
    //     }
    // }

    /* connecting to TIPTILT_CTRL */
    // if(vecAliasTipTilt.size() == 1)
    // {
    //     try
    //     {
    //         D_LLOG(vecAliasTipTilt[0])
    //         strSvcNode = "BCU.SVC";
    //         tmpNodeTipTilt = Nice::Application::config()->node(strNodeNameClientSide);
    //         m_ConfigClient = Nice::AppConfigClient::create(tmpNodeTipTilt->getAtNode<Nice::Endpoint>(vecAliasTipTilt[0] + ".CON"), tmpNodeTipTilt->node(vecAliasTipTilt[0]));

    //         m_bcuService = Sharknir::BcuServiceWorkerInterface::create(tmpNodeTipTilt->node(vecAliasTipTilt[0]));
    //         m_bcuService->changeState(Basda::STANDBY);
    //         m_bcuService->changeState(Basda::ONLINE);
    //     }
    //     catch(...)
    //     {
    //         W_LLOG("Could not start service TIPTILT_CTRL")
    //     }
    // }
    // else
    // {
    //     A_LLOG(":Issue with aliases: no alias found in " << _strAliasFileName);
    //     U8_LLOG("");
    //     throw std::runtime_error("Issue with aliases: no alias found in " + _strAliasFileName);
    // }

    if(m_ConfigClient && m_bcuService)
    {
        default_initialization(props);
    }

    U8_LLOG(__FUNCTION__ << " ~ Constructor done for tiptilt");
}

//! \brief ~tiptiltCtrl - class destructor
tiptiltCtrl::~tiptiltCtrl()
{
    U6_LLOG(__FUNCTION__);
    U8_LLOG("");
}


bool tiptiltCtrl::CheckOnline(bool throw_if_notonline)
{
    if(throw_if_notonline && !(m_service_status == su::SERVICE_ONLINE))
    {
        throw(std::runtime_error(m_service + " Service is not ONLINE"));
    }
    return m_service_status == su::SERVICE_ONLINE;
}


su::EServiceStatus tiptiltCtrl::Connect()
{
    std::string nodeTipTiltSvc = "tiptilt_ctrl-svr";
    Nice::JointPtr NodeTipTilt = Nice::Application::config()->node(nodeTipTiltSvc);
    std::string tiptilt_alias = "TIPTILT_CTRL.BCU.SVC";
    if(m_service_status == su::SERVICE_NOTCONNECTED)
    {
        try
        {
            U1_LLOG("Creating AppConfigClient for tiptilt_ctrl-svr");
            m_ConfigClient = Nice::AppConfigClient::create(NodeTipTilt->getAtNode<Nice::Endpoint>("ALIAS"), NodeTipTilt);
            m_service_status = su::SERVICE_ALLOCATED;
        }
        catch (const Nice::Exception & e){ A_LLOG(e.what());}
        catch(...){ A_LLOG(__FUNCTION__); }
    }
    if(m_service_status == su::SERVICE_ALLOCATED)
    {
        try
        {
            Sharknir::BcuServiceWorkerInterfacePtr bcuService = Sharknir::BcuServiceWorkerInterface::create(NodeTipTilt->node(tiptilt_alias));
            m_bcuService = bcuService;
            m_service_status = su::SERVICE_CONNECTED;
        }
        catch(...)
        {
            E_LLOG("Error connecting to tiptilt_ctrl-svr service");
        }
    }
    if(m_service_status == su::SERVICE_CONNECTED)
    {
        try
        {
            m_bcuService->changeState(Basda::STANDBY);
            m_bcuService->changeState(Basda::ONLINE);
            m_service_status = su::SERVICE_TOBEINITED;
        }
        catch(...)
        {
            E_LLOG("Erorr changing state to MultiStageX");
        }
    }
    if(m_service_status == su::SERVICE_TOBEINITED)
    {
        U1_LLOG("Connection established with service tiptilt_ctrl-svr");
        m_service_status = su::SERVICE_ONLINE;
    }
    return m_service_status;
}

bool tiptiltCtrl::CheckConnection(bool bThrow)
{
    if(m_bcuService)
    {
        return true;
    }
    else if(bThrow)
    {
        std::stringstream log_msg;
        log_msg << "Error : TipTilt Service not connected";
        E_LOG(log_msg.str());
        throw(std::runtime_error(log_msg.str()));
    }
    else
    {
        return false;
    }
}

bool get_property(Ice::PropertiesPtr props, const std::string & property, std::string & value)
{
    bool result = false;
    std::string val;
    val = props->getProperty(property);
    if(val.empty())
    {
        W_LLOG("Could not find property \"" << property << "\"");
    }
    else
    {
        value = val;
        result = true;
    }
    return result;
}

void init_setup(std::map<std::string, std::string> & curr_setup, const std::map<std::string, std::string> & defaults, const std::map<std::string, std::string> & properties, Ice::PropertiesPtr props)
{
    std::string value;
    std::string tiptilt = "Shins.TipTilt.Defaults.";
    std::map<std::string, std::string>::const_iterator itProp;
    for(itProp = properties.begin(); itProp != properties.end(); itProp++)
    {
        value = props->getProperty(tiptilt + itProp->second);
        if(value.empty())
        {
            W_LLOG("Could not find property \"" << tiptilt + itProp->second << "\"");
            curr_setup[itProp->first] = defaults.at(itProp->first);
        }
        else
        {
            curr_setup[itProp->first] = value;
        }
    }
}

void tiptiltCtrl::default_initialization(Ice::PropertiesPtr props)
{
    std::string value;
    std::string tiptilt = "Shins.TipTilt.Defaults.";

    // std::map<std::string, std::string> m_ConfigProperties, m_defaults;
    // m_ConfigProperties["TTM2CFILE"] = "M2cFile";
    // m_ConfigProperties["TTACTUATORSIGNMAPFILE"] = "ActuatorsSignMapFile";
    // m_ConfigProperties["TTACTUATORMAPFILE"] = "ActuatorsMapFile";
    // m_ConfigProperties["TTDMFLATFILE"] = "DmFlatFile";
    // m_ConfigProperties["TTDIAGRECORDIPV4"] = "DiagnosticIpv4";
    // m_ConfigProperties["TTDIAGRECORDMAC"] = "DiagnosticMac";
    // m_ConfigProperties["TTDIAGRECORDPORT"] = "DiagnosticPort";
    // m_ConfigProperties["TTTIMEHISTORYFILE"] = "TimeHistoryFile";

    if(get_property(props, tiptilt + "M2cFile", value))m_currentSetup["TTM2CFILE"] = value;
    else m_currentSetup["TTM2CFILE"] = "20220603-Zernike-FullPupil-Arcetri-1u-rms-4rtc.dat";

    if(get_property(props, tiptilt + "ActuatorsSignMapFile", value))m_currentSetup["TTACTUATORSIGNMAPFILE"] = value;
    else m_currentSetup["TTACTUATORSIGNMAPFILE"] = "BAX479-DM97-sign.dat";

    if(get_property(props, tiptilt + "ActuatorsMapFile", value))m_currentSetup["TTACTUATORMAPFILE"] = value;
    else m_currentSetup["TTACTUATORMAPFILE"] = "BAX479-DM97-map.dat";

    m_currentSetup["TTDMOUTPUTENABLED"] = "1";
    m_currentSetup["TTLOOPENABLED"] = "0";
    m_currentSetup["TTRTCMDENABLED"] = "0";
    m_currentSetup["TTDMMAXPOWER"] = "15";
    m_currentSetup["TTFRAMERATE"] = "1000";
    m_currentSetup["TTMAXTTCOMMAND"] = "3";
    m_currentSetup["TTMINTTCOMMAND"] = "-3";
    m_currentSetup["TTCAMTINT"] = "0.000994919";
    m_currentSetup["TTMAXCENTROIDVAL"] = "+1";
    m_currentSetup["TTMINCENTROIDVAL"] = "-1";
    m_currentSetup["TTWINCOORDX"] = "224";
    m_currentSetup["TTWINCOORDY"] = "212";

    if(get_property(props, tiptilt + "TtBiasFile", value))m_currentSetup["TTBIASFILE"] = value;
    else m_currentSetup["TTBIASFILE"] = "dark64x64_1000Hz+20deg_14_02_22.dat";

    m_currentSetup["TTPIXELGAINMODE"] = "FLATGAIN";
    m_currentSetup["TTPIXELGAINFILE"] = "flat_gain_64x64.dat";

    m_currentSetup["TTHISTORYENABLED"] = "0";
    m_currentSetup["TTTIMEHISTLEN"] = "50000";
    if(get_property(props, tiptilt + "TimeHistoryFile", value))m_currentSetup["TTTIMEHISTORYFILE"] = value;
    else m_currentSetup["TTTIMEHISTORYFILE"] = "TimeHistory.dat";


    if(get_property(props, tiptilt + "DmFlatFile", value))m_currentSetup["TTDMFLATFILE"] = value;
    else m_currentSetup["TTDMFLATFILE"] = "zeri.txt";

    //"BAX479-DM97-FLAT.dat";//"FLAT_19_02_2021.dat";
    //    m_currentSetup["TTDMFLATFILE"] = "BAX479-DM97-FLAT-02.dat"; // "BAX479-DM97-ALPAO-Offsets.dat";
    //    m_currentSetup["TTDMFLATFILE"] = "dm_command_2022-11-22-05-45-54_int_140b_90el_T1C.txt"; // "dm_command_2022-11-22-05-45-54_int_140b_90el_T1C.txt";
    //    m_currentSetup["TTDMFLATFILE"] = "dm_shape_2023-01-06-19-31-14_int_140b_90el_T0C_0as.txt";

    m_currentSetup["TTDIAGDECIMATION"] = "0";
    m_currentSetup["TTDIAGENABLED"] = "true"; // or "1"
    m_currentSetup["TTPIXELDECIMATION"] = "49";
    m_currentSetup["TTPIXELENABLED"] = "1";
    m_currentSetup["TTWINROWS"] = "64";
    m_currentSetup["TTWINCOLS"] = "64";
    m_currentSetup["TTNSLOPEPIXELS"] = "4096";
    m_currentSetup["TTCENTROIDGAINX"] = "-4.249796";
    m_currentSetup["TTCENTROIDGAINY"] = "3.7651188";
    m_currentSetup["TTCENTROIDORIGX"] = "0.00";
    m_currentSetup["TTCENTROIDORIGY"] = "0.00";
    m_currentSetup["TTMINTHRESHOLD"] = "100";
    m_currentSetup["TTMASTERDECENABLED"] = "true";
    m_currentSetup["TTPIDKP"] = "1";
    m_currentSetup["TTPIDKI"] = "0";
    m_currentSetup["TTPIDKD"] = "0";
    m_currentSetup["TTPIDTS"] = "1";
    m_currentSetup["TTPIDTF"] = "1";

    if(get_property(props, tiptilt + "DiagnosticIpv4", value))m_currentSetup["TTDIAGRECORDIPV4"] = value;
    else m_currentSetup["TTDIAGRECORDIPV4"] = "192.168.61.113";

    if(get_property(props, tiptilt + "DiagnosticMac", value))m_currentSetup["TTDIAGRECORDMAC"] = value;
    else m_currentSetup["TTDIAGRECORDMAC"] = "4c:d9:8f:a0:22:44"; // "4C:D9:8F:A0:22:43"

    if(get_property(props, tiptilt + "DiagnosticPort", value))m_currentSetup["TTDIAGRECORDPORT"] = value;
    else m_currentSetup["TTDIAGRECORDPORT"] = "8001";

    m_currentSetup["TTDOWNLOADDSPCODE"] = "true"; // false is for debug purposes

    m_pTipTilt = this;

    try
    {
        init_bcu(m_currentSetup);
    }
    catch(const std::exception& e)
    {
        E_LLOG(e.what());
    }
}

tiptiltCtrl * tiptiltCtrl::m_pTipTilt = NULL;

const std::string & tiptiltCtrl::getRtcStatus(const std::string & key)
{
    static std::string result;
    result.clear();
    if(m_pTipTilt)
    {
        if(key == "TTDMFLATFILE" || key == "TTM2CFILE" || key == "TTBIASFILE" || key == "TTPIXELGAINFILE" || key == "TTWINROWS" || key == "TTWINCOLS" || key == "TTCENTROIDGAINX" || key == "TTCENTROIDGAINY" || key == "TTCENTROIDORIGX" || key == "TTCENTROIDORIGY" || key == "TTWINCOORDX" || key == "TTWINCOORDY" || key == "TTDMMAXPOWER")
        {
            return m_pTipTilt->m_currentSetup[key];
        }
        else if(key == "TTLOOPENABLED")
        {
            if(m_pTipTilt->m_ptrSlopeComputer)
            {
                int loop_enabled = m_pTipTilt->m_ptrSlopeComputer->get_tt_control_loop_enable()[0];
                std::stringstream control_loop;
                control_loop << loop_enabled;
                result = control_loop.str();
                return result;
            }
            else
            {
                W_LLOG("Slope computer Object is not defined");
                return result;
            }
        }
        else if(key == "TTPIXELENABLED")
        {
            if(m_pTipTilt->m_ptrSlopeComputer)
            {
                int pixel_enabled = m_pTipTilt->m_ptrSlopeComputer->get_pixel_save_enabled();//[0];
                std::stringstream pix_en;
                pix_en << pixel_enabled;
                result = pix_en.str();
                return result;
            }
            else
            {
                W_LLOG("Slope computer Object is not defined");
                return result;
            }
        }
        else if(key == "TTPIXELDECIMATION")
        {
            if(m_pTipTilt->m_ptrSlopeComputer)
            {
                int pixel_dec = m_pTipTilt->m_ptrSlopeComputer->get_pixel_save_decimation();//[0];
                //result = *reinterpret_cast<float*>(&pixel_dec);
                std::stringstream p_d;
                p_d << pixel_dec;
                result = p_d.str();
                return result;
            }
            else
            {
                W_LLOG("Slope computer Object is not defined");
                return result;
            }
        }
        else if(key == "TTDIAGENABLED")
        {
            if(m_pTipTilt->m_ptrSlopeComputer)
            {
                int diag_enabled = m_pTipTilt->m_ptrSlopeComputer->get_diagnostic_enabled();//[0];
                std::stringstream diag_en;
                diag_en << diag_enabled;
                result = diag_en.str();
                return result;
            }
            else
            {
                W_LLOG("Slope computer Object is not defined");
                return result;
            }
        }
        else if(key == "TTDIAGDECIMATION")
        {
            if(m_pTipTilt->m_ptrSlopeComputer)
            {
                int diag_dec = m_pTipTilt->m_ptrSlopeComputer->get_decimation_factor();//[0];
                //result = *reinterpret_cast<float*>(&diag_dec);
                std::stringstream d_d;
                d_d << diag_dec;
                result = d_d.str();
                return result;
            }
            else
            {
                W_LLOG("Slope computer Object is not defined");
                return result;
            }
        }
        else if(key == "TTFRAMERATE")
        {
            if(m_pTipTilt->m_ptrDetector)
            {
                float framerate = m_pTipTilt->m_ptrDetector->get_frame_rate();
                //result = std::to_string(framerate);
                //result = su:seq_float_to_str(framerate);
                //result = *reinterpret_cast<float*>(&diag_dec);
                std::stringstream frm_rt;
                frm_rt << framerate;
                result = frm_rt.str();
                return result;
            }
            else
            {
                W_LLOG("Detector Object is not defined");
                return result;
            }
        }
        else if(key == "TTCAMTINT")
        {
            if(m_pTipTilt->m_ptrDetector)
            {
                float cam_tint = m_pTipTilt->m_ptrDetector->get_cam_integration_time();
                //result = su:seq_float_to_str(cam_tint);
                std::stringstream c_t;
                c_t << cam_tint;
                result = c_t.str();
                return result;
            }
            else
            {
                W_LLOG("Detector Object is not defined");
                return result;
            }
        }
        else if(key == "TTSENSORTEMP")
        {
            if(m_pTipTilt->m_ptrDetector)
            {
                try
                {
                    m_pTipTilt->send_command_to_camera("temperature snake raw", result);
                    float snake_temp = su::convertStringToType<float>(result);
                    m_pTipTilt->m_ptrDetector->set_last_snake_temperature(snake_temp);
                }
                catch(const std::runtime_error & e)
                {
                    W_LLOG(e.what());
                    std::stringstream temperature;
                    temperature << m_pTipTilt->m_ptrDetector->get_last_snake_temperature();
                    result = temperature.str();
                }
                catch(...)
                {
                    LogBoostDiagnosticInformationTipTilt(__LINE__);
                    std::stringstream temperature;
                    temperature << m_pTipTilt->m_ptrDetector->get_last_snake_temperature();
                    result = temperature.str();
                }
            }
            else
            {
                W_LLOG("Detector Object is not defined");
                return result;
            }
            return result;
        }
        else if(key == "TTDMMODESNUM")
        {
            if(m_pTipTilt->m_ptrDm)
            {
                unsigned int ModesNum = m_pTipTilt->m_ptrDm->get_raw_number_of_modes();
                //result = su:seq_float_to_str(cam_tint);
                std::stringstream c_t;
                c_t << ModesNum;
                result = c_t.str();
                return result;
            }
            else
            {
                W_LLOG("DM Object is not defined");
                return result;
            }
        }
        else
        {
            W_LLOG(__FUNCTION__ << " - Unrecognized Keyword : \"" << key << "\"");
        }
    }

    {
        result.clear();
        return result;
    }
}

//! \brief init_bcu - initializes BCU (Basic Control Unit)
//! \param _mapSetup - key/values from XML OB
//! \return
int tiptiltCtrl::init_bcu(const std::map<std::string, std::string>& _mapSetup)
{
    U6_LLOG(__FUNCTION__);
    int status = EXIT_SUCCESS;
    std::stringstream log_msg;
    log_msg << __FUNCTION__ << suc::tilde;
    m_isInit = false;
    if(_mapSetup.find("TTDOWNLOADDSPCODE") != _mapSetup.end())
    {
        m_currentSetup["TTDOWNLOADDSPCODE"] = _mapSetup.find("TTDOWNLOADDSPCODE")->second;
    }

    try
    {

    //////////////
    // DETECTOR //
    //////////////
    // Sharknir::util::rtcDetector detector(_mapSetup);
    m_ptrDetector = boost::make_shared<shins::util::rtcDetector>(_mapSetup);
    U2_LLOG("created detector object DONE");
    }
    catch(...)
    {
        LogBoostDiagnosticInformationTipTilt(__LINE__);
        log_msg << "Could not initialize Detector. OS may be in undefined state please retry init";
        E_LLOG(log_msg.str());
        U8_LLOG("");
        throw;
    }

    try
    {
    ////////
    // DM //
    ////////
    m_ptrDm = boost::make_shared<shins::util::rtcDM>(_mapSetup);
    U2_LLOG("created DM object DONE ~ " << m_ptrDm);
    }
    catch(...)
    {
        LogBoostDiagnosticInformationTipTilt(__LINE__);
        log_msg << "Could not initialize DM. OS may be in undefined state please retry init";
        E_LLOG(log_msg.str());
        U8_LLOG("");
        throw;
    }

    try
    {
    ////////////////////
    // SLOPE COMPUTER //
    ////////////////////
    m_ptrSlopeComputer = boost::make_shared<shins::util::rtcSlopeComputer>(_mapSetup,*m_ptrDetector, *m_ptrDm);
    // shins::util::rtcSlopeComputer slopeComputer(_mapSetup, dm);
    U2_LLOG("created SLOPE COMPUTER object DONE");
    }
    catch(...)
    {
        LogBoostDiagnosticInformationTipTilt(__LINE__);
        log_msg << "Could not initialize Slope Computer. OS may be in undefined state please retry init";
        E_LLOG(log_msg.str());
        U8_LLOG("");
        throw;
    }

    m_isInit = true;


    std::map<std::string, std::string>::const_iterator itSetup;
    std::map<std::string, std::string>::iterator itCurrent;
    for(itCurrent = m_currentSetup.begin(); itCurrent != m_currentSetup.end(); itCurrent++)
    {
        if(_mapSetup.find(itCurrent->first) == _mapSetup.end() && itCurrent->first != "TTDOWNLOADDSPCODE")
        {
            U9_LLOG("Missing Keyword : " << std::setfill('_') << std::setw(16) << itCurrent->first << "; Value : " << std::setw(40) << itCurrent->second);
        }
        else if(_mapSetup.find(itCurrent->first) != _mapSetup.end())
        {
            itCurrent->second = _mapSetup.find(itCurrent->first)->second;
        }
    }
    for(itSetup = _mapSetup.begin(); itSetup != _mapSetup.end(); itSetup++)
    {
        if(m_currentSetup.find(itSetup->first) == m_currentSetup.end())
        {
            U9_LLOG("Unrecognized Keyword : " << std::setfill('_') << std::setw(16) << itSetup->first << "; Value : " << std::setw(40) << itSetup->second);
        }
    }
    U8_LLOG("BCU initialized");
    return status;
}


bool tiptiltCtrl::is_init(bool except)
{
    CheckConnection(except);
    if(except)
    {
        if(!m_isInit)
        {
            E_LLOG("RTC not initialized");
            throw(std::domain_error("RTC not initialized"));
        }
    }
    return m_isInit;
}


/// \brief interruptCallback - handling interrupts
/// \param _sig
void tiptiltCtrl::interruptCallback(int _sig)
{
    U6_LLOG(__FUNCTION__);
    N_LLOG("Interrupt #" << _sig);
    U8_LLOG("");
}


//! \brief start_bcu - starting the BCU
//! \return
int tiptiltCtrl::start_bcu()
{
    U6_LLOG(__FUNCTION__);
    int status = EXIT_SUCCESS;

    Nice::SeqInt dspStart(1, 0);
    Nice::SeqInt writeData;
    std::string errMsg = "";

    try
    {
        is_init(true);
    }
    catch(...)
    {
        LogBoostDiagnosticInformationTipTilt(__LINE__);
        U8_LLOG("");
        throw;
    }

    status = write_bcu(Sharknir::Bcu::DSPstart, dspStart);
    U9_LLOG("Written DSPstart : " << dspStart[0]);

    // setup of parameters for the NIOS diagnostic IRQ
    if(status == EXIT_SUCCESS)
    {
        // Highest bit set to 1 to enable the swap between the two banks
        writeData = Nice::SeqInt(1, 0x80000000 + m_ptrSlopeComputer->get_diagnostic_header_ptr()[0]);
        status = write_bcu(Sharknir::Bcu::DiagRecordPtr, writeData, "sram");

        int diagPort = m_ptrSlopeComputer->get_diag_port();
        try
        {
            enable_diagnostic(m_ptrSlopeComputer->get_diagnostic_enabled(),
                              m_ptrSlopeComputer->get_decimation_factor(),
                              m_ptrSlopeComputer->get_pixel_save_enabled(),
                              m_ptrSlopeComputer->get_pixel_save_decimation(),
                              m_ptrSlopeComputer->get_diag_ipv4(),
                              m_ptrSlopeComputer->get_diag_mac(),
                              diagPort,
                              m_ptrSlopeComputer->get_master_decimation(),
                              m_ptrSlopeComputer->get_jumbo_enabled(),
                              m_ptrSlopeComputer->get_master_enabled());
        }
        catch(...)
        {
            LogBoostDiagnosticInformationTipTilt(__LINE__);
            U8_LLOG("");
            throw;
        }

        status = write_bcu(Sharknir::Bcu::DiagRecordReadPtr, Nice::SeqInt(1, 0), "sram");
        U2_LLOG("DiagRecordReadPtr returned : " << status);
        status = write_bcu(Sharknir::Bcu::DiagRecordWritePtr, Nice::SeqInt(1, 0), "sram");
        U2_LLOG("DiagRecordWritePtr returned : " << status);

        status = write_bcu(Sharknir::Bcu::DiagRecordDecFactorEna, Nice::SeqInt(1, 0), "sram");
        U9_LLOG("DiagRecordDecFactorEna : " << 0);

        // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        // % scarico la LUT sull'interfaccia AIA2PIO        %
        // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        try
        {
                status = write_bcu(Sharknir::Bcu::OcamInUsePixelMap, m_ptrDetector->get_in_use_pixel_map_for_bcu(), "shmram");
                U9_LLOG("OcamInUsePixelMap : " << su::seq_int_to_str(m_ptrDetector->get_in_use_pixel_map_for_bcu()));

                status = write_bcu(Sharknir::Bcu::OcamNumSlopeToDo, m_ptrDetector->get_slopes_todo_map_for_bcu(), "shmram");
                U9_LLOG("OcamNumSlopeToDo : " << su::seq_int_to_str(m_ptrDetector->get_slopes_todo_map_for_bcu()));

                status = write_bcu(Sharknir::Bcu::OcamToSavePixelMap, m_ptrDetector->get_pixel_to_save_map_for_bcu(), "shmram");
                U9_LLOG("OcamToSavePixelMap : " << su::seq_int_to_str(m_ptrDetector->get_pixel_to_save_map_for_bcu()));

                status = write_bcu(Sharknir::Bcu::OcamSlopePixelsMap, m_ptrDetector->get_slope_pixel_map_for_bcu(), "shmram");
                U9_LLOG("OcamSlopePixelsMap : " << su::seq_int_to_str(m_ptrDetector->get_slope_pixel_map_for_bcu()));

            if(m_ptrDetector->get_simulation())
            {
                status = write_bcu(Sharknir::Bcu::ContrlReg, Nice::SeqInt(1, 3), "shmram");
                U9_LLOG("ContrlReg : " << su::seq_int_to_str(Nice::SeqInt(1, 3)));
            }

            writeData = Nice::SeqInt(1, su::constants::boardFreq / (8 * m_ptrDetector->get_frame_rate()));
            status = write_bcu(Sharknir::Bcu::OcamFrameRate, writeData, "shmram");
            U9_LLOG("OcamFrameRate : " << su::seq_int_to_str(writeData));
        }
        catch(...)
        {
            LogBoostDiagnosticInformationTipTilt(__LINE__);
            U8_LLOG("");
            throw;
        }

        // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        // % reset DM Alpao interface module & set DM Alpao interface %
        // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        m_bcuService->writeDMIF(Sharknir::Bcu::Const::BCU_DSP,
                                Sharknir::Bcu::Const::BCU_DSP,
                                0, Nice::SeqInt(1, 1));
//        U2_LLOG("Write 1 at 0 to DMIF");
        m_bcuService->writeDMIF(Sharknir::Bcu::Const::BCU_DSP,
                                Sharknir::Bcu::Const::BCU_DSP,
                                2, Nice::SeqInt(1, 1));
//        U2_LLOG("Write 1 at 2 to DMIF");
        m_bcuService->writeDMIF(Sharknir::Bcu::Const::BCU_DSP,
                                Sharknir::Bcu::Const::BCU_DSP,
                                3, m_ptrDm->get_number_of_actuators_for_bcu());
        U9_LLOG("Written " << su::seq_int_to_str(m_ptrDm->get_number_of_actuators_for_bcu()) << " at 3 to DMIF");

        status = write_bcu(Sharknir::Bcu::DiagRecordDecFactorEna, Nice::SeqInt(1, 1), "sram");
        U9_LLOG("DiagRecordDecFactorEna : " << 1);

        status = write_bcu(Sharknir::Bcu::TimeHistoryAct, Nice::SeqInt(1, 2));
        U9_LLOG("TimeHistoryAct : " << 2);

//        status = write_bcu(Sharknir::Bcu::TTControlLoopEna, Nice::SeqInt(1, 0));
        enable_loop(false);
        U9_LLOG("TTControlLoopEna : " << 0);

        status = write_bcu(Sharknir::Bcu::CCDWaitForReset, Nice::SeqInt(1, 1));
        U9_LLOG("CCDWaitForReset : "  << 1);

        status = write_bcu(Sharknir::Bcu::ContrlReg, Nice::SeqInt(1, 1), "shmram");
        U9_LLOG("ContrlReg : "  << 1);

        status = write_bcu(Sharknir::Bcu::CCDInterfaceEnable, Nice::SeqInt(1, 1));
        U9_LLOG("CCDInterfaceEnable : " << 1);

    }
    U8_LLOG(__FUNCTION__  << " - EXIT");
    return status;
}

//! \brief setup_bcu - setups the BCU according to members of input objects
//! \return
int tiptiltCtrl::setup_bcu()
{
    su::os_log_banner(std::string("setup_bcu"));
    U6_LLOG(__FUNCTION__);
    int status = EXIT_SUCCESS;
    Nice::SeqSeqFloat influenceMatrixForBCU;
    Nice::SeqInt output;
    const shins::util::rtcDetectorPtr& _detector = m_ptrDetector;
    const shins::util::rtcDMPtr& _dm = m_ptrDm;
    const shins::util::rtcSlopeComputerPtr& _slopeComputer = m_ptrSlopeComputer;
    const shins::util::rtcSlopeComputer& _sc = *m_ptrSlopeComputer;

    std::map<std::string, std::string> cam_default;
    try
    {
        config_camera();

        cam_default["TTCAM_IMAGE_TAG_ENABLED"] = "true";
        cam_default["TTCAM_BADPIXEL_ENABLED"] = "true";
        cam_default["TTCAM_BIAS_ENABLED"] = "false";
        cam_default["TTCAM_FLAT_ENABLED"] = "false";
        cam_default["TTCAM_RAWIMG_ENABLED"] = "true";
        config_camera(cam_default);
    }
    catch(...)
    {
        LogBoostDiagnosticInformationTipTilt(__LINE__);
        U8_LLOG("");
        throw;
    }

    bool bDownloadDspCode = true;
//    su::log_dictionary(__FILE__, __LINE__,m_currentSetup);
    su::get_bool_from_setup(m_currentSetup, "TTDOWNLOADDSPCODE", bDownloadDspCode);

    //    std::string svcName = "DATA.TIPTILT_CTRL.SVC";
    std::string SHINS_CONFIG = shins::util::get_env("INSROOT");
    std::string dspProgramPath = "";
    std::string etcPath = SHINS_CONFIG + "/etc/tiptilt_ctrl/";
    try
    {
        dspProgramPath = etcPath + "rtc/" + m_bcuService->BcuDspProgramPath();
        if(bDownloadDspCode)
        {
            U2_LLOG("Uploading DSP code to BCU from " << dspProgramPath);
            m_bcuService->uploadDspCode(Sharknir::Bcu::Const::BCU_DSP,
                                        Sharknir::Bcu::Const::BCU_DSP,
                                        dspProgramPath);
        }
        else
        {
            U2_LLOG("Uploading DSP code to BCU skipped");
        }
    }
    catch(...)
    {
        LogBoostDiagnosticInformationTipTilt(__LINE__);
        U8_LLOG("");
        throw;
    }

    try
    {
        std::string log_str;

        U2_LLOG("############   slope computer block   ############");

        status = write_bcu(Sharknir::Bcu::CCDInterfaceEnable, Nice::SeqInt(1,0));
        U9_LLOG("CCDInterfaceEnable : " << su::seq_int_to_str(Nice::SeqInt(1,0)));

        status = write_bcu(Sharknir::Bcu::CCDWaitForReset, Nice::SeqInt(1,0));
        U9_LLOG("CCDWaitForReset : "  << su::seq_int_to_str(Nice::SeqInt(1,0)));

        status = write_bcu(Sharknir::Bcu::CCDSubFrameCnt, Nice::SeqInt(1,0));
        U9_LLOG("CCDSubFrameCnt : " << su::seq_int_to_str(Nice::SeqInt(1,0)));

        status = write_bcu_single(Sharknir::Bcu::CCDTotalFlux, Nice::SeqFloat(1,0.f));
        U9_LLOG("CCDTotalFlux : " << su::seq_float_to_str(Nice::SeqFloat(1,0.f)));

            status = write_bcu(Sharknir::Bcu::CentroidFactPtr, _slopeComputer->get_centroid_fact_ptr());
            U9_LLOG("CentroidFactPtr : " << su::seq_int_to_str(_slopeComputer->get_centroid_fact_ptr()));

        status = write_bcu_single(Sharknir::Bcu::CentroidGain, _slopeComputer->get_centroid_gain()); //setup
        U9_LLOG("CentroidGain : " << su::seq_float_to_str(_slopeComputer->get_centroid_gain()));

        status = write_bcu_single(Sharknir::Bcu::CentroidOrigin, _slopeComputer->get_centroid_origin()); //setup
        U9_LLOG("CentroidOrigin : " << su::seq_float_to_str(_slopeComputer->get_centroid_origin()));

        status = write_bcu_single(Sharknir::Bcu::CentroidOut, Nice::SeqFloat(2,0.f));
        U9_LLOG("CentroidOut : " << su::seq_float_to_str(Nice::SeqFloat(2,0.f)));

        status = write_bcu(Sharknir::Bcu::CentroidPixelAreaPtr, _slopeComputer->get_centroid_pixel_area_ptr()); //const
        U9_LLOG("CentroidPixelAreaPtr : " << su::seq_int_to_str(_slopeComputer->get_centroid_pixel_area_ptr()));

            status = write_bcu(Sharknir::Bcu::DiagBuffDataPtr, _slopeComputer->get_diagnostic_data_ptr());
            U9_LLOG("DiagBuffDataPtr : " << su::seq_int_to_str(_slopeComputer->get_diagnostic_data_ptr()));

            status = write_bcu(Sharknir::Bcu::DiagBuffFooterPtr, _slopeComputer->get_diagnostic_footer_ptr());
            U9_LLOG("DiagBuffFooterPtr : " << su::seq_int_to_str(_slopeComputer->get_diagnostic_footer_ptr()));

            status = write_bcu(Sharknir::Bcu::DiagBuffHeaderPtr, _slopeComputer->get_diagnostic_header_ptr());
            U9_LLOG("DiagBuffHeaderPtr : " << su::seq_int_to_str(_slopeComputer->get_diagnostic_header_ptr()));

        status = write_bcu(Sharknir::Bcu::DiagRecordLen, _slopeComputer->get_diag_record_len(), "sram");
        U2_LLOG("DiagRecordLen : " << su::seq_int_to_str(_slopeComputer->get_diag_record_len()));

        status = write_bcu(Sharknir::Bcu::DiagnosticFrameDec, _slopeComputer->get_diagnostic_frame_dword()); //setup
        U9_LLOG("DiagnosticFrameDec : " << su::seq_int_to_str(_slopeComputer->get_diagnostic_frame_dword()));

        status = write_bcu(Sharknir::Bcu::FramesCounter, Nice::SeqInt(1,0));
        U9_LLOG("FramesCounter : " << su::seq_int_to_str(Nice::SeqInt(1,0)));

        status = write_bcu(Sharknir::Bcu::FramesCounterIrq, Nice::SeqInt(1,0));
        U9_LLOG("FramesCounterIrq : " << su::seq_int_to_str(Nice::SeqInt(1,0)));

        status = write_bcu_single(Sharknir::Bcu::MaxCentroidValue, _slopeComputer->get_max_centroid_value()); // setup
        U9_LLOG("MaxCentroidValue : " << su::seq_float_to_str(_slopeComputer->get_max_centroid_value()));

        status = write_bcu_single(Sharknir::Bcu::MinCentroidValue, _slopeComputer->get_min_centroid_value()); //setup
        U9_LLOG("MinCentroidValue : " << su::seq_float_to_str(_slopeComputer->get_min_centroid_value()));

            status = write_bcu(Sharknir::Bcu::NiosIrqDelay, _detector->get_irq_nios_delay_for_bcu());
            U9_LLOG("NiosIrqDelay : " << su::seq_int_to_str(_detector->get_irq_nios_delay_for_bcu()));

            status = write_bcu(Sharknir::Bcu::NumCentroidPixels, _detector->get_slope_pixels_number_for_bcu());
            U2_LLOG("NumCentroidPixels" << su::seq_int_to_str(_detector->get_slope_pixels_number_for_bcu()));

        status = write_bcu(Sharknir::Bcu::NumPixelsToDoPtr, _slopeComputer->get_num_pixels_to_do_ptr()); // const
        U9_LLOG("NumPixelsToDoPtr : " << su::seq_int_to_str(_slopeComputer->get_num_pixels_to_do_ptr()));

        status = write_bcu_single(Sharknir::Bcu::PIDDelayLine, Nice::SeqFloat(12,0.f));
        U9_LLOG("PIDDelayLine : " << su::seq_float_to_str(Nice::SeqFloat(12,0.f)));

        status = write_bcu_single(Sharknir::Bcu::PIDFilterCoeff, _slopeComputer->get_PID_filter_coeff());
        U9_LLOG("PIDFilterCoeff : " << su::seq_float_to_str(_slopeComputer->get_PID_filter_coeff()));

        status = write_bcu(Sharknir::Bcu::ParamSelector, Nice::SeqInt(1,0));
        U9_LLOG("ParamSelector : " << su::seq_int_to_str(Nice::SeqInt(1,0)));

        status = write_bcu(Sharknir::Bcu::ParamSelectorIrq, Nice::SeqInt(1,0));
        U9_LLOG("ParamSelectorIrq : " << su::seq_int_to_str(Nice::SeqInt(1,0)));

        output = su::uint_to_seqint((_slopeComputer->get_pixel_save_enabled() << 31) + _slopeComputer->get_pixel_save_decimation());
        status = write_bcu(Sharknir::Bcu::PixelFrameDec, output); // setup
        U9_LLOG("PixelFrameDec : " << su::seq_int_to_str(Nice::SeqInt(1, _slopeComputer->get_pixel_save_decimation())) << "; Enabled : " << _slopeComputer->get_pixel_save_enabled());

            status = write_bcu(Sharknir::Bcu::PixelGainPtr, _slopeComputer->get_pixel_gain_ptr());
            U9_LLOG("PixelGainPtr : " << su::seq_int_to_str(_slopeComputer->get_pixel_gain_ptr()));

        status = write_bcu_single(Sharknir::Bcu::PixelMinThreshold, _slopeComputer->get_pixel_min_threshold()); // setup
        U9_LLOG("PixelMinThreshold (PixelMinValue) : " << su::seq_float_to_str(_slopeComputer->get_pixel_min_threshold()));

        status = write_bcu(Sharknir::Bcu::PixelOffsetPtr, _slopeComputer->get_pixel_offset_ptr()); //const
        U9_LLOG("PixelOffsetPtr : " << su::seq_int_to_str(_slopeComputer->get_pixel_offset_ptr()));

            status = write_bcu(Sharknir::Bcu::PreviousPixelAreaPtr, _slopeComputer->get_previous_pixel_area_ptr());
            U9_LLOG("PreviousPixelAreaPtr : " << su::seq_int_to_str(_slopeComputer->get_previous_pixel_area_ptr()));

        status = write_bcu(Sharknir::Bcu::ResetLoop, Nice::SeqInt(1,0));
        U9_LLOG("ResetLoop : " << su::seq_int_to_str(Nice::SeqInt(1,0)));

            status = write_bcu_single(Sharknir::Bcu::SubapIntensityThres, _slopeComputer->get_subap_intensity_thres());
            U9_LLOG("SubapIntensityThres : " << su::seq_float_to_str(_slopeComputer->get_subap_intensity_thres()));


        enable_loop(_slopeComputer->get_tt_control_loop_enable()[0]); // status = write_bcu(Sharknir::Bcu::TTControlLoopEna, _slopeComputer->get_tt_control_loop_enable()); //setup
        U9_LLOG("TTControlLoopEna : " << su::seq_int_to_str(_slopeComputer->get_tt_control_loop_enable()));

        status = write_bcu(Sharknir::Bcu::TimeHistoryAct, Nice::SeqInt(1,0));
        U9_LLOG("TimeHistoryAct : " << su::seq_int_to_str(Nice::SeqInt(1,0)));

        status = write_bcu(Sharknir::Bcu::TimeHistoryPtr, _slopeComputer->get_time_history_ptr()); //const
        U9_LLOG("TimeHistoryPtr : " << su::seq_int_to_str(_slopeComputer->get_time_history_ptr()));

        status = write_bcu(Sharknir::Bcu::TimeHistoryEndPtr, _slopeComputer->get_time_history_end_ptr());
        U9_LLOG("TimeHistoryEndPtr : " << su::seq_int_to_str(_slopeComputer->get_time_history_end_ptr()));

//        status = write_bcu_single(Sharknir::Bcu::TimeHistoryGain, _slopeComputer->get_time_history_gain());
//        U9_LLOG("TimeHistoryGain : " << su::seq_float_to_str(_slopeComputer->get_time_history_gain()));

        status = write_bcu(Sharknir::Bcu::enableRTCommands, _slopeComputer->get_enable_rt_commands());  //setup
        U9_LLOG("enableRTCommands : " << su::seq_int_to_str(_slopeComputer->get_enable_rt_commands()));

            U9_LLOG("Writing Centroid Fact directly to : " << std::hex << _sc.get_centroid_fact_ptr()[0] << std::dec << " (" << _sc.get_centroid_fact_ptr()[0] << ")");
            status = write_bcu_single(_slopeComputer->get_centroid_fact_ptr()[0], _slopeComputer->get_centroid_fact());
            U9_LLOG("CentroidFact : " << su::seq_float_to_str(_slopeComputer->get_centroid_fact()));

        U9_LLOG("Writing pixels to do A directly to " << std::hex << _sc.get_num_pixels_to_do_ptr()[0] << std::dec << " (" << _sc.get_num_pixels_to_do_ptr()[0] << ")");
        status = write_bcu(_slopeComputer->get_num_pixels_to_do_ptr()[0], Nice::SeqInt(4,0));
        U9_LLOG("Num pixels to do A : " << su::seq_int_to_str(Nice::SeqInt(4,0)));

        U9_LLOG("Writing pixels to do B directly to " << std::hex << _sc.get_num_pixels_to_do_ptr()[0] + 0x8000 << std::dec << " (" << _sc.get_num_pixels_to_do_ptr()[0] + 0x8000 << ")");
        status = write_bcu(_slopeComputer->get_num_pixels_to_do_ptr()[0] + 0x8000, Nice::SeqInt(4,0));
        U9_LLOG("Num pixels to do B : " << su::seq_int_to_str(Nice::SeqInt(4,0)));

            U9_LLOG("Writing Pixel gain directly to " << std::hex << _sc.get_pixel_gain_ptr()[0] << std::dec << " (" << _sc.get_pixel_gain_ptr()[0] << ")");
            status = write_bcu_single(_slopeComputer->get_pixel_gain_ptr()[0], _slopeComputer->get_pixel_gain());
            U9_LLOG("PixelGain : " << su::seq_float_to_str(_slopeComputer->get_pixel_gain()));

            U9_LLOG("Writing pixel bias directly to 0x" << std::hex << _sc.get_pixel_offset_ptr()[0] << std::dec << " (" << _sc.get_pixel_offset_ptr()[0] << ")");
            status = write_bcu(_slopeComputer->get_pixel_offset_ptr()[0], _detector->get_pixel_bias());
            U9_LLOG("PixelBias for bcu (PixelOffset): " << su::seq_int_to_str(_detector->get_pixel_bias()));

        U9_LLOG("Writing Time History directly to " << std::hex << _sc.get_time_history_ptr()[0] << std::dec << " (" << _sc.get_time_history_ptr()[0] << ")");
        write_time_history_to_bcu();
//        status = write_bcu_single(_slopeComputer->get_time_history_ptr()[0], _detector->get_time_history());
//        U9_LLOG("Time history for bcu : " << su::seq_float_to_str(_detector->get_time_history()));


        U2_LLOG("############         DM block         ############");

            status = write_bcu(Sharknir::Bcu::DMActuatorMapPtr, _slopeComputer->get_dm_actuator_map_ptr());
            U9_LLOG("DMActuatorMapPtr : " << su::seq_int_to_str(_slopeComputer->get_dm_actuator_map_ptr()));

            status = write_bcu(Sharknir::Bcu::DMActuatorSignPtr, _slopeComputer->get_dm_actuator_sign_map_ptr());
            U9_LLOG("DMActuatorSignPtr : " << su::seq_int_to_str(_slopeComputer->get_dm_actuator_sign_map_ptr()));

            status = write_bcu(Sharknir::Bcu::DMBiasVectorPtr, _slopeComputer->get_dm_bias_vector_ptr());
            U9_LLOG("DMBiasVectorPtr : " << su::seq_int_to_str(_slopeComputer->get_dm_bias_vector_ptr()));

        status = write_bcu_single(Sharknir::Bcu::DMBitOffset, Nice::SeqFloat(1,1.f));
        U9_LLOG("DMBitOffset : " << su::seq_float_to_str(Nice::SeqFloat(1,1.f)));

        status = write_bcu(Sharknir::Bcu::DMCmdSkippedCnt, Nice::SeqInt(1,0));
        U9_LLOG("DMCmdSkippedCnt : " << su::seq_int_to_str(Nice::SeqInt(1,0)));

            status = write_bcu(Sharknir::Bcu::DMCommandVectPtr, _slopeComputer->get_dm_command_vector_ptr());
            U9_LLOG("DMCommandVectPtr : " << su::seq_int_to_str(_slopeComputer->get_dm_command_vector_ptr()));

        status = write_bcu(Sharknir::Bcu::DMInuseNumAct, _dm->get_in_use_actuator_number_for_bcu());
        U9_LLOG("DMInuseNumAct : " << su::seq_int_to_str(_dm->get_in_use_actuator_number_for_bcu()));

        status = write_bcu(Sharknir::Bcu::DMNumActuators, _dm->get_number_of_actuators_for_bcu());
        U9_LLOG("DMNumActuators : " << su::seq_int_to_str(_dm->get_number_of_actuators_for_bcu()));

        status = write_bcu(Sharknir::Bcu::DMNumActuatorsIrq, Nice::SeqInt(1,0));
        U9_LLOG("DMNumActuatorsIrq : " << su::seq_int_to_str(Nice::SeqInt(1,0)));

        status = write_bcu(Sharknir::Bcu::DMOutputEnable, _slopeComputer->get_dm_output_enabled()); //setup
        U9_LLOG("DMOutputEnable : " << su::seq_int_to_str(_slopeComputer->get_dm_output_enabled()));

        status = write_bcu_single(Sharknir::Bcu::DMOutputVector, Nice::SeqFloat(256,0.f));
        U9_LLOG("DMOutputVector : " << su::seq_float_to_str(Nice::SeqFloat(256,0.f)));

            status = write_bcu(Sharknir::Bcu::DMTmpOutputVectorPtr, _slopeComputer->get_dm_tmp_output_vector_ptr());
            U9_LLOG("DMTmpOutputVectorPtr : " << _slopeComputer->get_dm_tmp_output_vector_ptr()[0]);

        status = write_bcu_single(Sharknir::Bcu::DMTotalPower, Nice::SeqFloat(1,0.f));
        U9_LLOG("DMTotalPower : " << su::seq_float_to_str(Nice::SeqFloat(1,0.f)));

        status = write_bcu_single(Sharknir::Bcu::DMm2bitConversion, _slopeComputer->get_dm_meters2bit_conversion());
        U9_LLOG("DMm2bitConversion : " << su::seq_float_to_str(_slopeComputer->get_dm_meters2bit_conversion()));

        status = write_bcu_single(Sharknir::Bcu::MaxAbsDMCommand, Nice::SeqFloat(1, 1.f));
        U9_LLOG("MaxAbsDMCommand : " << su::seq_float_to_str(Nice::SeqFloat(1, 1.f)));

        status = write_bcu(Sharknir::Bcu::MaxAbsDMCommandSkip, Nice::SeqInt(1, 0));
        U9_LLOG("MaxAbsDMCommandSkip : " << su::seq_int_to_str(Nice::SeqInt(1, 0)));

        status = write_bcu_single(Sharknir::Bcu::MaxDMPower, _dm->get_max_power()); //setup
        U9_LLOG("MaxDMPower : " << su::seq_float_to_str(_dm->get_max_power()));

        status = write_bcu_single(Sharknir::Bcu::MaxTTCommandValue, _slopeComputer->get_dm_max_tt_command_value()); // setup
        U9_LLOG("MaxTTCommandValue : " << su::seq_float_to_str(_slopeComputer->get_dm_max_tt_command_value()));

        status = write_bcu_single(Sharknir::Bcu::MinTTCommandValue, _slopeComputer->get_dm_min_tt_command_value()); // setup
        U9_LLOG("MinTTCommandValue : " << su::seq_float_to_str(_slopeComputer->get_dm_min_tt_command_value()));

            status = write_bcu(Sharknir::Bcu::ModeVector0Ptr, _slopeComputer->get_dm_mode_vector0_ptr());
            U9_LLOG("ModeVector0Ptr : " << su::seq_int_to_str(_slopeComputer->get_dm_mode_vector0_ptr()));

            status = write_bcu(Sharknir::Bcu::ModeVector1Ptr, _slopeComputer->get_dm_mode_vector1_ptr());
            U9_LLOG("ModeVector1Ptr : " << su::seq_int_to_str(_slopeComputer->get_dm_mode_vector1_ptr()));

        int modes_vector_len = _dm->get_length_of_modes_vector();
        status = write_bcu(Sharknir::Bcu::ModesVectorLen, Nice::SeqInt(1, modes_vector_len));
        U9_LLOG("ModesVectorLen : " << su::seq_int_to_str(Nice::SeqInt(1, modes_vector_len)));

        status = write_bcu(Sharknir::Bcu::NewModeVectorPtr, _slopeComputer->get_dm_new_mode_vector_ptr()); // const
        U9_LLOG("NewModeVectorPtr : " << su::seq_int_to_str(_slopeComputer->get_dm_new_mode_vector_ptr()));

        status = write_bcu(Sharknir::Bcu::NewModeVectorUpd, Nice::SeqInt(1, 0));
        U9_LLOG("NewModeVectorUpd : " << su::seq_int_to_str(Nice::SeqInt(1, 0)));

        U9_LLOG("Writing DM New mode vector directly to 0x" << std::hex << _sc.get_dm_new_mode_vector_ptr()[0] << std::dec << " (" << _sc.get_dm_new_mode_vector_ptr()[0] << ")");
        status = write_bcu_single(_slopeComputer->get_dm_new_mode_vector_ptr()[0], Nice::SeqFloat(modes_vector_len, 0.f));
        U9_LLOG("NewModeVector : " << su::seq_float_to_str(Nice::SeqFloat(modes_vector_len, 0.f)));

            U9_LLOG("Writing DM actuator map directly to 0x" << std::hex << _sc.get_dm_actuator_map_ptr()[0] << std::dec << " (" << _sc.get_dm_actuator_map_ptr()[0] << ")");
            status = write_bcu(_slopeComputer->get_dm_actuator_map_ptr()[0], _dm->get_actuator_map());
            U9_LLOG("DMActuatorMap : " << su::seq_int_to_str(_dm->get_actuator_map()));

            U9_LLOG("Writing DM actuator sign map directly to 0x" << std::hex << _sc.get_dm_actuator_sign_map_ptr()[0] << std::dec << " (" << _sc.get_dm_actuator_sign_map_ptr()[0] << ")");
            status = write_bcu_single(_slopeComputer->get_dm_actuator_sign_map_ptr()[0], _dm->get_signmap_for_bcu());
            U9_LLOG("DMActuatorSign map : " << su::seq_float_to_str(_dm->get_signmap_for_bcu()));

            U9_LLOG("Writing DM bias vector directly to 0x" << std::hex << _sc.get_dm_bias_vector_ptr()[0] << std::dec << " (" << _sc.get_dm_bias_vector_ptr()[0] << ")");
            status = write_bcu_single(_slopeComputer->get_dm_bias_vector_ptr()[0], _dm->get_flat());
            U9_LLOG("DMBiasVector : " << su::seq_float_to_str(_dm->get_flat()));

            U9_LLOG("Writing DM mode vector0 directly to 0x" << std::hex << _sc.get_dm_mode_vector0_ptr()[0] << std::dec << " (" << _sc.get_dm_mode_vector0_ptr()[0] << ")");
            status = write_bcu_single(_slopeComputer->get_dm_mode_vector0_ptr()[0], Nice::SeqFloat(modes_vector_len, 0.f));
            U9_LLOG("DM ModeVector0 : " << su::seq_float_to_str(Nice::SeqFloat(modes_vector_len, 0.f)));

            U9_LLOG("Writing DM mode vector1 directly to 0x" << std::hex << _sc.get_dm_mode_vector1_ptr()[0] << std::dec << " (" << _sc.get_dm_mode_vector1_ptr()[0] << ")");
            status = write_bcu_single(_slopeComputer->get_dm_mode_vector1_ptr()[0], Nice::SeqFloat(modes_vector_len, 0.f));
            U9_LLOG("DM ModeVector1 : " << su::seq_float_to_str(Nice::SeqFloat(modes_vector_len, 0.f)));

            U9_LLOG("Writing DM tmp outuput vector directly to 0x" << std::hex << _sc.get_dm_tmp_output_vector_ptr()[0] << std::dec << " (" << _sc.get_dm_tmp_output_vector_ptr()[0] << ")");
            status = write_bcu(_slopeComputer->get_dm_tmp_output_vector_ptr()[0], _slopeComputer->get_dm_tmp_output_vector());
            U9_LLOG("DMTmpOutputVector : " << su::seq_int_to_str(_slopeComputer->get_dm_tmp_output_vector()));

            status = write_bcu(Sharknir::Bcu::DMInfluenceMatrix0Ptr, _slopeComputer->get_dm_influence_matrix0_ptr());
            U9_LLOG("DMInfluenceMatrix0Ptr : " << _slopeComputer->get_dm_influence_matrix0_ptr()[0]);

            status = write_bcu(Sharknir::Bcu::DMInfluenceMatrix1Ptr, _slopeComputer->get_dm_influence_matrix1_ptr());
            U9_LLOG("DMInfluenceMatrix1Ptr : " << _slopeComputer->get_dm_influence_matrix1_ptr()[0]);

            U9_LLOG("Getting Influence Matrix");
            influenceMatrixForBCU = _dm->get_influence_matrix();
            U9_LLOG("Writing DM influence matrix0 directly to 0x" << std::hex << _sc.get_dm_influence_matrix0_ptr()[0] << std::dec << " (" << _sc.get_dm_influence_matrix0_ptr()[0] << ")");
            status = write_bcu_single(_slopeComputer->get_dm_influence_matrix0_ptr()[0], influenceMatrixForBCU[0]);
            U9_LLOG("DM influence matrix0 : " << su::seq_float_to_str(influenceMatrixForBCU[0]));

            U9_LLOG("Writing DM influence matrix1 directly to 0x" << std::hex << _sc.get_dm_influence_matrix1_ptr()[0] << std::dec << " (" << _sc.get_dm_influence_matrix1_ptr()[0] << ")");
            status = write_bcu_single(_slopeComputer->get_dm_influence_matrix1_ptr()[0], influenceMatrixForBCU[1]);
            U9_LLOG("DM influence matrix1 : " << su::seq_float_to_str(influenceMatrixForBCU[1]));
        // END
    }
    catch(...)
    {
        LogBoostDiagnosticInformationTipTilt(__LINE__);
        U8_LLOG("");
        throw;
    }
    influenceMatrixForBCU.clear();
    U8_LLOG(__FUNCTION__ << " ~ DONE");
    return status;
}


int tiptiltCtrl::set_centroid_pos(const float& centroidX, const float& centroidY)
{
    U6_LLOG(__FUNCTION__ << " ~ [" << centroidX << ", " << centroidY << "]");
    int status = EXIT_SUCCESS;

    Nice::SeqFloat seqCentroid;

    seqCentroid.push_back(centroidX);
    seqCentroid.push_back(centroidY);
    try
    {
        is_init(true);
        m_ptrSlopeComputer->set_centroid_origin(seqCentroid);
        status = write_bcu_single(Sharknir::Bcu::CentroidOrigin, m_ptrSlopeComputer->get_centroid_origin());

        std::stringstream updated_cx;
        updated_cx << centroidX;
        m_currentSetup["TTCENTROIDORIGX"] = updated_cx.str();

        std::stringstream updated_cy;
        updated_cy << centroidY;
        m_currentSetup["TTCENTROIDORIGY"] = updated_cy.str();
        
        //U9_LLOG("Updated CentroidOrigX and CentroidOrigY saved ON m_currentSetup : " << m_currentSetup["TTCENTROIDORIGX"] << " and " << m_currentSetup["TTCENTROIDORIGY"]);
    }
    catch(...)
    {
        LogBoostDiagnosticInformationTipTilt(__LINE__);
        U8_LLOG("");
        throw;
    }

    U8_LLOG("");
    return status;
}


int tiptiltCtrl::switch_ccd_mode(const std::map<std::string, std::string>& _mapSetup)
{
    U6_LLOG(__FUNCTION__);
    int status = EXIT_SUCCESS;

    write_bcu(Sharknir::Bcu::CCDInterfaceEnable, Nice::SeqInt(1, 0), "dsp");// disable CCD pixels transfer to DSM memory

    write_bcu(Sharknir::Bcu::DiagRecordReadPtr, Nice::SeqInt(2, 0), "sram");
    //write_bcu(Sharknir::Bcu::DiagRecordWritePtr, Nice::SeqInt(1, 0), "sram");


    m_ptrDetector->init(_mapSetup);
    if(_mapSetup.find("TTM2CFILE") != _mapSetup.end())
    {
        m_ptrDm->init_modalMode(_mapSetup.find("TTM2CFILE")->second);
        m_ptrSlopeComputer->setup_mode_vector();
    }
    else
    {
        m_ptrDm->init_zonalMode();
        m_ptrSlopeComputer->setup_mode_vector();
    }
    m_ptrSlopeComputer->update_n_slopes();

    enable_loop(false);
    U2_LLOG("Disabled TT control loop");

    config_camera();

    const shins::util::rtcDetector & _det = *m_ptrDetector;
    const shins::util::rtcDM & _dm = *m_ptrDm;
    const shins::util::rtcSlopeComputer & _sc = *m_ptrSlopeComputer;
    int modes_vector_len = _dm.get_length_of_modes_vector();

    status = write_bcu(Sharknir::Bcu::NumCentroidPixels, _det.get_slope_pixels_number_for_bcu());
    U2_LLOG("NumCentroidPixels" << su::seq_int_to_str(_det.get_slope_pixels_number_for_bcu()));

    status = write_bcu_single(Sharknir::Bcu::SubapIntensityThres, _sc.get_subap_intensity_thres());
    U9_LLOG("SubapIntensityThres : " << su::seq_float_to_str(_sc.get_subap_intensity_thres()));

    status = write_bcu(Sharknir::Bcu::NiosIrqDelay, _det.get_irq_nios_delay_for_bcu());
    U9_LLOG("NiosIrqDelay : " << su::seq_int_to_str(_det.get_irq_nios_delay_for_bcu()));

//    status = write_bcu_single(Sharknir::Bcu::CentroidGain, _sc.get_centroid_gain());
//    U9_LLOG("CentroidGain : " << su::seq_float_to_str(_sc.get_centroid_gain()));

//    status = write_bcu_single(Sharknir::Bcu::CentroidOrigin, _sc.get_centroid_origin());
//    U9_LLOG("CentroidOrigin : " << su::seq_float_to_str(_sc.get_centroid_origin()));

    status = write_bcu(Sharknir::Bcu::DiagBuffHeaderPtr, _sc.get_diagnostic_header_ptr());
    U9_LLOG("DiagBuffHeaderPtr : " << su::seq_int_to_str(_sc.get_diagnostic_header_ptr()));

    status = write_bcu(Sharknir::Bcu::DMCommandVectPtr, _sc.get_dm_command_vector_ptr());
    U9_LLOG("DMCommandVectPtr : " << su::seq_int_to_str(_sc.get_dm_command_vector_ptr()));

    status = write_bcu(Sharknir::Bcu::DiagBuffDataPtr, _sc.get_diagnostic_data_ptr());
    U9_LLOG("DiagBuffDataPtr : " << su::seq_int_to_str(_sc.get_diagnostic_data_ptr()));

    status = write_bcu(Sharknir::Bcu::DiagBuffFooterPtr, _sc.get_diagnostic_footer_ptr());
    U9_LLOG("DiagBuffFooterPtr : " << su::seq_int_to_str(_sc.get_diagnostic_footer_ptr()));

    status = write_bcu(Sharknir::Bcu::DiagRecordPtr, Nice::SeqInt(1, 0x80000000 + _sc.get_diagnostic_header_ptr()[0]), "sram");
    U9_LLOG("DiagRecordPtr : " << su::seq_float_to_str(_sc.get_centroid_fact()));

    status = write_bcu(Sharknir::Bcu::ModeVector0Ptr, _sc.get_dm_mode_vector0_ptr());
    U9_LLOG("ModeVector0Ptr : " << su::seq_int_to_str(_sc.get_dm_mode_vector0_ptr()));

    status = write_bcu_single(_sc.get_dm_mode_vector0_ptr()[0], Nice::SeqFloat(modes_vector_len, 0.f));
    U9_LLOG("ModeVector0 : " << su::seq_float_to_str(Nice::SeqFloat(modes_vector_len, 0.f)));

    status = write_bcu(Sharknir::Bcu::DMInfluenceMatrix0Ptr, _sc.get_dm_influence_matrix0_ptr());
    U9_LLOG("DMInfluenceMatrix0Ptr : " << _sc.get_dm_influence_matrix0_ptr()[0]);

    status = write_bcu(Sharknir::Bcu::DMInfluenceMatrix1Ptr, _sc.get_dm_influence_matrix1_ptr());
    U9_LLOG("DMInfluenceMatrix1Ptr : " << _sc.get_dm_influence_matrix1_ptr()[0]);

    U9_LLOG("Getting Influence Matrix");
    Nice::SeqSeqFloat influenceMatrixForBCU = _dm.get_influence_matrix();
    U9_LLOG("Writing DM influence matrix0 directly to 0x" << std::hex << _sc.get_dm_influence_matrix0_ptr()[0] << std::dec << " (" << _sc.get_dm_influence_matrix0_ptr()[0] << ")");
    status = write_bcu_single(_sc.get_dm_influence_matrix0_ptr()[0], influenceMatrixForBCU[0]);
    U9_LLOG("DM influence matrix0 : " << su::seq_float_to_str(influenceMatrixForBCU[0]));

    U9_LLOG("Writing DM influence matrix1 directly to 0x" << std::hex << _sc.get_dm_influence_matrix1_ptr()[0] << std::dec << " (" << _sc.get_dm_influence_matrix1_ptr()[0] << ")");
    status = write_bcu_single(_sc.get_dm_influence_matrix1_ptr()[0], influenceMatrixForBCU[1]);
    U9_LLOG("DM influence matrix1 : " << su::seq_float_to_str(influenceMatrixForBCU[1]));

    status = write_bcu(Sharknir::Bcu::PixelGainPtr, _sc.get_pixel_gain_ptr());
    U9_LLOG("PixelGainPtr : " << su::seq_int_to_str(_sc.get_pixel_gain_ptr()));

    U9_LLOG("Writing Pixel gain directly to " << std::hex << _sc.get_pixel_gain_ptr()[0] << std::dec << " (" << _sc.get_pixel_gain_ptr()[0] << ")");
    status = write_bcu_single(_sc.get_pixel_gain_ptr()[0], _sc.get_pixel_gain());
    U9_LLOG("PixelGain : " << su::seq_float_to_str(_sc.get_pixel_gain()));

    status = write_bcu(Sharknir::Bcu::CentroidFactPtr, _sc.get_centroid_fact_ptr());
    U9_LLOG("CentroidFactPtr : " << su::seq_int_to_str(_sc.get_centroid_fact_ptr()));

    U9_LLOG("Writing Centroid Fact directly to : " << std::hex << _sc.get_centroid_fact_ptr()[0] << std::dec << " (" << _sc.get_centroid_fact_ptr()[0] << ")");
    status = write_bcu_single(_sc.get_centroid_fact_ptr()[0], _sc.get_centroid_fact());
    U9_LLOG("Centroid Fact : " << su::seq_float_to_str(_sc.get_centroid_fact()));

    status = write_bcu(Sharknir::Bcu::PreviousPixelAreaPtr, _sc.get_previous_pixel_area_ptr());
    U9_LLOG("PreviousPixelAreaPtr : " << su::seq_int_to_str(_sc.get_previous_pixel_area_ptr()));

    status = write_bcu(Sharknir::Bcu::ModeVector1Ptr, _sc.get_dm_mode_vector1_ptr());
    U9_LLOG("ModeVector1Ptr : " << _sc.get_dm_mode_vector1_ptr()[0]);

    status = write_bcu_single(_sc.get_dm_mode_vector1_ptr()[0], Nice::SeqFloat(modes_vector_len, 0.f));
    U9_LLOG("ModeVector1 : " << su::seq_float_to_str(Nice::SeqFloat(modes_vector_len, 0.f)));

    status = write_bcu(Sharknir::Bcu::DMBiasVectorPtr, _sc.get_dm_bias_vector_ptr());
    U9_LLOG("DMBiasVectorPtr : " << su::seq_int_to_str(_sc.get_dm_bias_vector_ptr()));

    U9_LLOG("Writing DM bias vector directly to 0x" << std::hex << _sc.get_dm_bias_vector_ptr()[0] << std::dec << " (" << _sc.get_dm_bias_vector_ptr()[0] << ")");
    status = write_bcu_single(_sc.get_dm_bias_vector_ptr()[0], _dm.get_flat());
    U9_LLOG("DMBiasVector : " << su::seq_float_to_str(_dm.get_flat()));


    status = write_bcu(Sharknir::Bcu::DMActuatorSignPtr, _sc.get_dm_actuator_sign_map_ptr());
    U9_LLOG("DMActuatorSignPtr : " << su::seq_int_to_str(_sc.get_dm_actuator_sign_map_ptr()));

    U9_LLOG("Writing DM actuator sign map directly to 0x" << std::hex << _sc.get_dm_actuator_sign_map_ptr()[0] << std::dec << " (" << _sc.get_dm_actuator_sign_map_ptr()[0] << ")");
    status = write_bcu_single(_sc.get_dm_actuator_sign_map_ptr()[0], _dm.get_signmap_for_bcu());
    U9_LLOG("DMActuatorSign map : " << su::seq_float_to_str(_dm.get_signmap_for_bcu()));

    status = write_bcu(Sharknir::Bcu::DMTmpOutputVectorPtr, _sc.get_dm_tmp_output_vector_ptr());
    U9_LLOG("DMTmpOutputVectorPtr : " << _sc.get_dm_tmp_output_vector_ptr()[0]);

    U9_LLOG("Writing DM tmp outuput vector directly to 0x" << std::hex << _sc.get_dm_tmp_output_vector_ptr()[0] << std::dec << " (" << _sc.get_dm_tmp_output_vector_ptr()[0] << ")");
    status = write_bcu(_sc.get_dm_tmp_output_vector_ptr()[0], _sc.get_dm_tmp_output_vector());
    U9_LLOG("DMTmpOutputVector : " << su::seq_int_to_str(_sc.get_dm_tmp_output_vector()));

    status = write_bcu(Sharknir::Bcu::DMActuatorMapPtr, _sc.get_dm_actuator_map_ptr());
    U9_LLOG("DMActuatorMapPtr : " << su::seq_int_to_str(_sc.get_dm_actuator_map_ptr()));

    U9_LLOG("Writing DM actuator map directly to 0x" << std::hex << _sc.get_dm_actuator_map_ptr()[0] << std::dec << " (" << _sc.get_dm_actuator_map_ptr()[0] << ")");
    status = write_bcu(_sc.get_dm_actuator_map_ptr()[0], _dm.get_actuator_map());
    U9_LLOG("DMActuatorMap : " << su::seq_int_to_str(_dm.get_actuator_map()));





    U9_LLOG("Writing pixel bias directly to 0x" << std::hex << _sc.get_pixel_offset_ptr()[0] << std::dec << " (" << _sc.get_pixel_offset_ptr()[0] << ")");
    status = write_bcu(_sc.get_pixel_offset_ptr()[0], _det.get_pixel_bias());
    U9_LLOG("PixelBias for bcu (PixelOffset): " << su::seq_int_to_str(_det.get_pixel_bias()));

    status = write_bcu(Sharknir::Bcu::OcamInUsePixelMap, _det.get_in_use_pixel_map_for_bcu(), "shmram");
    U9_LLOG("OcamInUsePixelMap : " << su::seq_int_to_str(_det.get_in_use_pixel_map_for_bcu()));

    status = write_bcu(Sharknir::Bcu::OcamNumSlopeToDo, _det.get_slopes_todo_map_for_bcu(), "shmram");
    U9_LLOG("OcamNumSlopeToDo : " << su::seq_int_to_str(_det.get_slopes_todo_map_for_bcu()));

    status = write_bcu(Sharknir::Bcu::OcamToSavePixelMap, _det.get_pixel_to_save_map_for_bcu(), "shmram");
    U9_LLOG("OcamToSavePixelMap : " << su::seq_int_to_str(_det.get_pixel_to_save_map_for_bcu()));

    write_bcu(Sharknir::Bcu::OcamSlopePixelsMap, _det.get_slope_pixel_map_for_bcu(), "shmram");
    U9_LLOG("OcamSlopePixelsMap : " << su::seq_int_to_str(_det.get_slope_pixel_map_for_bcu()));


    write_bcu(Sharknir::Bcu::CCDWaitForReset, Nice::SeqInt(1, 1), "dsp");
    write_bcu(Sharknir::Bcu::ContrlReg, Nice::SeqInt(1, 1), "shmram"); // start CCD acquisition
    write_bcu(Sharknir::Bcu::CCDInterfaceEnable, Nice::SeqInt(1, 1), "dsp");


    U8_LLOG("");
    return status;
}


int tiptiltCtrl::write_bcu_single(const int& _address, const Nice::SeqFloat& _buffer, const std::string& _mem)
{
    return write_bcu(_address, shins::util::reinterpret_T_to_R<float, int>(_buffer), _mem);
}

//! \brief write_bcu - helper method to write in BCU memory
//! \param _address - address in BCU memory to write to
//! \param _buffer - data to be written
//! \return
int tiptiltCtrl::write_bcu(const int& _address, const Nice::SeqInt& _buffer, const std::string& _mem)
{
    m_BcuWriteMutex.lock();
    std::stringstream log_message;
    if(_buffer.size() == 1)
    {
        log_message << "tiptiltCtrl::" << __FUNCTION__ << " ~ Writing " << _buffer[0] << " at " << _address << " on " << _mem;
    }
    else
    {
        log_message << "tiptiltCtrl::" << __FUNCTION__ << " N " << _buffer.size() << " SAU at " << _address << " on " << _mem;
    }
    U6_LLOG(log_message.str());
    try
    {
    int status = EXIT_SUCCESS;
    Nice::SeqInt output(0, 0);
    Nice::SeqInt pre_output;
    const int max_udp_payload = 360;
    bool bPostCheckMemory = false;
    try
    {
        if(m_bCheckBcuWrite)
        {
            pre_output.resize(_buffer.size());
            if((_mem == "dsp") || (_mem == "sram"))
            {
                U9_LLOG(__FUNCTION__ << " Pre Read Start");
                read_bcu_preallocated(_address, _mem, pre_output);
            }
        }
        if(_mem == "dsp")
        {
//            U9_LLOG(__FUNCTION__ << " Write Start");
            m_bcuService->writeDsp(Sharknir::Bcu::Const::BCU_DSP,
                                   Sharknir::Bcu::Const::BCU_DSP,
                                   _address, _buffer);
            sleep(0.01f * _buffer.size() + 50);
            // following addresses are read back as zero so are avoided
            if(bPostCheckMemory && (_address != Sharknir::Bcu::NewModeVectorUpd) && (_address != Sharknir::Bcu::DMUpdateStart))
            {
                output.resize(_buffer.size());
                U9_LLOG(__FUNCTION__ << " Read Start");
                read_bcu_preallocated(_address, _mem, output);
            }
        }
        if(_mem == "shmram")
        {
//            U2_LLOG("Writing to SHared Memory RAM " << _address);
            int to_write_size = _buffer.size();
            int written_data = 0;
            Nice::SeqInt block;
            int block_size = std::min(to_write_size, max_udp_payload);
            while(written_data < to_write_size)
            {
                block.clear();
                block.insert(block.begin(), _buffer.begin() + written_data, _buffer.begin() + written_data + block_size);
                m_bcuService->writeShMRam(Sharknir::Bcu::Const::BCU_DSP,
                                          Sharknir::Bcu::Const::BCU_DSP,
                                          _address + written_data, block);
                written_data += block_size;
                if(written_data + block_size > to_write_size)
                    block_size = to_write_size - written_data;
                sleep(1);
            }
//            U2_LLOG("Shared mem written ");
            // output = read_bcu(_address, _buffer.size(), _mem);
        }
        if(_mem == "sram")
        {
            m_bcuService->writeSram(Sharknir::Bcu::Const::BCU_DSP,
                                    Sharknir::Bcu::Const::BCU_DSP,
                                    _address, _buffer);
            sleep(_buffer.size()); // sleep _buffer.size() millisecs
            if(bPostCheckMemory)
            {
                output = read_bcu(_address, _buffer.size(), _mem);
            }
        }
    }
    catch(const Sharknir::BcuServiceException& ex)
    {
        E_LLOG(ex.toString());
        E_LLOG("BCU Address " << std::showbase << std::hex << _address << std::noshowbase << std::dec);
        U8_LLOG("");
        E_THROW(Sharknir::BcuServiceException, ex.what());
    }
    catch(const std::exception& e)
    {
        E_LLOG(__FUNCTION__ << ":" << e.what());
        E_LLOG("Memory address on BCU: " << std::showbase << std::hex << _address << std::noshowbase << std::dec);
        U8_LLOG("");
        throw e;
    }
    catch(...)
    {
        LogBoostDiagnosticInformationTipTilt(__LINE__);
        U8_LLOG("");
        throw;
    }

    // avoiding to check read back values for addresses at which the read-back is not
    // performed
    if(bPostCheckMemory)
    {
        if(((_mem == "dsp") || (_mem == "sram")) &&
            (_address != Sharknir::Bcu::NewModeVectorUpd) &&
            (_address != Sharknir::Bcu::DMUpdateStart))
        {
            if(!(std::equal(_buffer.begin(), _buffer.end(), output.begin())))
            {
                status = EXIT_FAILURE;
                std::vector<Ice::Int>::iterator itOut = output.begin();
                std::vector<Ice::Int>::const_iterator it;
                for(it = _buffer.begin(); it < _buffer.end(); it++)
                {
                    if(*(it) != *(itOut))
                    {
                        U2_LLOG("Element " << it - _buffer.begin() << " differs : " << *it << " != " << *itOut);
                        break;
                    }
                    itOut++;
                }
            }
        }
    }

    if(m_bCheckBcuWrite && ((_mem == "dsp") || (_mem == "sram")))
    {
        std::vector<Ice::Int>::iterator itPreOut = pre_output.begin();
        std::vector<Ice::Int>::const_iterator it;
        int counter = 0, error_counter = 0;
        for(it = _buffer.begin(); it < _buffer.end(); it++)
        {
            if(*(it) != *(itPreOut))
            {
                float floatPreOut = 0, floatPostOut = 0;
                std::memcpy(&floatPreOut, &*itPreOut, sizeof(float));
                std::memcpy(&floatPostOut, &*it, sizeof(float));
                U7_LLOG("Element " << counter << " at " << _address + counter << " ( 0x" << std::hex << _address << " ) " << std::dec << " differs, was : " << *itPreOut << " (" << floatPreOut << ") becomed : " << *it << " (" << floatPostOut << ")");
                error_counter++;
                if(error_counter == 4)break;
            }
            counter++;
            itPreOut++;
        }
    }
    m_BcuWriteMutex.unlock();
    U8_LLOG("");
    return status;
    }
    catch(...)
    {
        m_BcuWriteMutex.unlock();
        LogBoostDiagnosticInformationTipTilt(__LINE__);
        U8_LLOG("");
        throw;
    }
}


// int tiptiltCtrl::write_cred(const int& _address, const Nice::SeqInt & _buffer)
// {
//   int status = EXIT_SUCCESS;
//   Nice::SeqInt output(0,0);
//   try
//     {
//       m_bcuService->writeShMRam(Sharknir::Bcu::Const::BCU_DSP,
//                                 Sharknir::Bcu::Const::BCU_DSP,
//                                 _address, _buffer);
//       sleep(_buffer.size()); // sleep _buffer.size() millisecs
//       output = read_ocam(_address, _buffer.size());
//     }
//   catch(const Sharknir::BcuServiceException& ex)
//     {
//       E_LLOG(ex.toString());
//       E_LLOG("BCU Address " << std::showbase << std::hex << _address << std::noshowbase << std::dec);
//       E_THROW(Sharknir::BcuServiceException, ex.what());
//       // throw ex;
//     }
//   catch(const std::exception& e)
//     {
//       E_LLOG(__FUNCTION__ << ":" << e.what());
//       E_LLOG("Memory address on BCU: " << std::showbase << std::hex << _address << std::noshowbase << std::dec);
//       throw e;
//     }
// }

Nice::SeqFloat tiptiltCtrl::read_bcu_single(const int & _address, const int & _lengthInDWords, const std::string& _mem)
{
    return shins::util::reinterpret_T_to_R<int, float>(read_bcu(_address, _lengthInDWords, _mem));
}

//! \brief read_bcu - helper method to read from address in BCU sequence (C++ standard vector)
//!                   of _lengthInDWords elements.
//! \param _address - address in BCU memory to read from
//! \param _lengthInDWords - length of memory block to be read, expressed in double words
//! \return
Nice::SeqInt tiptiltCtrl::read_bcu(const int& _address, const int& _lengthInDWords, const std::string& _mem)
{
    m_BcuReadMutex.lock();
    U6_LLOG(__FUNCTION__ << " address : " << _address << "; Num SAU : " << _lengthInDWords << "; on mem : " << _mem);
    try
    {
    Nice::SeqInt seq_int;
    if(_mem == "dsp")
    {
        seq_int = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, _address, _lengthInDWords);
    }
    else if(_mem == "shmram")
    {
        seq_int = m_bcuService->readShMRam(Sharknir::Bcu::Const::BCU_DSP, _address, _lengthInDWords);
    }
    else if(_mem == "sram")
    {
        seq_int = m_bcuService->readSram(Sharknir::Bcu::Const::BCU_DSP, _address, _lengthInDWords);
    }
    else if(_mem == "sdram")
    {
        seq_int = m_bcuService->readSdram(Sharknir::Bcu::Const::BCU_DSP, _address, _lengthInDWords);
    }
    else
    {
        E_LLOG(__FUNCTION__);
        E_THROW(Sharknir::BcuServiceException, "Error reading from BCU: wrong mem parameter " << _mem);
    }
    U8_LLOG("");
    m_BcuReadMutex.unlock();
    return seq_int;
    }
    catch(...)
    {
        LogBoostDiagnosticInformationTipTilt(__LINE__);
        m_BcuReadMutex.unlock();
        U8_LLOG("");
        throw;
    }
}


int tiptiltCtrl::read_bcu_preallocated(const int& _address, const std::string& _mem, Nice::SeqInt & result)
{
    m_BcuReadMutex.lock();
    int result_len = result.size();
    U6_LLOG(__FUNCTION__ << " address : " << _address << "; Num SAU : " << result_len << "; on mem : " << _mem);
    try
    {
    const int max_udp_payload = 364;
    Nice::SeqInt seq_int;
    int status = EXIT_SUCCESS;
    int block = std::min(max_udp_payload, result_len);
    int read_data = 0;
    while(read_data < result_len)
    {
        if(_mem == "dsp")
        {
            seq_int = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, _address + read_data, block);
        }
        else if(_mem == "shmram")
        {
            seq_int = m_bcuService->readShMRam(Sharknir::Bcu::Const::BCU_DSP, _address + read_data, block);
        }
        else if(_mem == "sram")
        {
            seq_int = m_bcuService->readSram(Sharknir::Bcu::Const::BCU_DSP, _address + read_data, block);
        }
        else if(_mem == "sdram")
        {
            seq_int = m_bcuService->readSdram(Sharknir::Bcu::Const::BCU_DSP, _address + read_data, block);
        }
        else
        {
            E_LLOG(__FUNCTION__);
            E_THROW(Sharknir::BcuServiceException, "Error reading from BCU: wrong mem parameter " << _mem);
        }
        std::memcpy(&(*(result.begin() + read_data)), &*seq_int.begin(), block*sizeof(int));
        sleep(1);
        read_data += block;
        if(read_data + block > result_len)
            block = result_len - read_data;
    }
    U8_LLOG(__FUNCTION__ << " ~ Read " << result.size() << " SAU");
    m_BcuReadMutex.unlock();
    return status;
    }
    catch(...)
    {
        LogBoostDiagnosticInformationTipTilt(__LINE__);
        m_BcuReadMutex.unlock();
        U8_LLOG("");
        throw;
    }
}


int tiptiltCtrl::reset_bcu()
{
    U6_LLOG(__FUNCTION__);
    int status = EXIT_SUCCESS;
    try
    {
        U2_LLOG("Sending reset command to BCU");
        Nice::SeqInt ResetAllDevices(2,0);
        ResetAllDevices[0] = 15;
        m_bcuService->resetDevices(Sharknir::Bcu::Const::BCU_DSP,
                                   Sharknir::Bcu::Const::BCU_DSP,
                                   Sharknir::Bcu::ResetDevice,
    //                               Nice::SeqInt(1, 2));
                                   ResetAllDevices);
    }
    catch(const Sharknir::BcuServiceException& ex)
    {
        usleep(10000000);
        I_LLOG("BCU restarting...");
    }
    catch(...)
    {
        LogBoostDiagnosticInformationTipTilt(__LINE__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
    return  status;
}


int tiptiltCtrl::enable_loop(const bool& _enableLoop)
{
    U6_LLOG(__FUNCTION__);
    int status = EXIT_SUCCESS;
    Nice::SeqInt rtctt_enable_loop(1, _enableLoop);
    try
    {
        // m_ptrSlopeComputer->get_centroid_origin();
        m_ptrSlopeComputer->set_tt_control_loop_enable(_enableLoop);
        status = write_bcu(Sharknir::Bcu::TTControlLoopEna, rtctt_enable_loop);
    }
    catch(...)
    {
        LogBoostDiagnosticInformationTipTilt(__LINE__);
        U8_LLOG("");
        throw;
    }

    U8_LLOG("");
    return status;
}


Nice::SeqInt tiptiltCtrl::check_loop()
{
    U6_LLOG(__FUNCTION__);
    Nice::SeqInt loop_enabled = read_bcu(Sharknir::Bcu::TTControlLoopEna, 1);
    m_ptrSlopeComputer->set_tt_control_loop_enable(loop_enabled[0]);
    U8_LLOG("");
    return loop_enabled;
}

//! brief write_time_history_to_bcu - writes a tiptilt ime history to
//!                                   BCU for tests purposes. Time history is
//! loaded from a single HDF5 file to specific  member of class rtcDetector,
//! then it is written to BCU, along with its gain
int tiptiltCtrl::write_time_history_to_bcu(const std::string & file_name)
{
    U6_LLOG(__FUNCTION__);
    int status = EXIT_SUCCESS;
    try
    {
        is_init(true);
        m_ptrSlopeComputer->set_time_history_file(file_name);
        write_time_history_to_bcu();
    }
    catch(...)
    {
        LogBoostDiagnosticInformationTipTilt(__LINE__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
    return status;
}

int tiptiltCtrl::write_time_history_to_bcu()
{
    U6_LLOG(__FUNCTION__);
    int status = EXIT_SUCCESS;

    try
    {
        is_init(true);
    }
    catch(...)
    {
        LogBoostDiagnosticInformationTipTilt(__LINE__);
        U8_LLOG("");
        throw;
    }
    std::string line;
    int i = 0;
    std::ifstream ifstrm;
    std::istringstream isstream;
    float read_value;
    std::string TIPTILT_CONFIG = su::get_env("INSROOT")+"/etc/tiptilt_ctrl/rtc/";
    std::string strTimeHistoryFile = TIPTILT_CONFIG + m_ptrSlopeComputer->get_time_history_file();
    Nice::SeqFloat time_history(m_ptrSlopeComputer->get_max_time_history_len(), 0);
    int max_history_len = m_ptrSlopeComputer->get_max_time_history_len();
    if(boost::filesystem::exists(boost::filesystem::path(strTimeHistoryFile)))
    {
        ifstrm.open(strTimeHistoryFile.c_str());
        while (std::getline(ifstrm, line))
        {
            isstream.clear();
            isstream.str(line);

            isstream >> read_value;
            time_history[i] = read_value;
            if(isstream.fail())
            {
                E_LLOG("Failed reading line " << i + 1  << " from " << strTimeHistoryFile << "; read_data = " << i);
                break;
            }
            i++;
            if(i == max_history_len)
            {
                E_LLOG("Time History DATA truncated to : " << max_history_len);
                break;
            }
            line = "";
        }
        ifstrm.close();
    }
    else if(!m_ptrSlopeComputer->get_time_history_file().empty())
    {
        E_LLOG("File " << strTimeHistoryFile << " does not exists");
        U8_LLOG("");
        throw (std::invalid_argument("File " + strTimeHistoryFile + " does not exists"));
    }
    m_ptrSlopeComputer->set_loaded_time_history_len(i);
    time_history.resize(i);


    U9_LLOG("Writing Time History directly to " << std::hex << m_ptrSlopeComputer->get_time_history_ptr()[0] << std::dec << " (" << m_ptrSlopeComputer->get_time_history_ptr()[0] << ")");
    status = write_bcu_single(Sharknir::Bcu::TimeHistory, time_history);//,
    U9_LLOG("Time history for bcu : " << su::seq_float_to_str(time_history));
    status = write_bcu_single(Sharknir::Bcu::TimeHistoryGain, m_ptrSlopeComputer->get_time_history_gain());
    U9_LLOG("TimeHistoryGain : " << su::seq_float_to_str(m_ptrSlopeComputer->get_time_history_gain()));

    U8_LLOG("");
    return status;
}


int tiptiltCtrl::enable_time_history(const bool& _enable)
{
    U6_LLOG(__FUNCTION__);
    int status = EXIT_SUCCESS;
    // As per Microgate scripts:
    // 1 to enable execution of time history,
    // 2 to disable it
    Nice::SeqInt enable(1, (_enable ? 1 : 2));

    status = write_bcu(Sharknir::Bcu::TimeHistoryAct, enable);
    U8_LLOG("");
    return status;
}

//! \brief apply_mode_vector - applies mode vector to DM
//! \param _modeVector - mode vector to be applied
//! \return
int tiptiltCtrl::apply_mode_vector(const Nice::SeqFloat& _modeVector, bool absolute)
{
    U6_LLOG(__FUNCTION__);
    int status = EXIT_SUCCESS;
    Nice::SeqInt NewModeVectorUpd(1, 1);
    Nice::SeqInt DMUpdateStart(1, 1);

    try
    {
        is_init(true);
        m_ptrSlopeComputer->apply_mode_vector(_modeVector, absolute);
        U9_LLOG(__FUNCTION__ << su::seq_float_to_str(m_ptrSlopeComputer->get_mode_vector(), 50));

        write_bcu_single(m_ptrSlopeComputer->get_dm_new_mode_vector_ptr()[0], m_ptrSlopeComputer->get_mode_vector());

        write_bcu(Sharknir::Bcu::NewModeVectorUpd, NewModeVectorUpd);
        write_bcu(Sharknir::Bcu::DMUpdateStart, DMUpdateStart);
    }
    catch(...)
    {
        LogBoostDiagnosticInformationTipTilt(__LINE__);
        U8_LLOG("");
        throw;
    }

    U8_LLOG("");
    return status;
}


int tiptiltCtrl::filter_known_commands(const std::string & command, std::string & reply)
{
    int result = EXIT_FAILURE;
    int status;
    std::string command_head;
    if(command.size() > 7)
    {
        command_head = su::left(command, 8);
        if(command_head == "set tint")
        {
            send_command_to_camera(command, reply);
            std::string cmd("tint raw");
            std::string reply2;
            status = send_command_to_camera(cmd, reply2);
            status = m_ptrDetector->set_cam_integration_time(su::convertStringToType<float>(reply2));
            return EXIT_SUCCESS;
        }
    }
    if(command == "tint" || command == "tint raw")
    {
        send_command_to_camera(command, reply);
        if(command == "tint")
        {
            size_t pos2 = reply.find("configuration:");
            if(pos2 != string::npos)
            {
                float tint = su::convertStringToType<float>(su::right(reply, reply.size() - pos2 - 14));
                m_ptrDetector->set_cam_integration_time(tint);
                return EXIT_SUCCESS;
            }
            else {
                return EXIT_FAILURE; }
        }
        else
        {
            status = m_ptrDetector->set_cam_integration_time(su::convertStringToType<float>(reply));
            return EXIT_SUCCESS;
        }
    }
    if(command.size() > 7)
    {
        if(command_head == "set fps ")
        {
            // U9_LLOG("Start FPS : " << m_ptrDetector->get_frame_rate());
            send_command_to_camera(command, reply);
            std::string cmd("fps raw");
            std::string reply2;
            status = send_command_to_camera(cmd, reply2);
            status = m_ptrDetector->set_frame_rate(su::convertStringToType<float>(reply2));
            // U9_LLOG("End FPS : " << m_ptrDetector->get_frame_rate());
            return EXIT_SUCCESS;
        }
    }
    if(command == "fps" || command == "fps raw")
    {
        send_command_to_camera(command, reply);
        if(command == "fps")
        {
            size_t pos2 = reply.find("second:");
            if(pos2 != string::npos)
            {
                float freq = su::convertStringToType<float>(su::right(reply, reply.size() - pos2 - 7));
                m_ptrDetector->set_frame_rate(freq);
                return EXIT_SUCCESS;
            }
            else {
                return EXIT_FAILURE; }
        }
        else
        {
            status = m_ptrDetector->set_frame_rate(su::convertStringToType<float>(reply));
            return EXIT_SUCCESS;
        }
    }
    else if(command.size() >= 11 && su::left(command, 11) == "temperature")
    {
        send_command_to_camera(command, reply);
        if(command == "temperatures sensor" || command == "temperature sensor")
        {
            float snake_temp = su::convertStringToType<float>(su::right(reply, reply.size() - 15));
            U9_LLOG("Reply Part = \"" << su::right(reply, reply.size() - 15) << "\" ; Sensor Temp = " << snake_temp);
            m_ptrDetector->set_last_snake_temperature(snake_temp);
            return EXIT_SUCCESS;
        }
        if(command == "temperatures snake" || command == "temperature snake")
        {
            float snake_temp = su::convertStringToType<float>(su::right(reply, reply.size() - 14));
            U9_LLOG("Reply Part = \"" << su::right(reply, reply.size() - 14) << "\" ; Snake Temp = " << snake_temp);
            m_ptrDetector->set_last_snake_temperature(snake_temp);
            return EXIT_SUCCESS;
        }
        else if(command == "temperatures sensor raw"|| command == "temperatures snake raw" || command == "temperature sensor raw"|| command == "temperature snake raw")
        {
            float snake_temp = su::convertStringToType<float>(reply);
            m_ptrDetector->set_last_snake_temperature(snake_temp);
            return EXIT_SUCCESS;
        }
        else if(command == "temperatures" || command == "temperature")
        {
            size_t pos = reply.find("Sensor");
            if(pos != string::npos)
            {
                std::string reply_part = su::right(reply, reply.size() - (pos + 8));
                size_t pos2 = reply_part.find("Peltier");
                if(pos2 != string::npos)
                {
                    float snake_temp = su::convertStringToType<float>(su::left(reply_part, pos2 - 2));
                    U9_LLOG("Reply Part = \"" << su::left(reply_part, pos2 - 2) << "\"" << "; pos2 = " << pos2 << "; snake_temp : " << snake_temp);
                    m_ptrDetector->set_last_snake_temperature(snake_temp);
                    return EXIT_SUCCESS;
                }
            }
        }
        else if(command == "temperatures raw")
        {

        }
    }
    return result;
}

/**
 * @brief      Sends a command to camera.
 *
 * @param[in]  _command  The commandØ
 *
 * @return     { EXIT_SUCCESS or EXIT_FAILURE }
 */
 //! \brief send_command_to_camera - Sends a command to camera, saves reply in out parameter
 //! \param[in]  _command - command to be sent
 //! \param[out] _reply - reply from the camera
 //! \return
int tiptiltCtrl::send_command_to_camera(const std::string & _command, std::string & _reply)
{

    U6_LLOG(__FUNCTION__ << " ~ " << _command);
    int status = EXIT_SUCCESS;
    std::vector<int> txBuffer;
    std::vector<int> rxBuffer;
    std::vector<int> readBuffer;
    Nice::SeqInt uartRxControlReg;
    Nice::SeqInt UartRxControlReg1;
    Nice::SeqInt UartRxControlReg2;
    Nice::Timer t;

    txBuffer = string_to_seqint(_command + "\r");

    m_BcuWriteMutex.lock();
    m_BcuReadMutex.lock();
    try
    {
        uartRxControlReg = m_bcuService->readSram(Sharknir::Bcu::Const::BCU_DSP,
                                                Sharknir::Bcu::UartRxControlReg,
                                                1);
        //  write the command to SRAM
        m_bcuService->writeSram(Sharknir::Bcu::Const::BCU_DSP,
                                Sharknir::Bcu::Const::BCU_DSP,
                                Sharknir::Bcu::UartTxBufferArea,
                                txBuffer);
    //    U2_LLOG("Command has been written to BCU SRAM memory");

        // read back command from SRAM to check it aa been written correctly
    //    readBuffer = m_bcuService->readSram(Sharknir::Bcu::Const::BCU_DSP,
    //                                      Sharknir::Bcu::UartTxBufferArea,
    //                                      txBuffer.size());

        // The command length has to be written in UartTxControlReg setting to 1 bit number 31 at the same time
        // this means adding to command length the hex value 0x80000000

        // tell BCU to send the command from SRAM to camera
        m_bcuService->writeSram(Sharknir::Bcu::Const::BCU_DSP,
                                Sharknir::Bcu::Const::BCU_DSP,
                                Sharknir::Bcu::UartTxControlReg,
                                Nice::SeqInt(1, int(0x80000000 + _command.size() + 1)));

        readBuffer = m_bcuService->readSram(Sharknir::Bcu::Const::BCU_DSP,
                                        Sharknir::Bcu::UartTxControlReg,
                                        1);

        // check for the reply which corresponds to a change in UARTRXControlReg
        UartRxControlReg1 = uartRxControlReg;
        UartRxControlReg2 = uartRxControlReg;
        t.start(10.);
        while ((UartRxControlReg2 == uartRxControlReg) || (UartRxControlReg1 != UartRxControlReg2))
        {
            usleep(100000);
            UartRxControlReg1 = UartRxControlReg2;
            UartRxControlReg2 = m_bcuService->readSram(Sharknir::Bcu::Const::BCU_DSP,
                                                       Sharknir::Bcu::UartRxControlReg,
                                                       1);
            if(t.elapsed())break;
        }

        if(t.elapsed())
        {
            W_LLOG("Timer has elapsed before receiveing reply from camera");
            _reply = "";
            U8_LLOG("");
            throw (std::runtime_error("No reply from camera"));
        }
        // reading full camera reply buffer as a sequence of integers (4kB)
        rxBuffer = m_bcuService->readSram(Sharknir::Bcu::Const::BCU_DSP,
                                        Sharknir::Bcu::UartRxBufferArea,
                                        Sharknir::Bcu::UartRxBufferArea - Sharknir::Bcu::UartTxBufferArea);

    //    U9_LLOG(string_from_seqint(rxBuffer, 0, rxBuffer.size()*4));
        if(UartRxControlReg2[0] > uartRxControlReg[0])
        {
            _reply = string_from_seqint(rxBuffer, uartRxControlReg[0], UartRxControlReg2[0]);
        }
        else
        {
            // Circular Buffer
            _reply = string_from_seqint(rxBuffer, uartRxControlReg[0], rxBuffer.size()*4) +
                string_from_seqint(rxBuffer, 0, UartRxControlReg2[0]);
        }
        if(_reply.size() > 10)
        {
            // removing next prompt from serial line
            _reply = su::left(_reply, _reply.size() - 10);
        }
        m_BcuWriteMutex.unlock();
        m_BcuReadMutex.unlock();
        U8_LLOG(__FUNCTION__ << " -> Reply : " << _reply << "; uartRxControlReg[0] : " << uartRxControlReg[0]);
        return status;
    }
    catch(...)
    {
        LogBoostDiagnosticInformationTipTilt(__LINE__);
        m_BcuWriteMutex.unlock();
        m_BcuReadMutex.unlock();
        U8_LLOG("");
        throw;
    }

    return status;
}


int tiptiltCtrl::config_camera_cropping()
{
    U6_LLOG(__FUNCTION__);
    std::string strKey;
    int status = EXIT_SUCCESS;

    try
    {
        is_init(true);
    }
    catch(...)
    {
        LogBoostDiagnosticInformationTipTilt(__LINE__);
        U8_LLOG("");
        throw;
    }

    std::stringstream cmd;
    std::string reply;

    std::vector<int> crop_rows = m_ptrDetector->get_window_size_rows();
    std::vector<int> crop_cols = m_ptrDetector->get_window_size_cols();
    try
    {
        if(crop_rows[1] < sud::cRedRows || crop_cols[1] < sud::cRedCols)
        {
            cmd.str("set cropping on");
            status = send_command_to_camera(cmd.str(), reply);
            U9_LLOG("command \"" << cmd.str() << "\" -> reply : \"" << reply << "\"");

            cmd.str("");
            cmd << "set cropping rows " << crop_rows[0] << "-" << crop_rows[0] + crop_rows[1] - 1;
            status = send_command_to_camera(cmd.str(), reply);
            U9_LLOG("command \"" << cmd.str() << "\" -> reply : \"" << reply << "\"");

            cmd.str("");
            cmd << "set cropping columns " << crop_cols[0] << "-" << crop_cols[0] + crop_cols[1] - 1;
            status = send_command_to_camera(cmd.str(), reply);
            U9_LLOG("command \"" << cmd.str() << "\" -> reply : \"" << reply << "\"");
        }
        else
        {
            cmd.str("set cropping off");
            status = send_command_to_camera(cmd.str(), reply);
            U9_LLOG("command \"" << cmd.str() << "\" -> reply : \"" << reply << "\"");
        }
    }
    catch(...)
    {
        LogBoostDiagnosticInformationTipTilt(__LINE__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
    return status;
}

int tiptiltCtrl::config_camera_framerate()
{
//    U6_LLOG(__FUNCTION__);
    std::string strKey;
    int status = EXIT_SUCCESS;

    std::stringstream cmd;
    std::string reply;

    try
    {
        is_init(true);
        cmd.str("");
        cmd << "set fps " << m_ptrDetector->get_frame_rate();
        status = send_command_to_camera(cmd.str(), reply);
//        U9_LLOG("command \"" << cmd.str() << "\" -> reply : \"" << reply << "\"");
        cmd.str("fps raw");
        reply.clear();
        status = send_command_to_camera(cmd.str(), reply);
        status = m_ptrDetector->set_frame_rate(su::convertStringToType<float>(reply));
//        U9_LLOG("command \"" << cmd.str() << "\" -> reply : \"" << reply << "\"");
    }
    catch(...)
    {
        LogBoostDiagnosticInformationTipTilt(__LINE__);
//        U8_LLOG("");
        throw;
    }
//    U8_LLOG("");
    return status;
}

int tiptiltCtrl::config_camera_tint()
{
//    U6_LLOG(__FUNCTION__);
    std::string strKey;
    int status = EXIT_SUCCESS;


    std::stringstream cmd;
    std::string reply;

    try
    {
        is_init(true);
        cmd.str("");
        cmd << "set tint " << m_ptrDetector->get_cam_integration_time();
        status = send_command_to_camera(cmd.str(), reply);
//        U9_LLOG("command \"" << cmd.str() << "\" -> reply : \"" << reply << "\"");
        cmd.str("tint raw");
        reply.clear();
        status = send_command_to_camera(cmd.str(), reply);
        status = m_ptrDetector->set_cam_integration_time(su::convertStringToType<float>(reply));
//        U9_LLOG("command \"" << cmd.str() << "\" -> reply : \"" << reply << "\"");
    }
    catch(...)
    {
        LogBoostDiagnosticInformationTipTilt(__LINE__);
//        U8_LLOG("");
        throw;
    }
//    U8_LLOG("");
    return status;
}

int tiptiltCtrl::config_camera()
{
    U6_LLOG(__FUNCTION__);
    // list of commands to be send to WFS camera:
    // # - setting
    // - FPS from loopMode
    // - croppping from loopMode
    // - integration time
    // - tag of images
    // - bad pixels
    // - bias
    // - flat
    // - production of RAW images
    std::string strKey;
    int status = EXIT_SUCCESS;


    std::stringstream cmd;
    std::string reply;

    try
    {
        is_init(true);
        cmd.str("set fps 10");
        status = send_command_to_camera(cmd.str(), reply);

        config_camera_cropping();

        config_camera_framerate();

        config_camera_tint();
    }
    catch(...)
    {
        LogBoostDiagnosticInformationTipTilt(__LINE__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
    return status;
}


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

    int status = EXIT_SUCCESS;
    bool flag;

    std::stringstream cmd;
    std::string reply;
    try
    {
        // setting image tag generation on
        if(su::get_bool_from_setup(_mapSetup, "TTCAM_IMAGE_TAG_ENABLED", flag))
        {
    //        flag = true;
            cmd.str("");
            cmd << "set imagetags " << ((flag) ? "on" : "off");
            status = send_command_to_camera(cmd.str(), reply);
        }
        // enabling camera bad pixel map
        if(su::get_bool_from_setup(_mapSetup, "TTCAM_BADPIXEL_ENABLED", flag))
        {
    //        flag = true;
            cmd.str("");
            cmd << "set badpixel " << ((flag) ? "on" : "off");
            status = send_command_to_camera(cmd.str(), reply);
        }

        // turning bias off
        if(su::get_bool_from_setup(_mapSetup, "TTCAM_BIAS_ENABLED", flag))
        {
    //        flag = false;
            cmd.str("");
            cmd << "set bias " << ((flag) ? "on" : "off");
            status = send_command_to_camera(cmd.str(), reply);
        }

        // turning flat offset
        if(su::get_bool_from_setup(_mapSetup, "TTCAM_FLAT_ENABLED", flag))
        {
    //        flag = false;
            cmd.str("");
            cmd << "set flat " << ((flag) ? "on" : "off");
            status = send_command_to_camera(cmd.str(), reply);
        }

        // enabling rawimages production
        if(su::get_bool_from_setup(_mapSetup, "TTCAM_RAWIMG_ENABLED", flag))
        {
    //        flag = true;
            cmd.str("");
            cmd << "set rawimages " << ((flag) ? "on" : "off");
            status = send_command_to_camera(cmd.str(), reply);
        }
    }
    catch(...)
    {
        LogBoostDiagnosticInformationTipTilt(__LINE__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
    return status;
}


std::map<std::string, bool> tiptiltCtrl::exec_setup(const std::map<std::string, std::string> & _execSetup, bool force)
{
    U6_LLOG(__FUNCTION__);
    int set_result = EXIT_FAILURE;
    std::map<std::string, bool> result;
    std::map<std::string, std::string> allowed_setup;
    std::stringstream log_msg;
    allowed_setup["TTWINCOORDX"] = "";
    allowed_setup["TTWINCOORDY"] = "";
    allowed_setup["TTWINROWS"] = "";
    allowed_setup["TTWINCOLS"] = "";
    allowed_setup["TTCAMTINT"] = "";
    allowed_setup["TTFRAMERATE"] = "";
    allowed_setup["TTDMMAXPOWER"] = "";
    allowed_setup["TTDMFLATFILE"] = "";
    allowed_setup["TTBIASFILE"] = "";
    allowed_setup["TTDIAGDECIMATION"] = "";
    allowed_setup["TTDIAGENABLED"] = "";
    allowed_setup["TTPIXELDECIMATION"] = "";
    allowed_setup["TTPIXELENABLED"] = "";
    allowed_setup["TTPIXELGAINMODE"]= "";
    allowed_setup["TTPIXELGAINFILE"] = "";
    allowed_setup["TTPIXELGAINRADIUS"] = "";
    allowed_setup["TTPIXELGAINAMPLITUDE"] = "";
//    allowed_setup["TTM2CFILE"] = "";
    allowed_setup["TTPIDKP"] = "";
    allowed_setup["TTPIDKI"] = "";
    allowed_setup["TTPIDKD"]= "";
    allowed_setup["TTPIDTF"] = "";
    allowed_setup["TTPIDTS"] = "";
    allowed_setup["TTTIMEHISTORYGAIN"] = "";

    bool diagnostic_frame_set = false, pixel_frame_set = false;
    std::map<std::string, std::string>::const_iterator itSetup;
    for(itSetup = _execSetup.begin(); itSetup != _execSetup.end(); itSetup++)
    {
        if(allowed_setup.find(itSetup->first) != allowed_setup.end())
        {
            result[itSetup->first] = false;
        }
        else
        {
            if(m_currentSetup.find(itSetup->first) != m_currentSetup.end())
            {
                log_msg << "Keyword " << itSetup->first << " not allowed for exec_setup and will be ignored";
                N_LLOG(log_msg.str());
            }
            else
            {
                log_msg << "Error : Keyword " << itSetup->first << " not allowed for exec_setup";
                E_LLOG(log_msg.str());
                U8_LLOG("");
                throw(std::invalid_argument(log_msg.str()));
            }
        }
    }
    try
    {
        is_init(true);
    }
    catch(...)
    {
        LogBoostDiagnosticInformationTipTilt(__LINE__);
        U8_LLOG("");
        throw;
    }
    shins::util::rtcSlopeComputer & _sc = *m_ptrSlopeComputer;
    shins::util::rtcDM & _dm = *m_ptrDm;
    shins::util::rtcDetector & _det = *m_ptrDetector;
    std::map<std::string, std::string> ExecSetup = _execSetup;
    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);
    if(!extractSetup.empty())
    {
        set_result = _det.set_cropping(extractSetup);
        if(set_result == EXIT_SUCCESS || force)
        {
            result["TTWINCOORDX"] = true;
            result["TTWINCOORDY"] = true;
            result["TTWINCOLS"] = true;
            result["TTWINROWS"] = true;
            config_camera_cropping();
        }
    }

    {
        std::vector<std::string> gainSetup;
        gainSetup.push_back("TTPIXELGAINMODE");
        gainSetup.push_back("TTPIXELGAINFILE");
        gainSetup.push_back("TTPIXELGAINRADIUS");
        gainSetup.push_back("TTPIXELGAINAMPLITUDE");
        extractSetup = shins::util::extract_setup(gainSetup, ExecSetup);
        if(!extractSetup.empty())
        {
            set_result = _sc.setupGain(extractSetup);
            if(set_result == EXIT_SUCCESS || force)
            {
                result["TTPIXELGAINMODE"] = true;
                result["TTPIXELGAINFILE"] = true;
                result["TTPIXELGAINRADIUS"] = true;
                result["TTPIXELGAINAMPLITUDE"] = true;
                write_bcu_single(_sc.get_pixel_gain_ptr()[0], _sc.get_pixel_gain());
                U9_LLOG("PixelGain : " << su::seq_float_to_str(_sc.get_pixel_gain()));
            }
        }
    }
    {
        std::vector<std::string> pidSetup;
        pidSetup.push_back("TTPIDKP");
        pidSetup.push_back("TTPIDKI");
        pidSetup.push_back("TTPIDKD");
        pidSetup.push_back("TTPIDTF");
        pidSetup.push_back("TTPIDTS");
        extractSetup = shins::util::extract_setup(pidSetup, ExecSetup);
        if(extractSetup.size() == 5)
        {
            set_result = _sc.setup_pid(extractSetup);
            if(set_result == EXIT_SUCCESS)
            {
                result["TTPIDKP"] = true;
                result["TTPIDKI"] = true;
                result["TTPIDKD"] = true;
                result["TTPIDTF"] = true;
                result["TTPIDTS"] = true;
                for(itSetup = _execSetup.begin(); itSetup != _execSetup.end(); itSetup++){
                    m_currentSetup[itSetup->first] = itSetup->second;
                    // questo for loop potrebbe essere eseguito alla fine della funzione. Aggiornando sempre tutto indistintamente. Bastaassicurarsi che tutte le chiavi in allowedSetup abbiano la loro controparte in m_currentSetup. 
                }
                write_bcu_single(Sharknir::Bcu::PIDDelayLine, Nice::SeqFloat(12,0.f));
                write_bcu_single(Sharknir::Bcu::PIDFilterCoeff, _sc.get_PID_filter_coeff());
                U9_LLOG("PIDDelayLine : " << su::seq_float_to_str(Nice::SeqFloat(12,0.f)));
                U9_LLOG("PIDFilterCoeff : " << su::seq_float_to_str(_sc.get_PID_filter_coeff()));
            }
        }
    }

    try
    {
        std::map<std::string, std::string>::const_iterator it;
        for(it = _execSetup.begin(); it != _execSetup.end(); it++)
        {
            if(allowed_setup.find(it->first) != allowed_setup.end())
            {
                if(it->first == "TTFRAMERATE")
                {
                    set_result = _det.set_frame_rate(su::convertStringToType<int>(it->second));
                    if(set_result == EXIT_SUCCESS || force)
                    {
                        result["TTFRAMERATE"] = true;
                        config_camera_framerate();
                    }
                    else
                    {
                        U9_LLOG(__FUNCTION__ << " : frame rate already satisfied");
                    }
                }
                else if(it->first == "TTCAMTINT")
                {
                    set_result = _det.set_cam_integration_time(su::convertStringToType<float>(it->second));
                    if(set_result == EXIT_SUCCESS || force)
                    {
                        result["TTCAMTINT"] = true;
                        config_camera_tint();
                    }
                }
                else if(it->first == "TTDIAGDECIMATION")
                {
                    if(!diagnostic_frame_set)
                    {
                        diagnostic_frame_set = true;
                        bool diag_enabled = m_ptrSlopeComputer->get_diagnostic_enabled();
                        unsigned int diag_decimation_factor = m_ptrSlopeComputer->get_decimation_factor();
                        if(get_value_from_setup<unsigned int>(_execSetup, "TTDIAGDECIMATION", diag_decimation_factor))
                        {
                            m_ptrSlopeComputer->set_diagnostic_decimation(diag_decimation_factor);
                        }
                        if(su::get_bool_from_setup(_execSetup, "TTDIAGENABLED", diag_enabled))
                        {
                            m_ptrSlopeComputer->set_diagnostic_enabled(diag_enabled);
                        }
                        write_bcu(Sharknir::Bcu::DiagnosticFrameDec, m_ptrSlopeComputer->get_diagnostic_frame_dword(), "dsp");
                        U9_LLOG("Diagnostic Decimation set to : " << (diag_enabled ? "enabled" : "disabled") << " ~ " << diag_decimation_factor);
                    }

                }
                else if(it->first == "TTPIXELDECIMATION")
                {
                    if(!pixel_frame_set)
                    {
                        pixel_frame_set = true;
                        bool pixel_save_enabled = m_ptrSlopeComputer->get_pixel_save_enabled();
                        unsigned int pixel_save_decimation = m_ptrSlopeComputer->get_pixel_save_decimation();
                        if(get_value_from_setup<unsigned int>(_execSetup, "TTPIXELDECIMATION", pixel_save_decimation))
                        {
                            m_ptrSlopeComputer->set_pixel_decimation(pixel_save_decimation);
                        }
                        if(su::get_bool_from_setup(_execSetup, "TTPIXELENABLED", pixel_save_enabled))
                        {
                            m_ptrSlopeComputer->set_pixel_enabled(pixel_save_enabled);
                        }
                        write_bcu(Sharknir::Bcu::PixelFrameDec, m_ptrSlopeComputer->get_pixel_frame_dword(), "dsp");
                        U9_LLOG("PixelFrameDec : " << (pixel_save_enabled ? "enabled" : "disabled") << " ~ " << pixel_save_decimation);
                    }
                }
                else if(it->first == "TTDMMAXPOWER")
                {
                    _dm.set_max_power(su::convertStringToType<float>(it->second));
                    write_bcu_single(Sharknir::Bcu::MaxDMPower, _dm.get_max_power());
                    U9_LLOG("MaxDMPower : " << su::seq_float_to_str(_dm.get_max_power()));
                }
                else if(it->first == "TTDMFLATFILE")
                {
                    _dm.update_flat(it->second);
                    m_currentSetup["TTDMFLATFILE"] = it->second;
                    write_bcu_single(_sc.get_dm_bias_vector_ptr()[0], _dm.get_flat());
                    U9_LLOG("DMBiasVector : " << su::seq_float_to_str(_dm.get_flat()));
                }
                else if(it->first == "TTBIASFILE")
                {
                    _det.init_bias(it->second);
                    m_currentSetup["TTBIASFILE"] = it->second;
                    write_bcu(_sc.get_pixel_offset_ptr()[0], _det.get_pixel_bias());
                    U9_LLOG("PixelBias for bcu (PixelOffset): " << su::seq_int_to_str(_det.get_pixel_bias()));
                }
                else if(it->first == "TTM2CFILE")// Not Used
                {
                    _dm.update_m2c(it->second);

                    U9_LLOG("Getting Influence Matrix");
                    Nice::SeqSeqFloat influenceMatrixForBCU = _dm.get_influence_matrix();
                    U9_LLOG("Writing DM influence matrix0 directly to 0x" << std::hex << _sc.get_dm_influence_matrix0_ptr()[0] << std::dec << " (" << _sc.get_dm_influence_matrix0_ptr()[0] << ")");
                    write_bcu_single(_sc.get_dm_influence_matrix0_ptr()[0], influenceMatrixForBCU[0]);
                    U9_LLOG("DM influence matrix0 : " << su::seq_float_to_str(influenceMatrixForBCU[0]));

                    U9_LLOG("Writing DM influence matrix1 directly to 0x" << std::hex << _sc.get_dm_influence_matrix1_ptr()[0] << std::dec << " (" << _sc.get_dm_influence_matrix1_ptr()[0] << ")");
                    write_bcu_single(_sc.get_dm_influence_matrix1_ptr()[0], influenceMatrixForBCU[1]);
                    U9_LLOG("DM influence matrix1 : " << su::seq_float_to_str(influenceMatrixForBCU[1]));
                }
                else if(it->first == "TTTIMEHISTORYGAIN")
                {
                    _sc.update_time_history_gain(_execSetup);
                    write_bcu_single(Sharknir::Bcu::TimeHistoryGain, _sc.get_time_history_gain());
                    U9_LLOG("TimeHistoryGain : " << su::seq_float_to_str(_sc.get_time_history_gain()));

                }
            }
            else
            {
                U9_LLOG(__FUNCTION__ << " : Invalid setup command -> Key : " << it->first << " Value : " << it->second);
            }
        }
    }
    catch(...)
    {
        LogBoostDiagnosticInformationTipTilt(__LINE__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
    return result;
}


int tiptiltCtrl::enable_diagnostic(const std::map<std::string, std::string> & _diagSetup)
{
    U6_LLOG(__FUNCTION__);
    int result = EXIT_SUCCESS;

    m_ptrSlopeComputer->setup_diagnostic(_diagSetup);

    std::stringstream log_msg;
//    log_msg << std::hex << std::setfill('0') << std::setw(8) << std::uppercase << "0x" << diagAddress[0] << " 0x" << diagAddress[1] << " 0x" << diagAddress[2];
    U9_LLOG("Diagnostic Address = " << log_msg.str());

    int diagPort = m_ptrSlopeComputer->get_diag_port();
    try
    {
        result = enable_diagnostic(m_ptrSlopeComputer->get_diagnostic_enabled(),
                          m_ptrSlopeComputer->get_decimation_factor(),
                          m_ptrSlopeComputer->get_pixel_save_enabled(),
                          m_ptrSlopeComputer->get_pixel_save_decimation(),
                          m_ptrSlopeComputer->get_diag_ipv4(),
                          m_ptrSlopeComputer->get_diag_mac(),
                          diagPort,
                          m_ptrSlopeComputer->get_master_decimation(),
                          m_ptrSlopeComputer->get_jumbo_enabled(),
                          m_ptrSlopeComputer->get_master_enabled());
    }
    catch(...)
    {
        LogBoostDiagnosticInformationTipTilt(__LINE__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
    return result;
}

/*
 * Enable diagnostics
 */
 //! \brief enable_diagnostic - enables diagnostic on BCU
 //! \param _remoteHostIPV4Address - IP address of host to receive diagnostic (the ws on which SHINS runs)
 //! \param _remoteHostMACAddress - MAC address of host to receive diagnostic (the ws on which SHINS runs)
 //! \param _remoteHostUDPPort - UDP port of host to receive diagnostics
 //! \param _masterDecimationFactor - decimation factor on diagnostic
 //! \param _saveAllFrames - flag
 //! \param _transfPXtoDSM - flag
 //! \param _enableJumboFrames - flag
 //! \return

int tiptiltCtrl::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)
{
    U6_LLOG(__FUNCTION__);
    int status = EXIT_SUCCESS;
    unsigned int meno_uno = 0xFFFFFFFF;

    status = write_bcu(Sharknir::Bcu::DiagRecordReadPtr, Nice::SeqInt(2, 0), "sram");
    //status = write_bcu(Sharknir::Bcu::DiagRecordWritePtr, Nice::SeqInt(1, 0), "sram");

    Nice::U16 masterDec = (Nice::U16)masterDecimation;
    if(masterDecimation == meno_uno)masterDec = 0;

    // m_bcuService function enableMaterDiagnostics tells to the service to listend to the same address
    // enableMasterDiagnostics enables diagnostic without jumboFrames
    m_bcuService->enableMasterDiagnostics(masterDec,
                                          diagIpv4,
                                          diagMac,
                                          (Nice::U16)(diagPort - 1));

    Nice::SeqInt diagAddress = m_ptrSlopeComputer->getDiagnosticRemoteAddress(diagMac, diagIpv4, diagPort);
    status = write_bcu(Sharknir::Bcu::DiagRecordRemoteAddr, diagAddress, "sram");
    U2_LLOG("DiagRecordRemoteAddress returned : " << status);


    U9_LLOG("diagIpv4 : " << diagIpv4 << "; diagMac : " << diagMac << "; diagPort : " << diagPort);

    U9_LLOG("masterDecimation : " << masterDecimation << "; masterEnabled : " << masterEnabled);
    if(masterEnabled == 1 && jumboEnabled == 1)
    {
        if(masterDecimation == meno_uno)masterDecimation = 0;
        Nice::SeqInt DiagEnable = Nice::SeqInt();
    // Jumbo frames not yet supported
    //    status = write_bcu(Sharknir::Bcu::DiagRecordDecFactorEna, su::uint_to_seqint((masterDecimation << 16) + (jumboEnabled << 1) + masterEnabled), "sram");
    // if(masterDecimation == meno_uno)masterDecimation = 0;
    }

    // if(jumboEnabled)
    // {
    //     W_LLOG("Jumbo frames not yet supportaed defaulting to no jumbo frames");
    //     jumboEnabled = 0;
    // }
    // status = write_bcu(Sharknir::Bcu::DiagRecordDecFactorEna, su::uint_to_seqint((masterDecimation << 16) + (jumboEnabled << 1) + masterEnabled), "sram");

    if(diagEnabled == 1)
    {
        if(decimationFactor == meno_uno)decimationFactor = 0;
        status = write_bcu(Sharknir::Bcu::DiagnosticFrameDec, su::uint_to_seqint((diagEnabled << 31) + decimationFactor), "dsp");
    }

    if(pixelEnabled == 1)
    {
        if(pixelDecimation == meno_uno)pixelDecimation = 0;
        status = write_bcu(Sharknir::Bcu::PixelFrameDec, su::uint_to_seqint((pixelEnabled << 31) + pixelDecimation), "dsp");
    }

    U8_LLOG("Enabling Diagnostic ... DONE");
    return status;
}



/*
 * Enable diagnostics overloaded
 */
 //! \brief enable_diagnostic - overloading previous method
 //! \param _decimationFactor
 //! \param _saveAllFrames
 //! \param _transfPXtoDSM
 //! \param enableJumboFrames
 //! \return
int tiptiltCtrl::enable_diagnostic(int& _decimationFactor,
                                   const int& _saveAllFrames,
                                   const int& _transfPXtoDSM,
                                   const int& enableJumboFrames)
{
    U6_LLOG(__FUNCTION__);
    int status = EXIT_SUCCESS;
    std::string remoteHostIPV4Address = "";
    std::string remoteHostMACAddress = "";
    int remoteHostUDPPort = 0;
    try
    {
        if(m_bcuService->isReachable())
        {
            remoteHostIPV4Address = m_bcuService->diagnosticsDestinationIp();
            U9_LLOG("diagnosticDestinationIp : " << remoteHostIPV4Address);
            remoteHostMACAddress = m_bcuService->diagnosticsDestinationMac();
            U9_LLOG("diagnosticsDestinationMac : " << remoteHostMACAddress);
            remoteHostUDPPort = m_bcuService->diagnosticsDestinationUdpPort();
            U9_LLOG("diagnosticDestinationUdpPort : " << remoteHostUDPPort);
        }
    }
    catch(const Nice::PropertyNotFoundException& e)
    {
        E_LLOG(e.what());
        E_LLOG("Unable to recover network related properties from property tree");
    }

    try
    {
        if(m_bcuService->isReachable())
        {
            unsigned int masterEnabled = 1;
            unsigned int pixelDecimation = 19;
            unsigned int decimationFactor = 0;
            unsigned int diagEnabled = 1;
            unsigned int pixelEnabled = 1;
            enable_diagnostic(diagEnabled,
                              decimationFactor,
                              pixelEnabled,
                              pixelDecimation,
                              remoteHostIPV4Address,
                              remoteHostMACAddress,
                              remoteHostUDPPort,
                              _decimationFactor,
                              enableJumboFrames,
                              masterEnabled);
        }
    }
    catch(const Nice::PropertyNotFoundException& e)
    {
        E_LLOG(e.what());
        U8_LLOG("");
        throw;
    }
    catch(...)
    {
        LogBoostDiagnosticInformationTipTilt(__LINE__);
        U8_LLOG("");
        throw;
    }
    U8_LLOG("");
    return status;
}


int tiptiltCtrl::enable_px_to_DSM(const bool& _enableFlag)
{
    U6_LLOG(__FUNCTION__);
    int status = EXIT_SUCCESS;

    status = write_bcu(Sharknir::Bcu::CCDWaitForReset, Nice::SeqInt(1, 1));

    if(_enableFlag)
    {
        status = write_bcu(Sharknir::Bcu::ContrlReg, Nice::SeqInt(1, 1));
        status = write_bcu(Sharknir::Bcu::CCDInterfaceEnable, Nice::SeqInt(1, 1)); // enable pix transfer to DSM
    }
    else
    {
        // status = write_bcu(Sharknir::Bcu::ContrlReg, Nice::SeqInt(1,0));
        status = write_bcu(Sharknir::Bcu::PixelFrameDec, Nice::SeqInt(1, 0)); // reset pixel frame decimation
        status = write_bcu(Sharknir::Bcu::CCDInterfaceEnable, Nice::SeqInt(1, 0)); // disable pix transfer to DSM
    }

    U8_LLOG("");
    return status;
}

//! getPastOverallDiagnosticRecordsInOnePass - one method to get diagnostic from BCU
//! \param _nMaxRecords - number of records to get from BCU
//! \return
Sharknir::Bcu::SeqOverallDiagnosticRecord tiptiltCtrl::getPastOverallDiagnosticRecordsInOnePass(const int& _nMaxRecords)
{
    U6_LLOG(__FUNCTION__);
    W_LLOG("Provide Implementation for getPastOverallDiagnosticRecordsInOnePass");
    Sharknir::Bcu::SeqOverallDiagnosticRecord diagRecord;

    try
    {
        diagRecord = m_bcuService->getPastOverallDiagnosticRecordsInOnePass(_nMaxRecords);
    }
    catch(const Sharknir::BcuServiceException& e)
    {
        E_LLOG(e.what());
        U8_LLOG("");
        throw e;
    }

    U8_LLOG("");
    return diagRecord;
}


Sharknir::Bcu::SeqOverallDiagnosticRecord tiptiltCtrl::getFutureOverallDiagnosticRecordsInOnePass(const int& _nMaxRecords, const Nice::Time& _timeout)
{
    U6_LLOG(__FUNCTION__);
    W_LLOG("Provide Implementation for getPastOverallDiagnosticRecordsInOnePass");
    Sharknir::Bcu::SeqOverallDiagnosticRecord diagRecord;

    try
    {
        diagRecord = m_bcuService->getFutureOverallDiagnosticRecordsInOnePass(_nMaxRecords, _timeout);
    }
    catch(const Sharknir::BcuServiceException& e)
    {
        E_LLOG(e.what());
        U8_LLOG("");
        throw e;
    }

    U8_LLOG("");
    return diagRecord;
}

//tech_cam_expose
//    setup
//    wait
//    download
//    process
//    save

int tiptiltCtrl::tech_cam_expose(const std::map<std::string, std::string>  &_dTechCamSetup, std::string & _fit_file)
{
    U6_LLOG(__FUNCTION__);
    int status = EXIT_SUCCESS;
    if(m_AcquiringCubeMutex.tryLock())
    {
        U9_LLOG(__FUNCTION__ << " ~ Entering");
        m_bAcquiringPixCube = true;
        try
        {
            is_init(true);
            int setup_result = setup_tech_cam_expose(_dTechCamSetup);
            if(setup_result == EXIT_SUCCESS)
            {
                bool saveBias = false;
                su::get_bool_from_setup(_dTechCamSetup, "TTSAVEASBIAS", saveBias);
                bool saveDmFlat = false;
                su::get_bool_from_setup(_dTechCamSetup, "TTSAVEASDMFLAT", saveDmFlat);
                tech_cam_process_data(saveBias);
                if(saveBias)
                {
                    int save_result = save_pixel_bias(m_pix_frames, _fit_file);
                    bool updateBias = false;
                    su::get_bool_from_setup(_dTechCamSetup, "TTUPDATEBIAS", updateBias);
                    if(save_result == EXIT_SUCCESS && updateBias)
                    {
                        std::map<std::string, std::string> biasSetup;
                        biasSetup["TTBIASFILE"] = _fit_file;
                        exec_setup(biasSetup);
                    }
                }
                else if(saveDmFlat)
                {
                    int save_result = EXIT_FAILURE; // save_dm_flat(m_pix_frames, _fit_file);
                    bool updateDmFlat = false;
                    su::get_bool_from_setup(_dTechCamSetup, "TTUPDATEDMFLAT", updateDmFlat);
                    if(save_result == EXIT_SUCCESS && updateDmFlat)
                    {
                        std::map<std::string, std::string> biasSetup;
                        biasSetup["TTDMFLATFILE"] = _fit_file;
                        exec_setup(biasSetup);
                    }
                }
                else
                {
                    std::vector<short int> current_bias;
                    std::vector<float> current_gain;
                    if(!m_pix_frames.empty() && !m_pix_frames[0].empty())
                    {
                        current_bias.resize(m_pix_frames[0].size());
                        std::vector<int> saved_bias = read_bcu(m_ptrSlopeComputer->get_pixel_offset_ptr()[0], m_pix_frames[0].size());
                        std::copy(saved_bias.begin(), saved_bias.end(), current_bias.begin());
                        current_gain = read_bcu_single(m_ptrSlopeComputer->get_pixel_gain_ptr()[0], m_pix_frames[0].size(), "dsp");
                    }
                    save_pix_cube(m_pix_frames, current_bias, current_gain, m_slope_frames, _fit_file);
                }
                m_pix_frames.clear();
                m_slope_frames.clear();
                if(m_bSaveRawDiagData)
                {
                    U9_LLOG("saving raw diag data on " << _fit_file << ".raw");
                    su::debug_save_memory_block((const char *)(&(*m_diagData.begin())), sizeof(int)*m_diagData.size(), su::get_char_star(_fit_file + ".raw"));
                    m_bSaveRawDiagData = false;
                }
            }
        }
        catch(...)
        {
            m_bAcquiringPixCube = false;
            m_AcquiringCubeMutex.unlock();
            LogBoostDiagnosticInformationTipTilt(__LINE__);
            U8_LLOG("");
            throw;
        }
        m_bAcquiringPixCube = false;
        m_AcquiringCubeMutex.unlock();
        U8_LLOG(__FUNCTION__);
        return status;
    }
    else
    {
        std::stringstream log_message;
        log_message << "Error : Instrument is busy.";
        E_LLOG(log_message.str());
        U8_LLOG("");
        throw (std::invalid_argument(log_message.str()));
    }
    return status;
}


int tiptiltCtrl::setup_tech_cam_expose(const std::map<std::string, std::string> & _dTechCamSetup)
{
    U6_LLOG(__FUNCTION__);
    int status = EXIT_FAILURE;
    std::vector<std::string> to_be_extracted;
    to_be_extracted.push_back("TTFRAMERATE");
    to_be_extracted.push_back("TTWINROWS");
    to_be_extracted.push_back("TTWINCOLS");
    to_be_extracted.push_back("TTWINCOORDX");
    to_be_extracted.push_back("TTWINCOORDY");
    std::map<std::string, std::string> residue_setup = _dTechCamSetup;
    std::map<std::string, std::string> extracted_setup;
    extracted_setup = shins::util::extract_setup(to_be_extracted, residue_setup);
    exec_setup(extracted_setup);

    su::rtcDetector & det = *m_ptrDetector;
    const unsigned int minus_one = 0xFFFFFFFF;
    unsigned int old_pixel_decimation = minus_one;
    unsigned int old_frame_decimation = minus_one;
    bool old_loopenabled = m_ptrSlopeComputer->get_tt_control_loop_enable()[0], bControlLoopChanged = false;
    std::string imagetags_status = " on";
    std::string reply;

    Nice::SeqInt read_result;
//    float subapThreshold;
//    read_result = read_bcu(Sharknir::Bcu::SubapIntensityThres, 1, "dsp");
//    std::memcpy(&subapThreshold, &*read_result.begin(), sizeof(subapThreshold));
//    unsigned int frame_size;// = m_ptrDetector->get_window_size() + 4;
//    frame_size = subapThreshold + 4;// size in words

//    U9_LLOG("FrameSize = " << frame_size);

    write_bcu(Sharknir::Bcu::CCDInterfaceEnable, Nice::SeqInt(1, 0), "dsp");// disable CCD pixels transfer to DSM memory
    usleep(200000);

    write_bcu(Sharknir::Bcu::DiagRecordReadPtr, Nice::SeqInt(2, 0), "sram");
    //write_bcu(Sharknir::Bcu::DiagRecordWritePtr, Nice::SeqInt(1, 0), "sram");

    //DiagRecordDecFactorEna concerns Master BCU diagnostics via UDP
    //write_bcu(Sharknir::Bcu::DiagRecordDecFactorEna, Nice::SeqInt(1, 1), "sram");

    bool saveBias = false;
    su::get_bool_from_setup(_dTechCamSetup, "TTSAVEASBIAS", saveBias);

    int getResult = 0;
    unsigned int diag_frame_dec = 0;
    unsigned int diag_enable = 0;
    bool diagEnable = false;
    read_result = read_bcu(Sharknir::Bcu::DiagnosticFrameDec, 1, "dsp");
    if(read_result.size() == 1)
    {
        old_frame_decimation = read_result[0];
    }
    m_CubeDiagDecimation = old_frame_decimation;
    if(get_value_from_setup<unsigned int>(_dTechCamSetup, "TTDIAGDECIMATION", diag_frame_dec))getResult += 2;
    if(su::get_bool_from_setup(_dTechCamSetup, "TTDIAGENABLED", diagEnable))getResult += 1;
    std::map<std::string, std::string>::const_iterator itFind;
    if(getResult & 3)
    {
        if(diagEnable)diag_enable = 1;
        m_CubeDiagDecimation = (diag_enable << 31) + diag_frame_dec;
        write_bcu(Sharknir::Bcu::DiagnosticFrameDec, su::uint_to_seqint((diag_enable << 31) + diag_frame_dec), "dsp");
    }

    getResult = 0;
    unsigned int pixel_frame_dec = 0;
    unsigned int pixel_enable = 0;
    float cam_tint, snake_temp;
    int frame_rate;
    bool pixelEnable = false;


    if(get_value_from_setup<float>(_dTechCamSetup, "TTCAMTINT", cam_tint))
    {
        std::stringstream cmd;
        det.set_cam_integration_time(cam_tint);
        config_camera_tint();
    }
    cam_tint = m_ptrDetector->get_cam_integration_time();


    send_command_to_camera("temperature snake", reply);
    snake_temp = su::convertStringToType<float>(su::right(reply, reply.size() - 14));
    det.set_last_snake_temperature(snake_temp);

    m_CubePixelDecimation = 0;
    read_result = read_bcu(Sharknir::Bcu::PixelFrameDec, 1, "dsp");
    if(read_result.size() == 1)
    {
        old_pixel_decimation = read_result[0];
        pixel_frame_dec = m_CubePixelDecimation = old_pixel_decimation;
    }
    if(get_value_from_setup<unsigned int>(_dTechCamSetup, "TTPIXELDECIMATION", pixel_frame_dec))getResult += 2;
    if(su::get_bool_from_setup(_dTechCamSetup, "TTPIXELENABLED", pixelEnable))getResult += 1;
    if(getResult & 3)
    {
        if(pixelEnable)pixel_enable = 1;
        m_CubePixelDecimation = (pixel_enable << 31) + pixel_frame_dec;
        write_bcu(Sharknir::Bcu::PixelFrameDec, su::uint_to_seqint(m_CubePixelDecimation), "dsp");
    }

    bool timehistory_enabled = false;
    bool enable_control_loop = false;
    if(!saveBias)
    {
        getResult = 0;
        if(su::get_bool_from_setup(_dTechCamSetup, "TTHISTORYENABLED", timehistory_enabled))getResult += 1;
        int time_history_act = 2;
        if(timehistory_enabled)time_history_act = 1;// Start time history
        if(getResult)
            write_bcu(Sharknir::Bcu::TimeHistoryAct, Nice::SeqInt(1, time_history_act), "dsp");

        getResult = 0;
        if(su::get_bool_from_setup(_dTechCamSetup, "TTLOOPENABLED", enable_control_loop))getResult += 1;
        if(getResult)
        {
            bControlLoopChanged = true;
            enable_loop(enable_control_loop);
        }
    }

    U9_LLOG("Starting Exposure");
    write_bcu(Sharknir::Bcu::CCDWaitForReset, Nice::SeqInt(1, 1), "dsp");
    write_bcu(Sharknir::Bcu::ContrlReg, Nice::SeqInt(1, 1), "shmram"); // start CCD acquisition
    write_bcu(Sharknir::Bcu::CCDInterfaceEnable, Nice::SeqInt(1, 1), "dsp");

    if(saveBias)
    {
        // Disabling image tags at beginning creates unpredictable results
        send_command_to_camera("imagetags", reply);
        imagetags_status = su::right(reply, 3);
        if(imagetags_status == " on")
        {
            send_command_to_camera("set imagetags off", reply);
        }
    }

    getResult = 0;
    unsigned int test_time = 500000;
    if(get_value_from_setup<unsigned int>(_dTechCamSetup, "TTTESTTIME", test_time))getResult += 1;
    if(getResult == 1)
    {
        if(test_time >= 2000 && test_time <= 60000000)
        {
        }
        else
        {
            std::stringstream log_msg;
            log_msg << "Error parameter TTTESTTIME value not in range [2000, 60000000] : " << test_time;
            E_LLOG(log_msg.str());
            U8_LLOG("");
            throw(std::invalid_argument(log_msg.str()));
        }
    }

    {
        unsigned int wait_time = 200000, waited_time = 0;
        if(wait_time > test_time)wait_time = test_time;
        while(waited_time < test_time)
        {
            if(m_abort & TIPTILT_ABORT_TECCAM_ACQ)
            {
                N_LLOG("Technical Camera acquisition aborted by user");
                status = EXIT_FAILURE;
                break;
            }
            usleep(wait_time);
            waited_time += wait_time;
            if(waited_time + wait_time > test_time)
                wait_time = test_time - waited_time;
            status = EXIT_SUCCESS;
        }
//        m_bTechCamDownload = false;
    }

    if(timehistory_enabled)
        write_bcu(Sharknir::Bcu::TimeHistoryAct, Nice::SeqInt(1, 2), "dsp");// stop time history

    if(old_pixel_decimation != minus_one)
    {
        if(old_pixel_decimation != (pixel_enable << 31) + pixel_frame_dec)
        {
            write_bcu(Sharknir::Bcu::PixelFrameDec, Nice::SeqInt(1, old_pixel_decimation), "dsp");
        }
    }
    if(old_frame_decimation != minus_one)
    {
        write_bcu(Sharknir::Bcu::DiagnosticFrameDec, su::uint_to_seqint(old_frame_decimation), "dsp");
    }
    if(bControlLoopChanged && enable_control_loop != old_loopenabled)
    {
        enable_loop(old_loopenabled);
    }

//    write_bcu(Sharknir::Bcu::CCDInterfaceEnable, Nice::SeqInt(1, 0), "dsp");// disable CCD pixels transfer to DSM memory

//    write_bcu(Sharknir::Bcu::ContrlReg, Nice::SeqInt(1, 0), "shmram"); // stop CCD acquisition

    if(saveBias && imagetags_status == " on")
    {
        send_command_to_camera("set imagetags on", reply);
    }
    U9_LLOG("Exposure completed; Reading DATA");
    U8_LLOG(__FUNCTION__);
    return status;
}

int tiptiltCtrl::tech_cam_process_data(bool save_as_bias)
{
    U6_LLOG(__FUNCTION__);

    tech_cam_download_data();

    const unsigned int diag_record_len = 120;
    std::vector<int> & read_buffer = m_diagData;
    std::vector<int> bcu_read(1,read_buffer.size()*4);
    //    read_bcu_preallocated(Sharknir::Bcu::DiagRecordWritePtr, "sram", bcu_read);
    bool bSaveRawData = false;
    //    if(bcu_read[0] > 0 && bcu_read[0] <= 128*1048576)// 128 MB
    unsigned int NumSau = bcu_read[0]/4;
//        read_bcu_preallocated(0,"sdram", read_buffer);
    unsigned int frame_size = m_ptrDetector->get_window_size() + 4;// size in words
    U9_LLOG("read_buffer.size() : " << read_buffer.size() << "; bcu_read[0] : " << bcu_read[0] << "; " << frame_size);
    std::vector<std::vector<int> > & slope_frames = m_slope_frames;
    std::vector<std::vector<short int> > & pix_frames = m_pix_frames;
    slope_frames.resize(NumSau/diag_record_len, std::vector<int>());
    pix_frames.resize((2*NumSau)/frame_size, std::vector<short int>());

    unsigned int offset = 0;
    int index_slopes = 0;
    int index_pixels = 0;
    while( offset < NumSau)
    {
        bool slope_test = false;
        bool pixel_test = false;
        if(offset + diag_record_len <= read_buffer.size())
        {
            slope_test = read_buffer[offset] == read_buffer[offset + diag_record_len - 4];
        }
        if(offset + frame_size/2 <= read_buffer.size())
        {
            pixel_test = read_buffer[offset] == read_buffer[offset + frame_size/2 - 1];
        }
        if(pixel_test && slope_test)
        {
            E_LLOG("Inconsistent DATA");
            U8_LLOG("");
            throw(std::runtime_error("Inconsistent DATA"));
        }
        else if(slope_test)
        {
            slope_frames[index_slopes].resize(diag_record_len);
            std::memcpy(&*slope_frames[index_slopes].begin(), &*(read_buffer.begin() + offset), diag_record_len*sizeof(int));
            offset += diag_record_len;
            index_slopes++;
        }
        else if(pixel_test)
        {
            pix_frames[index_pixels].resize(frame_size - 4);
            std::memcpy(&*pix_frames[index_pixels].begin(), &*(read_buffer.begin() + offset + 1), frame_size*sizeof(short int));
            if(!save_as_bias)
            {
                std::memcpy(&*(pix_frames[index_pixels].begin() + frame_size - 6), &*(read_buffer.begin() + offset), sizeof(int));
            }
            offset += frame_size/2;
            index_pixels++;
        }
        else
        {
            if(offset < diag_record_len + frame_size/2)
            {
                index_slopes = index_pixels = 0;
                offset = diag_record_len + frame_size/2;// discarding first frame (MICROGATE BUG)
                N_LLOG(__FUNCTION__ << " ~ Discarding first frame from DATA");
            }
            else
            {
                unsigned int max_size =  diag_record_len;
                if(frame_size/2 > max_size)max_size = frame_size/2;
                bool bOffsetNotFound = true;
                for(unsigned int j = 0; j < NumSau - offset - max_size; j++)
                {
                    if((read_buffer[offset + j] == read_buffer[offset + j + diag_record_len - 4]) ||
                        (read_buffer[offset + j] == read_buffer[offset + j + frame_size/2 - 1]))
                    {
                        offset += j;
                        N_LLOG(__FUNCTION__ << " ~ Found record at position : " << offset*4 << "; Discarded " << j << " SAU; Saving RAW data.");
                        bSaveRawData = true;
                        bOffsetNotFound = false;
                        break;
                    }
                }
                if(bOffsetNotFound)
                {
                    N_LLOG(__FUNCTION__ << " ~ Could not find new record. Saving RAW data");
                    bSaveRawData = true;
                    break;
                }
            }
        }
    }
    if(bSaveRawData)
    {

        FILE * pPixCubeRaw = fopen("PIXCUBE.raw", "wb");
        if(pPixCubeRaw)
        {
            fwrite(&*read_buffer.begin(), read_buffer.size(), sizeof(int), pPixCubeRaw);
            fclose(pPixCubeRaw);
        }
        else
        {
            E_LLOG("Error saving PIXCUBE.raw");
        }
    }
    read_buffer.clear();
    slope_frames.resize(index_slopes);
    pix_frames.resize(index_pixels);

    U8_LLOG("");
    return EXIT_SUCCESS;
}

#ifdef UNDEFINED
int tiptiltCtrl::get_last_diagnostic_frame(std::vector< float >  & diag_data)
{
    U6_LLOG(__FUNCTION__);
    int status = EXIT_FAILURE;
    // TODO return at least reconstructed centroid origin


    int last_read = 0;
    std::vector<int> & read_buffer = m_diagData;
    read_buffer.clear();
    std::vector<int> bcu_read(1,0);
    int bcu_memory_block = 640*480*2 + 480 + 1024*10;
    while(bcu_read[0] < bcu_memory_block)
    {
        bcu_read = read_bcu(Sharknir::Bcu::DiagRecordWritePtr, 1, "sram");
        usleep(100000);
        break;
    }
    U9_LLOG("DiagRecordWritePtr : " << bcu_read[0]);
        boost::posix_time::ptime date_time0 = boost::posix_time::microsec_clock::universal_time();
        boost::posix_time::ptime date_time = boost::posix_time::microsec_clock::universal_time();
        boost::posix_time::ptime date_time1 = boost::posix_time::microsec_clock::universal_time();
        last_read = sizeof(int)*read_buffer.size();
        if(bcu_read[0] > bcu_memory_block)
        {
            unsigned int NumSau = bcu_memory_block/4;
            date_time = boost::posix_time::microsec_clock::universal_time();
            bool bReadSuccess = false;
            int counter = 0;
            while(!bReadSuccess)
            {
                bool locked = true;
                if(m_abort & TIPTILT_ABORT_TECCAM_ACQ)
                {
                    break;
                }
                try
                {
                    m_BcuReadMutex.lock();
                    std::vector<int> read_data = m_bcuService->readDiags(Sharknir::Bcu::Const::BCU_DSP, read_buffer.size(), NumSau);
                    locked = false;
                    m_BcuReadMutex.unlock();
                    int old_size = read_buffer.size();
                    read_buffer.resize(old_size + NumSau);
                    std::memcpy(&*(read_buffer.begin() + old_size), &*read_data.begin(), sizeof(int)*read_data.size());
                    bReadSuccess = true;
                }
                catch(...)
                {
                    if(locked)m_BcuReadMutex.unlock();
                    counter ++;
                    LogBoostDiagnosticInformationTipTilt(__LINE__);
                    E_LLOG(__FUNCTION__);
                    usleep(5000000);
                    if(counter == 20)break;
                }
            }
            U9_LLOG("read_buffer.size() : " << read_buffer.size() << "; bcu_read[0] : " << bcu_read[0]);
            boost::posix_time::ptime date_time2 = boost::posix_time::microsec_clock::universal_time();
            U9_LLOG("posix_time : " << date_time1 - date_time0 << "; download :" << (4.f*read_buffer.size())/((date_time2 - date_time).total_microseconds()));
        }
        else
        {
            E_LLOG("Error");
            U8_LLOG("");
            throw (std::invalid_argument("Invalid Data Pointer : DiagRecordWritePtr < read_buffer.size()"));
        }
    }
    m_bTechCamDownload = true;

    U8_LLOG("");
    return status;
}
#endif


//void * tiptiltCtrl::acquire_pixcube(void * arg)
//{
//   U6_LLOG(__FUNCTION__);
//   tiptiltCtrl & me = *((tiptiltCtrl *)arg);
//   me.async_tech_cam_expose();
//   pthread_mutex_lock(&me.m_TiptiltMutex);
//       me.m_TeccamExpose.bAcquiringPixcube = false;
//       me.m_abort &= ~TIPTILT_ABORT_TECCAM_ACQ;
//   pthread_mutex_unlock(&me.m_TiptiltMutex);
//   U8_LLOG("");
//   return NULL;
//}

Ice::Int tiptiltCtrl::abortTeccamAcquisition()
{
    m_UpdateAcquiringCubeMutex.lock();
        if(m_bAcquiringPixCube)
            m_abort |= TIPTILT_ABORT_TECCAM_ACQ;
    m_UpdateAcquiringCubeMutex.unlock();
    return EXIT_SUCCESS;
}


char fit_header [] = "SIMPLE  =                    T / conforms to FITS standard                      BITPIX  =                   16 / array data type                                NAXIS   =                    3 / number of array dimensions                     NAXIS1  =                   64                                                  NAXIS2  =                   64                                                  NAXIS3  =                                                                       BZERO   =                32768 / offset data range to that of unsigned short    BSCALE  =                    1 / default scaling factor                         EXTEND  =                    T                                                  DATE-OBS=                                                                       INSTRUME= 'SHARKNIR'                                                            CAMERA  = 'C-RED2  '                                                            BUNIT   = 'ADU     '           / ADC Digital steps                              UNIT    = 'ADU     '           / ADC Digital steps [0, 16384]                   FPS     =                                                                       TINT    =                                                                       TSNAKE  =                                                                       NDFILTER=                                                                       WINDOW  =                                                                       SENSIBTY=                                                                       LOOPENBL=                                                                       THISTENA=                                                                       PID     =                                                                       CENORIGX=                                                                       CENORIGY=                                                                       DIAGDEC =                                                                       PIXELDEC=                                                                       END                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                             ";
char fit_header2[] = "XTENSION= 'BINTABLE'           / binary table extension                         BITPIX  =                    8 / 8-bit bytes                                    NAXIS   =                    2 / 2-dimensional binary table                     NAXIS1  =                  480 / width of record in bytes                       NAXIS2  =                      / number of records in table                     PCOUNT  =                    0 / size of special data area                      GCOUNT  =                    1 / one data group (required keyword)              TFIELDS =                   13 / number of fields in each row                   EXTNAME = 'DIAG_FRAMES'                                                         TTYPE1  = 'FrameCounter'       / label for field   1                            TFORM1  = '1J      '           / data format of field: 4-byte INTEGER           TTYPE2  = 'ParamSel'           / label for field   2                            TFORM2  = '1J      '           / data format of field: 4-byte INTEGER           TTYPE3  = 'c_red_burst_c'      / label for field   3                            TFORM3  = '1J      '           / data format of field: 4-byte INTEGER           TTYPE4  = 'TimeHistPtr'        / label for field   4                            TFORM4  = '1J      '           / data format of field: 4-byte INTEGER           TTYPE5  = 'Actuators'          / label for field   5                            TFORM5  = '104E    '           / data format of field: 4-byte FLOAT             TTYPE6  = 'DMPower '           / label for field   6                            TFORM6  = '1E      '           / data format of field: 4-byte FLOAT             TTYPE7  = 'DMSkipped'          / label for field   7                            TFORM7  = '1J      '           / data format of field: 4-byte INTEGER           TTYPE8  = 'CentroidOut1'       / label for field   8                            TFORM8  = '1E      '           / data format of field: 4-byte FLOAT             TTYPE9  = 'CentroidOut2'       / label for field   9                            TFORM9  = '1E      '           / data format of field: 4-byte FLOAT             TTYPE10 = 'NotUsed '           / label for field  10                            TFORM10 = '2J      '           / data format of field: 4-byte INTEGER           TTYPE11 = 'TT_RTC_CMD1'        / label for field  11                            TFORM11 = '1E      '           / data format of field: 4-byte FLOAT             TTYPE12 = 'TT_RTC_CMD2'        / label for field  12                            TFORM12 = '1E      '           / data format of field: 4-byte FLOAT             TTYPE13 = 'HeadDiag'           / label for field  13                            TFORM13 = '4J      '           / data format of field: 4-byte INTEGER           END                                                                             ";
char fit_header3[] = "XTENSION= 'IMAGE'                                                               BITPIX  =                   16 / array data type                                NAXIS   =                    2 / number of array dimensions                     NAXIS1  =                   64                                                  NAXIS2  =                   64                                                  PCOUNT  =                    0 / size of special data area                      GCOUNT  =                    1 / one data group (required keyword)              BZERO   =                32768 / offset data range to that of unsigned short    BSCALE  =                    1 / default scaling factor                         EXTNAME = 'PIXEL_BIAS'                                                          BUNIT   = 'ADU     '           / ADC Digital steps                              UNIT    = 'ADU     '           / ADC Digital steps                              END                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                             ";
char fit_header4[] = "XTENSION= 'IMAGE'                                                               BITPIX  =                   16 / array data type                                NAXIS   =                    2 / number of array dimensions                     NAXIS1  =                   64                                                  NAXIS2  =                   64                                                  PCOUNT  =                    0 / size of special data area                      GCOUNT  =                    1 / one data group (required keyword)              BZERO   =                32768 / offset data range to that of unsigned short    BSCALE  =                    1 / default scaling factor                         EXTNAME = 'PIXEL_GAIN'                                                          BUNIT   = 'ADIMENSIONAL'       / Factor used in centroid computation            UNIT    = '        '                                                            END                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                             ";

// #ifdef UNDEFINED
// char fit_header [] = "
// SIMPLE  =                    T / conforms to FITS standard                      
// BITPIX  =                   16 / array data type                                
// NAXIS   =                    3 / number of array dimensions                     
// NAXIS1  =                   64                                                  
// NAXIS2  =                   64                                                  
// NAXIS3  =                                                                       
// BZERO   =                32768 / offset data range to that of unsigned short    
// BSCALE  =                    1 / default scaling factor                         
// EXTEND  =                    T                                                  
// "
// #endif
// char fit_header2[] = "XTENSION= 'BINTABLE'           / binary table extension                         BITPIX  =                    8 / 8-bit bytes                                    NAXIS   =                    2 / 2-dimensional binary table                     NAXIS1  =                  480 / width of record in bytes                       NAXIS2  =                      / number of records in table                     PCOUNT  =                    0 / size of special data area                      GCOUNT  =                    1 / one data group (required keyword)              EXTNAME = 'DIAG_FRAMES'                                                         TFIELDS =                   13 / number of fields in each row                   TTYPE1  = 'FrameCounter'       / label for field   1                            TFORM1  = '1J      '           / data format of field: 4-byte INTEGER           TTYPE2  = 'ParamSel'           / label for field   2                            TFORM2  = '1J      '           / data format of field: 4-byte INTEGER           TTYPE3  = 'c_red_burst_c'      / label for field   3                            TFORM3  = '1J      '           / data format of field: 4-byte INTEGER           TTYPE4  = 'TimeHistPtr'        / label for field   4                            TFORM4  = '1J      '           / data format of field: 4-byte INTEGER           TTYPE5  = 'Actuators'          / label for field   5                            TFORM5  = '104E    '           / data format of field: 4-byte FLOAT             TTYPE6  = 'DMPower '           / label for field   6                            TFORM6  = '1E      '           / data format of field: 4-byte FLOAT             TTYPE7  = 'DMSkipped'          / label for field   7                            TFORM7  = '1J      '           / data format of field: 4-byte INTEGER           TTYPE8  = 'CentroidOut1'       / label for field   8                            TFORM8  = '1E      '           / data format of field: 4-byte FLOAT             TTYPE9  = 'CentroidOut2'       / label for field   9                            TFORM9  = '1E      '           / data format of field: 4-byte FLOAT             TTYPE10 = 'NotUsed '           / label for field  10                            TFORM10 = '2J      '           / data format of field: 4-byte INTEGER           TTYPE11 = 'TT_RTC_CMD1'        / label for field  11                            TFORM11 = '1E      '           / data format of field: 4-byte FLOAT             TTYPE12 = 'TT_RTC_CMD2'        / label for field  12                            TFORM12 = '1E      '           / data format of field: 4-byte FLOAT             TTYPE13 = 'HeadDiag'           / label for field  13                            TFORM13 = '4J      '           / data format of field: 4-byte INTEGER           END                                                                             ";
// char fit_header3[] = "XTENSION= 'IMAGE'                                                               BITPIX  =                   16 / array data type                                NAXIS   =                    2 / number of array dimensions                     NAXIS1  =                   64                                                  NAXIS2  =                   64                                                  PCOUNT  =                    0 / size of special data area                      GCOUNT  =                    1 / one data group (required keyword)              BZERO   =                32768 / offset data range to that of unsigned short    BSCALE  =                    1 / default scaling factor                         EXTNAME = 'PIXEL_BIAS'                                                          BUNIT   = 'ADU     '           / ADC Digital steps                              UNIT    = 'ADU     '           / ADC Digital steps                              END                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                             ";
// char fit_header4[] = "XTENSION= 'IMAGE'                                                               BITPIX  =                   16 / array data type                                NAXIS   =                    2 / number of array dimensions                     NAXIS1  =                   64                                                  NAXIS2  =                   64                                                  PCOUNT  =                    0 / size of special data area                      GCOUNT  =                    1 / one data group (required keyword)              BZERO   =                32768 / offset data range to that of unsigned short    BSCALE  =                    1 / default scaling factor                         EXTNAME = 'PIXEL_GAIN'                                                          BUNIT   = 'ADIMENSIONAL'       / Factor used in centroid computation            UNIT    = '        '                                                            END                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                             ";

//int tiptiltCtrl::async_tech_cam_expose()


#include <boost/date_time/posix_time/posix_time.hpp>

void tiptiltCtrl::tech_cam_download_data(void)
{
    U6_LLOG(__FUNCTION__);
//    tiptiltCtrl & tt = *this;//*((tiptiltCtrl *)pMe);
    int last_read = 0;
    std::vector<int> & read_buffer = m_diagData;
    read_buffer.clear();
//    bool bOneMore = true;
//    while(tt.m_bTechCamDownload)
//    {
//        usleep(25000);
//    }
//    while(tt.m_bTechCamDownload || bOneMore)
    {
//        if(!tt.m_bTechCamDownload)bOneMore = false;
        std::vector<int> bcu_read(1,0);
        bcu_read = read_bcu(Sharknir::Bcu::DiagRecordWritePtr, 1, "sram");
        U9_LLOG("DiagRecordWritePtr : " << bcu_read[0] << "; Delta : " << bcu_read[0] - last_read);
        boost::posix_time::ptime date_time0 = boost::posix_time::microsec_clock::universal_time();
        boost::posix_time::ptime date_time = boost::posix_time::microsec_clock::universal_time();
        boost::posix_time::ptime date_time1 = boost::posix_time::microsec_clock::universal_time();
        last_read = sizeof(int)*read_buffer.size();
        if(bcu_read[0] > last_read)
        {
            unsigned int NumSau = (bcu_read[0] - last_read)/4;
            date_time = boost::posix_time::microsec_clock::universal_time();
            bool bReadSuccess = false;
            int counter = 0;
            while(!bReadSuccess)
            {
                bool locked = true;
                if(m_abort & TIPTILT_ABORT_TECCAM_ACQ)
                {
                    break;
                }
                try
                {
                    m_BcuReadMutex.lock();
                    std::vector<int> read_data = m_bcuService->readDiags(Sharknir::Bcu::Const::BCU_DSP, read_buffer.size(), NumSau);
                    locked = false;
                    m_BcuReadMutex.unlock();
                    int old_size = read_buffer.size();
                    read_buffer.resize(old_size + NumSau);
                    std::memcpy(&*(read_buffer.begin() + old_size), &*read_data.begin(), sizeof(int)*read_data.size());
    //                read_buffer.insert(read_buffer.end(), read_data.begin(), read_data.end());
                    bReadSuccess = true;
                }
                catch(...)
                {
                    if(locked)m_BcuReadMutex.unlock();
                    counter ++;
                    LogBoostDiagnosticInformationTipTilt(__LINE__);
                    E_LLOG(__FUNCTION__);
                    usleep(5000000);
                    if(counter == 20)break;
                }
            }
            U9_LLOG("read_buffer.size() : " << read_buffer.size() << "; bcu_read[0] : " << bcu_read[0]);
            boost::posix_time::ptime date_time2 = boost::posix_time::microsec_clock::universal_time();
            U9_LLOG("posix_time : " << date_time1 - date_time0 << "; download :" << (4.f*read_buffer.size())/((date_time2 - date_time).total_microseconds()));
        }
        else
        {
            E_LLOG("Error");
            U8_LLOG("");
            throw (std::invalid_argument("Invalid Data Pointer : DiagRecordWritePtr < read_buffer.size()"));
        }
    }
    m_bTechCamDownload = true;
    U8_LLOG(__FUNCTION__);
}

void tiptiltCtrl::LogCfitsioStack(int line, int n, int & status)
{
    if(status)
    {
        char simple_test[84];
        *(simple_test + 80) = 0;
        U9_LLOG("Status : " << status);
        status = 0;
        for(int i = 0; i < 7; i++)
        {
            fits_read_errmsg(simple_test);
            if(*simple_test)
            {
                U9_LLOG(line << " ~ " << simple_test);
            }
        }
    }
}


int tiptiltCtrl::save_pix_cube(const std::vector< std::vector<short int> > & pix_frames,
                               const std::vector<short int> & bias,
                               const std::vector<float> & gain,
                               const std::vector< std::vector<int> > & slope_frames,
                               std::string & fit_file)
{
    U6_LLOG(__FUNCTION__);
    int status = EXIT_SUCCESS;
    int frames_size = 0;
    std::string time_string = su::get_iso_gmtime_str();
    su::SetTimeWorkingDirectory(time_string);
    if(!pix_frames.empty())frames_size = pix_frames[0].size();
    U9_LLOG("pix_cube sizes : (" << pix_frames.size() << ", " << frames_size << ")");
    frames_size = 0;
    if(!slope_frames.empty())frames_size = slope_frames[0].size();
    U9_LLOG("slope_frames sizes : (" << slope_frames.size() << ", " << frames_size << ")");
    su::rtcDetector & det = *m_ptrDetector;
    const int diag_record_len = 120;
    std::stringstream file_name;

        // m_pix_frames is the same of pix_frames but editable
        //file_name << "!ProvaTeccam.fits";
    file_name << "TECCAM." << time_string << "_" << det.get_frame_rate() << "Hz-" << std::setfill('0') << std::setw(3) << det.get_num_cols() << "x" << std::setw(3) << det.get_num_rows() << ".fits";

    fitsfile * ptrFitsTeccam = NULL;
    std::string command;
    std::string reply;
    int fit_status = 0;
    int naxis = 4; // (X, Y, Blocks, Block) ;  Blocks are used to speed up vision in ds9 when using high frame rate.
    int sub_block = 16;
    int bitpix = USHORT_IMG;
    int Blank = 0;
    float floatValue;
    int intValue;
    long naxes[4] = {det.get_window_size_cols()[1], det.get_window_size_rows()[1], sub_block, pix_frames.size()/sub_block};
    int frame_size = naxes[0]*naxes[1];
    fits_create_file(&ptrFitsTeccam, file_name.str().c_str(), &fit_status);
    if(fit_status == 0)
    {
        const int image_block_size = 1024;
        short int image_block[image_block_size];
        fits_create_img(ptrFitsTeccam, bitpix, naxis, naxes, &fit_status);
        if(fit_status){LogCfitsioStack(__LINE__, 5, fit_status); fit_status = 0; }
        // See Daniele Mail of 18 February
        fits_write_date(ptrFitsTeccam, &fit_status); // UST & LST
        if(fit_status){LogCfitsioStack(__LINE__, 5, fit_status); fit_status = 0; }
        fits_write_key(ptrFitsTeccam, TSTRING, "OBSERVER", (char *)"UNDEFINED", "observer who acquired the data", &fit_status);
        fits_write_key(ptrFitsTeccam, TSTRING, "OBJECT  ", (char *)"UNDEFINED", "name of the observed object", &fit_status);
        fits_write_key(ptrFitsTeccam, TSTRING, "ORIGIN  ", (char *)"INAF-OAPD", "jacopo.farinato[]inaf.it", &fit_status);
        fits_write_key(ptrFitsTeccam, TSTRING, "TELESCOP", (char *)"LBT", "LBT TUCSON ARIZONA", &fit_status);
        fits_write_key(ptrFitsTeccam, TSTRING, "INSTRUME", (char *)"SHARK-NIR", "Instrument SHARK-NIR @ LBT", &fit_status);
        fits_write_key(ptrFitsTeccam, TSTRING, "CAMERA  ", (char *)"C-RED2", "Technical Camera of the instrument", &fit_status);
        fits_write_key(ptrFitsTeccam, TSTRING, "BUNIT   ", (char *)"ADU   ", "14 bits ADC digital steps [0-16384]", &fit_status);
        if(fit_status){LogCfitsioStack(__LINE__, 5, fit_status); fit_status = 0; }
        floatValue = det.get_frame_rate();
        fits_write_key(ptrFitsTeccam, TFLOAT,  "FPS     ", &floatValue, "Frames per second", &fit_status);
        floatValue = det.get_cam_integration_time();
        fits_write_key(ptrFitsTeccam, TFLOAT,  "TINT    ", &floatValue, "Integration time of one single frame", &fit_status);
        floatValue = det.get_last_snake_temperature();
        fits_write_key(ptrFitsTeccam, TFLOAT,  "TSNAKE  ", &floatValue, "Temperature of camera sensor", &fit_status);
        fits_write_key(ptrFitsTeccam, TSTRING, "NDFILTER", (char *)"UNDEFINED", "ND Wheel Selection", &fit_status);
        if(fit_status){LogCfitsioStack(__LINE__, 5, fit_status); fit_status = 0; }
        intValue = det.get_window_size_rows()[0];
        fits_write_key(ptrFitsTeccam, TINT,    "WINDOWR0", &intValue, "Cropping : first row", &fit_status);
        intValue = det.get_window_size_rows()[1];
        fits_write_key(ptrFitsTeccam, TINT,    "WINDOWR1", &intValue, "Cropping : last row", &fit_status);
        intValue = det.get_window_size_cols()[0];
        fits_write_key(ptrFitsTeccam, TINT,    "WINDOWC0", &intValue, "Cropping : first column", &fit_status);
        intValue = det.get_window_size_cols()[1];
        if(fit_status){LogCfitsioStack(__LINE__, 5, fit_status); fit_status = 0; }
        fits_write_key(ptrFitsTeccam, TINT,    "WINDOWC1", &intValue, "Cropping : last column", &fit_status);
        send_command_to_camera("sensibility", reply);
        fits_write_key(ptrFitsTeccam, TSTRING, "SENSIBTY", su::get_char_star(reply), "14 bits ADC digital steps", &fit_status);
        fits_write_key(ptrFitsTeccam, TINT,    "LOOPENBL", &Blank, "Fast tip/tilt control loop enabled", &fit_status);
        fits_write_key(ptrFitsTeccam, TINT,    "THISTENA", &Blank, "Time History enabled", &fit_status);
        if(fit_status){LogCfitsioStack(__LINE__, 5, fit_status); fit_status = 0; }
        const Nice::SeqFloat & pid = m_ptrSlopeComputer->get_pid_pars();
        float Kp = 0, Ki = 0, Kd = 0, Tf = 0, Ts = 0;
        if(pid.size() == 5)
        {
            Kp = pid[0], Ki = pid[1], Kd = pid[2], Tf = pid[3], Ts = pid[4];
        }
        fits_write_key(ptrFitsTeccam, TFLOAT,  "PID-KP", &Kp, "Proportional Coefficient of PID", &fit_status);
        fits_write_key(ptrFitsTeccam, TFLOAT,  "PID-KI", &Ki, "Integral Ceofficient of PID", &fit_status);
        fits_write_key(ptrFitsTeccam, TFLOAT,  "PID-KD", &Kd, "Derivative Coefficient of PID", &fit_status);
        fits_write_key(ptrFitsTeccam, TFLOAT,  "PID-TF", &Tf, "Derivative first order time constant", &fit_status);
        fits_write_key(ptrFitsTeccam, TFLOAT,  "PID-TS", &Ts, "Sample time (1/f)", &fit_status);
        if(fit_status){LogCfitsioStack(__LINE__, 5, fit_status); fit_status = 0; }
        float centroid_origin_x = 0.f, centroid_origin_y = 0.f;
        const Nice::SeqFloat & centroid_origin = m_ptrSlopeComputer->get_centroid_origin();
        centroid_origin_x = centroid_origin[0];
        centroid_origin_y = centroid_origin[1];
        fits_write_key(ptrFitsTeccam, TFLOAT,  "CENORIGX", &centroid_origin_x, "Centroid Origin X at RTC", &fit_status);
        fits_write_key(ptrFitsTeccam, TFLOAT,  "CENORIGY", &centroid_origin_y, "Centroid Origin Y at RTC", &fit_status);
        int diag_enabled = 0, diag_dec = 0, pixel_enabled = 0, pixel_dec = 0;
        diag_dec = (m_CubeDiagDecimation & 0x7FFFFFFF); // m_ptrSlopeComputer->get_decimation_factor();
        diag_enabled = (m_CubeDiagDecimation & 0x80000000) == 0x80000000; // m_ptrSlopeComputer->get_diagnostic_enabled();
        pixel_enabled = (m_CubePixelDecimation & 0x80000000) == 0x80000000;// m_ptrSlopeComputer->get_pixel_save_enabled();
        pixel_dec = (m_CubePixelDecimation & 0x7FFFFFFF); // m_ptrSlopeComputer->get_pixel_save_decimation();
        fits_write_key(ptrFitsTeccam, TINT,    "DIAGENAB", &diag_enabled, "Diagnostic frames anabled", &fit_status);
        fits_write_key(ptrFitsTeccam, TINT,    "DIAGDEC",  &diag_dec, "Diagnostic frames decimation", &fit_status);
        fits_write_key(ptrFitsTeccam, TINT,    "PIXELENB", &pixel_enabled, "Enable technical frames saving", &fit_status);
        fits_write_key(ptrFitsTeccam, TINT,    "PIXELDEC", &pixel_dec, "Technical framse decimations", &fit_status);
        if(fit_status){LogCfitsioStack(__LINE__, 5, fit_status); fit_status = 0; }

        void * pImage = malloc(frame_size*sizeof(float));
        if(pImage)
        {
            long fpixel[4];
            fpixel[0] = 1;
            fpixel[1] = 1;
            for(unsigned int i = 0; i < pix_frames.size()/sub_block; i++)
            {
                for(int j = 0; j < sub_block; j++)
                {
                    memcpy(pImage, &*pix_frames[sub_block*i + j].begin(), frame_size*sizeof(short));
                    fpixel[2] = j + 1;
                    fpixel[3] = i + 1;
    //                        if(j == 0 || j == 8)
                    fits_write_pix(ptrFitsTeccam, TUSHORT, fpixel, frame_size, (unsigned short int *)pImage, &fit_status);
                }
            }
            if(fit_status)LogCfitsioStack(__LINE__, 5, fit_status);
            fits_write_chksum(ptrFitsTeccam, &fit_status);
            // Extension : Bias
            if(!bias.empty())
            {
                naxis = 3;
                naxes[2] = 1;
                U9_LLOG("Bias : " << bias[5] << " ~ " << bias[6] << " ~ " << bias[7] << " ~ " << bias[8] << " ~ " << bias[9] << " ~ ");
                fits_insert_img(ptrFitsTeccam, bitpix, naxis, naxes, &fit_status);
                fits_write_key(ptrFitsTeccam, TSTRING, "EXTNAME", (char *)"dark", "bias loaded on RTC", &fit_status);

                memcpy(pImage, &*bias.begin(), frame_size*sizeof(short));
                fits_write_img(ptrFitsTeccam, TSHORT, 1, frame_size, pImage, &fit_status);
                fits_write_chksum(ptrFitsTeccam, &fit_status);
            }
            if(fit_status)LogCfitsioStack(__LINE__, 5, fit_status);
            if(!gain.empty())
            {
                bitpix = FLOAT_IMG;
                naxis = 3;
                naxes[2] = 1;
                U9_LLOG("Gain : " << gain[5] << " ~ " << gain[6] << " ~ " << gain[7] << " ~ " << gain[8] << " ~ " << gain[9] << " ~ ");
                fits_insert_img(ptrFitsTeccam, bitpix, naxis, naxes, &fit_status);
                fits_write_key(ptrFitsTeccam, TSTRING, "EXTNAME", (char *)"gain", "gain loaded on RTC", &fit_status);
                memcpy(pImage, &*gain.begin(), frame_size*sizeof(float));
                fits_write_img(ptrFitsTeccam, TFLOAT, 1, frame_size, pImage, &fit_status);
                fits_write_chksum(ptrFitsTeccam, &fit_status);
            }
            if(fit_status)LogCfitsioStack(__LINE__, 5, fit_status);
            free(pImage);
        }

        fits_write_chksum(ptrFitsTeccam, &fit_status);
        if(fit_status)LogCfitsioStack(__LINE__, 5, fit_status);
        if(!slope_frames.empty())
        {
            int tbltype = BINARY_TBL;
#define TFIELDS 13
            int tfields = TFIELDS;
            const char * ttype[TFIELDS] = {"FrameCounter", "ParamSel", "c_red_burst_c", "TimeHistPtr", "Actuators", "DMPower", "DMSkipped", "CentroidOut1", "CentroidOut2", "NotUsed", "TT_RTC_CMD1", "TT_RTC_CMD2", "HeadDiag"};
            const char * tform[TFIELDS] = {"1J", "1J", "1J", "1J", "104E", "1E", "1J", "1E", "1E", "2J", "1E", "1E", "4J"};
    //            fits_insert_btble(ptrFitsTeccam, LONGLONG nrows, int tfields, char ** ttype, char ** tform, char ** tunit, char * extname, long pcount, &fit_status);
            fits_create_tbl(ptrFitsTeccam, tbltype, slope_frames.size(), tfields, (char **)&ttype, (char **)&tform, NULL, "DIAG_FRAMES", &fit_status);
            for(unsigned int i = 0; i < slope_frames.size(); i++)
            {
                su::swab4(&*slope_frames[i].begin(), image_block, diag_record_len*sizeof(int));
                fits_write_tblbytes(ptrFitsTeccam, 1 + i, 1, diag_record_len*sizeof(int), (unsigned char *)image_block, &fit_status);
                // fwrite(image_block, diag_record_len, sizeof(int), pPixCube);
            }
            fits_write_chksum(ptrFitsTeccam, &fit_status);
        }
        fits_flush_file(ptrFitsTeccam, &fit_status);
        if(fit_status)LogCfitsioStack(__LINE__, 5, fit_status);
    //            if(fit_status)LogCfitsioStack(5);
        fit_status = 0;
        fits_close_file(ptrFitsTeccam, &fit_status);
        if(fit_status)LogCfitsioStack(__LINE__, 5, fit_status);

        U9_LLOG("Saved fits file " << file_name.str());
    }
    else
    {
        if(fit_status){LogCfitsioStack(__LINE__, 5, fit_status); fit_status = 0; }
        status = EXIT_FAILURE;
        E_LLOG("Error opennig " << file_name.str());
    }
    U8_LLOG("");
    return status;
}



int tiptiltCtrl::save_pixel_bias(const std::vector< std::vector<short int> > pixel_frames, std::string & dat_file)
{
    int status = EXIT_SUCCESS;
    int num_frames = pixel_frames.size();
    shins::util::rtcDetector & det = *m_ptrDetector;
    int skip_frames = 0;
    if(num_frames > skip_frames)// Eliminating frames with image tag
    {
        std::vector<unsigned long long int> pixel_sums(pixel_frames[0].size(), 0);
        U9_LLOG("unsigned long long int size : " << sizeof(unsigned long long int));
        std::vector< std::vector< short int> >::const_iterator it_frame;
        for(it_frame = pixel_frames.begin() + skip_frames; it_frame < pixel_frames.end(); it_frame++)
        {
            int i;
            int frame_size = it_frame->size();
            for(i = 0; i < frame_size; i++)
            {
                pixel_sums[i] += (*it_frame)[i];
            }
        }
        chdir("/opt/tan/etc/tiptilt_ctrl/rtc/darks");
        std::stringstream file_name;
        int frame_width = det.get_num_cols();
        int frame_height = det.get_num_rows();
        file_name << su::get_iso_gmtime_str() << "_PixelBias_" << std::setprecision(3) << det.get_last_snake_temperature() << "C_" <<
            (int)(det.get_frame_rate()) << "Hz-" << std::setfill('0') << std::setw(3) <<
            frame_width << "x" << std::setw(3) << frame_height << ".dat";
        FILE * pPixCube = fopen(file_name.str().c_str(), "wb");
        if(pPixCube)
        {
            for(int i = 0; i < frame_height; i++)
            {
                for(int j = 0; j < frame_width - 1; j++)
                {
                    fprintf(pPixCube, "%d\t", (int)(pixel_sums[i*frame_width + j]/(num_frames - skip_frames)));
                }
                fprintf(pPixCube, "%d\n", (int)(pixel_sums[i*frame_width + frame_width - 1]/(num_frames - skip_frames)));
            }
            fclose(pPixCube);
            dat_file = file_name.str();
            U9_LLOG("Saved file : " << dat_file << " based on " << num_frames - skip_frames << " frames.");
        }
        else
        {
            W_LLOG("Could not save file : " << dat_file);
            status = EXIT_FAILURE;
        }
    }
    else
    {
        W_LLOG("Could not save TECCAM dark : too few frames try increasing TTTESTTIME or decreasing TTPIXELDECIMATION");
    }
    return status;
}

int tiptiltCtrl::getJsonStatus(std::string & jsonStatus)
{
    int status = EXIT_SUCCESS;

    if(!(m_ptrSlopeComputer && m_ptrDetector))return EXIT_FAILURE;

    {
        std::stringstream json_alias;
        json_alias << "\"" << "rtc_cred_tint" << "\": { \"name\": \"rtc_cred_tint\", \"value\": \"" << m_ptrDetector->get_cam_integration_time() << "\"}";
        jsonStatus += json_alias.str();
        jsonStatus += ", ";
    }
    {
        std::stringstream json_alias;
        json_alias << "\"" << "rtc_cred_freq" << "\": { \"name\": \"rtc_cred_freq\", \"value\": \"" << m_ptrDetector->get_frame_rate() << "\"}";
        jsonStatus += json_alias.str();
        jsonStatus += ", ";
    }
    {
        std::stringstream json_alias;
        json_alias << "\"" << "rtc_cred_temp" << "\": { \"name\": \"rtc_cred_temp\", \"value\": \"" << m_ptrDetector->get_last_snake_temperature() << "\"}";
        jsonStatus += json_alias.str();
        jsonStatus += ", ";
    }
    {
        std::stringstream json_alias;
        json_alias << "\"" << "rtc_cred_dark" << "\": { \"name\": \"rtc_cred_dark\", \"value\": \"" << m_currentSetup["TTBIASFILE"] << "\"}";
        jsonStatus += json_alias.str();
        jsonStatus += ", ";
    }
    {
        std::stringstream json_alias;
        json_alias << "\"" << "rtc_dm_loop" << "\": { \"name\": \"rtc_dm_loop\", \"value\": \"" << (m_ptrSlopeComputer->get_tt_control_loop_enable()[0] ? "CLOSED" : "OPEN") << "\"}";
        jsonStatus += json_alias.str();
        jsonStatus += ", ";
    }
    {
        std::stringstream json_alias;
        json_alias << "\"" << "rtc_dm_flat" << "\": { \"name\": \"rtc_dm_flat\", \"value\": \"" << m_currentSetup["TTDMFLATFILE"] << "\"}";
        jsonStatus += json_alias.str();
        jsonStatus += ", ";
    }
    {
        std::stringstream json_alias;
        json_alias << "\"" << "rtc_dm_mode" << "\": { \"name\": \"rtc_dm_mode\", \"value\": \"" << (m_currentSetup["TTM2CFILE"]=="ZONAL" ? "ZONAL" : "MODAL") << "\"}";
        jsonStatus += json_alias.str();
        jsonStatus += ", ";
    }
    {
        std::stringstream json_alias;
        json_alias << "\"" << "rtc_dm_pidKp" << "\": { \"name\": \"rtc_dm_pidKp\", \"value\": \"" << m_currentSetup["TTPIDKP"] << "\"}";
        jsonStatus += json_alias.str();
        jsonStatus += ", ";
    }
    {
        std::stringstream json_alias;
        json_alias << "\"" << "rtc_dm_pidKi" << "\": { \"name\": \"rtc_dm_pidKi\", \"value\": \"" << m_currentSetup["TTPIDKI"] << "\"}";
        jsonStatus += json_alias.str();
        jsonStatus += ", ";
    }
    {
        std::stringstream json_alias;
        json_alias << "\"" << "rtc_dm_pidKd" << "\": { \"name\": \"rtc_dm_pidKd\", \"value\": \"" << m_currentSetup["TTPIDKD"] << "\"}";
        jsonStatus += json_alias.str();
        jsonStatus += ", ";
    }
    {
        std::stringstream json_alias;
        json_alias << "\"" << "rtc_dm_pidTs" << "\": { \"name\": \"rtc_dm_pidTs\", \"value\": \"" << m_currentSetup["TTPIDTS"] << "\"}";
        jsonStatus += json_alias.str();
        jsonStatus += ", ";
    }
    {
        std::stringstream json_alias;
        json_alias << "\"" << "rtc_dm_pidTf" << "\": { \"name\": \"rtc_dm_pidTf\", \"value\": \"" << m_currentSetup["TTPIDTF"] << "\"}";
        jsonStatus += json_alias.str();
    }
    
    return status;
}


/////////////////////
// PRIVATE METHODS //
/////////////////////
void /*tiptiltCtrl::*/writeImage(std::string& _fitsFilename,
                             Eigen::Matrix<int16_t, Eigen::Dynamic, 0>& _eVec,
                             const uint& _nrows,
                             const uint& _ncols,
                             const uint& _nFrames)
{
    U6_LLOG(__FUNCTION__);
    // save on fits file frame cube acquired from CRED2
    fitsfile* fptr;
    int fitsioStatus = 0;
    std::string strFilename = "/data/fits/rtc/testFileCRED2.fits";
    char filename[] = "/data/fits/rtc/testFileCRED2.fits";
    int16_t* dataArray = _eVec.array().data();
    int bitpix = SHORT_IMG;
    long naxis = 3;
    long naxes[3] = { _ncols, _nrows, _nFrames };
    // long nelements = _ncols * _nrows * _nFrames;
    long group = 0;


    boost::filesystem::path pathToFits("/data/fits/rtc");
    boost::filesystem::path pathToFitsFile(strFilename);
    if(!(boost::filesystem::exists(pathToFits))) {
        boost::filesystem::create_directories(pathToFits);
        boost::filesystem::permissions(pathToFits, boost::filesystem::all_all);
    }

    if(boost::filesystem::exists(pathToFitsFile)) {
        boost::filesystem::remove(pathToFitsFile);
    }

    // create new FITS file
    if(fits_create_file(&fptr, filename, &fitsioStatus))
        E_LLOG("creating fits file " << strFilename << " has failed " << fitsioStatus);
    /* write the required keywords for the primary array image.     */
      /* Since bitpix = SHORT_IMG, this will cause cfitsio to create */
      /* a FITS image with BITPIX = 16 (signed short integers) with   */
      /* BSCALE = 1.0 and BZERO = 32768.  This is the convention that */
      /* FITS uses to store unsigned integers.  Note that the BSCALE  */
      /* and BZERO keywords will be automatically written by cfitsio  */
      /* in this case.*/

    if(fits_create_img(fptr, bitpix, naxis, naxes, &fitsioStatus))
        E_LLOG("creating fits image has failed " << fitsioStatus);

    //   int fits_write_2d_[byt, sht, usht, int, uint, lng, ulng, lnglng, ulnglng, flt, dbl] /
    //     ffp2d[b,i,ui,k,uk,j,uj,jj,ujj,e,d]
    //     (fitsfile *fptr, long group, LONGLONG dim1, LONGLONG naxis1,
    //      LONGLONG naxis2, DTYPE *array, > int *status)

    // int fits_write_3d_[byt, sht, usht, int, uint, lng, ulng, lnglng, ulnglng, flt, dbl] /
    //     ffp3d[b,i,ui,k,uk,j,uj,jj,ujj,e,d]
    //     (fitsfile *fptr, long group, LONGLONG dim1, LONGLONG dim2, LONGLONG naxis1,
    //      LONGLONG naxis2, LONGLONG naxis3, DTYPE *array, > int *status)


    if(_nFrames > 2) {
        if(fits_write_3d_sht(fptr, group,
                              naxes[0], naxes[1],
                              naxes[0], naxes[1], naxes[2],
                              dataArray, &fitsioStatus))
            E_LLOG("error writing 3D fits file " << fitsioStatus);
    }
    else {
        if(fits_write_2d_sht(fptr, group,
                              naxes[0],
                              naxes[0], naxes[1],
                              dataArray, &fitsioStatus))
            E_LLOG("error writing 2D fits file " << fitsioStatus);
    }

    free(dataArray);

    if(fits_close_file(fptr, &fitsioStatus))
        E_LLOG("Error closing FITS file " << strFilename);
    _fitsFilename = strFilename;
    U8_LLOG("");
}


Nice::SeqInt tiptiltCtrl::string_to_seqint(const std::string & _cmd)
{
//    U6_LLOG(__FUNCTION__);
    int rlen = (_cmd.size() + 3)/4;
    Nice::SeqInt result(rlen,0);
    std::memcpy(&(*result.begin()), &(*_cmd.begin()), _cmd.size());
//    U8_LLOG("");
    return result;
}


std::string tiptiltCtrl::string_from_seqint(const Nice::SeqInt &_inVector, int begin, int end)
{
//    U6_LLOG(__FUNCTION__);
    std::string result;
    result.resize(_inVector.size()*4);
    std::memcpy(&(*result.begin()), &(*_inVector.begin()), result.size());
//    U8_LLOG("");
    return result.substr(begin, end - begin);
}


void tiptiltCtrl::sleep(int _sleepCoeff)
{
    // usleep takes microseconds
    // we want to sleep for number of microseconds proportional to
    // number of written integers, passed through _sleepCoeff
    // Thus for 1 written integer to BCU, we will wait 1000 microsecs (1 millisec)
    // for 1664 written integers, we will wait 1664 millisecs ans so on.
//    U9_LLOG("wait for " << _sleepCoeff << " msec");
    usleep(m_writeBcuSleep * _sleepCoeff * 1.2);
}

void tiptiltCtrl::setFlags(uint64_t flags, uint64_t more_flags)
{
    if(flags & shins::flags::TT_SAVE_RAW_DIAG_DATA)
    {
        m_bSaveRawDiagData = true;
        U9_LLOG("Save raw diag data : ON");
    }
    if(flags & shins::flags::TT_CHECK_BCU_WRITE)
    {
        m_bCheckBcuWrite = true;
        U9_LLOG("Check Bcu Write : ON");
    }
};
void tiptiltCtrl::unsetFlags(uint64_t flags, uint64_t more_flags)
{
    if(flags & shins::flags::TT_SAVE_RAW_DIAG_DATA)
    {
        m_bSaveRawDiagData = false;
        U9_LLOG("Save raw diag data : OFF");
    }
    if(flags & shins::flags::TT_CHECK_BCU_WRITE)
    {
        m_bCheckBcuWrite = false;
        U9_LLOG("Check Bcu Write : OFF");
    }
};
void tiptiltCtrl::toggleFlags(uint64_t flags, uint64_t more_flags)
{
    if(flags & shins::flags::TT_SAVE_RAW_DIAG_DATA)
    {
        m_bSaveRawDiagData = !m_bSaveRawDiagData;
        U9_LLOG("Save raw diag data : " << (m_bSaveRawDiagData ? "ON" : "OFF"));
    }
    if(flags & shins::flags::TT_CHECK_BCU_WRITE)
    {
        m_bCheckBcuWrite = !m_bCheckBcuWrite;
        U9_LLOG("Check Bcu Write data : " << (m_bCheckBcuWrite ? "ON" : "OFF"));
    }
};


Ice::Int tiptiltCtrl::SequencerIConfigCamera(const std::map<std::string, std::string> &_dictTipTiltSetup)
{
    std::string key;
    std::string strCommand, strReply;
    std::string strWinX, strWinY, strNCols, strNRows;
    Ice::Int status;
    unsigned int winYMax, winXMax;
    key = "TTFRAMERATE";
    std::map<std::string, std::string>::const_iterator itFind;
    itFind = _dictTipTiltSetup.find(key);
    if(itFind != _dictTipTiltSetup.end())
        strCommand = "set fps " + itFind->second;
    else
    {
        W_LLOG("Missing framerate for camera, setting it to default 10 fps");
        strCommand = "set fps 10";
    }
    status = send_command_to_camera(strCommand, strReply);
    U1_LLOG("CRED2 camera reply: " << strReply);

    if(status == EXIT_FAILURE)
    {
        // do something like throwing
    }
    else
    {
        strCommand = "set cropping on";
        status = send_command_to_camera(strCommand, strReply);
        U1_LLOG("CRED2 camera reply: " << strReply);
    }

    if(status == EXIT_FAILURE)
    {
        // do something like throwing
    }
    else
    {
        key = "TTWINCOORDY";
        std::map<std::string, std::string>::const_iterator itFind;
        itFind = _dictTipTiltSetup.find(key);

        if(itFind != _dictTipTiltSetup.end())
        {
            strWinY = itFind->second;
            key = "TTWINROWS";
            itFind = _dictTipTiltSetup.find(key);
            if(itFind != _dictTipTiltSetup.end())
            {
                strNRows = itFind->second;
                winYMax = su::convertStringToType<unsigned int>(strWinY)
                    + su::convertStringToType<unsigned int>(strNRows);
                strCommand = "set cropping rows " + strWinY + "-"
                    + su::convertTypeToString<unsigned int>(winYMax - 1);
                status = send_command_to_camera(strCommand, strReply);
            }
            else
            {
                W_LLOG("Missing number of rows for for teccam windowing");
                // throw
            }
        }
        else
        {
            W_LLOG("Missing coordinate Y for teccam windowing");
            // throw
        }
    }
    if(status == EXIT_FAILURE)
    {
        // do something like throwing
    }
    else
    {
        key = "TTWINCOORDX";
        std::map<std::string, std::string>::const_iterator itFind;
        itFind = _dictTipTiltSetup.find(key);

        if(itFind != _dictTipTiltSetup.end())
        {
            strWinX = itFind->second;
            key = "TTWINCOLS";
            itFind = _dictTipTiltSetup.find(key);
            if(itFind != _dictTipTiltSetup.end())
            {
                strNCols = itFind->second;
                winXMax = su::convertStringToType<unsigned int>(strWinX)
                    + su::convertStringToType<unsigned int>(strNCols);
                strCommand = "set cropping columns " + strWinX + "-"
                    + su::convertTypeToString<unsigned int>(winXMax - 1);
                status = send_command_to_camera(strCommand, strReply);
            }
            else
            {
                W_LLOG("Missing number of rows for for teccam windowing");
                // throw
            }
        }
        else
        {
            W_LLOG("Missing coordinate Y for teccam windowing");
            // throw
        }
    }
    if(status == EXIT_FAILURE)
    {
        // do something like throwing
    }
    else
    {
        key = "TTCAMTINT";
        std::map<std::string, std::string>::const_iterator itFind;
        itFind = _dictTipTiltSetup.find(key);

        if(itFind != _dictTipTiltSetup.end())
        {
            strCommand = "set tint " + itFind->second;
            status = send_command_to_camera(strCommand, strReply);
        }
        else
        {
            W_LLOG("Missing integration time for TECCAM");
            // throw
        }
    }

    if(status == EXIT_FAILURE)
    {
        // do something like throwing
    }
    else
    {
        strCommand = "set badpixel on";
        status = send_command_to_camera(strCommand, strReply);
    }

    if(status == EXIT_FAILURE)
    {
        // do something like throwing
    }
    else
    {
        strCommand = "set bias off";
        status = send_command_to_camera(strCommand, strReply);
    }

    if(status == EXIT_FAILURE)
    {
        // do something like throwing
    }
    else
    {
        strCommand = "set flat off";
        status = send_command_to_camera(strCommand, strReply);
    }

    if(status == EXIT_FAILURE)
    {
        // do something like throwing
    }
    else
    {
        strCommand = "set rawimages on ";
        status = send_command_to_camera(strCommand, strReply);
    }
    return status;
}



