/*
==================================================================
    trival - 
      Matlab CMEX program
      Find eigenvalues of tridiagonal quasi-symmetric matrix

      eigenvalues = trival(lo,hi,diag,off_diag,eig_ct);

    Input:
       lo, hi  - define an interval. (lo typically 0)
        	 (hi should be set using Gerschgorin's thm.)
       diag    - the main diagonal of a quasi-symmetric
                 tridiagonal matrix.
       off_diag- (Optional) the product of the off diagonals.
                 defaults to all ones.
       eig_ct  - (Optional) the maximum # of eigenvalues to find.
       		 defaults to all eigenvalues within the interval.

   Output:
       eigenvalues - the first (largest) eig_ct eigenvalues
                     found within the interval <lo,hi>.
------------------------------------------------------------------

   Finds isolating intervals for each eigenvalue using the
   Sturm sequence method.  It then uses these intervals to
   find a more accurate eigenvalue via Brent's method, which
   combines bisection, linear interpolation, and inverse
   quadratic interpolation. Tries to take advantage of the
   fact that most entries of the off-diagagonals are 1.0 

   Calls C routines eiglimC(), sturmC(), sturmFnC(), zbrent().

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

/*   eigenvalues = trival(lo,hi,diag,off_diag,eig_ct); */
#define  LO      prhs[0]
#define  HI      prhs[1]
#define  D       prhs[2]
#define  E       prhs[3]
#define  CT      prhs[4]
#define  EVS     plhs[0]

#define sign(a) ( (a<0.0)?-1:(a>0.0)?1:0)
/* #define sign(a)  (iszero(a)?0:signbit(a)?-1:1) */
#define FALSE	(0)
#define TRUE	(1)
#define	EPS  	((double) 1.0e-15)

int eiglimC();
int sturmC();
double sturmFnC();
double zbrent();

/* package some parameters into a structure */
/*  NOTE:  pointers limL and limH are side-effected */

typedef struct
{
   float *limL;		/* low & hi eigenvalue brackets */
   float *limH;
   double *Amat;	/* The main diagonal */
   double *OffD;	/* The product of off diags;1st elem undef'n */
   int eig_ct;  	/* # of eigenvals to find */
   int AmatN;   	/* size of Amat */
   char *ones;		/* Used to identify non-1 off diags */
} eigdata;

 /*
 ---------------------------------------------------------------
   eigenvalues = trival(lo,hi,diag,off_diag,eig_ct); 
 ---------------------------------------------------------------
*/
void mexFunction (
    int nlhs, Matrix *plhs[], int nrhs, Matrix *prhs[])
{
    char *pon;
    double *pd;
    eigdata Dp;
    float lo, hi, *plo, *phi;
    int i, eig_ct, Slo, Shi, all_ones=FALSE, N;
    unsigned int m,n;
    double *pi;

  if (nrhs < 3)
    mex_error("Function trival must have at least three input arguments");

  m  = mxGetM( LO );
  n  = mxGetN( LO );
  pi = mxGetPi( LO );

/*  if ((m != 1) || (n != 1) || (!mxIsTypeReal( LO ) ) || (pi != NULL))
    mex_error("First argument of function trival must be a real scalar");
*/

  m  = mxGetM( HI );
  n  = mxGetN( HI );
  pi = mxGetPi( HI );

/*  if ((m != 1) || (n != 1) || (!mxIsTypeReal( HI ) ) || (pi != NULL))
    mex_error("2nd argument of function trival must be a real scalar");
*/

  m  = mxGetM( D );
  n  = mxGetN( D );
  pi = mxGetPi( D );

  if (((m == 1) && (n == 1)) || (pi != NULL))
    mex_error("3rd argument of function trival must be a real vector");

  m  = mxGetM( E );
  n  = mxGetN( E );
  pi = mxGetPi( E );

  if (nrhs >= 4)
  { if (((m == 1) && (n == 1)) || (pi != NULL))
      mex_error("4th argument of function trival must be a real vector");
  }
  else all_ones= TRUE;

  if (nrhs >= 5)
  {
  m  = mxGetM( CT );
  n  = mxGetN( CT );
  pi = mxGetPi( CT );

/*    if ((m != 1) || (n != 1) || (!mxIsTypeReal( CT ) == 1) || (pi != NULL))
      mex_error("5th argument of function trival must be a real scalar");
*/
    eig_ct= (int) *(mxGetPr(CT));
  }
  else  /* default to all eigenvalues */
     eig_ct= (int)(mxGetM(D)) * (int)(mxGetN(D));

  /* allocate 2 float arrays to hold low and high limits */
  plo= (float *) mex_calloc(eig_ct,sizeof(float));
  phi= (float *) mex_calloc(eig_ct,sizeof(float));

  /* initialize parameter structure */
  N= (int) (mxGetM(D) * mxGetN(D));	/* size of main diagonal */
  Dp.eig_ct= eig_ct;		/* # of eigenvalues to find */
  Dp.AmatN= N;			/* size of diagonal D */
  Dp.Amat= mxGetPr(D);		/* ptr to main diagonal */
  Dp.limL= plo;			/* ptr to memory for holding low limits */
  Dp.limH= phi;			/* and hi limits...Filled by eiglimC    */

  /* flag array - true if both off-diagonal elements are 1.0 */
  Dp.ones= (char *) mex_calloc(N, sizeof(char));
  if (all_ones)
  {
    pd= Dp.OffD= (double *) mex_calloc(N, sizeof(double));
    for(i=0,pon= Dp.ones;i<N;i++)
    {
       *pon++ = TRUE;
       *pd++ = 1.0;	/* note: elem 0 is ignored */
    }
  }
  else
    for(i=0,pon=Dp.ones,pd=Dp.OffD=mxGetPr(E); i<N; i++) 
      if (*pd++ == 1.0) *pon++ = TRUE;
      else *pon++ = FALSE;

  lo= (float) *(mxGetPr(LO));	/* start of interval to search */
  hi= (float) *(mxGetPr(HI));	/* upper limit of interval     */
  Slo= sturmC((double)lo,&Dp);	/* # of eigenvalues above lo limit */
  Shi= sturmC((double)hi,&Dp);	/* # above the hi limit */
  eig_ct= eiglimC(lo, hi, Slo, Shi, 0, &Dp);
  /* plo & phi now point to bracketing intervals */

  /* Now find eigenvalues using Brent's method */
  EVS= create_matrix(1,eig_ct,REAL);  /* for eigenvalues */
  pd= (double *) mxGetPr(EVS);
  for(i=0;i<eig_ct;i++)	/* zero in on eigenvalues */
    *pd++ = (double) zbrent(sturmFnC,(double)plo[i],(double)phi[i],
                            EPS,&Dp);

  /* cleanup */
  mex_free(plo); mex_free(phi); mex_free(Dp.ones);
  if (all_ones) mex_free(Dp.OffD);

}

