#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/pio.h"
#include "hardware/clocks.h"
#include "blink_led.pio.h"

// == TM1637Display ==
// #define TM1637_CMD_DATA_AUTO  0x40
#define TM1637_DELAY_US 20 // Delay time in microseconds
#define TM1637_CMD_DATA_FIXED 0x44
#define TM1637_CMD_ADDR_START 0xC0
#define TM1637_CMD_DISPLAY    0x88
#define TM1637_BRIGHTNESS  5 // min 0 max 7

typedef struct {
  uint clock_pin;
  uint data_pin;
} TM1637Display;

TM1637Display tm1637_init(uint clock_pin, uint data_pin);
void tm1637_write_command(const TM1637Display *d, uint8_t command);
void tm1637_display_number(const TM1637Display *d, uint num);
void tm1637_start(const TM1637Display *display);
void tm1637_stop(const TM1637Display *display);
void tm1637_write_bit(const TM1637Display *display, bool bit);
bool tm1637_write_byte(const TM1637Display *display, uint8_t byte);
void tm1637_write_command(const TM1637Display *display, uint8_t command);


const uint8_t tm1637_num_patterns[10] = {
    0x3F, // 0
    0x06, // 1
    0x5B, // 2
    0x4F, // 3
    0x66, // 4
    0x6D, // 5
    0x7D, // 6
    0x07, // 7
    0x7F, // 8
    0x6F  // 9
};

TM1637Display tm1637_init(uint clock_pin, uint data_pin) {
    TM1637Display display;
    display.clock_pin = clock_pin;
    display.data_pin = data_pin;


    gpio_init(display.clock_pin);
    gpio_init(display.data_pin);

    gpio_set_dir(display.clock_pin, GPIO_OUT);
    gpio_set_dir(display.data_pin, GPIO_OUT);
    tm1637_write_command(&display, TM1637_CMD_DISPLAY | TM1637_BRIGHTNESS);
    return display;
}

void tm1637_start(const TM1637Display *display) {
    gpio_set_dir(display->data_pin, GPIO_OUT);
    gpio_put(display->data_pin, 1);
    gpio_put(display->clock_pin, 1);
    sleep_us(TM1637_DELAY_US);
    gpio_put(display->data_pin, 0);
    sleep_us(TM1637_DELAY_US);
    gpio_put(display->clock_pin, 0);
}

void tm1637_stop(const TM1637Display *display) {
    gpio_set_dir(display->data_pin, GPIO_OUT);
    gpio_put(display->clock_pin, 0);
    gpio_put(display->data_pin, 0);
    sleep_us(TM1637_DELAY_US);
    gpio_put(display->clock_pin, 1);
    sleep_us(TM1637_DELAY_US);
    gpio_put(display->data_pin, 1);
    sleep_us(TM1637_DELAY_US);
}

void tm1637_write_bit(const TM1637Display *display, bool bit) {
    gpio_put(display->clock_pin, 0);
    if (bit)
    {
        gpio_set_dir(display->data_pin, GPIO_IN);
    }
    else
    {
        gpio_set_dir(display->data_pin, GPIO_OUT);
        gpio_put(display->data_pin, 0);
    }
    sleep_us(TM1637_DELAY_US);
    gpio_put(display->clock_pin, 1);
    sleep_us(TM1637_DELAY_US);
    gpio_put(display->clock_pin, 0);
    gpio_set_dir(display->data_pin, GPIO_OUT);
}

bool tm1637_write_byte(const TM1637Display *display, uint8_t byte) {
    for (int i = 0; i < 8; i++)
    {
        tm1637_write_bit(display, byte & 0x01);
        byte >>= 1;
    }
    // Wait for ACK
    gpio_set_dir(display->data_pin, GPIO_IN);
    sleep_us(TM1637_DELAY_US);
    gpio_put(display->clock_pin, 0);
    sleep_us(TM1637_DELAY_US);
    gpio_put(display->clock_pin, 1);
    sleep_us(TM1637_DELAY_US);
    bool ack = !gpio_get(display->data_pin);
    gpio_put(display->clock_pin, 0);
    gpio_set_dir(display->data_pin, GPIO_OUT);
    return ack;
}

