/**
 * @file engine_control_task.c
 * @brief Main engine control task
 */

#include "engine_control.h"
#include "fuel_injection.h"
#include "ignition_control.h"
#include "adc_driver.h"
#include "gpio_driver.h"
#include "pwm_driver.h"
#include "dtc_manager.h"
#include <string.h>
#include <math.h>

/* Engine Control State */
typedef struct {
    EngineState_t state;
    EngineSensorData_t sensors;
    EngineActuatorData_t actuators;
    EngineFaultCode_t faults[20];
    uint8_t fault_count;
    uint32_t engine_run_time;
    Mutex_t data_mutex;
    bool initialized;
} EngineControlState_t;

static EngineControlState_t engine_control;

/* PID Controller Structure */
typedef struct {
    PidGains_t gains;
    float integral;
    float previous_error;
    float output;
} PidController_t;

static PidController_t fuel_pid;
static PidController_t idle_pid;
static PidController_t boost_pid;

/* Initialize Engine Control */
KernelStatus_t engine_control_init(void) {
    if (engine_control.initialized) {
        return KERNEL_ERROR;
    }

    /* Initialize state */
    memset(&engine_control, 0, sizeof(EngineControlState_t));
    engine_control.state = ENGINE_STATE_OFF;
    engine_control.fault_count = 0;

    /* Initialize PID controllers */
    fuel_pid.gains = fuel_pid_gains;
    fuel_pid.integral = 0;
    fuel_pid.previous_error = 0;
    fuel_pid.output = 0;

    idle_pid.gains = idle_pid_gains;
    idle_pid.integral = 0;
    idle_pid.previous_error = 0;
    idle_pid.output = 0;

    boost_pid.gains = boost_pid_gains;
    boost_pid.integral = 0;
    boost_pid.previous_error = 0;
    boost_pid.output = 0;

    /* Create mutex */
    mutex_create(&engine_control.data_mutex, false);

    /* Initialize sensors */
    adc_init(0, &(AdcConfig_t){
        .resolution = ADC_RESOLUTION_12BIT,
        .mode = ADC_MODE_SCAN,
        .trigger_source = ADC_TRIGGER_TIMER,
        .reference = ADC_REFERENCE_VDD,
        .channel_count = 8,
        .channels = {
            {.channel = 0, .sampling_time = ADC_SAMPLING_28_5_CYCLES},  /* Coolant temp */
            {.channel = 1, .sampling_time = ADC_SAMPLING_28_5_CYCLES},  /* Intake temp */
            {.channel = 2, .sampling_time = ADC_SAMPLING_28_5_CYCLES},  /* Manifold pressure */
            {.channel = 3, .sampling_time = ADC_SAMPLING_28_5_CYCLES},  /* Throttle position */
            {.channel = 4, .sampling_time = ADC_SAMPLING_28_5_CYCLES},  /* Accelerator pedal */
            {.channel = 5, .sampling_time = ADC_SAMPLING_28_5_CYCLES},  /* Oil pressure */
            {.channel = 6, .sampling_time = ADC_SAMPLING_28_5_CYCLES},  /* Fuel pressure */
            {.channel = 7, .sampling_time = ADC_SAMPLING_28_5_CYCLES}   /* Battery voltage */
        }
    });

    /* Initialize actuators */
    pwm_init(0, &(PwmConfig_t){
        .frequency_hz = 1000,
        .alignment = PWM_ALIGNMENT_EDGE,
        .channel_count = 4,
        .channels = {
            {.channel = 0, .duty_cycle = 0},  /* Injector 1 */
            {.channel = 1, .duty_cycle = 0},  /* Injector 2 */
            {.channel = 2, .duty_cycle = 0},  /* Idle air control */
            {.channel = 3, .duty_cycle = 0}   /* Boost control */
        }
    });

    engine_control.initialized = true;
    return KERNEL_OK;
}

