/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See https://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Quadratic Programming
 *
 * Authors:
 * Olivier Kermorgant
 *
*****************************************************************************/

#include <algorithm>
#include <visp3/core/vpMatrixException.h>
#include <visp3/core/vpQuadProg.h>

#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)

/*!
  Changes a canonical quadratic cost \f$\min \frac{1}{2}\mathbf{x}^T\mathbf{H}\mathbf{x} + \mathbf{c}^T\mathbf{x}\f$
  to the formulation used by this class \f$ \min ||\mathbf{Q}\mathbf{x} - \mathbf{r}||^2\f$.

  Computes \f$(\mathbf{Q}, \mathbf{r})\f$ such that \f$\mathbf{H} = \mathbf{Q}^T\mathbf{Q}\f$ and \f$\mathbf{c} =
  -\mathbf{Q}^T\mathbf{r}\f$.

  \param H : canonical symmetric positive cost matrix (dimension n x n)
  \param c : canonical cost vector (dimension n)
  \param Q : custom cost matrix (dimension rank(H) x n)
  \param r : custom cost vector (dimension rank(H))
  \param tol : tolerance to test ranks

  \warning This method is only available if the Gnu Scientific Library
  (GSL) is detected as a third party library.

  Here is an example:

  \f$\begin{array}{lll}
  \mathbf{x} = &  \arg\min & 2x_1^2 + x_2^2 + x_1x_2 + x_1 + x_2\\
              & \text{s.t.}& x_1 + x_2 = 1
              \end{array}
  \Leftrightarrow
  \begin{array}{lll}
  \mathbf{x} = &  \arg\min & \frac{1}{2}\mathbf{x}^T\left[\begin{array}{cc}4 & 1 \\ 1 & 2\end{array}\right]\mathbf{x} +
  [1~1]\mathbf{x}\\ & \text{s.t.}& [1~1]\mathbf{x} = 1 \end{array} \f$ \code

  #include <visp3/core/vpLinProg.h>

  int main()
  {
    vpMatrix H(2,2), A(1,2);
    vpColVector c(2), b(1);

    H[0][0] = 4;
    H[0][1] = H[1][0] = 1;
    H[1][1] = 2;
    c[0] = c[1] = 1;
    A[0][0] = A[0][1] = 1;
    b[0] = 1;

    vpMatrix Q;vpColVector r;
    vpQuadProg::fromCanonicalCost(H, c, Q, r);

    vpColVector x;
    vpQuadProg::solveQPe(Q, r, A, b, x);
    std::cout << x.t() << std::endl;  // prints (0.25 0.75)
  }
  \endcode
*/
void vpQuadProg::fromCanonicalCost(const vpMatrix &H, const vpColVector &c, vpMatrix &Q, vpColVector &r,
                                   const double &tol)
{
#if defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV)
  unsigned int n = H.getCols();
  if (H.getRows() != n || c.getRows() != n) {
    throw(vpException(vpMatrixException::dimensionError,
                      "vpQuadProg::fromCanonicalCost: H is not square or not the same dimension as c"));
  }

  vpColVector d(n);
  vpMatrix V(n, n);
  // Compute the eigenvalues and eigenvectors
  H.eigenValues(d, V);
  // find first non-nullptr eigen value
  unsigned int k = 0;
  for (unsigned int i = 0; i < n; ++i) {
    if (d[i] > tol)
      d[i] = sqrt(d[i]);
    else if (d[i] < tol)
      throw(vpException(vpMatrixException::matrixError, "vpQuadProg::fromCanonicalCost: H is not positive"));
    else
      k = i + 1;
  }
  // build (Q,r) such that H = Q.^T.Q and c = -Q^T.r
  vpMatrix D(n - k, n - k);
  vpMatrix P(n - k, n);
  D.diag(d.extract(k, n - k));
  for (unsigned int i = 0; i < n - k; ++i)
    P[i][k + i] = 1;

  Q = D * P * V.transpose();
  r = -Q.t().pseudoInverse() * c;
#else
  throw(vpException(vpException::functionNotImplementedError, "Symmetric matrix decomposition is not implemented. You "
                                                              "should install Lapack, Eigen3 or OpenCV 3rd party"));
#endif
}

