#include <Arduino.h>
#include <stdint.h>
#include <stdbool.h>
#include <string.h>

#include "pico/stdlib.h"
#include "hardware/adc.h"
#include "hardware/clocks.h"
#include "hardware/i2c.h"
#include "hardware/pwm.h"

/*
 * Los archivos generados por SCADE están escritos en C y usan restrict.
 * El archivo .ino se compila como C++, por lo que se adapta restrict y se
 * conserva el enlazado C para evitar errores de compilación/enlazado.
 */
#ifndef restrict
#define restrict __restrict__
#endif

extern "C" {
#include "main_proyecto_1.h"
}

/* ------------------------------- Pines ---------------------------------- */
#define PIN_LCD_SDA                 0u
#define PIN_LCD_SCL                 1u

#define PIN_SERVO_AILERON_LEFT      2u
#define PIN_SERVO_AILERON_RIGHT     3u
#define PIN_SERVO_ELEVATOR_LEFT     4u
#define PIN_SERVO_ELEVATOR_RIGHT    5u
#define PIN_SERVO_RUDDER            6u
#define PIN_SERVO_FLAP_LEFT         7u
#define PIN_SERVO_FLAP_RIGHT        8u
#define PIN_SERVO_GEAR_FRONT        9u
#define PIN_SERVO_GEAR_LEFT        10u
#define PIN_SERVO_GEAR_RIGHT       11u

#define PIN_GEAR_BUTTON            12u
#define PIN_LED_RED                13u
#define PIN_LED_GREEN              14u
#define PIN_LED_WHITE              15u
#define PIN_BUZZER                 16u
#define PIN_CONTROLLER_CONNECTED   17u

#define PIN_STATE_POT              26u
#define PIN_JOYSTICK_X             27u
#define PIN_JOYSTICK_Y             28u

/* ---------------------------- Configuracion ----------------------------- */
#define MODEL_PERIOD_US          5000u
#define LCD_PERIOD_US          200000u
#define DEBUG_PERIOD_US        500000u
#define BUTTON_DEBOUNCE_US      25000u

#define LCD_I2C                 i2c0
#define LCD_ADDRESS             0x27u
#define LCD_BAUDRATE          100000u

#define SERVO_PERIOD_US         20000u
#define SERVO_MIN_US             1000u
#define SERVO_MAX_US             2000u
#define SERVO_SAFE_US            1500u

#define BUZZER_FREQUENCY_HZ       2000u

/*
 * En el joystick analógico de Wokwi:
 * HORZ entrega 0 hacia la derecha, por eso X se invierte.
 */
#define INVERT_JOYSTICK_X            1
#define INVERT_JOYSTICK_Y            0

/*
 * Cambia a 1 únicamente cuando un servo físico se mueva
 * en el sentido contrario al esperado.
 */
#define REVERSE_AILERON_LEFT         0
#define REVERSE_AILERON_RIGHT        0
#define REVERSE_ELEVATOR_LEFT        0
#define REVERSE_ELEVATOR_RIGHT       0
#define REVERSE_RUDDER               0
#define REVERSE_FLAP_LEFT            0
#define REVERSE_FLAP_RIGHT           0
#define REVERSE_GEAR_FRONT           0
#define REVERSE_GEAR_LEFT            0
#define REVERSE_GEAR_RIGHT           0

static const uint8_t SERVO_PINS[10] = {
    PIN_SERVO_AILERON_LEFT,
    PIN_SERVO_AILERON_RIGHT,
    PIN_SERVO_ELEVATOR_LEFT,
    PIN_SERVO_ELEVATOR_RIGHT,
    PIN_SERVO_RUDDER,
    PIN_SERVO_FLAP_LEFT,
    PIN_SERVO_FLAP_RIGHT,
    PIN_SERVO_GEAR_FRONT,
    PIN_SERVO_GEAR_LEFT,
    PIN_SERVO_GEAR_RIGHT
};

