// $Header$

#include "AdcWorkerThread.h"

AdcWorkerThread::AdcWorkerThread(const Basda::MoccaWheelServiceWorkerInterfacePtr & _adc1,
                                 const Basda::MoccaWheelServiceWorkerInterfacePtr & _adc2,
                                 bool (* adccallback)(double & pressure, double & temperature, double & elevation, double & drot_pos),
                                 Ice::PropertiesPtr props)
:
m_props(props),
m_adc1(_adc1),
m_adc2(_adc2)
{
    m_T0 = 273.14;
    m_CO = 0.503;
    m_P0 = 733.;

    m_coRotation = 0;
    m_counterRotation = 0;

    m_P0 = 0;
    m_T0 = 0;
    m_P1 = 0;
    m_T1 = 0;
    m_Adc1Ref = 0.;
    m_Adc2Ref = 0.;
    m_DrotRef = 0.;
    m_bTest0 = m_bTest1 = m_bTest2 = m_bTest3 = false;

    su::getValue<double>(props->getProperty("Shins.Tracking.Adc.P0"), m_P0);
    su::getValue<double>(props->getProperty("Shins.Tracking.Adc.T0"), m_T0);
    su::getValue<double>(props->getProperty("Shins.Tracking.Adc.P1"), m_P1);
    su::getValue<double>(props->getProperty("Shins.Tracking.Adc.T1"), m_T1);
    su::getValue<double>(props->getProperty("Shins.Tracking.Drot.RefAngle"), m_DrotRef);
    su::getValue<double>(props->getProperty("Shins.Tracking.Adc.Prism1RefAngle"), m_Adc1Ref);
    su::getValue<double>(props->getProperty("Shins.Tracking.Adc.Prism2RefAngle"), m_Adc2Ref);

    U9_LLOG(m_P1 << suc::tilde << m_T1 << suc::tilde << m_DrotRef << suc::tilde << m_Adc1Ref << suc::tilde << m_Adc2Ref << suc::tilde);

    m_UpdateInterval = 20;
    m_bSetPosition = false;
    SetMode("OFF");
    m_EndTask = false;
    m_bTaskEnded = false;

    m_adcCallback = adccallback;

    pthread_t tid1;
    pthread_create(&tid1, NULL, run, this);
}

AdcWorkerThread::~AdcWorkerThread()
{
    m_EndTask = true;
    m_StateMachine.setNext(Basda::SHUTDOWN);
    while(!m_bTaskEnded)
    {
        usleep(50000);
    }
}

void AdcWorkerThread::SetCorrectionParameters(double co, double p0, double p1, double t0, double t1)
{
    U6_LOG(__FUNCTION__ << " ~ CO -> " << co << "; P0 -> " << p0 << "; P1 -> " << p1 << "; T0 -> " << t0 << "; T1 -> " << t1);
    m_CO = co;
    m_P0 = p0;
    m_P1 = p1;
    m_T0 = t0;
    m_T1 = t1;
}

