praxis.c

Go to the documentation of this file.

 
/*****************************************************************************/
#include "tpcclibConfig.h"
/*****************************************************************************/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>
#include <string.h>
/*****************************************************************************/
#include "tpcextensions.h"
#include "tpcrand.h"
/*****************************************************************************/
#include "tpcnlopt.h"
/*****************************************************************************/
 
/*****************************************************************************/
/* Local functions */
int praxisMin(
  int j, unsigned int nits, double *d2, double *x1, double f1, 
  unsigned int fk, double *fx, double **v, unsigned int n, double h, double t,
  double macheps, double m2, double m4, double small,
  double dmin, double ldt, unsigned int *nl,
  unsigned int *nf, double qd0, double qd1, double *q0, double *q1,
  double *p, double (*_fun)(int, double*, void*), void *_fundata
);
double praxisFlin(
  double l, int j, double **v, unsigned int n,
  unsigned int *nf, double qd0, double qd1, double *q0, double *q1,
  double *p, double (*_fun)(int, double*, void*), void *_fundata
);
int praxisMinfit(
  int n, double eps, double tol, double **ab, double *q
);
void praxisSort(double *d, double **v, unsigned int n);
/*****************************************************************************/
 
/*****************************************************************************/
int nloptPowellBrent(
  NLOPT *nlo,
  unsigned int ktm,
  TPCSTATUS *status
) {
  int verbose=0; if(status!=NULL) verbose=status->verbose;
  if(verbose>0) printf("%s()\n", __func__);
  if(nlo==NULL) {
    statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_FAIL);
    return TPCERROR_FAIL;
  }
  if(nlo->totalNr<1 || nlo->xfull==NULL || nlo->_fun==NULL) {
    statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_NO_DATA);
    return TPCERROR_NO_DATA;
  }
  
 
  /* Check deltas */
  unsigned int fixedNr=0;
  for(unsigned int i=0; i<nlo->totalNr; i++) if(nlo->xdelta[i]<=0.0) fixedNr++;
  if(fixedNr==nlo->totalNr) {
    statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_INVALID_PARNR);
    return TPCERROR_INVALID_PARNR;
  }
 
  
  /* Set a safe machine epsilon and related values */
  double macheps=doubleMachEps()*100.0;
  if(verbose>1) printf("macheps := %g\n", macheps);
  double m2=sqrt(macheps);
  double m4=sqrt(m2);
  if(verbose>1) {
    printf("m2 := %e\n", m2);
    printf("m4 := %e\n", m4);
  }  
  double small=macheps*macheps;
  double vsmall=small*small;
  double large=1.0/small;
  double vlarge=1.0/vsmall;
  if(verbose>1) {
    printf("small := %e\n", small);
    printf("vsmall := %e\n", vsmall);
    printf("large := %e\n", large);
    printf("vlarge := %e\n", vlarge);
  }
 
  /* Set common initial step size and scaling parameter */
  double h=0.0, scbd=1.0, t=0.0, t2;
  {
    unsigned int i;
    double min=nan(""), max=nan("");
    for(i=0; i<nlo->totalNr; i++) if(nlo->xdelta[i]>0.0) {
      h+=nlo->xdelta[i];
      t+=nlo->xtol[i];
      if(isnan(min) || nlo->xdelta[i]<min) min=nlo->xdelta[i];
      if(isnan(max) || nlo->xdelta[i]>max) max=nlo->xdelta[i];
    }
    h/=(double)(nlo->totalNr-fixedNr);
    t/=(double)(nlo->totalNr-fixedNr);
    if(verbose>2) {printf("xdelta_min := %g\nxdelta_max := %g\n", min, max); fflush(stdout);}
    scbd+=log10(max/min); // >1 if param scales are different
    t2=small+fabs(t); t=t2;
    if(h<100.0*t) h=100.0*t;
  }
  if(verbose>1) {
    printf("step := %g\n", h);
    printf("scbd := %g\n", scbd);
    printf("t(2) := %g\n", t);
    fflush(stdout);
  }
  if(isnan(scbd)) { // checking that user provided xdeltas
    statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_INVALID_VALUE);
    return TPCERROR_INVALID_VALUE;
  }
 
 
  /* Is the problem ill-conditioned (1) or not?
     This variable is automatically set, when Praxis finds the problem to be 
     ill-conditioned during iterations. */
  int illc=0;
 
  /* Initialize */
  if(verbose>2) {printf("initializing\n"); fflush(stdout);}
  unsigned int i, j, dim=nlo->totalNr;
  double ldfac; if(illc) ldfac=0.1; else ldfac=0.01;
  unsigned int nl=0, kt=0;
  unsigned int nf=0; // nr of objective function calls
  double dmin=small;
  double ldt=h;
  double d[dim], y[dim], z[dim];
  // double v[dim][dim];
  double **v=malloc(dim*sizeof(double *)); if(v==NULL) {
    statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_OUT_OF_MEMORY);
    return TPCERROR_OUT_OF_MEMORY;
  }
  for(i=0; i<dim; i++) {
    v[i]=malloc(dim*sizeof(double));
    if(v[i]==NULL) {
      statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_OUT_OF_MEMORY);
      return TPCERROR_OUT_OF_MEMORY;
    }
  }
  /* Make a copy of parameter list */
  if(verbose>2) {printf("copying parameters\n"); fflush(stdout);}
  double p[dim];
  for(i=0; i<dim; i++) p[i]=nlo->xfull[i];
  
  for(i=0; i<dim; i++) {
    d[i]=y[i]=z[i]=0.0;
    for(j=0; j<dim; j++) v[i][j]=(i==j ? 1.0 : 0.0);
  }
  double qd0=0.0, q0[dim], q1[dim];
  for(i=0; i<dim; i++) q1[i]=q0[i]=nlo->xfull[i];
  
  
  /* Calculate function value with the initial guess */
  if(verbose>2) {printf("evaluating initial guess\n"); fflush(stdout);}
  double fx, qf1;
  fx=qf1=(*nlo->_fun)(nlo->totalNr, nlo->xfull, nlo->fundata);
  nf++;
  if(!isfinite(fx)) {
    for(i=0; i<dim; i++) free(v[i]); 
    free(v);
    statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_INVALID_VALUE);
    return TPCERROR_INVALID_VALUE;
  }
  if(verbose>1) {printf("initial_objfunc := %g\n", fx); fflush(stdout);}
  
  
  /* The main loop */
  double sf, s, dn, qd1=0.0;  
  int done=0, nLoop=0, ret=0;
  while(!done) {
    nLoop++;
    if(verbose>2) {
      printf("\n-------------------------------\nloop %d, with %d fcalls\n", nLoop, nf); 
      fflush(stdout);
    }
    sf=d[0]; s=d[0]=0.0;
    
    /* Minimize along the first direction */
    ret=praxisMin(0, 2, &d[0], &s, fx, 0, &fx, v, dim, h, t, 
                   macheps, m2, m4, small, dmin, ldt, &nl,
                   &nf, qd0, qd1, q0, q1, 
                   p, nlo->_fun, nlo->fundata);
    if(ret) {
      if(verbose>0) printf("Error %d in praxisMin()\n", ret);
      for(i=0; i<dim; i++) free(v[i]); 
      free(v);
      statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_FAIL);
      return TPCERROR_FAIL;
    }
 
    if((sf<=(0.9*d[0])) || ((0.9*sf)>=d[0])) for(i=1; i<dim; i++) d[i]=0.0;
 
    /* Other directions */
    unsigned int k, kl=0, k2;
    double sl, df, f1, lds;
    for(k=1; k<dim; k++) {
      if(verbose>5) printf("direction %d\n", k);
      for(i=0; i<dim; i++) y[i]=p[i];
      sf=fx; if(kt>0) illc=1;
      do { // Usually only once, but twice if noticed here that ill-conditioned
        kl=k; df=0.0;
        /* If ill-conditioned, take random step to get off resolution valley */
        if(illc) {
          for(i=0; i<dim; i++) {
            z[i]=(0.1*ldt+t2*pow(10.0,(double)kt)) * (drand()-0.5);
            s=z[i]; for(j=0; j<dim; j++) p[j]+=s*v[j][i];
          }
          /* Calculate function value */
          fx=(*nlo->_fun)(dim, p, nlo->fundata);
          if(!isfinite(fx)) {
            statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_INVALID_VALUE);
            return TPCERROR_INVALID_VALUE;
          }
          nf++;
        }
        /* Minimize along non-conjugate directions */
        for(k2=k; k2<dim; k2++) {
          if(verbose>6) printf("  non-conjugate direction %d\n", k2);
          sl=fx; s=0.0; 
          ret=praxisMin(k2, 2, &d[k2], &s, fx, 0, &fx, v, dim, h, t,
                         macheps, m2, m4, small, dmin, ldt, &nl,
                         &nf, qd0, qd1, q0, q1,
                         p, nlo->_fun, nlo->fundata);
          if(ret) {
            if(verbose>0) printf("Error %d in praxisMin()\n", ret);
            for(i=0; i<dim; i++) free(v[i]); 
            free(v);
            statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_FAIL);
            return TPCERROR_FAIL;
          }
          if(illc) s=d[k2]*(s+z[k2])*(s+z[k2]); else s=sl-fx;
          if(df<s) {df=s; kl=k2;}
        }
        if(illc) break;
        if(df>=fabs(100.0*macheps*fx)) break;
        illc=1;
      } while(1);
      
      /* Minimize along conjugate directions */
      for(k2=0; k2<k; k2++) {    // WHY NOT UNTIL k2==k ??????
//      for(k2=0; k2<=k; k2++) {    
        if(verbose>6) printf("  conjugate direction %d\n", k2);
        s=0.0;
        ret=praxisMin(k2, 2, &d[k2], &s, fx, 0, &fx, v, dim, h, t,
                       macheps, m2, m4, small, dmin, ldt, &nl,
                       &nf, qd0, qd1, q0, q1, 
                       p, nlo->_fun, nlo->fundata);
        if(ret) {
          if(verbose>0) printf("Error %d in praxisMin()\n", ret);
          for(i=0; i<dim; i++) free(v[i]); 
          free(v);
          statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_FAIL);
          return TPCERROR_FAIL;
        }
      }
      f1=fx; fx=sf; lds=0.0;
      for(i=0; i<dim; i++) {
        sl=p[i]; p[i]=y[i]; y[i]=sl-y[i]; sl=y[i]; lds+=sl*sl;
      }
      lds=sqrt(lds);
      if(lds>small) {
        for(i=kl-1; i>=k; i--) {
          for(j=0; j<dim; j++) v[j][i+1]=v[j][i];
          d[i+1]=d[i];
        }
        d[k]=0.0; for(i=0; i<dim; i++) v[i][k]=y[i]/lds;
        ret=praxisMin(k, 4, &d[k], &lds, f1, 1, &fx, v, dim, h, t,
                       macheps, m2, m4, small, dmin, ldt, &nl,
                       &nf, qd0, qd1, q0, q1,
                       p, nlo->_fun, nlo->fundata);
        if(ret) {
          if(verbose>0) printf("Error %d in praxisMin()\n", ret);
          for(i=0; i<dim; i++) free(v[i]); 
          free(v);
          statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_FAIL);
          return TPCERROR_FAIL;
        }
        if(lds<=0.0) {lds=-lds; for(i=0; i<dim; i++) v[i][k]=-v[i][k];}
      }
      ldt*=ldfac; if(ldt<lds) ldt=lds;
      for(i=0, t2=0.0; i<dim; i++) t2+=p[i]*p[i];
      t2=sqrt(t2); t2*=m2; t2+=t;
      
      if(verbose>1) printf("  objfunc := %g\n", fx);
      
      /* Stopping criterion */
      if(ldt>(0.5*t2)) kt=0; else kt++;
      if(verbose>3 && kt>0) printf("  kt := %d\n", kt);
      if(kt>ktm) {done=1; break;}
 
    } // next direction k
    if(done) break;
    
    /* 
     * We are probably in a curved valley, try quadratic extrapolation
     */
    /* but only if we have more than one parameter to fit */
    if(dim<2) {
      if(nf>1000) done=1;
      continue;
    }
 
    /* Praxis quad: 
       looks for the minimum along a curve defined by q0, q1, and x (q2). */
    {
      double qa, qb, qc, qs, ql; 
      qs=fx; fx=qf1; qf1=qs;
      for(i=0, qd1=0.0; i<dim; i++) {
        qs=p[i]; ql=q1[i]; p[i]=ql; q1[i]=qs;
        qd1+=(qs-ql)*(qs-ql);
      }
      qd1=sqrt(qd1); ql=qd1; qs=0.0;
      if(qd0>0.0 && qd1>0.0 && nl>=3*dim*dim) {
        ret=praxisMin(-1, 2, &qs, &ql, qf1, 1, &fx, v, dim, h, t,
                       macheps, m2, m4, small, dmin, ldt, &nl,
                       &nf, qd0, qd1, q0, q1,
                       p, nlo->_fun, nlo->fundata);
        if(ret) {
          if(verbose>0) printf("Error %d in praxisMin()\n", ret);
          for(i=0; i<dim; i++) free(v[i]); 
          free(v);
          statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_FAIL);
          return TPCERROR_FAIL;
        }
        qa = ql*(ql-qd1)/(qd0*(qd0+qd1));
        qb = (ql+qd0)*(qd1-ql)/(qd0*qd1);
        qc = ql*(ql+qd0)/(qd1*(qd0+qd1));
      } else {
        fx=qf1; qa=qb=0.0; qc=1.0;
      }
      if(!isfinite(qa)) qa=0.0;
      if(!isfinite(qb)) qb=0.0;
      if(!isfinite(qc)) qc=1.0;
      qd0=qd1;
      for(i=0; i<dim; i++) {
        qs=q0[i]; q0[i]=p[i];
        p[i]=qa*s + qb*p[i] + qc*q1[i];
      }
    }
    dn=0.0;
    for(i=0; i<dim; i++) {d[i]=1.0/sqrt(d[i]); if(dn<d[i]) dn=d[i];}
    for(j=0; j<dim; j++) {
      s=d[j]/dn; 
      if(isfinite(s)) for(i=0; i<dim; i++) v[i][j]*=s;
    }
    /* Scale axis to reduce condition number */
    if(scbd>1.0) {
      s=vlarge;
      for(i=0; i<dim; i++) {
        sl=0.0;
        for(j=0; j<dim; j++) sl+=v[i][j]*v[i][j];
        z[i]=sqrt(sl); if(!isfinite(z[i]) || z[i]<m4) z[i]=m4; 
        if(s>z[i]) s=z[i];
      }
      for(i=0; i<dim; i++) {
        sl=s/z[i]; z[i]=1.0/sl;
        if(z[i]>scbd) {sl=1.0/scbd; z[i]=scbd;}
        for(j=0; j<dim; j++) v[i][j]*=sl;
      }
    }
    /* Calculate new set of orthogonal directions before repeating 
       the main loop; first, transpose v[][] for the Praxis minfit */
    for(i=1; i<dim; i++) for(j=0; j<i; j++) {
      s=v[i][j]; v[i][j]=v[j][i]; v[j][i]=s;
    }
    /* Call Praxis minfit to find the SVD of v[][].
       This gives the principal values and principal directions of approximating
       quadratic form without squaring the condition number */
    ret=praxisMinfit((int)dim, macheps, vsmall, v, d);
    if(ret) {
      if(verbose>0) printf("Error %d in praxisMinfit()\n", ret);
      for(i=0; i<dim; i++) free(v[i]); 
      free(v);
      statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_FAIL);
      return TPCERROR_FAIL;
    }
    if(scbd>1.0) {
      for(i=0; i<dim; i++) {s=z[i]; for(j=0; j<dim; j++) v[i][j]*=s;}
      for(i=0; i<dim; i++) {
        s=0.0; for(j=0; j<dim; j++) s+=v[j][i]*v[j][i];
        s=sqrt(s); d[i]*=s; s=1.0/s; for(j=0; j<dim; j++) v[j][i]*=s;
      }
    }
    for(i=0; i<dim; i++) {
      if((dn*d[i]) > large) d[i]=vsmall; // dn was calculated above
      else if((dn*d[i]) < small) d[i]=vlarge;
      else d[i]=pow(dn*d[i], -2.0);
    }
    /* the new eigenvalues and eigenvectors */
    praxisSort(d, v, dim);
    dmin=d[dim-1]; if(dmin<small) dmin=small;
    illc=(m2*d[0]) > dmin;
 
 
 
 
    if(nf>1000) done=1;
  }
  if(verbose>1) printf("func_evals := %d\n", nf);
  
  
  /* Free allocated memory */
  for(i=0; i<dim; i++) free(v[i]);
  free(v);
  
  /* If ok, then copy parameters to the provided struct */
  for(i=0; i<dim; i++) nlo->xfull[i]=p[i];
 
  
  // Just to prevent warning for now
  if(sf>0.0) sf=0.0;
  if(kt>0) kt=0;
  if(nl>0) nl=0;
  if(ldfac>0.0) ldfac=0.0;
  if(ktm>0) ktm=0;
 
 
  return TPCERROR_OK;
}
/*****************************************************************************/
 
