/* See sym_eigen_trid_vectors.h */
/* Last edited on 2024-12-02 14:49:32 by stolfi */

#include <stdlib.h>
#include <stdio.h>
#include <stdint.h>
#include <assert.h>
#include <math.h>

#include <affirm.h>
#include <bool.h>

#include <sym_eigen_trid_vectors.h>

#define Pr fprintf
#define Er stderr
  /* Shoter names. */

void sym_eigen_trid_vectors(int32_t n, double *d, double *e, double *R, int32_t *p, int32_t absrt)
  {
    /* The algorithm is based on the EISPACK routines "tql1.f" and
      "tql2.f" by Burton S. Garbow, Argonne National Laboratory
      (aug/1983); orignally from the ALGOL procedures {tql1} and
      {tql2}, Num. Math. 11, 293-306(1968) by Bowdler, Martin,
      Reinsch, and Wilkinson. See Handbook for Auto. Comp., vol.II -
      Linear Algebra, 227-240(1971). Re-implemented in C by Jorge
      Stolfi, Unicamp (dec/2002). */
    
    bool_t debug = FALSE;
    
    if (debug) { Pr(Er, "  > --- %s  n = %d ---\n", __FUNCTION__, n); }
    
    double f, magn;
    int32_t maxiter = 30;
    
    if (n == 1) { *p = 1; return; }
    /* Downshift {e} one slot (so {e[i] = T[i+1,i]}), and clear {e[n-1]}: */
    for (int32_t i = 1;  i < n; i++) { e[i-1] = e[i]; }
    e[n-1] = 0.0;

    f = 0.0;
    magn = 0.0;
    int32_t L = 0;
    while(L < n)
      { if (debug) { Pr(Er, "    ...... L = %d  f = %24.16e  magn = %24.16e ......\n", L, f, magn); }
        int32_t niter = 0;
        { double w = fabs(d[L]) + fabs(e[L]); if (magn < w) { magn = w; } }
        double test = magn + fabs(e[L]);
        if (debug) { Pr(Er, "      magn = %24.16e  e[L] = %24.16f  test1 = %24.16f\n", magn, e[L], test); }
        if (test != magn)
          { /* Can't be at the sentinal {e[n-1]}. */
            assert(L < n-1); 
            /* Must clear {e[L]} */
            /* look for small sub-diagonal element {e[t]} in cols {L+1..n-1}; */
            /* Note that it will always find {e[n-1] == 0} as last resort. */
            int32_t t = L;
            do { t++; } while ((magn + fabs(e[t])) != magn);
            if (debug) { Pr(Er, "      (0) t = %d\n", t); }
            if (debug) { Pr(Er, "      magn = %24.16e  e[t] = %24.16f  test = %24.16f\n", magn, e[t], test); }
            assert((L >= 0) && (t > L) && (t < n));
            /* Try to anihilate {e[L]}: */
            do
              { /* Compute shift: */
                int32_t L1 = L+1;
                double g = d[L];
                double pp = (d[L1] - g)/(2.0*e[L]);
                double r = hypot(pp, 1.0);
                double s2 = 0, c3 = 0;
                d[L] = e[L]/(pp + copysign(r, pp));
                d[L1] = e[L]*(pp + copysign(r, pp));
                double h = g - d[L];
                if (debug) { Pr(Er, "        (1) h =  %24.16e  d[%d] = %24.16e  d[%d] = %24.16e\n", h, L, d[L], L1, d[L1]); }
                for (int32_t i = L + 2; i < n; i++) { d[i] -= h; }
                f = f + h;
                /* QL transformation: */
                pp = d[t];
                double c2 = 1.0;
                double c = 1.0; 
                double s = 0.0;
                double dL1 = d[L1]; 
                double eL1 = e[L1];
                if (debug) { Pr(Er, "        (2) pp = %24.16e  e[%d] = %24.16e\n", pp, L1, e[L1]); }
                /* Note {int} not {int} in case loop ends at -1. */
                int32_t i = t;
                while (i > L)
                  { assert(i > 0);
                    i--; /* safe here because {i > 0}. */
                    int32_t rowi = n*i;
                    int32_t rowi1 = n*(i+1);
                    if (debug) { Pr(Er, "          (3) i = %d  rowi = %d rowi1 = %d\n", i, rowi, rowi1); }
                    c3 = c2; 
                    c2 = c; s2 = s;
                    g = c*e[i];
                    h = c*pp;
                    r = hypot(pp, e[i]);
                    e[i+1] = s*r;
                    s = e[i]/r; c = pp/r;
                    pp = c*d[i] - s*g;
                    d[i+1] = h + s*(c*g + s*d[i]);
                    if (debug) { Pr(Er, "          (4) i = %d  d[%d] = %24.16e\n", i, i+1, d[i+1]); }
                    /* Update eigenvectors: */
                    for (int32_t j = 0;  j < n; j++)
                      { double R0 = R[rowi + j], R1 = R[rowi1 + j];
                        R[rowi  + j] = c*R0 - s*R1;
                        R[rowi1 + j] = s*R0 + c*R1;
                      }
                  }
                if (debug) { Pr(Er, "        (5) eL1 = %24.16e  e[%d] = %24.16f  dL1 = %24.16f\n", eL1, L, e[L], dL1); }
                pp = - s*s2*c3*eL1*e[L]/dL1;
                e[L] = s*pp; 
                d[L] = c*pp;
                if (debug) { Pr(Er, "        (6) e[%d] = %24.16f  d[%d] = %24.16f\n", L, e[L], L, d[L]); }
                niter++;
                test = magn + fabs(e[L]);
                if (debug) { Pr(Er, "        (7) magn = %24.16f  test = %24.16f\n", magn, test); }
              }
            while ((niter < maxiter) && (test > magn));
          }
        if (test != magn) { /* Failed to converge: */ break; }
        d[L] = d[L] + f;
        if (debug) { Pr(Er, "      (8) d[%d] = %24.16f\n", L, d[L]); }
        L++;
      }
    /* We did what we could: */
    assert(L >= 0);
    *p = (int32_t)L;
    if (debug) { Pr(Er, "      (9) *p = %d\n", *p); }
    /* Sort the eigenvalues {d[0..L-1]}, carrying along their eigenvectors: */
    for (int32_t i = 0; i < L; i++)
      { /* Find {(i+1)}th smallest eigenvalue {dk = d[k]} */
        int32_t k = i; 
        double dk = d[i];
        if (absrt) { dk = fabs(dk); }
        for (int32_t j = i+1; j < L; j++) 
          { double dj = d[j];
            if (absrt) { dj = fabs(d[j]); }
            if (dj < dk) { k = j; dk = dj; }
          }
        if (k != i)
          { /* Swap {d[k], R[k,*]} with {d[i], R[i,*]}: */
            d[k] = d[i]; d[i] = dk;
            int32_t rowi = n*i;
            int32_t rowk = n*k;
            for (int32_t j = 0;  j < n; j++)
              { double Rij = R[rowi+j]; R[rowi+j] = R[rowk+j]; R[rowk+j] = Rij; }
          }
      }
  }