/* 

for documentation see tensor.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 "tensor.h"

#ifdef debug
  int tensorcounter=0;           // counts calls to constructors
  int tensordestructorcounter=0; // counst calls to destructors
#endif

//constructors

/* matrix is linearised (efficiency!)
 / t1, t2, t3 \  
 | t4, t5, t6 | 
 \ t7, t8, t9 /
 corresponds to
 /_t[0], _t[1], _t[2] \
 |_t[3], _t[4], _t[5] |
 \_t[6], _t[7], _t[8] /
 which formerly was
 /_t[0][0], _t[0][1], _t[0][2] \
 |_t[1][0], _t[1][1], _t[1][2] |
 \_t[2][0], _t[2][1], _t[2][2] /
*/

tensor::tensor()
{
  _t[0]= 0;
  _t[1]= 0;
  _t[2]= 0;
  _t[3]= 0;
  _t[4]= 0;
  _t[5]= 0;
  _t[6]= 0;
  _t[7]= 0;
  _t[8] = 0.0;
#ifdef debug
  tensorcounter++;
  if(tensorcounter%10000 == 0){
    //cout << "tensorcounter in tensor() is: "<<tensorcounter<<endl;
  }
#endif
}


tensor::tensor(const tensor &t)
{
  _t[0] = t._t[0];
  _t[1] = t._t[1];
  _t[2] = t._t[2];
  _t[3] = t._t[3];
  _t[4] = t._t[4];
  _t[5] = t._t[5];
  _t[6] = t._t[6];
  _t[7] = t._t[7];
  _t[8] = t._t[8];
#ifdef debug
  tensorcounter++;
  if(tensorcounter%10000 == 0){
    //cout << "tensorcounter in tensor(tensor) is: "<<tensorcounter<<endl;
  }
#endif
}

tensor::tensor(float t1, float t2, float t3, float t4, float t5, float t6)
{
  _t[0] = t1;
  _t[1] = t4;
  _t[2] = t5;
  _t[3] = t4;
  _t[4] = t2;
  _t[5] = t6;
  _t[6] = t5;
  _t[7] = t6;
  _t[8] = t3;
#ifdef debug
  tensorcounter++;
  if(tensorcounter%10000 == 0){
    cout << "tensorcounter in tensor(6*float) is: "<<tensorcounter<<endl;
  }
#endif
}

tensor::tensor(float t1, float t2, float t3, float t4, float t5, float t6, float t7, float t8, float t9)
{
  _t[0] = t1;
  _t[1] = t2;
  _t[2] = t3;
  _t[3] = t4;
  _t[4] = t5;
  _t[5] = t6;
  _t[6] = t7;
  _t[7] = t8;
  _t[8] = t9;
#ifdef debug
  tensorcounter++;
  if(tensorcounter%10000 == 0){
    //cout << "tensorcounter in tensor(9*float) is: "<<tensorcounter<<endl;
  }
#endif
}

//destructor
tensor::~tensor()
{
  //delete[] _t; // why do I get an error when including this line?
#ifdef debug
  tensordestructorcounter++;
  if(tensordestructorcounter%10000 == 0){
    cout << "tensordestructorcounter is: "<<tensordestructorcounter<<endl;
  }
#endif
}

//operator overloadings
tensor& 
tensor::operator= (const tensor &t)
{
  if(this != &t) { 
    _t[0] = t._t[0];
    _t[1] = t._t[1];
    _t[2] = t._t[2];
    _t[3] = t._t[3];
    _t[4] = t._t[4];
    _t[5] = t._t[5];
    _t[6] = t._t[6];
    _t[7] = t._t[7];
    _t[8] = t._t[8];
  }
  return *this;
}

bool 
tensor::isZero() const
{
  //double floatprecision = 0;
  if(_t[0]==0 &&
     _t[1]==0 && 
     _t[2]==0 && 
     _t[3]==0 && 
     _t[4]==0 && 
     _t[5]==0 && 
     _t[6]==0 && 
     _t[7]==0 && 
     _t[8]==0    ) return true;
  return false;
}