/* --------------------------- Salidas del modelo ------------------------- */
typedef struct ModelOutputs {
    swan_float32 adc_percent;
    swan_bool adc_valid;
    swan_int32 flight_state;

    swan_float32 gear_cmd;
    swan_int32 display_code;

    swan_float32 elevator_out;
    swan_float32 rudder_out;
    swan_float32 aileron_left_out;
    swan_float32 aileron_right_out;
    swan_float32 flap_out;

    swan_bool buzzer_cmd;
    swan_bool led_green;
    swan_bool led_red;
    swan_bool nav_white_tail;

    swan_float32 gear_front_cmd;
    swan_float32 gear_left_cmd;
    swan_float32 gear_right_cmd;

    swan_bool pot_fault;

    swan_float32 aileron_left_pulse_ms;
    swan_float32 aileron_right_pulse_ms;
    swan_float32 elevator_pulse_ms;
    swan_float32 rudder_pulse_ms;
    swan_float32 flap_pulse_ms;

    swan_float32 gear_front_pulse_ms;
    swan_float32 gear_left_pulse_ms;
    swan_float32 gear_right_pulse_ms;
} ModelOutputs;

static outC_main_proyecto_1 g_scade_context;
static ModelOutputs g_model;

static uint16_t g_joystick_center_x = 2048u;
static uint16_t g_joystick_center_y = 2048u;

/* ------------------------------- Boton ---------------------------------- */
typedef struct DebouncedButton {
    bool raw_previous;
    bool stable;
    uint64_t changed_at_us;
} DebouncedButton;

static DebouncedButton g_gear_button;

static void button_init(
    DebouncedButton *button,
    bool raw,
    uint64_t now_us
) {
    button->raw_previous = raw;
    button->stable = raw;
    button->changed_at_us = now_us;
}

static bool button_update(
    DebouncedButton *button,
    bool raw,
    uint64_t now_us
) {
    if (raw != button->raw_previous) {
        button->raw_previous = raw;
        button->changed_at_us = now_us;
    }

    if (
        (now_us - button->changed_at_us) >=
        BUTTON_DEBOUNCE_US
    ) {
        button->stable = button->raw_previous;
    }

    return button->stable;
}

/* ------------------------------- ADC ------------------------------------ */
static uint16_t adc_read_gpio(uint gpio) {
    adc_select_input(gpio - 26u);
    return adc_read();
}

static float clamp_axis(float value) {
    if (value < -1.0f) {
        return -1.0f;
    }

    if (value > 1.0f) {
        return 1.0f;
    }

    return value;
}

static float normalize_axis(
    uint16_t raw,
    uint16_t center,
    bool invert
) {
    float value = 0.0f;

    if (raw >= center) {
        const uint16_t span =
            (uint16_t)(4095u - center);

        if (span > 0u) {
            value =
                (float)(raw - center) /
                (float)span;
        }
    }
    else {
        if (center > 0u) {
            value =
                -(
                    (float)(center - raw) /
                    (float)center
                );
        }
    }

    if (invert) {
        value = -value;
    }

    return clamp_axis(value);
}

static void calibrate_joystick(void) {
    uint32_t sum_x = 0u;
    uint32_t sum_y = 0u;

    const uint32_t sample_count = 64u;

    for (
        uint32_t index = 0u;
        index < sample_count;
        ++index
    ) {
        sum_x +=
            adc_read_gpio(
                PIN_JOYSTICK_X
            );

        sum_y +=
            adc_read_gpio(
                PIN_JOYSTICK_Y
            );

        sleep_ms(2u);
    }

    g_joystick_center_x =
        (uint16_t)(
            sum_x /
            sample_count
        );

    g_joystick_center_y =
        (uint16_t)(
            sum_y /
            sample_count
        );

    /*
     * Evita divisiones incorrectas cuando una entrada
     * analógica está desconectada durante el arranque.
     */
    if (
        g_joystick_center_x < 512u ||
        g_joystick_center_x > 3583u
    ) {
        g_joystick_center_x = 2048u;
    }

    if (
        g_joystick_center_y < 512u ||
        g_joystick_center_y > 3583u
    ) {
        g_joystick_center_y = 2048u;
    }
}

/* ------------------------------ Servos ---------------------------------- */
static uint16_t reverse_servo_pulse(
    uint16_t pulse_us,
    bool reverse
) {
    if (!reverse) {
        return pulse_us;
    }

    return (uint16_t)(
        SERVO_MIN_US +
        SERVO_MAX_US -
        pulse_us
    );
}

