#include "space_lib/space_lib.h"
#include "forklift/forklift.h"

namespace forklift
{
    FL::FL()
    {
        if (__readConf()) {
            sub_ = nh_.subscribe<std_msgs::String>("rudder_angle", 1, &FL::__rudderAngleCallback, this);
        }
    }

    FL::~FL()
    {}

    bool FL::__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["forklift"]["maxmal_next"], conf.maxmal_next) ?
            false : (c["forklift"]["maxmal_next"] = conf.maxmal_next = 0.25);
        space::jsonValueTo(c["forklift"]["minmal_next"], conf.minmal_next) ?
            false : (c["forklift"]["minmal_next"] = conf.minmal_next = 0.1);
        space::jsonValueTo(c["forklift"]["toler_start_theta"], conf.toler_start_theta) ?
            false : (c["forklift"]["toler_start_theta"] = conf.toler_start_theta = 0.1);
        space::jsonValueTo(c["forklift"]["toler_follow_theta"], conf.toler_follow_theta) ?
            false : (c["forklift"]["toler_follow_theta"] = conf.toler_follow_theta = 0.53);
        space::jsonValueTo(c["forklift"]["toler_theta"], conf.toler_theta) ?
            false : (c["forklift"]["toler_theta"] = conf.toler_theta = 0.01);
        space::jsonValueTo(c["forklift"]["angular_slow_p"], conf.angular_slow_p) ?
            false : (c["forklift"]["angular_slow_p"] = conf.angular_slow_p = 0.8);
        space::jsonValueTo(c["forklift"]["linear_slow_p"], conf.linear_slow_p) ?
            false : (c["forklift"]["linear_slow_p"] = conf.linear_slow_p = 0.3);

        bool ok = space::jsonValueTo(c["forklift"]["rudder_offset"], rudder_ant_.offset) &&
                  space::jsonValueTo(c["forklift"]["rudder_rot_vel"], rudder_ant_.vel) &&
                  space::jsonValueTo(c["forklift"]["max_rudder_angle"], rudder_ant_.max_rudder_angle) &&
                  space::jsonValueTo(c["forklift"]["min_rudder_angle"], rudder_ant_.min_rudder_angle);
        
        space::jsonValueTo(c["forklift"]["p"], rudder_ant_.p) ?
            false : (c["forklift"]["p"] = rudder_ant_.p = 4.0);

        c["forklift"]["rudder_offset"] = rudder_ant_.offset;
        c["forklift"]["rudder_rot_vel"] = rudder_ant_.vel;
        c["forklift"]["max_rudder_angle"] = rudder_ant_.max_rudder_angle;
        c["forklift"]["min_rudder_angle"] = rudder_ant_.min_rudder_angle;
        c["forklift"]["toler_rudder_angle"] = rudder_ant_.toler_angle;
        rudder_ant_.angle = 0;
        rudder_ant_.t = ros::Time(0);

        if (coy != c) {
            space::writeConfig(path, c);
        }
        setConfig(conf);

        return ok;
    }

    common::NavState FL::computeVelocityCommands(common::Point& sta_pose, common::Twist& cmd)
    {
        RudderAnt ant = __getRudderAnt();
        common::Config conf = getConfig();

        if ((ros::Time::now() - ant.t).toSec() > 0.5) {
            return common::NavState::DATA_MISS;
        }

        common::NavState state = computeVelocity(sta_pose, cmd);

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

        mo = cmd.x >= 0 ? mo : -mo;
        
        float trig = atan2(mo, den); // 预期舵轮角度

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

        // 舵轮线速度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 = rudder_ant_.offset * cmd.angular / sin(trig);
        }
        else {
            vel = cmd.x / cos(trig);
        }

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

        // if (fabs(e) > rudder_ant_.toler_angle) {
        //     relia = 0;
        // }
        cmd.x = relia;// > fabs(cmd.x) ? cmd.x : (cmd.x < 0 ? -vel : vel);
        cmd.angular = trig;
        // cmd.angle = trig;

        cmd.x = fabs(cmd.x) < 0.5 ? cmd.x : (cmd.x > 0 ? 0.5 : -0.5);

        if (trig > rudder_ant_.max_rudder_angle) {
            cmd.angular = rudder_ant_.max_rudder_angle;
            // return common::NavState::HEAD_WARN;
        }
        if (trig < rudder_ant_.min_rudder_angle) {
            cmd.angular = rudder_ant_.min_rudder_angle;
            // return common::NavState::HEAD_WARN;
        }
        return state;
    }

    void FL::destroy()
    {
        sub_.shutdown();
    }

    void FL::__rudderAngleCallback(const std_msgs::StringConstPtr &msg)
    {
        RUN_LOCK()
        Json::Value value;

        space::stringTo(msg->data, value);

        rudder_ant_.angle = value["data"].asFloat();
        rudder_ant_.t = ros::Time::now();
    }

    RudderAnt FL::__getRudderAnt()
    {
        RUN_LOCK()
        return rudder_ant_;
    }

}