/* Distributed Mulit-Message Threshold (DMMT) */

#include "dmmt.h"

#include <gcrypt.h>
#include <NTL/GF2X.h>
#include <NTL/GF2EX.h>
#include <NTL/mat_GF2E.h>

using namespace NTL;

enum {
    TAG_DERIV_PARAM = 0x0ADA1815
};

struct dmmt {
    size_t key_size;
    size_t block_size;
    int algo;
};

struct dmmt_dom {
    dmmt_t *d;
    uint8_t *tag;
    unsigned int threshold;
    gcry_cipher_hd_t ctr_handle;
    GF2EX polynomial;
};

dmmt_stat_t enc_ecb_blk(dmmt_t *d, uint8_t *in, uint8_t *out);
void fill_block_with_int(dmmt_t *d, uint64_t v, uint8_t *out);
void xor_block_with_int(dmmt_t *d, uint64_t v, uint8_t *out);
void fill_GF2X_from_bytes(GF2X &f, const uint8_t *bytes, size_t num_bytes);
int conv_GF2X_to_bytes(const GF2X &f, long max_len, uint8_t *out);

/* ############################################################################
 * # DMMT system
 * ############################################################################
 */

dmmt_t *dmmt_create(const char *block_cipher, size_t block_size,
                    size_t key_len_bits, dmmt_stat_t *status)
{

    int algo = GCRY_CIPHER_NONE;
    dmmt_t *d = NULL;

    if (!strcmp(block_cipher, "AES")) {
        if (block_size == 16 && key_len_bits == 128)
            algo = GCRY_CIPHER_AES128;
        else
            *status = DMMT_STAT_UNSUPPORTED_CIPHER_PARAMS;
    }
    else
        *status = DMMT_STAT_UNSUPPORTED_CIPHER;

    if (algo != GCRY_CIPHER_NONE) {
        d = (dmmt_t*)malloc(sizeof(dmmt_t));

        d->algo = algo;
        d->key_size = (key_len_bits + 7) / 8;
        d->block_size = block_size;

        /*
         * Modulus for GF(2^128) is set to x^128 + x^7 + x^2 + x + 1.
         * At the moment, only AES-128 is supported so this suffices .
         * However it is global. TODO: change this.
         */
        GF2X modulus;
        SetCoeff(modulus, 128);
        SetCoeff(modulus, 7);
        SetCoeff(modulus, 2);
        SetCoeff(modulus, 1);
        SetCoeff(modulus, 0);
        GF2E::init(modulus);

        *status = DMMT_STAT_OK;
    }
    else
        *status = DMMT_STAT_INTERNAL_ERROR;

    return d;
}

dmmt_dom_t *dmmt_new_dom_from_key(dmmt_t *d, const uint8_t *master_key,
                                  unsigned int threshold, dmmt_stat_t *stat)
{
    gcry_cipher_hd_t h_ecb;

    if (gcry_cipher_open(&h_ecb, d->algo, GCRY_CIPHER_MODE_ECB,
                         GCRY_CIPHER_SECURE) != 0) {
        *stat = DMMT_STAT_INTERNAL_ERROR;
        return NULL;
    }

    /* Temporary buffers and variables */
    uint8_t *ibuf = (uint8_t*)malloc(d->block_size);
    uint8_t *obuf = (uint8_t*)gcry_malloc_secure(d->block_size);
    GF2E coeff;
    GF2X coeff_poly;

    /* New Domain */
    dmmt_dom_t *dom = (dmmt_dom_t*)gcry_malloc_secure(sizeof(dmmt_dom_t));
    dom->d = d;
    dom->threshold = threshold;

    /* Generate Tag */
    uint8_t *tag = (uint8_t*)malloc(d->block_size);
    gcry_cipher_setkey(h_ecb, master_key, d->key_size);
    fill_block_with_int(d, TAG_DERIV_PARAM, ibuf);
    gcry_cipher_encrypt(h_ecb, tag, d->block_size, ibuf, d->block_size);
    dom->tag = tag;

    /* Copy tag to input buf */
    memcpy(ibuf, tag, d->block_size);

    /* Generate secret key */
    gcry_cipher_encrypt(h_ecb, obuf, d->block_size, ibuf, d->block_size);

    /* Setup cipher handle with secret key */
    gcry_cipher_open(&dom->ctr_handle, d->algo, GCRY_CIPHER_MODE_CTR,
                     GCRY_CIPHER_SECURE);
    gcry_cipher_setkey(dom->ctr_handle, obuf, d->key_size);

    /* Set secret as constant term of polynomial */
    fill_GF2X_from_bytes(coeff_poly, obuf, d->block_size);
    conv(coeff, coeff_poly);
    SetCoeff(dom->polynomial, 0, coeff);

    /* Derive the coefficents using the block cipher */
    uint64_t i;
    for (i = 1; i < threshold; i++) {
        /* XOR in next counter value */
        xor_block_with_int(d, i ^ (i - 1), ibuf);

        gcry_cipher_encrypt(h_ecb, obuf, d->block_size, ibuf, d->block_size);
        fill_GF2X_from_bytes(coeff_poly, obuf, d->block_size);
        conv(coeff, coeff_poly);
        SetCoeff(dom->polynomial, i, coeff);
    }

    /* Cleanup */
    gcry_cipher_close(h_ecb);
    free(ibuf);
    gcry_free(obuf);

    *stat = DMMT_STAT_OK;

    return dom;
}

dmmt_dom_t *dmmt_new_dom_from_shares(dmmt_t *d, const uint8_t *tag,
                                     unsigned int threshold,
                                     const uint8_t * const *shares,
                                     size_t n_shares, dmmt_stat_t *stat)
{
    if (n_shares < threshold) {
        *stat = DMMT_STAT_BELOW_THRESHOLD;
        return NULL;
    }

    /* New Domain */
    dmmt_dom_t *dom = (dmmt_dom_t*)gcry_malloc_secure(sizeof(dmmt_dom_t));
    dom->d = d;
    dom->threshold = threshold;

    /* Copy Tag */
    uint8_t *tag_copy = (uint8_t*)malloc(d->block_size);
    memcpy(tag_copy, tag, d->block_size);
    dom->tag = tag_copy;

    GF2X f;
    GF2E x;
    GF2E y;
    GF2E coeff;

    vec_GF2E yvec;
    vec_GF2E row;

    mat_GF2E V;
    V.SetDims(threshold, threshold);

    yvec.SetLength(threshold);
    row.SetLength(threshold);

    for (size_t i = 0; i < threshold; i++) {
        const uint8_t *share = shares[i];
        size_t j;

        /* Read x */
        fill_GF2X_from_bytes(f, share, d->block_size);
        conv(x, f);

        /* Populate row of Vandermonde matrix */
        conv(coeff, 1);
        for (j = 0; j < threshold; j++) {
            row[j] = coeff;
            coeff *= x;
        }

        V[i] = row; /* Set row */

        /* Read y */
        fill_GF2X_from_bytes(f, share + d->block_size, d->block_size);
        conv(yvec[i], f);
    }

    mat_GF2E Vinv;
    inv(Vinv, V);
    vec_GF2E rvec = Vinv * yvec;

    for (size_t i = 0; i < threshold; i++)
        SetCoeff(dom->polynomial, i, rvec[i]);

    uint8_t *skey = (uint8_t*)gcry_malloc_secure(d->key_size);
    memset(skey, 0, d->block_size);
    conv_GF2X_to_bytes(rep(rvec[0]), d->block_size * 8, skey);

    /* Setup cipher handle with secret key */
    gcry_cipher_open(&dom->ctr_handle, d->algo, GCRY_CIPHER_MODE_CTR,
                     GCRY_CIPHER_SECURE);
    gcry_cipher_setkey(dom->ctr_handle, skey, d->key_size);

    /* Cleanup */
    gcry_free(skey);

    *stat = DMMT_STAT_OK;

    return dom;
}

dmmt_stat_t dmmt_free(dmmt_t *d)
{
    if (d != NULL)
        free(d);

    return DMMT_STAT_OK;
}

/* ############################################################################
 * # Domain
 * ############################################################################
 */

dmmt_stat_t dmmt_dom_gen_share(dmmt_dom_t *dom, uint8_t *share_out)
{
    const dmmt_t *d = dom->d;
    uint8_t *buf = (uint8_t*)malloc(d->block_size);
    dmmt_stat_t stat = DMMT_STAT_INTERNAL_ERROR;

    /* Create random x to evaluate at (also first component of share) */
    GF2E x;
    GF2X coeff_poly;
    gcry_create_nonce(buf, d->block_size);
    fill_GF2X_from_bytes(coeff_poly, buf, d->block_size);
    conv(x, coeff_poly);

    /* Evaluate polynomial at x */
    GF2E y;
    eval(y, dom->polynomial, x);

    /* Write share */
    memset(share_out, 0, d->block_size * 2);
    int nb = conv_GF2X_to_bytes(rep(x), d->block_size * 8, share_out);
    if (nb > 0) {
        int nb = conv_GF2X_to_bytes(rep(y), d->block_size * 8,
                                    share_out + d->block_size);

        if (nb > 0)
            stat = DMMT_STAT_OK;
    }

    /* Cleanup */
    free(buf);

    return DMMT_STAT_OK;
}

const uint8_t *dmmt_dom_tag(dmmt_dom_t *dom)
{
    return dom->tag;
}

/* NOT THREAD SAFE - one encryption per domain at one time */
dmmt_stat_t dmmt_dom_encrypt(dmmt_dom_t *dom, const uint8_t *in, size_t in_size, uint8_t *out, size_t out_size)
{
    const dmmt_t *d = dom->d;

    /* IV */
    if (out_size < d->block_size)
        return DMMT_STAT_UNDERSIZED_BUFFER;

    gcry_create_nonce(out, d->block_size);
    gcry_cipher_setctr(dom->ctr_handle, out, d->block_size);

    /* Encrypt (skip over IV in the output buffer) */
    if (gcry_cipher_encrypt(dom->ctr_handle, out + d->block_size, out_size - d->block_size, in, in_size) != 0)
        return DMMT_STAT_INTERNAL_ERROR;

    return DMMT_STAT_OK;
}

/* NOT THREAD SAFE - one decryption per domain at one time */
dmmt_stat_t dmmt_dom_decrypt(dmmt_dom_t *dom, const uint8_t *in, size_t in_size, uint8_t *out, size_t out_size)
{
    const dmmt_t *d = dom->d;

    /* IV */
    if (in_size < d->block_size)
        return DMMT_STAT_UNDERSIZED_BUFFER;

    gcry_cipher_setctr(dom->ctr_handle, in, d->block_size);

    /* Decrypt (skip over IV in the input buffer) */
    if (gcry_cipher_decrypt(dom->ctr_handle, out, out_size, in + d->block_size, in_size - d->block_size) != 0)
        return DMMT_STAT_INTERNAL_ERROR;

    return DMMT_STAT_OK;
}

dmmt_stat_t dmmt_dom_free(dmmt_dom_t *dom)
{
    if (dom != NULL) {
        gcry_cipher_close(dom->ctr_handle);
        free(dom->tag);
        gcry_free(dom);
    }

    return DMMT_STAT_OK;
}

/* ############################################################################
 * # Utilities
 * ############################################################################
 */

inline void fill_block_with_int(dmmt_t *d, uint64_t v, uint8_t *out)
{
    const size_t sz = (d->block_size < sizeof(v)) ? d->block_size : sizeof(v);
    size_t i;

    for (i = 1; i <= sz; i++) {
        out[d->block_size - i] = v & 0xFF;
        v >>= 8;
    }

    for (; i <= d->block_size; i++)
        out[d->block_size - i] = 0;
}

inline void xor_block_with_int(dmmt_t *d, uint64_t v, uint8_t *out)
{
    const size_t sz = (d->block_size < sizeof(v)) ? d->block_size : sizeof(v);

    for (size_t i = 1; i <= sz; i++) {
        out[d->block_size - i] ^= v & 0xFF;
        v >>= 8;
    }
}

inline void fill_GF2X_from_bytes(GF2X &f, const uint8_t *bytes, size_t num_bytes)
{
    size_t bitn = 0;

    for (size_t i = 0; i < num_bytes; i++) {
        uint8_t b = bytes[i];

        for (size_t j = 0; j < 8; j++) {
            SetCoeff(f, bitn++, b & 0x01);
            b >>= 1;
        }
    }
}

/** Returns the number of BITS written */
inline int conv_GF2X_to_bytes(const GF2X &f, long max_len, uint8_t *out)
{
    size_t i;
    size_t j;
    size_t bitn = 0;
    const long lenf = deg(f) + 1;
    const long len = (lenf < max_len) ? lenf : max_len;

    if (len < 0)
        return -1;

    const size_t count = (size_t)len;
    const size_t nbytes = count / 8;
    uint8_t b;

    for (i = 0; i < nbytes; i++) {
        b = 0;

        for (j = 0; j < 8; j++) {
            b |= rep(coeff(f, bitn++)) << j;
        }

        out[i] = b;
    }

    if (bitn < count) {
        j = 0;
        b = 0;
        do {
            b |= rep(coeff(f, bitn++)) << j++;
        }
        while (bitn < count);
        out[i] = b;
    }

    return (int)count; // number of bits
}


#ifndef SH_PROCESS_HPP
#define SH_PROCESS_HPP
#include <vector>
#include <cmath>
#include <boost/math/special_functions/spherical_harmonic.hpp>
#include <boost/math/special_functions/legendre.hpp>
#include "image/image.hpp"
#define _USE_MATH_DEFINES

struct SHDecomposition : public BaseProcess
{

    std::vector<float> UPiB;
    unsigned int half_odf_size;
        std::vector<unsigned int> b0_index;

    static float Yj(int l,int m,float theta,float phi)
    {
        if (m == 0)
            return boost::math::spherical_harmonic_r(l,m,theta,phi);
        if (m < 0)
            return M_SQRT2*boost::math::spherical_harmonic_r(l,m,theta,phi);
        else
            return M_SQRT2*boost::math::spherical_harmonic_i(l,m,theta,phi);
    }

