#include "pico/stdlib.h"
#include "hardware/gpio.h"

#define VERDE_PIN 15
#define ROJO_PIN 14
#define AMARILLO_PIN 13
#define ENTRADA_1 27
#define ENTRADA_2 26
#define GIRO_L 1
#define GIRO_R 4 

int main() {
    stdio_init_all();

    gpio_init(VERDE_PIN);
    gpio_init(ROJO_PIN);
    gpio_init(AMARILLO_PIN);
    gpio_init(GIRO_L);
    gpio_init(GIRO_R);
    gpio_init(ENTRADA_1);
    gpio_init(ENTRADA_2);

    gpio_set_dir(VERDE_PIN, GPIO_OUT);
    gpio_set_dir(ROJO_PIN, GPIO_OUT);
    gpio_set_dir(AMARILLO_PIN, GPIO_OUT);
    gpio_set_dir(GIRO_L, GPIO_OUT);
    gpio_set_dir(GIRO_R, GPIO_OUT);
    
    gpio_set_dir(ENTRADA_1, GPIO_IN);
    gpio_set_dir(ENTRADA_2, GPIO_IN);

    while (true) {
  
        bool entrada1 = gpio_get(ENTRADA_1);
        bool entrada2 = gpio_get(ENTRADA_2);

        bool xor_result = entrada1 ^ entrada2;

        if (entrada1  == entrada2 ){
            gpio_put(ROJO_PIN, 1);
            gpio_put(VERDE_PIN, 0); 
            gpio_put(AMARILLO_PIN, 0); 
            gpio_put(GIRO_L, 0); 
            gpio_put(GIRO_R, 0);  
 
        }
        if (entrada1 == 1 && entrada2 == 0){
            gpio_put(VERDE_PIN, 0);  
            gpio_put(ROJO_PIN, 0);
            gpio_put(AMARILLO_PIN, 1); 
            gpio_put(GIRO_L, 0); 
            gpio_put(GIRO_R, 1); 

        }
        if (entrada2 == 1 && entrada1 == 0){
            gpio_put(AMARILLO_PIN, 0); 
            gpio_put(ROJO_PIN, 0);
            gpio_put(VERDE_PIN, 1); 
            gpio_put(GIRO_L, 1); 
            gpio_put(GIRO_R, 0);
 
        }

        printf("Entrada 1: %d, Entrada 2: %d, Salida: %d\n", entrada1, entrada2, xor_result);

        sleep_ms(200);
    }

    return 0;
}