void
tensor::add (const tensor &a,  const tensor &b)
{
  _t[0] = b._t[0] + a._t[0];
  _t[1] = b._t[1] + a._t[1];
  _t[2] = b._t[2] + a._t[2];
  _t[3] = b._t[3] + a._t[3];
  _t[4] = b._t[4] + a._t[4];
  _t[5] = b._t[5] + a._t[5];
  _t[6] = b._t[6] + a._t[6];
  _t[7] = b._t[7] + a._t[7];
  _t[8] = b._t[8] + a._t[8];
}

void
tensor::add (const tensor &a, const float x)
{
  _t[0] = x + a._t[0];
  _t[1] = x + a._t[1];
  _t[2] = x + a._t[2];
  _t[3] = x + a._t[3];
  _t[4] = x + a._t[4];
  _t[5] = x + a._t[5];
  _t[6] = x + a._t[6];
  _t[7] = x + a._t[7];
  _t[8] = x + a._t[8];
}

void
tensor::multiply (const tensor &a, const tensor &b)
{
  for(int j=0; j<3; j++){
    for(int i=0; i<3; i++){
      _t[i+j*3] = float(double(a._t[j*3])  *double(b._t[i  ]) +
	                double(a._t[j*3+1])*double(b._t[i+3]) +
	                double(a._t[j*3+2])*double(b._t[i+6]) );
    }
  }
}


void 
tensor::multiply (const tensor &a, const float x)
{
  for(int i=0; i<9; i++){
    _t[i] = float(double(x)*double(a._t[i]));
  }
}

/* t.divide(a, b) means t = a/b */
void
tensor::divide ( const tensor &aa, const tensor &b)
{
  // comments are from matlab computation of t/u

  // -u8*u3*t1 + t2*u3*u7 + u8*u4*t0 - t2*u4*u6 + t1*u5*u6 - u7*u5*t0
  _t[0] = 
     aa._t[7]*aa._t[3]*b._t[2] + b._t[1]*aa._t[5]*aa._t[6] + aa._t[4]*b._t[0]*aa._t[8]
   - aa._t[7]*aa._t[5]*b._t[0] - b._t[1]*aa._t[3]*aa._t[8] - aa._t[4]*b._t[2]*aa._t[6];

  // u8*u0*t1 + u1*t2*u6 - u1*u8*t0 - t2*u0*u7 - u2*t1*u6 + u2*u7*t0
  _t[1] = 
     aa._t[1]*aa._t[6]*b._t[2] + aa._t[2]*aa._t[7]*b._t[0] + aa._t[0]*aa._t[8]*b._t[1] 
   - aa._t[1]*aa._t[8]*b._t[0] - aa._t[0]*aa._t[7]*b._t[2] - aa._t[6]*aa._t[2]*b._t[1]; 

  // u2*u3*t1 + u1*u5*t0 - u3*u1*t2 + u4*u0*t2 - u2*u4*t0 - u0*u5*t1
  _t[2] =  
     aa._t[1]*aa._t[5]*b._t[0] + aa._t[3]*aa._t[2]*b._t[1] + aa._t[0]*aa._t[4]*b._t[2] 
   - aa._t[1]*aa._t[3]*b._t[2] - aa._t[2]*aa._t[4]*b._t[0] - aa._t[5]*aa._t[0]*b._t[1];

  // t5*u3*u7 + u8*u4*t3 + t4*u5*u6 - u8*u3*t4 - u7*u5*t3 - t5*u4*u6 
  _t[3] =  
     aa._t[7]*aa._t[3]*b._t[5] + aa._t[6]*aa._t[5]*b._t[4] + aa._t[4]*aa._t[8]*b._t[3] 
   - aa._t[7]*aa._t[5]*b._t[3] - aa._t[3]*aa._t[8]*b._t[4] - aa._t[4]*aa._t[6]*b._t[5]; 

  // u8*u0*t4 - t5*u0*u7 - u1*u8*t3 - u2*t4*u6 + u1*t5*u6 + u2*u7*t3
  _t[4] = 
     aa._t[1]*aa._t[6]*b._t[5] + aa._t[2]*aa._t[7]*b._t[3] + aa._t[0]*aa._t[8]*b._t[4] 
   - aa._t[1]*aa._t[8]*b._t[3] - aa._t[6]*aa._t[2]*b._t[4] - aa._t[0]*aa._t[7]*b._t[5];

  // u2*u3*t4 - u2*u4*t3 - u0*u5*t4 + u1*u5*t3 + u4*u0*t5 - u3*u1*t5
  _t[5] =
     aa._t[1]*aa._t[5]*b._t[3] + aa._t[3]*aa._t[2]*b._t[4] + aa._t[0]*aa._t[4]*b._t[5] 
   - aa._t[1]*aa._t[3]*b._t[5] - aa._t[2]*aa._t[4]*b._t[3] - aa._t[5]*aa._t[0]*b._t[4];
 
  //  - u7*u5*t6 + u8*u4*t6 - u8*u3*t7 + t8*u3*u7 + t7*u5*u6 - t8*u4*u6
  _t[6] =  
     aa._t[7]*aa._t[3]*b._t[8] + aa._t[5]*aa._t[6]*b._t[7] + aa._t[4]*aa._t[8]*b._t[6] 
   - aa._t[7]*aa._t[5]*b._t[6] - aa._t[3]*aa._t[8]*b._t[7] - aa._t[4]*aa._t[6]*b._t[8];
 
  //  - u1*u8*t6 - t8*u0*u7 + u1*t8*u6 - u2*t7*u6 + u2*u7*t6 + u8*u0*t7 
  _t[7] =  
     aa._t[1]*aa._t[6]*b._t[8] + aa._t[0]*aa._t[8]*b._t[7] + aa._t[2]*aa._t[7]*b._t[6] 
   - aa._t[1]*aa._t[8]*b._t[6] - aa._t[0]*aa._t[7]*b._t[8] - aa._t[6]*aa._t[2]*b._t[7];
 
  // u2*u3*t7 - u0*u5*t7 + u4*u0*t8 - u3*u1*t8 - u2*u4*t6 + u1*u5*t6
  _t[8] =  
     aa._t[3]*aa._t[2]*b._t[7] + aa._t[1]*aa._t[5]*b._t[6] + aa._t[0]*aa._t[4]*b._t[8] 
   - aa._t[1]*aa._t[3]*b._t[8] - aa._t[5]*aa._t[0]*b._t[7] - aa._t[2]*aa._t[4]*b._t[6];

  float tmp = aa.det();
  if(tmp == 0){
    cerr <<"ERROR: Dividing by zero in tensor.h divide()\nExit\n"; 
    exit(0);
  }
  for(int i=0; i<9; i++){
    _t[i] = _t[i]/tmp;
  }
}

