#ifndef __JMATH_JSVD3D__
#define __JMATH_JSVD3D__

#include <algorithm>
#include <cmath>
#include <array>

#include "JMath/JConstants.hh"
#include "JMath/JMatrix3D.hh"
#include "JMath/JMatrix3S.hh"


namespace JMATH {}
namespace JPP { using namespace JMATH; }

namespace JMATH {

  /**
   * Singular value decomposition.
   *
   * See:
   * "Computing the Singular Value Decomposition of 3x3 matrices 
   *  with minimal branching and elementary floating point operations",\n
   * A.\ McAdams, A.\ Selle, R.\ Tamstorf, J.\ Teran and E.\ Sifakis,\n
   * University of Wisconsin - Madison technical report TR1690, May 2011
   */
  class JSVD3D {
  public:
    /**
     * Default constructor.
     */
    JSVD3D()
    {}


    /**
     * Constructor.
     *
     * \param  A       matrix
     */
    JSVD3D(const JMatrix3D& A)
    {
      decompose(A);
    }


    /**
     * Decompose given matrix.
     *
     * \param  A       matrix
     */
    void decompose(const JMatrix3D& A)
    {
      using namespace std;

      // B = A^T * A

      B.a00  =  A.a00 * A.a00  +  A.a10 * A.a10  +  A.a20 * A.a20;
    
      B.a10  =  A.a01 * A.a00  +  A.a11 * A.a10  +  A.a21 * A.a20;
      B.a11  =  A.a01 * A.a01  +  A.a11 * A.a11  +  A.a21 * A.a21;

      B.a20  =  A.a02 * A.a00  +  A.a12 * A.a10  +  A.a22 * A.a20;
      B.a21  =  A.a02 * A.a01  +  A.a12 * A.a11  +  A.a22 * A.a21;
      B.a22  =  A.a02 * A.a02  +  A.a12 * A.a12  +  A.a22 * A.a22;

      B.a01  =  B.a10;
      B.a02  =  B.a20;
      B.a12  =  B.a21;


      // symmetric eigen alysis

      const JQuaternion q(B);

      const double w  = q[3];
      const double x  = q[0];
      const double y  = q[1];
      const double z  = q[2];

      const double xx = x*x;
      const double yy = y*y;
      const double zz = z*z;
      const double xz = x*z;
      const double xy = x*y;
      const double yz = y*z;
      const double wx = w*x;
      const double wy = w*y;
      const double wz = w*z;

      V.a00 = 1.0 - 2.0*(yy + zz);  V.a01 = 2.0*(xy - wz);        V.a02 = 2.0*(xz + wy);
      V.a10 = 2.0*(xy + wz);        V.a11 = 1.0 - 2.0*(xx + zz);  V.a12 = 2.0*(yz - wx);
      V.a20 = 2.0*(xz - wy);        V.a21 = 2.0*(yz + wx);        V.a22 = 1.0 - 2.0*(xx + yy);


      B.mul(A, V);


      // sort singular values.

      double rho1 = getLengthSquared(B.a00, B.a10, B.a20);
      double rho2 = getLengthSquared(B.a01, B.a11, B.a21);
      double rho3 = getLengthSquared(B.a02, B.a12, B.a22);

      if (rho1 < rho2) {
	swap(B.a00,B.a01);   B.a01 = -B.a01;   swap(V.a00,V.a01);   V.a01 = -V.a01;
	swap(B.a10,B.a11);   B.a11 = -B.a11;   swap(V.a10,V.a11);   V.a11 = -V.a11;
	swap(B.a20,B.a21);   B.a21 = -B.a21;   swap(V.a20,V.a21);   V.a21 = -V.a21;
	swap(rho1,rho2);  
      }

      if (rho1 < rho3) {
	swap(B.a00,B.a02);   B.a02 = -B.a02;   swap(V.a00,V.a02);   V.a02 = -V.a02;
	swap(B.a10,B.a12);   B.a12 = -B.a12;   swap(V.a10,V.a12);   V.a12 = -V.a12;
	swap(B.a20,B.a22);   B.a22 = -B.a22;   swap(V.a20,V.a22);   V.a22 = -V.a22;
	swap(rho1,rho3);  
      }

      if (rho2 < rho3) {
	swap(B.a01,B.a02);   B.a02 = -B.a02;   swap(V.a01,V.a02);   V.a02 = -V.a02;
	swap(B.a11,B.a12);   B.a12 = -B.a12;   swap(V.a11,V.a12);   V.a12 = -V.a12;
	swap(B.a21,B.a22);   B.a22 = -B.a22;   swap(V.a21,V.a22);   V.a22 = -V.a22;
      }


      // QR decomposition

      JMatrix3D& Q = U;
      JMatrix3D& R = S;

      double a, b;
    
      const JGivens q1(B.a00, B.a10);      // 1st Givens rotation (ch,0,0,sh)

      a = q1.geta();
      b = q1.getb();

      // apply B = Q' * B

      R.a00 =  a*B.a00 + b*B.a10;  R.a01 =  a*B.a01 + b*B.a11;  R.a02 =  a*B.a02 + b*B.a12;
      R.a10 = -b*B.a00 + a*B.a10;  R.a11 = -b*B.a01 + a*B.a11;  R.a12 = -b*B.a02 + a*B.a12;
      R.a20 =  B.a20;              R.a21 =  B.a21;              R.a22 =  B.a22;
  
      const JGivens q2(R.a00, R.a20);      // 2nd Givens rotation (ch,0,-sh,0)

      a = q2.geta();
      b = q2.getb();

      // apply B = Q' * B;

      B.a00 =  a*R.a00 + b*R.a20;  B.a01 =  a*R.a01 + b*R.a21;  B.a02 =  a*R.a02 + b*R.a22;
      B.a10 =  R.a10;              B.a11 =  R.a11;              B.a12 =  R.a12;
      B.a20 = -b*R.a00 + a*R.a20;  B.a21 = -b*R.a01 + a*R.a21;  B.a22 = -b*R.a02 + a*R.a22;

      const JGivens q3(B.a11, B.a21);      // 3rd Givens rotation (ch,sh,0,0)

      a = q3.geta();
      b = q3.getb();

      // R is now set to desired value

      R.a00 =  B.a00;              R.a01 =  B.a01;              R.a02 =  B.a02;
      R.a10 =  a*B.a10+b*B.a20;    R.a11 =  a*B.a11 + b*B.a21;  R.a12 =  a*B.a12 + b*B.a22;
      R.a20 = -b*B.a10+a*B.a20;    R.a21 = -b*B.a11 + a*B.a21;  R.a22 = -b*B.a12 + a*B.a22;

      // Q = Q1 * Q2 * Q3

      const double sp1 = -1.0 + 2.0*q1.sh*q1.sh;
      const double sp2 = -1.0 + 2.0*q2.sh*q2.sh;
      const double sp3 = -1.0 + 2.0*q3.sh*q3.sh;

      Q.a00 =  sp1*sp2; 
      Q.a01 =  4.0*q2.ch*q3.ch*sp1*q2.sh*q3.sh  +  2.0*q1.ch*q1.sh*sp3; 
      Q.a02 =  4.0*q1.ch*q3.ch*q1.sh*q3.sh      -  2.0*q2.ch*sp1*q2.sh*sp3;

      Q.a10 = -2.0*q1.ch*q1.sh*sp2;
      Q.a11 = -8.0*q1.ch*q2.ch*q3.ch*q1.sh*q2.sh*q3.sh  +  sp1*sp3; 
      Q.a12 = -2.0*q3.ch*q3.sh                          +  4.0*q1.sh*(q3.ch*q1.sh*q3.sh + q1.ch*q2.ch*q2.sh*sp3);
    
      Q.a20 =  2.0*q2.ch*q2.sh; 
      Q.a21 = -2.0*q3.ch*sp2*q3.sh; 
      Q.a22 =  sp2*sp3;
    }


