#include <cmath>
#include <stdio.h>

#include "bsp/board.h"
#include "hardware/adc.h"
#include "pico/multicore.h"
#include "pico/stdlib.h"
#include "pico/util/queue.h"
#include "tusb.h"

#include "keyer.h"
#include "settings.h"
#include "tusb_config.h"

namespace
{

}

extern const uint LED_PIN = PICO_DEFAULT_LED_PIN;
extern const uint LEFT_PADDLE_PIN = 14;
extern const uint RIGHT_PADDLE_PIN = 15;
extern const uint BUZZER_PIN = 18;
extern const uint CW_OUT_PIN = 17;
// extern const uint AUDIO_OUT_PIN = 16;
extern const uint ADC_PIN = 26;

// Stuff for communicating between cores
enum class KeyerQueueCommand {
    Run,
    Stop,
    Config,
    Wait,
    SendMessage,
};
struct KeyerQueueData {
    KeyerQueueCommand cmd;
    uint8_t wpm;
    Mode mode;
    std::string message;
};

queue_t keyerQueue;

void setup()
{
    stdio_init_all();
    gpio_init(LED_PIN);
    gpio_set_dir(LED_PIN, GPIO_OUT);
    gpio_put(LED_PIN, 0);

    // Setup pins for left and right paddles
    gpio_init(LEFT_PADDLE_PIN);
    gpio_set_dir(LEFT_PADDLE_PIN, GPIO_IN);
    gpio_pull_up(LEFT_PADDLE_PIN);
    gpio_init(RIGHT_PADDLE_PIN);
    gpio_set_dir(RIGHT_PADDLE_PIN, GPIO_IN);
    gpio_pull_up(RIGHT_PADDLE_PIN);
    gpio_init(CW_OUT_PIN);
    gpio_set_dir(CW_OUT_PIN, GPIO_OUT);
    gpio_put(CW_OUT_PIN, 0);

    // Setup ADC
    adc_init();
    gpio_init(ADC_PIN);
    adc_select_input(0);
}

/* Let's do all the keying stuff in the second core, so there are no timing problems. */
void core1_main()
{
    flash_safe_execute_core_init();
    printf("Hello from core1!\n");
    KeyerQueueData data;
    queue_remove_blocking(&keyerQueue, &data);

    Keyer keyer(data.wpm, data.mode);

    while (true) {
        queue_try_remove(&keyerQueue, &data);

        switch (data.cmd) {
        case KeyerQueueCommand::Run:
            keyer.run();
            break;
        case KeyerQueueCommand::Stop:
            keyer.stop();
            data.cmd = KeyerQueueCommand::Wait;
            break;
        case KeyerQueueCommand::Config:
            keyer.setSpeed(data.wpm);
            keyer.setMode(data.mode);
            data.cmd = KeyerQueueCommand::Run;
            break;
        case KeyerQueueCommand::SendMessage:
            keyer.sendMessage(data.message);
            data.cmd = KeyerQueueCommand::Run;
            break;
        case KeyerQueueCommand::Wait:
            break;
        default:
            break;
        }
    }
}

static void usbSend(uint8_t itf, uint8_t buf[], uint32_t count)
{
    for (uint32_t i = 0; i < count; i++) {
        tud_cdc_n_write_char(itf, buf[i]);
    }

    tud_cdc_n_write_flush(itf);
}

static void usbSend(uint8_t itf, uint8_t value)
{
    tud_cdc_n_write_char(itf, value);
    tud_cdc_n_write_flush(itf);
}

void cdc_task()
{
    const uint8_t USB_IF = 0;

    if (tud_cdc_n_available(USB_IF)) {
        uint8_t buf[64];

        // printf("AHA!!! %d\n", (int)tud_cdc_n_available(USB_IF));
        uint32_t count = tud_cdc_n_read(USB_IF, buf, sizeof(buf));

        if (count > 0) {
            switch (buf[0]) {
            case 'a' ... 'z':
            case 'A' ... 'Z':
            case '0' ... '9':
                printf("TODO: Send it!\n");
                break;
            case 0x00: // ADMIN COMMAND
                switch (buf[1]) {
                case 0x02:              // Host open
                    usbSend(USB_IF, 9); // Send WK1 (v9) for now (no WinKeyer PTT control)
                    break;
                case 0x04: // Echo test
                    printf("Echo test: %x\n", buf[2]);
                    printf("buf address: %p\n", &buf[2]);
                    usbSend(USB_IF, &buf[2], 1);
                    break;
                default:
                    printf("Unknown admin command: %x\n", buf[1]);
                }
                break;
            default:
                printf("Unknown command: %d.\n", buf[0]);
            }
        }
    }
}

/* Returns the voltage level in percent (3,3V == 100%) */
float potiRead()
{
    // 12-bit conversion, assume max value == ADC_VREF == 3.3 V
    const float conversion_factor = 3.3f / (1 << 12);
    float voltage = adc_read() * conversion_factor;

    return voltage * 100 / 3.3;
}

/* Calculates the WPM speed from the volt percentage */
uint8_t calcWPM(float percent, uint8_t wpmMin, uint8_t wpmMax)
{
    auto wpm = (percent * (wpmMax - wpmMin) / 100) + wpmMin;
    uint8_t result = static_cast<uint8_t>(std::round(wpm));
    return result;
}

int main()
{
    timer_hw->dbgpause = 0; // workaround for problem with debug and sleep_ms
    //  https://github.com/raspberrypi/pico-sdk/issues/1152#issuecomment-1418248639

    setup();
    board_init();
    tud_init(BOARD_TUD_RHPORT);

    printf("Hello from core0!\n");

    queue_init(&keyerQueue, sizeof(KeyerQueueData), 2);
    multicore_reset_core1();
    multicore_launch_core1(core1_main);

    Settings settings {read_settings()};

    uint8_t currentWpm {0};
    uint8_t lastWpm {0};

    // If WPM in settings is set to 0 -> take speed from poti
    if (settings.wpm == 0) {
        currentWpm = calcWPM(potiRead(), settings.wpmPotiMin, settings.wpmPotiMax);
    } else {
        currentWpm = settings.wpm;
    }
    lastWpm = currentWpm;

    KeyerQueueData keyerQueueData {KeyerQueueCommand::Run, currentWpm, settings.mode, ""};
    queue_add_blocking(&keyerQueue, &keyerQueueData);

    static bool used = false;

    while (true) {
        tud_task();
        cdc_task();

        currentWpm = calcWPM(potiRead(), settings.wpmPotiMin, settings.wpmPotiMax);

        // If WPM in settings is set to 0 -> take speed from poti
        if (settings.wpm == 0 && (currentWpm != lastWpm)) {
            KeyerQueueData keyerQueueData {KeyerQueueCommand::Config, currentWpm, settings.mode, ""};
            queue_add_blocking(&keyerQueue, &keyerQueueData);
            printf("WPM has changed to: %d\n", currentWpm);
            lastWpm = currentWpm;
        }

        // busy_wait_ms(5000);
        if (!used) {
            // KeyerQueueData keyerQueueData {KeyerQueueCommand::SendMessage, 0, settings.mode, "cq cq de dg2smb dg2smb
            // pse k"}; queue_add_blocking(&keyerQueue, &keyerQueueData);
            used = true;
        }
    }

    return 0;
}