void 
tensor::divide (const tensor &a,  const float x)
{
  if(x==0) {
    cerr <<"ERROR: Dividing by zero in tensor.h divide( float x)\nExit\n"; 
    exit(0);
  }
  for(int i=0; i<9; i++){
    _t[i] = float(double(a._t[i])/double(x));
  }
}
 
void
tensor::eigsort(const double *W, int *i) const
{
  double a = abs(W[0]);
  double b = abs(W[1]);
  double c = abs(W[2]);

  if(a>=b && b>=c){
    i[0]=0;
    i[1]=1;
    i[2]=2;
  } else if(a>=c && c>=b){
    i[0]=0;
    i[1]=2;
    i[2]=1;
  } else if(b>=a && a>=c){
    i[0]=1;
    i[1]=0;
    i[2]=2;
  } else if(b>=c && c>=a){
    i[0]=1;
    i[1]=2;
    i[2]=0;
  } else if(c>=a && a>=b){
    i[0]=2;
    i[1]=0;
    i[2]=1;
  } else if(c>=b && b>=a){
    i[0]=2;
    i[1]=1;
    i[2]=0;
  } else {  
    cout << "\n\nError in tensor.h eigsort()\n"
      "Values are: a: "<<a<<", b: "<<b<<", c: "<<c<<endl<<
      "Exit\n";
    exit(0);
  }
  
  return;
}

