#include "AgentMessage.h"

#include <QtEndian>

// ---------------------------------------------------------------------------
// Outbound
// ---------------------------------------------------------------------------
QByteArray AgentMessage::build(MsgId msgId, quint16 sequence, const QByteArray &payload)
{
    const quint32 payloadLen = static_cast<quint32>(payload.size());

    // Header: sync(2) + msg_id(2) + sequence(2) + payload_length(4) + header_crc(2) = 12 bytes
    QByteArray msg(HEADER_SIZE, Qt::Uninitialized);
    quint8 *header = reinterpret_cast<quint8 *>(msg.data());

    header[0] = SYNC[0];
    header[1] = SYNC[1];
    qToBigEndian<quint16>(static_cast<quint16>(msgId), header + SYNC_SIZE);
    qToBigEndian<quint16>(sequence, header + SYNC_SIZE + MSGID_SIZE);
    qToBigEndian<quint32>(payloadLen, header + SYNC_SIZE + MSGID_SIZE + SEQUENCE_SIZE);

    const quint16 hCrc = crc16ccitt(header, HEADER_SIZE - HEADER_CRC_SIZE);
    qToBigEndian<quint16>(hCrc, header + HEADER_SIZE - HEADER_CRC_SIZE);

    if (payloadLen == 0) {
        return msg;
    }

    msg.append(payload);

    const quint32 crc = crc32isohdlc(reinterpret_cast<const quint8 *>(payload.constData()), payload.size());
    QByteArray crcBytes(PAYLOAD_CRC_SIZE, Qt::Uninitialized);
    qToBigEndian<quint32>(crc, reinterpret_cast<quint8 *>(crcBytes.data()));
    msg.append(crcBytes);

    return msg;
}

// ---------------------------------------------------------------------------
// Inbound
// \details 1.  Bytes are consumed when: scanning for the sync word, a complete header is parsed,
//              or a full frame (header + payload) is parsed.
//          2.  Header and payload will try to be read in the same feed() call when possible, but they
//              may also be parsed across multiple feed() calls if the input buffer is fragmented.
//              The caller is responsible for accumulating incoming bytes into the buffer and invoking feed()
//              each time new bytes arrive.
//          3.  If a header fails CRC validation, only the sync word is discarded. The payload section is
//              skipped and the remaining bytes stay in the buffer to be re-scanned on the next feed() call.
//          4.  If a payload fails CRC validation, only the header is discarded (pos is not advanced past
//              the payload). The payload bytes stay in the buffer to be re-scanned on the next feed() call.
//          5.  bytes parameter is modified in-place to remove the consumed bytes. The caller is responsible for
//              appending new incoming bytes to the buffer and invoking feed() again.
//          6.  If PayloadError is returned, the caller can immediately call feed() again in order to try
//              to search for a header in the remaining payload bytes.
//              If HeaderError is returned, the caller can call feed() again if there is any data remaining in the
//              buffer to search for a header in the remaining bytes.
// ---------------------------------------------------------------------------
AgentMessage::FeedResult AgentMessage::feed(QByteArray &bytes)
{
    int pos = 0;
    FeedResult result = FeedResult::Incomplete;

    // scan for a valid header — skipped when _headerBuf is already populated from a prior feed() call
    while (_headerBuf.size() == 0 && bytes.size() - pos >= HEADER_SIZE && result != FeedResult::HeaderError) {
        if (static_cast<quint8>(bytes.at(pos)) == SYNC[0] && static_cast<quint8>(bytes.at(pos + 1)) == SYNC[1]) {
            _headerBuf.append(bytes.constData() + pos, HEADER_SIZE);
            if (crc16ccitt(reinterpret_cast<const quint8 *>(_headerBuf.constData()), HEADER_SIZE - HEADER_CRC_SIZE) ==
                qFromBigEndian<quint16>(reinterpret_cast<const quint8 *>(_headerBuf.constData() + HEADER_SIZE -
                                                                         HEADER_CRC_SIZE)))
            {
                const quint8 *header = reinterpret_cast<const quint8 *>(_headerBuf.constData());
                int header_pos = SYNC_SIZE;
                _rxMsgId = static_cast<MsgId>(qFromBigEndian<quint16>(header + header_pos));
                header_pos += MSGID_SIZE;
                _rxSequence = qFromBigEndian<quint16>(header + header_pos);
                header_pos += SEQUENCE_SIZE;
                _rxPayloadLen = qFromBigEndian<quint32>(header + header_pos);
                pos += HEADER_SIZE;
            }
            else {
                // TODO: log the header CRC failure
                _headerBuf.clear();
                pos += SYNC_SIZE;
                result = FeedResult::HeaderError;
            }
        }
        else {
            ++pos;
        }
    }

    // process payload if a valid header has been accumulated
    if (result != FeedResult::HeaderError && _headerBuf.size() == HEADER_SIZE) {
        if (_rxPayloadLen == 0) {
            result = FeedResult::Complete;
        }
        else if (_rxPayloadLen > MAX_PAYLOAD_LEN) {
            // TODO: log the oversized payload
            _headerBuf.clear();
            result = FeedResult::PayloadError;
        }
        else if (bytes.size() - pos >= static_cast<int>(_rxPayloadLen) + PAYLOAD_CRC_SIZE) {
            const quint8 *payload = reinterpret_cast<const quint8 *>(bytes.constData() + pos);
            if (crc32isohdlc(payload, static_cast<int>(_rxPayloadLen)) ==
                qFromBigEndian<quint32>(payload + _rxPayloadLen))
            {
                _rxPayload = QByteArray(reinterpret_cast<const char *>(payload), static_cast<int>(_rxPayloadLen));
                pos += static_cast<int>(_rxPayloadLen) + PAYLOAD_CRC_SIZE;
                result = FeedResult::Complete;
            }
            else {
                // TODO: log the payload CRC failure
                _headerBuf.clear();
                result = FeedResult::PayloadError;
            }
        }
    }

    // remove the consumed bytes from the input buffer
    bytes.remove(0, pos);

    return result;
}

