#pragma once

#include <utility>

#include <Eigen/Core>

namespace drake {
namespace math {
/**
 * For a symmetric positive semidefinite matrix Y, decompose it into XᵀX, where
 * the number of rows in X equals to the rank of Y.
 * Notice that this decomposition is not unique. For any orthonormal matrix U,
 * s.t UᵀU = identity, X_prime = UX also satisfies X_primeᵀX_prime = Y. Here
 * we only return one valid decomposition.
 * @param Y A symmetric positive semidefinite matrix.
 * @param zero_tol We will need to check if some value (for example, the
 * absolute value of Y's eigenvalues) is smaller than zero_tol. If it is, then
 * we deem that value as 0.
 * @retval X. The matrix X satisfies XᵀX = Y and X.rows() = rank(Y).
 * @pre 1. Y is positive semidefinite.
 *      2. zero_tol is non-negative.
 * @throws std::exception when the pre-conditions are not satisfied.
 * @note We only use the lower triangular part of Y.
 */
Eigen::MatrixXd DecomposePSDmatrixIntoXtransposeTimesX(
    const Eigen::Ref<const Eigen::MatrixXd>& Y, double zero_tol);

/**
 * Rewrite a quadratic form xᵀQx + bᵀx + c to
 * (Rx+d)ᵀ(Rx+d)
 * where
 * <pre>
 * RᵀR = Q
 * Rᵀd = b / 2
 * dᵀd = c
 * </pre>
 *
 * This decomposition requires the matrix
 * <pre>
 * ⌈Q     b/2⌉
 * ⌊bᵀ/2    c⌋
 * </pre>
 * to be positive semidefinite.
 *
 * We return R and d with the minimal number of rows, namely the rows of R
 * and d equal to the rank of the matrix
 * <pre>
 * ⌈Q     b/2⌉
 * ⌊bᵀ/2    c⌋
 * </pre>
 *
 * Notice that R might have more rows than Q, For example, the quadratic
 * expression x² + 2x + 5 =(x+1)² + 2², it can be decomposed as
 * <pre>
 * ⎛⌈1⌉ * x + ⌈1⌉⎞ᵀ * ⎛⌈1⌉ * x + ⌈1⌉⎞
 * ⎝⌊0⌋       ⌊2⌋⎠    ⎝⌊0⌋       ⌊2⌋⎠
 * </pre>
 * Here R has 2 rows while Q only has 1 row.
 *
 * On the other hand the quadratic expression x² + 2x + 1 can be decomposed
 * as (x+1) * (x+1), where R has 1 row, same as Q.
 *
 * Also notice that this decomposition is not unique. For example, with any
 * permutation matrix P, we can define
 * <pre>
 * R₁ = P*R
 * d₁ = P*d
 * </pre>
 * Then (R₁*x+d₁)ᵀ(R₁*x+d₁) gives the same quadratic form.
 * @param Q The square matrix.
 * @param b The vector containing the linear coefficients.
 * @param c The constant term.
 * @param tol We will determine if this quadratic form is always non-negative,
 * by checking the Eigen values of the matrix
 * [Q    b/2]
 * [bᵀ/2   c]
 * are all greater than -tol. @default is 0.
 * @retval (R, d). R and d have the same number of rows. R.cols() == x.rows().
 * R.rows() equals to the rank of the matrix
 * <pre>
 *    [Q    b/2]
 *    [bᵀ/2   c]
 * </pre>
 * @pre 1. The quadratic form is always non-negative, namely the matrix
 *         <pre>
 *         [Q    b/2]
 *         [bᵀ/2   c]
 *         </pre>
 *         is positive semidefinite.
 *      2. `Q` and `b` are of the correct size.
 *      3. `tol` is non-negative.
 * @throws std::exception if the precondition is not satisfied.
 */
std::pair<Eigen::MatrixXd, Eigen::MatrixXd>
DecomposePositiveQuadraticForm(const Eigen::Ref<const Eigen::MatrixXd>& Q,
                               const Eigen::Ref<const Eigen::VectorXd>& b,
                               double c, double tol = 0);

/** Given two quadratic forms, x'Sx > 0 and x'Px, (with P symmetric and full
 * rank), finds a change of variables x = Ty, which simultaneously diagonalizes
 * both forms (as inspired by "balanced truncation" in model-order reduction
 * [1]).  In this note, we use abs(M) to indicate the elementwise absolute
 * value.
 *
 * Adapting from [1], we observe that there is a family of coordinate systems
 * that can simultaneously diagonalize T'ST and T'PT.  Using D to denote a
 * diagonal matrix, we call the result S-normal if T'ST = I and abs(T'PT) = D⁻²,
 * call it P-normal if T'ST = D² and abs(T'PT) = I, and call it "balanced" if
 * T'ST = D and abs(T'PT) = D⁻¹. Note that if P > 0, then T'PT = D⁻¹.
 *
 * We find x=Ty such that T'ST = D and abs(T'PT) = D⁻¹, where D is diagonal. The
 * recipe is:
 * - Factorize S = LL', and choose R=L⁻¹.
 * - Take svd(RPR') = UΣV', and note that U=V for positive definite matrices,
 *     and V is U up to a sign flip of the singular vectors for all symmetric
 *     matrices.
 * - Choose T = R'U Σ^{-1/4}, where the matrix exponent can be taken elementwise
 *   because Σ is diagonal. This gives T'ST = Σ^{-1/2} (by using U'U=I), and
 *   abs(T'PT) = Σ^{1/2}.  If P > 0, then T'PT = Σ^{1/2}.
 *
 * Note that the numerical "balancing" can address the absolute scaling of the
 * quadratic forms, but not the relative scaling.  To understand this, consider
 * the scalar case: we have two quadratic functions, sx² and px², with s>0, p>0.
 * We'd like to choose x=Ty so that sT²y² and pT²y² are "balanced" (we'd like
 * them both to be close to y²).  We'll choose T=p^{-1/4}s^{-1/4}, which gives
 * sx² = sqrt(s/p)y², and px² = sqrt(p/s)y². For instance if s=1e8 and p=1e8,
 * then t=1e-4 and st^2 = pt^2 = 1.  But if s=10, p=1e7, then t=0.01, and st^2 =
 * 1e-3, pt^2 = 1e3.
 *
 * In the matrix case, the absolute scaling is important -- it ensures that the
 * two quadratic forms have the same matrix condition number and makes them as
 * close as possible to 1. Besides absolute scaling, in the matrix case the
 * balancing transform diagonalizes both quadratic forms.
 *
 * [1] B. Moore, “Principal component analysis in linear systems:
 * Controllability, observability, and model reduction,” IEEE Trans. Automat.
 * Contr., vol. 26, no. 1, pp. 17–32, Feb. 1981.
 */
Eigen::MatrixXd BalanceQuadraticForms(
    const Eigen::Ref<const Eigen::MatrixXd>& S,
    const Eigen::Ref<const Eigen::MatrixXd>& P);

}  // namespace math
}  // namespace drake