#include "SchemeAlgo.h"

#include <NTL/BasicThreadPool.h>
#include <NTL/ZZ.h>
#include <cmath>
#include <map>

#include "CZZ.h"
#include "EvaluatorUtils.h"
#include "Message.h"
#include "Params.h"
#include "SchemeAux.h"
#include "SecKey.h"

Cipher* SchemeAlgo::encryptSingleArray(CZZ*& vals, long size) {
	Cipher* res = new Cipher[size];
	for (long i = 0; i < size; ++i) {
		res[i] = scheme.encryptSingle(vals[i]);
	}
	return res;
}

CZZ* SchemeAlgo::decryptSingleArray(SecKey& secretKey, Cipher*& ciphers, long size) {
	CZZ* res = new CZZ[size];
	for (int i = 0; i < size; ++i) {
		Message msg = scheme.decryptMsg(secretKey, ciphers[i]);
		CZZ* gvals = scheme.decode(msg);
		res[i] = gvals[0];
	}
	return res;
}

Cipher SchemeAlgo::powerOf2(Cipher& cipher, const long precisionBits, const long logDegree) {
	Cipher res = cipher;
	for (long i = 0; i < logDegree; ++i) {
		scheme.squareAndEqual(res);
		scheme.modSwitchAndEqual(res, precisionBits);
	}
	return res;
}

Cipher* SchemeAlgo::powerOf2Extended(Cipher& cipher, const long precisionBits, const long logDegree) {
	Cipher* res = new Cipher[logDegree + 1];
	res[0] = cipher;
	for (long i = 1; i < logDegree + 1; ++i) {
		res[i] = scheme.square(res[i-1]);
		scheme.modSwitchAndEqual(res[i], precisionBits);
	}
	return res;
}

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

Cipher SchemeAlgo::power(Cipher& cipher, const long precisionBits, const long degree) {
	long logDegree = log2(degree);
	long po2Degree = 1 << logDegree;

	Cipher res = powerOf2(cipher, precisionBits, logDegree);
	long remDegree = degree - po2Degree;
	if(remDegree > 0) {
		Cipher tmp = power(cipher, precisionBits, remDegree);
		long bitsDown = tmp.cbits - res.cbits;
		scheme.modEmbedAndEqual(tmp, bitsDown);
		scheme.multAndEqual(res, tmp);
		scheme.modSwitchAndEqual(res, precisionBits);
	}
	return res;
}

Cipher* SchemeAlgo::powerExtended(Cipher& cipher, const long precisionBits, const long degree) {
	Cipher* res = new Cipher[degree];
	long logDegree = log2(degree);
	Cipher* cpows = powerOf2Extended(cipher, precisionBits, logDegree);
	long idx = 0;
	for (long i = 0; i < logDegree; ++i) {
		long powi = (1 << i);
		res[idx++] = cpows[i];
		for (int j = 0; j < powi-1; ++j) {
			long bitsDown = res[j].cbits - cpows[i].cbits;
			res[idx] = scheme.modEmbed(res[j], bitsDown);
			scheme.multAndEqual(res[idx], cpows[i]);
			scheme.modSwitchAndEqual(res[idx++], precisionBits);
		}
	}
	res[idx++] = cpows[logDegree];
	long degree2 = (1 << logDegree);
	for (int i = 0; i < (degree - degree2); ++i) {
		long bitsDown = res[i].cbits - cpows[logDegree].cbits;
		res[idx] = scheme.modEmbed(res[i], bitsDown);
		scheme.multAndEqual(res[idx], cpows[logDegree]);
		scheme.modSwitchAndEqual(res[idx++], precisionBits);
	}
	return res;
}

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

Cipher SchemeAlgo::prodOfPo2(Cipher*& ciphers, const long precisionBits, const long logDegree) {
	Cipher* res = ciphers;
	for (long i = logDegree - 1; i >= 0; --i) {
		long powih = (1 << i);
		Cipher* tmp = new Cipher[powih];
		NTL_EXEC_RANGE(powih, first, last);
		for (long j = first; j < last; ++j) {
			tmp[j] = scheme.mult(res[2 * j], res[2 * j + 1]);
			scheme.modSwitchAndEqual(tmp[j], precisionBits);
		}
		NTL_EXEC_RANGE_END;
		res = tmp;
	}
	return res[0];
}

Cipher SchemeAlgo::prod(Cipher*& ciphers, const long precisionBits, const long degree) {
	long logDegree = log2(degree) + 1;
	long idx = 0;
	bool isinit = false;
	Cipher res;
	for (long i = 0; i < logDegree; ++i) {
		if(bit(degree, i)) {
			long powi = (1 << i);
			Cipher* tmp = new Cipher[powi];
			for (long j = 0; j < powi; ++j) {
				tmp[j] = ciphers[idx + j];
			}
			Cipher iprod = prodOfPo2(tmp, precisionBits, i);
			if(isinit) {
				long bitsDown = res.cbits - iprod.cbits;
				scheme.modEmbedAndEqual(res, bitsDown);
				scheme.multAndEqual(res, iprod);
				scheme.modSwitchAndEqual(res, precisionBits);
			} else {
				res = iprod;
				isinit = true;
			}
			idx += powi;
		}
	}
	return res;
}

Cipher SchemeAlgo::sum(Cipher*& ciphers, const long size) {
	Cipher res = ciphers[0];
	for (long i = 1; i < size; ++i) {
		scheme.addAndEqual(res, ciphers[i]);
	}
	return res;
}

Cipher SchemeAlgo::distance(Cipher& cipher1, Cipher& cipher2, const long precisionBits) {
	Cipher cres = scheme.sub(cipher1, cipher2);
	scheme.squareAndEqual(cres);
	scheme.modSwitchAndEqual(cres, precisionBits);
	partialSlotsSumAndEqual(cres, cres.slots);
	return cres;
}

Cipher* SchemeAlgo::multVec(Cipher*& ciphers1, Cipher*& ciphers2, const long size) {
	Cipher* res = new Cipher[size];
	NTL_EXEC_RANGE(size, first, last);
	for (long i = first; i < last; ++i) {
		res[i] = scheme.mult(ciphers1[i], ciphers2[i]);
	}
	NTL_EXEC_RANGE_END;
	return res;
}

void SchemeAlgo::multAndEqualVec(Cipher*& ciphers1, Cipher*& ciphers2, const long size) {
	NTL_EXEC_RANGE(size, first, last);
	for (long i = first; i < last; ++i) {
		scheme.multAndEqual(ciphers1[i], ciphers2[i]);
	}
	NTL_EXEC_RANGE_END;
}

Cipher* SchemeAlgo::multAndModSwitchVec(Cipher*& ciphers1, Cipher*& ciphers2, const long precisionBits, const long size) {
	Cipher* res = new Cipher[size];
	NTL_EXEC_RANGE(size, first, last);
	for (long i = first; i < last; ++i) {
		res[i] = scheme.mult(ciphers1[i], ciphers2[i]);
		scheme.modSwitchAndEqual(res[i], precisionBits);
	}
	NTL_EXEC_RANGE_END;
	return res;
}

void SchemeAlgo::multModSwitchAndEqualVec(Cipher*& ciphers1, Cipher*& ciphers2, const long precisionBits, const long size) {
	NTL_EXEC_RANGE(size, first, last);
	for (long i = first; i < last; ++i) {
		scheme.multAndEqual(ciphers1[i], ciphers2[i]);
		scheme.modSwitchAndEqual(ciphers1[i], precisionBits);
	}
	NTL_EXEC_RANGE_END;
}

Cipher SchemeAlgo::innerProd(Cipher*& ciphers1, Cipher*& ciphers2, const long precisionBits, const long size) {
	Cipher cip = scheme.mult(ciphers1[size-1], ciphers2[size-1]);

	NTL_EXEC_RANGE(size-1, first, last);
	for (long i = first; i < last; ++i) {
		Cipher cprodi = scheme.mult(ciphers1[i], ciphers2[i]);
		scheme.addAndEqual(cip, cprodi);
	}
	NTL_EXEC_RANGE_END;

	scheme.modSwitchAndEqual(cip, precisionBits);
	return cip;
}

Cipher SchemeAlgo::partialSlotsSum(Cipher& cipher, const long slots) {
	long logslots = log2(slots);
	Cipher res = cipher;
	for (long i = 0; i < logslots; ++i) {
		Cipher rot = scheme.leftRotateByPo2(cipher, i);
		scheme.addAndEqual(res, rot);
	}
	return res;
}

void SchemeAlgo::partialSlotsSumAndEqual(Cipher& cipher, const long slots) {
	long logslots = log2(slots);
	for (long i = 0; i < logslots; ++i) {
		Cipher rot = scheme.leftRotateByPo2(cipher, i);
		scheme.addAndEqual(cipher, rot);
	}
}

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

Cipher SchemeAlgo::inverse(Cipher& cipher, const long precisionBits, const long steps) {
	ZZ precision = power2_ZZ(precisionBits);
	Cipher cpow = cipher;
	Cipher tmp = scheme.addConst(cipher, precision);
	scheme.modEmbedAndEqual(tmp, precisionBits);
	Cipher res = tmp;

	for (long i = 1; i < steps; ++i) {
		scheme.squareAndEqual(cpow);
		scheme.modSwitchAndEqual(cpow, precisionBits);
		tmp = cpow;
		scheme.addConstAndEqual(tmp, precision);
		scheme.multAndEqual(tmp, res);
		scheme.modSwitchAndEqual(tmp, precisionBits);
		res = tmp;
	}
	return res;
}

Cipher* SchemeAlgo::inverseExtended(Cipher& cipher, const long precisionBits, const long steps) {
	ZZ precision = power2_ZZ(precisionBits);

	Cipher* res = new Cipher[steps];
	Cipher cpow = cipher;
	Cipher tmp = scheme.addConst(cipher, precision);
	scheme.modEmbedAndEqual(tmp, precisionBits);
	res[0] = tmp;

	for (long i = 1; i < steps; ++i) {
		scheme.squareAndEqual(cpow);
		scheme.modSwitchAndEqual(cpow, precisionBits);
		tmp = cpow;
		scheme.addConstAndEqual(tmp, precision);
		scheme.multAndEqual(tmp, res[i - 1]);
		scheme.modSwitchAndEqual(tmp, precisionBits);
		res[i] = tmp;
	}
	return res;
}

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

Cipher SchemeAlgo::function(Cipher& cipher, string& funcName, const long precisionBits, const long degree) {
	Cipher* cpows = powerExtended(cipher, precisionBits, degree);

	long dprecisionBits = 2 * precisionBits;

	double* coeffs = scheme.aux.taylorCoeffsMap.at(funcName);

	ZZ tmp = EvaluatorUtils::evaluateVal(coeffs[1], precisionBits);
	Cipher res = scheme.multByConst(cpows[0], tmp);

	tmp = EvaluatorUtils::evaluateVal(coeffs[0], dprecisionBits);
	scheme.addConstAndEqual(res, tmp);

	for (int i = 1; i < degree; ++i) {
		if(abs(coeffs[i + 1]) > 1e-27) {
			tmp = EvaluatorUtils::evaluateVal(coeffs[i + 1], precisionBits);
			Cipher aixi = scheme.multByConst(cpows[i], tmp);
			long bitsDown = res.cbits - aixi.cbits;
			scheme.modEmbedAndEqual(res, bitsDown);
			scheme.addAndEqual(res, aixi);
		}
	}
	scheme.modSwitchAndEqual(res, precisionBits);
	return res;
}

Cipher SchemeAlgo::functionLazy(Cipher& cipher, string& funcName, const long precisionBits, const long degree) {
	Cipher* cpows = powerExtended(cipher, precisionBits, degree);

	long dprecisionBits = 2 * precisionBits;

	double* coeffs = scheme.aux.taylorCoeffsMap.at(funcName);

	ZZ tmp = EvaluatorUtils::evaluateVal(coeffs[1], precisionBits);
	Cipher res = scheme.multByConst(cpows[0], tmp);

	tmp = EvaluatorUtils::evaluateVal(coeffs[0], dprecisionBits);
	scheme.addConstAndEqual(res, tmp);

	for (int i = 1; i < degree; ++i) {
		if(abs(coeffs[i + 1]) > 1e-27) {
			tmp = EvaluatorUtils::evaluateVal(coeffs[i + 1], precisionBits);
			Cipher aixi = scheme.multByConst(cpows[i], tmp);
			long bitsDown = res.cbits - aixi.cbits;
			scheme.modEmbedAndEqual(res, bitsDown);
			scheme.addAndEqual(res, aixi);
		}
	}
	return res;
}

