#include <stdio.h>
#include <stdint.h>
#include "pico/stdlib.h"
#include "hardware/gpio.h"
#include <string.h>


#define DISP_INTERVAL 3


/* DOUT */
#define DISP1_CLK   17
#define DISP1_DIO  16
#define DISP2_CLK   19
#define DISP2_DIO  18
#define LED_PIN    PICO_DEFAULT_LED_PIN
#define BUTTON_P1     12
#define BUTTON_P2     8


/* DIN */
#define ROT_S0        2
#define ROT_S1        3
#define ROT_KEY       6

#define BUZZ_DURATION 2000
#define BUZZ_FREQ     220


/* GP7 is PWM B[3] */
#define SPK_PWM_CHAN PWM_CHAN_B
#define SPK_PWM_SLICE 3   

#define get_absolute_time_ms() us_to_ms(get_absolute_time())

/* TM1637 display driver */

static void tm_clk_start(uint8_t disp)
{
    uint8_t clk = DISP1_CLK, dio = DISP1_DIO;
    if (disp == 2)
        clk = DISP2_CLK, dio = DISP2_DIO;
    gpio_put(clk, 1);
    gpio_put(dio, 1);
    sleep_us(DISP_INTERVAL);
    gpio_put(dio, 0);
    sleep_us(DISP_INTERVAL);
}

static void tm_clk_stop(uint8_t disp)
{
    uint8_t clk = DISP1_CLK, dio = DISP1_DIO;
    if (disp == 2)
        clk = DISP2_CLK, dio = DISP2_DIO;
    gpio_put(clk, 0);
    sleep_us(DISP_INTERVAL);
    gpio_put(dio, 0);
    sleep_us(DISP_INTERVAL);
    gpio_put(clk, 1);
    sleep_us(DISP_INTERVAL);
    gpio_put(dio, 1);
    sleep_us(DISP_INTERVAL);
}

static uint8_t tm_wait_ack(uint8_t disp)
{
    uint8_t clk = DISP1_CLK, dio = DISP1_DIO;
    uint8_t ack;
    if (disp == 2)
        clk = DISP2_CLK, dio = DISP2_DIO;
    gpio_put(clk, 0);
    sleep_us(2 * DISP_INTERVAL);
    gpio_put(clk, 1);
    sleep_us(DISP_INTERVAL);
    gpio_put(clk, 0);
    return 0;
}

void tm_write(uint8_t disp, const uint8_t *b, uint8_t sz, int nostop)
{
    uint8_t clk = DISP1_CLK, dio = DISP1_DIO;
    int i, c;
    if (disp == 2)
        clk = DISP2_CLK, dio = DISP2_DIO;
    gpio_put(dio, 0);
    sleep_us(DISP_INTERVAL);
    gpio_put(clk, 0);
    sleep_us(DISP_INTERVAL);
    for (c = 0; c < sz; c++) {
        uint8_t sliding_buf = b[c];
        for (i = 0; i < 8; i++) {
            gpio_put(clk, 0);
            gpio_put(dio, sliding_buf & 0x01);
            //gpio_put(dio, (sliding_buf & 0x80) >> 7);
            sleep_us(DISP_INTERVAL);
            sliding_buf >>= 1;
            //sliding_buf = (sliding_buf & 0x7F) << 1;
            gpio_put(clk, 1);
            sleep_us(DISP_INTERVAL);
        }
        tm_wait_ack(disp);
    }
}

#define CMD_DATA 0x40
#define CMD_DATA_WRITE      0x00
#define CMD_DATA_READ        0x02
#define CMD_DATA_FLAG_AAUTO 0x04
#define CMD_DATA_FLAG_TEST  0x08

#define CMD_ADDRESS 0xC0

#define CMD_DISPLAY 0x80
#define CMD_DISPLAY_SWITCH_ON 0x08
#define CMD_DISPLAY_PULSE_MASK 0x07

const unsigned char numchar[10] = {
    /* 0 = 11111100 */ 0x3F,
    /* 1 = 01100000 */ 0x06,
    /* 2 = 11011010 */ 0x5B,
    /* 3 = 11110010 */ 0x4F,
    /* 4 = 01100110 */ 0x66,
    /* 5 = 10110110 */ 0x6D,
    /* 6 = 10111110 */ 0x7D,
    /* 7 = 11100000 */ 0x07,
    /* 8 = 11111110 */ 0x7F,
    /* 9 = 11110110 */ 0x6F
};

