#include "hoCuNDArray_utils.h"
#include "radial_utilities.h"
#include "hoNDArray_fileio.h"
#include "cuNDArray.h"
#include "imageOperator.h"
#include "identityOperator.h"
#include "hoPartialDerivativeOperator.h"
#include "hoCuConebeamProjectionOperator.h"
#include "cuConvolutionOperator.h"
#include "hoCuNDArray_blas.h"
#include "hoCuNDArray_elemwise.h"
#include "hoCuNDArray_blas.h"
#include "cgSolver.h"
#include "CBCT_acquisition.h"
#include "complext.h"
#include "encodingOperatorContainer.h"
#include "vector_td_io.h"
#include "hoPartialDerivativeOperator.h"
#include "hoCuGPBBSolver.h"
#include "hoCuTvOperator.h"
#include "hoCuTvPicsOperator.h"
#include "hoCuNCGSolver.h"
#include "hoCuCgDescentSolver.h"
#include "hoNDArray_utils.h"
#include "hoCuPartialDerivativeOperator.h"
#include "CBSubsetOperator.h"
#include "osMOMSolverD.h"
#include "cuSolverUtils.h"
#include <iostream>
#include <algorithm>
#include <sstream>
#include <math_constants.h>
#include <boost/program_options.hpp>
#include <boost/make_shared.hpp>
#include <solvers/osMOMSolverD3.h>
#include "CBSubsetWeightOperator.h"
#include "hdf5_utils.h"
using namespace std;
using namespace Gadgetron;

namespace po = boost::program_options;

class hoCuConvertOperator : public subsetOperator<hoNDArray<float>>{

public:
	hoCuConvertOperator(boost::shared_ptr<subsetOperator<hoCuNDArray<float>>> _op) : subsetOperator<hoNDArray<float>>(_op->get_number_of_subsets()),op(_op){};

	virtual void mult_M(hoNDArray<float> *in, hoNDArray<float> *out,int subset, bool accumulate) override { op->mult_M((hoCuNDArray<float>*)in,(hoCuNDArray<float>*) out, subset,accumulate);}
	virtual void mult_MH(hoNDArray<float> *in, hoNDArray<float> *out,int subset, bool accumulate) override { op->mult_MH((hoCuNDArray<float>*)in,(hoCuNDArray<float>*) out, subset, accumulate);}
	virtual void mult_MH_M(hoNDArray<float> *in, hoNDArray<float> *out,int subset, bool accumulate) override { op->mult_MH_M((hoCuNDArray<float>*)in,(hoCuNDArray<float>*) out, subset, accumulate);}

	virtual boost::shared_ptr< std::vector<size_t> > get_codomain_dimensions(int i) { return op->get_codomain_dimensions(i);}
	virtual boost::shared_ptr< std::vector<size_t> > get_domain_dimensions() { return op->get_domain_dimensions();}
	virtual void set_domain_dimensions( std::vector<size_t> * dims) { return op->set_domain_dimensions(dims);}
	virtual void set_codomain_dimensions( std::vector<size_t> * dims) { return op->set_codomain_dimensions(dims);}

protected:
	boost::shared_ptr<subsetOperator<hoCuNDArray<float>>> op;
};

boost::shared_ptr<hoCuNDArray<float> > calculate_prior(boost::shared_ptr<CBCT_binning>  binning,boost::shared_ptr<CBCT_acquisition> ps, hoCuNDArray<float>& projections, std::vector<size_t> is_dims, floatd3 imageDimensions){
	std::cout << "Calculating FDK prior" << std::endl;
	boost::shared_ptr<CBCT_binning> binning_pics=binning->get_3d_binning();
	std::vector<size_t> is_dims3d = is_dims;
	is_dims3d.pop_back();
	boost::shared_ptr< hoCuConebeamProjectionOperator >
	Ep( new hoCuConebeamProjectionOperator() );
	Ep->setup(ps,binning_pics,imageDimensions);
	Ep->set_codomain_dimensions(ps->get_projections()->get_dimensions().get());
	Ep->set_domain_dimensions(&is_dims3d);
	Ep->set_use_filtered_backprojection(true);
	boost::shared_ptr<hoCuNDArray<float> > prior3d(new hoCuNDArray<float>(&is_dims3d));
	Ep->mult_MH(&projections,prior3d.get());

	hoCuNDArray<float> tmp_proj(*ps->get_projections());
	Ep->mult_M(prior3d.get(),&tmp_proj);
	float s = dot(ps->get_projections().get(),&tmp_proj)/dot(&tmp_proj,&tmp_proj);
	*prior3d *= s;
	boost::shared_ptr<hoCuNDArray<float> > prior(new hoCuNDArray<float>(*expand( prior3d.get(), is_dims.back() )));
	std::cout << "Prior complete" << std::endl;
	return prior;
}

int main(int argc, char** argv)
{
	string acquisition_filename;
	string outputFile;
	uintd3 imageSize;
	floatd3 voxelSize;
	int device;
	floatd2 scale_factor;
	unsigned int iterations;
	unsigned int subsets;
	float rho;
	float tv_weight,tv_4d;
	po::options_description desc("Allowed options");

	desc.add_options()
    		("help", "produce help message")
    		("acquisition,a", po::value<string>(&acquisition_filename)->default_value("acquisition.hdf5"), "Acquisition data")
    		("samples,n",po::value<unsigned int>(),"Number of samples per ray")
    		("output,f", po::value<string>(&outputFile)->default_value("reconstruction.hdf5"), "Output filename")
    		("size,s",po::value<uintd3>(&imageSize)->default_value(uintd3(512,512,1)),"Image size in pixels")
    		("binning,b",po::value<string>(),"Binning file for 4d reconstruction")
    		("SAG","Use exact SAG correction if present")
    		("voxelSize,v",po::value<floatd3>(&voxelSize)->default_value(floatd3(0.488f,0.488f,1.0f)),"Voxel size in mm")
    		("dimensions,d",po::value<floatd3>(),"Image dimensions in mm. Overwrites voxelSize.")
    		("iterations,i",po::value<unsigned int>(&iterations)->default_value(10),"Number of iterations")
    		("device",po::value<int>(&device)->default_value(0),"Number of the device to use (0 indexed)")
					("downsample,D",po::value<floatd2>(&scale_factor)->default_value(floatd2(1,1)),"Downsample projections this factor")
    		("subsets,u",po::value<unsigned int>(&subsets)->default_value(10),"Number of subsets to use")
    		("TV",po::value<float>(&tv_weight)->default_value(0),"Total variation weight")
					("TV4D",po::value<float>(&tv_4d)->default_value(0),"Total variation weight in temporal dimension")
    		("use_prior","Use an FDK prior")
    		("3D","Only use binning data to determine wrong projections")
				  ("projection_weights",po::value<string>(),"Array containing weights to be applied to the projections.")
    		;

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

	if (vm.count("help")) {
		cout << desc << "\n";
		return 1;
	}

	std::cout << "Command line options:" << std::endl;
	for (po::variables_map::iterator it = vm.begin(); it != vm.end(); ++it){
		boost::any a = it->second.value();
		std::cout << it->first << ": ";
		if (a.type() == typeid(std::string)) std::cout << it->second.as<std::string>();
		else if (a.type() == typeid(int)) std::cout << it->second.as<int>();
		else if (a.type() == typeid(unsigned int)) std::cout << it->second.as<unsigned int>();
		else if (a.type() == typeid(float)) std::cout << it->second.as<float>();
		else if (a.type() == typeid(vector_td<float,3>)) std::cout << it->second.as<vector_td<float,3> >();
		else if (a.type() == typeid(vector_td<int,3>)) std::cout << it->second.as<vector_td<int,3> >();
		else if (a.type() == typeid(vector_td<unsigned int,3>)) std::cout << it->second.as<vector_td<unsigned int,3> >();
		else std::cout << "Unknown type" << std::endl;
		std::cout << std::endl;
	}

	cudaSetDevice(device);
	cudaDeviceReset();

	//Really weird stuff. Needed to initialize the device?? Should find real bug.
	cudaDeviceManager::Instance()->lockHandle();
	cudaDeviceManager::Instance()->unlockHandle();

	boost::shared_ptr<CBCT_acquisition> ps(new CBCT_acquisition());
	ps->load(acquisition_filename);
	ps->get_geometry()->print(std::cout);

    if (scale_factor[0] != 1 || scale_factor[1] != 1)
        ps->downsample(scale_factor[0],scale_factor[1]);

	float SDD = ps->get_geometry()->get_SDD();
	float SAD = ps->get_geometry()->get_SAD();

	boost::shared_ptr<CBCT_binning> binning(new CBCT_binning());
	if (vm.count("binning")){
		std::cout << "Loading binning data" << std::endl;
		binning->load(vm["binning"].as<string>());
		if (vm.count("3D"))
			binning = binning->get_3d_binning();
	} else binning->set_as_default_3d_bin(ps->get_projections()->get_size(2));
	binning->print(std::cout);

	floatd3 imageDimensions;
	if (vm.count("dimensions")){
		imageDimensions = vm["dimensions"].as<floatd3>();
		voxelSize = imageDimensions/imageSize;
	}
	else imageDimensions = voxelSize*imageSize;

	float lengthOfRay_in_mm = norm(imageDimensions);
	unsigned int numSamplesPerPixel = 3;
	float minSpacing = min(voxelSize)/numSamplesPerPixel;

	unsigned int numSamplesPerRay;
	if (vm.count("samples")) numSamplesPerRay = vm["samples"].as<unsigned int>();
	else numSamplesPerRay = ceil( lengthOfRay_in_mm / minSpacing );

	float step_size_in_mm = lengthOfRay_in_mm / numSamplesPerRay;
	size_t numProjs = ps->get_projections()->get_size(2);
	size_t needed_bytes = 2 * prod(imageSize) * sizeof(float);
	std::vector<size_t> is_dims = to_std_vector((uint64d3)imageSize);

	std::cout << "IS dimensions " << is_dims[0] << " " << is_dims[1] << " " << is_dims[2] << std::endl;
	std::cout << "Image size " << imageDimensions << std::endl;

	is_dims.push_back(binning->get_number_of_bins());

	// Define encoding matrix
	auto E = boost::make_shared<CBSubsetOperator<hoCuNDArray> >(subsets);

	if ( vm.count("projection_weights")) {
        auto weights = read_nd_array<float>(vm["projection_weights"].as<string>().c_str());
        if (scale_factor[0] != 1 || scale_factor[1] != 1) {
            cuNDArray<float> tmp_weights(*weights);
            auto dims = *tmp_weights.get_dimensions();
            tmp_weights = downsample_projections(&tmp_weights,scale_factor[0],scale_factor[1]);

            weights = tmp_weights.to_host();
        }

        if (!ps->get_projections()->dimensions_equal(weights.get()))
            throw std::runtime_error("Weight dimensions must match that of the projection data");
        auto EW  = boost::make_shared<CBSubsetWeightOperator<hoCuNDArray>>(subsets);
        EW->setup(ps,binning,imageDimensions,weights);
        E = EW;
    } else {
        std::cout <<"Normal projections" << std::endl;

        E->setup(ps, binning, imageDimensions);
    }
    E->set_domain_dimensions(&is_dims);
	//E->setup(ps,binning,imageDimensions);
	E->setup(ps,binning,imageDimensions);
	E->set_domain_dimensions(&is_dims);
	E->set_codomain_dimensions(ps->get_projections()->get_dimensions().get());

//	auto E2 = boost::make_shared<hoCuConvertOperator>(E);

	//hoCuGPBBSolver<float> solver;
	//hoCuCgDescentSolver<float> solver;
//	osSPSSolver<hoCuNDArray<float>> solver;
	osMOMSolverD3<hoCuNDArray<float>> solver;
	//osSPSSolver<hoNDArray<float>> solver;
	//hoCuNCGSolver<float> solver;
	solver.set_encoding_operator(E);
	//solver.set_domain_dimensions(&is_dims);
	solver.set_max_iterations(iterations);
	solver.set_output_mode(hoCuGPBBSolver<float>::OUTPUT_VERBOSE);
	solver.set_non_negativity_constraint(true);
	solver.set_tau(1e-4);
	solver.set_reg_steps(2);
	solver.set_dump(false);

	//solver.set_rho(rho);

	hoCuNDArray<float> projections = *ps->get_projections();
	E->offset_correct(&projections);

	boost::shared_ptr<hoCuNDArray<float> > prior;

	if (vm.count("use_prior")) {
		prior = calculate_prior(binning,ps,projections,is_dims,imageDimensions);
		solver.set_x0(prior);
	}
	/*
	if (tv_weight > 0){
		auto total_variation = boost::make_shared<hoCuTvOperator<float,4>>();
		total_variation->set_weight(tv_weight);
		//total_variation->set_weight_array(weight_array);
		solver.add_nonlinear_operator(total_variation);
		solver.set_kappa(tv_weight);
}
*/

  if (tv_weight > 0){

  	auto Dx = boost::make_shared<hoCuPartialDerivativeOperator<float,4>>(0);
  	Dx->set_weight(tv_weight);
  	Dx->set_domain_dimensions(&is_dims);
  	Dx->set_codomain_dimensions(&is_dims);

  	auto Dy = boost::make_shared<hoCuPartialDerivativeOperator<float,4>>(1);
  	Dy->set_weight(tv_weight);
  	Dy->set_domain_dimensions(&is_dims);
  	Dy->set_codomain_dimensions(&is_dims);

  	auto Dz = boost::make_shared<hoCuPartialDerivativeOperator<float,4>>(2);
  	Dz->set_weight(tv_weight);
  	Dz->set_domain_dimensions(&is_dims);
  	Dz->set_codomain_dimensions(&is_dims);

	  solver.add_regularization_group({Dx,Dy,Dz});

  }
if (tv_4d > 0){
  	auto Dt = boost::make_shared<hoCuPartialDerivativeOperator<float,4>>(3);
  	Dt->set_weight(tv_weight);
  	Dt->set_domain_dimensions(&is_dims);
  	Dt->set_codomain_dimensions(&is_dims);
  	solver.add_regularization_operator(Dt);
  	}

	auto result = solver.solve(&projections);

	//write_nd_array<float>( result.get(), outputFile.c_str());
	saveNDArray2HDF5(result.get(),outputFile,imageDimensions,floatd3(0,0,0),"",iterations);
}


