#include <fstream.h>
#include <iomanip.h>
#include <math.h>
#include <vector>

// Chapter 6.1, p.247
// Naive Gaussian
template <class Type>
void naive_gauss(int n, std::vector< std::vector<Type> > a, std::vector<Type> b, std::vector<Type>& x)
{
  int i, j, k;
  Type sum, xmult;

  
	for (k = 1; k <= n - 1; k++)
	{
		for (i = k + 1; i <= n; i++)
		{
			xmult = a[i][k] / a[k][k];
			a[i][k] = 0;
			for (j = k + 1; j <= n; j++)
				a[i][j] -= xmult * a[k][j];
			b[i] -= xmult * b[k];
		}
	}
	x[n] = b[n] / a[n][n];
	for (i = n - 1; i >= 1; i--)
	{
		sum = b[i];
		for (j = i + 1; j <= n; j++)
			sum -= a[i][j] * x[j];
		x[i] = sum / a[i][i];
	}

}


// Chapter 6.2, p.260
// Gaussian with Scaled Partial Pivoting
#define max(a,b) (a > b) ? a : b

template <class Type>
void gauss(int n, std::vector< std::vector<Type> >& a, std::vector<int>& l)
{
	std::vector<Type> s(n+1, 0.0);
	int i, j, k, temp_k;
	Type r, rmax, smax, xmult;
  
	for (i = 1; i <= n; i++)
	{
		l[i] = i;
		smax = 0.0;
		for (j = 1; j <= n; j++)
			smax = max(smax, fabs(a[i][j]));
		s[i] = smax;
	}

	for (k = 1; k <= n - 1; k++)
	{
		rmax = 0.0;
		for (i = k; i <= n; i++)
		{
			r = fabs(a[l[i]][k]/ s[l[i]]);
			if (r > rmax)
			{
				rmax = r;
				j = i;
			}
		}
		temp_k = l[j];
		l[j] = l[k];
		l[k] = temp_k;

		for (i = k + 1; i <= n; i++)
		{
			xmult = a[l[i]][k] / a[l[k]][k];
			a[l[i]][k] = xmult;
			for (j = k + 1; j <= n; j++)
				a[l[i]][j] -= xmult * a[l[k]][j];
		}
	}
}

// Chapter 6.2, p.262
// Gauss - solve
template <class Type>
void solve (int n, std::vector< std::vector<Type> > a, std::vector<int> l, std::vector<Type>& b, std::vector<Type>& x)
{
	int i, j, k;
	Type sum;
  
	for (k = 1; k <= n - 1; k++)
		for (i = k + 1; i <= n; i++)
			b[l[i]] -= a[l[i]][k] * b[l[k]];
	x[n] = b[l[n]] / a[l[n]][n];
	for (i = n - 1; i >= 1; i--)
	{
		sum = b[l[i]];
		for (j = i + 1; j <= n; j++)
			sum -= a[l[i]][j] * x[j];
		x[i] = sum / a[l[i]][i];
	}   
}

// Chapter 6.3, p.275-276
// Tridiagonal
template <class Type>
void tri( int n, std::vector<Type> a, std::vector<Type> d, std::vector<Type> c, std::vector<Type> b, std::vector<Type>& x)
{
 int i;
 Type xmult;

  for (i = 2; i <= n; i++)
  {
    xmult = a[i-1]/d[i-1];
    d[i] -= xmult * c[i-1];
    b[i] -= xmult * b[i-1];
  }
  x[n] = b[n]/d[n];
  for (i = n - 1; i >= 1; i--)
   x[i] = (b[i]- c[i]* x[i+1]) / d[i];

}


// Chapter 6.3, p.278
// Banded
template <class Type>
void penta(int n, std::vector<Type> e, std::vector<Type> a, std::vector<Type> d, std::vector<Type> c, std::vector<Type> f, std::vector<Type> b, std::vector<Type>& x) 
{ 
  int i; 
  Type xmult;
  for (i = 2; i < n; i++)
  {
    xmult = a[i-1]/d[i-1];
    d[i] -= xmult * c[i-1];
    c[i] -= xmult * f[i-1];
    b[i] -= xmult * b[i-1];
    xmult = e[i-1] / d[i-1];
    a[i] -= xmult * c[i-1];
    d[i+1] -= xmult * f[i-1];
    b[i+1] -= xmult * b[i-1];
  }
  xmult = a[n-1]/d[n-1];
  d[n] -= xmult * c[n-1];
  x[n] = (b[n] - xmult * d[n-1]) / d[n];
  x[n-1] = (b[n-1] - c[n-1] * x[n])/d[n-1];

  for (i = n - 2; i >= 1; i--)
   x[i] = (b[i]- f[i] * x[i+2] - c[i] * x[i+1]) / d[i];
}


// Chapter 6.5, p.310-311
// Jacobi
template <class Type>
bool epsilon_comp(int size, std::vector<Type> x, std::vector<Type> y)
{
  const Type epsilon = 0.00005;
  int i, count;
  
  count = 0;
  for (i = 1; i <= size; i++)
    if (fabs(x[i]-y[i]) < epsilon)
      count++;
  if (count == size)
      return true;
  else
    return false;
}

template <class Type>
void jacobi(ofstream& out, int n, std::vector< std::vector<Type> > A, std::vector<Type> b, std::vector<Type> x)
{
  const int k_max = 100;
  const Type delta = 0.0000000001;


  std::vector<Type> y(n+1, 0.0);
  int i, j, k;
  Type diag, sum;

  for (k = 1; k <= k_max; k++)
    {
	  y = x;

      for (i = 1; i <= n; i++)
	{
	  sum = b[i];
	  diag = A[i][i];
	  if (fabs(diag) < delta)
		{ 
		  out << "diagonal element too small" << endl;
	      return;
	    }
	  for (j = 1; j <= n; j++)
	    if (j != i)
	      sum -= A[i][j]*y[j];
	  x[i] = sum/diag;
       out << k << ":" << "x[" << i << "]"<< "=" << x[i] << endl; 
	}
      if (epsilon_comp(n, x, y))
      	{
	  out << "k = " << k << endl;
	  for (i = 1; i <= n; i++)
        out << setw(15) << x[i] << endl;
	  return;
	}
    }
  out << "maximum iterations reached" << endl;
}