const unsigned char CHAR_NONE = 0x00;
const unsigned char CHAR_A = 0x77;
const unsigned char CHAR_B = 0x7C;
const unsigned char CHAR_C = 0x39;
const unsigned char CHAR_D = 0x5E;
const unsigned char CHAR_E = 0x79;
const unsigned char CHAR_F = 0x71;
const unsigned char CHAR_G = numchar[9];
const unsigned char CHAR_H = 0x76;
const unsigned char CHAR_I = numchar[1];
const unsigned char CHAR_L = 0x38;
const unsigned char CHAR_N = 0x54;
const unsigned char CHAR_O = 0x5C;
const unsigned char CHAR_P = 0x73;
const unsigned char CHAR_R = 0x50;
const unsigned char CHAR_S = numchar[5];
const unsigned char CHAR_T = numchar[7];
const unsigned char CHAR_U = 0x3E;
const unsigned char CHAR_Y = 0x6E;
const unsigned char CHAR_Z = numchar[2];


const unsigned char txt_sel[4] = {CHAR_S, CHAR_E, CHAR_L, CHAR_NONE };
const unsigned char txt_cus[4] = {CHAR_C, CHAR_U, CHAR_S, CHAR_T };
const unsigned char txt_good[4] = {CHAR_G, CHAR_O, CHAR_O, CHAR_D };
const unsigned char txt_day[4] = {CHAR_NONE, CHAR_D, CHAR_A, CHAR_Y };
const unsigned char txt_done[4] = {CHAR_D, CHAR_O, CHAR_N, CHAR_E };
const unsigned char txt_pause0[4] = {CHAR_P, CHAR_A, CHAR_U, CHAR_S };
const unsigned char txt_pause1[4] = {CHAR_E, CHAR_D, CHAR_NONE, CHAR_NONE };

const unsigned char CHAR_DOTS = 0x80;
const unsigned char CHAR_MINUS = 0x40;


static void display_set_brightness(int disp, uint8_t brightness)
{
    uint8_t cmd;
    tm_clk_start(disp);
    cmd = CMD_DISPLAY | CMD_DISPLAY_SWITCH_ON | (brightness & CMD_DISPLAY_PULSE_MASK);
    tm_write(disp, &cmd, 1, 0);
    tm_clk_stop(disp);
}

static void display_show(uint8_t disp, const unsigned char *c, int colon)
{
    int i;
    unsigned char cmd = CMD_DATA;
    unsigned char payload[6];

    tm_clk_start(disp);
    tm_write(disp, &cmd, 1, 1);
    tm_clk_stop(disp);
    sleep_ms(1);

    memcpy(payload + 1, c, 4);
    payload[0] = CMD_ADDRESS;


    if (colon == 1) {
         payload[2] |= CHAR_DOTS;
    }
    if (colon == 2) {
         payload[3] |= CHAR_DOTS;
    }
    tm_clk_start(disp);
    tm_write(disp, payload, 5, 1);
    tm_clk_stop(disp);
    sleep_ms(1);
}

static void display_clear(uint8_t disp)
{
    const unsigned char zero[4] = {0, 0, 0, 0};
    display_show(disp, zero, 0);
}

void num_to_digits(uint16_t n, uint8_t *out, int lead_zero)
{
    memset(out, 0, 2);
    if (n > 99)
        return;
    if (n < 10) {
        if (lead_zero)
            out[0] = numchar[0];
        out[1] = numchar[n];
    } else {
        out[0] = numchar[n / 10];
        out[1] = numchar[n % 10];
    }
}

static void display_time(uint8_t disp, uint32_t t_sec, uint32_t t_usec, int col)
{
    uint32_t minutes = 0;
    uint32_t cent = 0;
    unsigned char out[4];
    int colon;
    memset(out, 0, 4);
    if (t_usec < 500000)
        colon = 1;
    if (t_sec > 59) {
        minutes = t_sec / 60;
        t_sec -= minutes * 60;
        num_to_digits(minutes, out, 0);
        num_to_digits(t_sec, out + 2, 1);
        display_show(disp, out, col);
    } else {
        cent = t_usec / 10000;
        num_to_digits(t_sec, out, 0);
        num_to_digits(cent, out + 2, 1);
        out[3] = CHAR_NONE;
        display_show(disp, out, col);
    }
}


/* States */