static uint16_t pulse_ms_to_us(
    swan_float32 pulse_ms
) {
    /*
     * Si SCADE entrega un pulso inválido,
     * el servo se coloca en posición segura.
     */
    if (
        !(
            pulse_ms >= 1.0f &&
            pulse_ms <= 2.0f
        )
    ) {
        return SERVO_SAFE_US;
    }

    uint32_t pulse_us =
        (uint32_t)(
            pulse_ms *
            1000.0f +
            0.5f
        );

    if (pulse_us < SERVO_MIN_US) {
        pulse_us = SERVO_MIN_US;
    }

    if (pulse_us > SERVO_MAX_US) {
        pulse_us = SERVO_MAX_US;
    }

    return (uint16_t)pulse_us;
}

static void servo_write_ms(
    uint gpio,
    swan_float32 pulse_ms,
    bool reverse
) {
    uint16_t pulse_us =
        pulse_ms_to_us(
            pulse_ms
        );

    pulse_us =
        reverse_servo_pulse(
            pulse_us,
            reverse
        );

    pwm_set_gpio_level(
        gpio,
        pulse_us
    );
}

static void servo_pwm_init(void) {
    bool slice_initialized[NUM_PWM_SLICES] = {
        false
    };

    /*
     * Ajusta el contador PWM a 1 MHz:
     * una cuenta equivale a un microsegundo.
     */
    const float divider =
        (float)clock_get_hz(clk_sys) /
        1000000.0f;

    for (
        size_t index = 0u;
        index < 10u;
        ++index
    ) {
        const uint gpio =
            SERVO_PINS[index];

        const uint slice =
            pwm_gpio_to_slice_num(
                gpio
            );

        gpio_set_function(
            gpio,
            GPIO_FUNC_PWM
        );

        if (!slice_initialized[slice]) {
            pwm_config config =
                pwm_get_default_config();

            pwm_config_set_clkdiv(
                &config,
                divider
            );

            /*
             * Periodo de 20 000 us:
             * frecuencia de 50 Hz.
             */
            pwm_config_set_wrap(
                &config,
                SERVO_PERIOD_US - 1u
            );

            pwm_init(
                slice,
                &config,
                true
            );

            slice_initialized[slice] =
                true;
        }

        pwm_set_gpio_level(
            gpio,
            SERVO_SAFE_US
        );
    }
}

static void write_all_servos(void) {
    servo_write_ms(
        PIN_SERVO_AILERON_LEFT,
        g_model.aileron_left_pulse_ms,
        REVERSE_AILERON_LEFT != 0
    );

    servo_write_ms(
        PIN_SERVO_AILERON_RIGHT,
        g_model.aileron_right_pulse_ms,
        REVERSE_AILERON_RIGHT != 0
    );

    /*
     * SCADE entrega una salida de elevador.
     * Se duplica para los dos servos físicos.
     */
    servo_write_ms(
        PIN_SERVO_ELEVATOR_LEFT,
        g_model.elevator_pulse_ms,
        REVERSE_ELEVATOR_LEFT != 0
    );

    servo_write_ms(
        PIN_SERVO_ELEVATOR_RIGHT,
        g_model.elevator_pulse_ms,
        REVERSE_ELEVATOR_RIGHT != 0
    );

    servo_write_ms(
        PIN_SERVO_RUDDER,
        g_model.rudder_pulse_ms,
        REVERSE_RUDDER != 0
    );

    /*
     * SCADE entrega una salida de flap.
     * Se duplica para los dos servos físicos.
     */
    servo_write_ms(
        PIN_SERVO_FLAP_LEFT,
        g_model.flap_pulse_ms,
        REVERSE_FLAP_LEFT != 0
    );

    servo_write_ms(
        PIN_SERVO_FLAP_RIGHT,
        g_model.flap_pulse_ms,
        REVERSE_FLAP_RIGHT != 0
    );

    servo_write_ms(
        PIN_SERVO_GEAR_FRONT,
        g_model.gear_front_pulse_ms,
        REVERSE_GEAR_FRONT != 0
    );

    servo_write_ms(
        PIN_SERVO_GEAR_LEFT,
        g_model.gear_left_pulse_ms,
        REVERSE_GEAR_LEFT != 0
    );

    servo_write_ms(
        PIN_SERVO_GEAR_RIGHT,
        g_model.gear_right_pulse_ms,
        REVERSE_GEAR_RIGHT != 0
    );
}

/* ------------------------------- Buzzer --------------------------------- */
static uint g_buzzer_slice = 0u;

