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

#define MAX_OUTPUT_SIZE 10
#define EPS 1e-14

int input_matrix (int n, double *a, double *b);

void print_matrix (int n, double *a, double *b, int print_rhs);

void print_vector (int n, double *x);

double diff (int n, double *a, double *b, double *x);

int solve (int n, double *a, double *b, double *x, int make_decomp);

int zeydel (double *a, double *xn, int n, double *x, double *f, int norm_type);

double diff_norm_L2 (double *vec1, double *vec2, int n);

void mtx_prod_special (double *a, double *x, double *res, int n);

void inverse_mtx (int n, double *a, double *x, double *inv_a);

static double f (int i, int j)
{
  double a = 10., b = 10.;

  if (i == j)
    return a;
  if (j == i + 1 || j == i - 1)
    return 1.;
  if (j == i + 2)
    return 1. / b;

  return 0.;
}

static void mtx_mult_vector (int n, double* a, double* x, double*   res)
{
  int i;
  int j;
  double tmp;

  for (i = 0; i < n; i++)
    {
      tmp = 0.;
      for (j = 0; j < n; j++)
        tmp += a[i * n + j] * x[j];

      res[i] = tmp;
    }
}

static double scalar_prod (int n, double *x1, double *x2)
{
  int i;
  double res = 0.;

  for (i = 0; i < n; i++)
    res += x1[i] * x2[i];

  return res;
}

static void MakeNorm (int n, double *x)
{
  int i;
  double tmp;

  tmp = 0.;
  for (i = 0; i < n; ++i)
    tmp += x[i] * x[i];
  tmp = sqrt(tmp);

  for (i = 0; i < n; ++i)
    x[i] /= tmp;
}

int find_max_val (int n, double *a, double *valueOut, double *x1, double *x2)
{
  int i;
  int iter;
  double tmp1;
  double tmp2;
  double value;
  double valuePrev;

  x1[0] = 1.0;
  for (i = 1; i < n; ++i)
    x1[i] = 0.0;

  iter = 0;
  valuePrev = -1e300;

  while (1)
    {
      iter++;

      mtx_mult_vector (n, a, x1, x2);

      tmp1 = scalar_prod (n, x1, x2);
      tmp2 = scalar_prod (n, x1, x1);

      if (tmp2 > 0)
        {
          value = tmp1 / tmp2;

          if (fabs(value - valuePrev) < EPS)
            break;

          valuePrev = value;
        }

      for (i = 0; i < n; ++i)
        x1[i] = x2[i];

      if (iter % 10 == 0)
        MakeNorm (n, x1);
    }

  MakeNorm (n, x1);
  *valueOut = value;

  return iter;
}


double diff_norm_L2 (double *vec1, double *vec2, int n)
{
  int i;
  double res = 0.;
  for (i = 0; i < n; i++)
    {
      res += (vec1[i] - vec2[i]) * (vec1[i] - vec2[i]);
    }
  return sqrt (res);
}

double diff_norm_Linf (double *vec1, double *vec2, int n)
{
  int i;
  double max = -1.;
  for (i = 0; i < n; i++)
    {
      if (fabs (vec1[i] - vec2[i]) > max)
        max = fabs (vec1[i] - vec2[i]);
    }
  return max;
}

void mtx_prod_special (double *a, double *x, double *res, int n)
{
  int i, j;

  for (i = 0; i < n; i++)
    res[i] = 0.;

  for (i = 0; i < n; i++)
    {
      for (j = 0; j < n; j++)
        {
          if (i == j)
            continue;
          res[i] += a[i * n + j] * x[j];
        }
    }
}

int zeydel (double *a, double *xn, int n, double *x, double *f, int norm_type)
{
  double eps = EPS;
  double norm = 9999999.;
  double *tmp;
  int i = 0;
  tmp = (double *) malloc (n * sizeof (double));

  for (i = 0; i < n; i++)
    {
      xn[i] = 0.;
      tmp[i] = 0.;
    }

  int iter = 0;
  while (norm >= eps)
    {
      mtx_prod_special (a, xn, tmp, n);
      for (i = 0; i < n; i++)
        {
          xn[i] = (1. / a[i * n + i]) * (-tmp[i] + f[i]);
        }
      if (norm_type == 1) //L2
        {
          norm = diff_norm_L2 (xn, x, n);
        }
      else if (norm_type == 2) //Linf
        {
          norm = diff_norm_Linf (xn, x, n);
        }
      else
        {
          break;
        }
      //printf ("%d: %e\n", iter,  norm);
      iter++;
    }

  printf ("\nSolution (Zeydel):\n");
  print_vector (n, xn);
  printf ("\n");
  printf ("|Ax - f| = %e\n", norm);

  free (tmp);
  return iter;
}


void fill_matrix (int n, double *a, double *b)
{
  int i, j;

  for (i = 0; i < n; i++)
    {
      for (j = 0; j < n; ++j)
        {
          a[i * n + j] = f (i, j);
        }
      b[i] = i + 1.;
    }
}

