/**********************************************************************************
 * Copyright 2010 Christoph Pflaum 
 * 		Department Informatik Lehrstuhl 10 - Systemsimulation
 *		Friedrich-Alexander Universität Erlangen-Nürnberg
 * 
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 **********************************************************************************/


// ------------------------------------------------------------
// math_lib.h
//
// ------------------------------------------------------------

#ifndef MATHLIB_H_
#define MATHLIB_H_
#include <limits>
#include "../../../common_source/mathlib/math_lib_common.h"
#include <iostream>
#include <fstream>

#define eps_for_tet_test 0.000001

//////////////////////////////////////////////////////////////////////
// 1. 3D vector class
// 2. a simple 3D matrix class and p in hexahedron test
// 3. geometric operators for 3D vectors
//////////////////////////////////////////////////////////////////////
inline bool isNaN(const double pV) {
    return (pV != pV) || (fabs(pV) == std::numeric_limits<double>::infinity());
}



//////////////////////////////////////////////////////////////////////
// 1. a simple 3D vector class
//////////////////////////////////////////////////////////////////////

class D3vector {
 public:
  double x,y,z;
  D3vector(double cx, double cy, double cz) : x(cx), y(cy), z(cz) {};
  explicit D3vector(double c) : x(c), y(c), z(c) {};
  D3vector() : x(0), y(0), z(0) {};
  ~D3vector(){};
  void Print();
  void PrintCoordinatesOnly();
  void Print(std::ofstream *Datei);
  void operator=(const D3vector& v) { x=v.x; y=v.y; z=v.z; }
  void operator+=(const D3vector& v) { x+=v.x; y+=v.y; z+=v.z; }
  void operator-=(const D3vector& v) { x-=v.x; y-=v.y; z-=v.z; }
  void operator*=(const double v) { x*=v; y*=v; z*=v; }
  void operator/=(const double v) { x/=v; y/=v; z/=v; }
  //bool operator==(const D3vector& v) {if(fabs(v.x - x) <1e-10 && fabs(v.y - y) <1e-10  && fabs(v.z - z) <1e-10 ){return true;} else {return false;} }
  bool operator==(const D3vector& v) {if( (v.x == x)  && (v.y == y) && (v.z == z) ){return true;} else {return false;} }
  bool operator==(const double v) {if( (v == x)  && (v == y) && (v == z) ){return true;} else {return false;} }
  bool operator!=(const double v) {if( (v != x)  || (v != y) || (v != z) ){return true;} else {return false;} }
  bool operator<(const D3vector& v) {if(v.x > x && v.y > y && v.z > z){return true;} else {return false;} }
  bool operator>(const D3vector& v) {if(v.x < x && v.y < y && v.z < z){return true;} else {return false;} }
//  bool operator<=(const D3vector& v) {if(v.x >= x && v.y >= y && v.z >= z){return true;} else {return false;} }
//  bool operator>=(const D3vector& v) {if(v.x <= x && v.y <= y && v.z <= z){return true;} else {return false;} }
  bool operator<=(const D3vector& v) {if(v.x-x >= -1e-10  && v.y-y >= -1e-10 && v.z-z >= -1e-10){return true;} else {return false;} }
  bool operator>=(const D3vector& v) {if(v.x-x <= +1e-10  && v.y-y <= +1e-10 && v.z-z <= +1e-10){return true;} else {return false;} }
  double operator[](int i) {
    if(i==0) return x;
    if(i==1) return y;
    return z;
  }
  
  bool testZwischen(D3vector A, D3vector B); // testet obPunkt auf Gerade zwischen A und B ist.
};

inline double MIN(D3vector V) {
  if(V.x<V.z && V.x<V.y) return V.x;
  if(V.y<V.z && V.y<V.x) return V.y;
    return V.z;
}

inline double MAX(D3vector V) {
  if(V.x>V.z && V.x>V.y) return V.x;
  if(V.y>V.z && V.y>V.x) return V.y;
    return V.z;
}

inline D3vector MAX(D3vector V1, D3vector V2) {
  return D3vector(MAX(V1.x,V2.x),MAX(V1.y,V2.y),MAX(V1.z,V2.z));
}

inline D3vector MIN(D3vector V1, D3vector V2) {
  return D3vector(MIN(V1.x,V2.x),MIN(V1.y,V2.y),MIN(V1.z,V2.z));
}

