/*
This is SCRIMP++, as published by Zhua, Yeh, Zimmerman et al. at https://sites.google.com/site/scrimpplusplus/
It is provided as a baseline for our work and republished with kind permission of Prof. Eamonn Keogh
All rights belong the original authors and they shall be asked for licensing, if required.
Few modifications to the code were made to adapt it in our framework

Details of the SCRIMP++ algorithm can be found at:
(author information ommited for ICDM review),
"SCRIMP++: Motif Discovery at Interactive Speeds", submitted to ICDM 2018.
*/
#include <stdio.h>
#include <stdlib.h>
#include <fftw3.h>
#include <math.h>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <limits>
#include <vector>
#include <algorithm>
#include <string>
#include <sstream>
#include <timing.h>
#include <logging.hpp>

#include <ScrimpppOrig.hpp>
#include <boost/filesystem.hpp>

using namespace matrix_profile;

static FactoryRegistration<ScrimpppOrig> s_origRegistration("scrimppp_orig");
static const int notification_interval_iter = 10000;

void ScrimpppOrig::initialize(const Scrimppp_params &params) {

}

void ScrimpppOrig::compute_matrix_profile(const Scrimppp_params& params)
{
	// start timer
	//time_t tstart, tend;
	//tstart = time(0);
	Timepoint tstart, tend;
	Timespan time_elapsed;

	// read time series and subsequence length (windowSize).
	std::fstream timeSeriesFile(params.time_series_filename, std::ios_base::in);

	int windowSize = params.query_window_len;
	int stepSize = floor(params.prescrimp_stride*windowSize);

	const std::string preoutfilename = params.output_filename + "_prescrimp";

	if (!timeSeriesFile.is_open())
	{
		throw std::runtime_error("Could not open input file");
	}

	std::vector<double> A;
	double tempval;
	int timeSeriesLength = 0;
	while (timeSeriesFile >> tempval)
	{
		A.push_back(tempval);
		timeSeriesLength++;
	}
	EXEC_INFO( "loaded time series of length " << timeSeriesLength );

	if (timeSeriesLength < windowSize) {
		throw std::runtime_error("ERROR: Time series is shorter than the window length, can not proceed");
	}

	timeSeriesFile.close();

	// set exclusion zone
	int exclusionZone = windowSize / 4;

	// set Matrix Profile Length
	int ProfileLength = timeSeriesLength - windowSize + 1;

	// preprocess, statistics, get the mean and standard deviation of every subsequence in the time series
	double* ACumSum = new double[timeSeriesLength];
	ACumSum[0] = A[0];
	for (int i = 1; i < timeSeriesLength; i++)
		ACumSum[i] = A[i] + ACumSum[i - 1];
	double* ASqCumSum = new double[timeSeriesLength];
	ASqCumSum[0] = A[0] * A[0];
	for (int i = 1; i < timeSeriesLength; i++)
		ASqCumSum[i] = A[i] * A[i] + ASqCumSum[i - 1];
	double* ASum = new double[ProfileLength];
	ASum[0] = ACumSum[windowSize - 1];
	for (int i = 0; i < timeSeriesLength - windowSize; i++)
		ASum[i + 1] = ACumSum[windowSize + i] - ACumSum[i];
	double* ASumSq = new double[ProfileLength];
	ASumSq[0] = ASqCumSum[windowSize - 1];
	for (int i = 0; i < timeSeriesLength - windowSize; i++)
		ASumSq[i + 1] = ASqCumSum[windowSize + i] - ASqCumSum[i];
	double* AMean = new double[ProfileLength];
	for (int i = 0; i < ProfileLength; i++)
		AMean[i] = ASum[i] / windowSize;
	double* ASigmaSq = new double[ProfileLength];
	for (int i = 0; i < ProfileLength; i++)
		ASigmaSq[i] = ASumSq[i] / windowSize - AMean[i] * AMean[i];
	double* ASigma = new double[ProfileLength];
	for (int i = 0; i < ProfileLength; i++)
		ASigma[i] = sqrt(ASigmaSq[i]);
	delete [] ACumSum;
	delete [] ASqCumSum;
	delete [] ASum;
	delete [] ASumSq;
	delete [] ASigmaSq;

	//Initialize Matrix Profile and Matrix Profile Index
	double* profile = new double[ProfileLength];
	int* profileIndex = new int[ProfileLength];
	for (int i=0; i<ProfileLength; i++)
	{
		profile[i]=std::numeric_limits<double>::infinity();
		profileIndex[i]=0;
	}
	tstart = get_cur_time();

	//int fftsize = pow(2,ceil(log2(timeSeriesLength)));
	int fftsize = timeSeriesLength; //fftsize must be at least 2*windowSize
	fftsize = fftsize > 2 * windowSize ? fftsize : 2 * windowSize;
	EXEC_TRACE ( "length of fft input: " << fftsize );

	/*******************************PreSCRIMP***************************************/

	fftw_plan plan;
	fftw_complex* ATime = (fftw_complex*)fftw_malloc(sizeof(fftw_complex) * fftsize);
	fftw_complex* AFreq = (fftw_complex*)fftw_malloc(sizeof(fftw_complex) * fftsize);

	for (int i = 0; i < fftsize; i++)
	{
		ATime[i][1] = 0;
		if (i < timeSeriesLength)
			ATime[i][0] = A[i];
		else
			ATime[i][0] = 0;
	}

	plan = fftw_plan_dft_1d(fftsize, ATime, AFreq, FFTW_FORWARD, FFTW_ESTIMATE);
	fftw_execute(plan);
	fftw_free(ATime);

	fftw_complex* queryTime = (fftw_complex*)fftw_malloc(sizeof(fftw_complex) * fftsize);
	fftw_complex* queryFreq = (fftw_complex*)fftw_malloc(sizeof(fftw_complex) * fftsize);
	fftw_complex* AQueryTime = (fftw_complex*)fftw_malloc(sizeof(fftw_complex) * fftsize);
	fftw_complex* AQueryFreq = (fftw_complex*)fftw_malloc(sizeof(fftw_complex) * fftsize);

	//Sample subsequences with a fixed stepSize, then random shuffle their computation order
	std::vector<int> idx;
	for (int i = 0; i < timeSeriesLength - windowSize + 1; i += stepSize)
		idx.push_back(i);
	std::random_shuffle(idx.begin(), idx.end());

	double* query = new double[windowSize];

	for (int idx_i = 0; idx_i < idx.size(); idx_i++)
	{
		int i = idx[idx_i];
		for (int j = 0; j < windowSize; j++)
		{
			query[j] = A[i + j];
		}
		double queryMean = AMean[i];
		double queryStd = ASigma[i];

		for (int j = 0; j < fftsize; j++)
		{
			queryTime[j][1] = 0;

			if (j < windowSize)
				queryTime[j][0] = query[windowSize - j - 1];
			else
				queryTime[j][0] = 0;
		}

		plan = fftw_plan_dft_1d(fftsize, queryTime, queryFreq, FFTW_FORWARD, FFTW_ESTIMATE);
		fftw_execute(plan);

		for (int j = 0; j < fftsize; j++)
		{
			AQueryFreq[j][0] = AFreq[j][0] * queryFreq[j][0] - AFreq[j][1] * queryFreq[j][1];
			AQueryFreq[j][1] = AFreq[j][1] * queryFreq[j][0] + AFreq[j][0] * queryFreq[j][1];
		}

		plan = fftw_plan_dft_1d(fftsize, AQueryFreq, AQueryTime, FFTW_BACKWARD, FFTW_ESTIMATE);
		fftw_execute(plan);

		int exclusionZoneStart = i - exclusionZone;
		int exclusionZoneEnd = i + exclusionZone;
		double minimumDistance = std::numeric_limits<double>::infinity();
		int minimumDistanceIndex;
		for (int j = 0; j < timeSeriesLength - windowSize + 1; j++)
		{
			double distance;
			if ((j > exclusionZoneStart) && (j < exclusionZoneEnd))
				distance = std::numeric_limits<double>::infinity();
			else
			{
				distance = 2 * (windowSize - (AQueryTime[windowSize + j - 1][0] / fftsize - windowSize * AMean[j] * queryMean) / (ASigma[j] * queryStd));
			}

			if (distance < minimumDistance)
			{
				minimumDistance = distance;
				minimumDistanceIndex = j;
			}

			if (distance < profile[j])
			{
				profile[j] = distance;
				profileIndex[j] = i;
			}
		}
		profile[i] = minimumDistance;
		profileIndex[i] = minimumDistanceIndex;

		int j = profileIndex[i];
		double lastz = (windowSize - profile[i] / 2) * (ASigma[j] * ASigma[i]) + windowSize * AMean[j] * AMean[i];
		double lastzz = lastz;
		double distance;
		for (int k = 1; k < stepSize && i + k < timeSeriesLength - windowSize + 1 && j + k < timeSeriesLength - windowSize + 1; k++)
		{
			lastz = lastz - A[i + k - 1] * A[j + k - 1] + A[i + k + windowSize - 1] * A[j + k + windowSize - 1];
			distance = 2 * (windowSize - (lastz - windowSize * AMean[j + k] * AMean[i + k]) / (ASigma[j + k] * ASigma[i + k]));
			if (distance < profile[i + k])
			{
				profile[i + k] = distance;
				profileIndex[i + k] = j + k;
			}
			if (distance < profile[j + k])
			{
				profile[j + k] = distance;
				profileIndex[j + k] = i + k;
			}
		}
		lastz = lastzz;
		for (int k = 1; k < stepSize && i - k >= 0 && j - k >= 0; k++)
		{
			lastz = lastz - A[i - k + windowSize] * A[j - k + windowSize] + A[i - k] * A[j - k];
			distance = 2 * (windowSize - (lastz - windowSize * AMean[j - k] * AMean[i - k]) / (ASigma[j - k] * ASigma[i - k]));
			if (distance < profile[i - k])
			{
				profile[i - k] = distance;
				profileIndex[i - k] = j - k;
			}
			if (distance < profile[j - k])
			{
				profile[j - k] = distance;
				profileIndex[j - k] = i - k;
			}
		}
	}

	fftw_destroy_plan(plan);
	fftw_free(AFreq);
	fftw_free(queryTime);
	fftw_free(queryFreq);
	fftw_free(AQueryTime);
	fftw_free(AQueryFreq);
	delete[] query;

	tend = get_cur_time();
	time_elapsed = tend - tstart;

	// output
	EXEC_INFO("finished prescrimp")
	PERF_LOG( "Time for PreSCRIMP: " << std::setprecision(std::numeric_limits<double>::digits10 + 2) << time_elapsed );

	std::fstream preprofileOutFile(preoutfilename.c_str(), std::ios_base::out);

	// Write PreSCRIMP Matrix Profile and Matrix Profile Index to file.
	for (int i = 0; i < timeSeriesLength - windowSize + 1; i++)
	{
		preprofileOutFile << std::setprecision(std::numeric_limits<double>::digits10 + 2) << sqrt(abs(profile[i])) << " " << std::setprecision(std::numeric_limits<int>::digits10 + 1) << profileIndex[i] << std::endl;
	}

	preprofileOutFile.close();

	/******************** SCRIMP ********************/

	//Random shuffle the computation order of the diagonals of the distance matrix
std::srand(1);//TODO: remove. Introduced for constistent evaluation order among all algorithms during debugging!
    idx.clear();
	for (int i = exclusionZone+1; i < ProfileLength; i++)
		idx.push_back(i);
	std::random_shuffle(idx.begin(), idx.end());

	double* dotproduct = new double[timeSeriesLength];

	//iteratively evaluate the diagonals of the distance matrix
	for (int ri = 0; ri < idx.size(); ri++)
	    {
		//select a random diagonal
		int diag = idx[ri];

		//calculate the dot product of every two time series values that ar diag away
		for (int j=diag; j < timeSeriesLength; j++)
			dotproduct[j]=A[j]*A[j-diag];

		//evaluate the fist distance value in the current diagonal
		double distance;
		double lastz=0; //the dot product of a subsequence
		for (int k = 0; k < windowSize; k++)
			lastz += dotproduct[k+diag];

		//j is the column index, i is the row index of the current distance value in the distance matrix
		int j=diag, i=j-diag;

		//evaluate the distance based on the dot product
		distance = 2 * (windowSize - (lastz - windowSize * AMean[j] * AMean[i]) / (ASigma[j] * ASigma[i]));

		//update matrix profile and matrix profile index if the current distance value is smaller
		if (distance < profile[j])
		{
			profile[j] = distance;
			profileIndex [j] = i;
		}
		if (distance < profile[i])
		{
			profile[i] = distance;
			profileIndex [i] = j;
		}

//std::cout << "diag " << diag << " lastz " << lastz << std::endl;
		//evaluate the second to the last distance values along the diagonal and update the matrix profile/matrix profile index.
		for (j=diag+1; j<ProfileLength; j++)
		{
			i=j-diag;
			lastz = lastz + dotproduct[j+windowSize-1] - dotproduct [j-1];
			distance = 2 * (windowSize - (lastz - windowSize * AMean[j] * AMean[i]) / (ASigma[j] * ASigma[i]));

//std::cout << "eval i: " << i << " j: " << j << " lastz" << lastz << std::endl;
			if (distance < profile[j])
			{
				profile[j] = distance;
				profileIndex [j] = i;
			}
			if (distance < profile[i])
			{
				profile[i] = distance;
				profileIndex [i] = j;
			}
		}

		//Show time per 10000 iterations
		if ( (ri+1) % notification_interval_iter == 0)
		{
			tend = get_cur_time();
			time_elapsed = tend - tstart;
			//std::cout << "Time spent: " << std::setprecision(std::numeric_limits<double>::digits10 + 2) << difftime(time(0), tstart) << " seconds." << std::endl;
			PERF_TRACE( "completed " << notification_interval_iter << " iterations in: " << time_elapsed );
		}

		//The following commented section is to produce provisional results. Basically, if you would like to enable interrupt and look at the current matrix profile/matrix profile index, you can uncomment this section, and revise line 182 according to the intterupt mechanism you're using.

		/*if (interrupt_detected) //revise this line to enable your interrupt mechanism
		{
			std::fstream prov_profileOutFile(outfilename_provisional.c_str(), std::ios_base::out);

			// Write Current Matrix Profile and Matrix Profile Index to file.
			for (int k = 0; k < timeSeriesLength - windowSize + 1; k++)
				prov_profileOutFile << std::setprecision(std::numeric_limits<double>::digits10 + 2) << sqrt(abs(profile[k])) << " " << std::setprecision(std::numeric_limits<int>::max()) << profileIndex[k] << std::endl;
			prov_profileOutFile.close();
		}
		*/
	}

	// end timer
	//tend = time(0);
	tend = get_cur_time();
	time_elapsed = tend - tstart;

	PERF_LOG ( "total computation time: " << time_elapsed << " seconds." );
	const double triang_len = ProfileLength-exclusionZone;
	PERF_LOG ( "throughput computations: " << triang_len * triang_len / get_seconds(time_elapsed) << " matrix entries/second");
	EXEC_INFO( "Writing result to file");

	if (params.output_filename.empty()) {
		throw std::runtime_error("Empty output file name specified!");
	}
	std::fstream profileOutFile(params.output_filename.c_str(), std::ios_base::out);

	// Write final Matrix Profile and Matrix Profile Index to file.
	for (int i = 0; i < timeSeriesLength - windowSize + 1; i++)
	{
		profile[i] = sqrt(abs(profile[i]));
		profileOutFile << std::setprecision(std::numeric_limits<double>::digits10 + 2) << profile[i] << " " << std::setprecision(std::numeric_limits<int>::digits10+1) << profileIndex[i] << std::endl;
	}

	profileOutFile.close();

	delete [] dotproduct;
	delete [] AMean;
	delete [] ASigma;
	delete [] profile;
	delete [] profileIndex;
}


