//
//  BluetoothManager.swift
//  Gran Fondo
//
//  Created by Willem L. Middelkoop on 27/04/2024.
//

import Foundation
import CoreBluetooth

extension CBUUID {
    static let heartRateServiceUUID = CBUUID(string: "180D")
    static let heartRateMeasurementCharacteristicUUID = CBUUID(string: "2A37")
    static let cyclingPowerServiceUUID = CBUUID(string: "1818")
    static let cyclingPowerMeasurementCharacteristicUUID = CBUUID(string: "2A63")
    static let cyclingSpeedCadenceServiceUUID = CBUUID(string: "1816")
    static let cyclingSpeedCadenceMeasurementCharacteristicUUID = CBUUID(string: "2A5B")
    static let cyclingSpeedCadenceFeatureCharacteristicUUID = CBUUID(string: "2A5C")
}

class BluetoothManager: NSObject, ObservableObject, CBCentralManagerDelegate, CBPeripheralDelegate {
    var centralManager: CBCentralManager?
    
    var cleanupTimer: Timer?
    private var sensorsUserDefaultsKey: String {
        "com.dsd164.granfondo.sensors"
    }
    @Published var sensors: [Sensor] = []
    private var detectedPeripherals: [DetectedPeripheral] = []
   
    @Published var heartRateMonitor: CBPeripheral?
    var powerMeter: CBPeripheral?
    var cadenceSensor: CBPeripheral?
    var speedSensor: CBPeripheral?
    var speedCadenceSensors: [CBPeripheral] = []
    
    // Flags to check if devices are connected
    @Published var isHeartRateMonitorConnected: Bool = false
    @Published var isPowerMeterConnected: Bool = false
    @Published var isCadenceSensorConnected: Bool = false
    @Published var isSpeedSensorConnected: Bool = false
    @Published var allSensorsConnected: Bool = false
    @Published var foundNewSensors: Bool = false
 
    @Published var isBluetoothRestricted: Bool = false
    
    private var lastCrankRevolutionData: RevolutionData?
    private var lastValidCadenceTime: Date?
    
    private var lastWheelRevolutionData: RevolutionData?
    private var lastValidWheelRpmTime: Date?
    
    @Published var currentHeartRate: Int = 0
    @Published var currentCyclingPower: Int = 0
    @Published var currentCyclingCadence: Int = 0
    @Published var currentCyclingWheelRpm: Int = 0
    
    override init() {
        super.init()
        loadSensors()
        
        cleanupTimer = Timer.scheduledTimer(withTimeInterval: 300, repeats: true) { [weak self] _ in
            self?.cleanupSensors()
        }
    }
    
    func startBluetooth(){
        centralManager = CBCentralManager(delegate: self, queue: nil, options: [CBCentralManagerOptionShowPowerAlertKey: true, CBCentralManagerOptionRestoreIdentifierKey: "granFondoBluetoothManager"])

    }
    
    func saveSensors() {
        let encoder = JSONEncoder()
        if let encoded = try? encoder.encode(sensors) {
            UserDefaults.standard.set(encoded, forKey: sensorsUserDefaultsKey)
        }
    }

    func loadSensors() {
        let decoder = JSONDecoder()
        if let data = UserDefaults.standard.data(forKey: sensorsUserDefaultsKey),
           let loadedSensors = try? decoder.decode([Sensor].self, from: data) {
            sensors = loadedSensors
        }
    }
    
    func toggleSensorSelection(for sensor: Sensor) {
        if let index = sensors.firstIndex(of: sensor) {
            sensors[index].isSelected.toggle()
            saveSensors()
            updateSensorConnectionStatus()
            
            if !sensors[index].isSelected { // unselected, see if we need to disconnect:
                let uuid = sensors[index].id
                
                if let heartRateMonitor = heartRateMonitor, uuid == heartRateMonitor.identifier {
                    print("BluetoothManager: Disconnecting from \(heartRateMonitor.name ?? "heart rate monitor") due to unselection.")
                    centralManager?.cancelPeripheralConnection(heartRateMonitor)
                }
                
                if let powerMeter = powerMeter, uuid == powerMeter.identifier {
                    print("BluetoothManager: Disconnecting from \(powerMeter.name ?? "power meter") due to unselection.")
                    centralManager?.cancelPeripheralConnection(powerMeter)
                }
                
                if let speedSensor = speedSensor, uuid == speedSensor.identifier {
                    print("BluetoothManager: Disconnecting from \(speedSensor.name ?? "speed sensor") due to unselection.")
                    centralManager?.cancelPeripheralConnection(speedSensor)
                }
                
                if let cadenceSensor = cadenceSensor, uuid == cadenceSensor.identifier {
                    print("BluetoothManager: Disconnecting from \(cadenceSensor.name ?? "cadence sensor") due to unselection.")
                    centralManager?.cancelPeripheralConnection(cadenceSensor)
                }
            }else{
                print("BluetoothManager: \(sensors[index].name) is now selected, must connect to it.")
                
                if let index = self.detectedPeripherals.firstIndex(where: { $0.id == sensors[index].id }) {
                    print("BluetoothManager: we detected \(self.detectedPeripherals[index].peripheral.name ?? "device" ), trying to connect to peripheral.")
                    
                    let detectedPeripheral = self.detectedPeripherals[index]
                 
                    manageConnectedPeripheral(detectedPeripheral.peripheral)
                    connectToPeripheral(peripheral: detectedPeripheral.peripheral, services: detectedPeripheral.services)
                }
                
                centralManager?.scanForPeripherals(withServices: [CBUUID.heartRateServiceUUID, CBUUID.cyclingPowerServiceUUID], options: [CBCentralManagerScanOptionAllowDuplicatesKey: NSNumber(value: false)])
            }
            
            
        }
    }

