#include <iostream>

#include <boost/timer.hpp>

#include <boost/random.hpp>
#include <boost/random/mersenne_twister.hpp>

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

namespace ublas = boost::numeric::ublas;
using namespace std;

    //const int numNodes = 196;
    //const int numTetrahedrons = 617;
// 	const int numNodes = 920;
// 	const int numTetrahedrons = 4196;
	const unsigned int numNodes = 32000ul;
	const unsigned int numTetrahedrons = numNodes*4;

struct use_append
{
    template < class MATRIX, class I, class T >
    static void apply (MATRIX & M, I i, I j, T value) {
        M.append_element(i,j,value);
    }
};

struct use_plus_assign
{
    template < class MATRIX, class I, class T >
    static void apply (MATRIX & M, I i, I j, T value) {
        M(i,j) += value;
    }
};

template < class MATRIX, class OPERATION >
MATRIX assemble()
{
    MATRIX K(numNodes*3, numNodes*3);

	ublas::matrix<float> ke[numTetrahedrons];//array of element stiffness matrices (12x12)
	ublas::vector<unsigned int> index(numTetrahedrons*12);//mapping from local element index to global index, 12 for each element
	boost::mt19937 mtrand;

    mtrand.seed(12345ul);

	//fill index vector with random unsigned integers in [0, numNodes-1] and fill element stiffness matrix with random stuff
	for(unsigned int i=0; i<numTetrahedrons; ++i)
	{
		for(unsigned int j=0; j<4; ++j)
		{
			double r = (double) mtrand();
			unsigned int ni = (unsigned int)( r * (numNodes - 1.0) / mtrand.max() );
            assert( 0 <= ni );
            assert( ni < numNodes );

			for(unsigned int k=0; k<3; ++k)
				index(i*12+j*3+k) = ni*3+k;
		}

		ke[i].resize(12, 12);

		for(unsigned int j=0; j<12; ++j)
			for(unsigned int k=0; k<12; ++k)
			{
				int r = mtrand();
				float v = (r*100000)/(double)(mtrand.max());
				ke[i](j, k) = v;
			}
	}

	boost::timer t;
	t.restart();

	//assemble K a few times
	const unsigned int N = 1;

	for(unsigned int a=0; a<N; ++a)
	{
		K.clear();

		for(unsigned int i=0; i<numTetrahedrons; ++i)
		{
			for(unsigned int j=0; j<12; ++j)
			{
				unsigned int jj = index(i*12+j);

				for(unsigned int k=0; k<12; ++k)
				{
					unsigned int kk = index(i*12+k);
                    
                    OPERATION::apply(K, jj, kk, ke[i](j, k));
                    OPERATION::apply(K, kk, jj, ke[i](k, j));

					//K(jj, kk) += ke[i](j, k);
					//K(kk, jj) += ke[i](k, j);
// 					K.append_element(jj, kk, ke[i](j, k));
// 					K.append_element(kk, jj, ke[i](k, j));
				}
			}
		}
        float a = K(0,0); // make sure force a sort
        K(0,0) = a;
	}

	cout << "time elapsed: " << t.elapsed()/N << endl;

    return K;

}


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

    cout << "numNodes       : " << numNodes << endl;
    cout << "numTetrahedrons: " << numTetrahedrons << endl;

    typedef ublas::generalized_vector_of_vector<float, ublas::row_major, ublas::vector<ublas::coordinate_vector<float> > > GVOV;
    typedef ublas::coordinate_matrix<float> COO;
    typedef ublas::compressed_matrix<float> CRS;

    cout << "GVOV with COO-vector\n";

    GVOV gvov = GVOV(
                     assemble<
                     GVOV
                     , use_append
                     >()
        );

    cout << "COO-matrix\n";

    COO coo = COO (
                   assemble<
                   COO
                   , use_append
                   >()
        );

    cout << "CRS-matrix\n";

    CRS crs;
    if (numNodes <= 500ul) {
        crs = CRS(
                  assemble<
                  CRS
                  , use_plus_assign
                  >()
        );
    } else {
        cout << "skipped due to size\n";
    }

    cout << "dense-matrix\n";

    if (numNodes <= 8000ul) {
        assemble<
            ublas::matrix<float> 
            , use_plus_assign
            >();
    } else {
        cout << "skipped due to size\n";
    }

    cout << "gvov - coo: " << norm_inf(gvov - coo) << endl; 
    if (numNodes <= 500ul) {
        cout << "gvov - crs: " << norm_inf(gvov - crs) << endl; 
        cout << "crs  - coo: " << norm_inf(crs  - coo) << endl; 
    }
	return 0;
}