/*!
  Saves internally the column reduction of the passed equality constraint:

  \f$\mathbf{A}\mathbf{x} = \mathbf{b} \Leftrightarrow \mathbf{x} = \mathbf{x}_1 + \mathbf{Z}\mathbf{z}
  \f$
  \param A : equality matrix (dimension m x n)
  \param b : equality vector (dimension m)
  \param tol : tolerance to test the ranks

  \return True if \f$\mathbf{A}\mathbf{x} = \mathbf{b}\f$ has a solution.

  \sa solveQPi(), solveQP()
*/
bool vpQuadProg::setEqualityConstraint(const vpMatrix &A, const vpColVector &b, const double &tol)
{
  x1 = b;
  Z = A;
  if (A.getRows() == b.getRows() && vpLinProg::colReduction(Z, x1, false, tol))
    return true;

  std::cout << "vpQuadProg::setEqualityConstraint: equality constraint infeasible" << std::endl;
  return false;
}

/*!
  Solves a Quadratic Program under equality constraints.

  \f$\begin{array}{lll}
  \mathbf{x} = &  \arg\min & ||\mathbf{Q}\mathbf{x} - \mathbf{r}||^2 \\
               & \text{s.t.}& \mathbf{A}\mathbf{x} = \mathbf{b}\end{array}
  \f$
  \param Q : cost matrix (dimension c x n)
  \param r : cost vector (dimension c)
  \param A : equality matrix (dimension m x n)
  \param b : equality vector (dimension m)
  \param x : solution (dimension n)
  \param tol : tolerance to test the ranks

  \return True if the solution was found.

  This function is for internal use, no dimension check is performed and A and b may be modified.
*/
bool vpQuadProg::solveByProjection(const vpMatrix &Q, const vpColVector &r, vpMatrix &A, vpColVector &b, vpColVector &x,
                                   const double &tol)
{
  if (A.getRows()) {
    if (!vpLinProg::colReduction(A, b, false, tol))
      return false;

    if (A.getCols() && (Q * A).infinityNorm() > tol)
      x = b + A * (Q * A).solveBySVD(r - Q * b);
    else
      x = b;
  } else
    x = Q.solveBySVD(r);
  return true;
}

/*!
  Solves a Quadratic Program under previously stored equality constraints (setEqualityConstraint())

  \f$\begin{array}{lll}
  \mathbf{x} = &  \arg\min & ||\mathbf{Q}\mathbf{x} - \mathbf{r}||^2 \\
               & \text{s.t.}& \mathbf{A}\mathbf{x} = \mathbf{b}\end{array}
  \f$
  \param Q : cost matrix (dimension c x n)
  \param r : cost vector (dimension c)
  \param x : solution (dimension n)
  \param tol : Tolerance.

  \return True if the solution was found.

  Here is an example where the two following QPs are solved:

  \f$\begin{array}{lll}
  \mathbf{x} = &  \arg\min  & x_1^2 + x_2^2  \\
               & \text{s.t.}& x_1 + x_2 = 1\end{array}\f$

  \f$\begin{array}{lll}
  \mathbf{x} = &  \arg\min & (x_1-1)^2 + x_2^2  \\
               & \text{s.t.}& x_1 + x_2 = 1\end{array}\f$
  \code

  #include <visp3/core/vpLinProg.h>

  int main()
  {
    vpMatrix Q(2,2), A(1,2);
    vpColVector r(2), b(1);

    Q[0][0] = Q[1][1] = 1;
    A[0][0] = A[0][1] = b[0] = 1;

    vpQuadProg qp;
    qp.setEqualityConstraint(A, b);
    vpColVector x;

    // solve x_1^2 + x_2^2
    qp.solveQPe(Q, r, x);
    std::cout << x.t() << std::endl;  // prints (0.5 0.5)

    // solve (x_1-1)^2 + x_2^2
    r[0] = 1;
    qp.solveQPe(Q, r, x);
    std::cout << x.t() << std::endl;  // prints (1 0)
  }
  \endcode

  \sa setEqualityConstraint(), solveQPe()
*/
bool vpQuadProg::solveQPe(const vpMatrix &Q, const vpColVector &r, vpColVector &x, const double &tol) const
{
  unsigned int n = Q.getCols();
  if (Q.getRows() != r.getRows() || Z.getRows() != n || x1.getRows() != n) {
    std::cout << "vpQuadProg::solveQPe: wrong dimension\n"
              << "Q: " << Q.getRows() << "x" << Q.getCols() << " - r: " << r.getRows() << std::endl;
    std::cout << "Z: " << Z.getRows() << "x" << Z.getCols() << " - x1: " << x1.getRows() << std::endl;
    throw vpMatrixException::dimensionError;
  }
  if (Z.getCols()) {
    if ((Q * Z).infinityNorm() > tol)
      x = x1 + Z * (Q * Z).solveBySVD(r - Q * x1);
    else
      x = x1;
  } else
    x = Q.solveBySVD(r);
  return true;
}

