VanAligneiOS/Shared/Models/SensorOrientation.swift

import Foundation

/// Wie der Neigungsmesser im Fahrzeug sitzt.
///
/// Der Sensor kann quer eingebaut, gedreht oder kopfüber montiert sein. Dann
/// stimmen seine Achsen nicht mit denen des Fahrzeugs überein: was er als
/// Längsneigung meldet, ist womöglich die Querneigung, und Vorzeichen können
/// vertauscht sein.
///
/// Statt das raten zu lassen, ermittelt der Einrichtungsassistent die Lage
/// durch zwei definierte Kippbewegungen.
struct SensorOrientation: Codable, Equatable, Hashable, Sendable {

    /// Welche Achse des Sensors die Längsneigung des Fahrzeugs liefert.
    enum Source: String, Codable, Hashable, Sendable {
        case pitch
        case roll
    }

    /// Woher die Längsneigung kommt. Die Querneigung kommt aus der jeweils
    /// anderen Achse.
    var longitudinalSource: Source = .pitch
    var invertLongitudinal = false
    var invertLateral = false

    /// Verdrehung des Sensors um die Hochachse, in Grad – der Rest, den der
    /// Achsentausch nicht abdeckt.
    ///
    /// Sitzt der Sensor schräg im Fahrzeug, verteilt sich eine reine
    /// Querneigung auf beide Sensorachsen: Das Fahrzeug kippt zur Seite, die
    /// Anzeige meldet zusätzlich Längsneigung. Achsentausch und Vorzeichen
    /// helfen dagegen nicht, die springen in 90°-Schritten.
    ///
    /// Der Wert wird beim Bestimmen der Einbaulage mitgemessen und kostet
    /// keinen zusätzlichen Handgriff. Beim **Kalibrieren** liesse er sich
    /// nicht ermitteln: Das misst eine einzige Lage und zieht sie als Nullpunkt
    /// ab – eine Drehung um die Hochachse ist darin nicht enthalten, denn eben
    /// sieht in jeder Verdrehung gleich aus.
    var twist: Double = 0

    static let identity = SensorOrientation()

    init() {}

    /// Von Hand geschrieben, nicht von Swift erzeugt – und das ist der Punkt.
    ///
    /// Die erzeugte Fassung verlangt beim Lesen jedes Feld; Standardwerte im
    /// Code zählen dabei nicht. Eine Einbaulage, die vor `twist` gespeichert
    /// wurde, liesse sich damit nicht mehr lesen, das Gerät dazu ebenso wenig,
    /// und die Geräteliste bliebe leer. Jedes neue Feld gehört deshalb hier
    /// hinein, mit `decodeIfPresent` und einem Rückfallwert.
    init(from decoder: Decoder) throws {
        let container = try decoder.container(keyedBy: CodingKeys.self)
        longitudinalSource = try container.decodeIfPresent(Source.self,
                                                           forKey: .longitudinalSource) ?? .pitch
        invertLongitudinal = try container.decodeIfPresent(Bool.self,
                                                           forKey: .invertLongitudinal) ?? false
        invertLateral = try container.decodeIfPresent(Bool.self, forKey: .invertLateral) ?? false
        twist = try container.decodeIfPresent(Double.self, forKey: .twist) ?? 0
    }

    var isIdentity: Bool { self == .identity }

    /// Rechnet Sensorwerte in Fahrzeugwerte um.
    ///
    /// Zwei Schritte, in dieser Reihenfolge: erst die grobe Zuordnung der
    /// Achsen samt Vorzeichen, dann die Verdrehung zurückdrehen. Für kleine
    /// Winkel verhält sich das Wertepaar wie ein Vektor in der Ebene – genau
    /// deshalb lässt sich die Verdrehung überhaupt herausrechnen.
    func apply(pitch: Double?, roll: Double?) -> (pitch: Double?, roll: Double?) {
        let longitudinal = longitudinalSource == .pitch ? pitch : roll
        let lateral = longitudinalSource == .pitch ? roll : pitch
        let mappedLongitudinal = longitudinal.map { invertLongitudinal ? -$0 : $0 }
        let mappedLateral = lateral.map { invertLateral ? -$0 : $0 }

        guard twist != 0, let long = mappedLongitudinal, let lat = mappedLateral else {
            return (mappedLongitudinal, mappedLateral)
        }
        let angle = twist * .pi / 180
        return (long * cos(angle) + lat * sin(angle),
                -long * sin(angle) + lat * cos(angle))
    }

    var summary: String {
        if isIdentity { return "Achsen unverändert" }
        var parts: [String] = []
        if longitudinalSource == .roll { parts.append("Achsen getauscht") }
        if invertLongitudinal { parts.append("längs umgekehrt") }
        if invertLateral { parts.append("quer umgekehrt") }
        if twist != 0 { parts.append(String(format: "um %.0f° verdreht", twist)) }
        return parts.joined(separator: ", ")
    }
}

/// Wertet die Kippbewegungen des Einrichtungsassistenten aus.
///
/// Zweimal wird gekippt: einmal die Front nach unten, einmal die linke Seite
/// nach unten. Welche Sensorachse sich dabei jeweils bewegt und in welche
/// Richtung, ergibt die Einbaulage.
enum OrientationDetection {