#pragma once
#include <ros/ros.h>
#include <Eigen/Dense>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <tf/transform_datatypes.h>

namespace Turtlebot
{

class ExtendedKalmanFilter
{
public:
    ExtendedKalmanFilter(ros::NodeHandle &nh, ros::NodeHandle &pnh);
    ~ExtendedKalmanFilter() = default;

private:

    void odomCallback(const nav_msgs::Odometry::ConstPtr &msg);
    void imuCallback(const sensor_msgs::Imu::ConstPtr &msg);

    void initializeFilter(ros::NodeHandle &pnh);
    void filterOdom();
    const Eigen::MatrixXf calcG();
    const Eigen::MatrixXf calcCovarianceBar(const Eigen::MatrixXf &G);
    const Eigen::MatrixXf calcUBar();
    const Eigen::MatrixXf calczBar(const Eigen::MatrixXf &u_);
    const Eigen::MatrixXf calcH(const Eigen::MatrixXf &u_);
    const Eigen::MatrixXf calcKalmanGain(const Eigen::MatrixXf &cov_, const Eigen::MatrixXf &H);
    const Eigen::MatrixXf calcU(const Eigen::MatrixXf &u_, const Eigen::MatrixXf &K);
    void calcCovariance(const Eigen::MatrixXf & cov_, const Eigen::MatrixXf &H, const Eigen::MatrixXf &K);

    void integrateIMUToOdom();
    void setz();
    void pubOdom(const Eigen::MatrixXf &u);

    ros::Subscriber m_odom_sub;
    ros::Subscriber m_imu_sub;
    ros::Publisher m_odom_pub;

    nav_msgs::Odometry m_odom_filtered;

    nav_msgs::Odometry::ConstPtr m_odom;
    Eigen::MatrixXf m_z;
    nav_msgs::Odometry::ConstPtr m_prev_odom;
    sensor_msgs::Imu::ConstPtr m_imu;
    sensor_msgs::Imu m_imu_at_odom;
    sensor_msgs::Imu m_imu_at_last_odom;
    sensor_msgs::Imu::ConstPtr m_prev_imu;

    Eigen::MatrixXf m_covariance;
    Eigen::MatrixXf m_imu_covariance;
    Eigen::MatrixXf m_odom_covariance;

    bool m_have_odom = false;
    bool m_have_imu = false;

    bool m_compensate;

};

}


#include "principal_curvatures_can.hpp"
#include <Eigen/Dense>

void PrincipalCurvaturesEstimationCAN::compute(
  PointCloudOut &output) {
  // From: Curvature Estimation of 3D Point Cloud Surfaces Through the Fitting of Normal Section Curvatures (Zhang et. al. 2008)
  output.resize(this->input_->size());
  std::vector<int> neighbor_ids;
  std::vector<float> sq_distances;
  Eigen::Vector3f x = (Eigen::Vector3f() << 1, 0, 0).finished();
  Eigen::Vector3f z = (Eigen::Vector3f() << 0, 0, 1).finished();
  Eigen::Vector3f N, p, q, Q, n, mu, max_eigenvector;
  Eigen::Matrix3f R;
  Eigen::MatrixXf M;
  Eigen::VectorXf r;
  Eigen::Matrix2f W;
  Eigen::EigenSolver<Eigen::Matrix2f> eigen_solver;
  Eigen::Vector2f::Index max_ind;
  Eigen::Vector2f eigenvalues;
  float nxy, k, theta;
  int jn, j, row_ind;
  for (int i=0; i<this->input_->size(); i++) {
    p = Eigen::Vector3f::Map(this->input_.get()->points[i].data);
    N = Eigen::Vector3f::Map(this->normals_.get()->points[i].data_n);
    R = (2 * ((N + z) * (N + z).transpose()) / ((N + z).transpose() * (N + z))) - Eigen::Matrix3f::Identity(); // https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d
    if(this->tree_->radiusSearch(this->input_.get()->points[i], this->search_radius_, neighbor_ids, sq_distances) > 2) {
      M.resize(neighbor_ids.size() - 1, 3);
      r.resize(neighbor_ids.size() - 1); // Subtracting one because we skip the search point.
      row_ind = 0;
      for(j=1; j<neighbor_ids.size(); j++) {
        jn = neighbor_ids[j];
        q = R * (Eigen::Vector3f::Map(this->input_.get()->points[jn].data) - p);
        Q << q.head<2>(), 0;
        n = R * Eigen::Vector3f::Map(this->normals_.get()->points[jn].data_n);
        nxy = (q(0) * n(0) + q(1) * n(1)) / sqrt(pow(q(0), 2) + pow(q(1), 2));
        k = -nxy / sqrt((pow(nxy, 2) + pow(n(2), 2)) * (pow(q(0), 2) + pow(q(1), 2)));
        theta = acos(x.dot(Q) / (x.norm() * Q.norm()));
        if(!isnan(k)) {
          M(row_ind, 0) = pow(cos(theta), 2);
          M(row_ind, 1) = 2 * cos(theta) * sin(theta);
          M(row_ind, 2) = pow(sin(theta), 2);
          r(row_ind) = k;
          ++row_ind;
        } else {
          M.resize(M.rows() - 1, 3);
          r.resize(r.rows() - 1);
        }
      }
      mu = M.colPivHouseholderQr().solve(r);
      if(!isnan(mu(0))) {
        W << mu(0), mu(1), mu(1), mu(2);
        eigen_solver.compute(W, true);
        eigenvalues = eigen_solver.eigenvalues().real();
        output[i].pc1 = eigenvalues.maxCoeff(&max_ind);
        output[i].pc2 = eigenvalues.minCoeff();
        max_eigenvector = R.transpose() * (Eigen::Vector3f() << eigen_solver.eigenvectors().col(max_ind).real(), 0).finished();
        output[i].principal_curvature_x = max_eigenvector(0);
        output[i].principal_curvature_y = max_eigenvector(1);
        output[i].principal_curvature_z = max_eigenvector(2);
      }
    }
  }
}


/***********************************************************************
This file is part of the librjmcmc project source files.

Copyright : Institut Geographique National (2008-2012)
Contributors : Mathieu Brédif, Olivier Tournaire, Didier Boldo
email : librjmcmc@ign.fr

This software is a generic C++ library for stochastic optimization.

This software is governed by the CeCILL license under French law and
abiding by the rules of distribution of free software. You can use,
modify and/or redistribute the software under the terms of the CeCILL
license as circulated by CEA, CNRS and INRIA at the following URL
"http://www.cecill.info".

As a counterpart to the access to the source code and rights to copy,
modify and redistribute granted by the license, users are provided only
with a limited warranty and the software's author, the holder of the
economic rights, and the successive licensors have only limited liability.

In this respect, the user's attention is drawn to the risks associated
with loading, using, modifying and/or developing or reproducing the
software by the user in light of its specific status of free software,
that may mean that it is complicated to manipulate, and that also
therefore means that it is reserved for developers and experienced
professionals having in-depth computer knowledge. Users are therefore
encouraged to load and test the software's suitability as regards their
requirements in conditions enabling the security of their systems and/or
data to be ensured and, more generally, to use and operate it in the
same conditions as regards security.

The fact that you are presently reading this means that you have had
knowledge of the CeCILL license and that you accept its terms.

***********************************************************************/

#ifndef RJMCMC_RASTER_VARIATE_HPP
#define RJMCMC_RASTER_VARIATE_HPP

#include <boost/random/uniform_real.hpp>
#include <vector>
#include <algorithm>

namespace rjmcmc {

    // this variate generates a point in [0,1]^N with a piecewise uniform density where the pieces are given by a uniform rectangular voxel grid.
    // int *size gives the number of elements of the grid in each dimension
    // T *pdf gives the unnormalized pdf values of each voxel as a N dimensional array of size (size)

    template<int N>
    class raster_variate
    {
        typedef boost::uniform_real<> rand_type;
        mutable rand_type m_rand;

    public:
        typedef double value_type;
        enum { dimension = N };
        template<typename Engine, typename OutputIterator>
        inline double operator()(Engine& e, OutputIterator it) const {
            double x = m_rand(e);
            int offset = int(std::upper_bound(m_cdf.begin()+1,m_cdf.end(),x)-(m_cdf.begin()+1));
            double pdf = (m_cdf[offset+1]-m_cdf[offset])*m_totsize;
            for(int i=0; i<N; ++i)
            {
                int ix = offset % m_size[i];
                *it++ = (ix+m_rand(e))/m_size[i];
                offset /= m_size[i];
            }
            return pdf;
        }
        template<typename InputIterator>
        inline double pdf(InputIterator it) const {
            int offset = 0;
            int stride = 1;
            for(int i=0; i<N; ++i)
            {
                double x = *it++;
                if(x<0. || x>=1.) return 0.;
                int ix = int(x*m_size[i]);
                offset += stride*ix;
                stride *= m_size[i];
            }
            return (m_cdf[offset+1]-m_cdf[offset])*m_totsize;
        }
        template<typename T>
        raster_variate(T* pdf, int *size) {
            m_size.resize(N);
            m_totsize = 1;
            for(int i=0; i<N; ++i) { m_totsize *= size[i]; m_size[i] = size[i]; }
            m_sum = 0;
            m_cdf.resize(m_totsize+1);
            m_cdf[0] = 0.;
            for(int i=0; i<m_totsize; ++i) m_sum = m_cdf[i+1] = m_sum + pdf[i]; // assert(pdf[i]>=0)
            for(int i=0; i<m_totsize; ++i) m_cdf[i+1]/=m_sum;
        }
    private:
        std::vector<double> m_cdf;
        std::vector<int> m_size;
        int m_totsize;
        double m_sum;
    };

}; // namespace rjmcmc

#endif // RJMCMC_RASTER_VARIATE_HPP


///////////////////////////////////////////////////////////////
//  Copyright 2012 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

#include <boost/multiprecision/cpp_int.hpp>
#include "test.hpp"

using Integer = boost::multiprecision::cpp_int;

void check_sqrt(const Integer& s, const Integer& r, const Integer& v) {
   BOOST_CHECK_EQUAL(s * s + r, v);
   BOOST_CHECK_GE(r, Integer(0));
   BOOST_CHECK_LE(s * s, v);
   BOOST_CHECK_GT((s + 1) * (s + 1), v);
}

template<typename I>
void check(const I& v) {
   I r;
   I s = boost::multiprecision::sqrt(v, r);
   check_sqrt(Integer(s), Integer(r), Integer(v));
}