Cipher* SchemeAlgo::functionExtended(Cipher& cipher, string& funcName, const long precisionBits, const long degree) {
	Cipher* cpows = powerExtended(cipher, precisionBits, degree);

	long dprecisionBits = 2 * precisionBits;
	double* coeffs = scheme.aux.taylorCoeffsMap.at(funcName);

	ZZ tmp = EvaluatorUtils::evaluateVal(coeffs[1], precisionBits);
	Cipher aixi = scheme.multByConst(cpows[0], tmp);

	tmp = EvaluatorUtils::evaluateVal(coeffs[0], dprecisionBits);
	scheme.addConstAndEqual(aixi, tmp);

	Cipher* res = new Cipher[degree];
	res[0] = aixi;
	for (long i = 1; i < degree; ++i) {
		if(abs(coeffs[i + 1]) > 1e-27) {
			tmp = EvaluatorUtils::evaluateVal(coeffs[i + 1], precisionBits);
			aixi = scheme.multByConst(cpows[i], tmp);
			long bitsDown = res[i - 1].cbits - aixi.cbits;
			Cipher ctmp = scheme.modEmbed(res[i - 1], bitsDown);
			scheme.addAndEqual(aixi, ctmp);
			res[i] = aixi;
		} else {
			res[i] = res[i - 1];
		}
	}
	NTL_EXEC_RANGE(degree, first, last);
	for (long i = first; i < last; ++i) {
		scheme.modSwitchAndEqual(res[i], precisionBits);
	}
	NTL_EXEC_RANGE_END;
	return res;
}

void SchemeAlgo::fftRaw(Cipher*& ciphers, const long size, const bool isForward) {
	for (long i = 1, j = 0; i < size; ++i) {
		long bit = size >> 1;
		for (; j >= bit; bit>>=1) {
			j -= bit;
		}
		j += bit;
		if(i < j) {
			swap(ciphers[i], ciphers[j]);
		}
	}

	for (long len = 2; len <= size; len <<= 1) {
		long shift = isForward ? ((scheme.params.N / len) << 1) : ((scheme.params.N - scheme.params.N / len) << 1);
		for (long i = 0; i < size; i += len) {
			NTL_EXEC_RANGE(len / 2, first, last);
			for (long j = first; j < last; ++j) {
				Cipher u = ciphers[i + j];
				scheme.multByMonomialAndEqual(ciphers[i + j + len / 2], shift * j);
				scheme.addAndEqual(ciphers[i + j], ciphers[i + j + len / 2]);
				scheme.subAndEqual2(u, ciphers[i + j + len / 2]);
			}
			NTL_EXEC_RANGE_END;
		}
	}
}

void SchemeAlgo::fft(Cipher*& ciphers, const long size) {
	fftRaw(ciphers, size, true);
}

void SchemeAlgo::fftInv(Cipher*& ciphers, const long size) {
	fftRaw(ciphers, size, false);
	long logsize = log2(size);

	NTL_EXEC_RANGE(size, first, last);
	for (long i = first; i < last; ++i) {
		scheme.modSwitchAndEqual(ciphers[i], logsize);
	}
	NTL_EXEC_RANGE_END;
}

void SchemeAlgo::fftInvLazy(Cipher*& ciphers, const long size) {
	return fftRaw(ciphers, size, false);
}


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

  Principal component analysis implementation using the covariance method.

  Copyright (c) 2012, 2013
  Alexander Rukletsov <rukletsov@gmail.com>
  All rights reserved.

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions
  are met:
  1.  Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
  2.  Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.

  THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS "AS IS" AND
  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  SUCH DAMAGE.

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

#ifndef PCA_HPP_C2EA373C_F360_43BA_BACB_4B26B78759BE_
#define PCA_HPP_C2EA373C_F360_43BA_BACB_4B26B78759BE_

#include <vector>
#include <algorithm>
#include <functional>
#include <stdexcept>
#include <boost/tuple/tuple.hpp>
#include <boost/array.hpp>
#include <boost/assert.hpp>

#include "bo/core/vector.hpp"
#include "bo/math/mean.hpp"
#include "bo/math/blas_extensions.hpp"
#include "bo/math/blas_conversions.hpp"

namespace bo {
namespace math {

using namespace boost::numeric::ublas;

// Performs PCA for the given data. Returns a tuple of eigenvalues and eigenvectors.
// Implemented as a functor in order to store induced typedefs inside.
template <typename RealType, std::size_t Dim>
struct PCA
{
    typedef Vector<RealType, Dim> Sample;
    typedef std::vector<Sample> Samples;

    typedef RealType EigenValue;
    typedef boost::array<EigenValue, Dim> EigenValues;
    typedef Vector<RealType, Dim> EigenVector;
    typedef boost::array<EigenVector, Dim> EigenVectors;
    typedef boost::tuples::tuple<EigenValues, EigenVectors> Result;

    Result operator() (Samples data)
    {
        typedef math::bounded_vector<RealType, Dim> BlasVector;
        typedef std::vector<RealType> StdVector;
        typedef math::bounded_matrix<RealType, Dim, Dim> Matrix;

        // If the data is empty, PCA is meaningless.
        if (data.size() == 0)
            throw std::logic_error("PCA for an empty set is meaningless.");

        // Find the mean among the neighbours.
        Sample mean_value = bo::math::mean(data);

        // Calculate the deviations from mean.
        std::transform(data.begin(), data.end(), data.begin(),
                       std::bind2nd(std::minus<Sample>(), mean_value));

        // Initialize covariance matrix.
        Matrix covar_matrix = math::zero_matrix<RealType>(Dim);

        // Iteratively compute the Dim x Dim covariance matrix (sample by sample).
        for (typename Samples::const_iterator pt = data.begin(); pt != data.end(); ++pt)
        {
            BlasVector vect = math::from_bo_vector(*pt);
            covar_matrix += math::outer_prod(vect, vect);
        }

        covar_matrix /= data.size();

        // Get the eigenvectors of the covariance matrix.
        StdVector covar_eigenvalues = math::eigen_symmetric(covar_matrix);
        BOOST_ASSERT((covar_eigenvalues.size() == Dim) &&
                     "Eigenvalues count differs from samples' dimensions.");

        // Convert eigenvectors to the output format.
        EigenValues eigenvalues;
        std::copy(covar_eigenvalues.begin(), covar_eigenvalues.end(), eigenvalues.begin());

        // Extract eignvectors from the columns of the updated covariance matrix and
        // convert them to the output format.
        EigenVectors eigenvectors;
        for (std::size_t col_idx = 0; col_idx < Dim; ++col_idx)
            eigenvectors[col_idx] = math::to_bo_vector(BlasVector(math::column(covar_matrix, col_idx)));

        // Put eigenvalues and eigenvectors into a tuple and return.
        return
            boost::tuples::make_tuple(eigenvalues, eigenvectors);
    }
};

} // namespace math
} // namespace bo

#endif // PCA_HPP_C2EA373C_F360_43BA_BACB_4B26B78759BE_


#ifndef USE_CUDA

#include <engine/Vectormath.hpp>
#include <engine/Manifoldmath.hpp>
#include <utility/Logging.hpp>
#include <utility/Exception.hpp>

#include <Eigen/Dense>

#include <array>

namespace Engine
{
	namespace Manifoldmath
	{
        scalar norm(const vectorfield & vf)
        {
            scalar x = Vectormath::dot(vf, vf);
            return std::sqrt(x);
        }

        void normalize(vectorfield & vf)
        {
            scalar x = 1.0/norm(vf);
            #pragma omp parallel for
            for (unsigned int i = 0; i < vf.size(); ++i)
                vf[i] *= x;
        }

        void project_parallel(vectorfield & vf1, const vectorfield & vf2)
        {
            vectorfield vf3 = vf1;
            project_orthogonal(vf3, vf2);
            // TODO: replace the loop with Vectormath Kernel
            #pragma omp parallel for
            for (unsigned int i = 0; i < vf1.size(); ++i)
                vf1[i] -= vf3[i];
        }

        void project_orthogonal(vectorfield & vf1, const vectorfield & vf2)
        {
            scalar x = Vectormath::dot(vf1, vf2);
            // TODO: replace the loop with Vectormath Kernel
            #pragma omp parallel for
            for (unsigned int i=0; i<vf1.size(); ++i)
                vf1[i] -= x*vf2[i];
        }

        void invert_parallel(vectorfield & vf1, const vectorfield & vf2)
        {
            scalar x = Vectormath::dot(vf1, vf2);
            // TODO: replace the loop with Vectormath Kernel
            #pragma omp parallel for
            for (unsigned int i=0; i<vf1.size(); ++i)
                vf1[i] -= 2*x*vf2[i];
        }

        void invert_orthogonal(vectorfield & vf1, const vectorfield & vf2)
        {
            vectorfield vf3 = vf1;
            project_orthogonal(vf3, vf2);
            // TODO: replace the loop with Vectormath Kernel
            #pragma omp parallel for
            for (unsigned int i = 0; i < vf1.size(); ++i)
                vf1[i] -= 2 * vf3[i];
        }

        void project_tangential(vectorfield & vf1, const vectorfield & vf2)
		{
            #pragma omp parallel for
			for (unsigned int i = 0; i < vf1.size(); ++i)
				vf1[i] -= vf1[i].dot(vf2[i]) * vf2[i];
		}

		scalar dist_greatcircle(const Vector3 & v1, const Vector3 & v2)
		{
			scalar r = v1.dot(v2);

			// Prevent NaNs from occurring
			r = std::fmax(-1.0, std::fmin(1.0, r));

			// Greatcircle distance
			return std::acos(r);
		}

		scalar dist_geodesic(const vectorfield & v1, const vectorfield & v2)
		{
			scalar dist = 0;
            #pragma omp parallel for reduction(+:dist)
			for (unsigned int i = 0; i < v1.size(); ++i)
				dist += pow(dist_greatcircle(v1[i], v2[i]), 2);
			return sqrt(dist);
		}

