/*
    Copyright (C) 1998  Dennis Roddeman
    email: dennis.roddeman@uibk.ac.at

    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 
    59 Temple Place, Suite 330, Boston, MA, 02111-1307, USA
*/

#include "tochnog.h"

#define EPS_Q 1.e-10
#define EPS_DET 1.e-12

void array_add( double a[], double b[], double c[], long int n )

{
  register long int i=0;

  for ( i=0; i<n; i++ ) c[i] = a[i] + b[i];

}

double array_distance( double a[], double b[], double work[], long int n )

{
  array_subtract( a, b, work, n );
  return array_size( work, n );
}

double array_inproduct( double a[], double b[], long int n )

{
  register long int i=0;
  double result=0.;

  for ( i=0; i<n; i++ ) result += a[i]*b[i];
  return result;

}

long int array_member( long int list[], long int i, long int n, long int &indx )

{
  register long int j=0, found=0;

  indx = -1;

  while ( j<n && !found ) {
    if ( list[j]==i ) {
      indx = j;
      found = 1;
    }
    j++;
  }
  return found;

}

void array_move( long int from[], long int to[], long int n )

{
  long int i=0;

  for ( i=0; i<n; i++ ) to[i] = from[i];

}


void array_move( double from[], double to[], long int n )

{
  long int i=0;

  for ( i=0; i<n; i++ ) to[i] = from[i];

}

void array_set( double *ptr, double value, long int n )

{
 register long int i=0;

 for ( i=0; i<n; i++ ) *(ptr+i) = value;

}

void array_set( long int *ptr, long int value, long int n )

{
 register long int i=0;

 for ( i=0; i<n; i++ ) *(ptr+i) = value;

}


void array_multiply( double a[], double b[], double c, long int n )

{
  register long int i=0;

  for ( i=0; i<n; i++ ) b[i] = c * a[i];

}

long int array_normalize( double a[], long int n )

{
  long int i=0;
  double l=0;

  l = array_size( a, n );
  if ( l<1.e-10 ) return 0;
  for ( i=0; i<n; i++ ) a[i] = a[i] / l;

  return 1;
}

long int array_null( double dval[], long int n )

{
  long int i=0;

  for ( i=0; i<n; i++ ) {
    if ( dval[i]!=0. ) return 0;
  }
  return 1;

}

void array_outproduct( double a[], double b[], double c[] )

{
  c[0] = a[1]*b[2] - a[2]*b[1];
  c[1] = a[2]*b[0] - a[0]*b[2];
  c[2] = a[0]*b[1] - a[1]*b[0];
}

void array_subtract( double a[], double b[], double c[], long int n )

{
  register long int i=0;

  for ( i=0; i<n; i++ ) c[i] = a[i] - b[i];

}

double array_size( double a[], long int n )

{
  double size=0.;

  size = sqrt( array_inproduct(a,a,n) );

  return size;
}

void matrix_ab( double *a, double *b, double *c, long int n, long int m,
  long int k )

  // c[n][k] = a[n][m] * b[m][k]

{
  register long int i=0, j=0, l=0;

  for ( i=1; i<=n; i++ ) {
    for ( j=1; j<=k; j++ ) {
      *( c + (i-1)*k + j-1 ) = 0;
      for ( l=1; l<=m; l++ ) {
        *( c + (i-1)*k + j-1 ) += ( *( a + (i-1)*m + l-1 ) ) *
                                  ( *( b + (l-1)*k + j-1 ) );
      }
    }
  }

}

void matrix_abat( double a[], double b[], double c[], 
  double work[], long int n )

{

  matrix_ab( a, b, work, n, n, n );
  matrix_abt( work, a, c, n, n, n );

}



void matrix_abt( double *a, double *b, double *c, long int n, long int m,
  long int k )

  // c[n][k] = a[n][m] * b[k][m]Transposed

{
  register long int i=0, j=0, l=0;

  for ( i=1; i<=n; i++ ) {
    for ( j=1; j<=k; j++ ) {
      *( c + (i-1)*k + j-1 ) = 0;
      for ( l=1; l<=m; l++ ) {
        *( c + (i-1)*k + j-1 ) += ( *( a + (i-1)*m + l-1 ) ) *
                                  ( *( b + (j-1)*k + l-1 ) );
      }
    }
  }

}

void matrix_atb( double *a, double *b, double *c, long int n, long int m,
  long int k )

  // c[m][k] = a[n][m]Transposed * b[n][k]

{
  register long int i=0, j=0, l=0;

  for ( i=1; i<=m; i++ ) {
    for ( j=1; j<=k; j++ ) {
      *( c + (i-1)*k + j-1 ) = 0;
      for ( l=1; l<=n; l++ ) {
        *( c + (i-1)*k + j-1 ) += ( *( a + (l-1)*m + i-1 ) ) *
                                  ( *( b + (l-1)*k + j-1 ) );
      }
    }
  }

}