void check_types(const Integer& v) {
   using namespace boost::multiprecision;
   size_t bits = 0;
   if (v > 0) {
      bits = msb(v);
   }
   if (bits < 32) {
      check(static_cast<uint32_t>(v));
   }
   if (bits < 64) {
      check(static_cast<uint64_t>(v));
   }
   if (bits < 128) {
      check(uint128_t(v));
   }
   if (bits < 256) {
      check(uint256_t(v));
   }
   if (bits < 512) {
      check(uint512_t(v));
   }
   check(v);
}

void check_near(const Integer& v) {
   check_types(v);
   for (size_t j = 0; j < 8; j++) {
      check_types(v - Integer(1 << j));
      check_types(v - Integer(1 << j) - 1);
      check_types(v - Integer(1 << j) + 1);
      check_types(v + Integer(1 << j));
      check_types(v + Integer(1 << j) - 1);
      check_types(v + Integer(1 << j) + 1);
   }
}

void test_first() {
   for (size_t i = 0; i < (1 << 16); i++) {
      check_types(Integer(i));
   }
}

void test_perfect() {
   for (size_t i = 256; i < (1 << 14); i++) {
      check_near(Integer(i) * Integer(i));
   }
}

void test_powers() {
   for (size_t i = 24; i < 2048; i++) {
      check_near(Integer(i) << i);
   }
}

void test_big() {
   for (size_t bits = 128; bits <= 2048; bits *= 2) {
      Integer i = Integer(1) << bits;
      Integer s = (i >> 8);
      Integer step = (i - s) / (1 << 8);
      for (Integer j = s; j <= i; j += step) {
         check_near(j);
      }
   }
}

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

   test_first();
   test_perfect();
   test_powers();
   test_big();

   return boost::report_errors();
}


/*
 * Copyright 2021 MusicScience37 (Kenta Kabashima)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
/*!
 * \file
 * \brief Test of rbf_kernel class.
 */
#include "num_collect/interp/kernel/rbf_kernel.h"

#include <Eigen/Core>
#include <catch2/catch_test_macros.hpp>
#include <catch2/matchers/catch_matchers_floating.hpp>

#include "num_collect/interp/kernel/euclidean_distance.h"
#include "num_collect/interp/kernel/gaussian_rbf.h"

TEST_CASE("num_collect::interp::kernel::rbf_kernel") {
    using num_collect::interp::kernel::euclidean_distance;
    using num_collect::interp::kernel::gaussian_rbf;
    using num_collect::interp::kernel::rbf_kernel;

    SECTION("calculate kernel") {
        const auto kernel = rbf_kernel<euclidean_distance<Eigen::Vector3d>,
            gaussian_rbf<double>>();

        const auto var1 = Eigen::Vector3d(1.234, 2.345, 3.456);
        const auto var2 = Eigen::Vector3d(1.357, 2.468, 3.579);
        const double expected = std::exp(-(var1 - var2).squaredNorm());
        REQUIRE_THAT(kernel(var1, var2), Catch::Matchers::WithinRel(expected));
    }

    SECTION("set length parameter") {
        auto kernel = rbf_kernel<euclidean_distance<Eigen::Vector3d>,
            gaussian_rbf<double>>();

        constexpr double len_param = 0.1;
        kernel.len_param(len_param);
        REQUIRE_THAT(kernel.len_param(), Catch::Matchers::WithinRel(len_param));

        const auto var1 = Eigen::Vector3d(1.234, 2.345, 3.456);
        const auto var2 = Eigen::Vector3d(1.357, 2.468, 3.579);
        const double expected =
            std::exp(-(var1 - var2).squaredNorm() / (len_param * len_param));
        REQUIRE_THAT(kernel(var1, var2), Catch::Matchers::WithinRel(expected));
    }

    SECTION("set length parameter via kernel parameter") {
        auto kernel = rbf_kernel<euclidean_distance<Eigen::Vector3d>,
            gaussian_rbf<double>>();

        constexpr double len_param = 0.1;
        kernel.kernel_param(std::log10(len_param));
        REQUIRE_THAT(kernel.kernel_param(),
            Catch::Matchers::WithinRel(std::log10(len_param)));

        const auto var1 = Eigen::Vector3d(1.234, 2.345, 3.456);
        const auto var2 = Eigen::Vector3d(1.357, 2.468, 3.579);
        const double expected =
            std::exp(-(var1 - var2).squaredNorm() / (len_param * len_param));
        REQUIRE_THAT(kernel(var1, var2), Catch::Matchers::WithinRel(expected));
    }

    SECTION("call kernel_param_search_region") {
        const auto kernel = rbf_kernel<euclidean_distance<Eigen::Vector3d>,
            gaussian_rbf<double>>();

        const auto list = std::vector<Eigen::Vector3d,
            Eigen::aligned_allocator<Eigen::Vector3d>>{
            Eigen::Vector3d(1.234, 2.345, 3.456),
            Eigen::Vector3d(1.357, 2.468, 3.579)};
        const auto [lower, upper] = kernel.kernel_param_search_region(list);
        REQUIRE(lower < upper);
    }

    SECTION("call kernel_param_search_region with only one variable") {
        const auto kernel = rbf_kernel<euclidean_distance<Eigen::Vector3d>,
            gaussian_rbf<double>>();

        const auto list = std::vector<Eigen::Vector3d,
            Eigen::aligned_allocator<Eigen::Vector3d>>{
            Eigen::Vector3d(1.234, 2.345, 3.456)};
        REQUIRE_THROWS((void)kernel.kernel_param_search_region(list));
    }

    SECTION("call kernel_param_search_region without variable") {
        const auto kernel = rbf_kernel<euclidean_distance<Eigen::Vector3d>,
            gaussian_rbf<double>>();

        const auto list = std::vector<Eigen::Vector3d,
            Eigen::aligned_allocator<Eigen::Vector3d>>{};
        REQUIRE_THROWS((void)kernel.kernel_param_search_region(list));
    }
}


/*
* Copyright 2019 © Centre Interdisciplinaire de développement en Cartographie des Océans (CIDCO), Tous droits réservés
*/

#ifndef GEOREFERENCING_HPP
#define GEOREFERENCING_HPP

#include <Eigen/Dense>
#include "math/CoordinateTransform.hpp"
#include "Raytracing.hpp"

/*!
* \brief Georeferencing class
* \author Guillaume Labbe-Morissette, Jordan McManus, Emile Gagne
* \date October 2, 2018, 9:39 AM
*/
class Georeferencing{
public:
  /**
  * Georeferences a ping
  *
  * @param georeferencedPing georeferenced ping in vector form
  * @param attitude the attitude of the ship in the IMU frame
  * @param position the position of the ship in the TRF
  * @param ping the ping of the georeference in the sonar frame
  * @param svp the SoundVelocityProfile
  * @param leverArm vector from the position reference point (PRP) to the acoustic center
  *
  */
  virtual void georeference(Eigen::Vector3d & georeferencedPing,Attitude & attitude,Position & position,Ping & ping,SoundVelocityProfile & svp,Eigen::Vector3d & leverArm,Eigen::Matrix3d & boresight){};
};

/*!
* \brief TRF Georeferencing class
*
* Extends Georeferencing class
*/
class GeoreferencingTRF : public Georeferencing{
public:

  /**
  * Georeferences a ping in the TRF
  *
  * @param georeferencedPing vector of a ping georeferenced
  * @param attitude the attitude of the ship in the IM frame
  * @param position the position of the ship in the TRF
  * @param ping the ping of the georeference in the sonar frame
  * @param svp the sound velocity profile
  * @param leverArm vector from the position reference point (PRP) to the acoustic center
  *
  */
  void georeference(Eigen::Vector3d & georeferencedPing,Attitude & attitude,Position & position,Ping & ping,SoundVelocityProfile & svp,Eigen::Vector3d & leverArm,Eigen::Matrix3d & boresight) {
    //Compute transform matrixes
    Eigen::Matrix3d ned2ecef;
    CoordinateTransform::ned2ecef(ned2ecef,position);

    Eigen::Matrix3d imu2ned;
    CoordinateTransform::getDCM(imu2ned,attitude);

    //Convert position to ECEF
    Eigen::Vector3d positionECEF;
    CoordinateTransform::getPositionECEF(positionECEF,position);

    //Convert ping to ECEF
    Eigen::Vector3d pingVector;
    Raytracing::rayTrace(pingVector,ping,svp);

    Eigen::Vector3d pingECEF = ned2ecef * (imu2ned * boresight * pingVector);

    //Convert lever arm to ECEF
    Eigen::Vector3d leverArmECEF =  ned2ecef * (imu2ned * leverArm);

    //Compute total ECEF vector

    georeferencedPing = positionECEF + pingECEF + leverArmECEF;
  }
};

/*!
* \brief LGF Georeferencing class
*/
class GeoreferencingLGF : public Georeferencing{
public:

  /**
  * Georeferences a ping in the LGF (NED)
  *
  * @param georeferencedPing vector of a ping georeferenced
  * @param attitude the attitude of the ship in the IM frame
  * @param position the position of the ship in the TRF
  * @param ping the ping of the georeference in the sonar frame
  * @param svp the sound velocity profile
  * @param leverArm vector from the position reference point (PRP) to the acoustic center
  *
  */
  void georeference(Eigen::Vector3d & georeferencedPing,Attitude & attitude,Position & position,Ping & ping,SoundVelocityProfile & svp,Eigen::Vector3d & leverArm,Eigen::Matrix3d & boresight) {
    Eigen::Matrix3d imu2ned;
    CoordinateTransform::getDCM(imu2ned,attitude);

    //Center position wrt centroid
    Position pos(
      position.getTimestamp(),
      position.getLatitude() 		- centroid->getLatitude(),
      position.getLongitude()		- centroid->getLongitude(),
      position.getEllipsoidalHeight()	- centroid->getEllipsoidalHeight()
    );

    //Convert position's geographic coordinates to ECEF, and then from ECEF to NED
    Eigen::Vector3d positionECEF;
    CoordinateTransform::getPositionECEF(positionECEF,pos);
    Eigen::Vector3d positionNED = ecef2ned * positionECEF;

    //Convert ping to NED
    Eigen::Vector3d pingVector;
    Raytracing::rayTrace(pingVector,ping,svp);

    Eigen::Vector3d pingNED = imu2ned * boresight * pingVector;

    //Convert lever arm to NED
    Eigen::Vector3d leverArmNED =  imu2ned * leverArm;

    //Compute total NED vector

    georeferencedPing = positionNED + pingNED + leverArmNED;
  }

  /**
  * Sets centroid and inits ECEF 2 NED matrix
  */
  void setCentroid(Position * centroid){
    this->centroid = centroid;
    CoordinateTransform::ned2ecef(ecef2ned,*this->centroid);
    ecef2ned.transposeInPlace();
  }

  /**
  *  Get a pointer to the centroid
  */

  Position * getCentroid(){ return centroid;};

private:
  Position * centroid = NULL; //in geographic coordinates
  Eigen::Matrix3d ecef2ned;
};

#endif


#define EIGEN_USE_MKL_ALL
#define EIGEN_VECTORIZE_SSE4_2

#include <cstdio>
#include <iostream>
#include <fstream>
#include <ctime>
#include <cmath>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <gflags/gflags.h>
#include <boost/math/special_functions/bessel.hpp>

#include "frpca/frpca.h"
#include "frpca/matrix_vector_functions_intel_mkl.h"
#include "frpca/matrix_vector_functions_intel_mkl_ext.h"

using namespace std;
using namespace Eigen;
using namespace boost;

const float EPS = 0.00000000001f;
typedef Eigen::SparseMatrix<float, Eigen::RowMajor> SMatrixXf;

DEFINE_string(filename, "data/PPI.ungraph", "Filename for edgelist file.");
DEFINE_string(emb1, "sparse.emb", "Filename for svd results.");
DEFINE_string(emb2, "spectral.emb", "Filename for svd results.");
DEFINE_int32(num_node, 3890, "Number of node in the graph.");
DEFINE_int32(num_rank, 128, "Embedding dimension.");
DEFINE_int32(num_step, 10, "Number of order for recursion.");
DEFINE_int32(num_iter, 5, "Number of iter in randomized svd.");
DEFINE_int32(num_thread, 10, "Number of threads.");
DEFINE_double(theta, 0.5, "Parameter of ProNE");
DEFINE_double(mu, 0.1, "Parameter of ProNE");

SMatrixXf readGraph(string filename, int num_node){
    SMatrixXf A(num_node, num_node);
    typedef Eigen::Triplet<float> T;
    vector<T> tripletList;
    ifstream fin(filename.c_str());
    while (1)
    {
        string x, y;
        if (!(fin >> x >> y))
            break;
        int a = atoi(x.c_str()), b = atoi(y.c_str());
        if (a==b) continue;
        tripletList.push_back(T(a, b, 1));
        tripletList.push_back(T(b, a, 1));
    }
    A.setFromTriplets(tripletList.begin(), tripletList.end());
    return A;
}

SMatrixXf l1Normalize(SMatrixXf & mat){
    SMatrixXf mat2(mat.rows(), mat.cols());
    for (int k=0; k<mat.outerSize(); ++k){
        int num_neighbor = mat.row(k).sum();
        for (SMatrixXf::InnerIterator it(mat,k); it; ++it)
            mat2.insert(k, it.col()) = it.value()/num_neighbor;
    }
    return mat2;
}