    static int getJ(int m,int n)
    {
        return (n*n+n)/2+m;
    }
public:
    virtual void init(Voxel& voxel)
    {

		b0_index.clear();
                for(unsigned int index = 0;index < voxel.bvalues.size();++index)
			if(voxel.bvalues[index] == 0)
			    b0_index.push_back(index);

        half_odf_size = voxel.ti.vertices_count/2;
        float lambda = voxel.param[0];
        unsigned int max_l = voxel.param[1];
        const unsigned int R = ((max_l+1)*(max_l+2)/2);
        std::vector<std::pair<int,int> > j_map(R);
        for (int k = 0; k <= max_l; k += 2)
            for (int m = -k; m <= k; ++m)
                j_map[getJ(m,k)] = std::make_pair(m,k);

        std::vector<float> Bt(R*voxel.bvectors.size());
        for (unsigned int j = 0,index = 0; j < R; ++j)
            for (unsigned int n = 0; n < voxel.bvectors.size(); ++n,++index)
            {
                float atan2_xy = std::atan2(voxel.bvectors[n][1],voxel.bvectors[n][0]);
                if (atan2_xy < 0.0f)
                    atan2_xy += float(2.0f*M_PI);
                Bt[index] = Yj(j_map[j].second,j_map[j].first,std::acos(voxel.bvectors[n][2]),atan2_xy);
            }
        std::vector<float> UP(half_odf_size*R);
        {
            std::vector<float> U(half_odf_size*R);
            for (unsigned int n = 0,index = 0; n < half_odf_size; ++n)
                for (unsigned int j = 0; j < R; ++j,++index)
                {
                    float atan2_xy = std::atan2(voxel.ti.vertices[n][1],voxel.ti.vertices[n][0]);
                    if (atan2_xy < 0.0f)
                        atan2_xy += float(2.0f*M_PI);
                    U[index] = Yj(j_map[j].second,j_map[j].first,std::acos(voxel.ti.vertices[n][2]),atan2_xy);
                }
            std::vector<float> P(R*R);
            for (unsigned int i = 0,index = 0; i < R; ++i,index += R+1)
                P[index] = boost::math::legendre_p(j_map[i].second,0.0)*2.0*M_PI;

            image::mat::product(U.begin(),P.begin(),UP.begin(),image::dyndim(half_odf_size,R),image::dyndim(R,R));
        }

        std::vector<float> iB(Bt.size());
        {
            std::vector<float> BtB(R*R); // BtB = Bt * trans(Bt);
            image::mat::square(Bt.begin(),BtB.begin(),image::dyndim(R,voxel.bvectors.size()));
            for (unsigned int i = 0,index = 0; i < R; ++i,index += R+1)
            {
                float l = j_map[i].second;
                BtB[index] += l*l*(l+1.0)*(l+1.0)*lambda;
            }
            std::vector<unsigned int> pivot(R);
            image::mat::lu_decomposition(BtB.begin(),pivot.begin(),image::dyndim(R,R));

            //iB = inv(BtB)*Bt;
            image::mat::lu_solve(BtB.begin(),pivot.begin(),Bt.begin(),iB.begin(),image::dyndim(R,R),image::dyndim(R,voxel.bvectors.size()));
        }

        UPiB.resize(half_odf_size*voxel.bvectors.size());
        image::mat::product(UP.begin(),iB.begin(),UPiB.begin(),image::dyndim(half_odf_size,R),image::dyndim(R,voxel.bvectors.size()));

    }
public:
    virtual void run(Voxel&, VoxelData& data)
    {

		// remove the b0 signal
                for(unsigned int index = 0;index < b0_index.size();++index)
			data.space[b0_index[index]] = 0;

                image::mat::vector_product(&*UPiB.begin(),&*data.space.begin(),&*data.odf.begin(),image::dyndim(half_odf_size,data.space.size()));
        for (unsigned int index = 0; index < data.odf.size(); ++index)
            if (data.odf[index] < 0.0)
                data.odf[index] = 0.0;
    }
};

#endif//SH_PROCESS_HPP


#include <iostream>
#include <fstream>
#include <vector>
#include <string>

#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>

#include "Day2.hpp"

int run_day2(std::vector<int> &values, int noun, int verb);

void day2() {
	std::ifstream inputFile("Data/Day2.txt");
	std::string line;
	std::getline(inputFile, line);

	std::vector<std::string> strings;
	boost::split(strings, line, boost::is_any_of(","), boost::token_compress_on);

	std::vector<int> numbersOriginal;

	//std::transform(strings.begin(), strings.end(), std::back_inserter(numbers), [](const std::string& str) { return std::stoi(str); });
	std::transform(strings.begin(), strings.end(), std::back_inserter(numbersOriginal), &boost::lexical_cast<int, std::string>);

	std::vector<int> numbers(numbersOriginal.size());

	// PART 1
	//std::copy(numbersOriginal.begin(), numbersOriginal.end(), numbers.begin());
	//int result = run_day2(numbers, 12, 2);

	// PART 2
	int nounPlusVerb = 0;
	for (int noun = 0; noun <= 99; ++noun) {
		for (int verb = 0; verb <= 99; ++verb) {
			std::copy(numbersOriginal.begin(), numbersOriginal.end(), numbers.begin());
			int result = run_day2(numbers, noun, verb);
			if (result == 19690720) {
				nounPlusVerb = 100 * noun + verb;
				break;
			}
		}
		if (nounPlusVerb != 0) {
			break;
		}
	}

	std::cout << "RESULT: " << nounPlusVerb << std::endl;
}

int run_day2(std::vector<int> &values, int noun, int verb) {
	// Replacing values
	values[1] = noun;
	values[2] = verb;

	//int index = 0;
	//for (auto number = values.begin(); values != values.end(); ++number) {
	//	std::cout << *number;
	//	if (index % 4 == 3) {
	//		std::cout << std::endl;
	//	}
	//	else {
	//		std::cout << " ";
	//	}
	//	index++;
	//}

	int cursorPosition = 0;
	int opcode = values[cursorPosition * 4];
	while (opcode != 99) {
		int opcode = values[cursorPosition * 4];

		if (opcode == 99)
			break;

		int leftPosition = values[(cursorPosition * 4) + 1];
		int rightPosition = values[(cursorPosition * 4) + 2];
		int targetPosition = values[(cursorPosition * 4) + 3];

		switch (opcode)
		{
		case 1:
			values[targetPosition] = values[leftPosition] + values[rightPosition];
			break;
		case 2:
			values[targetPosition] = values[leftPosition] * values[rightPosition];
			break;
		case 99:
			break;
		default:
			break;
		}
		cursorPosition++;
	}

	//index = 0;
	//for (auto number = values.begin(); number != values.end(); ++number) {
	//	std::cout << *number;
	//	if (index % 4 == 3) {
	//		std::cout << std::endl;
	//	}
	//	else {
	//		std::cout << " ";
	//	}
	//	index++;
	//}

	//std::cout << std::endl << std::endl << std::endl << values[0];

	std::cout << "noun: " << noun << " verb: " << verb << " => " << values[0] << std::endl;

	return values[0];
}


#include <iostream>
#include <memory>
#include <random>
#include <string>

#include <Eigen/Sparse>

#include "GeometricMultigridOperators.h"
#include "GeometricMultigridPoissonSolver.h"
#include "InitialMultigridTestDomains.h"
#include "Renderer.h"
#include "ScalarGrid.h"
#include "Transform.h"
#include "UniformGrid.h"
#include "Utilities.h"

using namespace FluidSim2D::RenderTools;
using namespace FluidSim2D::SimTools;

std::unique_ptr<Renderer> renderer;

static constexpr int gridSize = 512;
static constexpr bool useComplexDomain = true;
static constexpr bool useSolidSphere = true;

