import Foundation
import CoreMotion
import RideFormCore

enum CaptureMode: String, CaseIterable { case fusedOnly, combined, rawOnly }
enum CapturedRecord {
    case motion(MotionSample)
    case accelerometer(RawAccelerometerSample)
    case gyroscope(RawGyroscopeSample)
}
struct CaptureSummary {
    var capabilities: [StreamKind: CapabilityState]
    var acceptedCounts: [StreamKind: Int]
    var ended: ClockAnchor
    var clockMapping: ClockMapping
    var rotationConvention: RotationConvention
    var conventionDiagnostics: String = ""
}

/// Immutable sensor values cross asynchronously into one off-main serial control/state queue.
/// Core Motion owns a separate serial operation queue whose operations may be canceled on stop.
final class MotionCapture {
    let clock: SessionClock
    private let manager = CMMotionManager()
    private let sensorQueue: OperationQueue = {
        let queue = OperationQueue()
        queue.name = "RideForm.sensor"
        queue.maxConcurrentOperationCount = 1
        queue.qualityOfService = .userInitiated
        return queue
    }()
    private let controlQueue = DispatchQueue(label: "RideForm.capture.control", qos: .userInitiated)
    private var generation: UInt64 = 0
    private var active = false
    private var capabilities: [StreamKind: CapabilityState] = [:]
    private var counts: [StreamKind: Int] = [:]
    private var onRecord: ((CapturedRecord) -> Void)?
    private var onDiagnostic: ((DiagnosticRecord) -> Void)?
    private var mode: CaptureMode = .fusedOnly
    private var convention = RotationConvention()
    private var rotationChecks = 0
    private var identityResidual = 0.0
    private var inverseResidual = 0.0
    private var matrixMismatch = false
    private var matrixDirectPossible = true
    private var matrixTransposePossible = true
    private var firstConventionFailure: String?
    private var conventionDiagnostics: String {
        "gravity checks=\(rotationChecks); identity sum=\(identityResidual); inverse sum=\(inverseResidual); matrix mismatch=\(matrixMismatch); direct matrix possible=\(matrixDirectPossible); transpose matrix possible=\(matrixTransposePossible); first failure=\(firstConventionFailure ?? "none")"
    }
    private var fixtureOnly = false

    init(clock: SessionClock = SessionClock()) { self.clock = clock }

    var isDeviceMotionAvailable: Bool {
        manager.isDeviceMotionAvailable && CMMotionManager.availableAttitudeReferenceFrames().contains(.xArbitraryZVertical)
    }
    func snapshot(completion: @escaping (CaptureSummary) -> Void) {
        controlQueue.async { [self] in
            completion(CaptureSummary(capabilities: capabilities, acceptedCounts: counts, ended: clock.anchor(reason: "snapshot"), clockMapping: clock.mapping, rotationConvention: convention, conventionDiagnostics: conventionDiagnostics))
        }
    }