MatrixXf & l2Normalize(MatrixXf & mat){
    for (int i = 0; i < mat.rows(); ++i){
        float ssn = sqrt(mat.row(i).squaredNorm());
        if (ssn < EPS) ssn = EPS;
        mat.row(i) = mat.row(i) / ssn;
      }
    return mat;
}

SMatrixXf & validate(SMatrixXf & mat){
    for (int k=0; k<mat.outerSize(); ++k)
          for (SMatrixXf::InnerIterator it(mat,k); it; ++it)
              if (it.value() <=0)
                mat.coeffRef(k, it.col()) = 1;
    return mat;
}

SMatrixXf & smfLog(SMatrixXf & mat){
    for (int k=0; k<mat.outerSize(); ++k)
          for (SMatrixXf::InnerIterator it(mat,k); it; ++it)
              mat.coeffRef(it.row(), it.col()) = log(it.value());
    return mat;
}

float bessel(int a, float b){
    return boost::math::cyl_bessel_i(a, b);
}

MatrixXf getEmbbeddingViaDenseSvd(MatrixXf &data, int rank){
    Eigen::BDCSVD<Eigen::MatrixXf> svdOfC(data, Eigen::ComputeThinU);
    MatrixXf emb = svdOfC.matrixU() * svdOfC.singularValues().cwiseSqrt().asDiagonal();
    emb = l2Normalize(emb);
    return emb;
}

MatrixXf runFrPCA(SMatrixXf & input, int rank, int iter)
{
    int m = input.rows(), nnz = input.nonZeros();
    mat_coo *A = coo_matrix_new(m, m, nnz);
    A->nnz = nnz;
    int i=0;
    for (int k=0; k<input.outerSize(); ++k)
        for (SMatrixXf::InnerIterator it(input,k); it; ++it)
          {
            A->rows[i] = k+1;
            A->cols[i] = it.col()+1;
            A->values[i] = it.value();
            i += 1;
          }
    cout << "read matrix done..." <<endl;
    // coo_matrix_print(A);

    //transform it to CSR format
    mat_csr* D = csr_matrix_new();
    csr_init_from_coo(D, A);
    coo_matrix_delete(A);

    //the test for frPCA
    mat *U = matrix_new(m, rank);
    mat *S = matrix_new(rank, 1);
    mat *V = matrix_new(m, rank);
    frPCA(D, &U, &S, &V, rank, iter);

    // matrix_print(U);
    // matrix_print(S);

    MatrixXf emb = MatrixXf::Random(m, rank);
    for (int i=0; i<m; i++)
        for (int j=0; j<rank; j++)
            emb(i, j) = matrix_get_element(U,i,j) *  sqrt(matrix_get_element(S,j,0));
    cout << "matrix decomposition done" <<endl;
    return emb;
}

MatrixXf getSparseEmbedding(SMatrixXf & A, int rank, int num_iter){
    time_t t1 = time(NULL);
    int row = A.rows(), col = A.cols();
    SMatrixXf B = l1Normalize(A);
    SMatrixXf C = B.transpose();
    SMatrixXf D(col, col), E(row, col), F(row, col);
    for (int i = 0; i < row; ++i){
        D.insert(i, i) = pow(C.row(i).sum(), 0.75);
    }

    D = D / D.sum();
    E = A * D;

    B = validate(B);
    E = validate(E);

    B = smfLog(B);
    E = smfLog(E);
    F = B - E;
    cout << "preprocess time: "<< (time(NULL) - t1 + 0.0) << endl;
    cout << "number of nnz: "<< F.nonZeros() <<endl;

    MatrixXf emb = runFrPCA(F, rank, num_iter);

    emb = l2Normalize(emb);
    return emb;
}

MatrixXf getSpectralEmbedding(SMatrixXf & A, MatrixXf & a, int step, float theta, float mu){
    time_t t1 = time(NULL);
    cout << "Chebyshev series --------------- " << endl;
    if (step==1) return a;
    int num_node = a.rows(), rank = a.cols();
    SMatrixXf I(num_node, num_node);
    for (int i = 0; i < num_node; ++i)
        I.insert(i, i) = 1;
    A = A + I;
    SMatrixXf B = l1Normalize(A);
    SMatrixXf L = I - B;
    SMatrixXf M = L - mu * I;

    MatrixXf Lx0 = a;
    MatrixXf Lx1 = M * a, Lx2;
    Lx1 = 0.5 * M * Lx1 - a;

    MatrixXf conv = bessel(0, theta)* Lx0;
    conv -= 2 * bessel(1, theta)* Lx1;
    for(int i=2; i<step; i++){
        Lx2 = M * Lx1;
        Lx2 = (M * Lx2 - 2 * Lx1) - Lx0;

        if (i % 2 == 0)
            conv += 2 * bessel(i, theta) * Lx2;
        else
            conv -= 2 * bessel(i, theta) * Lx2;
        Lx0 = Lx1;
        Lx1 = Lx2;
        cout << "Bessell time: " << i <<"\t"<< (time(NULL) - t1 + 0.0) << endl;
    }
    MatrixXf emb = A * (a - conv);
    cout << "Chebyshev time: "<< (time(NULL) - t1 + 0.0) << endl;

    // time_t t2 = time(NULL);
    // MatrixXf emb = getEmbbeddingViaDenseSvd(emb, rank);
    // cout << "dense svd time: "<< (time(NULL) - t2 + 0.0) << endl;
    emb = l2Normalize(emb);
    return emb;
}

void saveEmbedding(MatrixXf &data, string output){
    int m = data.rows(), d = data.cols();
    FILE *emb = fopen(output.c_str(), "wb");
    fprintf(emb, "%d %d\n", m, d);
    for (int i = 0; i < m; i++)
    {
        fprintf(emb, "%d", i);
        for (int j = 0; j < d; j++)
            fprintf(emb, " %f", data(i, j));
        fprintf(emb, "\n");
    }
    fclose(emb);
}

int main(int argc, char** argv)
{
    gflags::ParseCommandLineFlags(&argc, &argv, true);
    Eigen::setNbThreads(FLAGS_num_thread);

    time_t t1 = time(NULL);
    SMatrixXf A = readGraph(FLAGS_filename, FLAGS_num_node);

    MatrixXf feature = getSparseEmbedding(A, FLAGS_num_rank, FLAGS_num_iter);
    time_t t2 = time(NULL);
    cout << "Running time of get sparse embedding: " << (t2 - t1 + 0.0) << endl;

    MatrixXf embedding = getSpectralEmbedding(A, feature, FLAGS_num_step, FLAGS_theta, FLAGS_mu);
    time_t t3 = time(NULL);
    cout << "Running time of get spectral embedding: " << (t3 - t2 + 0.0)  << endl;
    cout << "Running time of ProNE: " << (t3 - t1 + 0.0) << endl;

    saveEmbedding(feature, FLAGS_emb1);
    saveEmbedding(embedding, FLAGS_emb2);
    cout << "Embedding save done " << endl;

}


#include <iostream>
#include <fstream>
#include <iomanip>
#include <math.h>
#include <vector>
#include <random>
#include <thread>
#include <boost/multi_array.hpp>
#include <boost/python.hpp>
#include <boost/python/numpy.hpp>

using namespace std;
namespace p = boost::python;
namespace np = boost::python::numpy;

#include "Layers.cpp"

// precision for floating numbers written in files
constexpr int prec_write_file = 16;

// define a standard normal distribution
std::random_device rd;
std::mt19937 gen(rd());
std::normal_distribution<> dis(0.,1.);

// uniform dictribution
std::uniform_real_distribution<> uni(0.,1.);

struct double_and_int{
	double d = 0.;
	int i = 0;
};

class CNN3{

	private:

		int num_CLs; // number of convolution layers
		int num_FCs; // number of FullCon layers
		int img_h_i; // image height
		int img_w_i; // image width
		int img_h; // image height before the fully connected layers
		int img_w; // image width before the fully connected layers
		int num_images; // number of images
		int num_labels; // number of different possible labels
		int n_channels; // number of channels

		vector<int> CL_size_filters; // Convolution layers: filter sizes
		vector<int> CL_num_filters; // Convolution layers: numbers of filters
		vector<int> MP_size; // Maxpool layers: pool sizes
		vector<int> FC_size; // FullCon layers: number of neurons

		// These four vectors will contain the layers
		vector<ConvLayer> CLs; // Convolution layers
		vector<ReLU> RLUs; // ReLU layers
		vector<MaxPool> MPs; // Maxpool layers
		vector<FullCon> FCs; // FullCon layers
		vector<SoftMax> SMs; // Softmax layers

	public:

		CNN3(){
			np::initialize(); // required to create numpy arrays (otherwise leads to segmentation faults)
		}

		CNN3(
			int img_w_, // image width
			int img_h_, // image height
			int n_channels_, // number of channels
			p::list& CL_size_filters_, // list of filter sizes
			p::list& CL_num_filters_, // list of numbers of filters
			p::list& MP_size_, // list of pool sizes
			p::list& FC_size_, // list of FullCon sizes
			int num_labels_ // number of different possible labels
		)
		{
			// initialization
			img_w_i = img_w_;
			img_h_i = img_h_;
			img_w = img_w_;
			img_h = img_h_;
			num_labels = num_labels_;
			n_channels = n_channels_;
			CL_size_filters = int_list_to_vector(CL_size_filters_);
			CL_num_filters = int_list_to_vector(CL_num_filters_);
			MP_size = int_list_to_vector(MP_size_);
			FC_size = int_list_to_vector(FC_size_);
			num_CLs = len(CL_size_filters_);
			num_FCs = len(FC_size_);

			num_images = n_channels; // tracks the number of images

			// build the layers
			for(int i=0; i<num_CLs; i++){
				CLs.push_back(ConvLayer(CL_size_filters[i], num_images, CL_num_filters[i], gen, dis));
				num_images = CL_num_filters[i];
				img_w = img_w + 1 - CL_size_filters[i];
				img_h = img_h + 1 - CL_size_filters[i];
				RLUs.push_back(ReLU());
				MPs.push_back(MaxPool(MP_size[i]));
				img_w = (int) img_w / MP_size[i];
				img_h = (int) img_h / MP_size[i];
			}
			long int n_inputs = num_images*img_h*img_w;
			for(int i=0; i<num_FCs; i++){
				int n_neurons = FC_size[i];
				FCs.push_back(FullCon(n_inputs, n_neurons, gen, dis));
				n_inputs = n_neurons;
			}
			SMs.push_back(SoftMax(n_inputs, num_labels, gen, dis));

			np::initialize(); // required to create numpy arrays (otherwise leads to segmentation faults)
		}

		// save the CNN parameters to a file
		void save(char* filename){
			ofstream file;
			file.open(filename);
			file << fixed << setprecision(prec_write_file);
			file << num_CLs << sep_val << num_FCs << sep_val << n_channels << sep_val << img_w_i << sep_val << img_h_i << sep_val << img_w << sep_val << img_h << sep_val << num_images << sep_line;
			save_vector(CL_size_filters, file);
			save_vector(CL_num_filters, file);
			save_vector(MP_size, file);
			for(int i=0; i<num_CLs; i++){
				CLs[i].save(file);
				RLUs[i].save(file);
				MPs[i].save(file);
			}
			for(int i=0; i<num_FCs; i++){
				FCs[i].save(file);
			}
			SMs[0].save(file);
			file.close();
		}

		// load the CNN parameters from a file
		void load(char* filename){
			ifstream file;
			file.open(filename);
			char c;
			file >> num_CLs >> c >> num_FCs >> c >> n_channels >> c >> img_w_i >> c >> img_h_i >> c >> img_w >> c >> img_h >> c >> num_images >> c;
			CL_size_filters = load_vector<int>(file);
			CL_num_filters = load_vector<int>(file);
			MP_size = load_vector<int>(file);
			CLs.clear();
			RLUs.clear();
			CLs.clear();
			SMs.clear();
			for(int i=0; i<num_CLs; i++){
				CLs.push_back(ConvLayer());
				CLs[i].load(file);
				RLUs.push_back(ReLU());
				RLUs[i].load(file);
				MPs.push_back(MaxPool());
				MPs[i].load(file);
			}
			for(int i=0; i<num_FCs; i++){
				FCs.push_back(FullCon());
				FCs[i].load(file);
			}
			SMs.push_back(SoftMax());
			SMs[0].load(file);
			file.close();
            num_labels = SMs[0].output.size();
		}

