// =====================================================================
// LD2453Parser.cpp
//
// See the big comment at the top of LD2453Parser.h before touching
// this file. The acquisition/debug layer below is fully functional;
// tryParseFrame() is intentionally stubbed pending the official
// HLK-LD2453 protocol.
// =====================================================================
#include "LD2453Parser.h"
#include <cmath>

#if !defined(ARDUINO_ARCH_ESP32)
#warning "LD2453Parser was written/tested against the ESP32 Arduino core. Verify HardwareSerial pin-remap behavior on other cores."
#endif

// ---------------------------------------------------------------
// LD2453 PROTOCOL CONSTANTS (Confirmed from RAW capture)
// ---------------------------------------------------------------
static const uint8_t FRAME_HEADER[] = { 0xAA, 0xFF, 0x03, 0x00 };
static const uint8_t FRAME_TAIL[]   = { 0x55, 0xCC };
static const size_t  FRAME_LENGTH = 30;
static const size_t  HEADER_SIZE = 4;
static const size_t  TAIL_SIZE = 2;
static const size_t  DATA_SIZE = FRAME_LENGTH - HEADER_SIZE - TAIL_SIZE;
static const size_t  TARGET_RECORD_SIZE = 8;
static const size_t  MAX_TARGETS = 3;

void LD2453Parser::begin(HardwareSerial &serial, int rxPin, int txPin, uint32_t baud,
                          const char *nodeId, bool rawDebug) {
    _serial = &serial;
    _rawDebug = rawDebug;
    _nodeId = nodeId;
    _bufferLen = 0;
    _sequenceCounter = 0;
    _stats = LD2453Stats{};
    _ppsWindowStart = millis();
    _ppsWindowCount = 0;

    // HardwareSerial::begin(baud, config, rxPin, txPin) — ESP32 Arduino
    // core supports remapping RX/TX to any capable GPIO. 8N1 assumed;
    // confirm against the official manual.
    _serial->begin(baud, SERIAL_8N1, rxPin, txPin);
}

void LD2453Parser::dumpRawHex(const uint8_t *data, size_t len) {
    Serial.print(F("[RAW] "));
    for (size_t i = 0; i < len; i++) {
        if (data[i] < 0x10) Serial.print('0');
        Serial.print(data[i], HEX);
        Serial.print(' ');
    }
    Serial.print(F(" (len="));
    Serial.print(len);
    Serial.println(')');
}

void LD2453Parser::feedByte(uint8_t b) {
    if (_bufferLen >= RING_BUFFER_SIZE) {
        // Buffer overrun with no recognized frame boundary — drop the
        // oldest half so we can keep tolerating a stuck/garbage stream
        // without ever blocking or allocating.
        memmove(_ringBuffer, _ringBuffer + RING_BUFFER_SIZE / 2, RING_BUFFER_SIZE / 2);
        _bufferLen = RING_BUFFER_SIZE / 2;
        _stats.parseErrors++;
    }
    _ringBuffer[_bufferLen++] = b;
}

bool LD2453Parser::update() {
    if (!_serial) return false;

    bool gotFrame = false;

    // Drain whatever is available right now — non-blocking, no delay().
    // While rawDebug is on, dump exactly the newly-arrived bytes as
    // hex so you can capture real traffic to reverse-engineer the
    // frame format, independent of whether tryParseFrame() (still a
    // stub) recognizes anything.
    size_t newBytesStart = _bufferLen;
    while (_serial->available() > 0) {
        uint8_t b = (uint8_t)_serial->read();
        feedByte(b);
        _stats.bytesReceived++;
        _lastByteTime = millis();  // Cập nhật thời điểm nhận byte
    }
    if (_rawDebug && _bufferLen > newBytesStart) {
        dumpRawHex(_ringBuffer + newBytesStart, _bufferLen - newBytesStart);
    }

    // Try to pull as many complete frames out of the buffer as
    // possible in this call (in case several arrived since the last
    // loop() iteration).
    while (tryParseFrame()) {
        gotFrame = true;
        _stats.framesParsed++;
        _ppsWindowCount++;
    }

    // Update packets-per-second once a second, non-blocking.
    uint32_t now = millis();
    if (now - _ppsWindowStart >= 1000) {
        _stats.packetsPerSecond = _ppsWindowCount * 1000.0f / (now - _ppsWindowStart);
        _ppsWindowStart = now;
        _ppsWindowCount = 0;
    }

    return gotFrame;
}

