/*
 *  sources.c
 *  
 *
 *  Created by Georgy Gimel'farb on 11/03/2011.
 *  Copyright 2011 __Dept of Computer Science, UoA__. All rights reserved
 *        (modification of svd.c and pgm-ppm i/o subroutines)
 */

/*  svd.c -- Singular Value Decomposition. Translated to 'C' from the
 *           original Algol code in "Handbook for Automatic Computation,
 *           vol. II, Linear Algebra", Springer-Verlag, by C. Bond
 *
 *                              (C) 2000, C. Bond. All rights reserved.
 *
 *  This is almost an exact translation from the original, except that
 *  an iteration counter is added to prevent stalls. This corresponds
 *  to similar changes in other translations.
 *
 *  Returns an error code = 0, if no errors and 'k' if a failure to
 *  converge at the 'kth' singular value.
 * --------------------------------------------------------------------------------
 *  Modified by Georgy Gimel'farb, 04/02/2010, to fix errors of the
 *       initial version, which failed for non-equal m and n (see //===gg comments)
 *---------------------------------------------------------------------------------
 *  Corrected typo "w" instead of the true "q" in the comments below
 *                                                by Sina Masoud-Ansari, 01/04/2011 
 * --------------------------------------------------------------------------------
 *
 *     m - number of rows in input matrix a[m][n];      [!!! NOTE]: m should be >= n 
 *     n - number of columns in input matrix a[m][n]
 *      
 *     withu should be set to 1 if the matrix u[][] in the decomposition is desired, 
 *           and to 0 otherwise.
 *     withv should be set to 1 if the matrix v[][] in thedecomposition is desired, 
 *           and to 0 otherwise.
 *     eps   error threshold (e.g. 1.e-6)
 *     tol   tolerance threshold (e.g. 1.e-6)
 *	   a[m][n] - the rectangular input matrix mxn to be decomposed: a = u * d * v^T 
 *	             a[i][j] -- the element of a with row i and column j
 *     u[m][n] - the output column-ortogonal matrix (n top eigenvectors of a * a^T)
 *               if withu has been set to 1; u[i][j] -- the element of u with row i 
 *               and column j. Otherwise it is used as a temporary array. It may 
 *      		 coincide with a. If an error exit is made, the columns of the matrix
 *               u corresponding to indices of correct singular values should be 
 *               correct.
 *     q[n]    - the output non-negative singular values 
 *               [the unordered diagonal elements of the diagonal matrix d] 
 *               If an error exit is made, the singular values should be correct for 
 *               indices retval+1, retval+2, ..., n.
 *     v[n][n] - the output column-orthogonal matrix (n eigenvectors of a^T * a) 	     
 *	             if matv has been set to 1: v[i][j] -- the element of V with row i 
 *               and column j. Otherwise it is not referenced. It may also coincide 
 *               with the matrix a if u is not needed. If an error exit is made, the 
 *      		 columns of V corresponding to indices of correct singular values 
 *               should be correct.
 *     The output value retval is set to:
 *      	     zero for normal return,
 *               k if the k-th singular value has not been determined after 30 
 *                    iterations, and 
 * [!!!]         -1 if the number of rows m in the matrix a is smaller than the 
 *                    number of its columns.  
 *
 *      NOTE(!!!): m should be >= n    
 *	  
 *     Output:
 *         a  is unaltered (unless overwritten by U or V).
 *         w  contains the n (non-negative) singular values of aA (the diagonal 
 *            elements d[i][i] of d).  NOTE: they are unordered [!!!]. 
 ***/

