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

#include <gauss_elim.h>
#include <js.h>
#include <math.h>

void gsel_triangularize(int m, int n, double *A, int total)
  { int i = 0;
    int j = 0;
    double *Aij = &(A[0]);
    while ((i < m-1) && (j < n))
      { /* Elements {A[r][s]} with {r >= i} and {s < j} are all zero. */
        /* Clear elements {A[k][j]} with {k > i} */
        int k; double *Akj;
        for (k = i+1, Akj = Aij + n; k < m; k++, Akj += n)
          { /* Swap rows {i} and {k} if needed, negating one of them: */
            if (fabs(*Akj) > fabs(*Aij))
              { int r; double *Akr; double *Air;
                for (r = j, Akr = Akj, Air = Aij; r < n; r++, Akr++, Air++)
                  { double t = (*Akr); (*Akr) = - (*Air); (*Air) = t; }
              }
            /* Subtract from row {k} a multiple of row {i} that cancels {A[k,j]}: */
            if ((*Akj) != 0.0)
              { int r; double *Akr; double *Air;
                double s = (*Akj)/(*Aij);
                (*Akj) = 0.0;
                for (r = j+1, Akr = Akj+1, Air = Aij+1; r < n; r++, Akr++, Air++)
                  { (*Akr) -= s*(*Air); }
              }
          }
        if ((! total) || ((*Aij) != 0.0)) { i++; Aij += n; }
        j++; Aij++;
      }
  }

void gsel_diagonalize(int m, int n, double *A, int total)
  { int i = 0;
    int j = 0;
    double *Aij = &(A[0]);
    while ((i < m) && (j < n))
      { if ((*Aij) != 0.0)
          { /* Clear elements {A[k][j]} with {k < i} */
            int k; double *Akj;
            for (k = i-1, Akj = Aij-n; k >= 0; k--, Akj -= n)
              { /* Sub from row {k} a multiple of row {i} that cancels {A[k,j]}: */
                if ((*Akj) != 0.0)
                  { int r; double *Akr; double *Air;
                    double s = (*Akj)/(*Aij);
                    (*Akj) = 0.0;
                    for (r = j+1, Akr = Akj+1, Air = Aij+1; r < n; r++, Akr++, Air++)
                      { (*Akr) -= s*(*Air); }
                  }
              }
            i++; Aij += n;
          }
        else if (! total)
          { i++; Aij += n; }
        j++; Aij++;
      }
  }

void gsel_normalize(int m, int n, double *A, int total)
  { int i = 0;
    int j = 0;
    double *Aij = &(A[0]);
    while ((i < m) && (j < n))
      { if ((*Aij) != 0.0)
          { /* Scale row {i} by {1/A[i,j]}: */
            int r; double *Air;
            double s = (*Aij);
            (*Aij) = 1.0;
            for (r = j+1, Air = Aij+1; r < n; r++, Air++)
              { (*Air) /= s; }
            i++; Aij += n;
          }
        else if (! total)
          { i++; Aij += n; }
        j++; Aij++;
      }
  }

void gsel_extract_solution(int m, int n, double *A, int p, double *X, int total)
  { affirm(n >= m+p, "wrong dimensions");
    int q = n-p;
    /* Scan the leading elements of {A}: */
    int i = 0, j = 0;
    double *Aij = &(A[0]);
    while ((i < m) && (j < q))
      { double *Xjk = &(X[p*i]);
        double *Uik = &(A[n*i + q]);
        int k;
        for (k = 0; k < p; k++, Xjk++, Uik++) 
          { (*Xjk) = ((*Aij) == 0.0 ? 0.0 : -(*Uik)/(*Aij)); }
        if ((! total) || ((*Aij) != 0.0)) { i++; Aij += n; }
        j++; Aij++;
      }
  }
/* 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 */