/* 

for documentation see eigen.h 


Author:      Raimundo Sierra
             www.rsierra.com

Copyright:   Copyright (c) 2000 Raimundo Sierra. All rights reserved.
LICENSE:     This program is free software; you can redistribute it and/or modify
             it under the terms of the GNU General Public License as published by
             the Free Software Foundation; either version 2 of the License, or
             (at your option) any later version.

             This program is distributed in the hope that it will be useful,
             but WITHOUT ANY WARRANTY; without even the implied warranty of
             MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
             GNU General Public License for more details.

             You should have received a copy of the GNU General Public License
             along with this program; if not, write to the Free Software
             Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/

#include "eigen.h"


//constructors
eigen::eigen()
{
  _lambda1 = 0; // this defines an eigensystem as to be zero since abs() of the largest eigenvalue is zero;  
  /*_lambda2 = _lambda3 = 0;
  for(int i=0; i<3; i++){
    _vector1[i] = _vector2[i] = _vector3[i] = 0.0;
  }
  */
}

eigen::eigen(const float eig1, const float eig2, const float eig3, const float *vec1, const float *vec2, const float *vec3)
{
  if( eig1==0.0){
    _lambda1 = 0;
    return;
  }
  _lambda1 = eig1;
  _lambda2 = eig2;
  _lambda3 = eig3;
  if(_lambda1!=0 || _lambda2!=0 || _lambda3!=0){
    for(int i=0; i<3; i++){
      _vector1[i] = vec1[i];
      _vector2[i] = vec2[i];
      _vector3[i] = vec3[i];
    }
  }
}

eigen::~eigen()
{
  //delete[] _vector1;
  //delete[] _vector2;
  //delete[] _vector3; 
}

eigen::eigen(const eigen &sys)
{
  if(sys._lambda1==0.0){
    _lambda1 = 0;
    return;
  }
  _lambda1 = sys._lambda1;
  _lambda2 = sys._lambda2;
  _lambda3 = sys._lambda3;
  for(int i=0; i<3; i++){
    _vector1[i] = sys._vector1[i];
    _vector2[i] = sys._vector2[i];
    _vector3[i] = sys._vector3[i];  
  }
}
  
void
eigen::set(const float eig1, const float eig2, const float eig3, const float *vec1, const float *vec2, const float *vec3)
{ 
  if( eig1==0){
    _lambda1 = 0;
    return;
  }
  _lambda1 = eig1;
  _lambda2 = eig2;
  _lambda3 = eig3;
  for(int i=0; i<3; i++){
    _vector1[i] = vec1[i];
    _vector2[i] = vec2[i];
    _vector3[i] = vec3[i];
  }
}

void
eigen::set(const float eig1, const float eig2, const float eig3)
{ 
  if( eig1==0){
    _lambda1 = 0;
    return;
  }
  _lambda1 = eig1;
  _lambda2 = eig2;
  _lambda3 = eig3;
}
 
void
eigen::setVectors(const float *vec1, const float *vec2, const float *vec3)
{ 
  for(int i=0; i<3; i++){
    _vector1[i] = vec1[i];
    _vector2[i] = vec2[i];
    _vector3[i] = vec3[i];
  }
}

void 
eigen::setVector2(const float x, const float y, const float z)
{
  _vector2[0] = x;
  _vector2[1] = y;
  _vector2[2] = z;
}
  
void 
eigen::resetVector3()
{
  /*
    e1 x e2:
    
    e1x     e2x      e1y*e2z - e1z*e2y
    e1y  x  e2y   = -e1x*e2z + e1z*e2x
    e1z     e2z      e1x*e2y - e1y*e2x

  */
  float x =  _vector1[1]*_vector2[2] - _vector1[2]*_vector2[1];
  float y = -_vector1[0]*_vector2[2] + _vector1[2]*_vector2[0];
  float z =  _vector1[0]*_vector2[1] - _vector1[1]*_vector2[0];

  float distance1 = (_vector3[0]-x)*(_vector3[0]-x)+
                    (_vector3[1]-y)*(_vector3[1]-y)+
                    (_vector3[2]-z)*(_vector3[2]-z);
  float distance2 = (_vector3[0]+x)*(_vector3[0]+x)+
                    (_vector3[1]+y)*(_vector3[1]+y)+
                    (_vector3[2]+z)*(_vector3[2]+z);

  if(distance2<distance1){
    x = -x;
    y = -y;
    z = -z;
  }
  _vector3[0] = x;
  _vector3[1] = y;
  _vector3[2] = z;

}

