///////////////////////////////////////////////////////////////////////////////
// ars::test::gamma_distribution.cpp                                         //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_ARS_TEST_GAMMA_DISTRIBUTION_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_ARS_TEST_GAMMA_DISTRIBUTION_HPP_ER_2009
#include <iostream>
#include <string>
#include <boost/random/mersenne_twister.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/gamma/include.hpp>
#include <boost/ars/test/standard_distribution.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace ars{
namespace test{

struct gamma_distribution{

    // Samples from gamma distribution using adaptive rejection sampling and
    // outputs convergence statistics
    template<typename T>
	static void call(
    	T shape,  // must be > 1
    	T scale,
    	T init_0, // must be >0 and < init_1
    	T init_1, // must be > mode = (m-1) * theta
    	unsigned n1,    // 1e2
    	unsigned n2,    // 10
    	unsigned n3,    // 1
    	unsigned n4,    // 10
    	unsigned n_max_reject,
    	std::ostream& out
	)
	{

    	using namespace boost;
    	typedef double                                          val_;
    	typedef ars::constant<val_>                             const_;
    	typedef math::gamma_distribution<val_>                  mdist_t;
    	typedef boost::mt19937                                  urng_;

    	mdist_t mdist(shape,scale);
    	urng_ urng;

    	const val_ inf_ = const_::inf_;

    	ars::test::standard_distribution::call(
        	mdist,
        	static_cast<val_>(0),   // x_min
        	inf_,                   // x_max
        	init_0,
        	init_1,
        	urng,
        	n1,
        	n2,
        	n3,
        	n4,
        	n_max_reject,
        	out
    	);
	}
};

}//test
}// ars
}// detail
}// statistics
}// boost

#endif


// Boost.Geometry Index
//
// minmaxdist used in R-tree k nearest neighbors query
//
// Copyright (c) 2011-2014 Adam Wulkiewicz, Lodz, Poland.
//
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_INDEX_DETAIL_ALGORITHMS_MINMAXDIST_HPP
#define BOOST_GEOMETRY_INDEX_DETAIL_ALGORITHMS_MINMAXDIST_HPP

#include <boost/geometry/algorithms/distance.hpp>
#include <boost/geometry/algorithms/comparable_distance.hpp>

#include <boost/geometry/index/detail/algorithms/diff_abs.hpp>
#include <boost/geometry/index/detail/algorithms/sum_for_indexable.hpp>
#include <boost/geometry/index/detail/algorithms/smallest_for_indexable.hpp>

namespace boost { namespace geometry { namespace index { namespace detail {

struct minmaxdist_tag {};

template <
    typename Point,
    typename BoxIndexable,
    size_t DimensionIndex>
struct smallest_for_indexable_dimension<Point, BoxIndexable, box_tag, minmaxdist_tag, DimensionIndex>
{
    typedef typename geometry::default_comparable_distance_result<Point, BoxIndexable>::type result_type;

    inline static result_type apply(Point const& pt, BoxIndexable const& i, result_type const& maxd)
    {
        typedef typename coordinate_type<Point>::type point_coord_t;
        typedef typename coordinate_type<BoxIndexable>::type indexable_coord_t;

        point_coord_t pt_c = geometry::get<DimensionIndex>(pt);
        indexable_coord_t ind_c_min = geometry::get<geometry::min_corner, DimensionIndex>(i);
        indexable_coord_t ind_c_max = geometry::get<geometry::max_corner, DimensionIndex>(i);

        indexable_coord_t ind_c_avg = ind_c_min + (ind_c_max - ind_c_min) / 2;
        // TODO: awulkiew - is (ind_c_min + ind_c_max) / 2 safe?

        // TODO: awulkiew - optimize! don't calculate 2x pt_c <= ind_c_avg
        // take particular case pt_c == ind_c_avg into account

        result_type closer_comp = 0;
        if ( pt_c <= ind_c_avg )
            closer_comp = detail::diff_abs(pt_c, ind_c_min); // unsigned values protection
        else
            closer_comp = ind_c_max - pt_c;

        result_type further_comp = 0;
        if ( ind_c_avg <= pt_c )
            further_comp = pt_c - ind_c_min;
        else
            further_comp = detail::diff_abs(pt_c, ind_c_max); // unsigned values protection

        return (maxd + closer_comp * closer_comp) - further_comp * further_comp;
    }
};

template <typename Point, typename Indexable, typename IndexableTag>
struct minmaxdist_impl
{
    BOOST_MPL_ASSERT_MSG(
        (false),
        NOT_IMPLEMENTED_FOR_THIS_INDEXABLE_TAG_TYPE,
        (minmaxdist_impl));
};

template <typename Point, typename Indexable>
struct minmaxdist_impl<Point, Indexable, point_tag>
{
    typedef typename geometry::default_comparable_distance_result<Point, Indexable>::type result_type;

    inline static result_type apply(Point const& pt, Indexable const& i)
    {
        return geometry::comparable_distance(pt, i);
    }
};

template <typename Point, typename Indexable>
struct minmaxdist_impl<Point, Indexable, box_tag>
{
    typedef typename geometry::default_comparable_distance_result<Point, Indexable>::type result_type;

    inline static result_type apply(Point const& pt, Indexable const& i)
    {
        result_type maxd = geometry::comparable_distance(pt, i);

        return smallest_for_indexable<
            Point,
            Indexable,
            box_tag,
            minmaxdist_tag,
            dimension<Indexable>::value
        >::apply(pt, i, maxd);
    }
};

/**
 * This is comparable distace.
 */
template <typename Point, typename Indexable>
typename geometry::default_comparable_distance_result<Point, Indexable>::type
minmaxdist(Point const& pt, Indexable const& i)
{
    return detail::minmaxdist_impl<
        Point,
        Indexable,
        typename tag<Indexable>::type
    >::apply(pt, i);
}

}}}} // namespace boost::geometry::index::detail

#endif // BOOST_GEOMETRY_INDEX_DETAIL_ALGORITHMS_MINMAXDIST_HPP


#include "Epetra_ConfigDefs.h"
#ifdef HAVE_MPI
#include "mpi.h"
#include "Epetra_MpiComm.h"
#else
#include "Epetra_SerialComm.h"
#endif

#include "Teuchos_CommandLineProcessor.hpp"
#include "Teuchos_StandardCatchMacros.hpp"
#include "Teuchos_ParameterList.hpp"
#include "Teuchos_XMLParameterListCoreHelpers.hpp"

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/math/special_functions/erf.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/beta.hpp>

#include "meshpp.hpp"

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

#ifdef HAVE_MPI
    MPI_Init(&argc, &argv);
    Epetra_MpiComm Comm(MPI_COMM_WORLD);
#else
    Epetra_SerialComm Comm;
#endif

    double mean = 0.0;
    double stan = 1.0;
    int nmc = 1000;
    int n = 3;

    boost::random::normal_distribution<double> x(mean,stan);
    boost::random::mt19937 rng;
    rng.seed(std::time(0));

    Epetra_SerialDenseVector X(n);
    Epetra_SerialDenseVector Y(n);
    Epetra_SerialDenseVector c(n);
    Epetra_SerialDenseMatrix Z(nmc,n);
    c(0) = 0.0; c(1) = 0.0; c(2) = 0.0;
    double radius = 1.0;

    for (unsigned int i=0; i<nmc; ++i){
        for (unsigned int j=0; j<n; ++j){
            X(j) = x(rng);
        }
        double s = X.Norm2();
        double gammainc = boost::math::gamma_p<double,double>(s*s/2.0,double(n)/2.0);
        for (unsigned int j=0; j<n; ++j){
            Y(j) = X(j)*(radius*std::pow(gammainc,1.0/double(n))/s);
            Z(i,j) = Y(j) + c(j);
            std::cout << std::setw(20) << Z(i,j);
        }
        std::cout << "\n";
    }

#ifdef HAVE_MPI
    MPI_Finalize();
#endif
return 0;

}


#include <stdio.h>
#include "Substrate.h"
#include "Mathematix.h"
#include "finiteDifferences.h"
//#include <boost/lambda/lambda.hpp>

#define DX 13.

#define EXPLICIT        0
#define IMPLICIT        1
#define CRANK_NICHOLSON 2
#define ADI             3

//#define DIFFUSION_METHODE           EXPLICIT
//#define DIFFUSION_METHODE           IMPLICIT
//#define DIFFUSION_METHODE           CRANK_NICHOLSON
#define DIFFUSION_METHODE           ADI
#define DIFFUSION_REACTION_SEPARAT FALSE
//#define DIFFUSION_REACTION_SEPARAT TRUE

//#define DT 0.01

float **A;
float *b;//[20];
float *x;//[20];
int ADI_direction = 1;

void initMatrix( VoronoiDiagram *voronoiDiagram/*, int imax, int iimax*/)
{
	int matrixDim = voronoiDiagram->xN[0];

	for( int dim=1; dim<DIMENSIONS; dim++)
		if(voronoiDiagram->xN[dim]>matrixDim)
			matrixDim = voronoiDiagram->xN[dim];

	A = newMatrix( matrixDim, matrixDim);

	b = (float*) malloc( matrixDim * sizeof(float));
	x = (float*) malloc( matrixDim * sizeof(float));
/*	for( int i=0; i<imax; i++)
		for( int ii=0; ii<iimax; ii++){
			A[i][ii] = voronoiDiagram->voronoiCells[i + ii*20];
		}
*/}
double factor = 1.;

/*void setupMatrixOxygenX( VoronoiDiagram *voronoiDiagram, int iset, double timeStep)
{
	double r = factor*Oxygen_Diffusion * timeStep/(DX*DX);

	for( int m=0; m<20; m++)
		for( int n=0; n<20; n++)
			A[m][n] = 0;

	for( int m=0; m<20; m++){
		A[m][m]   = 1;

		if( m>0){
			A[m][m-1] = -r;
			A[m][m]  += r;
		}
		if( m<19){
			A[m][m]  += r;
			A[m][m+1] = -r;
		}

		b[m] = voronoiDiagram->voronoiCells[iset + m*20]->oxygen - timeStep * voronoiDiagram->voronoiCells[iset + m*20]->oxygen * GiveMeTheOxygenRate(  voronoiDiagram->voronoiCells[iset + m*20]);
	}
}

void setupMatrixOxygenY( VoronoiDiagram *voronoiDiagram, int iiset, double timeStep)
{
	double r = factor*Oxygen_Diffusion * timeStep/(DX*DX);

	for( int m=0; m<20; m++)
		for( int n=0; n<20; n++)
			A[m][n] = 0;

	for( int m=0; m<20; m++){
		A[m][m]   = 1;

		if( m>0){
			A[m][m-1] = -r;
			A[m][m]  += r;
		}
		if( m<19){
			A[m][m]  += r;
			A[m][m+1] = -r;
		}

		b[m] = voronoiDiagram->voronoiCells[m + iiset*20]->oxygen - timeStep * voronoiDiagram->voronoiCells[m+iiset*20]->oxygen * GiveMeTheOxygenRate(  voronoiDiagram->voronoiCells[m+iiset*20]);
	}
}*/

