#include <stdio.h>

#include "pico/stdlib.h"

#include "keyer.h"
#include "sidetone.h"
#include "morse.h"

extern const uint LED_PIN;
extern const uint LEFT_PADDLE_PIN;
extern const uint RIGHT_PADDLE_PIN;
extern const uint BUZZER_PIN;
extern const uint CW_OUT_PIN;
// extern const uint AUDIO_OUT_PIN;

const uint SIDETONE_FREQ = 622;

bool left_paddle_pressed()
{
    if (!gpio_get(LEFT_PADDLE_PIN)) {
        return true;
    }
    return false;
}

bool right_paddle_pressed()
{
    if (!gpio_get(RIGHT_PADDLE_PIN)) {
        return true;
    }
    return false;
}

uint64_t calcElementDurationUs(uint8_t wpm)
{
    uint64_t duration = static_cast<uint64_t>(1.2 / static_cast<uint64_t>(wpm) * 1000 * 1000);
    return duration;
}

Keyer::Keyer(uint8_t wpm, Mode mode) : m_wpm(wpm), m_mode(mode), m_buzzer(BUZZER_PIN) //, m_audioOut(AUDIO_OUT_PIN)
{
    m_elementDuration = calcElementDurationUs(m_wpm);
}

void Keyer::setSpeed(uint8_t wpm)
{
    m_wpm = wpm;
    m_elementDuration = calcElementDurationUs(wpm);
}

void Keyer::sendMessage(std::string msg)
{
    std::string morse = messageToMorse(msg);

    for (char c : morse) {
        m_messageQueue.push(c);
    }
}

void Keyer::sendCharacter(char ch)
{
    std::string morse = charToMorse(ch);

    for (char c : morse) {
        m_messageQueue.push(c);
    }
}