void matrix_atba( double a[], double b[], double c[], 
  double work[], long int n, long int m )

  // c[m][m] = a[n][m]Transposed * b[n][n] * a[n][m]
{

  matrix_atb( a, b, work, n, m, n );
  matrix_ab( work, a, c, m, n, m );
}

void matrix_a4b( double a[3][3][3][3], double b[], double c[] )

{
  register long int i=0, j=0, k=0, l=0;

  for ( i=0; i<3; i++ ) {
    for ( j=0; j<3; j++ ) {
      c[i*3+j] = 0.;
      for ( k=0; k<3; k++ ) {
        for ( l=0; l<3; l++ ) {
          c[i*3+j] += a[i][j][k][l] * b[k*3+l];
        }
      }
    }
  }

}

double matrix_determinant( double a[], long int n )

{
  double result=0.;

  if ( n==1 )
    result = a[0];
  else if ( n==2 )
    result = a[0]*a[3] - a[1]*a[2];
  else {
    assert( n==3 );
    result = a[0]*(a[4]*a[8]-a[7]*a[5]) -
      a[1]*(a[3]*a[8]-a[6]*a[5]) + a[2]*(a[3]*a[7]-a[6]*a[4]);
  }
  return result;
}

void matrix_eigenvalues( double sig[], double eigenvalues[] )

{
  double I1=0., I2=0., I3=0., r=0., s=0., t=0., p=0., q=0.,
    bigR=0., phi=0., y0=0., y1=0., y2=0., tmp=0., inv[3];

  matrix_invariants( sig, inv );
  I1 = inv[0];
  I2 = inv[1];
  I3 = inv[2];
  r = -I1;
  s = +I2;
  t = -I3;
  p = (3.*s-r*r)/3.;
  q = 2.*r*r*r/27. - r*s/3. + t;
  if ( scalar_dabs(q)<EPS_Q ) {
    y0 = -sqrt(scalar_dabs(p));
    y1 = +sqrt(scalar_dabs(p));
    y2 = 0.;
  }
  else {
    bigR = sqrt(scalar_dabs(p)/3.); if ( q<0. ) bigR = -bigR;
    tmp = q/(2.*bigR*bigR*bigR);
    if ( tmp<-1. ) tmp = -1.;
    if ( tmp>+1. ) tmp = +1.;
    phi = acos(tmp);
    y0 = -2.*bigR*cos(phi/3.);
    y1 = -2.*bigR*cos(phi/3.+2.*PIRAD/3.);
    y2 = -2.*bigR*cos(phi/3.+4.*PIRAD/3.);
  }
  eigenvalues[0] = y0 - r/3.;
  eigenvalues[1] = y1 - r/3.;
  eigenvalues[2] = y2 - r/3.;
}

void matrix_insert( double a[], long int n, long int m,
  double b[], long int k, long int l, long int p )

  // insert matrix a[n,m] into matrix b[*,p] at location k, l

{
  long int i=0, j=0, indxi=0, indxj=0;

  for ( i=0; i<n; i++ ) {
    for ( j=0; j<m; j++ ) {
      indxi = k + i;
      indxj = l + j;
      b[indxi*p+indxj] = a[i*m+j];
    }
  }
}

void matrix_invariants( double *mat, double *inv )

{

  inv[0] = mat[0] + mat[4] + mat[8];
  inv[1] = mat[0]*mat[4] + mat[4]*mat[8] + mat[8]*mat[0] -
           mat[1]*mat[3] - mat[5]*mat[7] - mat[6]*mat[2];
  inv[2] = matrix_determinant( mat, 3 );

}

long int matrix_inverse( double *mat, double *inv_mat, double &det, long int n )

{
  double inv_det=0., a1=0., a2=0., a3=0.;

  if ( n==1 ) {
    det = mat[0];
    if ( mat[0]==0 ) return 0;
    inv_mat[0] = 1. / mat[0];
  }
  else if ( n==2 ) {
    det = mat[0]*mat[3] - mat[1]*mat[2];
    if ( det==0. ) return 0;
    inv_det=1./det;
    inv_mat[0] =  mat[3]*inv_det;
    inv_mat[1] = -mat[1]*inv_det;
    inv_mat[2] = -mat[2]*inv_det;
    inv_mat[3] =  mat[0]*inv_det;
  }
  else {
    assert( n==3 );
    a1 = mat[4]*mat[8] - mat[7]*mat[5];
    a2 = mat[7]*mat[2] - mat[1]*mat[8];
    a3 = mat[1]*mat[5] - mat[4]*mat[2];
    det = mat[0]*a1+mat[3]*a2+mat[6]*a3;
    if ( scalar_dabs(det)<EPS_DET ) return 0;
    inv_det = 1./det;
    inv_mat[0] = inv_det*a1;
    inv_mat[1] = inv_det*a2;
    inv_mat[2] = inv_det*a3;
    inv_mat[3] = inv_det*(mat[5]*mat[6]-mat[3]*mat[8]);
    inv_mat[4] = inv_det*(mat[0]*mat[8]-mat[6]*mat[2]);
    inv_mat[5] = inv_det*(mat[3]*mat[2]-mat[0]*mat[5]);
    inv_mat[6] = inv_det*(mat[3]*mat[7]-mat[6]*mat[4]);
    inv_mat[7] = inv_det*(mat[6]*mat[1]-mat[0]*mat[7]);
    inv_mat[8] = inv_det*(mat[0]*mat[4]-mat[3]*mat[1]);
  }
  return 1;

}