/* Start Engine */
KernelStatus_t engine_control_start(void) {
    if (!engine_control.initialized) {
        return KERNEL_ERROR;
    }

    engine_control.state = ENGINE_STATE_CRANKING;

    /* Start PWM outputs */
    pwm_start(0);

    /* Start ADC conversions */
    adc_start_conversion(0);

    return KERNEL_OK;
}

/* Stop Engine */
KernelStatus_t engine_control_stop(void) {
    if (!engine_control.initialized) {
        return KERNEL_ERROR;
    }

    /* Stop actuators */
    pwm_stop(0);

    /* Stop ADC */
    adc_stop_conversion(0);

    engine_control.state = ENGINE_STATE_OFF;

    return KERNEL_OK;
}

/* Main Engine Control Task */
void engine_control_task(void* parameters) {
    (void)parameters;

    TickType_t last_wake_time = kernel_get_tick_count();

    while (1) {
        /* Wait for next control period */
        kernel_delay(ENGINE_CONTROL_PERIOD_MS);

        switch (engine_control.state) {
            case ENGINE_STATE_CRANKING:
                /* Check RPM for engine start */
                if (engine_control.sensors.rpm > 400) {
                    engine_control.state = ENGINE_STATE_RUNNING;
                }
                break;

            case ENGINE_STATE_RUNNING:
                /* Check for idle condition */
                if (engine_control.sensors.rpm < ENGINE_IDLE_RPM + 50 &&
                    engine_control.sensors.accelerator_pedal < 5) {
                    engine_control.state = ENGINE_STATE_IDLE;
                }

                /* Check for acceleration */
                if (engine_control.sensors.accelerator_pedal > 80) {
                    engine_control.state = ENGINE_STATE_ACCELERATING;
                }
                break;

            case ENGINE_STATE_IDLE:
                /* Idle speed control */
                idle_pid_control();

                /* Check if leaving idle */
                if (engine_control.sensors.accelerator_pedal > 5) {
                    engine_control.state = ENGINE_STATE_RUNNING;
                }
                break;

            case ENGINE_STATE_ACCELERATING:
                /* Acceleration enrichment */
                engine_control.actuators.injector_pulse_width *= 1.2f;

                /* Check if still accelerating */
                if (engine_control.sensors.accelerator_pedal < 80) {
                    engine_control.state = ENGINE_STATE_RUNNING;
                }
                break;

            case ENGINE_STATE_DECELERATING:
                /* Deceleration fuel cutoff */
                if (engine_control.sensors.rpm > 1500 &&
                    engine_control.sensors.accelerator_pedal < 2) {
                    engine_control.actuators.injector_pulse_width = 0;
                }

                /* Check if still decelerating */
                if (engine_control.sensors.accelerator_pedal > 2) {
                    engine_control.state = ENGINE_STATE_RUNNING;
                }
                break;

            case ENGINE_STATE_FAULT:
                /* Handle faults */
                handle_engine_faults();
                break;

            case ENGINE_STATE_LIMP_HOME:
                /* Limp home mode */
                engine_control.actuators.injector_pulse_width =
                    fuel_map_values[0][3];  /* Fixed fuel */
                engine_control.actuators.ignition_advance =
                    ignition_map_values[0][3];  /* Fixed timing */
                break;

            default:
                break;
        }

        /* Update engine run time */
        engine_control.engine_run_time += ENGINE_CONTROL_PERIOD_MS;

        /* Check for faults */
        check_engine_faults();

        last_wake_time = kernel_get_tick_count();
    }
}