/*
---------------------------------------------------------------
    eiglimC.c - 
      Finds an upper and lower limit for the first
      eig_ct eigenvalues.

   This C routine is called by a Matlab CMEX procedure "trival"

   inputs:
     Lo, Hi are low and high limits of an interval to search
     sturmL - the number of eigenvalues above Lo
     sturmH - the number of eigenvalues above Hi
     ct - the number of eigenvalues found thus far
     Dp - pointer to data structure which contains pointers
          to where to store the low and high limits of each
          eigenvalue.  Also contains eig_ct, the # of eigenvalues
          to search for, and *Amat, a pointer to matrix data
          used by the sturm routine.

  output:
     ct - the number of eigenvalues found
     low and high limits of eigenvalues
---------------------------------------------------------------
*/
int eiglimC(Lo, Hi, sturmL, sturmH, ct, Dp)
   float Lo, Hi;
   int sturmL, sturmH, ct;
   eigdata *Dp;
{
    int eignum,sturmMp;
    float mp;

    if (ct>=Dp->eig_ct) return ct;
    eignum= sturmL-sturmH;	/* # of eigenvalues in interval <Lo,Hi> */
    if (eignum<=0) return ct; /* none in this interval */
    if (eignum==1)   /* paydirt.  Save the limits. */
    {
      ct++;		/* number found so far. */
      *(Dp->limL)++ = Lo;	/* NOTE SIDE-EFFECT */
      *(Dp->limH)++ = Hi;
    }
    else  /* recurse */
    {
       mp= (Lo+Hi)*0.5;
       sturmMp= sturmC(mp, Dp);
       ct= eiglimC(mp, Hi, sturmMp, sturmH, ct, Dp);
       ct= eiglimC(Lo, mp, sturmL, sturmMp, ct, Dp);
    }
    return ct;
}