    func centralManagerDidUpdateState(_ central: CBCentralManager) {
        print("BluetoothManager: centralManagerDidUpdateState")
        switch central.state {
        case .poweredOn:
            print("BluetoothManager: Bluetooth is powered on")
          
            // Now handle any restored peripherals
            if let heartRateMonitor = heartRateMonitor, !isHeartRateMonitorConnected {
                print("BluetoothManager: centralManagerDidUpdateState Bluetooth is powered on, connecting to heartRateMonitor")
                central.connect(heartRateMonitor)
            }
            if let powerMeter = powerMeter, !isPowerMeterConnected {
                print("BluetoothManager: centralManagerDidUpdateState Bluetooth is powered on, connecting to powerMeter")
                central.connect(powerMeter)
            }
            if let cadenceSensor = cadenceSensor, !isCadenceSensorConnected {
                print("BluetoothManager: centralManagerDidUpdateState Bluetooth is powered on, connecting to cadenceSensor")
                central.connect(cadenceSensor)
            }
            if let speedSensor = speedSensor, !isSpeedSensorConnected {
                print("BluetoothManager: centralManagerDidUpdateState Bluetooth is powered on, connecting to speedSensor")
                central.connect(speedSensor)
            }
            
            if !isHeartRateMonitorConnected || !isPowerMeterConnected {
                scanForSensors()
            }
        case .poweredOff:
            print("BluetoothManager: Bluetooth is powered off")
        case .unauthorized:
            print("BluetoothManager: Bluetooth usage is unauthorized")
        default:
            print("BluetoothManager: Unhandled state \(central.state)")
        }
        
        switch CBCentralManager.authorization {
            case .allowedAlways:
                print("BluetoothManager: Bluetooth permission is granted.")
                self.isBluetoothRestricted = false;
            case .denied:
                print("BluetoothManager: Bluetooth permission is denied.")
                self.isBluetoothRestricted = true;
            case .restricted:
                print("BluetoothManager: Bluetooth usage is restricted.")
                self.isBluetoothRestricted = true;
            default:
                print("BluetoothManager: Bluetooth permission status: \(CBCentralManager.authorization)")
        }
    }
    
    func scanForSensors(){
        print("BluetoothManager: Start scanning for sensors")
        centralManager?.scanForPeripherals(withServices: [CBUUID.heartRateServiceUUID, CBUUID.cyclingPowerServiceUUID, CBUUID.cyclingSpeedCadenceServiceUUID], options: [CBCentralManagerScanOptionAllowDuplicatesKey: NSNumber(value: false)])
    }
    func stopScanningForSensors(){
        print("BluetoothManager: Stopped scanning for sensors")
        centralManager?.stopScan()
    }
    
    func centralManager(_ central: CBCentralManager, didDiscover peripheral: CBPeripheral, advertisementData: [String : Any], rssi RSSI: NSNumber) {
        print("BluetoothManager: centralManager didDiscover \(peripheral.name ?? "device")")
        guard let services = advertisementData[CBAdvertisementDataServiceUUIDsKey] as? [CBUUID] else {
            // print("- no services" )
            return
        }
        
        registerSensor(for: peripheral, services:services)
        
        let isWhitelisted = sensors.contains {
            $0.id == peripheral.identifier && $0.isSelected
        }
        
        if isWhitelisted {
           connectToPeripheral(peripheral: peripheral, services: services)
        }else{
            print("BluetoothManager: Peripheral \(peripheral.name ?? "Unknown") is not whitelisted. Skipping connection.")
        }
    }
    
    func connectToPeripheral(peripheral: CBPeripheral, services: [CBUUID] ){
        // Check if it's a heart rate monitor and we haven't connected to one yet
        if services.contains(CBUUID.heartRateServiceUUID){
            if !isHeartRateMonitorConnected {
                connectToHeartRateMonitor(peripheral)
            }else{
                if let heartRateMonitor = heartRateMonitor,
                   (getSensorPriority(peripheral.identifier) < getSensorPriority(heartRateMonitor.identifier)) {
                    
                    print("BluetoothManager: \(peripheral.name ?? "unkown") has higher priority than current connected heart rate monitor \(heartRateMonitor.name ?? "unkown")")
                    
                    centralManager?.cancelPeripheralConnection(heartRateMonitor);
                    connectToHeartRateMonitor(peripheral)
                }
            }
        }
        
        // Check if it's a power meter and we haven't connected to one yet
        if services.contains(CBUUID.cyclingPowerServiceUUID) {
            if !isPowerMeterConnected {
                connectToPowerMeter(peripheral)
            }else{
                if let powerMeter = powerMeter,
                   (getSensorPriority(peripheral.identifier) < getSensorPriority(powerMeter.identifier)){
                    print("BluetoothManager: \(peripheral.name ?? "unkown") has higher priority than current connected heart rate monitor \(powerMeter.name ?? "unkown")")
                    
                    centralManager?.cancelPeripheralConnection(powerMeter);
                    connectToPowerMeter(peripheral)
                }
            }
        }
        
        // Check if it's a speed/cadence sensor and we haven't connected to one yet
        if services.contains(CBUUID.cyclingSpeedCadenceServiceUUID){
            // we simply connect to all what is speed/cadence and whitelisted, and have the features
            // deal with the implementation of priority and sensor role
            connectToSpeedCadenceSensor(peripheral)
        }
    }
    
    func getSensorPriority(_ identifier: UUID) -> Int{
        var priority = 0;
        
        for (_, sensor) in sensors.enumerated() {
            if sensor.isSelected {
                priority+=1
            }
            
            if sensor.id == identifier {
                return priority
            }
        }
        
        return -1;
    }
    