    /// So weit muss gekippt werden, damit die Bewegung eindeutig ist.
    /// Darunter wäre nicht zu unterscheiden, ob überhaupt gekippt wurde.
    static let minimumTilt = 5.0

    /// Soviel deutlicher muss die gewinnende Deutung sein als die andere.
    static let ambiguityMargin = 1.3

    /// Ab hier gilt eine Verdrehung als echt und nicht als Wackeln der Hand.
    static let minimumTwist = 2.0

    struct Reading: Equatable {
        let pitch: Double
        let roll: Double

        static func - (lhs: Reading, rhs: Reading) -> Reading {
            Reading(pitch: lhs.pitch - rhs.pitch, roll: lhs.roll - rhs.roll)
        }
    }

    enum Failure: Error, Equatable {
        /// Es wurde zu wenig oder gar nicht gekippt.
        case tooLittleMovement
        /// Beide Achsen haben sich ähnlich stark bewegt – dann lässt sich
        /// nicht sagen, welche gemeint war.
        case ambiguous

        var message: String {
            switch self {
            case .tooLittleMovement:
                return "Zu wenig Bewegung. Deutlicher kippen, mindestens eine "
                     + "Handbreit."
            case .ambiguous:
                return "Beide Schritte haben dieselbe Achse bewegt. Im ersten "
                     + "Schritt nach vorne kippen, im zweiten zur Seite."
            }
        }
    }

    /// Ermittelt aus beiden Kippbewegungen die Einbaulage.
    ///
    /// Beide Bewegungen werden gemeinsam beurteilt. Kippt man von Hand zur
    /// Seite, geht die andere Achse fast immer ein Stück mit – jede Bewegung
    /// für sich betrachtet sähe das nach schrägem Kippen aus. Im Paar ist die
    /// Zuordnung trotzdem eindeutig: es gewinnt die Deutung, die beide
    /// Bewegungen zusammen am besten erklärt.
    ///
    /// - Parameters:
    ///   - nose: Änderung beim Kippen der Front nach unten.
    ///   - side: Änderung beim Kippen der linken Seite nach unten.
    static func orientation(nose: Reading,
                            side: Reading) -> Result<SensorOrientation, Failure> {
        guard max(abs(nose.pitch), abs(nose.roll)) >= minimumTilt,
              max(abs(side.pitch), abs(side.roll)) >= minimumTilt else {
            return .failure(.tooLittleMovement)
        }

        // Zwei mögliche Deutungen, bewertet danach, wieviel Bewegung sie
        // erklären: die erste Achse längs und die zweite quer – oder umgekehrt.
        let pitchIsLongitudinal = abs(nose.pitch) + abs(side.roll)
        let rollIsLongitudinal = abs(nose.roll) + abs(side.pitch)
        let winner = max(pitchIsLongitudinal, rollIsLongitudinal)
        let loser = min(pitchIsLongitudinal, rollIsLongitudinal)
        // Liegen beide Deutungen dicht beieinander, ist die Zuordnung wirklich
        // nicht zu treffen – etwa wenn zweimal um dieselbe Achse gekippt wurde.
        guard winner >= ambiguityMargin * loser else { return .failure(.ambiguous) }

        var orientation = SensorOrientation()
        orientation.longitudinalSource = pitchIsLongitudinal > rollIsLongitudinal ? .pitch : .roll
        let longitudinal = orientation.longitudinalSource == .pitch ? nose.pitch : nose.roll
        let lateral = orientation.longitudinalSource == .pitch ? side.roll : side.pitch
        // Die zugeordnete Achse muss in ihrem Schritt auch deutlich gewandert
        // sein, sonst stünde das Vorzeichen auf wackligem Grund.
        guard abs(longitudinal) >= minimumTilt, abs(lateral) >= minimumTilt else {
            return .failure(.tooLittleMovement)
        }
        // Front nach unten heisst: das Heck steht höher, die Längsneigung des
        // Fahrzeugs ist also positiv.
        orientation.invertLongitudinal = longitudinal < 0
        // Linke Seite nach unten heisst: rechts steht höher, die Querneigung
        // ist positiv.
        orientation.invertLateral = lateral < 0

        // Was nach dem Achsentausch noch übrig ist, ist die Verdrehung um die
        // Hochachse. Beim Kippen der Front nach unten dürfte sich nur die
        // Längsneigung ändern; wandert die Querneigung mit, sitzt der Sensor
        // schräg – und zwar um genau diesen Winkel.
        //
        // Gemessen wird an der Frontbewegung, nicht an der Seitenbewegung: Die
        // Front lässt sich am Fahrzeug genauer treffen, und beide Schritte
        // gemeinsam auszuwerten brächte hier nichts, weil eine Verdrehung auf
        // beide gleich wirkt.
        let corrected = orientation.apply(pitch: nose.pitch, roll: nose.roll)
        if let long = corrected.pitch, let lat = corrected.roll {
            let residual = atan2(lat, long) * 180 / .pi
            // Unter zwei Grad ist es Messrauschen. Zwei Kippbewegungen von Hand
            // sind nicht genauer, und eine erfundene Verdrehung wäre schlimmer
            // als keine.
            orientation.twist = abs(residual) >= minimumTwist ? residual : 0
        }
        return .success(orientation)
    }
}