#include "pre_operational_state.h"
#include "motor_controller.h"

#include "p_controller.h"

#include <stdlib.h>
#include <zephyr/kernel.h>
#include <zephyr/sys/printk.h>

PreOperationalState::PreOperationalState(P_controller &p_controller, P_controller &pi_controller)
    : p_controller(p_controller),
      pi_controller(pi_controller),
      pi_selected(true),
      previous_time(0),
      led_state(false),
      input_mode(NONE),
      input_index(0)
{
}

void PreOperationalState::enter()
{
    printk("State: Pre-operational\n");

    // The selection survives a reset, so report it (used by capture_serial.py).
    printk("Controller: %s\n", pi_selected ? "PI" : "P");

    context->stop_motor();

    context->led_off();

    led_state = false;

    previous_time = k_uptime_get();
}

void PreOperationalState::update()
{
    const int64_t current_time = k_uptime_get();

    if (current_time - previous_time >= 500)
    {
        previous_time = current_time;

        led_state = !led_state;

        if (led_state)
        {
            context->led_on();
        }
        else
        {
            context->led_off();
        }
    }
}

void PreOperationalState::command(char c)
{
    // We are currently entering a Kp or Ti value
    if (input_mode != NONE)
    {
        // Enter pressed
        if (c == '\n' || c == '\r')
        {
            if (input_index > 0)
            {
                // End the string
                input_buffer[input_index] = '\0';

                // Convert text to a number
                const double value = atof(input_buffer);

                if (input_mode == ENTER_KP)
                {
                    context->set_Kp(value);

                    printk("Kp set to: %.4f\n", value);
                }
                else if (input_mode == ENTER_TI)
                {
                    context->set_Ti(value);

                    printk("Ti set to: %.4f\n", value);
                }
            }

            // Finished entering a value
            input_index = 0;
            input_mode = NONE;

            return;
        }

        // Store number characters
        if ((c >= '0' && c <= '9') ||
            c == '.' ||
            c == '-')
        {
            if (input_index < sizeof(input_buffer) - 1)
            {
                input_buffer[input_index++] = c;
            }
        }

        return;
    }


    // Normal state commands
    if (c == 'o' || c == 'O')
    {
        printk("Command: set operational\n");

        context->to_operational();
    }

    else if (c == 'r' || c == 'R')
    {
         printk("Command: reset\n");

        context->to_initialization();
    }

    else if (c == 'k' || c == 'K')
    {
        input_mode = ENTER_KP;
        input_index = 0;

        printk("Enter Kp and press Enter:\n");
    }

    else if (c == 't' || c == 'T')
    {
        input_mode = ENTER_TI;
        input_index = 0;

        printk("Enter Ti and press Enter:\n");
    }

    // Select controller algorithm, the controller is passed to the motor controller
    else if (c == 'c' || c == 'C')
    {
        pi_selected = !pi_selected;

        if (pi_selected)
        {
            context->set_controller(pi_controller);
            printk("Controller: PI\n");
        }
        else
        {
            context->set_controller(p_controller);
            printk("Controller: P\n");
        }
    }
}

void PreOperationalState::exit()
{
    context->led_off();
}