/*
  Author:  Pate Williams (c) 1997

  "Algorithm 2.3.6 (Supplement of a Basis). Given an
  n by k matrix M with k <= n having coefficients in
  the field K, this algotithm either outputs a message
  saying that M is of rank less than k, or outputs an
  invertible n by n matrix B sucha that the first k
  columns of B form the matrix M." -Henri Cohen- See
  "A Course in Computational Algebraic Number Theory"
  pages 61-62.
*/

#include <stdio.h>
#include <stdlib.h>

#define DEBUG

double **create_matrix(long m, long n)
{
  double **matrix = calloc(m, sizeof(double *));
  long i;

  if (!matrix) {
    fprintf(stderr, "fatal error\ninsufficient memory\n");
    fprintf(stderr, "from create_matrix\n");
    exit(1);
  }
  for (i = 0; i < m; i++) {
    matrix[i] = calloc(n, sizeof(double));
    if (!matrix[i]) {
      fprintf(stderr, "fatal error\ninsufficient memory\n");
      fprintf(stderr, "from create_matrix\n");
      exit(1);
    }
  }
  return matrix;
}

void delete_matrix(long m, double **matrix)
{
  long i;

  for (i = 0; i < m; i++) free(matrix[i]);
  free(matrix);
}

void matrix_multiply(long m, long n, long r,
                     double **a, double **b,
                     double **c)
/* c = a * b */
{
  double sum;
  long i, j, k;

  for (i = 0; i < m; i++) {
    for (j = 0; j < r; j++) {
      sum = 0.0;
      for (k = 0; k < n; k++)
        sum += a[i][k] * b[k][j];
      c[i][j] = sum;
    }
  }
}

void print_matrix(long m, long n, double **a)
{
  long i, j;

  for (i = 0; i < m; i++) {
    for (j = 0; j < n; j++)
      printf("%+10.6lf ", a[i][j]);
    printf("\n");
  }
}

void supplement_basis(long n, long k, double **B,
                      double **M)
{
  double d, temp;
  double **CM = create_matrix(n, k);
  double **BM = create_matrix(n, k);
  int found;
  long i, j, s, t;

  /* make a local copy the original matrix */
  for (i = 0; i < n; i++)
    for (j = 0; j < k; j++)
      CM[i][j] = M[i][j];
  /* initialize B to the n by n identity matrix */
  for (i = 0; i < n; i++) {
    for (j = 0; j < n; j++) B[i][j] = 0.0;
    B[i][i] = 1.0;
  }
  for (s = 0; s < k; s++) {
    found = 0, t = s;
    while (!found && t < n) {
      found = CM[t][s] != 0.0;
      if (!found) t++;
    }
    if (!found) {
      fprintf(stderr, "fatal error\nthe matrix M is of rank less than k\n");
      fprintf(stderr, "from supplement_basis\n");
      exit(1);
    }
    d = 1.0 / CM[t][s];
    CM[t][s] = 1.0;
    if (t != s)
      for (i = 0; i < n; i++) B[i][t] = B[i][s];
    for (i = 0; i < n; i++) B[i][s] = M[i][s];
    for (j = 0; j < k; j++)
      for (i = 0; i < n; i++)
        if (i != s && i != t && i != j)
          CM[i][j] = 0.0;
    for (j = s + 1; j < k; j++) {
      if (t != s)
        temp = CM[s][j], CM[s][j] = CM[t][j], CM[t][j] = temp;
      d *= CM[s][j];
      for (i = 0; i < n; i++) {
        if (i != s && i != t)
          CM[i][j] -= CM[i][s] * d;
        else CM[i][j] = 0.0;
      }
    }
    #ifdef DEBUG
    matrix_multiply(n, n, k, B, CM, BM);
    printf("CM is:\n");
    print_matrix(n, k, BM);
    #endif
  }
  #ifdef DEBUG
  printf("C final is:\n");
  print_matrix(n, k, CM);
  #endif
  delete_matrix(n, BM);
  delete_matrix(n, CM);
}

int main(void)
{
  long i, j, k = 3, n = 4;
  double a[4][3] = {{1, 0, 2},
                    {0, 3, 4},
                    {5, 6, 7},
                    {8, 0, 9}};
  double **B = create_matrix(n, n);
  double **M = create_matrix(n, k);

  for (i = 0; i < n; i++)
    for (j = 0; j < k; j++)
      M[i][j] = a[i][j];
  printf("the original matrix is:\n");
  print_matrix(n, k, M);
  supplement_basis(n, k, B, M);
  printf("the supplement basis is:\n");
  print_matrix(n, n, B);
  delete_matrix(n, B);
  delete_matrix(n, M);
  return 0;
}