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 ein Auffahrkeil sein muss, um eine Neigung auszugleichen.
///
/// Rein geometrisch und damit exakt: Höhe = tan(Winkel) × Abstand der Achsen
/// beziehungsweise der Räder.
struct LevelingWedge: Equatable {
    /// Wo der Keil hin muss.
    let side: Side
    /// Höhe in Metern.
    let height: Double
    /// Der zugrundeliegende Winkel in Grad.
    let angle: Double

    enum Side: Equatable {
        case front, rear, left, right

        var text: String {
            switch self {
            case .front: return "vorne"
            case .rear:  return "hinten"
            case .left:  return "links"
            case .right: return "rechts"
            }
        }
    }

    var heightInCentimetres: Double { height * 100 }

    /// Quer: die tieferliegende Seite muss angehoben werden. Positiver Roll
    /// heisst, dass rechts höher steht – der Keil gehört also nach links.
    static func across(roll: Double, trackWidth: Double) -> LevelingWedge? {
        wedge(angle: roll, distance: trackWidth,
              whenPositive: .left, whenNegative: .right)
    }

    /// Längs: positiver Pitch heisst, dass das Heck höher steht – der Keil
    /// gehört unter die Vorderräder.
    static func along(pitch: Double, wheelbase: Double) -> LevelingWedge? {
        wedge(angle: pitch, distance: wheelbase,
              whenPositive: .front, whenNegative: .rear)
    }

    private static func wedge(angle: Double,
                              distance: Double,
                              whenPositive: Side,
                              whenNegative: Side) -> LevelingWedge? {
        guard distance > 0, abs(angle) > LevelState.levelTolerance else { return nil }
        let height = tan(abs(angle) * .pi / 180) * distance
        return LevelingWedge(side: angle > 0 ? whenPositive : whenNegative,
                             height: height,
                             angle: abs(angle))
    }
}