MicrOBU/app/src/main/java/com/hawhamburg/micr0bu/service/CamTransmitLoop.kt

package com.hawhamburg.micr0bu.service

import android.content.Context
import com.hawhamburg.micr0bu.data.GnssReading
import com.hawhamburg.micr0bu.data.SensorRepository
import com.hawhamburg.micr0bu.data.mqtt.ObuHardwarePreferences
import com.hawhamburg.micr0bu.data.transport.ObuHardware
import com.hawhamburg.micr0bu.data.transport.UsbSerialTransport
import com.hawhamburg.micr0bu.domain.asn1.RealAsn1UperCodec
import com.hawhamburg.micr0bu.domain.cam.CamTransmitConfig
import com.hawhamburg.micr0bu.domain.cam.PhoneCamBuilder
import com.hawhamburg.micr0bu.domain.usecase.GeoMath
import dagger.hilt.android.qualifiers.ApplicationContext
import kotlinx.coroutines.CoroutineScope
import kotlinx.coroutines.Dispatchers
import kotlinx.coroutines.Job
import kotlinx.coroutines.SupervisorJob
import kotlinx.coroutines.coroutineScope
import kotlinx.coroutines.delay
import kotlinx.coroutines.flow.collectLatest
import kotlinx.coroutines.launch
import javax.inject.Inject
import javax.inject.Singleton

/**
 * Real CAM transmit loop for the ESP32-C5 hardware path (Phase 03, Section 13) — the phone-side
 * counterpart to the OBU's old autonomous beacon, now driven from here since the ESP32-C5 has
 * no onboard CAM generation at all (see `obu-firmware/main/main.c`'s rewritten TX path, which
 * is purely receive-and-transmit-on-serial-arrival with no timer of its own).
 *
 * Started/stopped by [TripRecordingService] around an active recording session — per the
 * Section 13 spec, CAM transmission only runs while recording, matching the CiT One path's
 * behavior of "no traffic until there's a trip to correlate it with." Internally also gated on
 * [ObuHardwarePreferences] currently reporting [ObuHardware.ESP32_C5] — on the CiT One path
 * this loop stays parked (via [kotlinx.coroutines.flow.collectLatest] on the hardware
 * preference) and never sends anything.
 *
 * Rate policy: [CamTransmitConfig.baseRateHz] (1 Hz) everywhere, bumped to
 * [CamTransmitConfig.elevatedRateHz] inside a [com.hawhamburg.micr0bu.domain.cam.CamGeofence] or
 * for [ELEVATED_HOLD_MS] after an external event trigger (harsh braking/turning/stopping — see
 * [onDetectedEvent], called by [TripRecordingService] from the same
 * [com.hawhamburg.micr0bu.domain.detection.EventDetector] stream that already drives trip event
 * logging). Both rate figures are placeholders pending real-world tuning, per
 * [CamTransmitConfig]'s own disclaimer.
 */
