#include "trajectory_tool.h"
#include <iostream>
// 构造函数
Trajectory_tool::Trajectory_tool()
{
}

// 析构函数
Trajectory_tool::~Trajectory_tool()
{
}

// 处理90度附近斜率无限大的情况,角度(sin,cos)近似为89.5度(0.99995, 0.01)或90.5度(0.99995,-0.01)
void Trajectory_tool::max_gradient_limit(Eigen::Matrix2d &rotation)
{
    if (rotation(0, 0) > 0 && rotation(0, 0) < 0.01)
    {
        rotation << 0.01, -0.9995, 0.9995, 0.01;
    }
    else if (rotation(0, 0) < 0 && rotation(0, 0) > -0.01)
    {
        rotation << -0.01, -0.9995, 0.9995, -0.01;
    }
}

// 生成路径
bool Trajectory_tool::create_trajectory(Eigen::Isometry2d base, Eigen::Isometry2d target, trajectory_type &trajectory, int extend_num)
{
    // 1.将两点变换到x轴，base移到原点，先平移后旋转
    Eigen::Vector2d t_base_point(0, 0);
    Eigen::Vector2d t_target_point = target.translation() - base.translation();
    double t_euclidean_distance = sqrt(t_target_point.transpose() * t_target_point);
    // 基点与目标点相同，无法生成路径，直接退出
    if (t_euclidean_distance <= 0)
    {
        return false;
    }
    double t_rotation_cos = t_target_point.x() / t_euclidean_distance;
    double t_rotation_sin = t_target_point.y() / t_euclidean_distance;
    Eigen::Isometry2d t_origin_transformation; // 用于保存对齐x轴产生的变换
    t_origin_transformation.setIdentity();
    Eigen::Matrix2d t_rotation_matrix;
    t_rotation_matrix.setIdentity();
    t_rotation_matrix << t_rotation_cos, -t_rotation_sin, t_rotation_sin, t_rotation_cos;
    t_origin_transformation.rotate(t_rotation_matrix);
    t_origin_transformation.pretranslate(base.translation());
    t_target_point = t_rotation_matrix.transpose() * t_target_point; // 目标点反向旋转到x轴上
    // std::cout << "target: \n"<< t_target_point<<std::endl;

    // 2.处理90度附近斜率无限大的情况
    // 首先调整自身朝向到新坐标系
    Eigen::Matrix2d t_base_rotation(t_rotation_matrix.transpose() * base.rotation());
    Eigen::Matrix2d t_target_rotation(t_rotation_matrix.transpose() * target.rotation());
    max_gradient_limit(t_base_rotation);
    max_gradient_limit(t_target_rotation);
    // std::cout << "br: "<< t_base_rotation<<std::endl;
    // std::cout << "tr: "<< t_target_rotation<<std::endl;

    // 3.三阶多项式拟合，生成参数矩阵并计算参数
    Eigen::Matrix3d t_input_data;
    Eigen::Vector3d t_output_data;
    Eigen::Vector3d t_parameters;
    // t_input_data.col(0) << pow(t_base_point.x(), 3.0), pow(t_base_point.x(), 2.0), t_base_point.x();
    t_input_data.col(0) << pow(t_target_point.x(), 3.0), pow(t_target_point.x(), 2.0), t_target_point.x();
    t_input_data.col(1) << 3 * pow(t_base_point.x(), 2.0), 2 * t_base_point.x(), 1;
    t_input_data.col(2) << 3 * pow(t_target_point.x(), 2.0), 2 * t_target_point.x(), 1;
    t_output_data << t_target_point.y(), -(t_base_rotation(0, 1) / t_base_rotation(0, 0)), -(t_target_rotation(0, 1) / t_target_rotation(0, 0));
    t_parameters = (t_output_data.transpose() * t_input_data.inverse()).transpose();
    // std::cout << "input: \n"<< t_input_data<<std::endl;
    // std::cout << "output: \n"<< t_output_data<<std::endl;
    // std::cout << "params: \n"<< t_parameters<<std::endl;

    // 4.根据参数进行多项式插值(cos = dx/d, cos2 = 1/(1+tan2))，并转换回原坐标系
    double t_delta = 0.05;
    double t_current_x = t_base_point.x();
    double t_extend_front = 0;
    double t_extend_end = 0;
    std::lock_guard<std::mutex> lck(m_mutex_trajectory);
    m_trajectory.clear();
    trajectory.clear();
    while (t_current_x < t_target_point.x() || t_extend_end < extend_num)
    {
        double t_gradient = t_parameters.transpose() * Eigen::Vector3d(3 * pow(t_current_x, 2.0), 2 * t_current_x, 1.0);
        double t_delta_x = sqrt(pow(t_delta, 2.0) / (1 + pow(t_gradient, 2.0)));
        if (t_extend_front < extend_num)
        {
            t_current_x -= t_delta_x;
            t_extend_front++;
        }
        else
        {
            t_current_x += t_delta_x;
            if (t_current_x >= t_target_point.x())
                t_extend_end++;
            double t_current_y = t_parameters.transpose() * Eigen::Vector3d(pow(t_current_x, 3.0), pow(t_current_x, 2.0), t_current_x);
            Eigen::Isometry2d t_current_pose;
            Eigen::Matrix2d t_current_rotation;
            t_current_pose.setIdentity();
            t_current_rotation << t_delta_x / t_delta, -t_gradient * t_delta_x / t_delta, t_gradient * t_delta_x / t_delta, t_delta_x / t_delta;
            // 将自身朝向调整回来
            t_current_pose.rotate(t_origin_transformation.rotation() * t_current_rotation);
            // 平移时将坐标系变换回去
            t_current_pose.pretranslate(t_origin_transformation.rotation() * Eigen::Vector2d(t_current_x, t_current_y) + t_origin_transformation.translation());
            m_trajectory.push_back(t_current_pose);
            trajectory.push_back(t_current_pose);
        }
    }
    if(trajectory.size() * t_delta <= t_euclidean_distance * 5)
        return true;
    else
        return false;
}

// 获取已生成的路径
bool Trajectory_tool::get_trajectory(trajectory_type &trajectory)
{
    std::lock_guard<std::mutex> lck(m_mutex_trajectory);
    if (m_trajectory.size() <= 0)
        return false;
    trajectory.clear();
    for (size_t i = 0; i < m_trajectory.size(); i++)
    {
        Eigen::Isometry2d t_pose;
        t_pose.pretranslate(m_trajectory[i].translation());
        t_pose.rotate(m_trajectory[i].rotation());
        trajectory.push_back(t_pose);
    }
    return true;
}

// 获取下标对应的路径上的点
Eigen::Isometry2d Trajectory_tool::operator[](int trajectory_index)
{
    std::lock_guard<std::mutex> lck(m_mutex_trajectory);
    if (m_trajectory.size() <= trajectory_index)
        return Eigen::Isometry2d::Identity();
    else
        return m_trajectory[trajectory_index];
}