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

    static let identity = SensorOrientation()

    var isIdentity: Bool { self == .identity }

    /// Rechnet Sensorwerte in Fahrzeugwerte um.
    func apply(pitch: Double?, roll: Double?) -> (pitch: Double?, roll: Double?) {
        let longitudinal = longitudinalSource == .pitch ? pitch : roll
        let lateral = longitudinalSource == .pitch ? roll : pitch
        return (longitudinal.map { invertLongitudinal ? -$0 : $0 },
                lateral.map { invertLateral ? -$0 : $0 })
    }

    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") }
        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

    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
        return .success(orientation)
    }
}