/******************************************************************************
 * Copyright (C) 2013 by Jerome Maye                                          *
 * jerome.maye@gmail.com                                                      *
 *                                                                            *
 * This program is free software; you can redistribute it and/or modify       *
 * it under the terms of the Lesser GNU General Public License as published by*
 * the Free Software Foundation; either version 3 of the License, or          *
 * (at your option) any later version.                                        *
 *                                                                            *
 * This program is distributed in the hope that it will be useful,            *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of             *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the              *
 * Lesser GNU General Public License for more details.                        *
 *                                                                            *
 * You should have received a copy of the Lesser GNU General Public License   *
 * along with this program. If not, see <http://www.gnu.org/licenses/>.       *
 ******************************************************************************/

#include "aslam/calibration/algorithms/marginalize.h"

#include <cmath>

#include <Eigen/Dense>

#include <aslam/backend/CompressedColumnMatrix.hpp>

#include "aslam/calibration/exceptions/InvalidOperationException.h"
#include "aslam/calibration/exceptions/OutOfBoundException.h"

namespace aslam {
namespace calibration {

/******************************************************************************/
/* Methods                                                                    */
/******************************************************************************/

double colNorm(cholmod_sparse* A, size_t j) {
    if (j >= A->ncol)
        throw OutOfBoundException<size_t>(j, "colNorm(): index must be lower than the number of columns", __FILE__,
                                          __LINE__);
    const std::ptrdiff_t* col_ptr = reinterpret_cast<const std::ptrdiff_t*>(A->p);
    const double* values = reinterpret_cast<const double*>(A->x);
    const std::ptrdiff_t p = col_ptr[j];
    const std::ptrdiff_t numElements = col_ptr[j + 1] - p;
    double norm = 0;
    for (std::ptrdiff_t i = 0; i < numElements; ++i) norm += values[p + i] * values[p + i];
    return std::sqrt(norm);
}

Eigen::MatrixXd marginalJacobian(cholmod_sparse* J_x, cholmod_sparse* J_thetat, cholmod_common* cholmod) {
    // compute the QR factorization of J_x
    SuiteSparseQR_factorization<double>* QR =
        SuiteSparseQR_factorize<double>(SPQR_ORDERING_BEST, SPQR_DEFAULT_TOL, J_x, cholmod);
    if (QR == NULL)
        throw InvalidOperationException(
            "marginalJacobian(): "
            "SuiteSparseQR_factorize failed");

    // compute the Jacobian of the reduced system
    cholmod_sparse* J_thetatQFull = SuiteSparseQR_qmult<double>(SPQR_XQ, QR, J_thetat, cholmod);
    if (J_thetatQFull == NULL) {
        SuiteSparseQR_free(&QR, cholmod);
        throw InvalidOperationException(
            "marginalJacobian(): "
            "SuiteSparseQR_qmult failed");
    }
    std::ptrdiff_t* colIndices = new std::ptrdiff_t[J_x->ncol];
    for (size_t i = 0; i < J_x->ncol; ++i) colIndices[i] = i;
    cholmod_sparse* J_thetatQ = cholmod_l_submatrix(J_thetatQFull, NULL, -1, colIndices, J_x->ncol, 1, 0, cholmod);
    delete[] colIndices;
    if (J_thetatQ == NULL) {
        SuiteSparseQR_free(&QR, cholmod);
        cholmod_l_free_sparse(&J_thetatQFull, cholmod);
        throw InvalidOperationException(
            "marginalJacobian(): "
            "cholmod_l_submatrix failed");
    }
    cholmod_sparse* J_thetat2 = cholmod_l_aat(J_thetat, NULL, 0, 1, cholmod);
    if (J_thetat2 == NULL) {
        SuiteSparseQR_free(&QR, cholmod);
        cholmod_l_free_sparse(&J_thetatQFull, cholmod);
        cholmod_l_free_sparse(&J_thetatQ, cholmod);
        throw InvalidOperationException(
            "marginalJacobian(): "
            "cholmod_l_aat failed");
    }
    cholmod_sparse* J_thetatQ2 = cholmod_l_aat(J_thetatQ, NULL, 0, 1, cholmod);
    if (J_thetatQ2 == NULL) {
        SuiteSparseQR_free(&QR, cholmod);
        cholmod_l_free_sparse(&J_thetatQFull, cholmod);
        cholmod_l_free_sparse(&J_thetatQ, cholmod);
        cholmod_l_free_sparse(&J_thetat2, cholmod);
        throw InvalidOperationException(
            "marginalJacobian(): "
            "cholmod_l_aat failed");
    }
    double alpha[2];
    alpha[0] = 1;
    double beta[2];
    beta[0] = -1;
    cholmod_sparse* Omega = cholmod_l_add(J_thetat2, J_thetatQ2, alpha, beta, 1, 0, cholmod);
    if (Omega == NULL) {
        SuiteSparseQR_free(&QR, cholmod);
        cholmod_l_free_sparse(&J_thetatQFull, cholmod);
        cholmod_l_free_sparse(&J_thetatQ, cholmod);
        cholmod_l_free_sparse(&J_thetat2, cholmod);
        cholmod_l_free_sparse(&J_thetatQ2, cholmod);
        throw InvalidOperationException(
            "marginalJacobian(): "
            "cholmod_l_add failed");
    }
    aslam::backend::CompressedColumnMatrix<std::ptrdiff_t> OmegaCCM;
    OmegaCCM.fromCholmodSparse(Omega);
    Eigen::MatrixXd OmegaDense(Omega->nrow, Omega->ncol);
    OmegaCCM.toDenseInto(OmegaDense);

    // clean allocated memory
    SuiteSparseQR_free(&QR, cholmod);
    cholmod_l_free_sparse(&J_thetatQFull, cholmod);
    cholmod_l_free_sparse(&J_thetatQ, cholmod);
    cholmod_l_free_sparse(&J_thetat2, cholmod);
    cholmod_l_free_sparse(&J_thetatQ2, cholmod);
    cholmod_l_free_sparse(&Omega, cholmod);

    return OmegaDense;
}

double marginalize(const aslam::backend::CompressedColumnMatrix<std::ptrdiff_t>& Jt, size_t j, Eigen::MatrixXd& NS,
                   Eigen::MatrixXd& CS, Eigen::MatrixXd& Sigma, Eigen::MatrixXd& SigmaP, Eigen::MatrixXd& Omega,
                   double normTol, double epsTol) {
    // init cholmod
    cholmod_common cholmod;
    cholmod_l_start(&cholmod);

    // convert to cholmod_sparse
    cholmod_sparse JtCs;
    const_cast<aslam::backend::CompressedColumnMatrix<std::ptrdiff_t>&>(Jt).getView(&JtCs);
    cholmod_sparse* J = cholmod_l_transpose(&JtCs, 1, &cholmod);
    if (J == NULL) {
        cholmod_l_finish(&cholmod);
        throw InvalidOperationException(
            "marginalize(): "
            "cholmod_l_transpose failed");
    }

    // extract the part corresponding to the state/landmarks/...
    std::ptrdiff_t* colIndices = new std::ptrdiff_t[j];
    for (size_t i = 0; i < j; ++i) colIndices[i] = i;
    cholmod_sparse* J_x = cholmod_l_submatrix(J, NULL, -1, colIndices, j, 1, 0, &cholmod);
    delete[] colIndices;
    if (J_x == NULL) {
        cholmod_l_free_sparse(&J, &cholmod);
        cholmod_l_finish(&cholmod);
        throw InvalidOperationException(
            "marginalize(): "
            "cholmod_l_submatrix failed");
    }

    // extract the part corresponding to the calibration parameters
    colIndices = new std::ptrdiff_t[J->ncol - j];
    for (size_t i = j; i < J->ncol; ++i) colIndices[i - j] = i;
    cholmod_sparse* J_theta = cholmod_l_submatrix(J, NULL, -1, colIndices, J->ncol - j, 1, 0, &cholmod);
    delete[] colIndices;
    if (J_theta == NULL) {
        cholmod_l_free_sparse(&J, &cholmod);
        cholmod_l_free_sparse(&J_x, &cholmod);
        cholmod_l_finish(&cholmod);
        throw InvalidOperationException(
            "marginalize(): "
            "cholmod_l_submatrix failed");
    }
    cholmod_sparse* J_thetat = cholmod_l_transpose(J_theta, 1, &cholmod);
    if (J_thetat == NULL) {
        cholmod_l_free_sparse(&J, &cholmod);
        cholmod_l_free_sparse(&J_x, &cholmod);
        cholmod_l_free_sparse(&J_theta, &cholmod);
        cholmod_l_finish(&cholmod);
        throw InvalidOperationException(
            "marginalize(): "
            "cholmod_l_transpose failed");
    }

    // compute the marginal Jacobian
    try {
        Omega = marginalJacobian(J_x, J_thetat, &cholmod);
    } catch (const InvalidOperationException& e) {
        cholmod_l_free_sparse(&J, &cholmod);
        cholmod_l_free_sparse(&J_x, &cholmod);
        cholmod_l_free_sparse(&J_theta, &cholmod);
        cholmod_l_free_sparse(&J_thetat, &cholmod);
        cholmod_l_finish(&cholmod);
        throw;
    }

    // scale J_x
    cholmod_dense* G_x = cholmod_l_allocate_dense(J_x->ncol, 1, J_x->ncol, CHOLMOD_REAL, &cholmod);
    if (G_x == NULL) {
        cholmod_l_free_sparse(&J, &cholmod);
        cholmod_l_free_sparse(&J_x, &cholmod);
        cholmod_l_free_sparse(&J_theta, &cholmod);
        cholmod_l_free_sparse(&J_thetat, &cholmod);
        cholmod_l_finish(&cholmod);
        throw InvalidOperationException(
            "marginalize(): "
            "cholmod_l_allocate_dense failed");
    }
    try {
        double* values = reinterpret_cast<double*>(G_x->x);
        for (size_t j = 0; j < J_x->ncol; ++j) {
            const double normCol = colNorm(J_x, j);
            if (normCol < normTol)
                values[j] = 0.0;
            else
                values[j] = 1.0 / normCol;
        }
    } catch (const OutOfBoundException<size_t>& e) {
        cholmod_l_free_sparse(&J, &cholmod);
        cholmod_l_free_sparse(&J_x, &cholmod);
        cholmod_l_free_sparse(&J_theta, &cholmod);
        cholmod_l_free_sparse(&J_thetat, &cholmod);
        cholmod_l_free_dense(&G_x, &cholmod);
        cholmod_l_finish(&cholmod);
        throw;
    }
    if (cholmod_l_scale(G_x, CHOLMOD_COL, J_x, &cholmod) == 0) {
        cholmod_l_free_sparse(&J, &cholmod);
        cholmod_l_free_sparse(&J_x, &cholmod);
        cholmod_l_free_sparse(&J_theta, &cholmod);
        cholmod_l_free_sparse(&J_thetat, &cholmod);
        cholmod_l_free_dense(&G_x, &cholmod);
        cholmod_l_finish(&cholmod);
        throw InvalidOperationException(
            "marginalize(): "
            "cholmod_l_scale failed");
    }
    cholmod_l_free_dense(&G_x, &cholmod);

    // scale J_thetat
    cholmod_dense* G_theta = cholmod_l_allocate_dense(J_theta->ncol, 1, J_theta->ncol, CHOLMOD_REAL, &cholmod);
    if (G_theta == NULL) {
        cholmod_l_free_sparse(&J, &cholmod);
        cholmod_l_free_sparse(&J_x, &cholmod);
        cholmod_l_free_sparse(&J_theta, &cholmod);
        cholmod_l_free_sparse(&J_thetat, &cholmod);
        cholmod_l_finish(&cholmod);
        throw InvalidOperationException(
            "marginalize(): "
            "cholmod_l_allocate_dense failed");
    }
    try {
        double* values = reinterpret_cast<double*>(G_theta->x);
        for (size_t j = 0; j < J_theta->ncol; ++j) {
            const double normCol = colNorm(J_theta, j);
            if (normCol < normTol)
                values[j] = 0.0;
            else
                values[j] = 1.0 / normCol;
        }
    } catch (const OutOfBoundException<size_t>& e) {
        cholmod_l_free_sparse(&J, &cholmod);
        cholmod_l_free_sparse(&J_x, &cholmod);
        cholmod_l_free_sparse(&J_theta, &cholmod);
        cholmod_l_free_sparse(&J_thetat, &cholmod);
        cholmod_l_free_dense(&G_theta, &cholmod);
        cholmod_l_finish(&cholmod);
        throw;
    }
    if (cholmod_l_scale(G_theta, CHOLMOD_ROW, J_thetat, &cholmod) == 0) {
        cholmod_l_free_sparse(&J, &cholmod);
        cholmod_l_free_sparse(&J_x, &cholmod);
        cholmod_l_free_sparse(&J_theta, &cholmod);
        cholmod_l_free_sparse(&J_thetat, &cholmod);
        cholmod_l_free_dense(&G_theta, &cholmod);
        cholmod_l_finish(&cholmod);
        throw InvalidOperationException(
            "marginalize(): "
            "cholmod_l_scale failed");
    }
    cholmod_l_free_dense(&G_theta, &cholmod);

    // compute the scaled marginal Jacobian
    Eigen::MatrixXd OmegaScaled;
    try {
        OmegaScaled = marginalJacobian(J_x, J_thetat, &cholmod);
    } catch (const InvalidOperationException& e) {
        cholmod_l_free_sparse(&J, &cholmod);
        cholmod_l_free_sparse(&J_x, &cholmod);
        cholmod_l_free_sparse(&J_theta, &cholmod);
        cholmod_l_free_sparse(&J_thetat, &cholmod);
        cholmod_l_finish(&cholmod);
        throw;
    }

    // clean cholmod
    cholmod_l_free_sparse(&J, &cholmod);
    cholmod_l_free_sparse(&J_x, &cholmod);
    cholmod_l_free_sparse(&J_theta, &cholmod);
    cholmod_l_free_sparse(&J_thetat, &cholmod);
    cholmod_l_finish(&cholmod);

    // compute the thin SVD of OmegaScaled
    const Eigen::JacobiSVD<Eigen::MatrixXd> svdScaled(OmegaScaled, Eigen::ComputeThinU | Eigen::ComputeThinV);

    // compute the numerical rank
    size_t nrank = OmegaScaled.cols();
    const Eigen::VectorXd& SScaled = svdScaled.singularValues();
    const double tol = OmegaScaled.rows() * SScaled(0) * epsTol;
    for (std::ptrdiff_t i = OmegaScaled.cols() - 1; i > 0; --i) {
        if (SScaled(i) > tol)
            break;
        else
            nrank--;
    }

    // compute the thin SVD of Omega
    const Eigen::JacobiSVD<Eigen::MatrixXd> svd(Omega, Eigen::ComputeThinU | Eigen::ComputeThinV);
    const Eigen::MatrixXd& V = svd.matrixV();

    // compute the numerical column space
    CS = V.block(0, 0, V.rows(), nrank);

    // compute the numerical null space
    NS = V.block(0, nrank, V.rows(), Omega.cols() - nrank);

    // compute the projected covariance matrix
    Eigen::MatrixXd invS(Eigen::MatrixXd::Zero(Omega.cols(), Omega.cols()));
    SigmaP = Eigen::MatrixXd::Zero(nrank, nrank);
    double svLogSum = 0;
    const Eigen::VectorXd& S = svd.singularValues();
    for (size_t i = 0; i < nrank; ++i) {
        SigmaP(i, i) = 1.0 / S(i);
        svLogSum = svLogSum + log2(S(i));
        invS(i, i) = SigmaP(i, i);
    }

    // compute the covariance matrix
    Sigma = V * invS * V.transpose();
    return svLogSum;
}

}  // namespace calibration
}  // namespace aslam