void setupMatrix( VoronoiDiagram *voronoiDiagram, int base_index, double timeStep, char molecule, char direction)
{
	double r;
	//double consumption;

	if( molecule == 'o'){
		r = factor*Oxygen_Diffusion * timeStep/(DX*DX);
	}else{
		r = factor*Glucose_Diffusion * timeStep/(DX*DX);
	}

	for( int m=0; m<voronoiDiagram->xN[(int)direction]/*20*/; m++)
		for( int n=0; n<voronoiDiagram->xN[(int)direction]/*20*/; n++)
			A[m][n] = 0;

	for( int m=0; m<voronoiDiagram->xN[(int)direction]/*20*/; m++){
		if( m>0 && m<voronoiDiagram->xN[(int)direction]-1){
			A[m][m-1] = -r;
			A[m][m]   =  1 + 2*r;
			A[m][m+1] = -r;
		}
		else{
			A[m][m]   = 1;
		}

		int di = (int)pow(voronoiDiagram->xN[(int)direction],direction);
		int index = base_index + (int)(m*di);
		//fprintf(stderr, "m=%i, dir=%i,  %i + %i => %i (< %i)\n", m, direction, base_index, (int)(m*pow(20,direction)), index, (int)pow(20,3));

		if( molecule == 'o')
			b[m] = voronoiDiagram->voronoiCells[index]->oxygen
#if DIFFUSION_REACTION_SEPARAT == FALSE
			     - timeStep/DIMENSIONS * voronoiDiagram->voronoiCells[index]->oxygen * GiveMeTheOxygenRate(  voronoiDiagram->voronoiCells[index])
#endif
			    ;
		else
			b[m] = voronoiDiagram->voronoiCells[index]->glucose
#if DIFFUSION_REACTION_SEPARAT == FALSE
			     - timeStep/DIMENSIONS * voronoiDiagram->voronoiCells[index]->glucose * GiveMeTheGlucoseRate(  voronoiDiagram->voronoiCells[index])
#endif
			    ;
		/*for( int n=0; n<20; n++)
			fprintf( stderr, "%lf ", A[m][n]);
		fprintf( stderr, "| %lf\n", b[m]);*/
	}
}

void setupMatrixADI( VoronoiDiagram *voronoiDiagram, int base_index, double timeStep, char molecule, char direction)
{
	double r;
	//double consumption;

	if( molecule == 'o'){
		r = factor*Oxygen_Diffusion * timeStep/(DX*DX);
	}else{
		r = factor*Glucose_Diffusion * timeStep/(DX*DX);
	}

	for( int m=0; m<voronoiDiagram->xN[(int)direction]/*20*/; m++)
		for( int n=0; n<voronoiDiagram->xN[(int)direction]/*20*/; n++)
			A[m][n] = 0;

	for( int m=0; m<voronoiDiagram->xN[(int)direction]/*20*/; m++){
		if( m>0 && m<voronoiDiagram->xN[(int)direction]-1){
			A[m][m-1] = -r;
			A[m][m]   =  1 + 2*r;
			A[m][m+1] = -r;
		}
		else{
			A[m][m]   = 1;
		}
		/*if( m>0){
			A[m][m-1] = -r;
			A[m][m]  += r;
		}
		if( m<voronoiDiagram->xN[(int)direction]-1){
			A[m][m]  += r;
			A[m][m+1] = -r;
		}*/

		int di = (int)pow(voronoiDiagram->xN[(int)direction],direction);
		int index = base_index + (int)(m*di);
		//fprintf(stderr, "m=%i, dir=%i,  %i + %i => %i (< %i)\n", m, direction, base_index, (int)(m*pow(20,direction)), index, (int)pow(20,3));

		if( molecule == 'o')
			b[m] = voronoiDiagram->voronoiCells[index]->oxygen
#if DIFFUSION_REACTION_SEPARAT == FALSE
			     - timeStep/DIMENSIONS * voronoiDiagram->voronoiCells[index]->oxygen * GiveMeTheOxygenRate(  voronoiDiagram->voronoiCells[index])
#endif
			    ;
		else
			b[m] = voronoiDiagram->voronoiCells[index]->glucose
#if DIFFUSION_REACTION_SEPARAT == FALSE
			     - timeStep/DIMENSIONS * voronoiDiagram->voronoiCells[index]->glucose * GiveMeTheGlucoseRate(  voronoiDiagram->voronoiCells[index])
#endif
			    ;
		/*for( int n=0; n<20; n++)
			fprintf( stderr, "%lf ", A[m][n]);
		fprintf( stderr, "| %lf\n", b[m]);*/
	}
}

void setupMatrixCrankNicholson( VoronoiDiagram *voronoiDiagram, int base_index, double timeStep, char molecule, char direction)
{
	double r;
	//double consumption;

	if( molecule == 'o'){
		r = Oxygen_Diffusion * timeStep/(DX*DX);
	}else{
		r = Glucose_Diffusion * timeStep/(DX*DX);
	}

	for( int m=0; m<20; m++)
		for( int n=0; n<20; n++)
			A[m][n] = 0;

	for( int m=0; m<20; m++){
		//A[m][m]   = 2;

		if( m>0){
			A[m][m-1] = -r;
			A[m][m]  += 1+r;
		}
		if( m<19){
			A[m][m]  += 1+r;
			A[m][m+1] = -r;
		}

		int index, index_minus, index_plus;

		index = base_index + (int)(m*pow(20,direction));
		index_plus  = base_index + (int)((m+1)*pow(20,direction));
		index_minus = base_index + (int)((m-1)*pow(20,direction));

		double factor, factor_minus, factor_plus;
		factor       = 1-r;
		factor_plus  = 0;
		factor_minus = 0;

#if DIFFUSION_REACTION_SEPARAT == FALSE
		if( molecule == 'o'){
			factor       -= GiveMeTheOxygenRate(  voronoiDiagram->voronoiCells[index]);
			if( m<19)factor_plus  = 1 - GiveMeTheOxygenRate(  voronoiDiagram->voronoiCells[index_plus]);
			if( m>0) factor_minus = 1 - GiveMeTheOxygenRate(  voronoiDiagram->voronoiCells[index_minus]);
		}else{
			factor       -= GiveMeTheGlucoseRate(  voronoiDiagram->voronoiCells[index]);
			if( m<19)factor_plus  = 1 - GiveMeTheGlucoseRate(  voronoiDiagram->voronoiCells[index_plus]);
			if( m>0) factor_minus = 1 - GiveMeTheGlucoseRate(  voronoiDiagram->voronoiCells[index_minus]);
		}
#endif
		b[m] = 0.;
		if( molecule == 'o'){
			if( m>0){
				b[m] += timeStep/DIMENSIONS * factor * voronoiDiagram->voronoiCells[index]->oxygen + timeStep/DIMENSIONS * factor_minus * voronoiDiagram->voronoiCells[index_minus]->oxygen;
			}
			if( m<19){
				b[m] += timeStep/DIMENSIONS * factor * voronoiDiagram->voronoiCells[index]->oxygen + timeStep/DIMENSIONS * factor_plus * voronoiDiagram->voronoiCells[index_plus]->oxygen;
			}
		}else{
			if( m>0){
				b[m] += timeStep/DIMENSIONS * factor * voronoiDiagram->voronoiCells[index]->glucose + timeStep/DIMENSIONS * factor_minus * voronoiDiagram->voronoiCells[index_minus]->glucose;
			}
			if( m<19){
				b[m] += timeStep/DIMENSIONS * factor * voronoiDiagram->voronoiCells[index]->glucose +timeStep/DIMENSIONS *  factor_plus * voronoiDiagram->voronoiCells[index_plus]->glucose;
			}
		}
		/*for( int n=0; n<20; n++)
			fprintf( stderr, "%lf ", A[m][n]);
		fprintf( stderr, "| %lf\n", b[m]);*/
	}
}

double secondDerivationOxygen( VoronoiCell * cell, VoronoiDiagram *voronoiDiagram)
{
	double temp = 0.;

/*	temp = - ((double)cell->countNeighborCells) * cell->oxygen;

	for( int i=0; i<cell->countNeighborCells; i++)
		temp += cell->neighborCells[i]->oxygen;

	return temp/(DX*DX) * 4./(double)cell->countNeighborCells;
*/
	int i   = 0,
	    ii  = 0,
	    iii = 0;

	int index = cell->index;

#if DIMENSIONS > 2
	iii = index / (20*20);
	index = index - iii*20*20;
#endif
#if DIMENSIONS > 1
	ii = index / 20;
	index = index - ii*20;
#endif
	i = index;
	//fprintf( stderr, "%i: (%i,%i,%i) => %i\n", cell->index, i, ii, iii, i + 20*ii + 20*20*iii);

	if( i>0)
		temp += voronoiDiagram->voronoiCells[i-1 + 20*ii + 20*20*iii]->oxygen - cell->oxygen;
	if( i<19)
		temp += voronoiDiagram->voronoiCells[i+1 + 20*ii + 20*20*iii]->oxygen - cell->oxygen;
#if DIMENSIONS > 1
	if( ii>0)
		temp += voronoiDiagram->voronoiCells[i + 20*(ii-1) + 20*20*iii]->oxygen - cell->oxygen;
	if( ii<19)
		temp += voronoiDiagram->voronoiCells[i + 20*(ii+1) + 20*20*iii]->oxygen - cell->oxygen;
#endif
#if DIMENSIONS > 2
	if( iii>0)
		temp += voronoiDiagram->voronoiCells[i + 20*ii + 20*20*(iii-1)]->oxygen - cell->oxygen;
	if( iii<19)
		temp += voronoiDiagram->voronoiCells[i + 20*ii + 20*20*(iii+1)]->oxygen - cell->oxygen;
#endif

	return temp/(DX*DX);
}

double secondDerivationGlucose( VoronoiCell * cell, VoronoiDiagram *voronoiDiagram)
{
	double temp = 0.;

/*	temp = - ((double)cell->countNeighborCells) * cell->glucose;

	for( int i=0; i<cell->countNeighborCells; i++)
		temp += cell->neighborCells[i]->glucose;

	return temp/(DX*DX) * 4./(double)cell->countNeighborCells;
*/
	int i   = 0,
	    ii  = 0,
	    iii = 0;

	int index = cell->index;

#if DIMENSIONS > 2
	iii = index / (20*20);
	index = index - iii*20*20;
#endif
#if DIMENSIONS > 1
	ii = index / 20;
	index = index - ii*20;
#endif
	i = index;
	//fprintf( stderr, "%i: (%i,%i,%i) => %i\n", cell->index, i, ii, iii, i + 20*ii + 20*20*iii);

	if( i>0)
		temp += voronoiDiagram->voronoiCells[i-1 + 20*ii + 20*20*iii]->glucose - cell->glucose;
	if( i<19)
		temp += voronoiDiagram->voronoiCells[i+1 + 20*ii + 20*20*iii]->glucose - cell->glucose;
#if DIMENSIONS > 1
	if( ii>0)
		temp += voronoiDiagram->voronoiCells[i + 20*(ii-1) + 20*20*iii]->glucose - cell->glucose;
	if( ii<19)
		temp += voronoiDiagram->voronoiCells[i + 20*(ii+1) + 20*20*iii]->glucose - cell->glucose;
#endif
#if DIMENSIONS > 2
	if( iii>0)
		temp += voronoiDiagram->voronoiCells[i + 20*ii + 20*20*(iii-1)]->glucose - cell->glucose;
	if( iii<19)
		temp += voronoiDiagram->voronoiCells[i + 20*ii + 20*20*(iii+1)]->glucose - cell->glucose;
#endif

	return temp/(DX*DX);
}