		/*
		Calculates the 'tangent' vectors, i.e.in crudest approximation the difference between an image and the neighbouring
		*/
		void Tangents(std::vector<std::shared_ptr<vectorfield>> configurations, const std::vector<scalar> & energies, std::vector<vectorfield> & tangents)
		{
			int noi = configurations.size();
			int nos = (*configurations[0]).size();

			for (int idx_img = 0; idx_img < noi; ++idx_img)
			{
				auto& image = *configurations[idx_img];

				// First Image
				if (idx_img == 0)
				{
					auto& image_plus = *configurations[idx_img + 1];
					Vectormath::set_c_a( 1, image_plus, tangents[idx_img]);
					Vectormath::add_c_a(-1, image,      tangents[idx_img]);
				}
				// Last Image
				else if (idx_img == noi - 1)
				{
					auto& image_minus = *configurations[idx_img - 1];
					Vectormath::set_c_a( 1, image,       tangents[idx_img]);
					Vectormath::add_c_a(-1, image_minus, tangents[idx_img]);
				}
				// Images Inbetween
				else
				{
					auto& image_plus  = *configurations[idx_img + 1];
					auto& image_minus = *configurations[idx_img - 1];

					// Energies
					scalar E_mid = 0, E_plus = 0, E_minus = 0;
					E_mid   = energies[idx_img];
					E_plus  = energies[idx_img + 1];
					E_minus = energies[idx_img - 1];

					// Vectors to neighbouring images
					vectorfield t_plus(nos), t_minus(nos);

					Vectormath::set_c_a( 1, image_plus, t_plus);
					Vectormath::add_c_a(-1, image,      t_plus);

					Vectormath::set_c_a( 1, image,       t_minus);
					Vectormath::add_c_a(-1, image_minus, t_minus);

					// Near maximum or minimum
					if ((E_plus < E_mid && E_mid > E_minus) || (E_plus > E_mid && E_mid < E_minus))
					{
						// Get a smooth transition between forward and backward tangent
						scalar E_max = std::max(std::abs(E_plus - E_mid), std::abs(E_minus - E_mid));
						scalar E_min = std::min(std::abs(E_plus - E_mid), std::abs(E_minus - E_mid));

						if (E_plus > E_minus)
						{
							Vectormath::set_c_a(E_max, t_plus,  tangents[idx_img]);
							Vectormath::add_c_a(E_min, t_minus, tangents[idx_img]);
						}
						else
						{
							Vectormath::set_c_a(E_min, t_plus,  tangents[idx_img]);
							Vectormath::add_c_a(E_max, t_minus, tangents[idx_img]);
						}
					}
					// Rising slope
					else if (E_plus > E_mid && E_mid > E_minus)
					{
						Vectormath::set_c_a(1, t_plus,  tangents[idx_img]);
					}
					// Falling slope
					else if (E_plus < E_mid && E_mid < E_minus)
					{
						Vectormath::set_c_a(1, t_minus,  tangents[idx_img]);
						//tangents = t_minus;
						for (int i = 0; i < nos; ++i)
						{
							tangents[idx_img][i] = t_minus[i];
						}
					}
					// No slope(constant energy)
					else
					{
						Vectormath::set_c_a(1, t_plus,  tangents[idx_img]);
						Vectormath::add_c_a(1, t_minus, tangents[idx_img]);
					}
				}

				// Project tangents into tangent planes of spin vectors to make them actual tangents
        		project_tangential(tangents[idx_img], image);

				// Normalise in 3N - dimensional space
				Manifoldmath::normalize(tangents[idx_img]);

			}// end for idx_img
		}// end Tangents
    }
}

#endif


//----------------------------------*-C++-*----------------------------------//
/**
 *  @file   CLP.cc
 *  @brief  CLP
 *  @author Jeremy Roberts
 *  @date   Jan 8, 2013
 */
//---------------------------------------------------------------------------//

#include "CLP.hh"
#ifdef DETRAN_ENABLE_BOOST
#include <boost/math/special_functions/legendre.hpp>
#endif

namespace detran_orthog
{

//---------------------------------------------------------------------------//
CLP::CLP(const size_t   order,
         const vec_dbl &x,
         const vec_dbl &qw,
         const double   x_0,
         const double   x_1)
  : ContinuousOrthogonalBasis(order, x, qw)
{
#ifndef DETRAN_ENABLE_BOOST
  THROW("CLP needs boost to be enabled.");
#else
  Require(x_1 > x_0);

  // Allocate the basis matrix
  d_basis = new callow::MatrixDense(d_order + 1, d_size, 0.0);

  // Allocate the normalization array
  d_a = Vector::Create(d_order + 1, 0.0);

  // The weights are just the qw's.
  double L = x_1 - x_0;
  for (size_t i = 0; i < d_w->size(); ++i)
  {
    d_x[i] =  2.0*(d_x[i] - x_0)/L - 1.0;
    (*d_w)[i] = d_qw[i];
  }

  // Build the basis
  for (size_t l = 0; l <= d_order; ++l)
  {
    for (size_t i = 0; i < d_size; ++i)
    {
      (*d_basis)(l, i) = boost::math::legendre_p(l, d_x[i]);
    }
    // Inverse of normalization coefficient.
    (*d_a)[l] = (2.0 * l + 1.0) / 2.0;
  }
  d_orthonormal = true;
  compute_a();
#endif
}

} // end namespace detran_orthog

//---------------------------------------------------------------------------//
//              end of file CLP.cc
//---------------------------------------------------------------------------//


#pragma once
#include <Eigen/Core>
#include <memory>
#include "ear/common/point_source_panner.hpp"

namespace ear {

  class ExtentPanner {};

  /** @brief Modify an extent parameter given a distance.
   *
   * A right triangle if formed, with the adjacent edge being the distance, and
   * the opposite edge being determined from the extent. The angle formed is
   * then used to determine the new extent.
   *
   * - at distance=0, the extent is always 360
   * - at distance=1, the original extent is used
   * - at distance>1, the extent decreases
   * - in 0 < distance < 1, the extent changes more steeply around 0 for smaller
   *   extents
   */
  double extentMod(double extent, double distance);

  /** @brief Calculate basis vectors that rotate (0, 1, 0)
     onto source_pos. */
  Eigen::Matrix3d calcBasis(Eigen::Vector3d position);

  /** @brief Polar to Cartesian in radians with no distance, in a given basis.*/
  Eigen::Vector3d cartOnBasis(Eigen::Matrix3d basis, double azimuth,
                              double elevation);

  std::pair<double, double> azimuthElevationOnBasis(
      Eigen::Matrix3d basis, Eigen::RowVector3d position);

  /** @brief Weighting function for spread sources.
   *
   * The weighting function is one inside a region approximately determined by a
   * width x height rectangle in azimuth-elevation space, with maximally-sized
   * rounded corners; the shape of the corners is calculated using the vector
   * angle from their centres (always directly above or below the source
   * position) so as to avoid issues at the poles.
   *
   * The two straight edges of the rectangle are always parallel in Cartesian
   * space; this is achieved by following azimuth lines; for tall sources, the
   * whole coordinate system is rotated 90 degrees about the source position to
   * achieve this.
   *
   * Note that for sources where width == height, this degrades to a circular
   * region relative to the source position.
   *
   * To make the two ends meet, the width is adjusted such that a width of 180
   * degrees is mapped to width + height.
   *
   */
  class WeightingFunction {
   public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    /** @brief Ctor
     *
     * @param position Centre of the extent.
     * @param width Width of the extent in degrees from one edge to the other.
     * @param height Height of the extent in degrees from one edge to the other.
     */
    WeightingFunction(Eigen::Vector3d position, double width, double height);

    /** @brief Calculate weight for position */
    double operator()(Eigen::Vector3d position) const;

   private:
    const double _fadeWidth = 10.0;
    double _width;
    double _height;
    double _circleRadius;
    Eigen::Matrix3d _flippedBasis;
    double _circlePos;
    Eigen::Matrix<double, 3, 2> _circlePositions;
  };

  class SpreadingPanner {
   public:
    SpreadingPanner(std::shared_ptr<PointSourcePanner> psp, int nRows);

    /** @brief Panning values for a given weighting function.
     *
     * @param weightFunc function from Cartesian position to weight in range
     * (0, 1)
     *
     * @return panning value for each speaker.
     */
    Eigen::VectorXd panningValuesForWeight(const WeightingFunction& weightFunc);

   private:
    /** @brief Generate points spread evenly on the sphere.
     *
     * Based on
     * http://web.archive.org/web/20150108040043/http://www.math.niu.edu/~rusin/known-math/95/equispace.elect
     *
     * @param nRows number of rows to place on sphere, e.g. 37 for 5 degree
     * spacing
     *
     * @returns cartesian array.
     */
    Eigen::MatrixXd _generatePanningPositionsEven();
    Eigen::MatrixXd _generatePanningPositionsResults();

    std::shared_ptr<PointSourcePanner> _psp;
    int _nRows;
    Eigen::MatrixXd _panningPositions;
    Eigen::MatrixXd _panningPositionsResults;
  };

  class PolarExtentPanner : public ExtentPanner {
   public:
    PolarExtentPanner(std::shared_ptr<PointSourcePanner> psp);

    /** @brief Calculate loudspeaker gains given position and extent parameters.
     *
     * @param position  Cartesian source position
     * @param width block format width parameter
     * @param height block format height parameter
     * @param depth block format depth parameter
     *
     * @returns loudspeaker gains for each channel
     */
    Eigen::VectorXd handle(Eigen::Vector3d position, double width,
                           double height, double depth);

    /** @brief Calculate the speaker panning values for the position, width, and
     * height of a source; this just deals with the positioning and spreading.
     */
    Eigen::VectorXd calcPvSpread(Eigen::Vector3d position, double width,
                                 double height);

   private:
    const int _nRows = 37;  // 5 degrees per row
    const double _fadeWidth = 10.0;  // degrees
    std::shared_ptr<PointSourcePanner> _psp;
    SpreadingPanner _spreadingPanner;
  };

  class CartesianExtentPanner : public ExtentPanner {};

}  // namespace ear


