#include "operational_state.h"
#include "motor_controller.h"

//#include <Arduino.h>

#include <zephyr/sys/printk.h>


void OperationalState::enter()
{
    //Serial.println("State: Operational");
    printk("State: Operational\n");

    context->reset_controller();

    context->led_on();

    sample_count = 0;

#if LOG_CSV
    //Serial.println("time_ms,ref_rpm,actual_rpm,pwm");
    printk("time_ms,ref_rpm,actual_rpm,pwm\n");
#endif
}

// Part 1 

//void OperationalState::update()
//{
//    context->run_speed_controller();
//}

// Part 2

void OperationalState::update()
{
    if (context->fault_active())
    {
        //Serial.println("Fault detected");
        printk("Fault detected\n");

        context->to_stopped();

        return;
    }

    if (context->run_speed_controller())
    {
        ++sample_count;

#if LOG_CSV
        // 4 ms á milli sýna, tíminn byrjar á 0 við hverja innkomu í Operational
        // Serial.print(sample_count * 4UL);
        // Serial.print(',');
        // Serial.print(context->get_reference_rpm(), 1);
        // Serial.print(',');
        // Serial.print(context->get_actual_rpm(), 2);
        // Serial.print(',');
        // Serial.println(context->get_pwm(), 1);
        printk("%u,%.1f,%.2f,%.1f\n",
            static_cast<unsigned int>(sample_count * 4U),
            context->get_reference_rpm(),
            context->get_actual_rpm(),
            context->get_pwm());
#endif
    }
}


void OperationalState::command(char c)
{
    if (c == 'r' || c == 'R')
    {
        //Serial.println("Command: reset");
        printk("Command: reset\n");

        context->to_initialization();
    }

    else if (c == 'p' || c == 'P')
    {
        //Serial.println("Command: set pre-operational");
         printk("Command: set pre-operational\n");

        context->to_pre_operational();
    }
}


void OperationalState::exit()
{
    context->stop_motor();

    context->led_off();
}