inline double SUM(D3vector V) {
  return V.x+V.y+V.z;
}

inline D3vector operator+(const D3vector& v,const D3vector& w) {
  return D3vector(v.x+w.x,v.y+w.y,v.z+w.z);
}

inline D3vector operator*(const D3vector& v,const D3vector& w) {
  return D3vector(v.x*w.x,v.y*w.y,v.z*w.z);
}

inline D3vector operator/(const D3vector& v,const D3vector& w) {
  return D3vector(v.x/w.x,v.y/w.y,v.z/w.z);
}

inline D3vector operator-(const D3vector& v,const D3vector& w) {
  return D3vector(v.x-w.x,v.y-w.y,v.z-w.z);
}

inline D3vector operator/(const D3vector& v,const double f) {
  return D3vector(v.x/f,v.y/f,v.z/f);
}

inline D3vector operator*(const D3vector& v,const double f) {
  return D3vector(v.x*f,v.y*f,v.z*f);
}

inline D3vector operator*(const double f, const D3vector& v) {
  return D3vector(v.x*f,v.y*f,v.z*f);
}

inline bool operator<(const D3vector& v,const D3vector& w) {
  return (v.x<w.x && v.y<w.y && v.z<w.z);
}
inline bool operator==(const D3vector& v,const D3vector& w) {
  return (v.x==w.x && v.y==w.y && v.z==w.z);
}

//////////////////////////////////////////////////////////////////////
// 2. a simple 3D matrix class and p in hexahedron test
//////////////////////////////////////////////////////////////////////


class D3matrix {
 private:
  double x1,y1,z1; // first  row
  double x2,y2,z2; // second row
  double x3,y3,z3; // third  row

 public:
  D3matrix(D3vector cx, D3vector cy, D3vector cz) : 
    x1(cx.x), y1(cy.x), z1(cz.x),
    x2(cx.y), y2(cy.y), z2(cz.y),
    x3(cx.z), y3(cy.z), z3(cz.z)   {}

  D3matrix() :
    x1(1), y1(0), z1(0),
    x2(0), y2(1), z2(0),
    x3(0), y3(0), z3(1)   {}

  double Determinante() {
    return   (x1 * (y2*z3 - y3*z2) - y1 * (x2*z3 - x3*z2) + z1 * (x2*y3 - x3*y2));
  }

    /*
  D3matrix(D3vector cx, D3vector cy, D3vector cz) {
    x1 = cx.x; y1 = cy.x;  z1 = cz.x;
    x2 = cx.y; y2 = cy.y;  z2 = cz.y;
    x3 = cx.z; y3 = cy.z;  z3 = cz.z;  };
    */
  ~D3matrix(){};

  /*
  double invert_apply_test_0_1(const D3vector& v) {
    double t;
    double det =  x1 * (y2*z3 - y3*z2) - y1 * (x2*z3 - x3*z2) * v.y + z1 * (x2*y3 - x3*y2);

    t =  ( (y2*z3 - y3*z2) * v.x - (x2*z3 - x3*z2) * v.y + (x2*y3 - x3*y2) * v.z ) / det;
    if(t < -eps_for_tet_test || t > 1.0+eps_for_tet_test) return false;

    t =  (- (y1*z3 - y3*z1) * v.x + (x1*z3 - x3*z1) * v.y - (x1*y3 - x3*y1) * v.z) / det;
    if(t < -eps_for_tet_test || t > 1.0+eps_for_tet_test) return false;

    t =  ((y1*z2 - y2*z1) * v.x - (x1*z2 - x2*z1) * v.y + (x1*y2 - x2*y1) * v.z ) / det;
    if(t < -eps_for_tet_test || t > 1.0+eps_for_tet_test) return false;

    return true;
  }
  */