void Keyer::run()
{
    auto timestamp = get_absolute_time();

    // If there is some message to send …
    if (!m_messageQueue.empty() || m_messageKeyingState != MessageState::Wait) {
        // Stop all paddle keying, if necessary
        if (m_paddleKeyingState != State::Wait) {
            gpio_put(LED_PIN, 0);
            gpio_put(CW_OUT_PIN, 0);
            m_buzzer.off();
            m_keyNextIambicB = false;
            m_currentlyPausing = false;
            m_currentlyKeying = false;
            m_previousState = State::Abort;
            m_paddleKeyingState = State::Wait;
        }

        switch (m_messageKeyingState) {
        case MessageState::Wait:
            m_messageChar = m_messageQueue.front();
            m_messageQueue.pop();

            switch (m_messageChar) {
            case '.':
                m_messageKeyingState = MessageState::Dit;
                break;
            case '-':
                m_messageKeyingState = MessageState::Dah;
                break;
            case 'i':
                m_messageKeyingState = MessageState::IntraCharSpace;
                break;
            case 'c':
                m_messageKeyingState = MessageState::InterCharSpace;
                break;
            case 'w':
                m_messageKeyingState = MessageState::InterWordSpace;
                break;
            }

            break;
        case MessageState::Dit:
            if (!m_currentlyKeying) {
                m_currentlyKeying = true;
                m_keying_until = make_timeout_time_us(m_elementDuration);
                gpio_put(LED_PIN, 1);
                gpio_put(CW_OUT_PIN, 1);
                m_buzzer.on(SIDETONE_FREQ);
            }
            if (left_paddle_pressed() || right_paddle_pressed()) {
                m_messageKeyingState = MessageState::Abort;
            }
            if (absolute_time_diff_us(timestamp, m_keying_until) <= 0) {
                m_currentlyKeying = false;
                gpio_put(LED_PIN, 0);
                gpio_put(CW_OUT_PIN, 0);
                m_buzzer.off();
                m_messageKeyingState = MessageState::Wait;
            }
            break;
        case MessageState::Dah:
            if (!m_currentlyKeying) {
                m_currentlyKeying = true;
                m_keying_until = make_timeout_time_us(m_elementDuration * 3);
                gpio_put(LED_PIN, 1);
                gpio_put(CW_OUT_PIN, 1);
                m_buzzer.on(SIDETONE_FREQ);
            }
            if (left_paddle_pressed() || right_paddle_pressed()) {
                m_messageKeyingState = MessageState::Abort;
            }
            if (absolute_time_diff_us(timestamp, m_keying_until) <= 0) {
                m_currentlyKeying = false;
                gpio_put(LED_PIN, 0);
                gpio_put(CW_OUT_PIN, 0);
                m_buzzer.off();
                m_messageKeyingState = MessageState::Wait;
            }
            break;
        case MessageState::IntraCharSpace:
            if (!m_currentlyPausing) {
                m_pausing_until = make_timeout_time_us(m_elementDuration);
                m_currentlyPausing = true;
            }
            if (left_paddle_pressed() || right_paddle_pressed()) {
                m_messageKeyingState = MessageState::Abort;
            }
            if (absolute_time_diff_us(timestamp, m_pausing_until) <= 0) {
                m_currentlyPausing = false;
                m_messageKeyingState = MessageState::Wait;
            }
            break;
        case MessageState::InterCharSpace:
            if (!m_currentlyPausing) {
                m_pausing_until = make_timeout_time_us(m_elementDuration * 3);
                m_currentlyPausing = true;
            }
            if (left_paddle_pressed() || right_paddle_pressed()) {
                m_messageKeyingState = MessageState::Abort;
            }
            if (absolute_time_diff_us(timestamp, m_pausing_until) <= 0) {
                m_currentlyPausing = false;
                m_messageKeyingState = MessageState::Wait;
            }
            break;
        case MessageState::InterWordSpace:
            if (!m_currentlyPausing) {
                m_pausing_until = make_timeout_time_us(m_elementDuration * (7-3)); // 7-3, because we aleady have the InterCharSpace of 3
                m_currentlyPausing = true;
            }
            if (left_paddle_pressed() || right_paddle_pressed()) {
                m_messageKeyingState = MessageState::Abort;
            }
            if (absolute_time_diff_us(timestamp, m_pausing_until) <= 0) {
                m_currentlyPausing = false;
                m_messageKeyingState = MessageState::Wait;
            }
            break;
        case MessageState::Abort:
        {
            gpio_put(LED_PIN, 0);
            gpio_put(CW_OUT_PIN, 0);
            m_buzzer.off();
            m_currentlyPausing = false;
            m_currentlyKeying = false;
            std::queue<char> emptyQueue;
            std::swap(m_messageQueue, emptyQueue);
            m_messageKeyingState = MessageState::Wait;
            break;
        }
        default:
            break;
        }

        return;
    }

    // If we are in Straight key mode …
    if (m_mode == Mode::Straight) {
        if (left_paddle_pressed()) {
            gpio_put(LED_PIN, 1);
            gpio_put(CW_OUT_PIN, 1);
            m_buzzer.on(SIDETONE_FREQ);
        } else {
            gpio_put(LED_PIN, 0);
            gpio_put(CW_OUT_PIN, 0);
            m_buzzer.off();
        }

        return;
    }

    // If we are in IambicA or IambicB-Mode …
    switch (m_paddleKeyingState) {
    case State::Wait:
        if (left_paddle_pressed()) {
            m_keyNextIambicB = false;
            m_paddleKeyingState = State::Dit;
        } else if (right_paddle_pressed()) {
            m_keyNextIambicB = false;
            m_paddleKeyingState = State::Dah;
        } else {
            if (m_mode == Mode::IambicB && m_keyNextIambicB) {
                if (m_previousState == State::Dit)
                    m_paddleKeyingState = State::Dah;
                else
                    m_paddleKeyingState = State::Dit;

                m_keyNextIambicB = false;
            }
        }
        break;
    case State::Dit:
        if (!m_currentlyKeying) {
            m_currentlyKeying = true;
            m_keying_until = make_timeout_time_us(m_elementDuration);
            gpio_put(LED_PIN, 1);
            gpio_put(CW_OUT_PIN, 1);
            m_buzzer.on(SIDETONE_FREQ);
            // m_audioOut.on(SIDETONE_FREQ);
        } else {
            // If right paddle üressed -> note for Iambic B
            if (right_paddle_pressed() && !m_keyNextIambicB) {
                m_keyNextIambicB = true;
            }

            if (absolute_time_diff_us(timestamp, m_keying_until) <= 0) {
                m_currentlyKeying = false;
                gpio_put(LED_PIN, 0);
                gpio_put(CW_OUT_PIN, 0);
                m_buzzer.off();
                // m_audioOut.off();
                m_previousState = State::Dit;
                m_paddleKeyingState = State::DitPause;
            }
        }
        break;
    case State::Dah:
        if (!m_currentlyKeying) {
            m_currentlyKeying = true;
            m_keying_until = make_timeout_time_us(m_elementDuration * 3);
            gpio_put(LED_PIN, 1);
            gpio_put(CW_OUT_PIN, 1);
            m_buzzer.on(SIDETONE_FREQ);
            // m_audioOut.on(SIDETONE_FREQ);
        } else {
            // If left paddle pressed -> Note for Iambic B
            if (left_paddle_pressed() && !m_keyNextIambicB) {
                m_keyNextIambicB = true;
            }

            if (absolute_time_diff_us(timestamp, m_keying_until) <= 0) {
                m_currentlyKeying = false;
                gpio_put(LED_PIN, 0);
                gpio_put(CW_OUT_PIN, 0);
                m_buzzer.off();
                // m_audioOut.off();
                m_previousState = State::Dah;
                m_paddleKeyingState = State::DahPause;
            }
        }
        break;
    case State::DitPause:
        if (!m_currentlyPausing) {
            m_pausing_until = make_timeout_time_us(m_elementDuration);
            m_currentlyPausing = true;
        } else {
            if (absolute_time_diff_us(timestamp, m_pausing_until) <= 0) {
                m_currentlyPausing = false;

                if (right_paddle_pressed()) {
                    m_paddleKeyingState = State::Dah;
                    m_keyNextIambicB = false;
                } else if (left_paddle_pressed()) {
                    m_paddleKeyingState = State::Dit;
                    m_keyNextIambicB = false;
                } else {
                    m_paddleKeyingState = State::Wait;
                }
            }
        }
        break;
    case State::DahPause:
        if (!m_currentlyPausing) {
            m_pausing_until = make_timeout_time_us(m_elementDuration);
            m_currentlyPausing = true;
        } else {
            if (absolute_time_diff_us(timestamp, m_pausing_until) <= 0) {
                m_currentlyPausing = false;

                if (left_paddle_pressed()) {
                    m_paddleKeyingState = State::Dit;
                    m_keyNextIambicB = false;
                } else if (right_paddle_pressed()) {
                    m_paddleKeyingState = State::Dah;
                    m_keyNextIambicB = false;
                } else {
                    m_paddleKeyingState = State::Wait;
                }
            }
        }
        break;
    case State::Abort:
        gpio_put(LED_PIN, 0);
        gpio_put(CW_OUT_PIN, 0);
        m_buzzer.off();
        // m_audioOut.off();
        m_keyNextIambicB = false;
        m_currentlyPausing = false;
        m_currentlyKeying = false;
        m_previousState = State::Abort;
        m_paddleKeyingState = State::Wait;
        break;
    default:
        break;
    }
}

void Keyer::stop() { m_paddleKeyingState = State::Abort; }