#define STATE_WALLCLOCK 0 /* Press B1 or B2: GAME_SET, long press KEY: clock adjust */
#define STATE_GAME_SET  1 /* Setup menu, confirm = GAME_READY */
#define STATE_GAME_CUSTOM 2 /* Setup custom time, confirm = GAME_READY */
#define STATE_GAME_READY 3 /* Press B1 or B2: GAME_PLAY P2/P1 */
#define STATE_GAME_PLAY_P1 4 /* Press B1: GAME_PLAY_P2, press B1+B2: GAME_PAUSE */
#define STATE_GAME_PLAY_P2 5 /* Bress B2: GAME_PLAY_P1, press B1+B2: GAME_PAUSE */
#define STATE_GAME_PAUSE 6 /* Press B1: GAME_PLAY_P2, B2: GAME_PLAY_P1, KEY: GAME_SET */
#define STATE_GAME_TIMEOUT   7 /* Press any key: GAME_SET */
#define STATE_CLOCK_ADJUST   8 /* */

static uint8_t State = STATE_WALLCLOCK;

struct play_mode {
    uint32_t minutes;
    uint32_t increment_seconds;
};


static struct play_mode Modes[] = {
    { 10, 0 },
    { 15, 10},
    { 30, 0 },
    { 45, 45},
    { 60, 0 },
    {  5, 5 },
    {  3, 2 },
    {  2, 1 },
    {  0, 0 } //Custom
};

#define N_MODES 9

static int current_mode = 0;

static void prev_mode(void)
{
    if (current_mode == 0)
        current_mode = N_MODES - 1;
    else
        current_mode--;
}

static void next_mode(void)
{
    if (current_mode == N_MODES - 1)
        current_mode = 0;
    else
        current_mode++;
}

struct game {
    int black_player;
    uint32_t p1_time;
    uint32_t p2_time;
    uint32_t move_count;
};

static struct game Game = { 0, 0, 0, 0 };


static void game_start(void)
{
    Game.p1_time = Modes[current_mode].minutes * 60000;
    Game.p2_time = Modes[current_mode].minutes * 60000;
    Game.move_count = 0;
}

#define BUTTON_DEBOUNCE_TIME 5
#define BUTTON_LONG_PRESS_TIME 80

uint32_t rotary_action(void)
{
    uint32_t action = 0;
    if (gpio_get(ROT_S0))
        action |= 1;
    if (gpio_get(ROT_S1))
        action |= 2;
    return action;
}


#define ROT_UP 1
#define ROT_DOWN 2

int rot_updown(void)
{
    static int rot_up = 0;
    static int rot_down = 0;
    static uint32_t rotary_pos = 0;
    uint32_t rotary_now;
    rotary_now = rotary_action();
    if (rotary_now != rotary_pos) {
        if ((rotary_pos == 0 && rotary_now == 3) ||
                (rotary_pos == 3 && rotary_now == 2) ||
                (rotary_pos == 2 && rotary_now == 0) ||
                (rotary_pos == 1 && rotary_now == 3)) {
            rot_down++;
            rot_up = 0;
        }
        if ((rotary_pos == 0 && rotary_now == 1) ||
                (rotary_pos == 3 && rotary_now == 1) ||
                (rotary_pos == 2 && rotary_now == 3) ||
                (rotary_pos == 1 && rotary_now == 0)) {
            rot_up++;
            rot_down = 0;
        }
        rotary_pos = rotary_now;
    }
    if (rot_up > 1) {
        rot_up = 0;
        return ROT_UP;
    }
    if (rot_down > 1) {
        rot_down = 0;
        return ROT_DOWN;
    }
    return 0;
}