/* Idle Speed Control */
static void idle_pid_control(void) {
    int32_t rpm_error = ENGINE_IDLE_RPM - engine_control.sensors.rpm;

    /* PID calculation */
    idle_pid.integral += rpm_error * (IDLE_CONTROL_PERIOD_MS / 1000.0f);

    /* Limit integral */
    if (idle_pid.integral > idle_pid.gains.integral_limit) {
        idle_pid.integral = idle_pid.gains.integral_limit;
    } else if (idle_pid.integral < -idle_pid.gains.integral_limit) {
        idle_pid.integral = -idle_pid.gains.integral_limit;
    }

    float derivative = (rpm_error - idle_pid.previous_error) /
                       (IDLE_CONTROL_PERIOD_MS / 1000.0f);

    idle_pid.output = idle_pid.gains.kp * rpm_error +
                      idle_pid.gains.ki * idle_pid.integral +
                      idle_pid.gains.kd * derivative;

    /* Limit output */
    if (idle_pid.output > idle_pid.gains.output_limit) {
        idle_pid.output = idle_pid.gains.output_limit;
    } else if (idle_pid.output < 0) {
        idle_pid.output = 0;
    }

    /* Update actuator */
    engine_control.actuators.idle_air_control = (uint16_t)idle_pid.output;

    idle_pid.previous_error = rpm_error;
}

/* Check Engine Faults */
static void check_engine_faults(void) {
    /* Check coolant temperature */
    if (engine_control.sensors.coolant_temp > ENGINE_MAX_COOLANT_TEMP) {
        add_engine_fault(ENGINE_FAULT_OVERHEAT);
        engine_control.state = ENGINE_STATE_FAULT;
    }

    /* Check oil pressure */
    if (engine_control.sensors.oil_pressure < ENGINE_MIN_OIL_PRESSURE &&
        engine_control.sensors.rpm > 1000) {
        add_engine_fault(ENGINE_FAULT_LOW_OIL_PRESSURE);
        engine_control.state = ENGINE_STATE_FAULT;
    }

    /* Check battery voltage */
    if (engine_control.sensors.battery_voltage < 9.0f ||
        engine_control.sensors.battery_voltage > 16.0f) {
        add_engine_fault(ENGINE_FAULT_NONE);  // Just log the condition
    }
}

/* Add Engine Fault */
static void add_engine_fault(EngineFaultCode_t fault) {
    if (engine_control.fault_count < 20) {
        /* Check if fault already exists */
        for (uint8_t i = 0; i < engine_control.fault_count; i++) {
            if (engine_control.faults[i] == fault) {
                return;
            }
        }

        engine_control.faults[engine_control.fault_count] = fault;
        engine_control.fault_count++;

        /* Add DTC */
        DtcCode_t dtc_code = {
            .high_byte = 0x00,  /* Powertrain */
            .middle_byte = 0x00,
            .low_byte = fault
        };
        dtc_manager_add_dtc(&dtc_code, 3);  /* Medium severity */
    }
}

/* Handle Engine Faults */
static void handle_engine_faults(void) {
    for (uint8_t i = 0; i < engine_control.fault_count; i++) {
        switch (engine_control.faults[i]) {
            case ENGINE_FAULT_OVERHEAT:
                /* Enable cooling fan */
                engine_control.actuators.cooling_fan_duty = 100;

                /* Reduce power */
                engine_control.actuators.injector_pulse_width *= 0.5f;
                break;

            case ENGINE_FAULT_LOW_OIL_PRESSURE:
                /* Immediate engine shutdown */
                engine_control_stop();
                break;

            default:
                /* Enter limp home mode */
                engine_control.state = ENGINE_STATE_LIMP_HOME;
                break;
        }
    }
}

/* Get Engine State */
EngineState_t engine_control_get_state(void) {
    return engine_control.state;
}

/* Get Sensor Data */
KernelStatus_t engine_control_get_sensor_data(EngineSensorData_t* data) {
    if (data == NULL) {
        return KERNEL_ERROR;
    }

    mutex_lock(&engine_control.data_mutex, 100);
    memcpy(data, &engine_control.sensors, sizeof(EngineSensorData_t));
    mutex_unlock(&engine_control.data_mutex);

    return KERNEL_OK;
}

/* Get Actuator Data */
KernelStatus_t engine_control_get_actuator_data(EngineActuatorData_t* data) {
    if (data == NULL) {
        return KERNEL_ERROR;
    }

    mutex_lock(&engine_control.data_mutex, 100);
    memcpy(data, &engine_control.actuators, sizeof(EngineActuatorData_t));
    mutex_unlock(&engine_control.data_mutex);

    return KERNEL_OK;
}