#include <Eigen/Eigen>
  #include <assert.h>
  #include <cmath>
  #include <iostream>
  #include <tuple>
  #include <typeinfo>
  #include <vector>

  int MaxId(const Eigen::VectorXd &h) {
    int max_id = -1;
    int id = 0;
    double max_ele = std::numeric_limits<double>::lowest();
    while (id < h.rows() - 1) {
      double ele = std::abs(h(id));
      if (ele > max_ele) {
        max_ele = ele;
        max_id = id;
      }
      id++;
    }
    assert(max_id >= 0);
    return max_id;
  }

  double sign(double c) {
    if (c >= 0.0) {
      return 1.0;
    }
    return -1.0;
  }

  struct Constrains {
    Constrains(int dim) {
      A = Eigen::MatrixXd::Zero(0, dim);
      b = Eigen::VectorXd::Zero(0);
    }
    Constrains(const Eigen::MatrixXd &_A, const Eigen::VectorXd &_b) {
      assert(A.rows() == b.rows());
      A = _A;
      b = _b;
    }
    int dim() const { return A.cols(); }
    int size() const { return A.rows(); }
    void insert(const Eigen::VectorXd &_A, const double _b) {
      A.conservativeResize(A.rows() + 1, A.cols());
      A.row(A.rows() - 1) = _A;
      b.conservativeResize(b.rows() + 1);
      b(b.rows() - 1) = _b;
    }
    Eigen::MatrixXd A;
    Eigen::VectorXd b;
  };

  bool OneDimMinNorm(const Constrains &H, Eigen::VectorXd *y) {
    assert(H.A.cols() == 1);
    double low = std::numeric_limits<double>::lowest();
    double up = std::numeric_limits<double>::max();
    for (int i = 0; i < H.A.rows(); ++i) {
      if (H.A(i, 0) > 0) {
        up = std::min(up, H.b(i) / H.A(i, 0));
      } else if (H.A(i, 0) < 0.0) {
        low = std::max(low, H.b(i) / H.A(i, 0));
      }
    }
    if (low > up) {
      return false;
    }
    (*y)(0) = std::min(up, std::max(0.0, low));
    return true;
  }

  std::tuple<Eigen::MatrixXd, Eigen::VectorXd, Constrains>
  HouseholderProj(const Constrains &I, const Eigen::VectorXd &g, double f) {
    int dim = g.rows();
    // calcualte origin v
    Eigen::VectorXd v(dim);
    v = (f * g) / g.dot(g);
    // calcualte orth basis M
    int max_id = MaxId(g);
    Eigen::MatrixXd e = Eigen::MatrixXd::Identity(dim, dim);
    e(max_id, max_id) = (-sign(g(max_id)) * g.norm());
    Eigen::VectorXd u = g - e.col(max_id);
    Eigen::MatrixXd H = Eigen::MatrixXd::Identity(dim, dim) -
                        2.0 * u * u.transpose() / (u.dot(u));
    Eigen::MatrixXd transformed_e = H.transpose() * e;
    double dist = (transformed_e.col(max_id) - g).norm();
    assert(dist <= 0.0000001);
    Eigen::MatrixXd M(dim, dim - 1);
    M << transformed_e.leftCols(max_id),
        transformed_e.rightCols(dim - max_id - 1);
    // calcualte H_dot
    Constrains H_dot(I.A * M, I.b - I.A * v);
    return std::make_tuple(M, v, H_dot);
  }

  bool InConstrain(const Eigen::VectorXd &A, const double b,
                  const Eigen::VectorXd &y) {
    assert(A.rows() == y.rows());
    return A.dot(y) <= b;
  }

  // H: {a.T * x <= b}
  bool LowDimMinNorm(const Constrains &H, Eigen::VectorXd *y) {
    *y = Eigen::VectorXd::Zero(H.dim());
    if (H.size() == 0) {
      return true;
    }
    if (H.dim() == 1) {
      return OneDimMinNorm(H, y);
    }
    Constrains I(H.dim());
    for (int j = 0; j < H.size(); ++j) {
      if (!InConstrain(H.A.row(j), H.b(j), *y)) {
        Eigen::MatrixXd M;
        Eigen::VectorXd v;
        Constrains H_dot(H.dim() - 1);
        std::tie(M, v, H_dot) = HouseholderProj(I, H.A.row(j), H.b(j));
        Eigen::VectorXd y_dot(H.dim() - 1);
        if (!LowDimMinNorm(H_dot, &y_dot)) {
          return false;
        }
        *y = M * y_dot + v;
      }
      I.insert(H.A.row(j), H.b(j));
    }
    return true;
  }

  int main() {
    const int d = 3;
    int m = 7;
    Eigen::Matrix<double, 3, 3> Q;
    Eigen::Matrix<double, 3, 1> c;
    Eigen::Matrix<double, 3, 1> x;        // decision variables
    Eigen::Matrix<double, -1, 3> A(m, 3); // constraint matrix
    Eigen::VectorXd b(m);                 // constraint bound
    Q << 2.0, 1.0, 1.0, 1.0, 2.0, 1.0, 1.0, 1.0, 2.0;
    c << 1.2, 2.5, -10.0;
    A << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, -0.7, 0.5, 0.0, 0.5, -1.0,
        0.0, 0.0, 0.13, -1.0, 0.1, -3.0, -1.3;
    b << 10.0, 10.0, 10.0, 1.7, -7.1, -3.31, 2.59;
    // 将qp问题转化为min norm问题
    Eigen::LLT<Eigen::Matrix<double, d, d>> llt(Q);
    if (llt.info() != Eigen::Success) {
      std::cout << "infinity\n";
      return 0;
    }
    const Eigen::Matrix<double, -1, d> As =
        llt.matrixU().template solve<Eigen::OnTheRight>(A);
    const Eigen::Matrix<double, d, 1> v = llt.solve(c);
    const Eigen::Matrix<double, -1, 1> bs = A * v + b;

    // 求解min norm问题
    Constrains H(As, bs);
    Eigen::VectorXd z(H.dim());
    if (LowDimMinNorm(H, &z)) {
      llt.matrixU().template solveInPlace<Eigen::OnTheLeft>(z);
      z -= v;
      std::cout << "optimal sol: " << z.transpose() << std::endl;
      std::cout << "minobj: " << 0.5 * (Q * z).dot(z) + c.dot(z) << std::endl;
      std::cout << "cons precision: " << (A * z - b).maxCoeff() << std::endl;
    } else {
      std::cout << "infeasible\n";
    }
    return 0;
  }