#include "AntennaSimulator.h"
//#include "AntennaHistory.h"
#include "QDebug"


#define FIRST_BAR_INDEX   1

const float DEGS_TO_RADS=1;

static float Tag_To_Time(AntennaSimulator::timetag_t t)
{
    return t*1.0E-6;
}

AntennaSimulator::AntennaSimulator(QObject *parent) : QObject(parent)
{
    m_simStatus = simIdle;
    memset(&m_antStatus,0,sizeof(m_antStatus));
}

AntennaSimulator::~AntennaSimulator()
{
}

void AntennaSimulator::initialize(const antennaCfg_t& antCfg)
{
    m_antStatus.updateTime  = 0;
    m_antStatus.scanStatus  = idle;
    m_antStatus.barInProg   = false;
    m_antStatus.frameInProg = false;
    m_antStatus.barNumber   = 0;
    m_antStatus.azimuth     = 0.0F;
    m_antStatus.elevation   = 0.0F;

    //m_antCfg.numBars         = NUMBER_OF_BARS_T_MAX;
    m_antCfg.numBars         = antCfg.nbars;
    m_antCfg.scanRate        = antCfg.scanRate * (DEGS_TO_RADS);
    m_antCfg.scanWidth       = antCfg.scanWidth * (DEGS_TO_RADS);
    if (m_antCfg.numBars == _2bars)
    {
        m_antCfg.barElevation[0] =  (antCfg.elCoverage / 2.0) * (DEGS_TO_RADS);
        m_antCfg.barElevation[1] = -(antCfg.elCoverage / 2.0) * (DEGS_TO_RADS);
    }
    else
    {
        m_antCfg.barElevation[0] =  (antCfg.elCoverage / 2.0) * (DEGS_TO_RADS);
        m_antCfg.barElevation[1] = -(antCfg.elCoverage / 4.0) * (DEGS_TO_RADS);
        m_antCfg.barElevation[2] = -(antCfg.elCoverage / 2.0) * (DEGS_TO_RADS);
        m_antCfg.barElevation[3] =  (antCfg.elCoverage / 4.0) * (DEGS_TO_RADS);
    }
}

