/*
 * bandtest4.c
 */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "matrix.h"

void lu1(matrix, int, int);
void solve1(matrix, vector, int, int);
void lu3(matrix, int, int);
void solve2(matrix, vector, int, int);
void testmatrix(int, int, matrix, vector, vector);
void disp(int, matrix, vector);

#define FORMAT " %6.4f "

void testmatrix(int n, int m, matrix a, vector x, vector b)
{
  int i, j, mm;
  /* テスト用の A を作る */
  for (i = 0; i <= n; i++)
    for (j = 0; j <= n; j++)
      a[i][j] = 0;
  for (i = 1; i <= n; i++) {
    mm = i + m; if (mm > n) mm = n;
    for (j = i; j <= mm; j++) {
      a[j][i] = a[i][j] = rand() / (double) RAND_MAX;
    }
    a[i][i] = i + 1;
  }
  /* x=(1,2,...,n)^T として b:=A x を計算 */
  for (i = 1; i <= n; i++)
    x[i] = i;
  for (i = 1; i <= n; i++) {
    b[i] = 0;
    for (j = 1; j <= n; j++)
      b[i] += a[i][j] * x[j];
  }
}

void disp(int n, matrix a, vector x)
{
  int i, j;
  printf("L,U=\n");
  for (i = 1; i <= n; i++) {
    for (j = 1; j <= n; j++)
      printf(FORMAT, a[i][j]);
    printf("\n");
  }
  printf("x=\n");
  for (i = 1; i <= n; i++)
    printf(FORMAT, x[i]);
  printf("\n");

}

int main()
{
  int i, j, k, n, mm, m;
  matrix a, a0;
  vector x, b, b0;
  n = 8;
  m = 3;
  a  = new_matrix(n + 1, n + 1);
  a0 = new_matrix(n + 1, n + 1);
  b  = new_vector(n + 1);
  b0 = new_vector(n + 1);
  x = new_vector(n + 1);

  testmatrix(n, m, a0, x, b0);
  for (i = 1; i <= n; i++) for (j = 1; j <= n; j++) a[i][j] = a0[i][j];
  for (i = 1; i <= n; i++) b[i] = b0[i];

  /* LU分解して解く */
  lu1(a, n, m); solve1(a, b, n, m);
  disp(n, a, b);

  /* LU分解して解く */
  lu3(a0, n, m); solve2(a0, b0, n, m);
  disp(n, a0, b0);
}

void lu1(matrix a, int n, int m)
{
  int i, j, k, mm;
  double q;
  for (i = 1; i < n; i++) {
    mm = i + m; if (mm > n) mm = n;
    for (j = i + 1; j <= mm; j++) {
      q = a[j][i] / a[i][i];
      for (k = i + 1; k <= mm; k++)
	a[j][k] -= q * a[i][k];
      a[j][i] = q;
    }
  }
}

/*
 * lu2() から a[i][j] (i<=j) だけ使うようにする
 *   値を読む必要があるときは a[j][i] を持ってくる
 *   a[i][j] (i>j) に値を書き込むのはさぼる (solve2() では使わないので)
 */
void lu3(matrix a, int n, int m)
{
  int i, j, k, mm;
  double q;
  for (i = 1; i < n; i++) {
    mm = i + m; if (mm > n) mm = n;
    for (j = i + 1; j <= mm; j++) {
      /* q = a[j][i] / a[i][i]; だったけれど a[j][i] の代りに a[i][j] を読む */
      q = a[i][j] / a[i][i];
      /* for (k = i + 1; k <= mm; k++) だったけれど対角線の下は何もしない */
      for (k = j; k <= mm; k++)
	a[j][k] -= q * a[i][k];
    }
  }
}

void solve1(matrix a, vector b, int n, int m)
{
  int i, j, mm;
  for (i = 1; i < n; i++) {
    mm = i + m; if (mm > n) mm = n;
    for (j = i + 1; j <= mm; j++)
      b[j] -= a[j][i] * b[i];
  }

  b[n] /= a[n][n];
  for (i = n - 1; i >= 1; i--) {
    mm = i + m; if (mm > n) mm = n;
    for (j = i + 1; j <= mm; j++)
      b[i] -= a[i][j] * b[j];
    b[i] /= a[i][i];
  }
}

/*
 * solve2() の段階で a[i][j] (i<=j) しか使っていないことに注意
 */

void solve2(matrix a, vector b, int n, int m)
{
  int i, j, mm;
  double q;
  for (i = 1; i < n; i++) {
    mm = i + m; if (mm > n) mm = n;
    for (j = i + 1; j <= mm; j++) {
      /*
       * b[j] -= L[j][i] * b[i];
       * としたいのだが、L[j][i] は a[i][j] / a[i][i] に等しい。
       */
      q = a[i][j] / a[i][i];
      b[j] -= q * b[i];
    }
  }

  b[n] /= a[n][n];
  for (i = n - 1; i >= 1; i--) {
    mm = i + m; if (mm > n) mm = n;
    for (j = i + 1; j <= mm; j++)
      b[i] -= a[i][j] * b[j];
    b[i] /= a[i][i];
  }
}