/*******************************************************************************
 FileName:     main.c (Modificado - v1.9)
 Dependencies: Veja a secao de includes
 Processor:    Raspberry Pico
 Compiler:     SDK 1.5.1
 Company:      FATEC Santo Andre
 Author:       Prof. Edson Kitani / Brenda Biassio
 Date:         17/04/2025
 Software License Agreement: Somente para fins didaticos
 *******************************************************************************
 File Description: Sistema ABS para Carrinho - Controle de Frenagem com/sem ABS
 
 MODIFICAÇÕES v1.9:
 - ABS ON + Freio: Desacelera de 100% -> 25% e volta para 100% (não para)
 - Buzzer ativo APENAS durante desaceleração (BRAKING)
 - Buzzer desliga ao retornar para RUNNING
 - Sistema não entra em STOPPED quando ABS está ON
 - LEDs acendem durante BRAKING com ABS
*******************************************************************************/

#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/adc.h"
#include "hardware/i2c.h"
#include <string.h>
#include <assert.h>
#include <ctype.h>
#include "pico/bootrom.h"
#include "ssd1306.h"
#include "pico/time.h" 
#include "hardware/pwm.h"
#include "hardware/clocks.h"
#include "pwm_fatec.h"
#include "hardware/gpio.h"
#include "pcf8574.h"

//====================================================================
// Definicoes do Projeto - Hardware Wokwi
//====================================================================

#define IN_74LS245        15
#define BRAKE_BUTTON      22
#define START_BUTTON      2
#define ABS_SWITCH        3
#define BUZZER_PIN        8

// Motores DC das rodas
#define MOTOR_WHEEL_1     9
#define MOTOR_WHEEL_2     10
#define MOTOR_WHEEL_3     11
#define MOTOR_WHEEL_4     12

// PCF8574 - LEDs indicadores ABS
#define PCF8574_LED_WHEEL_1  7
#define PCF8574_LED_WHEEL_2  6
#define PCF8574_LED_WHEEL_3  5
#define PCF8574_LED_WHEEL_4  4

// Display OLED I2C
#define OLED_SDA          20
#define OLED_SCL          21

// Configurações do sistema
#define N_WHEELS          4
#define TICK_MS           50
#define PULSES_WINDOW_MS  200
#define TARGET_START_DUTY 70      // 100% = velocidade de cruzeiro
#define MIN_ABS_DUTY      18      // 25% de 70 = 17.5 ≈ 18
#define DEBOUNCE_TIME_MS  300

#define NUM_PWM_SLICES 8

//===================================================================
// Variáveis Globais

uint8_t display_buffer[128 * SSD1306_HEIGHT / 8] = {0};

static int wheel_pulses[N_WHEELS] = {0, 0, 0, 0};
static int prev_wheel_pulses[N_WHEELS] = {0, 0, 0, 0};
static int wheel_duty[N_WHEELS] = {0, 0, 0, 0};

// Estados do sistema
typedef enum {
    STATE_STOPPED,
    STATE_RUNNING,
    STATE_BRAKING
} SystemState;

static SystemState current_state = STATE_STOPPED;
static SystemState prev_state = STATE_STOPPED;
static bool abs_enabled = false;
static bool prev_abs_enabled = false;
static bool ignition_can = true;
static bool display_needs_update = true;

// Simulação de pressão de óleo do freio
static bool brake_pressure_ok = true;
static int brake_pressure_counter = 0;

// Flags voláteis para interrupções
volatile bool start_button_pressed = false;
volatile bool brake_button_pressed = false;
volatile uint32_t last_start_time = 0;
volatile uint32_t last_brake_time = 0;
volatile bool button_lock = false;

//===================================================================
// HANDLER DE INTERRUPÇÕES GPIO
//===================================================================

void gpio_callback(uint gpio, uint32_t events) {
    uint32_t current_time = to_ms_since_boot(get_absolute_time());
    
    if (events & GPIO_IRQ_EDGE_FALL) {
        if (gpio == START_BUTTON) {
            if ((current_time - last_start_time) < DEBOUNCE_TIME_MS) {
                return;
            }
            last_start_time = current_time;
            
            if (!button_lock) {
                start_button_pressed = true;
                button_lock = true;
            }
        }
        else if (gpio == BRAKE_BUTTON) {
            if ((current_time - last_brake_time) < DEBOUNCE_TIME_MS) {
                return;
            }
            last_brake_time = current_time;
            
            if (!button_lock || current_state == STATE_RUNNING) {
                brake_button_pressed = true;
                button_lock = true;
            }
        }
    }
    
    if (events & GPIO_IRQ_EDGE_RISE) {
        if (gpio == START_BUTTON || gpio == BRAKE_BUTTON) {
            if (gpio_get(START_BUTTON) && gpio_get(BRAKE_BUTTON)) {
                button_lock = false;
            }
        }
    }
}