static void buzzer_init(void) {
    const float divider =
        (float)clock_get_hz(clk_sys) /
        1000000.0f;

    const uint32_t period_us =
        1000000u /
        BUZZER_FREQUENCY_HZ;

    gpio_set_function(
        PIN_BUZZER,
        GPIO_FUNC_PWM
    );

    g_buzzer_slice =
        pwm_gpio_to_slice_num(
            PIN_BUZZER
        );

    pwm_config config =
        pwm_get_default_config();

    pwm_config_set_clkdiv(
        &config,
        divider
    );

    pwm_config_set_wrap(
        &config,
        period_us - 1u
    );

    pwm_init(
        g_buzzer_slice,
        &config,
        true
    );

    pwm_set_gpio_level(
        PIN_BUZZER,
        0u
    );
}

static void buzzer_set(bool enabled) {
    const uint32_t period_us =
        1000000u /
        BUZZER_FREQUENCY_HZ;

    pwm_set_gpio_level(
        PIN_BUZZER,
        enabled
            ? period_us / 2u
            : 0u
    );
}

/* ------------------------------- LCD ------------------------------------ */
#define LCD_RS         0x01u
#define LCD_ENABLE     0x04u
#define LCD_BACKLIGHT  0x08u

static void lcd_expander_write(
    uint8_t value
) {
    (void)i2c_write_blocking(
        LCD_I2C,
        LCD_ADDRESS,
        &value,
        1u,
        false
    );
}

static void lcd_pulse_enable(
    uint8_t value
) {
    lcd_expander_write(
        (uint8_t)(
            value |
            LCD_ENABLE
        )
    );

    sleep_us(1u);

    lcd_expander_write(
        (uint8_t)(
            value &
            (uint8_t)~LCD_ENABLE
        )
    );

    sleep_us(50u);
}

static void lcd_write4(
    uint8_t nibble,
    bool data_mode
) {
    uint8_t value =
        (uint8_t)(
            (nibble << 4u) |
            LCD_BACKLIGHT
        );

    if (data_mode) {
        value =
            (uint8_t)(
                value |
                LCD_RS
            );
    }

    lcd_expander_write(value);
    lcd_pulse_enable(value);
}

static void lcd_send(
    uint8_t value,
    bool data_mode
) {
    lcd_write4(
        (uint8_t)(
            value >> 4u
        ),
        data_mode
    );

    lcd_write4(
        (uint8_t)(
            value & 0x0Fu
        ),
        data_mode
    );
}

static void lcd_command(
    uint8_t command
) {
    lcd_send(
        command,
        false
    );
}

static void lcd_init(void) {
    i2c_init(
        LCD_I2C,
        LCD_BAUDRATE
    );

    gpio_set_function(
        PIN_LCD_SDA,
        GPIO_FUNC_I2C
    );

    gpio_set_function(
        PIN_LCD_SCL,
        GPIO_FUNC_I2C
    );

    gpio_pull_up(
        PIN_LCD_SDA
    );

    gpio_pull_up(
        PIN_LCD_SCL
    );

    sleep_ms(50u);

    lcd_write4(0x03u, false);
    sleep_us(4500u);

    lcd_write4(0x03u, false);
    sleep_us(4500u);

    lcd_write4(0x03u, false);
    sleep_us(150u);

    lcd_write4(0x02u, false);

    lcd_command(0x28u);
    lcd_command(0x08u);
    lcd_command(0x01u);

    sleep_ms(2u);

    lcd_command(0x06u);
    lcd_command(0x0Cu);
}

static void lcd_set_cursor(
    uint8_t column,
    uint8_t row
) {
    static const uint8_t offsets[2] = {
        0x00u,
        0x40u
    };

    if (row > 1u) {
        row = 1u;
    }

    lcd_command(
        (uint8_t)(
            0x80u |
            (
                column +
                offsets[row]
            )
        )
    );
}

static void lcd_write_line(
    uint8_t row,
    const char *text
) {
    char line[17];

    memset(
        line,
        ' ',
        16u
    );

    line[16] = '\0';

    if (text != NULL) {
        size_t length =
            strlen(text);

        if (length > 16u) {
            length = 16u;
        }

        memcpy(
            line,
            text,
            length
        );
    }

    lcd_set_cursor(
        0u,
        row
    );

    for (
        size_t index = 0u;
        index < 16u;
        ++index
    ) {
        lcd_send(
            (uint8_t)line[index],
            true
        );
    }
}