  D3vector invert_apply(const D3vector& v) {
    double x,y,z;
    double det =  Determinante(); //x1 * (y2*z3 - y3*z2) - y1 * (x2*z3 - x3*z2) + z1 * (x2*y3 - x3*y2);
   // std::cout << "det "<< det << std::endl;
    /*
    x =  (  (y2*z3 - y3*z2) * v.x - (x2*z3 - x3*z2) * v.y + (x2*y3 - x3*y2) * v.z ) / det;
    y =  (- (y1*z3 - y3*z1) * v.x + (x1*z3 - x3*z1) * v.y - (x1*y3 - x3*y1) * v.z ) / det;
    z =  (  (y1*z2 - y2*z1) * v.x - (x1*z2 - x2*z1) * v.y + (x1*y2 - x2*y1) * v.z ) / det;
    */

    x =  (  (y2*z3 - y3*z2) * v.x - (y1*z3 - y3*z1) * v.y + (y1*z2 - y2*z1) * v.z ) / det;
    y =  (- (x2*z3 - x3*z2) * v.x + (x1*z3 - x3*z1) * v.y - (x1*z2 - x2*z1) * v.z ) / det;
    z =  (  (x2*y3 - x3*y2)* v.x -  (x1*y3 - x3*y1) * v.y + (x1*y2 - x2*y1) * v.z ) / det;

    return D3vector(x,y,z);
  }

  inline void Print() {
    std::cout << "Matrix: " << ";\n";
    std::cout << x1 << ", " << y1 << ", " << z1 << ";\n" ;
    std::cout << x2 << ", " << y2 << ", " << z2 << ";\n" ;
    std::cout << x3 << ", " << y3 << ", " << z3 << ";" <<std::endl;
  }

  D3matrix matrixMultiply(D3matrix m)
  {
      D3vector cx = {x1 * m.x1 + y1 * m.x2 +z1 * m.x3,
                      x2 * m.x1 + y2 * m.x2 +z2 * m.x3,
                      x3 * m.x1 + y3 * m.x2 +z3 * m.x3};
      D3vector cy = {x1 * m.y1 + y1 * m.y2 +z1 * m.y3,
                      x2 * m.y1 + y2 * m.y2 +z2 * m.y3,
                      x3 * m.y1 + y3 * m.y2 +z3 * m.y3};
      D3vector cz = {x1 * m.z1 + y1 * m.z2 +z1 * m.z3,
                      x2 * m.z1 + y2 * m.z2 +z2 * m.z3,
                      x3 * m.z1 + y3 * m.z2 +z3 * m.z3};


      return D3matrix(cx,cy,cz);

  }



  D3vector vectorMultiply(D3vector m)
  {
//      D3vector res = {x1 * m.x + x2 * m.x + x3 * m.x,
//                      y1 * m.y + y2 * m.y + y3 * m.y,
//                      z1 * m.z + z2 * m.z + z3 * m.z};
//      D3vector res = {x1 * m.x + x2 * m.y + x3 * m.z,
//                      y1 * m.x + y2 * m.y + y3 * m.z,
//                      z1 * m.x + z2 * m.y + z3 * m.z};
      D3vector res = {x1 * m.x + y1 * m.y + z1 * m.z,
                      x2 * m.x + y2 * m.y + z2 * m.z,
                      x3 * m.x + y3 * m.y + z3 * m.z};


      return res;

  }

