#include <math.h>

#define min(a,b) ((a)<(b)?(a):(b))

/* Redukuje rzeczywista macierz symetryczna do postaci trójdiagonalnej */
/* za pomoca obrotow Householdera. Macierze niesymetryczne sa          */
/* redukowane do postaci Hessenberga                                   */
int trdreduce( int N, float **A)
{
  float *u,*s,*w;
  float K,norm,mult;
  int k,kp1;
  int i,j;

  
  /* alokujemy pamięć na wektory pomocnicze */
  u = (float *)calloc(N, sizeof(float));
  if(u == NULL) {
    perror("trdreduce: Brak pamięci by zaalokować wektor pomocniczy u");
    return -1;
  }
  s = (float *)calloc(N, sizeof(float));
  if(s == NULL) {
    free(u);
    perror("trdreduce: Brak pamięci by zaalokować wektor pomocniczy s");
    return -2;
  } 
  w = (float *)calloc(N, sizeof(float));
  if(w == NULL) {
    free(s);
    free(u);
    perror("trdreduce: Brak pamięci by zaalokować wektor pomocniczy w");
    return -3;
  }


  /* sprowadzamy do postaci trójdiagonalnej kolejne elementy macierzy */
  for (k = 0; k < N-1; k++) {
    kp1 = k + 1;
    
    /* obliczamy normę wektora a_k,...,a_N */
    K = 0;
    for (i = kp1; i < N; i++)
      K += A[i][k]*A[i][k];
    norm = sqrtf(K);


    /* obliczamy wektory pomocnicze */
    /* wektor u */
    if (A[kp1][k] >= 0)
      u[k] = A[kp1][k] + norm;
    else
      u[k] = A[kp1][k] - norm;
    for (i = kp1; i < N; i++)
      u[i] = A[i+1][k];
    
    /* skalar K: K = || a ||^2 + | a_k | || a || */
    K += fabsf(A[kp1][k])*norm;
    if (K == 0) K = 1;

    /* wektor w */
    for (i = k; k < N; k++) {
      w[i] = 0;
      for (j = k; j < N; j++) /* u ma niezerowe elementy k,...,N */
	w[i] += A[i][j]*u[j];
      w[i] /= K;
    }
    
    /* wektor s */
    mult = 0;
    for (i = k; i < N; i++)
      mult += u[i]*w[i];
    mult /= 2*K;
    
    for (i = 0; i < N; i++)
      s[i] = w[i] - mult*u[i];
    

    /* obliczamy now± macierz */
    for (i = k; i < N; i++)
      for (j = k; j < N; j++)
	A[i][j] -= (u[i]*s[k] + s[k]*u[i]);
    
  }
      
  /* zwalniamy wektory pomocnicze */
  free(w);
  free(u);
  free(s);
}



/* Sprowadza macierz do postaci trójdiagonalnej metoda Householdera */
/* obliczajac macierz transformacji Q                               */
int trdreduceq( int N, float **A, float **Q)
{
  float *K,*s,*w;
  float K,norm,mult;
  int k,kp1;
  int i,j;

  
  /* alokujemy pamięć na wektory pomocnicze */
  K = (float *)calloc(N, sizeof(float));
  if(u == NULL) {
    perror("trdreduce: Brak pamięci by zaalokować wektor pomocniczy K");
    return -1;
  }
  s = (float *)calloc(N, sizeof(float));
  if(s == NULL) {
    free(u);
    perror("trdreduce: Brak pamięci by zaalokować wektor pomocniczy s");
    return -2;
  } 
  w = (float *)calloc(N, sizeof(float));
  if(w == NULL) {
    free(s);
    free(u);
    perror("trdreduce: Brak pamięci by zaalokować wektor pomocniczy w");
    return -3;
  }


  /* sprowadzamy do postaci trójdiagonalnej kolejne elementy macierzy */
  for (k = 0; k < N-2; k++) {
    kp1 = k + 1;
    
    /* obliczamy normę wektora a_k,...,a_N */
    K[k] = 0;
    for (i = kp1; i < N; i++)
      K[k] += A[i][k]*A[i][k];
    norm = sqrtf(K[k]);

    
    /* obliczamy wektory pomocnicze */
    /* wektor u[*] = Q[*][k] w k-tej iteracji */
    if (A[kp1][k] >= 0)
      Q[k][k] = A[kp1][k] + norm;
    else
      Q[k][k] = A[kp1][k] - norm;
    for (i = kp1; i < N; i++)
      Q[i][k] = A[i+1][k];
    
    /* skalar K: K = || a ||^2 + | a_k | || a || */
    K[k] += fabsf(A[kp1][k])*norm;
    if (K[k] == 0) K[k] = 1;

    /* wektor pomoczniczy w = 1/K A u */
    for (i = k; k < N; k++) {
      w[i] = 0;
      for (j = k; j < N; j++) /* u ma niezerowe elementy k,...,N */
	w[i] += A[i][j]*Q[j][k];
      w[i] /= K[k];
    }
    
    /* wektor pomoczniczy s = w - 1/(2*K) u u^T w */
    mult = 0;
    for (i = k; i < N; i++)
      mult += Q[i][k]*w[i];
    mult /= 2*K[k];
    
    for (i = 0; i < N; i++)
      s[i] = w[i] - mult*Q[i][k];
    

    /* obliczamy now± macierz A^(nowe) */
    /* A^(nowe) = A - u^T s - s^T u    */
    for (i = k; i < N; i++)
      for (j = k; j < N; j++)
	A[i][j] -= (Q[i][k]*s[k] + s[k]*Q[i][k]);
    
  }
      

  /* obliczamy macierz Q */
  /* obliczamy kolumne k-ta tej macierzy obliczajac Q e_k */
  /* P^(i) y = y - 1/K^(i) u^(i)^T y u^(i)                */

  /* dla kolejnych kolumn macierzy Q */
  for (k = N-1; k >= 0; k--) {
    /* w = e_k */
    for (i = 0; i < N; i++)
      w[k] = 0;
    w[k] = 1;

    /* liczymy działanie kolejnej macierzy P^(i)       */
    /* korzystamy z tego, że P^(i) e_k = e_k dla i > k */
    kp1 = min(N-3,k);
    for (i = kp1; i >= 0; i--) {
      /* liczymy u^(i)T e_k = u^(i)_k */
      /* i dzielimy przez K^(i)       */
      mult = -Q[k][i]/K[i]; /* u^(i) znajduje się w i-tej kolumnie Q */
      
      /* obliczamy działanie P^(i) na e_k */
      for (j = 0; i < N; i++)
	w[j] = w[j] + mult*Q[j][i];
    }

    /* przepisujemy w na odpowiedni± kolumnę Q                     */
    /* dane u nie będzie nam już potrzebne w dalszych obliczeniach */
    for (j = 0; j < N; j++)
      Q[j][k] = w[j];
      
  } /* zakończyli¶my wyznaczanie macierzy Q */
  /* zwalniamy wektory pomocnicze */
  free(w);
  free(K);
  free(s);
}