static void update_lcd(
    bool controller_connected
) {
    char line1[17];
    char line2[17];

    const int percent =
        (int)(
            g_model.adc_percent +
            0.5f
        );

    switch (g_model.display_code) {
        case 0:
            snprintf(
                line1,
                sizeof(line1),
                "TIERRA"
            );

            snprintf(
                line2,
                sizeof(line2),
                "P:%3d%% T:ABA",
                percent
            );
            break;

        case 1:
            snprintf(
                line1,
                sizeof(line1),
                "DESPEGUE"
            );

            snprintf(
                line2,
                sizeof(line2),
                "P:%3d%% T:ABA",
                percent
            );
            break;

        case 2:
            snprintf(
                line1,
                sizeof(line1),
                "CRUCERO"
            );

            snprintf(
                line2,
                sizeof(line2),
                "P:%3d%% T:ARR",
                percent
            );
            break;

        case 4:
            snprintf(
                line1,
                sizeof(line1),
                "ALERTA DE TREN"
            );

            snprintf(
                line2,
                sizeof(line2),
                "ACCION BLOQUEADA"
            );
            break;

        case 3:
        default:
            snprintf(
                line1,
                sizeof(line1),
                "FALLA SISTEMA"
            );

            if (!controller_connected) {
                snprintf(
                    line2,
                    sizeof(line2),
                    "CTRL DESCONECT."
                );
            }
            else if (
                g_model.pot_fault !=
                swan_false
            ) {
                snprintf(
                    line2,
                    sizeof(line2),
                    "FALLA SELECTOR"
                );
            }
            else {
                snprintf(
                    line2,
                    sizeof(line2),
                    "PONGA EN TIERRA"
                );
            }
            break;
    }

    lcd_write_line(
        0u,
        line1
    );

    lcd_write_line(
        1u,
        line2
    );
}

/* ---------------------------- GPIO discretos ---------------------------- */
static void discrete_io_init(void) {
    gpio_init(
        PIN_GEAR_BUTTON
    );

    gpio_set_dir(
        PIN_GEAR_BUTTON,
        GPIO_IN
    );

    gpio_pull_up(
        PIN_GEAR_BUTTON
    );

    gpio_init(
        PIN_CONTROLLER_CONNECTED
    );

    gpio_set_dir(
        PIN_CONTROLLER_CONNECTED,
        GPIO_IN
    );

    gpio_pull_up(
        PIN_CONTROLLER_CONNECTED
    );

    gpio_init(
        PIN_LED_RED
    );

    gpio_set_dir(
        PIN_LED_RED,
        GPIO_OUT
    );

    gpio_put(
        PIN_LED_RED,
        false
    );

    gpio_init(
        PIN_LED_GREEN
    );

    gpio_set_dir(
        PIN_LED_GREEN,
        GPIO_OUT
    );

    gpio_put(
        PIN_LED_GREEN,
        false
    );

    gpio_init(
        PIN_LED_WHITE
    );

    gpio_set_dir(
        PIN_LED_WHITE,
        GPIO_OUT
    );

    gpio_put(
        PIN_LED_WHITE,
        false
    );
}

static void update_discrete_outputs(void) {
    gpio_put(
        PIN_LED_GREEN,
        g_model.led_green !=
        swan_false
    );

    gpio_put(
        PIN_LED_RED,
        g_model.led_red !=
        swan_false
    );

    gpio_put(
        PIN_LED_WHITE,
        g_model.nav_white_tail !=
        swan_false
    );

    buzzer_set(
        g_model.buzzer_cmd !=
        swan_false
    );
}

/* ---------------- Correccion de superficies coordinadas ---------------- */
static swan_float32 clamp_float32(
    swan_float32 value,
    swan_float32 minimum,
    swan_float32 maximum
) {
    if (value < minimum) {
        return minimum;
    }

    if (value > maximum) {
        return maximum;
    }

    return value;
}

static swan_float32 angle_to_pulse_ms(
    swan_float32 angle,
    swan_float32 minimum_angle,
    swan_float32 maximum_angle
) {
    angle = clamp_float32(
        angle,
        minimum_angle,
        maximum_angle
    );

    return 1.0f +
        (
            (angle - minimum_angle) /
            (maximum_angle - minimum_angle)
        );
}

/*
 * Corrige la coordinación del eje horizontal:
 *  - los dos alerones se mueven siempre en sentidos opuestos;
 *  - el rudder puede moverse hacia ambos lados;
 *  - se respetan los límites de cada estado de vuelo.
 *
 * Joystick X:
 *   izquierda/derecha -> alerones opuestos + rudder bidireccional.
 */
