/*
==================================================================
    trivec - 
      Matlab CMEX program
      Find eigenvectors of tridiagonal quasi-symmetric matrix

      eigenvectors = trivec(eigenvalues,diag,off_diag);

    Input:
       eigenvalues - find vectors for each of these values.
       diag    - the main diagonal of a quasi-symmetric
                 tridiagonal matrix.
       off_diag- (Optional) the product of the off diagonals.
                 defaults to all ones.
   Output:
       eigenvectors - SET TO 0 if routine did not converge.
------------------------------------------------------------------
   Uses inverse iteration to find eigenvectors.  This is not
   efficient unless we are looking for less than 25% of the
   total number of vectors.

   Calls C routine sinvitC()

    Author:   Robert Bernecky
    Date  :   6/10/94
==================================================================
*/
#include <stdio.h>
#include <math.h>
#include "cmex.h"

/*   eigenvectors = trivec(eigenvalues,diag,off_diag); */
#define  EV      prhs[0]
#define  Dm      prhs[1]
#define  Em      prhs[2]
#define  EVS     plhs[0]

#define  FALSE	(0)
#define  TRUE	(1)

int sinvitC (int N, double Adiag[], double subDiag[], double eigVal, double *eigVec);

 /*
 ---------------------------------------------------------------
   eigenvectors = trivec(eigenvalues,diag,off_diag);
 ---------------------------------------------------------------
*/
void mexFunction (int nlhs, Matrix *plhs [], int nrhs, Matrix *prhs [])
{
    double *pd, *ofd, *offdiag, *eigval;
    int i, eig_ct, all_ones=FALSE, N, stat;

  if (nrhs < 2)
    mex_error("Function trivec must have at least two input arguments");

  if (mxGetPi(EV) != NULL)
    mex_error("First argument of function trivec must be real");

  if (((mxGetM(Dm) == 1) && (mxGetN(Dm) == 1)) || (mxGetPi(Dm) != NULL))
    mex_error("2nd argument of function trivec must be a real vector");

  if (nrhs >= 3)
  { if (((mxGetM(Em) == 1) && (mxGetN(Em) == 1)) || (mxGetPi(Em) != NULL))
      mex_error("3rd argument of function trivec must be a real vector");
  }
  else all_ones= TRUE;

  eig_ct= (int)(mxGetM(EV) * mxGetN(EV));
  N= (int) (mxGetM(Dm) * mxGetN(Dm));	/* size of main diagonal */
  pd=  offdiag= (double *) mex_calloc(N, sizeof(double));
  if (all_ones)
    for(i=0;i<N;i++) *pd++ = 1.0;	/* note: elem 0 is ignored */
  else  /* must replace product with sqrt(product) */
    for(i=0,ofd=mxGetPr(Em);i<N;i++,pd++,ofd++) 
      if (*ofd==1.0) *pd= *ofd; else *pd= sqrt(*ofd);

  /* Find eigenvectors */
  EVS= create_matrix(N,eig_ct,REAL);  /* for eigenvectors */
  pd= (double *) mxGetPr(EVS);
  eigval= (double *) mxGetPr(EV);
  for(i=0;i<eig_ct;i++,eigval++)
  {
     stat= sinvitC(N,mxGetPr(Dm),offdiag,*eigval,(pd+i*N));
     if (stat<0)
      printf("trivec did not converge for eigenvalue %d=%f\n",i,*eigval);
  }
  /* cleanup */
  mex_free(offdiag);

}


/*
=================================================================
  sinvitC
     Finds the eigenvector of a tridiagonal quasi-symmetric matrix
     corresponding to the given eigenvalue, using inverse iteration.

      Input
       N	order of the matrix
       Adiag	main diagonal of tridiagonal, symmetric matrix
       subDiag 	subdiagonal elements. subDiag[0] is set to 0.
                These must be the sqrt of the products.
       eigVal	eigenvalue

     Output:
       eigVec	the eigenvector corresponding to eigVal
       sinvitC	return 0 normally,
       		      -1 if eigenvector fails to converge in 5
       		         iterations.
------------------------------------------------------------------
   This routine is a modified version of TINVIT in EISPACK.
   Specialized to the case of finding one eigenvector.

   WARNING:  The modifications have not been done extremely
    carefully.  Very probably this routine will fail for
    certain difficult cases for which the original TINVIT
    would not; for instance, problems with nearly degenerate
    eigenvectors or separable problems.

  See ALGOL procedure TRISTURM by Peters and Wilkinson,
  "Handbook for Auto. Comp., Vol. II Linear Algebra 418-439. (1971)

  Revision History
  	7/1/85	Michael Porter  mod'f TINVIT to fortran 77
  	6/4/94  W. Robert Bernecky C version suitable for matlab

=================================================================
*/
#define macheps (1.0e-12)	/* a touchy parameter */

