/* See rmxn.h. */
/* Last edited on 2005-01-16 15:19:14 by stolfi */
/* Copyright © 2005 Jorge Stolfi, UNICAMP. See note at end of file. */

#include <rmxn.h>
#include <rn.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <js.h>
#include <gauss_elim.h>

void rmxn_zero(int m, int n, double *R)
  { int i, j, t;
    t = 0;
    for (i = 0; i < m; i++)
      for (j = 0; j < n; j++)
        { R[t] = 0.0; t++; }
  }

void rmxn_ident(int m, int n, double *R)
  { int i, j, t;
    t = 0;
    for (i = 0; i < m; i++)
      for (j = 0; j < n; j++)
        { R[t] = (i == j ? 1.0 : 0.0); t++; }
  }

void rmxn_map_row (int m, int n, double *x, double *A, double *r)
  { int i, j;
    for (j = 0; j < n; j++)
      { double sum = 0.0, corr = 0.0; 
        int t = j;
        for (i = 0; i < m; i++) 
          { double term = x[i] * A[t];
            /* Kahan's summation: */
            double tcorr = term - corr;
            double newSum = sum + tcorr;
            corr = (newSum - sum) - tcorr;
            sum = newSum;
            t += n;
          }
        r[j] = sum;
      }
  }

void rmxn_map_col (int m, int n, double *A, double *x, double *r)
  { int i, j, t = 0;
    for (i = 0; i < m; i++)
      { double sum = 0.0, corr = 0.0; 
        for (j = 0; j < n; j++)
          { double term = A[t] * x[j]; 
            /* Kahan's summation: */
            double tcorr = term - corr;
            double newSum = sum + tcorr;
            corr = (newSum - sum) - tcorr;
            sum = newSum;
            t++;
          }
        r[i] = sum;
      }
  }

void rmxn_mul (int m, int p, int n, double *A, double *B, double *R)
  { int i, j, k, r = 0, v = 0;
    for (i = 0; i < m; i++)
      { for (j = 0; j < n; j++)
          { double sum = 0.0, corr = 0.0;
            int t = j;
            for (k = 0; k < p; k++)
              { double term = A[r+k]*B[t];
                /* Kahan's summation: */
                double tcorr = term - corr;
                double newSum = sum + tcorr;
                corr = (newSum - sum) - tcorr;
                sum = newSum;
                t+= n;
              }
            R[v] = sum; v++;
          }
        r += p;
      }
  }

void rmxn_mul_tr (int m, int n, int p, double *A, double *B, double *R)
  { int i, j, k, r = 0, s, v = 0;
    for (i = 0; i < m; i++)
      { s = 0;
        for (j = 0; j < n; j++)
          { double sum = 0.0, corr = 0.0;
            for (k = 0; k < p; k++) 
              { double term = A[r+k]*B[s+k];
                /* Kahan's summation: */
                double tcorr = term - corr;
                double newSum = sum + tcorr;
                corr = (newSum - sum) - tcorr;
                sum = newSum;
              }
            R[v] = sum; v++;
            s += p;
          }
        r += p;
      }
  }

double rmxn_det (int n, double *A)
  { int n2 = n*n, t;
    double *C = (double *)notnull(malloc(n2*sizeof(double)), "no mem for C");
    double d = 1.0;
    for (t = 0; t < n2; t++) { C[t] = A[t]; }
    gsel_triangularize(n, n, C, 0);
    for (t = 0; t < n2; t += n+1) { d *= C[t]; }
    free(C);
    return d;
  }