void
eigen::setZero()
{
  _lambda1 = 0;  
}

void 
eigen::eigen2tensor(tensor &t) const
{
  if(this->isZero()){
    t.setZero();
    return;
  }
  tensor x(_vector1[0], _vector2[0], _vector3[0], _vector1[1], _vector2[1], _vector3[1], _vector1[2], _vector2[2], _vector3[2]);
  tensor y(_lambda1, 0, 0, 0, _lambda2, 0, 0, 0, _lambda3);
  tensor x_inverse;
  x.inv(x_inverse);
  t = x*y*x_inverse;
}

int
eigen::tensor2eigen(const tensor &t)
{
  return t.eig(_lambda1, _lambda2, _lambda3, _vector1, _vector2, _vector3, 1);
}

float 
eigen::value1() const
{
  return _lambda1;
}

float 
eigen::value2() const
{
  if(this->isZero()) return 0;
  return _lambda2;
}
  
float 
eigen::value3() const
{
  if(this->isZero()) return 0;
  return _lambda3;
}

float 
eigen::vector1(const int index) const
{
  if(this->isZero()) return 0;
  if(index<3 && index>=0){
    return _vector1[index];
  } 
  else return 0;
}

float 
eigen::vector2(const int index) const
{
  if(this->isZero()) return 0;
  if(index<3 && index>=0){
    return _vector2[index];
  } 
  else return 0;
}

float 
eigen::vector3(const int index) const
{
  if(this->isZero()) return 0;
  if(index<3 && index>=0){
    return _vector3[index];
  } 
  else return 0;
}
  
float 
eigen::close_line() const
{
  if(this->isZero()) return 0;
  return (_lambda1-_lambda2)/_lambda1;
}

float 
eigen::close_plane() const
{
  if(this->isZero()) return 0;
  return (_lambda2-_lambda3)/_lambda1;
}

float 
eigen::close_sphere() const
{
  if(this->isZero()) return 0;
  return _lambda3/_lambda1;
}

float 
eigen::anisotropy() const
{
  if(this->isZero()) return 0;
  return (1 - (_lambda3/_lambda1));
}

void 
eigen::print() const
{  
  if(this->isZero()){
    cout << "Eigenvalues and corresponding eigenvectors:\n";
    cout << "Eigenvalues: \t0 ;\t 0 ;\t 0\n";
    return;
  }      
  cout << "Eigenvalues and corresponding eigenvectors:\n";
  cout << "Eigenvalues: \t"<< _lambda1 << " ;\t " << _lambda2 << " ;\t " << _lambda3 << endl;
  cout << "Eigenvectors:\n";
  for(int i=0; i<3; i++){
    cout << "\t\t"<<_vector1[i] << "  \t " <<_vector2[i]<< "  \t "<< _vector3[i]<< endl;
  }
}

eigen& 
eigen::operator= (const eigen &e)
{
  if(this != &e){
    _lambda1=e._lambda1;
    if(!e.isZero()){ // if isZero then i'm done since vectors have no meaning
      _lambda2=e._lambda2;
      _lambda3=e._lambda3;
      for(int i=0; i<3; i++){
	_vector1[i] = e._vector1[i];
	_vector2[i] = e._vector2[i];
	_vector3[i] = e._vector3[i];
      }
    }
  }
  return *this;
}
  
bool 
eigen::isZero() const
{
  if(_lambda1==0) return true;
  else return false;
}

bool 
eigen::operator==(const eigen &e) const
{
  if(_lambda1!=e._lambda1 || _lambda2!=e._lambda2 || _lambda3!=e._lambda3){
    return false;
  }
  if(e.isZero()) return true; // both are zero; then vectors have no meaning
  for(int i=0; i<3; i++){
    if(_vector1[i]!=e._vector1[i] || _vector2[i]!=e._vector2[i] || _vector3[i]!=e._vector3[i]){
      return false;
    }
  }
  return true;
}
  