int main(int argc, char** argv)
{
	using namespace GeometricMultigridOperators;

	using StoreReal = double;
	using SolveReal = double;

	using Vector = std::conditional<std::is_same<SolveReal, float>::value, Eigen::VectorXf, Eigen::VectorXd>::type;

	UniformGrid<CellLabels> domainCellLabels;
	VectorGrid<StoreReal> boundaryWeights;
	int mgLevels;
	{
		UniformGrid<CellLabels> baseDomainCellLabels;
		VectorGrid<StoreReal> baseBoundaryWeights;

		// Complex domain set up
		if (useComplexDomain)
			buildComplexDomain(baseDomainCellLabels,
								baseBoundaryWeights,
								gridSize,
								useSolidSphere);
		// Simple domain set up
		else
			buildSimpleDomain(baseDomainCellLabels,
								baseBoundaryWeights,
								gridSize,
								1 /*dirichlet band*/);

		// Build expanded domain
		std::pair<Vec2i, int> mgSettings = buildExpandedDomain(domainCellLabels, boundaryWeights, baseDomainCellLabels, baseBoundaryWeights);

		mgLevels = mgSettings.second;
	}

	SolveReal dx = boundaryWeights.dx();

	UniformGrid<StoreReal> rhsA(domainCellLabels.size(), 0);
	UniformGrid<StoreReal> rhsB(domainCellLabels.size(), 0);

	tbb::parallel_for(tbb::blocked_range<int>(0, domainCellLabels.voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int> &range)
	{
		std::default_random_engine generator;
		std::uniform_real_distribution<StoreReal> distribution(0, 1);

		for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
		{
			Vec2i cell = domainCellLabels.unflatten(cellIndex);

			if (domainCellLabels(cell) == CellLabels::INTERIOR_CELL ||
				domainCellLabels(cell) == CellLabels::BOUNDARY_CELL)
			{
				rhsA(cell) = distribution(generator);
				rhsB(cell) = distribution(generator);
			}
		}
	});

	Transform xform(dx, Vec2f(0));
	std::cout.precision(10);
	{
		UniformGrid<StoreReal> solutionA(domainCellLabels.size(), 0);
		UniformGrid<StoreReal> solutionB(domainCellLabels.size(), 0);

		std::vector<Vec2i> boundaryCells = buildBoundaryCells(domainCellLabels, 3);

		// Test Jacobi symmetry
		boundaryJacobiPoissonSmoother<SolveReal>(solutionA, rhsA, domainCellLabels, boundaryCells, dx, &boundaryWeights);
		boundaryJacobiPoissonSmoother<SolveReal>(solutionB, rhsB, domainCellLabels, boundaryCells, dx, &boundaryWeights);

		interiorJacobiPoissonSmoother<SolveReal>(solutionA, rhsA, domainCellLabels, dx, &boundaryWeights);
		interiorJacobiPoissonSmoother<SolveReal>(solutionB, rhsB, domainCellLabels, dx, &boundaryWeights);

		boundaryJacobiPoissonSmoother<SolveReal>(solutionA, rhsA, domainCellLabels, boundaryCells, dx, &boundaryWeights);
		boundaryJacobiPoissonSmoother<SolveReal>(solutionB, rhsB, domainCellLabels, boundaryCells, dx, &boundaryWeights);

		SolveReal dotA = dotProduct<SolveReal>(solutionA, rhsB, domainCellLabels);
		SolveReal dotB = dotProduct<SolveReal>(solutionB, rhsA, domainCellLabels);

		std::cout << "Jacobi smoother symmetry test: " << dotA << ", " << dotB << std::endl;
		assert(fabs(dotA - dotB) / fabs(std::max(dotA, dotB)) < 1E-10);
	}
	{
		// Test direct solve symmetry
		Eigen::SimplicialCholesky<Eigen::SparseMatrix<SolveReal>> myCoarseSolver;
		Eigen::SparseMatrix<SolveReal> sparseMatrix;

		// Pre-build matrix at the coarsest level
		int interiorCellCount = 0;
		UniformGrid<int> directSolverIndices(domainCellLabels.size(), -1);
		{
			forEachVoxelRange(Vec2i(0), domainCellLabels.size(), [&](const Vec2i &cell)
			{
				if (domainCellLabels(cell) == CellLabels::INTERIOR_CELL ||
					domainCellLabels(cell) == CellLabels::BOUNDARY_CELL)
					directSolverIndices(cell) = interiorCellCount++;
			});

			// Build rows
			std::vector<Eigen::Triplet<SolveReal>> sparseElements;

			SolveReal gridScale = 1. / sqr(dx);
			forEachVoxelRange(Vec2i(0), domainCellLabels.size(), [&](const Vec2i &cell)
			{
				if (domainCellLabels(cell) == CellLabels::INTERIOR_CELL)
				{
					int index = directSolverIndices(cell);
					assert(index >= 0);
					for (int axis : {0, 1})
						for (int direction : {0, 1})
						{
							Vec2i adjacentCell = cellToCell(cell, axis, direction);

							assert(domainCellLabels(adjacentCell) == CellLabels::INTERIOR_CELL ||
									domainCellLabels(adjacentCell) == CellLabels::BOUNDARY_CELL);

							Vec2i face = cellToFace(cell, axis, direction);
							assert(boundaryWeights(face, axis) == 1);

							int adjacentIndex = directSolverIndices(adjacentCell);
							assert(adjacentIndex >= 0);

							sparseElements.emplace_back(index, adjacentIndex, -gridScale);
						}
					sparseElements.emplace_back(index, index, 4. * gridScale);
				}
				else if (domainCellLabels(cell) == CellLabels::BOUNDARY_CELL)
				{
					int index = directSolverIndices(cell);
					assert(index >= 0);

					SolveReal diagonal = 0;

					for (int axis : {0, 1})
						for (int direction : {0, 1})
						{
							Vec2i adjacentCell = cellToCell(cell, axis, direction);

							if (domainCellLabels(adjacentCell) == CellLabels::INTERIOR_CELL)
							{
								int adjacentIndex = directSolverIndices(adjacentCell);
								assert(adjacentIndex >= 0);

								Vec2i face = cellToFace(cell, axis, direction);
								assert(boundaryWeights(face, axis) == 1);

								sparseElements.emplace_back(index, adjacentIndex, -gridScale);
								++diagonal;
							}
							else if (domainCellLabels(adjacentCell) == CellLabels::BOUNDARY_CELL)
							{
								int adjacentIndex = directSolverIndices(adjacentCell);
								assert(adjacentIndex >= 0);

								Vec2i face = cellToFace(cell, axis, direction);
								SolveReal weight = boundaryWeights(face, axis);

								sparseElements.emplace_back(index, adjacentIndex, -gridScale * weight);
								diagonal += weight;
							}
							else if (domainCellLabels(adjacentCell) == CellLabels::DIRICHLET_CELL)
							{
								int adjacentIndex = directSolverIndices(adjacentCell);
								assert(adjacentIndex == -1);

								Vec2i face = cellToFace(cell, axis, direction);
								SolveReal weight = boundaryWeights(face, axis);

								diagonal += weight;
							}
							else
							{
								assert(domainCellLabels(adjacentCell) == CellLabels::EXTERIOR_CELL);
								int adjacentIndex = directSolverIndices(adjacentCell);
								assert(adjacentIndex == -1);

								Vec2i face = cellToFace(cell, axis, direction);
								assert(boundaryWeights(face, axis) == 0);
							}
						}

					sparseElements.emplace_back(index, index, gridScale * diagonal);
				}
			});

			// Solve system
			sparseMatrix = Eigen::SparseMatrix<SolveReal>(interiorCellCount, interiorCellCount);
			sparseMatrix.setFromTriplets(sparseElements.begin(), sparseElements.end());
			sparseMatrix.makeCompressed();

			myCoarseSolver.compute(sparseMatrix);

			assert(myCoarseSolver.info() == Eigen::Success);
		}

		UniformGrid<StoreReal> solutionA(domainCellLabels.size(), 0);

		{
			Vector coarseRHSVector = Vector::Zero(interiorCellCount);
			// Copy to Eigen and direct solve
			tbb::parallel_for(tbb::blocked_range<int>(0, domainCellLabels.voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
			{
				for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
				{
					Vec2i cell = domainCellLabels.unflatten(cellIndex);

					if (domainCellLabels(cell) == CellLabels::INTERIOR_CELL ||
						domainCellLabels(cell) == CellLabels::BOUNDARY_CELL)
					{
						int index = directSolverIndices(cell);
						assert(index >= 0);

						coarseRHSVector(index) = rhsA(cell);
					}
				}
			});

			Vector directSolution = myCoarseSolver.solve(coarseRHSVector);

			// Copy solution back
			tbb::parallel_for(tbb::blocked_range<int>(0, domainCellLabels.voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
			{
				for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
				{
					Vec2i cell = domainCellLabels.unflatten(cellIndex);

					if (domainCellLabels(cell) == CellLabels::INTERIOR_CELL ||
						domainCellLabels(cell) == CellLabels::BOUNDARY_CELL)
					{
						int index = directSolverIndices(cell);
						assert(index >= 0);

						solutionA(cell) = directSolution(index);
					}
				}
			});
		}

		UniformGrid<StoreReal> solutionB(domainCellLabels.size(), 0);

		{
			Vector coarseRHSVector = Vector::Zero(interiorCellCount);
			// Copy to Eigen and direct solve
			tbb::parallel_for(tbb::blocked_range<int>(0, domainCellLabels.voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
			{
				for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
				{
					Vec2i cell = domainCellLabels.unflatten(cellIndex);

					if (domainCellLabels(cell) == CellLabels::INTERIOR_CELL ||
						domainCellLabels(cell) == CellLabels::BOUNDARY_CELL)
					{
						int index = directSolverIndices(cell);
						assert(index >= 0);

						coarseRHSVector(index) = rhsB(cell);
					}
				}
			});

			Vector directSolution = myCoarseSolver.solve(coarseRHSVector);

			// Copy solution back
			tbb::parallel_for(tbb::blocked_range<int>(0, domainCellLabels.voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
			{
				for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
				{
					Vec2i cell = domainCellLabels.unflatten(cellIndex);

					if (domainCellLabels(cell) == CellLabels::INTERIOR_CELL ||
						domainCellLabels(cell) == CellLabels::BOUNDARY_CELL)
					{
						int index = directSolverIndices(cell);
						assert(index >= 0);

						solutionB(cell) = directSolution(index);
					}
				}
			});
		}

		// Compute dot products
		SolveReal dotA = dotProduct<SolveReal>(solutionA, rhsB, domainCellLabels);
		SolveReal dotB = dotProduct<SolveReal>(solutionB, rhsA, domainCellLabels);

		std::cout << "Direct solver symmetry test: " << dotA << ", " << dotB << std::endl;
		assert(fabs(dotA - dotB) / fabs(std::max(dotA, dotB)) < 1E-10);
	}

	{
		// Test down and up sampling
		UniformGrid<CellLabels> coarseDomainLabels = buildCoarseCellLabels(domainCellLabels);

		assert(unitTestBoundaryCells<StoreReal>(coarseDomainLabels) && unitTestBoundaryCells<StoreReal>(domainCellLabels, &boundaryWeights));
		assert(unitTestExteriorCells(coarseDomainLabels) && unitTestExteriorCells(domainCellLabels));
		assert(unitTestCoarsening(coarseDomainLabels, domainCellLabels));

		UniformGrid<StoreReal> coarseRhs(coarseDomainLabels.size(), 0);

		UniformGrid<StoreReal> solutionA(domainCellLabels.size(), 0);

		{
			downsample<SolveReal>(coarseRhs, rhsA, coarseDomainLabels, domainCellLabels);
			upsampleAndAdd<SolveReal>(solutionA, coarseRhs, domainCellLabels, coarseDomainLabels);
		}

		UniformGrid<StoreReal> solutionB(domainCellLabels.size(), 0);

		{
			downsample<SolveReal>(coarseRhs, rhsB, coarseDomainLabels, domainCellLabels);
			upsampleAndAdd<SolveReal>(solutionB, coarseRhs, domainCellLabels, coarseDomainLabels);
		}

		// Compute dot products
		SolveReal dotA = dotProduct<SolveReal>(solutionA, rhsB, domainCellLabels);
		SolveReal dotB = dotProduct<SolveReal>(solutionB, rhsA, domainCellLabels);

		std::cout << "Coarse transfer symmetry test: " << dotA << ", " << dotB << std::endl;
		assert(fabs(dotA - dotB) / fabs(std::max(dotA, dotB)) < 1E-10);
	}
	{
		// Test single level correction
		UniformGrid<CellLabels> coarseDomainLabels = buildCoarseCellLabels(domainCellLabels);

		assert(unitTestBoundaryCells<StoreReal>(coarseDomainLabels) && unitTestBoundaryCells<StoreReal>(domainCellLabels, &boundaryWeights));
		assert(unitTestExteriorCells(coarseDomainLabels) && unitTestExteriorCells(domainCellLabels));
		assert(unitTestCoarsening(coarseDomainLabels, domainCellLabels));

		Eigen::SimplicialCholesky<Eigen::SparseMatrix<SolveReal>> myCoarseSolver;
		Eigen::SparseMatrix<SolveReal> sparseMatrix;

		// Pre-build matrix at the coarsest level
		int interiorCellCount = 0;
		UniformGrid<int> directSolverIndices(coarseDomainLabels.size(), -1);
		{
			forEachVoxelRange(Vec2i(0), coarseDomainLabels.size(), [&](const Vec2i &cell)
			{
				if (coarseDomainLabels(cell) == CellLabels::INTERIOR_CELL ||
					coarseDomainLabels(cell) == CellLabels::BOUNDARY_CELL)
					directSolverIndices(cell) = interiorCellCount++;
			});

			// Build rows
			std::vector<Eigen::Triplet<SolveReal>> sparseElements;

			SolveReal gridScale = 1. / sqr(2. * dx);
			forEachVoxelRange(Vec2i(0), coarseDomainLabels.size(), [&](const Vec2i &cell)
			{
				if (coarseDomainLabels(cell) == CellLabels::INTERIOR_CELL)
				{
					int index = directSolverIndices(cell);
					assert(index >= 0);
					for (int axis : {0, 1})
						for (int direction : {0, 1})
						{
							Vec2i adjacentCell = cellToCell(cell, axis, direction);

							auto adjacentLabels = coarseDomainLabels(adjacentCell);
							assert(adjacentLabels == CellLabels::INTERIOR_CELL ||
								adjacentLabels == CellLabels::BOUNDARY_CELL);

							int adjacentIndex = directSolverIndices(adjacentCell);
							assert(adjacentIndex >= 0);

							sparseElements.emplace_back(index, adjacentIndex, -gridScale);
						}

					sparseElements.emplace_back(index, index, 4. * gridScale);
				}
				else if (coarseDomainLabels(cell) == CellLabels::BOUNDARY_CELL)
				{
					SolveReal diagonal = 0;
					int index = directSolverIndices(cell);
					assert(index >= 0);
					for (int axis : {0, 1})
						for (int direction : {0, 1})
						{
							Vec2i adjacentCell = cellToCell(cell, axis, direction);

							auto cellLabels = coarseDomainLabels(adjacentCell);
							if (cellLabels == CellLabels::INTERIOR_CELL ||
								cellLabels == CellLabels::BOUNDARY_CELL)
							{
								int adjacentIndex = directSolverIndices(adjacentCell);
								assert(adjacentIndex >= 0);

								sparseElements.emplace_back(index, adjacentIndex, -gridScale);
								++diagonal;
							}
							else if (cellLabels == CellLabels::DIRICHLET_CELL)
								++diagonal;
						}

					sparseElements.emplace_back(index, index, diagonal * gridScale);
				}
			});

			sparseMatrix = Eigen::SparseMatrix<SolveReal>(interiorCellCount, interiorCellCount);
			sparseMatrix.setFromTriplets(sparseElements.begin(), sparseElements.end());
			sparseMatrix.makeCompressed();

			myCoarseSolver.compute(sparseMatrix);

			assert(myCoarseSolver.info() == Eigen::Success);
		}

		// Transfer rhs to coarse rhs as if it was a residual with a zero initial guess
		UniformGrid<StoreReal> solutionA(domainCellLabels.size(), 0);
		{
			// Pre-smooth to get an initial guess
			std::vector<Vec2i> boundaryCells = buildBoundaryCells(domainCellLabels, 3);

			// Test Jacobi symmetry
			for (int iteration = 0; iteration < 3; ++iteration)
				boundaryJacobiPoissonSmoother<SolveReal>(solutionA, rhsA, domainCellLabels, boundaryCells, dx, &boundaryWeights);

			interiorJacobiPoissonSmoother<SolveReal>(solutionA, rhsA, domainCellLabels, dx, &boundaryWeights);

			for (int iteration = 0; iteration < 3; ++iteration)
				boundaryJacobiPoissonSmoother<SolveReal>(solutionA, rhsA, domainCellLabels, boundaryCells, dx, &boundaryWeights);

			// Compute new residual
			UniformGrid<StoreReal> residualA(domainCellLabels.size(), 0);

			computePoissonResidual<SolveReal>(residualA, solutionA, rhsA, domainCellLabels, dx, &boundaryWeights);

			UniformGrid<StoreReal> coarseRhs(coarseDomainLabels.size(), 0);
			downsample<SolveReal>(coarseRhs, residualA, coarseDomainLabels, domainCellLabels);

			Vector coarseRHSVector = Vector::Zero(interiorCellCount);

			// Copy to Eigen and direct solve
			tbb::parallel_for(tbb::blocked_range<int>(0, coarseDomainLabels.voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
			{
				for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
				{
					Vec2i cell = coarseDomainLabels.unflatten(cellIndex);

					if (coarseDomainLabels(cell) == CellLabels::INTERIOR_CELL ||
						coarseDomainLabels(cell) == CellLabels::BOUNDARY_CELL)
					{
						int index = directSolverIndices(cell);
						assert(index >= 0);

						coarseRHSVector(index) = coarseRhs(cell);
					}
				}
			});

			UniformGrid<StoreReal> coarseSolution(coarseDomainLabels.size(), 0);

			Vector directSolution = myCoarseSolver.solve(coarseRHSVector);

			// Copy solution back
			tbb::parallel_for(tbb::blocked_range<int>(0, coarseDomainLabels.voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
			{
				for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
				{
					Vec2i cell = coarseDomainLabels.unflatten(cellIndex);

					if (coarseDomainLabels(cell) == CellLabels::INTERIOR_CELL ||
						coarseDomainLabels(cell) == CellLabels::BOUNDARY_CELL)
					{
						int index = directSolverIndices(cell);
						assert(index >= 0);

						coarseSolution(cell) = directSolution(index);
					}
				}
			});

			upsampleAndAdd<SolveReal>(solutionA, coarseSolution, domainCellLabels, coarseDomainLabels);

			for (int iteration = 0; iteration < 3; ++iteration)
				boundaryJacobiPoissonSmoother<SolveReal>(solutionA, rhsA, domainCellLabels, boundaryCells, dx, &boundaryWeights);

			interiorJacobiPoissonSmoother<SolveReal>(solutionA, rhsA, domainCellLabels, dx, &boundaryWeights);

			for (int iteration = 0; iteration < 3; ++iteration)
				boundaryJacobiPoissonSmoother<SolveReal>(solutionA, rhsA, domainCellLabels, boundaryCells, dx, &boundaryWeights);
		}

		UniformGrid<StoreReal> solutionB(domainCellLabels.size(), 0);
		{
			// Pre-smooth to get an initial guess
			std::vector<Vec2i> boundaryCells = buildBoundaryCells(domainCellLabels, 3);

			// Test Jacobi symmetry
			for (int iteration = 0; iteration < 3; ++iteration)
				boundaryJacobiPoissonSmoother<SolveReal>(solutionB, rhsB, domainCellLabels, boundaryCells, dx, &boundaryWeights);

			interiorJacobiPoissonSmoother<SolveReal>(solutionB, rhsB, domainCellLabels, dx, &boundaryWeights);

			for (int iteration = 0; iteration < 3; ++iteration)
				boundaryJacobiPoissonSmoother<SolveReal>(solutionB, rhsB, domainCellLabels, boundaryCells, dx, &boundaryWeights);

			// Compute new residual
			UniformGrid<StoreReal> residualB(domainCellLabels.size(), 0);

			computePoissonResidual<SolveReal>(residualB, solutionB, rhsB, domainCellLabels, dx, &boundaryWeights);

			UniformGrid<StoreReal> coarseRhs(coarseDomainLabels.size(), 0);
			downsample<SolveReal>(coarseRhs, residualB, coarseDomainLabels, domainCellLabels);

			Vector coarseRHSVector = Vector::Zero(interiorCellCount);

			// Copy to Eigen and direct solve
			tbb::parallel_for(tbb::blocked_range<int>(0, coarseDomainLabels.voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
			{
				for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
				{
					Vec2i cell = coarseDomainLabels.unflatten(cellIndex);

					if (coarseDomainLabels(cell) == CellLabels::INTERIOR_CELL ||
						coarseDomainLabels(cell) == CellLabels::BOUNDARY_CELL)
					{
						int index = directSolverIndices(cell);
						assert(index >= 0);

						coarseRHSVector(index) = coarseRhs(cell);
					}
				}
			});

			UniformGrid<StoreReal> coarseSolution(coarseDomainLabels.size(), 0);

			Vector directSolution = myCoarseSolver.solve(coarseRHSVector);

			// Copy solution back
			tbb::parallel_for(tbb::blocked_range<int>(0, coarseDomainLabels.voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
			{
				for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
				{
					Vec2i cell = coarseDomainLabels.unflatten(cellIndex);

					if (coarseDomainLabels(cell) == CellLabels::INTERIOR_CELL ||
						coarseDomainLabels(cell) == CellLabels::BOUNDARY_CELL)
					{
						int index = directSolverIndices(cell);
						assert(index >= 0);

						coarseSolution(cell) = directSolution(index);
					}
				}
			});

			upsampleAndAdd<SolveReal>(solutionB, coarseSolution, domainCellLabels, coarseDomainLabels);

			for (int iteration = 0; iteration < 3; ++iteration)
				boundaryJacobiPoissonSmoother<SolveReal>(solutionB, rhsB, domainCellLabels, boundaryCells, dx, &boundaryWeights);

			interiorJacobiPoissonSmoother<SolveReal>(solutionB, rhsB, domainCellLabels, dx, &boundaryWeights);

			for (int iteration = 0; iteration < 3; ++iteration)
				boundaryJacobiPoissonSmoother<SolveReal>(solutionB, rhsB, domainCellLabels, boundaryCells, dx, &boundaryWeights);
		}

		SolveReal dotA = dotProduct<SolveReal>(solutionA, rhsB, domainCellLabels);
		SolveReal dotB = dotProduct<SolveReal>(solutionB, rhsA, domainCellLabels);

		std::cout << "One level correction symmetry: " << dotA << ", " << dotB << std::endl;
		assert(fabs(dotA - dotB) / fabs(std::max(dotA, dotB)) < 1E-10);
	}

	{
		// Pre-build multigrid preconditioner
		GeometricMultigridPoissonSolver mgSolver(domainCellLabels, boundaryWeights, mgLevels, dx);

		UniformGrid<StoreReal> solutionA(domainCellLabels.size(), 0);
		mgSolver.applyMGVCycle(solutionA, rhsA);
		mgSolver.applyMGVCycle(solutionA, rhsA, true);
		mgSolver.applyMGVCycle(solutionA, rhsA, true);
		mgSolver.applyMGVCycle(solutionA, rhsA, true);

		UniformGrid<StoreReal> solutionB(domainCellLabels.size(), 0);
		mgSolver.applyMGVCycle(solutionB, rhsB);
		mgSolver.applyMGVCycle(solutionB, rhsB, true);
		mgSolver.applyMGVCycle(solutionB, rhsB, true);
		mgSolver.applyMGVCycle(solutionB, rhsB, true);

		SolveReal dotA = dotProduct<SolveReal>(solutionA, rhsB, domainCellLabels);
		SolveReal dotB = dotProduct<SolveReal>(solutionB, rhsA, domainCellLabels);

		std::cout << "4 v-cycle symmetry: " << dotA << ", " << dotB << std::endl;
		assert(fabs(dotA - dotB) / fabs(std::max(dotA, dotB)) < 1E-10);
	}

	// Print domain labels to make sure they are set up correctly
	int pixelHeight = 1080;
	int pixelWidth = pixelHeight;
	renderer = std::make_unique<Renderer>("MG Symmetry Test", Vec2i(pixelWidth, pixelHeight), Vec2f(0), 1, &argc, argv);

	ScalarGrid<float> tempGrid(Transform(dx, Vec2f(0)), domainCellLabels.size());

	tbb::parallel_for(tbb::blocked_range<int>(0, tempGrid.voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
	{
		for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
		{
			Vec2i cell = tempGrid.unflatten(cellIndex);

			tempGrid(cell) = float(domainCellLabels(cell));
		}
	});

	tempGrid.drawVolumetric(*renderer, Vec3f(0), Vec3f(1), float(CellLabels::INTERIOR_CELL), float(CellLabels::BOUNDARY_CELL));

	renderer->run();
}


#pragma once
#include <Eigen/Core>

//! Makes a coordinate transform that maps
//!
//! e1 to a1
//! e2 to a2
//!
//! where {e1, e2} is the standard basis for R^2.
template<class Point>
Eigen::Matrix2d makeCoordinateTransform(const Point& a1, const Point& a2) {
    Eigen::Matrix2d coordinateTransform;

    coordinateTransform << a1(0), a2(0),
                           a1(1), a2(1);

    return coordinateTransform;
}


#include <iostream>
#include <boost/numeric/ublas/io.hpp>
#include "square_matrix_multiply.hpp"

int main ()
{
    using namespace boost::numeric::ublas;
    matrix<int> lhs(2,2), rhs(2,2);

    lhs(0,0) = 1;
    lhs(0,1) = 3;
    lhs(1,0) = 7;
    lhs(1,1) = 5;

    rhs(0,0) = 6;
    rhs(0,1) = 8;
    rhs(1,0) = 4;
    rhs(1,1) = 2;

    std::cout << clrs::ch4::square_matrix_multiply_recursive(lhs,rhs)  << std::endl;
    std::cout << clrs::ch4::square_matrix_multiply(lhs,rhs)            << std::endl;
    std::cout << clrs::ch4::square_matrix_multiply_strassen(lhs,rhs)   << std::endl;

}


/* test_chi_squared_distribution.cpp
 *
 * Copyright Steven Watanabe 2011
 * 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)
 *
 * $Id$
 *
 */

#include <boost/random/chi_squared_distribution.hpp>
#include <limits>

#define BOOST_RANDOM_DISTRIBUTION boost::random::chi_squared_distribution<>
#define BOOST_RANDOM_ARG1 n
#define BOOST_RANDOM_ARG1_DEFAULT 1.0
#define BOOST_RANDOM_ARG1_VALUE 7.5

#define BOOST_RANDOM_DIST0_MIN 0
#define BOOST_RANDOM_DIST0_MAX (std::numeric_limits<double>::infinity)()
#define BOOST_RANDOM_DIST1_MIN 0
#define BOOST_RANDOM_DIST1_MAX (std::numeric_limits<double>::infinity)()
#define BOOST_RANDOM_DIST2_MIN 0
#define BOOST_RANDOM_DIST2_MAX (std::numeric_limits<double>::infinity)()

#define BOOST_RANDOM_TEST1_PARAMS
#define BOOST_RANDOM_TEST1_MIN 0.0
#define BOOST_RANDOM_TEST1_MAX 100.0

#define BOOST_RANDOM_TEST2_PARAMS (10000.0)
#define BOOST_RANDOM_TEST2_MIN 100.0

#include "test_distribution.ipp"


//
//  Copyright (c) 2018-2019, Cem Bassoy, cem.bassoy@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)
//
//  The authors gratefully acknowledge the support of
//  Fraunhofer IOSB, Ettlingen, Germany
//

#include <boost/numeric/ublas/tensor.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>

#include <ostream>

int main()
{
	using namespace boost::numeric::ublas;
	using namespace boost::multiprecision;

	// creates a three-dimensional tensor with extents 3,4 and 2
	// tensor A stores single-precision floating-point number according
	// to the first-order storage format
	using ftype = float;
	auto A = tensor<ftype>{3,4,2};

	// initializes the tensor with increasing values along the first-index
	// using a single index.
	auto vf = ftype(0);
	for(auto i = 0u; i < A.size(); ++i, vf += ftype(1))
		A[i] = vf;

	// formatted output
	std::cout << "% --------------------------- " << std::endl;
	std::cout << "% --------------------------- " << std::endl << std::endl;
	std::cout << "A=" << A << ";" << std::endl << std::endl;

	// creates a four-dimensional tensor with extents 5,4,3 and 2
	// tensor A stores complex floating-point extended double precision numbers
	// according to the last-order storage format
	// and initializes it with the default value.
	using ctype = std::complex<cpp_bin_float_double_extended>;
	auto B = tensor<ctype,last_order>(shape{5,4,3,2},ctype{});

	// initializes the tensor with increasing values along the last-index
	// using a single-index
	auto vc = ctype(0,0);
	for(auto i = 0u; i < B.size(); ++i, vc += ctype(1,1))
		B[i] = vc;

	// formatted output
	std::cout << "% --------------------------- " << std::endl;
	std::cout << "% --------------------------- " << std::endl << std::endl;
	std::cout << "B=" << B << ";" << std::endl << std::endl;

	auto C = tensor<ctype,last_order>(B.extents());
	// computes the complex conjugate of elements of B
	// using multi-index notation.
	for(auto i = 0u; i < B.size(0); ++i)
		for(auto j = 0u; j < B.size(1); ++j)
			for(auto k = 0u; k < B.size(2); ++k)
				for(auto l = 0u; l < B.size(3); ++l)
					C.at(i,j,k,l) = std::conj(B.at(i,j,k,l));

	std::cout << "% --------------------------- " << std::endl;
	std::cout << "% --------------------------- " << std::endl << std::endl;
	std::cout << "C=" << C << ";" << std::endl << std::endl;

	// computes the complex conjugate of elements of B
	// using iterators.
	auto D = tensor<ctype,last_order>(B.extents());
	std::transform(B.begin(), B.end(), D.begin(), [](auto const& b){ return std::conj(b); });
	std::cout << "% --------------------------- " << std::endl;
	std::cout << "% --------------------------- " << std::endl << std::endl;
	std::cout << "D=" << D << ";" << std::endl << std::endl;

	// reshaping tensors.
	auto new_extents = B.extents().base();
	std::next_permutation( new_extents.begin(), new_extents.end() );
	D.reshape( shape(new_extents)  );
	std::cout << "% --------------------------- " << std::endl;
	std::cout << "% --------------------------- " << std::endl << std::endl;
	std::cout << "newD=" << D << ";" << std::endl << std::endl;
}


/*
 * Copyright (c) 2016 Abhishek Agrawal (abhishek.agrawal@protonmail.com)
 * Distributed under the MIT License.
 * See accompanying file LICENSE.md or copy at http://opensource.org/licenses/MIT
 */

#include <iostream>
#include <sstream>
#include <stdexcept>
#include <string>
#include <vector>
#include <fstream>
#include <exception>
#include <cstdlib>
#include <execinfo.h>

#include <boost/exception/info.hpp>

#include <gsl/gsl_multiroots.h>
#include <gsl/gsl_vector.h>

#include <libsgp4/DateTime.h>
#include <libsgp4/Eci.h>
#include <libsgp4/Globals.h>
#include <libsgp4/SGP4.h>
#include <libsgp4/Tle.h>

#include <Astro/astro.hpp>
#include <SML/sml.hpp>
#include <SML/constants.hpp>
#include <SML/basicFunctions.hpp>

#include <Atom/printFunctions.hpp>

#include <Astro/orbitalElementConversions.hpp>
#include <Atom/convertCartesianStateToTwoLineElements.hpp>

#include "/media/abhishek/work/TU delft/INTERNSHIP at DINAMICA/work/github/pykep/src/core_functions/par2ic.h"

#include "CppProject/KepToCartToTLE.hpp"

namespace KepToCartToTLE{

	typedef double Real;
	typedef std::vector< Real > Vector6;
	typedef std::vector< Real > Vector3;
	typedef std::vector< Real > Vector2;
	typedef std::vector < std::vector < Real > > Vector2D;

	void KepToCartToTLE( int newLimit, Vector2D randKepElem )
	{
		// grav. parameter 'mu' of earth
    	const double muEarth = kMU*( pow( 10, 9 ) ); // unit m^3/s^2
	    // generate sets of cartesian elements corresponding to each of the psuedo random orbital element set.
	    // The conversion from keplerian elements to the cartesian elements is done using the PyKep library from ESA.
	    Vector2D CartPos( newLimit, std::vector< Real >( 3 ) ); // empty 2D vector to store position coordinates
	    Vector2D CartVel( newLimit, std::vector< Real >( 3 ) ); // empty 2D vector to store velocity components
	    Vector6 tempKep( 6 ); // temporary storage vector to store a given set of keplerian elements
	    Vector3 tempPos( 3 ); // temp storage vector for cartesian position
	    Vector3 tempVel( 3 ); // temp storage vector for cartesian velocity
	    Real rangeMag = 0; // magnitude range
	    Real velocityMag = 0; // velocity magnitude
	    for(int k = 0; k < newLimit; k++)
	    {
	        tempKep = randKepElem[ k ]; // transferring an entire row of the 2D vector which contains a single set of orbital elements
	        kep_toolbox::par2ic( tempKep, muEarth, tempPos, tempVel );
	        CartPos[ k ] = tempPos; // storing the output position coordinates in a row of the final 2D vector
	        CartVel[ k ] = tempVel; // same as above comment but this time for velocity
	    }
	    // verification of the conversion process using Ron Noomen's lecture notes from TUDelft
	    // Document ID ae4878.basics.v4-16.pdf
	    Vector6 testKep = { 6787746.891, 0.000731104, sml::convertDegreesToRadians( 51.68714486 ),
	        sml::convertDegreesToRadians( 127.5486706 ), sml::convertDegreesToRadians( 74.21987137 ), sml::convertDegreesToRadians( 24.08317766 ) };
	    Vector3 testPos( 3 );
	    Vector3 testVel( 3 );
	    kep_toolbox::par2ic( testKep, muEarth, testPos, testVel );
	    std::cout << testPos[ 0 ] << std::endl;
	    std::cout << testPos[ 1 ] << std::endl;
	    std::cout << testPos[ 2 ] << std::endl;
	    std::cout << testVel[ 0 ] << std::endl;
	    std::cout << testVel[ 1 ] << std::endl;
	    std::cout << testVel[ 2 ] << std::endl;

	    //store randomly generated keplerian element sets into a CSV file for easy viewing and use in future debugging of ATOM
	    std::ofstream RandomKepElemFile;
	    RandomKepElemFile.open("RandomKepElemFile_deletedScenes.csv"); //file will be overwritten each time the code is run unless the name is changed here and the code recompiled
	    RandomKepElemFile << "semi-major axis [km]" << "," << "eccentricity" << ",";
	    RandomKepElemFile << "Inclination [deg]" << "," << "RAAN [deg]" << ",";
	    RandomKepElemFile << "AOP [deg]" << "," << "Eccentric Anomaly [deg]" << std::endl;
	    for(int i = 0; i < newLimit; i++)
	    {
	        RandomKepElemFile << ( randKepElem[ i ][ 0 ]/1000 ) << ",";
	        RandomKepElemFile << randKepElem[ i ][ 1 ] << ",";
	        RandomKepElemFile << sml::convertRadiansToDegrees( randKepElem[ i ][ 2 ] ) << ",";
	        RandomKepElemFile << sml::convertRadiansToDegrees( randKepElem[ i ][ 3 ] ) << ",";
	        RandomKepElemFile << sml::convertRadiansToDegrees( randKepElem[ i ][ 4 ] ) << ",";
	        RandomKepElemFile << sml::convertRadiansToDegrees( randKepElem[ i ][ 5 ] ) << std::endl;
	    }
	    RandomKepElemFile.close();

	    //store the converted cartesian elements in a CSV file
	    std::ofstream RandomCartesianFile;
	    RandomCartesianFile.open("RandomCartesianFile_deletedScenes.csv");
	    RandomCartesianFile << "X [km]" << "," << "Y [km]" << "," << "Z [km]" << "," << "Range [km]" << ",";
	    RandomCartesianFile << "Vx [km/s]" << "," << "Vy [km/s]" << "," << "Vz [km/s]" << "," << "Velocity [km/s]" << std::endl;
	    for(int j = 0; j < newLimit; j++)
	    {
	        RandomCartesianFile << ( CartPos[ j ][ 0 ]/1000 ) << ",";
	        RandomCartesianFile << ( CartPos[ j ][ 1 ]/1000 ) << ",";
	        RandomCartesianFile << ( CartPos[ j ][ 2 ]/1000 ) << ",";
	        rangeMag = sqrt( pow( CartPos[ j ][ 0 ], 2 ) + pow( CartPos[ j ][ 1 ], 2 ) + pow( CartPos[ j ][ 2 ], 2 ) );
	        RandomCartesianFile << ( rangeMag/1000 ) << ",";

	        RandomCartesianFile << ( CartVel[ j ][ 0 ]/1000 ) << ",";
	        RandomCartesianFile << ( CartVel[ j ][ 1 ]/1000 ) << ",";
	        RandomCartesianFile << ( CartVel[ j ][ 2 ]/1000 ) << ",";
	        velocityMag = sqrt( pow( CartVel[ j ][ 0 ], 2 ) + pow( CartVel[ j ][ 1 ], 2 ) + pow( CartVel[ j ][ 2 ], 2 ) );
	        RandomCartesianFile << ( velocityMag/1000 ) << std::endl;
	    }
	    RandomCartesianFile.close();

	    // convert the cartesian elements to the corresponding TLE format using the ATOM toolbox
	    Vector6 cartesianState( 6 );
	    // cartesianState[ 0 ] = -7.1e3;
	    // cartesianState[ 1 ] = 2.7e3;
	    // cartesianState[ 2 ] = 1.3e3;
	    // cartesianState[ 3 ] = -2.5;
	    // cartesianState[ 4 ] = -5.5;
	    // cartesianState[ 5 ] = 5.5;
	    Tle convertedTle;
	    Tle referenceTle = Tle(); // empty TLE for reference
	    // std::cout << referenceTle << std::endl << std::endl;
	    std::string SolverStatus;
	    const Real absTol = 1.0e-10; // absolute tolerance
	    const Real relTol = 1.0e-5; // relative tolerance
	    const int maxItr = 100; // maximum allowed iterations per conversion run
	    // some other bookkeeping variables, these are not used in the convert to tle function
	    std::size_t findSuccess;
	    // file storage
	    std::ofstream tlefile;
	    tlefile.open("TLEfile.csv");
	    tlefile << "Conversion Status" << "," << "Iteration Count" << "," << "," << "Converted TLE" << "," << "Failure/Success Index" << std::endl;

	    for(int i = 0; i < newLimit; i++)
	    {
	        // std::cout << "loop count = " << i << std::endl;
	        int IterationCount = 0; // counter for total iterations undertaken in a given instance of cartesian to TLE conversion
	        // important note, the atom function converting cartesian to TLEs takes in values in km and km/s.
	        cartesianState[ 0 ] = CartPos[ i ][ 0 ]/1000;
	        cartesianState[ 1 ] = CartPos[ i ][ 1 ]/1000;
	        cartesianState[ 2 ] = CartPos[ i ][ 2 ]/1000;
	        cartesianState[ 3 ] = CartVel[ i ][ 0 ]/1000;
	        cartesianState[ 4 ] = CartVel[ i ][ 1 ]/1000;
	        cartesianState[ 5 ] = CartVel[ i ][ 2 ]/1000;
	        convertedTle = atom::convertCartesianStateToTwoLineElements< Real, Vector6 >( cartesianState, DateTime( ), SolverStatus,
	            IterationCount, referenceTle, kMU, kXKMPER, absTol, relTol, maxItr );
	        findSuccess = SolverStatus.find("success");
	        if(findSuccess == std::string::npos)
	        {
	            std::cout << "Cartesian to TLE Conversion Failed" << std::endl;
	            tlefile << "Failure" << ",";
	            tlefile << "," << "," << "," << "," << "," << i+2 << std::endl;

	        }
	        else
	        {
	            tlefile << "Success" << ",";
	            tlefile << IterationCount << "," << ",";
	            tlefile << "Epoch = " << convertedTle.Epoch() << "," << i+2 << std::endl;
	            tlefile << "," << "," << "," << "Mean motion Dt2 = " << convertedTle.MeanMotionDt2() << std::endl;
	            tlefile << "," << "," << "," << "Mean motion Ddt6 = " << convertedTle.MeanMotionDdt6() << std::endl;
	            tlefile << "," << "," << "," << "B(*) = " << convertedTle.BStar() << std::endl;
	            tlefile << "," << "," << "," << "Inclination = " << convertedTle.Inclination(1) << std::endl;
	            tlefile << "," << "," << "," << "RAAN = " << convertedTle.RightAscendingNode(1) << std::endl;
	            tlefile << "," << "," << "," << "Eccentricity = " << convertedTle.Eccentricity() << std::endl;
	            tlefile << "," << "," << "," << "AOP = " << convertedTle.ArgumentPerigee(1) << std::endl;
	            tlefile << "," << "," << "," << "Mean Anomaly = " << convertedTle.MeanAnomaly(1) << std::endl;
	            tlefile << "," << "," << "," << "Mean motion = " << convertedTle.MeanMotion() << std::endl << std::endl;
	        }
	    }
	    tlefile.close();
	}
}


/*! \file 2d_full.cpp
  \brief 2D plot of functions from map of doubles showing use of more options.
  \author Jacob Voytko
  \date 2007
*/

// Copyright Jacob Voytko 2007
// Distributed under the Boost Software License, Version 1.0.
// For more information, see http://www.boost.org

#include <boost/svg_plot/svg_2d_plot.hpp>
using namespace boost::svg;

#include <map>
using std::map;
#include <cmath>
using ::sqrt;

// Some functions to plot.
double f(double x)
{
  return sqrt(x);
}

double g(double x)
{
  return -2 + x*x;
}

double h(double x)
{
  return -1 + 2*x;
}

int main()
{
  map<double, double> data1, data2, data3;

  for(double i=0; i<=10.; i+=1.)
  {
    data1[i] = f(i);
    data2[i] = g(i);
    data3[i] = h(i);
  }

  svg_2d_plot my_plot;

  // Size/scale settings.
  my_plot.size(700, 500)
         .x_range(-1, 10)
         .y_range(-5, 100);

  // Text settings.
  my_plot.title("Plot of Mathematical Functions")
         .title_font_size(29)
         .x_label("X Axis Units");

  // Commands.
  my_plot.legend_on(true)
         .plot_window_on(true)
         .x_label_on(true)
         .x_major_labels_side(true);

  // Color settings.
  my_plot.background_color(svg_color(67, 111, 69))
         .legend_background_color(svg_color(207, 202,167))
         .legend_border_color(svg_color(102, 102, 84))
         .plot_background_color(svg_color(136, 188, 126))
         .title_color(white);

  //X axis settings.
  my_plot.x_major_interval(2)
         .x_major_tick_length(14)
         .x_major_tick_width(1)
         .x_minor_tick_length(7)
         .x_minor_tick_width(1)
         .x_num_minor_ticks(3)

  //Y axis settings.
         .y_major_interval(10)
         .y_num_minor_ticks(3);

  //legend settings
  my_plot.legend_title_font_size(15);

  my_plot.plot(data1, "Sqrt(x)").stroke_color(red);
  my_plot.plot(data2, "-2 + x^2").stroke_color(orange);
  my_plot.plot(data3, "-1 + 2x").stroke_color(yellow).shape(square).size(5);

  my_plot.write("./2d_full.svg");

  return 0;
} // int main()


/*
 * This file is part of bogus, a C++ sparse block matrix library.
 *
 * Copyright 2013 Gilles Daviet <gdaviet@gmail.com>
 *
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/

/*! \file
	Necessary bindings to use Eigen matrices as block types, and
	\c operator* specialization for matrix/vector products
*/

#ifndef BLOCK_EIGENBINDINGS_HPP
#define BLOCK_EIGENBINDINGS_HPP

#include <Eigen/Core>

#ifndef BOGUS_BLOCK_WITHOUT_EIGEN_SPARSE
#include "SparseHeader.hpp"
#endif

#include "../Block/BlockMatrixBase.hpp"
#include "../Block/Expressions.hpp"

#ifndef BOGUS_BLOCK_WITHOUT_LINEAR_SOLVERS
#include "EigenLinearSolvers.hpp"
#ifndef BOGUS_BLOCK_WITHOUT_EIGEN_SPARSE
#include "EigenSparseLinearSolvers.hpp"
#endif
#endif

#include "../Utils/CppTools.hpp"

#define BOGUS_EIGEN_NEW_EXPRESSIONS EIGEN_VERSION_AT_LEAST(3,2,90)

namespace bogus
{

// transpose_block, is_zero, resize, set_identity

template< typename EigenDerived >
inline bool is_zero ( const Eigen::MatrixBase< EigenDerived >& block,
                 typename EigenDerived::Scalar precision )
{
	return block.isZero( precision ) ;
}

template< typename EigenDerived >
inline void set_zero ( Eigen::MatrixBase< EigenDerived >& block )
{
	block.derived().setZero( ) ;
}

template< typename EigenDerived >
inline void set_identity ( Eigen::MatrixBase< EigenDerived >& block )
{
	block.derived().setIdentity( ) ;
}

template< typename EigenDerived >
inline void resize ( Eigen::MatrixBase< EigenDerived >& block, int rows, int cols )
{
	block.derived().resize( rows, cols ) ;
}

template< typename EigenDerived >
inline const typename EigenDerived::Scalar* data_pointer ( const Eigen::MatrixBase< EigenDerived >& block )
{
	return block.derived().data() ;
}

#ifndef BOGUS_BLOCK_WITHOUT_EIGEN_SPARSE

#if !BOGUS_EIGEN_NEW_EXPRESSIONS
template < typename BlockT >
struct BlockTransposeTraits< Eigen::SparseMatrixBase < BlockT > > {
	typedef const Eigen::Transpose< const BlockT > ReturnType ;
} ;
template<typename _Scalar, int _Flags, typename _Index>
struct BlockTransposeTraits< Eigen::SparseMatrix < _Scalar, _Flags, _Index > >
        : public BlockTransposeTraits< Eigen::SparseMatrixBase< Eigen::SparseMatrix < _Scalar, _Flags, _Index > > >
{} ;
template<typename _Scalar, int _Flags, typename _Index>
struct BlockTransposeTraits< Eigen::SparseVector < _Scalar, _Flags, _Index > >
        : public BlockTransposeTraits< Eigen::SparseMatrixBase< Eigen::SparseVector < _Scalar, _Flags, _Index > > >
{} ;
template<typename _Scalar, int _Flags, typename _Index>
struct BlockTransposeTraits< Eigen::MappedSparseMatrix < _Scalar, _Flags, _Index > >
        : public BlockTransposeTraits< Eigen::SparseMatrixBase< Eigen::MappedSparseMatrix < _Scalar, _Flags, _Index > > >
{} ;

template< typename EigenDerived >
inline const Eigen::Transpose< const EigenDerived >
transpose_block( const Eigen::SparseMatrixBase< EigenDerived >& block )
{
	return block.transpose() ;
}
#endif

template< typename EigenDerived >
inline bool is_zero ( const Eigen::SparseMatrixBase< EigenDerived >& block,
                 typename EigenDerived::Scalar precision )
{
	return block.isZero( precision ) ;
}

template < typename Scalar, int Options, typename Index >
inline void set_identity ( Eigen::SparseMatrix< Scalar, Options, Index >& block )
{
	return block.setIdentity( ) ;
}

template < typename Scalar, int Options, typename Index >
inline void resize ( Eigen::SparseMatrix< Scalar, Options, Index >& block, Index rows, Index cols )
{
	block.resize( rows, cols ) ;
}

#endif

// Block traits for Eigen::Matrix

template< typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols >
struct BlockTraits < Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> >
{
	typedef _Scalar Scalar ;
	enum {
		RowsAtCompileTime = _Rows,
		ColsAtCompileTime = _Cols,
		uses_plain_array_storage = 1,
		is_row_major = !!( _Options & Eigen::RowMajor ),
		is_self_transpose = ( _Rows == _Cols ) && ( _Rows == 1 )
	} ;

	// Manipulates _Options so that row and column vectorz have the correct RowMajor value
	// Should we default to _Options ^ Eigen::RowMajor ?
	typedef Eigen::Matrix< _Scalar, _Cols, _Rows,
	( _Options | ((_Cols==1&&_Rows!=1)?Eigen::RowMajor:0)) & ~((_Rows==1&&_Cols!=1)?Eigen::RowMajor:0),
	_MaxCols, _MaxRows >
	TransposeStorageType ;

} ;

// Block/block product return type

template<
    typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols,
    typename _Scalar2, int _Rows2, int _Cols2, int _Options2, int _MaxRows2, int _MaxCols2,
    bool TransposeLhs, bool TransposeRhs >
struct BlockBlockProductTraits <
         Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>,
         Eigen::Matrix<_Scalar2, _Rows2, _Cols2, _Options2, _MaxRows2, _MaxCols2>,
        TransposeLhs, TransposeRhs >
{
	typedef Eigen::Matrix< _Scalar,
	    SwapIf< TransposeLhs, _Rows, _Cols >::First,
	    SwapIf< TransposeRhs, _Rows2, _Cols2 >::Second,
	    _Options,
	    SwapIf< TransposeLhs, _MaxRows, _MaxCols >::First,
	    SwapIf< TransposeRhs, _MaxRows2, _MaxCols2 >::Second >
	ReturnType ;
} ;

template< typename Derived >
inline typename Eigen::internal::plain_matrix_type<Derived>::type
get_mutable_vector( const Eigen::MatrixBase< Derived > & )
{
	return typename Eigen::internal::plain_matrix_type<Derived>::type() ;
}

} //ns bogus

// Matrix vector product return types and operator*

namespace bogus {
namespace mv_impl {

//! Wrapper so our SparseBlockMatrix can be used inside Eigen expressions
template< typename Derived >
struct EigenBlockWrapper : public Eigen::EigenBase< EigenBlockWrapper< Derived > >
{
	typedef EigenBlockWrapper Nested ;
	typedef EigenBlockWrapper NestedExpression ;
	typedef EigenBlockWrapper PlainObject ;
	typedef Eigen::internal::traits< EigenBlockWrapper< Derived > > Traits ;
	typedef typename Traits::Scalar Scalar ;
	typedef typename Traits::Index Index ;

	enum { Flags = Traits::Flags, IsVectorAtCompileTime = 0,
	MaxRowsAtCompileTime = Traits::MaxRowsAtCompileTime,
	MaxColsAtCompileTime = Traits::MaxColsAtCompileTime
	     } ;

	EigenBlockWrapper ( const Derived &obj_, Scalar s = 1 )
	    : obj( obj_ ), scaling(s)
	{}

	Index rows() const { return obj.rows() ; }
	Index cols() const { return obj.cols() ; }

	// Mult by scalar

	inline EigenBlockWrapper
	operator*(const Scalar& scalar) const
	{ return EigenBlockWrapper( obj, scaling * scalar ) ; }

	inline friend EigenBlockWrapper
	operator*(const Scalar& scalar, const EigenBlockWrapper& matrix)
	{ return EigenBlockWrapper( matrix.obj, matrix.scaling * scalar ) ; }

	// Product with other Eigen expr
#if BOGUS_EIGEN_NEW_EXPRESSIONS
	template < typename EigenDerived >
	inline Eigen::Product< EigenBlockWrapper, EigenDerived > operator* (
	    const EigenDerived &rhs ) const
	{
		return Eigen::Product< EigenBlockWrapper, EigenDerived > (*this, rhs) ;
	}
	template < typename EigenDerived >
	friend inline Eigen::Product< EigenDerived, EigenBlockWrapper > operator* (
	    const EigenDerived &lhs, const EigenBlockWrapper &matrix )
	{
		return Eigen::Product< EigenDerived, EigenBlockWrapper > (lhs, matrix) ;
	}
#endif

	const Derived &obj ;
	const Scalar scaling ;
};

//! SparseBlockMatrix / Dense producty expression

template< typename Lhs, typename Rhs >
struct block_product_impl;

#if BOGUS_EIGEN_NEW_EXPRESSIONS
template < typename Lhs, typename Rhs >
struct BlockEigenProduct
        : public Eigen::Product< Lhs, Rhs >
{
	typedef Eigen::Product< Lhs, Rhs > Base ;

	BlockEigenProduct( const Lhs& lhs, const Rhs& rhs )
	    : Base( lhs, rhs)
	{}
} ;
#else

template < typename Lhs, typename Rhs >
struct BlockEigenProduct
        : public Eigen::ProductBase< BlockEigenProduct< Lhs, Rhs>, Lhs, Rhs >
{
	typedef Eigen::ProductBase< BlockEigenProduct, Lhs, Rhs > Base ;

	BlockEigenProduct( const Lhs& lhs, const Rhs& rhs )
	    : Base( lhs, rhs)
	{}

	EIGEN_DENSE_PUBLIC_INTERFACE( BlockEigenProduct )
	using Base::m_lhs;
	using Base::m_rhs;
	typedef block_product_impl< typename Base::LhsNested, typename Base::RhsNested > product_impl ;

	template<typename Dest> inline void evalTo(Dest& dst) const
	{
		product_impl::evalTo( dst, m_lhs, this->m_rhs ) ;
	}
	template<typename Dest> inline void scaleAndAddTo(Dest& dst, Scalar alpha) const
	{
		product_impl::scaleAndAddTo( dst, m_lhs, m_rhs, alpha ) ;
	}
};
#endif

template< typename Derived, typename EigenDerived >
BlockEigenProduct< EigenBlockWrapper< Derived >, EigenDerived >  block_eigen_product(
        const Derived &matrix, const EigenDerived &vector, typename Derived::Scalar scaling = 1 )
{
	return BlockEigenProduct< EigenBlockWrapper< Derived >, EigenDerived > ( EigenBlockWrapper< Derived >(matrix, scaling), vector ) ;
}

template< typename Derived, typename EigenDerived >
BlockEigenProduct< EigenDerived, EigenBlockWrapper< Derived > >  eigen_block_product(
        const EigenDerived &vector, const Derived &matrix, typename Derived::Scalar scaling = 1 )
{
	return BlockEigenProduct< EigenDerived, EigenBlockWrapper< Derived > > ( vector, EigenBlockWrapper< Derived >(matrix, scaling) ) ;
}

template<typename Derived, typename EigenDerived >
struct block_product_impl< bogus::mv_impl::EigenBlockWrapper<Derived>, EigenDerived >
{
	typedef bogus::mv_impl::EigenBlockWrapper<Derived> Lhs ;
	typedef EigenDerived Rhs ;
	typedef typename Derived::Scalar Scalar;

	template<typename Dst>
	static void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
	{
		lhs.obj.template multiply< false >( rhs.derived(), dst, lhs.scaling, 0 ) ;
	}
	template<typename Dst>
	static void scaleAndAddTo(Dst& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha)
	{
		lhs.obj.template multiply< false >( rhs.derived(), dst, alpha*lhs.scaling, 1 ) ;
	}
};

template<typename Derived, typename EigenDerived >
        struct block_product_impl< EigenDerived, bogus::mv_impl::EigenBlockWrapper<Derived> >
{
	typedef EigenDerived Lhs ;
	typedef bogus::mv_impl::EigenBlockWrapper<Derived> Rhs ;
	typedef typename Derived::Scalar Scalar;

	template<typename Dst>
	static void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
	{
		Eigen::Transpose< Dst > transposed( dst.transpose() ) ;
		rhs.obj.template multiply< true >( lhs.transpose(), transposed, rhs.scaling, 0 ) ;
	}
	template<typename Dst>
	static void scaleAndAddTo(Dst& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha)
	{
		Eigen::Transpose< Dst > transposed( dst.transpose() ) ;
		rhs.obj.template multiply< true >( lhs.transpose(), transposed, alpha * rhs.scaling, 1 ) ;
	}
};

} //namespace mv_impl
} //namespace bogus

#if BOGUS_EIGEN_NEW_EXPRESSIONS
namespace Eigen {
namespace internal {

template<typename Derived, typename EigenDerived, int ProductType >
struct generic_product_impl< bogus::mv_impl::EigenBlockWrapper<Derived>, EigenDerived, SparseShape, DenseShape, ProductType>
        : public generic_product_impl_base <
            bogus::mv_impl::EigenBlockWrapper<Derived>, EigenDerived,
            generic_product_impl< bogus::mv_impl::EigenBlockWrapper<Derived>, EigenDerived, SparseShape, DenseShape, ProductType > >
{
	typedef bogus::mv_impl::EigenBlockWrapper<Derived> Lhs ;
	typedef EigenDerived Rhs ;
	typedef typename Derived::Scalar Scalar;

	typedef typename nested_eval<Rhs,Dynamic>::type RhsNested;
	typedef bogus::mv_impl::block_product_impl< Lhs, RhsNested > product_impl ;

	template<typename Dst>
	static void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
	{
		RhsNested rhsNested( rhs ) ;
		product_impl::evalTo( dst, lhs, rhsNested ) ;
	}
	template<typename Dst>
	static void scaleAndAddTo(Dst& dst, const Lhs& lhs, const Rhs& rhs, Scalar alpha)
	{
		RhsNested rhsNested( rhs ) ;
		product_impl::scaleAndAddTo( dst, lhs, rhsNested, alpha ) ;
	}
};

template<typename Derived, typename EigenDerived, int ProductType >
struct generic_product_impl< EigenDerived, bogus::mv_impl::EigenBlockWrapper<Derived>, DenseShape, SparseShape, ProductType >
        : public generic_product_impl_base <
            EigenDerived, bogus::mv_impl::EigenBlockWrapper<Derived>,
            generic_product_impl< EigenDerived, bogus::mv_impl::EigenBlockWrapper<Derived>, DenseShape, SparseShape, ProductType > >
{
	typedef EigenDerived Lhs ;
	typedef bogus::mv_impl::EigenBlockWrapper<Derived> Rhs ;
	typedef typename Derived::Scalar Scalar;

	typedef typename nested_eval<Lhs,Dynamic>::type LhsNested;
	typedef bogus::mv_impl::block_product_impl< LhsNested, Rhs > product_impl ;

	template<typename Dst>
	static void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
	{
		LhsNested lhsNested(lhs);
		product_impl::evalTo( dst, lhsNested, rhs) ;
	}
	template<typename Dst>
	static void scaleAndAddTo(Dst& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha)
	{
		LhsNested lhsNested(lhs);
		product_impl::scaleAndAddTo( dst, lhsNested, rhs, alpha ) ;
	}
};

// s * (A * V) -> ( (s*A) * V )
// (A already includes a scaling parameter)
// TODO adapt to other orderings
template<typename Derived, typename Rhs, typename Scalar1, typename Scalar2, typename Plain1>
struct evaluator<CwiseBinaryOp<internal::scalar_product_op<Scalar1,Scalar2>,
                               const CwiseNullaryOp<internal::scalar_constant_op<Scalar1>, Plain1>,
                               const Product<bogus::mv_impl::EigenBlockWrapper<Derived>, Rhs, DefaultProduct> > >
 : public evaluator<Product<bogus::mv_impl::EigenBlockWrapper<Derived>, Rhs, DefaultProduct> >
{
  typedef CwiseBinaryOp<internal::scalar_product_op<Scalar1,Scalar2>,
	                           const CwiseNullaryOp<internal::scalar_constant_op<Scalar1>, Plain1>,
	                           const Product<bogus::mv_impl::EigenBlockWrapper<Derived>, Rhs, DefaultProduct> > XprType;
  typedef evaluator<Product<bogus::mv_impl::EigenBlockWrapper<Derived>, Rhs, DefaultProduct> > Base;

  explicit evaluator(const XprType& xpr)
	: Base( bogus::mv_impl::EigenBlockWrapper<Derived>(
	            xpr.rhs().lhs().obj, xpr.lhs().functor().m_other * xpr.rhs().lhs().scaling)
	        * xpr.rhs().rhs() )
   {}

 };

} //internal
} //Eigen

#endif

// Eigen traits for our new structs
namespace Eigen{
namespace internal {

template < typename Lhs, typename Rhs >
struct traits< bogus::mv_impl::BlockEigenProduct< Lhs, Rhs > >
#if BOGUS_EIGEN_NEW_EXPRESSIONS
        : public traits< typename bogus::mv_impl::BlockEigenProduct< Lhs, Rhs >::Base >
        #else
        : public traits< ProductBase< bogus::mv_impl::BlockEigenProduct< Lhs, Rhs >, Lhs, Rhs > >
        #endif
{
  typedef Dense StorageKind;
} ;

template<typename Derived>
struct traits<bogus::mv_impl::EigenBlockWrapper<Derived> >
{
  typedef typename Derived::Scalar Scalar;
  typedef typename Derived::Index Index;
  typedef typename Derived::Index StorageIndex;
#if BOGUS_EIGEN_NEW_EXPRESSIONS
  typedef Sparse StorageKind;
#else
  typedef Dense StorageKind;
#endif
  typedef MatrixXpr XprKind;
  enum {
	RowsAtCompileTime = Dynamic,
	ColsAtCompileTime = Dynamic,
	MaxRowsAtCompileTime = Dynamic,
	MaxColsAtCompileTime = Dynamic,
	Flags = 0
  };
};
}

} //namespace Eigen

// Matrix-Vector operators

namespace bogus{

template < typename Derived, typename EigenDerived >
mv_impl::BlockEigenProduct< mv_impl::EigenBlockWrapper<Derived>, EigenDerived > operator* (
        const BlockObjectBase< Derived >& lhs,
        const Eigen::MatrixBase< EigenDerived > &rhs )
{
	assert( rhs.rows() == lhs.cols() ) ;
	return mv_impl::block_eigen_product ( lhs.derived(), rhs.derived() ) ;
}

template < typename Derived, typename EigenDerived >
mv_impl::BlockEigenProduct< mv_impl::EigenBlockWrapper<Derived>, EigenDerived > operator* (
        const Scaling< Derived >& lhs,
        const Eigen::MatrixBase< EigenDerived > &rhs )
{
	assert( rhs.rows() == lhs.cols() ) ;
	return mv_impl::block_eigen_product ( lhs.operand.object, rhs.derived(), lhs.operand.scaling ) ;
}

template < typename Derived, typename EigenDerived >
mv_impl::BlockEigenProduct< EigenDerived, mv_impl::EigenBlockWrapper<Derived> > operator* (
        const Eigen::MatrixBase< EigenDerived > &lhs,
        const BlockObjectBase< Derived >& rhs )
{
	assert( lhs.cols() == rhs.rows() ) ;
	return mv_impl::eigen_block_product ( lhs.derived(), rhs.derived() ) ;
}

template < typename Derived, typename EigenDerived >
mv_impl::BlockEigenProduct< EigenDerived, mv_impl::EigenBlockWrapper<Derived> > operator* (
        const Eigen::MatrixBase< EigenDerived > &lhs,
        const Scaling< Derived >& rhs )
{
	assert( lhs.cols() == rhs.rows() ) ;
	return mv_impl::eigen_block_product ( lhs.derived(), rhs.operand.object, rhs.operand.scaling ) ;
}

} //namespace bogus

#endif // EIGENBINDINGS_HPP


#include "ear/decorrelate.hpp"

#include <boost/math/constants/constants.hpp>
#include <cmath>
#include <complex>
#include <random>
#include "ear/layout.hpp"
#include "kissfft/kissfft.hh"

const double PI = boost::math::constants::pi<double>();

namespace ear {

  std::vector<long> genRandMt19937(int seed, int n) {
    std::mt19937 mtRand(seed);
    std::vector<long> ret;
    for (int i = 0; i < n; ++i) {
      ret.push_back(mtRand());
    }
    return ret;
  }

  std::vector<double> genRandFloat(int seed, int n) {
    std::vector<double> ret;
    for (long randomValue : genRandMt19937(seed, n)) {
      ret.push_back(randomValue / static_cast<double>(0x100000000l));
    }
    return ret;
  }

  /** @brief Design an all-pass random-phase FIR filter.
   *
   * @param decorrelator_id Random seed, to obtain different filters.
   * @param size filter length.
   *
   * @return  Filter coefficients.
   */
  std::vector<double> designDecorrelatorBasic(int decorrelatorId, int size) {
    std::vector<double> rand = genRandFloat(decorrelatorId, size / 2 - 1);
    std::vector<std::complex<double>> freqDomainData(size);
    freqDomainData[0] = std::complex<double>(1.0, 0.0);
    for (size_t i = 0; i < rand.size(); ++i) {
      freqDomainData[i + 1] =
          std::exp(std::complex<double>(0.0, 2.0 * PI * rand[i]));
    }
    freqDomainData[size / 2] = std::complex<double>(1.0, 0.0);
    for (size_t i = 0; i < freqDomainData.size() / 2; ++i) {
      freqDomainData[size / 2 + i] = std::conj(freqDomainData[size / 2 - i]);
    }
    kissfft<double> fft(size, true);
    std::vector<std::complex<double>> timeDomainData(size);
    fft.transform(&freqDomainData[0], &timeDomainData[0]);
    std::vector<double> timeDomainDataReal(size);
    for (size_t i = 0; i < timeDomainData.size(); ++i) {
      timeDomainDataReal[i] = timeDomainData[i].real() / size;
    }
    return timeDomainDataReal;
  }

  const int decorrelator_size = 512;

  template <>
  std::vector<std::vector<double>> designDecorrelators<double>(Layout layout) {
    std::vector<std::string> channelNames = layout.channelNames();
    std::vector<std::string> channelNamesSorted(channelNames);
    std::sort(channelNamesSorted.begin(), channelNamesSorted.end());
    std::vector<std::vector<double>> decorrelators;
    for (auto channelName : channelNames) {
      auto it =
          std::find_if(channelNamesSorted.begin(), channelNamesSorted.end(),
                       [&channelName](const std::string name) -> bool {
                         return channelName == name;
                       });
      int index =
          static_cast<int>(std::distance(channelNamesSorted.begin(), it));
      std::vector<double> coefficients =
          designDecorrelatorBasic(index, decorrelator_size);
      decorrelators.push_back(coefficients);
    }
    return decorrelators;
  }

  template <>
  EAR_EXPORT std::vector<std::vector<float>> designDecorrelators<float>(
      Layout layout) {
    auto decorrelators = designDecorrelators<double>(layout);
    std::vector<std::vector<float>> decorrelators_float;

    for (auto &decorrelator : decorrelators) {
      std::vector<float> decorrelator_float(decorrelator.size());

      for (size_t i = 0; i < decorrelator.size(); i++)
        decorrelator_float[i] = (float)decorrelator[i];

      decorrelators_float.emplace_back(std::move(decorrelator_float));
    }

    return decorrelators_float;
  }

  int decorrelatorCompensationDelay() { return (decorrelator_size - 1) / 2; }
}  // namespace ear


//          Copyright Gavin Band 2008 - 2012.
// 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 <Eigen/Core>
#include "genfile/Error.hpp"
#include "components/HaplotypeFrequencyComponent/HaplotypeFrequencyLogLikelihood.hpp"
#include "config/config.hpp"
#include "test_case.hpp"

using std::log ;

AUTO_TEST_CASE( test_haplotype_frequency_loglikelihood_value ) {
	typedef Eigen::MatrixXd Matrix ;
	typedef Eigen::VectorXd Vector ;

	double const tolerance = 0.0000000000001 ;

	Matrix table( 3, 3 ) ;

	{
		table <<
			1,	0,	0,
			0,	0,	0,
			0,	0,	0 ;

		HaplotypeFrequencyLogLikelihood ll( table ) ;

		Vector params( 3 ) ;
		params << 0.0, 0.0, 0.0 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), log( 1.0 ) ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_first_derivative(), Vector::Constant( 3, -2.0 ) ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_second_derivative(), -2.0 * Vector::Constant( 3, -1.0 ) * Vector::Constant( 3, -1.0 ).transpose() ) ;
		params << 0.2, 0.0, 0.0 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), 2.0 * log( 0.8 ) ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_first_derivative(), Vector::Constant( 3, -2.0 / 0.8 ) ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_second_derivative(), -( 2.0 / ( 0.8 * 0.8 )) * Vector::Constant( 3, -1.0 ) * Vector::Constant( 3, -1.0 ).transpose() ) ;
		params << 0.0, 0.2, 0.0 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), 2.0 * log( 0.8 ) ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_first_derivative(), Vector::Constant( 3, -2.0 / 0.8 ) ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_second_derivative(), -( 2.0 / ( 0.8 * 0.8 )) * Vector::Constant( 3, -1.0 ) * Vector::Constant( 3, -1.0 ).transpose() ) ;
		params << 0.0, 0.0, 0.2 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), 2.0 * log( 0.8 ) ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_first_derivative(), Vector::Constant( 3, -2.0 / 0.8 ) ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_second_derivative(), -( 2.0 / ( 0.8 * 0.8 )) * Vector::Constant( 3, -1.0 ) * Vector::Constant( 3, -1.0 ).transpose() ) ;
		params << 0.2, 0.2, 0.0 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), 2.0 * log( 0.6 ) ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_first_derivative(), Vector::Constant( 3, -2.0 / 0.6 ) ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_second_derivative(), -( 2.0 / ( 0.6 * 0.6 )) * Vector::Constant( 3, -1.0 ) * Vector::Constant( 3, -1.0 ).transpose() ) ;
		params << 0.2, 0.0, 0.2 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), 2.0 * log( 0.6 ) ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_first_derivative(), Vector::Constant( 3, -2.0 / 0.6 ) ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_second_derivative(), -( 2.0 / ( 0.6 * 0.6 )) * Vector::Constant( 3, -1.0 ) * Vector::Constant( 3, -1.0 ).transpose() ) ;
		params << 0.0, 0.2, 0.2 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), 2.0 * log( 0.6 ) ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_first_derivative(), Vector::Constant( 3, -2.0 / 0.6 ) ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_second_derivative(), -( 2.0 / ( 0.6 * 0.6 )) * Vector::Constant( 3, -1.0 ) * Vector::Constant( 3, -1.0 ).transpose() ) ;
		params << 0.2, 0.2, 0.2 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_CLOSE( ll.get_value_of_function(), 2.0 * log( 0.4 ), tolerance ) ;
		BOOST_CHECK_SMALL( ( ll.get_value_of_first_derivative() - Vector::Constant( 3, -2.0 / 0.4 ) ).array().abs().maxCoeff(), tolerance ) ;
		BOOST_CHECK_SMALL( ( ll.get_value_of_second_derivative() - ( -( 2.0 / ( 0.4 * 0.4 )) * Vector::Constant( 3, -1.0 ) * Vector::Constant( 3, -1.0 ).transpose() ) ).array().abs().maxCoeff(), tolerance ) ;
		params << 1.0, 0.0, 0.0 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), -std::numeric_limits< double >::infinity() ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_first_derivative(), Vector::Constant( 3, -std::numeric_limits< double >::infinity() ) ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_second_derivative(), -Matrix::Constant( 3, 3, std::numeric_limits< double >::infinity() ) ) ;
		params << 0.0, 1.0, 0.0 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), -std::numeric_limits< double >::infinity() ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_first_derivative(), Vector::Constant( 3, -std::numeric_limits< double >::infinity() ) ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_second_derivative(), -Matrix::Constant( 3, 3, std::numeric_limits< double >::infinity() ) ) ;
		params << 0.0, 0.0, 1.0 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), -std::numeric_limits< double >::infinity() ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_first_derivative(), Vector::Constant( 3, -std::numeric_limits< double >::infinity() ) ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_second_derivative(), -	Matrix::Constant( 3, 3, std::numeric_limits< double >::infinity() ) ) ;
	}

	{
		table <<
			0,	0,	0,
			0,	0,	0,
			0,	0,	1 ;

		HaplotypeFrequencyLogLikelihood ll( table ) ;

		Vector params( 3 ) ;
		params << 0.0, 0.0, 1.0 ;

		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), log( 1.0 ) ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_first_derivative(), 2.0 * Vector::Unit( 3, 2 ) ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_second_derivative(), -2.0 * Vector::Unit( 3, 2 ) * Vector::Unit( 3, 2 ).transpose() ) ;
		params << 0.0, 0.0, 0.8 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), 2.0 * log( 0.8 ) ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_first_derivative(), 2.0 * Vector::Unit( 3, 2 ) / 0.8 ) ;
		params << 0.2, 0.0, 0.8 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), 2.0 * log( 0.8 ) ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_first_derivative(), 2.0 * Vector::Unit( 3, 2 ) / 0.8 ) ;
		params << 0.0, 0.2, 0.8 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), 2.0 * log( 0.8 ) ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_first_derivative(), 2.0 * Vector::Unit( 3, 2 ) / 0.8 ) ;
		params << 0.2, 0.0, 0.6 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), 2.0 * log( 0.6 ) ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_first_derivative(), 2.0 * Vector::Unit( 3, 2 ) / 0.6 ) ;
		params << 0.0, 0.2, 0.6 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), 2.0 * log( 0.6 ) ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_first_derivative(), 2.0 * Vector::Unit( 3, 2 ) / 0.6 ) ;
		params << 0.2, 0.2, 0.6 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), 2.0 * log( 0.6 ) ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_first_derivative(), 2.0 * Vector::Unit( 3, 2 ) / 0.6 ) ;
		params << 0.2, 0.2, 0.4 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_CLOSE( ll.get_value_of_function(), 2.0 * log( 0.4 ), tolerance ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_first_derivative(), 2.0 * Vector::Unit( 3, 2 ) / 0.4 ) ;
		params << 0.0, 0.0, 0.0 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), -std::numeric_limits< double >::infinity() ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_first_derivative(), Vector::Constant( 3, -std::numeric_limits< double >::infinity() )) ;
		params << 1.0, 0.0, 0.0 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), -std::numeric_limits< double >::infinity() ) ;
		params << 0.0, 1.0, 0.0 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), -std::numeric_limits< double >::infinity() ) ;
	}

	{
		table <<
			0,	0,	1,
			0,	0,	0,
			0,	0,	0 ;

		HaplotypeFrequencyLogLikelihood ll( table ) ;

		Vector params( 3 ) ;
		params << 1.0, 0.0, 0.0 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), log( 1.0 ) ) ;
		params << 0.8, 0.0, 0.0 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), 2.0 * log( 0.8 ) ) ;
		params << 0.8, 0.2, 0.0 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), 2.0 * log( 0.8 ) ) ;
		params << 0.8, 0.0, 0.2 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), 2.0 * log( 0.8 ) ) ;
		params << 0.6, 0.2, 0.0 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), 2.0 * log( 0.6 ) ) ;
		params << 0.6, 0.0, 0.2 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), 2.0 * log( 0.6 ) ) ;
		params << 0.6, 0.2, 0.2 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), 2.0 * log( 0.6 ) ) ;
		params << 0.4, 0.2, 0.4 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_CLOSE( ll.get_value_of_function(), 2.0 * log( 0.4 ), tolerance ) ;
		params << 0.0, 0.0, 0.0 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), -std::numeric_limits< double >::infinity() ) ;
		params << 0.0, 1.0, 0.0 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), -std::numeric_limits< double >::infinity() ) ;
		params << 0.0, 0.0, 1.0 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), -std::numeric_limits< double >::infinity() ) ;
	}

	{
		table <<
			0,	0,	0,
			0,	0,	0,
			1,	0,	0 ;

		HaplotypeFrequencyLogLikelihood ll( table ) ;

		Vector params( 3 ) ;
		params << 0.0, 1.0, 0.0 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), log( 1.0 ) ) ;
		params << 0.0, 0.8, 0.0 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), 2.0 * log( 0.8 ) ) ;
		params << 0.2, 0.8, 0.0 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), 2.0 * log( 0.8 ) ) ;
		params << 0.0, 0.8, 0.2 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), 2.0 * log( 0.8 ) ) ;
		params << 0.2, 0.6, 0.0 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), 2.0 * log( 0.6 ) ) ;
		params << 0.0, 0.6, 0.2 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), 2.0 * log( 0.6 ) ) ;
		params << 0.2, 0.6, 0.2 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), 2.0 * log( 0.6 ) ) ;
		params << 0.4, 0.4, 0.2 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_CLOSE( ll.get_value_of_function(), 2.0 * log( 0.4 ), tolerance ) ;
		params << 0.0, 0.0, 0.0 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), -std::numeric_limits< double >::infinity() ) ;
		params << 1.0, 0.0, 0.0 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), -std::numeric_limits< double >::infinity() ) ;
		params << 0.0, 0.0, 1.0 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), -std::numeric_limits< double >::infinity() ) ;
	}

	{
		table <<
			0,	0,	0,
			0,	0,	0,
			1,	0,	0 ;

		HaplotypeFrequencyLogLikelihood ll( table ) ;

		Vector params( 3 ) ;
		params << 0.0, 1.0, 0.0 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), log( 1.0 ) ) ;
		params << 0.0, 0.8, 0.0 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), 2.0 * log( 0.8 ) ) ;
		params << 0.2, 0.8, 0.0 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), 2.0 * log( 0.8 ) ) ;
		params << 0.0, 0.8, 0.2 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), 2.0 * log( 0.8 ) ) ;
		params << 0.2, 0.6, 0.0 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), 2.0 * log( 0.6 ) ) ;
		params << 0.0, 0.6, 0.2 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), 2.0 * log( 0.6 ) ) ;
		params << 0.2, 0.6, 0.2 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), 2.0 * log( 0.6 ) ) ;
		params << 0.4, 0.4, 0.2 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_CLOSE( ll.get_value_of_function(), 2.0 * log( 0.4 ), tolerance ) ;
		params << 0.0, 0.0, 0.0 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), -std::numeric_limits< double >::infinity() ) ;
		params << 1.0, 0.0, 0.0 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), -std::numeric_limits< double >::infinity() ) ;
		params << 0.0, 0.0, 1.0 ;
		ll.evaluate_at( params ) ;
		BOOST_CHECK_EQUAL( ll.get_value_of_function(), -std::numeric_limits< double >::infinity() ) ;
	}
}