/*!
  Solves a Quadratic Program under equality constraints.

  \f$\begin{array}{lll}
  \mathbf{x} = &  \arg\min & ||\mathbf{Q}\mathbf{x} - \mathbf{r}||^2 \\
               & \text{s.t.}& \mathbf{A}\mathbf{x} = \mathbf{b}\end{array}\f$

  If the equality constraints \f$(\mathbf{A}, \mathbf{b})\f$ are the same between two calls,
  it is more efficient to use setEqualityConstraint() and solveQPe().

  \param Q : cost matrix (dimension c x n)
  \param r : cost vector (dimension c)
  \param A : equality matrix (dimension m x n)
  \param b : equality vector (dimension m)
  \param x : solution (dimension n)
  \param tol : tolerance to test the ranks

  \return True if the solution was found.

  Here is an example:

  \f$\begin{array}{lll}
  \mathbf{x} = &  \arg\min & (x_1-1)^2 + x_2^2  \\
               & \text{s.t.}& x_1 + x_2 = 1\end{array}\f$
  \code

  #include <visp3/core/vpLinProg.h>

  int main()
  {
    vpMatrix Q(2,2), A(1,2);
    vpColVector r(2), b(1);

    Q[0][0] = Q[1][1] = 1;
    r[0] = 1;
    A[0][0] = A[0][1] = b[0] = 1;

    vpColVector x;

    vpQuadProg::solveQPe(Q, r, A, b, x);
    std::cout << x.t() << std::endl;  // prints (1 0)
  }
  \endcode

  \sa setEqualityConstraint(), solveQP(), solveQPi()
*/
bool vpQuadProg::solveQPe(const vpMatrix &Q, const vpColVector &r, vpMatrix A, vpColVector b, vpColVector &x,
                          const double &tol)
{
  checkDimensions(Q, r, &A, &b, nullptr, nullptr, "solveQPe");

  if (!solveByProjection(Q, r, A, b, x, tol)) {
    std::cout << "vpQuadProg::solveQPe: equality constraint infeasible" << std::endl;
    return false;
  }
  return true;
}

