#include <iostream>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>

using std::cout;
using std::endl;

using namespace boost::numeric::ublas;

int main(int argc, char *argv[])
{

  typedef sparse_matrix<double>  MY_STIMA;

  MY_STIMA M(4,4,16);

  for (int i=0; i<M.size1(); ++i) M(i,i)=1.0;
  M(0,0)=3;
  M(0,1)=-1;
  M(0,2)=-2;
  M(1,0)=-1;
  M(1,1)=4;
  M(1,2)=-2;
  M(2,0)=-2;
  M(2,1)=-2;
  M(2,2)=9;

  cout << "input M = " << M << "\n";

  MY_STIMA::iterator1 r_it1 = M.begin1();
  MY_STIMA::iterator2 c_it1 = M.begin2();
  while ( (r_it1 != M.end1()) && (c_it1 != M.end2()) ) {
	MY_STIMA::iterator1  r_it2 = c_it1.begin();
	MY_STIMA::iterator2  c_it2 = r_it1.begin();
	MY_STIMA::iterator2  c;

	c = c_it2;
	double temp;
	while ( (r_it2 != c_it1.end()) && (c != r_it1.end()) ) {
	  while (c.index2() < r_it2.index1()) { ++c; }
	  while (c.index2() > r_it2.index1()) { ++r_it2; }
	  if (c.index2() == r_it2.index1()) {
		cout << r_it2.index1() << ", " << c.index2() << ": " << (*c) << " += " << (*r_it2);
		// working version:
		//   temp = (*r_it2);
		//   (*c) += temp;
		// also working version:
		(*c) += (*r_it2);
		cout << " --> " << (*c) << " ? \n";
		++ r_it2;
		++ c;
	  }
	}
	++ r_it1;
	++ c_it1;
  }

  cout << "output M = " << M << endl;
  cout << "expected output M = [4,4]((6,-2,-4,0),(-3,8,-4,0),(-6,-6,18,0),(0,0,0,2))" << endl;

  return EXIT_SUCCESS;
};