namespace {
	void test_haplotype_frequency_estimation( Eigen::MatrixXd const& genotypes, Eigen::VectorXd const& expected ) {
		HaplotypeFrequencyLogLikelihood ll( genotypes ) ;
		std::cerr << "test_haplotype_frequency_estimation: genotypes: " << genotypes
			<< "\nexpected: " << expected << "\n" ;
		std::cerr << "     got: " << ll.get_MLE_by_EM() << ".\n" ;
		BOOST_CHECK_SMALL( ( ll.get_MLE_by_EM() - expected ).maxCoeff(), 0.000000000001 ) ;
	}
}

AUTO_TEST_CASE( test_haplotype_frequency_exceptions ) {
	{
		Eigen::MatrixXd	genotypes = Eigen::MatrixXd::Zero( 3, 3 ) ;
		BOOST_CHECK_THROW( { HaplotypeFrequencyLogLikelihood ll( genotypes ) ; }, genfile::BadArgumentError ) ;
	}
}

AUTO_TEST_CASE( test_single_person_haplotype_frequency_estimation ) {
	Eigen::MatrixXd	genotypes( 3, 3 ) ;
	Eigen::VectorXd params( 3 ) ;

	genotypes.setZero() ; params.setZero() ;
	genotypes( 0, 0 ) = 1 ;
	test_haplotype_frequency_estimation( genotypes, params ) ;

	genotypes.setZero() ; params.setZero() ;
	genotypes( 0, 1 ) = 1 ;
	params(0) = 0.5 ;
	test_haplotype_frequency_estimation( genotypes, params ) ;

	genotypes.setZero() ; params.setZero() ;
	genotypes( 0, 2 ) = 1 ;
	params(0) = 1 ;
	test_haplotype_frequency_estimation( genotypes, params ) ;

	genotypes.setZero() ; params.setZero() ;
	genotypes( 1, 0 ) = 1 ;
	params(1) = 0.5 ;
	test_haplotype_frequency_estimation( genotypes, params ) ;

	genotypes.setZero() ; params.setZero() ;
	genotypes( 1, 1 ) = 1 ;
	// both SNPs heterozygores; half a chance of AB_ab and half of Ab_aB
	params(0) = 0.25 ;
	params(1) = 0.25 ;
	params(2) = 0.25 ;
	test_haplotype_frequency_estimation( genotypes, params ) ;

	genotypes.setZero() ; params.setZero() ;
	genotypes( 1, 2 ) = 1 ;
	params(0) = 0.5 ;
	params(2) = 0.5 ;
	test_haplotype_frequency_estimation( genotypes, params ) ;

	genotypes.setZero() ; params.setZero() ;
	genotypes( 2, 0 ) = 1 ;
	params(1) = 1 ;
	test_haplotype_frequency_estimation( genotypes, params ) ;

	genotypes.setZero() ; params.setZero() ;
	genotypes( 2, 1 ) = 1 ;
	params(1) = 0.5 ;
	params(2) = 0.5 ;
	test_haplotype_frequency_estimation( genotypes, params ) ;

	genotypes.setZero() ; params.setZero() ;
	genotypes( 2, 2 ) = 1 ;
	params(2) = 1 ;
	test_haplotype_frequency_estimation( genotypes, params ) ;
}