double AdcWorkerThread::getAdc(double lbt_pressure, double lbt_temp, double elevation)
{
    // See SHARK-NIR-INAFP-SDA-001 System Design Analysis section 17.1
    double CO = 0.503;              // Constant Offset;
    double P0 = 733.;               // Pressure Offset [mbar] (default=733)
    double P1 = 0.00063;            // Pressure slope [arcsec/mbar] (for complete SHARK-NIR wavelength range P1 = 0.00063)
    double T0 = 273.14;             // Temperature Offset [K] (default 273.14);
    double T1 = -0.0018;            // Temperature Slope [arcse/K] (for complete SHARK-NIR wavelength range T1 = -0.0018)
    double ZA, P, T;                // Zenital Angle [rad], Pressure [mbar], Temperature [C]
    const double OneAtm = 101325.;
    const double nmbar = 1013.25;

    double altitude = 0;

    double Degree = M_PI/180.;
    double phi = 0.;
    altitude = (elevation/3600)*Degree;
    ZA = 90*Degree - altitude; // getAlt returns radians
    if(elevation >= 324000)ZA = 0;
    T = lbt_temp; // m_iif->getDouble("LBTTemp", "env.weather.lbt.temperature");// temperature in C
    P = 1000*100*(lbt_pressure/OneAtm); //m_iif->getDouble("LBTPressure", "env.weather.lbt.pressure")/OneAtm; // pressure in hPa

    int phi_selector = 2;
    if(m_bTest0 == 0)
    {
        if(phi_selector == 1)
        {
            // phi formula from SDA
        //    double P2 = 1000*100*m_iif->getDouble("SMTPressure", "env.weather.smt.pressure")/OneAtm; // pressure in hPa
        //    double T2 = m_iif->getDouble("SMTTemp", "env.weather.smt.temperature");// temperature in C
        //    U7_LLOG(P << " ~ " << T << " ~ " << altitude << " ~ " <<  P2 << " ~ " << T2 );
            if(ZA > 0. && ZA < 90*Degree)
            {
        //        phi = std::acos((m_CO + m_P1*(P - m_P0) + m_T1*(T - m_T0))*std::tan(ZA));
                phi = std::acos((m_CO + m_P1*(P - m_P0) + m_T1*T)*std::tan(ZA));
            }
        }
        else if(phi_selector == 2)
        {
            // Phi formula from Davide's document
            double ke = 1.;
            const double ref_tan = std::tan(65*Degree);
            P = lbt_pressure; // lbt_pressure is in hPa == mbar
            if(ZA >= 0. && ZA < 90*Degree)
            {
                double val = (ke*std::tan(ZA)/ref_tan)*(1 - m_P1*(m_P0 - P))*(1 -m_T1*(T + T0 - m_T0));
                if(val >= -1 && val <= 1)
                {
                    phi = std::acos(val);
                }
            }
//            U7_LLOG("m_P1 = " << m_P1 << "; m_P0 = " << m_P0 << "; m_T1 : " << m_T1 << "; T0 = " << T0 << "; m_T0 : " << m_T0);
//            U7_LLOG("lbt_pressure = " << lbt_pressure << "; lbt_temp = " << lbt_temp << "; elevation : " << elevation << "; ZA = " << ZA << ";P : " << P << "; T = " << T << "; phi = " << phi);
        }
    }
//    U7_LLOG("lbt_pressure = " << lbt_pressure << "; lbt_temp = " << lbt_temp << "; elevation : " << elevation << "; ZA = " << ZA << ";P : " << P << "; T = " << T);
//    U7_LLOG("m_CO = " << m_CO << "; m_P1 = " << m_P1 << "; m_P0 = " << m_P0 << "; m_T1 = " << m_T1 << "; m_T0 = " << m_T0 << "; Phi = " << phi*180/3.14159265358979);
    return phi/Degree; //  return value in DEGREE
}

void * AdcWorkerThread::run(void * pContext)
{
    AdcWorkerThread & Me = *((AdcWorkerThread *)pContext);
    Me.WorkerThread();
    return NULL;
}


// void AdcWorkerThread::moveAbsoluteWait(Nice::Time _timeout)
// {
//     bool wait = true;
//     int microseconds = _timeout.seconds()*1000000 + _timeout.milliseconds()*1000;
//     int elapsed = 0;
//     if(wait)
//     {
//         while(!m_bSetPosition &&  wait)
//         {
//             usleep(50000);
//             elapsed += 50000;
//             wait = elapsed < microseconds;
//         }
//     }
//     if(!wait)
//     {
//         W_LOG("moveAbsolute ADC wait timed out");
//     }
// }

// void AdcWorkerThread::moveAbsoluteStart(double pos1, double pos2, std::string _unit)
// {
//     if(m_StateMachine.now() == Basda::STANDBY)
//     {
//         if(m_bSetPosition)
//         {
//             std::stringstream log_msg;
//             log_msg << "Error : ADC is busy ";
//             E_LOG(log_msg.str());
//             throw(std::runtime_error(log_msg.str()));
//         }
//         m_TargetPos1 = pos1;
//         m_TargetPos2 = pos2;
//         m_TargetUnit = _unit;
//         m_bSetPosition = true;
//     }
//     else
//     {
//         std::stringstream log_msg;
//         std::string state;
//         switch(m_StateMachine.now())
//         {
//             case Basda::ONLINE :
//                 state = "ONLINE";
//                 break;
//             case Basda::WORKING :
//                 state = "WORKING";
//                 break;
//             case Basda::SHUTDOWN :
//                 state = "SHUTDOWN";
//                 break;
//             case Basda::OFFLINE :
//                 state = "OFFLINE";
//                 break;
//             case Basda::ERROR :
//                 state = "ERROR";
//                 break;
//             default :
//                 {
//                     std::stringstream now;
//                     now << m_StateMachine.now();
//                     state = now.str();
//                 }
//                 break;
//         }
//         log_msg << "Error : cannot accept commands when in state " << state;
//         E_LOG(log_msg.str());
//         throw(std::runtime_error(log_msg.str()));
//     }
// }