    func registerSensor(for peripheral: CBPeripheral, services: [CBUUID]) {
        detectedPeripherals.append( DetectedPeripheral(id: peripheral.identifier, peripheral: peripheral, services: services))
        
        DispatchQueue.main.async {
            // Check for the sensor and update its last seen date within the same async block
            if let index = self.sensors.firstIndex(where: { $0.id == peripheral.identifier }) {
                self.sensors[index].lastSeen = Date()
            } else {
                // If the sensor is not found, create a new sensor and append it to the sensors array
                let newSensor = Sensor(id: peripheral.identifier, name: peripheral.name ?? "Unknown (\(peripheral.identifier))", isSelected: false, lastSeen: Date(), isConnected: false)
                self.sensors.append(newSensor)
            }
            // Save the sensors list after modification
            self.saveSensors()
            self.updateSensorConnectionStatus()
        }
    }
    
    func updateSensorConnectionStatus(for peripheral: CBPeripheral, isConnected: Bool){
        DispatchQueue.main.async {
            // Check for the sensor and update its last seen date within the same async block
            if let index = self.sensors.firstIndex(where: { $0.id == peripheral.identifier }) {
                self.sensors[index].isConnected = isConnected
            }
            
            // Save the sensors list after modification
            self.saveSensors()
            self.updateSensorConnectionStatus()
        }
    }
    
    func updateSensorConnectionStatus(){
        DispatchQueue.main.async {
            var allConnected = true
            var foundNewSensors = false
            var connectedCount = 0
            let now = Date()
            for (_, sensor) in self.sensors.enumerated() {
                if sensor.isConnected {
                    connectedCount = connectedCount+1
                }
                if !sensor.isConnected && sensor.isSelected {
                    allConnected = false
                    break;
                }
                if !sensor.isSelected && now.timeIntervalSince(sensor.lastSeen) < 300 {
                    foundNewSensors = true
                }
            }
            self.foundNewSensors = foundNewSensors
            self.allSensorsConnected = connectedCount > 0 && allConnected
        }
    }
    
    func cleanupSensors() {
        let timeoutInterval: TimeInterval = 300  // 5 minutes
        let now = Date()
        DispatchQueue.main.async {
            self.sensors.removeAll { sensor in
                // Remove sensors that are not selected and have not been seen for more than the timeout interval
                return !sensor.isSelected && now.timeIntervalSince(sensor.lastSeen) > timeoutInterval
            }
            
            
            self.saveSensors()
        }
    }
    
    private func connectToHeartRateMonitor(_ peripheral: CBPeripheral) {
        print("BluetoothManager: connectToHeartRateMonitor called")
        heartRateMonitor = peripheral
        heartRateMonitor?.delegate = self
        centralManager?.connect(heartRateMonitor!)
        isHeartRateMonitorConnected = true
    }
    
    private func connectToPowerMeter(_ peripheral: CBPeripheral) {
        print("BluetoothManager: connectToPowerMeter called")
        powerMeter = peripheral
        powerMeter?.delegate = self
        centralManager?.connect(powerMeter!)
        isPowerMeterConnected = true
        
        // we assume that the power sensor will deliver us cadence info, too:
        if cadenceSensor == nil {
            isCadenceSensorConnected = true
            cadenceSensor = powerMeter;
        }
    }
    private func connectToSpeedCadenceSensor(_ peripheral: CBPeripheral) {
        print("BluetoothManager: connectToSpeedCadenceSensor called")
        
        peripheral.delegate = self
        speedCadenceSensors.append(peripheral);
        centralManager?.connect(peripheral)
    }
    
    func centralManager(_ central: CBCentralManager, didConnect peripheral: CBPeripheral) {
        print("BluetoothManager: centralManager didConnect to \(peripheral.name ?? "device")")
        // peripheral.discoverServices([CBUUID.heartRateServiceUUID, CBUUID.cyclingPowerServiceUUID])
        manageConnectedPeripheral(peripheral)
        updateSensorConnectionStatus(for: peripheral, isConnected: true)
    }
    
    func centralManager(_ central: CBCentralManager, willRestoreState dict: [String: Any]) {
        print("BluetoothManager: centralManager willRestoreState")
        if let restoredPeripherals = dict[CBCentralManagerRestoredStatePeripheralsKey] as? [CBPeripheral] {
            for peripheral in restoredPeripherals {
                peripheral.delegate = self
                // Store peripherals for later use but do not connect or discover services yet
                if peripheral.services?.contains(where: { $0.uuid == CBUUID.heartRateServiceUUID }) == true {
                    heartRateMonitor = peripheral
                    isHeartRateMonitorConnected = (central.state == .poweredOn && peripheral.state == .connected)
                    // print("BluetoothManager: centralManager heartRateMonitor, connected \(isHeartRateMonitorConnected)")
                } else if peripheral.services?.contains(where: { $0.uuid == CBUUID.cyclingPowerServiceUUID }) == true {
                    powerMeter = peripheral
                    isPowerMeterConnected = (central.state == .poweredOn && peripheral.state == .connected)
                    // print("BluetoothManager: centralManager powerMeter, isPowerMeterConnected \(isPowerMeterConnected)")
                }else if peripheral.services?.contains(where: { $0.uuid == CBUUID.cyclingSpeedCadenceServiceUUID }) == true {
                    speedCadenceSensors.append(peripheral);
                }
                
                updateSensorConnectionStatus(for: peripheral, isConnected: (central.state == .poweredOn && peripheral.state == .connected))
            }
        }
    }
   
