#include "cons_planner/cons_planner.h"

namespace cons {

static common::Path __Comp(common::Point& s, common::Point& e)  // 补全路径
{
    common::Path comp;

    float x = e.x - s.x; 
    float y = e.y - s.y;
    
    float step_x = fabs(x) / __rlu;
    float step_y = fabs(y) / __rlu;
    float n = std::max(step_x, step_y);

    float dx = x / n;
    float dy = y / n;

    common::Point p = s;

    comp.push_back(p);
    for (int i = 0;i < n;i++) {
        p.x += dx;
        p.y += dy;
        comp.push_back(p);
    }
    return comp;
}

void consPlanner::setPath(common::Path& path)
{
    reset();
    common::Path compent;

    // 稠密化路径
    for (int i = 0;i < path.size() - 1;i++) {
        if (common::dist(path[i], path[i + 1]) > 2.0 * __rlu) {
            common::Path tmp = __Comp(path[i], path[i + 1]);
            compent.insert(compent.end(), tmp.begin(), tmp.end());
        }
        else {
            compent.push_back(path[i]);
        }
    }
    compent.push_back(path.back());
    compent.back().head = compent.front().head; 

    common::Path dis;

    // 离散路径
    for (int i = 0;i < compent.size();i++) {
        if (!dis.empty() && i != compent.size() - 1) {
            if (common::dist(dis.back(), compent[i]) < __rlu ||
                common::dist(compent[i], compent.back()) < __rlu
            ) {
                continue;
            }
        }
        dis.push_back(compent[i]);
    }
    path_ = std::move(dis);

    LOG(INFO)<<"size: "<<path.size()<<" path_ size: "<<path_.size();
}

void consPlanner::reset()
{
    i_ = 0;
    path_.clear();
    step_ = common::NavStep::STANDBY; // 待命状态
}

void consPlanner::destroy()
{
    return;
}

common::NavState consPlanner::computeVelocity(common::Point& sta_pose, common::Twist& cmd)
{
    if (!__pathCheck()) return common::NavState::DONE;

    float maxmal_next;
    
    Index min_nex = (Index)(config_.minmal_next / __rlu);
    Index back_i = path_.size();
    Index i = i_;

    maxmal_next = std::max(config_.maxmal_next, config_.max_linear);
    maxmal_next = std::max(maxmal_next, __rlu);
    
    common::Path path = __exten(path_, (Index)(maxmal_next / __rlu) + 1);

    conti: NULL;
    {
        i_ = __nib(sta_pose, i, path_, config_.max_linear);    // 找路径上的最近点
        sta_pose.head = path_[i_].head; // * (M_PI / 180.0);

        Index max_nex = (Index)(maxmal_next / __rlu);
        Index set = i_ + max_nex; // 引导点

        bool closr = 
                     (common::dist(path_[i_], path_.back()) < 0.21) ||
                     ((path_.size() - i_) < 10);    // 接近终点

        if (closr) {
            if (__isArrv(sta_pose)) {
                return common::NavState::DONE;
            }
        }
        // 旋转调整处理
        {
            common::Point base = path[i_];  // i_一定小于path size
            common::Point tip = path[i_ + 1];

            base.theta = sta_pose.theta + sta_pose.head;
            base.theta = common::norm(base.theta);
            switch(step_) {
                case common::NavStep::STANDBY:
                    step_ = common::NavStep::TANGENT;
                    return common::NavState::ROTATE;
                break;

                case common::NavStep::TANGENT: // 切线调整
                    /*需要改进*/
                    if (__rotaTangent(base, tip, cmd) == common::NavState::DONE) {
                        step_ = common::NavStep::TRACK;
                    }
                    return common::NavState::ROTATE;
                break;

                // case common::NavStep::ANGLE:
                //     common::Point base = sta_pose;
                //     common::Point tip = path_.back();
                    
                //     base.theta = sta_pose.theta + sta_pose.head;
                //     base.theta = common::norm(base.theta);
                //     if (__rotaAngle(base.theta, tip.theta, cmd) == common::NavState::DONE) {
                //         step_ = common::NavStep::COMPLETE;
                //     }
                //     return common::NavState::ROTATE;
                // break;

                // case common::NavStep::COMPLETE:
                //     reset();
                //     return common::NavState::DONE;
                // break;

                case common::NavStep::TRACK:
                    NULL;
                break;

                default:
                    reset();
                    return common::NavState::DONE;
                break;
            }
        }
        float goto_angle = 1e10;

        closr = (common::lineL(path_, (int)i_, (int)path_.size()) < config_.close_ad);

        // for (int i = (i_ + 1);i < set;i++) {
        for (int i = set;i > (i_ + min_nex - 1);i--) {
            float angle = __pathTangent(path, i) - __c(sta_pose, path[i]);

            angle = fabs(common::norm(angle));
            if (angle < goto_angle) {
                goto_angle = angle;
                set = i;
            }
        }
        if (set > path_.size() - 1 && (i_ + min_nex - 1) < path_.size()) {   // 终点牵引
            set = path_.size() - 1;
        }
        float liner = common::dist(sta_pose, path_.back()) * config_.linear_slow_p;
        float comp = 0; // 角速度补偿量
        {
            common::Point goal = path[set];

            goal.theta = common::norm(__pathTangent(path, set) + M_PI);
            goal.head = 0;

            float boto = common::norm(__c(goal, sta_pose) + M_PI);
            float keep = common::norm(boto - sta_pose.head - sta_pose.theta);

            comp = fabs(keep) < config_.max_angular ? keep :
                (keep < 0 ? -config_.max_angular : config_.max_angular);
            if (closr) {    // 接近终点
                if (fabs(keep) > config_.toler_theta) {
                    comp = fabs(keep) < config_.max_angular ? keep :
                        (keep < 0 ? -config_.max_angular : config_.max_angular);
                }
                float ld = common::PLD(path[i_], path[i_ + 1], sta_pose);

                if (ld > hypot(config_.toler_x, config_.toler_y)) {
                    liner *= exp(-10.0 * ld);
                }
                liner = liner < 0.05 ? 0.05 : liner;
            }
            else {
                if (fabs(keep) > config_.toler_follow_theta || 
                    fabs(keep) > 0.85) {  // 跟随容忍角度误差
                    cmd.x = 0;
                    cmd.angular = fabs(keep) < config_.max_angular ? keep :
                        (keep < 0 ? -config_.max_angular : config_.max_angular);
                    return common::NavState::ROTATE;
                }
            }
        }
        liner = std::min(liner, maxmal_next);
        liner = std::min(liner, common::dist(path[i_], path[set]));
        // liner = std::min(liner, common::dist(path_[i_], path_.back()));
        liner = std::min(liner, common::dist(sta_pose, path[set]));
        liner = std::min(liner, config_.max_linear);
        liner = std::min(liner, path_[i_].max_linear);

        Index locli = set < ((int)path_.size() - 1) ? set : ((int)path_.size() - 1);
        local_path_ = __getPah(sta_pose, path[locli], __rlu); // 计算局部路径
        
        common::NavState sd = __cal(sta_pose, path[set], cmd, liner);

        comp = fabs(comp) < 0.1 ? comp : (comp > 0 ? 0.1 : -0.1);   // 最大补偿10°/s
        cmd.angular += comp * 0.1;
        // cmd.x *= exp(-10.0 * fabs(comp));

        return sd;
    }
    return common::NavState::ING;
}

common::NavState consPlanner::computeVelocityCommands(common::Point& sta_pose, common::Twist& cmd)
{
    return computeVelocity(sta_pose, cmd);
}

/**
 * 在path_上截取a长度，起始点i_
 */
common::Path consPlanner::interPath(float a)
{
    common::Path path;

    for (int i = i_;i < (int)path_.size() - 1;i++) {
        a -= common::dist(path_[i + 1], path_[i]);

        if (a < 0) {
            break;        
        }
        path.push_back(path_[i]);
    }
    return path;
}

common::Config consPlanner::getConfig()
{
    return config_;
}

void consPlanner::setConfig(common::Config& conf)
{
    config_ = conf;

    if (config_.angular_slow_p > 1.0) config_.angular_slow_p = 1.0;
    if (config_.linear_slow_p > 1.0) config_.linear_slow_p = 1.0;
}

common::Path consPlanner::getLocalPath()
{
    return local_path_;
}

common::Path consPlanner::getGlobalPath()
{
    return path_;
}

Index consPlanner::__nib(common::Point& p, Index ci, common::Path& path, float range)
{
    Index ni = ci;
    float ns = 1e10;

    for (Index i = ci;i < path.size();i++) {
        float ds = common::dist(p, path[i]);

        if (ds < ns) {
            ns = ds;
            ni = i;
        } 
        if (i != 0) {
            range -= common::dist(path[i], path[i - 1]);
        }
        if (range < 0.0) {
            break;
        }
    }
    return ni;
}

common::Path consPlanner::__exten(common::Path& path, Index i)
{
    common::Path tp = path;

    if (path.size() < 2) return tp;

    Index i2 = path.size() - 2;
    float x = path.back().x - path[i2].x; 
    float y = path.back().y - path[i2].y;
    
    float step_x = fabs(x) / __rlu;
    float step_y = fabs(y) / __rlu;
    float s = std::max(step_x, step_y);

    float dx = x / s;
    float dy = y / s;
    
    common::Point p = tp.back();

    for (int m = i;m > -1;m--) {
        p.x += dx;
        p.y += dy;

        tp.push_back(p);      
    }
    return tp;
}

bool consPlanner::__isArrv(common::Point p)
{
    if (path_.size() < 2) return true;

    Index i2 = path_.size() - 2;
    float x = path_.back().x - path_[i2].x; 
    float y = path_.back().y - path_[i2].y;
    float a = atan2(y, x);  // 点的切线方向为到达方向

    float cr_x = p.x - path_.back().x;
    float cr_y = p.y - path_.back().y;
    float cr_ex = cr_x * cos(-a) - cr_y * sin(-a);
    float cr_ey = cr_x * sin(-a) + cr_y * cos(-a);

    if (cr_ex > 0 ||
       (fabs(cr_ex) < config_.toler_x && fabs(cr_ey) < config_.toler_y)) {
        return true;
    }
    return false;
}

common::Path consPlanner::__getPah(const common::Point& start, const common::Point& end, const float rlu) 
{
    common::Path path;

    if (rlu < 1e-3) {
        return path;
    }
    float angle = common::norm(start.theta + start.head);   // config_.for_angle);

    const float x = (end.x - start.x) * cos(-angle) - (end.y - start.y) * sin(-angle);
    const float y = (end.x - start.x) * sin(-angle) + (end.y - start.y) * cos(-angle);

    const float kangle = atan2(y, x);
    const float l = hypot(x, y);
    const float r = 0.5 * l / sin(kangle);
    const float step_rad = rlu / r;
    const float step_cl = fabs(r * sin(step_rad * 0.5) * 2.f);

    float dx = 0.f, dy = 0.f, dangle = 0.f;
    int c = 0;
    
    common::Point path_element = start;
    
    while (1) {
        dangle += step_rad;
        
        dx += step_cl * cos(dangle);
        dy += step_cl * sin(dangle);

        path_element.x = start.x +
                dx * cos(-angle) + dy * sin(-angle); // trans footprint to map
        path_element.y = start.y +
                dy * cos(-angle) - dx * sin(-angle);

        path_element.theta = common::norm(angle + dangle);

        path.push_back(path_element);
        
        if (hypot(path_element.x - end.x, path_element.y - end.y) < rlu) {
            path.push_back(end);
            break;
        }
        c++;
        if (c > 1000) break;
    }

    return path;
}


common::NavState consPlanner::__cal(common::Point& start, common::Point& goal, common::Twist& twist, float linear)
{    
    float a = start.theta + start.head;
    float x = (goal.x - start.x) * cos(-a) - (goal.y - start.y) * sin(-a);
    float y = (goal.x - start.x) * sin(-a) + (goal.y - start.y) * cos(-a);

    float l = x * x + y * y;
    float theta = 2.0 * y;
    float at = fabs(theta) / config_.max_angular;
    float lt =  l / std::min(config_.max_linear, linear);
    float t = std::max(at, lt);
    float dir = (fabs(start.head) > M_PI_2) ? -1.0 : 1.0;

    if (t < 1e-5) {
        twist = common::Twist();
        return common::NavState::TOO_CLOSE;
    }

    twist.x = l / t * dir;
    twist.angular = theta / t;// * dir;

    return common::NavState::ING;
}

/**
 * 从start到goal的圆弧角
 */
float consPlanner::__c(common::Point& start, common::Point& goal)
{
    float a = start.theta + start.head;
    float x = (goal.x - start.x) * cos(-a) - (goal.y - start.y) * sin(-a);
    float y = (goal.x - start.x) * sin(-a) + (goal.y - start.y) * cos(-a);
    float l = x * x + y * y;
    // float theta = 2.0 * y;
    float theta = 2.0 * asin(y / sqrt(l));

    if (x < 0) {
        theta = (theta > 0) ? (theta - M_PI) : (theta + M_PI);
    }
    return common::norm(a + theta);
}

common::NavState consPlanner::__rotaTangent(common::Point& base, common::Point& tip, common::Twist& twist)
{
    float x = tip.x - base.x;
    float y = tip.y - base.y;
    float e = common::norm(atan2(y, x) - base.theta);
    float rotate = e * config_.linear_slow_p;

    if (fabs(e) < config_.toler_start_theta) {
        return common::NavState::DONE;
    }
    twist.x = 0;
    twist.angular = fabs(rotate) < config_.max_angular ? rotate : 
        (rotate < 0 ? -config_.max_angular : config_.max_angular);
    return common::NavState::ROTATE;
}

common::NavState consPlanner::__rotaAngle(float base, float tip, common::Twist& twist)
{
    float e = tip - base;
    float rotate = e * config_.linear_slow_p;

    if (fabs(e) < config_.toler_theta) {
        return common::NavState::DONE;
    }
    twist.x = 0;
    twist.angular = fabs(rotate) < config_.max_angular ? rotate : 
        (rotate < 0 ? -config_.max_angular : config_.max_angular);
    return common::NavState::ROTATE;
}

float consPlanner::__maxmalDist(common::Path& pt1, common::Path& pt2)
{
    float di = 0;

    for (common::Point& p1 : pt1) {
        float minmal = 1e10;

        for (common::Point& p2 : pt2) {
            minmal = std::min(minmal, common::dist(p2, p1));
        }
        di = std::max(minmal, di);
    }

    return di;
}

float consPlanner::__pathTangent(common::Path& path, Index i)
{
    float x = path[i + 1].x - path[i].x;
    float y = path[i + 1].y - path[i].y;

    return atan2(y, x);
}

bool consPlanner::__pathCheck()
{
    if (path_.size() < 2 ||
        i_ >= path_.size() ||
        false
    ) {
        return false;
    }

    return true;
}

}