/*****************************************************************************/
 
int praxisMin(
  int j, 
  unsigned int nits, 
  double *d2, 
  double *x1, 
  double f1, 
  unsigned int fk,
  double *fx, 
  double **v,
  unsigned int dim, 
  double h, 
  double t,
  double macheps,
  double m2,
  double m4,
  double small,
  double dmin,
  double ldt,
  unsigned int *nl,
  unsigned int *nf,
  double qd0,
  double qd1,
  double *q0,
  double *q1,
  double *p,
  double (*_fun)(int, double*, void*),
  void *_fundata
) {
  /* Check input */
  if(dim<1 || d2==NULL || x1==NULL || fx==NULL || v==NULL) return(1);
 
  /* Initialize */
  double sf1=f1;
  double sx1=*x1;
  double xm=0.0;
  double fm=*fx;
  double f0=*fx;
  int dz; if((*d2)<macheps) dz=1; else dz=0; // prevent div by (almost) zero
  // find step size
  unsigned int i;
  double s=0.0; for(i=0; i<dim; i++) s+=p[i]*p[i]; s=sqrt(s);
  double t2;
  if(dz) t2=m4*sqrt(fabs(*fx)/dmin + s*ldt) + m2*ldt;
  else t2=m4*sqrt(fabs(*fx)/(*d2) + s*ldt) + m2*ldt;
  if(!isfinite(t2)) return(2);
  s*=m4; s+=t; if(dz && t2>s) t2=s;
  if(t2<small) t2=small;
  if(t2>0.01*h) t2=0.01*h;
  if(fk && f1<=fm) {xm=*x1; fm=f1;}
  if(!fk || fabs(*x1)<t2) {
    *x1=(*x1>0.0 ? t2:-t2);
    f1 = praxisFlin(*x1, j, v, dim, 
                    nf, qd0, qd1, q0, q1, 
                    p, _fun, _fundata);
    if(!isfinite(f1)) return(2);
  }
  if(f1<=fm) {xm=*x1; fm=f1;}
 
  int done=1;
  unsigned int k=0;
  double x2, f2, d1;
  do {
    if(dz) {
      x2=(f0<f1 ? -(*x1) : 2.0*(*x1)); 
      f2=praxisFlin(x2, j, v, dim,
                    nf, qd0, qd1, q0, q1, 
                    p, _fun, _fundata);
      if(!isfinite(f2)) return(3);
      if(f2<=fm) {xm=x2; fm=f2;}
      *d2=(x2*(f1-f0)-(*x1)*(f2-f0))/((*x1)*x2*((*x1)-x2));
    }
    d1=(f1-f0)/(*x1) - (*x1)*(*d2); dz=1;
    if(*d2<=small) x2=(d1<0 ? h:-h); else x2=-0.5*d1/(*d2);
    if(fabs(x2)>h) x2=(x2>0 ? h:-h);
    f2=praxisFlin(x2, j, v, dim,
                  nf, qd0, qd1, q0, q1, 
                  p, _fun, _fundata);
    if(!isfinite(f2)) return(4);
    done=1;
    while((k<nits) && (f2>f0)) {
      k++;
      if((f0<f1) && ((*x1)*x2>0.0)) {done=0; break;}
      x2*=0.5;
      f2=praxisFlin(x2, j, v, dim,
                    nf, qd0, qd1, q0, q1, 
                    p, _fun, _fundata);
      if(!isfinite(f2)) return(5);
    }
  } while(!done);
 
  *nl=(*nl)+1;
  if(f2>fm) x2=xm; else fm=f2;
  if(fabs(x2*(x2-(*x1))) > small) {
    *d2=(x2*(f1-f0) - (*x1)*(fm-f0))/((*x1)*x2*((*x1)-x2));
  } else {
    if(k>0) *d2=0.0;
  }
  if(*d2<small) *d2=small;
  *x1=x2; *fx=fm; if(sf1<*fx) {*fx=sf1; *x1=sx1;}
  if(j!=-1) for(i=0; i<dim; i++) p[i]+=(*x1)*v[i][j];
 
  return(0);
}
/*****************************************************************************/
 
