#include "PhiBLE.h"

/*
 ===========================================================
    PhiBLE.cpp - VERSION v5.2 SPECTRUM
    Bluetooth Low Energy Communication + 8-band Spectrum
 ===========================================================
*/

PhiBLE::PhiBLE()
: _service(nullptr),
  _imuChar(nullptr),
  _magChar(nullptr),
  _proxChar(nullptr),
  _baroChar(nullptr),
  _audioChar(nullptr),
  _emiChar(nullptr),
  _tempChar(nullptr),
  _humChar(nullptr),
  _statusChar(nullptr),
  _gpsChar(nullptr),
  _magAngleChar(nullptr),
  _spectrumChar(nullptr),
  _updateCount(0),
  _initialized(false)
{}

bool PhiBLE::begin(const char* deviceName) {
    Serial.println("Initializing BLE...");
    
    // Init BLE
    if (!BLE.begin()) {
        Serial.println("✗ BLE.begin() failed");
        return false;
    }
    
    // Configure device
    BLE.setLocalName(deviceName);
    BLE.setDeviceName(deviceName);
    
    // Create service
    _service = new BLEService(BLE_SERVICE_UUID);
    BLE.setAdvertisedService(*_service);
    
    // Create characteristics
    _imuChar = new BLEFloatCharacteristic(BLE_CHAR_IMU_UUID, BLERead | BLENotify);
    _magChar = new BLEFloatCharacteristic(BLE_CHAR_MAG_UUID, BLERead | BLENotify);
    _proxChar = new BLEUnsignedCharCharacteristic(BLE_CHAR_PROX_UUID, BLERead | BLENotify);
    _baroChar = new BLEFloatCharacteristic(BLE_CHAR_BARO_UUID, BLERead | BLENotify);
    _audioChar = new BLEUnsignedIntCharacteristic(BLE_CHAR_AUDIO_UUID, BLERead | BLENotify);
    _emiChar = new BLEUnsignedIntCharacteristic(BLE_CHAR_EMI_UUID, BLERead | BLENotify);
    _tempChar = new BLEFloatCharacteristic(BLE_CHAR_TEMP_UUID, BLERead | BLENotify);
    _humChar = new BLEFloatCharacteristic(BLE_CHAR_HUM_UUID, BLERead | BLENotify);
    _statusChar = new BLEUnsignedCharCharacteristic(BLE_CHAR_STATUS_UUID, BLERead | BLENotify);
    _gpsChar = new BLECharacteristic(BLE_CHAR_GPS_UUID, BLERead | BLENotify, 13);
    _magAngleChar = new BLEFloatCharacteristic(BLE_CHAR_MAG_ANGLE_UUID, BLERead | BLENotify);
    _spectrumChar = new BLECharacteristic(BLE_CHAR_SPECTRUM_UUID, BLERead | BLENotify, 8);

    // Add characteristics to service
    _service->addCharacteristic(*_imuChar);
    _service->addCharacteristic(*_magChar);
    _service->addCharacteristic(*_proxChar);
    _service->addCharacteristic(*_baroChar);
    _service->addCharacteristic(*_audioChar);
    _service->addCharacteristic(*_emiChar);
    _service->addCharacteristic(*_tempChar);
    _service->addCharacteristic(*_humChar);
    _service->addCharacteristic(*_statusChar);
    _service->addCharacteristic(*_gpsChar);
    _service->addCharacteristic(*_magAngleChar);
    _service->addCharacteristic(*_spectrumChar);

    // Add service
    BLE.addService(*_service);
    
    // Set initial values
    _imuChar->writeValue(0.0f);
    _magChar->writeValue(0.0f);
    _proxChar->writeValue(0);
    _baroChar->writeValue(0.0f);
    _audioChar->writeValue(0);
    _emiChar->writeValue(0);
    _tempChar->writeValue(0.0f);
    _humChar->writeValue(0.0f);
    _statusChar->writeValue(0);
    _gpsChar->writeValue((uint8_t*)"\0\0\0\0\0\0\0\0\0\0\0\0\0", 13);
    _magAngleChar->writeValue(0.0f);
    _spectrumChar->writeValue((uint8_t*)"\0\0\0\0\0\0\0\0", 8);
    
    // Start advertising
    BLE.advertise();
    
    _initialized = true;
    
    Serial.println("✓ PhiBLE initialized");
    Serial.print("   Device: ");
    Serial.println(deviceName);
    Serial.print("   Service: ");
    Serial.println(BLE_SERVICE_UUID);
    Serial.println("   Advertising...");
    
    return true;
}

void PhiBLE::updateSensors(
    float imuMag,
    float magField,
    int proximity,
    float pressure,
    uint16_t audioPeak,
    uint16_t emiPeak,
    float temperature,
    float humidity
) {
    if (!_initialized)
        return;
    
    // Update all characteristics
    _imuChar->writeValue(imuMag);
    _magChar->writeValue(magField);
    _proxChar->writeValue((uint8_t)constrain(proximity, 0, 255));
    _baroChar->writeValue(pressure);
    _audioChar->writeValue(audioPeak);
    _emiChar->writeValue(emiPeak);
    _tempChar->writeValue(temperature);
    _humChar->writeValue(humidity);
    
    _updateCount++;
}

void PhiBLE::updateGPS(float lat, float lon, float alt, bool fix) {
    if (!_initialized)
        return;
    
    uint8_t d[13];
    memcpy(d, &lat, 4);
    memcpy(d+4, &lon, 4);
    memcpy(d+8, &alt, 4);
    d[12] = fix ? 1 : 0;
    _gpsChar->writeValue(d, 13);
}

void PhiBLE::updateMagAngle(float angleDegrees) {
    if (!_initialized)
        return;
    
    _magAngleChar->writeValue(angleDegrees);
}

void PhiBLE::updateSpectrum(const uint8_t* spectrum8) {
    if (!_initialized)
        return;
    
    _spectrumChar->writeValue(spectrum8, 8);
}

void PhiBLE::setSensorStatus(uint8_t status) {
    if (!_initialized)
        return;
    
    _statusChar->writeValue(status);
}

bool PhiBLE::isConnected() const {
    if (!_initialized)
        return false;
    
    BLEDevice central = BLE.central();
    return central.connected();
}

const char* PhiBLE::getClientAddress() const {
    if (!_initialized || !isConnected())
        return "None";
    
    BLEDevice central = BLE.central();
    return central.address().c_str();
}

float PhiBLE::getSignalStrength() const {
    if (!_initialized || !isConnected())
        return 0.0f;
    
    BLEDevice central = BLE.central();
    int rssi = central.rssi();  // -100 to -30 dBm typical
    
    // Normalize: -100 dBm = 0.0, -30 dBm = 1.0
    float normalized = (rssi + 100.0f) / 70.0f;
    return constrain(normalized, 0.0f, 1.0f);
}