//===================================================================
// Funções de Controle de Hardware

void hardware_init(void) {
    gpio_init(IN_74LS245);
    gpio_set_dir(IN_74LS245, GPIO_OUT);
    gpio_put(IN_74LS245, 0);

    // Botão START
    gpio_init(START_BUTTON);
    gpio_set_dir(START_BUTTON, GPIO_IN);
    gpio_pull_up(START_BUTTON);
    gpio_set_irq_enabled(START_BUTTON, GPIO_IRQ_EDGE_FALL | GPIO_IRQ_EDGE_RISE, true);

    // Botão BRAKE
    gpio_init(BRAKE_BUTTON);
    gpio_set_dir(BRAKE_BUTTON, GPIO_IN);
    gpio_pull_up(BRAKE_BUTTON);
    gpio_set_irq_enabled(BRAKE_BUTTON, GPIO_IRQ_EDGE_FALL | GPIO_IRQ_EDGE_RISE, true);
    gpio_set_irq_enabled_with_callback(START_BUTTON, GPIO_IRQ_EDGE_FALL | GPIO_IRQ_EDGE_RISE, 
                                        true, &gpio_callback);

    // Chave ABS
    gpio_init(ABS_SWITCH);
    gpio_set_dir(ABS_SWITCH, GPIO_IN);
    gpio_pull_down(ABS_SWITCH);

    // Motores DC com PWM
    pwm_init_pin(MOTOR_WHEEL_1);
    pwm_init_pin(MOTOR_WHEEL_2);
    pwm_init_pin(MOTOR_WHEEL_3);
    pwm_init_pin(MOTOR_WHEEL_4);
    
    pwm_set_duty_percent(MOTOR_WHEEL_1, 0);
    pwm_set_duty_percent(MOTOR_WHEEL_2, 0);
    pwm_set_duty_percent(MOTOR_WHEEL_3, 0);
    pwm_set_duty_percent(MOTOR_WHEEL_4, 0);

    // PCF8574 para LEDs - aguarda OLED inicializar primeiro
    sleep_ms(200);
    
    // Inicializa PCF8574 no mesmo I2C do OLED (i2c0)
    pcf8574_init(0xFF);
    sleep_ms(100);
    
    // Teste de LEDs
    printf("Testando LEDs do PCF8574...\n");
    for (int i = 0; i < N_WHEELS; i++) {
        pcf8574_write_pin(7-i, false);  // Acende (LOW)
    }
    sleep_ms(1000);
    
    // Apaga todos os LEDs após o teste e mantém apagados
    for (int i = 0; i < N_WHEELS; i++) {
        pcf8574_write_pin(7-i, true);  // Apaga (HIGH)
    }
    sleep_ms(100);
    printf("Teste de LEDs concluido - LEDs desligados\n");

    // Buzzer
    gpio_init(BUZZER_PIN);
    gpio_set_dir(BUZZER_PIN, GPIO_OUT);
    gpio_put(BUZZER_PIN, 0);
}

//===================================================================
// Controla LEDs
//===================================================================
void led_abs_set_all(bool state) {
    const uint8_t led_pins[] = {
        PCF8574_LED_WHEEL_1,
        PCF8574_LED_WHEEL_2,
        PCF8574_LED_WHEEL_3,
        PCF8574_LED_WHEEL_4
    };
    
    for (int i = 0; i < N_WHEELS; i++) {
        pcf8574_write_pin(led_pins[i], !state);
    }
}

//===================================================================
// Controla velocidade dos motores DC via PWM
//===================================================================
void motor_set_speed(int wheel_index, uint8_t duty_percent) {
    const uint motor_pins[] = {
        MOTOR_WHEEL_1, 
        MOTOR_WHEEL_2, 
        MOTOR_WHEEL_3, 
        MOTOR_WHEEL_4
    };
    
    if (wheel_index >= 0 && wheel_index < N_WHEELS) {
        if (duty_percent > 100) duty_percent = 100;
        pwm_set_duty_percent(motor_pins[wheel_index], duty_percent);
    }
}

