#pragma once

#include <optional>
#include "car_nav_lite/types.hpp"
#include "car_nav_lite/grid_map.hpp"
#include "car_nav_lite/localizer.hpp"
#include "car_nav_lite/global_planner.hpp"
#include "car_nav_lite/local_planner.hpp"

namespace car_nav_lite {

class BTExecutor {
public:
    BTExecutor();

    void setGoal(const Pose2D& goal);
    void setTestPath(const std::vector<Pose2D>& path);
    void setParams(double la, double ms, double mr, double sl, double cf);
    void setFailCallback(std::function<void()> cb) { fail_callback_ = std::move(cb); }
    bool hasGoal() const { return goal_.has_value(); }

    Twist tick(const Pose2D& pose, const GridMap& map);

    NavState state() const { return state_; }
    const std::vector<Pose2D>& currentPath() const { return current_path_; }

private:
    NavState state_ = NavState::IDLE;
    std::optional<Pose2D> goal_;
    std::vector<Pose2D> current_path_;
    GlobalPlanner planner_;
    LocalPlanner  local_planner_;
    std::function<void()> fail_callback_;

    bool atGoal(const Pose2D& pose) const;
};

}  // namespace car_nav_lite