/*
 * seqITcs.cpp
 *
 *  Created on: Apr 6, 2022
 *      Author: fulvio
 */
#include "seqITcs.hpp"

namespace suc = shins::util::constants;


SeqTcs * SeqTcs::m_pSeqTcs = NULL;

SeqTcs::SeqTcs(Ice::PropertiesPtr prop):
    m_ReferenceSide("LEFT"),
    m_service_status(su::SERVICE_NOTCONNECTED)
{
    U6_LLOG(__FUNCTION__);
    m_iif_svc = NULL;
    m_service = prop->getProperty("OcsTcsService.Name");
    m_BinocularLeftPreset = m_BinocularRightPreset = m_BinocularRightOffset = m_BinocularLeftOffset = false;
    m_pSeqTcs = this;
    m_bSingleSideMode = true;
        m_TcsPreset.epoch = 0;
        m_TcsPreset.objra = 0;
        m_TcsPreset.objdec = 0;
        m_TcsPreset.objmag = 0;
        m_TcsPreset.pmra = 0;
        m_TcsPreset.pmdec = 0;
        m_TcsPreset.aora = 0;
        m_TcsPreset.aodec = 0;
        m_TcsPreset.aomag = 0;
        m_TcsPreset.pmaora = 0;
        m_TcsPreset.pmaodec = 0;
        m_TcsPreset.gsra = 0;
        m_TcsPreset.gsdec = 0;
        m_TcsPreset.gsmag = 0;
        m_TcsPreset.pmgsra = 0;
        m_TcsPreset.pmgsdec = 0;
        m_TcsPreset.object_name = "Undefined";
    U8_LLOG("");
}


SeqTcs::~SeqTcs()
{
}

