// this program contains a routine to compute eigendecomposition of a 2D
// symmetric matrix.
// to test it on Unix, run "g++ -o eig2x2 eig2x2.cpp; ./eig2x2"
//
// Aaron Hertzmann, hertzman@cs.washington.edu

#include <stdio.h>
#include <math.h>
#include <float.h>
#include <assert.h>

// compute the eigenvalue decomposition of a symmetric 2x2
// matrix in the form A=[a b;b c], so that
//    A*v1   = v1   *lambda1
//    A*  v2 =   v2 *       lambda2
bool evdecomposesymm(double a, double b,double c,
		     double & lambda1, double & lambda2,
		     double & v1x, double & v1y, 
		     double & v2x, double & v2y)
{
  double disc = sqrt((a-c)*(a-c)+4*b*b)/2;

  lambda1 = (a+c)/2 + disc;
  lambda2 = (a+c)/2 - disc;

  if (fabs(lambda1) < FLT_EPSILON || fabs(lambda2) < FLT_EPSILON)
    {
      printf("\twarning: lambda1 = %f, lambda2 = %f\n",lambda1,lambda2);
      return false;
    }

  v1x = -b;  v1y = a-lambda1;
  v2x = -b;  v2y = a-lambda2;
  double v1mag = sqrt(v1x*v1x + v1y*v1y);  
  double v2mag = sqrt(v2x*v2x + v2y*v2y);
  v1x/= v1mag;  v1y/=v1mag;
  v2x/= v2mag;  v2y/=v2mag;

  /*
  // make sure we have eigenvectors (this may fail for badly-conditioned
  // matrices)
  assert( fabs( (a*v1x + b*v1y) - lambda1*v1x) < 1e-4);
  assert( fabs( (b*v1x + c*v1y) - lambda1*v1y) < 1e-4);
  
  assert( fabs( (a*v2x + b*v2y) - lambda2*v2x) < 1e-4);
  assert( fabs( (b*v2x + c*v2y) - lambda2*v2y) < 1e-4);
  */

  return true;
}


void printMatrix(double m_11,double m_21, double m_12, double m_22)
{
  printf("\t[%f %f]\n",m_11,m_12);
  printf("\t[%f %f]\n",m_21,m_22);
}


void matmult(const double a[], const double b[], double c[])
{
  c[0] = a[0]*b[0] + a[2]*b[1];
  c[1] = a[1]*b[0] + a[3]*b[1];
  c[2] = a[0]*b[2] + a[2]*b[3];
  c[3] = a[1]*b[2] + a[3]*b[3];
}


// invert a symmetric matrix in the form [a_in b_in; b_in c_in]
// return the result as [a_out b_out; b_out c_out]
bool invertSymm(double a_in, double b_in, double c_in,
		double & a_out, double & b_out, double & c_out)
{
  double lambda1, lambda2, v1x, v1y, v2x, v2y;

  bool result =  evdecomposesymm(a_in, b_in, c_in, lambda1, lambda2,
				 v1x, v1y, v2x, v2y);
  
  if (!result)
    {
      printf("matrix inverse failed\n");
      return false;
    }

  double vlam[4] = {v1x/lambda1, v1y/lambda1, v2x/lambda2, v2y/lambda2 };
  double vt[4] =  {v1x, v2x, v1y, v2y };

  double int1[4];

  matmult(vlam,vt,int1);
  
  a_out = int1[0];
  b_out = (int1[1] + int1[2])/2;
  c_out = int1[3];
}



// test the eigendecomposition
int main(int argc, char * argv[])
{
  double a= 5, b= 3, c =6;

  printf("Input matrix =\n");
  printMatrix(a,b,b,c);

  double lambda1, lambda2, v1x, v1y, v2x, v2y;

  bool result =  evdecomposesymm(a, b, c, lambda1, lambda2,
				 v1x, v1y, v2x, v2y);
  
  if (!result)
    {
      printf("eigendecomposition failed\n");
      return 0;
    }
  
  printf("eigenvalue 1 = %f, eigenvector 1 = [%f %f]\n",lambda1,v1x,v1y);  
  printf("eigenvalue 2 = %f, eigenvector 2 = [%f %f]\n",lambda2,v2x,v2y);


  double vlam[4] = {lambda1*v1x, lambda1*v1y, 
		    lambda2*v2x, lambda2*v2y };
  double vt[4] =  {v1x, v2x, v1y, v2y };

  double int1[4];

  matmult(vlam,vt,int1);

  printf("recomposed matrix (should be same as original matrix) =\n");
  printMatrix(int1[0], int1[1], int1[2], int1[3]);

  double a_out, b_out, c_out;
  
  invertSymm(a,b,c,a_out,b_out,c_out);

  printf("inverted matrix =\n");
  printMatrix(a_out, b_out, b_out, c_out);

}