		// forward pass
		vector<double> forward(d3_array_type &input, double p_dropout = 0.){

			// number and dimensions of images
			int nim = input.shape()[0];
			int h = input.shape()[1];
			int w = input.shape()[2];
			if(h != img_h_i || w != img_w_i || nim != n_channels){
				cout << "\nInvalid input dimensions!\n" << endl;
			}
			for(int i=0; i<num_CLs; i++){
				d3_array_type output1 = CLs[i].forward(input);
				input.resize(boost::extents[output1.shape()[0]][output1.shape()[1]][output1.shape()[2]]);
				input = output1;

				d3_array_type output2 = MPs[i].forward(input);
				input.resize(boost::extents[output2.shape()[0]][output2.shape()[1]][output2.shape()[2]]);
				input = output2;

				d3_array_type output3 = RLUs[i].forward(input);
				input.resize(boost::extents[output3.shape()[0]][output3.shape()[1]][output3.shape()[2]]);
				input = output3;
			}

			vector<double> input_vec;
			auto input_shape = input.shape();
			for(int i=0; i<num_images; i++){
				for(int j=0; j<img_h; j++){
					for(int k=0; k<img_w; k++){
						input_vec.push_back(input[i][j][k]);
					}
				}
			}

			for(int i=0; i<num_FCs; i++){
				input_vec = FCs[i].forward(input_vec, gen, uni, p_dropout);
			}

			return SMs[0].forward(input_vec);
		}

		// backpropagation
		void backprop(vector<double> d_L_d_out_i, double learn_rate){

			d3_array_type d_L_d_in(boost::extents[num_images][img_h][img_w]);

			vector<double> d_L_d_in_vec = SMs[0].backprop(d_L_d_out_i, learn_rate);

			for(int i=num_FCs-1; i>=0; i--){
				d_L_d_in_vec = FCs[i].backprop(d_L_d_in_vec, learn_rate);
			}

			for(int i=0; i<num_images; i++){
				for(int j=0; j<img_h; j++){
					for(int k=0; k<img_w; k++){
						d_L_d_in[i][j][k] = d_L_d_in_vec[i*img_h*img_w + j*img_w + k];
					}
				}
			}

			for(int i=num_CLs-1; i>=0; i--){
				d3_array_type d_L_d_in3 = RLUs[i].backprop(d_L_d_in);
				auto shape = d_L_d_in3.shape();
				d_L_d_in.resize(boost::extents[shape[0]][shape[1]][shape[2]]);
				d_L_d_in = d_L_d_in3;

				d3_array_type d_L_d_in2 = MPs[i].backprop(d_L_d_in);
				shape = d_L_d_in2.shape();
				d_L_d_in.resize(boost::extents[shape[0]][shape[1]][shape[2]]);
				d_L_d_in = d_L_d_in2;

				d3_array_type d_L_d_in1 = CLs[i].backprop(d_L_d_in, learn_rate);
				shape = d_L_d_in1.shape();
				d_L_d_in.resize(boost::extents[shape[0]][shape[1]][shape[2]]);
				d_L_d_in = d_L_d_in1;
			}
		}

		// loss function and accuracy (1 if correct answer, 0 otherwise)
		double_and_int loss_acc(vector<double> &output, int label){
			double_and_int results;
			results.d = -log(output[label]);
			results.i = 1;
			for(int i=0; i<output.size(); i++){
				if(output[i] > output[label]){
					results.i = 0;
				}
			}
			return results;
		}

		// Completes a training step on the image 'image' with label 'label'.
		// Returns the corss-entropy and accuracy.
		double_and_int train(d3_array_type image, int label, double learn_rate = 0.005, double p_dropout = 0.) {

			// forward pass
			vector<double> output_forward = forward(image, p_dropout);

			// gradient of the loss function with respect to the output
			vector<double> d_L_d_out;
			for(int i=0; i<num_labels; i++){
				d_L_d_out.push_back(0.);
			}
			d_L_d_out[label] = -1./output_forward[label];

			// backpropagation
			backprop(d_L_d_out, learn_rate);

			// return loss and accuracy
			return loss_acc(output_forward, label);
		}

		// full forward propagation - Python wrapper
		// input: 2d numpy array
		np::ndarray forward_python(np::ndarray image){
			d3_array_type input = d3_numpy_to_multi_array(image);
			return vector_to_numpy(forward(input));
		}

		// forward - return loss and accuracy - Python wrapper
		p::list forward_la_python(np::ndarray image, int label){
			d3_array_type input = d3_numpy_to_multi_array(image);
			vector<double> output = forward(input);
			double_and_int results = loss_acc(output, label);
			p::list results_p;
			results_p.append(results.d);
			results_p.append(results.i);
			return results_p;
		}

		// full backpropagation - Python wrapper
		void backprop_python(np::ndarray d_L_d_out, double learn_rate){
			backprop(numpy_to_vector(d_L_d_out), learn_rate);
		}

		// train - Python wrapper
		p::list train_python(np::ndarray image, int label, double learn_rate, double p_dropout) {
			double_and_int results;
			results = train(d3_numpy_to_multi_array(image), label, learn_rate, p_dropout);
			p::list results_p;
			results_p.append(results.d);
			results_p.append(results.i);
			return results_p;
		}

};

BOOST_PYTHON_MODULE(CNN3)
{
    p::class_<CNN3>("CNN3", p::init<int, int, int, p::list&, p::list&, p::list&, p::list&, int>())
		.def(p::init<>())
		.def("forward", &CNN3::forward_python)
		.def("backprop", &CNN3::backprop_python)
		.def("train", &CNN3::train_python)
		.def("save", &CNN3::save)
		.def("load", &CNN3::load)
		.def("forward_la", &CNN3::forward_la_python)
	;
}


#include "formant.h"
#include <Eigen/Dense>

using namespace Analysis::Formant;
using Analysis::FormantResult;

using namespace Eigen;

struct Analysis::Formant::KarmaState
{
    int numF;
    VectorXd y;
    MatrixXd F;
    MatrixXd Q;
    MatrixXd R;
    VectorXd m_up;
    VectorXd P_up;
};

constexpr int ncep = 15;
constexpr int numF = 3;

Karma::Karma()
    : state(new KarmaState)
{
    state->F.setIdentity(2 * numF, 2 * numF);

    state->Q.setZero(2 * numF, 2 * numF);
    state->Q.diagonal().head(numF).setConstant(320 * 320);
    state->Q.diagonal().tail(numF).setConstant(100 * 100);

    state->R.setZero(ncep, ncep);
    for (int i = 0; i < ncep; ++i) {
        state->R(i, i) = 1.0 / (double) (i + 1);
    }

    VectorXd x0(2 * numF);
    x0 << 500, 1500, 2500,
           80,  120,  160;

    state->m_up = x0;
    state->P_up = state->Q;
}

Karma::~Karma()
{
    delete state;
}

static VectorXd calcCepstrumCoefs(const double *lpc, int lpcOrder, int ncep);
static MatrixXd calcMatrixH(const VectorXd& m, int cepOrder, double Fs);
static VectorXd calcCepstrumMapping(const VectorXd& m, int cepOrder, double Fs);

FormantResult Karma::solve(const double *lpc, int lpcOrder, double sampleRate)
{
    auto& F  = state->F;
    auto  Ft = state->F.transpose();
    auto& Q  = state->Q;
    auto& R  = state->R;

    auto m_pred = F * state->m_up;
    auto P_pred = F * state->P_up * Ft + Q;

    auto H  = calcMatrixH(m_pred, ncep, sampleRate);
    auto Ht = H.transpose();

    auto S = H * P_pred * Ht + R;
    auto K = (P_pred * Ht) * S.colPivHouseholderQr().inverse();

    auto y_pred = calcCepstrumMapping(m_pred, ncep, sampleRate);
    auto y      = calcCepstrumCoefs(lpc, lpcOrder, ncep);

    state->m_up = m_pred + K * (y - y_pred);
    state->P_up = P_pred - K * H * P_pred;

    rpm::vector<FormantData> formants(numF);
    for (int i = 0; i < numF; ++i) {
        formants[i] = {
            .frequency = state->m_up(i),
            .bandwidth = state->m_up(numF + i),
        };
    }

    return { .formants = formants };
}

VectorXd calcCepstrumCoefs(const double *lpc, int lpcOrder, int ncep)
{
    VectorXd C(ncep);

    for (int n = 1; n <= ncep; ++n) {
        if (n == 1) {
            C(n - 1) = lpc[n - 1];
        }
        else if (n <= lpcOrder) {
            C(n - 1) = lpc[n - 1];
            for (int i = 1; i <= ncep - 1; ++i) {
                C(n - 1) += (double) i / (double) n * lpc[n - i - 1] * C(i - 1);
            }
        }
        else {
            C(n - 1) = 0.0;
            for (int i = ncep - lpcOrder; i <= ncep - 1; ++i) {
                C(n - 1) += (double) i / (double) n * lpc[n - i - 1] * C(i - 1);
            }
        }
    }

    return C;
}

MatrixXd calcMatrixH(const VectorXd &m, int cepOrder, double Fs)
{
    const int numF = m.size() / 2;
    auto freq = m.segment(0, numF);
    auto band = m.segment(numF, numF);

    MatrixXd H(cepOrder, 2 * numF);

    for (int i = 0; i < cepOrder; ++i) {
        for (int j = 0; j < numF; ++j) {
            const double bwTerm = exp((-M_PI * (i + 1) * band(j)) / Fs);

            H(i, j)        = -4.0 * M_PI / Fs * bwTerm * sin((2.0 * M_PI * (i + 1) * freq(j)) / Fs);
            H(i, numF + j) = -2.0 * M_PI / Fs * bwTerm * cos((2.0 * M_PI * (i + 1) * freq(j)) / Fs);
        }
    }

    return H;
}

VectorXd calcCepstrumMapping(const VectorXd& m, int cepOrder, double Fs)
{
    const int numF = m.size() / 2;
    auto freq = m.segment(0, numF);
    auto band = m.segment(numF, numF);

    VectorXd C_int(numF);
    VectorXd C(cepOrder);

    for (int i = 0; i < cepOrder; ++i) {
        for (int p = 0; p < numF; ++p) {
            const double bwTerm = (2.0 / (double) (i + 1)) * exp((-M_PI * (i + 1) * band(p)) / Fs);
            C_int(p) = bwTerm * cos((2.0 * M_PI * (i + 1) * freq(p)) / Fs);
        }
        C(i) = C_int.sum();
    }

    return C;
}


#include <iostream>
#include <armadillo>

#ifndef MLMATH
#define MLMATH

#include "MLMath.h"

#endif

#ifndef PLOTTER
#define PLOTTER

#include "plot.h"

#endif

using namespace std;
using namespace arma;

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

  // for(int i=0; i<argc; i++){
  //   cout << argv[i] << endl;
  // }

  if(argc > 1){
    cout << "param: " << argv[1] << endl;
    string param_1 = argv[1];
    if(param_1 == "test"){
      cout << "running all tests" << endl;
      cout << "------------------------------" << endl;
    }
  }

  // if(test == "test"){
  //   cout << "testing: " << test << endl;
  // }else{
  //   cout << "not testing: " << test << endl;
  // }

  vec identity, input;
  mat hypotheses, inputMatrix;

  //the input values in single value calculations
  // input << 2104 << 1416 << 1534 << 852;

  //identity matrix for input
  // identity << 1 << 1 << 1 << 1;

  //join identity matrix to input values
  // inputMatrix = join_rows(identity,input);
  inputMatrix.load("input.txt", raw_ascii);

  //load hypothesis values
  hypotheses.load("hypotheses.txt", raw_ascii);

  //create MLMath object with inputmatrix, and hypotheses
  MLMath<mat> math(inputMatrix,hypotheses);

  //calculate input matrix times hypotheses matrix
  math.linearMultiply();

  //demo_3d();

  //callthis();
  //return 0;

}


// Copyright (c) Dewetron 2017
#include "otfft.h"

#undef VALGRIND_MEM_LEAK_DETECTION

#include <boost/test/unit_test.hpp>

#include <algorithm>
#include <cmath>
#include <cstdint>
#include <vector>

namespace
{
    /**
     * Test that spectrum of a delta signal is constant.
     */
    template <std::size_t SIZE>
    void deltaSpectrumTest()
    {
        // delta signal: 1, 0, 0, 0...
        double testArray[SIZE] = {0};
        testArray[0] = 1;

        std::vector<OTFFT::complex_t> spectrum(SIZE);
        {
            auto fft = OTFFT::Factory::createRealFFT(static_cast<int>(SIZE));
            OTFFT::double_vector fft_in{testArray};
            OTFFT::complex_vector fft_out{spectrum.data()};
            fft->fwd0(fft_in, fft_out);
        }

        double absSpectrum[SIZE] = {0};
        std::transform(std::begin(spectrum), std::end(spectrum), absSpectrum, [] (OTFFT::complex_t x) {
            return std::sqrt(OTFFT::norm(x));
        });

        double expected[SIZE];
        std::fill_n(expected, SIZE, 1.0);
        for (std::size_t idx{0}; idx < SIZE / 2; ++idx)
        {
            if (std::fabs(expected[idx]) < 1e-10)
            {
                BOOST_CHECK_SMALL(absSpectrum[idx], 1e-8);
            }
            else
            {
                BOOST_CHECK_CLOSE(expected[idx], absSpectrum[idx], .1);
            }
        }
    }