/*****************************************************************************/
double praxisFlin(
  double la,
  int j, 
  double **v,
  unsigned int dim,
  unsigned int *nf,
  double qd0,
  double qd1,
  double *q0,
  double *q1,
  double *p,
  double (*_fun)(int, double*, void*),
  void *_fundata
) {
  unsigned int i;
  double tflin[dim];
 
  if(j<-1 || j>=(int)dim) return(nan(""));
  
  if(j>=0) {  // linear search
    for(i=0; i<dim; i++) tflin[i]=p[i]+la*v[i][j];
  } else {  // search along parabolic space curve
    double qa, qb, qc, f=qd0+qd1;
    if(qd0*f!=0.0) qa=la*(la-qd1)/(qd0*f); else qa=0.0;
    if(!isfinite(qa)) qa=0.0;
    if(f!=0.0) qb=(la+qd0)*(qd1-la)/f; else qb=0.;
    if(!isfinite(qb)) qb=0.0;
    if(qd1*f!=0.0) qc=la*(la+qd0)/(qd1*f); else qc=0.;
    if(!isfinite(qc)) qc=0.0;
    for(i=0; i<dim; i++) tflin[i]= qa*q0[i] + qb*p[i] + qc*q1[i];
  }
  *nf=(*nf)+1;
  return(*_fun)(dim, tflin, _fundata);
}
/*****************************************************************************/
 