@Singleton
class CamTransmitLoop @Inject constructor(
    @ApplicationContext private val context: Context,
    private val obuHardwarePrefs: ObuHardwarePreferences,
    private val usbSerialTransport: UsbSerialTransport,
    private val codec: RealAsn1UperCodec,
) {
    private val config = CamTransmitConfig()
    private val sensorRepository = SensorRepository(context)

    private var job: Job? = null
    private val scope = CoroutineScope(SupervisorJob() + Dispatchers.Default)

    @Volatile private var latestGnss: GnssReading? = null
    @Volatile private var latestGyroZ: Float? = null
    @Volatile private var elevatedUntilMs: Long = 0L

    /** Previous GNSS fix, kept only to derive along-track acceleration — see [longitudinalAccel]. */
    @Volatile private var previousGnss: GnssReading? = null

    /**
     * Own station id for the ESP32-C5 path, loaded once per [start] from
     * [ObuHardwarePreferences.getOrCreateOwnStationId]. 0 means "not loaded yet" — the loop waits
     * for the real value rather than beaconing as station 0, which would be indistinguishable
     * from every other MicrOBU to any receiver.
     */
    @Volatile var stationId: Long = 0L

    /**
     * Call when a braking/turning/stopping event fires during an active trip — bumps the CAM
     * rate to [CamTransmitConfig.elevatedRateHz] for [ELEVATED_HOLD_MS] so nearby stations get
     * denser updates through the maneuver, not just at the instant it was detected.
     */
    fun onDetectedEvent() {
        elevatedUntilMs = System.currentTimeMillis() + ELEVATED_HOLD_MS
    }

    /**
     * Starts the loop for the duration of a recording session. Internally stays idle (no
     * transmission) unless/until the ESP32-C5 is the selected OBU hardware, and automatically
     * pauses/resumes if that selection changes mid-trip.
     */
    fun start() {
        if (job?.isActive == true) return
        elevatedUntilMs = 0L
        previousGnss = null
        job = scope.launch {
            stationId = obuHardwarePrefs.getOrCreateOwnStationId()
            obuHardwarePrefs.obuHardwareFlow.collectLatest { hardware ->
                if (hardware != ObuHardware.ESP32_C5) return@collectLatest
                runTransmitLoop()
            }
        }
    }

    fun stop() {
        job?.cancel()
        job = null
        elevatedUntilMs = 0L
    }

    private suspend fun runTransmitLoop() = coroutineScope {
        launch { sensorRepository.gnssFlow().collect { latestGnss = it } }
        launch { sensorRepository.gyroscopeFlow().collect { latestGyroZ = it.z } }

        while (true) {
            val gnss = latestGnss
            if (gnss != null) {
                val cam = PhoneCamBuilder.build(gnss, latestGyroZ, stationId, longitudinalAccel(gnss))
                val bytes = codec.encodeCam(cam)
                usbSerialTransport.sendCamTx(bytes)
            }
            delay((1000.0 / currentRateHz(gnss)).toLong())
        }
    }

    /**
     * Along-track acceleration in m/s², from the change in GNSS speed since the previous fix.
     *
     * Deliberately not from the accelerometer: CAM's `longitudinalAcceleration` is acceleration
     * along the direction of travel, while the raw accelerometer reads in the device frame with
     * gravity included — extracting the along-track component from it needs a full orientation
     * estimate, which this path doesn't have (the detection engine sidesteps the same problem by
     * working on orientation-independent magnitudes, which is not what CAM wants here).
     *
     * Returns null — encoded as ASN.1 `unavailable` — when there's no usable previous fix, when
     * the gap is too short to divide by safely, or when it's long enough that the two samples
     * aren't really consecutive. Better an honest "unavailable" than a fabricated number a
     * receiving vehicle might brake on.
     */
    private fun longitudinalAccel(gnss: GnssReading): Double? {
        val prev = previousGnss
        previousGnss = gnss
        if (prev == null) return null

        val dtSec = (gnss.timestamp - prev.timestamp) / 1000.0
        if (dtSec < MIN_ACCEL_DT_SEC || dtSec > MAX_ACCEL_DT_SEC) return null

        val dv = gnss.speedMs.toDouble() - prev.speedMs.toDouble()
        return dv / dtSec
    }

    private fun currentRateHz(gnss: GnssReading?): Double {
        val now = System.currentTimeMillis()
        val inGeofence = gnss != null && config.geofences.any { fence ->
            GeoMath.haversineMeters(gnss.latitude, gnss.longitude, fence.latitude, fence.longitude) <= fence.radiusM
        }
        val eventBoosted = now < elevatedUntilMs
        return if (inGeofence || eventBoosted) config.elevatedRateHz else config.baseRateHz
    }

    companion object {
        private const val ELEVATED_HOLD_MS = 5_000L

        /** Below this gap, GNSS speed noise divided by a tiny dt produces absurd accelerations. */
        private const val MIN_ACCEL_DT_SEC = 0.2

        /** Above this gap the two fixes aren't consecutive enough to call the result acceleration. */
        private const val MAX_ACCEL_DT_SEC = 3.0
    }
}