    func centralManager(_ central: CBCentralManager, didDisconnectPeripheral peripheral: CBPeripheral, error: Error?) {
         print("BluetoothManager: centralManager didDisconnectPeripheral  \(peripheral.name ?? "device")")
        
        // mark it as disconnected:
        updateSensorConnectionStatus(for: peripheral, isConnected: false)
        
        // Check which device was disconnected and update the status
        if peripheral == heartRateMonitor {
            isHeartRateMonitorConnected = false
            heartRateMonitor = nil
            currentHeartRate = 0;
        } else if peripheral == powerMeter {
            isPowerMeterConnected = false
            
            powerMeter = nil
            currentCyclingPower = 0
        }
        if peripheral == cadenceSensor {
            isCadenceSensorConnected = isPowerMeterConnected
            cadenceSensor = nil
            currentCyclingCadence = 0
            lastCrankRevolutionData = nil
        }
        if peripheral == speedSensor {
            isSpeedSensorConnected = false
            speedSensor = nil
            currentCyclingWheelRpm = 0;
            lastWheelRevolutionData = nil
        }
        
        if speedCadenceSensors.contains(peripheral){
            speedCadenceSensors.removeAll { $0 == peripheral }
        }

        // Resume scanning if any device is not connected and Bluetooth is powered on
            if (!isHeartRateMonitorConnected || !isPowerMeterConnected || !isCadenceSensorConnected || !isSpeedSensorConnected) && central.state == .poweredOn {
                scanForSensors()
            } else {
                // print("BluetoothManager: Bluetooth is not powered on or both devices are connected.")
            }
    }
    
    func manageConnectedPeripheral(_ peripheral: CBPeripheral) {
        print("BluetoothManager: manageConnectedPeripheral \(peripheral.name ?? "device")")
        // Assign the peripheral delegate
        peripheral.delegate = self
        
        // Discover specific services again, only if needed
        if peripheral.services == nil {
            print("BluetoothManager: manageConnectedPeripheral \(peripheral.name ?? "device") discovering services now")
            peripheral.discoverServices([CBUUID.heartRateServiceUUID, CBUUID.cyclingPowerServiceUUID, CBUUID.cyclingSpeedCadenceServiceUUID])
        } else {
            print("BluetoothManager: manageConnectedPeripheral \(peripheral.name ?? "device") services already discovered")
            // If services are already discovered, directly discover characteristics
            for service in peripheral.services! {
                if service.uuid == CBUUID.heartRateServiceUUID {
                    if service.characteristics == nil {
                        print("BluetoothManager: manageConnectedPeripheral \(peripheral.name ?? "device") characteristics nil, discovering now")
                        peripheral.discoverCharacteristics([CBUUID.heartRateMeasurementCharacteristicUUID], for: service)
                    } else {
                        print("BluetoothManager: manageConnectedPeripheral \(peripheral.name ?? "device") characteristics already known, manageCharacteristics now")
                        // Ensure the notification is setup correctly
                        manageCharacteristics(service.characteristics!, for: peripheral)
                    }
                }
                
                if service.uuid == CBUUID.cyclingPowerServiceUUID {
                    if service.characteristics == nil {
                        peripheral.discoverCharacteristics([CBUUID.cyclingPowerMeasurementCharacteristicUUID], for: service)
                    } else {
                        // Ensure the notification is setup correctly
                        manageCharacteristics(service.characteristics!, for: peripheral)
                    }
                }
                
                if service.uuid == CBUUID.cyclingSpeedCadenceServiceUUID {
                    if service.characteristics == nil {
                        peripheral.discoverCharacteristics([CBUUID.cyclingPowerMeasurementCharacteristicUUID, CBUUID.cyclingSpeedCadenceFeatureCharacteristicUUID], for: service)
                    } else {
                        // Ensure the notification is setup correctly
                        manageCharacteristics(service.characteristics!, for: peripheral)
                    }
                }
                
            }
        }
    }
    
    func peripheral(_ peripheral: CBPeripheral, didDiscoverServices error: Error?) {
        print("BluetoothManager: peripheral \(peripheral.name ?? "device") didDiscoverServices")
        guard let services = peripheral.services else {
            print("BluetoothManager: - services nil")
            return
        }
        for service in services {
            print("BluetoothManager: - service \(service.uuid) ")
            
            if service.uuid == CBUUID.heartRateServiceUUID {
                print("BluetoothManager: - peripheral didDiscoverServices heartRateServiceUUID")
                peripheral.discoverCharacteristics([CBUUID.heartRateMeasurementCharacteristicUUID], for: service)
            }
            if service.uuid == CBUUID.cyclingPowerServiceUUID {
                print("BluetoothManager: - peripheral didDiscoverServices cyclingPowerServiceUUID")
                peripheral.discoverCharacteristics([CBUUID.cyclingPowerMeasurementCharacteristicUUID], for: service)
            }
            if service.uuid == CBUUID.cyclingSpeedCadenceServiceUUID {
                print("BluetoothManager: - peripheral didDiscoverServices cyclingPowerServiceUUID")
                peripheral.discoverCharacteristics([CBUUID.cyclingSpeedCadenceMeasurementCharacteristicUUID, CBUUID.cyclingSpeedCadenceFeatureCharacteristicUUID], for: service)
            }
        }
    }

    func peripheral(_ peripheral: CBPeripheral, didDiscoverCharacteristicsFor service: CBService, error: Error?) {
        if let characteristics = service.characteristics, !characteristics.isEmpty {
            manageCharacteristics(characteristics, for: peripheral)
        } else {
            print("BluetoothManager: No characteristics or empty characteristics found, retrying...")
            centralManager?.cancelPeripheralConnection(peripheral)
        }
    }
    
    func manageCharacteristics(_ characteristics: [CBCharacteristic], for peripheral: CBPeripheral) {
        print("BluetoothManager: manageCharacteristics \(peripheral.name ?? "device")")
        print(characteristics)
        
        for characteristic in characteristics {
            
            if characteristic.uuid == CBUUID.heartRateMeasurementCharacteristicUUID {
                // Check if notifications are enabled; if not, enable them
                if !characteristic.isNotifying {
                    peripheral.setNotifyValue(true, for: characteristic)
                }
            }
            
            if characteristic.uuid == CBUUID.cyclingPowerMeasurementCharacteristicUUID {
                // Check if notifications are enabled; if not, enable them
                if !characteristic.isNotifying {
                    peripheral.setNotifyValue(true, for: characteristic)
                }
            }
            
            if characteristic.uuid == CBUUID.cyclingSpeedCadenceFeatureCharacteristicUUID {
                print("BluetoothManager: got ourselves cyclingSpeedCadenceFeatureCharacteristicUUID on \(peripheral.name ?? "device")")
                print(characteristic)
                peripheral.readValue(for: characteristic)
            }
            
            if characteristic.uuid == CBUUID.cyclingSpeedCadenceMeasurementCharacteristicUUID {
                if !characteristic.isNotifying {
                    peripheral.setNotifyValue(true, for: characteristic)
                }
            }
        }
    }
    
