//
// Created by gf on 23-7-21.
//

#include "rotate_mpc.h"
#include <chrono>
#include <cppad/cppad.hpp>
#include <cppad/ipopt/solve.hpp>

size_t mpc_rotate_steps = 20;                  //优化考虑后面多少步
size_t mpc_rotate_delay = 0;  // 发送到执行的延迟，即几个周期的时间
size_t mpc_rotate_down_count = 3;//下发前多少步
#define N mpc_rotate_steps
#define delay mpc_rotate_delay
#define down_count mpc_rotate_down_count

class FG_eval_rotate {
public:
    Eigen::VectorXd m_statu;                  //当前状态
    Eigen::VectorXd m_condition;              //搜索条件参数
    FG_eval_rotate(Eigen::VectorXd status, Eigen::VectorXd condition) {
        m_statu = status;
        m_condition = condition;
    }

    typedef CPPAD_TESTVECTOR(CppAD::AD<double>) ADvector;

    void operator()(ADvector &fg, const ADvector &vars) {
        fg[0] = 0;

        double dt = m_condition[0];
        double target_theta = m_condition[1];
        double obs_w = m_condition[2];
        double obs_h = m_condition[3];

        double angular_velocity = m_statu[0];

        // Weights for how "important" each cost is - can be tuned
        const double y_cost_weight = 1000;
        const double angular_cost_weight = 4000;
        const double v_cost_weight = 5000;
        const double angular_velocity_cost_weight = 1000;
        const double acceleration_cost_weight = 1;
        const double angular_acceleration_cost_weight = 10;
        const double obs_distance_weight = 5000.0;

        // 代价函数计算
        for (int t = 0; t < N - 1; ++t) {
            /// 角速度，角加速度weight loss
            fg[0] += angular_velocity_cost_weight * CppAD::pow(vars[N + t], 2);
            fg[0] += angular_acceleration_cost_weight * CppAD::pow(vars[N + t + 1] - vars[N + t], 2);
            /// 目标角度 loss
            fg[0] += angular_cost_weight * CppAD::pow(target_theta - vars[t], 2);
        }

        // 约束条件
        int n_angular = 1;
        int n_angular_acceleration = n_angular + N;
        int n_obs = n_angular_acceleration + N;

        /// 角度等式约束 1~N
        fg[n_angular] = vars[0] - angular_velocity * dt;
        for (int t = 1; t < N; ++t) {
            // State at time t + 1
            CppAD::AD<double> theta1 = vars[t];

            // State at time t
            CppAD::AD<double> theta0 = vars[t - 1];
            CppAD::AD<double> angular_velocity0 = vars[N + t - 1];

            // Setting up the rest of the model constraints
            fg[n_angular + t] = theta1 - (theta0 + angular_velocity0 * dt);
        }

        /// 角加速度等式约束 N+1~2N
        fg[n_angular_acceleration] = (vars[0 + N] - angular_velocity) / dt;
        for (int t = 1; t < N; ++t) {
            fg[n_angular_acceleration + t] = (vars[t + N] - vars[t + N - 1]) / dt;
        }

        /// 障碍物距离等式约束 2N+1~3N
        if (m_statu.size() == 1 + 3) {
            float obs_x = m_statu[1];
            float obs_y = m_statu[2];
            float obs_theta = m_statu[3];
            Pose2d obs_relative_pose(obs_x, obs_y, obs_theta);
            std::vector<Pose2d> obs_vertexes = Pose2d::generate_rectangle_vertexs(obs_relative_pose, obs_w * 2,
                                                                                  obs_h * 2);
            for (auto t = 0; t < N; ++t) {
                CppAD::AD<double> min_distance = 1e19;
                for (auto i = 0; i < 8; ++i) {
                    CppAD::AD<double> ra = obs_vertexes[i].x() * CppAD::cos(vars[t]) -
                                           obs_vertexes[i].y() * CppAD::sin(vars[t]);
                    CppAD::AD<double> rb = obs_vertexes[i].x() * CppAD::sin(vars[t]) +
                                           obs_vertexes[i].y() * CppAD::cos(vars[t]);
                    CppAD::AD<double> distance = CppAD::pow(ra / obs_w, 8) + CppAD::pow(rb / obs_h, 8);

                    if (distance < min_distance) min_distance = distance;
                }
                fg[n_obs + t] = min_distance;
            }
//            std::cout << " fg[n_obs]: " << fg[n_obs] << " | __LINE__:" << __LINE__ << std::endl;
        }
    }
};

RotateMPC::RotateMPC(const Pose2d &obs, double obs_w, double obs_h, double min_velocity, double max_velocity) {
    min_velocity_ = min_velocity;
    max_velocity_ = max_velocity;
    obs_relative_pose_ = obs;
    obs_w_ = obs_w;
    obs_h_ = obs_h;
}

RotateMPC::~RotateMPC() {}

