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

#include <Arduino.h>
#include <stdlib.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),
      //waiting_for_kp(false),
      //waiting_for_ti(false),
      input_mode(NONE),
      input_index(0)
{
}

void PreOperationalState::enter()
{
    Serial.println("State: Pre-operational");

    context->stop_motor();

    context->led_off();

    led_state = false;

    previous_time = millis();
}

void PreOperationalState::update()
{
    unsigned long current_time = millis();

    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
                double value = atof(input_buffer);

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

                    Serial.print("Kp set to: ");
                    Serial.println(value);
                }
                else if (input_mode == ENTER_TI)
                {
                    context->set_Ti(value);

                    Serial.print("Ti set to: ");
                    Serial.println(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;
                input_index++;
            }
        }

        return;
    }


    // Normal state commands
    if (c == 'o' || c == 'O')
    {
        Serial.println("Command: set operational");

        context->to_operational();
    }

    else if (c == 'r' || c == 'R')
    {
        Serial.println("Command: reset");

        context->to_initialization();
    }

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

        Serial.println("Enter Kp and press Enter:");
    }

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

        Serial.println("Enter Ti and press Enter:");
    }

    // 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);
            Serial.println("Controller: PI");
        }
        else
        {
            context->set_controller(p_controller);
            Serial.println("Controller: P");
        }
    }
}

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