#include "prediction_behavior.h"
#include "scene_elements/person_behavior.h"
//#include <math.h>
#include <algorithm>
#include <stdio.h>
#include <Eigen/Dense>
#include <iostream>

Cprediction_behavior::Cprediction_behavior( double horizon_time , bool behavior_flag,
		Cperson_abstract::filtering_method filter, Cperson_abstract::update_person_method update_method ) :
			Cprediction_bhmip(filter,update_method),
	horizon_time_(horizon_time), behavior_flag_(behavior_flag)
{
	horizon_time_index_ =  (unsigned int) (horizon_time_/dt_) ;
	// paramters = {k,lambda,a,b,d} according to FerrerIcra2014
	force_params_balanced_ = {2.3, 0.56,3.13,2.91,0.2};
	force_params_aware_ = {2.3, 0.56,4.78,6.22,0.2};
	force_params_unaware_= {2.3, 0.56,0.98,0.16,0.2};

}

Cprediction_behavior::~Cprediction_behavior()
{
	//deletion of robot and persons is done on its parent class Cprediction_bhmip
}

Cperson_abstract* Cprediction_behavior::add_person_container( unsigned int id,
		std::list<Cperson_abstract*>::iterator iit )
{
	//this is the implementation of the abstract interface defined in Cscene_abstract, and although it
	// it has been defined in the father class, when calling 'update_scene()' the new container used will
	//be the one defined here
	Cperson_behavior* person = new Cperson_behavior(id,Cperson_abstract::Person, scene_force_type_, filtering_time_window_);

	person_list_.insert( iit , person );
	person->set_destinations(destinations_);

	return (Cperson_abstract*) person;
}

void Cprediction_behavior::scene_prediction()
{
	// Intentionality prediction
	Cprediction_bhmip::scene_intentionality_prediction_bhmip();

	if( behavior_flag_  )
	{
		scene_behavior_estimation();
	}

	//calculation of the trajectories
	scene_trajectory_prediction();

}

void Cprediction_behavior::scene_behavior_estimation()
{
	Sforce obs, goal;
	double dt;
	for( Cperson_abstract* observed_person: person_list_ )
	{
		//calculate the estimation for each force, according to the observed position and velocities
		dt = observed_person->get_diff_position().time_stamp;
		obs = Sforce( observed_person->get_diff_position().vx /dt , observed_person->get_diff_position().vy /dt);
		goal = observed_person->force_goal( observed_person->get_best_dest(), this->get_sfm_params( observed_person) );
		for ( Cperson_abstract* center_person : person_list_ )
		{
			if( *observed_person != *center_person  &&
					observed_person->get_current_pointV().distance( center_person->get_current_pointV() ) < 4.0 )//threshold of 4[m]
				this->behavior_update( observed_person, center_person, obs-goal );
		}
		//same calculation for the robot
		if ( robot_in_the_scene_ ){
		if( *observed_person != *robot_ &&
				observed_person->get_current_pointV().distance( robot_->get_current_pointV() ) < 4.0 )//threshold of 4[m]
			this->behavior_update( observed_person, robot_, obs-goal );
		}
	}

}

void Cprediction_behavior::behavior_update( Cperson_abstract* observed_person, Cperson_abstract* center_person, const Sforce& observed_int)
{
	//calculate the expected force
	double lh,force_mod, marginalization, norm(0.0);
	std::vector<double> posterior_prob( 3, 0.0 );
	Sforce expected_force;
	//find behavior for person observed, according to center person
	Sbehavior * behavior = center_person->find_behavior_estimation ( observed_person->get_id() );
	assert( behavior != NULL );
	for ( unsigned int i = 0; i< behavior->expectation.size() ; i++)
	{
		marginalization = 0.0;
		for( unsigned int j = 0; j< behavior->expectation.size() ; j++)
		{
			marginalization += behavior->expectation[j] * ( i == j ? 0.9 : 0.05 );//transition matrix parameters hrdcoded for 3 states
		}
		//HMM : the observed person recalculates its interaction force according to the sfm behaviors after interacting with center person,
		// which updates its behavior information upon these results
		expected_force = observed_person->force( center_person->get_current_pointV()  ,
				get_behavior_sfm_params( (Cperson_abstract::behavior_type)i  )  );
		force_mod = ( observed_int - expected_force ).module2();
		//std::cout << "force_mod " << force_mod << "for iteration " << i << std::endl;
		lh = exp( -0.5 * force_mod );//std = 1 TODO
		posterior_prob[i] = marginalization * lh;
		norm += posterior_prob[i];
	}

	//normalize HMM
	if ( norm <= 0 ) norm = 1e-20;
	for ( unsigned int i = 0; i< behavior->expectation.size() ; i++ )
		behavior->expectation[i] =  posterior_prob[i] / norm;

	//behavior->print();

}

const std::vector<double>*
Cprediction_behavior::get_behavior_sfm_params( Cperson_abstract::behavior_type best_behavior_type )
{
	switch( best_behavior_type)
	{
	  case Cperson_abstract::Balanced:
		return &force_params_balanced_;
	  case Cperson_abstract::Aware:
		return &force_params_aware_;
	  case Cperson_abstract::Unaware :
	  default :
		return &force_params_unaware_;
	}
}

const std::vector<double>*
Cprediction_behavior::get_sfm_int_params( const Cperson_abstract * center_person,	const Cperson_abstract * interacting_person )
{
	const std::vector<double>* result = Cscene_abstract::get_sfm_int_params(center_person, interacting_person);

	//if flag enabled, certain combinations of interactions may change, we modify only those that are different
	if ( behavior_flag_ )
	{
		result = get_behavior_sfm_params( center_person->get_best_behavior_to_person( interacting_person->get_id() ) );
	}
	return result;
}

void Cprediction_behavior::scene_trajectory_prediction(  )
{

	SpointV_cov virtual_next_pose;
	Sforce virtual_force_goal, virtual_force_int_person, virtual_force_obstacle, virtual_force_robot;
	for( Cperson_abstract* iit: person_list_ )
		iit->clear_prediction_trajectory();
	for( unsigned int t = 0;  t < horizon_time_index_; ++t)
	{
		for( Cperson_abstract* iit: person_list_ )
		{
			if ( iit->get_person_type() == Cperson_abstract::Person)
			{
				//1 time step propagation for all people
				virtual_force_goal = iit->force_goal(  iit->get_best_dest() , this->get_sfm_params(iit),
						&(iit->get_prediction_trajectory()->at(t)) );
				virtual_force_int_person = force_persons_int_virtual( iit, t );
				// force depending on map to be deprecated in real environments, but still used on simulations
				if( read_force_map_success_ )
					virtual_force_obstacle = get_force_map( iit->get_prediction_trajectory()->at(t).x,
							iit->get_prediction_trajectory()->at(t).y ) * 0.5;
				if(read_laser_obstacle_success_)
					virtual_force_obstacle = force_objects_laser_int_prediction( iit , t );
				//robot stays idle, no prediction is done
				if ( robot_in_the_scene_ )
				{
					virtual_force_robot =  iit->force( robot_->get_current_pointV() , this->get_sfm_int_params(iit,robot_),
						&(iit->get_prediction_trajectory()->at(t)) );
				}
				else
					virtual_force_robot = Sforce();
				iit->set_forces_person( virtual_force_goal, virtual_force_int_person, virtual_force_robot, virtual_force_obstacle );
				iit->prediction_propagation(dt_, iit->get_force_person() , t);
			}
			//else //robot case: no prediction is done since it is the objective of the planning class.
		}
	}

}

Sforce Cprediction_behavior::force_objects_laser_int_prediction( Cperson_abstract* person, unsigned int prediction_index)
{
	Sforce force_res;
	const SpointV_cov* virtual_current_point;
	if ( prediction_index == 0)
	{
		virtual_current_point = &(person->get_current_pointV());
	}
	else
	{
		virtual_current_point = &(person->get_prediction_trajectory()->at(prediction_index) );
	}
	for( Spoint iit : laser_obstacle_list_)
	{
		//there is a list for persons and another for robot(s). This function is for obstacles
		if( person->get_current_pointV().distance2( iit ) < 25.0)//square distance 5^2
		{
			force_res += person->force_sphe( iit , this->get_sfm_int_params(person) , virtual_current_point );
		}
	}
	return force_res;

}