void tm1637_write_command(const TM1637Display *display, uint8_t command) {
    tm1637_start(display);
    tm1637_write_byte(display, command);
    tm1637_stop(display);
}

void tm1637_display_number(const TM1637Display *display, uint num) {
    if (num > 9999) return;

    uint8_t digits[4] = {0};

    // Extract digits and store them in the array from most significant digit to least significant digit
    for (int i = 0; i < 4; i++)
    {
        digits[3 - i] = num % 10;
        num /= 10;
    }

    tm1637_start(display);
    bool ack = tm1637_write_byte(display, TM1637_CMD_DATA_FIXED);
    tm1637_stop(display);
    if(!ack) return tm1637_display_number(display, num);

    for (int i = 0; i < 4; i++)
    {
        tm1637_start(display);
        bool ack1 = tm1637_write_byte(display, TM1637_CMD_ADDR_START + i);
        bool ack2 = tm1637_write_byte(display, tm1637_num_patterns[digits[i]]);
        tm1637_stop(display);
        if(!ack1 || !ack2) return tm1637_display_number(display, num);
    }
}

// == LEDS ==
void init_leds() {
    gpio_init(PICO_DEFAULT_LED_PIN);
    gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
}
void set_led(bool state) {
    gpio_put(PICO_DEFAULT_LED_PIN, state);
}
// == RGB_LED ==

// #define RGB_LED_PIN PICO_DEFAULT_LED_PIN
#define RGB_PIO_FREQ 2000
// PIO blink example: https://www.digikey.com/en/maker/projects/raspberry-pi-pico-and-rp2040-cc-part-3-how-to-use-pio/123ff7700bc547c79a504858c1bd8110
// static const uint led_pin = 25;
// static const float pio_freq = 2000;
void init_rgb_leds() {
    //Choose PIO instance (0 or 1)
    PIO pio = pio0;
    // Get first free state machine in PIO 0
    uint sm = pio_claim_unused_sm(pio, true);

    // Add PIO program to PIO instruction memory. SDK will find location and
    // return with the memory offset of the program.
    uint offset = pio_add_program(pio, &blink_program);

    // Calculate the PIO clock divider
    float div = (float)clock_get_hz(clk_sys) / RGB_PIO_FREQ;

    // Initialize the program using the helper function in our .pio file
    blink_program_init(pio, sm, offset, PICO_DEFAULT_LED_PIN, div);

    // Start running our PIO program in the state machine
    pio_sm_set_enabled(pio, sm, true);

}

// == Buttons ==
#define ARM_BTN_GPIO 15
void init_btn(uint pin) {
    gpio_init(pin);
    gpio_set_dir(pin, GPIO_IN);
    gpio_pull_up(pin);
}

alarm_id_t armed_timer_id;
int64_t armed_cb() {
    printf("ROCKET ARMED\n");
    return 0;
}

bool arm_btn_pressed = false;
void check_btns() {
    if(!gpio_get(ARM_BTN_GPIO) && !arm_btn_pressed)  {
        printf("BTN PRESSED\n");
        armed_timer_id = add_alarm_in_ms(5000, armed_cb, NULL, false);
        arm_btn_pressed = true;
        set_led(true);
    }

    if(gpio_get(ARM_BTN_GPIO) && arm_btn_pressed)  {
        printf("BTN LET GO\n");
        cancel_alarm(armed_timer_id);
        arm_btn_pressed = false;
        set_led(false);
    }
}

int main() {
    stdio_init_all();
    // const TM1637Display digit_display_001 = tm1637_init(12, 13);
    // const TM1637Display digit_display_002 = tm1637_init(14, 15);

    // init_btn(ARM_BTN_GPIO);
    // init_leds();
    init_rgb_leds();

    // static absolute_time_t delayed_by_ms (const absolute_time_t t, uint32_t ms)

    // volatile bool timer_fired = false;

    while (true) {
        check_btns();
        sleep_us(50);

        // for (int i = 0; i <= 9999; i ++) {
        //     printf("Display: %d \n", i);
        //     // tm1637_display_number(&digit_display_001, i);
        //     tm1637_display_number(&digit_display_002, i);
        //     sleep_ms(100);
        // }
    }
}