#include "UWBHelper.h"

UWBHelper::UWBHelper(HardwareSerial* serial, int reset) {
    uwbSerial = serial;
    resetPin = reset;
}

bool UWBHelper::begin() {
    pinMode(resetPin, OUTPUT);
    digitalWrite(resetPin, HIGH);
    
    uwbSerial->begin(115200);
    delay(1000);
    
    // Test communication
    String response = sendCommand("AT?", 2000);
    return isResponseOK(response);
}

void UWBHelper::reset() {
    digitalWrite(resetPin, LOW);
    delay(100);
    digitalWrite(resetPin, HIGH);
    delay(1000);
}

bool UWBHelper::configureDevice(int deviceId, bool isAnchor, int dataRate, int rangeFilter) {
    String cmd = "AT+SETCFG=" + String(deviceId) + "," + 
                 String(isAnchor ? 1 : 0) + "," + 
                 String(dataRate) + "," + 
                 String(rangeFilter);
    
    String response = sendCommand(cmd, 2000);
    return isResponseOK(response);
}

bool UWBHelper::setCapacity(int tagCount, int timeSlot, int extMode) {
    String cmd = "AT+SETCAP=" + String(tagCount) + "," + 
                 String(timeSlot) + "," + 
                 String(extMode);
    
    String response = sendCommand(cmd, 2000);
    return isResponseOK(response);
}

bool UWBHelper::setNetwork(int networkId) {
    String cmd = "AT+SETPAN=" + String(networkId);
    String response = sendCommand(cmd, 2000);
    return isResponseOK(response);
}

bool UWBHelper::enableReporting(bool enable) {
    String cmd = "AT+SETRPT=" + String(enable ? 1 : 0);
    String response = sendCommand(cmd, 2000);
    return isResponseOK(response);
}

bool UWBHelper::saveConfiguration() {
    String response = sendCommand("AT+SAVE", 2000);
    return isResponseOK(response);
}

bool UWBHelper::restartDevice() {
    String response = sendCommand("AT+RESTART", 2000);
    return isResponseOK(response);
}

String UWBHelper::sendCommand(String command, int timeout) {
    String response = "";
    
    Serial.println("Sending: " + command);
    uwbSerial->println(command);
    
    unsigned long startTime = millis();
    while ((millis() - startTime) < timeout) {
        while (uwbSerial->available()) {
            char c = uwbSerial->read();
            response += c;
        }
    }
    
    if (response.length() > 0) {
        Serial.println("Response: " + response);
    }
    return response;
}

bool UWBHelper::parseRangeData(String data, DeviceData devices[], int maxDevices) {
    if (!data.startsWith("AT+RANGE=")) {
        return false;
    }
    
    // Parse tid (Tag ID)
    int tidIndex = data.indexOf("tid:");
    if (tidIndex == -1) return false;
    
    int commaPos = data.indexOf(',', tidIndex);
    if (commaPos == -1) return false;
    
    int deviceId = data.substring(tidIndex + 4, commaPos).toInt();
    
    // Parse range data
    int rangeIndex = data.indexOf("range:(");
    if (rangeIndex == -1) return false;
    
    int rangeStart = rangeIndex + 7;
    int rangeEnd = data.indexOf(')', rangeStart);
    if (rangeEnd == -1) return false;
    
    String rangeData = data.substring(rangeStart, rangeEnd);
    float distance = rangeData.toFloat() / 100.0; // Convert cm to meters
    
    // Parse RSSI data
    int rssiIndex = data.indexOf("rssi:(");
    float rssi = 0.0;
    if (rssiIndex != -1) {
        int rssiStart = rssiIndex + 6;
        int rssiComma = data.indexOf(',', rssiStart);
        if (rssiComma == -1) rssiComma = data.indexOf(')', rssiStart);
        if (rssiComma != -1) {
            rssi = data.substring(rssiStart, rssiComma).toFloat();
        }
    }
    
    // Update device data
    for (int i = 0; i < maxDevices; i++) {
        if (devices[i].deviceId == deviceId || !devices[i].active) {
            devices[i].deviceId = deviceId;
            devices[i].distance = distance;
            devices[i].rssi = rssi;
            devices[i].lastUpdate = millis();
            devices[i].active = true;
            return true;
        }
    }
    
    return false;
}

String UWBHelper::getVersion() {
    return sendCommand("AT+GETVER?", 2000);
}

bool UWBHelper::isResponseOK(String response) {
    return response.indexOf("OK") >= 0;
}

void UWBHelper::printDiagnostics() {
    Serial.println("=== UWB Diagnostics ===");
    Serial.println("Version: " + getVersion());
    Serial.println("Config: " + sendCommand("AT+GETCFG?", 2000));
    Serial.println("Capacity: " + sendCommand("AT+GETCAP?", 2000));
    Serial.println("Power: " + sendCommand("AT+GETPOW?", 2000));
}

// DistanceFilter Implementation
DistanceFilter::DistanceFilter() : index(0), filled(false) {
    for (int i = 0; i < FILTER_SIZE; i++) readings[i] = 0;
}

float DistanceFilter::addReading(float distance) {
    readings[index] = distance;
    index = (index + 1) % FILTER_SIZE;
    if (index == 0) filled = true;
    
    return getFilteredValue();
}

float DistanceFilter::getFilteredValue() {
    if (!filled && index == 0) return readings[0];
    
    // Median filter
    float sorted[FILTER_SIZE];
    int count = filled ? FILTER_SIZE : index;
    
    for (int i = 0; i < count; i++) {
        sorted[i] = readings[i];
    }
    
    // Simple bubble sort
    for (int i = 0; i < count - 1; i++) {
        for (int j = 0; j < count - i - 1; j++) {
            if (sorted[j] > sorted[j + 1]) {
                float temp = sorted[j];
                sorted[j] = sorted[j + 1];
                sorted[j + 1] = temp;
            }
        }
    }
    
    return sorted[count / 2]; // Return median
}

void DistanceFilter::reset() {
    index = 0;
    filled = false;
    for (int i = 0; i < FILTER_SIZE; i++) readings[i] = 0;
}