#include <mission_adapters/mission_adapters.h>
#include <move_base_core/navigation.h>

#include <utility>

using namespace mission_adapters;

class RobotControlTest
{
    robot::move_base_core::BaseNavigation::Ptr move_base_ptr_;

    mission_adapters::MissionManager  mission_manager_;
    mission_adapters::EventProcessor  event_processor_{mission_manager_};
    mission_adapters::MissionExecutor mission_executor_{mission_manager_};

    // FIX #7: Fully qualified namespace for the initial value.
    robot::move_base_core::State prev_nav_state_ = robot::move_base_core::State::PENDING;

    // Tracks whether the actions of the current mission are complete.
    // FIX #8: Stub — replace with real action-done check from your action executor.
    bool areActionsDone() const
    {
        // TODO: query your action executor for completion status.
        return true;
    }

public:
    explicit RobotControlTest(robot::move_base_core::BaseNavigation::Ptr move_base);
    ~RobotControlTest();

    void run();

private:
    void executeMission(const Mission& mission);
};

RobotControlTest::RobotControlTest(robot::move_base_core::BaseNavigation::Ptr move_base)
    : move_base_ptr_(std::move(move_base))
{}

RobotControlTest::~RobotControlTest()
{
    event_processor_.stop();
    mission_executor_.stop();
}

void RobotControlTest::run()
{
    if (!move_base_ptr_)
    {
        robot::log_error("RobotControlTest requires a valid BaseNavigation pointer");
        return;
    }

    robot::Rate rate(50);

    mission_executor_.setMissionCallback(
        [this](const std::shared_ptr<Mission>& mission)
        {
            executeMission(*mission);
        });

    event_processor_.start();
    mission_executor_.start();

    while (robot::ok())
    {
        auto feedback = move_base_ptr_->getFeedback();
        if (!feedback)
        {
            rate.sleep();
            continue;
        }

        auto nav_state = feedback->navigation_state;

        if (nav_state != prev_nav_state_ && nav_state == robot::move_base_core::State::SUCCEEDED && areActionsDone())
        {
            event_processor_.navDoneEvent();
        }
        else if (nav_state != prev_nav_state_ && nav_state == robot::move_base_core::State::ABORTED)
        {
            event_processor_.navFailedEvent();
        }
        prev_nav_state_ = nav_state;

        // Example: receive an order (replace condition with your real source)
        if (/* new order available */ false)
        {
            robot_protocol_msgs::Order order;
            // ... populate order ...
            event_processor_.orderEvent(order);
        }

        rate.sleep();
    }
}

void RobotControlTest::executeMission(const Mission& mission)
{
    // TODO: send mission goal to move_base_ptr_
    (void)mission;
}