#include "bms.h"

#include <Arduino.h>
#include <NimBLEDevice.h>

static const char* BMS_ADDRESS = "a5:c2:37:2c:05:dc";
static const char* BMS_SERVICE_UUID = "ff00";
static const char* BMS_NOTIFY_UUID = "ff01";
static const char* BMS_WRITE_UUID = "ff02";

static NimBLEClient* client = nullptr;
static NimBLERemoteCharacteristic* notifyChar = nullptr;
static NimBLERemoteCharacteristic* writeChar = nullptr;

static uint8_t responseBuffer[128];
static size_t responseLength = 0;
static bool responseReady = false;

static unsigned long lastReadAttempt = 0;
static const unsigned long READ_INTERVAL_MS = 5000;

BmsData bmsData;

static uint8_t JBD_STATUS_REQUEST[] = {
    0xDD, 0xA5, 0x03, 0x00, 0xFF, 0xFD, 0x77
};

static uint8_t JBD_CELL_REQUEST[] = {
    0xDD, 0xA5, 0x04, 0x00, 0xFF, 0xFC, 0x77
};

static uint16_t readU16(const uint8_t* data, int index) {
    return ((uint16_t)data[index] << 8) | data[index + 1];
}

static int16_t readS16(const uint8_t* data, int index) {
    return (int16_t)readU16(data, index);
}

static void resetResponseBuffer() {
    responseLength = 0;
    responseReady = false;
    memset(responseBuffer, 0, sizeof(responseBuffer));
}

static void handleNotify(
    NimBLERemoteCharacteristic* characteristic,
    uint8_t* data,
    size_t length,
    bool isNotify
) {
    if (responseLength + length > sizeof(responseBuffer)) {
        resetResponseBuffer();
        return;
    }

    memcpy(responseBuffer + responseLength, data, length);
    responseLength += length;

    if (responseLength > 0 && responseBuffer[responseLength - 1] == 0x77) {
        responseReady = true;
    }
}

static bool connectBms() {
    if (client && client->isConnected()) {
        bmsData.connected = true;
        return true;
    }

    if (client && !client->isConnected()) {
        Serial.println("BMS: Disconnected, resetting client");
        NimBLEDevice::deleteClient(client);
        client = nullptr;
        notifyChar = nullptr;
        writeChar = nullptr;
        bmsData.connected = false;
    }

    Serial.println("BMS: Connecting...");

    NimBLEAddress address(std::string(BMS_ADDRESS), BLE_ADDR_PUBLIC);

    client = NimBLEDevice::createClient();

    if (!client->connect(address)) {
        Serial.println("BMS: Connect failed");
        bmsData.connected = false;
        NimBLEDevice::deleteClient(client);
        client = nullptr;
        return false;
    }

    NimBLERemoteService* service = client->getService(BMS_SERVICE_UUID);
    if (!service) {
        Serial.println("BMS: Service ff00 not found");
        client->disconnect();
        bmsData.connected = false;
        return false;
    }

    notifyChar = service->getCharacteristic(BMS_NOTIFY_UUID);
    writeChar = service->getCharacteristic(BMS_WRITE_UUID);

    if (!notifyChar || !writeChar) {
        Serial.println("BMS: Characteristics not found");
        client->disconnect();
        bmsData.connected = false;
        return false;
    }

    if (!notifyChar->subscribe(true, handleNotify)) {
        Serial.println("BMS: Notify subscribe failed");
        client->disconnect();
        bmsData.connected = false;
        return false;
    }

    Serial.println("BMS: Connected");
    bmsData.connected = true;
    return true;
}

static bool requestPacket(uint8_t* command, size_t commandLength) {
    resetResponseBuffer();

    bool ok = writeChar->writeValue(command, commandLength, false);
    if (!ok) {
        Serial.println("BMS: Write failed");
        bmsData.connected = false;
        if (client) {
            client->disconnect();
        }
        return false;
    }

    unsigned long start = millis();
    while (!responseReady && millis() - start < 2000) {
        delay(10);
    }

    if (!responseReady) {
        Serial.println("BMS: Read timeout");
        return false;
    }

    return true;
}