void buzzer_set(bool state) {
    gpio_put(BUZZER_PIN, state ? 1 : 0);
}

//===================================================================
// Simulação de Velocidade das Rodas

void simulate_wheel_speeds(int target_duty) {
    for (int i = 0; i < N_WHEELS; i++) {
        int variation = (rand() % 5) - 2;
        wheel_pulses[i] = (target_duty / 10) + variation;
        if (wheel_pulses[i] < 0) wheel_pulses[i] = 0;
    }
}

//===================================================================
// Simulação de pressão de óleo do freio
//===================================================================
void simulate_brake_pressure(void) {
    brake_pressure_counter++;
    
    if (brake_pressure_counter >= 100) {
        brake_pressure_counter = 0;
        brake_pressure_ok = (rand() % 100) < 95;
    }
}

//===================================================================
// Frenagem com ABS: Desacelera até 25% e volta para 100%
//===================================================================
void abs_control_braking(void) {
    // Desacelera gradualmente até 25% (MIN_ABS_DUTY)
    for (int i = 0; i < N_WHEELS; i++) {
        if (wheel_duty[i] > MIN_ABS_DUTY) {
            wheel_duty[i] -= 2;  // Redução gradual
            if (wheel_duty[i] < MIN_ABS_DUTY) {
                wheel_duty[i] = MIN_ABS_DUTY;
            }
        }
        motor_set_speed(i, wheel_duty[i]);
    }
    
    // Verifica se atingiu 25% em todas as rodas
    bool all_at_minimum = true;
    for (int i = 0; i < N_WHEELS; i++) {
        if (wheel_duty[i] > MIN_ABS_DUTY) {
            all_at_minimum = false;
            break;
        }
    }
    
    // Se atingiu 25%, volta automaticamente para RUNNING (100%)
    if (all_at_minimum) {
        current_state = STATE_RUNNING;
        display_needs_update = true;
        printf("ABS: Atingiu 25%%, retornando para RUNNING (100%%)\n");
    }
}

void brake_without_abs(void) {
    // Freio total sem ABS - para completamente
    for (int i = 0; i < N_WHEELS; i++) {
        if (wheel_duty[i] > 0) {
            wheel_duty[i] -= 5;  // Redução rápida
            if (wheel_duty[i] < 0) wheel_duty[i] = 0;
        }
        motor_set_speed(i, wheel_duty[i]);
    }
}

//===================================================================
// Atualização do Display OLED
//===================================================================
void update_display(void) {
    char line[32];
    
    ssd1306_clear_buffer(display_buffer);
    
    // Linha 0: Status ABS e Sistema
    snprintf(line, sizeof(line), "ABS:%s ST:%s", 
             abs_enabled ? "ON " : "OFF",
             current_state == STATE_STOPPED ? "STOP" :
             current_state == STATE_RUNNING ? "RUN " : "BRAK");
    ssd1306_write_text(display_buffer, 0, 0, line);
    
    // Linha 1: Estado da ignição
    snprintf(line, sizeof(line), "Ignicao: %s", ignition_can ? "ON " : "OFF");
    ssd1306_write_text(display_buffer, 0, 16, line);
    
    // Linha 2: Pressão de freio
    snprintf(line, sizeof(line), "Pr: %s", brake_pressure_ok ? "OK   " : "FALHA");
    ssd1306_write_text(display_buffer, 0, 32, line);
    
    ssd1306_send_data(display_buffer, 128 * SSD1306_HEIGHT / 8);
}

void check_display_update_needed(void) {
    static bool last_brake_pressure = true;
    
    if (current_state != prev_state) {
        display_needs_update = true;
        prev_state = current_state;
    }
    
    if (abs_enabled != prev_abs_enabled) {
        display_needs_update = true;
        prev_abs_enabled = abs_enabled;
    }
    
    if (brake_pressure_ok != last_brake_pressure) {
        display_needs_update = true;
        last_brake_pressure = brake_pressure_ok;
    }
}

//===================================================================
// Máquina de Estados Principal
//===================================================================