bool 
eigen::operator!=(const eigen &e) const
{
  if(e.isZero() && this->isZero()) return false; // both are zero; then vectors have no meaning
  if(_lambda1!=e._lambda1 || _lambda2!=e._lambda2 || _lambda3!=e._lambda3){
    return true;
  }
  // all lambdas are equal, compare vectors now
  for(int i=0; i<3; i++){
    if(_vector1[i]!=e._vector1[i] || _vector2[i]!=e._vector2[i] || _vector3[i]!=e._vector3[i]){
      return true;
    }
  }  
  return false;
}
  
bool
eigen::operator< (const eigen &e) const
{
  if(this->measure() < e.measure()) return true;
  return false;
}
 
bool 
eigen::operator> (const eigen &e) const
{
  if(this->measure() > e.measure()) return true;
  return false;
}
 
bool 
eigen::operator<=(const eigen &e) const
{
  if(this->measure() <= e.measure()) return true; 
  return false;
}
 
bool 
eigen::operator>=(const eigen &e) const
{
  if(this->measure() >= e.measure()) return true;  
  return false;
}
  
void 
eigen::scale(const float factor){
  if(!this->isZero()){
    _lambda1 = _lambda1*factor;
    _lambda2 = _lambda2*factor;
    _lambda3 = _lambda3*factor;
  }
}

void 
eigen::scale(const float x, const float y, const float z)
{
  _lambda1 = _lambda1 * sqrt( (_vector1[0]*x)*(_vector1[0]*x) + 
			      (_vector1[1]*y)*(_vector1[1]*y) + 
			      (_vector1[2]*z)*(_vector1[2]*z));
  _lambda2 = _lambda2 * sqrt( (_vector2[0]*x)*(_vector2[0]*x) + 
			      (_vector2[1]*y)*(_vector2[1]*y) + 
			      (_vector2[2]*z)*(_vector2[2]*z));
  _lambda3 = _lambda2 * sqrt( (_vector3[0]*x)*(_vector3[0]*x) + 
			      (_vector3[1]*y)*(_vector3[1]*y) + 
			      (_vector3[2]*z)*(_vector3[2]*z));
}
                                        