void AntennaSimulator::execute(timetag_t ttag)
{
    switch(m_antStatus.scanStatus)
    {
    case idle:
        m_antStatus.updateTime  = ttag;
        m_antStatus.barInProg   = true;
        m_antStatus.frameInProg = true;
        m_antStatus.barNumber   = FIRST_BAR_INDEX;
        m_antStatus.azimuth     = +(m_antCfg.scanWidth / 2);
        m_antStatus.elevation   = m_antCfg.barElevation[m_antStatus.barNumber-1];
        m_antStatus.scanStatus  = scanBar1;
        break;

    case scanBar1:
        m_antStatus.azimuth -= calcDeltaAz(ttag);
        if (m_antStatus.azimuth <= (-(m_antCfg.scanWidth / 2)))
        {
            m_antStatus.azimuth    = -(m_antCfg.scanWidth / 2);
            m_antStatus.barInProg  = false;
            m_antStatus.scanStatus = positioningToBar2;
        }
        m_antStatus.updateTime  = ttag;
        break;

    case scanBar2:
        m_antStatus.azimuth += calcDeltaAz(ttag);
        if (m_antStatus.azimuth >= (+(m_antCfg.scanWidth / 2)))
        {
            m_antStatus.azimuth    = +(m_antCfg.scanWidth / 2);
            m_antStatus.barInProg  = false;
            if (m_antCfg.numBars == _2bars)
            {
                m_antStatus.scanStatus = positioningToBar1;
                m_antStatus.frameInProg = false;
            }
            else
                m_antStatus.scanStatus = positioningToBar3;
        }
        m_antStatus.updateTime  = ttag;
        break;

    case scanBar3:
        m_antStatus.azimuth -= calcDeltaAz(ttag);
        if (m_antStatus.azimuth <= (-(m_antCfg.scanWidth / 2)))
        {
            m_antStatus.azimuth    = -(m_antCfg.scanWidth / 2);
            m_antStatus.barInProg  = false;
            m_antStatus.scanStatus = positioningToBar4;
        }
        m_antStatus.updateTime  = ttag;
        break;

    case scanBar4:
        m_antStatus.azimuth += calcDeltaAz(ttag);
        if (m_antStatus.azimuth >= (+(m_antCfg.scanWidth / 2)))
        {
            m_antStatus.azimuth     = +(m_antCfg.scanWidth / 2);
            m_antStatus.barInProg   = false;
            m_antStatus.frameInProg = false;
            m_antStatus.scanStatus  = positioningToBar1;
        }
        m_antStatus.updateTime  = ttag;
        break;

    case positioningToBar1:
        m_antStatus.elevation += calcDeltaEl(ttag);
        if (m_antStatus.elevation >= m_antCfg.barElevation[0])
        {
            m_antStatus.elevation   = m_antCfg.barElevation[0];
            m_antStatus.barInProg   = true;
            m_antStatus.frameInProg = true;
            m_antStatus.scanStatus  = scanBar1;
            m_antStatus.barNumber   = FIRST_BAR_INDEX;
        }
        m_antStatus.updateTime  = ttag;
        break;

    case positioningToBar2:
        m_antStatus.elevation -= calcDeltaEl(ttag);
        if (m_antStatus.elevation <= m_antCfg.barElevation[1])
        {
            m_antStatus.elevation  = m_antCfg.barElevation[1];
            m_antStatus.barInProg  = true;
            m_antStatus.scanStatus = scanBar2;
            m_antStatus.barNumber  = FIRST_BAR_INDEX+1;
        }
        m_antStatus.updateTime  = ttag;
        break;

    case positioningToBar3:
        m_antStatus.elevation -= calcDeltaEl(ttag);
        if (m_antStatus.elevation <= m_antCfg.barElevation[2])
        {
            m_antStatus.elevation  = m_antCfg.barElevation[2];
            m_antStatus.barInProg  = true;
            m_antStatus.scanStatus = scanBar3;
            m_antStatus.barNumber  = FIRST_BAR_INDEX+2;
        }
        m_antStatus.updateTime  = ttag;
        break;

    case positioningToBar4:
        m_antStatus.elevation += calcDeltaEl(ttag);
        if (m_antStatus.elevation >= m_antCfg.barElevation[3])
        {
            m_antStatus.elevation  = m_antCfg.barElevation[3];
            m_antStatus.barInProg  = true;
            m_antStatus.scanStatus = scanBar4;
            m_antStatus.barNumber  = FIRST_BAR_INDEX+3;
        }
        m_antStatus.updateTime  = ttag;
        break;

    default:
        qDebug("__FUNCTION__: invalid scan status");
    }
}

AntennaSimulator::radians_t AntennaSimulator::calcDeltaAz(timetag_t ttag)
{
    timetag_t deltaTime;

    deltaTime = ttag - m_antStatus.updateTime;
    return (m_antCfg.scanRate * Tag_To_Time(deltaTime));
}

AntennaSimulator::radians_t AntennaSimulator::calcDeltaEl(timetag_t ttag)
{
    // assuming rate on elevation is the same as rate on azimuth, otherwise change following code
    return calcDeltaAz(ttag);
}

void AntennaSimulator::getStatus(antennaStatus_t& antStatus)
{
    antStatus = m_antStatus;
}

void AntennaSimulator::setSimStatus(simStatus_t status)
{
    m_simStatus = status;
}

AntennaSimulator::simStatus_t& AntennaSimulator::getSimStatus()
{
    return m_simStatus;
}

void AntennaSimulator::setElevation(radians_t elevation)
{
    for (int i=0; i<4; i++)
        m_antCfg.barElevation[i] += elevation;
}

AntennaSimulator::antennaParam_t& AntennaSimulator::getAntCfg()
{
    return m_antCfg;
}