int svd( int m, int n, int withu, int withv, double eps, double tol, 
	                              double **a, double *q, double **u, double **v ) {
	int i,j,k,l,l1,iter,retval;
	double c,f,g,h,s,x,y,z;
	double *e;

	e = (double *)calloc( n, sizeof(double) );
	retval = 0;

    // Copy 'a' to 'u'     
	for ( i = 0; i < m; i++ ) {
		for ( j = 0; j < n; j++ )
			u[i][j] = a[i][j];
	}
    // Householder's reduction to bidiagonal form. 
	g = x = 0.0;    
	for (i = 0;i < n;i++) {
		e[i] = g;
		s = 0.0;
		l = i+1;
		for (j = i;j < m; j++ )
			s += ( u[j][i] * u[j][i] );
		if (s < tol)
			g = 0.0;
		else {
			f = u[i][i];
			g = (f < 0) ? sqrt(s) : -sqrt(s);
			h = f * g - s;
			u[i][i] = f - g;
			for ( j = l;j < n; j++ ) {
				s = 0.0;
				for ( k = i; k < m; k++ )
					s += ( u[k][i] * u[k][j] );
				f = s / h;
				for ( k=i; k<m; k++ )
					u[k][j] += (f * u[k][i]);
			} // end j 
		} // end s 
		q[i] = g;
		s = 0.0;
		for (j=l; j<n; j++)
			s += (u[i][j] * u[i][j]);
		if (s < tol)
			g = 0.0;
		else {
			f = u[i][i+1];
			g = (f < 0) ? sqrt(s) : -sqrt(s);
			h = f * g - s;
			u[i][i+1] = f - g;
			for (j=l;j<n;j++) 
				e[j] = u[i][j]/h;
			for (j=l;j<m;j++) {
				s = 0.0;
				for (k=l;k<n;k++) 
					s += (u[j][k] * u[i][k]);
				for (k=l;k<n;k++)
					u[j][k] += (s * e[k]);
			} // end j 
		} // end s 
		y = fabs(q[i]) + fabs(e[i]);                         
		if (y > x)
			x = y;
	} // end i 

// accumulation of right-hand transformations 
	if (withv) {
		for( i = n - 1; i >= 0; i--) {
		   if ( i < n - 1 ) {                   //============================gg
			  if ( g != 0.0)  {
				 h = u[ i ][ i + 1 ]  *  g;     //==============in other implementation: h[i][l]
				 for( j = l; j < n; j++ )
					v[ j ][ i ] = u[ i ][ j ] / h;
				 for( j = l; j < n; j++ ) {
					s = 0.0;
					for (k = l; k < n; k++ ) 
						s += ( u[ i ][ k ] * v[ k ][ j ] );
					for (k = l; k < n; k++ )
						v[ k ][ j ] += ( s * v[ k ][ i ] );

				 } // end j 
			  } // end g 
			  for ( j = l; j < n; j++ )
				  v[ i ][ j ] = v[ j ][ i ] = 0.0;
			}                                   // end if(i < n-1) ===========gg
		    v[ i ][ i ] = 1.0;
			g = e[ i ];
			l = i;
		} // end i 
 
	} // end withv, parens added for clarity 

	if ( withu ) {
		for ( i = n - 1; i >= 0; i-- ) {
			l = i + 1;
			g = q[ i ];
			if ( i < n - 1 )                    //============================gg
			   for( j = l; j < n; j++ )         // upper limit was 'n' (--> 'm' in original===gg) 
				  u[ i ][ j ] = 0.0;
			if ( g != 0.0 ) {
			   if ( i != n - 1 ) {              //============================gg
			      h = u[ i ][ i ] * g;
			      for( j = l; j < n; j++ ) {    // upper limit was 'n' (--> 'm' in original===gg)
				     s = 0.0;
				     for( k = l; k < m; k++ )
						s += ( u[ k ][ i ] * u[ k ][ j ] );
				     f = s / h;
				     for( k = i; k < m; k++ ) 
					    u[ k ][ j ] += ( f * u[ k ][ i ] );
				   }  // end j 
				}     // end if( i != n-1 )      =============================gg
				for ( j = i; j < m; j++ ) 
					u[ j ][ i ] /= g;
			  } // end if (g) 
			  else {
				for ( j = i; j < m; j++)
					u[ j ][ i ] = 0.0;
			}
			u[ i ][ i ] += 1.0;
		} // end i
	} // end withu, parens added for clarity 

// diagonalization of the bidiagonal form 
	eps *= x;
	for (k = n - 1; k >= 0; k--) {
		iter = 0;
test_f_splitting:
		for (l = k ; l >= 0;l-- ) {
			if (fabs(e[l]) <= eps) goto test_f_convergence;
			if (fabs(q[l-1]) <= eps) goto cancellation;
		} // end l 

// cancellation of e[l] if l > 0 
cancellation:
		c = 0.0;
		s = 1.0;
		l1 = l - 1;
		for (i = l; i <= k; i++) {
			f = s * e[ i ];
			e[ i ] *= c;
			if ( fabs(f) <= eps ) goto test_f_convergence;
			g = q[ i ];
			h = q[ i ] = sqrt( f*f + g*g );
			c = g / h;
			s = -f / h;
			if ( withu ) {
			   for( j = 0; j < m; j++ ) {
					y = u[ j ][ l1 ];
					z = u[ j ][ i ];
					u[ j ][ l1 ] = y * c + z * s;
					u[ j ][ i ] = -y * s + z * c;
				} // end j 
			} // end withu, parens added for clarity 
		} // end i 
test_f_convergence:
		z = q[ k ];
		if (l == k) goto convergence;

// shift from bottom 2x2 minor 
		iter++;
		if (iter > 30) {
			retval = k;
			break;
		}
		x = q[l];
		y = q[k-1];
		g = e[k-1];
		h = e[k];
		f = ((y-z)*(y+z) + (g-h)*(g+h)) / (2*h*y);
		g = sqrt(f*f + 1.0);
		f = ((x-z)*(x+z) + h*(y/((f<0)?(f-g):(f+g))-h))/x;
// next QR transformation 
		c = s = 1.0;
		for (i=l+1;i<=k;i++) {
			g = e[i];
			y = q[i];
			h = s * g;
			g *= c;
			e[i-1] = z = sqrt(f*f+h*h);
			c = f / z;
			s = h / z;
			f = x * c + g * s;
			g = -x * s + g * c;
			h = y * s;
			y *= c;
			if (withv) {
				for (j=0;j<n;j++) {
					x = v[j][i-1];
					z = v[j][i];
					v[j][i-1] = x * c + z * s;
					v[j][i] = -x * s + z * c;
				} // end j 
			} // end withv, parens added for clarity 
			q[i-1] = z = sqrt(f*f + h*h);
			c = f/z;
			s = h/z;
			f = c * g + s * y;
			x = -s * g + c * y;
			if (withu) {
				for (j=0;j<m;j++) {
					y = u[j][i-1];
					z = u[j][i];
					u[j][i-1] = y * c + z * s;
					u[j][i] = -y * s + z * c;
				} // end j 
			} // end withu, parens added for clarity 
		} // end i 
		e[l] = 0.0;
		e[k] = f;
		q[k] = x;
		goto test_f_splitting;
convergence:
		if (z < 0.0) {
// q[k] is made non-negative 
			q[k] = - z;
			if (withv) {
				for (j=0;j<n;j++)
					v[j][k] = -v[j][k];
			} // end withv, parentheses added for clarity 
		} // end z 
	} // end k 
	
	free(e);
	return retval;
}