void 
eigen::rotate(const float pitch, const float roll, const float yaw)
{
  double radPitch = double(pitch)*GRAD2RAD;
  double radRoll  = double(roll )*GRAD2RAD;
  double radYaw   = double(yaw  )*GRAD2RAD;

  double cosYaw   = cos(radYaw);
  double sinYaw   = sin(radYaw);

  double cosRoll  = cos(radRoll);
  double sinRoll  = sin(radRoll);
  
  double cosPitch = cos(radPitch);
  double sinPitch = sin(radPitch);

  float ev1a[3], ev2a[3], ev3a[3];
  float ev1b[3], ev2b[3], ev3b[3];
  float ev1c[3], ev2c[3], ev3c[3];

  // rotate x-axis
  
  ev1a[0] = _vector1[0]                                                             ;
  ev1a[1] =                       + cosPitch* _vector1[1]   + sinPitch* _vector1[2] ;
  ev1a[2] =                       - sinPitch* _vector1[1]   + cosPitch* _vector1[2] ;

  ev2a[0] = _vector2[0]                                                             ;
  ev2a[1] =                       + cosPitch* _vector2[1]   + sinPitch* _vector2[2] ;
  ev2a[2] =                       - sinPitch* _vector2[1]   + cosPitch* _vector2[2] ;

  ev3a[0] = _vector3[0]                                                             ;
  ev3a[1] =                       + cosPitch* _vector3[1]   + sinPitch* _vector3[2] ;
  ev3a[2] =                       - sinPitch* _vector3[1]   + cosPitch* _vector3[2] ;
  /*
  ev1a[0] = cosYaw  * _vector1[0] + sinYaw * _vector1[1]                        ;
  ev1a[1] = -sinYaw * _vector1[0] + cosYaw * _vector1[1]                        ;
  ev1a[2] =                                                 _vector1[2]         ;
  
  ev2a[0] = cosYaw  * _vector2[0] + sinYaw * _vector2[1]                        ;
  ev2a[1] = -sinYaw * _vector2[0] + cosYaw * _vector2[1]                        ;
  ev2a[2] =                                                 _vector2[2]         ;
  
  ev3a[0] = cosYaw  * _vector3[0] + sinYaw * _vector3[1]                        ;
  ev3a[1] = -sinYaw * _vector3[0] + cosYaw * _vector3[1]                        ;
  ev3a[2] =                                                 _vector3[2]         ;
  */
  // rotate y-axis
  ev1b[0] = cosRoll * ev1a[0]                               - sinRoll * ev1a[2] ;
  ev1b[1] =                         ev1a[1]                                     ;
  ev1b[2] = sinRoll * ev1a[0]                               + cosRoll * ev1a[2] ;

  ev2b[0] = cosRoll * ev2a[0]                               - sinRoll * ev2a[2] ;
  ev2b[1] =                         ev2a[1]                                     ;
  ev2b[2] = sinRoll * ev2a[0]                               + cosRoll * ev2a[2] ;

  ev3b[0] = cosRoll * ev3a[0]                               - sinRoll * ev3a[2] ;
  ev3b[1] =                         ev3a[1]                                     ;
  ev3b[2] = sinRoll * ev3a[0]                               + cosRoll * ev3a[2] ;

  // rotate z-axis
  
  ev1c[0] = cosYaw  * ev1b[0]     + sinYaw * ev1b[1]                            ;
  ev1c[1] = -sinYaw * ev1b[0]     + cosYaw * ev1b[1]                            ;
  ev1c[2] =                                                   ev1b[2]           ;
  
  ev2c[0] = cosYaw  * ev2b[0]     + sinYaw * ev2b[1]                            ;
  ev2c[1] = -sinYaw * ev2b[0]     + cosYaw * ev2b[1]                            ;
  ev2c[2] =                                                   ev2b[2]           ;
  
  ev3c[0] = cosYaw  * ev3b[0]     + sinYaw * ev3b[1]                            ;
  ev3c[1] = -sinYaw * ev3b[0]     + cosYaw * ev3b[1]                            ;
  ev3c[2] =                                                   ev3b[2]           ;
  /*
  ev1c[0] = ev1b[0]                                                             ;
  ev1c[1] =                       + cosPitch* ev1b[1]       + sinPitch* ev1b[2] ;
  ev1c[2] =                       - sinPitch* ev1b[1]       + cosPitch* ev1b[2] ;

  ev2c[0] = ev2b[0]                                                             ;
  ev2c[1] =                       + cosPitch* ev2b[1]       + sinPitch* ev2b[2] ;
  ev2c[2] =                       - sinPitch* ev2b[1]       + cosPitch* ev2b[2] ;

  ev3c[0] = ev3b[0]                                                             ;
  ev3c[1] =                       + cosPitch* ev3b[1]       + sinPitch* ev3b[2] ;
  ev3c[2] =                       - sinPitch* ev3b[1]       + cosPitch* ev3b[2] ;
  */
  for(int i=0; i<3; i++){
    _vector1[i] = ev1c[i];
    _vector2[i] = ev2c[i];
    _vector3[i] = ev3c[i];
  }
}
   
float 
eigen::diffusion_x() const
{
  if(this->isZero()) return 0;
  return _lambda1*_vector1[0] + _lambda2*_vector2[0] + _lambda3*_vector3[0];
}

float 
eigen::diffusion_y() const
{
  if(this->isZero()) return 0;
  return _lambda1*_vector1[1] + _lambda2*_vector2[1] + _lambda3*_vector3[1];
}

float 
eigen::diffusion_z() const
{
  if(this->isZero()) return 0;
  return _lambda1*_vector1[2] + _lambda2*_vector2[2] + _lambda3*_vector3[2];
}
 
float 
eigen::magnitude() const
{
  //if(this->isZero()) return 0;
  float tmp = _lambda1 * _lambda2 * _lambda3;
  if(tmp<0) return -tmp;
  return tmp;
  //return this->diffusion_x()*this->diffusion_y()*this->diffusion_z();
}
   