static void correct_lateral_surfaces(
    swan_float32 joystick_x,
    swan_bool controller_connected
) {
    swan_float32 aileron_limit = 0.0f;
    swan_float32 rudder_limit = 0.0f;

    switch (g_model.flight_state) {
        case 0: /* TIERRA */
            aileron_limit = 45.0f;
            rudder_limit = 55.0f;
            break;

        case 1: /* DESPEGUE */
            aileron_limit = 20.0f;
            rudder_limit = 20.0f;
            break;

        case 2: /* CRUCERO */
            aileron_limit = 10.0f;
            rudder_limit = 5.0f;
            break;

        case 3: /* FALLA */
        default:
            aileron_limit = 0.0f;
            rudder_limit = 0.0f;
            break;
    }

    if (
        controller_connected == swan_false ||
        g_model.pot_fault != swan_false ||
        g_model.flight_state == 3
    ) {
        joystick_x = 0.0f;
    }

    /* Zona muerta para evitar vibración con el joystick centrado. */
    if (
        joystick_x > -0.05f &&
        joystick_x < 0.05f
    ) {
        joystick_x = 0.0f;
    }

    joystick_x = clamp_float32(
        joystick_x,
        -1.0f,
        1.0f
    );

    g_model.aileron_left_out =
        joystick_x * aileron_limit;

    g_model.aileron_right_out =
        -joystick_x * aileron_limit;

    g_model.rudder_out =
        joystick_x * rudder_limit;

    /*
     * Los convertidores físicos usan el rango global del actuador.
     * Alerones: -45 a +45 grados.
     * Rudder:   -55 a +55 grados.
     */
    g_model.aileron_left_pulse_ms =
        angle_to_pulse_ms(
            g_model.aileron_left_out,
            -45.0f,
            45.0f
        );

    g_model.aileron_right_pulse_ms =
        angle_to_pulse_ms(
            g_model.aileron_right_out,
            -45.0f,
            45.0f
        );

    g_model.rudder_pulse_ms =
        angle_to_pulse_ms(
            g_model.rudder_out,
            -55.0f,
            55.0f
        );
}

/* --------------------------- Ejecucion SCADE ---------------------------- */
static void execute_scade_cycle(void) {
    const uint64_t now_us =
        time_us_64();

    const uint16_t adc_state_raw =
        adc_read_gpio(
            PIN_STATE_POT
        );

    const uint16_t joystick_x_raw =
        adc_read_gpio(
            PIN_JOYSTICK_X
        );

    const uint16_t joystick_y_raw =
        adc_read_gpio(
            PIN_JOYSTICK_Y
        );

    const swan_bool controller_connected = swan_true;
    
    const bool raw_button_pressed =
        !gpio_get(
            PIN_GEAR_BUTTON
        );

    const bool gear_button_pressed =
        button_update(
            &g_gear_button,
            raw_button_pressed,
            now_us
        );

    const swan_float32 joystick_x =
        normalize_axis(
            joystick_x_raw,
            g_joystick_center_x,
            INVERT_JOYSTICK_X != 0
        );

    const swan_float32 joystick_y =
        normalize_axis(
            joystick_y_raw,
            g_joystick_center_y,
            INVERT_JOYSTICK_Y != 0
        );

    /*
     * Llamada al modelo generado por SCADE One.
     */
    main_proyecto_1(
        (swan_float32)adc_state_raw,
        gear_button_pressed
            ? swan_true
            : swan_false,
        joystick_x,
        joystick_y,
        controller_connected,

        &g_model.adc_percent,
        &g_model.adc_valid,
        &g_model.flight_state,

        &g_model.gear_cmd,
        &g_model.display_code,

        &g_model.elevator_out,
        &g_model.rudder_out,
        &g_model.aileron_left_out,
        &g_model.aileron_right_out,
        &g_model.flap_out,

        &g_model.buzzer_cmd,
        &g_model.led_green,
        &g_model.led_red,
        &g_model.nav_white_tail,

        &g_model.gear_front_cmd,
        &g_model.gear_left_cmd,
        &g_model.gear_right_cmd,

        &g_model.pot_fault,

        &g_model.aileron_left_pulse_ms,
        &g_model.aileron_right_pulse_ms,
        &g_model.elevator_pulse_ms,
        &g_model.rudder_pulse_ms,
        &g_model.flap_pulse_ms,

        &g_model.gear_front_pulse_ms,
        &g_model.gear_left_pulse_ms,
        &g_model.gear_right_pulse_ms,

        &g_scade_context
    );

    /*
     * La versión generada separaba el movimiento lateral por lado:
     * un alerón se movía y el otro quedaba centrado. También dejaba
     * el rudder activo únicamente para una dirección.
     */
    correct_lateral_surfaces(
        joystick_x,
        controller_connected
    );

    write_all_servos();
    update_discrete_outputs();
}

