/* EGAD: moremath.cpp
   
Navin Pokala and Tracy Handel
Dept. of Molecular and Cell Biology
University of California, Berkeley
Copyright (C) 2003 Regents of the University of California
GNU Public License
Aug 12 2003

Absolutely no warranties are made or are implied with the use of this program or its parts.

This file contains math-related functions for many tasks such as 3D vector math, statistics, random-number
	generation, linear-fiiting, determinant evaluation, and solving linear systems of equations.

*/


#include "moremath.h"

long IDUM;


double average_array(int n, double *array)
{
	int i;
	double avg; 	

	avg=0;
	for(i=1;i<=n;++i)
		avg += array[i];

	return(avg/n);
}

/* This function assigns a 0 to each element of an array of type double */
/* The array must be ended w/ a ENDFLAG value */

void erase_double_array(double *array)
{
  int i;

  i=0;
  while(array[i]!=ENDFLAG)
    {
      array[i]=0;
      ++i;
    }
  array[i]=0;
}

/* multiply the dimensions of b with a; return the scaled vector */
CARTESIAN scalar_multiply_CARTESIAN(double a, CARTESIAN b) 
{
    CARTESIAN return_this;
    
    return_this.x = a*b.x;
    return_this.y = a*b.y;
    return_this.z = a*b.z;

    return(return_this);
} 

/* return CARTESIAN whose dimensions are the sum of the dimensions of a and b */
CARTESIAN add_CARTESIAN(CARTESIAN a, CARTESIAN b)
{
    CARTESIAN return_this;
    
    return_this.x = a.x + b.x;
    return_this.y = a.y + b.y;
    return_this.z = a.z + b.z;
    
    return(return_this);
}

/* return CARTESIAN whose dimensions are the difference of the dimensions of a and b */
CARTESIAN subtract_CARTESIAN(CARTESIAN a, CARTESIAN b)
{
    CARTESIAN return_this;
    
    return_this.x = a.x - b.x;
    return_this.y = a.y - b.y;
    return_this.z = a.z - b.z;
    
    return(return_this);
}

/* return the vector between a and b */
CARTESIAN vector_ab(CARTESIAN a, CARTESIAN b)
{
    CARTESIAN return_this;
    
    return_this.x = b.x - a.x;
    return_this.y = b.y - a.y;
    return_this.z = b.z - a.z;
    
    return(return_this);
}


/* return the midpoint between two CARTESIAN points a & b */
CARTESIAN midpoint(CARTESIAN a, CARTESIAN b)
{
  CARTESIAN mdpt;

  mdpt.x = (a.x + b.x)/2;
  mdpt.y = (a.y + b.y)/2;
  mdpt.z = (a.z + b.z)/2;
  
  return mdpt;
}

/* Random number generator ran2 from Numerical Recipies in C */

#define IM1 2147483563
#define IM2 2147483399
#define AM (1.0/IM1)
#define IMM1 (IM1-1)
#define IA1 40014
#define IA2 40692
#define IQ1 53668
#define IQ2 52774
#define IR1 12211
#define IR2 3791
#define NTAB 32
#define NDIV (1+IMM1/NTAB)
#define RNMX (1.0-TINY)

 
double rand2()
{
	int j;
	long k;
	static long idum2=123456789;
	static long iy=0;
	static long iv[NTAB];
	double temp;
	extern long IDUM;

	if (IDUM <= 0) {
		if (-(IDUM) < 1) IDUM=1;
		else IDUM = -(IDUM);
		idum2=(IDUM);
		for (j=NTAB+7;j>=0;j--) {
			k=(IDUM)/IQ1;
			IDUM=IA1*(IDUM-k*IQ1)-k*IR1;
			if (IDUM < 0) IDUM += IM1;
			if (j < NTAB) iv[j] = IDUM;
		}
		iy=iv[0];
	}
	k=(IDUM)/IQ1;
	IDUM=IA1*(IDUM-k*IQ1)-k*IR1;
	if (IDUM < 0) IDUM += IM1;
	k=idum2/IQ2;
	idum2=IA2*(idum2-k*IQ2)-k*IR2;
	if (idum2 < 0) idum2 += IM2;
	j=iy/NDIV;
	iy=iv[j]-idum2;
	iv[j] = IDUM;
	if (iy < 1) iy += IMM1;
	if ((temp=AM*iy) > RNMX) return RNMX;
	else return temp;
}