void AgentMessage::reset()
{
    _headerBuf.clear();
    _rxMsgId = MsgId::ClinicalData;
    _rxSequence = 0;
    _rxPayloadLen = 0;
    _rxPayload.clear();
}

// ---------------------------------------------------------------------------
// CRC-16/CCITT  (poly 0x1021, init 0xFFFF, no reflection, no final XOR)
// Check value for "123456789": 0x29B1
// ---------------------------------------------------------------------------
quint16 AgentMessage::crc16ccitt(const quint8 *data, int len)
{
    quint16 crc = 0xFFFF;
    for (int i = 0; i < len; ++i) {
        crc ^= static_cast<quint16>(data[i]) << 8;
        for (int bit = 0; bit < 8; ++bit) {
            if (crc & 0x8000) {
                crc = (crc << 1) ^ 0x1021;
            }
            else {
                crc <<= 1;
            }
        }
    }
    return crc;
}

// ---------------------------------------------------------------------------
// CRC-32/ISO-HDLC  (IEEE 802.3, reflected poly 0xEDB88320,
//                   init 0xFFFFFFFF, input/output reflected, final XOR 0xFFFFFFFF)
// Check value for "123456789": 0xCBF43926
// ---------------------------------------------------------------------------
quint32 AgentMessage::crc32isohdlc(const quint8 *data, int len)
{
    quint32 crc = 0xFFFFFFFF;
    for (int i = 0; i < len; ++i) {
        crc ^= data[i];
        for (int bit = 0; bit < 8; ++bit) {
            if (crc & 1) {
                crc = (crc >> 1) ^ 0xEDB88320;
            }
            else {
                crc >>= 1;
            }
        }
    }
    return crc ^ 0xFFFFFFFF;
}