/**
 * @file gauge_control.c
 * @brief Analog gauge control for dashboard
 */

#include "kernel.h"
#include "pwm_driver.h"
#include "adc_driver.h"
#include "can_driver.h"
#include <string.h>
#include <math.h>

/* Gauge Control State */
typedef struct {
    bool initialized;
    uint16_t speed_gauge_position;
    uint16_t rpm_gauge_position;
    uint16_t fuel_gauge_position;
    uint16_t temp_gauge_position;
    uint16_t target_positions[4];
    Mutex_t mutex;
} GaugeControlState_t;

static GaugeControlState_t gauge_control;

/* Initialize Gauge Control */
KernelStatus_t gauge_control_init(void) {
    if (gauge_control.initialized) {
        return KERNEL_ERROR;
    }

    memset(&gauge_control, 0, sizeof(GaugeControlState_t));

    /* Initialize PWM for gauges */
    pwm_init(3, &(PwmConfig_t){
        .frequency_hz = 100,  /* 100 Hz for smooth gauge movement */
        .channel_count = 4,
        .channels = {
            {.channel = 0, .duty_cycle = 0},  /* Speedometer */
            {.channel = 1, .duty_cycle = 0},  /* Tachometer */
            {.channel = 2, .duty_cycle = 0},  /* Fuel gauge */
            {.channel = 3, .duty_cycle = 0}   /* Temperature gauge */
        }
    });

    mutex_create(&gauge_control.mutex, false);

    gauge_control.initialized = true;
    return KERNEL_OK;
}

/* Gauge Control Task */
void gauge_control_task(void* parameters) {
    (void)parameters;

    while (1) {
        /* Read CAN messages for gauge data */
        CanMessage_t message;

        while (can_receive_message(&message, 0) == KERNEL_OK) {
            process_gauge_message(&message);
        }

        /* Smooth gauge movement */
        smooth_gauge_movement();

        /* 10ms update rate */
        kernel_delay(10);
    }
}

/* Process Gauge Message */
static void process_gauge_message(const CanMessage_t* message) {
    mutex_lock(&gauge_control.mutex, 100);

    switch (message->id.id) {
        case 0x300:  /* Engine data */
            /* RPM: 0-8000 RPM maps to 0-10000 duty cycle */
            gauge_control.target_positions[1] =
                ((message->data[0] << 8) | message->data[1]) * 10000 / 8000;

            /* Temperature: -40 to 120°C maps to 0-10000 */
            int16_t temp = (message->data[2] << 8) | message->data[3];
            gauge_control.target_positions[3] =
                (temp + 40) * 10000 / 160;
            break;

        case 0x301:  /* Vehicle speed */
            /* Speed: 0-240 km/h maps to 0-10000 */
            gauge_control.target_positions[0] =
                ((message->data[0] << 8) | message->data[1]) * 10000 / 240;
            break;

        case 0x302:  /* Fuel level */
            /* Fuel: 0-100% maps to 0-10000 */
            gauge_control.target_positions[2] = message->data[0] * 100;
            break;
    }

    mutex_unlock(&gauge_control.mutex);
}

/* Smooth Gauge Movement */
static void smooth_gauge_movement(void) {
    mutex_lock(&gauge_control.mutex, 100);

    /* Smooth movement for each gauge */
    for (int i = 0; i < 4; i++) {
        uint16_t current = 0;
        uint16_t target = gauge_control.target_positions[i];

        /* Get current position */
        switch (i) {
            case 0:
                current = gauge_control.speed_gauge_position;
                break;
            case 1:
                current = gauge_control.rpm_gauge_position;
                break;
            case 2:
                current = gauge_control.fuel_gauge_position;
                break;
            case 3:
                current = gauge_control.temp_gauge_position;
                break;
        }

        /* Calculate new position with smoothing */
        int32_t delta = target - current;
        uint16_t new_position = current + (delta / 10);  /* 10% movement per update */

        /* Update position */
        switch (i) {
            case 0:
                gauge_control.speed_gauge_position = new_position;
                break;
            case 1:
                gauge_control.rpm_gauge_position = new_position;
                break;
            case 2:
                gauge_control.fuel_gauge_position = new_position;
                break;
            case 3:
                gauge_control.temp_gauge_position = new_position;
                break;
        }

        /* Update PWM output */
        pwm_set_duty_cycle(3, i, new_position);
    }

    mutex_unlock(&gauge_control.mutex);
}

/* Calibrate Gauges */
KernelStatus_t gauge_calibrate(uint8_t gauge, uint16_t min_position,
                               uint16_t max_position) {
    if (!gauge_control.initialized || gauge >= 4) {
        return KERNEL_ERROR;
    }

    mutex_lock(&gauge_control.mutex, 100);

    /* Set gauge to minimum position */
    pwm_set_duty_cycle(3, gauge, min_position);
    kernel_delay(1000);  /* Wait 1 second */

    /* Set gauge to maximum position */
    pwm_set_duty_cycle(3, gauge, max_position);
    kernel_delay(1000);  /* Wait 1 second */

    /* Return to zero */
    pwm_set_duty_cycle(3, gauge, min_position);

    mutex_unlock(&gauge_control.mutex);

    return KERNEL_OK;
}

/* Self-Test Gauges */
KernelStatus_t gauge_self_test(void) {
    if (!gauge_control.initialized) {
        return KERNEL_ERROR;
    }

    /* Perform gauge sweep */
    for (uint16_t position = 0; position <= 10000; position += 100) {
        for (int i = 0; i < 4; i++) {
            pwm_set_duty_cycle(3, i, position);
        }
        kernel_delay(10);
    }

    /* Return to zero */
    for (int i = 0; i < 4; i++) {
        pwm_set_duty_cycle(3, i, 0);
    }

    return KERNEL_OK;
}