/* 
---------------------------------------------------------------
   sturmC -
      given matrix data and a value mu, determine the number
      of eigenvalues greater than mu.

     input:
       mu   - lower bound 
       Dp   - data structure which holds diagonal, off-diags,
	      and count.
    output:
       eigNum - number of eigenvalues above mu
---------------------------------------------------------------
*/
int sturmC(mu, Dp)
  double mu;
  eigdata *Dp;
{
    int N;
    register double s,s1,s2,*Amat;
    register int i,ps=1,num=0,sg;
    register char *ones;

    Amat= (double *) Dp->Amat;  /* main diag */
    N= Dp->AmatN;		/* size of diag */
    ones= (char *) Dp->ones;	/* 1 if off-diag is 1 */
    ones++;			/* skip 1st entry */
    s2= (double) 1.0;
    s1= mu- *Amat++;
    if (s1<0.0) { ps= -1; num++;}
    for(i=1;i<N;i++)
    {
      s= (mu - *Amat++)*s1;
      if (*ones++) s -= s2;	/* no need for multiply */
      else s -= (Dp->OffD[i]*s2);
      if ((sg=sign(s))!=0)
       if (sg!=ps)
       {
          ps= sg;
          num++;	/* count sign changes */
      }
      s2= s1;
      s1= s;
    } /* for i */
    return num;
}
    


/* 
---------------------------------------------------------------
   sturmFnC -
      evaluate characteristic equation of tridiagonal
      matrix at value mu.

     input:
       mu   - value at which to evaluate polynomial.
       Dp   - data structure which holds
              Amat - pointer to main diagonal
              AmatN  - size of Amat

    output:
       sturmFnC - value of polynomial evaluated at mu.
---------------------------------------------------------------
*/

double sturmFnC(mu,Dp)
    double mu;
    eigdata *Dp;
{
    int N;
    register double s,s1,s2,*Amat;
    register int i;
    register char *ones;

    Amat= (double *) Dp->Amat;
    N= Dp->AmatN;
    ones= (char *) Dp->ones;
    s2= (double) 1.0;
    s1= mu- *Amat++;
    for(i=1;i<N;i++)
    {
      s= (mu - *Amat++)*s1;
      if (*(++ones)) s -=  s2;
      else s -= (Dp->OffD[i]*s2);
      s2= s1;
      s1= s;
    } /* for i */
    return (double) s;
}


/*
---------------------------------------------------------------
     zbrent.c
       Given an interval which contains exactly one eigenvalue,
       solve the characteristic equation by Brent's method,
       which combines bisection, linear interpolation, and
       inverse quadratic interpolation. Convergence is guaranteed
       to the isolated eigenvalue.

       This version is from "Numerical Recipes" by Press, et al.

---------------------------------------------------------------
*/
#define ITMAX 100

double zbrent(func,x1,x2,tol,Dp)
     double x1,x2,tol;
     double (*func)();	/* ANSI: float (*func)(float); */
     eigdata *Dp;	/* data for func */
{
	int iter;
	double a=x1,b=x2,c,d,e,min1,min2;
	double fa,fb,fc,p,q,r,s,tol1,xm;

	fa=(*func)((double) a, Dp);
	fb=(*func)((double) b, Dp);
	if (fb*fa > 0.0) perror("Root must be bracketed in ZBRENT");
	fc=fb;
	for (iter=1;iter<=ITMAX;iter++) {
		if (fb*fc > 0.0) {
			c=a;
			fc=fa;
			e=d=b-a;
		}
		if (fabs(fc) < fabs(fb)) {
			a=b;
			b=c;
			c=a;
			fa=fb;
			fb=fc;
			fc=fa;
		}
		tol1=2.0*EPS*fabs(b)+0.5*tol;
		xm=0.5*(c-b);
		if (fabs(xm) <= tol1 || fb == 0.0) return b;
		if (fabs(e) >= tol1 && fabs(fa) > fabs(fb)) {
			s=fb/fa;
			if (a == c) {
				p=2.0*xm*s;
				q=1.0-s;
			} else {
				q=fa/fc;
				r=fb/fc;
				p=s*(2.0*xm*q*(q-r)-(b-a)*(r-1.0));
				q=(q-1.0)*(r-1.0)*(s-1.0);
			}
			if (p > 0.0)  q = -q;
			p=fabs(p);
			min1=3.0*xm*q-fabs(tol1*q);
			min2=fabs(e*q);
			if (2.0*p < (min1 < min2 ? min1 : min2)) {
				e=d;
				d=p/q;
			} else {
				d=xm;
				e=d;
			}
		} else {
			d=xm;
			e=d;
		}
		a=b;
		fa=fb;
		if (fabs(d) > tol1)
			b += d;
		else
			b += (xm > 0.0 ? fabs(tol1) : -fabs(tol1));
	        fb=(*func)((double) b, Dp);
	}
	perror("Maximum number of iterations exceeded in ZBRENT");
}

#undef ITMAX