#include "space_lib/space_lib.h"
#include "diff/diff.h"

#include <glog/logging.h>
#include <ros/ros.h>

namespace diff {

    Diff::Diff()
    {
        __readConf();
    }

    Diff::~Diff()
    {}

    void Diff::__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["diff"]["maxmal_next"], conf.maxmal_next) ?
            false : (c["diff"]["maxmal_next"] = conf.maxmal_next = 0.6);
        space::jsonValueTo(c["diff"]["minmal_next"], conf.minmal_next) ?
            false : (c["diff"]["minmal_next"] = conf.minmal_next = 0.25);
        space::jsonValueTo(c["diff"]["close_ad"], conf.close_ad) ?
            false : (c["diff"]["close_ad"] = conf.close_ad = 1.0);
        space::jsonValueTo(c["diff"]["toler_theta"], conf.toler_theta) ?
            false : (c["diff"]["toler_theta"] = conf.toler_theta = 0.01);
        space::jsonValueTo(c["diff"]["toler_start_theta"], conf.toler_start_theta) ?
            false : (c["diff"]["toler_start_theta"] = conf.toler_start_theta = 0.1);
        space::jsonValueTo(c["diff"]["toler_follow"], conf.toler_follow) ?
            false : (c["diff"]["toler_follow"] = conf.toler_follow = 0.2);
        space::jsonValueTo(c["diff"]["toler_follow_theta"], conf.toler_follow_theta) ?
            false : (c["diff"]["toler_follow_theta"] = conf.toler_follow_theta = 0.51);
        space::jsonValueTo(c["diff"]["angular_slow_p"], conf.angular_slow_p) ?
            false : (c["diff"]["angular_slow_p"] = conf.angular_slow_p = 0.8);
        space::jsonValueTo(c["diff"]["linear_slow_p"], conf.linear_slow_p) ?
            false : (c["diff"]["linear_slow_p"] = conf.linear_slow_p = 0.3);
            
        if (coy != c) {
            space::writeConfig(path, c);
        }

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

        static float angle = 0;

        float mo = 1 * cmd.angular;
        float den = fabs(cmd.x);

        mo = cmd.x >= 0 ? mo : -mo;

        float trig = atan2(mo, den); // 预期舵轮角度

        float e = trig - angle;
        float e_dis = fabs(e) + 1e-3;   // 防止除0
        float t = e_dis / 0.225;

        // 舵轮线速度v,角转速w,舵轮角度误差e,允许最大偏移d = 0.1
        // 为使车体不偏移 (e / w) * v * sin(e) < d
        // 则v < d / ((e / w) * sin(e)), 其中e > M_PI_2 则 e = M_PI_2

        float vel;
        
        if (fabs(trig) > 0.765) {
            vel = fabs(1.0 * cmd.angular / sin(trig));
            vel = cmd.x > 0 ? vel : -vel;
        }
        else {
            vel = cmd.x / cos(trig);
        }

        float relia = vel * exp(-fabs(e));

        if (fabs(e) > 0.017 * 10) {
            relia = 0;
        }

        static ros::Time time = ros::Time::now();

        float vv = e > 0 ? 0.3825 : -0.3825;
        
        angle += (ros::Time::now() - time).toSec() * vv;

        // LOG(INFO)<<"angle: "<<angle<<" relia: "<<relia<<" cmd x: "<<cmd.x;

        time = ros::Time::now();

        return state;
    }

}