// $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/src/util/shins_util.cpp 3975 2022-06-14 18:34:48Z depa $

#include "shins_util.h"
#include <boost/algorithm/string.hpp>


namespace shins {
namespace util {

  
std::map<std::string, std::string> build_cfg_files_map()     //mapCfgFiles;
{
    U6_LLOG(__FUNCTION__);
    std::string SHINS_CONFIG = get_env("SHINS_CONFIG");
    std::string strConfigDir;
    if(SHINS_CONFIG.empty())
        strConfigDir = "/opt/tan/etc/";
    else
        strConfigDir = SHINS_CONFIG + "/etc/";
    U4_LLOG("shins_util.cpp::build_cfg_files_map::strConfigDir : " << strConfigDir);
    std::map<std::string, std::string> mapCfgFiles;
    mapCfgFiles[constants::APODIZER_W]     = strConfigDir+"motion_ctrl/wheels/motion_ctrl.wheels.apodizer_w-dev.cfg";
    mapCfgFiles[constants::CORO_SLIT_W]    = strConfigDir+"motion_ctrl/wheels/motion_ctrl.wheels.coro_slit_w-dev.cfg";
    mapCfgFiles[constants::DB_FILT_W]      = strConfigDir+"motion_ctrl/wheels/motion_ctrl.wheels.db_filt_w-dev.cfg";
    mapCfgFiles[constants::LYOT_GRISM_W]   = strConfigDir+"motion_ctrl/wheels/motion_ctrl.wheels.lyot_grism_w-dev.cfg";
    mapCfgFiles[constants::ND_FILT_W]      = strConfigDir+"motion_ctrl/wheels/motion_ctrl.wheels.nd_filt_w-dev.cfg";
    mapCfgFiles[constants::SCI_FILT_W1]    = strConfigDir+"motion_ctrl/wheels/motion_ctrl.wheels.sci_filt_w1-dev.cfg";
    mapCfgFiles[constants::SCI_FILT_W2]    = strConfigDir+"motion_ctrl/wheels/motion_ctrl.wheels.sci_filt_w2-dev.cfg";
    mapCfgFiles[constants::INBEAM_DEP]     = strConfigDir+"motion_ctrl/stages/motion_ctrl.stagex.inbeam_dep-dev.cfg";
    mapCfgFiles[constants::PUPIL_LENS_DEP] = strConfigDir+"motion_ctrl/stages/motion_ctrl.stagex.pupil_lens_dep-dev.cfg";
    mapCfgFiles[constants::SHUTTER]        = strConfigDir+"motion_ctrl/stages/motion_ctrl.stagex.shutter-dev.cfg";
    mapCfgFiles[constants::INBEAM_TT]      = strConfigDir+"motion_ctrl/stages/motion_ctrl.stagexy.inbeam_tt-dev.cfg";

    mapCfgFiles[constants::CAL_MIRROR_DEP] = strConfigDir+"calunit_ctrl/calunit_ctrl.cal_mirror_dep-dev.cfg";
    mapCfgFiles[constants::CAL_FIBER_DEP]  = strConfigDir+"calunit_ctrl/calunit_ctrl.cal_fiber_dep-dev.cfg";
    mapCfgFiles[constants::CAL_FILTER_DEP] = strConfigDir+"calunit_ctrl/calunit_ctrl.cal_filter_dep-dev.cfg";

    // mapCfgFiles[constants::calFFLamp]    = "";
    // mapCfgFiles[constants::calFiberLamp1] = "";
    // mapCfgFiles[constants::calFiberLamp2] = "";

    mapCfgFiles[constants::ADC1]          = strConfigDir+"track_ctrl/track_ctrl.adc.prisms.1-dev.cfg";
    mapCfgFiles[constants::ADC2]          = strConfigDir+"track_ctrl/track_ctrl.adc.prisms.2-dev.cfg";
    mapCfgFiles[constants::DROT]          = strConfigDir+"track_ctrl/track_ctrl.drot.derotator-dev.cfg";
    U8_LLOG("");
    return mapCfgFiles;
}

std::map<std::string, int> initMap()
{
    U6_LLOG(__FUNCTION__);
    std::map<std::string, int> mapTanPorts;

    U4_LLOG("initMap::mapTanPorts 2");
    mapTanPorts.insert(std::pair<std::string, int>("ltcs.iif-svr", 10000));
    mapTanPorts.insert(std::pair<std::string, int>("motion_ctrl",  50200));    // DUMMY 60200;
    mapTanPorts.insert(std::pair<std::string, int>("track_ctrl",   50300));    // DUMMY 60300
    mapTanPorts.insert(std::pair<std::string, int>("calunit_ctrl", 50400));    // DUMMY 60400
    mapTanPorts.insert(std::pair<std::string, int>("tiptilt_ctrl", 50220));
    U8_LLOG("");
    return mapTanPorts;
}

//std::map<std::string, int> mapTanPorts = initMap();

/**
 *  \brief  string using boost algorithms
 *
 *  Detailed description
 *
 *  \param string _str: the string to be split
 *  \param string _token: string containing characheters delimiting split
 *  \return std::vector of string
 */
std::vector< std::string > split_with_boost(const std::string& _str, const std::string& _strTokens)
{
//    U6_LLOG(__FUNCTION__);
    std::vector< std::string > SplitVec;
    boost::split( SplitVec, _str, boost::algorithm::is_any_of(_strTokens), boost::algorithm::token_compress_on );
//    U8_LLOG("");
    return SplitVec;
}


mapPosType get_main_named_pos_map_boost(const std::map<std::string, std::string>& _mapCfgFiles)
{
    U6_LLOG(__FUNCTION__);
    mapPosType mapMain;
    std::map<std::string, std::tr1::tuple<int, int> > mapDevNamedPos;
    std::ifstream ifConfig;
    std::string line = "";
    std::string strNamedPos = "", strTokens = "+#.=:\"";
    std::vector<std::string> vecSplit;
    int idxNamedPos = -1, stepsNamedPos, invalid_steps = -1234567890;
    std::vector<std::string>::iterator itFind;

    std::stringstream log_message(" ~ MapConfigFiles ");
    log_message << " ~ " << _mapCfgFiles.size() << " devices :";
    std::map<std::string, std::string>::const_iterator it;
    for(it = _mapCfgFiles.begin(); it !=_mapCfgFiles.end(); ++it)
    {
        ifConfig.open(it->second.c_str());
//        if(!(ifConfig.is_open()))
//            A_LLOG("Could not open file : " << it->second.c_str());

        log_message << "* " << it->first << " ~ ";
//        std::stringstream log_message2("Named positions for ");
//M        log_message2 << it->first << " :";
        int counter = 0;
        while (std::getline(ifConfig, line))
        {
            if(boost::algorithm::starts_with(line, "CFG.NAMED_POS.ABS"))
            {
                counter++;
                stepsNamedPos = invalid_steps;
                vecSplit = split_with_boost(line, strTokens);
                itFind = std::find(vecSplit.begin(), vecSplit.end(), "ABS");
                ++itFind;
                std::istringstream( *itFind ) >> idxNamedPos;
                ++itFind; ++itFind;
                // if positive steps are preceded by plus sign one more element is to be skipped.
                std::string candidate = *itFind;
                if(std::remove(candidate.begin(), candidate.end(), ' ') == candidate.begin())itFind++;
                std::istringstream( *itFind ) >> stepsNamedPos;
                if(stepsNamedPos == invalid_steps)
                {
                    E_LLOG(__FUNCTION__ << " ~ Error reading steps in named position");
                }
                itFind = std::find(vecSplit.begin(), vecSplit.end(), "NAME");
                ++itFind;
                strNamedPos = *itFind;
                log_message << strNamedPos << " steps = " << stepsNamedPos << " * ";

//                log_message2 << strNamedPos << " " << idxNamedPos << " ~ ";
//                U9_LLOG(__FUNCTION__ << " " << it->first << " ~ " << strNamedPos << " " << idxNamedPos);

                mapDevNamedPos.insert(std::pair<std::string, std::tr1::tuple<int, int> >(strNamedPos, std::tr1::tuple<int, int>(idxNamedPos, stepsNamedPos)));
            }
        }
        if(counter == 0){
            A_LLOG("Could not find Named positions in file :" << it->second.c_str()); }
        else
        {
            U7_LLOG("Found " << counter << " Named positions in file : " << it->second.c_str());
            U7_LLOG(log_message.str());
            log_message.str("");
        }
        mapMain.insert(std::pair<std::string, std::map<std::string, std::tr1::tuple<int, int> > >(it->first, mapDevNamedPos));
        mapDevNamedPos.clear();
        ifConfig.close();
    }
    U8_LLOG("");
    return mapMain;
}

/**
 *  \brief get alias from alias file and return a standard C++ vector
 *
 *  The method returns a STL vector containing endpoint names as [ALIAS=aliasName].
 *  It scans the alias file looking for the [ALIAS= substring and extracts
 *  the substring between the '=' and the first ','
 *  an example of alias string is
 *  motion_ctrl-svr.nd_w.ALIAS:Endpoint=[ALIAS=nd_w, NAME=motion_ctrl, PATH=MOTION_CTRL.ND_W.SVC]
 *
 *  \param string _strFileName: path to aliases file
 *  \param string _component:   sw componet, which can assume specific values: motion_ctrl, calunit_ctrl, track_ctrl
 *
 *  \return return type
 */
std::vector<std::string> getAliasesFromFile(const std::string& _strFileName, const std::string& _component)
{
    U6_LLOG(__FUNCTION__ << " ~ "  << _strFileName);
    std::ifstream ifsAlias(_strFileName.c_str(), std::ifstream::in);

    std::vector<std::string> vecAlias;
    std::vector<std::string> tmpVecSplit;
    std::vector<std::string>::iterator itFind;

    std::string line = "";
    std::string strAlias = "";
    std::string strTokens = "[+#,=:\"";

    std::stringstream log_msg(" ~ Aliases: ");

    while (std::getline(ifsAlias, line))
    {
        if(boost::algorithm::starts_with(line, _component) &&
            !(boost::algorithm::contains(line, "-dev"))     &&    // excluding device node aliases
            (boost::algorithm::contains(line, "ALIAS="))    &&    // excluding alias endpoint for server
            !(boost::algorithm::contains(line, "LABTEST"))  &&    //
            !(boost::algorithm::contains(line, "motionCtrl")))
        {
            tmpVecSplit = shins::util::split_with_boost(line, strTokens);
            itFind = std::find(tmpVecSplit.begin(), tmpVecSplit.end(), "ALIAS");
            if(itFind < tmpVecSplit.end())
            {
                ++itFind;
                vecAlias.push_back(*itFind);
                tmpVecSplit.clear();
                log_msg << *itFind << "; ";
            }
            else
            {
                E_LLOG("Did not find alias in line");
                throw std::runtime_error("Did not find alias in line " + line);
            }
        }
    }
    ifsAlias.close();
    U8_LLOG(__FUNCTION__ << " - Aliases : " << log_msg.str());
    return vecAlias;
}


boost::chrono::steady_clock::duration timer(const int& _movingDevs, const boost::chrono::steady_clock::duration& _delay)
{
    U6_LLOG(__FUNCTION__);
    boost::chrono::steady_clock::time_point start = boost::chrono::steady_clock::now();
    boost::chrono::steady_clock::duration elTime  = boost::chrono::steady_clock::now()-start;
    while ((_movingDevs > 0) && (elTime<_delay))
    {
        elTime = boost::chrono::steady_clock::now()-start;
        // N_LLOG(m_moving_devs);
        // continue;
    }
    U8_LLOG("");
    return elTime;

    // if(elTime > _delay)
    //   {
    //     E_LLOG(" TIMED OUT in motion to named position!");
    //     N_LLOG("N wheels = " << m_ptrMultiWheel->getNdevices());
    //     N_LLOG("N stageX = " << m_ptrMultiStageX->getNdevices());

    //     throw std::runtime_error("motion_ctrl TIMED OUT in motion to named position.  Elapsed time = " + shins::util::convertToString(elTime));
    //   }
}


/** @brief      Reinterpret float array as integer array to be written in BCU memory
  * @param[in]  _vecFloat  The vector float
  * @return     { description_of_the_return_value } */
std::vector<int> reinterpret_float_to_int(const std::vector<float>& _vecFloat)
{
    U6_LLOG(__FUNCTION__);
    std::vector<int> vecInt;

    vecInt.resize(_vecFloat.size(), 0);
#ifdef BOOST_NO_CXX11_LAMBDAS
    for(unsigned int i=0; i<_vecFloat.size(); i++)
    {
        std::memcpy(&vecInt[i], &_vecFloat[i], sizeof(_vecFloat[i]));
    }
#else
    std::transform(vecInt.begin(), vecInt.end(), _vecFloat.begin(), vecInt.begin(),
                   [](int& _n, const float& _f){
                std::memcpy(&_n, &_f, sizeof(_f));
                return _n;
            });
#endif
    U8_LLOG("");
    return vecInt;
}

std::vector<int> uint_to_seqint(unsigned int sau)
{
    std::vector<int> vecInt(1,0);
    std::memcpy(&*vecInt.begin(), &sau, vecInt.size()*sizeof(int));
    return vecInt;
}

/** @brief      Returns value for environment variable
  * @param[in]  _vecFloat  The vector float
  * @return     { description_of_the_return_value } */
std::string get_env(const std::string& _var)
{
    U6_LLOG(__FUNCTION__);
    const char * val = std::getenv( _var.c_str() );
    if( val == 0 )
    {
        W_LLOG(__FUNCTION__ << " ~ std::getenv(" << _var.c_str() << ") Returned NULL");
        U8_LLOG("");
        return "";
    }
    else
    {
        U8_LLOG(__FUNCTION__ << " ~ std::getenv(" << _var.c_str() << ") Returned : " << val);
        return val;
    }
}


void io_service_thread(boost::shared_ptr< boost::asio::io_service > _ptrIoService)
{
    U6_LLOG(__FUNCTION__);
    N_LLOG("Thread [" << boost::this_thread::get_id() << "] Started (" << _ptrIoService << ")");
    U8_LLOG("");

    _ptrIoService->run();// never returns

    {
        U6_LLOG(__FUNCTION__);
        N_LLOG("Thread [" << boost::this_thread::get_id() << "] Finished (" << _ptrIoService << ")");
        U8_LLOG("");
    }
}

std::string get_login()
{
    U6_LLOG(__FUNCTION__);
    char *buf;

    buf = new(char);
    buf = getlogin();

    U8_LLOG("");
    return std::string(buf);
}


int create_telemetry_dirs()
{
    U6_LLOG(__FUNCTION__);
    std::string telemetryRoot = "/data/telemetry/shins";
    std::string sashaRoot = telemetryRoot + "/sasha";
    std::ostringstream pathToTelemetry;
    boost::posix_time::ptime UTCtime;
    long month;
    int status = EXIT_SUCCESS;
    UTCtime = boost::posix_time::second_clock::universal_time();
    month = UTCtime.date().month();

    pathToTelemetry << "/" << UTCtime.date().year() << "/"
                    << std::setfill('0') << std::setw(2) << month << "/"
                    << std::setw(2) << UTCtime.date().day() << "/"
                    << std::setfill(' ');

    boost::filesystem::path dir(sashaRoot+pathToTelemetry.str());
    

    try
    {
        if(!(boost::filesystem::exists(dir)))
        {
            boost::filesystem::create_directories(dir);

            if(boost::filesystem::exists(dir))
            {
                status = EXIT_SUCCESS;
                boost::filesystem::permissions(dir, boost::filesystem::all_all);
            }
            else
            {
                status = EXIT_FAILURE;
            }
        }
    }
    catch(boost::filesystem::filesystem_error e)
    {
        U4_LLOG(e.what());
        U8_LLOG("");
        throw (e);
    }
    U8_LLOG("");
    return status;
}

std::string seq_int_to_str(Nice::SeqInt seq_int, int halfsize)
{
    std::stringstream result;
    result << " ~ ";
    std::vector<Nice::Int>::const_iterator it;
    int size = seq_int.size();
    if(size <= 2*halfsize)
    {
        int counter = 0;
        for(it = seq_int.begin(); it < seq_int.end(); it++)
        {
            if(counter % 10 == 9)
                result << *it << " % ";
            else
                result << *it << " ~ ";
            counter++;
        }
    }
    else
    {
        int counter = 0;
        for(it = seq_int.begin(); it < seq_int.end(); it++)
        {
            if(counter == halfsize)
            {
                result << "####" << size - 2*halfsize;
                if((size - halfsize) % 10 == 9)
                    result << "#### % ";
                else
                    result << "#### ~ ";
            }
            else if(counter < halfsize || counter >= size - halfsize)
            {
                if(counter % 10 == 9)
                    result << *it << " % ";
                else
                    result << *it << " ~ ";
            }
            counter++;
        }
    }
    return result.str();
}

std::string seq_float_to_str(Nice::SeqFloat seq_float, int halfsize)
{
    std::stringstream result;
    result << std::showpoint << " ~ ";
    std::vector<Nice::Float>::const_iterator it;
    int size = seq_float.size();
    if(size <= 2*halfsize)
    {
        int counter = 0;
        for(it = seq_float.begin(); it < seq_float.end(); it++)
        {
            if(counter % 10 == 0)
                result << *it << " % ";
            else
                result << *it << " ~ ";
            counter++;
        }
    }
    else
    {
        int counter = 0;
        for(it = seq_float.begin(); it < seq_float.end(); it++)
        {
            if(counter == halfsize)
            {
                result << "####" << size - 2*halfsize;
                if((size - halfsize) % 10 == 9)
                    result << "#### % ";
                else
                    result << "#### ~ ";
            }
            else if(counter < halfsize || counter >= size - halfsize)
            {
                if(counter % 10 == 0)
                    result << *it << " % ";
                else
                    result << *it << " ~ ";
            }
            counter++;
        }
    }

    return result.str();
}


std::string seq_double_to_str(std::vector<double> seq_double, int halfsize)
{
    std::stringstream result;
    result << std::showpoint << " ~ ";
    std::vector<double>::const_iterator it;
    int size = seq_double.size();
    if(size <= 2*halfsize)
    {
        int counter = 0;
        for(it = seq_double.begin(); it < seq_double.end(); it++)
        {
            if(counter % 10 == 0)
                result << *it << " % ";
            else
                result << *it << " ~ ";
            counter++;
        }
    }
    else
    {
        int counter = 0;
        for(it = seq_double.begin(); it < seq_double.end(); it++)
        {
            if(counter == halfsize)
            {
                result << "####" << size - 2*halfsize;
                if((size - halfsize) % 10 == 9)
                    result << "#### % ";
                else
                    result << "#### ~ ";
            }
            else if(counter < halfsize || counter >= size - halfsize)
            {
                if(counter % 10 == 0)
                    result << *it << " % ";
                else
                    result << *it << " ~ ";
            }
            counter++;
        }
    }

    return result.str();
}


std::map<std::string, std::tr1::tuple<double, double> > get_main_units_one_axe(const std::string & cfgFile)
{
    U6_LLOG(__FUNCTION__);
    std::map<std::string, std::tr1::tuple<double, double> > result;
    std::ifstream ifConfig;
    std::string line = "";
    std::string strTokens = "#=:";
    std::vector<std::string> vecSplit;
    ifConfig.open(cfgFile.c_str());
    int counter = 0, counter2 = 0;
    int NumAxes = 0;

    //Detect TWO AXIS device:
    {
        while(std::getline(ifConfig, line))
        {
            counter++;
            std::string id = "CFG.DATASHEET.CONFIG";
            std::string clear_line = remove_whitespaces(line);
            size_t identifier = clear_line.find(id);
            if(identifier != std::string::npos)
            {
                size_t comment = left(clear_line, identifier).find("#");
                if(comment != std::string::npos)continue;
                NumAxes++;
            }
        }
        ifConfig.clear();
        ifConfig.seekg(0, ifConfig.beg);
    }
    counter = 0;
    while (std::getline(ifConfig, line))
    {
        counter++;
        std::string id = "CFG.TRANS.";
        std::string clear_line = remove_whitespaces(line);
        size_t identifier = clear_line.find(id);
        if(identifier != std::string::npos)
        {
            size_t comment = left(clear_line, identifier).find("#");
            if(comment != std::string::npos)continue;
            vecSplit = split_with_boost(right(clear_line, clear_line.size() - (identifier + id.size())), strTokens);
            switch(NumAxes)
            {
            case 1:
                {
                    std::string transform_tokens = "[,]";
                    std::vector<std::string> trans = split_with_boost(remove_whitespaces(vecSplit[2]), transform_tokens);
                    std::vector<std::string>::iterator itSplit;
                    for(itSplit = trans.begin(); itSplit != trans.end(); /*itSplit++*/)
                    {
                        if(itSplit->empty())trans.erase(itSplit);
                        else itSplit++;
                    }
                    // vecSplit[2] == "[6.867e-3,0]+LABEL";
                    if(trans.size() == 2 || trans.size() == 3)
                    {
                        counter2++;
                        double scale, offset;
                        std::istringstream(trans[0]) >> scale;
                        std::istringstream(trans[1]) >> offset;
                        result.insert(std::pair<std::string, std::tr1::tuple<double, double> >(
                            vecSplit[0], std::tr1::tuple<double, double>(scale, offset)));
                    }
                    else
                    {
                        E_LLOG("Found " << counter2 << " transformation parameters in : " << cfgFile << " for unit : " << vecSplit[0]);
                    }
                }
                break;
            default:
                {
                    std::stringstream log_msg;
                    log_msg << "Found " << NumAxes << " axes in : " << cfgFile << ". Managed One and axe devices!";
                    E_LLOG(log_msg.str());
                    U8_LLOG("");
                    throw std::invalid_argument(log_msg.str());
                }
            }
        }
    }
    if(counter == 0){
        A_LLOG("Could not find UNITS in file :" << cfgFile); }
    else
    {
        U7_LLOG("Found " << counter2 << " UNITS in file : " << cfgFile);
    }
    ifConfig.close();

    U8_LLOG("");
    return result;
}

std::map<std::string, double > get_named_positions_one_axe(const std::string & cfgFile)
{
    U6_LLOG(__FUNCTION__);
    std::map<std::string, double > result;
    std::ifstream ifConfig;
    std::string line = "";
    std::string strNamedPos = "", strTokens = "+#.=:\"";
    std::vector<std::string> vecSplit;
    int idxNamedPos = -1;
    double stepsNamedPos, invalid_steps = -1234567890.0;
    std::vector<std::string>::iterator itFind;

    ifConfig.open(cfgFile.c_str());
    int counter = 0;
    while (std::getline(ifConfig, line))
    {
        if(boost::algorithm::starts_with(line, "CFG.NAMED_POS.ABS"))
        {
            counter++;
            stepsNamedPos = invalid_steps;
            vecSplit = split_with_boost(line, strTokens);
            itFind = std::find(vecSplit.begin(), vecSplit.end(), "ABS");
            ++itFind;
            std::istringstream( *itFind ) >> idxNamedPos;
            ++itFind; ++itFind;
            // if positive steps are preceded by plus sign one more element is to be skipped.
            std::string candidate = *itFind;
            if(std::remove(candidate.begin(), candidate.end(), ' ') == candidate.begin())itFind++;
            std::istringstream( *itFind ) >> stepsNamedPos;
            if(stepsNamedPos == invalid_steps)
            {
                E_LLOG(__FUNCTION__ << " ~ Error reading steps in named position");
            }
            itFind = std::find(vecSplit.begin(), vecSplit.end(), "NAME");
            ++itFind;
            strNamedPos = *itFind;
            result.insert(std::pair<std::string, double>(strNamedPos, stepsNamedPos));
        }
    }
    if(counter == 0){
        A_LLOG("Could not find Named positions in file :" << cfgFile); }
    ifConfig.close();
    U8_LLOG("");
    return result;
}

std::string get_alias_from_config(const std::string & cfgFile)
{
    U6_LLOG(__FUNCTION__);
    std::string result;
    std::ifstream ifConfig;
    std::string line = "";
    std::string strTokens = "+#.=:\"[,]";
    std::vector<std::string> vecSplit;

    ifConfig.open(cfgFile.c_str());
    int counter = 0;
    while (std::getline(ifConfig, line))
    {
        counter++;
        std::string id = "LOAD";
        size_t identifier = line.find(id);
        if(identifier != std::string::npos)
        {
            size_t comment = left(line, identifier).find("#");
            if(comment != std::string::npos)continue;
            //[NAME=PUPIL_LENS_DEP, LIB=BasdaMoccaMpiaNDevice]
            vecSplit = split_with_boost(remove_whitespaces(line), strTokens);
            std::vector<std::string>::iterator itSplit;
            for(itSplit = vecSplit.begin(); itSplit != vecSplit.end(); /*itSplit++*/)
            {
                if(itSplit->empty())vecSplit.erase(itSplit);
                else itSplit++;
            }
            std::vector<std::string>::iterator itTokens = std::find(vecSplit.begin(), vecSplit.end(), id);
            if(itTokens != vecSplit.end())
            {
                result = vecSplit[2];
                break;
            }
        }
    }
    if(counter == 0){
        A_LLOG("Could not find ALIAS in file :" << cfgFile); }
    ifConfig.close();
    U8_LLOG("");
    return result;
}

std::map<std::string, std::tr1::tuple<double, double, double, double, double, double> > get_main_units_two_axes(const std::string & cfgFile)
{
    U6_LLOG(__FUNCTION__);
    std::map<std::string, std::tr1::tuple<double, double, double, double, double, double> > result;
    std::ifstream ifConfig;
    std::string line = "";
    std::string strTokens = "#=:";
    std::vector<std::string> vecSplit;
    ifConfig.open(cfgFile.c_str());
    int counter = 0, counter2 = 0;
    int NumAxes = 0;

    //Detect TWO AXIS device:
    {
        while(std::getline(ifConfig, line))
        {
            counter++;
            std::string id = "CFG.DATASHEET.CONFIG";
            std::string clear_line = remove_whitespaces(line);
            size_t identifier = clear_line.find(id);
            if(identifier != std::string::npos)
            {
                size_t comment = left(clear_line, identifier).find("#");
                if(comment != std::string::npos)continue;
                NumAxes++;
            }
        }
        ifConfig.clear();
        ifConfig.seekg(0, ifConfig.beg);
    }

    while (std::getline(ifConfig, line))
    {
        counter++;
        std::string id = "CFG.TRANS.";
        std::string clear_line = remove_whitespaces(line);
        size_t identifier = clear_line.find(id);
        if(identifier != std::string::npos)
        {
            size_t comment = left(clear_line, identifier).find("#");
            if(comment != std::string::npos)continue;
            vecSplit = split_with_boost(right(clear_line, clear_line.size() - (identifier + id.size())), strTokens);
            switch(NumAxes)
            {
            case 2:
                {
                    std::string transform_tokens = "[,]";
                    std::vector<std::string> trans = split_with_boost(remove_whitespaces(vecSplit[2]), transform_tokens);
                    std::vector<std::string>::iterator itSplit;
                    for(itSplit = trans.begin(); itSplit != trans.end(); /*itSplit++*/)
                    {
                        if(itSplit->empty())trans.erase(itSplit);
                        else itSplit++;
                    }
                    std::vector<std::string>::iterator itPrint = trans.begin();
                    // vecSplit[2] == "[[6.867e-6,0],[0, 6.867e-6],[0,0]]+LABEL";
                    if(trans.size() == 6 || trans.size() == 7)
                    {
                        counter2++;
                        double ScaleX, ShearX, ShearY, ScaleY, OffsetX, OffsetY;
                        std::istringstream(trans[0]) >> ScaleX;
                        std::istringstream(trans[1]) >> ShearX;
                        std::istringstream(trans[2]) >> ShearY;
                        std::istringstream(trans[3]) >> ScaleY;
                        std::istringstream(trans[4]) >> OffsetX;
                        std::istringstream(trans[5]) >> OffsetY;
                        result.insert(std::pair<std::string, std::tr1::tuple<double, double, double, double, double, double> >(
                            vecSplit[0], std::tr1::tuple<double, double, double, double, double, double>(
                                ScaleX, ShearX, ShearY, ScaleY, OffsetX, OffsetY)));
                    }
                    else
                    {
                        E_LLOG("Found " << counter2 << " transformation parameters in : " << cfgFile << " for unit : " << vecSplit[0]);
                    }
                }
                break;
            default:
                {
                    std::stringstream log_msg;
                    log_msg << "Found " << NumAxes << " axes in : " << cfgFile << ". Managed Two axes devices!";
                    E_LLOG(log_msg.str());
                    U8_LLOG("");
                    throw std::invalid_argument(log_msg.str());
                }
            }
        }
    }
    if(counter == 0){
        A_LLOG("Could not find UNITS in file :" << cfgFile); }
    else
    {
        U7_LLOG("Found " << counter2 << " UNITS in file : " << cfgFile);
    }
    ifConfig.close();

    U8_LLOG("");
    return result;
}


void os_log_banner(const std::string & _banner)
{
    int banner_len = 60;
    U9_LLOG(std::string(banner_len, '#'));
    U9_LLOG(banner_string(_banner, '#', banner_len, true));
    U9_LLOG(std::string(banner_len, '#'));
}

void debug_save_memory_block(const char * pBlock, size_t block_size, const char * pFilename)
{
    std::string debug_file = pFilename;
    std::ofstream fout;
    fout.open(debug_file.c_str(), std::ios::binary | std::ios::out);
    if(fout.fail())
    {
        E_LLOG(__FUNCTION__ << " ~ Error: could not open file " << debug_file.c_str());
    }
    else
    {
        U9_LLOG(__FUNCTION__ << " ~ Saved file : " << pFilename);
        fout.write(pBlock, block_size);
        fout.close();
    }
}

}
}