    /**
     * Test that spectrum of a constant signal is a scaled delta.
     */
    template <std::size_t SIZE>
    void constSpectrumTest()
    {
        double testArray[SIZE];
        std::fill_n(testArray, SIZE, 1.0);

        std::vector<OTFFT::complex_t> spectrum(SIZE);
        {
            auto fft = OTFFT::Factory::createRealFFT(static_cast<int>(SIZE));
            OTFFT::double_vector fft_in{testArray};
            OTFFT::complex_vector fft_out{spectrum.data()};
            fft->fwd0(fft_in, fft_out);
        }

        double absSpectrum[SIZE] = {0};
        std::transform(std::begin(spectrum), std::end(spectrum), absSpectrum, [] (OTFFT::complex_t x) {
            return std::sqrt(OTFFT::norm(x));
        });

        // expecting a delta scaled by SIZE
        double expected[SIZE] = {0};
        expected[0] = SIZE * 1.0;
        for (std::size_t idx{0}; idx < SIZE / 2; ++idx)
        {
            if (std::fabs(expected[idx]) < 1e-10)
            {
                BOOST_CHECK_SMALL(absSpectrum[idx], 1e-8);
            }
            else
            {
                BOOST_CHECK_CLOSE(expected[idx], absSpectrum[idx], .1);
            }
        }
    }

    /**
     * Test that inverse of a delta spectrum is a constant signal.
     */
    template <std::size_t SIZE>
    void deltaInverseTest()
    {
        std::vector<OTFFT::complex_t> spectrum(SIZE, 0);
        spectrum[0] = SIZE * 1.0;
        std::vector<double> output(SIZE);
        {
            auto fft = OTFFT::Factory::createRealFFT(static_cast<int>(SIZE));
            OTFFT::complex_vector fft_in{spectrum.data()};
            OTFFT::double_vector fft_out{output.data()};
            fft->invn(fft_in, fft_out);
        }

        double expected[SIZE];
        std::fill_n(expected, SIZE, 1.0);
        for (std::size_t idx{0}; idx < SIZE / 2; ++idx)
        {
            if (std::fabs(expected[idx]) < 1e-10)
            {
                BOOST_CHECK_SMALL(output[idx], 1e-8);
            }
            else
            {
                BOOST_CHECK_CLOSE(expected[idx], output[idx], .1);
            }
        }
    }

    /**
     * Test that inverse of a constant spectrum is a delta signal.
     */
    template <std::size_t SIZE>
    void constInverseTest()
    {
        std::vector<OTFFT::complex_t> spectrum(SIZE, 1.0);
        std::vector<double> output(SIZE);
        {
            auto fft = OTFFT::Factory::createRealFFT(static_cast<int>(SIZE));
            OTFFT::complex_vector fft_in{spectrum.data()};
            OTFFT::double_vector fft_out{output.data()};
            fft->invn(fft_in, fft_out);
        }

        double expected[SIZE] = {0};
        expected[0] = 1.0;
        for (std::size_t idx{0}; idx < SIZE / 2; ++idx)
        {
            if (std::fabs(expected[idx]) < 1e-10)
            {
                BOOST_CHECK_SMALL(output[idx], 1e-8);
            }
            else
            {
                BOOST_CHECK_CLOSE(expected[idx], output[idx], .1);
            }
        }
    }

    /**
     * Test that IFFT(FFT(x)) == x.
     */
    template <std::size_t SIZE>
    void identityTest()
    {
        std::vector<double> testArray(SIZE);
        std::fill_n(testArray.begin(), SIZE, 1.0);

        std::vector<OTFFT::complex_t> spectrum(SIZE);
        {
            auto fft = OTFFT::Factory::createRealFFT(static_cast<int>(SIZE));
            OTFFT::double_vector fft_in{testArray.data()};
            OTFFT::complex_vector fft_out{spectrum.data()};
            fft->fwd(fft_in, fft_out);
        }

        std::vector<double> output(SIZE);
        {
            auto fft = OTFFT::Factory::createRealFFT(static_cast<int>(SIZE));
            OTFFT::complex_vector fft_in{spectrum.data()};
            OTFFT::double_vector fft_out{output.data()};
            fft->inv(fft_in, fft_out);
        }

        for (std::size_t idx{0}; idx < SIZE / 2; ++idx)
        {
            BOOST_CHECK_CLOSE(testArray[idx], output[idx], .1);
        }
    }

    void fftUtilize(std::size_t fft_size, OTFFT::TransformationType fft_type)
    {
        switch (fft_type)
        {
        case OTFFT::TransformationType::TRANSFORM_FFT_REAL:
            {
                std::vector<double> workspace_real(fft_size, 1.0);
                std::vector<OTFFT::complex_t> workspace_complex(fft_size);
                {
                    auto fft = OTFFT::Factory::createRealFFT(static_cast<int>(fft_size));
                    OTFFT::double_vector fft_in{workspace_real.data()};
                    OTFFT::complex_vector fft_out{workspace_complex.data()};
                    fft->fwd(fft_in, fft_out);
                }

                {
                    auto fft = OTFFT::Factory::createRealFFT(static_cast<int>(fft_size));
                    OTFFT::complex_vector fft_in{workspace_complex.data()};
                    OTFFT::double_vector fft_out{workspace_real.data()};
                    fft->inv(fft_in, fft_out);
                }
            }
            break;
        case OTFFT::TransformationType::TRANSFORM_FFT_COMPLEX:
            {
                std::vector<OTFFT::complex_t> workspace(fft_size, OTFFT::complex_t(1.0, 1.0));
                {
                    auto fft = OTFFT::Factory::createComplexFFT(static_cast<int>(fft_size));
                    OTFFT::complex_vector workspace_ptr{workspace.data()};
                    fft->fwd(workspace_ptr);
                }

                {
                    auto fft = OTFFT::Factory::createComplexFFT(static_cast<int>(fft_size));
                    OTFFT::complex_vector workspace_ptr{workspace.data()};
                    fft->inv(workspace_ptr);
                }
            }
            break;
        case OTFFT::TransformationType::TRANSFORM_DCT:
            {
                std::vector<double> workspace(fft_size, 1.0);
                {
                    auto fft = OTFFT::Factory::createDCT(static_cast<int>(fft_size));
                    OTFFT::double_vector workspace_ptr{workspace.data()};
                    fft->fwd(workspace_ptr);
                }

                {
                    auto fft = OTFFT::Factory::createDCT(static_cast<int>(fft_size));
                    OTFFT::double_vector workspace_ptr{workspace.data()};
                    fft->inv(workspace_ptr);
                }
            }
            break;
        case OTFFT::TransformationType::TRANSFORM_BLUESTEIN:
            {
                std::vector<OTFFT::complex_t> workspace(fft_size, OTFFT::complex_t(1.0, 1.0));
                {
                    auto fft = OTFFT::Factory::createBluesteinFFT(static_cast<int>(fft_size));
                    OTFFT::complex_vector workspace_ptr{workspace.data()};
                    fft->fwd(workspace_ptr);
                }

                {
                    auto fft = OTFFT::Factory::createBluesteinFFT(static_cast<int>(fft_size));
                    OTFFT::complex_vector workspace_ptr{workspace.data()};
                    fft->inv(workspace_ptr);
                }
            }
            break;
        }
    }
}

BOOST_AUTO_TEST_SUITE(otfft_transform_test)

BOOST_AUTO_TEST_CASE(TestDeltaSpectrum)
{
    deltaSpectrumTest<8>();
    deltaSpectrumTest<16>();
    deltaSpectrumTest<256>();
    deltaSpectrumTest<512>();
    deltaSpectrumTest<1024>();
    deltaSpectrumTest<8192>();
    deltaSpectrumTest<16384>();
}

BOOST_AUTO_TEST_CASE(TestConstSpectrum)
{
    constSpectrumTest<8>();
    constSpectrumTest<16>();
    constSpectrumTest<256>();
    constSpectrumTest<512>();
    constSpectrumTest<1024>();
    constSpectrumTest<8192>();
    constSpectrumTest<16384>();
}

BOOST_AUTO_TEST_CASE(TestDeltaInverse)
{
    deltaInverseTest<8>();
    deltaInverseTest<16>();
    deltaInverseTest<256>();
    deltaInverseTest<512>();
    deltaInverseTest<1024>();
    deltaInverseTest<8192>();
    deltaInverseTest<16384>();
}

BOOST_AUTO_TEST_CASE(TestConstInverse)
{
    constInverseTest<8>();
    constInverseTest<16>();
    constInverseTest<256>();
    constInverseTest<512>();
    constInverseTest<1024>();
    constInverseTest<8192>();
    constInverseTest<16384>();
}

BOOST_AUTO_TEST_CASE(TestIdentity)
{
    identityTest<8>();
    identityTest<16>();
    identityTest<256>();
    identityTest<512>();
    identityTest<1024>();
    identityTest<8192>();
    identityTest<16384>();
}

#ifdef VALGRIND_MEM_LEAK_DETECTION
BOOST_AUTO_TEST_CASE(TestValgrind)
{
    const std::size_t N = static_cast<std::size_t>(std::pow(2, 24));

    for (std::size_t n = 8; n < N;)
    {
        fftUtilize(n, OTFFT::TransformationType::TRANSFORM_FFT_COMPLEX);
        fftUtilize(n, OTFFT::TransformationType::TRANSFORM_BLUESTEIN);

        if ((n & 1) == 0)
        {
            fftUtilize(n, OTFFT::TransformationType::TRANSFORM_FFT_REAL);
            fftUtilize(n, OTFFT::TransformationType::TRANSFORM_DCT);
        }

        const std::size_t log_n = static_cast<std::size_t>(std::log2(n));
        n += static_cast<std::size_t>(std::ceil(std::pow(log_n, std::sqrt(log_n))));
    }

    BOOST_CHECK(true);
}
#endif

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#include <armadillo>

class AmbientFluid;

/*!
 * Namespace for some utilities.
 */
namespace utils {

    /*!
     * \brief Calculate thermal conductivity of natural gas at given pressure.
     *
     * This is some kind of empirical relation or fitting, with an unknown source.
     * It should probably be replaced with something else. See for example
     * A simple correlation to estimate natural gas thermal conductivity (Azad Jarrahiana and Ehsan Heidaryan, Journal of Natural Gas Science and Engineering, Volume 18, May 2014).
     *
     * \param pressure Gas pressure [Pa].
     * \return Thermal conductivity [W/(m K)]
     */
    double calcGasThermalConductivity(const double pressure);

    /*!
     * \brief Calculate outer film coefficient for a given outer diameter and
     * AmbientFluid.
     *
     * This is just a wrapper around utils::calcOuterWallFilmCoefficient(const double, const double, const double, const double, const double, const double)
     *
     * \param diameter Outer diameter [m]
     * \param fluid AmbientFluid describing the fluid
     * \return Outer film coefficient [W/(m2 K)]
     */
    double calcOuterWallFilmCoefficient(
            const double diameter,
            const AmbientFluid& fluid
            );

    /*!
     * \brief Calculate the outer film coefficient for external flow normal to a
     * circular cylinder.
     *
     * Uses eq. 7.52 from Fundamentals of heat and mass transfer (7th Ed, 2011) (Bergman, Lavine, Incropera, DeWitt).
     *
     * \param diameter Outer diameter [m]
     * \param heatCapacityConstantPressure Fluid heat capacity (\f$c_p\f$) [J/(kg K)]
     * \param viscosity Fluid dynamic viscosity [Pa s] = [kg/m*s]
     * \param thermalConductivity Fluid thermal conductivity [W/(m K)]
     * \param density Fluid density [kg/m3]
     * \param velocity Fluid velocity [m/s]
     * \return Outer film coefficient [W/(m2 K)]
     */
    double calcOuterWallFilmCoefficient(
            const double diameter,
            const double heatCapacityConstantPressure = 4200, // [J/kg K]
            const double viscosity = 1.05/1000.0, // [Pa s] = [kg/m*s]
            const double thermalConductivity = 0.57, // [W/m K]
            const double density = 1020, // [kg/m3]
            const double velocity = 0.1 // [m/s]
            );

    /*!
     * \brief Calculate inner wall film coefficient for flow inside a cylinder.
     *
     * This uses the Dittus-Boelter equation at Reynolds numbers abve 1e4,
     * eq. 8.55 Fundamentals of heat and mass transfer (7th Ed, 2011) (Bergman, Lavine, Incropera, DeWitt)
     * at Reynolds number between 1e4 and 4e4, and returns 0 below 4e4.
     *
     * \param diameter Inner diameter [m]
     * \param fluidPressure Fluid pressure [Pa]
     * \param fluidReynoldsNumber Fluid Reynolds number [-]
     * \param fluidHeatCapacityConstantPressure Fluid heat capacity (\f$c_p\f$) [J/(kg K)]
     * \param fluidViscosity Fluid dynamic viscosity [Pa s] = [kg/m*s]
     * \return Inner film coefficient [W/(m2 K)]
     */
    double calcInnerWallFilmCoefficient(
            const double diameter,
            const double fluidPressure,
            const double fluidReynoldsNumber,
            const double fluidHeatCapacityConstantPressure,
            const double fluidViscosity);