void print_matrix (int n, double *a, double *b, int print_rhs)
{
  int i, j;
  int N = (n > MAX_OUTPUT_SIZE) ? MAX_OUTPUT_SIZE : n;

  for (i = 0; i < N; i++)
    {
      for (j = 0; j < N; j++)
        printf(" %6.3f", a[i * n + j]);
      if (print_rhs == 1)
        printf("\t\t%6.3f", b[i]);
      printf ("\n");
    }
}

void print_vector (int n, double *x)
{
  int i;
  int nPrint = (n > MAX_OUTPUT_SIZE) ? MAX_OUTPUT_SIZE : n;

  for (i = 0; i < nPrint; i++)
    printf (" %6.3f", x[i]);
  printf ("\n");
}

double diff (int n, double *a, double *b, double *x)
{
  int i, j;
  double tmp;
  double result = 0.0;

  for (i = 0; i < n; i++)
    {
      tmp = 0.0;
      for (j = 0; j < n; j++)
        tmp += a[i * n + j] * x[j];
      tmp -= b[i];

      result += tmp * tmp;
    }

  return sqrt (result);
}

int solve (int n, double *a, double *b, double *x, int make_decomp)
{
  int i, j, k;
  double tmp;

  if (make_decomp == 1)
    {
      for (i = 0; i < n; i++)
        {
          if (fabs (a[i * n + i]) < EPS)
            return -1;
          for (j = i; j < n; j++)
            {
              tmp = a[j * n + i];
              for (k = 0; k < i; k++)
                tmp -= a[j * n + k] * a[k * n + i];
              a[j * n + i] = tmp;
            }
          for (j = i + 1; j < n; j++)
            {
              tmp = a[i * n + j];
              for (k = 0; k < i; k++)
                tmp -= a[i * n + k] * a[k * n + j];
              a[i * n + j] = tmp / a[i * n + i];
            }
        }
    }

  for (i = 0; i < n; i++)
    {
      tmp = b[i];
      for (j = 0; j < i; j++)
        tmp -= a[i * n + j] * x[j];
      x[i] = tmp / a[i * n + i];
    }

  for (i = n - 1; i >= 0; i--)
    {
      tmp = x[i];
      for (j = i + 1; j < n; j++)
        tmp -= a[i * n + j] * x[j];
      x[i] = tmp;
    }

  return 0;
}

void inverse_mtx (int n, double *a, double *x, double *inv_a)
{
  int j = 0, i;
  double *b;
  b = (double *) malloc (n * sizeof (double));
  for (j = 0; j < n; j++)
    {
      //fill_matrix (n, a, b);
      for (i = 0; i < n; i++)
        b[i] = 0.;
      b[j] = 1.;
      solve (n, a, b, x, j == 0 ? 1 : 0);
      for (i = 0; i < n; i++)
        inv_a[i * n + j] = x[i];
    }
  free (b);
}

int main (void)
{
  int iters = 0.;
  int n = 100;
  int result = 0;
  double *a, *inv_a, *b, *x, *xn;
  double *x1, *x2;
  double max_val = 0., min_val = 0.;
  clock_t t;

  a = (double *) malloc (n * n * sizeof (double));
  b = (double *) malloc (n * sizeof (double));
  x = (double *) malloc (n * sizeof (double));

  if (!(a && b && x))
    {
      printf ("Not enough memory!\n");
      if (a)
        free (a);
      if (b)
        free (b);
      if (x)
        free (x);
      return -1;
    }

  fill_matrix (n, a, b);

  printf ("n = %d\n\n", n);

  printf ("Matrix A:\n");
  print_matrix (n, a, b, 1);
  printf ("\n");

  t = clock();
  result = solve (n, a, b, x, 1);
  t = clock() - t;

  if (result < 0)
    {
      printf ("Can't solve - matrix is deteriorated.\n");
      free (a);
      free (b);
      free (x);
      return -1;
    }

  printf ("Solution (LU):\n");
  print_vector (n, x);
  printf ("\n");

  printf ("Elapsed: %.3f sec.\n", (double)t / CLOCKS_PER_SEC);

  fill_matrix (n, a, b);

  printf ("|Ax - f| = %e\n", diff (n, a, b, x));

  xn = (double *) malloc (n * sizeof (double));
  iters = zeydel (a, xn, n, x, b, 1);

  printf ("Zeydel iterations: %d\n",iters);

  free (xn);

  x1 = (double *) malloc (n * sizeof (double));
  x2 = (double *) malloc (n * sizeof (double));

  printf ("\n");
  iters = find_max_val (n, a, &max_val, x1, x2);
  printf ("Max lambda of A: %f\n\n", max_val);

  fill_matrix (n, a, b);

  inv_a = (double *) malloc (n * n * sizeof (double));
  inverse_mtx (n, a, x, inv_a);
  printf ("Matrix A^-1:\n");
  print_matrix (n, inv_a, b, 0);
  printf ("\n");

  printf ("\n");
  iters = find_max_val (n, inv_a, &min_val, x1, x2);
  printf ("Min lambda of A: %f\n\n", 1. / min_val);

  printf ("mu = %f\n", max_val * min_val);

  free (a);
  free (b);
  free (x);
  free (inv_a);
  free (x1);
  free (x2);

  return 0;
}