#undef IM1
#undef IM2
#undef AM
#undef IMM1
#undef IA1
#undef IA2
#undef IQ1
#undef IQ2
#undef IR1
#undef IR2
#undef NTAB
#undef NDIV
#undef RNMX


/* Returns the linear correlation between two double arrays x and y  
   Modified from the pearsn function from Numerical Recipies in C */
double correllation(double *x, double *y, int n)
{
  
  int j;
  double yt,xt;
  double syy=0.0,sxy=0.0,sxx=0.0,ay=0.0,ax=0.0;
  double r;

  for(j=1;j<=n;++j)
    {
      ax += x[j];
      ay += y[j];
    } 
  ax = ax/((double)n);
  ay = ay/((double)n);
  for (j=1;j<=n;++j) 
    {
      xt=x[j]-ax;
      yt=y[j]-ay;
      sxx += xt*xt;
      syy += yt*yt;
      sxy += xt*yt;
    }
  r=sxy/sqrt(sxx*syy);
  
  if(r>0)
    return (r*r);
  else
     return (-1.0*r*r); 
}

/* this function returns the r^2, rmsd, and rse between two arrays of n doubles */

STATISTICS statistics(double *actual, double *calc, int n)
{
  STATISTICS stats;
  int i;
  double avg_calc,avg_actual;
  double dev_calc,dev_actual;
  double sigma_dev_calc_sqrd, sigma_dev_actual_sqrd, sigma_dev_calc_actual;
  double diff;
  
  avg_calc = 0; avg_actual=0; dev_calc=0; dev_actual=0; 
  sigma_dev_calc_sqrd=0; sigma_dev_actual_sqrd=0; sigma_dev_calc_actual=0;
  
  stats.rmsd = 0; stats.rse = 0;

  i=1;
  while(i<=n)
    {
      avg_calc = avg_calc + calc[i];
      avg_actual = avg_actual + actual[i];
      diff = actual[i]-calc[i];
      stats.rmsd += diff*diff;
      if(actual[i]!=0)
	stats.rse += fabs(diff/actual[i]); 
      else
	stats.rse += fabs(diff);
      ++i;
    }
  stats.rmsd = stats.rmsd/n;
  stats.rmsd = sqrt(stats.rmsd);
  stats.rse = stats.rse/n;  
  

  avg_calc = avg_calc/n;
  avg_actual = avg_actual/n;

  i=1;
  while(i<=n)
    {
      dev_calc = calc[i] - avg_calc;
      dev_actual = actual[i] - avg_actual;
      sigma_dev_calc_sqrd = sigma_dev_calc_sqrd + dev_calc*dev_calc;
      sigma_dev_actual_sqrd = sigma_dev_actual_sqrd + dev_actual*dev_actual;
      sigma_dev_calc_actual = sigma_dev_calc_actual + dev_calc*dev_actual;
      ++i;
    }
  stats.r2 = sigma_dev_calc_actual/sqrt(sigma_dev_calc_sqrd*sigma_dev_actual_sqrd);
  if(stats.r2<0)
    stats.r2 = -(stats.r2*stats.r2);
  else
    stats.r2 = stats.r2*stats.r2;


  return(stats);

}

/* for two double arrays a and b, return the sum of a[i]*b[i], i=start->end */
double dot_prd(double *a, double *b, int start, int end)
{
    int i;
    double answer;
    
    answer = 0;
    
    for(i=start;i<=end;++i)
        answer += a[i]*b[i];
    
    return(answer);
}

