#ifndef MPCTEST_H
#define MPCTEST_H

#include "math.h"

#include "mpc_lateral_controller/mpc.hpp"
#include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp"
#include "mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp"
#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp"
#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp"
#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp"

using namespace autoware::motion::control::mpc_lateral_controller;

class MPCTest
{
public:
    MPCTest();

protected:
    MPCParam param;
    // Test inputs
    MPCTrajectory dummy_straight_trajectory;
    MPCTrajectory dummy_right_turn_trajectory;
    iv::ADCSteeringReport neutral_steer;
    iv::ADCPose pose_zero;
    double default_velocity = 1.0;
 //   rclcpp::Logger logger = rclcpp::get_logger("mpc_test_logger");

    // Vehicle model parameters
    double wheelbase = 2.7;
    double steer_limit = 1.0;
    double steer_tau = 0.1;
    double mass_fl = 600.0;
    double mass_fr = 600.0;
    double mass_rl = 600.0;
    double mass_rr = 600.0;
    double cf = 155494.663;
    double cr = 155494.663;

    // Filters parameter
    double steering_lpf_cutoff_hz = 3.0;
    double error_deriv_lpf_cutoff_hz = 5.0;

    // Test Parameters
    double admissible_position_error = 5.0;
    double admissible_yaw_error_rad = M_PI_2;
    double steer_lim = 0.610865;      // 35 degrees
    double steer_rate_lim = 2.61799;  // 150 degrees
    double ctrl_period = 0.03;

    bool use_steer_prediction = true;

    TrajectoryFilteringParam trajectory_param;
    MPCTrajectoryPoint makePoint(const double x, const double y, const float vx)
    {
        MPCTrajectoryPoint p;
        p.x = x;
        p.y = y;
        p.vx = vx;
        return p;
    }

    void SetUp()
    {
        param.prediction_horizon = 50;
        param.prediction_dt = 0.1;
        param.zero_ff_steer_deg = 0.5;
        param.input_delay = 0.0;
        param.acceleration_limit = 2.0;
        param.velocity_time_constant = 0.3;
        param.min_prediction_length = 5.0;
        param.steer_tau = 0.1;
        param.nominal_weight.lat_error = 1.0;
        param.nominal_weight.heading_error = 1.0;
        param.nominal_weight.heading_error_squared_vel = 1.0;
        param.nominal_weight.terminal_lat_error = 1.0;
        param.nominal_weight.terminal_heading_error = 0.1;
        param.low_curvature_weight.lat_error = 0.1;
        param.low_curvature_weight.heading_error = 0.0;
        param.low_curvature_weight.heading_error_squared_vel = 0.3;
        param.nominal_weight.steering_input = 1.0;
        param.nominal_weight.steering_input_squared_vel = 0.25;
        param.nominal_weight.lat_jerk = 0.0;
        param.nominal_weight.steer_rate = 0.0;
        param.nominal_weight.steer_acc = 0.000001;
        param.low_curvature_weight.steering_input = 1.0;
        param.low_curvature_weight.steering_input_squared_vel = 0.25;
        param.low_curvature_weight.lat_jerk = 0.0;
        param.low_curvature_weight.steer_rate = 0.0;
        param.low_curvature_weight.steer_acc = 0.000001;
        param.low_curvature_thresh_curvature = 0.0;

        trajectory_param.traj_resample_dist = 0.1;
        trajectory_param.path_filter_moving_ave_num = 35;
        trajectory_param.curvature_smoothing_num_traj = 1;
        trajectory_param.curvature_smoothing_num_ref_steer = 35;
        trajectory_param.enable_path_smoothing = true;
        trajectory_param.extend_trajectory_for_end_yaw_control = true;

//        dummy_straight_trajectory.push_back(0,0.0,0.0,0.0,1.0,0,0,0);
//        dummy_straight_trajectory.push_back(1,0.0,0.0,0.0,1.0,0,0,0);
//        dummy_straight_trajectory.push_back(2,0.0,0.0,0.0,1.0,0,0,0);
//        dummy_straight_trajectory.push_back(3,0.0,0.0,0.0,1.0,0,0,0);
//        dummy_straight_trajectory.push_back(36,30.9,0.0,0.0,1.0,0,0,0);

        dummy_straight_trajectory.push_back(0,0.0,0.0,0.0,1.0,0,0,0);
        dummy_straight_trajectory.push_back(1,0.1,0.0,0.0,1.0,0,0,0);
        dummy_straight_trajectory.push_back(2,0.2,0.0,0.0,1.0,0,0,0);
        dummy_straight_trajectory.push_back(3,0.3,0.0,0.0,1.0,0,0,0);
        dummy_straight_trajectory.push_back(4,0.4,0.0,0.0,1.0,0,0,0);


        dummy_right_turn_trajectory.push_back(-1.0,-1.0,0.0,0.0,1.0,0,0,0);
        dummy_right_turn_trajectory.push_back(0.0,0.0,0.0,0.0,1.0,0,0,0);
        dummy_right_turn_trajectory.push_back(1.0,-1.0,0.0,0.0,1.0,0,0,0);
        dummy_right_turn_trajectory.push_back(2.0,-2.0,0.0,0.0,1.0,0,0,0);

//        dummy_straight_trajectory.points.push_back(makePoint(0.0, 0.0, 1.0f));
//        dummy_straight_trajectory.points.push_back(makePoint(1.0, 0.0, 1.0f));
//        dummy_straight_trajectory.points.push_back(makePoint(2.0, 0.0, 1.0f));
//        dummy_straight_trajectory.points.push_back(makePoint(3.0, 0.0, 1.0f));
//        dummy_straight_trajectory.points.push_back(makePoint(4.0, 0.0, 1.0f));

//        dummy_right_turn_trajectory.points.push_back(makePoint(-1.0, -1.0, 1.0f));
//        dummy_right_turn_trajectory.points.push_back(makePoint(0.0, 0.0, 1.0f));
//        dummy_right_turn_trajectory.points.push_back(makePoint(1.0, -1.0, 1.0f));
//        dummy_right_turn_trajectory.points.push_back(makePoint(2.0, -2.0, 1.0f));

        neutral_steer.steering_tire_angle = 0.0;
        pose_zero.position.x = 0.0;
        pose_zero.position.y = 0.0;
    }

    void initializeMPC(MPC & mpc)
    {
        mpc.m_param = param;
        mpc.m_admissible_position_error = admissible_position_error;
        mpc.m_admissible_yaw_error_rad = admissible_yaw_error_rad;
        mpc.m_steer_lim = steer_lim;
        mpc.m_steer_rate_lim_map_by_curvature.emplace_back(0.0, steer_rate_lim);
        mpc.m_steer_rate_lim_map_by_velocity.emplace_back(0.0, steer_rate_lim);
        mpc.m_ctrl_period = ctrl_period;
        mpc.m_use_steer_prediction = use_steer_prediction;

        mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz);
        mpc.initializeSteeringPredictor();


//        const auto current_kinematics =
//            makeOdometry(pose_zero, 0.0);
//        mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics);
        // Init trajectory

    }

    iv::ADCOdometry makeOdometry(const iv::ADCPose & pose, const double velocity)
    {
        iv::ADCOdometry odometry;
        odometry.pose = pose;
        odometry.twist.linear_x = velocity;
        return odometry;
    }

public:
    void InitializeAndCalculate();
};

#endif // MPCTEST_H