  void transpose()
  {
      double temp;
      temp = x2; x2 = y1 ; y1 = temp;
      temp = x3; x3 = z1 ; z1 = temp;
      temp = y3; y3 = z2 ; z2 = temp;

  }
  void invert_gauss_elimination()
  {
      //for 3x3 matrix only
       int n = 3;
         double a[3][6];
         for (int i = 0 ; i<3;i++)
         {
             for (int j = 0 ; j<6;j++)
             {
                 //std::cout << "i j " << i << " " << j << std::endl;
                 a[i][j] = 0;
             }
         }
         int order = 3;
         //n = 3;
         a[0][0] = x1;
         a[1][0] = y1;
         a[2][0] = z1;
         a[0][1] = x2;
         a[1][1] = y2;
         a[2][1] = z2;
         a[0][2] = x3;
         a[1][2] = y3;
         a[2][2] = z3;



             double temp;


             for (int i = 0; i < order; i++) {

                 for (int j = 0; j < 2 * order; j++) {

                     // Add '1' at the diagonal places of
                     // the matrix to create a identity matirx
                     if (j == (i + order))
                         a[i][j] = 1;
                 }
             }
//             std::cout << "matrix before: \n" ;
//             for (int i = 0; i < n; i++) {
//                 for (int j = 0; j < 6; j++) {
//                     std::cout << a[i][j] << "  ";
//                 }
//                 std::cout<<"\n";
//             }
             for (int i = order - 1; i > 0; i--) {

                  //Swapping each and every element of the two rows
                 if (a[i - 1][0] < a[i][0])// &&  a[i][0]!=0)
                  for (int j = 0; j < 2 * order; j++) {

                         // Swapping of the row, if above
                         // condition satisfied.
                  temp = a[i][j];
                  a[i][j] = a[i - 1][j];
                  a[i - 1][j] = temp;
                     }
             }
//             std::cout << "matrix after swapping: \n" ;
//             for (int i = 0; i < n; i++) {
//                 for (int j = 0; j < 6; j++) {
//                     std::cout << a[i][j] << "  ";
//                 }
//                 std::cout<<"\n";
//             }


             // Replace a row by sum of itself and a
             // constant multiple of another row of the matrix
             for (int i = 0; i < order; i++) {

                 for (int j = 0; j < order; j++) {

                     if (j != i) {

                         if (a[i][i] == 0)
                         {
                             std::cout << "debug here" << std::endl;
                         }
                         temp = a[j][i] / a[i][i];
                         for (int k = 0; k < 2 * order; k++) {

                             a[j][k] -= a[i][k] * temp;
//                             if (fabs(a[j][k]) < 1e-20 )
//                             {
//                                 std::cout << "is small???";
//                             }
                         }
                     }
                 }
             }

             // Multiply each row by a nonzero integer.
             // Divide row element by the diagonal element
             for (int i = 0; i < order; i++) {
//                 if (a[i][i] == 0)
//                 {
//                     std::cout << "debug here";
//                 }
                 temp = a[i][i];
                 for (int j = 0; j < 2 * order; j++) {

                     a[i][j] = a[i][j] / temp;
//                     if (a[i][j] == 0)
//                     {
//                         std::cout << "is zero here";
//                     }
                 }
             }
//             std::cout << "inverted matrix : " << std::endl;
//             for (int i = 0; i < n; i++) {
//                 for (int j = order; j < 6; j++) {
//                     std::cout << a[i][j] << "  ";
//                 }
//                 std::cout<<std::endl;
//             }

             x1 = a[0][0+order];
             y1 = a[1][0+order];
             z1 = a[2][0+order];
             x2 = a[0][1+order];
             y2 = a[1][1+order];
             z2 = a[2][1+order];
             x3 = a[0][2+order];
             y3 = a[1][2+order];
             z3 = a[2][2+order];

             return;
  }


};

/*
int Is_p_in_Hexahedron(D3vector p,
		       D3vector cWSD, D3vector cESD,
		       D3vector cWND, D3vector cEND,
		       D3vector cWST, D3vector cEST,
		       D3vector cWNT, D3vector cENT);
*/

D3vector lambda_of_p_in_tet(D3vector p,
			    D3vector cA, D3vector cB,
			    D3vector cC, D3vector cD);

template <class DTyp>
DTyp interpolate_in_tet(D3vector lambda,
			DTyp vA, DTyp vB,
            DTyp vC, DTyp vD) {
  return vA + (vB-vA) * lambda.x + (vC-vA) * lambda.y + (vD-vA) * lambda.z;
}

inline D3vector interpolate_in_tet_D3vector(D3vector lambda,
            D3vector vA, D3vector vB,
            D3vector vC, D3vector vD) {
  return vA + (vB-vA) * lambda.x + (vC-vA) * lambda.y + (vD-vA) * lambda.z;
}


template <class DTyp>
DTyp interpolate_in_tet_trilinear(D3vector lambda,
                                  DTyp x0, DTyp x1,
                                  DTyp x2, DTyp x3,
                                  DTyp x4, DTyp x5,
                                  DTyp x6, DTyp x7) {
    DTyp A = x0;
    DTyp B = x1-x0;
    DTyp C = x3-x0;
    DTyp D = x7-x0;
    DTyp E = x2-x1-x3+x0;
    DTyp F = x4-x3-x7+x0;
    DTyp G = x6-x1-x7+x0;
    DTyp H = x1+x3+x5+x7-x0-x2-x4-x6;
    DTyp R = A    + B * lambda.x + C * lambda.y + D * lambda.z + E * lambda.x*lambda.y + F * lambda.y*lambda.z + G * lambda.x * lambda.z + H * lambda.x*lambda.y*lambda.z;


    return R;
  //  return vA + (vB-vA) * lambda.x + (vC-vA) * lambda.y + (vD-vA) * lambda.z;
}