#define ROTATE(a,i,j,k,l) g=a[i*n+j];h=a[k*n+l];\
  a[i*n+j]=g-s*(h+g*tau);\
  a[k*n+l]=h+s*(g-h*tau);

void matrix_jacobi(double *a, long int n, double d[], double *v, long int *nrot)

   // destroys *a!

{
	long int j,iq,ip,i;
	double tresh,theta,tau,t,sm,s,h,g,c,b[10],z[10];

	if ( n>10 ) {
	  pri( "Program error: maximum of n exceeded in jacobi." );
          pri( "Use smaller time steps." );
	  exit(TN_EXIT_STATUS);
	}

	for (ip=0;ip<n;ip++) {
		for (iq=0;iq<n;iq++) v[ip*n+iq]=0.0;
		v[ip*n+ip]=1.0;
	}
	for (ip=0;ip<n;ip++) {
		b[ip]=d[ip]=a[ip*n+ip];
		z[ip]=0.0;
	}
	*nrot=0;
	for (i=1;i<=50;i++) {
		sm=0.0;
		for (ip=0;ip<n-1;ip++) {
			for (iq=ip+1;iq<n;iq++)
				sm += scalar_dabs(a[ip*n+iq]);
		}
		if (sm == 0.0) {
			return;
		}
		if (i < 4)
			tresh=0.2*sm/(n*n);
		else
			tresh=0.0;
		for (ip=0;ip<n-1;ip++) {
			for (iq=ip+1;iq<n;iq++) {
				g=100.0*scalar_dabs(a[ip*n+iq]);
				if (i > 4 && (double)(scalar_dabs(d[ip])+g) == (double)scalar_dabs(d[ip])
					&& (double)(scalar_dabs(d[iq])+g) == (double)scalar_dabs(d[iq]))
					a[ip*n+iq]=0.0;
				else if (scalar_dabs(a[ip*n+iq]) > tresh) {
					h=d[iq]-d[ip];
					if ((double)(scalar_dabs(h)+g) == (double)scalar_dabs(h))
						t=(a[ip*n+iq])/h;
					else {
						theta=0.5*h/(a[ip*n+iq]);
						t=1.0/(scalar_dabs(theta)+sqrt(1.0+theta*theta));
						if (theta < 0.0) t = -t;
					}
					c=1.0/sqrt(1+t*t);
					s=t*c;
					tau=s/(1.0+c);
					h=t*a[ip*n+iq];
					z[ip] -= h;
					z[iq] += h;
					d[ip] -= h;
					d[iq] += h;
					a[ip*n+iq]=0.0;
					for (j=0;j<=ip-1;j++) {
						ROTATE(a,j,ip,j,iq)
					}
					for (j=ip+1;j<=iq-1;j++) {
						ROTATE(a,ip,j,j,iq)
					}
					for (j=iq+1;j<n;j++) {
						ROTATE(a,ip,j,iq,j)
					}
					for (j=0;j<n;j++) {
						ROTATE(v,j,ip,j,iq)
					}
					++(*nrot);
				}
			}
		}
		for (ip=0;ip<n;ip++) {
			b[ip] += z[ip];
			d[ip]=b[ip];
			z[ip]=0.0;
		}
	}
	pri( "Error: max. number of iterations in JACOBI exceeded." );
	pri( "Use smaller time steps." );
	exit(TN_EXIT_STATUS);
}

double scalar_dabs( double a )

{
  double result=0.;

  if ( a < 0. ) result = -a;
  else result = a;

  return result;

}

double scalar_dmax( double a, double b )

{
  double result=0;

  if ( a > b ) result = a;
  else result = b;

  return result;
}

double scalar_dmin( double a, double b )

{
  double result=0;

  if ( a < b ) result = a;
  else result = b;

  return result;
}


long int scalar_iabs( long int i )

{
  if ( i>=0 ) return i;
  else return -i;
}

