#ifndef __JMATRIX3S__
#define __JMATRIX3S__

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

#include "JLang/JException.hh"

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


/**
 * \author mdejong
 */

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

namespace JMATH {

  using JLANG::JDivisionByZero;


  /**
   * 3 x 3 symmetric matrix
   */
  class JMatrix3S :
    public JMatrix3D
  {
  public:
    /**
     * Type definition of Eigen vakues.
     */
    typedef std::array<double, 3>  eigen_values;


    /**
     * Default constructor.
     */
    JMatrix3S() :
      JMatrix3D()
    {}


    /**
     * Contructor.
     *
     * \param   A      matrix
     */
    JMatrix3S(const JMatrix3D& A) :
      JMatrix3D(A)
    {}


    /**
     * Contructor.\n
     * The upper triangle is internally set.
     *
     * \param   __a00   (0,0)
     * \param   __a10   (1,0)
     * \param   __a11   (1,1)
     * \param   __a20   (2,0)
     * \param   __a21   (2,1)
     * \param   __a22   (2,2)
     */
    JMatrix3S(const double __a00,
	      const double __a10, const double __a11,
	      const double __a20, const double __a21, const double __a22) :
      JMatrix3D(__a00, __a10, __a20,
		__a10, __a11, __a21,
		__a20, __a21, __a22)
    {}

    
    /**
     * Invert matrix.
     */
    void invert()
    {
      using std::swap;

      // LDU decomposition

      int    p0 = 0; // permute row 0
      int    p1 = 0; // permute row 1

      double val;

      val = a00;

      if (fabs(a10) > fabs(val)) {
	p0  = 1;
	val = a10;
      }

      if (fabs(a20) > fabs(val)) {
	p0  = 2;
	val = a20;
      }

      switch (p0) {

      case 1:
	swap(a00,a10);
	swap(a01,a11);
	swap(a02,a12);
	break;

      case 2:
	swap(a00,a20);
	swap(a01,a21);
	swap(a02,a22);
	break;
      }

      if (val == 0) {
	throw JDivisionByZero("LDU decomposition zero pivot");	
      }

      a10 /= val;
      a11 -= a10 * a01;
      a12 -= a10 * a02;

      a20 /= val;
      a21 -= a20 * a01;
      a22 -= a20 * a02;

      val = a11;

      if (fabs(a21) > fabs(val)) {
	p1  = 2;
	val = a21;
      }

      switch (p1) {

      case 2:
	swap(a10,a20);
	swap(a11,a21);
	swap(a12,a22);
	break;
      }

      if (val == 0) {
	throw JDivisionByZero("LDU decomposition zero pivot");
      }

      a21 /= val;
      a22 -= a21 * a12;

      // invert D

      if (a22 == 0) {
	throw JDivisionByZero("D matrix not invertable");
      }

      a00  = 1.0 / a00;
      a11  = 1.0 / a11;
      a22  = 1.0 / a22;

      // invert U

      a01 *= -a00;
      a01 *=  a11;

      a02 *= -a00;
      a02 -=  a01 * a12;
      a02 *=  a22;

      a12 *= -a11;
      a12 *=  a22;

      // invert L

      a21  = -a21;
      a20  = -a20;
      a20 -=  a21 * a10;
      a10  = -a10;

      // U^-1 x L^-1

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

      switch (p1) {

      case 2:
	swap(a01,a02);
	swap(a11,a12);
	swap(a21,a22);
	break;
      }

      switch (p0) {

      case 1:
	swap(a00,a01);
	swap(a10,a11);
	swap(a20,a21);
	break;

      case 2:
	swap(a00,a02);
	swap(a10,a12);
	swap(a20,a22);
	break;
      }
    }


    /**
     * Get eigen values.\n
     * The eigen values sorted in decreasing order of absolute values.\n
     * Algorithm taken from "Eigenvalues of a symmetric 3x3 matrix"\n
     * by Oliver K. Smith; see <a href="https://dl.acm.org/doi/10.1145/355578.366316">reference</a>.
     *
     * \param  epsilon     precision 
     * \return             eigen values
     */
    eigen_values getEigenValues(const double epsilon = 1e-6) const
    {
      using namespace std;

      eigen_values eigenvalues;

      const double p1 = a01*a01 + a02*a02 + a12*a12;

      if (fabs(p1 - 1.0) > epsilon) {
      
	const double q  = (a00 + a11 + a22) / 3.0;
	const double p2 = (a00 - q)*(a00 - q) + (a11 - q)*(a11 - q) + (a22 - q)*(a22 - q) + 2*p1;
	const double p  = sqrt(p2 / 6.0);

	JMatrix3S B(*this);

	B.sub(q * JMatrix3D::getIdentity());
	B.div(p);

	const double r   = B.getDeterminant() / 2.0;
	const double phi = (r < -1.0 ? PI / 3.0 : (r >  1.0 ? 0.0 : acos(r) / 3.0));

	eigenvalues[0] = q + 2*p*cos(phi);
	eigenvalues[2] = q + 2*p*cos(phi + 2*PI/3.0);
	eigenvalues[1] = 3 * q - eigenvalues[0] - eigenvalues[2];

      } else {

	eigenvalues = eigen_values{ a00, a11, a12 };
      }

      // sort absolute values in descending order

      if (fabs(eigenvalues[0]) < fabs(eigenvalues[1])) {
	swap(eigenvalues[0], eigenvalues[1]);
      }

      if (fabs(eigenvalues[1]) < fabs(eigenvalues[2])) {

	swap(eigenvalues[1], eigenvalues[2]);

	if (fabs(eigenvalues[0]) < fabs(eigenvalues[1])) {
	  swap(eigenvalues[0], eigenvalues[1]);
	}
      }

      return eigenvalues;
    }
  };
}

#endif