int
tensor::eig( float &eig1, float &eig2, float &eig3, float *vector1, float *vector2, float *vector3, const int sort=0) const
{
  if(isZero()){
    eig1 = 0;
    return 1;
  }

  int *w = new int[3];
  // the following variables are used to call the function dsygv_ from the CLAPACK library
  integer k, l, ans, lwork;
  char u = 'U';
  char n = 'V';
  k = 2;
  l = 3;
  lwork = 20;  // tells how much memory to use in clapack, can probably be optimized

  double  * A = new double[9]; // input: tensormatrix, output: eigenvectors
  double  * B = new double[9]; // needed in clapack, since dsygv_ computes A*B*x = eig*x so B is a unit-matrix
  double  * W = new double[3];   // resultvector containing eigenvalues
  double  * WORK = new double[lwork];

  A[0] = double(_t[0]) ;      // convert tensor to single row array in CLAPACK format
  A[1] = double(_t[1]) ;      // A[1]
  A[2] = double(_t[2]) ;      // A[2]
  A[3] = double(_t[3]) ;      /* A[3]   / 0 1 2 \     */
  A[4] = double(_t[4]) ;      /* A[4]   | 3 4 5 |     */
  A[5] = double(_t[5]) ;      /* A[5]   \ 6 7 8 /     */
  A[6] = double(_t[6]) ;      // A[6]
  A[7] = double(_t[7]) ;      // A[7]
  A[8] = double(_t[8]) ;      // A[8]

  if(fabs(A[1]-A[3])<0.01*fabs(A[1])) A[3]=A[1]; // the error is probably an float precision issue;
  if(fabs(A[2]-A[6])<0.01*fabs(A[2])) A[6]=A[2];
  if(fabs(A[5]-A[7])<0.01*fabs(A[5])) A[7]=A[5];
  if(fabs(A[1]-A[3])<0.001) A[3]=A[1]; // the error is probably an float precision issue;
  if(fabs(A[2]-A[6])<0.001) A[6]=A[2];
  if(fabs(A[5]-A[7])<0.001) A[7]=A[5];

  if(A[1]!=A[3] || A[2]!=A[6] || A[5]!=A[7]){
    cout << "WARNING: Decomposing unsimmetric tensor with simmetric lapack function does not work!\n";
    cout << "Errors are: "<<A[1]-A[3]<<", "<< A[2]-A[6]<<", "<< A[5]-A[7]<<endl;
    return -1;
  }

  B[0] = B[4] = B[8] = 1.0;
  B[1] = B[2] = B[3] = B[5] = B[6] = B[7] = 0.0;
  // call the CLAPACK function to compute the eigenvectors and eigenvalues
  ans = dsygv_(&k, &n, &u, &l, A, &l, B, &l, W, WORK, &lwork, &ans);
#ifdef debug
  if (ans != 0){
    cout << "An error ocurred when calling lapack function dsygv_() in function eig() in tensor.h\n";
    return -2;
  }
#endif
  if(sort == 1){                 // sort by largest eigenvalues
    eigsort(W,w);

    eig1 = float(W[w[0]]);
    eig2 = float(W[w[1]]);
    eig3 = float(W[w[2]]);
#ifdef debug
    if(eig1 == 0 && !this->isZero()){
      cout << "Eigenvalue 1 is zero, eigenvalue 2 and/or 3 not\n";
      return -3;
    }
#endif   
    vector1[0] = float(A[w[0]*3]);
    vector1[1] = float(A[w[0]*3+1]);
    vector1[2] = float(A[w[0]*3+2]);
    
    vector2[0] = float(A[w[1]*3]);
    vector2[1] = float(A[w[1]*3+1]);
    vector2[2] = float(A[w[1]*3+2]);
    
    vector3[0] = float(A[w[2]*3]);
    vector3[1] = float(A[w[2]*3+1]);
    vector3[2] = float(A[w[2]*3+2]);

  } else { 
    eig1 = W[0];
    eig2 = W[1];
    eig3 = W[2];
    
    vector1[0] = A[0];
    vector1[1] = A[1];
    vector1[2] = A[2];
    
    vector2[0] = A[3];
    vector2[1] = A[4];
    vector2[2] = A[5];
    
    vector3[0] = A[6];
    vector3[1] = A[7];
    vector3[2] = A[8];
  }
  delete[] A;
  delete[] B;
  delete[] w;
  delete[] W;
  delete[] WORK;
  return 1;
}

  
int 
tensor::svd( float &r1, float &r2, float &r3, tensor &s, tensor &q)
  //tensor::svd( float &r1, float &r2, float &r3, tensor &q, tensor &s)
{
  if(isZero()){
    r1 = 0;
    r2 = 0;
    r3 = 0;
    q.setZero();
    s.setZero();
    return 1;
  }

  // the following variables are used to call the function dsygv_ from the CLAPACK library
  integer l, ans, lwork;
  char jobu  = 'A';
  char jobvt = 'A';
  //char n = 'V';
  //k = 2;
  l = 3;
  lwork = 20;  // tells how much memory to use in clapack, can probably be optimized

  float  * S    = new float[3];
  float  * A    = new float[9]; // input: tensormatrix, output: eigenvectors
  float  * U    = new float[9]; // 
  float  * VT   = new float[9]; // 
  //float  * W  = new float[3];   // resultvector containing eigenvalues
  float  * WORK = new float[lwork];

  A[0] = float(_t[0]) ;      // convert tensor to single row array in CLAPACK format
  A[1] = float(_t[1]) ;      // A[1]
  A[2] = float(_t[2]) ;      // A[2]
  A[3] = float(_t[3]) ;      /* A[3]   / 0 1 2 \     */
  A[4] = float(_t[4]) ;      /* A[4]   | 3 4 5 |     */
  A[5] = float(_t[5]) ;      /* A[5]   \ 6 7 8 /     */
  A[6] = float(_t[6]) ;      // A[6]
  A[7] = float(_t[7]) ;      // A[7]
  A[8] = float(_t[8]) ;      // A[8]

  ans = sgesvd_(&jobu, &jobvt, &l, &l, A, &l, S, U, &l, VT, &l, WORK, &lwork, &ans);
  //#ifdef debug
  if (ans != 0){
    cout << "An error ocurred when calling lapack function sgesvd_() in function svd() in tensor.h\n";
    return -2;
  }
  //#endif
  r1 = S[0];
  r2 = S[1];
  r3 = S[2];
  q.set(U[0], U[1], U[2], U[3], U[4], U[5], U[6], U[7], U[8]);
  s.set(VT[0], VT[1], VT[2], VT[3], VT[4], VT[5], VT[6], VT[7], VT[8]);
  delete[] A;
  delete[] S;
  delete[] U;
  delete[] VT;
  delete[] WORK;
  return 1;
}

