import Foundation import Combine import RideFormCore /// Ordered raw samples drive the guide; final fitting still requires the adapter's verified convention. @MainActor final class CalibrationController: ObservableObject { enum SetupMethod: String, CaseIterable { case guided, standardMount } enum Stage: Equatable { case idle, preparingNeutral, neutral, left, returnToRestart, returnToNeutral, forward, returnToFinish, awaitingConvention, ready, retry } @Published private(set) var setupMethod: SetupMethod = .guided @Published private(set) var stage: Stage = .idle @Published private(set) var currentCalibration: Calibration? @Published private(set) var error: CalibrationError? @Published private(set) var sampleCount = 0 @Published private(set) var countdown = 3 @Published private(set) var handsFree = false @Published private(set) var waitingForVoice = false @Published private(set) var returnHelpNeeded = false private var failureMessage: String? private var recoveryMessage: String? private(set) var automaticRetries = 0 private(set) var latestAngleRadians: Double? private(set) var latestRateRadiansPerSecond: Double? var evidenceCounts: [String: Int] { ["neutral": neutral.count, "left": left.count, "forward": forward.count, "hold": hold.count] } private var neutral: [MotionSample] = [] private var left: [MotionSample] = [] private var forward: [MotionSample] = [] private var hold: [MotionSample] = [] private var neutralMean: Quaternion? private var stageStart: Double? private var lastTimestamp: Double? private var configuration: CalibrationConfiguration private let provisional = RotationConvention(capturedToCanonical: "identity", verified: true) var canRecord: Bool { currentCalibration?.verified == true && !isCalibrating } var isCalibrating: Bool { ![.idle, .ready, .retry].contains(stage) } var canAdvance: Bool { switch stage { case .neutral: return hasCoverage(neutral, duration: configuration.neutralDurationSeconds) case .left: return hasCoverage(left, duration: 0.3) case .forward: return hasCoverage(forward, duration: 0.3) default: return false } } var prompt: String { if let recoveryMessage { return recoveryMessage } switch stage { case .idle: return "Calibrate with the phone rigidly mounted." case .preparingNeutral: return "Sit centered with the phone mounted. Breathe normally. Listen for the countdown." case .neutral: return "Stay centered. Capturing neutral. Small movements are okay." case .left: return "Lean LEFT about 15 degrees and pause comfortably." case .returnToRestart: return "Return to your starting center and pause. We will repeat the leans." case .returnToNeutral: return returnHelpNeeded ? "Still waiting for center. Sit upright like you did at the start and pause. Next is a forward lean." : "Left captured. Return to your starting center and pause. Then we will lean forward." case .forward: return "Lean FORWARD about 15 degrees and pause comfortably." case .returnToFinish: return "Forward captured. All positions are saved. Return comfortably to center while I check calibration." case .awaitingConvention: return "Checking calibration." case .ready: return "Calibration ready." case .retry: return failureMessage ?? "\(explanation(error)) Calibration is paused. Tap Retry calibration when ready." } } var waitingPrompt: String? { switch stage { case .preparingNeutral: return "Waiting for the countdown. Keep the phone mounted and stay centered." case .neutral: return "Still capturing center. Sit comfortably upright and pause. Small movements are okay." case .left: return "Still waiting for the left lean. Lean left about 15 degrees and pause." case .returnToRestart: return "Center is saved. Return to your starting posture and pause before repeating the leans." case .returnToNeutral: return "Still waiting for center. Return to your starting posture and pause. Forward is next." case .forward: return "Still waiting for the forward lean. Lean forward about 15 degrees and pause." case .returnToFinish: return "All positions are saved. Return comfortably to center. No exact center hold is needed." case .awaitingConvention: return "Still checking calibration. Stay centered while I check the sensor direction." case .idle, .ready, .retry: return nil } } init(calibration: Calibration? = nil, configuration: CalibrationConfiguration = CalibrationSolver.defaultConfiguration) { currentCalibration = calibration self.configuration = CalibrationSolver.resolvedConfiguration(configuration) if calibration?.verified == true { stage = .ready } } @discardableResult func begin(recording: Bool, method: SetupMethod = .guided, handsFree: Bool = false) -> Bool { guard !recording else { error = .recording; return false } setupMethod = method; self.handsFree = handsFree neutral = []; left = []; forward = []; neutralMean = nil automaticRetries = 0; recoveryMessage = nil; latestAngleRadians = nil; latestRateRadiansPerSecond = nil configuration.parameters.removeValue(forKey: "guidedRetriesUsed") configuration.parameters["guidedMaximumRetries"] = 2 sampleCount = 0; error = nil; failureMessage = nil; countdown = 3; lastTimestamp = nil // Save collection policy alongside numeric fit limits; raw accepted inputs remain unchanged. configuration.parameters["rollingNeutralWindowSeconds"] = configuration.neutralDurationSeconds configuration.parameters["guidedCountdownSeconds"] = 3 configuration.parameters["guidedStageTimeoutSeconds"] = 30 configuration.parameters["guidedHoldSeconds"] = 0.6 configuration.parameters["guidedHoldRangeRadians"] = 0.05 configuration.parameters["guidedReturnRadians"] = 0.10 configuration.parameters["guidedReturnExitRadians"] = 0.12 configuration.parameters["guidedReturnTimeoutSeconds"] = 15 configuration.parameters["guidedFinalReturnAdvisory"] = 1 configuration.parameters["guidedFeedbackSilenceSeconds"] = 4 move(to: handsFree ? .preparingNeutral : .neutral) return true } /// The speaker releases a stage only after its instruction has finished. func guidanceDidFinish() { waitingForVoice = false; stageStart = nil; lastTimestamp = nil } func ingest(_ record: CapturedRecord) { if case .motion(let sample) = record { ingest(sample) } } func ingest(_ sample: MotionSample) { guard isCalibrating, stage != .awaitingConvention, !waitingForVoice else { return } guard sample.sensorTimestamp.isFinite else { fail(.invalidInput); return } if let lastTimestamp, sample.sensorTimestamp <= lastTimestamp { fail(.invalidInput); return } let gapped = lastTimestamp.map { sample.sensorTimestamp-$0 > configuration.parameters["maximumSampleGapSeconds"]! } ?? false lastTimestamp = sample.sensorTimestamp latestRateRadiansPerSecond = OrientationMath.norm(sample.rotationRate) if let neutralMean, let q = try? OrientationMath.canonical(sample.attitudeQuaternion, convention: provisional) { latestAngleRadians = distance(neutralMean, q) } if stageStart == nil { stageStart = sample.sensorTimestamp } let elapsed = sample.sensorTimestamp-stageStart! if handsFree && elapsed > 30 { fail(.timedOut); return } sampleCount += 1 if stage == .preparingNeutral { countdown = max(0, 3-Int(elapsed)) if elapsed >= 3 { move(to: .neutral) } return } switch stage { case .neutral: if gapped && handsFree { neutral = [] } neutral.append(sample) trim(&neutral, duration: configuration.neutralDurationSeconds) if handsFree, canAdvance, (try? estimateNeutral()) != nil { _ = advance() } case .left, .forward: if gapped && handsFree { if stage == .left { left = [] } else { forward = [] } hold = [] } if stage == .left { left.append(sample) } else { forward.append(sample) } if handsFree, gestureHeld(sample) { move(to: stage == .left ? .returnToNeutral : .returnToFinish) } case .returnToFinish: // The fit uses the original neutral and both leans, not this return. // Keep the spoken return cue, then validate without another pose gate. move(to: .awaitingConvention) case .returnToRestart, .returnToNeutral: if elapsed >= 15 { fail(.timedOut, message: "Could not confirm your return to center. Check that the phone has not shifted, then retry calibration.") return } returnHelpNeeded = elapsed >= 6 if gapped { hold = [] } if let neutralMean, let q = try? OrientationMath.canonical(sample.attitudeQuaternion, convention: provisional), // Enter near center; small boundary crossings do not restart the hold. distance(neutralMean, q) <= (hold.isEmpty ? 0.10 : 0.12), OrientationMath.norm(sample.rotationRate) <= 0.15 { hold.append(sample) if hasCoverage(hold, duration: 0.6) { move(to: stage == .returnToRestart ? .left : .forward) } } else { hold = [] } default: break } } @discardableResult func advance() -> Bool { guard canAdvance else { error = .tooShort; return false } error = nil switch stage { case .neutral: do { neutralMean = try estimateNeutral(); move(to: .left) } catch let failure as CalibrationError { fail(failure); return false } catch { fail(.invalidInput); return false } case .left: move(to: .forward) case .forward: move(to: .awaitingConvention) default: return false } return true } @discardableResult func finish(summary: CaptureSummary) -> Bool { guard stage == .awaitingConvention else { return false } do { let replacement: Calibration if setupMethod == .standardMount { replacement = try CalibrationSolver.fitStandardMount(neutral: neutral, left: left, forward: forward, convention: summary.rotationConvention, configuration: configuration) } else { replacement = try CalibrationSolver.fit(neutral: neutral, left: left, forward: forward, convention: summary.rotationConvention, configuration: configuration) } currentCalibration = replacement; move(to: .ready); error = nil return true } catch let failure as CalibrationError { fail(failure); return false } catch { fail(.invalidInput); return false } } func fail(_ failure: CalibrationError, message: String? = nil, allowRecovery: Bool = true) { if allowRecovery, handsFree, automaticRetries < 2 { var next: Stage? var reason = "" if failure == .timedOut, stage != .awaitingConvention { next = stage reason = "That step needed more time. Let's try it again." if stage == .left { left = [] } if stage == .forward { forward = [] } if stage == .neutral { neutral = [] } } else if stage == .awaitingConvention, failure == .collinearGestures { forward = []; next = .returnToNeutral reason = "The forward lean looked too much like the left lean. Center and left are saved. Let's repeat forward." } else if stage == .awaitingConvention, [.weakGesture, .tooShort].contains(failure) { left = []; forward = []; next = .returnToRestart reason = "A lean did not contain enough usable motion. Center is saved. Let's repeat the leans." } if let next { automaticRetries += 1; configuration.parameters["guidedRetriesUsed"] = Double(automaticRetries) error = failure; move(to: next) recoveryMessage = reason + " " + prompt return } } error = failure; failureMessage = message; move(to: .retry) } func cancel() { neutral = []; left = []; forward = []; hold = []; sampleCount = 0; error = nil waitingForVoice = false; stageStart = nil; lastTimestamp = nil stage = currentCalibration?.verified == true ? .ready : .idle } private func move(to next: Stage) { stage = next; stageStart = nil; hold = []; lastTimestamp = nil; returnHelpNeeded = false; recoveryMessage = nil waitingForVoice = handsFree && ![.ready, .retry, .awaitingConvention].contains(next) } private func estimateNeutral() throws -> Quaternion { try CalibrationSolver.neutralOrientation(neutral, convention: provisional, configuration: configuration).mean } private func trim(_ samples: inout [MotionSample], duration: Double) { guard let last = samples.last else { return } // Retain the sample just before the boundary to cover a full actual-time interval. let cut = samples.lastIndex { $0.sensorTimestamp <= last.sensorTimestamp-duration } ?? 0 if cut > 0 { samples.removeFirst(cut) } } private func hasCoverage(_ samples: [MotionSample], duration: Double) -> Bool { guard let first = samples.first, let last = samples.last, samples.count >= 2 else { return false } return last.sensorTimestamp-first.sensorTimestamp >= duration } private func distance(_ a: Quaternion, _ b: Quaternion) -> Double { OrientationMath.norm(OrientationMath.rotationVector(OrientationMath.multiply(OrientationMath.inverse(a), b))) } private func gestureHeld(_ sample: MotionSample) -> Bool { guard let neutralMean, let q = try? OrientationMath.canonical(sample.attitudeQuaternion, convention: provisional) else { fail(.invalidInput); return false } let angle = distance(neutralMean, q) guard angle >= configuration.parameters["minimumGestureRadians"]!, angle <= configuration.parameters["maximumGestureRadians"]!, OrientationMath.norm(sample.rotationRate) <= 0.2 else { hold = []; return false } hold.append(sample); trim(&hold, duration: 0.6) guard hasCoverage(hold, duration: 0.6) else { return false } let stable = hold.allSatisfy { point in guard let other = try? OrientationMath.canonical(point.attitudeQuaternion, convention: provisional) else { return false } return distance(other, q) <= 0.05 } return stable } private func explanation(_ error: CalibrationError?) -> String { switch error { case .movingNeutral: return "Too much movement in neutral. Settle comfortably and breathe normally." case .tooShort: return "Not enough continuous sensor data." case .weakGesture: return "The lean was too small or too large. Aim for about 15 degrees." case .collinearGestures: return "Left and forward looked too similar. Keep the phone fixed to your torso." case .standardMountSignMismatch: return "The phone did not match the standard mount. Try Guided calibration." case .unverifiedConvention: return "The sensor orientation check failed. This is not a center position failure. Share the calibration log so we can inspect the sensor readings." case .timedOut: return "This step timed out. Check the mount and retry when ready." default: return "Sensor data was invalid or interrupted." } } }