#include "crsf.h"

// rawBuffer — полный CRSF-пакет:
// [0] = preamble (0xC8)
// [1] = size
// [2] = address
// [3] = type
// [4..N-2] = payload
// [N-1] = crc
crsf::CrsfFrame::CrsfFrame(std::span<const uint8_t> rawBuffer) {
    if (rawBuffer.size() < 6) {
        // минимальный размер CRSF кадра — 6 байт
        return;
    }

    uint8_t size = rawBuffer[1];
    if (rawBuffer.size() != size + 2) {
        // размер несоответствует фактической длине буфера
        return;
    }

    address = rawBuffer[2];
    type    = rawBuffer[3];

    size_t payloadSize = size - 2; // address + type занимают первые 2
    payload.assign(rawBuffer.begin() + 4, rawBuffer.begin() + 4 + payloadSize);

    crc = rawBuffer[rawBuffer.size() - 1];
}

bool crsf::CrsfFrame::checkCrc() const {
    // CRC считается по address + type + payload
    uint8_t expected = crsfComputeCrc(std::span<const uint8_t>(payload.empty()
            ? nullptr : &payload[0], payload.size() + 2));

    // Нам нужно подготовить временный буфер, потому что адрес и тип не в payload
    std::vector<uint8_t> tmp;
    tmp.reserve(payload.size() + 2);
    tmp.push_back(address);
    tmp.push_back(type);
    tmp.insert(tmp.end(), payload.begin(), payload.end());

    expected = crsfComputeCrc(tmp);

    return expected == crc;
}
void crsf::CrsfFrame::setCrc() {
    // аналогично checkCrc, но записываем CRC в поле структуры
    std::vector<uint8_t> tmp;
    tmp.reserve(payload.size() + 2);
    tmp.push_back(address);
    tmp.push_back(type);
    tmp.insert(tmp.end(), payload.begin(), payload.end());

    crc = crsfComputeCrc(tmp);
}
void crsf::CrsfFrame::writeToBuffer(std::vector<uint8_t>& dest) const {
    dest.clear();

    const uint8_t preamble = 0xC8;
    const uint8_t size = static_cast<uint8_t>(2 + payload.size() + 1); // addr + type + payload + crc

    dest.reserve(size + 2);

    dest.push_back(preamble);
    dest.push_back(size);
    dest.push_back(address);
    dest.push_back(type);

    dest.insert(dest.end(), payload.begin(), payload.end());

    // финальный байт crc
    dest.push_back(crc);
}


uint8_t crsf::crsfComputeCrc(std::span<const uint8_t> data) {
    uint8_t crc = 0;
    for (const auto i: data) {
        crc += i;
    }
    return crc;
}

void crsf::CrsfParser::reset() {
    bufferPos = 0;
    frameSize = 0;
}

void crsf::CrsfParser::parseBytes(const std::vector<uint8_t>& data) {
    for (auto byte : data) {
        // преамбула
        if (bufferPos == 0) {
            if (byte == PREAMBLE) {
                buffer[0] = byte;
                bufferPos = 1;
            }
            continue;
        }

        // чтение size
        if (bufferPos == 1) {
            buffer[1] = byte;
            frameSize = byte;

            if (frameSize < 3 || frameSize > (CRSF_MAX_FRAME_SIZE - 2)) {
                reset();
                continue;
            }

            bufferPos = 2;
            continue;
        }

        // обработка тела кадра
        buffer[bufferPos++] = byte;

        const size_t fullFrameSize = frameSize + 2; // preamble + size + frameSize bytes

        if (bufferPos == fullFrameSize) {
            auto frame = std::make_unique<CrsfFrame>(std::span(buffer, bufferPos));
            if (frame->checkCrc()) {
                frames.push_back(std::move(frame));
            }

            reset();
        }
    }
}

std::unique_ptr<crsf::CrsfFrame> crsf::CrsfParser::pullPacket() {
    std::unique_ptr<CrsfFrame> out;
    if (!this->frames.empty()) {
        out = std::move(this->frames.front());
        frames.pop_front();
    }
    return out;
}