std::string AdcWorkerThread::GetMode()
{
    Basda::ServiceState state = m_StateMachine.now();
    if(state == Basda::ONLINE)
        return std::string("ON");
    else if(state == Basda::STANDBY)
        return std::string("OFF");
}

void AdcWorkerThread::SetMode(std::string mode)
{
    if(mode == "ON")
    {
        m_StateMachine.setNext(Basda::ONLINE);
    }
    else if(mode == "OFF")
    {
        m_StateMachine.setNext(Basda::STANDBY);
    }
    else
    {
        std::stringstream log_msg;
        log_msg << "Error : MODE must be one of { \"ON\", \"OFF\"}";
        E_LLOG(log_msg.str());
        throw(std::invalid_argument(log_msg.str()));
    }
    U9_LLOG("AdcWorkerThread ~ Mode set to " << mode);
}


void AdcWorkerThread::WorkerThread()
{
    U6_LOG(__FUNCTION__);
    double za = 0.0;
    double cosUpdate;
    double update;
    int counter = 0;
    int cycle_interval = 200000;
    int cycle_rep = 20;
    Nice::Date lastTime = Nice::Date::now();
    while(m_StateMachine.now() != Basda::SHUTDOWN)
    {
        if(m_StateMachine.now() != m_StateMachine.next())
        {
            m_StateMachine.setNow(m_StateMachine.next());
        }
        switch(m_StateMachine.now())
        {
        case Basda::STANDBY : // PUPIL MODE -> Accepts commands
            {
                if(m_bSetPosition)
                {
                    try
                    {
                        U6_LOG(__FUNCTION__ << " ~ Moving to " << m_TargetUnit);
                        m_adc1->moveAbsoluteStart(m_TargetPos1, m_TargetUnit);
                        m_adc2->moveAbsolute(m_TargetPos2, m_TargetUnit);
                        m_adc1->moveAbsoluteWait(Nice::Time::seconds(1));
                    }
                    catch(...)
                    {
                        E_LOG(__FUNCTION__);
                        m_bSetPosition = false;
                    }

                    m_bSetPosition = false;
                }
            }
            break;
        case Basda::ONLINE : // FIELD MODE -> Automatically Follows telescope
            {
                Nice::Date currentTime = Nice::Date::now();
                int currentSeconds = currentTime.hour()*3600 + currentTime.minute()*60 + currentTime.second();
                Nice::Time interval = currentTime - lastTime + Nice::Time::milliSeconds(cycle_interval/1000);
                if(((currentSeconds + 2) % m_UpdateInterval) == 0 && currentTime.microSecond() > 700000)// interval > Nice::Time::seconds(m_UpdateInterval))
                {
                    lastTime = currentTime;
                    double pressure = 0., temperature = 0., elevation = 0., drot_pos = 0.;
                    bool callback_result = m_adcCallback(pressure, temperature, elevation, drot_pos);
                    if(callback_result)
                    {
                        update = getAdc(pressure, temperature, elevation);
                        try
                        {
                            if(m_bTest2)update = 0.;
                            if(m_bTest3)update = 90.;
                            double ctf = 1.0;
//                            U7_LLOG("m_counterRotation = " << m_counterRotation << "; m_coRotation = " << m_coRotation << "; m_Adc1Ref : " << m_Adc1Ref << "; m_Adc2Ref = " << m_Adc2Ref);
//                            U7_LLOG("m_DrotRef : " << m_DrotRef << "; drot_pos = " << drot_pos);
                            float adc1_pos =  update + m_counterRotation + m_coRotation + m_Adc1Ref + ctf*(drot_pos - m_DrotRef);
                            float adc2_pos = -update - m_counterRotation + m_coRotation + m_Adc2Ref + ctf*(drot_pos - m_DrotRef);
                            int adc1_pos_steps = (int)((adc1_pos/360)*7372800);
                            int adc2_pos_steps = (int)((adc2_pos/360)*7372800);
                            m_adc1->moveAbsoluteStart(adc1_pos_steps, "STEPS");
                            m_adc2->moveAbsolute(adc2_pos_steps, "STEPS");
                            m_adc1->moveAbsoluteWait(Nice::Time::seconds(60));
                            U9_LLOG("(ADC1_PosSteps, ADC2_PosSteps) : (" << (int)((adc1_pos/360)*7372800) % 7372800 << ", " << (int)((adc2_pos/360)*7372800) % 7372800 << ")");
                        }
                        catch(const std::exception & e)
                        {
                            E_LOG(e.what());
                        }
                        catch(...)
                        {
                            E_LOG(__FUNCTION__);
                        }
                    }
                }
            }
            break;
        case Basda::OFFLINE : // When connection to one of the adcs is impossible
            break;
        case Basda::WORKING :
            break;
        case Basda::ERROR :
//            m_adc1->changeState(Basda::STANDBY);
//            m_adc2->changeState(Basda::STANDBY);
            break;
        case Basda::SHUTDOWN :
            m_EndTask = true;
            break;
        default :
            break;
        }
        usleep(cycle_interval);
        counter++;
    }
    m_bTaskEnded = true;
    U8_LOG(__FUNCTION__);
}
// #include <cmath>
void AdcWorkerThread::setAdcCounterRotationOffset(double counterrotation)
{
    m_counterRotation = std::fmod(counterrotation, 360);//2*M_PI);
}