AUTO_TEST_CASE( test_N_person_haplotype_frequency_estimation ) {
	Eigen::MatrixXd	genotypes( 3, 3 ) ;
	Eigen::VectorXd params( 3 ) ;

	for( std::size_t N = 1; N < 10; ++N ) {
		genotypes.setZero() ; params.setZero() ;
		genotypes( 0, 0 ) = 1 ;
		test_haplotype_frequency_estimation( genotypes, params ) ;

		genotypes.setZero() ; params.setZero() ;
		genotypes( 0, 1 )= N ;
		params(0) = 0.5 ;
		test_haplotype_frequency_estimation( genotypes, params ) ;

		genotypes.setZero() ; params.setZero() ;
		genotypes( 0, 2 )= N ;
		params(0) = 1 ;
		test_haplotype_frequency_estimation( genotypes, params ) ;

		genotypes.setZero() ; params.setZero() ;
		genotypes( 1, 0 )= N ;
		params(1) = 0.5 ;
		test_haplotype_frequency_estimation( genotypes, params ) ;

		genotypes.setZero() ; params.setZero() ;
		genotypes( 1, 1 )= N ;
		// both SNPs heterozygores; half a chance of AB_ab and half of Ab_aB
		params(0) = 0.25 ;
		params(1) = 0.25 ;
		params(2) = 0.25 ;
		test_haplotype_frequency_estimation( genotypes, params ) ;

		genotypes.setZero() ; params.setZero() ;
		genotypes( 1, 2 )= N ;
		params(0) = 0.5 ;
		params(2) = 0.5 ;
		test_haplotype_frequency_estimation( genotypes, params ) ;

		genotypes.setZero() ; params.setZero() ;
		genotypes( 2, 0 )= N ;
		params(1) = 1 ;
		test_haplotype_frequency_estimation( genotypes, params ) ;

		genotypes.setZero() ; params.setZero() ;
		genotypes( 2, 1 )= N ;
		params(1) = 0.5 ;
		params(2) = 0.5 ;
		test_haplotype_frequency_estimation( genotypes, params ) ;

		genotypes.setZero() ; params.setZero() ;
		genotypes( 2, 2 )= N ;
		params(2) = 1 ;
		test_haplotype_frequency_estimation( genotypes, params ) ;
	}
}