Sforce Cprediction_behavior::force_persons_int_virtual(Cperson_abstract* center , unsigned long t)
{
	Sforce force_res;
	SpointV_cov person_int;
    for( auto iit : person_list_)
    {
		person_int = iit->get_prediction_trajectory( )->at(t);

		if(iit->get_person_type() == Cperson_abstract::Person &&
			*center != *iit && center->get_prediction_trajectory()->at(t)
			.distance( person_int ) < 15.0)
		{
		force_res += center->force( person_int ,  this->get_sfm_int_params(center,iit),
				&(center->get_prediction_trajectory()->at(t)) );
		//std::cout << "prediction, calculation of force using " << center->get_best_behavior_to_person( iit->get_id() ) << std::endl;
		}
    }
	return force_res;
}

void Cprediction_behavior::set_horizon_time( double t )
{
	horizon_time_ = t;
	horizon_time_index_ =  (unsigned int) (horizon_time_/dt_) ;
}

void Cprediction_behavior::clear_scene()
{

}

//calculates the current forces in the scene and updates each of the Cpersons and Crobot with
//instantaneous force information according to observations (current positions)
void Cprediction_behavior::calculate_current_forces( int pr_force_mode )
{
	SpointV_cov virtual_next_pose;
	Sforce virtual_force_goal, virtual_force_int_person, virtual_force_obstacle, virtual_force_robot;

	for( Cperson_abstract* iit: person_list_ )
	{
		if ( iit->get_person_type() == Cperson_abstract::Person)
		{
			//1 time step propagation for all people
			virtual_force_goal = iit->force_goal(  iit->get_best_dest() , this->get_sfm_params(iit) );
			virtual_force_int_person = Cscene_abstract::force_persons_int( iit);
			// force depending on map to be deprecated in real environments, but still used on simulations
			if( read_force_map_success_ )
				virtual_force_obstacle = get_force_map( iit->get_current_pointV().x,
						iit->get_current_pointV().y ) * 0.5;
			if(read_laser_obstacle_success_)
				virtual_force_obstacle = Cscene_abstract::force_objects_laser_int( iit  );
			//robot stays idle, no prediction is done
			if ( robot_in_the_scene_ )
			{
				switch( pr_force_mode )
				{
				case 0: //deterministic force, classical definition
					virtual_force_robot = iit->force( robot_->get_current_pointV() , this->get_sfm_int_params(iit,robot_), &(iit->get_current_pointV()) );
					break;
				case 1: //probabilistic force, sampling around the elipsoid
					virtual_force_robot = iit->force_sphe_prob( robot_->get_current_pointV(), get_sfm_int_params(iit,robot_),&(iit->get_current_pointV()) );
					break;
				case 2: //probabilistic force, mahalanobis distance
					virtual_force_robot = iit->force_sphe_mahalanobis( robot_->get_current_pointV(), get_sfm_int_params(iit,robot_), &(iit->get_current_pointV()));
					break;
				case 3: //probabilistic force, worst case scenario: covaraince ellipsoid nearer to the center point
					virtual_force_robot = iit->force_sphe_worst( robot_->get_current_pointV(), get_sfm_int_params(iit,robot_),&(iit->get_current_pointV()));
					break;
				}
			}
			else
				virtual_force_robot = Sforce();
			iit->set_forces_person( virtual_force_goal, virtual_force_int_person, virtual_force_robot, virtual_force_obstacle );
		}
	}
	//robot update, if any
	if ( robot_in_the_scene_ )
	{
		virtual_force_goal = robot_->force_goal(  robot_->get_best_dest() , this->get_sfm_params(robot_) );
		virtual_force_int_person = Cscene_abstract::force_persons_int( robot_ );
		// force depending on map to be deprecated in real environments, but still used on simulations
		if( read_force_map_success_ )
			virtual_force_obstacle = get_force_map( robot_->get_current_pointV().x,
					robot_->get_current_pointV().y ) * 0.5;
		if(read_laser_obstacle_success_)
			virtual_force_obstacle = Cscene_abstract::force_objects_laser_int( robot_  );
		virtual_force_robot = Sforce();
		robot_->set_forces_person( virtual_force_goal, virtual_force_int_person, virtual_force_robot, virtual_force_obstacle );
	}
}


//
//  Oscillator.cpp
//  Synthesis
//
//  Created by Devin Mooers on 1/12/14.
//
//

#include "Oscillator.h"
//#include <vector>
#include <boost/array.hpp>
#include <OpenCL/OpenCL.h>

//#include <limits>
//#include <cmath>
//double epsilon = std::numeric_limits<double>::denorm_min();

double Oscillator::mSampleRate = 44100.0;

void Oscillator::setMode(OscillatorMode mode) {
    mOscillatorMode = mode;
}

void Oscillator::setFrequency(double frequency) {
    mFrequency = frequency;
    if (mFrequency != mLastFrequency) {
        mLastFrequency = mFrequency;
        updateInstrumentModel(); // update instrument model only when new frequency is different than last calculated/stored one /// ERROR - this means it won't re-run if you hit the same note twice in a row! figure out a better method... call from onNoteOn?
    }
}

void Oscillator::setVelocity(int velocity) {
    mVelocity = velocity;
}

void Oscillator::setSampleRate(double sampleRate) {
    mSampleRate = sampleRate;
    mTimeStep = 1.0 / sampleRate;
    updateInstrumentModel();
}

void Oscillator::updateTime() {
    mTime = 0.0; // reset amplitude increment on every new note trigger (this is used to calculate accelerating decays for higher partials)
}

// only need to update numpartials and string frequencies with every noteOn, I think (or pitch bend, actually), but DEFINITELY not needed with every stinkin sample
void Oscillator::updateInstrumentModel() {

    updateTime();

    /// update frequency based on pitch mod

    double pitchModAsFrequency = pow(2.0, fabs(mPitchMod) * 14.0) - 1;
    if (mPitchMod < 0) {
        pitchModAsFrequency = -pitchModAsFrequency;
    }
    double calculatedFrequency = fmin(fmax(mFrequency + pitchModAsFrequency, 0), mSampleRate / 2.0); // ensures that f is greater than 0 and less than nyquist

    mFrequency = calculatedFrequency;

    /// update string ratio
    float LO = 1.0 - mStringDetuneRange;
    float HI = 1.0 + mStringDetuneRange;
    mStringRatio = LO + static_cast <float> (rand()) /( static_cast <float> (RAND_MAX/(HI-LO)));

    /// update chaotic attractor values per note hit
    //mLRand = (static_cast <float> (rand()) / static_cast <float> (RAND_MAX)) * 0.5 + 0.5; // 0.5 to 1.0

    /// update random partial frequency multipliers and amplitudes
    float randAmpScalar = 0.0;
    float freqOffsetRange = 0.002;
    float freqLO = 1.0 - freqOffsetRange;
    float freqHI = 1.0 + freqOffsetRange;
    float ampOffsetRange = 0.0;
    float ampLO = 1.0 - ampOffsetRange;
    float ampHI = 1.0 + ampOffsetRange;

    double partialInharmonicityCoeffTimesFreq;

    for (int i = 1; i < mNumPartials + 1; i++) {

        /// OPTIMIZATION: actually just calculate an array of these whenever B changes - one for each partial index up to max number of partials. That way I don't have to calc a bunch of sqrt's every time a new note is hit - only when B is changed.
        partialInharmonicityCoeffTimesFreq = mFrequency * sqrt((1 + mB * i * i)); // only need to calc once per approx partial freq

        /// calculate first string partial frequencies
        mPartialFrequencies[(i-1)*2] = partialInharmonicityCoeffTimesFreq * i * (freqLO + static_cast <float> (rand()) /( static_cast <float> (RAND_MAX/(freqHI-freqLO)))); // Harvey Fletcher's equation for partial inharmonicity, just a multiplier here (for multiplying actual partial frequency);

        /// calculate second string partial frequencies
        mPartialFrequencies[(i-1)*2+1] = partialInharmonicityCoeffTimesFreq * i * (freqLO + static_cast <float> (rand()) /( static_cast <float> (RAND_MAX/(freqHI-freqLO)))) * mStringRatio;

        /// calculate first string random partial amplitudes
        randAmpScalar = (ampLO + static_cast <float> (rand()) /( static_cast <float> (RAND_MAX/(ampHI-ampLO))));
        mPartialAmplitudes[(i-1)*2] = randAmpScalar; // assign random partial amplitude multiplier to each

        /// calculate second string random partial amplitudes
        randAmpScalar = (ampLO + static_cast <float> (rand()) /( static_cast <float> (RAND_MAX/(ampHI-ampLO))));
        mPartialAmplitudes[(i-1)*2+1] = randAmpScalar; // assign random partial amplitude multiplier to each

        /// calculate first string partial pan values
        mPartialPanValues[(i - 1)*2] = static_cast <float> (rand()) / static_cast <float> (RAND_MAX); // 0 to 1

        /// calculate second string partial pan values
        mPartialPanValues[(i - 1)*2+1] = static_cast <float> (rand()) / static_cast <float> (RAND_MAX); // 0 to 1

    }
}

void Oscillator::updateInharmonicityCoeff(double mBNew) {
    mB = mBNew;
    //std::cout << "\nmB = " << mB;
    updateInstrumentModel();
}

void Oscillator::updateNumPartials(int mNumPartialsNew) {
    mNumPartials = mNumPartialsNew;
    //std::cout << "\nmNumPartials = " << mNumPartials;
    updateInstrumentModel();
}

void Oscillator::setPitchMod(double amount) {
    mPitchMod = amount;
    updateInstrumentModel();
}

void Oscillator::getLocationBasedAmplitude(int i) {
    // takes partial index (harmonic index) and string strike location and returns amplitude of harmonic

    mStringHitLocation = 0.2 - mVelocity / 635.0; // range between 0 and 0.2 - the higher the velocity, the closer it is toward the bridge! (makes for brighter sound)

    mLocationBasedAmplitude = sin(mPI*mStringHitLocation*i); // string strike location = x, wavelength gamma (relative to string length) = 2 / i, and amplitude = sin(2*pi*x / gamma)

    // edit this to instead be an array of location-based amplitudes that gets updated with every note trigger (### or add it to mpartialamplitudemultipliers calculation above!!!)
}