    /*!
     * \brief Calculate equivalent burial layer thickness.
     *
     * For a pipeline buried in a medium at a given depth, this function
     * calculates the thickness of an equivalent cylinder shell of the same
     * medium around the pipeline which gives the same heat transfer between
     * the fluid in the pipeline and the ambient medium. The calculation is
     * based on equations in the documentation of the OLGA simulation software.
     *
     * Using the thickness from this function ensures the same results are
     * achieved with steady state and unsteady heat transfer models.
     *
     * \see utils::calcEquivalentBurialLayerRadius()
     *
     * \param innerDiameter Pipeline inner diameter [m]
     * \param wallThickness Pipeline wall thickness [m]
     * \param burialDepth Distance from top of pipe to top of burial medium [m]
     * \param burialMediumConductivity Thermal conductivity of burial medium [W/(m K)]
     * \return
     */
    double calcEquivalentBurialLayerWidth(
            const double innerDiameter,
            const double wallThickness,
            const double burialDepth,
            const double burialMediumConductivity = 2.0);

    /*!
     * \brief calcEquivalentBurialLayerRadius
     *
     * For a pipeline buried in a medium at a given depth, this function
     * calculates the outer radius of an equivalent cylinder shell of the same
     * medium around the pipeline which gives the same heat transfer between
     * the fluid in the pipeline and the ambient medium. The calculation is
     * based on equations in the documentation of the OLGA simulation software.
     *
     * \param innerDiameter Pipeline inner diameter [m]
     * \param wallThickness Pipeline wall thickness [m]
     * \param burialDepth Distance from top of pipe to top of burial medium [m]
     * \param burialMediumConductivity Thermal conductivity of burial medium [W/(m K)]
     * \return Outer radius of equivalent cylinder shell of burial medium [m]
     */
    double calcEquivalentBurialLayerRadius(
            const double innerDiameter,
            const double wallThickness,
            const double burialDepth,
            const double burialMediumConductivity = 2.0);

    /*!
     * \brief Calculate logarithmically (log10) spaced cylinder shell widths.
     *
     * This function calculates the widths of nShells cylinder shells,
     * logarithmically (log10) spaced between innerRadius and outerRadius.
     *
     * \param innerRadius Inner radius [m]
     * \param outerRadius Outer radius [m]
     * \param nShells Number of cylinder shells
     * \return arma::vec of shell widths [m]
     */
    arma::vec calcLogSpacedShellWidths(
            const double innerRadius,
            const double outerRadius,
            const arma::uword nShells = 10);

    /*!
     * \brief Calculate the widths of equivalent burial cylinder shells.
     *
     * This is just a wrapper around utils::calcEquivalentBurialLayerRadius()
     * and utils::calcLogSpacedShellWidths() that divides the shell into
     * several logarithmically spaced shells.
     *
     * \see utils::calcEquivalentBurialLayerRadius()
     * \see utils::calcLogSpacedShellWidths()
     *
     * \param innerDiameter Inner diameter [m]
     * \param wallThickness Wall thickness [m]
     * \param burialDepth Distance from top of pipe to top of burial medium [m]
     * \param burialMediumConductivity Thermal conductivity of burial medium [W/(m K)]
     * \param nShells Number of shells
     * \return Widths of logarithmically spaced cylinder shells.
     */
    arma::vec calcEquivalentBurialLayerWidths(
            const double innerDiameter,
            const double wallThickness,
            const double burialDepth,
            const double burialMediumConductivity = 2.0,
            const arma::uword nShells = 10);
}


#include "trinary.h"
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE(_1_yields_decimal_1)
{
    BOOST_REQUIRE_EQUAL(1, trinary::to_decimal("1"));
}

BOOST_AUTO_TEST_CASE(_2_yields_decimal_2)
{
    BOOST_REQUIRE_EQUAL(2, trinary::to_decimal("2"));
}

BOOST_AUTO_TEST_CASE(_10_yields_decimal_3)
{
    BOOST_REQUIRE_EQUAL(3, trinary::to_decimal("10"));
}

BOOST_AUTO_TEST_CASE(_11_yields_decimal_4)
{
    BOOST_REQUIRE_EQUAL(4, trinary::to_decimal("11"));
}

BOOST_AUTO_TEST_CASE(_100_yields_decimal_9)
{
    BOOST_REQUIRE_EQUAL(9, trinary::to_decimal("100"));
}

BOOST_AUTO_TEST_CASE(_112_yields_decimal_14)
{
    BOOST_REQUIRE_EQUAL(14, trinary::to_decimal("112"));
}

BOOST_AUTO_TEST_CASE(_222_yields_decimal_26)
{
    BOOST_REQUIRE_EQUAL(26, trinary::to_decimal("222"));
}

BOOST_AUTO_TEST_CASE(_1122000120_yields_decimal_32091)
{
    BOOST_REQUIRE_EQUAL(32091, trinary::to_decimal("1122000120"));
}

BOOST_AUTO_TEST_CASE(invalid_yields_decimal_0)
{
    BOOST_REQUIRE_EQUAL(0, trinary::to_decimal("carrot"));
}
#if defined(EXERCISM_RUN_ALL_TESTS)
#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//[register_box_2d_4values
//` Show the use of the macro BOOST_GEOMETRY_REGISTER_BOX_2D_4VALUES

#include <iostream>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/register/point.hpp>
#include <boost/geometry/geometries/register/box.hpp>

struct my_point
{
    int x, y;
};

struct my_box
{
    int left, top, right, bottom;
};

BOOST_GEOMETRY_REGISTER_POINT_2D(my_point, int, cs::cartesian, x, y)

// Register the box type, also notifying that it is based on "my_point"
// (even if it does not contain it)
BOOST_GEOMETRY_REGISTER_BOX_2D_4VALUES(my_box, my_point, left, top, right, bottom)

int main()
{
    my_box b = boost::geometry::make<my_box>(0, 0, 2, 2);
    std::cout << "Area: "  << boost::geometry::area(b) << std::endl;
    return 0;
}

//]

//[register_box_2d_4values_output
/*`
Output:
[pre
Area: 4
]
*/
//]


/*****************************************************************************
 * slicing.cpp        Blitz++ Array slicing & subarrays example
 *****************************************************************************/

#include <blitz/array.h>

BZ_USING_NAMESPACE(blitz)

int main()
{
    Array<int,2> A(6,6), B(3,3);

    // Set the upper left quadrant of A to 5
    A(Range(0,2), Range(0,2)) = 5;

    // Set the upper right quadrant of A to an identity matrix
    B = 1, 0, 0,
        0, 1, 0,
        0, 0, 1;
    A(Range(0,2), Range(3,5)) = B;

    // Set the fourth row to 1

#ifdef BZ_HAVE_PARTIAL_ORDERING
    A(3, Range::all()) = 1;
#else
    cout << "Warning: your compiler does not support partial ordering of"
         << endl << "member templates; using kludge." << endl;
    A(Range(3,3), Range::all()) = 1;
#endif

    // Set the last two rows to 0
    A(Range(4, toEnd), Range::all()) = 0;

    // Set the bottom right element to 8
    A(5,5) = 8;

    cout << "A = " << A << endl;

    return 0;
}


//==============================================================================
//         Copyright 2015 - J.T. Lapreste
//
//          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
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_SCALAR_TENPOWER_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SCALAR_TENPOWER_HPP_INCLUDED

#include <boost/simd/arithmetic/functions/tenpower.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/ten.hpp>
#include <boost/simd/include/functions/abs.hpp>
#include <boost/simd/include/functions/sqr.hpp>
#include <boost/simd/include/functions/scalar/is_odd.hpp>
#include <boost/simd/include/functions/scalar/rec.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( tenpower_, tag::cpu_
                            , (A0)
                            , (scalar_< int_<A0> >)
                            )
  {
    typedef  typename dispatch::meta::as_floating<A0>::type result_type;

    BOOST_FORCEINLINE result_type operator()(A0 exp) const
    {
      result_type result = One<result_type>();
      result_type base = Ten<result_type>();
      bool neg = exp < 0;
      exp =  boost::simd::abs(exp);
      while(exp)
      {
        if (is_odd(exp)) result *= base;
        exp >>= 1;
        base = sqr(base);
      }
      return neg ? rec(result) : result;
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( tenpower_, tag::cpu_
                            , (A0)
                            , (scalar_< uint_<A0> >)
                            )
  {
    typedef  typename dispatch::meta::as_floating<A0>::type result_type;

    BOOST_FORCEINLINE result_type operator()(A0 exp) const
    {
      result_type result = One<result_type>();
      result_type base = Ten<result_type>();
      while(exp)
      {
        if (is_odd(exp)) result *= base;
        exp >>= 1;
        base = sqr(base);
      }
      return result;
    }
  };
} } }

#endif


// Copyright (c) 2020 Chris Richardson & Garth Wells
// FEniCS Project
// SPDX-License-Identifier:    MIT

#include "lattice.h"
#include "cell.h"
#include "elements/lagrange.h"
#include "quadrature.h"
#include <Eigen/Dense>

using namespace basix;

namespace
{
//-----------------------------------------------------------------------------
Eigen::ArrayXd warp_function(int n, Eigen::ArrayXd& x)
{
  [[maybe_unused]] auto [pts, wts]
      = quadrature::gauss_lobatto_legendre_line_rule(n + 1);
  wts.setZero();

  pts *= 0.5;
  for (int i = 0; i < n + 1; ++i)
    pts[i] += (0.5 - static_cast<double>(i) / static_cast<double>(n));

  FiniteElement L = create_dlagrange(cell::type::interval, n);
  Eigen::MatrixXd v = L.tabulate(0, x)[0];
  return v * pts.matrix();
}
//-----------------------------------------------------------------------------

} // namespace