float* 
tensor::eigvec1(float *eigvec1) const
{
  if(this->isZero()){
    eigvec1[0] = eigvec1[1] = eigvec1[2] = 0.0;
    return eigvec1;
  }
  float eigvec2[3], eigvec3[3];
  float eigval1, eigval2, eigval3;
  eig( eigval1, eigval2, eigval3, eigvec1, eigvec2, eigvec3, 1);
  return eigvec1;
}
  
float* 
tensor::eigvec2(float *eigvec2) const
{
  if(this->isZero()){
    eigvec2[0] = eigvec2[1] = eigvec2[2] = 0.0;
    return eigvec2;
  }
  float eigvec1[3], eigvec3[3];
  float eigval1, eigval2, eigval3;
  eig( eigval1, eigval2, eigval3, eigvec1, eigvec2, eigvec3, 1);
  return eigvec2;
}

  
float* 
tensor::eigvec3(float *eigvec3) const
{
  if(this->isZero()){
    eigvec3[0] = eigvec3[1] = eigvec3[2] = 0.0;
    return eigvec3;
  }
  float eigvec1[3], eigvec2[3];
  float eigval1, eigval2, eigval3;
  eig( eigval1, eigval2, eigval3, eigvec1, eigvec2, eigvec3, 1);
  return eigvec3;
}

  
float 
tensor::eigval1() const
{
  if(this->isZero()) return 0;
  float eigvec1[3], eigvec2[3], eigvec3[3];
  float eigval1, eigval2, eigval3;
  eig( eigval1, eigval2, eigval3, eigvec1, eigvec2, eigvec3, 1);
  return eigval1;
}
  
float 
tensor::eigval2() const
{
  if(this->isZero()) return 0;
  float eigvec1[3], eigvec2[3], eigvec3[3];
  float eigval1, eigval2, eigval3;
  eig( eigval1, eigval2, eigval3, eigvec1, eigvec2, eigvec3, 1);
  return eigval2;
}
  