long int scalar_imax( long int a, long int b )

{
  long int result=0;

  if ( a > b ) result = a;
  else result = b;

  return result;

}

double scalar_sign( double a )

{
  if ( a>0. )
    return 1.;
  else
    return -1.;
}

double scalar_square( double a )

{
  return a * a;
}

void sort( double val[], double vec[] )

{
  long int idim=0, min_indx=0, middle_indx=0, max_indx=0;
  double min_val=1.e20, max_val=-1.e20, work_val[MDIM], work_vec[MDIM*MDIM];

  array_move( val, work_val, MDIM );
  array_move( vec, work_vec, MDIM*MDIM );

  for ( idim=0; idim<MDIM; idim++ ) {
    if ( work_val[idim]<min_val ) {
      min_indx = idim;
      min_val = work_val[idim];
    }
    if ( work_val[idim]>max_val ) {
      max_indx = idim;
      max_val = work_val[idim];
    }
  } 
  assert( max_indx>=0 && max_indx<MDIM ); 
  assert( min_indx>=0 && min_indx<MDIM ); 
  if ( min_indx!=0 && max_indx!=0 )
    middle_indx = 0;
  else if ( min_indx!=1 && max_indx!=1 )
    middle_indx = 1;
  else
    middle_indx = 2;

  val[0] = work_val[max_indx];
  val[1] = work_val[middle_indx];
  val[2] = work_val[min_indx];
  array_move( &work_vec[max_indx*MDIM], &vec[0*MDIM], MDIM );
  array_move( &work_vec[middle_indx*MDIM], &vec[1*MDIM], MDIM );
  array_move( &work_vec[min_indx*MDIM], &vec[2*MDIM], MDIM );

}

void string_convert_to_lower_case( char str[] )

  // convert upper case to lower case

{
  int i=0, length=0;

  length = strlen(str);
  for ( i=0; i<length; i++ ) str[i] = tolower(str[i]);
}

long int string_isinteger( char name[] )

  // test if string is an integer

{
  long int i=0, length=0, result=1;

  length = strlen(name);
  for ( i=0; i<length; i++ ) {
    if ( !isdigit(name[i]) ) {
      if ( name[i]!='-' && name[i]!='+' && name[i]!='e' ) result = 0;
    }
  }

  return result;
}


long int string_isdouble( char name[] )

  // test if string is a double

{
  long int i=0, length=0, result=1;

  length = strlen(name);
  for ( i=0; i<length; i++ ) {
    if ( !isdigit(name[i]) ) {
      if ( name[i]!='-' && name[i]!='+' &&
           name[i]!='e' && name[i]!='.' ) result = 0;
    }
  }

  return result;
}

void string_reverse( char s[] )

  // reverse string

{
  int c=0, i=0, j=0;

  for ( i=0, j=strlen(s)-1; i<j; i++, j-- ) {
    c = s[i];
    s[i] = s[j];
    s[j] = c;
  }
}

double triangle_area( double c0[], double c1[], double c2[] )

{
  double area=0., vec0[MDIM], vec1[MDIM], vec2[MDIM];

  array_set( vec0, 0., MDIM );
  array_set( vec1, 0., MDIM );
  array_set( vec2, 0., MDIM );

  array_subtract( c1, c0, vec0, ndim );
  array_subtract( c2, c0, vec1, ndim );
  array_outproduct( vec0, vec1, vec2 ); 
  area = array_size( vec2, MDIM ) / 2.;
  return area;
}

double tetrahedron_volume( double c0[], double c1[], double c2[], double c3[] )

{
  long int nnol=4;
  double volume=0., detj=0., p[MDIM*MNOL], d[MDIM*MNOL], 
    coord[MNOL*MDIM], xj[MDIM*MDIM], xj_inv[MDIM*MDIM];

  p[0]  =  1.;
  p[1]  =  0.;
  p[2]  =  0.;
  p[3]  = -1.;
  p[4]  =  0.;
  p[5]  =  0.;
  p[6]  =  1.;
  p[7]  = -1.;
  p[8]  =  0.;
  p[9]  =  1.;
  p[10] =  0.;
  p[11] = -1.;

  array_move( c0, &coord[0*MDIM], MDIM );
  array_move( c1, &coord[1*MDIM], MDIM );
  array_move( c2, &coord[2*MDIM], MDIM );
  array_move( c3, &coord[3*MDIM], MDIM );

  matrix_ab( p, coord, xj, MDIM, nnol, MDIM );
  if ( matrix_inverse( xj, xj_inv, detj, MDIM ) ) {
    matrix_ab( xj_inv, p, d, MDIM, MDIM, nnol );
    detj = scalar_dabs( detj );
    volume = detj / 6.;
  }
  else volume = 0.;

  return volume;

}