    func peripheral(_ peripheral: CBPeripheral, didUpdateValueFor characteristic: CBCharacteristic, error: Error?) {
        // print("BluetoothManager: peripheral \(peripheral.name ?? "device" ) didUpdateValueFor ")
        if let error = error {
            print("BluetoothManager: Error updating value for characteristic: \(error.localizedDescription)")
            return
        }
       
        switch characteristic.uuid {
            case CBUUID.heartRateMeasurementCharacteristicUUID:
                if let data = characteristic.value {
                    currentHeartRate = decodeHeartRate(from: data)
                }
            case CBUUID.cyclingPowerMeasurementCharacteristicUUID:
                if let data = characteristic.value {
                    // print("BluetoothManager: Received didUpdateValueFor (power)")
                    if let measurement = decodeCyclingPowerMeasurement(data: data){
                        currentCyclingPower = measurement.instantaneousPower
                        // printCyclingPowerMeasurement(measurement)
                        
                        if self.cadenceSensor == peripheral {
                            if let cadence = calculateCyclingCadenceFromPowerMeasurement(currentMeasurement: measurement) {
                                currentCyclingCadence = cadence
                                lastValidCadenceTime = Date()
                            }else {
                                if let lastTime = lastValidCadenceTime {
                                    if Date().timeIntervalSince(lastTime) > 3 {  // More than 5 seconds since the last valid cadence
                                        currentCyclingCadence = 0
                                        print("BluetoothManager: Cadence data timeout, setting cadence to 0.")
                                    }
                                }
                            }
                        }
                    }
                }
            case CBUUID.cyclingSpeedCadenceFeatureCharacteristicUUID:
                if let data = characteristic.value{
                    print("BluetoothManager: Received didUpdateValueFor (speed/cadence FEATURE) from \(peripheral.name ?? "device") ")
                    // TODO: implement
                    let bytes = [UInt8](data)
                    let wheelRevolutionDataSupported = (bytes[0] & 0x01) == 0x01
                    let crankRevolutionDataSupported = (bytes[0] & 0x02) == 0x02
                     
                    determineRoleOfSpeedCadencePeripheral(peripheral, wheelRevolutionDataSupported: wheelRevolutionDataSupported, crankRevolutionDataSupported: crankRevolutionDataSupported)
                    /*
                    // Speed (Wheel RPM) sensor
                    if(wheelRevolutionDataSupported && !isSpeedSensorConnected){
                        print("BluetoothManager: Now using \(peripheral.name ?? "device") for speed (wheel RPM)")
                        speedSensor = peripheral;
                    }else{
                        if let existingSpeedSensor = speedSensor,
                           (getSensorPriority(peripheral.identifier) < getSensorPriority(existingSpeedSensor.identifier)){
                            print("BluetoothManager: \(peripheral.name ?? "unkown") has higher priority than current connected speed sensor \(existingSpeedSensor.name ?? "unkown")")
                            
                            self.speedSensor = peripheral
                            isSpeedSensorConnected = true;
                        }
                    }
                   
                    // Cadence sensor
                    if(crankRevolutionDataSupported && !isCadenceSensorConnected){
                        print("BluetoothManager: Now using \(peripheral.name ?? "device") for cadence")
                        self.cadenceSensor = peripheral;
                        isCadenceSensorConnected = true;
                    }else{
                        if let existingCadenceSensor = cadenceSensor,
                           (getSensorPriority(peripheral.identifier) < getSensorPriority(existingCadenceSensor.identifier)){
                            print("BluetoothManager: \(peripheral.name ?? "unkown") has higher priority than current connected cadence sensor \(existingCadenceSensor.name ?? "unkown")")
                            
                            self.cadenceSensor = peripheral
                            isCadenceSensorConnected = true;
                        }
                    }*/
                }
            case CBUUID.cyclingSpeedCadenceMeasurementCharacteristicUUID:
                if let data = characteristic.value{
                    print("BluetoothManager: Received didUpdateValueFor (speed/cadence MEASUREMENT) from \(peripheral.name ?? "device") ")
                    
                    if let measurement = decodeCyclingSpeedCadenceMeasurement(data: data){
                        print("BluetoothManager: got a measurement value ")
                        print(measurement)
                        
                        let wheelRevolutionDataSupported = (measurement.cumulativeWheelRevs != nil)
                        let crankRevolutionDataSupported = (measurement.cumulativeCrankRevs != nil)
                         
                        determineRoleOfSpeedCadencePeripheral(peripheral, wheelRevolutionDataSupported: wheelRevolutionDataSupported, crankRevolutionDataSupported: crankRevolutionDataSupported)
                        
                        if self.cadenceSensor == peripheral {
                            if let cadence = calculateCyclingCadenceFromSpeedCadenceMeasurement(currentMeasurement: measurement) {
                                currentCyclingCadence = cadence
                                lastValidCadenceTime = Date()
                            }else {
                                if let lastTime = lastValidCadenceTime {
                                    if Date().timeIntervalSince(lastTime) > 3 {  // More than 5 seconds since the last valid cadence
                                        currentCyclingCadence = 0
                                        print("BluetoothManager: Cadence data timeout, setting cadence to 0.")
                                    }
                                }
                            }
                        }else{
                            print("BluetoothManager: got a !cadenceSensor value ")
                        }
                        
                        if self.speedSensor == peripheral {
                            if let wheelRpm = calculateCyclingSpeedFromSpeedCadenceMeasurement(currentMeasurement: measurement) {
                                currentCyclingWheelRpm = wheelRpm
                                lastValidWheelRpmTime = Date()
                            }else {
                                if let lastTime = lastValidWheelRpmTime {
                                    if Date().timeIntervalSince(lastTime) > 3 {  // More than 5 seconds since the last valid speed
                                        currentCyclingWheelRpm = 0
                                        print("BluetoothManager: Speed data timeout, setting currentCyclingWheelRpm to 0.")
                                    }
                                }
                            }
                        }
                    }
                }
            
            default:
                break
            }
    }
    