/**********************************************************************
  id = 1 -- greyscale PGM image; id = 3 -- colour ppm image
  
  name -- the input image file specification (<***,pgm> or <***.ppm>)
  row -- the number of the input image rows (the output 
  col -- the number of the input image columns
  
  Returns the 1D byte array (unsigned char) of image intensities:
      the array size is col * row for a pgm and 3*col*row for a ppm image;
	  
	  A pgm (grayscale) image:
      the intensity for row j and column i is in position p = i + j*col 
	        of the output array
	  
	  A ppm image: the R (red), G (green), and B (blue) components of the
	       colour vector for row j and column i are in the three successive
		   positions p = 3*i + (3*col)*j (R); p+1 (G); p+2 (B) of the
		   output array
 
 Modified by Jeremy Meese, 11-03-2012 (replacing "r" to "rb" in fopen())
      in order to ensure workability in the Windows OS environment
 **********************************************************************/
unsigned char *readFilePGM_PPM( char *name, int *id,
                                unsigned int *row, unsigned int *col ) {
    char str[ 81 ];
    FILE *fin;
    unsigned int i, j;
	int k, k2, num;
    char rowstr[10], colstr[10];
    unsigned char ch;
    unsigned int base, line, maxpts, column, shift;
    unsigned char *image;
	extern strcmp();
    
    if ( NULL == ( fin = fopen( name, "rb" ) ) ) {    //======================== jm
      printf( "\nreadFilePGM_PPM(): error opening file %s", name );
	  exit( 0 );
    }
	
    fgets( str, 81, fin );    
    if ( ( strcmp( str, "P5\n" ) != 0 ) && ( strcmp( str, "P6\n" ) != 0 ) ) {
      printf( "\nreadFilePGM_PPM():\nFile %s with magic code %s is neither a PGM (P5) nor a PPM (P6)\n", name, str);
      exit( 0 );
    }
	if ( strcmp( str, "P5\n" ) == 0 ) { *id = 1; }
	if ( strcmp( str, "P6\n" ) == 0 ) { *id = 3; }

    do { fgets( str, 81, fin ); } while( str[0]=='#' );

    k = 0;
    do { colstr[k] = str[k];
         k++;
    } while( str[k] != ' ' );
    colstr[k]='\0';
    
    k++;
    k2=0;
    do{ rowstr[k2] = str[k2 + k];
        k2++;
    } while( str[k2 + k] != '\n' );
    rowstr[k2] = '\0';
    
    line   = *row = atoi( rowstr );
    column = *col = atoi( colstr );
    
	if ( *id == 3 ) column *= 3;
    maxpts= line * column;
	 
    image = calloc( maxpts, sizeof (unsigned char) );

    fgets( str, 81, fin ); 
	k = 0;
    for( i = 0, base = 0; i < line; i++, base += column ) {
      for( j = 0; j < column; j++ ) {
        fscanf( fin, "%c", &ch );
        image[ base + j ] = ch;
      }
    }
    fclose(fin);

    return image;
}