void rmxn_inv (int n, double *A, double *R)
  { int i, j;
    int n2 = n*n;
    double *C = (double *)notnull(malloc(2*n2*sizeof(double)), "no mem for C");
    /* Copy {A} into the left half of {C}, fill the right half with the identity: */
    for (i = 0; i < n; i++) 
      { double *Cij = &(C[2*n*i]); 
        double *Aij = &(A[n*i]);
        for (j = 0; j < n; j++) { (*Cij) = (*Aij); Cij++; Aij++; }
        for (j = 0; j < n; j++) { (*Cij) = (j == i ? 1.0 : 0.0); Cij++; }
      }
    gsel_triangularize(n, 2*n, C, 0);
    gsel_diagonalize(n, 2*n, C, 0);
    gsel_normalize(n, 2*n, C, 0);
    for (i = 0; i < n; i++) 
      { double *Cij = &(C[2*n*i + n]); 
        double *Rij = &(R[n*i]);
        for (j = 0; j < n; j++) { (*Rij) = (*Cij); Rij++; Cij++; }
      }
    free(C);
  }

void rmxn_scale(int m, int n, double s, double *A, double *R) 
  { int i, j, k = 0;
    for (i = 0; i < m; i++)
      { for (j = 0; j < n; j++)
          { R[k] = s*A[k]; k++; }
      }
  }

void rmxn_mix(int m, int n, double s, double *A, double t, double *B, double *R)
  { int i, j, k = 0;
    for (i = 0; i < m; i++)
      { for (j = 0; j < n; j++)
          { R[k] = s*A[k] + t*B[k]; k++; }
      }
  }

void rmxn_rel_diff(int m, int n, double *A, double *B, double *R)
  { int i, j, k = 0;
    for (i = 0; i < m; i++)
      { for (j = 0; j < n; j++)
          { R[k] = rel_diff(A[k], B[k]); k++; }
      }
  }

void rmxn_LT_inv_map_row(int n, double *y, double *L, double *r)
  { int i, j;
    for (j = n-1; j >= 0; j--)
      { double sum = y[j], corr = 0.0;
        for (i = j+1; i < n; i++)
          { double term = -r[i]*L[n*i + j];
            /* Kahan's summation: */
            double tcorr = term - corr;
            double newSum = sum + tcorr;
            corr = (newSum - sum) - tcorr;
            sum = newSum;
          }
        r[j] = sum/L[n*j + j];
      }
  }

void rmxn_LT_inv_map_col(int m, double *L, double *y, double *r)
  { int i, j;
    for (i = 0; i < m; i++)
      { double sum = y[i], corr = 0.0;
        for (j = 0; j < i; j++)
          { double term = -L[m*i + j]*r[j];
            /* Kahan's summation: */
            double tcorr = term - corr;
            double newSum = sum + tcorr;
            corr = (newSum - sum) - tcorr;
            sum = newSum;
          }
        r[i] = sum/L[m*i + i];
      }
  }

void rmxn_LT_pos_div(int m, int n, double *A, double *L, double *R)
  { int i, j, k;
    for (k = 0; k < m; k++)
      { for (j = n-1; j >= 0; j--)
          { double sum = A[n*k + j], corr = 0.0;
            for (i = j+1; i < n; i++)
              { double term = -R[n*k + i]*L[n*i + j];
                /* Kahan's summation: */
                double tcorr = term - corr;
                double newSum = sum + tcorr;
                corr = (newSum - sum) - tcorr;
                sum = newSum;
              }
            R[n*k + j] = sum/L[n*j + j];
          }
      }
  }

void rmxn_LT_pre_div(int m, int n, double *L, double *A, double *R)
  { int i, j, k;
    for (k = 0; k < n; k++)
      { for (i = 0; i < m; i++)
          { double sum = A[n*i + k], corr = 0.0;
            for (j = 0; j < i; j++)
              { double term = -L[m*i + j]*R[n*j + k];
                /* Kahan's summation: */
                double tcorr = term - corr;
                double newSum = sum + tcorr;
                corr = (newSum - sum) - tcorr;
                sum = newSum;
              }
            R[n*i + k] = sum/L[m*i + i];
          }
      }
  }

