#include "bms.h"

#include <Arduino.h>
#include <NimBLEDevice.h>
#include "logger.h"
#include "app_config.h"

// BMS address is loaded from appConfig.bms.address.
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 unsigned long bmsReconnectPausedUntil = 0;
static bool bmsSetupMode = false;
static const unsigned long READ_INTERVAL_MS = 5000;

BmsData bmsData;

static BleScanResult bleScanResults[MAX_BLE_SCAN_RESULTS];
static int bleScanResultCount = 0;

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;
    }
}



class BleScanCallbacks : public NimBLEScanCallbacks {
    void onResult(const NimBLEAdvertisedDevice* device) override {
        if (bleScanResultCount >= MAX_BLE_SCAN_RESULTS) {
            return;
        }

        String address = device->getAddress().toString().c_str();

        for (int i = 0; i < bleScanResultCount; i++) {
            if (bleScanResults[i].address == address) {
                bleScanResults[i].rssi = device->getRSSI();

                if (bleScanResults[i].name.length() == 0 && device->haveName()) {
                    bleScanResults[i].name = device->getName().c_str();
                }

                return;
            }
        }

        String name = "";
        if (device->haveName()) {
            name = device->getName().c_str();
        }

        bleScanResults[bleScanResultCount].address = address;
        bleScanResults[bleScanResultCount].name = name;
        bleScanResults[bleScanResultCount].rssi = device->getRSSI();

        Serial.print(logTimestamp());
        Serial.print(" BLE: found ");
        Serial.print(bleScanResultCount + 1);
        Serial.print(") ");

        if (name.length() > 0) {
            Serial.print(name);
        } else {
            Serial.print("(no name)");
        }

        Serial.print(" | ");
        Serial.print(address);
        Serial.print(" | RSSI ");
        Serial.println(device->getRSSI());

        bleScanResultCount++;
    }
};

static BleScanCallbacks bleScanCallbacks;

void pauseBmsReconnect(uint32_t pauseMs) {
    bmsReconnectPausedUntil = millis() + pauseMs;
}


void enterBmsSetupMode() {
    Serial.print(logTimestamp());
    Serial.println(" BMS setup: entering setup mode");

    bmsSetupMode = true;
    pauseBmsReconnect(3600000UL);

    if (client && client->isConnected()) {
        Serial.print(logTimestamp());
        Serial.println(" BMS setup: disconnecting BMS");
        client->disconnect();
    }

    if (client) {
        NimBLEDevice::deleteClient(client);
        client = nullptr;
    }

    notifyChar = nullptr;
    writeChar = nullptr;
    bmsData.connected = false;

    Serial.print(logTimestamp());
    Serial.println(" BMS setup: ready for BLE scans");
}

void exitBmsSetupMode() {
    Serial.print(logTimestamp());
    Serial.println(" BMS setup: exiting setup mode");

    bmsSetupMode = false;
    bmsReconnectPausedUntil = 0;
}

bool isBmsSetupMode() {
    return bmsSetupMode;
}

int getBleScanResultCount() {
    return bleScanResultCount;
}

const BleScanResult* getBleScanResult(int index) {
    if (index < 0 || index >= bleScanResultCount) {
        return nullptr;
    }

    return &bleScanResults[index];
}

int scanBleDevices(uint32_t scanSeconds) {
    Serial.print(logTimestamp());
    Serial.println(" BLE: Starting setup scan...");

    if (!bmsSetupMode) {
        Serial.print(logTimestamp());
        Serial.println(" BLE: auto-entering BMS setup mode for scan");
        enterBmsSetupMode();
    }

    bleScanResultCount = 0;

    Serial.print(logTimestamp());
    Serial.println(" BLE: waiting 5 seconds before scan");
    delay(5000);

    NimBLEScan* scan = NimBLEDevice::getScan();
    scan->setActiveScan(true);
    scan->setInterval(100);
    scan->setWindow(99);
    scan->clearResults();

    Serial.print(logTimestamp());
    Serial.print(" BLE: scanning for ");
    Serial.print(scanSeconds);
    Serial.println(" seconds...");

    unsigned long scanStart = millis();

    NimBLEScanResults results = scan->getResults(scanSeconds, false);

    unsigned long elapsed = (millis() - scanStart) / 1000;

    Serial.print(logTimestamp());
    Serial.print(" BLE: scan returned after ");
    Serial.print(elapsed);
    Serial.println(" seconds");

    int resultCount = results.getCount();

    Serial.print(logTimestamp());
    Serial.print(" BLE: raw devices seen: ");
    Serial.println(resultCount);

    for (int i = 0; i < resultCount && bleScanResultCount < MAX_BLE_SCAN_RESULTS; i++) {
        const NimBLEAdvertisedDevice* device = results.getDevice(i);

        String address = device->getAddress().toString().c_str();

        bool alreadySeen = false;
        for (int existing = 0; existing < bleScanResultCount; existing++) {
            if (bleScanResults[existing].address == address) {
                alreadySeen = true;
                break;
            }
        }

        if (alreadySeen) {
            continue;
        }

        String name = "";
        if (device->haveName()) {
            name = device->getName().c_str();
        }

        bleScanResults[bleScanResultCount].address = address;
        bleScanResults[bleScanResultCount].name = name;
        bleScanResults[bleScanResultCount].rssi = device->getRSSI();
        bleScanResultCount++;
    }

    scan->clearResults();

    Serial.print(logTimestamp());
    Serial.print(" BLE: devices found: ");
    Serial.println(bleScanResultCount);

    if (bleScanResultCount == 0) {
        Serial.println("No BLE devices found.");
        return 0;
    }

    Serial.println("BLE devices:");

    for (int i = 0; i < bleScanResultCount; i++) {
        Serial.print(i + 1);
        Serial.print(") ");

        if (bleScanResults[i].name.length() > 0) {
            Serial.print(bleScanResults[i].name);
        } else {
            Serial.print("(no name)");
        }

        Serial.print(" | ");
        Serial.print(bleScanResults[i].address);
        Serial.print(" | RSSI ");
        Serial.println(bleScanResults[i].rssi);
    }

    Serial.println("Use: select bms <number>");
    Serial.println("Use: exit setup when finished");

    return bleScanResultCount;
}

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...");

    if (!appConfig.bms.enabled || appConfig.bms.address.length() == 0) {
        bmsData.connected = false;
        return false;
    }

    NimBLEAddress address(
        std::string(appConfig.bms.address.c_str()),
        appConfig.bms.addressType == "random" ? BLE_ADDR_RANDOM : 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;

    if (currentLogLevel >= LOG_DEBUG) {
        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;

    if (currentLogLevel >= LOG_DEBUG) {
        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 (bmsSetupMode) {
        return;
    }

    if ((long)(bmsReconnectPausedUntil - millis()) > 0) {
        return;
    }

    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();
    }
}