#include "adcqueue.h"

ADCQueue::ADCQueue(QObject *parent)
    : QObject{parent},
      _cmdItems(new QQueue<ADCQueue_ns::cmdItem>),
      _state(ADCQueue_ns::queueState::stopped),
      _currentCmd(0),
      _timer(new QTimer()),
      _repeatCount(0)
{
    _timer->setInterval(QUEUE_TIMEOUT);

    QObject::connect(_timer, &QTimer::timeout, [=]{
       if (_currentItem.waiting == true) {
           _timer->stop();
           _repeatCount++;
           _repeatCurrent();
       }
    });
}

ADCQueue::~ADCQueue()
{
    #ifdef STP_DEBUG
        qDebug() << "queue desctructor";
    #endif
}

bool ADCQueue::clear()
{
    if (_state !=ADCQueue_ns::queueState::stopped) return false;

    _cmdItems->clear();
    _currentCmd = 0;

    return true;
}

void ADCQueue::appendCmd(ADCQueue_ns::cmdItem cmd)
{
    _currentCmd++;
    cmd.id = _currentCmd;
    _cmdItems->enqueue(cmd);

    if (_state == ADCQueue_ns::queueState::stopped) start();
}

void ADCQueue::appendCmd(stpProtocol_ns::stpCommand cmd, uint16_t regNumber, QByteArray * regData)
{
    ADCQueue_ns::cmdItem __cmd;
    __cmd.cmd = cmd;
    __cmd.regNumber = regNumber;
    __cmd.regData = regData;
    appendCmd(__cmd);
}

bool ADCQueue::start()
{
    if (_state !=ADCQueue_ns::queueState::stopped) return false;
    if (_cmdItems->isEmpty()) return false;

    _startNext();

    return true;
}

bool ADCQueue::stop()
{
    if (_state == ADCQueue_ns::queueState::stopped) return false;

    _state = ADCQueue_ns::queueState::stopped;

    clear();

    return true;
}

bool ADCQueue::receiveData(QByteArray data)
{

    if (_headerCheck(&data)) {
        QByteArray * __header = new QByteArray(data.first(CONST_REGISTER_HEADER_SIZE));

        _currentItem.waiting = false;
        _timer->stop();

        switch ((*__header)[0]) {
            case stpProtocol_ns::stpCommand::REG_READ_ACK:
                _currentItem.regData = new QByteArray(data.last(data.size() - CONST_REGISTER_HEADER_SIZE));
                emit registerReadFinished(_currentItem);
            break;

            case stpProtocol_ns::stpCommand::REG_READ_ERROR:
                emit registerReadError(_currentItem);
            break;

            case stpProtocol_ns::stpCommand::REG_WRITE_ACK:
                emit registerWriteFinished(_currentItem);
            break;

            case stpProtocol_ns::stpCommand::REG_WRITE_ERROR:
                emit registerWriteError(_currentItem);
            break;
        }

        _startNext();
        return true;
    }
    return false;
}

void ADCQueue::_startNext()
{
    if (!_cmdItems->isEmpty()) {
        _currentItem = _cmdItems->dequeue();
        _currentItem.waiting = true;
        emit sendData(_packCommand(_currentItem));

        _repeatCount = 0;
        _timer->start();

        _state = ADCQueue_ns::queueState::waiting;
    } else {
        _state = ADCQueue_ns::queueState::stopped;
        emit queueIsEmpty();
    }
}

void ADCQueue::_repeatCurrent()
{
    if (_repeatCount == QUEUE_TIMEOUT_COUNT) {
        _startNext();
    } else {
        emit sendData(_packCommand(_currentItem));
        _timer->start();
    }
}

bool ADCQueue::_headerCheck(QByteArray *data)
{
    if (_state != ADCQueue_ns::queueState::waiting) return false;

    if (data->size() < CONST_REGISTER_HEADER_SIZE) return false;

    QByteArray * __header = new QByteArray(data->first(CONST_REGISTER_HEADER_SIZE));

    if (_currentItem.cmd == stpProtocol_ns::stpCommand::REG_READ)
        if (!((*__header)[0] == stpProtocol_ns::stpCommand::REG_READ_ACK || (*__header)[0] == stpProtocol_ns::stpCommand::REG_READ_ERROR)) return false;

    if (_currentItem.cmd == stpProtocol_ns::stpCommand::REG_WRITE)
        if (!((*__header)[0] == stpProtocol_ns::stpCommand::REG_WRITE_ACK || (*__header)[0] == stpProtocol_ns::stpCommand::REG_WRITE_ERROR)) return false;

    trans_ns::bytes2word __regNumber;
    __regNumber.bytes.L = (*__header)[3];
    __regNumber.bytes.H = (*__header)[4];

    if (__regNumber.word != _currentItem.regNumber) return false;

    trans_ns::bytes2word __id;
    __id.bytes.L = (*__header)[1];
    __id.bytes.H = (*__header)[2];

    if (__id.word != _currentItem.id) return false;

    return true;
}

QByteArray ADCQueue::_packCommand(ADCQueue_ns::cmdItem cmd)
{
    QByteArray __result;

    trans_ns::bytes2word __currentCmd;
    __currentCmd.word = cmd.id;

    trans_ns::bytes2word __regNum;
    __regNum.word = cmd.regNumber;

    __result.resize(CONST_REGISTER_HEADER_SIZE);
    __result[0]=cmd.cmd;
    __result[1]=__currentCmd.bytes.L;
    __result[2]=__currentCmd.bytes.H;
    __result[3]=__regNum.bytes.L;
    __result[4]=__regNum.bytes.H;

    if (cmd.cmd == stpProtocol_ns::stpCommand::REG_WRITE) {
        __result.append(*cmd.regData);
    }

    return __result;
}