saltylab-ios/SulTee/SulTee/SensorManager.swift

import Foundation
import CoreLocation
import CoreMotion
import Combine

/// Manages all iPhone sensors and forwards data to the WebSocket client
/// and MQTT broker (topic: saltybot/ios/gps, 1 Hz).
final class SensorManager: NSObject, ObservableObject {

    // MARK: - Published state for UI

    @Published var isStreaming = false
    @Published var wsState: WebSocketClient.ConnectionState = .disconnected
    @Published var gpsRate: Double = 0
    @Published var imuRate: Double = 0
    @Published var headingRate: Double = 0
    @Published var baroRate: Double = 0
    @Published var botDistanceMeters: Double? = nil

    // MARK: - WebSocket

    static let defaultOrinURL = "ws://100.64.0.2:9090"
    private static let orinURLKey = "orinURL"

    private(set) var ws: WebSocketClient

    /// Current Orin WebSocket URL string (persisted in UserDefaults).
    var orinURLString: String {
        get { UserDefaults.standard.string(forKey: Self.orinURLKey) ?? Self.defaultOrinURL }
        set { UserDefaults.standard.set(newValue, forKey: Self.orinURLKey) }
    }

    // MARK: - MQTT

    private let mqtt = MQTTClient(config: .init(
        host: "192.168.87.29",
        port: 1883,
        username: "mqtt_seb",
        password: "mqtt_pass",
        clientID: "sultee-ios-\(UUID().uuidString.prefix(8))"
    ))
    private static let mqttGPSTopic = "saltybot/ios/gps"
    private var lastKnownLocation: CLLocation?
    private var mqttPublishTimer: Timer?

    // MARK: - Sensors

    private let locationManager = CLLocationManager()
    private let motionManager = CMMotionManager()
    private let altimeter = CMAltimeter()
    private var cancellables = Set<AnyCancellable>()

    // MARK: - Rate counters (counts per second)

    private var gpsCounts: [Date] = []
    private var imuCounts: [Date] = []
    private var headingCounts: [Date] = []
    private var baroCounts: [Date] = []
    private var rateTimer: Timer?

    // MARK: - Init

    override init() {
        let urlString = UserDefaults.standard.string(forKey: Self.orinURLKey) ?? Self.defaultOrinURL
        self.ws = WebSocketClient(url: URL(string: urlString) ?? URL(string: Self.defaultOrinURL)!)
        super.init()
        locationManager.delegate = self
        locationManager.desiredAccuracy = kCLLocationAccuracyBestForNavigation
        // Use dual-frequency GPS (L1+L5) on iPhone 15 Pro — automatic when accuracy is set to Best
        locationManager.distanceFilter = kCLDistanceFilterNone
        locationManager.allowsBackgroundLocationUpdates = true
        locationManager.pausesLocationUpdatesAutomatically = false
        locationManager.showsBackgroundLocationIndicator = true

        ws.$state
            .receive(on: DispatchQueue.main)
            .assign(to: \.wsState, on: self)
            .store(in: &cancellables)
    }

    // MARK: - Public control

    func startStreaming() {
        guard !isStreaming else { return }
        isStreaming = true
        ws.connect()
        mqtt.connect()
        requestPermissionsAndStartSensors()
        startRateTimer()
        startMQTTPublishTimer()
    }

    func stopStreaming() {
        guard isStreaming else { return }
        isStreaming = false
        ws.disconnect()
        mqtt.disconnect()
        stopSensors()
        rateTimer?.invalidate()
        rateTimer = nil
        mqttPublishTimer?.invalidate()
        mqttPublishTimer = nil
    }

    /// Call when the user edits the Orin URL. Persists the value and updates
    /// the client URL; takes effect on the next connect().
    func updateURL(_ urlString: String) {
        guard !isStreaming else { return }
        orinURLString = urlString
        if let url = URL(string: urlString), url.scheme?.hasPrefix("ws") == true {
            ws.url = url
        }
    }

    // MARK: - MQTT GPS publish (1 Hz)

    private func startMQTTPublishTimer() {
        mqttPublishTimer = Timer.scheduledTimer(withTimeInterval: 1.0, repeats: true) { [weak self] _ in
            self?.publishGPSToMQTT()
        }
    }

    private func publishGPSToMQTT() {
        guard let loc = lastKnownLocation else { return }
        let payload: [String: Any] = [
            "ts":          loc.timestamp.timeIntervalSince1970,
            "lat":         loc.coordinate.latitude,
            "lon":         loc.coordinate.longitude,
            "alt_m":       loc.altitude,
            "accuracy_m":  max(0, loc.horizontalAccuracy),
            "speed_ms":    max(0, loc.speed),
            "bearing_deg": loc.course >= 0 ? loc.course : 0.0,
            "provider":    "gps"
        ]
        guard let data = try? JSONSerialization.data(withJSONObject: payload),
              let json = String(data: data, encoding: .utf8) else { return }
        mqtt.publish(topic: Self.mqttGPSTopic, payload: json)
    }

    // MARK: - Sensor start / stop

