#include  "mpc_tools.h"

constexpr double pi() { return M_PI; }
double deg2rad(double x) { return x * pi() / 180; }
double rad2deg(double x) { return x * 180 / pi(); }


mpc_tools::mpc_tools()
{
}

mpc_tools::~mpc_tools()
{
}

//获取小车坐标系中，x前进方向 num 个点，用于拟合
bool mpc_tools::filterPath(tf::Pose pose,std::vector<tf::Pose> poses, std::vector<tf::Pose>& out,
    int numOfPoints,bool direct){
  if(poses.size() < numOfPoints)
    return false;

 out.clear();
  // 平移加反向旋转到小车坐标系
    double theta,a,b;
    pose.getBasis().getEulerZYX(theta,a,b);
    if(direct==true) theta+=M_PI;
  for (int i = 0; i < poses.size(); i++)
  {

    double x = poses[i].getOrigin().getX()-pose.getOrigin().getX();
    double y = poses[i].getOrigin().getY()-pose.getOrigin().getY();
    double trans_x = x * cos(-theta) - y * sin(-theta);
    double trans_y = x * sin(-theta) + y * cos(-theta);
    if (trans_x>=0  && out.size() < numOfPoints)
    {
        tf::Pose point;
        float nx=trans_x * cos(theta) - trans_y * sin(theta);
        float ny=trans_x*sin(theta)+trans_y*cos(theta);
        point.setOrigin(tf::Vector3(nx+pose.getOrigin().getX(),
            ny+pose.getOrigin().getY(),0));
        out.push_back(point);

    }
  }
  return true;

}

// 求y
double mpc_tools::polyeval(Eigen::VectorXd coeffs, double x) {
  double result = 0.0;
  for (int i = 0; i < coeffs.size(); i++) {
    result += coeffs[i] * pow(x, i);
  }
  return result;
}

// 求参数
Eigen::VectorXd mpc_tools::polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order) {
  assert(xvals.size() == yvals.size());
  assert(order >= 1 && order <= xvals.size() - 1);
  Eigen::MatrixXd A(xvals.size(), order + 1);

  for (int i = 0; i < xvals.size(); i++) {
    A(i, 0) = 1.0;
  }

  for (int j = 0; j < xvals.size(); j++) {
    for (int i = 0; i < order; i++) {
      A(j, i + 1) = A(j, i) * xvals(j);
    }
  }

  auto Q = A.householderQr();
  auto result = Q.solve(yvals);
  return result;
}