boost::array<double, 2> Oscillator::nextSample() {

    samples[0] = samples[1] = 0.0; // reset samples to zeroes

    switch (mOscillatorMode) {

        /// Make an OpenCL kernel built into Oscillator class... :D (copy "square" kernel from hello_world example and make it sin)

        /// make mTransientLength into a knob! (so I can select on the fly)
        /// make mStringDetuneRange into a knob!
        /// make mAttack into a knob! ranging from 1000 to 1,000,000 perhaps.
        /// implement an on/off switch for transient noise! (and eventually a selector for which type of transient noise - lorenz? tinkerbell? etc., along with parameters)
        /// make a gain knob!

        /// implement mB scaling, so lower notes get a lower mB, and higher notes get higher mB (so high notes sound piano-like, but low notes don't sound disgustingly out of tune)

        /// Synthesis.cpp: improve the keyboard graphic using spacing here (gotta make BG graphic, and uplaod new key images) http://www.martin-finke.de/blog/articles/audio-plugins-015-redesign/

        /// Start reading about OpenCL... how to code with OpenCL in C++? Include a framework? Then how to do audio processing on it? Can I do it per-sample?

        /// look up more efficient sine oscillator methods here: http://www.musicdsp.org/archive.php?classid=1#241 // http://www.musicdsp.org/archive.php?classid=1
        // or here: https://ccrma.stanford.edu/~jos/pasp04/Digital_Waveguide_Oscillator.html and here for freq masking improvements: http://www.csis.ul.ie/dafx01/proceedings/papers/lagrange.pdf

        /// OPTIMIZATION: in updateInstrumentModel, actually just calculate an array of inharmonicity coefficients whenever B changes - one for each partial index up to max number of partials. That way I don't have to calc a bunch of sqrt's every time a new note is hit - only when B is changed.

        /// try adding in fastsin() again (taylor series) to see if it runs faster now that I'm calculating more partials!

        /// PERFORMANCE: try simplifying the partial amplitude calculations - the 2500-8000Hz one has THIRTEEN MULTIPLIES/DIVIDES! That's ridiculous! Doesn't need to be that complex... could be WAY simpler. Could strip to half that many or LESS multiplies. Same with < 2500Hz, maybe the others.

        /// NEXT: implement pitch mod! http://www.martin-finke.de/blog/articles/audio-plugins-017-polyphony-ii/

        /// try solving differential equations instead of calculating sines - calculate TWO differential equations, one for each string! that should be complex enough, shouldn't it? especially if i run it through the dulcimer impulse response afterward? (wouldn't be surprised if that's how PianoTeq works)

        /// research TRANSIENTS... how to synthesize different types of transients that sound really good / rich / fuzzy / plucky / fingerpicking / pick-picking / etc.? (maybe get an audio sample of a picked mandolin string and inspect the waveform... also could record a dulcimer hammer hit (damped with no string sound, just transient attack) and inspect THAT waveform for clues... and do reassigned spectrogram on it for an even HIGHER resolution window into the sound!)

        /// extract an even LONGER dulcimer impulse response for even higher quality sound!

        /// NEXT: FIGURE OUT THE DAMN CLICK. whenever we trigger a new note, we need to WAIT before going to the attack stage, before resetting time counter, etc. - we need to DELAY this by ~1ms and fade out the existing sound, otherwise we'll get a major click. The only alternative to this would be by implementing polyphony, then I won't need this delay/fadeout. But seeing as this already kills my computer with one voice... try eliminating click first. (Or try faster sine calculations first, and see if I can get more like 500 partials going at once.)

        /// LATER:

            /// Implement a second oscillator, that's actually just the second string (maybe). That way, you can indepdently change mB, numpartials, resonant body impulse response, etc. for each voice. (I can do this later on. Follow tutorial here: http://www.martin-finke.de/blog/articles/audio-plugins-017-polyphony-ii/ )

            /// implement frequency masking, i.e. stop calculating inaudible partials, esp. important in polyphony http://www.csis.ul.ie/dafx01/proceedings/papers/lagrange.pdf

        // implement some more attractors from Evernote to see what they sound like (better noisy transients? harmonic material?)

        // do performance profiling in Xcode, with both "Analyze" and "Instruments" (and/or use "gprof", which should show me running time of different functions so I can really drill down and find the worst ones)

        // improve code using BOOST (read about "boost performance enhacements" or something)

        ///////////////////////////---------////////////////////////////

        case OSCILLATOR_MODE_SINE:

            if (mTime > mPartialsDelay) { // delay partials slightly so transient/attack/noise happens slightly earlier

                for (int j = 1; j < mNumPartials + 1; j++) { // start on first partial

                    mPartialFrequency = mPartialFrequencies[(j-1)*2]; // only approx, for calculating amplitude envelopes, only using the first string partial freq, not the second, for efficiency

                    // I should really calculate a running amplitude here for each partial... so that when partials get below the audible threshold, just stop rendering them (for performance)??? or just keep using the per-sample "break" method below

                    /// calculate partial amplitude

                    if (mPartialFrequency < 2500) {
                        //samples += sin(mPhase * (j + 1)) / ((j + 1) * 127.0 * mTime / mVelocity + 1); // increasing partials, where mPhase * j is proportional to partial frequency; amplitude scales down as 1/x with increasing partials; also, sound gets darker w/lower velocity (b/c higher partials get scaled down in amplitude faster the lower the velocity is)
                        // mTime increases as time goes on, and used here, it makes higher partials decay faster. use the equation "y = 1 / (x * (127 * incr / velocity) + 1)" in Grapher to test this out
                        mAmplitude = 4.0/(j+1)/(mTime+0.5/(j+1))*(1.0/(sqrt(j+1)))*(1.0/(j+1)) * (1.0-(1-mVelocity/127.0)*mPartialFrequency/2500);
                    } else if (mPartialFrequency < 8000) {
                        if (mAmplitude < 0) {
                            mAmplitude = 0;
                        } else {
                            mAmplitude = 2.0/(j+1)/(mTime+0.5/(j+1))*(1.0/((j+1))*(1.0/(j+1))) * (1.0-mTime*mPartialFrequency/5000) * (1.0-(1-mVelocity/127.0)*mPartialFrequency/8000);
                        }
                    } else if (mPartialFrequency < 20000) {
                        mAmplitude = 1/((pow((j+1), 3))*(mTime+0.0001)) - 0.00001;
                        if (mAmplitude < 0) {
                            mAmplitude = 0;
                        } else {
                            mAmplitude *= (1.0-mTime*mPartialFrequency/3000); // linear decay term
                        }
                    } else {
                        mAmplitude = 0;
                        break;
                    }

                    if (mAmplitude <= 0) {
                        break;
                    }

//                    if ( std::abs(mAmplitude) < epsilon ) {
//                        //log_debug("detected small num, %Le, %Le", x, y);
//                        std::cout << "\nDenormal detected";
//                    }

                    /// envelope follower:
                    // for first partial (Fundamental frequency), i.e. if j=1, just store the current mAmplitude in a member variable called mAmplitudeFollower or mCurrentEnvelope or mEnvelopeValue. Then, when voice stealing, find the voice with the lowest mEnvelopeValue and steal THAT one (since it'll be the quietest).

                    // adjust partial amplitude based on where the string was struck
                    /// edit: I should really just store an array of these, and calculate it once on every string hit, rather than once with every sample, right?!

                    //getLocationBasedAmplitude(j);
                    //mAmplitude *= mLocationBasedAmplitude;

                    // adjust partial "brightness" based on velocity and partial frequency (higher partials get muted at lower velocities, and no effect on any partials at high velocity)
                    /// performance boost: calculate an array of these brightness amplitudes only on each string hit (build them in to amplitude calculation that already happens in updateInstrumentModel() ) --> improve it, too, not doing much currently
                    //mAmplitude *= (1 - (1 - mVelocity/127.0) * mPartialFrequency/mMaxFreq);

                    /// calculate this partial for this sample (one for each of two strings)

                    //double twoPITimesTime = twoPI * mTime;

                    //values[0] = values[1] = 0;

                    values[0] = (sin(twoPI * (mTime - mPartialsDelay) * mPartialFrequencies[((j-1)*2)]) * mPartialAmplitudes[((j-1)*2)]) * mAmplitude;
                    values[1] = (sin(twoPI * (mTime - mPartialsDelay) * mPartialFrequencies[((j-1)*2+1)]) * mPartialAmplitudes[((j-1)*2+1)]) * mAmplitude;

                    /// non-reverb wash (just random panning)

                    // set right channel amplitude using left channel samples
                    //samples[0] += (values[0] * (1 - mPartialPanValues[((j-1)*2)]) + values[1] * (1-mPartialPanValues[((j-1)*2+1)])) * 0.1;
                    // set left channel amplitude
                    //samples[1] += (values[0] * mPartialPanValues[((j-1)*2)] + values[1] * mPartialPanValues[((j-1)*2+1)]) * 0.1;

                    /// reverb wash (sound starts at center, then widens L/R over time)

                    double panLeft1 = 1 - mPartialPanValues[((j-1)*2)];
                    double panLeft2 = 1 - mPartialPanValues[((j-1)*2+1)];
                    double panRight1 = mPartialPanValues[((j-1)*2)];
                    double panRight2 = mPartialPanValues[((j-1)*2+1)];
                    double panMult = 5;

                    samples[0] += (values[0] * (panLeft1 - (panLeft1 - 0.5)/(panMult * mTime + 1)) + values[1] * (panLeft2 - (panLeft2 - 0.5)/(panMult * mTime + 1))) * 0.03;
                    samples[1] += (values[0] * (panRight1 - (panRight1 - 0.5)/(panMult * mTime + 1)) + values[1] * (panRight2 - (panRight2 - 0.5)/(panMult * mTime + 1))) * 0.03;

                    if (fabs(samples[0]) >= 1 && fabs(values[0]) >= 1) {
                        std::cout << "\n CLIP -- value = " << values[0] << ", sample = " << samples[0];
                    }

//                    if ( std::abs(values[0]) < epsilon ) {
//                        //log_debug("detected small num, %Le, %Le", x, y);
//                        std::cout << "\nDenormal detected";
//                    }

                    //samples[1] += (values[0] * mPartialPanValues[((j-1)*2)] + values[1] * mPartialPanValues[((j-1)*2+1)]) * 0.1;

                    //std::cout << "pan value (string1) = " << mPartialPanValues[((j-1)*2)] << std::endl;
                    //std::cout << "pan value (string2) = " << mPartialPanValues[((j-1)*2+1)] << std::endl;

                }
            }

            //std::cout << "freq of string 1 = " << mPhase * mSampleRate / twoPI / (mTime/mTimeStep) << std::endl;

            // ADDING TRANSIENTS

            /// exponential decay transient (lorenz)

            /// LATER: make Rho into a knob, so you can change the transient flavor (range somewhere 24-44 ish, explore this)

            if (mCalculateNoisyTransient) {
                if (mTime < mTransientLength) { // only calculate for the initial transient attack, then stop doing attractor updates (may want to remove this behavior later, for doing long-lasting noise / radical sound design)

                    //                // Lorenz attractor:
                    //
                    //                mLXdot = mLSigma * (mLY - mLX);
                    //                mLYdot = mLX * (mLRho - mLZ) - mLY;
                    //                mLZdot = mLX * mLY - mLBeta * mLZ;
                    //
                    //                mLX += mLXdot * mTimeStep * 1005;
                    //                mLY += mLYdot * mTimeStep * 990;
                    //                mLZ += mLZdot * mTimeStep * 970;
                    //
                    //                noise = ((mLX * mLY) * 0.001 / (mTime * mAttack) - 0.05);
                    //                // used to be this: noise = ((mLX * mLY + mLY * mLZ + mLZ * mLX) * 0.01 * (mVelocity / 127.0) / (mTime * mAttack) - 0.05)) * 0.2;

                    // Tinkerbell map:

                    for (int l = 0; l < 2; l++) {
                        mTXnew = mTX * mTX - mTY * mTY + mTa * mTX + mTb * mTY;
                        mTYnew = 2 * mTY * mTX + mTc * mTX + mTd * mTY;

                        mTX = mTXnew;
                        mTY = mTYnew;
                    }

                    noise = (mTX) * 0.2 / (mTime * mAttack);

                    /// randomly pan the noisy transient
                    float randPan = static_cast<float>(rand()) / static_cast<float>(RAND_MAX);
                    //std::cout << "\nRandPan = " << randPan;

                    samples[0] += noise * (1.0-randPan);
                    samples[1] += noise * randPan;

                    //                samples[0] += noise;
                    //                samples[1] += noise;
                }
            }

            break;

        //////////////////////////---------/////////////////////////////

        case OSCILLATOR_MODE_SAW:

            samples[0] = samples[1] = 0;

            // original saw wave
            //value = 1.0 - (2.0 * mPhase / twoPI);
            break;
        case OSCILLATOR_MODE_SQUARE:
//            if (mPhase <= mPI) {
//                samples[0] = 1.0;
//            } else {
//                samples[0] = -1.0;
//            }
            samples[0] = samples[1] = 0;
            break;
        case OSCILLATOR_MODE_TRIANGLE:
//            samples[0] = -1.0 + (2.0 * mPhase / twoPI);
//            samples[0] = 2.0 * (fabs(samples[0]) - 0.5);
            samples[0] = samples[1] = 0;
            break;
        default:
            break;
    }
    //mPhase += mPhaseIncrement;
    // update time counter (seconds since last note trigger)
    mTime += mTimeStep;
    // update sample index (running index of which sample we're on since last note trigger)
    //mSampleIndex++;
//    while (mPhase >= twoPI) {
//        mPhase -= twoPI;
//    }
    //std::cout << "\nOscillator -> samples[0] = " << samples[0];

    return samples;
}


///////////////////////////////////////////////////////////////////////////////
// range::chain::joint_view.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_DETAIL_RANGE_EX_CHAIN_JOINT_VIEW_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_RANGE_EX_CHAIN_JOINT_VIEW_HPP_ER_2009
#include <vector>
#include <boost/mpl/void.hpp>
#include <boost/range.hpp>

#include <boost/fusion/include/zip_view.hpp>
#include <boost/fusion/include/transform.hpp>
#include <boost/fusion/include/make_vector.hpp>
#include <boost/fusion/include/transform_view.hpp>

#include <boost/statistics/detail/fusion/joint_view/bind_range.hpp>
#include <boost/statistics/detail/range_ex/chain/nest.hpp>

namespace boost {
namespace statistics{
namespace detail{
namespace range{

namespace result_of{

    // Seq1 : {e[j]:j=1,...,n}; e[j] is a fusion container
    // Seq2 : {r[j]:j=1,...,n}; r[j] is a range of fusion containers
    //
	// Returns a range formed by chaining {f(e[j],r[j]): j=1,...,n}
    // f(e[j],r[j]) = {joint_view(e[j],r[j][k]):k=1,...,m}
    template<typename Seq1,typename Seq2>
	struct chain_joint_view{

    	struct f{
        	f(){}
			template<typename T>
			struct remove_cref : boost::remove_cv<
            	typename boost::remove_reference<T>::type
            >{};

			template<typename Seq>
        	struct result_impl{