    func start(mode: CaptureMode = .fusedOnly, onRecord: @escaping (CapturedRecord) -> Void, onDiagnostic: @escaping (DiagnosticRecord) -> Void = { _ in }) {
        controlQueue.async { [self] in
            guard !active else { return }
            generation &+= 1
            let callbackGeneration = generation
            active = true
            fixtureOnly = false
            clock.resetValidation()
            self.mode = mode
            self.onRecord = onRecord
            self.onDiagnostic = onDiagnostic
            counts = [:]
            convention = RotationConvention()
            rotationChecks = 0; identityResidual = 0; inverseResidual = 0; matrixMismatch = false; firstConventionFailure = nil
            matrixDirectPossible = true; matrixTransposePossible = true
            let frameSupported = CMMotionManager.availableAttitudeReferenceFrames().contains(.xArbitraryZVertical)
            capabilities = [
                .motion: mode == .rawOnly ? .unavailable(reason: "raw-only diagnostic mode") : (manager.isDeviceMotionAvailable && frameSupported ? .available : .unavailable(reason: "device motion or xArbitraryZVertical unavailable")),
                .accelerometer: mode == .fusedOnly ? .unavailable(reason: "fused-only mode") : (manager.isAccelerometerAvailable ? .available : .unavailable(reason: "accelerometer unavailable")),
                .gyroscope: mode == .fusedOnly ? .unavailable(reason: "fused-only mode") : (manager.isGyroAvailable ? .available : .unavailable(reason: "gyroscope unavailable"))]
            for (kind, capability) in capabilities {
                if case .unavailable(let reason) = capability { diagnostic("stream_unavailable", reason, kind) }
            }
            manager.deviceMotionUpdateInterval = 0.01
            manager.accelerometerUpdateInterval = 0.01
            manager.gyroUpdateInterval = 0.01
            if capabilities[.motion] == .available {
                let magnetometerAvailable = manager.isMagnetometerAvailable
                manager.startDeviceMotionUpdates(using: .xArbitraryZVertical, to: sensorQueue) { [weak self] data, error in
                    guard let self else { return }
                    let receipt = self.clock.now()
                    let sample = data.map { self.copyMotion($0, receipt: receipt, magnetometerAvailable: magnetometerAvailable) }
                    self.enqueueSensor(record: sample.map(CapturedRecord.motion), error: error?.localizedDescription, stream: .motion, receipt: receipt, generation: callbackGeneration)
                }
            }
            if capabilities[.accelerometer] == .available {
                manager.startAccelerometerUpdates(to: sensorQueue) { [weak self] data, error in
                    guard let self else { return }
                    let receipt = self.clock.now()
                    let record = data.map { CapturedRecord.accelerometer(RawAccelerometerSample(sensorTimestamp: $0.timestamp, receiptUptime: receipt, acceleration: Vector3(x: $0.acceleration.x, y: $0.acceleration.y, z: $0.acceleration.z))) }
                    self.enqueueSensor(record: record, error: error?.localizedDescription, stream: .accelerometer, receipt: receipt, generation: callbackGeneration)
                }
            }
            if capabilities[.gyroscope] == .available {
                manager.startGyroUpdates(to: sensorQueue) { [weak self] data, error in
                    guard let self else { return }
                    let receipt = self.clock.now()
                    let record = data.map { CapturedRecord.gyroscope(RawGyroscopeSample(sensorTimestamp: $0.timestamp, receiptUptime: receipt, rotationRate: Vector3(x: $0.rotationRate.x, y: $0.rotationRate.y, z: $0.rotationRate.z))) }
                    self.enqueueSensor(record: record, error: error?.localizedDescription, stream: .gyroscope, receipt: receipt, generation: callbackGeneration)
                }
            }
        }
    }

    /// Accepted records finish before this barrier; later callbacks are suppressed.
    /// Completion runs within the executing barrier after stopping; it needs no later queued operation.
    func stop(completion: @escaping (CaptureSummary) -> Void) {
        controlQueue.async { [self] in
            active = false
            generation &+= 1
            let summary = CaptureSummary(capabilities: capabilities, acceptedCounts: counts, ended: clock.anchor(reason: "capture stopped"), clockMapping: clock.mapping, rotationConvention: convention, conventionDiagnostics: conventionDiagnostics)
            if !fixtureOnly {
                manager.stopDeviceMotionUpdates()
                manager.stopAccelerometerUpdates()
                manager.stopGyroUpdates()
            }
            #if targetEnvironment(simulator)
            if fixtureOnly && simulateSensorCancellationOnFixtureStop { sensorQueue.cancelAllOperations() }
            #endif
            completion(summary)
            onRecord = nil; onDiagnostic = nil
        }
    }

    private func diagnostic(_ code: String, _ message: String, _ stream: StreamKind) {
        onDiagnostic?(DiagnosticRecord(code: code, message: message, hostUptime: clock.now(), stream: stream))
    }
    private func accept(_ record: CapturedRecord) {
        guard active else { return }
        switch record {
        case .motion(var value):
            value.sequence = UInt64(counts[.motion, default: 0]); counts[.motion, default: 0] += 1
            onRecord?(.motion(value))
        case .accelerometer(var value):
            value.sequence = UInt64(counts[.accelerometer, default: 0]); counts[.accelerometer, default: 0] += 1
            onRecord?(.accelerometer(value))
        case .gyroscope(var value):
            value.sequence = UInt64(counts[.gyroscope, default: 0]); counts[.gyroscope, default: 0] += 1
            onRecord?(.gyroscope(value))
        }
    }
    private func enqueueSensor(record: CapturedRecord?, error: String?, stream: StreamKind, receipt: Double, generation callbackGeneration: UInt64) {
        controlQueue.async { [self] in
            guard active && generation == callbackGeneration else { return }
            if let error { onDiagnostic?(DiagnosticRecord(code: "acquisition_error", message: error, hostUptime: receipt, stream: stream)) }
            guard let record else { return }
            switch record {
            case .motion(let sample):
                clock.validateMapping(sensorTimestamp: sample.sensorTimestamp, receiptUptime: sample.receiptUptime)
                inspectConventionValue(sample)
            case .gyroscope(let sample) where mode == .rawOnly:
                clock.validateMapping(sensorTimestamp: sample.sensorTimestamp, receiptUptime: sample.receiptUptime)
            default: break
            }
            accept(record)
        }
    }