    /**
     * Get inverted matrix.
     *
     * \param  precision   precision
     * \return             inverted matrix
     */
    const JMatrix3D& invert(const double precision = 1.0e-12) const
    {
      double w = fabs(S.a00);

      if (fabs(S.a11) > w) { w = fabs(S.a11); }
      if (fabs(S.a22) > w) { w = fabs(S.a22); }

      w *= precision;

      const double s00 = (fabs(S.a00) >= w ? 1.0 / S.a00 : 0.0);
      const double s11 = (fabs(S.a11) >= w ? 1.0 / S.a11 : 0.0);
      const double s22 = (fabs(S.a22) >= w ? 1.0 / S.a22 : 0.0);

      // B = V * S^-1

      B.a00 = V.a00*s00;  B.a01 = V.a01*s11;  B.a02 = V.a02*s22;
      B.a10 = V.a10*s00;  B.a11 = V.a11*s11;  B.a12 = V.a12*s22;
      B.a20 = V.a20*s00;  B.a21 = V.a21*s11;  B.a22 = V.a22*s22;

      U.transpose();

      // B = V * S^-1 * U^T

      B.mul(U);

      U.transpose();

      return B;
    }

    mutable JMatrix3D U;
    mutable JMatrix3D S;
    mutable JMatrix3D V;

  private:

    mutable JMatrix3S B;     // work space