/**********************************************************************
  id = 1 -- greyscale PGM image; id = 3 -- colour ppm image
  
  name -- the image file specification (<***,pgm> or <***.ppm>)
  image -- the output array
  row -- the output number of image rows
  col -- the output number of image columns
  
 Writes to a file the 1D byte array "image" (unsigned char) of image 
      intensities or colour vectors:
      the array size is col * row for a pgm and 
	                    3 * col * row for a ppm image
	  
	  A pgm (grayscale) image:
      the intensity for row j and column i is in position p = i + j*col 
	      in the "image" array
	  
	  A ppm image: the R (red), G (green), and B (blue) components of the
	       colour vector for row j and column i are in the successive
		   positions p = 3*i + (3*col)*j (R); p+1 (G); p+2 (B) in the
		   "image" array
 **********************************************************************/
int saveFilePGM_PPM(char *name, unsigned char* image, int id,
                            unsigned int row, unsigned int col) {
    FILE *fout;
    unsigned int base, i, j, len;
    unsigned char ch;
    
    if ( ( id != 1 ) && (id != 3 ) ) {
	   printf( "\nsaveFilePGM_PPM(): wrong image identifier; can't write into the file %s", name );
	   exit( 0 );
	}
    if ( NULL == ( fout = fopen( name,"w" ) ) ) {        
       printf( "\nsaveFilePGM_PPM(): error opening file %s", name );
	   exit( 0 );
    }
    
    if ( id == 1 )
	   fprintf(fout,"P5\n%d %d\n255\n", col, row );
	else
	   fprintf(fout,"P6\n#.\n%d %d\n255\n", col, row );
	len = col;
	if ( id == 3 ) len *= 3;
    base = 0;
    for( i = 0; i < row; i++ ) {
      for( j = 0; j < len; j++) {
         ch = image[ base + j ];
         fprintf( fout, "%c", ch );
      }
      base += len;
    }
    fclose(fout);
    return (1);
}