#include <stdio.h>
#include <stdlib.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_linalg.h>
 

void test_hesstri(int);
int linalg_hesstri_decomp(gsl_matrix * A, gsl_matrix * B, gsl_matrix * U, gsl_matrix * V, gsl_vector * work, int do_round);
void create_givens (const double a, const double b, double *c, double *s);
void print_matrix(gsl_matrix *);
void print_vector(gsl_vector *);
void apply_threshold(gsl_matrix *, double);

int main (void) {
  test_hesstri(0);
  test_hesstri(1);
 
  return 0;
}

void test_hesstri(int do_round) {
  //int n = 4;
  int n = 3;
  gsl_matrix *A = gsl_matrix_alloc(n, n);
  gsl_matrix *B = gsl_matrix_alloc(n, n);


  gsl_matrix_set(A, 0, 0, 10);
  gsl_matrix_set(A, 0, 1, 1);
  gsl_matrix_set(A, 0, 2, 2);
  gsl_matrix_set(A, 1, 0, 1);
  gsl_matrix_set(A, 1, 1, 2);
  gsl_matrix_set(A, 1, 2, -1);
  gsl_matrix_set(A, 2, 0, 1);
  gsl_matrix_set(A, 2, 1, 1);
  gsl_matrix_set(A, 2, 2, 2);


  gsl_matrix_set(B, 0, 0, 1);
  gsl_matrix_set(B, 0, 1, 2);
  gsl_matrix_set(B, 0, 2, 3);
  gsl_matrix_set(B, 1, 0, 4);
  gsl_matrix_set(B, 1, 1, 5);
  gsl_matrix_set(B, 1, 2, 6);
  gsl_matrix_set(B, 2, 0, 7);
  gsl_matrix_set(B, 2, 1, 8);
  gsl_matrix_set(B, 2, 2, 9);

  gsl_matrix *U = gsl_matrix_alloc(n, n);
  gsl_matrix *V = gsl_matrix_alloc(n, n);
 
  gsl_vector *work = gsl_vector_alloc(n);

  linalg_hesstri_decomp(A, B, U, V, work, do_round);

  printf(":::::::::::::::::::::::::::::::::::::::\n");
  printf("[D]Matriz A:\n");
  print_matrix(A);
  printf("[D]Matriz B:\n");
  print_matrix(B);
  printf("[D]Matriz U:\n");
  print_matrix(U);
  printf("[D]Matriz V:\n");
  print_matrix(V);
  printf("Vector work:\n");
  print_vector(work);
}