int main(void)
{
    stdio_init_all();
    
    gpio_init(BRAKE_BUTTON);
    gpio_set_dir(BRAKE_BUTTON, GPIO_IN);
    gpio_pull_up(BRAKE_BUTTON);
    
    if(gpio_get(BRAKE_BUTTON) == 0) {
        reset_usb_boot(0, 0);
    }
    
    hardware_init();
    
    SSD1306_init();
    ssd1306_clear_buffer(display_buffer);
    ssd1306_write_text(display_buffer, 0, 0, "Sistema ABS v1.9");
    ssd1306_write_text(display_buffer, 0, 16, "ABS: 100%->25%");
    ssd1306_write_text(display_buffer, 0, 32, "Volta p/ 100%");
    ssd1306_send_data(display_buffer, 128 * SSD1306_HEIGHT / 8);
    sleep_ms(2000);
    
    display_needs_update = true;
    prev_state = STATE_RUNNING;
    
    int pulse_accumulator = 0;
    
    printf("Sistema ABS v1.9 iniciado\n");
    printf("ABS ON: Desacelera 100%% -> 25%% e volta para 100%%\n");
    
    while (true)
    {
        sleep_ms(TICK_MS);
        
        abs_enabled = gpio_get(ABS_SWITCH);
        simulate_brake_pressure();
        
        pulse_accumulator += TICK_MS;
        if (pulse_accumulator >= PULSES_WINDOW_MS) {
            for (int i = 0; i < N_WHEELS; i++) {
                prev_wheel_pulses[i] = wheel_pulses[i];
            }
            pulse_accumulator = 0;
        }
        
        // Processa interrupções
        if (start_button_pressed) {
            start_button_pressed = false;
            
            if (current_state == STATE_STOPPED && ignition_can) {
                current_state = STATE_RUNNING;
                for (int i = 0; i < N_WHEELS; i++) {
                    wheel_duty[i] = TARGET_START_DUTY;
                }
                display_needs_update = true;
                printf("Estado: RUNNING (100%%)\n");
            }
        }
        
        if (brake_button_pressed) {
            brake_button_pressed = false;
            
            if (current_state == STATE_RUNNING) {
                current_state = STATE_BRAKING;
                display_needs_update = true;
                printf("Estado: BRAKING (ABS %s)\n", abs_enabled ? "ON" : "OFF");
            }
        }
        
        // Máquina de Estados
        switch (current_state) {
            case STATE_STOPPED:
                for (int i = 0; i < N_WHEELS; i++) {
                    wheel_duty[i] = 0;
                    wheel_pulses[i] = 0;
                    motor_set_speed(i, 0);
                }
                led_abs_set_all(false);
                buzzer_set(false);
                break;
                
            case STATE_RUNNING:
                // Acelera de volta para 100% (TARGET_START_DUTY)
                for (int i = 0; i < N_WHEELS; i++) {
                    if (wheel_duty[i] < TARGET_START_DUTY) {
                        wheel_duty[i] += 2;  // Aceleração gradual
                        if (wheel_duty[i] > TARGET_START_DUTY) {
                            wheel_duty[i] = TARGET_START_DUTY;
                        }
                    }
                    motor_set_speed(i, wheel_duty[i]);
                }
                led_abs_set_all(false);
                buzzer_set(false);  // Buzzer OFF em RUNNING
                simulate_wheel_speeds(TARGET_START_DUTY);
                break;
                
            case STATE_BRAKING:
                if (!abs_enabled) {
                    // Sem ABS - para completamente
                    brake_without_abs();
                    led_abs_set_all(false);
                    buzzer_set(false);
                    
                    int avg_duty = 0;
                    for (int i = 0; i < N_WHEELS; i++) {
                        avg_duty += wheel_duty[i];
                    }
                    avg_duty /= N_WHEELS;
                    simulate_wheel_speeds(avg_duty);
                    
                    bool all_stopped = true;
                    for (int i = 0; i < N_WHEELS; i++) {
                        if (wheel_duty[i] > 0) {
                            all_stopped = false;
                            break;
                        }
                    }
                    
                    if (all_stopped) {
                        current_state = STATE_STOPPED;
                        display_needs_update = true;
                        printf("Estado: STOPPED (sem ABS)\n");
                    }
                } else {
                    // Com ABS - desacelera até 25% e volta para RUNNING
                    abs_control_braking();
                    led_abs_set_all(true);   // LEDs ON durante BRAKING
                    buzzer_set(true);        // Buzzer ON durante BRAKING
                    
                    int avg_duty = 0;
                    for (int i = 0; i < N_WHEELS; i++) {
                        avg_duty += wheel_duty[i];
                    }
                    avg_duty /= N_WHEELS;
                    simulate_wheel_speeds(avg_duty);
                }
                break;
        }
        
        check_display_update_needed();
        if (display_needs_update) {
            update_display();
            display_needs_update = false;
        }
    }
    
    return 0;
}