    private func requestPermissionsAndStartSensors() {
        locationManager.requestAlwaysAuthorization()
        locationManager.startUpdatingLocation()
        locationManager.startUpdatingHeading()
        startIMU()
        startBarometer()
    }

    private func stopSensors() {
        locationManager.stopUpdatingLocation()
        locationManager.stopUpdatingHeading()
        motionManager.stopDeviceMotionUpdates()
        altimeter.stopRelativeAltitudeUpdates()
    }

    private func startIMU() {
        guard motionManager.isDeviceMotionAvailable else { return }
        motionManager.deviceMotionUpdateInterval = 1.0 / 100.0  // 100 Hz
        motionManager.startDeviceMotionUpdates(to: .main) { [weak self] motion, error in
            guard let self, let motion else { return }
            self.recordEvent(in: &self.imuCounts)
            let ts = Date().timeIntervalSince1970
            self.ws.send([
                "type": "imu",
                "timestamp": ts,
                "data": [
                    "accel": [
                        "x": motion.userAcceleration.x,
                        "y": motion.userAcceleration.y,
                        "z": motion.userAcceleration.z
                    ],
                    "gyro": [
                        "x": motion.rotationRate.x,
                        "y": motion.rotationRate.y,
                        "z": motion.rotationRate.z
                    ],
                    "attitude": [
                        "roll": motion.attitude.roll,
                        "pitch": motion.attitude.pitch,
                        "yaw": motion.attitude.yaw
                    ],
                    "gravity": [
                        "x": motion.gravity.x,
                        "y": motion.gravity.y,
                        "z": motion.gravity.z
                    ],
                    "magneticField": [
                        "x": motion.magneticField.field.x,
                        "y": motion.magneticField.field.y,
                        "z": motion.magneticField.field.z,
                        "accuracy": motion.magneticField.accuracy.rawValue
                    ]
                ]
            ])
        }
    }

    private func startBarometer() {
        guard CMAltimeter.isRelativeAltitudeAvailable() else { return }
        altimeter.startRelativeAltitudeUpdates(to: .main) { [weak self] data, error in
            guard let self, let data else { return }
            self.recordEvent(in: &self.baroCounts)
            self.ws.send([
                "type": "baro",
                "timestamp": Date().timeIntervalSince1970,
                "data": [
                    "relativeAltitude": data.relativeAltitude.doubleValue,
                    "pressure": data.pressure.doubleValue
                ]
            ])
        }
    }

    // MARK: - Rate tracking

    private func startRateTimer() {
        rateTimer = Timer.scheduledTimer(withTimeInterval: 1.0, repeats: true) { [weak self] _ in
            self?.updateRates()
        }
    }

    private func recordEvent(in list: inout [Date]) {
        list.append(Date())
    }

    private func updateRates() {
        let cutoff = Date().addingTimeInterval(-1.0)
        gpsCounts = gpsCounts.filter { $0 > cutoff }
        imuCounts = imuCounts.filter { $0 > cutoff }
        headingCounts = headingCounts.filter { $0 > cutoff }
        baroCounts = baroCounts.filter { $0 > cutoff }

        DispatchQueue.main.async {
            self.gpsRate = Double(self.gpsCounts.count)
            self.imuRate = Double(self.imuCounts.count)
            self.headingRate = Double(self.headingCounts.count)
            self.baroRate = Double(self.baroCounts.count)
        }
    }
}

// MARK: - CLLocationManagerDelegate

extension SensorManager: CLLocationManagerDelegate {

    func locationManager(_ manager: CLLocationManager, didUpdateLocations locations: [CLLocation]) {
        guard let loc = locations.last else { return }
        lastKnownLocation = loc
        recordEvent(in: &gpsCounts)
        ws.send([
            "type": "gps",
            "timestamp": loc.timestamp.timeIntervalSince1970,
            "data": [
                "latitude": loc.coordinate.latitude,
                "longitude": loc.coordinate.longitude,
                "altitude": loc.altitude,
                "horizontalAccuracy": loc.horizontalAccuracy,
                "verticalAccuracy": loc.verticalAccuracy,
                "speed": loc.speed,
                "speedAccuracy": loc.speedAccuracy,
                "course": loc.course,
                "courseAccuracy": loc.courseAccuracy
            ]
        ])
    }

    func locationManager(_ manager: CLLocationManager, didUpdateHeading newHeading: CLHeading) {
        recordEvent(in: &headingCounts)
        ws.send([
            "type": "heading",
            "timestamp": Date().timeIntervalSince1970,
            "data": [
                "magneticHeading": newHeading.magneticHeading,
                "trueHeading": newHeading.trueHeading,
                "headingAccuracy": newHeading.headingAccuracy,
                "x": newHeading.x,
                "y": newHeading.y,
                "z": newHeading.z
            ]
        ])
    }

    func locationManager(_ manager: CLLocationManager, didFailWithError error: Error) {
        print("[Location] error: \(error)")
    }

    func locationManagerDidChangeAuthorization(_ manager: CLLocationManager) {
        switch manager.authorizationStatus {
        case .authorizedAlways, .authorizedWhenInUse:
            if isStreaming {
                manager.startUpdatingLocation()
                manager.startUpdatingHeading()
            }
        default:
            break
        }
    }
}