void poll_buttons(void)
{
    static uint32_t b1_press_counter = 0;
    static uint32_t b2_press_counter = 0;
    static uint32_t key_press_counter = 0;
    int rot_ud = 0;
    uint8_t oldstate;
    uint8_t black_player = 0;
    static uint32_t last_update = 0;

    if (gpio_get(ROT_KEY) == 0) {
        printf("K ON\n");
        key_press_counter++;
    } else {
        key_press_counter = 0;
    }

    if (gpio_get(BUTTON_P1) == 0)
        b1_press_counter++;
    else
        b1_press_counter = 0;

    if (gpio_get(BUTTON_P2) == 0)
        b2_press_counter++;
    else
        b2_press_counter = 0;

    oldstate = State;

    if (b1_press_counter >= BUTTON_DEBOUNCE_TIME)
        printf("b1: pressed\n");
    if (b2_press_counter >= BUTTON_DEBOUNCE_TIME)
        printf("b2: pressed\n");
    
    switch(State) {
        case STATE_WALLCLOCK:
            if ((b1_press_counter == BUTTON_DEBOUNCE_TIME) ||
                (b2_press_counter == BUTTON_DEBOUNCE_TIME) || 
                (key_press_counter == BUTTON_DEBOUNCE_TIME)) {
                State = STATE_GAME_SET;
            }
            break;
        case STATE_GAME_SET:
            rot_ud = rot_updown();
            if (rot_ud == ROT_UP) {
                next_mode();
            }
            if (rot_ud == ROT_DOWN) {
                prev_mode();
            }

            // display_current_mode
            //
            if (key_press_counter == BUTTON_LONG_PRESS_TIME) {
                if (current_mode == N_MODES - 1) {

                }
                game_start();
                State = STATE_GAME_READY;
            }
            break;
        case STATE_GAME_READY:
            if (b1_press_counter > 0)  {
                /* P1 is black */
                State = STATE_GAME_PLAY_P2;
                last_update = get_absolute_time_ms();
            } else if (b2_press_counter > 0)  {
                /* P2 is black */
                State = STATE_GAME_PLAY_P1;
                last_update = get_absolute_time_ms();
            }
            break;
            
        case STATE_GAME_PLAY_P1:
            {
                uint32_t now = get_absolute_time_ms();
                int diff = now - last_update;
                if (diff >= Game.p1_time) {
                    Game.p1_time = 0;
                    State = STATE_GAME_TIMEOUT;
                    break;
                }
                Game.p1_time -= diff;
                last_update = now;
                if (b1_press_counter > 0) {
                    Game.p1_time += Modes[current_mode].increment_seconds * 1000;
                    State = STATE_GAME_PLAY_P2;
                    break;
                }
                if (key_press_counter > BUTTON_DEBOUNCE_TIME) {
                    b2_press_counter = 0;
                    key_press_counter = 0;
                    State = STATE_GAME_PAUSE;
                }
            }
            break;
        case STATE_GAME_PLAY_P2:
            {
                uint32_t now = get_absolute_time_ms();
                int diff = now - last_update;
                if (diff >= Game.p2_time) {
                    Game.p2_time = 0;
                    State = STATE_GAME_TIMEOUT;
                    break;
                }
                Game.p2_time -= diff;
                last_update = now;
                if (b2_press_counter > 0) {
                    Game.p2_time += Modes[current_mode].increment_seconds * 1000;
                    State = STATE_GAME_PLAY_P1;
                }
                if (key_press_counter > BUTTON_DEBOUNCE_TIME) {
                    State = STATE_GAME_PAUSE;
                }
            }
            break;
        case STATE_GAME_PAUSE:
            if (b2_press_counter == BUTTON_DEBOUNCE_TIME) {
                State = STATE_GAME_PLAY_P1;
            } else if (b1_press_counter == BUTTON_DEBOUNCE_TIME) {
                State = STATE_GAME_PLAY_P2;
            }
            break;
       case STATE_GAME_TIMEOUT:
            if (key_press_counter == BUTTON_LONG_PRESS_TIME) {
                State = STATE_WALLCLOCK;
            }
            break;

    }
    if (oldstate != State) { 
        b1_press_counter = 0;
        b2_press_counter = 0;
        key_press_counter = 0;
        printf("State: %d\n", State);
    }
}


void system_boot(void)
{
    int i;
    set_sys_clock_48mhz();
    stdio_init_all();
    /* LED and Douts */
    gpio_init(LED_PIN);
    gpio_set_dir(LED_PIN, GPIO_OUT);
    gpio_put(LED_PIN,1);
    sleep_ms(1000);
    gpio_put(LED_PIN, 0);

    printf("Setting LEDs and digital outputs...\n");
    gpio_init(DISP1_CLK);
    gpio_init(DISP1_DIO);
    gpio_init(DISP2_CLK);
    gpio_init(DISP2_DIO);
    gpio_set_dir(DISP1_CLK, GPIO_OUT);
    gpio_set_dir(DISP1_DIO, GPIO_OUT);
    gpio_set_dir(DISP2_CLK, GPIO_OUT);
    gpio_set_dir(DISP2_DIO, GPIO_OUT);
    gpio_set_pulls(DISP1_CLK, 1, 0);
    gpio_set_pulls(DISP1_DIO, 1, 0);
    gpio_set_pulls(DISP2_CLK, 1, 0);
    gpio_set_pulls(DISP2_DIO, 1, 0);
    gpio_put(DISP1_CLK, 1);
    gpio_put(DISP1_DIO, 1);
    gpio_put(DISP2_CLK, 1);
    gpio_put(DISP2_DIO, 1);
    printf("Done.\n");

    /* BTN Dins */
    printf("Setting buttons and digital inputs...\n");
    gpio_init(BUTTON_P1);
    gpio_init(BUTTON_P2);
    gpio_init(ROT_KEY);
    gpio_init(ROT_S1);
    gpio_init(ROT_S0);
    gpio_set_dir(BUTTON_P1, GPIO_IN);
    gpio_set_dir(BUTTON_P2, GPIO_IN);
    gpio_set_dir(ROT_KEY, GPIO_IN);
    gpio_set_dir(ROT_S0, GPIO_IN);
    gpio_set_dir(ROT_S1, GPIO_IN);
    gpio_pull_down(ROT_KEY);
    gpio_pull_up(BUTTON_P1);
    gpio_pull_up(BUTTON_P2);

    printf("Wating up...\n");
    sleep_ms(2000);
    printf("Done.\n");
}