/*
 [auto_generated]
 boost/numeric/odeint/stepper/implicit_euler.hpp

 [begin_description]
 Impementation of the implicit Euler method. Works with ublas::vector as state type.
 [end_description]

 Copyright 2009-2011 Karsten Ahnert
 Copyright 2009-2011 Mario Mulansky

 Distributed under the Boost Software License, Version 1.0.
 (See accompanying file LICENSE_1_0.txt or
 copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef OMPLEXT_BOOST_NUMERIC_ODEINT_STEPPER_IMPLICIT_EULER_HPP_INCLUDED
#define OMPLEXT_BOOST_NUMERIC_ODEINT_STEPPER_IMPLICIT_EULER_HPP_INCLUDED

#include <utility>

#include <omplext_odeint/boost/numeric/odeint/util/bind.hpp>
#include <omplext_odeint/boost/numeric/odeint/util/unwrap_reference.hpp>
#include <omplext_odeint/boost/numeric/odeint/stepper/stepper_categories.hpp>

#include <omplext_odeint/boost/numeric/odeint/util/ublas_wrapper.hpp>
#include <omplext_odeint/boost/numeric/odeint/util/is_resizeable.hpp>
#include <omplext_odeint/boost/numeric/odeint/util/resizer.hpp>

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

namespace boost {
namespace numeric {
namespace omplext_odeint {

template< class ValueType , class Resizer = initially_resizer >
class implicit_euler
{

public:

    typedef ValueType value_type;
    typedef value_type time_type;
    typedef boost::numeric::ublas::vector< value_type > state_type;
    typedef state_wrapper< state_type > wrapped_state_type;
    typedef state_type deriv_type;
    typedef state_wrapper< deriv_type > wrapped_deriv_type;
    typedef boost::numeric::ublas::matrix< value_type > matrix_type;
    typedef state_wrapper< matrix_type > wrapped_matrix_type;
    typedef boost::numeric::ublas::permutation_matrix< size_t > pmatrix_type;
    typedef state_wrapper< pmatrix_type > wrapped_pmatrix_type;
    typedef Resizer resizer_type;
    typedef stepper_tag stepper_category;
    typedef implicit_euler< ValueType , Resizer > stepper_type;

    implicit_euler( value_type epsilon = 1E-6 )
    : m_epsilon( epsilon )
    { }

    template< class System >
    void do_step( System system , state_type &x , time_type t , time_type dt )
    {
        typedef typename omplext_odeint::unwrap_reference< System >::type system_type;
        typedef typename omplext_odeint::unwrap_reference< typename system_type::first_type >::type deriv_func_type;
        typedef typename omplext_odeint::unwrap_reference< typename system_type::second_type >::type jacobi_func_type;
        system_type &sys = system;
        deriv_func_type &deriv_func = sys.first;
        jacobi_func_type &jacobi_func = sys.second;

        m_resizer.adjust_size( x , detail::bind( &stepper_type::template resize_impl<state_type> , detail::ref( *this ) , detail::_1 ) );

        for( size_t i=0 ; i<x.size() ; ++i )
            m_pm.m_v[i] = i;

        t += dt;

        // apply first Newton step
        deriv_func( x , m_dxdt.m_v , t );

        m_b.m_v = dt * m_dxdt.m_v;

        jacobi_func( x , m_jacobi.m_v  , t );
        m_jacobi.m_v *= dt;
        m_jacobi.m_v -= boost::numeric::ublas::identity_matrix< value_type >( x.size() );

        solve( m_b.m_v , m_jacobi.m_v );

        m_x.m_v = x - m_b.m_v;

        // iterate Newton until some precision is reached
        // ToDo: maybe we should apply only one Newton step -> linear implicit one-step scheme
        while( boost::numeric::ublas::norm_2( m_b.m_v ) > m_epsilon )
        {
            deriv_func( m_x.m_v , m_dxdt.m_v , t );
            m_b.m_v = x - m_x.m_v + dt*m_dxdt.m_v;

            // simplified version, only the first Jacobian is used
            //            jacobi( m_x , m_jacobi , t );
            //            m_jacobi *= dt;
            //            m_jacobi -= boost::numeric::ublas::identity_matrix< value_type >( x.size() );

            solve( m_b.m_v , m_jacobi.m_v );

            m_x.m_v -= m_b.m_v;
        }
        x = m_x.m_v;
    }

    template< class StateType >
    void adjust_size( const StateType &x )
    {
        resize_impl( x );
    }

private:

    template< class StateIn >
    bool resize_impl( const StateIn &x )
    {
        bool resized = false;
        resized |= adjust_size_by_resizeability( m_dxdt , x , typename is_resizeable<deriv_type>::type() );
        resized |= adjust_size_by_resizeability( m_x , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_b , x , typename is_resizeable<deriv_type>::type() );
        resized |= adjust_size_by_resizeability( m_jacobi , x , typename is_resizeable<matrix_type>::type() );
        resized |= adjust_size_by_resizeability( m_pm , x , typename is_resizeable<pmatrix_type>::type() );
        return resized;
    }

    void solve( state_type &x , matrix_type &m )
    {
        int res = boost::numeric::ublas::lu_factorize( m , m_pm.m_v );
        if( res != 0 ) exit(0);
        boost::numeric::ublas::lu_substitute( m , m_pm.m_v , x );
    }

private:

    value_type m_epsilon;
    resizer_type m_resizer;
    wrapped_deriv_type m_dxdt;
    wrapped_state_type m_x;
    wrapped_deriv_type m_b;
    wrapped_matrix_type m_jacobi;
    wrapped_pmatrix_type m_pm;

};

} // odeint
} // numeric
} // boost

#endif // BOOST_NUMERIC_ODEINT_STEPPER_IMPLICIT_EULER_HPP_INCLUDED


// sphere_fit.cpp
//
// Example from Foundations of Geometric Algebra Computing
// by Dietmar Hildenbrand
//
// Page 68.

#include <iostream>
#include <string>
#include <boost/numpy.hpp>
#include <ceres/ceres.h>
#include <glog/logging.h>
#include <hep/ga.hpp>

#include "game/types.h"
#include "game/ceres_python_utils.h"

namespace bp = boost::python;
namespace np = boost::numpy;

using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;

namespace game {

struct SphereFit {
  SphereFit(const SphereFit &sphere_fit) {}
  SphereFit() {}

  void SetSolverOptions(const bp::dict &solver_options) {
    bp::extract<std::string> linear_solver_type(
        solver_options["linear_solver_type"]);
    if (linear_solver_type.check()) {
      ceres::StringToLinearSolverType(linear_solver_type(),
                                      &options_.linear_solver_type);
    }

    bp::extract<int> max_num_iterations(solver_options["max_num_iterations"]);
    if (max_num_iterations.check()) {
      options_.max_num_iterations = max_num_iterations();
    }

    bp::extract<int> num_threads(solver_options["num_threads"]);
    if (num_threads.check()) {
      options_.num_threads = num_threads();
    }

    bp::extract<int> num_linear_solver_threads(
        solver_options["num_linear_solver_threads"]);
    if (num_linear_solver_threads.check()) {
      options_.num_linear_solver_threads = num_linear_solver_threads();
    }

    bp::extract<double> parameter_tolerance(
        solver_options["parameter_tolerance"]);
    if (parameter_tolerance.check()) {
      options_.parameter_tolerance = parameter_tolerance();
    }

    bp::extract<double> function_tolerance(
        solver_options["function_tolerance"]);
    if (function_tolerance.check()) {
      options_.function_tolerance = function_tolerance();
    }

    bp::extract<std::string> trust_region_strategy_type(
        solver_options["trust_region_strategy_type"]);
    if (trust_region_strategy_type.check()) {
      ceres::StringToTrustRegionStrategyType(
          trust_region_strategy_type(), &options_.trust_region_strategy_type);
    }

    bp::extract<bool> minimizer_progress_to_stdout(
        solver_options["minimizer_progress_to_stdout"]);
    if (minimizer_progress_to_stdout.check()) {
      options_.minimizer_progress_to_stdout = minimizer_progress_to_stdout();
    }

    bp::extract<std::string> minimizer_type(solver_options["minimizer_type"]);
    if (minimizer_type.check()) {
      ceres::StringToMinimizerType(minimizer_type(), &options_.minimizer_type);
    }

    bp::extract<bp::list> trust_region_minimizer_iterations_to_dump(
        solver_options["trust_region_minimizer_iterations_to_dump"]);
    if (trust_region_minimizer_iterations_to_dump.check()) {
      std::vector<int> iterations_to_dump{};
      bp::list list = trust_region_minimizer_iterations_to_dump();
      for (int i = 0; i < bp::len(list); ++i) {
        iterations_to_dump.push_back(bp::extract<int>(list[i])());
      }
      options_.trust_region_minimizer_iterations_to_dump = iterations_to_dump;
    }

    bp::extract<std::string> trust_region_problem_dump_directory(
        solver_options["trust_region_problem_dump_directory"]);
    if (trust_region_problem_dump_directory.check()) {
      options_.trust_region_problem_dump_directory =
          trust_region_problem_dump_directory();
    }
  }

  template <typename T, typename MultivectorT>
  static auto Inverse(const MultivectorT &multivector) -> MultivectorT {
    return cga::Scalar<T>{static_cast<T>(1.0) /
                          hep::eval((multivector * ~multivector))[0]} *
           ~multivector;
  }

  struct CostFunctor {
    CostFunctor(const double *point) : point_(point) {}

    template <typename T>
    bool operator()(const T *const s /* sphere: 5 parameters */,
                    T *residual /* 1 parameter */) const {
      // Conformal split
      cga::Infty<T> ni{static_cast<T>(-1.0), static_cast<T>(1.0)};
      cga::Orig<T> no{static_cast<T>(0.5), static_cast<T>(0.5)};

      // Euclidean basis
      cga::E1<T> e1{static_cast<T>(1.0)};
      cga::E2<T> e2{static_cast<T>(1.0)};
      cga::E3<T> e3{static_cast<T>(1.0)};

      cga::Scalar<T> half{static_cast<T>(0.5)};

      // Create Euclidean point (vector)
      cga::EuclideanPoint<T> euc_point{static_cast<T>(point_[0]),
                                       static_cast<T>(point_[1]),
                                       static_cast<T>(point_[2])};

      // Create conformal point
      cga::Point<T> point =
          hep::grade<1>(euc_point + half * euc_point * euc_point * ni + no);

      // Create conformal sphere
      //      cga::Sphere<T> sphere = hep::eval(
      //          static_cast<T>(s[0] / s[3]) * e1 + static_cast<T>(s[1] / s[3])
      //          * e2 +
      //          static_cast<T>(s[2] / s[3]) * e3 + static_cast<T>(s[4] / s[3])
      //          * ni +
      //          static_cast<T>(s[3] / s[3]) * no);

      cga::Sphere<T> sphere =
          hep::eval(static_cast<T>(s[0]) * e1 + static_cast<T>(s[1]) * e2 +
                    static_cast<T>(s[2]) * e3 + static_cast<T>(s[4]) * ni +
                    static_cast<T>(s[3]) * no);

      // Evaluate distance
      //      auto distance = hep::eval(hep::inner_prod(point, sphere) *
      //                                hep::inner_prod(point,
      //                                Inverse<T>(sphere)));

      auto distance = hep::eval(hep::inner_prod(point, sphere));
      auto rho_squared = hep::eval(hep::inner_prod(sphere, sphere));

      residual[0] = distance[0] / sqrt(rho_squared[0]);
      //      residual[0] = distance[0];

      return true;
    }

   private:
    const double *point_;
  };

  static ceres::CostFunction *Create(const double *point) {
    return (new ceres::AutoDiffCostFunction<SphereFit::CostFunctor, 1, 5>(
        new SphereFit::CostFunctor(point)));
  }

  np::ndarray Run(np::ndarray sphere, np::ndarray points) {
    if (!(points.get_flags() & np::ndarray::C_CONTIGUOUS)) {
      throw "input array a must be contiguous";
    }

    auto rows_points = points.shape(0);
    auto cols_points = points.shape(1);
    auto rows_sphere = sphere.shape(0);
    auto cols_sphere = sphere.shape(1);

    if (!((rows_sphere == 5) && (cols_sphere == 1))) {
      throw "parameter array must have shape (5,1)";
    }

    double *sphere_data = reinterpret_cast<double *>(sphere.get_data());
    double *points_data = reinterpret_cast<double *>(points.get_data());

    for (int i = 0; i < rows_points; ++i) {
      ceres::CostFunction *cost_function =
          SphereFit::Create(&points_data[cols_points * i]);
      problem_.AddResidualBlock(cost_function, NULL, sphere_data);
    }

    Solve(options_, &problem_, &summary_);

    return sphere;
  }

  auto Summary() -> bp::dict { return game::SummaryToDict(summary_); }

  Problem problem_;
  Solver::Options options_;
  Solver::Summary summary_;
};
}

BOOST_PYTHON_MODULE(libsphere_fit) {
  np::initialize();

  bp::class_<game::SphereFit>("SphereFit")
      .def("run", &game::SphereFit::Run)
      .def("summary", &game::SphereFit::Summary)
      .def("set_solver_options", &game::SphereFit::SetSolverOptions);
}


#include <NTL/FFT.h>

#include <NTL/new.h>

NTL_START_IMPL

long NumFFTPrimes = 0;

long *FFTPrime = 0;
long **RootTable = 0;
long **RootInvTable = 0;
long **TwoInvTable = 0;
double *FFTPrimeInv = 0;

static
long IsFFTPrime(long n, long& w)
{
   long  m, x, y, z;
   long j, k;

   if (n % 3 == 0) return 0;

   if (n % 5 == 0) return 0;

   if (n % 7 == 0) return 0;

   m = n - 1;
   k = 0;
   while ((m & 1) == 0) {
      m = m >> 1;
      k++;
   }

   for (;;) {
      x = RandomBnd(n);

      if (x == 0) continue;
      z = PowerMod(x, m, n);
      if (z == 1) continue;

      x = z;
      j = 0;
      do {
         y = z;
         z = MulMod(y, y, n);
         j++;
      } while (j != k && z != 1);

      if (z != 1 || y !=  n-1) return 0;

      if (j == k)
         break;
   }

   /* x^{2^k} = 1 mod n, x^{2^{k-1}} = -1 mod n */

   long TrialBound;

   TrialBound = m >> k;
   if (TrialBound > 0) {
      if (!ProbPrime(n, 5)) return 0;

      /* we have to do trial division by special numbers */

      TrialBound = SqrRoot(TrialBound);

      long a, b;

      for (a = 1; a <= TrialBound; a++) {
         b = (a << k) + 1;
         if (n % b == 0) return 0;
      }
   }

   /* n is an FFT prime */

   for (j = NTL_FFTMaxRoot; j < k; j++)
      x = MulMod(x, x, n);

   w = x;
   return 1;
}

static
void NextFFTPrime(long& q, long& w)
{
   static long m = NTL_FFTMaxRootBnd + 1;
   static long k = 0;

   long t, cand;

   for (;;) {
      if (k == 0) {
         m--;
         if (m < 5) Error("ran out of FFT primes");
         k = 1L << (NTL_SP_NBITS-m-2);
      }

      k--;

      cand = (1L << (NTL_SP_NBITS-1)) + (k << (m+1)) + (1L << m) + 1;

      if (!IsFFTPrime(cand, t)) continue;
      q = cand;
      w = t;
      return;
   }
}

long CalcMaxRoot(long p)
{
   p = p-1;
   long k = 0;
   while ((p & 1) == 0) {
      p = p >> 1;
      k++;
   }

   if (k > NTL_FFTMaxRoot)
      return NTL_FFTMaxRoot;
   else
      return k;
}

