#ifndef __JMATRIX5S__
#define __JMATRIX5S__

#include <cmath>
#include <algorithm>

#include "JMath/JMatrix5D.hh"
#include "JLang/JException.hh"


/**
 * \author mdejong
 */

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

namespace JMATH {

  using JLANG::JDivisionByZero;


  /**
   * 5 x 5 symmetric matrix
   */
  class JMatrix5S :
    public JMatrix5D
  {
  public:
    /**
     * Default constructor.
     */
    JMatrix5S() :
      JMatrix5D()
    {}


    /**
     * Contructor.
     *
     * \param   A      matrix
     */
    JMatrix5S(const JMatrix5D& A) :
      JMatrix5D(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)
     * \param   __a30   (3,0)
     * \param   __a31   (3,1)
     * \param   __a32   (3,2)
     * \param   __a33   (3,3)
     * \param   __a40   (4,0)
     * \param   __a41   (4,1)
     * \param   __a42   (4,2)
     * \param   __a43   (4,3)
     * \param   __a44   (4,4)
     */
    JMatrix5S(const double __a00,
	      const double __a10, const double __a11,
	      const double __a20, const double __a21, const double __a22,
	      const double __a30, const double __a31, const double __a32, const double __a33,
	      const double __a40, const double __a41, const double __a42, const double __a43, const double __a44) :
      JMatrix5D(__a00, __a10, __a20, __a30, __a40,
		__a10, __a11, __a21, __a31, __a41,
		__a20, __a21, __a22, __a32, __a42,
		__a30, __a31, __a32, __a33, __a43,
		__a40, __a41, __a42, __a43, __a44)
    {}


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

      if (isIdentity()) {
	return;
      }

      // LDU decomposition

      int    p0 = 0; // permute row 0
      int    p1 = 0; // permute row 1
      int    p2 = 0; // permute row 2
      int    p3 = 0; // permute row 3

      double val;

      val = a00;

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

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

      if (fabs(a30) > fabs(val)) {
	p0  = 3;
	val = a30;
      }

      if (fabs(a40) > fabs(val)) {
	p0  = 4;
	val = a40;
      }

      switch (p0) {

      case 1:
	swap(a00,a10);
	swap(a01,a11);
	swap(a02,a12);
	swap(a03,a13);
	swap(a04,a14);
	break;

      case 2:
	swap(a00,a20);
	swap(a01,a21);
	swap(a02,a22);
	swap(a03,a23);
	swap(a04,a24);
	break;

      case 3:
	swap(a00,a30);
	swap(a01,a31);
	swap(a02,a32);
	swap(a03,a33);
	swap(a04,a34);
	break;

      case 4:
	swap(a00,a40);
	swap(a01,a41);
	swap(a02,a42);
	swap(a03,a43);
	swap(a04,a44);
	break;
      }

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

      a10 /= val;
      a11 -= a10 * a01;
      a12 -= a10 * a02;
      a13 -= a10 * a03;
      a14 -= a10 * a04;

      a20 /= val;
      a21 -= a20 * a01;
      a22 -= a20 * a02;
      a23 -= a20 * a03;
      a24 -= a20 * a04;

      a30 /= val;
      a31 -= a30 * a01;
      a32 -= a30 * a02;
      a33 -= a30 * a03;
      a34 -= a30 * a04;

      a40 /= val;
      a41 -= a40 * a01;
      a42 -= a40 * a02;
      a43 -= a40 * a03;
      a44 -= a40 * a04;

      val = a11;

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

      if (fabs(a31) > fabs(val)) {
	p1  = 3;
	val = a31;
      }

      if (fabs(a41) > fabs(val)) {
	p1  = 4;
	val = a41;
      }

      switch (p1) {

      case 2:
	swap(a10,a20);
	swap(a11,a21);
	swap(a12,a22);
	swap(a13,a23);
	swap(a14,a24);
	break;

      case 3:
	swap(a10,a30);
	swap(a11,a31);
	swap(a12,a32);
	swap(a13,a33);
	swap(a14,a34);
	break;

      case 4:
	swap(a10,a40);
	swap(a11,a41);
	swap(a12,a42);
	swap(a13,a43);
	swap(a14,a44);
	break;
      }

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

      a21 /= val;
      a22 -= a21 * a12;
      a23 -= a21 * a13;
      a24 -= a21 * a14;

      a31 /= val;
      a32 -= a31 * a12;
      a33 -= a31 * a13;
      a34 -= a31 * a14;

      a41 /= val;
      a42 -= a41 * a12;
      a43 -= a41 * a13;
      a44 -= a41 * a14;

      val = a22;

      if (fabs(a32) > fabs(val)) {
	p2  = 3;
	val = a32;
      }

      if (fabs(a42) > fabs(val)) {
	p2  = 4;
	val = a42;
      }

      switch (p2) {

      case 3:
	swap(a20,a30);
	swap(a21,a31);
	swap(a22,a32);
	swap(a23,a33);
	swap(a24,a34);
	break;

      case 4:
	swap(a20,a40);
	swap(a21,a41);
	swap(a22,a42);
	swap(a23,a43);
	swap(a24,a44);
	break;
      }
      
      if (val == 0) {
        throw JDivisionByZero("LDU decomposition zero pivot");
      }

      a32 /= val;
      a33 -= a32 * a23;
      a34 -= a32 * a24;

      a42 /= val;
      a43 -= a42 * a23;
      a44 -= a42 * a24;

      val = a33;

      if (fabs(a43) > fabs(val)) {
	p3  = 4;
	val = a43;
      }

      switch (p3) {

      case 4:
	swap(a30,a40);
	swap(a31,a41);
	swap(a32,a42);
	swap(a33,a43);
	swap(a34,a44);
	break;
      }
      
      if (val == 0) {
        throw JDivisionByZero("LDU decomposition zero pivot");
      }

      a43 /= val;
      a44 -= a43 * a34;

      // invert D

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

      a00  = 1.0 / a00;
      a11  = 1.0 / a11;
      a22  = 1.0 / a22;
      a33  = 1.0 / a33;
      a44  = 1.0 / a44;

      // invert U

      a01 *= -a00;
      a01 *=  a11;

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

      a03 *= -a00;
      a03 -=  a01 * a13;
      a03 -=  a02 * a23;
      a03 *=  a33;

      a04 *= -a00;
      a04 -=  a01 * a14;
      a04 -=  a02 * a24;
      a04 -=  a03 * a34;
      a04 *=  a44;

      a12 *= -a11;
      a12 *=  a22;

      a13 *= -a11;
      a13 -=  a12 * a23;
      a13 *=  a33;

      a14 *= -a11;
      a14 -=  a12 * a24;
      a14 -=  a13 * a34;
      a14 *=  a44;

      a23 *= -a22;
      a23 *=  a33;

      a24 *= -a22;
      a24 -=  a23 * a34;
      a24 *=  a44;

      a34 *= -a33;
      a34 *=  a44;

      // invert L

      a43  = -a43;

      a42  = -a42;
      a42 -=  a43 * a32;

      a41  = -a41;
      a41 -=  a42 * a21;
      a41 -=  a43 * a31;

      a40  = -a40;
      a40 -=  a41 * a10;
      a40 -=  a42 * a20;
      a40 -=  a43 * a30;

      a32  = -a32;

      a31  = -a31;
      a31 -=  a32 * a21;

      a30  = -a30;
      a30 -=  a31 * a10;
      a30 -=  a32 * a20;

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

      // U^-1 x L^-1

      a00 += a01 * a10  +  a02 * a20  +  a03 * a30  +  a04 * a40;
      a01 += a02 * a21  +  a03 * a31  +  a04 * a41;
      a02 += a03 * a32  +  a04 * a42;
      a03 += a04 * a43;

      a10 *= a11;
      a10 += a12 * a20  +  a13 * a30  +  a14 * a40;
      a11 += a12 * a21  +  a13 * a31  +  a14 * a41;
      a12 += a13 * a32  +  a14 * a42;
      a13 += a14 * a43;

      a20 *= a22;
      a20 += a23 * a30  +  a24 * a40;
      a21 *= a22;
      a21 += a23 * a31  +  a24 * a41;
      a22 += a23 * a32  +  a24 * a42;
      a23 += a24 * a43;

      a30 *= a33;
      a30 += a34 * a40;
      a31 *= a33;
      a31 += a34 * a41;
      a32 *= a33;
      a32 += a34 * a42;
      a33 += a34 * a43;

      a40 *= a44;
      a41 *= a44;
      a42 *= a44;
      a43 *= a44;

      switch (p3) {

      case 4:
	swap(a03,a04);
	swap(a13,a14);
	swap(a23,a24);
	swap(a33,a34);
	swap(a43,a44);
	break;
      }

      switch (p2) {

      case 3:
	swap(a02,a03);
	swap(a12,a13);
	swap(a22,a23);
	swap(a32,a33);
	swap(a42,a43);
	break;

      case 4:
	swap(a02,a04);
	swap(a12,a14);
	swap(a22,a24);
	swap(a32,a34);
	swap(a42,a44);
	break; 
      }
      
      switch (p1) {

      case 2:
	swap(a01,a02);
	swap(a11,a12);
	swap(a21,a22);
	swap(a31,a32);
	swap(a41,a42);
	break;

      case 3:
	swap(a01,a03);
	swap(a11,a13);
	swap(a21,a23);
	swap(a31,a33);
	swap(a41,a43);
	break;

      case 4:
	swap(a01,a04);
	swap(a11,a14);
	swap(a21,a24);
	swap(a31,a34);
	swap(a41,a44);
	break;
      }
      
      switch (p0) {
	
      case 1:
	swap(a00,a01);
	swap(a10,a11);
	swap(a20,a21);
	swap(a30,a31);
	swap(a40,a41);
	break;

      case 2:
	swap(a00,a02);
	swap(a10,a12);
	swap(a20,a22);
	swap(a30,a32);
	swap(a40,a42);
	break;

      case 3:
	swap(a00,a03);
	swap(a10,a13);
	swap(a20,a23);
	swap(a30,a33);
	swap(a40,a43);
	break;

      case 4:
	swap(a00,a04);
	swap(a10,a14);
	swap(a20,a24);
	swap(a30,a34);
	swap(a40,a44);
	break;
      }
    }
  };
}

#endif