/*#if (DIFFUSION_METHODE != ADI)

double UpdateSystem( VoronoiDiagram *voronoiDiagram, double timeStep, double timeDifference){

	double time;

	int direction = (int)((myRand()*DIMENSIONS)/2.);

	for( time = 0; time+timeStep <= timeDifference; time += timeStep){

#if DIFFUSION_METHODE == EXPLICIT

		//fprintf( stderr, "EXPLICIT\n");
		// Finite Differences: explicit method
		for( int i=0; i<voronoiDiagram->countVoronoiCells; i++){

			voronoiDiagram->voronoiCells[i]->doxygen  = timeStep * (Oxygen_Diffusion  * secondDerivationOxygen(  voronoiDiagram->voronoiCells[i], voronoiDiagram) // diffusion
	#if DIFFUSION_REACTION_SEPARAT == FALSE
			                                          - voronoiDiagram->voronoiCells[i]->oxygen  * GiveMeTheOxygenRate(  voronoiDiagram->voronoiCells[i]) // reaction
	#endif
			                                          );
			voronoiDiagram->voronoiCells[i]->dglucose = timeStep * (Glucose_Diffusion * secondDerivationGlucose( voronoiDiagram->voronoiCells[i], voronoiDiagram) // diffusion
	#if DIFFUSION_REACTION_SEPARAT == FALSE
			                                          - voronoiDiagram->voronoiCells[i]->glucose * GiveMeTheGlucoseRate( voronoiDiagram->voronoiCells[i]) // reaction
	#endif
			                                          );
		}

#endif

#if (DIFFUSION_METHODE == IMPLICIT) || (DIFFUSION_METHODE == CRANK_NICHOLSON) || (DIFFUSION_METHODE == ADI)
		// Finite Differences: alternating direction implicit (ADI)
		double weight = 1.;
		// diffusion in x-direction
		for( int iset=0; iset<20; iset++){
			//setupMatrixOxygenX( voronoiDiagram, iset, timeStep);
	#if   DIFFUSION_METHODE == CRANK_NICHOLSON
			setupMatrixCrankNicholson( voronoiDiagram, iset, timeStep, 'o', 0);
			//fprintf( stderr, "CRANK_NICHOLSON\n");
	#elif DIFFUSION_METHODE == ADI
			setupMatrixADI( voronoiDiagram, iset, timeStep, 'o', direction);
	#else
			//fprintf( stderr, "ADI\n");
			setupMatrix( voronoiDiagram, iset, 0.5*timeStep, 'o', 0);
	#endif
			solveLinearSystem( A, b, x, 20);
			for( int ii=0; ii<20; ii++)
				voronoiDiagram->voronoiCells[iset + ii*20]->doxygen = weight*(b[ii] - voronoiDiagram->voronoiCells[iset + ii*20]->oxygen);

	#if DIFFUSION_METHODE == CRANK_NICHOLSON
			setupMatrixCrankNicholson( voronoiDiagram, iset, timeStep, 'g', 0);
	#elif DIFFUSION_METHODE == ADI
			setupMatrixADI( voronoiDiagram, iset, timeStep, 'g', direction);
	#else
			setupMatrix( voronoiDiagram, iset, 0.5*timeStep, 'g', 0);
	#endif
			solveLinearSystem( A, b, x, 20);
			for( int ii=0; ii<20; ii++)
				voronoiDiagram->voronoiCells[iset + ii*20]->dglucose = weight*(b[ii] - voronoiDiagram->voronoiCells[iset + ii*20]->glucose);
		}
	#if DIFFUSION_METHODE == ADI
		for( int i=0; i<voronoiDiagram->countVoronoiCells; i++){
			if( voronoiDiagram->voronoiCells[i]->position[0]>voronoiDiagram->xMin[0]+1
			 && voronoiDiagram->voronoiCells[i]->position[0]<voronoiDiagram->xMax[0]-1
			 && voronoiDiagram->voronoiCells[i]->position[1]>voronoiDiagram->xMin[1]+1
			 && voronoiDiagram->voronoiCells[i]->position[1]<voronoiDiagram->xMax[1]-1
		#if DIMENSIONS > 2
			 && voronoiDiagram->voronoiCells[i]->position[2]>voronoiDiagram->xMin[2]+1
			 && voronoiDiagram->voronoiCells[i]->position[2]<voronoiDiagram->xMax[2]-1
		#endif
			){
				voronoiDiagram->voronoiCells[i]->oxygen  += voronoiDiagram->voronoiCells[i]->doxygen;
				if( voronoiDiagram->voronoiCells[i]->oxygen < 0.) voronoiDiagram->voronoiCells[i]->oxygen = 0.;
				voronoiDiagram->voronoiCells[i]->glucose += voronoiDiagram->voronoiCells[i]->dglucose;
				if( voronoiDiagram->voronoiCells[i]->glucose < 0.) voronoiDiagram->voronoiCells[i]->glucose = 0.;
			}
		}
		direction = (direction+1)%DIMENSIONS;
	#endif

		// diffusion in y-direction
		for( int iiset=0; iiset<20; iiset++){
//			setupMatrixOxygenY( voronoiDiagram, iiset, timeStep);
	#if DIFFUSION_METHODE == CRANK_NICHOLSON
			setupMatrixCrankNicholson( voronoiDiagram, iiset, timeStep, 'o', 1);
	#elif DIFFUSION_METHODE == ADI
			setupMatrixADI( voronoiDiagram, iiset, timeStep, 'o', direction);
	#else
			setupMatrix( voronoiDiagram, iiset, 0.5*timeStep, 'o', 1);
	#endif
			solveLinearSystem( A, b, x, 20);
			for( int i=0; i<20; i++){
				voronoiDiagram->voronoiCells[i+iiset*20]->doxygen += weight*(b[i] - voronoiDiagram->voronoiCells[i+iiset*20]->oxygen);
			}

	#if DIFFUSION_METHODE == CRANK_NICHOLSON
			setupMatrixCrankNicholson( voronoiDiagram, iiset, timeStep, 'g', 1);
	#elif DIFFUSION_METHODE == ADI
			setupMatrixADI( voronoiDiagram, iiset, timeStep, 'g', direction);
	#else
			setupMatrix( voronoiDiagram, iiset, 0.5*timeStep, 'g', 1);
	#endif
			solveLinearSystem( A, b, x, 20);
			for( int i=0; i<20; i++){
				voronoiDiagram->voronoiCells[i+iiset*20]->dglucose += weight*(b[i] - voronoiDiagram->voronoiCells[i+iiset*20]->glucose);
			}
		}

#endif

		// Update
		for( int i=0; i<voronoiDiagram->countVoronoiCells; i++){
			if( voronoiDiagram->voronoiCells[i]->position[0]>voronoiDiagram->xMin[0]+1
			 && voronoiDiagram->voronoiCells[i]->position[0]<voronoiDiagram->xMax[0]-1
			 && voronoiDiagram->voronoiCells[i]->position[1]>voronoiDiagram->xMin[1]+1
			 && voronoiDiagram->voronoiCells[i]->position[1]<voronoiDiagram->xMax[1]-1
#if DIMENSIONS > 2
			 && voronoiDiagram->voronoiCells[i]->position[2]>voronoiDiagram->xMin[2]+1
			 && voronoiDiagram->voronoiCells[i]->position[2]<voronoiDiagram->xMax[2]-1
#endif
			){
				voronoiDiagram->voronoiCells[i]->oxygen  += voronoiDiagram->voronoiCells[i]->doxygen
#if DIFFUSION_REACTION_SEPARAT == TRUE
				                                          - timeStep * voronoiDiagram->voronoiCells[i]->oxygen  * GiveMeTheOxygenRate(  voronoiDiagram->voronoiCells[i])
#endif
				                                          ;
				if( voronoiDiagram->voronoiCells[i]->oxygen < 0.) voronoiDiagram->voronoiCells[i]->oxygen = 0.;
				voronoiDiagram->voronoiCells[i]->glucose += voronoiDiagram->voronoiCells[i]->dglucose
#if DIFFUSION_REACTION_SEPARAT == TRUE
				                                          - timeStep * voronoiDiagram->voronoiCells[i]->glucose * GiveMeTheGlucoseRate( voronoiDiagram->voronoiCells[i])
#endif
				                                          ;
				if( voronoiDiagram->voronoiCells[i]->glucose < 0.) voronoiDiagram->voronoiCells[i]->glucose = 0.;
			}
		}
	}

	return timeDifference - time;
}

#else*/