        		typedef typename boost::fusion::result_of::at_c<Seq,0>::type cref0_;
        		typedef typename boost::fusion::result_of::at_c<Seq,1>::type cref1_;
        		typedef typename remove_cref<cref0_>::type at0_;
        		typedef typename remove_cref<cref1_>::type at1_;
        		typedef detail::fusion::joint_view::result_of::bind_range<
                	at0_,at1_
                > meta_;
        		typedef typename meta_::type type;
        		static type call(const Seq& seq){
            		return meta_::call(
                		boost::fusion::at_c<0>(seq),
                		boost::fusion::at_c<1>(seq)
            		);
        		}
    		};

			template<typename S> struct result{};

        	template<typename F,typename Seq>
			struct result<F(const Seq&)> : result_impl<Seq>{};

			template<typename Seq>
			typename result_impl<Seq>::type
        	operator()(const Seq& seq)const{
        		typedef result_impl<Seq> meta_;
        		return meta_::call(seq);
        	}

		};
		typedef boost::fusion::result_of::make_vector<
        	const Seq1&,
            const Seq2&
        > seqs_;
        typedef boost::fusion::zip_view<seqs_> view_;

        typedef typename
            boost::fusion::result_of::transform<view_,f>::type res1_;

        typedef typename statistics::detail::range::result_of
            ::nest_chain<res1_>::type type;

        static type call(const Seq1& seq1,const Seq2& seq2){
            return statistics::detail::range::nest_chain(
                boost::fusion::transform(
                    boost::fusion::zip_view<seqs_>(
                        boost::fusion::make_vector(seq1,seq2)
                    ),
                    f()
                )
            );
        }
	};

}// result_of

	template<typename Seq1,typename Seq2>
    typename detail::range::result_of::chain_joint_view<Seq1,Seq2>::type
    chain_joint_view(const Seq1& seq_x,const Seq2& seq_ry){
		typedef detail::range::result_of::chain_joint_view<Seq1,Seq2> meta_;
        return meta_::call(seq_x,seq_ry);
    }

}// range
}// detail
}// statistics
}// boost

#endif


//          Copyright Carl Philipp Reh 2009 - 2018.
// 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)

#include <fcppt/boost_units_value.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/systems/si/length.hpp>
#include <catch2/catch.hpp>
#include <fcppt/config/external_end.hpp>

TEST_CASE(
	"boost_units_value",
	"[boost_units_value]"
)
{
	fcppt::boost_units_value const op{};

	CHECK(
		op(
			42
			*
			boost::units::si::meter
		)
		==
		42
	);
}


// Copyright 2014 The CRAVE developers. All rights reserved.

#pragma once

#include <boost/random.hpp>
#include <limits>
#include <vector>
#include "../ir/Node.hpp"
#include "WeightedRange.hpp"
#include "../RandomSeedManager.hpp"

namespace crave {

extern RandomSeedManager rng;

template <typename T>
struct distribution : Node {
  distribution() : ranges_() {}

  distribution& operator()(const weighted_range<T>& range) {
    addRange(range);
    return *this;
  }

  static distribution create(const weighted_range<T>& range) {
    distribution dist;
    dist(range);
    return dist;
  }

  static distribution simple_range(T left, T right) {
    distribution dist;
    dist(range<T>(left, right));
    return dist;
  }

  void reset() { ranges_.clear(); }

  std::vector<weighted_range<T> >& ranges() { return ranges_; }

  T nextValue() const {
    if (ranges_.empty()) {
      return boost::uniform_int<T>(std::numeric_limits<T>::min(), std::numeric_limits<T>::max())(*rng.get());
    }
    weighted_range<T> selected = ranges_.back();
    if (ranges_.size() > 1) {
      unsigned r = boost::uniform_int<unsigned>(0, selected.accumWeight_ - 1)(*rng.get());
      for (uint i = 0; i < ranges_.size(); i++)
        if (r < ranges_[i].accumWeight_) {
          selected = ranges_[i];
          break;
        }
    }
    return boost::uniform_int<T>(selected.left_, selected.right_)(*rng.get());
  }

 private:
  void addRange(weighted_range<T> wr) {
    for (uint i = 0; i < ranges_.size(); i++)
      if (ranges_[i].overlap(wr)) {
        throw std::runtime_error("Overlapping range exists.");
      }
    if (ranges_.empty()) {
      wr.accumWeight_ = wr.weight_;
    } else {
      wr.accumWeight_ = ranges_.back().accumWeight_ + wr.weight_;
    }
    ranges_.push_back(wr);
  }

 private:
  std::vector<weighted_range<T> > ranges_;
};

template <>
struct distribution<bool> : Node {
  explicit distribution(const double prob = 0.5);

  static distribution create(const double prob);

  bool nextValue() const;

  std::vector<weighted_range<bool> >& ranges();

 private:
  double prob_;
};

}  // namespace crave


/*
 * Copyright 2015 by Philip N. Garner
 *
 * See the file COPYING for the licence associated with this software.
 *
 * Author(s):
 *   Phil Garner, June 2015
 */

#include "lube/graph.h"
#include <fstream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graphviz.hpp>

namespace libube
{
    typedef boost::adjacency_list<
        boost::vecS,
        boost::vecS,
        boost::bidirectionalS,
        var,
        var
        >
    adjacency_list;
    typedef boost::graph_traits<adjacency_list>::vertex_descriptor vertex;
    typedef boost::graph_traits<adjacency_list>::edge_descriptor edge;

    /** Concrete implementation of graph module */
    class Graph : public graph
    {
    public:
        Graph(var iArg) {};
        void addEdge(ind iVertex1, ind iVertex2);
        ind addVertex();
        void writeGraphViz(var iFileName);
    private:
        adjacency_list mGraph;
    };

    /** Factory function to create a class */
    void factory(Module** oModule, var iArg)
    {
        *oModule = new Graph(iArg);
    }
};

using namespace libube;

void Graph::addEdge(ind iVertex1, ind iVertex2)
{
    // An edge is some non-trivial thing; it does ot cast to an integer type
    std::pair<edge, bool> ret;
    ret = add_edge(iVertex1, iVertex2, mGraph);
}

ind Graph::addVertex()
{
    // A vertex is just an index; at least, it can be cast to an ind.  So we
    // can return an ind
    vertex v = add_vertex(mGraph);
    return v;
}

class LabelWriter
{
public:
    LabelWriter(adjacency_list& iGraph) : mGraph(iGraph) {}
    template <class VertexOrEdge>
    void operator()(std::ostream& iOut, const VertexOrEdge& iV) const
    {
        var v = mGraph[iV];
        if (v.index("NAME"))
            // Should replace with escaped quote rather than nothing
            iOut << "[label=\""
                 << v.at("NAME").replace("\"", "").str()
                 << "\"]";
    }
private:
    adjacency_list& mGraph;
};

void Graph::writeGraphViz(var iFileName)
{
    std::ofstream ofs(iFileName.str(), std::ofstream::out);
    LabelWriter lw(mGraph);
    write_graphviz(ofs, mGraph, lw);
}


#include "domain/finite_element/finite_element_gaussian.h"

#include <vector>

#include <deal.II/grid/grid_generator.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/fe/fe_dgq.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>
#include <gtest/gtest.h>

#include "problem/parameter_types.h"
#include "finite_element_test.h"
#include "test_helpers/gmock_wrapper.h"

namespace {

using namespace bart;

using ::testing::ContainsRegex;

/* Tests for the FiniteElementGaussian class.
 *
 * As this class mostly just instantiates other classes and forwards to them
 * it's mostly interested in verifying dynamic casts to the classes we expect
 * don't return nullptr.
 */
template <typename DimensionWrapper>
class DomainFiniteElementGaussianTest : public ::testing::Test {
 protected:
  static constexpr int dim = DimensionWrapper::value;
  std::array<int, 4> dofs_per_cell_{1, 3, 9, 27};
  std::array<problem::DiscretizationType, 2> discretization_types {
    problem::DiscretizationType::kContinuousFEM,
    problem::DiscretizationType::kDiscontinuousFEM
  };
};

TYPED_TEST_CASE(DomainFiniteElementGaussianTest, bart::testing::AllDimensions);

// Verify that the continuous case instantiates all the correct objects
TYPED_TEST(DomainFiniteElementGaussianTest, ConstructorObjects) {
  /* Spatial dimension of the problem, this will be used to determine many of
   * the expected values for tests */
  constexpr int dim = this->dim;
  for (const auto discretization_type : this->discretization_types) {

    domain::finite_element::FiniteElementGaussian<dim> test_fe{discretization_type, 2};

    std::string expected_description{"(Default) deal.II Gaussian, " + std::to_string(dim) + "D, "};

    // Verify correct objects were instantiated
    ASSERT_NE(nullptr, dynamic_cast<dealii::FEValues<dim> *>(test_fe.values()));
    ASSERT_NE(nullptr,
              dynamic_cast<dealii::FEFaceValues<dim> *>(test_fe.face_values()));
    ASSERT_NE(nullptr,
              dynamic_cast<dealii::QGauss<dim> *>(test_fe.cell_quadrature()));
    ASSERT_NE(nullptr,
              dynamic_cast<dealii::QGauss<dim - 1> *>(test_fe.face_quadrature()));

    // Neighbor values are only for discontinuous so this should be a null object
    if (discretization_type == problem::DiscretizationType::kDiscontinuousFEM) {
      ASSERT_NE(nullptr,
                dynamic_cast<dealii::FE_DGQ<dim>*>(test_fe.finite_element()));
      ASSERT_NE(nullptr,
                dynamic_cast<dealii::FEFaceValues<dim> *>(
                    test_fe.neighbor_face_values()));
      expected_description += "Discontinuous, ";
    } else {
      ASSERT_NE(nullptr,
                dynamic_cast<dealii::FE_Q<dim> *>(test_fe.finite_element()));
      ASSERT_EQ(nullptr,
                dynamic_cast<dealii::FEFaceValues<dim> *>(
                    test_fe.neighbor_face_values()));
      expected_description += "Continuous, ";
    }
    expected_description += "Q = 2";
    EXPECT_EQ(expected_description, test_fe.description());
  }
}

// Verify that the instantiated objects were initialized correctly
TYPED_TEST(DomainFiniteElementGaussianTest, ConstructorValues) {
  /* Spatial dimension of the problem, this will be used to determine many of
   * the expected values for tests */
  constexpr int dim = this->dim;
  domain::finite_element::FiniteElementGaussian<dim> test_fe{
      problem::DiscretizationType::kContinuousFEM, 2};

  // Verify correct values returned
  ASSERT_EQ(test_fe.polynomial_degree(), 2);
  ASSERT_EQ(test_fe.dofs_per_cell(), this->dofs_per_cell_.at(dim));
  ASSERT_EQ(test_fe.n_cell_quad_pts(), this->dofs_per_cell_.at(dim));
  ASSERT_EQ(test_fe.n_face_quad_pts(), this->dofs_per_cell_.at(dim - 1));
}

TYPED_TEST(DomainFiniteElementGaussianTest, ConstructorNone) {
  constexpr int dim = this->dim;
  ASSERT_ANY_THROW({
    domain::finite_element::FiniteElementGaussian<dim> test_fe(problem::DiscretizationType::kNone, 2);
                   });
}

TYPED_TEST(DomainFiniteElementGaussianTest, ValueTest) {
  /* To use the ShapeValue, ShapeGraident, and Jacobian functions, the various
   * value objects inside our FiniteElement object need to be associated with
   * a DOF handler, which it does not own. So we will instantiate one to
   * associate it with and check that the functions are forwarding the right
   * values */
  constexpr int dim = this->dim;
  bart::domain::finite_element::FiniteElementGaussian<dim> test_fe{
      problem::DiscretizationType::kContinuousFEM, 2};

  // Triangulation and DOF handler to link to our values
  dealii::Triangulation<dim> triangulation;
  dealii::GridGenerator::hyper_cube(triangulation, -1, 1);
  triangulation.refine_global(2);

  dealii::DoFHandler dof_handler(triangulation);
  dof_handler.distribute_dofs(*test_fe.finite_element());

  test_fe.SetCell(dof_handler.begin_active());

  int cell_dofs = test_fe.dofs_per_cell();
  int cell_quad_points = test_fe.n_cell_quad_pts();

  for (int i = 0; i < cell_dofs; ++i) {
    for (int q = 0; q < cell_quad_points; ++q) {
      EXPECT_DOUBLE_EQ(test_fe.values()->shape_value(i, q),
                       test_fe.ShapeValue(i, q));
      EXPECT_TRUE(test_fe.values()->shape_grad(i, q)
                      == test_fe.ShapeGradient(i, q));
      EXPECT_DOUBLE_EQ(test_fe.values()->JxW(q),
                       test_fe.Jacobian(q));
    }
  }

  test_fe.SetFace(dof_handler.begin_active(), domain::FaceIndex(0));

  int face_quad_points = test_fe.n_face_quad_pts();
  for (int i = 0; i < cell_dofs; ++i) {
    for (int q = 0; q < face_quad_points; ++q) {
      EXPECT_DOUBLE_EQ(test_fe.face_values()->shape_value(i, q),
                       test_fe.FaceShapeValue(i, q));
      EXPECT_DOUBLE_EQ(test_fe.face_values()->JxW(q),
                       test_fe.FaceJacobian(q));
    }
  }

}

TYPED_TEST(DomainFiniteElementGaussianTest, FaceNormalTest) {
  constexpr int dim = this->dim;
  bart::domain::finite_element::FiniteElementGaussian<dim> test_fe{
      problem::DiscretizationType::kContinuousFEM, 2};

  // Triangulation and DOF handler to link to our values
  dealii::Triangulation<dim> triangulation;
  dealii::GridGenerator::hyper_cube(triangulation, -1, 1);
  triangulation.refine_global(2);

  dealii::DoFHandler dof_handler(triangulation);
  dof_handler.distribute_dofs(*test_fe.finite_element());

  test_fe.SetFace(dof_handler.begin_active(), domain::FaceIndex(0));

  EXPECT_EQ(test_fe.face_values()->normal_vector(0),
            test_fe.FaceNormal());
}

// BASE CLASS TESTS ============================================================
template <typename DimensionWrapper>
class DomainFiniteElementGaussianBaseMethodsTest :
    public domain::finite_element::testing::FiniteElementBaseClassTest<DimensionWrapper::value> {
 protected:
  static constexpr int dim = DimensionWrapper::value;
  void SetUp() override {
    domain::finite_element::testing::FiniteElementBaseClassTest<dim>::SetUp();
  }
};

TYPED_TEST_CASE(DomainFiniteElementGaussianBaseMethodsTest,
                bart::testing::AllDimensions);

TYPED_TEST(DomainFiniteElementGaussianBaseMethodsTest, BaseSetCell) {
  bart::domain::finite_element::FiniteElementGaussian<this->dim> test_fe{
    problem::DiscretizationType::kDiscontinuousFEM, 2};
  this->TestSetCell(&test_fe);
}

TYPED_TEST(DomainFiniteElementGaussianBaseMethodsTest, BaseSetCellAndFace) {
  bart::domain::finite_element::FiniteElementGaussian<this->dim> test_fe{
      problem::DiscretizationType::kDiscontinuousFEM, 2};
  this->TestSetCellAndFace(&test_fe);
}

TYPED_TEST(DomainFiniteElementGaussianBaseMethodsTest, BaseValueAtQuadrature) {
  bart::domain::finite_element::FiniteElementGaussian<this->dim> test_fe{
      problem::DiscretizationType::kDiscontinuousFEM, 2};
  this->TestValueAtQuadrature(&test_fe);
}

} // namespace


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<double, 4, Eigen::Dynamic> test_double_4_D(const Eigen::Matrix<double, 4, Eigen::Dynamic> & M)
{
	return M;
}
void export_double_4_D()
{
	boost::python::def("test_double_4_D",test_double_4_D);
}