static const char *flight_state_name(
    swan_int32 state
) {
    switch (state) {
        case 0:
            return "TIERRA";

        case 1:
            return "DESPEGUE";

        case 2:
            return "CRUCERO";

        case 3:
            return "FALLA";

        default:
            return "INVALIDO";
    }
}

static void print_debug(void) {
    Serial.printf(
        "EST=%s "
        "ADC=%.1f "
        "valid=%u "
        "fault=%u "
        "AIL_L=%.1f "
        "AIL_R=%.1f "
        "ELEV=%.1f "
        "RUD=%.1f "
        "FLAP=%.1f "
        "GEAR=%.1f "
        "PULSE=%.3f/%.3f/%.3f/%.3f/%.3f "
        "DISP=%ld "
        "BUZ=%u\n",

        flight_state_name(
            g_model.flight_state
        ),

        (double)g_model.adc_percent,
        (unsigned)g_model.adc_valid,
        (unsigned)g_model.pot_fault,

        (double)g_model.aileron_left_out,
        (double)g_model.aileron_right_out,
        (double)g_model.elevator_out,
        (double)g_model.rudder_out,
        (double)g_model.flap_out,
        (double)g_model.gear_cmd,

        (double)g_model.aileron_left_pulse_ms,
        (double)g_model.aileron_right_pulse_ms,
        (double)g_model.elevator_pulse_ms,
        (double)g_model.rudder_pulse_ms,
        (double)g_model.flap_pulse_ms,

        (long)g_model.display_code,
        (unsigned)g_model.buzzer_cmd
    );
}

/* --------------------------- Arduino setup/loop ------------------------- */
static uint64_t g_next_model_us = 0u;
static uint64_t g_next_lcd_us = 0u;
static uint64_t g_next_debug_us = 0u;

void setup(void) {
    Serial.begin(115200);

    adc_init();

    adc_gpio_init(
        PIN_STATE_POT
    );

    adc_gpio_init(
        PIN_JOYSTICK_X
    );

    adc_gpio_init(
        PIN_JOYSTICK_Y
    );

    discrete_io_init();
    servo_pwm_init();
    buzzer_init();
    lcd_init();

    lcd_write_line(
        0u,
        "CENTRE JOYSTICK"
    );

    lcd_write_line(
        1u,
        "CALIBRANDO"
    );

    /*
     * Mantén el joystick centrado durante el arranque.
     */
    sleep_ms(1000u);

    calibrate_joystick();

    const uint64_t now_us =
        time_us_64();

    button_init(
        &g_gear_button,
        !gpio_get(
            PIN_GEAR_BUTTON
        ),
        now_us
    );

    memset(
        &g_model,
        0,
        sizeof(g_model)
    );

    main_init_proyecto_1(
        &g_scade_context
    );

    lcd_write_line(
        0u,
        "SCADE INICIADO"
    );

    lcd_write_line(
        1u,
        "SISTEMA LISTO"
    );

    sleep_ms(500u);

    g_next_model_us = time_us_64();
    g_next_lcd_us = g_next_model_us;
    g_next_debug_us = g_next_model_us;
}

void loop(void) {
    const uint64_t current_us = time_us_64();

    /* Ejecuta el modelo cada 5 ms. */
    if ((int64_t)(current_us - g_next_model_us) >= 0) {
        g_next_model_us += MODEL_PERIOD_US;
        execute_scade_cycle();
    }

    /* Actualiza LCD cada 200 ms. */
    if ((int64_t)(current_us - g_next_lcd_us) >= 0) {
        g_next_lcd_us += LCD_PERIOD_US;

        /* En Wokwi se considera conectado el controlador analógico. */
        update_lcd(true);
    }

    /* Envía diagnóstico por USB cada 500 ms. */
    if ((int64_t)(current_us - g_next_debug_us) >= 0) {
        g_next_debug_us += DEBUG_PERIOD_US;
        print_debug();
    }

    tight_loop_contents();
}