void UseFFTPrime(long index)
{
   if (index < 0 || index > NumFFTPrimes)
      Error("invalid FFT prime index");

   if (index < NumFFTPrimes) return;

   long q, w;

   NextFFTPrime(q, w);

   long mr = CalcMaxRoot(q);

   // tables are allocated in increments of 100

   if (index == 0) {
      FFTPrime = (long *) NTL_MALLOC(100, sizeof(long), 0);
      RootTable = (long **) NTL_MALLOC(100, sizeof(long *), 0);
      RootInvTable = (long **) NTL_MALLOC(100, sizeof(long *), 0);
      TwoInvTable = (long **) NTL_MALLOC(100, sizeof(long *), 0);
      FFTPrimeInv = (double *) NTL_MALLOC(100, sizeof(double), 0);
   }
   else if ((index % 100) == 0) {
      FFTPrime = (long *) NTL_REALLOC(FFTPrime, index+100, sizeof(long), 0);
      RootTable = (long **)
                  NTL_REALLOC(RootTable, index+100, sizeof(long *), 0);
      RootInvTable = (long **)
                     NTL_REALLOC(RootInvTable, index+100, sizeof(long *), 0);
      TwoInvTable = (long **)
                    NTL_REALLOC(TwoInvTable, index+100, sizeof(long *), 0);
      FFTPrimeInv = (double *)
                    NTL_REALLOC(FFTPrimeInv, index+100, sizeof(double), 0);
   }

   if (!FFTPrime || !RootTable || !RootInvTable || !TwoInvTable ||
       !FFTPrimeInv)
      Error("out of space");

   FFTPrime[index] = q;

   long *rt, *rit, *tit;

   if (!(rt = RootTable[index] = (long*) NTL_MALLOC(mr+1, sizeof(long), 0)))
      Error("out of space");
   if (!(rit = RootInvTable[index] = (long*) NTL_MALLOC(mr+1, sizeof(long), 0)))
      Error("out of space");
   if (!(tit = TwoInvTable[index] = (long*) NTL_MALLOC(mr+1, sizeof(long), 0)))
      Error("out of space");

   long j;
   long t;

   rt[mr] = w;
   for (j = mr-1; j >= 0; j--)
      rt[j] = MulMod(rt[j+1], rt[j+1], q);

   rit[mr] = InvMod(w, q);
   for (j = mr-1; j >= 0; j--)
      rit[j] = MulMod(rit[j+1], rit[j+1], q);

   t = InvMod(2, q);
   tit[0] = 1;
   for (j = 1; j <= mr; j++)
      tit[j] = MulMod(tit[j-1], t, q);

   FFTPrimeInv[index] = 1/double(q);

   NumFFTPrimes++;
}

static
long RevInc(long a, long k)
{
   long j, m;

   j = k;
   m = 1L << (k-1);

   while (j && (m & a)) {
      a ^= m;
      m >>= 1;
      j--;
   }
   if (j) a ^= m;
   return a;
}

static
void BitReverseCopy(long *A, const long *a, long k)
{
   static long* mem[NTL_FFTMaxRoot+1];

   long n = 1L << k;
   long* rev;
   long i, j;

   rev = mem[k];
   if (!rev) {
      rev = mem[k] = (long *) NTL_MALLOC(n, sizeof(long), 0);
      if (!rev) Error("out of memory in BitReverseCopy");
      for (i = 0, j = 0; i < n; i++, j = RevInc(j, k))
         rev[i] = j;
   }

   for (i = 0; i < n; i++)
      A[rev[i]] = a[i];
}

/*
 * Our FFT is based on the routine in Cormen, Leiserson, Rivest, and Stein.
 * For very large inputs, it should be relatively cache friendly.
 * The inner loop has been unrolled and pipelined, to exploit any
 * low-level parallelism in the machine.
 */

void FFT(long* A, const long* a, long k, long q, const long* root)

// performs a 2^k-point convolution modulo q

{
   if (k <= 1) {
      if (k == 0) {
	 A[0] = a[0];
	 return;
      }
      if (k == 1) {
	 A[0] = AddMod(a[0], a[1], q);
	 A[1] = SubMod(a[0], a[1], q);
	 return;
      }
   }

   // assume k > 1

   static long tab_size = 0;
   static long *wtab = 0;
   static mulmod_precon_t *wqinvtab = 0;

   if (!tab_size) {
      tab_size = k;

      wtab = (long *) NTL_MALLOC(1L << (k-2), sizeof(long), 0);
      wqinvtab = (mulmod_precon_t *)
                 NTL_MALLOC(1L << (k-2), sizeof(mulmod_precon_t), 0);
      if (!wtab || !wqinvtab) Error("out of space");
   }
   else if (tab_size < k) {
      tab_size = k;

      wtab = (long *) NTL_REALLOC(wtab, 1L << (k-2), sizeof(long), 0);
      wqinvtab = (mulmod_precon_t *)
                 NTL_REALLOC(wqinvtab, 1L << (k-2), sizeof(mulmod_precon_t), 0);
      if (!wtab || !wqinvtab) Error("out of space");
   }

   double qinv = 1/((double) q);

   wtab[0] = 1;
   wqinvtab[0] = PrepMulModPrecon(1, q, qinv);

   BitReverseCopy(A, a, k);

   long n = 1L << k;

   long s, m, m_half, m_fourth, i, j, t, u, t1, u1, uu, uu1, tt, tt1;

   long w;
   mulmod_precon_t wqinv;

   // s = 1

   for (i = 0; i < n; i += 2) {
      t = A[i + 1];
      u = A[i];
      A[i] = AddMod(u, t, q);
      A[i+1] = SubMod(u, t, q);
   }

   for (s = 2; s < k; s++) {
      m = 1L << s;
      m_half = 1L << (s-1);
      m_fourth = 1L << (s-2);

      // prepare wtab...

      w = root[s];
      wqinv = PrepMulModPrecon(w, q, qinv);

      for (i = m_half-1, j = m_fourth-1; i >= 0; i -= 2, j--) {
         wtab[i-1] = wtab[j];
         wqinvtab[i-1] = wqinvtab[j];
         wtab[i] = MulModPrecon(wtab[i-1], w, q, wqinv);
         wqinvtab[i] = PrepMulModPrecon(wtab[i], q, qinv);
      }

      for (i = 0; i < n; i+= m) {

         t = A[i + m_half];
         u = A[i];
         t1 = MulModPrecon(A[i + 1+ m_half], w, q, wqinv);
         u1 = A[i+1];

         for (j = 0; j < m_half-2; j += 2) {
            tt = MulModPrecon(A[i + j + 2 + m_half], wtab[j+2], q, wqinvtab[j+2]);
            uu = A[i + j + 2];

            tt1 = MulModPrecon(A[i + j + 3+ m_half], wtab[j+3], q, wqinvtab[j+3]);
            uu1 = A[i + j + 3];

            A[i + j] = AddMod(u, t, q);
            A[i + j + m_half] = SubMod(u, t, q);
            A[i + j + 1] = AddMod(u1, t1, q);
            A[i + j + 1 + m_half] = SubMod(u1, t1, q);
            t = tt;
            t1 = tt1;
            u = uu;
            u1 = uu1;
         }

         A[i + j] = AddMod(u, t, q);
         A[i + j + m_half] = SubMod(u, t, q);
         A[i + j + 1] = AddMod(u1, t1, q);
         A[i + j + 1 + m_half] = SubMod(u1, t1, q);

      }
   }

   // s == k...special case

   m = 1L << s;
   m_half = 1L << (s-1);
   m_fourth = 1L << (s-2);

   w = root[s];
   wqinv = PrepMulModPrecon(w, q, qinv);

   // j = 0, 1

   t = A[m_half];
   u = A[0];
   t1 = MulModPrecon(A[1+ m_half], w, q, wqinv);
   u1 = A[1];

   A[0] = AddMod(u, t, q);
   A[m_half] = SubMod(u, t, q);
   A[1] = AddMod(u1, t1, q);
   A[1 + m_half] = SubMod(u1, t1, q);

   for (j = 2; j < m_half; j += 2) {
      t = MulModPrecon(A[j + m_half], wtab[j >> 1], q, wqinvtab[j >> 1]);
      u = A[j];
      t1 = MulModPrecon(A[j + 1+ m_half], wtab[j >> 1], q,
                        wqinvtab[j >> 1]);
      t1 = MulModPrecon(t1, w, q, wqinv);
      u1 = A[j + 1];

      A[j] = AddMod(u, t, q);
      A[j + m_half] = SubMod(u, t, q);
      A[j + 1] = AddMod(u1, t1, q);
      A[j + 1 + m_half] = SubMod(u1, t1, q);

   }
}

NTL_END_IMPL


#include <Engine/MeshEdit/MinSurf.h>

#include <Engine/Primitive/TriMesh.h>

#include <Eigen/Sparse>
#include <Eigen/SparseQR>

using namespace Ubpa;

using namespace std;
using namespace Eigen;

MinSurf::MinSurf(Ptr<TriMesh> triMesh)
	: heMesh(make_shared<HEMesh<V>>())
{
	Init(triMesh);
}

void MinSurf::Clear() {
	heMesh->Clear();
	triMesh = nullptr;
}

bool MinSurf::Init(Ptr<TriMesh> triMesh) {
	Clear();

	if (triMesh == nullptr)
		return true;

	if (triMesh->GetType() == TriMesh::INVALID) {
		printf("ERROR::MinSurf::Init:\n"
			"\t""trimesh is invalid\n");
		return false;
	}

	// init half-edge structure
	size_t nV = triMesh->GetPositions().size();
	vector<vector<size_t>> triangles;
	triangles.reserve(triMesh->GetTriangles().size());
	for (auto triangle : triMesh->GetTriangles())
		triangles.push_back({ triangle->idx[0], triangle->idx[1], triangle->idx[2] });
	heMesh->Reserve(nV);
	heMesh->Init(triangles);

	if (!heMesh->IsTriMesh() || !heMesh->HaveBoundary()) {
		printf("ERROR::MinSurf::Init:\n"
			"\t""trimesh is not a triangle mesh or hasn't a boundaries\n");
		heMesh->Clear();
		return false;
	}

	// triangle mesh's positions ->  half-edge structure's positions
	for (int i = 0; i < nV; i++) {
		auto v = heMesh->Vertices().at(i);
		v->pos = triMesh->GetPositions()[i].cast_to<vecf3>();
	}

	this->triMesh = triMesh;
	return true;
}

bool MinSurf::Run() {
	if (heMesh->IsEmpty() || !triMesh) {
		printf("ERROR::MinSurf::Run\n"
			"\t""heMesh->IsEmpty() || !triMesh\n");
		return false;
	}

	Minimize();

	// half-edge structure -> triangle mesh
	size_t nV = heMesh->NumVertices();
	size_t nF = heMesh->NumPolygons();
	vector<pointf3> positions;
	vector<unsigned> indice;
	positions.reserve(nV);
	indice.reserve(3 * nF);
	for (auto v : heMesh->Vertices())
		positions.push_back(v->pos.cast_to<pointf3>());
	for (auto f : heMesh->Polygons()) { // f is triangle
		for (auto v : f->BoundaryVertice()) // vertices of the triangle
			indice.push_back(static_cast<unsigned>(heMesh->Index(v)));
	}

	triMesh->Init(indice, positions);

	return true;
}

