#include <mission_adapters/mission_config.h>

#include <cmath>

#include <robot/robot.h>

namespace mission_adapters
{
    bool MissionConfig::loadFromParams(robot::NodeHandle& nh, const std::string& ns)
    {
        const std::string prefix = ns.empty() ? std::string() : ns + "/";

        nh.getParam(prefix + "mission_timeout", mission_timeout, mission_timeout);
        nh.getParam(prefix + "clear_queue_on_failure", clear_queue_on_failure,
                    clear_queue_on_failure);

        if (!validate())
            return false;

        print();
        return true;
    }

    bool MissionConfig::validate() const
    {
        if (!std::isfinite(mission_timeout) || mission_timeout < 0.0)
        {
            robot::log_error("MissionConfig: mission_timeout = %.3f is invalid (needs >= 0 [s], 0 "
                             "= off)", mission_timeout);
            return false;
        }

        return true;
    }

    void MissionConfig::print() const
    {
        robot::log_info("MissionConfig: mission_timeout = %.3f [s] (%s), clear_queue_on_failure = %s",
                        mission_timeout,
                        mission_timeout > 0.0 ? "on" : "off",
                        clear_queue_on_failure ? "true" : "false");
    }

} // namespace mission_adapters
