text: /**
  SVD based on implicit QR with Wilkinson Shift
  */
  #ifndef IMPLICIT_QR_SVD_H
  #define IMPLICIT_QR_SVD_H
  
  #include <Eigen/Core>
  #include <Eigen/Dense>
  #include <Eigen/SVD>
  #include <Ziran/Math/Linear/DenseExt.h>
  #include <Ziran/Math/Linear/Givens.h>
  
  namespace ZIRAN {
  /**
     \brief 1x1 polar decomposition.
     \param[in] A matrix.
     \param[out] R Robustly a rotation matrix
     \param[out] S_Sym Symmetric.
  
     Polar guarantees negative sign is on the small magnitude singular value.
     S is guaranteed to be the closest one to identity.
     R is guaranteed to be the closest rotation to A.
  */
  template <class TA, class TR, class TS>
  inline std::enable_if_t<isSize<TA>(1, 1) && isSize<TR>(1, 1) && isSize<TS>(1, 1)>
  polarDecomposition(const Eigen::MatrixBase<TA>& A,
      Eigen::MatrixBase<TR>& R,
      Eigen::MatrixBase<TS>& S_Sym)
  {
      S_Sym = A;
      R(0, 0) = 1;
  }
  
  /**
     \brief 2x2 polar decomposition.
     \param[in] A matrix.
     \param[out] R Robustly a rotation matrix in givens form
     \param[out] S_Sym Symmetric. Whole matrix is stored
  
     Whole matrix S is stored since its faster to calculate due to simd vectorization
     Polar guarantees negative sign is on the small magnitude singular value.
     S is guaranteed to be the closest one to identity.
     R is guaranteed to be the closest rotation to A.
  */
  template <class TA, class T, class TS>
  inline std::enable_if_t<isSize<TA>(2, 2) && isSize<TS>(2, 2)>
  polarDecomposition(const Eigen::MatrixBase<TA>& A,
      GivensRotation<T>& R,
      const Eigen::MatrixBase<TS>& S_Sym)
  {
      Vector<T, 2> x(A(0, 0) + A(1, 1), A(1, 0) - A(0, 1));
      T denominator = x.norm();
      R.c = (T)1;
      R.s = (T)0;
      if (denominator != 0) {
          /*
            No need to use a tolerance here because x(0) and x(1) always have
            smaller magnitude then denominator, therefore overflow never happens.
          */
          R.c = x(0) / denominator;
          R.s = -x(1) / denominator;
      }
      Eigen::MatrixBase<TS>& S = const_cast<Eigen::MatrixBase<TS>&>(S_Sym);
      S = A;
      R.rowRotation(S);
  }
  
  /**
     \brief 2x2 polar decomposition.
     \param[in] A matrix.
     \param[out] R Robustly a rotation matrix.
     \param[out] S_Sym Symmetric. Whole matrix is stored
  
     Whole matrix S is stored since its faster to calculate due to simd vectorization
     Polar guarantees negative sign is on the small magnitude singular value.
     S is guaranteed to be the closest one to identity.
     R is guaranteed to be the closest rotation to A.
  */
  template <class TA, class TR, class TS>
  inline std::enable_if_t<isSize<TA>(2, 2) && isSize<TR>(2, 2) && isSize<TS>(2, 2)>
  polarDecomposition(const Eigen::MatrixBase<TA>& A,
      const Eigen::MatrixBase<TR>& R,
      const Eigen::MatrixBase<TS>& S_Sym)
  {
      using T = ScalarType<TA>;
      GivensRotation<T> r(0, 1);
      polarDecomposition(A, r, S_Sym);
      r.fill(R);
  }
  
  /**
     \brief 2x2 SVD (singular value decomposition) A=USV'
     \param[in] A Input matrix.
     \param[out] U Robustly a rotation matrix in Givens form
     \param[out] Sigma Vector of singular values sorted with decreasing magnitude. The second one can be negative.
     \param[out] V Robustly a rotation matrix in Givens form
  */
  template <class TA, class T, class Ts>
  inline std::enable_if_t<isSize<TA>(2, 2) && isSize<Ts>(2, 1)>
  singularValueDecomposition(
      const Eigen::MatrixBase<TA>& A,
      GivensRotation<T>& U,
      const Eigen::MatrixBase<Ts>& Sigma,
      GivensRotation<T>& V,
      const ScalarType<TA> tol = 64 * std::numeric_limits<ScalarType<TA>>::epsilon())
  {
      using std::sqrt;
      Eigen::MatrixBase<Ts>& sigma = const_cast<Eigen::MatrixBase<Ts>&>(Sigma);
  
      Matrix<T, 2, 2> S_Sym;
      polarDecomposition(A, U, S_Sym);
      T cosine, sine;
      T x = S_Sym(0, 0);
      T y = S_Sym(0, 1);
      T z = S_Sym(1, 1);
      T y2 = y * y;
      if (y2 == 0) {
          // S is already diagonal
          cosine = 1;
          sine = 0;
          sigma(0) = x;
          sigma(1) = z;
      }
      else {
          T tau = T(0.5) * (x - z);
          T w = sqrt(tau * tau + y2);
          // w > y > 0
          T t;
          if (tau > 0) {
              // tau + w > w > y > 0 ==> division is safe
              t = y / (tau + w);
          }
          else {
              // tau - w < -w < -y < 0 ==> division is safe
              t = y / (tau - w);
          }
          cosine = T(1) / sqrt(t * t + T(1));
          sine = -t * cosine;
          /*
            V = [cosine -sine; sine cosine]
            Sigma = V'SV. Only compute the diagonals for efficiency.
            Also utilize symmetry of S and don't form V yet.
          */
          T c2 = cosine * cosine;
          T csy = 2 * cosine * sine * y;
          T s2 = sine * sine;
          sigma(0) = c2 * x - csy + s2 * z;
          sigma(1) = s2 * x + csy + c2 * z;
      }
  
      // Sorting
      // Polar already guarantees negative sign is on the small magnitude singular value.
      if (sigma(0) < sigma(1)) {
          std::swap(sigma(0), sigma(1));
          V.c = -sine;
          V.s = cosine;
      }
      else {
          V.c = cosine;
          V.s = sine;
      }
      U *= V;
  }
  
  /**
     \brief 1x1 SVD (singular value decomposition) A=USV'
     \param[in] A Input matrix.
     \param[out] U Robustly a rotation matrix.
     \param[out] Sigma Vector of singular values sorted with decreasing magnitude. The second one can be negative.
     \param[out] V Robustly a rotation matrix.
  */
  template <class T>
  inline void
  singularValueDecomposition(
      const Vector<T, 1>& A,
      Vector<T, 1>& U,
      Vector<T, 1>& Sigma,
      Vector<T, 1>& V,
      const T tol = 64 * std::numeric_limits<T>::epsilon())
  {
      U(0, 0) = 1;
      V(0, 0) = 1;
      Sigma = A;
  }
  
  /**
     \brief 2x2 SVD (singular value decomposition) A=USV'
     \param[in] A Input matrix.
     \param[out] U Robustly a rotation matrix.
     \param[out] Sigma Vector of singular values sorted with decreasing magnitude. The second one can be negative.
     \param[out] V Robustly a rotation matrix.
  */
  template <class TA, class TU, class Ts, class TV>
  inline std::enable_if_t<isSize<TA>(2, 2) && isSize<TU>(2, 2) && isSize<TV>(2, 2) && isSize<Ts>(2, 1)>
  singularValueDecomposition(
      const Eigen::MatrixBase<TA>& A,
      const Eigen::MatrixBase<TU>& U,
      const Eigen::MatrixBase<Ts>& Sigma,
      const Eigen::MatrixBase<TV>& V,
      const ScalarType<TA> tol = 64 * std::numeric_limits<ScalarType<TA>>::epsilon())
  {
      using T = ScalarType<TA>;
      GivensRotation<T> gv(0, 1);
      GivensRotation<T> gu(0, 1);
      singularValueDecomposition(A, gu, Sigma, gv);
  
      gu.fill(U);
      gv.fill(V);
  }
  
  /**
    \brief compute wilkinsonShift of the block
    a1     b1
    b1     a2
    based on the wilkinsonShift formula
    mu = c + d - sign (d) \ sqrt (d*d + b*b), where d = (a-c)/2
  
    */
  template <class T>
  inline T wilkinsonShift(const T a1, const T b1, const T a2)
  {
      using std::copysign;
      using std::fabs;
      using std::sqrt;
  
      T d = (T)0.5 * (a1 - a2);
      T bs = b1 * b1;
  
      T mu = a2 - copysign(bs / (fabs(d) + sqrt(d * d + bs)), d);
      // T mu = a2 - bs / ( d + sign_d*sqrt (d*d + bs));
      return mu;
  }
  
  /**
    \brief Helper function of 3X3 SVD for processing 2X2 SVD
    */
  template <int t, class T>
  inline ZIRAN_FORCE_INLINE void process(Matrix<T, 3, 3>& B, Matrix<T, 3, 3>& U, Vector<T, 3>& sigma, Matrix<T, 3, 3>& V)
  {
      int other = (t == 1) ? 0 : 2;
      GivensRotation<T> u(0, 1);
      GivensRotation<T> v(0, 1);
      sigma(other) = B(other, other);
      singularValueDecomposition(B.template block<2, 2>(t, t), u, sigma.template block<2, 1>(t, 0), v);
      u.rowi += t;
      u.rowk += t;
      v.rowi += t;
      v.rowk += t;
      u.columnRotation(U);
      v.columnRotation(V);
  }
  
  /**
    \brief Helper function of 3X3 SVD for flipping signs due to flipping signs of sigma
    */
  template <class T>
  inline ZIRAN_FORCE_INLINE void flipSign(int i, Matrix<T, 3, 3>& U, Vector<T, 3>& sigma)
  {
      sigma(i) = -sigma(i);
      U.col(i) = -U.col(i);
  }
  
  /**
    \brief Helper function of 3X3 SVD for sorting singular values
    */
  template <int t, class T>
  inline std::enable_if_t<t == 0> sort(Matrix<T, 3, 3>& U, Vector<T, 3>& sigma, Matrix<T, 3, 3>& V)
  {
      using std::fabs;
  
      // Case: sigma(0) > |sigma(1)| >= |sigma(2)|
      if (fabs(sigma(1)) >= fabs(sigma(2))) {
          if (sigma(1) < 0) {
              flipSign(1, U, sigma);
              flipSign(2, U, sigma);
          }
          return;
      }
  
      //fix sign of sigma for both cases
      if (sigma(2) < 0) {
          flipSign(1, U, sigma);
          flipSign(2, U, sigma);
      }
  
      //swap sigma(1) and sigma(2) for both cases
      std::swap(sigma(1), sigma(2));
      U.col(1).swap(U.col(2));
      V.col(1).swap(V.col(2));
  
      // Case: |sigma(2)| >= sigma(0) > |simga(1)|
      if (sigma(1) > sigma(0)) {
          std::swap(sigma(0), sigma(1));
          U.col(0).swap(U.col(1));
          V.col(0).swap(V.col(1));
      }
  
      // Case: sigma(0) >= |sigma(2)| > |simga(1)|
      else {
          U.col(2) = -U.col(2);
          V.col(2) = -V.col(2);
      }
  }
  
  /**
    \brief Helper function of 3X3 SVD for sorting singular values
    */
  template <int t, class T>
  inline std::enable_if_t<t == 1> sort(Matrix<T, 3, 3>& U, Vector<T, 3>& sigma, Matrix<T, 3, 3>& V)
  {
      using std::fabs;
  
      // Case: |sigma(0)| >= sigma(1) > |sigma(2)|
      if (fabs(sigma(0)) >= sigma(1)) {
          if (sigma(0) < 0) {
              flipSign(0, U, sigma);
              flipSign(2, U, sigma);
          }
          return;
      }
  
      //swap sigma(0) and sigma(1) for both cases
      std::swap(sigma(0), sigma(1));
      U.col(0).swap(U.col(1));
      V.col(0).swap(V.col(1));
  
      // Case: sigma(1) > |sigma(2)| >= |sigma(0)|
      if (fabs(sigma(1)) < fabs(sigma(2))) {
          std::swap(sigma(1), sigma(2));
          U.col(1).swap(U.col(2));
          V.col(1).swap(V.col(2));
      }
  
      // Case: sigma(1) >= |sigma(0)| > |sigma(2)|
      else {
          U.col(1) = -U.col(1);
          V.col(1) = -V.col(1);
      }
  
      // fix sign for both cases
      if (sigma(1) < 0) {
          flipSign(1, U, sigma);
          flipSign(2, U, sigma);
      }
  }
  
  /**
    \brief 3X3 SVD (singular value decomposition) A=USV'
    \param[in] A Input matrix.
    \param[out] U is a rotation matrix.
    \param[out] sigma Diagonal matrix, sorted with decreasing magnitude. The third one can be negative.
    \param[out] V is a rotation matrix.
    */
  template <class T>
  inline ZIRAN_FORCE_INLINE int singularValueDecompositionHelper(const Matrix<T, 3, 3>& A,
      Matrix<T, 3, 3>& U,
      Vector<T, 3>& sigma,
      Matrix<T, 3, 3>& V,
      T tol)
  {
      using std::fabs;
      using std::max;
      using std::sqrt;
      Matrix<T, 3, 3> B = A;
      U = Matrix<T, 3, 3>::Identity();
      V = Matrix<T, 3, 3>::Identity();
  
      makeUpperBidiag(B, U, V);
  
      int count = 0;
      T mu = (T)0;
      GivensRotation<T> r(0, 1);
  
      T alpha_1 = B(0, 0);
      T beta_1 = B(0, 1);
      T alpha_2 = B(1, 1);
      T alpha_3 = B(2, 2);
      T beta_2 = B(1, 2);
      T gamma_1 = alpha_1 * beta_1;
      T gamma_2 = alpha_2 * beta_2;
      tol *= max((T)0.5 * sqrt(alpha_1 * alpha_1 + alpha_2 * alpha_2 + alpha_3 * alpha_3 + beta_1 * beta_1 + beta_2 * beta_2), (T)1);
  
      /**
        Do implicit shift QR until A^T A is block diagonal
        */
  
      while (fabs(beta_2) > tol && fabs(beta_1) > tol
          && fabs(alpha_1) > tol && fabs(alpha_2) > tol
          && fabs(alpha_3) > tol) {
          mu = wilkinsonShift(alpha_2 * alpha_2 + beta_1 * beta_1, gamma_2, alpha_3 * alpha_3 + beta_2 * beta_2);
  
          r.compute(alpha_1 * alpha_1 - mu, gamma_1);
          r.columnRotation(B);
  
          r.columnRotation(V);
          zeroChase(B, U, V);
  
          alpha_1 = B(0, 0);
          beta_1 = B(0, 1);
          alpha_2 = B(1, 1);
          alpha_3 = B(2, 2);
          beta_2 = B(1, 2);
          gamma_1 = alpha_1 * beta_1;
          gamma_2 = alpha_2 * beta_2;
          count++;
      }
      /**
        Handle the cases of one of the alphas and betas being 0
        Sorted by ease of handling and then frequency
        of occurrence
  
        If B is of form
        x x 0
        0 x 0
        0 0 x
        */
      if (fabs(beta_2) <= tol) {
          process<0>(B, U, sigma, V);
          sort<0>(U, sigma, V);
      }
      /**
        If B is of form
        x 0 0
        0 x x
        0 0 x
        */
      else if (fabs(beta_1) <= tol) {
          process<1>(B, U, sigma, V);
          sort<1>(U, sigma, V);
      }
      /**
        If B is of form
        x x 0
        0 0 x
        0 0 x
        */
      else if (fabs(alpha_2) <= tol) {
          /**
          Reduce B to
          x x 0
          0 0 0
          0 0 x
          */
          GivensRotation<T> r1(1, 2);
          r1.computeUnconventional(B(1, 2), B(2, 2));
          r1.rowRotation(B);
          r1.columnRotation(U);
  
          process<0>(B, U, sigma, V);
          sort<0>(U, sigma, V);
      }
      /**
        If B is of form
        x x 0
        0 x x
        0 0 0
        */
      else if (fabs(alpha_3) <= tol) {
          /**
          Reduce B to
          x x +
          0 x 0
          0 0 0
          */
          GivensRotation<T> r1(1, 2);
          r1.compute(B(1, 1), B(1, 2));
          r1.columnRotation(B);
          r1.columnRotation(V);
          /**
          Reduce B to
          x x 0
          + x 0
          0 0 0
          */
          GivensRotation<T> r2(0, 2);
          r2.compute(B(0, 0), B(0, 2));
          r2.columnRotation(B);
          r2.columnRotation(V);
  
          process<0>(B, U, sigma, V);
          sort<0>(U, sigma, V);
      }
      /**
        If B is of form
        0 x 0
        0 x x
        0 0 x
        */
      else if (fabs(alpha_1) <= tol) {
          /**
          Reduce B to
          0 0 +
          0 x x
          0 0 x
          */
          GivensRotation<T> r1(0, 1);
          r1.computeUnconventional(B(0, 1), B(1, 1));
          r1.rowRotation(B);
          r1.columnRotation(U);
  
          /**
          Reduce B to
          0 0 0
          0 x x
          0 + x
          */
          GivensRotation<T> r2(0, 2);
          r2.computeUnconventional(B(0, 2), B(2, 2));
          r2.rowRotation(B);
          r2.columnRotation(U);
  
          process<1>(B, U, sigma, V);
          sort<1>(U, sigma, V);
      }
  
      return count;
  }
  inline ZIRAN_FORCE_INLINE int singularValueDecomposition(const Matrix<float, 3, 3>& A,
      Matrix<float, 3, 3>& U,
      Vector<float, 3>& sigma,
      Matrix<float, 3, 3>& V,
      float tol = 128 * std::numeric_limits<float>::epsilon())
  {
      return singularValueDecompositionHelper(A, U, sigma, V, tol);
  }
  inline ZIRAN_FORCE_INLINE int singularValueDecomposition(const Matrix<double, 3, 3>& A,
      Matrix<double, 3, 3>& U,
      Vector<double, 3>& sigma,
      Matrix<double, 3, 3>& V,
      double tol = 1024 * std::numeric_limits<double>::epsilon())
  {
      return singularValueDecompositionHelper(A, U, sigma, V, tol);
  }
  
  /**
         \brief 3X3 polar decomposition.
         \param[in] A matrix.
         \param[out] R Robustly a rotation matrix.
         \param[out] S_Sym Symmetric. Whole matrix is stored
  
         Whole matrix S is stored
         Polar guarantees negative sign is on the small magnitude singular value.
         S is guaranteed to be the closest one to identity.
         R is guaranteed to be the closest rotation to A.
      */
  template <class T>
  inline ZIRAN_FORCE_INLINE void polarDecomposition(const Matrix<T, 3, 3>& A,
      Matrix<T, 3, 3>& R,
      Matrix<T, 3, 3>& S_Sym)
  {
      Matrix<T, 3, 3> U;
      Vector<T, 3> sigma;
      Matrix<T, 3, 3> V;
  
      singularValueDecomposition(A, U, sigma, V);
      R.noalias() = U * V.transpose();
      S_Sym.noalias() = V * Eigen::DiagonalMatrix<T, 3, 3>(sigma) * V.transpose();
  }
  } // namespace ZIRAN
  #endif
  