float 
tensor::eigval3() const
{
  if(this->isZero()) return 0;
  float eigvec1[3], eigvec2[3], eigvec3[3];
  float eigval1, eigval2, eigval3;
  eig( eigval1, eigval2, eigval3, eigvec1, eigvec2, eigvec3, 1);
  return eigval3;
}
  
float 
tensor::anisotropy() const
{
  if(this->isZero()) return 1;
  float eigvec1[3], eigvec2[3], eigvec3[3];
  float eigval1, eigval2, eigval3;
  eig( eigval1, eigval2, eigval3, eigvec1, eigvec2, eigvec3, 1);
  return (1 - eigval3/eigval1);
}
 
bool 
tensor::operator== (const tensor &t) const
{  
  for(int i=0; i<9; i++){
    if (_t[i] != t._t[i]) return false;
  }
  return true;
}

bool 
tensor::operator!= (const tensor &t) const
{
  int tmp=0;
  for(int i=0; i<9; i++){
    if (_t[i] != t._t[i]) tmp=1;
  }
  if(tmp==1) return true;
  return false;
}

bool 
tensor::operator< (const tensor &t) const
{
  //cout << "DEBUG: operator < this->euclid "<<this->euclid()<< "and t.euclid() "<<t.euclid()<<"\n";
  if(this->measure() < t.measure()) return true;
  return false;
}

bool 
tensor::operator> (const tensor &t) const
{
  if(this->measure() > t.measure()) return true;
  return false;
}

bool 
tensor::operator<= (const tensor &t) const
{
  if(this->measure() > t.measure()) return false;
  return true;
}

bool 
tensor::operator>= (const tensor &t) const
{
  if(this->measure() < t.measure()) return false;
  return true;
}

//functions
  
void 
tensor::scale(const float factor)
{
  for(int i=0; i<9; i++){
    _t[i] = float(double(factor)*double(_t[i]));
  }
}

float
tensor::det() const
{
  return float(double(_t[0])*double(_t[4])*double(_t[8])
       + double(_t[3])*double(_t[7])*double(_t[2])
       + double(_t[6])*double(_t[1])*double(_t[5])
       - double(_t[0])*double(_t[5])*double(_t[7])
       - double(_t[1])*double(_t[3])*double(_t[8])
       - double(_t[2])*double(_t[4])*double(_t[6]));
}
  
float 
tensor::trace() const
{
  return _t[0] + _t[4] + _t[8];
}

tensor
tensor::inv(tensor &result) const
{
  result._t[0] = _t[8]*_t[4] - _t[5]*_t[7]; 
  result._t[1] = _t[2]*_t[7] - _t[8]*_t[1]; 
  result._t[2] = _t[1]*_t[5] - _t[2]*_t[4]; 
  result._t[3] = _t[5]*_t[6] - _t[8]*_t[3]; 
  result._t[4] = _t[8]*_t[0] - _t[2]*_t[6]; 
  result._t[5] = _t[2]*_t[3] - _t[0]*_t[5]; 
  result._t[6] = _t[3]*_t[7] - _t[4]*_t[6]; 
  result._t[7] = _t[1]*_t[6] - _t[0]*_t[7]; 
  result._t[8] = _t[0]*_t[4] - _t[1]*_t[3];  
  float tmp = det();
  if(tmp==0) {
    cerr <<"WARNING: Dividing by zero in tensor.h inv()   ";
#ifdef debug
    cout << endl;
    this->print();
    cout << endl;
#endif
    cout <<"Setting tensor to zero\n"; 
    result.setZero();
    return result;
  }
  result = result/tmp;
  return result;
}

tensor
tensor::trans(tensor &result) const
{
  result._t[0] = _t[0];
  result._t[1] = _t[3];
  result._t[2] = _t[6];
  result._t[3] = _t[1];
  result._t[4] = _t[4];
  result._t[5] = _t[7];
  result._t[6] = _t[2];
  result._t[7] = _t[5];
  result._t[8] = _t[8];
  return result;
}

float
tensor::euclid() const
{
  float tmp = 0;
  for(int i=0; i<9; i++){
      tmp = tmp + _t[i]*_t[i];
  }
  return sqrt(tmp);
}


float
tensor::Max() const
{
  float tmp = -INF;
  for(int i=0; i<9; i++){
      if(_t[i] > tmp) tmp = _t[i];
  }
  return tmp;
}