#include <iostream>
#include <time.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/ia_ransac.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/point_types.h>
#include <pcl/features/pfh.h>
#include <pcl/filters/passthrough.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/point_cloud.h>
#include <pcl/point_representation.h>
#include <limits>
#include <fstream>
#include <vector>
#include <Eigen/Core>

using namespace pcl;
using namespace std;
using pcl::visualization::PointCloudColorHandlerGenericField;
using pcl::visualization::PointCloudColorHandlerCustom;

namespace pc{

void view(pcl::PointCloud<pcl::PointXYZ> &cloud)
{
	pcl::visualization::CloudViewer viewer1("Cloud Viewer");
	viewer1.showCloud(cloud.makeShared());
	while (!viewer1.wasStopped());
	return;
}

void viewPair(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2,
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1al, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2al){

	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
	viewer->initCameraParameters();

	int v1(0), v2(0);
	viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
	viewer->setBackgroundColor(0, 0, 0, v1);
	viewer->addText("Before Alignment", 10, 10, "v1 text", v1);
	PointCloudColorHandlerCustom<PointXYZ> green(cloud1, 0, 255, 0);
	PointCloudColorHandlerCustom<PointXYZ> red(cloud2, 255, 0, 0);
	viewer->addPointCloud(cloud1, green, "v1_target", v1);
	viewer->addPointCloud(cloud2, red, "v1_sourse", v1);

	viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);
	viewer->setBackgroundColor(0, 0, 0, v2);
	viewer->addText("After Alignment", 10, 10, "v2 text", v2);
	PointCloudColorHandlerCustom<PointXYZ> green2(cloud1al, 0, 255, 0);
	PointCloudColorHandlerCustom<PointXYZ> red2(cloud2al, 255, 0, 0);
	viewer->addPointCloud(cloud1al, green2, "v2_target", v2);
	viewer->addPointCloud(cloud2al, red2, "v2_sourse", v2);
	viewer->spin();

	return;
}

PointCloud<pcl::PointXYZRGB>::Ptr coloredMerge(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2){

	PointCloud<pcl::PointXYZRGB>::Ptr cloud1color(new pcl::PointCloud<pcl::PointXYZRGB>);
	PointCloud<pcl::PointXYZRGB>::Ptr cloud2color(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::copyPointCloud(*cloud1, *cloud1color);
	pcl::copyPointCloud(*cloud2, *cloud2color);
	for (size_t i = 0; i < cloud1color->points.size(); ++i){
		cloud1color->points[i].r = 255;
		cloud1color->points[i].g = 0;
		cloud1color->points[i].b = 0;
	}
	for (size_t i = 0; i < cloud2color->points.size(); ++i){
		cloud2color->points[i].r = 0;
		cloud2color->points[i].g = 0;
		cloud2color->points[i].b = 255;
	}
	*cloud1color += *cloud2color;
	return cloud1color;
}

}


#ifndef _maps_Types_hpp_
#define _maps_Types_hpp_

#include <memory>
#include <pcl/pcl_base.h>
#include <pcl/point_types.h>
#include <Eigen/Geometry>

namespace maps {
  typedef pcl::PointXYZRGB PointType;
  typedef pcl::PointCloud<PointType> PointCloud;

  struct PointSet {
    int64_t mTimestamp;
    PointCloud::Ptr mCloud;
  };

  struct TriangleMesh {
    std::vector<Eigen::Vector3f> mVertices;
    std::vector<Eigen::Vector3i> mFaces;
    std::vector<Eigen::Vector3f> mNormals;
    typedef std::shared_ptr<TriangleMesh> Ptr;
  };
}

#endif


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#ifndef BOOST_COMPUTE_FUNCTIONAL_RELATIONAL_HPP
#define BOOST_COMPUTE_FUNCTIONAL_RELATIONAL_HPP

#include <boost/compute/functional/detail/macros.hpp>