MpcError
RotateMPC::solve(Pose2d current_to_target, Eigen::VectorXd statu, MPC_parameter mpc_param, std::vector<double> &out) {
    auto start = std::chrono::steady_clock::now();
    // State vector holds all current values needed for vars below
    Pose2d pose_agv = Pose2d(statu[0], statu[1], statu[2]);
    double line_velocity = statu[3];
    double angular_velocity = statu[4];

    /// 定义最大最小角度,角速度,角加速度(不区分方向)
    double max_angular = M_PI;
//    double max_angular_velocity = max_velocity_ / (1 + exp(-10 * fabs(current_to_target.theta()) - 0.1));
    double max_angular_velocity = max_velocity_ / (1 + exp(-4 * (fabs(current_to_target.theta()) - 0.57*max_velocity_)));
    double min_angular_velocity = min_velocity_ ;
    double max_angular_acceleration_velocity = mpc_param.acc_angular/ (1 + exp(-7 * (fabs(current_to_target.theta()) + 0.1*mpc_param.acc_angular)));
    //double max_angular_acceleration_velocity=0.44;///(1+exp(-36.*(angular_velocity-0.15)))+0.085;
    /// 调整输入
    if (angular_velocity > max_angular_velocity) angular_velocity = max_angular_velocity;
    if (angular_velocity < -max_angular_velocity) angular_velocity = -max_angular_velocity;

    // 优化
    typedef CPPAD_TESTVECTOR(double) Dvector;

    /// 步长
    double dt = mpc_param.dt;

    /// 初始化参数
    size_t n_vars = N * 2; // 0-N为角度，N-2N为角速度
    Dvector vars(n_vars); // 0-N为角度，N-2N为角速度

    for (int i = 0; i < n_vars; i++) vars[i] = 0.0;

    // 参数上下限
    Dvector vars_lowerbound(n_vars);
    Dvector vars_upperbound(n_vars);
    for (int i = 0; i < n_vars; ++i) {
        vars_lowerbound[i] = -1.0e19;
        vars_upperbound[i] = 1.0e19;
    }

    /// limit of angular velocity
    for (auto i = N; i < 2 * N; ++i) {
        vars_lowerbound[i] = -max_angular_velocity;
        vars_upperbound[i] = max_angular_velocity;
    }

    // Lower and upper limits for the constraints
    /// 障碍物是否进入 碰撞检测范围内
//    float distance = Pose2d::distance(obs_relative_pose_, Pose2d(0, 0, 0));
    float distance = 1000;//屏蔽障碍物
    bool find_obs = distance < 5;
    if (find_obs) printf(" find obs ..............................\n");
    size_t n_constraints = 2 * N + find_obs * N;// 0~N角度约束，N~2N角加速度约束, 2N~3N障碍物约束
    Dvector constraints_lowerbound(n_constraints);
    Dvector constraints_upperbound(n_constraints);
    for (auto i = 0; i < n_constraints; ++i) {
        constraints_lowerbound[i] = 0;
        constraints_upperbound[i] = 0;
    }
    /// limit of angular acceleration
    for (auto i = N; i < 2 * N; ++i) {
        //延迟处理,前delay个周期内认定为匀速运动(加速度为0)
        if (i < N + delay) {
            constraints_lowerbound[i] = 0;
            constraints_upperbound[i] = 0;
        } else{
            constraints_lowerbound[i] = -max_angular_acceleration_velocity;
            constraints_upperbound[i] = max_angular_acceleration_velocity;
        }
    }
    /// limit of obs distance
    if (find_obs)
        for (auto i = 2 * N; i < 3 * N; ++i) {
            constraints_lowerbound[i] = 1.0;
            constraints_upperbound[i] = 1e19;
        }

    // fg_eval for IPOPT solver
    size_t n_fg_status = 1 + 3 * find_obs;
    Eigen::VectorXd fg_status(n_fg_status);
    fg_status[0] = angular_velocity;
    if (find_obs) {
        fg_status[1] = obs_relative_pose_.x();
        fg_status[2] = obs_relative_pose_.y();
        fg_status[3] = obs_relative_pose_.theta();
    }
//    std::cout << " size of fg_status: " << fg_status.size() << " | __LINE__: " << __LINE__ << std::endl;


    Pose2d targetAngularInAGV = current_to_target;

    Eigen::VectorXd condition(4);
    condition << dt, targetAngularInAGV.theta(), obs_w_, obs_h_;

    FG_eval_rotate fg_eval(fg_status, condition);


    // options for IPOPT solver
    std::string options;
    /// Uncomment this if you'd like more print information
    options += "Integer print_level  0\n";
    options += "Sparse  true        forward\n";
    options += "Sparse  true        reverse\n";
    options += "Numeric max_cpu_time          0.5\n";

    // place to return solution
    CppAD::ipopt::solve_result<Dvector> solution;

    // solve the problem
    CppAD::ipopt::solve<Dvector, FG_eval_rotate>(
            options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound,
            constraints_upperbound, fg_eval, solution);

    auto now = std::chrono::steady_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::microseconds>(now - start);
    double time =
            double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;

    // invalid_number_detected
    if (solution.status != CppAD::ipopt::solve_result<Dvector>::success) {
        printf(" mpc failed status : %d  input: %.5f max_wmg:%.5f\n", solution.status,
               angular_velocity, max_angular_velocity);
        if (solution.status == CppAD::ipopt::solve_result<Dvector>::local_infeasibility)
            return no_solution;
        return failed;
    }

    // Cost
    auto cost = solution.obj_value;

    // 输出第一步的角速度
    out.clear();
    double solve_angular_velocity;
    for (int i = 0; i < down_count; ++i) {
        solve_angular_velocity = solution.x[i + N + delay];
        out.push_back(solve_angular_velocity);
//        printf("P[%d]: %f\n", i, solve_angular_velocity);
    }

    return success;
}