#pragma once

#include <rclcpp/rclcpp.hpp>

#include "action_executor.hpp"
#include "navigation_helper.hpp"
#include "missions/mission_base.hpp"

#include <std_msgs/msg/bool.hpp>
#include <std_msgs/msg/int64.hpp>
#include <std_msgs/msg/int8.hpp>

#include <modelec_interfaces/msg/strat_state.hpp>
#include <modelec_interfaces/msg/spawn.hpp>

#include <modelec_interfaces/srv/odometry_start.hpp>
#include <std_msgs/msg/empty.hpp>


namespace Modelec
{
    enum class State
    {
        INIT = modelec_interfaces::msg::StratState::INIT,
        WAIT_START = modelec_interfaces::msg::StratState::WAIT_START,
        SELECT_MISSION = modelec_interfaces::msg::StratState::SELECT_MISSION,
        SELECT_GAME_ACTION = modelec_interfaces::msg::StratState::SELECT_GAME_ACTION,

        TAKE_MISSION = modelec_interfaces::msg::StratState::TAKE_MISSION,
        FREE_MISSION = modelec_interfaces::msg::StratState::FREE_MISSION,
        THERMO_MISSION = modelec_interfaces::msg::StratState::THERMO_MISSION,

        DO_GO_HOME = modelec_interfaces::msg::StratState::DO_GO_HOME,
        STOP = modelec_interfaces::msg::StratState::STOP,
    };

    class StratFMS : public rclcpp::Node
    {
    public:
        StratFMS();

        void Init();

        void ReInit();

        void Reset();

        void ResetStrat();

    protected:
        void Transition(State next, const std::string& reason);

        void Update();

    private:
        State state_;
        rclcpp::TimerBase::SharedPtr timer_;
        rclcpp::Time match_start_time_;
        bool started_ = false;
        bool setup_ = false;
        bool team_selected_ = false;
        std::unique_ptr<Mission> current_mission_;
        int team_id_ = 0;

        std::queue<State> game_action_sequence_;
        bool static_strat_ = false;

        float factor_obs_;
        float factor_thermo_;
        int timer_period_ms_ = 100;

        std::shared_ptr<NavigationHelper> nav_;
        std::shared_ptr<ActionExecutor> action_executor_;

        rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr tir_sub_;
        rclcpp::Publisher<modelec_interfaces::msg::StratState>::SharedPtr state_pub_;
        rclcpp::Publisher<std_msgs::msg::Int64>::SharedPtr start_time_pub_;
        rclcpp::Subscription<std_msgs::msg::Int8>::SharedPtr team_id_sub_;
        rclcpp::Subscription<modelec_interfaces::msg::Spawn>::SharedPtr spawn_id_sub_;
        rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr reset_strat_sub_;
        rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr tir_arm_sub_;
        rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr tir_arm_set_pub_;

        rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr start_odo_pub_;
    };
}