/* for arrays x and y of ndata points, fit to a line; m = slope, b = y-intercept, sigma = fitting stat */
/* adapted from numerical recipies in C */
void linear_fit(double *x, double *y, int ndata, double *sigma, double *m, double *b)
{
        int i;
        double wt,t,sxoss,sx=0.0,sy=0.0,st2=0.0,ss;

        *m=0.0;
        if(sigma!=NULL) 
	{
                ss=0.0;
                for (i=1;i<=ndata;i++) 
		{
                        wt=1.0/(sigma[i]*sigma[i]);
                        ss += wt;
                        sx += x[i]*wt;
                        sy += y[i]*wt;
                }
        } 
	else 
	{
                for (i=1;i<=ndata;i++) 
		{
                        sx += x[i];
                        sy += y[i];
                }
                ss=ndata;
        }
        sxoss=sx/ss;
        if (sigma!=NULL) 
	{
                for (i=1;i<=ndata;i++) 
		{
                        t=(x[i]-sxoss)/sigma[i];
                        st2 += t*t;
                        *m += t*y[i]/sigma[i];
                }
        } 
	else 
	{
                for (i=1;i<=ndata;i++) 
		{
                        t=x[i]-sxoss;
                        st2 += t*t;
                        *m += t*y[i];
                }
        }


        *m /= st2;
        *b=(sy-sx*(*m))/ss;
}

/* given a line y[i] = m*x[i] + b and and array of n x-values, calculate the y-values 
	y must be allocated by the calling function */
void linear_transform(double *y, double *x, int n, double *m, double *b)
{
    int i;
    
    for(i=1;i<=n;++i)
    {
	y[i] = (*m)*x[i] + (*b);
    }
}



/* determinant for an n x n matrix using Laplace's method */
double laplace_determinant(int n, double **matrix)
{
	int i,j,ii,jj,k;
	double det;
	double **laplace_matrix;

	if(n==2)
	{
		det = matrix[1][1]*matrix[2][2] - matrix[1][2]*matrix[2][1];
		return(det);
	}
	
	laplace_matrix = (double **)calloc(n,sizeof(double *));
	for(i=1;i<=(n-1);++i)
		laplace_matrix[i] = (double *)calloc(n,sizeof(double));
	
	det = 0;
	for(k=1;k<=n;++k)
	{
		ii=1; i=2;
		while(ii<=(n-1))
		{
			jj=1; j=1;
			
			while(j<k)
			{
				laplace_matrix[ii][jj] = matrix[i][j];
				++j; ++jj;
			}				
			++j; 
			while(j<=n)
			{
				laplace_matrix[ii][jj] = matrix[i][j];
				++j; ++jj;
			}

			++ii; ++i;		
		}

		if(k%2==0)
			det = det - matrix[1][k]*laplace_determinant((n-1),laplace_matrix);
		else
			det = det + matrix[1][k]*laplace_determinant((n-1),laplace_matrix);
	}

	for(i=1;i<=(n-1);++i)
		free_memory(laplace_matrix[i]);
	free_memory(laplace_matrix);

	return(det);

}

/* solves a system of n linear equations w/ n unknowns; form of  AX=D where */
/* matrix_A contains coefficients; vector_X = variable vector; vector_D = constant term */
/* ie:  2x + 3y + 8z = 12
	5x + 8y + 3z =  9
	7x + 4y +  z =  3

 	2 3 8	       x		  12
    A = 5 8 3      X = y      D =  9
	7 4 1          z 		   3
*/