void MinSurf::Minimize() {
	// First, detect and fix boundary
	random_set<V*> boundary_points;
	random_set<V*> inner_points;

	auto boundaries = this->heMesh->Boundaries();
	if (boundaries.size() != 1) {
		cout << "ERROR::MinSurf::Minimize:" << endl
			 << "\t" << "got boundaries = " << boundaries.size()
			 << " (expect 1)" << endl;
		return;
	}

	for (auto v: boundaries[0]) {
		boundary_points.insert(v->Origin());
	}

	for (auto v: heMesh->Vertices()) {
		if (!boundary_points.contains(v)) {
			inner_points.insert(v);
		}
	}
	// Build sparse matrix
	size_t n = inner_points.size();
	SparseMatrix<float> coeff_mat(n, n);
	coeff_mat.setZero();
	VectorXf b_vec_x = VectorXf::Zero(n);
	VectorXf b_vec_y = VectorXf::Zero(n);
	VectorXf b_vec_z = VectorXf::Zero(n);

	cout << "coeff mat build start" << endl;

	int current_row = 0;
	for (auto v: inner_points) {
		// vidx CERTAINLY follows order (and it's redundant)
		size_t vidx = inner_points.idx(v);
		auto adj = v->AdjVertices();
		size_t degree = v->Degree();
		for (auto adjv : adj) {
			// check type
			if (boundary_points.contains(adjv)) { // this set is usually smaller
				b_vec_x(current_row) += (1.0f / degree) * adjv->pos[0];
				b_vec_y(current_row) += (1.0f / degree) * adjv->pos[1];
				b_vec_z(current_row) += (1.0f / degree) * adjv->pos[2];
			} else { // inner
				assert(inner_points.contains(adjv));
				size_t adjidx = inner_points.idx(adjv);
				// todo add assert = 0
				coeff_mat.insert(current_row, adjidx) = - 1.0f / degree;
			}
		}

		// add itself
		// todo add assert
		coeff_mat.insert(current_row, vidx) = 1;
		current_row++;
	}

	cout << "coeff mat build complete" << endl;

	// Solve
	SparseQR<SparseMatrix<float>, COLAMDOrdering<int>> solver;

	cout << "begin makeCompressed()" << endl;
	coeff_mat.makeCompressed();

	cout << "begin compute()" << endl;
	solver.compute(coeff_mat);
	if (solver.info() != Eigen::Success) {
		cout << "solver: decomposition was not successfull." << endl;
		return;
	}

	cout << "begin solve() for x" << endl;
	VectorXf res_x = solver.solve(b_vec_x);

	cout << "begin solve() for y" << endl;
	VectorXf res_y = solver.solve(b_vec_y);

	cout << "begin solve() for z" << endl;
	VectorXf res_z = solver.solve(b_vec_z);

	// Update vertex coordinates
	for (int i = 0; i < n; i++) {
		// find the corresponding point
		auto v = inner_points[i];
		vecf3 new_pos = { res_x(i), res_y(i), res_z(i) }; // works?

		//cout << new_pos << endl;
		v->pos = new_pos;
	}

}


#ifndef KALMAN_FILTER_H_
#define KALMAN_FILTER_H_

#include <Eigen/Dense>

template<typename T, class System>
class KalmanFilter
{
typedef Eigen::Matrix<T, Eigen::Dynamic, 1> VectorXt;    //列向量
typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> MatrixXt;
public:

    KalmanFilter(const System& _system, int _state_dim, int _input_dim, int _measurement_dim,
                 const MatrixXt& _process_noise, const MatrixXt& _measurement_noise,
                 const VectorXt& _mean, const MatrixXt& _cov):
        system(_system),
        state_dim(_state_dim),
        input_dim(_input_dim),
        measurement_dim(_measurement_dim),
        N(_state_dim),
        M(_measurement_dim),
        mean(_mean),
        cov(_cov),
        process_noise(_process_noise),
        measurement_noise(_measurement_noise)
    {

    }

    ~KalmanFilter()
    {
    }

    virtual void predict(const VectorXt& control) = 0;
    virtual void correct(const VectorXt& measurement) = 0;

    virtual const VectorXt& getMean() const { return mean; }
    virtual const MatrixXt& getCov() const { return cov; }

    virtual System& getSystem() { return system; }
    virtual const System& getSystem() const { return system; }
    virtual const MatrixXt& getProcessNoiseCov() const { return process_noise; }
    virtual const MatrixXt& getMeasurementNoiseCov() const { return measurement_noise; }
    virtual const MatrixXt& getKalmanGain() const { return kalman_gain; }

    /*			setter			*/
    virtual void setMean(const VectorXt& m) { mean = m;}
    virtual void setCov(const MatrixXt& s) { cov = s;}

    virtual void setProcessNoiseCov(const MatrixXt& p) { process_noise = p;}
    virtual void setMeasurementNoiseCov(const MatrixXt& m) { measurement_noise = m;}

    /**
    * @brief make covariance matrix positive finite
    * @param cov  covariance matrix
    */
    virtual void ensurePositiveFinite(MatrixXt& cov)
    {
        return;
        const double eps = 1e-9;

        Eigen::EigenSolver<MatrixXt> solver(cov);
        MatrixXt D = solver.pseudoEigenvalueMatrix();
        MatrixXt V = solver.pseudoEigenvectors();
        for (int i = 0; i < D.rows(); i++)
        {
            if (D(i, i) < eps)
            D(i, i) = eps;
        }

        cov = V * D * V.inverse();
    }

public:
    const int state_dim,       N; //状态向量维度
    const int input_dim;
    const int measurement_dim, M; //测量向量维度

    VectorXt mean;                //均值
    MatrixXt cov;                 //协方差

    System system;                //控制系统
    MatrixXt process_noise;		  //过程噪声 Q
    MatrixXt measurement_noise;	  //测量噪声 R

    MatrixXt kalman_gain;         //卡尔曼增益K

};

#endif


#pragma once
#include <Eigen/Core>
#include <map>
#include <optional>
#include <tuple>

#include "geohash/geometry.hpp"
#include "geohash/math.hpp"

namespace geohash::int64 {

// Returns the precision in longitude/latitude and degrees for the given
// precision
[[nodiscard]] inline auto constexpr error_with_precision(
    const uint32_t precision) -> std::tuple<double, double> {
  auto lat_bits = static_cast<int32_t>(precision >> 1U);
  auto lng_bits = static_cast<int32_t>(precision - lat_bits);

  return std::make_tuple(360 * power2(-lng_bits), 180 * power2(-lat_bits));
}

// Encode a point into geohash with the given precision
[[nodiscard]] auto encode(const Point& point, uint32_t precision) -> uint64_t;

// Encode points into geohash with the given precision
[[nodiscard]] inline auto encode(
    const Eigen::Ref<const Eigen::Matrix<Point, -1, 1>>& points,
    uint32_t precision) -> Eigen::Matrix<uint64_t, -1, 1> {
  auto result = Eigen::Matrix<uint64_t, -1, 1>(points.size());
  for (Eigen::Index ix = 0; ix < points.size(); ++ix) {
    result(ix) = encode(points(ix), precision);
  }
  return result;
}

// Returns the region encoded by the integer geohash with the specified
// precision.
[[nodiscard]] auto bounding_box(uint64_t hash, uint32_t precision) -> Box;

// Decode a hash into a spherical equatorial point with the given precision.
// If round is true, the coordinates of the points will be rounded to the
// accuracy defined by the GeoHash.
[[nodiscard]] inline auto decode(const uint64_t hash, const uint32_t precision,
                                 const bool round) -> Point {
  auto bbox = bounding_box(hash, precision);
  return round ? bbox.round() : bbox.center();
}

// Decode hashs into a spherical equatorial points with the given bit depth.
// If round is true, the coordinates of the points will be rounded to the
// accuracy defined by the GeoHash.
[[nodiscard]] inline auto decode(
    const Eigen::Ref<const Eigen::Matrix<uint64_t, -1, 1>>& hashs,
    const uint32_t precision, const bool center)
    -> Eigen::Matrix<Point, -1, 1> {
  auto result = Eigen::Matrix<Point, -1, 1>(hashs.size());
  for (Eigen::Index ix = 0; ix < hashs.size(); ++ix) {
    result(ix) = decode(hashs(ix), precision, center);
  }
  return result;
}

// Returns all neighbors hash clockwise from north around northwest at the given
// precision.
// 7 0 1
// 6 x 2
// 5 4 3
[[nodiscard]] auto neighbors(const uint64_t hash, const uint32_t precision)
    -> Eigen::Matrix<uint64_t, 8, 1>;

// Returns the property of the grid covering the given box: geohash of the
// minimum corner point, number of boxes in longitudes and latitudes.
[[nodiscard]] auto grid_properties(const Box& box, uint32_t precision)
    -> std::tuple<uint64_t, size_t, size_t>;

// Returns all the GeoHash codes within the box.
[[nodiscard]] auto bounding_boxes(const std::optional<Box>& box, uint32_t chars)
    -> Eigen::Matrix<uint64_t, -1, 1>;

// Returns all the GeoHash codes within the polygon.
[[nodiscard]] inline auto bounding_boxes(const Polygon& polygon, uint32_t chars)
    -> Eigen::Matrix<uint64_t, -1, 1> {
  auto box = Box();
  boost::geometry::envelope<Polygon, Box>(polygon, box);
  return bounding_boxes(box, chars);
}

// Returns the start and end indexes of the different GeoHash boxes.
[[nodiscard]] auto where(
    const Eigen::Ref<const Eigen::Matrix<uint64_t, -1, -1>>& hashs)
    -> std::map<uint64_t, std::tuple<std::tuple<int64_t, int64_t>,
                                     std::tuple<int64_t, int64_t>>>;

}  // namespace geohash::int64


#include <math.h>
#include <vector>
#include <limits>
using namespace std;

#include <armadillo>
using namespace arma;

#include "optimize_state_reconstructor.h"
#include "rate_model.h"
#include "state_reconstructor.h"

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

OptimizeStateReconstructor::OptimizeStateReconstructor(RateModel * _rm,StateReconstructor * _sr, mat * _free_mask, int _nfree):
    rm(_rm),sr(_sr),free_variables(_free_mask),nfree_variables(_nfree),maxiterations(10000),stoppingprecision(0.001) {}

double OptimizeStateReconstructor::GetLikelihoodWithOptimized(const gsl_vector * variables) {
    for (unsigned int i=0; i < free_variables->n_rows; i++) {
        for (unsigned int j=0; j < free_variables->n_cols; j++) {
            if (i != j) {
                rm->set_Q_cell(i,j,gsl_vector_get(variables,(*free_variables)(i, j)));
                if (rm->get_Q()(i, j) < 0 || rm->get_Q()(i, j) >= 1000) {
                    return 100000000;
                }
            }
        }
    }
    double like;
    rm->set_Q_diag();
    like = sr->eval_likelihood();
    //cout << like << endl;
    if (like < 0 || like == std::numeric_limits<double>::infinity()) {
        like = 100000000;
    }
    return like;
}

double OptimizeStateReconstructor::GetLikelihoodWithOptimized_gsl(const gsl_vector * variables, void *obj) {
    double temp;
    temp= ((OptimizeStateReconstructor*)obj)->GetLikelihoodWithOptimized(variables);
    return temp;
}

/*
 * USES THE SIMPLEX ALGORITHM
 *
 */
mat OptimizeStateReconstructor::optimize() {
    const gsl_multimin_fminimizer_type *T = gsl_multimin_fminimizer_nmsimplex2;
    gsl_multimin_fminimizer *s = NULL;
    gsl_vector *ss, *x;
    size_t np = nfree_variables;
    size_t iter = 0, i;
    int status;
    double size;
    /* Initial vertex size vector */
    ss = gsl_vector_alloc (np);
    /* Set all step sizes to .01 */ //Note that it was originally 1
    gsl_vector_set_all (ss, .2);
    /* Starting point */
    //cout<<"Now in OPtimizaRateWithGivenTipVariance in OptimizationFn"<<endl;
    x = gsl_vector_alloc (np);
    for (unsigned int i=0; i < np; i++) {
        gsl_vector_set (x, i, 0.1);
    }
    OptimizeStateReconstructor *pt;
    pt=(this);
    double (*F)(const gsl_vector *, void *);
    F = &OptimizeStateReconstructor::GetLikelihoodWithOptimized_gsl;
    /* Initialize method and iterate */
    gsl_multimin_function minex_func;
    minex_func.f =* F;
    minex_func.params = pt;
    minex_func.n = np;
    s = gsl_multimin_fminimizer_alloc (T, np);
    gsl_multimin_fminimizer_set (s, &minex_func, x, ss);
    do {
        //cout<<"Now on iteration "<<iter<<endl;
        iter++;
        status = gsl_multimin_fminimizer_iterate(s);
        if (status != 0) { //0 Means it's a success
        //    printf ("error: %s\n", gsl_strerror (status));
            break;
        }
        size = gsl_multimin_fminimizer_size (s);
        //status = gsl_multimin_test_size (size, 1e-2);
        status = gsl_multimin_test_size (size, stoppingprecision); //since we want more precision
        if (status == GSL_SUCCESS) {
        //    printf ("converged to minimum at\n");
        }
        //printf ("%5d ", iter);
        for (i = 0; i < np; i++) {
        //    printf ("%10.3e ", gsl_vector_get (s->x, i));
        }
        //printf ("f() = %7.3f size = %.3f\n", s->fval, size);
    }
    while (status == GSL_CONTINUE && iter < maxiterations);
    mat results (free_variables->n_rows, free_variables->n_cols); results.fill(0);
    for (unsigned int i=0; i < results.n_rows; i++) {
        for (unsigned int j=0; j < results.n_cols; j++) {
            if (i != j) {
                results(i, j) = (gsl_vector_get(s->x, (*free_variables)(i, j)));
            }
        }
    }
    gsl_vector_free(x);
    gsl_vector_free(ss);
    gsl_multimin_fminimizer_free (s);
    return results;
}