/*!
  Solves a Quadratic Program under equality and inequality constraints.

  If the equality constraints \f$(\mathbf{A}, \mathbf{b})\f$ are the same between two calls, it is more efficient to use
  setEqualityConstraint() and solveQPi().

  \f$\begin{array}{lll}
  \mathbf{x} = &  \arg\min & ||\mathbf{Q}\mathbf{x} - \mathbf{r}||^2 \\
               & \text{s.t.}& \mathbf{A}\mathbf{x} = \mathbf{b} \\
               & \text{s.t.}& \mathbf{C}\mathbf{x} \leq \mathbf{d}
  \end{array}
  \f$
  \param Q : cost matrix (dimension c x n)
  \param r : cost vector (dimension c)
  \param A : equality matrix (dimension m x n)
  \param b : equality vector (dimension m)
  \param C : inequality matrix (dimension p x n)
  \param d : inequality vector (dimension p)
  \param x : solution (dimension n)
  \param tol : tolerance to test the ranks

  \return True if the solution was found.

  Here is an example:

  \f$\begin{array}{lll}
  \mathbf{x} = &  \arg\min & (x_1-1)^2 + x_2^2  \\
               & \text{s.t.}& x_1 + x_2 = 1  \\
                & \text{s.t.} & x_2 \geq 1\end{array}\f$
  \code

  #include <visp3/core/vpLinProg.h>

  int main()
  {
    vpMatrix Q(2,2), A(1,2), C(1,2);
    vpColVector r(2), b(1), d(1);

    Q[0][0] = Q[1][1] = 1;    r[0] = 1;
    A[0][0] = A[0][1] = 1;    b[0] = 1;
    C[0][1] = -1; d[0] = -1;

    vpColVector x;
    vpQuadProg qp;

    qp.solveQP(Q, r, A, b, C, d, x);
    std::cout << x.t() << std::endl;  // prints (0 1)
  }
  \endcode

  \sa resetActiveSet()
*/
bool vpQuadProg::solveQP(const vpMatrix &Q, const vpColVector &r, vpMatrix A, vpColVector b, const vpMatrix &C,
                         const vpColVector &d, vpColVector &x, const double &tol)
{
  if (A.getRows() == 0)
    return solveQPi(Q, r, C, d, x, false, tol);

  checkDimensions(Q, r, &A, &b, &C, &d, "solveQP");

  if (!vpLinProg::colReduction(A, b, false, tol)) {
    std::cout << "vpQuadProg::solveQP: equality constraint infeasible" << std::endl;
    return false;
  }

  if (A.getCols() && solveQPi(Q * A, r - Q * b, C * A, d - C * b, x, false, tol)) {
    x = b + A * x;
    return true;
  } else if (vpLinProg::allLesser(C, b, d, tol)) // Ax = b has only 1 solution
  {
    x = b;
    return true;
  }
  std::cout << "vpQuadProg::solveQP: inequality constraint infeasible" << std::endl;
  return false;
}