int linalg_hesstri_decomp(gsl_matrix * A, gsl_matrix * B, gsl_matrix * U, gsl_matrix * V, gsl_vector * work, int do_round) {
  const double eps = 1e-8;
  const size_t N = A->size1;

  if ((N != A->size2) || (N != B->size1) || (N != B->size2))
    {
      GSL_ERROR ("Hessenberg-triangular reduction requires square matrices",
                 GSL_ENOTSQR);
    }
  else if (N != work->size)
    {
      GSL_ERROR ("length of workspace must match matrix dimension",
                 GSL_EBADLEN);
    }
  else
    {
      double cs, sn;          /* rotation parameters */
      size_t i, j;            /* looping */
      gsl_vector_view xv, yv; /* temporary views */

      /* B -> Q^T B = R (upper triangular) */
      gsl_linalg_QR_decomp(B, work);
      if (do_round) {
        apply_threshold(B, eps);
      }
      /* A -> Q^T A */
      gsl_linalg_QR_QTmat(B, work, A);
      if (do_round) {
        apply_threshold(A, eps);
      }
      /* initialize U and V if desired */
      if (U) {
          gsl_linalg_QR_unpack(B, work, U, B);
        }
      else
        {
          /* zero out lower triangle of B */
          for (j = 0; j < N - 1; ++j)
            {
              for (i = j + 1; i < N; ++i)
                gsl_matrix_set(B, i, j, 0.0);
            }
        }

      if (V)
        gsl_matrix_set_identity(V);

      if (N < 3)
        return GSL_SUCCESS; /* nothing more to do */

      /* reduce A and B */
      for (j = 0; j < N - 2; ++j) {
          for (i = N - 1; i >= (j + 2); --i)
            {
              /* step 1: rotate rows i - 1, i to kill A(i,j) */

              /*
               * compute G = [ CS SN ] so that G^t [ A(i-1,j) ] = [ * ]
               *             [-SN CS ]             [ A(i, j)  ]   [ 0 ]
               */
              create_givens(gsl_matrix_get(A, i - 1, j),
                            gsl_matrix_get(A, i, j),
                            &cs,
                            &sn);
              /* invert so drot() works correctly (G -> G^t) */
              sn = -sn;
              /* compute G^t A(i-1:i, j:n) */
              xv = gsl_matrix_subrow(A, i - 1, j, N - j);
              yv = gsl_matrix_subrow(A, i, j, N - j);
              gsl_blas_drot(&xv.vector, &yv.vector, cs, sn);

              /* compute G^t B(i-1:i, i-1:n) */
              xv = gsl_matrix_subrow(B, i - 1, i - 1, N - i + 1);
              yv = gsl_matrix_subrow(B, i, i - 1, N - i + 1);
              gsl_blas_drot(&xv.vector, &yv.vector, cs, sn);

              if (U) {
                /* accumulate U: U -> U G */
                xv = gsl_matrix_column(U, i - 1);
                yv = gsl_matrix_column(U, i);
                gsl_blas_drot(&xv.vector, &yv.vector, cs, sn);
              }

              /* step 2: rotate columns i, i - 1 to kill B(i, i - 1) */

              create_givens(-gsl_matrix_get(B, i, i),
                            gsl_matrix_get(B, i, i - 1),
                            &cs,
                            &sn);
             
              /* invert so drot() works correctly (G -> G^t) */
              sn = -sn;
              /* compute B(1:i, i-1:i) G */
              xv = gsl_matrix_subcolumn(B, i - 1, 0, i + 1);
              yv = gsl_matrix_subcolumn(B, i, 0, i + 1);
              gsl_blas_drot(&xv.vector, &yv.vector, cs, sn);

              /* apply to A(1:n, i-1:i) */
              xv = gsl_matrix_column(A, i - 1);
              yv = gsl_matrix_column(A, i);
              gsl_blas_drot(&xv.vector, &yv.vector, cs, sn);
             
              if (V)
                {
                  /* accumulate V: V -> V G */
                  xv = gsl_matrix_column(V, i - 1);
                  yv = gsl_matrix_column(V, i);
                  gsl_blas_drot(&xv.vector, &yv.vector, cs, sn);
                }
            }
        }
    }
  return GSL_SUCCESS;
}

void create_givens (const double a, const double b, double *c, double *s) {
  if (b == 0) {
    *c = 1;
    *s = 0;
  } else if (fabs (b) > fabs (a)) {
    double t = -a / b;
    double s1 = 1.0 / sqrt (1 + t * t);
    *s = s1;
    *c = s1 * t;
  } else {
    double t = -b / a;
    double c1 = 1.0 / sqrt (1 + t * t);
    *c = c1;
    *s = c1 * t;
  }
}

void apply_threshold(gsl_matrix *A, double eps) {
  int i, j;
  for (i = 0; i < A->size1; i++) {
    for (j = 0; j < A->size2; j++) {
      if (fabs(gsl_matrix_get(A, i, j)) <= eps) {
        gsl_matrix_set(A, i, j, 0.);
      }
    }
  }
}

void print_matrix(gsl_matrix *A) {
  int i, j;

  for (i = 0; i < A->size1; i++) {
    for (j = 0; j < A->size2; j++) {
      printf("\t%.8f", gsl_matrix_get(A, i, j));
    }
    printf("\n");
  }
}

void print_vector(gsl_vector *V) {
  int i;

  for (i = 0; i < V->size; i++) {
    printf("\t%.8f", gsl_vector_get(V, i));
  }
  printf("\n");
}

void print_vector_complex(gsl_vector_complex *V) {
  int i;

  for (i = 0; i < V->size; i++) {
    gsl_complex z = gsl_vector_complex_get (V, i);
    printf("\t(%.8f,%.8fi)", GSL_REAL(z), GSL_IMAG(z));
  }
  printf("\n");
}