su::EServiceStatus SeqTcs::Connect()
{
    if(m_service_status == su::SERVICE_NOTCONNECTED)
    {
        try
        {
            U1_LLOG("Creating AppConfigClient for ltcs_iif-svr");
        //     Nice::Endpoint ep = Nice::Endpoint::fromString("[ALIAS=ltcs_iif-svr.IIF, TIMEOUT=180]");
        //     m_ConfigClient = Nice::AppConfigClient::connectWithEndpointAtNode ( ep, "ltcs_iif-svr.IIF" );
            Nice::JointPtr NodeTcs = Nice::Application::config()->node("ltcs_iif-svr");
            m_ConfigClient = Nice::AppConfigClient::create(NodeTcs->getAtNode<Nice::Endpoint> ("ALIAS"), NodeTcs);
            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
        {
            Nice::JointPtr NodeTcs = Nice::Application::config()->node("ltcs_iif-svr");
            Ltcs::IifServiceWorkerInterfacePtr iif = Ltcs::IifServiceWorkerInterface::create(NodeTcs);
            m_iif_svc = iif;
            m_service_status = su::SERVICE_CONNECTED;
        }
        catch(...)
        {
            E_LLOG("Erorr connecting to ltcs_iif-svr service");
        }
    }
    if(m_service_status == su::SERVICE_CONNECTED)
    {
        try
        {
            m_iif_svc->changeState(Basda::STANDBY);
            usleep(500000);
            m_iif_svc->changeState(Basda::ONLINE);
            m_service_status = su::SERVICE_ONLINE;
            U1_LLOG("Connection established with service ltcs_iif-svr");
        }
        catch(...)
        {
            E_LLOG("Erorr changing state to MultiStageX");
        }
    }
    return m_service_status;
}


void SeqTcs::LogResult(const lbto::result & res, int line)
{
    std::string error = "Error : ";
    std::string warning = "Warning : ";
    if(res.rescode == EXIT_FAILURE)
    {
        std::stringstream log_msg;
        log_msg << error << line << " : rescode = " << res.rescode << "; resmsg[0] = " << res.resmsg[0];
        std::stringstream full_result;
        E_LLOG(log_msg.str());
        for(unsigned int j = 1; j < res.resmsg.size(); j++)
        {
            log_msg.str("");
            log_msg << error << res.resmsg[j];
            full_result << log_msg.str() << suc::tilde;
            E_LLOG(log_msg.str());
        }
        throw(std::runtime_error(full_result.str()));
    }
    else if(res.rescode == EXIT_WARNING)
    {
        std::stringstream log_msg;
        log_msg << warning << line << " rescode = " << res.rescode << "; resmsg[0] = " << res.resmsg[0];
        W_LLOG(log_msg.str());
        for(unsigned int j = 1; j < res.resmsg.size(); j++)
        {
            log_msg.str("");
            log_msg << warning << res.resmsg[j];
            W_LLOG(log_msg.str());
        }
    }
}

const su::STcsPreset & SeqTcs::GetPresetInformation()
{
    static su::STcsPreset preset;
    if(m_pSeqTcs)
        return m_pSeqTcs->m_TcsPreset;
    else
        return preset;
}


int SeqTcs::BinocularControl(const std::string &_flag)
{
    U6_LLOG(__FUNCTION__);
    lbto::result res;
    Ice::Int status = EXIT_SUCCESS;
    CheckConnection();
    if (m_iif_svc)
    {
        res = m_iif_svc->iifBinocularControl(_flag);
        status = res.rescode;
    }
    U8_LLOG(__FUNCTION__);
    return status;
}


int SeqTcs::IsReachable(bool except)
{
    int result = EXIT_SUCCESS;
    std::string errorMsg;
    CheckConnection();
    if(m_iif_svc)
    {
        if(!(m_iif_svc->isReachable()))
        {
            errorMsg = "Error :TCS is not reachable";
            E_LLOG(errorMsg);
            if(except)
            {
                throw std::runtime_error(errorMsg);
            }
            else
            {
                result = EXIT_FAILURE;
            }
        }
    }
    return result;
}


int SeqTcs::ValidatePresetSetup(const std::map<std::string, std::string> & _telSetup)
{
    std::vector<std::string> allowed_keys;
    allowed_keys.push_back("OBJRA");                // Object Right Ascension
    allowed_keys.push_back("OBJDEC");               // Object Declination
    allowed_keys.push_back("OBJMAG");               // Object Magnitude
    allowed_keys.push_back("PMRA");                 // Proper Motion Right Ascension
    allowed_keys.push_back("PMDEC");                // Proper Motion Declination
    allowed_keys.push_back("GSRA");                 // Guide Star Right Ascension
    allowed_keys.push_back("GSDEC");                // Guide Star Declination
    allowed_keys.push_back("GSMAG");                // Guide Star Magnitude
    allowed_keys.push_back("PMGSRA");               // Proper Motion Guide Star Right Ascension
    allowed_keys.push_back("PMGSDEC");              // Proper Motion Guide Star Declination
    allowed_keys.push_back("AORA");                 // Adaptive Optics Right Ascension
    allowed_keys.push_back("AODEC");                // Adaptive Optics Declination
    allowed_keys.push_back("AOMAG");                // Adaptive Optics Magnitude
    allowed_keys.push_back("AOMODE");               // Adaptive Optics Mode    "NORMAL", "REPOINT", "SKIPREF", "SKIPSTART", "SKIPSTARTREPOINT", "SKIPALL"
    allowed_keys.push_back("PMAORA");               // Proper Motion Adaptive Optics Right Ascension
    allowed_keys.push_back("PMAODEC");              // Proper Motion Adaptive Optics Declination

    allowed_keys.push_back("CoordinateSystem");
    allowed_keys.push_back("Epoch");
    allowed_keys.push_back("Equinox");
    allowed_keys.push_back("TelescopeMode");
    allowed_keys.push_back("TelescopeSide");
    allowed_keys.push_back("OBJECTNAME");
    allowed_keys.push_back("OBJEQUIN");

    allowed_keys.push_back("BinocularFlag");
    allowed_keys.push_back("OFFSET");

    std::map<std::string, std::string>::const_iterator itCheck;
    for(itCheck = _telSetup.begin(); itCheck != _telSetup.end(); itCheck++)
    {
        if(std::find(allowed_keys.begin(), allowed_keys.end(), itCheck->first) == allowed_keys.end())
        {
            std::stringstream log_msg;
            log_msg << "Warning : key " << itCheck->first << " not expected. Consider updating";
            W_LLOG(log_msg.str());
        }
    }
    return EXIT_SUCCESS;
}


int SeqTcs::PresetTelescope(const std::map< ::std::string, ::std::string> &_telSup)
{
    U6_LLOG(__FUNCTION__);
    ValidatePresetSetup(_telSup);

    // try {
    //     Nice::Time  tcs_wait = Nice::Time::seconds(150);
    //     m_iif_svc->changeState(Basda::ONLINE, tcs_wait);
    // } catch(...){E_LLOG(__FUNCTION__); throw; }
    bool settingLeftPreset = false, settingRightPreset = false;

    Ice::Int status = EXIT_SUCCESS;

    su::STcsPreset current_preset;

    try { current_preset.object_name = su::get_value_from_map<std::string>(_telSup, "OBJECTNAME"); } catch(const std::invalid_argument & e){W_LLOG("Missing Keyword OBJECTNAME");}

    try
    {
        m_ReferenceSide = su::str_tolower(su::get_value_from_map<std::string>(_telSup, "TelescopeSide"));
    }
    catch(const std::invalid_argument &e)
    {
        std::stringstream log_msg;
        log_msg << "Error : missing mandatory key TelescopeSide";
        E_LLOG(log_msg.str());
        U8_LLOG("");
        throw(std::runtime_error(log_msg.str()));
    }
    try
    {
        m_BinocularFlag = su::get_value_from_map<std::string>(_telSup, "BinocularFlag");
    }
    catch(const std::invalid_argument &e)
    {
        std::stringstream log_msg;
        log_msg << "Error : missing mandatory key BinocularFlag";
        E_LLOG(log_msg.str());
        U8_LLOG("");
        throw(std::runtime_error(log_msg.str()));
    }

    if(m_BinocularFlag == "OFF")
    {
        if(m_ReferenceSide == "both" || m_ReferenceSide == "left")
        {
            settingLeftPreset = true;
        }
        else if(m_ReferenceSide == "right")
        {
            settingRightPreset = true;
        }
    }

    if(m_BinocularFlag == "OFF" && (m_BinocularLeftPreset || m_BinocularRightPreset || m_BinocularLeftOffset || m_BinocularRightOffset))
    {
        m_BinocularLeftPreset = m_BinocularRightPreset = m_BinocularLeftOffset = m_BinocularRightOffset = false;
        std::stringstream log_msg;
        log_msg << "Error : Cannot start a preset when a Binocular synchronous operation is in progress. Synchronous operation will be cancelled";
        E_LLOG(log_msg.str());
        U8_LLOG("");
        throw(std::runtime_error(log_msg.str()));
    }
    if(m_BinocularFlag == "SYNCPRESET")
    {
        current_preset.binocular = true;
        if(m_ReferenceSide == "left" || m_ReferenceSide == "right")
        {
            if(m_ReferenceSide == "left")
            {
                if(m_BinocularLeftPreset == false && (m_BinocularLeftOffset == false && m_BinocularRightOffset == false))
                {
                    settingLeftPreset = m_BinocularLeftPreset = true;
                }
                else if(m_BinocularLeftPreset == false && (m_BinocularLeftOffset == true || m_BinocularRightOffset == true))
                {
                    std::stringstream log_msg;
                    log_msg << "Error : Cannot start preset when synchronous offset is in progress. Synchronous Offset will be cancelled";
                    m_BinocularLeftOffset = m_BinocularRightOffset = false;
                    E_LLOG(log_msg.str());
                    U8_LLOG("");
                    throw(std::runtime_error(log_msg.str()));
                }
                else
                {
                    std::stringstream log_msg;
                    log_msg << "Error : Binocular Left Preset already set. Synchronous Preset will be cancelled";
                    m_BinocularLeftPreset = false;
                    E_LLOG(log_msg.str());
                    U8_LLOG("");
                    throw(std::runtime_error(log_msg.str()));
                }
            }
            else
            {
                if(m_BinocularRightPreset == false && (m_BinocularLeftOffset == false && m_BinocularRightOffset == false))
                {
                    settingRightPreset = m_BinocularRightPreset = true;
                }
                else if(m_BinocularRightPreset == false && (m_BinocularLeftOffset == true || m_BinocularRightOffset == true))
                {
                    std::stringstream log_msg;
                    log_msg << "Error : Cannot start preset when synchronous offset is in progress. Synchronous Offset will be cancelled";
                    m_BinocularLeftOffset = m_BinocularRightOffset = false;
                    E_LLOG(log_msg.str());
                    U8_LLOG("");
                    throw(std::runtime_error(log_msg.str()));
                }
                else
                {
                    std::stringstream log_msg;
                    log_msg << "Error : Binocular Right Preset already set. Synchronous Preset will be cancelled";
                    m_BinocularRightPreset = false;
                    E_LLOG(log_msg.str());
                    U8_LLOG("");
                    throw(std::runtime_error(log_msg.str()));
                }
            }
        }
        else
        {
            std::stringstream log_msg;
            log_msg << "Error : When in binocular mode the telescope side must be \"left\" or \"right\"";
            E_LLOG(log_msg.str());
            U8_LLOG("");
            throw(std::runtime_error(log_msg.str()));
        }
    }
    else if(m_BinocularFlag == "SYNCOFFSET") // TODO
    {
        std::stringstream log_msg;
        log_msg << "Error : Flag SYNCOFFSET not allowed in preset telescope";
        E_LLOG(log_msg.str());
        U8_LLOG("");
        throw(std::runtime_error(log_msg.str()));
    }

    try
    {
        lbto::position2 target, guide, AOReference;
        lbto::result res;
        double epoch = 0;
        std::string LBTMode;
        std::string AoMode;
        std::string key = "", value = "", errorMsg = "";
        std::string fitsKey = "";

        try
        {
            key = "OBJRA";
            int degrees, minutes, seconds, milliseconds, ra_hours, ra_minutes, ra_seconds, ra_milliseconds;
            std::stringstream ra_string;
            value = su::get_value_from_map<std::string>(_telSup, key);
            target.coord1 = su::convertStringToType<double>(value);
            current_preset.objra = target.coord1;
            double coord1 = su::convertStringToType<double>(_telSup.find(key)->second);
            double degree_coord1 = coord1*180/3.141592653;
            degrees = degree_coord1;
            minutes = (degree_coord1 - degrees)*60;
            seconds = (degree_coord1 - degrees - minutes/60.)*3600;
            milliseconds = (degree_coord1 - degrees - minutes/60. - seconds/3600. + 0.5/3600000)*3600000;
            ra_hours = degree_coord1/15;
            ra_minutes = (degree_coord1 - ra_hours*15)*4;
            ra_seconds = (degree_coord1 - ra_hours*15 - ra_minutes/4.)*240;
            ra_milliseconds = (degree_coord1 - ra_hours*15 - ra_minutes/4. - ra_seconds/240. + 0.5/240000)*240000;
            ra_string << std::setfill('0') << std::setw(2) << ra_hours << ":" << std::setw(2) << ra_minutes << ":" << std::setw(2) << ra_seconds << "." << std::setw(3) << ra_milliseconds << " (RA)";
            U9_LLOG("OBJRA = " << target.coord1 << " (RAD) ~ " << degree_coord1 << " (DEG) ~ " << std::setfill('0') << std::setw(2) << degrees << ":" << std::setw(2) << minutes << ":" << std::setw(2) << seconds << "." << std::setw(3) << milliseconds << " (DEG) ~ " << ra_string.str());

            key = "OBJDEC";
            value = su::get_value_from_map<std::string>(_telSup, key);
            target.coord2 = su::convertStringToType<double>(value);
            current_preset.objdec = target.coord2;
            double degree_coord2 = target.coord2*180/3.141592653;
            degrees = degree_coord2;
            minutes = (degree_coord2 - degrees)*60;
            seconds = (degree_coord2 - degrees - minutes/60.)*3600;
            milliseconds = (degree_coord2 - degrees - minutes/60. - seconds/3600. + 0.5/3600000)*3600000;
            U9_LLOG("OBJDEC = " << target.coord2 << " (RAD) ~ " << degree_coord2 << " (DEG) ~ " << std::setfill('0') << std::setw(2) << degrees << ":" << std::setw(2) << minutes << ":" << std::setw(2) << seconds << "." << std::setw(3) << milliseconds << " (DEG)");

            key = "PMRA";
            value = su::get_value_from_map<std::string>(_telSup, key);
            target.pmcoord1 = su::convertStringToType<double>(value);
            current_preset.pmra = target.pmcoord1;

            key = "PMDEC";
            value = su::get_value_from_map<std::string>(_telSup, key);
            target.pmcoord2 = su::convertStringToType<double>(value);
            current_preset.pmdec = target.pmcoord2;

            key = "OBJMAG";
            value = su::get_value_from_map<std::string>(_telSup, key);
            target.apparentMagnitude = su::convertStringToType<double>(value);
            current_preset.objmag = target.apparentMagnitude;

        }
        catch(...)
        {
            std::stringstream log_msg;
            log_msg << "Error : Missing target parameter";
            E_LLOG(log_msg.str());
            throw(std::runtime_error(log_msg.str()));
        }

        // GUIDE (optional)
        try
        {
            // key = "GSRA";
            // value = su::get_value_from_map<std::string>(_telSup, key);
            // guide.coord1 = su::convertStringToType<double>(value);

            // key = "GSDEC";
            // value = su::get_value_from_map<std::string>(_telSup, key);
            // guide.coord2 = su::convertStringToType<double>(value);

            // key = "PMGSRA";
            // value = su::get_value_from_map<std::string>(_telSup, key);
            // guide.pmcoord1 = su::convertStringToType<double>(value);

            // key = "PMGSDEC";
            // value = su::get_value_from_map<std::string>(_telSup, key);
            // guide.pmcoord2 = su::convertStringToType<double>(value);

            // key = "GSMAG";
            // value = su::get_value_from_map<std::string>(_telSup, key);
            // guide.apparentMagnitude = su::convertStringToType<double>(value);
            throw(std::invalid_argument("Ignoring guide star"));
        }
        catch(const std::invalid_argument &e)
        {
//            U7_LLOG("Missing guide star parameter. Defaulting to target coordinates");
            guide.coord1 = target.coord1;
            guide.coord2 = target.coord2;
            guide.pmcoord1 = 0.0; // target.pmcoord1;
            guide.pmcoord2 = 0.0; // target.pmcoord2;
            guide.apparentMagnitude = target.apparentMagnitude;
        }

        current_preset.gsra = guide.coord1;
        current_preset.gsdec = guide.coord2;
        current_preset.pmgsra = guide.pmcoord1;
        current_preset.pmgsdec = guide.pmcoord2;
        current_preset.gsmag = guide.apparentMagnitude;

        // AO REFERENCE (Optional)
        try
        {
            key = "AORA";
            value = su::get_value_from_map<std::string>(_telSup, key);
            AOReference.coord1 = su::convertStringToType<double>(value);

            key = "AODEC";
            value = su::get_value_from_map<std::string>(_telSup, key);
            AOReference.coord2 = su::convertStringToType<double>(value);

            key = "PMAORA";
            value = su::get_value_from_map<std::string>(_telSup, key);
            AOReference.pmcoord1 = su::convertStringToType<double>(value);

            key = "PMAODEC";
            value = su::get_value_from_map<std::string>(_telSup, key);
            AOReference.pmcoord2 = su::convertStringToType<double>(value);
        }
        catch(const std::invalid_argument &e)
        {
            U7_LLOG("Missing AO reference star parameter. Defaulting to target coordinates");
            AOReference.coord1 = target.coord1;
            AOReference.coord2 = target.coord2;
            AOReference.pmcoord1 = target.pmcoord1;
            AOReference.pmcoord2 = target.pmcoord2;
            AOReference.apparentMagnitude = target.apparentMagnitude;
        }

        current_preset.aora = AOReference.coord1;
        current_preset.aodec = AOReference.coord2;
        current_preset.pmaora = AOReference.pmcoord1;
        current_preset.pmaodec = AOReference.pmcoord2;
        current_preset.aomag = AOReference.apparentMagnitude;

        // Extracting coordinate system and epoch from input map
        key = "CoordinateSystem";
        value = su::get_value_from_map<std::string>(_telSup, key);
        target.system = value; // "RADEC", "AZALT"
        guide.system = value;
        AOReference.system = value;
        current_preset.target_coordsys = target.system;

        key = "Epoch";
        value = su::get_value_from_map<std::string>(_telSup, key);
        epoch = su::convertStringToType<double>(value);
        target.epoch = epoch; // epoch (Year)
        guide.epoch = epoch;
        AOReference.epoch = epoch;
        current_preset.epoch = epoch;

        key = "Equinox";
        value = su::get_value_from_map<std::string>(_telSup, key);
        target.equinox = value; // "J2000"
        guide.equinox = value;
        AOReference.equinox = value;
        current_preset.equinox = value;

        try
        {
            m_TargetType = su::str_tolower(su::get_value_from_map<std::string>(_telSup, "TargetType"));
        }
        catch(const std::invalid_argument &e)
        {
            m_TargetType = "SIDEREAL";
            std::stringstream log_msg;
            log_msg << "Missing  key TargetType: defaulting to sidereal target";
            I_LLOG(log_msg.str());
//            U8_LLOG("");
//            throw(std::runtime_error(log_msg.str()));
        }

        if(m_TargetType == "DIFFERENTIAL")
        {
            // TODO add to telescope setup the following information
            lbto::nonsidereal ns;
            ns.type = m_TargetType;
            ns.wavelength = 0.5f;
            ns.file = ""; // file is an ephemeris file from JPL Horizions and must be present in the required place in the TCS
            ns.value0 = target.time; // differential MJD
            ns.value1 = target.coord1; // differential RA
            ns.value2 = target.coord2; // differential DEC
            ns.value3 = target.RARate; // differential dRa/dt*cos(DEC) (radian/day)
            ns.value4 = target.DECRate; // differential dDEC/dt (radian/day)
            try
            {
                m_TargetRaRate = su::get_value_from_map<double>(_telSup, "TargetRaRate");
            }
            catch(const std::invalid_argument &e)
            {
                m_TargetRaRate = 0;
                std::stringstream log_msg;
                log_msg << "Missing  key TargeRaRate";
                I_LLOG(log_msg.str());
            }
            try
            {
                m_TargetDecRate = su::get_value_from_map<double>(_telSup, "TargetDecRate");
            }
            catch(const std::invalid_argument &e)
            {
                m_TargetFile = "";
                std::stringstream log_msg;
                log_msg << "Missing  key TargeDecRate";
                I_LLOG(log_msg.str());
            }
            try
            {
                m_TargetTime = su::get_value_from_map<double>(_telSup, "TargetTime");
            }
            catch(const std::invalid_argument &e)
            {
                m_TargetFile = "";
                std::stringstream log_msg;
                log_msg << "Missing  key TargeTime";
                I_LLOG(log_msg.str());
            }
            target.RARate = m_TargetRaRate;
            target.DECRate = m_TargetDecRate;
            target.time = m_TargetTime;
//            SetNonSidereal(ns);
            // {“DIFFERENTIAL”,2.399,1.0009,“RADEC”,2000.0,“J2000”,0,0,0,”U”,0,”U_B”,0.8,””,12345678.123,10.0,20.0}
            lbto::position2 targetfile;
            targetfile.type = m_TargetType;
            targetfile.coord1 = 2.399;
            targetfile.coord2 = 1.0089;
            targetfile.system = "RADEC";
            targetfile.epoch = 2000.0;
            targetfile.equinox = "J2000";
            targetfile.pmcoord1 = 0;
            targetfile.pmcoord2 = 0;
            targetfile.apparentMagnitude = 0;
            targetfile.filter = "u";
            targetfile.color = 0;
            targetfile.colorType = "U_B";
            targetfile.wavelength = 0.8;
            targetfile.file = "";
            targetfile.time = 12345678.123;
            targetfile.RARate = 10.0;
            targetfile.DECRate = 20.0;
        }
        if(false && m_TargetType == "FILE")
        {
            try
            {
                m_TargetFile = su::get_value_from_map<std::string>(_telSup, "TargetFile");
            }
            catch(const std::invalid_argument &e)
            {
                m_TargetFile = "";
                std::stringstream log_msg;
                log_msg << "Missing  key TargeFile";
                I_LLOG(log_msg.str());
            }
            target.file = m_TargetFile;
//            {“FILE”,2.399,1.0009,“RADEC”,2000.0,“J2000”,0,0,0,”U”,0,”U_B”,0.8,”ephemeris_file”,0,0,0}
            lbto::position2 targetfile;
            targetfile.type = m_TargetType;
            targetfile.coord1 = 2.399;
            targetfile.coord2 = 1.0089;
            targetfile.system = "RADEC";
            targetfile.epoch = 2000.0;
            targetfile.equinox = "J2000";
            targetfile.pmcoord1 = 0;
            targetfile.pmcoord2 = 0;
            targetfile.apparentMagnitude = 0;
            targetfile.filter = "u";
            targetfile.color = 0;
            targetfile.colorType = "U_B";
            targetfile.wavelength = 0.8;
            targetfile.file = "ephemeris_file";
            targetfile.time = 0;
            targetfile.RARate = 0;
            targetfile.DECRate = 0;
        }

        target.type = m_TargetType; // "SIDEREAL", "sidereal", "FILE", "DIFFERENTIAL", Planet Name es "SATURN"
        target.filter = "NONE";
        target.color = 0.0;
        target.colorType = "NONE";
        target.wavelength = 1.3;
        // target.file = "";     // same as file in lbto::nonsidereal
        // target.time = 0.0;    // Same as value0 in lbto::nonsidereal
        // target.RARate = 0.0;  // same as value3 in lbto::nonsidereal
        // target.DECRate = 0.0; // same as value4 in lbto.nonsidereal

        guide.type = "SIDEREAL"; // guide star cannot be non sidereal
        guide.filter = "NONE";
        guide.color = 0.0;
        guide.colorType = "NONE";
        guide.wavelength = 1.3;

        AOReference.type = "SIDEREAL";
        AOReference.filter = "NONE";
        AOReference.color = 0.0;
        AOReference.colorType = "NONE";
        AOReference.wavelength = 1.3;


        key = "TelescopeMode";
        LBTMode = su::get_value_from_map<std::string>(_telSup, key);
        try { AoMode = su::get_value_from_map<std::string>(_telSup, "AOMODE");} catch (const std::invalid_argument &e){}
        current_preset.telescope_mode = LBTMode;
        current_preset.ao_mode = AoMode;

        // Filling stars and refs vector
        if(settingLeftPreset)
        {
            m_leftStars.clear();
            m_leftRef.clear();
            m_leftStars.push_back(target);
//            m_leftStars.push_back(guide);
            m_leftRef.push_back(AOReference);
            m_leftTelescopeMode = LBTMode;
            m_leftAoMode = AoMode;
        }
        else if(settingRightPreset)
        {
            m_rightStars.clear();
            m_rightRef.clear();
            m_rightStars.push_back(target);
//            m_rightStars.push_back(guide);
            m_rightRef.push_back(AOReference);
            m_rightTelescopeMode = LBTMode;
            m_rightAoMode = AoMode;
        }
        else
        {
            std::stringstream log_msg;
            log_msg << "Error : Invalid value for mandatory key TelescopeSide";
            E_LLOG(log_msg.str());
            U8_LLOG("");
            throw(std::invalid_argument(log_msg.str()));
        }

        m_TcsPreset = current_preset;
        if(m_ReferenceSide == "left" || m_ReferenceSide == "both")
        {
            status = PresetTelescope(m_leftStars, m_leftRef, LBTMode, AoMode);
        }
        else if(m_ReferenceSide == "right")
        {
            status = PresetTelescope(m_rightStars, m_rightRef, LBTMode, AoMode);
        }
    }
    catch(...)
    {
        U8_LLOG(__FUNCTION__ << " : Error");
        throw;
    }
    U8_LLOG(__FUNCTION__);
    return status;
}


/* SetNonSidereal
 * input:
 * - ns : nonsidereal structure */
int SeqTcs::SetNonSidereal(const lbto::nonsidereal & ns)
{
    U6_LLOG(__FUNCTION__);
    Ice::Int status = EXIT_FAILURE;
    CheckConnection();
    if(m_iif_svc)
    {
        m_iif_svc->iifSetNonSidereal(ns, false);
    }
    U8_LLOG("");
    return status;
}

/* PresetTelescope
 * input:
 * - _stars : {target  coordinates, guide  coordinates}
 * - _refs  : {AO reference coordinates}
 * - _telescope mode (string): ADAPTIVETTM_TRACK, ADAPTIVEACE_TRACK, ADAPTIVEICE_TRACK
 * - _AO mode (string): "NORMAL", "REPOINT", "SKIPREF", "SKIPSTART", "SKIPSTARTREPOINT", "SKIPALL"
 * all expressed in radian and as either RA DEC or AZ ALT  */
int SeqTcs::PresetTelescope(const lbto::SeqPos2 &_stars, const lbto::SeqPos2 & _refs, const std::string &_telescopeMode, const std::string &_AoMode)
{
    U6_LLOG(__FUNCTION__);
    Ice::Int status = EXIT_SUCCESS;
    lbto::result res;
    double rotatorAngle = 0.0; // LBT rotator is not used
    std::string rotatorMode = "POSITION"; // "IDLE";
    double leftRotatorAngle = 0.0, rightRotatorAngle = 0.0;
//    static int switch_rotator_mode = 0;
//    switch_rotator_mode++;
//    if(switch_rotator_mode == 2)rotatorMode = "PARALLACTIC";
//    else if(switch_rotator_mode == 3)rotatorMode = "POSITION";

    std::string leftRotatorMode = rotatorMode, rightRotatorMode = rotatorMode;

    U9_LLOG("Rotator mode set to : " << rotatorMode);

    try
    {
        if(m_iif_svc && m_iif_svc->isReachable())
        {
            res = m_iif_svc->iifAuthorize2(m_ReferenceSide);
            if (res.rescode == EXIT_SUCCESS)
            {
                res = m_iif_svc->iifSetStars22(_stars, m_ReferenceSide);
            }
            else { E_LLOG("iifAuthorize");}

            if(res.rescode == EXIT_SUCCESS)
            {
                if(!_AoMode.empty() && _AoMode != "OFF")
                    res = m_iif_svc->iifSetReference22(_refs, m_ReferenceSide);
            }
            else { E_LLOG("iifSetStars2");}

            if(res.rescode == EXIT_SUCCESS)
            {
                if(m_BinocularFlag == "OFF" || (m_bSingleSideMode && m_BinocularFlag == "SYNCPRESET"))
                {
                    if(m_BinocularFlag == "SYNCPRESET")
                    {
                        res = m_iif_svc->iifBinocularControl(m_BinocularFlag);
                        LogResult(res, __LINE__);
                    }
                    m_BinocularRightPreset = m_BinocularLeftPreset = false;
                    res = m_iif_svc->iifPresetTelescope(rotatorAngle, rotatorMode, _telescopeMode, m_ReferenceSide);
                    if(res.rescode == EXIT_SUCCESS || res.rescode == EXIT_WARNING)
                    {
                        if(res.rescode == EXIT_WARNING)
                        {
                            LogResult(res, __LINE__);
                        }
                        if(!_AoMode.empty() && _AoMode != "OFF")
                        {
                            res = m_iif_svc->iifRunAO(_AoMode, m_ReferenceSide);
                            if(res.rescode != EXIT_SUCCESS)
                            {
                                E_LLOG("iifRunAO");
                            }
                        }
                    }
                    else
                    {
                        LogResult(res, __LINE__);
                    }
                }
                else if(m_BinocularFlag == "SYNCPRESET")
                {
                    if(m_BinocularRightPreset && m_BinocularLeftPreset)
                    {
                        m_BinocularRightPreset = m_BinocularLeftPreset = false;
                        res = m_iif_svc->iifSyncPresetTelescope(leftRotatorAngle, leftRotatorMode, m_leftTelescopeMode, rightRotatorAngle, rightRotatorMode, m_rightTelescopeMode);
                        if (res.rescode == EXIT_SUCCESS && (!_AoMode.empty() && _AoMode != "OFF"))
                        {
                            res = m_iif_svc->iifRunAO(_AoMode, m_ReferenceSide);
                            if(res.rescode != EXIT_SUCCESS)
                            {
                                E_LLOG("iifRunAO");
                            }
                        }
                        else { E_LLOG("iifSyncPresetTelescope");}
                    }
                }
            }
            else { E_LLOG("iifSetReference2");}

            LogResult(res, __LINE__);
        }
        else
        {
            if(m_iif_svc)
            {
                std::stringstream log_msg;
                log_msg << "Error : Not Reachable";
                E_LOG(log_msg.str());
                U8_LLOG("");
                throw(std::runtime_error(log_msg.str()));
            }
            else
            {
                std::stringstream log_msg;
                log_msg << "Error : Tcs Service not connected";
                E_LOG(log_msg.str());
                U8_LLOG("");
                throw(std::runtime_error(log_msg.str()));
            }
        }
    }
    catch(...)
    {
        m_BinocularRightPreset = m_BinocularLeftPreset = false;
        E_LLOG(__FUNCTION__);
        U8_LLOG("");
        throw;
    }
    
    status = res.rescode;
    U8_LLOG(__FUNCTION__);
    return status;
}

bool SeqTcs::CheckConnection(bool bThrow)
{
    if(m_iif_svc)
    {
        return true;
    }
    else if(bThrow)
    {
        std::stringstream log_msg;
        log_msg << "Error : Tcs Service not connected";
        E_LLOG(log_msg.str());
        throw(std::runtime_error(log_msg.str()));
    }
    else
    {
        return false;
    }
}


int SeqTcs::ValidateOffsetSetup(const std::map<std::string, std::string> & _telSetup)
{
    std::vector<std::string> allowed_keys;
    allowed_keys.push_back("BinocularFlag");
    allowed_keys.push_back("OFFSETX");
    allowed_keys.push_back("OFFSETY");
    allowed_keys.push_back("CoordinateSystem");
    allowed_keys.push_back("MoveType");
    allowed_keys.push_back("TelescopeSide");

    std::map<std::string, std::string>::const_iterator itCheck;
    for(itCheck = _telSetup.begin(); itCheck != _telSetup.end(); itCheck++)
    {
        if(std::find(allowed_keys.begin(), allowed_keys.end(), itCheck->first) == allowed_keys.end())
        {
            std::stringstream log_msg;
            log_msg << "Warning : key " << itCheck->first << " not expected will be ignored.";
            W_LLOG(log_msg.str());
        }
    }
    return EXIT_SUCCESS;
}

int SeqTcs::OffsetPointing(const std::map< ::std::string, ::std::string> &_tel_sup)
{
    static int call_counter = 0;
    U6_LLOG(__FUNCTION__ << suc::tilde << call_counter);
    Ice::Int status = ValidateOffsetSetup(_tel_sup);

    double telescopeRotAngle = 0;
    double offsetX = 0;
    double offsetY = 0;
    std::string coordSys;
    std::string moveType;
    //std::string rotOffsetUnit = "DEGREE";
    lbto::result res;

    std::string offsetSide;
    std::string binocularFlag;

    CheckConnection();

    try
    {
        offsetX = su::get_value_from_map<double>(_tel_sup, "OFFSETX"); // in radians if coordSys == "RADEC"
        offsetY = su::get_value_from_map<double>(_tel_sup, "OFFSETY"); // in radians if coordSys == "RADEC"
        coordSys = su::get_value_from_map<std::string>(_tel_sup, "CoordinateSystem");
        moveType = su::get_value_from_map<std::string>(_tel_sup, "MoveType");
        offsetSide = su::get_value_from_map<std::string>(_tel_sup, "TelescopeSide");

        binocularFlag = su::get_value_from_map<std::string>(_tel_sup, "BinocularFlag");
        if(m_BinocularLeftPreset || m_BinocularRightPreset)
        {
            std::stringstream log_msg;
            log_msg << "Error : Cannot start an offset when a Binocular preset is in progress. Preset operation will be cancelled";
            m_BinocularLeftOffset = m_BinocularRightOffset = false;
            m_BinocularLeftPreset = m_BinocularRightPreset = false;
            E_LLOG(log_msg.str());
            U8_LLOG("");
            throw(std::runtime_error(log_msg.str()));
        }
        if(binocularFlag == "OFF"){}
        else
        {
            if(binocularFlag == "SYNCOFFSET")
            {
                res = m_iif_svc->iifBinocularControl(binocularFlag);
                LogResult(res, __LINE__);
                if(offsetSide == "left" && m_BinocularLeftOffset == false)
                {
                    m_BinocularLeftOffset = true;
                    m_LeftOffsetX = offsetX;
                    m_LeftOffsetY = offsetY;
                    m_LeftCoordSys = coordSys;
                    m_LeftMoveType = moveType;
                    call_counter++;
                }
                else if(offsetSide == "right" && m_BinocularRightOffset == false)
                {
                    m_BinocularRightOffset = true;
                    m_RightOffsetX = offsetX;
                    m_RightOffsetY = offsetY;
                    m_RightCoordSys = coordSys;
                    m_RightMoveType = moveType;
                    call_counter++;
                }
                else if((offsetSide == "left" && m_BinocularLeftOffset) || (offsetSide == "right" && m_BinocularRightOffset))
                {
                    std::stringstream log_msg;
                    log_msg << "Error : " << offsetSide << " already received for Synchronous Binocular Offset. Synchronous operation will be cancelled.";
                    m_BinocularLeftOffset = m_BinocularRightOffset = false;
                    call_counter = 0;
                    E_LLOG(log_msg.str());
                    U8_LLOG("");
                    throw(std::runtime_error(log_msg.str()));
                }
                else if(offsetSide == "both")
                {
                    std::stringstream log_msg;
                    log_msg << "Error : TelescopeSide must be \"left\" or \"right\" in synchronous offset pointing";
                    E_LLOG(log_msg.str());
                    U8_LLOG("");
                    throw(std::runtime_error(log_msg.str()));
                }
            }
            else
            {
                std::stringstream log_msg;
                log_msg << "Error : Flag " << binocularFlag << " not allowed in offset telescope";
                E_LLOG(log_msg.str());
                U8_LLOG("");
                throw(std::runtime_error(log_msg.str()));
            }
        }

        if(offsetX*offsetX + offsetY*offsetY > (3.141592653*3.141592653/(180*180*4)))
        {
            std::stringstream log_msg;
            m_BinocularLeftOffset = m_BinocularRightOffset = false;
            log_msg << "Error : Not allowed an offset pointing greater than 1800 arcsec.";
            E_LLOG(log_msg.str());
            U8_LLOG("");
            throw(std::invalid_argument(log_msg.str()));
        }
        if(!m_bSingleSideMode && binocularFlag == "SYNCOFFSET")
        {
            if(m_BinocularLeftOffset && m_BinocularRightOffset)
            {
                m_BinocularLeftOffset = m_BinocularRightOffset = false;
                call_counter = 0;
                res = m_iif_svc->iifSyncOffsetPointing( m_LeftOffsetX,  m_LeftOffsetY,  m_LeftCoordSys,  m_LeftMoveType,
                                                       m_RightOffsetX, m_RightOffsetY, m_RightCoordSys, m_RightMoveType);
            }
        }
        else
        {
            m_BinocularLeftOffset = m_BinocularRightOffset = false;
            res = m_iif_svc->iifOffsetPointing2(telescopeRotAngle, offsetX, offsetY, coordSys, moveType, offsetSide);
        }
        status = res.rescode;
        LogResult(res, __LINE__);
    }
    catch(const Basda::ServiceIsBusyException &e)
    {
        m_BinocularLeftOffset = m_BinocularRightOffset = false;
        U8_LLOG("");
        E_THROW(Basda::ServiceIsBusyException, e.toString());
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
        m_BinocularLeftOffset = m_BinocularRightOffset = false;
        U8_LLOG("");
        throw;
    }

    U8_LLOG("");
    return status;
}


/* RunAO
 *
 * input
 * - _mode: can be "NORMAL", "REPOINT", "SKIPREF", "SKIPSTART", "SKIPSTARTREPOINT", "SKIPALL"  */

int SeqTcs::RunAO(const std::map< ::std::string, ::std::string> &_telSup)
{
    U6_LLOG(__FUNCTION__);
    // Ice::LoggerPtr logger = _c.adapter->getCommunicator()->getLogger();
    Ice::Int status = EXIT_SUCCESS;
    lbto::result res;
    std::string strAOMode = "";
    std::string key = "", errorMsg = "";

    key = "AOMode";
    strAOMode = su::get_value_from_map<std::string>(_telSup, key);

    CheckConnection();

    res = m_iif_svc->iifRunAO(strAOMode, m_ReferenceSide);

    if(res.rescode != EXIT_SUCCESS)
    {
        errorMsg = "Unable to run AO";
        A_LLOG(errorMsg << " with messages:");
        E_LLOG(errorMsg + " with messages:");
        for(unsigned int i = 0; i < res.resmsg.size(); i++)
        {
            E_LLOG(res.resmsg[i]);
        }
        std::runtime_error err(errorMsg);
        U8_LLOG("");
        throw err;
    }
    status = res.rescode;
    U8_LLOG(__FUNCTION__);
    return status;
}


int SeqTcs::SendWavefront(const lbto::SeqWF &_zernikeC, const std::string &_ope)
{
    U6_LLOG(__FUNCTION__);
    int result = EXIT_SUCCESS;
    lbto::result res;
    CheckConnection();
    res = m_iif_svc->iifSendWavefront(_zernikeC, _ope, m_ReferenceSide);
    if(res.rescode != EXIT_SUCCESS)
    {
        for(unsigned int i = 0; i < res.resmsg.size(); i++)
        {
            E_LLOG(res.resmsg[i]);
        }
        result = res.rescode;
    }
    U8_LLOG("");
    return result;
}


int SeqTcs::MoveFocus(double _absPos, const std::string &_ope)
{
    U6_LLOG(__FUNCTION__);
    int result = EXIT_SUCCESS;
    lbto::result res;
    CheckConnection();
    res = m_iif_svc->iifMoveFocus(_absPos, _ope, m_ReferenceSide);
    if(res.rescode != EXIT_SUCCESS)
    {
        for(unsigned int i = 0; i < res.resmsg.size(); i++)
        {
            E_LLOG(res.resmsg[i]);
        }
        result = res.rescode;
    }
    U8_LLOG("");
    return EXIT_SUCCESS;
}

int SeqTcs::OffsetZAO(double deltaZ, const std::string & side)
{
    U6_LLOG(__FUNCTION__);
    int result = EXIT_SUCCESS;
    lbto::result res;
    CheckConnection();
    try
    {
        res = m_iif_svc->iifOffsetZAO(deltaZ, side);
        LogResult(res, __LINE__);
    }
    catch(const std::runtime_error &e)
    {
        E_LLOG(e.what());
        U8_LLOG("");
        throw(e);
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
    }

    if(res.rescode != EXIT_SUCCESS)
    {
        for (unsigned int i = 0; i < res.resmsg.size(); i++)
        {
            E_LLOG(res.resmsg[i]);
        }
        result = res.rescode;
    }
    U8_LLOG("");
    return EXIT_SUCCESS;
}

int SeqTcs::OffsetXYAO(double DeltaX, double DeltaY, const std::string & side)
{
    U6_LLOG(__FUNCTION__ << " ~ [" << DeltaX << ", " << DeltaY << "] ~ " << side);
    int result = EXIT_SUCCESS;
    lbto::result res;
    CheckConnection();
    try
    {
        res = m_iif_svc->iifOffsetXYAO(DeltaX, DeltaY, side);
        LogResult(res, __LINE__);
    }
    catch(const std::runtime_error &e)
    {
        E_LLOG(e.what());
        U8_LLOG("");
        throw(e);
    }
    catch(...)
    {
        E_LLOG(__FUNCTION__);
    }

    U8_LLOG("");
    return EXIT_SUCCESS;
}

int SeqTcs::ApplyWaffleAO(const std::map< ::std::string, ::std::string> &_telSup)
{
    U6_LLOG(__FUNCTION__);
    // CorrectModesAO will be changed with ApplyWaffleAO with the same signature
    // The vector of modes to obtain a waffle is constant (TBC). What changes is
    // probably the amplitude
    //
    // how many modes?
    // modes to produce waffle have to be mirror modes (since this is the kind of input
    // expected by the IIF method). Someone has to give the conversion between modes
    // expressed in some base (Zernike?) to mirror modes.
    std::string strAOMode;
    std::string key;
    lbto::result res;
    lbto::SeqModes modes(672, 0); // 672 max number of mirror modes, initialized to 0

    key = "AOMode";
    strAOMode = su::get_value_from_map<std::string>(_telSup, key);

    CheckConnection();

    res = m_iif_svc->iifStopAO("Need to correct AO modes for waffle", m_ReferenceSide); // not necessary using the new ApplyWaffleAO
    if (res.rescode == EXIT_SUCCESS)
    {
        res = m_iif_svc->iifCorrectModesAO(modes, m_ReferenceSide);
        if (res.rescode == EXIT_SUCCESS)
            res = m_iif_svc->iifRunAO(strAOMode, m_ReferenceSide);
    }

    U8_LLOG("");
    return res.rescode;
}


/* pointingCorrectionStart()
 * runs in separate thread for the duration of observation.
 * reads periodically position of target on teccam, if out of threshold
 * sends offset to telescope accordingly. Implementation from
 * SHARKNIR_gen_tec_PointingCorrection template
 */
int SeqTcs::pointingCorrectionStart(Ice::Float _freq, Ice::Int &_thrdID)
{
    U6_LLOG(__FUNCTION__);
    /*
     - check RTC is init, RTC should be init outside thid method
     - configure teccam form the task: NOTE: since has to run continuosly, this
     has not to clash with RTCTT teccam configuration
     - expose teccam
     - get exposure
     - calculate reference position

     in thread
     - loop with frequency = _freq
     - expose teccam
     - get exposure
     - calc target barycenter
     - calculate displacement wrt reference
     - converrt displacement to tiptilt signal for DM
     - upload TT signale to DM

     store thread id in out variable
     */
    W_LLOG("Provide implementation for pointingCorrectionStart");
    Ice::Int status = EXIT_SUCCESS;
    bool rtcInit;
    // std::string filename = "";
    std::map<std::string, std::string> dictTTSetup;
    Nice::SeqInt checkLoop;
    // ----------------------------------------------------------
//    status = RTCTTIsInit(rtcInit, _c);
    if(status == EXIT_SUCCESS)
    {
        if(!(rtcInit))
        {
            U8_LLOG("");
            throw std::runtime_error("RTC is not initialized");
        }
        dictTTSetup["TTFRAMERATE"] = ""; // default values for pointing correction
        dictTTSetup["TTWINCOORDY"] = ""; // default values for pointing correction
        dictTTSetup["TTWINROWS"] = ""; // default values for pointing correction
        dictTTSetup["TTWINCOORDX"] = ""; // default values for pointing correction
        dictTTSetup["TTWINCOLS"] = ""; // default values for pointing correction
        dictTTSetup["TTCAMTINT"] = ""; // default values for pointing correction
        // check if loop is running
//        checkLoop = m_tiptilt->check_loop();
        if(checkLoop[0])
        {
            U8_LLOG("");
            throw std::runtime_error("real-time tiptilt loop running");
        }
//        status = RTCTTConfigCamera(dictTTSetup, _c);
        // modify dictTTSetup according to following method call
//        status = RTCTTTeccamExpose(dictTTSetup, filename, _c);
        // reference position corresponds to PSF barycenter. Can be computed offline
        // from acquire image on TECCAM or retrieved from RTC diagnostic as
        // CentroidOut
        // in the first case
        // modify dictTTSetup according to following method call
//        status = RTCTTTeccamGetImage(dictTTSetup, filename, _c);
        // then compute barycenter on image (with external python script)
        // in the second case nothing else has to be done
        //
    }
    else
    {
        E_LLOG("RTCTTIsInit returned EXIT_FAILURE");
    }

    U8_LLOG("");
    return status;
}


/* pointingCorrectionStop()
 * stops thread running pointing Correction, reference to thread must be provided by pointingCorrectionStart
 */
int SeqTcs::pointingCorrectionStop(Ice::Int _thrdID)
{
    U6_LLOG(__FUNCTION__);
    Ice::Int status = EXIT_FAILURE;
    U8_LLOG("");
    throw(std::runtime_error("Function Under Development"));
    return status;
}