/*!
  Solves a Quadratic Program under inequality constraints

  \f$\begin{array}{lll}
  \mathbf{x} = &  \arg\min & ||\mathbf{Q}\mathbf{x} - \mathbf{r}||^2 \\
               & \text{s.t.}& \mathbf{C}\mathbf{x} \leq \mathbf{d}
  \end{array}
  \f$
  \param Q : cost matrix (dimension c x n)
  \param r : cost vector (dimension c)
  \param C : inequality matrix (dimension p x n)
  \param d : inequality vector (dimension p)
  \param x : solution (dimension n)
  \param use_equality : if a previously saved equality constraint (setEqualityConstraint()) should be considered
  \param tol : tolerance to test the ranks

  \return True if the solution was found.

  Here is an example:

  \f$\begin{array}{lll}
  \mathbf{x} = &  \arg\min & (x_1-1)^2 + x_2^2  \\
               & \text{s.t.}& x_1 + x_2 \leq 1  \\
                & \text{s.t.} & x_1, x_2 \geq 0\end{array}\f$
  \code

  #include <visp3/core/vpLinProg.h>

  int main()
  {
    vpMatrix Q(2,2), C(3,2);
    vpColVector r(2), d(1);

    Q[0][0] = Q[1][1] = 1;    r[0] = 1;
    C[0][0] = C[0][1] = 1;    d[0] = 1;
    C[1][0] = C[2][1] = -1;

    vpColVector x;
    vpQuadProg qp;

    qp.solveQPi(Q, r, C, d, x);
    std::cout << x.t() << std::endl;  // prints (1 0)
  }
  \endcode
*/
bool vpQuadProg::solveQPi(const vpMatrix &Q, const vpColVector &r, const vpMatrix &C, const vpColVector &d,
                          vpColVector &x, bool use_equality, const double &tol)
{
  unsigned int n = checkDimensions(Q, r, nullptr, nullptr, &C, &d, "solveQPi");

  if (use_equality) {
    if (Z.getRows() == n) {
      if (Z.getCols() && solveQPi(Q * Z, r - Q * x1, C * Z, d - C * x1, x, false, tol)) {
        // back to initial solution
        x = x1 + Z * x;
        return true;
      } else if (vpLinProg::allLesser(C, x1, d, tol)) {
        x = x1;
        return true;
      }
      std::cout << "vpQuadProg::solveQPi: inequality constraint infeasible" << std::endl;
      return false;
    } else
      std::cout << "vpQuadProg::solveQPi: use_equality before setEqualityConstraint" << std::endl;
  }

  const unsigned int p = C.getRows();

  // look for trivial solution
  // r = 0 and d > 0 -> x = 0
  if (vpLinProg::allZero(r, tol) && (d.getRows() == 0 || vpLinProg::allGreater(d, -tol))) {
    x.resize(n);
    return true;
  }

  // go for solver
  // build feasible point
  vpMatrix A;
  vpColVector b;
  // check active set - all values should be < rows of C
  for (auto v : active) {
    if (v >= p) {
      active.clear();
      std::cout << "vpQuadProg::solveQPi: some constraints have been removed since last call\n";
      break;
    }
  }

  // warm start from previous active set
  A.resize((unsigned int)active.size(), n);
  b.resize((unsigned int)active.size());
  for (unsigned int i = 0; i < active.size(); ++i) {
    for (unsigned int j = 0; j < n; ++j)
      A[i][j] = C[active[i]][j];
    b[i] = d[active[i]];
  }
  if (!solveByProjection(Q, r, A, b, x, tol))
    x.resize(n);

  // or from simplex if we really have no clue
  if (!vpLinProg::allLesser(C, x, d, tol)) {
    // feasible point with simplex:
    //  min r
    //   st C.(x + z1 - z2) + y - r = d
    //   st z1, z2, y, r >= 0
    // dim r = violated constraints
    // feasible if r can be minimized to 0

    // count how many violated constraints
    vpColVector e = d - C * x;
    unsigned int k = 0;
    for (unsigned int i = 0; i < p; ++i) {
      if (e[i] < -tol)
        k++;
    }
    // cost vector
    vpColVector c(2 * n + p + k);
    for (unsigned int i = 0; i < k; ++i)
      c[2 * n + p + i] = 1;

    vpColVector xc(2 * n + p + k);

    vpMatrix A_lp(p, 2 * n + p + k);
    unsigned int l = 0;
    for (unsigned int i = 0; i < p; ++i) {
      // copy [C -C] part
      for (unsigned int j = 0; j < n; ++j) {
        A_lp[i][j] = C[i][j];
        A_lp[i][n + j] = -C[i][j];
      }
      // y-part
      A_lp[i][2 * n + i] = 1;
      if (e[i] < -tol) {
        // r-part
        A_lp[i][2 * n + p + l] = -1;
        xc[2 * n + p + l] = -e[i];
        l++;
      } else
        xc[2 * n + i] = e[i];
    }
    vpLinProg::simplex(c, A_lp, e, xc);

    // r-part should be 0
    if (!vpLinProg::allLesser(xc.extract(2 * n + p, k), tol)) {
      std::cout << "vpQuadProg::solveQPi: inequality constraints not feasible" << std::endl;
      return false;
    }

    // update x to feasible point
    x += xc.extract(0, n) - xc.extract(n, n);
    // init active/inactive sets from y-part of x
    active.clear();
    active.reserve(p);
    inactive.clear();
    for (unsigned int i = 0; i < p; ++i) {
      if (C.getRow(i) * x - d[i] < -tol)
        inactive.push_back(i);
      else
        active.push_back(i);
    }
  } else // warm start feasible
  {
    // using previous active set, check that inactive is sync
    if (active.size() + inactive.size() != p) {
      inactive.clear();
      for (unsigned int i = 0; i < p; ++i) {
        if (std::find(active.begin(), active.end(), i) == active.end())
          inactive.push_back(i);
      }
    }
  }

  vpMatrix Ap;
  bool update_Ap = true;
  unsigned int last_active = C.getRows();

  vpColVector u, g = r - Q * x, mu;

  // solve at one iteration
  while (true) {
    A.resize((unsigned int)active.size(), n);
    b.resize((unsigned int)active.size());
    for (unsigned int i = 0; i < active.size(); ++i) {
      for (unsigned int j = 0; j < n; ++j)
        A[i][j] = C[active[i]][j];
    }

    if (update_Ap && active.size())
      Ap = A.pseudoInverse(); // to get Lagrange multipliers if needed

    if (!solveByProjection(Q, g, A, b, u, tol)) {
      std::cout << "vpQuadProg::solveQPi: QP seems infeasible, too many constraints activated\n";
      return false;
    }

    // 0-update = optimal or useless activated constraints
    if (vpLinProg::allZero(u, tol)) {
      // compute multipliers if any
      unsigned int ineqInd = (unsigned int)active.size();
      if (active.size()) {
        mu = -Ap.transpose() * Q.transpose() * (Q * u - g);
        // find most negative one if any - except last activated in case of degeneracy
        double ineqMax = -tol;
        for (unsigned int i = 0; i < mu.getRows(); ++i) {
          if (mu[i] < ineqMax && active[i] != last_active) {
            ineqInd = i;
            ineqMax = mu[i];
          }
        }
      }

      if (ineqInd == active.size()) // KKT condition no useless constraint
        return true;

      // useless inequality, deactivate
      inactive.push_back(active[ineqInd]);
      if (active.size() == 1)
        active.clear();
      else
        active.erase(active.begin() + ineqInd);
      update_Ap = true;
    } else // u != 0, can improve xc
    {
      unsigned int ineqInd = 0;
      // step length to next constraint
      double alpha = 1;
      for (unsigned int i = 0; i < inactive.size(); ++i) {
        const double Cu = C.getRow(inactive[i]) * u;
        if (Cu > tol) {
          const double a = (d[inactive[i]] - C.getRow(inactive[i]) * x) / Cu;
          if (a < alpha) {
            alpha = a;
            ineqInd = i;
          }
        }
      }
      if (alpha < 1) {
        last_active = inactive[ineqInd];
        if (active.size()) {
          auto it = active.begin();
          while (it != active.end() && *it < inactive[ineqInd])
            it++;
          active.insert(it, inactive[ineqInd]);
        } else
          active.push_back(inactive[ineqInd]);
        inactive.erase(inactive.begin() + ineqInd);
        update_Ap = true;
      } else
        update_Ap = false;
      // update x for next iteration
      x += alpha * u;
      g -= alpha * Q * u;
    }
  }
}

/*!
  Pick either SVD (over-constrained) or QR (square or under-constrained)

  Assumes A is full rank, hence uses SVD iif A has more row than columns.

  \param A : matrix (dimension m x n)
  \param b : vector (dimension m)

  \return least-norm solution to \f$\arg\min||\mathbf{A}\mathbf{x} - \mathbf{b}||\f$
*/
vpColVector vpQuadProg::solveSVDorQR(const vpMatrix &A, const vpColVector &b)
{
  // assume A is full rank
  if (A.getRows() > A.getCols())
    return A.solveBySVD(b);
  return A.solveByQR(b);
}
#else
void dummy_vpQuadProg(){};
#endif