float 
eigen::det() const
{
  return _lambda1 * _lambda2 * _lambda3;
}

float 
eigen::relation_Lambda1_Lambda2() const
{
  float tmp = _lambda1 / _lambda2;
  if(tmp<0) return -tmp;
  return tmp;
}

float 
eigen::relation_Lambda1_Lambda3() const
{
  float tmp = _lambda1 / _lambda3;
  if(tmp<0) return -tmp;
  return tmp;
}

float 
eigen::cosinMaxEigen(const eigen &e) const
{
  if(this->isZero() || e.isZero()){
    return 1;  // cos(0) is 1!
  }
  /* removed since tmp should always be ==1 (eigenvectors are length 1)
  double tmp;
  tmp = sqrt(_vector1[0]*_vector1[0] + _vector1[1]*_vector1[1] + _vector1[2]*_vector1[2]);
  tmp = tmp*sqrt(e._vector1[0]*e._vector1[0] + e._vector1[1]*e._vector1[1] + e._vector1[2]*e._vector1[2]);
  if(tmp==0){
    cerr <<"WARNING: dividing by 0 in cosinMaxEigen() in eigen.h\nExit\n";
    exit(0);
  }
  tmp = _vector1[0]*e._vector1[0] + _vector1[1]*e._vector1[1] + _vector1[2]*e._vector1[2]/tmp;
  return float(tmp);
  */
  return _vector1[0]*e._vector1[0] + _vector1[1]*e._vector1[1] + _vector1[2]*e._vector1[2];
}

bool 
eigen::isSimilar(const eigen &e, const float tolerance, const int similarity) const
{
  if(this->isZero() || e.isZero()) return 0;
  switch (similarity) {
  case 0: return this->hasSimilarShape(    e, tolerance); break;
  case 1: return this->hasSimilarSize(     e, tolerance); break;
  case 2: return this->hasSimilarDirection(e, tolerance); break;
  case 3: if(    this->hasSimilarShape(    e, tolerance) && 
		 this->hasSimilarDirection(e, tolerance)) return 1; break;
  case 4: if(    this->hasSimilarShape(    e, tolerance) &&
		 this->hasSimilarSize(     e, tolerance) &&
		 this->hasSimilarDirection(e, tolerance)) return 1; break;
  default: cout << "Similarity not implemented in eigen.h isSimilar()\n"; return 0; break;
  }
  return 0;
}

bool 
eigen::hasSimilarShape(const eigen &e, const float tolerance) const
{
  if(this->isZero() || e.isZero()) return 0;

  float smallfactor = 1-tolerance;
  float bigfactor   = 1+tolerance;
  
  if(this->relation_Lambda1_Lambda2() >= smallfactor*e.relation_Lambda1_Lambda2() && 
     this->relation_Lambda1_Lambda2() <= bigfactor  *e.relation_Lambda1_Lambda2() && 
     this->relation_Lambda1_Lambda3() >= smallfactor*e.relation_Lambda1_Lambda2() && 
     this->relation_Lambda1_Lambda3() <= bigfactor  *e.relation_Lambda1_Lambda2() ) return 1;
  return 0;
}

bool 
eigen::hasSimilarSize(const eigen &e, const float tolerance) const
{
  if(this->isZero() || e.isZero()) return 0;

  float smallfactor = 1-tolerance;
  float bigfactor   = 1+tolerance;

  if(this->magnitude() >= smallfactor*e.magnitude() &&
     this->magnitude() <= bigfactor  *e.magnitude()) return 1;
  return 0;
}
  
bool 
eigen::hasSimilarDirection(const eigen &e, const float tolerance) const
{
  if(this->isZero() || e.isZero()) return 0;
  float tmp = this->cosinMaxEigen(e);
  if(tmp<0) tmp = -tmp;
  if(tmp > 1-tolerance) return 1;
  return 0;
}

bool 
eigen::threshold(const float threshold=0.8) const
{
  if(this->isZero()) return false; 
  if(this->close_line()>threshold)  return true;
  if(this->close_plane()>threshold) return true;
  return false;
}

ostream &operator << (ostream &out, const eigen &sys)
{
  sys.print();
  return out;
}