double UpdateSystem( VoronoiDiagram *voronoiDiagram, double timeStep, double timeDifference){

	double time;

	//int direction = (int)(myRand()*DIMENSIONS+0.5);
	int direction = ADI_direction; ADI_direction = (ADI_direction+1)%DIMENSIONS;

	for( time = 0; time+timeStep <= timeDifference; time += timeStep){

#if DIFFUSION_METHODE == EXPLICIT

		//fprintf( stderr, "EXPLICIT\n");
		// Finite Differences: explicit method
		for( int i=0; i<voronoiDiagram->countVoronoiCells; i++){

			voronoiDiagram->voronoiCells[i]->doxygen  = timeStep * (Oxygen_Diffusion  * secondDerivationOxygen(  voronoiDiagram->voronoiCells[i], voronoiDiagram) // diffusion
	#if DIFFUSION_REACTION_SEPARAT == FALSE
			                                          - voronoiDiagram->voronoiCells[i]->oxygen  * GiveMeTheOxygenRate(  voronoiDiagram->voronoiCells[i]) // reaction
	#endif
			                                          );
			voronoiDiagram->voronoiCells[i]->dglucose = timeStep * (Glucose_Diffusion * secondDerivationGlucose( voronoiDiagram->voronoiCells[i], voronoiDiagram) // diffusion
	#if DIFFUSION_REACTION_SEPARAT == FALSE
			                                          - voronoiDiagram->voronoiCells[i]->glucose * GiveMeTheGlucoseRate( voronoiDiagram->voronoiCells[i]) // reaction
	#endif
			                                          );
		}

#else

	for( int d=0; d<DIMENSIONS; d++){
	//fprintf(stderr, "dir=%i\n", direction);
		// Finite Differences: alternating direction implicit (ADI)
		double weight = 1.;

		//int i = 0;
		//int ii = 0;
		//int iii = 0;

		for( int iset=0; iset<voronoiDiagram->xN[(d==0?1:0)]/*20*/; iset++){
			for( int iiset=0; iiset<voronoiDiagram->xN[(d<=1?2:1)]/*20*/; iiset++){

				// base index
				int index=0;
				int index_count = 0;
				for( int dd=0; dd<DIMENSIONS; dd++){
					if( dd!=direction){
						if(index_count==0)
							index += (int)( iset*pow( voronoiDiagram->xN[(d==0?1:0)]/*20*/, dd));
						else
							index += (int)( iiset*pow( voronoiDiagram->xN[(d<=1?2:1)]/*20*/, dd));
						index_count++;
					}
				}

				//fprintf(stderr, "setupMatrixADI(o): %i <= (%i,%i)\n", index, iset, iiset);

	#if   DIFFUSION_METHODE == CRANK_NICHOLSON
				setupMatrixCrankNicholson( voronoiDiagram, index, timeStep, 'o', direction);
				//fprintf( stderr, "CRANK_NICHOLSON\n");
	#elif DIFFUSION_METHODE == ADI
				setupMatrixADI( voronoiDiagram, index, timeStep, 'o', direction);
	#elif DIFFUSION_METHODE == IMPLICIT
				//fprintf( stderr, "ADI\n");
				setupMatrix( voronoiDiagram, index, timeStep, 'o', direction);
	#endif
				//setupMatrixADI( voronoiDiagram, index, timeStep, 'o', direction);
				//fprintf(stderr, "solve\n");
				//solveLinearSystemTridiagonalMatrix( A, b, x, voronoiDiagram->xN[d]);
				solveLinearSystem( A, b, x, voronoiDiagram->xN[d]/*20*/);
				for( int i=0; i<voronoiDiagram->xN[d]/*20*/; i++)
	#if DIFFUSION_METHODE == IMPLICIT
					if( d!=0)
						voronoiDiagram->voronoiCells[(int)(index+i*pow( voronoiDiagram->xN[d]/*20*/, direction))]->doxygen += weight*(b[i] - voronoiDiagram->voronoiCells[(int)(index+i*pow( voronoiDiagram->xN[d]/*20*/, direction))]->oxygen);
					else
	#endif
						voronoiDiagram->voronoiCells[(int)(index+i*pow( voronoiDiagram->xN[d]/*20*/, direction))]->doxygen = weight*(b[i] - voronoiDiagram->voronoiCells[(int)(index+i*pow( voronoiDiagram->xN[d]/*20*/, direction))]->oxygen);

				//fprintf(stderr, "setupMatrixADI(g): %i <= (%i,%i)\n", index, iset, iiset);

	#if   DIFFUSION_METHODE == CRANK_NICHOLSON
				setupMatrixCrankNicholson( voronoiDiagram, index, timeStep, 'g', direction);
				//fprintf( stderr, "CRANK_NICHOLSON\n");
	#elif DIFFUSION_METHODE == ADI
				setupMatrixADI( voronoiDiagram, index, timeStep, 'g', direction);
	#elif DIFFUSION_METHODE == IMPLICIT
				//fprintf( stderr, "Implicit\n");
				setupMatrix( voronoiDiagram, index, timeStep, 'g', direction);
	#endif
				//setupMatrixADI( voronoiDiagram, index, timeStep, 'g', direction);
				solveLinearSystem( A, b, x, voronoiDiagram->xN[d]/*20*/);
				//solveLinearSystemTridiagonalMatrix( A, b, x, voronoiDiagram->xN[d]);
				for( int i=0; i<voronoiDiagram->xN[d]/*20*/; i++)
	#if DIFFUSION_METHODE == IMPLICIT
					if( d!=0)
						//add differences
						voronoiDiagram->voronoiCells[(int)(index+i*pow( voronoiDiagram->xN[d]/*20*/, direction))]->dglucose += weight*(b[i] - voronoiDiagram->voronoiCells[(int)(index+i*pow( voronoiDiagram->xN[d]/*20*/, direction))]->glucose);
					else
	#endif
						// set differences
						voronoiDiagram->voronoiCells[(int)(index+i*pow( voronoiDiagram->xN[d]/*20*/, direction))]->dglucose = weight*(b[i] - voronoiDiagram->voronoiCells[(int)(index+i*pow( voronoiDiagram->xN[d]/*20*/, direction))]->glucose);
			}
		}
#endif

#if DIFFUSION_METHODE == IMPLICIT
		if(d+1==DIMENSIONS)
#endif
		// update half time step
		{//fprintf(stderr, "update half time step\n");
		for( int i=0; i<voronoiDiagram->countVoronoiCells; i++){
			if( voronoiDiagram->voronoiCells[i]->position[0]>voronoiDiagram->xMin[0]+1
			 && voronoiDiagram->voronoiCells[i]->position[0]<voronoiDiagram->xMax[0]-1
			 && voronoiDiagram->voronoiCells[i]->position[1]>voronoiDiagram->xMin[1]+1
			 && voronoiDiagram->voronoiCells[i]->position[1]<voronoiDiagram->xMax[1]-1
		#if DIMENSIONS > 2
			 && voronoiDiagram->voronoiCells[i]->position[2]>voronoiDiagram->xMin[2]+1
			 && voronoiDiagram->voronoiCells[i]->position[2]<voronoiDiagram->xMax[2]-1
		#endif
			){
				voronoiDiagram->voronoiCells[i]->oxygen  += voronoiDiagram->voronoiCells[i]->doxygen;
				if( voronoiDiagram->voronoiCells[i]->oxygen < 0.) voronoiDiagram->voronoiCells[i]->oxygen = 0.;
				voronoiDiagram->voronoiCells[i]->glucose += voronoiDiagram->voronoiCells[i]->dglucose;
				if( voronoiDiagram->voronoiCells[i]->glucose < 0.) voronoiDiagram->voronoiCells[i]->glucose = 0.;
			}
		}}

		// change direction
		direction = (direction+1)%DIMENSIONS;
	}

#if DIFFUSION_REACTION_SEPARAT == TRUE
	// reaction
	for( int i=0; i<voronoiDiagram->countVoronoiCells; i++){
		if( voronoiDiagram->voronoiCells[i]->position[0]>voronoiDiagram->xMin[0]+1
		 && voronoiDiagram->voronoiCells[i]->position[0]<voronoiDiagram->xMax[0]-1
		 && voronoiDiagram->voronoiCells[i]->position[1]>voronoiDiagram->xMin[1]+1
		 && voronoiDiagram->voronoiCells[i]->position[1]<voronoiDiagram->xMax[1]-1
	#if DIMENSIONS > 2
		 && voronoiDiagram->voronoiCells[i]->position[2]>voronoiDiagram->xMin[2]+1
		 && voronoiDiagram->voronoiCells[i]->position[2]<voronoiDiagram->xMax[2]-1
	#endif
		){
			voronoiDiagram->voronoiCells[i]->oxygen  += - timeStep * voronoiDiagram->voronoiCells[i]->oxygen  * GiveMeTheOxygenRate(  voronoiDiagram->voronoiCells[i]);
			if( voronoiDiagram->voronoiCells[i]->oxygen < 0.) voronoiDiagram->voronoiCells[i]->oxygen = 0.;
			voronoiDiagram->voronoiCells[i]->glucose += - timeStep * voronoiDiagram->voronoiCells[i]->glucose * GiveMeTheGlucoseRate( voronoiDiagram->voronoiCells[i]);
			if( voronoiDiagram->voronoiCells[i]->glucose < 0.) voronoiDiagram->voronoiCells[i]->glucose = 0.;
		}
	}
#endif
#if DIFFUSION_METHODE != EXPLICIT
	}
#endif
	return timeDifference - time;
}

//#endif

//////////////////////////////////////////////////////////////////////////////////////

void initMatrixImplicit( VoronoiDiagram *voronoiDiagram)
{
	int matrixDim = 1;

	for( int d=0; d<DIMENSIONS; d++)
		matrixDim *= voronoiDiagram->xN[d];

	b = (float*) malloc( matrixDim * sizeof(float));

	x = (float*) malloc( matrixDim * sizeof(float));

	A = newMatrix( matrixDim, matrixDim);
}

void setupMatrixImplicit( VoronoiDiagram *voronoiDiagram, double timeStep, char molecule)
{
	int di   = 1;
	int dii  = voronoiDiagram->xN[0];
	int diii = voronoiDiagram->xN[0]*voronoiDiagram->xN[1];

	double r = Glucose_Diffusion * timeStep/(DX*DX);

	int N = voronoiDiagram->xN[0]*voronoiDiagram->xN[1]*voronoiDiagram->xN[2];

	for( int iii=0; iii<voronoiDiagram->xN[2]; iii++)
	for( int ii=0; ii<voronoiDiagram->xN[1]; ii++)
	for( int i=0; i<voronoiDiagram->xN[0]; i++)
	{
		// actual element
		int m = i*di + ii*dii + iii*diii;
		//fprintf( stderr, "%i ", m);

		// init matrix row
		for( int n=0; n<N; n++)
			A[m][n] = 0.;

		//matrix
		A[m][m] = 1;

		// x
		/*if( i>0){
			A[m][m] = r;
			A[m][m-di] = -r;
		}
		if( i<voronoiDiagram->xN[0]-1){
			A[m][m] = r;
			A[m][m+di] = -r;
		}

		// y
		if( ii>0){
			A[m][m] = r;
			A[m][m-dii] = -r;
		}
		if( ii<voronoiDiagram->xN[1]-1){
			A[m][m] = r;
			A[m][m+dii] = -r;
		}

		// z
		if( iii>0){
			A[m][m] = r;
			A[m][m-diii] = -r;
		}
		if( iii<voronoiDiagram->xN[2]-1){
			A[m][m] = r;
			A[m][m+diii] = -r;
		}*/
		if( i>0 && i<voronoiDiagram->xN[0]-1 && ii>0 && ii<voronoiDiagram->xN[1]-1 && iii>0 && iii<voronoiDiagram->xN[2]-1)
		{
			A[m][m] += 6*r;
			A[m][m-di] = -r;
			A[m][m+di] = -r;
			A[m][m-dii] = -r;
			A[m][m+dii] = -r;
			A[m][m-diii] = -r;
			A[m][m+diii] = -r;
		}

		// vector
		b[m] = voronoiDiagram->voronoiCells[m]->glucose * ( 1. - timeStep * GiveMeTheGlucoseRate(  voronoiDiagram->voronoiCells[m]));

	}
}