    func determineRoleOfSpeedCadencePeripheral(_ peripheral: CBPeripheral, wheelRevolutionDataSupported: Bool, crankRevolutionDataSupported: Bool ){
        
        // Speed (Wheel RPM) sensor
        if speedSensor != peripheral{
            
            if(wheelRevolutionDataSupported && !isSpeedSensorConnected){
                print("BluetoothManager: Now using \(peripheral.name ?? "device") for speed (wheel RPM)")
                speedSensor = peripheral;
                isSpeedSensorConnected = true;
            }else{
                if let existingSpeedSensor = speedSensor,
                   (getSensorPriority(peripheral.identifier) < getSensorPriority(existingSpeedSensor.identifier)){
                    print("BluetoothManager: \(peripheral.name ?? "unkown") has higher priority than current connected speed sensor \(existingSpeedSensor.name ?? "unkown")")
                    
                    speedSensor = peripheral
                    isSpeedSensorConnected = true;
                }
            }
            
        }
        
        // Cadence sensor
        if cadenceSensor != peripheral{
            if(crankRevolutionDataSupported && !isCadenceSensorConnected){
                print("BluetoothManager: Now using \(peripheral.name ?? "device") for cadence")
                self.cadenceSensor = peripheral;
                isCadenceSensorConnected = true;
            }else{
                if let existingCadenceSensor = cadenceSensor,
                   (getSensorPriority(peripheral.identifier) < getSensorPriority(existingCadenceSensor.identifier)){
                    print("BluetoothManager: \(peripheral.name ?? "unkown") has higher priority than current connected cadence sensor \(existingCadenceSensor.name ?? "unkown")")
                    
                    self.cadenceSensor = peripheral
                    isCadenceSensorConnected = true;
                }
            }
        }
    }
    
    func decodeHeartRate(from data: Data) -> Int {
        var buffer = [UInt8](repeating: 0, count: data.count)
        data.copyBytes(to: &buffer, count: data.count)
        
        // The first byte in the data contains flags that tell us how to process the rest of the data.
        let flags = buffer[0]
        let heartRateValueFormat = (flags & 0x01) == 0 // 0x01 = 1 means 16-bit format, 0 means 8-bit format
        
        if heartRateValueFormat {
            // If the heart rate value format bit is 0, heart rate value format is in an 8-bit format.
            return Int(buffer[1]) // The second byte contains the heart rate measurement
        } else {
            // If the heart rate value format bit is 1, heart rate value format is in a 16-bit format.
            // Heart rate value is contained in the second and third bytes.
            return Int(buffer[1]) | (Int(buffer[2]) << 8) // Combine bytes for 16-bit value
        }
    }
    
    func decodeCyclingSpeedCadenceMeasurement(data: Data) -> CyclingSpeedCadenceMeasurement? {
        var byteOffset = 0
        
        guard data.count >= 2 else { return nil }
        let flags = UInt8(littleEndian: data.subdata(in: byteOffset..<byteOffset+1).withUnsafeBytes { $0.load(as: UInt8.self) })
        byteOffset += 1
        print("BluetoothManager: - flags value \(flags)")
        
        var measurement = CyclingSpeedCadenceMeasurement()
        
        if (flags & 0x01) > 0 { // Wheel Revolution Data Present
            print("BluetoothManager: - flag 0x01 wheel revolution data present")
            if data.count > byteOffset + 5 {
                //guard data.count > byteOffset + 5 else { return measurement }
                measurement.cumulativeWheelRevs = UInt32(littleEndian: data.subdata(in: byteOffset..<byteOffset+4).withUnsafeBytes { $0.load(as: UInt32.self) })
                byteOffset += 4
                measurement.lastWheelEventTime = UInt16(littleEndian: data.subdata(in: byteOffset..<byteOffset+2).withUnsafeBytes { $0.load(as: UInt16.self) })
                byteOffset += 2
            }
        }
        if (flags & 0x02) > 0 { // Crank Revolution Data Present
            print("BluetoothManager: - flag 0x02 crank revolution data present")
            if data.count > byteOffset + 3 {
                //guard data.count > byteOffset + 3 else { return measurement }
                measurement.cumulativeCrankRevs = UInt16(littleEndian: data.subdata(in: byteOffset..<byteOffset+2).withUnsafeBytes { $0.load(as: UInt16.self) })
                byteOffset += 2
                measurement.lastCrankEventTime = UInt16(littleEndian: data.subdata(in: byteOffset..<byteOffset+2).withUnsafeBytes { $0.load(as: UInt16.self) })
                byteOffset += 2
            }
        }
        return measurement;
    }
    