void cramers_rule(int n, double **matrix_A, double *vector_X, double *vector_D)
{
	int i,j,k;
	double detA, detXi;
	double **matrix_Xi;

	matrix_Xi = (double **)calloc((n+1),sizeof(double *));
	for(i=1;i<=n;++i)
		matrix_Xi[i] = (double *)calloc((n+1),sizeof(double));

	detA = laplace_determinant(n,matrix_A);

	if(detA==0)
	{
		fprintf(stderr,"ERROR cannot use Cramer's rule for det(matrix_A)=0\n");
		abort(); 
	}

	for(k=1;k<=n;++k)
	{
		/* copy matrix_A into matrix_Xi */

		for(i=1;i<=n;++i)
			for(j=1;j<=n;++j)
				matrix_Xi[i][j] = matrix_A[i][j];

		/* replace column k w/ vector_D */

		for(i=1;i<=n;++i)
			matrix_Xi[i][k] = vector_D[i];

		detXi = laplace_determinant(n,matrix_Xi);	

		vector_X[k] = detXi/detA;
	}

	for(i=1;i<=n;++i)
		free_memory(matrix_Xi[i]);
	free_memory(matrix_Xi);
}

void fprintf_bool_array(FILE *file_ptr, bool *bool_array, int start_index, int stop_index)
{
	int i;

	for(i=start_index;i<=stop_index;++i)
		fprintf(file_ptr,"%d ",bool_array[i]);

	fprintf(file_ptr,"\n");	

}


void find_cube_corners_near_far_from_point(double min_x, double min_y, double min_z, 
									double max_x, double max_y, double max_z, 
				CARTESIAN *point, CARTESIAN *near_cube_corner, CARTESIAN *far_cube_corner)
{
	CARTESIAN cube_corner;
	double r_max, r_min, r;


	r_max = -1e10; r_min = 1e10;

		cube_corner.x = min_x;

		cube_corner.y = min_y;

		cube_corner.z = min_z;    
			r = Distance((*point), cube_corner); 
			if(r > r_max)
			{
				r_max = r;
				*far_cube_corner = cube_corner;
			}
			if(r < r_min)
			{
				r_min= r;
				*near_cube_corner = cube_corner;
			}
		cube_corner.z = max_z;
			r = Distance((*point), cube_corner); 
			if(r > r_max)
			{
				r_max = r;
				*far_cube_corner = cube_corner;
			}
			if(r < r_min)
			{
				r_min= r;
				*near_cube_corner = cube_corner;
			}
			
		cube_corner.y = max_y;	 
	
		cube_corner.z = min_z;    
			r = Distance((*point), cube_corner); 
			if(r > r_max)
			{
				r_max = r;
				*far_cube_corner = cube_corner;
			}
			if(r < r_min)
			{
				r_min= r;
				*near_cube_corner = cube_corner;
			}
		cube_corner.z = max_z;
			r = Distance((*point), cube_corner); 
			if(r > r_max)
			{
				r_max = r;
				*far_cube_corner = cube_corner;
			}
			if(r < r_min)
			{
				r_min= r;
				*near_cube_corner = cube_corner;
			}
		
		cube_corner.x = max_x;

		cube_corner.y = min_y;

		cube_corner.z = min_z;    
			r = Distance((*point), cube_corner); 
			if(r > r_max)
			{
				r_max = r;
				*far_cube_corner = cube_corner;
			}
			if(r < r_min)
			{
				r_min= r;
				*near_cube_corner = cube_corner;
			}
		cube_corner.z = max_z;
			r = Distance((*point), cube_corner); 
			if(r > r_max)
			{
				r_max = r;
				*far_cube_corner = cube_corner;
			}
			if(r < r_min)
			{
				r_min= r;
				*near_cube_corner = cube_corner;
			}
			
		cube_corner.y = max_y;	 
	
		cube_corner.z = min_z;    
			r = Distance((*point), cube_corner); 
			if(r > r_max)
			{
				r_max = r;
				*far_cube_corner = cube_corner;
			}
			if(r < r_min)
			{
				r_min= r;
				*near_cube_corner = cube_corner;
			}
		cube_corner.z = max_z;
			r = Distance((*point), cube_corner); 
			if(r > r_max)
			{
				r_max = r;
				*far_cube_corner = cube_corner;
			}
			if(r < r_min)
			{
				r_min= r;
				*near_cube_corner = cube_corner;
			}

}