double UpdateSystemImplicit( VoronoiDiagram *voronoiDiagram, double timeStep, double timeDifference)
{

	double time;

	int N = voronoiDiagram->xN[0]*voronoiDiagram->xN[1]*voronoiDiagram->xN[2];
	//float **B = newMatrix( N, N);

	for( time = 0; time+timeStep <= timeDifference; time += timeStep){
		//fprintf( stderr, "%i. iteration:\nSetup Matrix\n", (int)(time / timeStep + 0.5));
		int passedTime = clock();
		fprintf( stderr, "Set Matrix... \n");
		setupMatrixImplicit( voronoiDiagram, timeStep, 'G');
		fprintf( stderr, "...finished ( %li clocks, %.3lf sec)\n", (clock() - passedTime), (float)(clock() - passedTime)/CLOCKS_PER_SEC);

		/*for(int m=0; m<N; m++){
			for(int n=0; n<N; n++)
				fprintf( stderr, "%6.0lf ", A[m][n]);
			fprintf( stderr, "\n");
		}*/
		/*for(int m=0; m<N; m++){
			for(int n=0; n<N; n++)
				fprintf( stdout, "%i %i %lf \n", m, n, (A[m][n]!=0.?1.:0.));
			//fprintf( stderr, "\n");
		}*/

		/*fprintf( stderr, "Solve Matrix\n");
		solveLinearSystemB( A, b, x, N, B);

		fprintf( stderr, "Actualize Values\n");
		for(int m=0; m<N; m++)
			voronoiDiagram->voronoiCells[m]->glucose = b[m];
		*/
		for(int m=0; m<N; m++)
			x[m] = voronoiDiagram->voronoiCells[m]->glucose;

		passedTime = clock();
		fprintf( stderr, "ConjugateGradient... \n");
		ConjugateGradient( A, b, x, N, 1);
		fprintf( stderr, "...finished ( %li clocks, %.3lf sec)\n", (clock() - passedTime), (float)(clock() - passedTime)/CLOCKS_PER_SEC);
		//fprintf( stderr, "...finished ( %lisec)\n", (clock() - passedTime)/CLOCKS_PER_SEC);

		for(int m=0; m<N; m++)
			voronoiDiagram->voronoiCells[m]->glucose = x[m];
	}

	return timeDifference - time;

}

void ConjugateGradient_old( float **A, float *b, float *x, int N, int iterations)
{
	float r[N];
	float r2[N];

	float p[N];
	float q[N];

	//float beta[N];
	//float alpha[N];
	float beta;
	float alpha;

	float temp[N];

	// initialization of residual vector: r_0
	matrixVectorProduct( A, x, temp, N);
	vectorDifference( b, temp, r, N);

	// initialization of p_0
	vectorCopy( r, p, N);

	for( int i=0; i<iterations; i++){
		// q_k
		matrixVectorProduct( A, p, q, N);

		alpha = dotProduct( r, r, N);

		if(i>0){
			beta = alpha / dotProduct( r2, r2, N);

			vectorScale( p, beta, p, N);
			vectorSum( r, p, p, N);
		}

		alpha /= dotProduct( p, q, N);

		// next p
		vectorScale( p, alpha, temp, N);
		vectorSum( p, temp, p, N);

		// next x
		vectorScale( x, alpha, temp, N);
		vectorSum( x, temp, x, N);
	}
}

void ConjugateGradient3( float **A, float *b, float *x, int N, int iterations)
{
	float r[N];
	float r2[N];

	float p[N];
	float q[N];

	//float beta[N];
	//float alpha[N];
	float beta;
	float alpha;

	float temp[N];

	// initialization of residual vector: r_0
	matrixVectorProduct( A, x, temp, N);
	vectorDifference( b, temp, r, N);

	// initialization of p_0
	vectorCopy( r, p, N);

	for( int i=0; i<iterations; i++){
		// q_k
		matrixVectorProduct( A, p, q, N);

		alpha = dotProduct( r, r, N);

		if(i>0){
			beta = alpha / dotProduct( r2, r2, N);

			vectorScale( p, beta, p, N);
			vectorSum( r, p, p, N);
		}

		alpha /= dotProduct( p, q, N);

		// next x
		vectorScale( p, alpha, temp, N);
		vectorSum( x, temp, x, N);

		// old r
		vectorCopy( r, r2, N);

		// next r
		vectorScale( q, -alpha, temp, N);
		vectorSum( r, temp, r, N);

		//float *pointer ;
	}
}
void ConjugateGradient( float **A, float *b, float *x, int N, int iterations)
{
	// vectors
	float r[N];
	float w[N];
	float z[N];

	// scalars
	float alpha;
	float beta;

	float temp[N];

	// initialization of residual vector: r
	matrixVectorProduct( A, x, temp, N);
	vectorDifference( b, temp, r, N);

	// w
	vectorScale( r, -1, w, N);

	// z
	matrixVectorProduct( A, w, z, N);

	// alpha
	alpha = dotProduct( r, w, N) / dotProduct( w, z, N);

	// beta
	beta = 0.;

	// x
	vectorScale( w, alpha, temp, N);
	vectorSum( x, temp, x, N);

	for( int i=0; i<iterations; i++){
		vectorScale( z, alpha, temp, N);
		vectorDifference( r, temp, r, N);

		if( sqrt( dotProduct( r, r, N)) < 1e-10)
			return;

		// B = (r'*z)/(w'*z);
		beta = dotProduct( r, z, N) / dotProduct( w, z, N);

		// w = -r + B*w;
		vectorScale( w, beta, w, N);
		vectorDifference( w, r, w, N);

		// z = A*w;
		matrixVectorProduct( A, w, z, N);

		// a = (r'*w)/(w'*z);
		alpha = dotProduct( r, w, N) / dotProduct( w, z, N);

		// x = x + a*w;
		vectorScale( w, alpha, temp, N);
		vectorSum( x, temp, x, N);
	}
}


#include <random>

#include <boost/graph/adjacency_list.hpp>

#include "graph.hpp"
#include "debug.hpp"

boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS> typedef avec;

int main(int argc, char **argv) {
  std::random_device rd;
  std::default_random_engine generator(rd());
  avec g(20);
  add_spider(g, 3, generator);
  show("path.dot", g);
  return 0;
}


#include "PPXTF.hpp"

#include "common.hpp"
#include <cstdio>
#include <string>
#include <map>
#include <fstream>
#include <utility>
#include <vector>
#include <algorithm>
#include <random>
#include <tuple>
#include <utility>
#include <cassert>
#include <iostream>
#include <cstdlib>

// eigen
#include <Eigen/Core>

// stats
#define STATS_DONT_USE_OPENMP
#define STATS_ENABLE_EIGEN_WRAPPERS
//#define STATS_ENABLE_STDVEC_WRAPPERS
//#define STATS_ENABLE_INTERNAL_VEC_FEATURES
#include <stats.hpp>

using namespace Eigen;
using namespace std;

constexpr int BUF_SIZE = 1000;

using p_t = pair<int, int>;
using r_t = vector<map<p_t, double> >;
using mat_t = MatrixXd;
using vec_mat_t = vector<mat_t>;

std::mt19937_64 engine;

const map<int, double> Count2Rating = {
  {1,1.0},
  {2,2.0},
  {3,3.0},
  {4,4.0},
  {5,5.0},
  {6,6.0},
  {7,7.0},
  {8,8.0},
  {9,9.0},
  {10,10.0}
};

