// =============================================================================
//  rudder_sensor.cpp -- AI1 rudder-angle sensor with median filter
// =============================================================================

#include "rudder_sensor.h"

#include <Arduino.h>

#include "../filters/median.h"
#include "../system/ar_log.h"
#include "../system/task_config.h"
#include "pinout.h"

namespace arautopilot::hal {

namespace {
constexpr const char* TAG = "AR/HAL";

// Calibration defaults: full-range ADC mapped to +-35 deg of rudder.
// Reasonable for an unconfigured bench but NEVER ship this to a customer
// vessel -- it must be replaced during sea-trial commissioning (Sprint 7).
constexpr int16_t DEFAULT_RAW_AT_MIN = 0;
constexpr int16_t DEFAULT_RAW_AT_MAX = 4095;
constexpr float DEFAULT_MIN_DEG = -35.0f;
constexpr float DEFAULT_MAX_DEG = +35.0f;

filters::MedianFilter<int16_t, 5> g_median{};
portMUX_TYPE g_mux = portMUX_INITIALIZER_UNLOCKED;
RudderReading g_latest{0, 0.0f, 0, false};

struct Calibration {
    int16_t raw_at_min;
    int16_t raw_at_max;
    float min_deg;
    float max_deg;
};
Calibration g_calib{DEFAULT_RAW_AT_MIN, DEFAULT_RAW_AT_MAX, DEFAULT_MIN_DEG,
                    DEFAULT_MAX_DEG};

float raw_to_deg(int16_t raw, const Calibration& c) {
    const float span_raw = static_cast<float>(c.raw_at_max - c.raw_at_min);
    if (span_raw == 0.0f) {
        return 0.0f;
    }
    const float t = (static_cast<float>(raw - c.raw_at_min)) / span_raw;
    return c.min_deg + t * (c.max_deg - c.min_deg);
}

void SensorTask(void* /*pv*/) {
    AR_LOGI(TAG, "rudder_sensor task started on core %d, pin %d (ADC1)",
            xPortGetCoreID(), PIN_AI1_RUDDER_ANGLE);
    TickType_t last_wake = xTaskGetTickCount();
    for (;;) {
        const int raw = analogRead(PIN_AI1_RUDDER_ANGLE);
        const int16_t filtered = g_median.push(static_cast<int16_t>(raw));
        Calibration cal;
        portENTER_CRITICAL(&g_mux);
        cal = g_calib;
        portEXIT_CRITICAL(&g_mux);
        const float angle = raw_to_deg(filtered, cal);

        portENTER_CRITICAL(&g_mux);
        g_latest.raw_adc = filtered;
        g_latest.angle_deg = angle;
        g_latest.timestamp_ms = millis();
        g_latest.valid = g_median.is_full();
        portEXIT_CRITICAL(&g_mux);

        vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(AR_PERIOD_MS_RUDDER_SENSOR));
    }
}
}  // namespace

void rudder_sensor_init() {
    analogReadResolution(12);                 // 0..4095
    analogSetAttenuation(ADC_11db);            // ~0..3.3 V input range
    g_median.reset();
    AR_LOGI(TAG, "rudder_sensor_init: 12-bit ADC, 11dB attn, %d-sample median",
            5);
}

void rudder_sensor_start_task() {
    xTaskCreatePinnedToCore(SensorTask, "rudder_sensor",
                            AR_TASK_STACK_RUDDER_SENSOR, nullptr,
                            AR_TASK_PRIO_RUDDER_SENSOR, nullptr,
                            AR_TASK_CORE_REALTIME);
}

RudderReading rudder_sensor_latest() {
    RudderReading copy;
    portENTER_CRITICAL(&g_mux);
    copy = g_latest;
    portEXIT_CRITICAL(&g_mux);
    return copy;
}

void rudder_sensor_set_calibration(int16_t raw_at_min_deg, int16_t raw_at_max_deg,
                                   float min_deg, float max_deg) {
    portENTER_CRITICAL(&g_mux);
    g_calib = {raw_at_min_deg, raw_at_max_deg, min_deg, max_deg};
    portEXIT_CRITICAL(&g_mux);
    AR_LOGI(TAG,
            "rudder_sensor calibration updated: raw [%d..%d] -> deg [%.2f..%.2f]",
            raw_at_min_deg, raw_at_max_deg, min_deg, max_deg);
}

}  // namespace arautopilot::hal