    /**
     * Auxiliary class for quaternion computation.
     */
    struct JQuaternion :
      public std::array<double, 4>
    {
      /**
       * Constructor.
       *
       * Finds transformation that diagonalizes the given symmetric matrix by using Jacobi eigen analysis.
       *
       * \param  S                 matrix
       */
      JQuaternion(JMatrix3S& S)
      {
	// follow same indexing convention as GLM

	(*this)[3] = 1.0; 
	(*this)[0] = 0.0; 
	(*this)[1] = 0.0; 
	(*this)[2] = 0.0;

	for (int i = 0; i != 4; ++i) {

	  // eliminate maximum off-diagonal element on every iteration,
	  // while cycling over all 3 possible rotations in fixed order.
	  // (p,q) = (0,1), (1,2), (0,2) still retains asymptotic convergence.

	  conjugate(0, 1, 2, S);  // (p,q) = (0,1)
	  conjugate(1, 2, 0, S);  // (p,q) = (1,2)
	  conjugate(2, 0, 1, S);  // (p,q) = (0,2)
	}
      }

    private:

      /**
       * Get conjugate of symmetric matrix for given order.
       *
       * \param  x                 1st index
       * \param  y                 2nd index
       * \param  z                 3rd index
       * \param  S                 matrix
       */
      inline void conjugate(const int  x, 
			    const int  y,
			    const int  z,
			    JMatrix3S& S)
      {
	static const double GAMMA  =  sqrt(8.0) + 3.0;
	static const double CSTAR  =  cos(PI/8.0);
	static const double SSTAR  =  sin(PI/8.0);

	// approximate Givens quaternion

	double ch = 2.0*(S.a00 - S.a11);
	double sh = S.a10;

	if (GAMMA*sh*sh < ch*ch) {

	  const double w = 1.0 / sqrt(ch*ch+sh*sh);

	  ch *= w;
	  sh *= w;

	} else {

	  ch  = CSTAR;
	  sh  = SSTAR;
	}

	const double scale =  ch*ch + sh*sh;
	const double a     = (ch+sh)*(ch-sh) / scale;
	const double b     = (2.0*sh*ch)     / scale;

	// make temporary copy of S

	double s00 = S.a00;
	double s10 = S.a10;  double s11 = S.a11;
	double s20 = S.a20;  double s21 = S.a21;  double s22 = S.a22;

	// perform conjugation S = Q' * S * Q
	// where Q is already implicitly solved from a and b

	S.a00 =  a*( a*s00 + b*s10) + b*( a*s10 + b*s11);
	S.a10 =  a*(-b*s00 + a*s10) + b*(-b*s10 + a*s11);
	S.a11 = -b*(-b*s00 + a*s10) + a*(-b*s10 + a*s11);
	S.a20 =  a*s20 + b*s21;
	S.a21 = -b*s20 + a*s21;
	S.a22 =  s22;

	// update cumulative rotation

	double tmp[] = {
	  (*this)[0]*sh,
	  (*this)[1]*sh,
	  (*this)[2]*sh
	};

	sh *= (*this)[3];

	(*this)[0] *= ch;
	(*this)[1] *= ch;
	(*this)[2] *= ch;
	(*this)[3] *= ch;

	// (x,y,z) corresponds to {(0,1,2), (1,2,0), (2,0,1)}
	// for (p,q) = {(0,1), (1,2), (0,2)}, respectively.

	(*this)[z] += sh;
	(*this)[3] -= tmp[z]; // w
	(*this)[x] += tmp[y];
	(*this)[y] -= tmp[x];

	// re-arrange matrix for next iteration

	s00   = S.a11;
	s10   = S.a21; s11   = S.a22;
	s20   = S.a10; s21   = S.a20; s22   = S.a00;

	S.a00 = s00;
	S.a10 = s10;   S.a11 = s11;
	S.a20 = s20;   S.a21 = s21;   S.a22 = s22;
      }
    };


    /**
     * Get length squared.
     *
     * \param  x             x
     * \param  y             y
     * \param  z             z
     * \return               square of length
     */
    static inline double getLengthSquared(const double x, const double y, const double z)
    {
      return x*x + y*y + z*z;
    }


    /**
     * Givens quaternion.
     */
    struct JGivens {

      static constexpr double EPSILON  =  1.0e-6;

      /**
       * Constructor.
       *
       * \param  a            pivot point on diagonal
       * \param  b            lower triangular entry we want to annihilate
       */
      JGivens(const double a,
	      const double b)
      {
	using namespace std;

	const double rho = sqrt(a*a + b*b);

	sh = rho > EPSILON ? b : 0.0;
	ch = fabs(a) + max(rho,EPSILON);

	if (a < 0.0) {
	  swap(sh,ch);
	}

	const double w = 1.0 / sqrt(ch*ch + sh*sh);

	ch *= w;
	sh *= w;
      }

      inline double geta() const { return 1.0 - 2.0*sh*sh; }  //!< get new a value
      inline double getb() const { return 2.0*ch*sh; }        //!< get new b value

      double ch;    //!< cosine rotation angle
      double sh;    //!< sine   rotation angle
    };
  };
}

#endif