/*****************************************************************************/
int praxisMinfit(
  int n, 
  double eps,
  double tol,
  double **ab,
  double *q
) {
  int l, kt, l2, i, j, k, done, fdone;
  double c, f, g, h, s, x, y, z;
  double e[n];
 
  /* Check the input */
  if(n<1) return(1);
  if(eps<=0.0 || ab==NULL || q==NULL) return(2);
  if(tol<eps) tol=eps;
  if(n==1) {
    q[0]=ab[0][0]; ab[0][0]=1.0;
    return(0);
  }
 
  /* Householder's reduction to bidiagonal form */
  for(i=0, x=g=0.0; i<n; i++) {
    e[i]=g; s=0.0; l=i+1; for(j=i; j<n; j++) s+=ab[j][i]*ab[j][i];
    if(s<tol) {
      g=0.0;
    } else {
      g=sqrt(s); f=ab[i][i]; if(f>=0.0) g=-g;
      h=f*g-s; ab[i][i]=f-g;
      for(j=l; j<n; j++) {
        f=0.0; for(k=i; k<n; k++) f+=ab[k][i]*ab[k][j];
        f/=h;  for(k=i; k<n; k++) ab[k][j]+=f*ab[k][i];
      }
    }
    q[i]=g; s=0.0; 
    if(i<n) for(j=l; j<n; j++) s+=ab[i][j]*ab[i][j];
    if(s<tol || i>n-2) {
      g=0.0;
    } else {
      f=ab[i][i+1];
      g=sqrt(s); if(f>=0.0) g=-g;
      h=f*g-s;
      ab[i][i+1]=f-g;
      for(j=l; j<n; j++) e[j]=ab[i][j]/h;
      for(j=l; j<n; j++) {
        s=0.0;
        for(k=l; k<n; k++) s+=ab[j][k]*ab[i][k];
        for(k=l; k<n; k++) ab[j][k]+=s*e[k];
      }
    }
    y=fabs(q[i])+fabs(e[i]); 
    if(y>x) x=y;
  }
  
  /* Accumulation of right-hand transformations */
  l=n-1; ab[l][l]=1.0; g=e[l];
  for(i=l-1; i>=0; i--) {
    if(g!=0.0) {
      h=ab[i][i+1]*g; 
      for(j=l; j<n; j++) ab[j][i]=ab[i][j]/h;
      for(j=l; j<n; j++) {
        for(k=l, s=0.0; k<n; k++) s+=ab[i][k]*ab[k][j];
        for(k=l; k<n; k++) ab[k][j]+=s*ab[k][i];
      }
    }
    for(j=l; j<n; j++) ab[i][j]=ab[j][i]=0.0; 
    ab[i][i]=1.0; g=e[i]; l=i;
  }
  
  /* Diagonalization to bi-diagonal form */
  eps*=x;
  for(k=n-1; k>=0; k--) {
    kt=0; done=0;
    while(!done) {
      if(++kt>30) e[k]=0.0; // QR failed
      fdone=0;
      for(l2=k; l2>=0; l2--) {
        l=l2; 
        if(fabs(e[l])<=eps) {fdone=1; break;}  // F convergence
        if(l>0 && fabs(q[l-1])<=eps) break;
      }
      if(!fdone) for(i=l, c=0.0, s=1.0; i<=k; i++) {
        f=s*e[i]; e[i]*=c; if(fabs(f)<=eps) break;  // F convergence
        g=q[i];
        if(fabs(f)<fabs(g)) {
          double fg=f/g; h=fabs(g)*sqrt(1.0+fg*fg);
        } else {
          double gf=g/f; h=(f!=0.0 ? fabs(f)*sqrt(1.0+gf*gf) : 0.0);
        }
        q[i]=h; if(h==0.0) {h=1.0; g=1.0;} 
        c=g/h; s=-f/h;
      }
      /* test convergence */
      z=q[k]; if(l==k) {done=1; break;}  // convergence
      /* Also stop if k==0 because below there is index k-1 */
      if(k==0) {done=1; break;}
      // Shift from bottom 2x2 minor
      x=q[l]; 
      y=q[k-1]; // In many source codes index is k-L but in Brents book k-1
      g=e[k-1]; 
      h=e[k];
      if(h!=0.0 && y!=0.0) f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y); else f=0.0;
      g=sqrt(f*f+1.0);
      if(f<=0.0) f=((x-z)*(x+z)+h*(y/(f-g)-h))/x;
      else f=((x-z)*(x+z)+h*(y/(f+g)-h))/x;
      // Next QR transformation
      for(i=l+1, s=c=1.0; i<=k; i++) {
        g=e[i]; y=q[i]; h=s*g; g*=c;
        if(fabs(f)<fabs(h)) {
          double fh=f/h; z=fabs(h)*sqrt(1.0+fh*fh);
        } else {
          double hf=h/f; z=(f!=0.0 ? fabs(f)*sqrt(1.0+hf*hf) : 0.0);
        }
        e[i-1]=z; if(z==0.0) f=z=1.0;
        c=f/z; s=h/z; f=x*c+g*s; g=-x*s+g*c; h=y*s; y*=c;
        for(j=0; j<n; j++) {
          x=ab[j][i-1]; z=ab[j][i]; ab[j][i-1]=x*c+z*s; ab[j][i]=-x*s+z*c; }
        if(fabs(f)<fabs(h)) {
          double fh=f/h; z=fabs(h)*sqrt(1.0+fh*fh);
        } else {
          double hf=h/f; z=(f!=0.0 ? fabs(f)*sqrt(1.0+hf*hf) : 0.0);
        }
        q[i-1]=z; if(z==0.0) z=f=1.0; c=f/z; s=h/z; f=c*g+s*y; x=-s*g+c*y;
      }
      e[l]=0.0; e[k]=f; q[k]=x;
    } // while !done
    if(z<0.0) {q[k]=-z; for(j=0; j<n; j++) ab[j][k]=-ab[j][k]; }
  }
  return(0);
}
/*****************************************************************************/
 
/*****************************************************************************/
void praxisSort(
  double *d, 
  double **v,
  unsigned int n
) {
  unsigned int k, i, j;
  double s;
 
  for(i=0; i<n-1; i++) {
    k=i; s=d[i];
    for(j=i+1; j<n; j++) {if(d[j]>s) {k=j; s=d[j];}}
    if(k>i) {
      d[k]=d[i]; d[i]=s;
      for(j=0; j<n; j++) {s=v[j][i]; v[j][i]=v[j][k]; v[j][k]=s;}
    }
  }
}
/*****************************************************************************/
 
/*****************************************************************************/