first commit

Shared/Models/AlignmentAssistant.swift

@@ -0,0 +1,213 @@
+import Foundation
+
+/// Hilft beim Ausrichten des Fahrzeugs während des Rangierens.
+///
+/// Bewusst **ohne** Positionsbestimmung: Aus einem MEMS-Beschleunigungssensor
+/// lässt sich keine brauchbare Strecke ableiten, weil der Fehler beim
+/// zweifachen Integrieren quadratisch mit der Zeit wächst. Beim Rangieren im
+/// Schritttempo gehen die tatsächlichen Beschleunigungen ohnehin im Rauschen
+/// unter.
+///
+/// Gebraucht wird das auch gar nicht. Die Frage beim Einparken lautet nie „wo
+/// stehe ich“, sondern „wird es besser oder schlechter, und wo war es am
+/// besten“. Beides steckt bereits im zeitlichen Verlauf der Neigung – ganz
+/// ohne Annahmen über das Gelände.
+struct AlignmentAssistant {
+
+    struct Sample: Equatable {
+        let time: Date
+        let pitch: Double
+        let roll: Double
+
+        /// Gesamtabweichung von der Waagerechten.
+        var deviation: Double { (pitch * pitch + roll * roll).squareRoot() }
+    }
+
+    /// Wie lange zurückgeschaut wird.
+    static let memory: TimeInterval = 90
+    /// Ab dieser Verbesserung lohnt der Hinweis auf einen früheren Punkt.
+    static let worthGoingBack = 0.2
+    /// Unterhalb dieser Änderung gilt die Lage als unverändert.
+    static let trendThreshold = 0.08
+
+    private(set) var samples: [Sample] = []
+
+    enum Trend: Equatable {
+        case improving
+        case worsening
+        case steady
+        case unknown
+
+        var text: String {
+            switch self {
+            case .improving: return "Wird besser"
+            case .worsening: return "Wird schlechter"
+            case .steady:    return "Bleibt gleich"
+            case .unknown:   return "Messe…"
+            }
+        }
+
+        var symbol: String {
+            switch self {
+            case .improving: return "arrow.down.right.circle.fill"
+            case .worsening: return "arrow.up.right.circle.fill"
+            case .steady:    return "equal.circle.fill"
+            case .unknown:   return "hourglass.circle"
+            }
+        }
+    }
+
+    mutating func add(pitch: Double, roll: Double, at time: Date = Date()) {
+        samples.append(Sample(time: time, pitch: pitch, roll: roll))
+        let cutoff = time.addingTimeInterval(-Self.memory)
+        samples.removeAll { $0.time < cutoff }
+    }
+
+    mutating func reset() {
+        samples.removeAll()
+    }
+
+    var current: Sample? { samples.last }
+
+    /// Der flachste Punkt, den wir gesehen haben.
+    var best: Sample? {
+        samples.min { $0.deviation < $1.deviation }
+    }
+
+    /// Ob und wie stark sich die Lage gerade ändert.
+    ///
+    /// Verglichen wird das jüngste Drittel mit dem davorliegenden. Einzelne
+    /// Messwerte wären zu unruhig; das Fahrzeug wippt beim Rangieren.
+    var trend: Trend {
+        guard samples.count >= 6 else { return .unknown }
+        let recent = samples.suffix(3)
+        let previous = samples.dropLast(3).suffix(3)
+        guard !previous.isEmpty else { return .unknown }
+
+        let now = recent.map(\.deviation).reduce(0, +) / Double(recent.count)
+        let before = previous.map(\.deviation).reduce(0, +) / Double(previous.count)
+        let change = now - before
+
+        if abs(change) < Self.trendThreshold { return .steady }
+        return change < 0 ? .improving : .worsening
+    }
+
+    /// Wieviel besser der beste Punkt gegenüber jetzt war – nil, wenn es sich
+    /// nicht lohnt oder wir gerade selbst am besten Punkt stehen.
+    var improvementAtBest: Double? {
+        guard let current, let best, best.time < current.time else { return nil }
+        let gain = current.deviation - best.deviation
+        return gain >= Self.worthGoingBack ? gain : nil
+    }
+
+    /// Wie lange der beste Punkt zurückliegt.
+    var timeSinceBest: TimeInterval? {
+        guard improvementAtBest != nil, let current, let best else { return nil }
+        return current.time.timeIntervalSince(best.time)
+    }
+
+    /// Was der Fahrer jetzt tun soll.
+    var advice: String {
+        guard let current else { return "Warte auf Messwerte…" }
+        if current.deviation <= LevelState.levelTolerance {
+            return "Steht eben – anhalten"
+        }
+        if let seconds = timeSinceBest {
+            return String(format: "Vor %.0f s stand es besser – ein Stück zurück",
+                          seconds.rounded())
+        }
+        switch trend {
+        case .improving: return "Wird besser – weiter so"
+        case .worsening: return "Wird schlechter – andere Richtung"
+        case .steady:    return "Ändert sich kaum – andere Richtung versuchen"
+        case .unknown:   return "Langsam weiterfahren"
+        }
+    }
+
+    var hasReachedTarget: Bool {
+        (current?.deviation ?? .infinity) <= LevelState.levelTolerance
+    }
+}
+
+/// Wie hoch jedes einzelne Rad unterlegt werden muss.
+///
+/// Getrennte Angaben wie „rechts 8 cm" und „vorne 4 cm" beschreiben dasselbe
+/// Fahrzeug und lassen sich nicht getrennt ausführen: Unter „rechts" liegen
+/// zwei Räder, unter „vorne" auch, und zwei davon sind dieselben. Gefragt ist
+/// deshalb die Höhe **je Rad**.
+///
+/// Das ist reine Geometrie und damit exakt. Das Fahrzeug steht auf vier
+/// Punkten; deren Höhen ergeben sich aus Spurweite, Radstand und den beiden
+/// Winkeln. Angehoben wird auf die Höhe des höchsten Rades – das bleibt
+/// liegen, die anderen bekommen die Differenz.
+struct LevelingLift: Equatable {
+
+    enum Wheel: CaseIterable, Hashable {
+        case frontLeft, frontRight, rearLeft, rearRight
+
+        var text: String {
+            switch self {
+            case .frontLeft:  return "vorne links"
+            case .frontRight: return "vorne rechts"
+            case .rearLeft:   return "hinten links"
+            case .rearRight:  return "hinten rechts"
+            }
+        }
+    }
+
+    /// Höhen in Metern, je Rad. Das höchste Rad steht auf 0.
+    let frontLeft: Double
+    let frontRight: Double
+    let rearLeft: Double
+    let rearRight: Double
+
+    func height(_ wheel: Wheel) -> Double {
+        switch wheel {
+        case .frontLeft:  return frontLeft
+        case .frontRight: return frontRight
+        case .rearLeft:   return rearLeft
+        case .rearRight:  return rearRight
+        }
+    }
+
+    func centimetres(_ wheel: Wheel) -> Double { height(wheel) * 100 }
+
+    /// Das am tiefsten stehende Rad – dort liegt die grösste Höhe.
+    var deepest: Wheel {
+        Wheel.allCases.max { height($0) < height($1) } ?? .frontLeft
+    }
+
+    var maximumCentimetres: Double { centimetres(deepest) }
+
+    /// Unter einem Zentimeter lohnt kein Keil; so genau steht kein Fahrzeug,
+    /// und so genau misst der Sensor auch nicht.
+    var isNegligible: Bool { maximumCentimetres < 1 }
+
+    /// `pitch` positiv heisst, das Heck steht höher; `roll` positiv, die rechte
+    /// Seite steht höher. Ohne Fahrzeugmasse gibt es nichts zu rechnen.
+    static func compute(pitch: Double,
+                        roll: Double,
+                        trackWidth: Double,
+                        wheelbase: Double) -> LevelingLift? {
+        guard trackWidth > 0, wheelbase > 0 else { return nil }
+
+        // Wie viel höher die rechte Seite und wie viel höher das Heck steht.
+        // Vorzeichen inbegriffen: tan(-2°) ist negativ, dann steht links höher.
+        let rightRise = tan(roll * .pi / 180) * trackWidth
+        let rearRise = tan(pitch * .pi / 180) * wheelbase
+
+        // Höhen der vier Aufstandspunkte, bezogen auf vorne links.
+        let heights: [Wheel: Double] = [
+            .frontLeft: 0,
+            .frontRight: rightRise,
+            .rearLeft: rearRise,
+            .rearRight: rightRise + rearRise,
+        ]
+        let top = heights.values.max() ?? 0
+
+        return LevelingLift(frontLeft: top - heights[.frontLeft]!,
+                            frontRight: top - heights[.frontRight]!,
+                            rearLeft: top - heights[.rearLeft]!,
+                            rearRight: top - heights[.rearRight]!)
+    }
+}