#include <zephyr/device.h>
#include <zephyr/devicetree.h>
#include <zephyr/drivers/uart.h>
#include <zephyr/kernel.h>
#include <zephyr/sys/printk.h>

#include "rotary_encoder_driver.h"
#include "motor_driver.h"
#include "pi_controller.h"
#include "p_controller.h"
#include "motor_controller.h"

#include "initialization_state.h"

#include "pre_operational_state.h"

#include "operational_state.h"
#include "stopped_state.h"

static RotaryEncoderDriver encoder;

static MotorDriver motor;

const double Kp_P = 10.0;     // P-controller from Project 2

const double Kp = 10.125;     // Ziegler-Nichols PI
const double Ti = 0.0334;
const double T = 0.004;

static P_controller p_controller(Kp_P);
static PI_controller pi_controller(Kp, Ti, T);


static InitializationState initialization_state;
// Pre-operational velur á milli stýringanna með 'c'
static PreOperationalState pre_operational_state(p_controller, pi_controller);
static OperationalState operational_state;
static StoppedState stopped_state;

// Stöðurnar verða að vera skilgreindar á undan motor_controller
static MotorController motor_controller(
    encoder,
    motor,
    pi_controller,      // Sjálfgefin stýring
    initialization_state,
    pre_operational_state,
    operational_state,
    stopped_state
);


static const struct device *const console_dev =
    DEVICE_DT_GET(DT_CHOSEN(zephyr_console));

// Received characters are queued by the UART ISR, like the RX buffer that
// Arduino's Serial had. The USART has a single receive register, so
// polling it from the main loop loses characters that arrive in a burst.
K_MSGQ_DEFINE(rx_queue, sizeof(char), 64, 1);

static void uart_rx_isr(const struct device *dev, void *user_data)
{
    (void)user_data;

    if (!uart_irq_update(dev)) {
        return;
    }

    while (uart_irq_rx_ready(dev)) {
        unsigned char c;

        if (uart_fifo_read(dev, &c, 1) != 1) {
            break;
        }

        // Drop the character if the queue is full.
        (void)k_msgq_put(&rx_queue, &c, K_NO_WAIT);
    }
}

extern "C" void motor_application_main(void)
{
    if (!device_is_ready(console_dev)) {
        printk("Console UART is not ready\n");
        return;
    }

    uart_irq_callback_user_data_set(console_dev, uart_rx_isr, nullptr);
    uart_irq_rx_enable(console_dev);

    const int encoder_result = encoder.begin();
    if (encoder_result < 0) {
        printk("Encoder setup failed: %d\n", encoder_result);
        return;
    }

    motor.begin();
    motor_controller.begin();

    while (true) {
        char received_char;

        // Handle every character the ISR has queued since the last pass.
        while (k_msgq_get(&rx_queue, &received_char, K_NO_WAIT) == 0) {
            motor_controller.command(received_char);
        }

        motor_controller.update();

        // Sleep until the encoder has a new speed sample (every 4 ms), so
        // the controller runs right after sampling. The timeout only
        // matters if the timer stops; the thread is idle while it waits.
        encoder.wait_for_sample(K_MSEC(10));
    }
}