//-----------------------------------------------------------------------------
Eigen::ArrayXXd lattice::create(cell::type celltype, int n,
                                lattice::type lattice_type, bool exterior)
{
  switch (celltype)
  {
  case cell::type::point:
    return Eigen::ArrayXXd::Zero(1, 1);
  case cell::type::interval:
  {
    if (n == 0)
      return Eigen::ArrayXXd::Constant(1, 1, 0.5);

    Eigen::ArrayXd x;
    if (exterior)
      x = Eigen::VectorXd::LinSpaced(n + 1, 0.0, 1.0);
    else
    {
      const double h = 1.0 / static_cast<double>(n);
      x = Eigen::VectorXd::LinSpaced(n - 1, h, 1.0 - h);
    }

    if (lattice_type == lattice::type::gll_warped)
      x += warp_function(n, x);

    return x;
  }
  case cell::type::quadrilateral:
  {
    if (n == 0)
      return Eigen::ArrayXXd::Constant(1, 2, 0.5);

    Eigen::ArrayXd r;
    if (exterior)
      r = Eigen::VectorXd::LinSpaced(n + 1, 0.0, 1.0);
    else
    {
      const double h = 1.0 / static_cast<double>(n);
      r = Eigen::VectorXd::LinSpaced(n - 1, h, 1.0 - h);
    }

    if (lattice_type == lattice::type::gll_warped)
      r += warp_function(n, r);

    const int m = r.size();
    Eigen::ArrayX2d x(m * m, 2);
    int c = 0;
    for (int j = 0; j < m; ++j)
      for (int i = 0; i < m; ++i)
        x.row(c++) << r[i], r[j];

    return x;
  }
  case cell::type::hexahedron:
  {
    if (n == 0)
      return Eigen::ArrayXXd::Constant(1, 3, 0.5);

    Eigen::ArrayXd r;
    if (exterior)
      r = Eigen::VectorXd::LinSpaced(n + 1, 0.0, 1.0);
    else
    {
      const double h = 1.0 / static_cast<double>(n);
      r = Eigen::VectorXd::LinSpaced(n - 1, h, 1.0 - h);
    }
    if (lattice_type == lattice::type::gll_warped)
      r += warp_function(n, r);

    const int m = r.size();
    Eigen::ArrayXXd x(m * m * m, 3);
    int c = 0;
    for (int k = 0; k < m; ++k)
      for (int j = 0; j < m; ++j)
        for (int i = 0; i < m; ++i)
          x.row(c++) << r[i], r[j], r[k];

    return x;
  }
  case cell::type::triangle:
  {
    if (n == 0)
      return Eigen::ArrayXXd::Constant(1, 2, 1.0 / 3.0);

    // Warp points: see Hesthaven and Warburton, Nodal Discontinuous Galerkin
    // Methods, pp. 175-180

    const int b = exterior ? 0 : 1;

    // Points
    Eigen::ArrayX2d p((n - 3 * b + 1) * (n - 3 * b + 2) / 2, 2);

    // Displacement from GLL points in 1D, scaled by 1/(r(1-r))
    Eigen::ArrayXd r = Eigen::VectorXd::LinSpaced(2 * n + 1, 0.0, 1.0);
    Eigen::ArrayXd wbar = warp_function(n, r);
    const auto s = r.segment(1, 2 * n - 1);
    wbar.segment(1, 2 * n - 1) /= s * (1 - s);

    int c = 0;
    for (int j = b; j < (n - b + 1); ++j)
    {
      for (int i = b; i < (n - b + 1 - j); ++i)
      {
        const int l = n - j - i;
        const double x = r[2 * i];
        const double y = r[2 * j];
        const double a = r[2 * l];
        p.row(c) << x, y;
        if (lattice_type == lattice::type::gll_warped)
        {
          p(c, 0) += x * (a * wbar(n + i - l) + y * wbar(n + i - j));
          p(c, 1) += y * (a * wbar(n + j - l) + x * wbar(n + j - i));
        }

        ++c;
      }
    }

    return p;
  }
  case cell::type::tetrahedron:
  {
    if (n == 0)
      return Eigen::ArrayXXd::Constant(1, 3, 0.25);

    const int b = exterior ? 0 : 1;
    Eigen::ArrayX3d p((n - 4 * b + 1) * (n - 4 * b + 2) * (n - 4 * b + 3) / 6,
                      3);
    Eigen::ArrayXd r = Eigen::VectorXd::LinSpaced(2 * n + 1, 0.0, 1.0);
    Eigen::ArrayXd wbar = warp_function(n, r);
    const auto s = r.segment(1, 2 * n - 1);
    wbar.segment(1, 2 * n - 1) /= s * (1 - s);
    int c = 0;
    for (int k = b; k < (n - b + 1); ++k)
    {
      for (int j = b; j < (n - b + 1 - k); ++j)
      {
        for (int i = b; i < (n - b + 1 - j - k); ++i)
        {
          const int l = n - k - j - i;
          const double x = r[2 * i];
          const double y = r[2 * j];
          const double z = r[2 * k];
          const double a = r[2 * l];
          p.row(c) << x, y, z;
          if (lattice_type == lattice::type::gll_warped)
          {
            const double dx = x
                              * (a * wbar(n + i - l) + y * wbar(n + i - j)
                                 + z * wbar(n + i - k));
            const double dy = y
                              * (a * wbar(n + j - l) + z * wbar(n + j - k)
                                 + x * wbar(n + j - i));
            const double dz = z
                              * (a * wbar(n + k - l) + x * wbar(n + k - i)
                                 + y * wbar(n + k - j));
            p(c, 0) += dx;
            p(c, 1) += dy;
            p(c, 2) += dz;
          }

          ++c;
        }
      }
    }

    return p;
  }
  case cell::type::prism:
  {
    if (n == 0)
    {
      Eigen::ArrayXXd x = Eigen::ArrayXXd::Constant(1, 3, 1.0 / 3.0);
      x(0, 2) = 0.5;
      return x;
    }

    const Eigen::ArrayXXd tri_pts
        = lattice::create(cell::type::triangle, n, lattice_type, exterior);
    const Eigen::ArrayXXd line_pts
        = lattice::create(cell::type::interval, n, lattice_type, exterior);

    Eigen::ArrayX3d x(tri_pts.rows() * line_pts.rows(), 3);
    x.leftCols(2) = tri_pts.replicate(line_pts.rows(), 1);
    for (int i = 0; i < line_pts.rows(); ++i)
      x.block(i * tri_pts.rows(), 2, tri_pts.rows(), 1) = line_pts(i, 0);
    return x;
  }
  case cell::type::pyramid:
  {
    if (n == 0)
    {
      Eigen::ArrayXXd x = Eigen::ArrayXXd::Constant(1, 3, 0.4);
      x(0, 2) = 0.2;
      return x;
    }
    else
    {
      const double h = 1.0 / static_cast<double>(n);

      // Interpolate warp factor along interval
      std::tuple<Eigen::ArrayXXd, Eigen::ArrayXd> pw
          = quadrature::gauss_lobatto_legendre_line_rule(n + 1);
      Eigen::VectorXd pts = std::get<0>(pw) * 0.5;
      for (int i = 0; i < n + 1; ++i)
        pts[i] += (0.5 - static_cast<double>(i) / static_cast<double>(n));
      FiniteElement L = create_dlagrange(cell::type::interval, n);

      // Get interpolated value at r in range [-1, 1]
      auto w = [&](double r) {
        Eigen::ArrayXd rr = Eigen::ArrayXd::Constant(1, 0.5 * (r + 1.0));
        Eigen::VectorXd v = L.tabulate(0, rr)[0].row(0);
        return v.dot(pts);
      };

      int b = (exterior == false) ? 1 : 0;
      n -= b * 3;
      int m = (n + 1) * (n + 2) * (2 * n + 3) / 6;
      Eigen::ArrayX3d points(m, 3);
      int c = 0;
      for (int k = 0; k < n + 1; ++k)
        for (int j = 0; j < n + 1 - k; ++j)
          for (int i = 0; i < n + 1 - k; ++i)
          {
            double x = h * (i + b);
            double y = h * (j + b);
            double z = h * (k + b);

            if (lattice_type == lattice::type::gll_warped)
            {
              // Barycentric coordinates of triangle in x-z plane
              const double l1 = x;
              const double l2 = z;
              const double l3 = 1 - x - z;
              // Barycentric coordinates of triangle in y-z plane
              const double l4 = y;
              const double l5 = z;
              const double l6 = 1 - y - z;

              // b1-b6 are the blending factors for each edge
              double b1, f1, f2;
              if (std::fabs(l1) < 1e-12)
              {
                b1 = 1.0;
                f1 = 0.0;
                f2 = 0.0;
              }
              else
              {
                b1 = 2.0 * l3 / (2.0 * l3 + l1) * 2.0 * l2 / (2.0 * l2 + l1);
                f1 = l1 / (l1 + l4);
                f2 = l1 / (l1 + l6);
              }

              // r1-r4 are the edge positions for each of the z>0 edges
              // calculated so that they use the barycentric coordinates
              // of the triangle, if the point lies on a triangular face.
              // f1-f4 are face selecting functions, which blend between
              // adjacent triangular faces
              const double r1 = (l2 - l3) * f1 + (l5 - l6) * (1 - f1);
              const double r2 = (l2 - l3) * f2 + (l5 - l4) * (1 - f2);

              double b2;
              if (std::fabs(l2) < 1e-12)
                b2 = 1.0;
              else
                b2 = 2.0 * l3 / (2.0 * l3 + l2) * 2.0 * l1 / (2.0 * l1 + l2);

              double b3, f3, f4;
              if (std::fabs(l3) < 1e-12)
              {
                b3 = 1.0;
                f3 = 0.0;
                f4 = 0.0;
              }
              else
              {
                b3 = 2.0 * l2 / (2.0 * l2 + l3) * 2.0 * l1 / (2.0 * l1 + l3);
                f3 = l3 / (l3 + l4);
                f4 = l3 / (l3 + l6);
              }

              const double r3 = (l2 - l1) * f3 + (l5 - l6) * (1.0 - f3);
              const double r4 = (l2 - l1) * f4 + (l5 - l4) * (1.0 - f4);

              double b4;
              if (std::fabs(l4) < 1e-12)
                b4 = 1.0;
              else
                b4 = 2 * l6 / (2.0 * l6 + l4) * 2.0 * l5 / (2.0 * l5 + l4);

              double b5;
              if (std::fabs(l5) < 1e-12)
                b5 = 1.0;
              else
                b5 = 2.0 * l6 / (2.0 * l6 + l5) * 2.0 * l4 / (2.0 * l4 + l5);

              double b6;
              if (std::fabs(l6) < 1e-12)
                b6 = 1.0;
              else
                b6 = 2.0 * l4 / (2.0 * l4 + l6) * 2.0 * l5 / (2.0 * l5 + l6);

              double dx = -b3 * b4 * w(r3) - b3 * b6 * w(r4) + b2 * w(l1 - l3);
              double dy = -b1 * b6 * w(r2) - b3 * b6 * w(r4) + b5 * w(l4 - l6);
              double dz = b1 * b4 * w(r1) + b1 * b6 * w(r2) + b3 * b4 * w(r3)
                          + b3 * b6 * w(r4);

              x += dx;
              y += dy;
              z += dz;
            }

            points.row(c++) << x, y, z;
          }

      return points;
    }
  }
  default:
    throw std::runtime_error("Unsupported cell for lattice");
  }
}

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


#pragma once
#include <iostream>
#include <array>
#include <Eigen/Dense>
#include "Types/SFINAE.hpp"

///@file
///@brief Contains the class \ref mackey::Z_mod

namespace mackey {
	///////////////////////////////////
	///The class of Z/N coefficients where N is prime.

	///The operators are self explanatory. User must ensure that N is prime for division to work.
	/////////////////////////////////
	template<int64_t N, typename T = int64_t>
	class Z_mod {
	public:
		constexpr static int64_t order = N; ///<The N of Z/N
		T x; ///< A modulo N number
		Z_mod() : x(0) {} ///<Default value 0
		Z_mod(bool x) : x(x) {} ///<Initialize from 0,1
		Z_mod(int x); ///<Initialize from int
		Z_mod(int64_t x); ///<Initialize from 64bit int
		explicit operator char() const;
		explicit operator short() const;
		explicit operator int() const;
		explicit operator int64_t() const;
		explicit operator unsigned char() const;
		explicit operator unsigned short() const;
		explicit operator unsigned int() const;
		explicit operator uint64_t() const;
		Z_mod<N, T> operator +(Z_mod<N, T> b) const;
		Z_mod<N, T> operator -(Z_mod<N, T> b) const;
		Z_mod<N, T>& operator +=(Z_mod<N, T> b);
		Z_mod<N, T>& operator -=(Z_mod<N, T> b);
		Z_mod<N, T>& operator *=(Z_mod<N, T> b);
		Z_mod<N, T>& operator /=(Z_mod<N, T> b);
		bool operator ==(Z_mod<N, T> a) const;
		bool operator !=(Z_mod<N, T> a) const;
		bool operator <=(Z_mod<N, T> a) const; ///<Needed for Eigen pruning; standard order on 0,...,N-1
	};

	template<int64_t N, typename T>
	Z_mod<N, T> operator -(Z_mod<N, T> a);

	template<int64_t N, typename T>
	Z_mod<N, T> operator *(Z_mod<N, T> a, Z_mod<N, T> b); //Eigen needs this to be non member

	template<int64_t N, typename T>
	Z_mod<N, T> operator /(Z_mod<N, T> a, Z_mod<N, T> b);

	///The usual absolute value for integer and Z/N types
	template<typename T>
	T abs(T a);

	template<int64_t N, typename T> //Eigen needs this to be non member
	std::ostream& operator<<(std::ostream& out, const Z_mod<N, T> a);

	///The \f$\mathbf Z/2\f$ coefficients
	using Z2 = Z_mod<2, bool>;
}

///See the Eigen documentation for this.
namespace Eigen {
	using namespace mackey;

	///Specializing NumTraits to Z/nZ coefficients
	template<int64_t N, typename T>
	struct NumTraits<Z_mod<N, T>>
	{
		typedef Z_mod<N, T> Real;
		typedef Z_mod<N, T> Nested;
		typedef int Literal;
		enum {
			IsComplex = 0,
			IsInteger = 0,
			IsSigned = 0,
			RequireInitialization = 0,
			ReadCost = 1,
			AddCost = 1,
			MulCost = 1
		};
		static inline Z_mod<N, T> dummy_precision() { return Z_mod<N, T>(0); }
		static inline int digits10() { return 0; }
	};
}

#include "impl/Z_n.ipp"


//==================================================================================================
/*!
  @file

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/pack.hpp>
#include <boost/simd/function/rem_2pi.hpp>
#include <boost/simd/meta/cardinal_of.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/eps.hpp>
#include <boost/simd/constant/pi.hpp>
#include <boost/simd/constant/pio_2.hpp>
#include <boost/simd/constant/twopi.hpp>
#include <boost/simd/constant/ten.hpp>

template <typename T, std::size_t N, typename Env>
void test(Env& $)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  using p_t = bs::pack<T, N>;

  T a1[N], m[N];
  for(std::size_t i = 0; i < N; ++i)
  {
    a1[i] = (i%2) ? T(i) : -T((i));
     m[i] = bs::rem_2pi(a1[i]);
   }
  p_t aa1(&a1[0], &a1[0]+N);
  p_t mm(&m[0], &m[0]+N);
  STF_IEEE_EQUAL( bs::rem_2pi(aa1), mm);
}

STF_CASE_TPL("Check rem_2pi on pack" , STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T>;
  static const std::size_t N = bs::cardinal_of<p_t>::value;
  test<T, N>($);
  test<T, N/2>($);
  test<T, N*2>($);
}

STF_CASE_TPL (" rem_2pi",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::rem_2pi;
  using p_t = bs::pack<T>;

  {
    p_t res = rem_2pi(bs::Zero<p_t>());
    STF_ULP_EQUAL( res, bs::Zero<p_t>(), 1.5);
    res = rem_2pi(bs::Pi<p_t>()-bs::Ten<p_t>()*bs::Eps<p_t>());
    STF_ULP_EQUAL( res, bs::Pi<p_t>()-bs::Ten<p_t>()*bs::Eps<p_t>(), 1.5);
    res = rem_2pi(bs::Pi<p_t>()+bs::Ten<p_t>()*bs::Eps<p_t>());
    STF_ULP_EQUAL( res, bs::Ten<p_t>()*bs::Eps<p_t>()-bs::Pi<p_t>(), 1.5);
    res = rem_2pi(bs::Twopi<p_t>());
    STF_ULP_EQUAL( res, bs::Zero<p_t>(), 1.5);
    res = rem_2pi(bs::Pio_2<p_t>());
    STF_ULP_EQUAL( res, bs::Pio_2<p_t>(), 1.5);
  }
} // end of test for floating_