#pragma once
#include "rust/cxx.h"
#include <memory>
#include <cstdint>
#include <NTL/GF2X.h>
#include <NTL/GF2E.h>

namespace poly
{
    struct PolyI64Pair;
    class Poly
    {
    public:
        // internal representation of polynomial
        NTL::GF2X int_pol;
        Poly();
        // copy
        Poly(const Poly &b);
        // move
        Poly(Poly &&b);
        void add_to(const Poly &b);
        void mul_to(const Poly &b);
        void div_to(const Poly &b);
        void gcd_to(const Poly &b);
        void rem_to(const Poly &b);
        void sqr();
        bool div_to_checked(const Poly &b);
        bool coeff(int64_t idx) const;
        bool eq(const Poly &b) const;
        bool is_zero() const;
        std::unique_ptr<std::vector<uint8_t>> to_bytes(int64_t min_bytes) const;
    };

    int64_t deg(const Poly &a);
    std::unique_ptr<Poly> new_poly_shifted(rust::Slice<uint8_t> bytes, int64_t shift, bool msb_first);
    std::unique_ptr<Poly> new_poly(rust::Slice<uint8_t> bytes);
    std::unique_ptr<Poly> new_zero();
    std::unique_ptr<Poly> copy_poly(const Poly &p);
    std::unique_ptr<Poly> add(const Poly &b, const Poly &c);
    std::unique_ptr<Poly> mul(const Poly &b, const Poly &c);
    std::unique_ptr<Poly> div(const Poly &b, const Poly &c);
    std::unique_ptr<Poly> gcd(const Poly &b, const Poly &c);
    std::unique_ptr<Poly> xgcd(Poly &x, Poly &y, const Poly &b, const Poly &c);
    std::unique_ptr<Poly> rem(const Poly &b, const Poly &c);
    std::unique_ptr<Poly> power(const Poly &p, int64_t n);
    std::unique_ptr<Poly> shift(const Poly &p, int64_t n);
    std::unique_ptr<std::vector<PolyI64Pair>> factor(const Poly &p, int64_t verbosity);

    class PolyRem
    {
    public:
        NTL::GF2E int_pol;
        // from modulus
        PolyRem(const Poly &p);
        // copy
        PolyRem(const PolyRem &b);
        // move
        PolyRem(PolyRem &&b);
        PolyRem(const NTL::GF2E &p);
        void add_to(const PolyRem &b);
        void mul_to(const PolyRem &b);
        void div_to(const PolyRem &b);
        void sqr();
        std::unique_ptr<Poly> rep() const;
    };
    std::unique_ptr<PolyRem> new_polyrem(const Poly &rem, const Poly &m);
    std::unique_ptr<PolyRem> powermod(const PolyRem &p, int64_t n);
    std::unique_ptr<PolyRem> copy_polyrem(const PolyRem &p);
} // namespace poly


#ifndef QP_SOLVER_H
#define QP_SOLVER_H

#include <Eigen/Dense>
#ifdef QP_SOLVER_USE_SPARSE
#include <Eigen/Sparse>
#endif
#include <cmath>
#include <limits>

#ifndef SOLVER_ASSERT
#define SOLVER_ASSERT(x) eigen_assert(x)
#endif

namespace qp_solver {

#ifdef QP_SOLVER_USE_SPARSE
template <int _n, int _m, typename _Scalar = double>
struct QP {
    using Scalar = _Scalar;
    enum {
        n=_n,
        m=_m
    };
    Eigen::SparseMatrix<Scalar> P;
    Eigen::Matrix<int, n, 1> P_col_nnz;
    Eigen::Matrix<Scalar, n, 1> q;
    Eigen::SparseMatrix<Scalar> A;
    Eigen::Matrix<int, n, 1> A_col_nnz;
    Eigen::Matrix<Scalar, m, 1> l, u;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
#else
template <int _n, int _m, typename _Scalar = double>
struct QP {
    using Scalar = _Scalar;
    enum {
        n=_n,
        m=_m
    };
    Eigen::Matrix<Scalar, n, n> P;
    Eigen::Matrix<Scalar, n, 1> q;
    Eigen::Matrix<Scalar, m, n> A;
    Eigen::Matrix<Scalar, m, 1> l, u;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
#endif

template <typename Scalar>
struct qp_sover_settings_t {
    Scalar rho = 1e-1;          /**< ADMM rho step, 0 < rho */
    Scalar sigma = 1e-6;        /**< ADMM sigma step, 0 < sigma, (small) */
    Scalar alpha = 1.0;         /**< ADMM overrelaxation parameter, 0 < alpha < 2,
                                     values in [1.5, 1.8] give good results (empirically) */
    Scalar eps_rel = 1e-3;      /**< Relative tolerance for termination, 0 < eps_rel */
    Scalar eps_abs = 1e-3;      /**< Absolute tolerance for termination, 0 < eps_abs */
    int max_iter = 1000;        /**< Maximal number of iteration, 0 < max_iter */
    int check_termination = 25; /**< Check termination after every Nth iteration, 0 (disabled) or 0 < check_termination */
    bool warm_start = false;    /**< Warm start solver, reuses previous x,z,y */
    bool adaptive_rho = false;  /**< Adapt rho to optimal estimate */
    Scalar adaptive_rho_tolerance = 5;  /**< Minimal for rho update factor, 1 < adaptive_rho_tolerance */
    int adaptive_rho_interval = 25; /**< change rho every Nth iteration, 0 < adaptive_rho_interval,
                                         set equal to check_termination to save computation  */
    bool verbose = false;

#ifdef QP_SOLVER_PRINTING
    void print() const
    {
        printf("ADMM settings:\n");
        printf("  sigma %.2e\n", sigma);
        printf("  rho %.2e\n", rho);
        printf("  alpha %.2f\n", alpha);
        printf("  eps_rel %.1e\n", eps_rel);
        printf("  eps_abs %.1e\n", eps_abs);
        printf("  max_iter %d\n", max_iter);
        printf("  adaptive_rho %d\n", adaptive_rho);
        printf("  warm_start %d\n", warm_start);
    }
#endif
};

typedef enum {
    SOLVED,
    MAX_ITER_EXCEEDED,
    UNSOLVED,
    UNINITIALIZED
} status_t;

template <typename Scalar>
struct qp_solver_info_t {
    status_t status = UNINITIALIZED; /**< Solver status */
    int iter = 0;               /**< Number of iterations */
    int rho_updates = 0;        /**< Number of rho updates (factorizations) */
    Scalar rho_estimate = 0;    /**< Last rho estimate */
    Scalar res_prim = 0;        /**< Primal residual */
    Scalar res_dual = 0;        /**< Dual residual */

#ifdef QP_SOLVER_PRINTING
    void print() const
    {
        printf("ADMM info:\n");
        printf("  status ");
        switch (status) {
        case SOLVED:
            printf("SOLVED\n");
            break;
        case MAX_ITER_EXCEEDED:
            printf("MAX_ITER_EXCEEDED\n");
            break;
        case UNSOLVED:
            printf("UNSOLVED\n");
            break;
        default:
            printf("UNINITIALIZED\n");
        };
        printf("  iter %d\n", iter);
        printf("  rho_updates %d\n", rho_updates);
        printf("  rho_estimate %f\n", rho_estimate);
        printf("  res_prim %f\n", res_prim);
        printf("  res_dual %f\n", res_dual);
    }
#endif
};

/**
 *  minimize        0.5 x' P x + q' x
 *  subject to      l <= A x <= u
 *
 *  with:
 *    x element of R^n
 *    Ax element of R^m
 */
template <typename QPType,
#ifdef QP_SOLVER_USE_SPARSE
          template <typename, int, typename... Args> class LinearSolver = Eigen::SimplicialLDLT,
#else
          template <typename, int, typename... Args> class LinearSolver = Eigen::LDLT,
#endif
          int LinearSolver_UpLo = Eigen::Lower>
class QPSolver {
public:
    enum {
        n=QPType::n,
        m=QPType::m
    };

    using qp_t = QPType;
    using Scalar = typename QPType::Scalar;
    using var_t = Eigen::Matrix<Scalar, n, 1>;
    using constraint_t = Eigen::Matrix<Scalar, m, 1>;
    using dual_t = Eigen::Matrix<Scalar, m, 1>;
    using kkt_vec_t = Eigen::Matrix<Scalar, n + m, 1>;
#ifdef QP_SOLVER_USE_SPARSE
    using SpMat = Eigen::SparseMatrix<Scalar, Eigen::ColMajor>;
    using kkt_mat_t = SpMat;
#else
    using kkt_mat_t = Eigen::Matrix<Scalar, n + m, n + m>;
#endif
    using settings_t = qp_sover_settings_t<Scalar>;
    using info_t = qp_solver_info_t<Scalar>;
    using linear_solver_t = LinearSolver<kkt_mat_t, LinearSolver_UpLo>;

    static constexpr Scalar RHO_MIN = 1e-6;
    static constexpr Scalar RHO_MAX = 1e+6;
    static constexpr Scalar RHO_TOL = 1e-4;
    static constexpr Scalar RHO_EQ_FACTOR = 1e+3;
    static constexpr Scalar LOOSE_BOUNDS_THRESH = 1e+16;
    static constexpr Scalar DIV_BY_ZERO_REGUL = std::numeric_limits<Scalar>::epsilon();

    // Solver state variables
    int iter;
    var_t x;
    constraint_t z;
    dual_t y;
    var_t x_tilde;
    constraint_t z_tilde;
    constraint_t z_prev;
    dual_t rho_vec;
    dual_t rho_inv_vec;
    Scalar rho;

    // State
    Scalar res_prim;
    Scalar res_dual;
    Scalar _max_Ax_z_norm;
    Scalar _max_Px_ATy_q_norm;

    enum {
        INEQUALITY_CONSTRAINT,
        EQUALITY_CONSTRAINT,
        LOOSE_BOUNDS
    } constr_type[m]; /**< constraint type classification */

    settings_t _settings;
    info_t _info;

    kkt_mat_t kkt_mat;
    linear_solver_t linear_solver;

    // enforce 16 byte alignment https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    QPSolver() { }

    void setup(const qp_t &qp)
    {
        x.setZero();
        z.setZero();
        y.setZero();

        // Set QP constraint type
        constr_type_init(qp);

        // initialize step size (rho) vector
        rho_vec_update(_settings.rho);

        // construct KKT system and compute decomposition
        construct_KKT_mat(qp);
        compute_KKT();

        _info.status = UNSOLVED;
    }

    void update_qp(const qp_t &qp)
    {
        // Set QP constraint type
        constr_type_init(qp);

        // initialize step size (rho) vector
        rho_vec_update(_settings.rho);

        // update KKT system and do factorization
        update_KKT_mat(qp);
        factorize_KKT();

        _info.status = UNSOLVED;
    }