int main() {
    uint val = 1;
    uint8_t btn;
    uint16_t pot = 0;
    static uint16_t oldpot = 0;
    int i;
    system_boot();
    printf("Loop started.\n");

    display_set_brightness(1, 7);
    display_set_brightness(2, 7);
    display_clear(1);
    display_clear(2);
    while (1) {
        poll_buttons();
        switch (State) {
            case STATE_WALLCLOCK:
                {
                    display_set_brightness(1, 7);
                    display_set_brightness(2, 7);
                    display_show(2, txt_good, 0);
                    display_show(1, txt_day, 0);
                }
                break;
            case STATE_GAME_SET:
            {
                struct play_mode *md = &Modes[current_mode];
                display_show(2, txt_sel, 0);
                if (current_mode == N_MODES - 1) {
                    display_show(1, txt_cus, 0);
                } else {
                    uint8_t dn[4];
                    num_to_digits(md->minutes, dn, 0);
                    num_to_digits(md->increment_seconds, dn + 2, 0); 
                    display_show(1, dn, 1);
                }
            }
            break;
            case STATE_GAME_CUSTOM:
            {
                struct play_mode *md = &Modes[N_MODES - 1];
                uint8_t dn[4];
                display_show(2, txt_cus, 0);
                num_to_digits(md->minutes, dn,  0);
                num_to_digits(md->increment_seconds, dn + 2, 0);
                display_show(1, dn, 1); 
            }
            break;
            case STATE_GAME_READY:
            {
                char txt_2[4] = { CHAR_P, CHAR_U, CHAR_S, CHAR_H };
                char txt_1[4] = { CHAR_B, CHAR_L, CHAR_A, CHAR_C };
                display_show(2, txt_2, 0);
                display_show(1, txt_1, 0);
            }
            break;
            case STATE_GAME_PLAY_P1:
            {
                int col = 0;
                if ((Game.p1_time % 1000) < 500)
                    col = 1;
                display_time(1, Game.p1_time / 1000, (Game.p1_time % 1000) * 1000, col);
                display_time(2, Game.p2_time / 1000, (Game.p2_time % 1000) * 1000, 1);
                display_set_brightness(1, 7);
                display_set_brightness(2, 1);
            }
            break;
            case STATE_GAME_PLAY_P2:
            {
                int col = 0;
                if ((Game.p2_time % 1000) < 500)
                    col = 1;
                display_time(1, Game.p1_time / 1000, (Game.p1_time % 1000) * 1000, 1);
                display_time(2, Game.p2_time / 1000, (Game.p2_time % 1000) * 1000, col);
                display_set_brightness(1, 1);
                display_set_brightness(2, 7);
            }
            break;
            case STATE_GAME_PAUSE:
            {
                display_show(2, txt_pause0, 0);
                display_show(1, txt_pause1, 0);
            }
            break;
            case STATE_GAME_TIMEOUT:
            {
                uint32_t now = get_absolute_time_ms();
                if ((now % 1000) > 500) {
                    if (Game.p1_time == 0) {
                        display_clear(1);
                    }
                    if (Game.p2_time == 0) {
                        display_clear(2);
                    }
                } else {
                    if (Game.p1_time == 0) {
                        display_time(1,0,0,1);
                    }
                    if (Game.p2_time == 0) {
                        display_time(2,0,0,1);
                    }
                }

            }
            break;
            default:
                display_show(2, txt_good, 0);
                display_show(1, txt_done, 0);
        }
        sleep_ms(10);
    }
    return 0;
}