    private func copyMotion(_ data: CMDeviceMotion, receipt: Double, magnetometerAvailable: Bool) -> MotionSample {
        let a = data.attitude, q = a.quaternion, m = a.rotationMatrix
        let field = data.magneticField
        let magnetic = Vector3(x: field.field.x, y: field.field.y, z: field.field.z)
        let code = Int(field.accuracy.rawValue)
        var accuracy: OptionalMeasurement<Double> = .unavailable(reason: "headingAccuracy requires iOS 27 or watchOS 27")
        if #available(iOS 27.0, watchOS 27.0, *) {
            accuracy = .invalid(value: data.headingAccuracy * .pi / 180, reason: "arbitrary reference frame; negative accuracy also indicates invalid")
        }
        return MotionSample(sensorTimestamp: data.timestamp, receiptUptime: receipt,
            attitudeRadians: Vector3(x: a.roll, y: a.pitch, z: a.yaw),
            attitudeQuaternion: Quaternion(x: q.x, y: q.y, z: q.z, w: q.w),
            attitudeRotationMatrix: RotationMatrix(m11: m.m11, m12: m.m12, m13: m.m13, m21: m.m21, m22: m.m22, m23: m.m23, m31: m.m31, m32: m.m32, m33: m.m33),
            rotationRate: Vector3(x: data.rotationRate.x, y: data.rotationRate.y, z: data.rotationRate.z),
            gravity: Vector3(x: data.gravity.x, y: data.gravity.y, z: data.gravity.z),
            userAcceleration: Vector3(x: data.userAcceleration.x, y: data.userAcceleration.y, z: data.userAcceleration.z),
            magneticField: !magnetometerAvailable ? .unavailable(reason: "magnetometer unavailable", rawValidityCode: code) : (field.accuracy == .uncalibrated ? .invalid(value: magnetic, reason: "magnetic field uncalibrated", rawValidityCode: code) : .available(value: magnetic, rawValidityCode: code)),
            headingRadians: .invalid(value: data.heading * .pi / 180, reason: "xArbitraryZVertical heading is invalid"), headingAccuracyRadians: accuracy)
    }

    /// Gravity distinguishes active direction when tilt is nontrivial; yaw-only/identity is ambiguous.
    func inspectConvention(_ sample: MotionSample) {
        controlQueue.async { [self] in inspectConventionValue(sample) }
    }
    private func inspectConventionValue(_ sample: MotionSample) {
        let q = sample.attitudeQuaternion
        let m = sample.attitudeRotationMatrix
        let values = [q.x, q.y, q.z, q.w, m.m11, m.m12, m.m13, m.m21, m.m22, m.m23, m.m31, m.m32, m.m33,
                      sample.gravity.x, sample.gravity.y, sample.gravity.z,
                      sample.attitudeRadians.x, sample.attitudeRadians.y, sample.attitudeRadians.z,
                      sample.rotationRate.x, sample.rotationRate.y, sample.rotationRate.z,
                      sample.userAcceleration.x, sample.userAcceleration.y, sample.userAcceleration.z]
        guard values.allSatisfy({ $0.isFinite }) else {
            if firstConventionFailure == nil { firstConventionFailure = "nonfinite sample: \(values)" }
            invalidateConvention(); return
        }
        func rotate(_ v: Vector3, inverse: Bool) -> Vector3 {
            let x = inverse ? -q.x : q.x, y = inverse ? -q.y : q.y, z = inverse ? -q.z : q.z
            let tx = 2 * (y * v.z - z * v.y), ty = 2 * (z * v.x - x * v.z), tz = 2 * (x * v.y - y * v.x)
            return Vector3(x: v.x + q.w * tx + y * tz - z * ty, y: v.y + q.w * ty + z * tx - x * tz, z: v.z + q.w * tz + x * ty - y * tx)
        }
        func distance(_ a: Vector3, _ b: Vector3) -> Double { sqrt(pow(a.x - b.x, 2) + pow(a.y - b.y, 2) + pow(a.z - b.z, 2)) }
        // Core Motion's DCM may represent the inverse of the active quaternion
        // rotation. Require one consistent relationship across the whole capture;
        // gravity independently selects the quaternion's canonical direction.
        for (v, mv) in [(Vector3(x: 1), Vector3(x: m.m11, y: m.m21, z: m.m31)), (Vector3(y: 1), Vector3(x: m.m12, y: m.m22, z: m.m32)), (Vector3(z: 1), Vector3(x: m.m13, y: m.m23, z: m.m33))] {
            let direct = distance(rotate(v, inverse: false), mv)
            let transposed = distance(rotate(v, inverse: true), mv)
            matrixDirectPossible = matrixDirectPossible && direct.isFinite && direct <= 0.001
            matrixTransposePossible = matrixTransposePossible && transposed.isFinite && transposed <= 0.001
        }
        if !matrixDirectPossible && !matrixTransposePossible {
            if firstConventionFailure == nil {
                firstConventionFailure = "inconsistent quaternion/matrix relationship; q=\(q); matrix=\(m); gravity=\(sample.gravity)"
            }
            matrixMismatch = true
        }
        guard !matrixMismatch else { invalidateConvention(); return }
        let target = Vector3(z: -1)
        let forward = distance(rotate(sample.gravity, inverse: false), target)
        let inverse = distance(rotate(sample.gravity, inverse: true), target)
        guard forward.isFinite && inverse.isFinite else { invalidateConvention(); return }
        guard abs(forward - inverse) > 0.1 else { return }
        rotationChecks += 1; identityResidual += forward; inverseResidual += inverse
        guard rotationChecks >= 10 else { return }
        let identity = identityResidual / Double(rotationChecks), inverted = inverseResidual / Double(rotationChecks)
        let verified = !matrixMismatch && min(identity, inverted) < 0.03 && max(identity, inverted) > 0.1
        if !convention.verified && verified {
            diagnostic("rotation_convention_validated", "gravity checks=\(rotationChecks), identity residual=\(identity), inverse residual=\(inverted), matrix mismatch=\(matrixMismatch); physical signs pending", .motion)
        }
        let matrixRelation = matrixDirectPossible && matrixTransposePossible ? "ambiguous" : matrixDirectPossible ? "direct" : "transpose"
        convention = RotationConvention(capturedAttitude: "unchanged Core Motion quaternion and matrix; matrix relative to active quaternion: \(matrixRelation)", capturedToCanonical: verified ? (identity < inverted ? "identity" : "quaternion conjugate") : "unverified", verified: verified)
    }

    private func invalidateConvention() {
        matrixMismatch = true
        convention = RotationConvention(capturedAttitude: "unchanged Core Motion quaternion and matrix", capturedToCanonical: "unverified", verified: false)
    }

    #if targetEnvironment(simulator)
    var simulateSensorCancellationOnFixtureStop = false
    func makeSimulatorFixtureCallback(completion: @escaping (@escaping (CapturedRecord) -> Void) -> Void) {
        controlQueue.async { [self] in
            let callbackGeneration = generation
            completion { [self] record in
                let stream: StreamKind
                switch record {
                case .motion: stream = .motion
                case .accelerometer: stream = .accelerometer
                case .gyroscope: stream = .gyroscope
                }
                enqueueSensor(record: record, error: nil, stream: stream, receipt: clock.now(), generation: callbackGeneration)
            }
        }
    }
    /// Explicit synthetic source. Compiled only for simulator; never routes into physical capture.
    func startSimulatorFixture(mode: CaptureMode = .fusedOnly, onRecord: @escaping (CapturedRecord) -> Void) {
        controlQueue.async { [self] in
            guard !active else { return }
            generation &+= 1
            fixtureOnly = true; active = true; self.mode = mode; counts = [:]; self.onRecord = onRecord
            self.onDiagnostic = nil
            clock.resetValidation()
            convention = RotationConvention()
            rotationChecks = 0; identityResidual = 0; inverseResidual = 0; matrixMismatch = false; firstConventionFailure = nil
            matrixDirectPossible = true; matrixTransposePossible = true
            capabilities = [.motion: .available, .accelerometer: .available, .gyroscope: .available]
        }
    }
    func deliverSimulatorFixture(_ record: CapturedRecord) { controlQueue.async { [self] in accept(record) } }
    #endif
}