#include <cblas.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>

/* =============== Main functions body ====================*/

int sympowerit(double *E, int n, double *T, int amax, double tol, 
	       int maxiter, int verbose)
{
/*
PURPOSE:   Estimate eigenvectos of a symmetric matrix using the power method.

CALLING SEQUENCE:
           info = sympowerit(*E, n, *T, amax, tol, maxiter);

INPUTS:
           E        symmetric matrix from XX^T/X^TX of centered data matrix
                    type:  *double
	   n        elements in X first dimension
                    type:  int 
                    type:  *double
	   T        workspace for scores (m, amax)
	            type:  *double
	   amax     maximum number of principal components
	            type:  int
	   tol      convergence limit
	            type: double
	   maxiter  maximum number of iteraitons
	            type: int
	   verbose  the amount of debug output
*/
  
  int iter, a, j;
  int info = 0;
  double sumsq, l2norm, *y,*x, sgn, diff, alpha;
  double lambda = 0.0;
  
  if (verbose>1) verbose=1;
  
  /* Allocate space for working vector and eigenvector */
  y = (double *) malloc(n*sizeof(double));
  x = (double *) malloc(n*sizeof(double));

  /* Loop over all components to be estimated*/
  for (a=0; a<=amax-1; a++){
    /* Reset work-/eigen-vector*/
    for (j=0; j<n; j++) {
      y[j] = 0.0;
      x[j] = T[(j*amax)+a];
    }
    /*x[0] = 1.0;*/
    
    /*  Main power-iteration loop */
    for ( iter = 0; iter <= maxiter; iter++ ) {
      /* Matrix-vector product */
      /*cblas_dsymv (CblasRowMajor, CblasUpper, n, 1.0, E, n,
	x, 1, 0.0, y, 1);*/
      cblas_dgemv (CblasRowMajor, CblasNoTrans, n, n, 1.0, E, n,
		   x, 1, 0.0, y, 1);

       /* Normalise y */
       sumsq = y[0] * y[0];
       lambda = x[0] * y[0];
       for ( j = 1; j < n; j++ ) {
	 sumsq += y[j] * y[j];
	 lambda += x[j] * y[j];
       }
       l2norm = sqrt ( sumsq );
       for ( j = 0; j < n; j++ ) y[j] /= l2norm;
       
       /*Check for convergence */
       sgn = ( lambda < 0 ? -1.0 : 1.0 );
       diff = x[0] - sgn * y[0];
       sumsq = diff * diff;   
       x[0] = y[0];
       for ( j = 0; j < n; j++ ) {
	 diff = x[j] - sgn * y[j];
	 sumsq += diff * diff;   
	 x[j] = y[j];
       }
       if ( sqrt ( sumsq ) < tol ) {
	 if (verbose == 1){
	   printf("\nComp: %d\n", a);
	   printf("Converged in %d iterations\n", iter);
	 }
	 break;
       }
       if (iter >= maxiter){
	 if (verbose == 1){
	   printf("\nComp: %d\n", a);
	   printf("Max iter reached.\n");
	   printf("Error: %.2E\n", sumsq);
	 }
	 info = 1;
	 break;
       }
    }
    /* Calculate T */
    for (j=0; j<n; j++){
      y[j] = sgn*sqrt(sgn*lambda)*x[j];
      T[(j*amax)+a] = y[j];
    }
    
    /* rank one deflation of residual matrix */
    /*cblas_dsyr (CblasRowMajor, CblasUpper, n, -1.0, x, 1, E, n);*/
    alpha = -1.0*lambda;
    cblas_dger (CblasRowMajor, n, n, alpha, x, 1, x, 1, E, n);
  }
  
  /* Free used space */
  free(x);
  free(y);
  
  return (info);
}