/*
  Author:  Pate Williams (c) 1997

  "Algorithm 2.2.9 (Hessenberg). Given an n by n
  matrix M = (m[i][j]) with coefficients in the
  field, this algorithm determines the character-
  istic polynomial of M by first transforming M
  into a Hessenberg matrix."-Henri Cohen- See "A
  Course in Computational Algebraic Number Theory"
  by Henri Cohen page 56. We specialize to the
  field Q (the rational numbers).
*/

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

double **create_square_matrix(long n)
{
  double **matrix = calloc(n, 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 < n; 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 n, double **matrix)
{
  long i;

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

void poly_mul(long m, long n, double *a, double *b,
              double *c, long *p)
{
  long i, j, k;
  double ai, bj, sum;

  *p = m + n;
  for (k = 0; k <= *p; k++) {
    sum = 0.0;
    for (i = 0; i <= k; i++) {
      j = k - i;
      if (i > m) ai = 0.0;
      else ai = a[i];
      if (j > n) bj = 0.0;
      else bj = b[j];
      sum += ai * bj;
    }
    c[k] = sum;
  }
}

void Hessenberg(long n, double *p1, double **H)
{
  double t, temp, u;
  double *q = calloc(n + 1, sizeof(double));
  double **P = create_square_matrix(n + 1);
  int found;
  long dp0, dp1, dq, i, j, m = 1, m1, mi;

  L2:
  i = m + 1;
  m1 = m - 1;
  found = 0;
  while (!found && i < n) {
    found = H[i][m1] != 0;
    if (!found) i++;
  }
  if (!found) goto L4;
  t = H[i][m1];
  if (i > m) {
    for (j = m1; j < n; j++)
      temp = H[i][j], H[i][j] = H[m][j], H[m][j] = temp;
    for (j = 0; j < n; j++)
      temp = H[j][i], H[j][i] = H[j][m], H[j][m] = temp;
  }
  for (i = m + 1; i < n; i++) {
    if (H[i][m1] != 0.0) {
      u = H[i][m1] / t;
      for (j = m; j < n; j++) {
        H[i][j] -= u * H[m][j];
        H[i][m1] = 0.0;
      }
      for (j = 0; j < n; j++)
        H[j][m] += u * H[j][i];
    }
  }
  L4:
  if (m < n - 2) {
    m++;
    goto L2;
  }
  P[0][0] = 1.0;
  m = 1;
  L6:
  dp0 = m - 1;
  q[0] = - H[m - 1][m - 1];
  q[1] = 1.0;
  dq = 1;
  poly_mul(dq, dp0, q, P[m - 1], P[m], &dp1);
  t = 1.0;
  for (i = 1; i <= m - 1; i++) {
    mi = m - i;
    t = t * H[mi][mi - 1];
    temp = t * H[mi - 1][m - 1];
    for (j = 0; j <= mi - 1; j++)
      P[m][j] -= temp * P[mi - 1][j];
  }
  if (m < n) {
    m++;
    goto L6;
  }
  for (i = 0; i <= n; i++) p1[i] = P[n][i];
  delete_matrix(n, P);
  free(q);
}

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

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

int main(void)
{
  long i, j, n = 4;
  double a[4][4] = {{- 2, - 2, -  1,   2},
                    {- 1, - 3, - 13, - 2},
                    {- 1,   0,    2,   0},
                    {- 8, - 8, - 14,   3}};
  double p1[5];
  double **H = create_square_matrix(n);

  for (i = 0; i < n; i++)
    for (j = 0; j < n; j++)
      H[i][j] = a[i][j];
  printf("the original matrix is:\n");
  print_square_matrix(n, H);
  Hessenberg(n, p1, H);
  printf("the upper Hessenberg matrix is:\n");
  print_square_matrix(n, H);
  printf("the characteristic polynomial is:\n");
  for (i = n; i >= 0; i--)
    printf("%+10.6lf ", p1[i]);
  printf("\n");
  delete_matrix(n, H);
  return 0;
}