// BMW I/K-Bus protocol handler
// Extracted from IbusEsp32 — same FSM logic, reads from KLineTransport.

#include "IbusHandler.h"

IbusHandler::IbusHandler(KLineTransport& transport)
    : _transport(transport),
      _state(FIND_SOURCE),
      _source(0),
      _length(0),
      _findMsgEnteredMs(0),
      _callback(nullptr),
      _filterEnabled(false),
      _filterCount(0) {
    memset(_msgBuf, 0, sizeof(_msgBuf));
    memset(_filterAddrs, 0, sizeof(_filterAddrs));
}

void IbusHandler::process() {
    switch (_state) {
    case FIND_SOURCE:
        if (_transport.rxAvailable() >= 1) {
            _source = _transport.rxPeek(0);
            _state = FIND_LENGTH;
        }
        break;

    case FIND_LENGTH:
        if (_transport.rxAvailable() >= 2) {
            _length = _transport.rxPeek(1);
            if (_length >= IBUS_MIN_LENGTH && _length <= IBUS_MAX_LENGTH) {
                _state = FIND_MESSAGE;
                _findMsgEnteredMs = millis();
            } else {
                _transport.rxRemove(1);
                _state = FIND_SOURCE;
            }
        }
        break;

    case FIND_MESSAGE:
        if (_transport.rxAvailable() >= _length + 2) {
            uint8_t checksum = 0;
            for (int i = 0; i <= _length; i++) {
                checksum ^= _transport.rxPeek(i);
            }
            if (_transport.rxPeek(_length + 1) == checksum) {
                _state = GOOD_CHECKSUM;
            } else {
                _state = BAD_CHECKSUM;
            }
        } else if (millis() - _findMsgEnteredMs > 100) {
            // Timeout: partial message never completed (bus glitch).
            // At 9600 baud, max-length message takes ~44ms. 100ms is generous.
#ifdef IBUS_DEBUG
            Serial.print("RX TIMEOUT: len=0x");
            Serial.print(_length, HEX);
            Serial.print(" have=");
            Serial.println(_transport.rxAvailable());
#endif
            _transport.rxRemove(1);
            _state = FIND_SOURCE;
        }
        break;

    case GOOD_CHECKSUM:
        if (_filterEnabled && _filterCount > 0) {
            bool match = false;
            for (uint8_t i = 0; i < _filterCount; i++) {
                if (_source == _filterAddrs[i]) { match = true; break; }
            }
            if (!match) {
                _transport.rxRemove(_length + 2);
                _state = FIND_SOURCE;
                return;
            }
        }

        for (int i = 0; i < _length + 2; i++) {
            _msgBuf[i] = _transport.rxRead();
        }

        _transport.ledOn();

        if (_callback) {
            _callback(_msgBuf, _length + 2);
        }

        _transport.ledOff();

        _state = FIND_SOURCE;
        break;

    case BAD_CHECKSUM:
#ifdef IBUS_DEBUG
        {
            uint8_t n = min((int)(_length + 2), _transport.rxAvailable());
            for (uint8_t i = 0; i < n; i++) {
                _msgBuf[i] = _transport.rxPeek(i);
            }
            Serial.print("BAD CHK: ");
            for (uint8_t i = 0; i < n; i++) {
                if (_msgBuf[i] < 0x10) Serial.print('0');
                Serial.print(_msgBuf[i], HEX);
                Serial.print(' ');
            }
            Serial.println();
        }
#endif
        _transport.rxRemove(1);
        _state = FIND_SOURCE;
        break;
    }
}

void IbusHandler::write(const uint8_t* message, uint8_t size) {
    _transport.write(message, size);
}

void IbusHandler::onPacket(PacketCallback callback) {
    _callback = callback;
}

void IbusHandler::setFilterEnabled(bool enabled) {
    _filterEnabled = enabled;
}

void IbusHandler::setFilterAddress(uint8_t addr) {
    _filterAddrs[0] = addr;
    _filterCount = 1;
    _filterEnabled = true;
}

void IbusHandler::setFilterAddresses(const uint8_t* addrs, uint8_t count) {
    uint8_t n = min(count, (uint8_t)16);
    memcpy(_filterAddrs, addrs, n);
    _filterCount = n;
    _filterEnabled = true;
}