#include "space_lib/space_lib.h"
#include "mecanum/mecanum.h"

#include <ros/ros.h>

namespace mecanum {

    Mecanum::Mecanum()
    {
        __readConf();
    }

    Mecanum::~Mecanum()
    {}

    void Mecanum::__readConf()
    {
        common::Config conf = getConfig();

        std::string path = space::home() + "configure/nav_config.json";
        Json::Value c = space::readConfig(path);
        Json::Value coy = c;

        space::jsonValueTo(c["mecanum"]["maxmal_next"], conf.maxmal_next) ?
            false : (c["mecanum"]["maxmal_next"] = conf.maxmal_next = 0.6);
        space::jsonValueTo(c["mecanum"]["minmal_next"], conf.minmal_next) ?
            false : (c["mecanum"]["minmal_next"] = conf.minmal_next = 0.25);
        space::jsonValueTo(c["mecanum"]["close_ad"], conf.close_ad) ?
            false : (c["mecanum"]["close_ad"] = conf.close_ad = 1.0);
        space::jsonValueTo(c["mecanum"]["toler_theta"], conf.toler_theta) ?
            false : (c["mecanum"]["toler_theta"] = conf.toler_theta = 0.01);
        space::jsonValueTo(c["mecanum"]["toler_start_theta"], conf.toler_start_theta) ?
            false : (c["mecanum"]["toler_start_theta"] = conf.toler_start_theta = 0.1);
        space::jsonValueTo(c["mecanum"]["toler_follow"], conf.toler_follow) ?
            false : (c["mecanum"]["toler_follow"] = conf.toler_follow = 0.2);
        space::jsonValueTo(c["mecanum"]["toler_follow_theta"], conf.toler_follow_theta) ?
            false : (c["mecanum"]["toler_follow_theta"] = conf.toler_follow_theta = 0.51);
        space::jsonValueTo(c["mecanum"]["angular_slow_p"], conf.angular_slow_p) ?
            false : (c["mecanum"]["angular_slow_p"] = conf.angular_slow_p = 0.8);
        space::jsonValueTo(c["mecanum"]["linear_slow_p"], conf.linear_slow_p) ?
            false : (c["mecanum"]["linear_slow_p"] = conf.linear_slow_p = 0.3);
            
        if (coy != c) {
            space::writeConfig(path, c);
        }

        setConfig(conf);
    }
    
    common::NavState Mecanum::computeVelocityCommands(common::Point& sta_pose, common::Twist& cmd)
    {
        common::NavState state = computeVelocity(sta_pose, cmd);

        return state;
    }

}