#include <stdio.h>
#include "pico/stdlib.h"
#include "pico/cyw43_arch.h"

#define Rows 4
#define Cols 4
#define length(arr) (sizeof(arr)/sizeof(arr[0]))

const uint8_t row_pins[Rows] = {8, 7, 6, 5};
const uint8_t col_pins[Cols] = {4, 3, 2, 1};
const uint8_t targetLedPins[3] = {28, 27, 26};

const uint8_t red_mask = 0b001;
const uint8_t green_mask = 0b010;
const uint8_t blue_mask = 0b100;
const uint8_t black_mask = 0b000;

void write_rgb_led(uint8_t mask) {
    for (int pin = 0; pin < length(targetLedPins); pin++) {
        gpio_put(targetLedPins[pin], (bool)(mask >> pin & true));
    }
}

const char key_map[Rows][Cols] = {
    {'1', '2', '3', 'A'},
    {'4', '5', '6', 'B'},
    {'7', '8', '9', 'C'},
    {'*', '0', '#', 'D'}
};

void keypad_init() {
    for (int i = 0; i < Rows; i++) {
        gpio_init(row_pins[i]);
        gpio_set_dir(row_pins[i], GPIO_OUT);
        gpio_put(row_pins[i], 0);
    }
    for (int i = 0; i < Cols; i++) {
        gpio_init(col_pins[i]);
        gpio_set_dir(col_pins[i], GPIO_IN);
        gpio_pull_up(col_pins[i]);  // Ativa o pull-up interno
    }
    for (int ii = 0; ii < length(targetLedPins); ii++) {
        gpio_init(targetLedPins[ii]);
        gpio_set_dir(targetLedPins[ii], GPIO_OUT);
    }
}

char read_keypad() {
    for (int row = 0; row < Rows; row++) {
        gpio_put(row_pins[row], 1);
        for (int col = 0; col < Cols; col++) {
            if (gpio_get(col_pins[col]) == 0) {  // Verifica se a coluna está em LOW
                gpio_put(row_pins[row], 0);
                return key_map[row][col];
            }
        }
        gpio_put(row_pins[row], 0);
    }
    return '\0';
}

#define HANDLER(color, key1, key2, key3, key4) \
void color##Handler(char key) { \
    switch (key) { \
        case key1: \
        case key2: \
        case key3: \
        case key4: \
            write_rgb_led(color##_mask); \
            break; \
        default: \
            return; \
    } \
}

HANDLER(red, '1', '4', '7', '*')
HANDLER(green, '2', '5', '8', '0')
HANDLER(blue, '3', '6', '9', '#')
HANDLER(black, 'A', 'B', 'C', 'D')

void setup() {
    Serial.begin(9600);
    keypad_init();
    for (int ii = 0; ii < length(targetLedPins); ii++) {
        gpio_init(targetLedPins[ii]);
        gpio_set_dir(targetLedPins[ii], GPIO_OUT);
    }
    Serial.println("Pressione uma tecla!");
}

void loop() {
    char key = read_keypad();
    if (key != '\0') {
        Serial.printf("Tecla pressionada: %c\n", key);
        redHandler(key);
        blueHandler(key);
        greenHandler(key);
        blackHandler(key);
    }
    sleep_ms(200);
}