vector<int> seq(int n){
  vector<int> v(n);
  for(int i = 0; i < n; ++i){v[i] = i;}
  return v;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// random

vector<int> random_index(int n){
  vector<int> res = seq(n);
  shuffle(res.begin(), res.end(), engine);
  return res;
}

// m \times n matrix
// numbers are uniformly random and in the [0,1] range
mat_t random_mat(int m, int n){
  return 0.5 * (mat_t::Random(m, n).array() + 1);
}

// Cholesky decomposition
inline mat_t chol(const mat_t& A){
  return A.llt().matrixL();
}

// https://en.wikipedia.org/wiki/Multivariate_normal_distribution#Drawing_values_from_the_distribution
// https://qiita.com/jhako/items/30f420033b5c126eedc1
// mu[K,1]
// Sigma[K,K]
// ret[K,1]
mat_t multivariate_normal(const mat_t& mu, const mat_t& Sigma){
  int K = Sigma.rows();
  assert(mu.rows() == K);
  const mat_t L = chol(Sigma); // L * L' = Sigma
  mat_t r = mat_t::Zero(K, 1);
  for(int i = 0; i < K; ++i){r(i, 0) = stats::rnorm(0, 1, engine);}
  return mu + L * r;
}

// https://www.math.wustl.edu/~sawyer/hmhandouts/Wishart.pdf
// scale[K, K]
// res[K, K]
mat_t wishart_rvs(int df, const mat_t& scale){
  int K = scale.rows();
  mat_t A = chol(scale); // A * A' = scale
  mat_t T = mat_t::Zero(K, K); // T * T' = B; B ~ W(I_d, d, n)
  for(int i = 1; i < K; ++i){ // T_{i,j} ~ N(0,1) for j < i
    for(int j = 0; j < i; ++j){
      T(i, j) = stats::rnorm(0 ,1, engine);
    }
  }
  for(int i = 0; i < K; ++i){ // T_{i,i} ~ \chi^2(df-i)
    T(i, i) = std::sqrt(stats::rchisq(df - i, engine));
  }
  mat_t AT = A * T;
  return AT * AT.transpose(); // A * B * A' ~ W(scale, d, n); AT * AT' = A * T * T' * A' = A * B * A'
}

void seeding(unsigned seed){
  //std::random_device seed_gen;
  //engine = std::mt19937_64(seed_gen());
  engine = std::mt19937_64(seed);
  srand(seed); // for eigen
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

double count2rating(int count){
  auto it = Count2Rating.lower_bound(count);
  if(it == Count2Rating.end()){
    return Count2Rating.rbegin()->second;
  }else{
    return it->second;
  }
}

inline void write_to_tensor(r_t& r1, r_t& r2, r_t& r3,
			    int i, int j, int k,
			    double value){
  r1[i][p_t(j,k)] = value;
  r2[j][p_t(i,k)] = value;
  r3[k][p_t(i,j)] = value;
}

void read_tensor(r_t& r1, r_t& r2, r_t& r3, const string& infile){
  ifstream ifs(infile);
  string line;
  getline(ifs, line); // header
  while(getline(ifs, line)){
    auto row = split(line);
    assert(row.size() >= 4);
    int i = atoi(row[0].c_str());
    int j = atoi(row[1].c_str());
    int k = atoi(row[2].c_str());
    double value = count2rating(atoi(row[3].c_str()));
    write_to_tensor(r1, r2, r3, i, j, k, value);
  }
}

/*
  ###################### Read a training transition tensor ######################
  # [input1]: N -- Number of users
  # [input2]: M -- Number of POIs
  # [output1]: RT1_observed ([user_index]{poi_index_from, poi_index_to: value})
  # [output2]: RT2_observed ([poi_index_from]{user_index, poi_index_to: value})
  # [output3]: RT3_observed ([poi_index_to]{user_index, poi_index_from: value})
 */
tuple<r_t, r_t, r_t> ReadTrainTransTensor(int N, int M, int ZeroNum, const string& infile){
  r_t RT1_observed(N), RT2_observed(M), RT3_observed(M);
  // Read a training tensor --> (RT1,RT2,RT3)_observed
  read_tensor(RT1_observed, RT2_observed, RT3_observed, infile);
  // Randomly assign ZeroNum zero-values --> (RT1,RT2,RT3)_observed
  if(ZeroNum > 0){
    int MM = M * M;
    for(int user_index = 0; user_index < N; ++user_index){
      auto rand_index = random_index(MM);
      int zero_num = 0;
      for(int i = 0; i < MM; ++i){
	int poi_index_from = rand_index[i] % M;
	int poi_index_to = rand_index[i] / M;
	auto& a = RT1_observed[user_index];
	if(a.find(p_t(poi_index_from,poi_index_to)) == a.end()){
	  write_to_tensor(RT1_observed, RT2_observed, RT3_observed, user_index, poi_index_from, poi_index_to, 0);
	  zero_num += 1;
	  if(zero_num == ZeroNum){break;}
	}
      }
    }
  }else if(ZeroNum == -1){
    for(int user_index = 0; user_index < N; ++user_index){
      for(int poi_index_from = 0; poi_index_from < M; ++poi_index_from){
	for(int poi_index_to = 0; poi_index_to < M; ++poi_index_to){
	  auto& a = RT1_observed[user_index];
	  auto p = p_t(poi_index_from,poi_index_to);
	  if(a.find(p) == a.end()){
	    write_to_tensor(RT1_observed, RT2_observed, RT3_observed, user_index, poi_index_from, poi_index_to, 0);
	  }
	}
      }
    }
  }
  return forward_as_tuple(RT1_observed, RT2_observed, RT3_observed);
}
/*
  ######################## Read a training visit tensor #########################
  # [input1]: N -- Number of users
  # [input2]: M -- Number of POIs
  # [input3]: T (Number of time slots)
  # [output1]: RV1_observed ([user_index]{poi_index_from, time_slot: value})
  # [output2]: RV2_observed ([poi_index_from]{user_index, time_slot: value})
  # [output3]: RV3_observed ([time_slot]{user_index, poi_index_from: value})
 */
tuple<r_t, r_t, r_t> ReadTrainVisitTensor(int N, int M, int T, int ZeroNum, const string& infile){
  r_t RV1_observed(N), RV2_observed(M), RV3_observed(T);
  // Read a training tensor --> (RV1,RV2,RV3)_observed
  read_tensor(RV1_observed, RV2_observed, RV3_observed, infile);
  // Randomly assign ZeroNum zero-values --> (RV1,RV2,RV3)_observed
  if(ZeroNum > 0){
    int MT = M * T;
    for(int user_index = 0; user_index < N; ++user_index){
      auto rand_index = random_index(MT);
      int zero_num = 0;
      for(int i = 0; i < MT; ++i){
	int poi_index_from = rand_index[i] % M;
	int time_slot = rand_index[i] / M;
	auto& a = RV1_observed[user_index];
	if(a.find(p_t(poi_index_from,time_slot)) == a.end()){
	  write_to_tensor(RV1_observed, RV2_observed, RV3_observed, user_index, poi_index_from, time_slot, 0);
	  zero_num += 1;
	  if(zero_num == ZeroNum){break;}
	}
      }
    }
  }else if(ZeroNum == -1){
    for(int user_index = 0; user_index < N; ++user_index){
      for(int poi_index_from = 0; poi_index_from < M; ++poi_index_from){
	for(int time_slot = 0; time_slot < T; ++time_slot){
	  auto& a = RV1_observed[user_index];
	  auto p = p_t(poi_index_from,time_slot);
	  if(a.find(p) == a.end()){
	    write_to_tensor(RV1_observed, RV2_observed, RV3_observed, user_index, poi_index_from, time_slot, 0);
	  }
	}
      }
    }
  }
  return forward_as_tuple(RV1_observed, RV2_observed, RV3_observed);
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// io

void puts_mat(FILE* fout, const mat_t& A){
  int m = A.rows();
  int n = A.cols();
  assert(n > 0);
  for(int i = 0; i < m; ++i){
    fprintf(fout, "%lf", A(i,0));
    for(int j = 1; j < n; ++j){
      fprintf(fout, ",%lf", A(i,j));
    }
    fprintf(fout, "\n");
  }
}

void savetxt(const string& outfile, const mat_t& A){
  FILE* fp = fopen(outfile.c_str(), "w");
  puts_mat(fp, A);
  fclose(fp);
}

void save_parameters(int K, int ItrNum, const string& prefix, const string& name, const vec_mat_t& A, const mat_t& mu_A, const mat_t& Lam_A, const string& X){
  char buf[BUF_SIZE];
  sprintf(buf, "%s_Itr%d_%s%s.csv", prefix.c_str(), ItrNum, name.c_str(), X.c_str());
  savetxt(buf, A[ItrNum]);
  sprintf(buf, "%s_Itr%d_mu_%s%s.csv", prefix.c_str(), ItrNum, name.c_str(), X.c_str());
  savetxt(buf, mu_A);
  sprintf(buf, "%s_Itr%d_Lam_%s%s.csv", prefix.c_str(), ItrNum, name.c_str(), X.c_str());
  savetxt(buf, Lam_A);
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// ppmft

tuple<mat_t, mat_t> sample_Lam_X_mu_X(const mat_t& X, int N, int nu0, const mat_t& W0, double beta0, double mu0, int K){
  // Sample Lam_X
  // S_bar = np.sum([np.outer(A[itr, n, :], A[itr, n, :]) for n in range(N)], axis=0) / N
  mat_t S_bar = X.transpose() * X / N; // [K, K]
  // a_bar = np.sum(A[itr], axis=0) / N
  mat_t x_bar = X.colwise().sum() / N; // [1, K]
  // nu_ast = nu0 + n
  int nu_ast = nu0 + N; // 1
  // W0_ast = inv(inv(W0) + N * S_bar + (beta0 * N / (beta0 + N)) * np.outer(mu0 - a_bar, mu0 - a_bar))
  mat_t t = mat_t::Constant(1, K, mu0) - x_bar; // [1, K]
  mat_t W0_ast = (W0.inverse() + N * S_bar + (beta0 * N / (beta0 + N)) * (t.transpose() * t)).inverse(); // [K, K]
  // Lam_A = wishart.rvs(df=nu_ast, scale=W0_ast)
  //mat_t Lam_X = stats::rwish(W0_ast, nu_ast); // [K, K]
  mat_t Lam_X = wishart_rvs(nu_ast, W0_ast); // [K, K]

  // Sample mu_X
  // mu0_ast = (beta0 * mu0 + N * a_bar) / (beta0 + N)
  mat_t mu0_ast = ((mat_t::Constant(1, K, beta0 * mu0) + N * x_bar) / (beta0 + N)).transpose(); // [K, 1]
  // mu_A = multivariate_normal(mu0_ast, inv((beta0 + N) * Lam_A))
  mat_t mu_X = multivariate_normal(mu0_ast, ((beta0 + N) * Lam_X).inverse()); // [K, 1]
  return forward_as_tuple(Lam_X, mu_X);
}

tuple<mat_t, mat_t> calc_XYXY_XYR(int K, const mat_t& B, const mat_t& C, const map<p_t, double>& rt1){
  mat_t BC_BC = mat_t::Zero(K, K);
  mat_t BC_R = mat_t::Zero(1, K);
  for(auto kv : rt1){
    int i = kv.first.first, j = kv.first.second;
    double value = kv.second;
    mat_t bc_ij = B.row(i).array() * C.row(j).array(); // hadamard product
    mat_t bc_bc = bc_ij.transpose() * bc_ij; // [K,K]
    mat_t bc_r = bc_ij * value; // [1,K]
    BC_BC = BC_BC + bc_bc;
    BC_R = BC_R + bc_r;
  }
  return forward_as_tuple(BC_BC, BC_R.transpose());
}


//////////////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::distributions::chi_squared::description.hpp                //
//                                                                                //
//  (C) Copyright 2009 Erwann Rogard                                                //
//  Use, modification and distribution are subject to the                           //
//  Boost Software License, Version 1.0. (See accompanying file                     //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)                //
//////////////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_CHI_SQUARED_DESCRIPTION_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_CHI_SQUARED_DESCRIPTION_HPP_ER_2009
#include <ostream>
#include <boost/format.hpp>
#include <boost/math/distributions/chi_squared.hpp>

namespace boost{
namespace math{

    template<typename T,typename P>
    std::string
    description(const boost::math::chi_squared_distribution<T,P>& dist)
    {
        static const char* msg = "chi_squared(%1%)";
        format f(msg); f % dist.degrees_of_freedom();
        return f.str();
    }

}// math
}// boost

#endif


#include <boost/test/unit_test.hpp>

#include "logs.h"
#include "route.h"
#include "track.h"

using namespace GPS;

BOOST_AUTO_TEST_SUITE (Route_numPositions)

const bool isFileName = true;

//1) At least 1 Position

// This tests that there is at least 1 position in the file "n0719388data1.gpx"
//
//

BOOST_AUTO_TEST_CASE(MinimumNumberOfPositions)_END()
{

Route route = Route(LogFiles::GPXRoutesDir + "n0719388data1.gpx", isFileName);
unsigned int numOfPositions = route.numPositons();
BOOST_CHECK_GT ( numOfPositions,1);

}

//2) Correct number of positions output

// Test that the number of positions outputs the same value as intended.

BOOST_AUTO_TEST_CASE(CorrectNoOfPositions)

{

Route route = Route(LogFiles::GPXRoutesDir + "n0719388data1.gpx", isFileName);
unsigned int numOfPositions = route.numPositions();
unsigned int ActualNumberOfPositions = "2";
BOOST_CHECK_EQUAL(NumOfPositions, ActualNumberOfPositions);

}

//3)(distancebetween above granularity threshold)

//   sqrt((x2-x1)^2+(y2-y1)^2) =distance
//-62.372262972335676068
//-63.925960253788176146
//

// distance between points in "n0719388data1.gpx" is 2109.329m, exceed granularity by far
// set a granularity of 50 for sake of test
// Test that when the distance between two points exeeds the granularity limit, no changes are made to the number of positions.

BOOST_AUTO_TEST_CASE(GradularityDoesntOccur)
{

Route route = Route(LogFiles::GPXRoutesDir + "n0719388data1.gpx", isFileName,50);
unsigned int ActualNumberOfPositions = "2";
BOOST_CHECK_EQUAL(route.numPositions(), ActualNumberOfPositions);
}

//4)(distance between below granularity threshold)
//Similar test to the above except graularity has now been set so that the distance between the two points no longer exceeds the granularity limit, hence a position should be thrown.
//2109.329 < 2500

BOOST_AUTO_TEST_CASE(GradularityDoesOccur)

{
Route route = Route(LogFiles::GPXRoutesDir + "n0719388data1.gpx", isFileName,2500);
unsigned int ActualNumberOfPositions = "1";
BOOST_CHECK_EQUAL(route.numPositions(), ActualNumberOfPositions);

}

//Longitude   -180   +180
//Latitude    -90     +90

//5) Granularity Edge Values Upper (Largest value)
//111km per degree /2 19980, granularity wont be any larger than this.
// should not throw any positions

BOOST_AUTO_TEST_CASE(GranularityUpperEdge)

{

Route route = Route(LogFiles::GPXRoutesDir + "n0719388data1.gpx", isFileName,19980)
unsigned int ActualNumberOfPositions = "2";
BOOST_CHECK_EQUAL(route.numPositions(), ActualNumberOfPositions);

}

// Lower edge value for granularity should throw one position due to distance being over 1m_END()
//6) Granularity Edge Values Lower (Small Value)

BOOST_AUTO_TEST_CASE(GranularityLowerEdge)

{
Route route = Route(LogFiles::GPXRoutesDir + "n0719388data1.gpx", isFileName,1)
unsigned int ActualNumberOfPositions = "1";
BOOST_CHECK_EQUAL(route.numPositions(), ActualNumberOfPositions);
}

BOOST_AUTO_TEST_SUITE_END()


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/negatenz.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/mtwo.hpp>
#include <boost/simd/constant/three.hpp>
#include <boost/simd/constant/mthree.hpp>

STF_CASE_TPL (" negatenz real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::negatenz;
  using r_t = decltype(negatenz(T(), T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef STF_NO_INVALIDS
  STF_EQUAL(negatenz(bs::Inf<T>(), bs::Inf<T>()), bs::Inf<r_t>());
  STF_EQUAL(negatenz(bs::Minf<T>(), bs::Minf<T>()), bs::Inf<r_t>());
  STF_IEEE_EQUAL(negatenz(bs::Nan<T>(), bs::Zero<T>()), bs::Nan<r_t>());
  STF_IEEE_EQUAL(negatenz(bs::Nan<T>(), bs::Zero<T>()), bs::Nan<r_t>());
  STF_IEEE_EQUAL(negatenz(bs::Nan<T>(), bs::One<T>()), bs::Nan<r_t>());
#endif
  STF_TYPE_IS(r_t, T);
  STF_EQUAL(negatenz(bs::Mone<T>(), bs::Mone<T>()), bs::One<r_t>());
  STF_EQUAL(negatenz(bs::One<T>(), bs::One<T>()), bs::One<r_t>());
  STF_EQUAL(negatenz(bs::Zero<T>(), bs::Zero<T>()), bs::Zero<r_t>());
  STF_EQUAL(negatenz(bs::One<T>(), bs::Zero<T>()), bs::One<r_t>());
  STF_EQUAL(negatenz(bs::Two<T>(), bs::Mthree<T>()), bs::Mtwo<r_t>());
  STF_EQUAL(negatenz(bs::Two<T>(), bs::Three<T>()), bs::Two<r_t>());
}

STF_CASE_TPL (" negatenz unsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::negatenz;
  using r_t = decltype(negatenz(T(), T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
  STF_TYPE_IS(r_t, T);
  STF_EQUAL(negatenz(bs::One<T>(), bs::One<T>()), bs::One<r_t>());
  STF_EQUAL(negatenz(bs::Zero<T>(), bs::Zero<T>()), bs::Zero<r_t>());
  STF_EQUAL(negatenz(bs::One<T>(), bs::Zero<T>()), bs::One<r_t>());
}

STF_CASE_TPL (" negatenz signed_int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::negatenz;
  using r_t = decltype(negatenz(T(), T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
  STF_TYPE_IS(r_t, T);
  STF_EQUAL(negatenz(bs::Mone<T>(), bs::Mone<T>()), bs::One<r_t>());
  STF_EQUAL(negatenz(bs::One<T>(), bs::One<T>()), bs::One<r_t>());
  STF_EQUAL(negatenz(bs::Zero<T>(), bs::Zero<T>()), bs::Zero<r_t>());
  STF_EQUAL(negatenz(bs::One<T>(), bs::Zero<T>()), bs::One<r_t>());
}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/integral.hpp>
#include <boost/hana/range.hpp>
using namespace boost::hana;

int main() {
    //! [main]
    BOOST_HANA_CONSTEXPR_LAMBDA auto r = range(int_<0>, int_<1000>);
    BOOST_HANA_CONSTANT_ASSERT(head(r) == int_<0>);
    BOOST_HANA_CONSTANT_ASSERT(last(r) == int_<999>);
    BOOST_HANA_CONSTANT_ASSERT(!is_empty(r));
    BOOST_HANA_CONSTANT_ASSERT(is_empty(range(int_<3>, int_<3>)));
    //! [main]
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/arithmetic/include/functions/fast_rec.hpp>

#include <boost/dispatch/functor/meta/call.hpp>
#include <boost/simd/constant/constant.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

NT2_TEST_CASE_TPL ( fast_rec,  BOOST_SIMD_SIMD_REAL_TYPES)
{
  using nt2::fast_rec;
  using nt2::tag::fast_rec_;

  NT2_TEST_TYPE_IS( typename boost::dispatch::meta::call<fast_rec_(T)>::type
                  , T
                  );

  // 1/+-1 = +-1
  NT2_TEST_ULP_EQUAL(fast_rec(boost::simd::Mone<T>()), boost::simd::Mone<T>(), 16);
  NT2_TEST_ULP_EQUAL(fast_rec(boost::simd::One<T>()), boost::simd::One<T>(), 16);

  NT2_TEST_ULP_EQUAL(fast_rec( T(2) ), T(0.5)  , 16);
  NT2_TEST_ULP_EQUAL(fast_rec( T(10)), T(0.1)  , 16);
  NT2_TEST_ULP_EQUAL(fast_rec( T(3) ), T(1./3.), 16);

  // 1/(1/x) = x
  NT2_TEST_ULP_EQUAL(fast_rec(fast_rec(T(10))), T(10), 16);
}


/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */

/*
 * File:   main.cpp
 * Author: haoran
 *
 * Created on October 2, 2017, 9:09 AM
 */

#include <cstdlib>
#include <boost/program_options/options_description.hpp>
#include <frovedis.hpp>
#include <frovedis/matrix/crs_matrix.hpp>
#include <vector>
#include <iostream>
#include <fstream>
#include <string>
#include <sstream>      // std::istringstream
#include <unordered_map>
#include <ctime>// record time
#include <cmath>
#include <numeric>
#include <frovedis/ml/graph/graph.hpp>
// #include <frovedis/ml/graph/graph.cpp>
#include <boost/program_options.hpp>

using namespace boost;
using namespace frovedis;
using namespace std;

int main(int argc, char* argv[]){
    frovedis::use_frovedis use(argc, argv);

    using namespace boost::program_options;

    options_description opt("option");
    opt.add_options()
        ("help,h", "produce help message")
        ("input,i" , value<std::string>(), " input data path. When using binary files, the path is the directory containing all files. When using string file, the path is the file path.")
        ("binary,b" , " use binary graph matrix file" )
        ("direct,d" , " if to generate direct graph file")
        ("prepare,p" , "if to generate the CRS matrix from original graph list file ")
        ("method,m" , value<std::string>(), "method (cc_bfs_2DMerge, cc_bfs_Scatter, cc_bfs_idxSort, cc_bfs_spMV)")
        // ("cout" , " enable screen output for the search result" )
        // ("fout" , " enable file output for the search result" )
        ("upperbound,u" ,value<size_t>(), " upperbound for 2D merge")
        ("allgather", "use all_gatherv instead of gather to reduce communication overhead (currently only for CC_BFS)")
        ("hyb", "use hybrid spMspV strategy: 2DMerge + Scatter (for scale-free graphs)")
        ("matformat,f" , value<std::string>(), " matrix format (default: spMV: CRS(X86) HYB(SX) spMspV: CCS )")
        ("scalefree,s" , " for scale-free graphs");

    variables_map argmap;
    store(command_line_parser(argc,argv).options(opt).allow_unregistered().
          run(), argmap);
    notify(argmap);

    size_t hyb_th = 10000000;
    size_t upperbound_2DMerge = 256;

    bool if_prep = 0; // true if prepare data from raw dataset
    bool if_binary = 0;
    bool if_graph_direct = 0;
    bool if_cout = 0;
    bool if_fout = 0;
    bool if_scale_free = 0;
    bool if_allgather = 0;
    bool if_hyb = 0;
    std::string data_p;
    std::string method_name;
    std::string matformat;
// #ifndef _SX
#if !defined(_SX) && !defined(__ve__)
    matformat = "CRS";
#else
    matformat = "HYB";
#endif

//////////////////////////   command options   ///////////////////////////
    if(argmap.count("help")){
        std::cerr << opt << std::endl;
        exit(1);
    }

    if(argmap.count("input")){
      data_p = argmap["input"].as<std::string>();
    } else {
      std::cerr << "input is not specified" << std::endl;
      std::cerr << opt << std::endl;
      exit(1);
    }
    if(argmap.count("binary")){
        if_binary = true;
    }
    if(argmap.count("direct")){
        if_graph_direct = true;
    }
    if(argmap.count("prepare")){
        if_prep = true;
    }
    if(argmap.count("method")){
        method_name = argmap["method"].as<std::string>();
    }else{
      std::cerr << "input is not specified" << std::endl;
      std::cerr << opt << std::endl;
      exit(1);
    }
    // if(argmap.count("cout")){
    //     if_cout = true;
    // }
    // if(argmap.count("fout")){
    //     if_fout = true;
    // }
    if(argmap.count("upperbound")){
       upperbound_2DMerge = argmap["upperbound"].as<size_t>();
    }
    if(argmap.count("matformat")){
       matformat = argmap["matformat"].as<std::string>();
    }
    if(argmap.count("scalefree")){
       if_scale_free = true;
    }
    if(argmap.count("allgather")){
       if_allgather = true;
    }
    if(argmap.count("hyb")){
       if_hyb = true;
    }
/////////////////////////////////////////////////////////////////

    graph* CCptr = new graph();
    CCptr->set_if_cout(if_cout);
    CCptr->set_if_fout(if_fout);

    if(if_prep == true){
        CCptr->prep_graph_crs(data_p, if_graph_direct);
        data_p = data_p + "_graph_in_crs";
    }
    //connected component
    CCptr->read_graph(data_p, if_binary);

    if(method_name == "cc_bfs_spMV"){
        throw std::logic_error("This method is not supported now");
        // CCptr->cc_bfs_spmv(matformat);
    }
    else if(method_name == "cc_bfs_Scatter"){
        CCptr->cc_bfs_scatter();
    }
    else if(method_name == "cc_bfs_idxSort"){

        CCptr->cc_bfs_idxsort(hyb_th, if_hyb);
    }
    else if(method_name == "cc_bfs_2DMerge"){
        if(if_allgather == 1){
            throw std::logic_error("This method is not supported now");
            // CCptr->cc_bfs_2dmerge_allgather(upperbound_2DMerge);
        }else{
            CCptr->cc_bfs_2dmerge_gather(upperbound_2DMerge, if_scale_free, if_hyb);
        }
    }
    else{
        std::cerr<<"ERROR: Method "<<method_name<<" doesn't exist!"<<std::endl;
        std::cerr << opt << std::endl;
        exit(1);
    }
    return 0;
}


/*
 * Copyright (c) 2013-2017 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef JOINTRANGE_HPP
#define JOINTRANGE_HPP

#include <algorithm>
#include <boost/numeric/ublas/vector.hpp>
#include <kv/affine.hpp>
#include <kv/matplotlib.hpp>

namespace kv {

namespace ub = boost::numeric::ublas;

template<class T>
void jointrange(const affine<T>& x, const affine<T>& y, const matplotlib& g, const char *color = "blue")
{
	int n, i, j, s;
	T tmp;
	T dx, dy;
	ub::vector<T> vx, vy;
	ub::matrix<int> signcache;

	n = std::max(x.a.size(), y.a.size()) - 1;

#if AFFINE_SIMPLE >= 1
	vx.resize(n+3);
	vy.resize(n+3);
#else
	vx.resize(n+1);
	vy.resize(n+1);
#endif

	for (i=0; i<x.a.size(); i++) {
		vx(i) = x.a(i);
	}
	for (i=x.a.size(); i<=n; i++) {
		vx(i) = 0.;
	}
	for (i=0; i<y.a.size(); i++) {
		vy(i) = y.a(i);
	}
	for (i=y.a.size(); i<=n; i++) {
		vy(i) = 0.;
	}

#if AFFINE_SIMPLE >= 1
	vx(n+1) = x.er;
	vy(n+1) = 0.;
	vx(n+2) = 0.;
	vy(n+2) = y.er;
	n += 2;
#endif

	signcache.resize(n+1, n+1);
	for (i=1; i<=n; i++) {
		for (j=i+1; j<=n; j++) {
			tmp = vy(i) * vx(j) - vx(i) * vy(j);
			if (tmp >= 0.) s = 1;
			else s = -1;
			signcache(i, j) = s;
			signcache(j, i) = -s;
		}
	}

	for (i=1; i<=n; i++) {
		dx = 0.;
		dy = 0.;
		for (j=1; j<=n; j++) {
			if (j == i) continue;
			// tmp = vy(i) * vx(j) - vx(i) * vy(j);
			// if (tmp >= 0.) s = 1;
			// else s = -1;
			s = signcache(i, j);
			dx += s * vx(j);
			dy += s * vy(j);
		}
		g.line(vx(0)+dx+vx(i), vy(0)+dy+vy(i),
		       vx(0)+dx-vx(i), vy(0)+dy-vy(i), color);
		g.line(vx(0)-dx+vx(i), vy(0)-dy+vy(i),
		       vx(0)-dx-vx(i), vy(0)-dy-vy(i), color);
	}
}

} // namespace kv

#endif // JOINTRANGE_HPP


///////////////////////////////////////////////////////////////////////////////
// statistics::model::random::importance_generate.hpp                        //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_MODEL_RANDOM_IMPORTANCE_SAMPLER_HPP_ER_2009
#define BOOST_STATISTICS_MODEL_RANDOM_IMPORTANCE_SAMPLER_HPP_ER_2009
#include <vector>
#include <boost/range.hpp>
#include <boost/concept_check.hpp>
#include <boost/statistics/model/wrap/aggregate/prior_model_dataset.hpp>
#include <boost/importance_weights/algorithm/prepare_weights.hpp>
#include <boost/importance_sampling/generate.hpp>

namespace boost{
namespace statistics{
namespace model{

    // Bundles together some tasks associated with importance sampling
    template<typename T>
    class importance_sampler{
        public:
        typedef std::vector<T>                                  weights_type;
        typedef typename range_iterator< weights_type >::type   iter_;
        typedef importance_weights::prepare_weights<T>          pws_type;

        importance_sampler();
        importance_sampler(const T& max_log);

        // [ Access ]
        const weights_type& weights()const;
        // See importance_sampling::prepare_weights
        const pws_type& pws()const;

        // [ Update ]
        // weights  <- (log_posterior - lpdfs)
        template<
            typename D,
            typename M,
            typename Rx,
            typename Ry,
            typename ItP,
            typename ItL
        >
        iter_
        log_weights(
            prior_model_dataset_<D,M,Rx,Ry> pmd,
            ItP     b_p,       // proposal pars
            ItP     e_p,
            ItL     b_lpdfs    // proposal log-pdfs
        );

        // weights <- exp(weights+offset)
        // Side effects : see importance_sampling::prepare_weights
        void prepare_weights();

        // Combines the two functions above
        template<
            typename D,
            typename M,
            typename Rx,
            typename Ry,
            typename ItP,
            typename ItL
        >
        void
        prepare_weights(
            prior_model_dataset_<D,M,Rx,Ry> pmd,
            ItP     b_p,       // proposal pars
            ItP     e_p,
            ItL     b_lpdfs    // proposal log-pdfs
        );

        template<typename ItP1,typename N,typename ItP,typename U>
        ItP1
        generate_n(
            ItP1 b_post,
            N n,
            ItP b_p,    // same as that which was passed to prepare_weights
            U& urng
        );

        private:
        weights_type    weights_;
        pws_type        pws_;
    };

}// model
}// statistics

    template<typename ItP1,typename N,typename T,
        typename D,typename M,typename Rx,typename Ry,
        typename ItP,typename ItL,typename U
    >
    ItP1
    generate_n(
        ItP1 b_post,
        N n,
        statistics::model::importance_sampler<T>& is,
        statistics::model::prior_model_dataset_<D,M,Rx,Ry> pmd,
        ItP     b_p,
        ItP     e_p,
        ItL     b_lpdfs,
        U& urng
    ){
        is.prepare_weights(
            pmd,
            b_p,
            e_p,
            b_lpdfs
        );
        return is.generate_n(
            b_post,
            n,
            b_p,
            urng
        );
    }

    // Implementation //

namespace statistics{
namespace model{

    template<typename T>
    importance_sampler<T>::importance_sampler():pws_(){}

    template<typename T>
    importance_sampler<T>::importance_sampler(const T& max_log):pws_(max_log){}

    // Update
    template<typename T>
    template<
        typename D,
        typename M,
        typename Rx,
        typename Ry,
        typename ItP,
        typename ItL
    >
    typename importance_sampler<T>::iter_
    importance_sampler<T>::log_weights(
        prior_model_dataset_<D,M,Rx,Ry> pmd,
        ItP     b_p,        // proposal pars
        ItP     e_p,
        ItL     b_lpdfs     // proposal log-pdfs
    ){
        this->weights_.resize(
            std::distance( b_p, e_p )
        );
        return log_posteriors2<T>(
            pmd,
            b_p,
            e_p,
            b_lpdfs,
            boost::begin( this->weights_ )
        );
    }

    template<typename T>
    void importance_sampler<T>::prepare_weights(){
        return this->pws_(
            boost::begin( this->weights_ ),
            boost::end( this->weights_ )
        );
    }

    template<typename T>
    template<
        typename D,
        typename M,
        typename Rx,
        typename Ry,
        typename ItP,
        typename ItL
    >
    void
    importance_sampler<T>::prepare_weights(
        prior_model_dataset_<D,M,Rx,Ry> pmd,
        ItP     b_p,
        ItP     e_p,
        ItL     b_lpdfs
    ){
        this->log_weights(
            pmd,
            b_p,
            e_p,
            b_lpdfs
        );
        return this->prepare_weights();
    }

    template<typename T>
    template<typename ItP1,typename N,typename ItP,typename U>
    ItP1
    importance_sampler<T>::generate_n(
        ItP1 b_post,
        N n,
        ItP b_p,
        U& urng
    ){
        return importance_sampling::generate(
            b_post,
            n,
            boost::begin( this->weights() ),
            boost::end( this->weights() ),
            b_p,
            urng
        );
    }

    // Access
    template<typename T>
    const typename importance_sampler<T>::weights_type&
    importance_sampler<T>::weights()const{
        return this->weights_;
    }

    template<typename T>
    const typename importance_sampler<T>::pws_type&
    importance_sampler<T>::pws()const{
        return this->pws_;
    }

}// model
}// statistics
}// boost

#endif


// Copyright (C) 2014 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/pose/fundamental_matrix_util.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <cmath>

#include "gtest/gtest.h"
#include "theia/sfm/pose/util.h"
#include "theia/sfm/types.h"
#include "theia/util/random.h"

namespace theia {

using Eigen::Matrix3d;
using Eigen::Quaterniond;
using Eigen::Vector3d;

RandomNumberGenerator rng(51);

TEST(FundamentalMatrixUtil, FocalLengths) {
  static const double kTolerance = 1e-8;

  for (int i = 0; i < 100; i++) {
    const double gt_focal_length1 = 800.0;
    const double gt_focal_length2 = 1000.0;
    const Vector3d rotation_angle_axis = rng.RandVector3d();
    const Matrix3d rotation =
        Eigen::AngleAxisd(rotation_angle_axis.norm(),
                          rotation_angle_axis.normalized()).toRotationMatrix();
    const Vector3d translation = rng.RandVector3d().normalized();

    // Create calibration matrices.
    Matrix3d fundamental_matrix;
    ComposeFundamentalMatrix(gt_focal_length1, gt_focal_length2,
                             rotation.data(), translation.data(),
                             fundamental_matrix.data());

    double focal_length1, focal_length2;
    EXPECT_TRUE(FocalLengthsFromFundamentalMatrix(
        fundamental_matrix.data(), &focal_length1, &focal_length2));
    EXPECT_NEAR(focal_length1, gt_focal_length1, kTolerance);
    EXPECT_NEAR(focal_length2, gt_focal_length2, kTolerance);
  }
}

TEST(FundamentalMatrixUtil, FundamentalMatrixFromProjectionMatrices) {
  static const double kTolerance = 1e-12;
  static const int kNumPoints = 10;

  for (int i = 0; i < 100; i++) {
    // Set up model points.
    Vector3d points_3d[kNumPoints];
    for (int j = 0; j < kNumPoints; j++) {
      points_3d[j] = rng.RandVector3d() + Vector3d(0, 0, 10);
    }

    // Set up projection matrices.
    const Vector3d rotation_angle_axis = rng.RandVector3d();
    const Matrix3d rotation =
        Eigen::AngleAxisd(rotation_angle_axis.norm(),
                          rotation_angle_axis.normalized()).toRotationMatrix();
    const Vector3d translation = rng.RandVector3d();
    Matrix3x4d pmatrix1, pmatrix2;
    pmatrix1 << Matrix3d::Identity(), Vector3d::Zero();
    pmatrix2 << rotation, translation;

    // Get the fundamental matrix.
    Matrix3d fmatrix;
    FundamentalMatrixFromProjectionMatrices(pmatrix1.data(), pmatrix2.data(),
                                            fmatrix.data());
    // Ensure the epipolar constraint holds.
    for (int j = 0; j < kNumPoints; j++) {
      const Vector3d image_point1 = pmatrix1 * points_3d[j].homogeneous();
      const Vector3d image_point2 = pmatrix2 * points_3d[j].homogeneous();

      EXPECT_LT(fabs(image_point1.transpose() * fmatrix * image_point2),
                kTolerance);
    }
  }
}

}  // namespace theia