template <class DTyp>
DTyp interpolate_in_tet_bilinear(D3vector lambda,
                                 DTyp x1, DTyp x2,
                                 DTyp x3, DTyp x4)
{
    DTyp A = x1;
    DTyp B = x2-x1;
    DTyp C = x3-x1;
    DTyp D = x4-x2+x1-x3;
    DTyp R = A + lambda.x * B + lambda.y * C + lambda.x * lambda.y * D;
    return R;
}


//////////////////////////////////////////////////////////////////////
// 3. geometric operators for 3D vectors

/*
inline double L_infty(const D3vector& v) {
  return MAX(ABS(v.x),ABS(v.y),ABS(v.z));
}
*/

inline double D3VectorNorm(const D3vector& v) {
  return my_sqrt(v.x*v.x+v.y*v.y+v.z*v.z);
}
inline double D3VectorNormSquared(const D3vector& v){
    return (v.x*v.x+v.y*v.y+v.z*v.z);
  }
// scalar product
inline double product(const D3vector& v, const D3vector& w) {
  return v.x*w.x+v.y*w.y+v.z*w.z;
}

inline D3vector cross_product(const D3vector& v, const D3vector& w) {
  return D3vector(v.y*w.z-v.z*w.y,
                  v.z*w.x-v.x*w.z,
                  v.x*w.y-v.y*w.x);
}

inline D3vector normal_vector_of_triangle(const D3vector& va,
					  const D3vector& vb,
					  const D3vector& vc) {
  D3vector z;
  z = cross_product(va-vb,vc-vb);
  return z / D3VectorNorm(z);
}


inline double angle_between_vectors(const D3vector& va,
				    const D3vector& vb) {
    double var =   product(va,vb) / (D3VectorNorm(va)*D3VectorNorm(vb)) ;
    if (var > 1.0)
    {
        var = 1.0;
    }
    else if (var < -1.0)
    {
        var = -1.0;
    }
  return acos(var) / M_PI * 180 ;
}

inline double angle_between_vectors_rad(const D3vector& va,
                    const D3vector& vb) {
    /*
     * calculates angle in rad, also checks if var in range [-1,1]. can result in error if not!
     */
    double var =   product(va,vb) / (D3VectorNorm(va)*D3VectorNorm(vb)) ;
    if (var > 1.0)
    {
        var = 1.0;
    }
    else if (var < -1.0)
    {
        var = -1.0;
    }
    return acos(var);
}


inline double max_interior_angel_of_triangle(const D3vector& va,
					     const D3vector& vb,
					     const D3vector& vc) {
  return MAX(angle_between_vectors(vc-va,vb-va),
	     angle_between_vectors(va-vb,vc-vb),
	     angle_between_vectors(vb-vc,va-vc));
}

// va, vb, vc is one face
// vb, vd, vc is one face
inline double angle_between_faces(const D3vector& va,
				  const D3vector& vb,
				  const D3vector& vc,
				  const D3vector& vd) {
  return 180 - angle_between_vectors(normal_vector_of_triangle(va,vb,vc),
				     normal_vector_of_triangle(vb,vd,vc));
}

double calc_maximal_edge_angle(const D3vector& va,
			       const D3vector& vb,
			       const D3vector& vc,
			       const D3vector& vd);

double calc_maximal_face_angle(const D3vector& va,
			       const D3vector& vb,
			       const D3vector& vc,
			       const D3vector& vd);



D3matrix rotationMatrix(D3vector vIn, D3vector vOut);

//////////////////////////////////////////////
// Implementierung einiger Memberfunktionen
//////////////////////////////////////////////


// D3vector 
// ----------

inline void D3vector::Print() {
  std::cout << "Coordinate: " << x << ", " << y << ", " << z << ";\n";
}

inline void D3vector::PrintCoordinatesOnly() {
  std::cout << x << ", " << y << ", " << z;
}
inline void D3vector::Print(std::ofstream *Datei) {
  *Datei  << x << " " << y << " " << z;
}


#endif // MATHLIB_H_