/*****************************************************************

    This file includes the utility routines used in the program

******************************************************************/
#define UTIL_SOURCE_C_C
#include "util.h"

float sqrtf(float x)
{
  return (float)sqrt((double)x);
}


/*********************************************/
int  Round(float x)
{ 
  if(x>0) 
    return( (int)(x+0.5) );
  return( (int)(x-0.5) );
}

/**********************************************/
/*             return random [0,1]            */
/**********************************************/
float Unit_Random(void)
{
  float r;
  
 /*  return(rand()/2147483647.0); */   /* 2**31-1 */

  r=rand()/32767.0;
  while(r==0 || r==1)  r=rand()/32767.0;

  return(r); 
}

MAT_VECT Create_3D_Matrix(int height,int width, int length)
{
  MAT_VECT p;
  int i;
  printf("Creating a vector image with size %d x %d x %d ... ",
	 height, width, length);
  fflush(stdout);
  p = (MAT_VECT)malloc(sizeof(MAT)*height);
  for (i=0; i< height; i++) {
    if ( (i%(height/20))==0) {
      printf("-");
      fflush(stdout);
    }
    p[i] = Create_Matrix(width, length);
  }
  printf(" Done!\n");
  return p;
}

void Free_3D_Matrix(MAT_VECT a3Dmat, int height, int width)
{
  int i;
  for (i=0; i < height; i++) 
    Free_Matrix(a3Dmat[i], width);
  free(a3Dmat);
}

/*******************************************************
         allocate the space for matrix
*******************************************************/
MAT  Create_Matrix(int height,int width)
{
  MAT  p;
  int  i;
  
  p=(float **)malloc(height*sizeof(float *));
  for(i=0; i<height; i++) {
    p[i]=(float *)malloc(width*sizeof(float));
    if (p[i] == NULL) {
      printf("Cannot create row %d for image %d x %d\n",
	     i, height, width);
      exit(-1);
    }
  }
  return(p);
}

void Free_Matrix(MAT anmat, int height)
{
  int i;
  for (i=0; i<height; i++) 
    free(anmat[i]);
  free(anmat);
}

/*****************************************************/
MAT_I  Create_Int_Matrix(int height,int width)
{
  MAT_I  p;
  int  i;
  
  p=(int **)malloc(height*sizeof(int *));
  for(i=0; i<height; i++) {
    p[i]=(int *)malloc(width*sizeof(int));
    if (p[i] == NULL) {
      printf("Cannot create row %d for image %d x %d\n",
	     i, height, width);
      exit(-1);
    }
  }
  return(p);
}

void Free_Int_Matrix(MAT_I anImat, int height)
{
  int i;
  for (i=0; i<height; i++) 
    free(anImat[i]);
  free(anImat);
}

/****************************************************/
ARRAY Create_Array(int length)
{
  return( (float *)malloc(length*sizeof(float)) );
}

void Free_Array(ARRAY anarrary)
{
  free(anarrary);
}

int Free_Double_Matrix(DMAT aDmat,int size)
{
  int i;
  for (i=0; i<size; i++)
    free(aDmat[i]);
  free(aDmat);
  return 0;
}

/*******************************************************
         allocate the space for matrix
*******************************************************/
DMAT  Create_Double_Matrix(int height, int width)
{
  DMAT  p;
  int  i;
  
  p=(double **)malloc(height*sizeof(double *));
  for(i=0; i<height; i++)
    p[i]=(double *)malloc(width*sizeof(double));
  
  return(p);
}


/************************************************************/
int Copy_Double_Matrix(DMAT from, DMAT to, int size)
{
  int i,j;
  
  for(i=0; i<size; i++)
    for(j=0; j<size; j++)
      to[i][j]=from[i][j];
}


/************************************************
   initialize the pyramid based on the bottom  matrix
************************************************/
MAT_I Subsample(MAT_I mat0, int row_size, int col_size)
{
  int   i, j;
  MAT_I  mat;

  mat=Create_Int_Matrix(row_size, col_size);
  
  for(i=0; i<row_size; i++)
    for(j=0; j<col_size; j++)
      mat[i][j]=mat0[2*i][2*j];
  
  return(mat);
}

/****************************************************/
void Normalize_Int_Matrix(MAT_I mat, 
			  int height,int width,int low,int high)
{
  int i,j;
  float min, max, ratio;
  
  min=1000; max=0;
  for(i=0; i<height; i++)
   for(j=0; j<width; j++)
     if(mat[i][j]<min) min=mat[i][j];
     else if (mat[i][j]>max) max=mat[i][j];
  if (min == max)
    max = min +1;

  ratio= ((float)(high-low-1.0))/((float)(max-min));
  for(i=0; i<height; i++)
    for(j=0; j<width; j++)
      mat[i][j]=(int)( (mat[i][j]-min)*ratio+low );
}

# define DELTA 1e-9     /* allowable minimum positive */



/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
/*           Sweep                 */
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

/* 
  Input: A -- (n x n) matrix.  
         n -- the dimensions of A. 
         k -- to sweep with respect to the pivotal element A[k][k]
              k=0,..,n-1.                                             
         flag = 1 for Sweep, = -1 for reverse Sweep.  
  return: A -- sweeped matrix. 
          0 -- succesful. 
*/

int Sweep(DMAT A, int n, int k,  int flag)
{
  static double *u=NULL;
  static int    m=0;
  int i, j;
  double c;
  
  if (m<n)  {
    if (u!=NULL) { free(u); }
    if((u=(double*)calloc(n,sizeof(double)))==NULL) {
      printf("Sweep: no memory available!\n"); 
      exit(1); 
    }  
    m=n;
  }
  
  if ((c=A[k][k])*flag<DELTA) {
    printf("Sweep %d: nonpositive matrix!\n", k);
    return 1;
  }
  
  c=1.0/c;
  for (j=0; j<=k; j++)  {
    u[j]=A[k][j];
    A[k][j]=0.0;
  }
  for (i=k+1; i<n; i++) {
    u[i]=A[i][k];
    A[i][k]=0.0;
  }
 
  u[k]=-1.0*flag;
  for (i=0; i<n; i++)  {
    for (j=0; j<=i; j++) {
      A[i][j] -= c*u[i]*u[j];
      A[j][i] = A[i][j];
    }
    
  }
  return 0;
}

/************************************************
   initialize the matrix as random noise
************************************************/
MAT_I Create_Noise_Matrix(int height,int width, int minVal, int maxVal)
{
  int i, j;
  MAT_I mat;
  
  mat=Create_Int_Matrix(height, width);
  
  for(i=0; i<height; i++)
    for(j=0; j<width; j++) {
      mat[i][j]=minVal+(int)(Unit_Random()*(maxVal-minVal));
      if (mat[i][j] > maxVal)
	mat[i][j] = maxVal;
    }
  return(mat);
}

/************************************************
   initialize the matrix as random noise
************************************************/
MAT_I Create_Const_Matrix(int height,int width, int cVal)
{
  int i, j;
  MAT_I mat;
   
   mat=Create_Int_Matrix(height, width);

   for(i=0; i<height; i++)
     for(j=0; j<width; j++)
       mat[i][j]=cVal;

   return(mat);
}