int sinvitC (int N, double Adiag[], double subDiag[], double eigVal, double *eigVec)
{
   int i,iter,N1;
   double eps3, eps4, norm, U, UK, XU, V;
   double *pRV1, *RV1, *pRV2, *RV2, *pRV3, *RV3;
   double *pRV4, *RV4, *pRV6, *RV6;
   double *D, *E;

   /* allocate some temporary arrays */
   RV1= pRV1=  (double *) malloc(N*sizeof(double));
   RV2= pRV2=  (double *) malloc(N*sizeof(double));
   RV3= pRV3=  (double *) malloc(N*sizeof(double));
   RV4= pRV4=  (double *) malloc(N*sizeof(double));
   RV6= pRV6=  (double *) malloc(N*sizeof(double));

   /* compute (infinity) norm of matrix */
   subDiag[0]=0.0;
   for(i=0,D=Adiag,E=subDiag,norm=0.0;i<N;i++)
     norm+= fabs(*D++) + fabs(*E++);

  /*
     eps3 replaces zero pivots.
     eps4 is a very small # used in scaling down the iterates
  */
  eps3= macheps * norm;
  eps4= (double)N*20.0 * eps3;
  UK= eps3*sqrt((double) N);

  /*  ***** Elimination with interchanges *****   */
  XU= 1.0;
  U= *Adiag - eigVal;
  V= subDiag[1];
  D= (double *) &Adiag[1];	/* D is the main diagonal */
  E= (double *) &subDiag[1];	/* E is the sqrt of off-diags' products */

  for(i=1;i<N;i++)
  {
     if (fabs(*E)>=fabs(U))
     {
       *(++RV4) = XU= U/ *E;
       *RV1= *E;
       *RV2= *D - eigVal;
       *RV3= 0.0;
       if (i<N-1) *RV3= *(E+1);
       U= V - XU * *RV2;
       V= -XU * *RV3;
     }
     else
     {
       *(++RV4) = XU= *E/U;
       *RV1= U;
       *RV2= V;
       *RV3= 0.0;
       U= *D - eigVal - XU*V;
       if (i<N-1) V= *(E+1);
     }
     E++; D++; RV1++; RV2++; RV3++;
  } /* for i.  elimination */

  if (U==0.0) U= eps3;	/* no divide by zero */
  *RV1= U;
  *RV2= *RV3= 0.0;	/* RVx[N-1] (0 based -> Nth element */


  /* *****   Main loop of inverse iteration *****  */

  for(i=0;i<N; i++) *RV6++ = UK;  /* set initial vector to all UK */
  N1= N-1;
  for(iter=0;iter<5;iter++)	/* try at least five times...*/
  {
    RV1= (double *) (pRV1+N1); /* RV1,2,3,6 all point to Nth element */
    RV2= (double *) (pRV2+N1); 
    RV3= (double *) (pRV3+N1); 
    RV6= (double *) (pRV6+N1); 
    for(i=0;i<N;i++)	/* BACK substitution */
    {
	*RV6= (*RV6 - U* *RV2-- - V* *RV3--)/ *RV1--;
	V= U;
	U= *RV6--;
    }

    /* compute norm of vector */
    for(i=0,norm=0.0,RV6=pRV6;i<N;i++) norm+= fabs(*RV6++);

    /* Convergence? */
    if (norm<1.0)
    {  /* not yet.  Scale the vector down. */
       XU= eps4/norm;
       eps4*= 1.0;	/* so we don't get in a rut. wrb */
       for(i=0,RV6=pRV6;i<N;i++) *RV6++ *= XU;

       /* Do forward elimination */
       RV1= (double *) pRV1;		/* start at the beginning */
       RV4= (double *) (pRV4+1);
       RV6= (double *) (pRV6+1);
       E= (double *) &subDiag[1];
       for(i=1;i<N;i++)
       {
	  U= *RV6;
	  if (*RV1++ == *E)
	  {  /* a row interchange was performed earlier in the
		triangularization process. */
	     U= *(RV6-1);
	     *(RV6-1)= *RV6;
	  }
	  *RV6++ = U - *RV4++ * *(RV6-1);
       }/* for i */
    }/* if norm<1.0 */
    else
    {   /* Convergence.  Set the norm to unity and return */
       XU= 1.0/norm;
       for(i=0,RV6=pRV6;i<N;i++) *eigVec++ = *RV6++ * XU;
       break;
    }
  }/* for iter */
  free(pRV1); free(pRV2); free(pRV3); free(pRV4); free(pRV6);

  /* a fall through the loop means we failed to converge */
  if (iter>=5)
  {
     for(i=0;i<N;i++) *eigVec++ = 0.0;
     return -1;
  }
  else  return  0;
}