    void solve(const qp_t &qp)
    {
        kkt_vec_t rhs, x_tilde_nu;
        bool check_termination = false;

        if (_info.status == UNINITIALIZED) {
            SOLVER_ASSERT(_info.status == UNINITIALIZED);
            return;
        }

#ifdef QP_SOLVER_PRINTING
        if (_settings.verbose) {
            _settings.print();
        }
#endif
        if (!_settings.warm_start) {
            x.setZero();
            z.setZero();
            y.setZero();
        }

        for (iter = 1; iter <= _settings.max_iter; iter++) {
            z_prev = z;

            // update x_tilde z_tilde
            form_KKT_rhs(qp, rhs);
            x_tilde_nu = linear_solver.solve(rhs);

            x_tilde = x_tilde_nu.template head<n>();
            z_tilde = z_prev + rho_inv_vec.cwiseProduct(x_tilde_nu.template tail<m>() - y);

            // update x
            x = _settings.alpha * x_tilde + (1 - _settings.alpha) * x;

            // update z
            z = _settings.alpha * z_tilde + (1 - _settings.alpha) * z_prev + rho_inv_vec.cwiseProduct(y);
            box_projection(z, qp.l, qp.u); // euclidean projection

            // update y
            y = y + rho_vec.cwiseProduct(_settings.alpha * z_tilde + (1 - _settings.alpha) * z_prev - z);

            if (_settings.check_termination != 0 && iter % _settings.check_termination == 0) {
                check_termination = true;
            } else {
                check_termination = false;
            }

            if (check_termination) {
                update_state(qp);

#ifdef QP_SOLVER_PRINTING
                if (_settings.verbose) {
                    print_status(qp);
                }
#endif
                if (termination_criteria(qp)) {
                    _info.status = SOLVED;
                    break;
                }
            }

            if (_settings.adaptive_rho && iter % _settings.adaptive_rho_interval == 0) {
                if (!check_termination) {
                    // state was not yet updated
                    update_state(qp);
                }
                Scalar new_rho = rho_estimate(rho, qp);
                new_rho = fmax(RHO_MIN, fmin(new_rho, RHO_MAX));
                _info.rho_estimate = new_rho;

                if (new_rho < rho / _settings.adaptive_rho_tolerance ||
                    new_rho > rho * _settings.adaptive_rho_tolerance) {
                    rho_vec_update(new_rho);
                    update_KKT_rho();
                    /* Note: KKT Sparsity pattern unchanged by rho update. Only factorize. */
                    factorize_KKT();
                }
            }
        }

        if (iter > _settings.max_iter) {
            _info.status = MAX_ITER_EXCEEDED;
        }
        _info.iter = iter;

#ifdef QP_SOLVER_PRINTING
        if (_settings.verbose) {
            _info.print();
        }
#endif
    }

    inline const var_t& primal_solution() const { return x; }
    inline var_t& primal_solution() { return x; }

    inline const dual_t& dual_solution() const { return y; }
    inline dual_t& dual_solution() { return y; }

    inline const settings_t& settings() const { return _settings; }
    inline settings_t& settings() { return _settings; }

    inline const info_t& info() const { return _info; }
    inline info_t& info() { return _info; }

private:
    /* Construct the KKT matrix of the form
     *
     * [[ P + sigma*I,        A' ],
     *  [ A,           -1/rho.*I ]]
     *
     * If LinearSolver_UpLo parameter is Eigen::Lower, then only the lower
     * triangular part is constructed to optimize memory.
     *
     * Note: For Eigen::ConjugateGradient it is advised to set Upper|Lower for
     *       best performance.
     */
    void construct_KKT_mat(const qp_t &qp)
    {
        static_assert(LinearSolver_UpLo == Eigen::Lower ||
                      LinearSolver_UpLo == (Eigen::Upper|Eigen::Lower),
                      "LinearSolver_UpLo must be Lower or Upper|Lower");

#ifdef QP_SOLVER_USE_SPARSE
        kkt_mat.resize(n+m,n+m); // sets all elements to 0
        Eigen::Matrix<int, n+m, 1> nnz_col;

        nnz_col.setConstant(1);
        nnz_col.template head<n>() = qp.P_col_nnz + qp.A_col_nnz;
        if (LinearSolver_UpLo == (Eigen::Upper|Eigen::Lower)) {
            // We don't know nnz of A rows, assume worst case
            nnz_col.template tail<m>().array() += n;
        }
        kkt_mat.reserve(nnz_col);

        // top left:  P + sigma*I
        sparse_insert_at(kkt_mat, 0, 0, qp.P);
        for (int i = 0; i < n; i++) {
            kkt_mat.coeffRef(i,i) += _settings.sigma;
        }

        // bottom left:  A
        sparse_insert_at(kkt_mat, n, 0, qp.A);

        // top right:  A'
        if (LinearSolver_UpLo == (Eigen::Upper|Eigen::Lower)) {
            sparse_insert_at(kkt_mat, 0, n, qp.A.transpose());
        }

        // bottom right:  -1/rho.*I
        for (int i = 0; i < m; i++) {
            kkt_mat.insert(n+i, n+i) = -rho_inv_vec(i);
        }

        kkt_mat.makeCompressed();
#else
        kkt_mat.template topLeftCorner<n, n>() = qp.P + _settings.sigma * qp.P.Identity();
        if (LinearSolver_UpLo == (Eigen::Upper|Eigen::Lower)) {
            kkt_mat.template topRightCorner<n, m>() = qp.A.transpose();
        }
        kkt_mat.template bottomLeftCorner<m, n>() = qp.A;
        kkt_mat.template bottomRightCorner<m, m>() = -1.0 * rho_inv_vec.asDiagonal();
#endif
    }

    /** KKT matrix value update, assumes same sparsity pattern */
    void update_KKT_mat(const qp_t &qp)
    {
#ifdef QP_SOLVER_USE_SPARSE
        // construct_KKT_mat(qp);
        // TODO: more efficient update?
        for (int k = 0; k < kkt_mat.outerSize(); ++k) {
            for (typename SpMat::InnerIterator it(kkt_mat, k); it; ++it) {
                int row, col;
                row = it.row();
                col = it.col();
                if (row < n) {
                    if (col < n) {
                        // top left:  P + sigma*I
                        it.valueRef() = qp.P.coeff(row, col) + _settings.sigma;
                    } else {
                        // top right:  A'
                        it.valueRef() = qp.A.coeff(col, row-n);
                    }
                } else {
                    if (col < n) {
                        // bottom left:  A
                        it.valueRef() = qp.A.coeff(row-n, col);
                    } else {
                        // bottom right:  -1/rho.*I
                        it.valueRef() = -rho_inv_vec(row-n);
                    }
                }
            }
        }
#else
        construct_KKT_mat(qp);
#endif
    }

    void update_KKT_rho()
    {
#ifdef QP_SOLVER_USE_SPARSE
        // Note: optimize by writing kkt_mat.valuePtr(). needs to be compressed and Eigen::Lower.
        for (int i = 0; i < m; i++) {
            kkt_mat.coeffRef(n+i, n+i) = -rho_inv_vec(i);
        }
#else
        kkt_mat.template bottomRightCorner<m, m>() = -1.0 * rho_inv_vec.asDiagonal();
#endif
    }

    void factorize_KKT()
    {
#ifdef QP_SOLVER_USE_SPARSE
        linear_solver.factorize(kkt_mat);
#else
        linear_solver.compute(kkt_mat);
#endif
        SOLVER_ASSERT(linear_solver.info() == Eigen::Success);
    }

    void compute_KKT()
    {
        linear_solver.compute(kkt_mat);
        SOLVER_ASSERT(linear_solver.info() == Eigen::Success);
    }

#ifdef QP_SOLVER_USE_SPARSE
    void sparse_insert_at(SpMat &dst, int row, int col, const SpMat &src) const
    {
        for (int k = 0; k < src.outerSize(); ++k) {
            for (typename SpMat::InnerIterator it(src, k); it; ++it) {
                dst.insert(row + it.row(), col + it.col()) = it.value();
            }
        }
    }
#endif

    void form_KKT_rhs(const qp_t &qp, kkt_vec_t& rhs)
    {
        rhs.template head<n>() = _settings.sigma * x - qp.q;
        rhs.template tail<m>() = z - rho_inv_vec.cwiseProduct(y);
    }

    void box_projection(constraint_t& z, const constraint_t& l, const constraint_t& u)
    {
        z = z.cwiseMax(l).cwiseMin(u);
    }

    void constr_type_init(const qp_t &qp)
    {
        for (int i = 0; i < qp.l.RowsAtCompileTime; i++) {
            if (qp.l[i] < -LOOSE_BOUNDS_THRESH && qp.u[i] > LOOSE_BOUNDS_THRESH) {
                constr_type[i] = LOOSE_BOUNDS;
            } else if (qp.u[i] - qp.l[i] < RHO_TOL) {
                constr_type[i] = EQUALITY_CONSTRAINT;
            } else {
                constr_type[i] = INEQUALITY_CONSTRAINT;
            }
        }
    }

    void rho_vec_update(Scalar rho0)
    {
        for (int i = 0; i < rho_vec.RowsAtCompileTime; i++) {
            switch (constr_type[i]) {
            case LOOSE_BOUNDS:
                rho_vec[i] = RHO_MIN;
                break;
            case EQUALITY_CONSTRAINT:
                rho_vec[i] = RHO_EQ_FACTOR*rho0;
                break;
            case INEQUALITY_CONSTRAINT: /* fall through */
            default:
                rho_vec[i] = rho0;
            };
        }
        rho_inv_vec = rho_vec.cwiseInverse();
        rho = rho0;
        _info.rho_updates += 1;
    }

    void update_state(const qp_t& qp)
    {
        Scalar norm_Ax, norm_z;
        norm_Ax = (qp.A*x).template lpNorm<Eigen::Infinity>();
        norm_z = z.template lpNorm<Eigen::Infinity>();
        _max_Ax_z_norm = fmax(norm_Ax, norm_z);

        Scalar norm_Px, norm_ATy, norm_q;
        norm_Px = (qp.P*x).template lpNorm<Eigen::Infinity>();
        norm_ATy = (qp.A.transpose()*y).template lpNorm<Eigen::Infinity>();
        norm_q = qp.q.template lpNorm<Eigen::Infinity>();
        _max_Px_ATy_q_norm = fmax(norm_Px, fmax(norm_ATy, norm_q));

        _info.res_prim = residual_prim(qp);
        _info.res_dual = residual_dual(qp);
    }

    Scalar rho_estimate(const Scalar rho0, const qp_t &qp) const
    {
        Scalar rp_norm, rd_norm;
        rp_norm = _info.res_prim / (_max_Ax_z_norm + DIV_BY_ZERO_REGUL);
        rd_norm = _info.res_dual / (_max_Px_ATy_q_norm + DIV_BY_ZERO_REGUL);

        Scalar rho_new = rho0 * sqrt(rp_norm/(rd_norm + DIV_BY_ZERO_REGUL));
        return rho_new;
    }

    Scalar eps_prim(const qp_t &qp) const
    {
        return _settings.eps_abs + _settings.eps_rel * _max_Ax_z_norm;
    }

    Scalar eps_dual(const qp_t &qp) const
    {
        return _settings.eps_abs + _settings.eps_rel * _max_Px_ATy_q_norm ;
    }

    Scalar residual_prim(const qp_t &qp) const
    {
        return (qp.A*x - z).template lpNorm<Eigen::Infinity>();
    }

    Scalar residual_dual(const qp_t &qp) const
    {
        return (qp.P*x + qp.q + qp.A.transpose()*y).template lpNorm<Eigen::Infinity>();
    }

    bool termination_criteria(const qp_t &qp)
    {
        // check residual norms to detect optimality
        if (_info.res_prim <= eps_prim(qp) && _info.res_dual <= eps_dual(qp)) {
            return true;
        }

        return false;
    }

#ifdef QP_SOLVER_PRINTING
    void print_status(const qp_t &qp) const
    {
        Scalar obj = 0.5 * x.dot(qp.P*x) + qp.q.dot(x);

        if (iter == _settings.check_termination) {
            printf("iter   obj       rp        rd\n");
        }
        printf("%4d  %.2e  %.2e  %.2e\n", iter, obj, _info.res_prim, _info.res_dual);
    }
#endif
};

} // namespace qp_solver

#endif // QP_SOLVER_H