    func decodeCyclingPowerMeasurement(data: Data) -> CyclingPowerMeasurement? {
        var byteOffset = 0
        
        guard data.count >= 2 else { return nil }
        let flags = UInt16(littleEndian: data.subdata(in: byteOffset..<byteOffset+2).withUnsafeBytes { $0.load(as: UInt16.self) })
        byteOffset += 2
        // print("BluetoothManager: - flags value \(flags)")

        guard data.count > byteOffset + 1 else { return nil }
        let instantaneousPower = Int(littleEndian: data.subdata(in: byteOffset..<byteOffset+2).withUnsafeBytes { Int($0.load(as: Int16.self)) })
        byteOffset += 2

        var measurement = CyclingPowerMeasurement(instantaneousPower: instantaneousPower)

        // Decode additional fields based on flags
        if (flags & 0x01) > 0 { // Pedal Power Balance Present
            // print("BluetoothManager: - flag 0x01 pedal power balance present")
            
            if data.count > byteOffset {
            //guard data.count > byteOffset else { return measurement }
                measurement.pedalPowerBalance = Int(data[byteOffset])
                byteOffset += 1
            }
        }
        if (flags & 0x02) > 0 { // Accumulated Torque Present
            // print("BluetoothManager: - flag 0x02 accumulated torque present")
            
            if data.count > byteOffset + 1 {
             //   guard data.count > byteOffset + 1 else { return measurement }
                measurement.accumulatedTorque = Int(littleEndian: data.subdata(in: byteOffset..<byteOffset+2).withUnsafeBytes { Int($0.load(as: UInt16.self)) })
                byteOffset += 2
            }
        }
        if (flags & 0x04) > 0 { // Wheel Revolution Data Present
            // print("BluetoothManager: - flag 0x04 wheel revolution data present")
            if data.count > byteOffset + 5 {
                //guard data.count > byteOffset + 5 else { return measurement }
                measurement.cumulativeWheelRevs = UInt32(littleEndian: data.subdata(in: byteOffset..<byteOffset+4).withUnsafeBytes { $0.load(as: UInt32.self) })
                byteOffset += 4
                measurement.lastWheelEventTime = UInt16(littleEndian: data.subdata(in: byteOffset..<byteOffset+2).withUnsafeBytes { $0.load(as: UInt16.self) })
                byteOffset += 2
            }
        }
        if (flags & 0x08) > 0 { // Crank Revolution Data Present
            // print("BluetoothManager: - flag 0x08 crank revolution data present")
            if data.count > byteOffset + 3 {
                //guard data.count > byteOffset + 3 else { return measurement }
                measurement.cumulativeCrankRevs = UInt16(littleEndian: data.subdata(in: byteOffset..<byteOffset+2).withUnsafeBytes { $0.load(as: UInt16.self) })
                byteOffset += 2
                measurement.lastCrankEventTime = UInt16(littleEndian: data.subdata(in: byteOffset..<byteOffset+2).withUnsafeBytes { $0.load(as: UInt16.self) })
                byteOffset += 2
            }
        }
        if (flags & 0x10) > 0 { // Extreme Magnitudes Present
            // print("BluetoothManager: - flag 0x10 Extreme Magnitudes Present data present")
            if data.count > byteOffset + 3 {
                //guard data.count > byteOffset + 3 else { return measurement }
                measurement.maximumForceMagnitude = Int(littleEndian: data.subdata(in: byteOffset..<byteOffset+2).withUnsafeBytes { Int($0.load(as: Int16.self)) })
                byteOffset += 2
                measurement.minimumForceMagnitude = Int(littleEndian: data.subdata(in: byteOffset..<byteOffset+2).withUnsafeBytes { Int($0.load(as: Int16.self)) })
                byteOffset += 2
            }
        }
        if (flags & 0x20) > 0 { // Extreme Angles Present
            // print("BluetoothManager: - flag 0x20 Extreme Magnitudes Angles present")
            if data.count > byteOffset + 2{
                //guard data.count > byteOffset + 2 else { return measurement }
                measurement.topDeadSpotAngle = UInt16(littleEndian: data.subdata(in: byteOffset..<byteOffset+2).withUnsafeBytes { $0.load(as: UInt16.self) })
                byteOffset += 2
                measurement.bottomDeadSpotAngle = UInt16(littleEndian: data.subdata(in: byteOffset..<byteOffset+2).withUnsafeBytes { $0.load(as: UInt16.self) })
                byteOffset += 2
            }
        }
        if (flags & 0x40) > 0 { // Accumulated Energy Present
            // print("BluetoothManager: - flag 0x40 Extreme Magnitudes Angles present")
            if data.count > byteOffset + 1{
                //guard data.count > byteOffset + 1 else { return measurement }
                measurement.accumulatedEnergy = Int(littleEndian: data.subdata(in: byteOffset..<byteOffset+2).withUnsafeBytes { Int($0.load(as: UInt16.self)) })
            }
        }
       
        return measurement
    }
    
    func calculateCyclingCadenceFromSpeedCadenceMeasurement(currentMeasurement: CyclingSpeedCadenceMeasurement) -> Int? {
        guard let currentRevs = currentMeasurement.cumulativeCrankRevs, let currentTime = currentMeasurement.lastCrankEventTime else {
            print("BluetoothManager: Current measurement data incomplete.")
            print(currentMeasurement)
            return nil
        }
        return calculateCyclingCadence(currentRevs: currentRevs, currentTime: currentTime)
    }
    
    func calculateCyclingSpeedFromSpeedCadenceMeasurement(currentMeasurement: CyclingSpeedCadenceMeasurement) -> Int? {
        guard let currentRevs = currentMeasurement.cumulativeWheelRevs, let currentTime = currentMeasurement.lastWheelEventTime else {
            print("BluetoothManager: Current measurement data incomplete.")
            return nil
        }
        return calculateCyclingWheelRpm(currentRevs: currentRevs, currentTime: currentTime)
    }
    func calculateCyclingWheelRpm(currentRevs: UInt32, currentTime: UInt16) -> Int? {
        print("BluetoothManager: calculateCyclingWheelRpm ")
        if let lastData = lastWheelRevolutionData {
            // Calculate the differences, taking into account the possible wrap-around of time
            let timeDifference = (Int(currentTime) >= Int(lastData.time)) ?
            Int(currentTime - lastData.time) :
            (65536 + Int(currentTime) - Int(lastData.time))  // 65536 = 2^16, handle wrap-around
            
            let revDifference = Int(currentRevs) - Int(lastData.revolutions)
            
            // Update the last data for the next calculation
            lastWheelRevolutionData = RevolutionData(revolutions: Int(currentRevs), time: currentTime)
            
            // Ensure time difference is positive and not zero to avoid division by zero
            if timeDifference > 0 && revDifference > 0 {
                // Calculate cadence in revolutions per minute (RPM)
                let cadence = (Double(revDifference) / Double(timeDifference)) * (1024 * 60)
                return Int(cadence)
            }
        } else {
            // Initialize the last data if not already set
            lastWheelRevolutionData = RevolutionData(revolutions: Int(currentRevs), time: currentTime)
        }
        
        // Return nil if cadence cannot be calculated due to insufficient data
        return nil
    }

