#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <limits>
#include <cmath>
#include <assert.h>

void LoadVector(std::string file, std::vector<double> & v)
{
	int N, row;
	double val;
	std::ifstream input(file.c_str());
	v.clear();
	while( !input.eof() )
	{
		int c = input.peek();
		if( isspace(c) ) continue;
		if( c == '%' )
		{
			input.ignore(std::numeric_limits<std::streamsize>::max(),'\n');
			continue;
		}
		input >> N;
		v.resize(N);
		break;
	}
	assert( !v.empty() );
	row = 0;
	while( !(input >> val).eof() )
		v[row++] = val;
	input.close();
}

void SaveVector(std::string file, std::vector<double> & v)
{
	std::ofstream output(file.c_str());
	output << v.size() << std::endl;
	output << std::scientific;
       	output.precision(16);
	for(int i = 0; i < (int)v.size(); ++i)
		output << v[i] << std::endl;
	output.close();
}

class CSRMatrix
{
	std::vector<int> ia, ja;
	std::vector<double> a;
public:
	void Load(std::string file)
	{
		int N,M,L;
		int row, col, last_row;
		double val;
		std::ifstream input(file.c_str());
		ia.clear();
		ja.clear();
		a.clear();
		while( !input.eof() )
		{
			int c = input.peek();
			if( isspace(c) ) continue;
			if( c == '%' ) 
			{
				input.ignore(std::numeric_limits<std::streamsize>::max(),'\n');
				continue;
			}
			input >> N >> M >> L;
			assert(N == M);
			ia.resize(N+1);
			ja.reserve(L);
			a.reserve(L);
			break;
		}
		assert( !ia.empty() );
		last_row = 0;
		ia[0] = 0;
		while( !(input >> row >> col >> val).eof() )
		{
			assert(row == last_row || row == last_row+1);
			if( row != last_row )
			{
				ia[row] = ia[last_row];
				last_row = row;
			}
			ia[row]++;
			ja.push_back(col-1);
			a.push_back(val);
		}
		input.close();
	}
	void Save(std::string file) const
	{
		std::ofstream output(file.c_str());
		output << "%%MatrixMarket matrix coordinate real general" << std::endl;
		output << Size() << " " << Size() << " " << ja.size() << std::endl;
		output << std::scientific;
	       	output.precision(16);
		for(int i = 0; i < Size(); ++i)
		{
			for(int j = ia[i]; j < ia[i+1]; ++j)
				output << i+1 << " " << ja[j]+1 << " " << a[j] << std::endl;
		}
		output.close();
	}
	// y = alpha*A*x + beta*y
	void Multiply(double alpha, const std::vector<double> & x, double beta, std::vector<double> & y) const
	{
		assert(x.size() == y.size() && x.size() == Size());
		for(int i = 0; i < Size(); ++i)
		{
			y[i] *= beta;
			for(int j = ia[i]; j < ia[i+1]; ++j)
				y[i] += alpha*a[j]*x[ja[j]];
		}
	}
	int Size() const {return (int)ia.size()-1;}
	int RowSize(int i) const {return ia[i+1]-ia[i];}
	int Col(int i, int k) const {return ja[ia[i]+k];}
	double Val(int i, int k) const {return a[ia[i]+k];}
};

class Method
{
public:
	virtual bool Setup(const CSRMatrix & A) = 0;
	virtual bool Solve(const std::vector<double> & b, std::vector<double> & x) const = 0;
	virtual ~Method() {}
};

class GaussSeidel : public Method
{
	double tol;
	int maxiters;
	std::vector<int> d;
	const CSRMatrix * ptr_A;
public:
	GaussSeidel(double tol, int maxiters) : tol(tol), maxiters(maxiters) {}
	bool Setup(const CSRMatrix & A)
	{
		ptr_A = &A;
		//search diagonal
		d.resize(A.Size());
		for(int i = 0; i < A.Size(); ++i)
		{
			d[i] = -1;
			for(int j = 0; j < A.RowSize(i); ++j)
			{
				if( A.Col(i,j) == i ) 
				{
					d[i] = j;
					break;
				}
			}
			if( d[i] == -1 )
			{
				std::cout << "No diagonal for row " << i << std::endl;
				return false;
			}
			else if( fabs(A.Val(i,d[i])) < 1.0e-7 )
			{
				std::cout << "Tiny diagonal for row " << i << ", diag " << A.Val(i,d[i]) << std::endl;
				for(int j = 0; j < A.RowSize(i); ++j)
					std::cout << "(" <<A.Col(i,j) << "," << A.Val(i,j) << ") ";
				std::cout << std::endl;
				return false;
			}
		}
		return true;
	}
	bool Solve(const std::vector<double> & b, std::vector<double> & x) const
	{
		const CSRMatrix & A = *ptr_A;
		std::vector<double> r;
		double resid = 0;
		int iters = 0;
		r.resize(A.Size(),0.0);
		x.resize(A.Size(),0.0);
		do
		{
			for(int it = 0; it < A.Size(); ++it)
			{
				int i = it; //forward substitution only
				//int i = A.Size()-1-it; //backward substitution only
				//int i = iters%2 ? A.Size()-1-it : it; //alternate backward and forward subsitution
				r[i] = x[i] = b[i];
				for(int j = 0; j < A.RowSize(i); ++j) if( j != d[i] )
					x[i] -= A.Val(i,j)*x[A.Col(i,j)];
				x[i] /= A.Val(i,d[i]);
			}
			A.Multiply(-1,x,1,r);
			resid = 0;
			for(int i = 0; i < A.Size(); ++i)
				resid += r[i]*r[i];
			resid = sqrt(resid);
			std::cout << iters << " " << resid << std::endl;
			iters++;
		} while( resid > tol && iters < maxiters );
		return resid < tol;
	}
};

int main(int argc, char ** argv)
{
	if( argc < 3 )
		std::cout << argv[0] << " matrix.mtx rhs.mtx [sol.mtx]" << std::endl;
	else
	{
		CSRMatrix A;
		std::vector<double> x,b;
		A.Load(std::string(argv[1]));
		LoadVector(std::string(argv[2]),b);
		if( argc > 3 ) LoadVector(std::string(argv[3]),x);
		GaussSeidel Solver(1.0e-9,100000);
		if( Solver.Setup(A) && Solver.Solve(b,x) )
		{
			SaveVector(std::string("solution"),x);
			return 0;
		}
	}
	return 1;
}