void rmxn_cholesky(int n, double *A, double *L)
  { 
    /* Andre-Louis Cholesky (spelled with a 'y'), born in France in
      1875, was a geodesist in the French military. He developed his
      computational procedure to solve geodetic problems, among which
      was, in the words of his eulogy, the "problem of levelling in
      Morocco." He died in battle in 1918. Benoit published the
      computational method in "Bulletin geodesique" in 1924. 
        -- David Pattison, Washington, DC

      Although the name is originally Polish or Russian, Cholesky 
      probably pronounced it himself in the French fashion, i.e.
      with an "sh" sound, as in "Chopin" and "Chostakovich"; and not
      with a "tsh" sound --- which in French would be spelled "Tch",
      as in "Tchaikovski" or "Tchekov". */
  
    int i, j, k;

    for (i = 0; i < n; i++)
      { double Lii;
        /* Compute {L[i,j] = X[i,j] - SUM{ L[i,k]*A[j,k] : k = 0..j-1 })/L[j,j]} */
        /* where X[i,j] = (j <= i ? A[i,j] : 0) */
        for (j = 0; j < i; j++)
          { double sum = 0.0, corr = 0.0;
            for (k = 0; k < j; k++)
              { double term = L[n*i + k]*L[n*j + k];
                /* Kahan's summation formula: */
                double tcorr = term - corr;
                double newSum = sum + tcorr;
                corr = (newSum - sum) - tcorr;
                sum = newSum;
              }
            { double Ljj = L[n*j + j];
              affirm(Ljj != 0.0, "zero element in diagonal");
              L[n*i + j] = (A[n*i + j] - sum)/Ljj;
            } 
          }
        
        /* Compute {L[i,i] = sqrt(A[i,i] - SUM{ L[i,j]^2 : j = 0..i-1 })} */
        { double sum = 0.0, corr = 0.0;
          for (j = 0; j < i; j++)
            { double w = L[n*i+j], term = w*w; 
              /* Kahan's summation formula: */
              double tcorr = term - corr;
              double newSum = sum + tcorr;
              corr = (newSum - sum) - tcorr;
              sum = newSum;
            }
          Lii = A[n*i+i] - sum;
        }
        affirm (Lii >= 0.0, "matrix is not positive definite?");
        L[n*i + i] = sqrt(Lii);
        for (j = i+1; j < n; j++) { L[n*i + j] = 0.0; }
      }
  }

void rmxn_print (FILE *f, int m, int n, double *A)
  { rmxn_gen_print(f, m, n, A, NULL, NULL, NULL, NULL, NULL, NULL, NULL); }

void rmxn_gen_print
  ( FILE *f, int m, int n, double *A,
    char *fmt, 
    char *olp, char *osep, char *orp,
    char *ilp, char *isep, char *irp
  )
  {
    int i,j, t;
    if (olp == NULL) { olp = "(\n"; }
    if (osep == NULL) { osep = "\n"; }
    if (orp == NULL) { orp = ")"; }
    if (ilp == NULL) { ilp = "  ("; }
    if (isep == NULL) { isep = " "; }
    if (irp == NULL) { irp = ")"; }
    if (fmt == NULL) { fmt = "%16.8e"; }
    fputs(olp, f);
    t = 0;
    for (i = 0; i < m; i++)
      {
        if (i > 0) { fputs(osep, f); }
        fputs(ilp, f);
        for (j = 0; j < n; j++) 
          { if (j > 0) { fputs(isep, f); }
            fprintf(f, fmt, A[t]); t++;
          }
        fputs(irp, f);
      }
    fputs(orp, f);
    fflush(f);
  }  


double *rmxn_alloc(int m, int n)
  { void *p = malloc(m*n*sizeof(double));
    affirm(p != NULL, "no memory for rmxn_t");
    return (double *)p;
  }
/* COPYRIGHT AND AUTHORSHIP NOTICE

  Copyright © 2005 Jorge Stolfi, Universidade Estadual de Campinas (UNICAMP).
  Created by Jorge Stolfi in 1992--2005.
  
  This source file can be freely distributed, used, and modified,
  provided that this copyright and authorship notice is preserved in
  all copies, and that any modified versions of this file are clearly
  marked as such.
  
  This software has NO WARRANTY of correctness or applicability for
  any purpose. Neither the author nor his employers shall be held
  responsible for any losses or damages that may result from its use.

  END OF NOTICE */