    func calculateCyclingCadenceFromPowerMeasurement(currentMeasurement: CyclingPowerMeasurement) -> Int? {
        guard let currentRevs = currentMeasurement.cumulativeCrankRevs, let currentTime = currentMeasurement.lastCrankEventTime else {
            print("BluetoothManager: Current measurement data incomplete.")
            return nil
        }
        return calculateCyclingCadence(currentRevs: currentRevs, currentTime: currentTime)
    }

    func calculateCyclingCadence(currentRevs: UInt16, currentTime: UInt16) -> Int? {
        if let lastData = lastCrankRevolutionData {
            // Calculate the differences, taking into account the possible wrap-around of time
            let timeDifference = (Int(currentTime) >= Int(lastData.time)) ?
            Int(currentTime - lastData.time) :
            (65536 + Int(currentTime) - Int(lastData.time))  // 65536 = 2^16, handle wrap-around
            
            let revDifference = Int(currentRevs) - Int(lastData.revolutions)
            
            // Update the last data for the next calculation
            lastCrankRevolutionData = RevolutionData(revolutions: Int(currentRevs), time: currentTime)
            
            print("BluetoothManager: calculateCyclingCadence: currentRevs \(currentRevs)")
            print("BluetoothManager: calculateCyclingCadence: currentRevs \(currentTime)")
            
            print("BluetoothManager: calculateCyclingCadence: timeDifference \(timeDifference)")
            print("BluetoothManager: calculateCyclingCadence: revDifference \(revDifference)")
            // Ensure time difference is positive and not zero to avoid division by zero
            if timeDifference > 0 && revDifference > 0 {
                // Calculate cadence in revolutions per minute (RPM)
                let cadence = (Double(revDifference) / Double(timeDifference)) * (1024 * 60)
                return Int(cadence)
            }
        } else {
            // Initialize the last data if not already set
            lastCrankRevolutionData = RevolutionData(revolutions: Int(currentRevs), time: currentTime)
        }
        print("BluetoothManager: Unable to calculate cadence data")
        // Return nil if cadence cannot be calculated due to insufficient data
        return nil
    }

    struct RevolutionData {
        var revolutions: Int
        var time: UInt16
    }
    
    func printCyclingPowerMeasurement(_ measurement: CyclingPowerMeasurement) {
        print("""
        Cycling Power Measurement:
        Instantaneous Power: \(measurement.instantaneousPower) Watts
        Accumulated Energy: \(measurement.accumulatedEnergy.map { "\($0) kJ" } ?? "N/A")
        Pedal Power Balance: \(measurement.pedalPowerBalance.map { "\($0)%" } ?? "N/A")
        Accumulated Torque: \(measurement.accumulatedTorque.map { "\($0) Nm" } ?? "N/A")
        Cumulative Wheel Revolutions: \(measurement.cumulativeWheelRevs.map { "\($0) revs" } ?? "N/A")
        Last Wheel Event Time: \(measurement.lastWheelEventTime.map { "\($0) ms" } ?? "N/A")
        Cumulative Crank Revolutions: \(measurement.cumulativeCrankRevs.map { "\($0) revs" } ?? "N/A")
        Last Crank Event Time: \(measurement.lastCrankEventTime.map { "\($0) ms" } ?? "N/A")
        Maximum Force Magnitude: \(measurement.maximumForceMagnitude.map { "\($0) Newtons" } ?? "N/A")
        Minimum Force Magnitude: \(measurement.minimumForceMagnitude.map { "\($0) Newtons" } ?? "N/A")
        Maximum Torque Magnitude: \(measurement.maximumTorqueMagnitude.map { "\($0) Nm" } ?? "N/A")
        Minimum Torque Magnitude: \(measurement.minimumTorqueMagnitude.map { "\($0) Nm" } ?? "N/A")
        Top Dead Spot Angle: \(measurement.topDeadSpotAngle.map { "\($0) degrees" } ?? "N/A")
        Bottom Dead Spot Angle: \(measurement.bottomDeadSpotAngle.map { "\($0) degrees" } ?? "N/A")
        """)
    }

    // Define the structure to hold the cycling power measurement
    struct CyclingPowerMeasurement {
        var instantaneousPower: Int
        var accumulatedEnergy: Int?
        var pedalPowerBalance: Int?
        var accumulatedTorque: Int?
        var cumulativeWheelRevs: UInt32?
        var lastWheelEventTime: UInt16?
        var cumulativeCrankRevs: UInt16?
        var lastCrankEventTime: UInt16?
        var maximumForceMagnitude: Int?
        var minimumForceMagnitude: Int?
        var maximumTorqueMagnitude: Int?
        var minimumTorqueMagnitude: Int?
        var topDeadSpotAngle: UInt16?
        var bottomDeadSpotAngle: UInt16?
    }
    
    struct CyclingSpeedCadenceMeasurement {
        var cumulativeWheelRevs: UInt32?
        var lastWheelEventTime: UInt16?
        var cumulativeCrankRevs: UInt16?
        var lastCrankEventTime: UInt16?
    }
}

struct Sensor: Identifiable, Equatable, Codable {
    let id: UUID
    var name: String
    var isSelected: Bool
    var lastSeen: Date
    var isConnected: Bool
}

struct DetectedPeripheral{
    let id: UUID
    var peripheral: CBPeripheral
    var services: [CBUUID]
}