static bool parseBasicInfo() {
    if (responseLength < 40) {
        Serial.println("BMS: Basic response too short");
        return false;
    }

    if (responseBuffer[0] != 0xDD || responseBuffer[1] != 0x03) {
        Serial.println("BMS: Unexpected basic response header");
        return false;
    }

    uint16_t voltageRaw = readU16(responseBuffer, 4);
    int16_t currentRaw = readS16(responseBuffer, 6);
    uint16_t remainingRaw = readU16(responseBuffer, 8);
    uint16_t capacityRaw = readU16(responseBuffer, 10);
    uint16_t cyclesRaw = readU16(responseBuffer, 12);

    uint8_t socRaw = responseBuffer[23];

    uint8_t offset = 24;
    uint8_t cellCount = responseBuffer[offset + 1];
    uint8_t ntcCount = responseBuffer[offset + 2];

    int tempOffset = offset + 3;
    float tempF = 0;

    if (ntcCount > 0 && responseLength > (size_t)(tempOffset + 1)) {
        uint16_t tempRaw = readU16(responseBuffer, tempOffset);
        float tempC = (tempRaw - 2731) / 10.0;
        tempF = tempC * 9.0 / 5.0 + 32.0;
    }

    bmsData.voltage = voltageRaw / 100.0;
    bmsData.current = currentRaw / 100.0;
    bmsData.remainingAh = remainingRaw / 100.0;
    bmsData.capacityAh = capacityRaw / 100.0;
    bmsData.cycleCount = cyclesRaw;
    bmsData.soc = socRaw;
    bmsData.cellCount = cellCount;
    bmsData.ntcCount = ntcCount;
    bmsData.temperatureF = tempF;
    bmsData.valid = true;

    Serial.print("BMS: SOC ");
    Serial.print(bmsData.soc);
    Serial.print("%, V ");
    Serial.print(bmsData.voltage);
    Serial.print(", A ");
    Serial.print(bmsData.current);
    Serial.print(", Ah ");
    Serial.println(bmsData.remainingAh);

    return true;
}

static bool parseCellVoltages() {
    if (responseLength < 7) {
        Serial.println("BMS: Cell response too short");
        return false;
    }

    if (responseBuffer[0] != 0xDD || responseBuffer[1] != 0x04) {
        Serial.println("BMS: Unexpected cell response header");
        return false;
    }

    uint8_t payloadLength = responseBuffer[3];
    int cellsReported = payloadLength / 2;

    if (cellsReported > BMS_MAX_CELLS) {
        cellsReported = BMS_MAX_CELLS;
    }

    if (cellsReported <= 0) {
        Serial.println("BMS: No cell voltages reported");
        return false;
    }

    float minV = 99.0;
    float maxV = 0.0;

    for (int i = 0; i < cellsReported; i++) {
        uint16_t mv = readU16(responseBuffer, 4 + (i * 2));
        float volts = mv / 1000.0;

        bmsData.cellVoltages[i] = volts;

        if (volts < minV) {
            minV = volts;
        }

        if (volts > maxV) {
            maxV = volts;
        }
    }

    bmsData.cellCount = cellsReported;
    bmsData.cellMinVoltage = minV;
    bmsData.cellMaxVoltage = maxV;
    bmsData.cellDeltaMv = (int)((maxV - minV) * 1000.0 + 0.5);
    bmsData.cellsValid = true;

    Serial.print("BMS: Cells ");
    Serial.print(cellsReported);
    Serial.print(", min ");
    Serial.print(minV, 3);
    Serial.print("V, max ");
    Serial.print(maxV, 3);
    Serial.print("V, delta ");
    Serial.print(bmsData.cellDeltaMv);
    Serial.println("mV");

    return true;
}

void initBms() {
    NimBLEDevice::init("XterraESP32");
    NimBLEDevice::setPower(ESP_PWR_LVL_P9);
    resetResponseBuffer();
}

void updateBms() {
    if (millis() - lastReadAttempt < READ_INTERVAL_MS) {
        return;
    }

    lastReadAttempt = millis();

    if (!connectBms()) {
        return;
    }

    if (requestPacket(JBD_STATUS_REQUEST, sizeof(JBD_STATUS_REQUEST))) {
        parseBasicInfo();
    }

    delay(100);

    if (requestPacket(JBD_CELL_REQUEST, sizeof(JBD_CELL_REQUEST))) {
        parseCellVoltages();
    }
}