float
tensor::Min() const
{
  float tmp = INF;
  for(int i=0; i<9; i++){
      if(_t[i] < tmp) tmp = _t[i];
  }
  return tmp;
}

void
tensor::print() const
{
  cout << "Tensor values:\n";
  cout << "\t[ "   << _t[0] << "\t" << _t[1] << "\t" << _t[2] << " ;\n";
  cout << "\t  "   << _t[3] << "\t" << _t[4] << "\t" << _t[5] << " ;\n";
  cout << "\t  "   << _t[6] << "\t" << _t[7] << "\t" << _t[8] << " ]\n";
}

void
tensor::set(float t1, float t2, float t3, float t4, float t5, float t6)
{
  _t[0] = t1;
  _t[1] = t4;
  _t[2] = t5;
  _t[3] = t4;
  _t[4] = t2;
  _t[5] = t6;
  _t[6] = t5;
  _t[7] = t6;
  _t[8] = t3;
}
  
void
tensor::set(float t1, float t2, float t3, float t4, float t5, float t6, float t7, float t8, float t9)
{
#ifdef debug
  if(t4!=t2 || t7!=t3 || t8!=t6){
    cout << "WARNING: setting non-symmetric tensor!\n";
  }
#endif
  _t[0] = t1;
  _t[1] = t2;
  _t[2] = t3;
  _t[3] = t4;
  _t[4] = t5;
  _t[5] = t6;
  _t[6] = t7;
  _t[7] = t8;
  _t[8] = t9;
}

void 
tensor::setZero()
{
  _t[0] = 0.0;
  _t[1] = 0.0;
  _t[2] = 0.0;
  _t[3] = 0.0;
  _t[4] = 0.0;
  _t[5] = 0.0;
  _t[6] = 0.0;
  _t[7] = 0.0;
  _t[8] = 0.0;
}

/*
void
tensor::makeSymmetric()
{
  _t[1] = (_t[1] + _t[3])/2;
  _t[2] = (_t[2] + _t[6])/2;
  _t[5] = (_t[5] + _t[7])/2;
  _t[3] = _t[1];
  _t[6] = _t[2];
  _t[7] = _t[5];
  
}
*/

float 
tensor::get(const int i, const int j) const
{
  if ( i<0 || i>2 || j<0 || j>2){
    cout <<"Not a valid tensor range: 0<i,j<3, returning 0\n";
    return 0;
  }
  return _t[i+j*3];
}

float 
tensor::cosinAngle(const tensor &s)
{
  double a, b, c;
  a = euclid();
  b = s.euclid();
  if( a==0 || b==0){
    return 0;
  }
  c = 0;
  for(int i=0; i<9; i++){
    c = c + _t[i] * s._t[i];
  }
  return float(c/a/b);
}

tensor operator+ (const tensor &s, const tensor &t){
  tensor res;
  res.add(s, t);
  return res;
}
tensor operator+ (const tensor &t, const float x){
  tensor res;
  res.add(t, x);
  return res;
}
tensor operator+ (const float x,   const tensor &t){
  tensor res;
  res.add(t, x);
  return res;
}

tensor operator- (const tensor &s, const tensor &t){
  tensor res;
  res.add(s, -t);
  return res;
}
tensor operator- (const tensor &t, const float x){
  tensor res;
  res.add(t, -x);
  return res;
}
tensor operator- (const float x,   const tensor &t){
  tensor res;
  res.add(-t, x);
  return res;
}

tensor operator* (const tensor &s, const tensor &t){
  tensor res;
  res.multiply(s, t);
  return res;
}
tensor operator* (const tensor &t, const float x){
  tensor res;
  res.multiply(t, x);
  return res;
}
tensor operator* (const float x,   const tensor &t){
  tensor res;
  res.multiply(t, x);
  return res;
}

tensor operator/ (const tensor &s, const tensor &t){
  tensor res;
  res.divide(t, s);
  return res;
}
tensor operator/ (const tensor &t, const float x){
  tensor res;
  res.divide(t, x);
  return res;
}
tensor operator- (const tensor &t){
  return t*(-1);
}

ostream &operator << (ostream &out, const tensor &t){
  t.print();
  return out;
}