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

    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 "Es wurde in beide Richtungen gleichzeitig gekippt. "
                     + "Möglichst nur um die eine Achse kippen."
            }
        }
    }

    /// Ermittelt aus beiden Kippbewegungen die Einbaulage.
    ///
    /// - 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> {
        // Welche Achse reagiert auf das Kippen nach vorne?
        guard let longitudinal = dominantAxis(nose) else {
            return .failure(max(abs(nose.pitch), abs(nose.roll)) < minimumTilt
                            ? .tooLittleMovement : .ambiguous)
        }
        guard let lateral = dominantAxis(side) else {
            return .failure(max(abs(side.pitch), abs(side.roll)) < minimumTilt
                            ? .tooLittleMovement : .ambiguous)
        }
        // Beide Bewegungen dürfen nicht auf dieselbe Achse zeigen – dann wurde
        // zweimal dasselbe gekippt.
        guard longitudinal.source != lateral.source else { return .failure(.ambiguous) }

        var orientation = SensorOrientation()
        orientation.longitudinalSource = longitudinal.source
        // Front nach unten heisst: das Heck steht höher, die Längsneigung des
        // Fahrzeugs ist also positiv.
        orientation.invertLongitudinal = longitudinal.change < 0
        // Linke Seite nach unten heisst: rechts steht höher, die Querneigung
        // ist positiv.
        orientation.invertLateral = lateral.change < 0
        return .success(orientation)
    }

    /// Die Achse, die sich deutlich stärker bewegt hat.
    private static func dominantAxis(_ change: Reading)
        -> (source: SensorOrientation.Source, change: Double)? {
        let byPitch = abs(change.pitch), byRoll = abs(change.roll)
        guard max(byPitch, byRoll) >= minimumTilt else { return nil }
        // Die schwächere Achse muss deutlich zurückstehen, sonst wurde schräg
        // gekippt und die Zuordnung wäre Zufall.
        guard max(byPitch, byRoll) >= 2 * min(byPitch, byRoll) else { return nil }
        return byPitch > byRoll ? (.pitch, change.pitch) : (.roll, change.roll)
    }
}