namespace boost {
namespace compute {

BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION_UNDERSCORE(isequal, int (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION_UNDERSCORE(isnotequal, int (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION_UNDERSCORE(isgreater, int (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION_UNDERSCORE(isgreaterequal, int (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION_UNDERSCORE(isless, int (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION_UNDERSCORE(islessequal, int (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION_UNDERSCORE(islessgreater, int (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION_UNDERSCORE(isfinite, int (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION_UNDERSCORE(isinf, int (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION_UNDERSCORE(isnan, int (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION_UNDERSCORE(isnormal, int (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION_UNDERSCORE(isordered, int (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION_UNDERSCORE(isunordered, int (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION_UNDERSCORE(signbit, int (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(any, int (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(all, int (T), class T)

} // end compute namespace
} // end boost namespace

#endif // BOOST_COMPUTE_FUNCTIONAL_RELATIONAL_HPP


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN partition tests

#include <boost/test/unit_test.hpp>

#include <algorithm>
#include <deque>
#include <iterator>
#include <forward_list>
#include <vector>

BOOST_AUTO_TEST_SUITE( partition_test_suite )

BOOST_AUTO_TEST_CASE( partition_integers_by_less )
{
    std::forward_list<int> numbers{15, -14, -5, -15, 1, 20, -2, 2, -7, -9, -11, 19, -16, 4, 3, -10, -1, 16, 12, 14};

    auto comp = [](int n){ return n < 0; };
    auto it = std::partition(std::begin(numbers), std::end(numbers), comp);

    /*! There are 10 numbers that satisfy the predicate, so that's the expected
     * distance from begin to the returned iterator. */
    BOOST_CHECK_EQUAL(std::distance(std::begin(numbers), it), 10U);
    BOOST_CHECK(std::for_each(std::begin(numbers), it, [](int n){ return n < 0; }));
    BOOST_CHECK(std::for_each(it, std::end(numbers), [](int n){ return n >= 0; }));
}

BOOST_AUTO_TEST_CASE( partition_copy_integers_by_less )
{
    std::forward_list<int> numbers{15, -14, -5, -15, 1, 20, -2, 2, -7, -9, -11, 19, -16, 4, 3, -10, -1, 16, 12, 14};

    std::vector<int> negative;
    std::deque<int> nonnegative;
    auto comp = [](int n){ return n < 0; };
    std::partition_copy(std::begin(numbers), std::end(numbers), std::back_inserter(negative), std::back_inserter(nonnegative), comp);

    BOOST_CHECK_EQUAL(negative.size(), 10);
    BOOST_CHECK_EQUAL(nonnegative.size(), 10);
    BOOST_CHECK(std::for_each(std::begin(negative), std::end(negative), [](int n){ return n < 0; }));
    BOOST_CHECK(std::for_each(std::begin(nonnegative), std::end(nonnegative), [](int n){ return n >= 0; }));
}

BOOST_AUTO_TEST_CASE( stable_partition_integers_by_less )
{
    std::forward_list<int> numbers{15, -14, -5, -15, 1, 20, -2, 2, -7, -9, -11, 19, -16, 4, 3, -10, -1, 16, 12, 14};

    auto comp = [](int n){ return n < 0; };
    auto it = std::stable_partition(std::begin(numbers), std::end(numbers), comp);

    BOOST_CHECK_EQUAL(std::distance(std::begin(numbers), it), 10U);

    const std::forward_list<int> expected{-14, -5, -15, -2, -7, -9, -11, -16, -10, -1, 15, 1, 20, 2, 19, 4, 3, 16, 12, 14};
    BOOST_CHECK_EQUAL_COLLECTIONS(std::begin(numbers), std::end(numbers), std::begin(expected), std::end(expected));
}

BOOST_AUTO_TEST_CASE( partition_integers_by_equality )
{
    std::forward_list<int> numbers{1, 2, 1, 2, 1, 2, 1, 2, 1, 2, 1, 2};

    auto comp = [](int n){ return n == 2; };
    auto it = std::partition(std::begin(numbers), std::end(numbers), comp);

    BOOST_CHECK_EQUAL(std::distance(std::begin(numbers), it), 6U);

    const std::forward_list<int> expected{2, 2, 2, 2, 2, 2, 1, 1, 1, 1, 1, 1};
    BOOST_CHECK_EQUAL_COLLECTIONS(std::begin(numbers), std::end(numbers), std::begin(expected), std::end(expected));
}

BOOST_AUTO_TEST_CASE( partition_copy_integers_by_equality )
{
    std::forward_list<int> numbers{1, 2, 1, 2, 1, 2, 1, 2, 1, 2, 1, 2, 1};

    std::vector<int> one;
    std::deque<int> two;
    auto comp = [](int n){ return n == 1; };
    std::partition_copy(std::begin(numbers), std::end(numbers), std::back_inserter(one), std::back_inserter(two), comp);

    BOOST_CHECK_EQUAL(one.size(), 7);
    BOOST_CHECK_EQUAL(two.size(), 6);

    const std::deque<int> expected_one{1, 1, 1, 1, 1, 1, 1};
    const std::vector<int> expected_two{2, 2, 2, 2, 2, 2};
    BOOST_CHECK_EQUAL_COLLECTIONS(std::begin(one), std::end(one), std::begin(expected_one), std::end(expected_one));
    BOOST_CHECK_EQUAL_COLLECTIONS(std::begin(two), std::end(two), std::begin(expected_two), std::end(expected_two));
}

BOOST_AUTO_TEST_SUITE_END( /* partition_test_suite */ )


#ifndef STAN_MATH_PRIM_PROB_NEG_BINOMIAL_2_RNG_HPP
#define STAN_MATH_PRIM_PROB_NEG_BINOMIAL_2_RNG_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/err.hpp>
#include <stan/math/prim/fun/constants.hpp>
#include <stan/math/prim/fun/max_size.hpp>
#include <boost/random/gamma_distribution.hpp>
#include <boost/random/poisson_distribution.hpp>
#include <boost/random/variate_generator.hpp>

namespace stan {
namespace math {

/** \ingroup prob_dists
 * Return a negative binomial random variate with the specified location and
 * precision parameters using the given random number generator.
 *
 * mu and phi can each be a scalar or a one-dimensional container. Any
 * non-scalar inputs must be the same size.
 *
 * @tparam T_loc type of location parameter
 * @tparam T_prec type of precision parameter
 * @tparam RNG type of random number generator
 * @param mu (Sequence of) positive location parameter(s)
 * @param phi (Sequence of) positive precision parameter(s)
 * @param rng random number generator
 * @return (Sequence of) negative binomial random variate(s)
 * @throw std::domain_error if mu or phi are nonpositive
 * @throw std::invalid_argument if non-scalar arguments are of different
 * sizes
 */
template <typename T_loc, typename T_prec, class RNG>
inline typename VectorBuilder<true, int, T_loc, T_prec>::type
neg_binomial_2_rng(const T_loc& mu, const T_prec& phi, RNG& rng) {
  using boost::gamma_distribution;
  using boost::variate_generator;
  using boost::random::poisson_distribution;
  static const char* function = "neg_binomial_2_rng";
  check_positive_finite(function, "Location parameter", mu);
  check_positive_finite(function, "Precision parameter", phi);
  check_consistent_sizes(function, "Location parameter", mu,
                         "Precision parameter", phi);

  scalar_seq_view<T_loc> mu_vec(mu);
  scalar_seq_view<T_prec> phi_vec(phi);
  size_t N = max_size(mu, phi);
  VectorBuilder<true, int, T_loc, T_prec> output(N);

  for (size_t n = 0; n < N; ++n) {
    double mu_div_phi = static_cast<double>(mu_vec[n]) / phi_vec[n];

    // gamma_rng params must be positive and finite
    check_positive_finite(function,
                          "Location parameter divided by the "
                          "precision parameter",
                          mu_div_phi);

    double rng_from_gamma = variate_generator<RNG&, gamma_distribution<> >(
        rng, gamma_distribution<>(phi_vec[n], mu_div_phi))();

    // same as the constraints for poisson_rng
    check_less(function, "Random number that came from gamma distribution",
               rng_from_gamma, POISSON_MAX_RATE);
    check_not_nan(function, "Random number that came from gamma distribution",
                  rng_from_gamma);
    check_nonnegative(function,
                      "Random number that came from gamma distribution",
                      rng_from_gamma);

    output[n] = variate_generator<RNG&, poisson_distribution<> >(
        rng, poisson_distribution<>(rng_from_gamma))();
  }

  return output.data();
}

}  // namespace math
}  // namespace stan
#endif


/*
Lightweight Automated Planning Toolkit
Copyright (C) 2012
Miquel Ramirez <miquel.ramirez@rmit.edu.au>
Nir Lipovetzky <nirlipo@gmail.com>

This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

// MRJ: In this example, we'll show how to create a search problem out of
// a planning problem we acquired from some external source
#include <iostream>
#include <fstream>

#include <ff_to_aptk.hxx>
#include <strips_prob.hxx>
#include <fluent.hxx>
#include <action.hxx>
#include <cond_eff.hxx>
#include <strips_state.hxx>
#include <fwd_search_prob.hxx>

#include <h_1.hxx>
#include <layered_h_max.hxx>

#include <fstream>

#include <boost/program_options.hpp>
#include <aptk/resources_control.hxx>

namespace po = boost::program_options;

using	aptk::STRIPS_Problem;

using	aptk::agnostic::Fwd_Search_Problem;
using	aptk::Action;

using 	aptk::agnostic::H1_Heuristic;
using	aptk::agnostic::H_Max_Evaluation_Function;
using	aptk::agnostic::Layered_H_Max;

// MRJ: Now we define the heuristics
typedef 	H1_Heuristic<Fwd_Search_Problem, H_Max_Evaluation_Function>	H_Max_Fwd;
typedef 	Layered_H_Max< Fwd_Search_Problem >				Alt_H_Max;

void process_command_line_options( int ac, char** av, po::variables_map& vars ) {
	po::options_description desc( "Options:" );

	desc.add_options()
		( "help", "Show help message" )
		( "domain", po::value<std::string>(), "Input PDDL domain description" )
		( "problem", po::value<std::string>(), "Input PDDL problem description" )
	;

	try {
		po::store( po::parse_command_line( ac, av, desc ), vars );
		po::notify( vars );
	}
	catch ( std::exception& e ) {
		std::cerr << "Error: " << e.what() << std::endl;
		std::exit(1);
	}
	catch ( ... ) {
		std::cerr << "Exception of unknown type!" << std::endl;
		std::exit(1);
	}

	if ( vars.count("help") ) {
		std::cout << desc << std::endl;
		std::exit(0);
	}

}

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

	po::variables_map vm;

	process_command_line_options( argc, argv, vm );

	if ( !vm.count( "domain" ) ) {
		std::cerr << "No PDDL domain was specified!" << std::endl;
		std::exit(1);
	}

	if ( !vm.count( "problem" ) ) {
		std::cerr << "No PDDL problem was specified!" << std::endl;
		std::exit(1);
	}

	STRIPS_Problem	prob;

	aptk::FF_Parser::get_problem_description( vm["domain"].as<std::string>(), vm["problem"].as<std::string>(), prob );
	std::cout << "PDDL problem description loaded: " << std::endl;

	prob.make_effect_tables();

	std::cout << "\tDomain: " << prob.domain_name() << std::endl;
	std::cout << "\tProblem: " << prob.problem_name() << std::endl;
	std::cout << "\t#Actions: " << prob.num_actions() << std::endl;
	std::cout << "\t#Fluents: " << prob.num_fluents() << std::endl;

	Fwd_Search_Problem	search_prob( &prob );

	H_Max_Fwd	dijkstra_h1( search_prob );
	Alt_H_Max	layered_h1( search_prob );
	float		dijkstra_h_val;
	unsigned	layered_h_val;
	float t0, tf, dijkstra_t, layered_t;

	t0 = aptk::time_used();
	dijkstra_h1.eval( *(search_prob.init()), dijkstra_h_val );
	tf = aptk::time_used();
	dijkstra_t = tf - t0;

	t0 = aptk::time_used();
	layered_h1.eval( *(search_prob.init()), layered_h_val );
	tf = aptk::time_used();
	layered_t = tf - t0;

	std::cout << "Dijkstra h_{max} = " << dijkstra_h_val << " Layered h_{max} = " << layered_h_val << std::endl;
	std::cout << "Dijkstra t = " << dijkstra_t << " secs Layered t = " << layered_t << " secs" << std::endl;

	std::ofstream	dijkstra_out( "dijkstra.values" );
	dijkstra_h1.print_values( dijkstra_out );
	dijkstra_out.close();

	std::ofstream layered_out( "layered.values" );
	layered_h1.print_values( layered_out );
	layered_out.close();

	return 0;
}


/*
 * Copyright (c) 2015-2016, Luca Fulchir<luca@fulchir.it>, All rights reserved.
 *
 * This file is part of "libRaptorQ".
 *
 * libRaptorQ is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as
 * published by the Free Software Foundation, either version 3
 * of the License, or (at your option) any later version.
 *
 * libRaptorQ is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * and a copy of the GNU Lesser General Public License
 * along with libRaptorQ.  If not, see <http://www.gnu.org/licenses/>.
 */

#pragma once

#include "RaptorQ/v1/common.hpp"
#include "RaptorQ/v1/degree.hpp"
#include "RaptorQ/v1/Rand.hpp"
#include "RaptorQ/v1/table2.hpp"
#include <cmath>
#include <Eigen/Core>
#include <vector>

namespace RaptorQ__v1 {
namespace Impl {

class RAPTORQ_LOCAL Tuple
{
    // d  1-30  LT-degree of encoded symbol
    // a  0-(W-1)
    // b  0-(W-1)
    // d1 PI-degree of encoded symbol (2 or 3)
    // a1 0-(P1-1)
    // b1 0-(P1-1)
public:
    Tuple() = default;
    Tuple (const Tuple&) = default;
    Tuple& operator= (const Tuple&) = default;
    Tuple (Tuple&&) = default;
    Tuple& operator= (Tuple&&) = default;
    ~Tuple() = default;

    uint16_t d, a, b, d1, a1, b1;   // great names. thanks rfc6330!
};

class RAPTORQ_API Parameters
{
public:
    explicit Parameters (const uint16_t symbols);
    Parameters() = delete;
    Parameters (const Parameters&) = default;
    Parameters& operator= (const Parameters&) = default;
    Parameters (Parameters&&) = default;
    Parameters& operator= (Parameters&&) = default;
    ~Parameters() {}

    uint16_t Deg (const uint32_t v) const;
    Tuple tuple (const uint32_t ISI) const;
    std::vector<uint16_t> get_idxs (const uint32_t ISI) const;

    uint16_t K_padded, S, H, W, L, P, P1, U, B; // RFC 6330, pg 22
    uint16_t J;
private:
    static bool is_prime (const uint16_t n);
};

inline Parameters::Parameters (const uint16_t symbols)
{
    uint16_t idx;
    for (idx = 0; idx < RaptorQ__v1::Impl::K_padded.size(); ++idx) {
        if (RaptorQ__v1::Impl::K_padded[idx] >= symbols) {
            K_padded = RaptorQ__v1::Impl::K_padded[idx];
            break;
        }
    }

    J = RaptorQ__v1::Impl::J_K_padded[idx];
    std::tie (S, H, W) = RaptorQ__v1::Impl::S_H_W [idx];

    L = K_padded + S + H;
    P = L - W;
    U = P - H;
    B = W - S;
    P1 = P + 1;         // first prime number bigger than P. turns out its
                        // always between 1 and 14 more numbers.
    while (!is_prime (P1))  // so this while will be really quick anyway
        ++P1;
}

inline bool Parameters::is_prime (const uint16_t n)
{
    // 1 as prime, don't care. Not in our scope anyway.
    // thank you stackexchange for the code
    if (n <= 3)
        return true;
    if (n % 2 == 0 || n % 3 == 0)
        return false;

    uint32_t i = 5;
    uint32_t w = 2;
    while (i * i <= n) {
        if (n % i == 0)
            return false;
        i += w;
        w = 6 - w;
    }
    return true;
}

inline uint16_t Parameters::Deg (const uint32_t v) const
{
    // rfc 6330, pg 27

    for (uint16_t d = 0; d < RaptorQ__v1::Impl::degree_distribution.size();++d){
        if (v < RaptorQ__v1::Impl::degree_distribution[d])
            return (d < (W - 2)) ? d : (W - 2);
    }
    return 0;   // never get here, but don't make the compiler complain
}

inline Tuple RaptorQ__v1::Impl::Parameters::tuple (const uint32_t ISI) const
{
    RaptorQ__v1::Impl::Tuple ret;

    // taken straight from RFC6330, pg 30
    // so thank them for the *beautiful* names
    // also, don't get confused with "B": this one is different,
    // and thus named "B1"

    size_t A = 53591 + J * 997;

    if (A % 2 == 0)
        ++A;
    size_t B1 = 10267 * (J + 1);
    uint32_t y = static_cast<uint32_t> (B1 + ISI * A);
    uint32_t v = rnd_get (y, 0, static_cast<uint32_t> (std::pow(2, 20)));
    ret.d = Deg (v);
    ret.a = 1 + static_cast<uint16_t> (rnd_get (y, 1, W - 1));
    ret.b = static_cast<uint16_t> (rnd_get (y, 2, W));
    if (ret.d < 4) {
        ret.d1 = 2 + static_cast<uint16_t> (rnd_get (ISI, 3, 2));
    } else {
        ret.d1 = 2;
    }
    ret.a1 = 1 + static_cast<uint16_t> (rnd_get (ISI, 4, P1 - 1));
    ret.b1 = static_cast<uint16_t> (rnd_get (ISI, 5, P1));

    return ret;
}

inline std::vector<uint16_t> Parameters::get_idxs (const uint32_t ISI) const
{
    // Needed to generate G_ENC: We need the ids of the symbols we would
    // use on a "Enc" call. So this is the "enc algorithm, but returns the
    // indexes instead of computing the result.
    // rfc6330, pg29

    std::vector<uint16_t> ret;
    Tuple t = tuple (ISI);

    ret.reserve (t.d + t.d1);
    ret.push_back (t.b);

    for (uint16_t j = 1; j < t.d; ++j) {
        t.b = (t.b + t.a) % W;
        ret.push_back (t.b);
    }
    while (t.b1 >= P)
        t.b1 = (t.b1 + t.a1) % P1;

    ret.push_back (W + t.b1);
    for (uint16_t j = 1; j < t.d1; ++j) {
        t.b1 = (t.b1 + t.a1) % P1;
        while (t.b1 >= P)
            t.b1 = (t.b1 + t.a1) % P1;
        ret.push_back (W + t.b1);
    }
    return ret;
}

}   // namespace Impl
}   // namespace RaptorQ