self-balancing-robot

// Copyright 2026 Filip Dymczyk and Konrad Grucel

#include "control_loop.h"
#include "ble_protocol_types.h"
#include "drivers_initializer.h"
#include "interface.h"
#include "logger.h"
#include "main_state_machine.h"
#include "motor_controller.h"
#include "robot_controller.h"
#include "zephyr/kernel.h"

#ifdef CONFIG_MODEL_IDENTIFICATION_DRV
#include "model_identification.h"
#endif  // CONFIG_MODEL_IDENTIFICATION_DRV

#ifdef CONFIG_WATCHDOG_CONTROLLER_DRV
#include "watchdog_controller.h"
#endif  // CONFIG_WATCHDOG_CONTROLLER_DRV

#ifdef CONFIG_BLUETOOTH_DRV
#include "ble_service.h"
#endif  // CONFIG_BLUETOOTH_DRV

static Logger<IS_ENABLED(CONFIG_ROBOT_CONTROL_LOG)> robot_control_logger("ROBOT_CONTROL");

namespace Robot_Control
{

#ifdef CONFIG_BLUETOOTH_DRV
void
ble_packet_callback(BLE_Protocol::Received_Packet const& received_packet)
{
    Robot_Controller::instance().handle_ble_packet(received_packet);
}
#endif  // CONFIG_BLUETOOTH_DRV

int
control_loop_init()
{
#ifdef CONFIG_MODEL_IDENTIFICATION_DRV
    robot_control_logger.platform_log(LOG_LEVEL::INF, "Model identification driver is enabled.");
#endif  // CONFIG_MODEL_IDENTIFICATION_DRV

    Drivers_Initializer::init();

#ifdef CONFIG_BLUETOOTH_DRV
    ble_packet_received_cb_register(&ble_packet_callback);
#endif  // CONFIG_BLUETOOTH_DRV

    set_enable_controller(true);

    robot_control_logger.platform_log(LOG_LEVEL::INF, "Robot control init finished");

    return 0;
}

static void
control_loop_work_handler(k_work* work)
{
    ARG_UNUSED(work);
#ifdef CONFIG_WATCHDOG_CONTROLLER_DRV
    feed_watchdog();
#endif  // CONFIG_WATCHDOG_CONTROLLER_DRV

    using State = Main_State_Machine::State;

    Main_State_Machine::instance().update();
    State const state = Main_State_Machine::instance().get_state();
    switch(state)
    {
        case State::OPERATION:
        {
#ifndef CONFIG_MODEL_IDENTIFICATION_DRV
            bool const disable_motors_command = Robot_Controller::instance().normal_motors_control();
            Main_State_Machine::instance().set_disable_motors_command(disable_motors_command);
#else
            Robot_Controller::instance().model_identification();

            if(!Model_Identification::instance().identification_active())
            {
                Model_Identification::instance().acknowledge_identification_stop();
                Main_State_Machine::instance().set_stop_command();
            }
#endif  // CONFIG_MODEL_IDENTIFICATION_DRV

            break;
        }
        case State::SOFT_STOP:
        {
            bool const motors_stopped = Robot_Controller::instance().soft_stop_motors();
            Main_State_Machine::instance().set_motors_stopped(motors_stopped);
            break;
        }
        case State::RESET_AFTER_STOP:
#ifndef CONFIG_MODEL_IDENTIFICATION_DRV
            Robot_Controller::instance().reset();
#endif  // CONFIG_MODEL_IDENTIFICATION_DRV
            break;
        default:
            break;
    }
}

static K_WORK_DELAYABLE_DEFINE(s_control_work, control_loop_work_handler);

void
trigger_control_loop()
{
    k_work_submit(&s_control_work.work);
}

void
stop_control_loop()
{
    set_enable_controller(false);
    led_stop_periodic_blinking();
}

}  // namespace Robot_Control