AUTO_TEST_CASE( test_2_person_haplotype_frequency_estimation ) {
	Eigen::MatrixXd	genotypes( 3, 3 ) ;
	Eigen::VectorXd params( 3 ) ;

	genotypes.setZero() ; params.setZero() ;
	genotypes( 0, 0 ) = 1 ;
	genotypes( 2, 1 ) = 1 ;
	params(1) = 0.25 ;
	params(2) = 0.25 ;
	test_haplotype_frequency_estimation( genotypes, params ) ;

	genotypes.setZero() ; params.setZero() ;
	genotypes( 2, 2 ) = 1 ;
	genotypes( 2, 1 ) = 1 ;
	params(1) = 0.25 ;
	params(2) = 0.75 ;
	test_haplotype_frequency_estimation( genotypes, params ) ;

	genotypes.setZero() ; params.setZero() ;
	genotypes( 2, 2 ) = 1 ;
	genotypes( 1, 0 ) = 1 ;
	params(1) = 0.25 ;
	params(2) = 0.5 ;
	test_haplotype_frequency_estimation( genotypes, params ) ;

	genotypes.setZero() ; params.setZero() ;
	genotypes( 2, 2 ) = 1 ;
	genotypes( 1, 1 ) = 1 ;
	params(0) = 0.125 ;
	params(1) = 0.125 ;
	params(2) = 0.5 + ( 0.25 * 0.5 );
	test_haplotype_frequency_estimation( genotypes, params ) ;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
/*!
 * \file
**/
#ifndef BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_ULPDIST_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_ULPDIST_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

/*!
 * \ingroup boost_simd_ieee
 * \defgroup boost_simd_ieee_ulpdist ulpdist
 *
 * \par Description
 * Returns ulp distance of the two values.
 * \par
 * \arg If the common type is integer it is the same as \c dist
 * \arg If the common type is floating point the ulpdist is is computed,
 * by the above described method
 * \par
 * It is often difficult to  answer to the following question:
 * \arg are these two floating computations results similar enough?
 * \par
 * The ulpdist is a way to answer which is tuned for relative errors estimations
 * and peculiarly adapted to cope with the limited bits accuracy of floating point
 * representations.
 * The method is the following:
 * \par
 * Properly normalize the two numbers by the same factor in a way that
 * the largest of the two numbers exponents will be brought to zero
 * Return the absolute difference of these normalized numbers
 * divided by the rounding error Eps
 * \par
 * The roundind error is the ulp (unit in the last place) value, i.e. the
 * floating number, the exponent of which is 0 and the mantissa is all zeros
 * but a 1 in the last digit (it is not hard coded that way however).
 * This means 2^-23 for floats and 2^-52 for double
 * \arg For instance if two floating numbers (of same type) have an ulpdist of
 * zero that means that their floating representation are identical.
 * \arg Generally equality up to 0.5 ulp is the best that one can wish beyond
 * strict equality.
 * \arg Typically if a double is compared to the float representation of
 * its floating conversion (they are exceptions as for fully representable
 * reals) the ulpdist will be around 2^26.5 (~10^8)
 * \par
 * The ulpdist is also roughly equivalent to the number of representable
 * floating points values between two given floating points values.
 * \par
 * \arg  \c ulpdist(1.0,1+Eps\<double\>())==0.5
 * \arg  \c ulpdist(1.0,1+Eps\<double\>()/2)==0.0
 * \arg  \c ulpdist(1.0,1-Eps\<double\>()/2)==0.25
 * \arg  \c ulpdist(1.0,1-Eps\<double\>())==0.5
 * \arg  \c ulpdist(double(Pi\<float\>()),Pi\<double\>())==9.84293e+07
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/ulpdist.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class A0>
 *     meta::call<tag::ulpdist_(A0,A0)>::type
 *     ulpdist(const A0 & a0,const A0 & a1);
 * }
 * \endcode
 *
 * \param a0 the first parameter of ulpdist
 * \param a1 the second parameter of ulpdist
 *
 * \return a value of the common type of the parameters
 *
 * \par Notes
 * In SIMD mode, this function acts elementwise on the inputs vectors elements
 * \par
 *
**/

namespace boost { namespace simd { namespace tag
  {
    /*!
     * \brief Define the tag ulpdist_ of functor ulpdist
     *        in namespace boost::simd::tag for toolbox boost.simd.ieee
    **/
    struct ulpdist_ : ext::elementwise_<ulpdist_> { typedef ext::elementwise_<ulpdist_> parent; };
  }
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::ulpdist_, ulpdist, 2)
} }

#endif

// modified by jt the 25/12/2010