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]!)
    }
}