void AdcWorkerThread::setAdcCoRotationOffset(double corotation)
{
    m_coRotation = std::fmod(corotation, 360);//2*M_PI);
}

void AdcWorkerThread::setFlags(uint64_t flags, uint64_t more_flags)
{
    if(flags & shins::flags::TRACK_ADC_TEST0)
    {
        m_bTest0 = true;
        U9_LLOG("Test 0 : ON");
    }
    if(flags & shins::flags::TRACK_ADC_TEST1)
    {
        m_bTest1 = true;
        U9_LLOG("Test 1 : ON");
    }
    if(flags & shins::flags::TRACK_ADC_TEST2)
    {
        m_bTest2 = true;
        U9_LLOG("Test 2 : ON");
    }
    if(flags & shins::flags::TRACK_ADC_TEST3)
    {
        m_bTest3 = true;
        U9_LLOG("Test 3 : ON");
    }
};
void AdcWorkerThread::unsetFlags(uint64_t flags, uint64_t more_flags)
{
    if(flags & shins::flags::TRACK_ADC_TEST0)
    {
        m_bTest0 = false;
        U9_LLOG("Test 0 : OFF");
    }
    if(flags & shins::flags::TRACK_ADC_TEST1)
    {
        m_bTest1 = false;
        U9_LLOG("Test 1 : OFF");
    }
    if(flags & shins::flags::TRACK_ADC_TEST2)
    {
        m_bTest2 = false;
        U9_LLOG("Test 2 : OFF");
    }
    if(flags & shins::flags::TRACK_ADC_TEST3)
    {
        m_bTest3 = false;
        U9_LLOG("Test 3 : OFF");
    }
};
void AdcWorkerThread::toggleFlags(uint64_t flags, uint64_t more_flags)
{
    if(flags & shins::flags::TRACK_ADC_TEST0)
    {
        m_bTest0 = !m_bTest0;
        U9_LLOG("Test 0 : " << (m_bTest0 ? "ON" : "OFF"));
    }
    if(flags & shins::flags::TRACK_ADC_TEST1)
    {
        m_bTest1 = !m_bTest1;
        U9_LLOG("Test 1 : " << (m_bTest1 ? "ON" : "OFF"));
    }
    if(flags & shins::flags::TRACK_ADC_TEST2)
    {
        m_bTest2 = !m_bTest2;
        U9_LLOG("Test 2 : " << (m_bTest2 ? "ON" : "OFF"));
    }
    if(flags & shins::flags::TRACK_ADC_TEST3)
    {
        m_bTest3 = !m_bTest3;
        U9_LLOG("Test 3 : " << (m_bTest3 ? "ON" : "OFF"));
    }
};