bool LD2453Parser::tryParseFrame() {
    // Cần ít nhất đủ bytes cho 1 frame hoàn chỉnh
    if (_bufferLen < FRAME_LENGTH) {
        return false;
    }

    // Tìm frame header
    size_t headerPos = 0;
    bool foundHeader = false;
    
    for (size_t i = 0; i <= _bufferLen - FRAME_LENGTH; i++) {
        if (memcmp(_ringBuffer + i, FRAME_HEADER, HEADER_SIZE) == 0) {
            headerPos = i;
            foundHeader = true;
            break;
        }
    }

    if (!foundHeader) {
        // Không tìm thấy header, drop bytes cũ để tránh tràn buffer
        if (_bufferLen >= RING_BUFFER_SIZE / 2) {
            memmove(_ringBuffer, _ringBuffer + RING_BUFFER_SIZE / 4, 
                    _bufferLen - RING_BUFFER_SIZE / 4);
            _bufferLen -= RING_BUFFER_SIZE / 4;
        }
        return false;
    }

    // Kiểm tra đủ bytes cho frame hoàn chỉnh
    if (headerPos + FRAME_LENGTH > _bufferLen) {
        return false; // Chưa đủ bytes, chờ thêm
    }

    // Verify frame tail
    size_t tailPos = headerPos + FRAME_LENGTH - TAIL_SIZE;
    if (memcmp(_ringBuffer + tailPos, FRAME_TAIL, TAIL_SIZE) != 0) {
        // Header đúng nhưng tail sai, bỏ qua header này
        memmove(_ringBuffer, _ringBuffer + headerPos + 1, _bufferLen - headerPos - 1);
        _bufferLen -= (headerPos + 1);
        _stats.parseErrors++;
        return false;
    }

    // Frame hợp lệ! Parse data
    uint8_t *data = _ringBuffer + headerPos + HEADER_SIZE;
    
    // Reset frame
    _lastFrame.timestamp_ms = millis();
    _lastFrame.sequence_number = ++_sequenceCounter;
    _lastFrame.node_id = _nodeId;
    _lastFrame.targetCount = 0;

    // Parse targets (3 targets, mỗi target 8 bytes)
    for (uint8_t t = 0; t < MAX_TARGETS; t++) {
        uint8_t *targetData = data + (t * TARGET_RECORD_SIZE);
        
        // Byte layout (giả định, cần verify với manual):
        // [0-1]: X (little endian, signed 16-bit, mm)
        // [2-3]: Y (little endian, signed 16-bit, mm)
        // [4-5]: Speed (little endian, signed 16-bit, cm/s)
        // [6-7]: Reserved hoặc distance
        
        int16_t x = (int16_t)(targetData[0] | (targetData[1] << 8));
        int16_t y = (int16_t)(targetData[2] | (targetData[3] << 8));
        int16_t speed = (int16_t)(targetData[4] | (targetData[5] << 8));
        
        // Target valid nếu không phải tất cả đều 0
        bool valid = (x != 0 || y != 0 || speed != 0);
        
        _lastFrame.targets[t].valid = valid;
        _lastFrame.targets[t].x_mm = x;
        _lastFrame.targets[t].y_mm = y;
        _lastFrame.targets[t].speed = speed;
        
        if (valid) {
            // Tính distance và angle từ x, y
            _lastFrame.targets[t].distance_mm = sqrt(x*x + y*y);
            _lastFrame.targets[t].angle_deg = atan2(x, y) * 180.0 / PI;
            _lastFrame.targetCount++;
        } else {
            _lastFrame.targets[t].distance_mm = 0;
            _lastFrame.targets[t].angle_deg = 0;
        }
    }

    // Lưu frame length vào stats
    _stats.lastFrameLength = FRAME_LENGTH;

    // Remove frame đã parse khỏi buffer
    size_t consumedBytes = headerPos + FRAME_LENGTH;
    memmove(_ringBuffer, _ringBuffer + consumedBytes, _bufferLen - consumedBytes);
    _bufferLen -= consumedBytes;

    return true;
}