#include <k52/optimization/hooke_jeeves_method.h>

#ifdef BUILD_WITH_MPI

#include <boost/mpi.hpp>
#include <k52/parallel/mpi/constants.h>

#endif

#include <iostream>
#include <cmath>
#include <algorithm>
#include <stdexcept>

#include <k52/common/floating_point.h>
#include <k52/optimization/params/i_continuous_parameters.h>

using ::std::vector;

namespace k52
{
namespace optimization
{

HookeJeevesMethod::HookeJeevesMethod(
    double acceleration,
    double init_step,
    size_t max_iteration_number,
    double precision,
    double step_divider)
    : acceleration_(acceleration)
    , init_step_(init_step)
    , max_iteration_number_(max_iteration_number)
    , precision_(precision)
    , step_divider_(step_divider)
{
}

HookeJeevesMethod* HookeJeevesMethod::Clone() const
{
    return new HookeJeevesMethod(
        acceleration_,
        init_step_,
        max_iteration_number_,
        precision_,
        step_divider_);
}

std::string HookeJeevesMethod::get_name() const
{
    return "Hooke-Jeeves Method";
}

#ifdef BUILD_WITH_MPI
void HookeJeevesMethod::Send(boost::mpi::communicator* communicator, int target) const
{
    communicator->send(target, k52::parallel::mpi::constants::kCommonTag, acceleration_);
    communicator->send(target, k52::parallel::mpi::constants::kCommonTag, precision_);
    communicator->send(target, k52::parallel::mpi::constants::kCommonTag, max_iteration_number_);
    communicator->send(target, k52::parallel::mpi::constants::kCommonTag, init_step_);
    communicator->send(target, k52::parallel::mpi::constants::kCommonTag, step_divider_);
}

void HookeJeevesMethod::Receive(boost::mpi::communicator* communicator, int source)
{
    communicator->recv(source, k52::parallel::mpi::constants::kCommonTag, acceleration_);
    communicator->recv(source, k52::parallel::mpi::constants::kCommonTag, precision_);
    communicator->recv(source, k52::parallel::mpi::constants::kCommonTag, max_iteration_number_);
    communicator->recv(source, k52::parallel::mpi::constants::kCommonTag, init_step_);
    communicator->recv(source, k52::parallel::mpi::constants::kCommonTag, step_divider_);
}
#endif

vector<double> HookeJeevesMethod::FindOptimalParameters(const vector<double>& initial_parameters)
{
    vector<double> arguments(initial_parameters);
    vector<double> arguments_after_search(initial_parameters.size());
    vector<double> steps_array = vector<double>(arguments.size(), init_step_);
    size_t iteration = 0;

    do
    {
        arguments_after_search = CoordinatewiseSearch(arguments, steps_array);
        if (common::FloatingPoint::AreEqual(arguments, arguments_after_search))
        {
            for (size_t i = 0; i < steps_array.size(); ++i)
            {
                steps_array[i] /= step_divider_;
            }
            continue;
        }

        vector<double> next_step_arguments(arguments.size());
        vector<double> next_step_arguments_after_search(arguments.size());
        while (true)
        {
            for (size_t i = 0; i<arguments.size(); ++i)
            {
                next_step_arguments[i] = arguments[i] + acceleration_
                        * (arguments_after_search[i] - arguments[i]);
            }
            next_step_arguments_after_search = CoordinatewiseSearch(next_step_arguments, steps_array);

            arguments = arguments_after_search;
            if (CountObjectiveFunctionValueToMinimize(arguments_after_search) <=
                    CountObjectiveFunctionValueToMinimize(next_step_arguments_after_search))
            {
                break;
            }
            arguments_after_search = next_step_arguments_after_search;

            iteration++;
            // Check number of iterations
            if (iteration == max_iteration_number_)
            {
                std::cout << "Solution not found" << std::endl;
                return arguments_after_search;
            }
        }
    } while (!IsExitCriteriaFulfilled(steps_array));
    return arguments_after_search;
}

bool HookeJeevesMethod::IsExitCriteriaFulfilled(
    const vector<double>& steps_array) const
{
    double temp = 0;
    // Check if step is small enough
    for (size_t j = 0; j<steps_array.size(); ++j)
    {
        temp += steps_array[j] * steps_array[j];
    }
    if (std::sqrt(temp) <= precision_)
    {
        return true;
    }
    return false;
}

vector<double> HookeJeevesMethod::CoordinatewiseSearch(
    const vector<double>& arguments,
    const vector<double>& steps_array) const
{
    vector<double> new_arguments(arguments);
    for (size_t i = 0; i < arguments.size(); ++i)
    {
        double new_function_value = CountObjectiveFunctionValueToMinimize(new_arguments);
        vector<double> temp(new_arguments);

        temp[i] = new_arguments[i] + steps_array[i];
        if (CountObjectiveFunctionValueToMinimize(temp) < new_function_value)
        {
            new_arguments = temp;
            continue;
        }

        temp[i] = new_arguments[i] - steps_array[i];
        if (CountObjectiveFunctionValueToMinimize(temp) < new_function_value)
        {
            new_arguments = temp;
        }
    }
    return new_arguments;
}

}/* namespace optimization */
}/* namespace k52 */


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

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T.Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_DETAIL_SCALAR_F_TRIG_EVALUATION_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_DETAIL_SCALAR_F_TRIG_EVALUATION_HPP_INCLUDED

#include <boost/simd/function/horn.hpp>
#include <boost/simd/constant/mhalf.hpp>
#include <boost/simd/function/scalar/fma.hpp>
#include <boost/simd/function/scalar/oneplus.hpp>
#include <boost/simd/function/scalar/rec.hpp>
#include <boost/simd/function/scalar/sqr.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>

namespace boost { namespace simd
{
  namespace detail
  {
    namespace bd =  boost::dispatch;
    namespace bs =  boost::simd;

    template < class A0,
               class style,
               class base_A0 = bd::scalar_of_t<A0>
             >
    struct trig_evaluation {};

    // This class exposes the public static members:
    // sin_eval
    // cos_eval
    // tan_eval
    // which evaluate a polynomial approximation of each standard trigonometric
    // functions in the range [pi/4, -pi/4]

    template < class A0> struct trig_evaluation < A0,  tag::not_simd_type, float>
    {
      static BOOST_FORCEINLINE A0 cos_eval(A0 z) BOOST_NOEXCEPT
      {
        const A0 y = horn<A0
          , 0x3d2aaaa5
          , 0xbab60619
          , 0x37ccf5ce
          > (z);
        return oneplus( fma(z,Mhalf<A0>(), y* sqr(z)));
      }

      static BOOST_FORCEINLINE A0 sin_eval(A0 z, A0 x) BOOST_NOEXCEPT
      {
        const A0 y1 = horn<A0
          , 0xbe2aaaa2
          , 0x3c08839d
          , 0xb94ca1f9
          > (z);
        return fma(y1*z,x,x);
      }

      static BOOST_FORCEINLINE A0 base_tan_eval(A0 z) BOOST_NOEXCEPT
      {
        const A0 zz = sqr(z);
        A0 y = horn<A0,
          0x3eaaaa6f,
          0x3e0896dd,
          0x3d5ac5c9,
          0x3cc821b5,
          0x3b4c779c,
          0x3c19c53b
          >(zz)*zz*z+z;
        return y;
      }

      static BOOST_FORCEINLINE A0 tan_eval(A0 z, const int n) BOOST_NOEXCEPT
      {
        const A0 y = base_tan_eval(z);
        if (n == 1) return y;  else return -rec(y);
      }
      static BOOST_FORCEINLINE A0 cot_eval(A0 z, const int n) BOOST_NOEXCEPT
      {
        const A0 y = base_tan_eval(z);
        if (n == 1) return rec(y);  else return -y;
      }
    };
  }
} }
#endif


/* ----------------------------------------------------------------------
 *
 *                    *** Smooth Mach Dynamics ***
 *
 * This file is part of the USER-SMD package for LAMMPS.
 * Copyright (2014) Georg C. Ganzenmueller, georg.ganzenmueller@emi.fhg.de
 * Fraunhofer Ernst-Mach Institute for High-Speed Dynamics, EMI,
 * Eckerstrasse 4, D-79104 Freiburg i.Br, Germany.
 *
 * ----------------------------------------------------------------------- */

/* ----------------------------------------------------------------------
 LAMMPS - Large-scale Atomic/Molecular Massively Parallel Simulator
 http://lammps.sandia.gov, Sandia National Laboratories
 Steve Plimpton, sjplimp@sandia.gov

 Copyright (2003) Sandia Corporation.  Under the terms of Contract
 DE-AC04-94AL85000 with Sandia Corporation, the U.S. Government retains
 certain rights in this software.  This software is distributed under
 the GNU General Public License.

 See the README file in the top-level LAMMPS directory.
 ------------------------------------------------------------------------- */
#include <iostream>
#include "math_special.h"
#include <stdio.h>

#include <Eigen/Eigen>

using namespace LAMMPS_NS::MathSpecial;
using namespace std;
using namespace Eigen;

#define MIN(A,B) ((A) < (B) ? (A) : (B))
#define MAX(A,B) ((A) > (B) ? (A) : (B))

/* ----------------------------------------------------------------------
 linear EOS for use with linear elasticity
 input: initial pressure pInitial, isotropic part of the strain rate d, time-step dt
 output: final pressure pFinal, pressure rate p_rate
 ------------------------------------------------------------------------- */
void LinearEOS(double lambda, double pInitial, double d, double dt, double &pFinal, double &p_rate) {

	/*
	 * pressure rate
	 */
	p_rate = lambda * d;

	pFinal = pInitial + dt * p_rate; // increment pressure using pressure rate
	//cout << "hurz" << endl;

}

/* ----------------------------------------------------------------------
 shock EOS
 input:
 current density rho
 reference density rho0
 current energy density e
 reference energy density e0
 reference speed of sound c0
 shock Hugoniot parameter S
 Grueneisen parameter Gamma
 initial pressure pInitial
 time step dt

 output:
 pressure rate p_rate
 final pressure pFinal

 ------------------------------------------------------------------------- */
void ShockEOS(double rho, double rho0, double e, double e0, double c0, double S, double Gamma, double pInitial, double dt,
		double &pFinal, double &p_rate) {

	double mu = rho / rho0 - 1.0;
	double pH = rho0 * square(c0) * mu * (1.0 + mu) / square(1.0 - (S - 1.0) * mu);

	pFinal = (pH + rho * Gamma * (e - e0));

	//printf("shock EOS: rho = %g, rho0 = %g, Gamma=%f, c0=%f, S=%f, e=%f, e0=%f\n", rho, rho0, Gamma, c0, S, e, e0);
	//printf("pFinal = %f\n", pFinal);
	p_rate = (pFinal - pInitial) / dt;

}

/* ----------------------------------------------------------------------
 polynomial EOS
 input:
 current density rho
 reference density rho0
 coefficients 0 .. 6
 initial pressure pInitial
 time step dt

 output:
 pressure rate p_rate
 final pressure pFinal

 ------------------------------------------------------------------------- */
void polynomialEOS(double rho, double rho0, double e, double C0, double C1, double C2, double C3, double C4, double C5, double C6,
		double pInitial, double dt, double &pFinal, double &p_rate) {

	double mu = rho / rho0 - 1.0;

	if (mu > 0.0) {
		pFinal = C0 + C1 * mu + C2 * mu * mu + C3 * mu * mu * mu; // + (C4 + C5 * mu + C6 * mu * mu) * e;
	} else {
		pFinal = C0 + C1 * mu + C3 * mu * mu * mu; //  + (C4 + C5 * mu) * e;
	}
	pFinal = -pFinal; // we want the mean stress, not the pressure.

	//printf("pFinal = %f\n", pFinal);
	p_rate = (pFinal - pInitial) / dt;

}

/* ----------------------------------------------------------------------
 Tait EOS based on current density vs. reference density.

 input: (1) reference sound speed
 (2) equilibrium mass density
 (3) current mass density

 output:(1) pressure
 (2) current speed of sound
 ------------------------------------------------------------------------- */
void TaitEOS_density(const double exponent, const double c0_reference, const double rho_reference, const double rho_current,
		double &pressure, double &sound_speed) {

	double B = rho_reference * c0_reference * c0_reference / exponent;
	double tmp = pow(rho_current / rho_reference, exponent);
	pressure = B * (tmp - 1.0);
	double bulk_modulus = B * tmp * exponent; // computed as rho * d(pressure)/d(rho)
	sound_speed = sqrt(bulk_modulus / rho_current);

//	if (fabs(pressure) > 0.01) {
//		printf("tmp = %f, press=%f, K=%f\n", tmp, pressure, bulk_modulus);
//	}

}

/* ----------------------------------------------------------------------
 perfect gas EOS
 input: gamma -- adiabatic index (ratio of specific heats)
 J -- determinant of deformation gradient
 volume0 -- reference configuration volume of particle
 energy -- energy of particle
 pInitial -- initial pressure of the particle
 d -- isotropic part of the strain rate tensor,
 dt -- time-step size

 output: final pressure pFinal, pressure rate p_rate
 ------------------------------------------------------------------------- */
void PerfectGasEOS(const double gamma, const double vol, const double mass, const double energy, double &pFinal, double &c0) {

	/*
	 * perfect gas EOS is p = (gamma - 1) rho e
	 */

	if (energy > 0.0) {

		pFinal = (1.0 - gamma) * energy / vol;
//printf("gamma = %f, vol%f, e=%g ==> p=%g\n", gamma, vol, energy, *pFinal__/1.0e-9);

		c0 = sqrt((gamma - 1.0) * energy / mass);

	} else {
		pFinal = c0 = 0.0;
	}

}

/* ----------------------------------------------------------------------
 linear strength model for use with linear elasticity
 input: lambda, mu : Lame parameters
 input: sigmaInitial_dev, d_dev: initial stress deviator, deviatoric part of the strain rate tensor
 input: dt: time-step
 output:  sigmaFinal_dev, sigmaFinal_dev_rate__: final stress deviator and its rate.
 ------------------------------------------------------------------------- */
void LinearStrength(const double mu, const Matrix3d sigmaInitial_dev, const Matrix3d d_dev, const double dt,
		Matrix3d &sigmaFinal_dev__, Matrix3d &sigma_dev_rate__) {

	/*
	 * deviatoric rate of unrotated stress
	 */
	sigma_dev_rate__ = 2.0 * mu * d_dev;

	/*
	 * elastic update to the deviatoric stress
	 */
	sigmaFinal_dev__ = sigmaInitial_dev + dt * sigma_dev_rate__;
}

/* ----------------------------------------------------------------------
 linear strength model for use with linear elasticity
 input: lambda, mu : Lame parameters
 input: F: deformation gradient
 output:  total stress tensor, deviator + pressure
 ------------------------------------------------------------------------- */
//void PairTlsph::LinearStrengthDefgrad(double lambda, double mu, Matrix3d F, Matrix3d *T) {
//	Matrix3d E, PK2, eye, sigma, S, tau;
//
//	eye.setIdentity();
//
//	E = 0.5 * (F * F.transpose() - eye); // strain measure E = 0.5 * (B - I) = 0.5 * (F * F^T - I)
//	tau = lambda * E.trace() * eye + 2.0 * mu * E; // Kirchhoff stress, work conjugate to above strain
//	sigma = tau / F.determinant(); // convert Kirchhoff stress to Cauchy stress
//
////printf("l=%f, mu=%f, sigma xy = %f\n", lambda, mu, sigma(0,1));
//
////    E = 0.5 * (F.transpose() * F - eye); // Green-Lagrange Strain E = 0.5 * (C - I)
////    S = lambda * E.trace() * eye + 2.0 * mu * Deviator(E); // PK2 stress
////    tau = F * S * F.transpose(); // convert PK2 to Kirchhoff stress
////    sigma = tau / F.determinant();
//
//	//*T = sigma;
//
//	/*
//	 * neo-hookean model due to Bonet
//	 */
////    lambda = mu = 100.0;
////    // left Cauchy-Green Tensor, b = F.F^T
//	double J = F.determinant();
//	double logJ = log(J);
//	Matrix3d b;
//	b = F * F.transpose();
//
//	sigma = (mu / J) * (b - eye) + (lambda / J) * logJ * eye;
//	*T = sigma;
//}
/* ----------------------------------------------------------------------
 linear strength model for use with linear elasticity
 input: lambda, mu : Lame parameters
 input: sigmaInitial_dev, d_dev: initial stress deviator, deviatoric part of the strain rate tensor
 input: dt: time-step
 output:  sigmaFinal_dev, sigmaFinal_dev_rate__: final stress deviator and its rate.
 ------------------------------------------------------------------------- */
void LinearPlasticStrength(const double G, const double yieldStress, const Matrix3d sigmaInitial_dev, const Matrix3d d_dev,
		const double dt, Matrix3d &sigmaFinal_dev__, Matrix3d &sigma_dev_rate__, double &plastic_strain_increment) {

	Matrix3d sigmaTrial_dev, dev_rate;
	double J2;

	/*
	 * deviatoric rate of unrotated stress
	 */
	dev_rate = 2.0 * G * d_dev;

	/*
	 * perform a trial elastic update to the deviatoric stress
	 */
	sigmaTrial_dev = sigmaInitial_dev + dt * dev_rate; // increment stress deviator using deviatoric rate

	/*
	 * check yield condition
	 */
	J2 = sqrt(3. / 2.) * sigmaTrial_dev.norm();

	if (J2 < yieldStress) {
		/*
		 * no yielding has occured.
		 * final deviatoric stress is trial deviatoric stress
		 */
		sigma_dev_rate__ = dev_rate;
		sigmaFinal_dev__ = sigmaTrial_dev;
		plastic_strain_increment = 0.0;
		//printf("no yield\n");

	} else {
		//printf("yiedl\n");
		/*
		 * yielding has occured
		 */
		plastic_strain_increment = (J2 - yieldStress) / (3.0 * G);

		/*
		 * new deviatoric stress:
		 * obtain by scaling the trial stress deviator
		 */
		sigmaFinal_dev__ = (yieldStress / J2) * sigmaTrial_dev;

		/*
		 * new deviatoric stress rate
		 */
		sigma_dev_rate__ = sigmaFinal_dev__ - sigmaInitial_dev;
		//printf("yielding has occured.\n");
	}
}

/* ----------------------------------------------------------------------
 Johnson Cook Material Strength model
 input:
 G : shear modulus
 cp : heat capacity
 espec : energy / mass
 A : initial yield stress under quasi-static / room temperature conditions
 B : proportionality factor for plastic strain dependency
 a : exponent for plastic strain dpendency
 C : proportionality factor for logarithmic plastic strain rate dependency
 epdot0 : dimensionality factor for plastic strain rate dependency
 T : current temperature
 T0 : reference (room) temperature
 Tmelt : melting temperature
 input: sigmaInitial_dev, d_dev: initial stress deviator, deviatoric part of the strain rate tensor
 input: dt: time-step
 output:  sigmaFinal_dev, sigmaFinal_dev_rate__: final stress deviator and its rate.
 ------------------------------------------------------------------------- */
void JohnsonCookStrength(const double G, const double cp, const double espec, const double A, const double B, const double a,
		const double C, const double epdot0, const double T0, const double Tmelt, const double M, const double dt, const double ep,
		const double epdot, const Matrix3d sigmaInitial_dev, const Matrix3d d_dev, Matrix3d &sigmaFinal_dev__,
		Matrix3d &sigma_dev_rate__, double &plastic_strain_increment) {

	Matrix3d sigmaTrial_dev, dev_rate;
	double J2, yieldStress;

	double deltaT = espec / cp;
	double TH = deltaT / (Tmelt - T0);
	TH = MAX(TH, 0.0);
	double epdot_ratio = epdot / epdot0;
	epdot_ratio = MAX(epdot_ratio, 1.0);
	//printf("current temperature delta is %f, TH=%f\n", deltaT, TH);

	yieldStress = (A + B * pow(ep, a)) * (1.0 + C * log(epdot_ratio)); // * (1.0 - pow(TH, M));

	/*
	 * deviatoric rate of unrotated stress
	 */
	dev_rate = 2.0 * G * d_dev;

	/*
	 * perform a trial elastic update to the deviatoric stress
	 */
	sigmaTrial_dev = sigmaInitial_dev + dt * dev_rate; // increment stress deviator using deviatoric rate

	/*
	 * check yield condition
	 */
	J2 = sqrt(3. / 2.) * sigmaTrial_dev.norm();

	if (J2 < yieldStress) {
		/*
		 * no yielding has occured.
		 * final deviatoric stress is trial deviatoric stress
		 */
		sigma_dev_rate__ = dev_rate;
		sigmaFinal_dev__ = sigmaTrial_dev;
		plastic_strain_increment = 0.0;
		//printf("no yield\n");

	} else {
		//printf("yiedl\n");
		/*
		 * yielding has occured
		 */
		plastic_strain_increment = (J2 - yieldStress) / (3.0 * G);

		/*
		 * new deviatoric stress:
		 * obtain by scaling the trial stress deviator
		 */
		sigmaFinal_dev__ = (yieldStress / J2) * sigmaTrial_dev;

		/*
		 * new deviatoric stress rate
		 */
		sigma_dev_rate__ = sigmaFinal_dev__ - sigmaInitial_dev;
		//printf("yielding has occured.\n");
	}
}

/* ----------------------------------------------------------------------
 isotropic maximum strain damage model
 input:
 current strain
 maximum value of allowed principal strain

 output:
 return value is true if any eigenvalue of the current strain exceeds the allowed principal strain

 ------------------------------------------------------------------------- */

bool IsotropicMaxStrainDamage(const Matrix3d E, const double maxStrain) {

	/*
	 * compute Eigenvalues of strain matrix
	 */
	SelfAdjointEigenSolver < Matrix3d > es;
	es.compute(E); // compute eigenvalue and eigenvectors of strain

	double max_eigenvalue = es.eigenvalues().maxCoeff();

	if (max_eigenvalue > maxStrain) {
		return true;
	} else {
		return false;
	}
}

/* ----------------------------------------------------------------------
 isotropic maximum stress damage model
 input:
 current stress
 maximum value of allowed principal stress

 output:
 return value is true if any eigenvalue of the current stress exceeds the allowed principal stress

 ------------------------------------------------------------------------- */

bool IsotropicMaxStressDamage(const Matrix3d S, const double maxStress) {

	/*
	 * compute Eigenvalues of strain matrix
	 */
	SelfAdjointEigenSolver < Matrix3d > es;
	es.compute(S); // compute eigenvalue and eigenvectors of strain

	double max_eigenvalue = es.eigenvalues().maxCoeff();

	if (max_eigenvalue > maxStress) {
		return true;
	} else {
		return false;
	}
}

/* ----------------------------------------------------------------------
 Johnson-Cook failure model
 input:

 output:

 ------------------------------------------------------------------------- */

double JohnsonCookFailureStrain(const double p, const Matrix3d Sdev, const double d1, const double d2, const double d3,
		const double d4, const double epdot0, const double epdot) {

	double vm = sqrt(3. / 2.) * Sdev.norm(); // von-Mises equivalent stress
	if (vm < 0.0) {
		cout << "this is sdev " << endl << Sdev << endl;
		printf("vm=%f < 0.0, surely must be an error\n", vm);
		exit(1);
	}

	// determine stress triaxiality
	double triax = p / (vm + 0.01 * fabs(p)); // have softening in denominator to avoid divison by zero
	if (triax < 0.0) {
		triax = 0.0;
	} else if (triax > 3.0) {
		triax = 3.0;
	}

	// Johnson-Cook failure strain, dependence on stress triaxiality
	double jc_failure_strain = d1 + d2 * exp(d3 * triax);

	// include strain rate dependency if parameter d4 is defined and current plastic strain rate exceeds reference strain rate
	if (d4 > 0.0) { //
		if (epdot > epdot0) {
			double epdot_ratio = epdot / epdot0;
			jc_failure_strain *= (1.0 + d4 * log(epdot_ratio));
			//printf("epsdot=%f, epsdot0=%f, factor = %f\n", epdot, epdot0, (1.0 + d4 * log(epdot_ratio)));
			//exit(1);

		}
	}

	return jc_failure_strain;

}


/**************************************************************************
 *
 * (C) Copyright VMware, Inc 2010.
 * (C) Copyright John Maddock 2006.
 * Use, modification and distribution are subject to the
 * Boost Software License, Version 1.0. (See accompanying file
 * LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
 *
 **************************************************************************/

/*
 * This file allows to compute the minimax polynomial coefficients we use
 * for fast exp2/log2.
 *
 * How to use this source:
 *
 * - Download and build the NTL library from
 *   http://shoup.net/ntl/download.html , or install libntl-dev package if on
 *   Debian.
 *
 * - Download boost source code matching to your distro.
 *
 * - Goto libs/math/minimax and replace f.cpp with this file.
 *
 * - Build as
 *
 *   g++ -o minimax -I /path/to/ntl/include main.cpp f.cpp /path/to/ntl/src/ntl.a
 *
 * - Run as
 *
 *    ./minimax
 *
 * - For example, to compute exp2 5th order polynomial between [0, 1] do:
 *
 *    variant 0
 *    range 0 1
 *    order 5 0
 *    step 200
 *    info
 *
 *  and take the coefficients from the P = { ... } array.
 *
 * - To compute log2 4th order polynomial between [0, 1/9] do:
 *
 *    variant 1
 *    range 0 0.111111112
 *    order 4 0
 *    step 200
 *    info
 *
 * - For more info see
 * http://www.boost.org/doc/libs/1_47_0/libs/math/doc/sf_and_dist/html/math_toolkit/toolkit/internals2/minimax.html
 */

#define L22
#include <boost/math/bindings/rr.hpp>
#include <boost/math/tools/polynomial.hpp>

#include <cmath>

boost::math::ntl::RR exp2(const boost::math::ntl::RR& x)
{
      return exp(x*log(2.0));
}

boost::math::ntl::RR log2(const boost::math::ntl::RR& x)
{
      return log(x)/log(2.0);
}

boost::math::ntl::RR f(const boost::math::ntl::RR& x, int variant)
{
   switch(variant)
   {
   case 0:
      return exp2(x);

   case 1:
      return log2((1.0 + sqrt(x))/(1.0 - sqrt(x)))/sqrt(x);
   }

   return 0;
}

void show_extra(
   const boost::math::tools::polynomial<boost::math::ntl::RR>& n,
   const boost::math::tools::polynomial<boost::math::ntl::RR>& d,
   const boost::math::ntl::RR& x_offset,
   const boost::math::ntl::RR& y_offset,
   int variant)
{
   switch(variant)
   {
   default:
      // do nothing here...
      ;
   }
}


#include <Python.h>
#include <iostream>
#include <Eigen/Dense>

using namespace Eigen;

typedef struct
{
    PyObject_HEAD
    MatrixXd *matrix=nullptr;
} PyMatrixObject;

static PyObject *
PyMatrix_new(PyTypeObject *type, PyObject *args, PyObject *kwds)
{
    PyMatrixObject *self;
    self = (PyMatrixObject *)type->tp_alloc(type, 0);

    char *kwlist[] = {"width", "height", NULL};
    int width = 0;
    int height = 0;

    if (!PyArg_ParseTupleAndKeywords(args, kwds, "ii", kwlist,
                                     &width, &height))
    {
        Py_DECREF(self);
        return NULL;
    }
    if (width <= 0 or height <= 0)
    {
        PyErr_SetString(PyExc_ValueError, "The height and width must be greater than 0.");
        return NULL;
    }

    self->matrix = new MatrixXd(width, height);
    return (PyObject *)self;
}

static void
*PyMatrix_dealloc(PyObject *obj)
{
    delete ((PyMatrixObject *)obj)->matrix;
    Py_TYPE(obj)->tp_free(obj);
}

inline MatrixXd *ParseMatrix(PyObject *obj){
    return ((PyMatrixObject *)obj)->matrix;
}

inline PyObject *ReturnMatrix(MatrixXd *m, PyTypeObject *type){
    PyMatrixObject *obj = PyObject_NEW(PyMatrixObject, type);
    obj->matrix = m;
    return (PyObject *)obj;
}

static PyObject *
PyMatrix_add(PyObject *a, PyObject *b)
{
    MatrixXd *matrix_a = ParseMatrix(a);
    MatrixXd *matrix_b = ParseMatrix(b);

    if (matrix_a->cols() != matrix_b->cols() or matrix_a->rows() != matrix_b->rows()){
        PyErr_SetString(PyExc_ValueError, "The input matrix must be the same shape.");
        return NULL;
    }

    MatrixXd *matrix_c = new MatrixXd(matrix_a->cols(), matrix_b->rows());
    *matrix_c = *matrix_a + *matrix_b;

    return ReturnMatrix(matrix_c, a->ob_type);
}

static PyObject *
PyMatrix_minus(PyObject *a, PyObject *b)
{
    MatrixXd *matrix_a = ParseMatrix(a);
    MatrixXd *matrix_b = ParseMatrix(b);

    if (matrix_a->cols() != matrix_b->cols() or matrix_a->rows() != matrix_b->rows()){
        PyErr_SetString(PyExc_ValueError, "The input matrix must be the same shape.");
        return NULL;
    }

    MatrixXd *matrix_c = new MatrixXd(matrix_a->cols(), matrix_b->rows());
    *matrix_c = *matrix_a + *matrix_b;
    return ReturnMatrix(matrix_c, a->ob_type);
}

static PyObject *
PyMatrix_multiply(PyObject *a, PyObject *b)
{
    MatrixXd *matrix_a = ParseMatrix(a);
    MatrixXd *matrix_b = ParseMatrix(b);

    if (matrix_a->cols() != matrix_b->rows()){
        PyErr_SetString(PyExc_ValueError, "The colonm rank of matrix A must be the same as the row rank of matrix B.");
        return NULL;
    }
    MatrixXd *matrix_c = new MatrixXd(matrix_a->rows(), matrix_b->cols());
    *matrix_c = (*matrix_a) * (*matrix_b);
    return ReturnMatrix(matrix_c, a->ob_type);
}

static PyObject *PyMatrix_str(PyObject *a)
{
    MatrixXd *matrix = ParseMatrix(a);
    std::stringstream ss;
    ss << *matrix;
    return Py_BuildValue("s", ss.str().c_str());
}

static PyNumberMethods numberMethods = {
    PyMatrix_add,      //nb_add
    PyMatrix_minus,    //nb_subtract;
    PyMatrix_multiply, //nb_multiply
    nullptr,           //nb_remainder;
    nullptr,           //nb_divmod;
    nullptr,           // nb_power;
    nullptr,           // nb_negative;
    nullptr,           // nb_positive;
    nullptr,           // nb_absolute;
    nullptr,           // nb_bool;
    nullptr,           // nb_invert;
    nullptr,           // nb_lshift;
    nullptr,           // nb_rshift;
    nullptr,           // nb_and;
    nullptr,           // nb_xor;
    nullptr,           // nb_or;
    nullptr,           // nb_int;
    nullptr,           // nb_reserved;
    nullptr,           // nb_float;

    nullptr, // nb_inplace_add;
    nullptr, // nb_inplace_subtract;
    nullptr, // nb_inplace_multiply;
    nullptr, // nb_inplace_remainder;
    nullptr, // nb_inplace_power;
    nullptr, // nb_inplace_lshift;
    nullptr, // nb_inplace_rshift;
    nullptr, // nb_inplace_and;
    nullptr, // nb_inplace_xor;
    nullptr, // nb_inplace_or;

    nullptr, // nb_floor_divide;
    nullptr, // nb_true_divide;
    nullptr, // nb_inplace_floor_divide;
    nullptr, // nb_inplace_true_divide;

    nullptr, // nb_index;

    nullptr, //nb_matrix_multiply;
    nullptr  //nb_inplace_matrix_multiply;

};

PyObject *PyMatrix_data(PyObject *self, void *closure)
{

    PyMatrixObject *obj = (PyMatrixObject *)self;
    Py_ssize_t width = obj->matrix->cols();
    Py_ssize_t height = obj->matrix->rows();

    PyObject *list = PyList_New(height);
    for (int i = 0; i < height; i++)
    {
        PyObject *internal = PyList_New(width);

        for (int j = 0; j < width; j++)
        {
            PyObject *value = PyFloat_FromDouble((*obj->matrix)(i, j));
            PyList_SetItem(internal, j, value);
        }

        PyList_SetItem(list, i, internal);
    }
    return list;
}

PyObject *PyMatrix_rows(PyObject *self, void *closure)
{
    PyMatrixObject *obj = (PyMatrixObject *)self;
    return Py_BuildValue("i", obj->matrix->rows());
}

PyObject *PyMatrix_cols(PyObject *self, void *closure)
{
    PyMatrixObject *obj = (PyMatrixObject *)self;
    return Py_BuildValue("i", obj->matrix->cols());
}

static PyGetSetDef MatrixGetSet[] = {
    {"data", (getter)PyMatrix_data, nullptr, nullptr},
    {"row", (getter)PyMatrix_rows, nullptr, nullptr},
    {"colunm", (getter)PyMatrix_cols, nullptr, nullptr},
    {nullptr}};

PyObject *PyMatrix_tolist(PyObject *self, PyObject *args)
{
    return PyMatrix_data(self, nullptr);
}

static PyMethodDef MatrixMethods[] = {
    {"to_list", (PyCFunction)PyMatrix_tolist, METH_VARARGS, "Return the matrix data to a list object."},
    {nullptr}};

static PyTypeObject MatrixType = {
    PyVarObject_HEAD_INIT(nullptr, 0) "matrix.Matrix", /* tp_name */
    sizeof(PyMatrixObject),                            /* tp_basicsize */
    0,                                                 /* tp_itemsize */
    (destructor)PyMatrix_dealloc,                      /* tp_dealloc */
    nullptr,                                           /* tp_print */
    nullptr,                                           /* tp_getattr */
    nullptr,                                           /* tp_setattr */
    nullptr,                                           /* tp_reserved */
    nullptr,                                           /* tp_repr */
    &numberMethods,                                    /* tp_as_number */
    nullptr,                                           /* tp_as_sequence */
    nullptr,                                           /* tp_as_mapping */
    nullptr,                                           /* tp_hash  */
    nullptr,                                           /* tp_call */
    PyMatrix_str,                                      /* tp_str */
    nullptr,                                           /* tp_getattro */
    nullptr,                                           /* tp_setattro */
    nullptr,                                           /* tp_as_buffer */
    Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE,          /* tp_flags */
    "Coustom matrix class.",                           /* tp_doc */
    nullptr,                                           /* tp_traverse */
    nullptr,                                           /* tp_clear */
    nullptr,                                           /* tp_richcompare */
    0,                                                 /* tp_weaklistoffset */
    nullptr,                                           /* tp_iter */
    nullptr,                                           /* tp_iternext */
    MatrixMethods,                                     /* tp_methods */
    nullptr,                                           /* tp_members */
    MatrixGetSet,                                      /* tp_getset */
    nullptr,                                           /* tp_base */
    nullptr,                                           /* tp_dict */
    nullptr,                                           /* tp_descr_get */
    nullptr,                                           /* tp_descr_set */
    0,                                                 /* tp_dictoffset */
    nullptr,                                           /* tp_init */
    nullptr,                                           /* tp_alloc */
    PyMatrix_new                                       /* tp_new */
};

static PyObject *PyMatrix_ones(PyObject *self, PyObject *args, PyObject *kwargs)
{
    PyMatrixObject *m = (PyMatrixObject *)PyMatrix_new(&MatrixType, args, kwargs);
    m->matrix->setOnes();
    return (PyObject *)m;
}

static PyObject *PyMatrix_zeros(PyObject *self, PyObject *args, PyObject *kwargs)
{
    PyMatrixObject *m = (PyMatrixObject *)PyMatrix_new(&MatrixType, args, kwargs);
    m->matrix->setZero();
    return (PyObject *)m;
}

static PyObject *PyMatrix_random(PyObject *self, PyObject *args, PyObject *kwargs)
{
    PyMatrixObject *m = (PyMatrixObject *)PyMatrix_new(&MatrixType, args, kwargs);
    m->matrix->setRandom();
    return (PyObject *)m;
}

static PyObject *PyMatrix_matrix(PyObject *self, PyObject *args)
{
    PyObject *data = nullptr;
    if (!PyArg_ParseTuple(args, "O", &data))
    {
        PyErr_SetString(PyExc_ValueError, "Please pass a 2 dimensions list object. 1");
        return nullptr;
    }
    if (!PyList_Check(data))
    {
        PyErr_SetString(PyExc_ValueError, "Please pass a 2 dimensions list object. 2");
        return nullptr;
    }
    int height = PyList_GET_SIZE(data);
    if (height <= 0)
    {
        PyErr_SetString(PyExc_ValueError, "Please pass a 2 dimensions list object. 2");
        return nullptr;
    }
    PyObject *list = PyList_GET_ITEM(data, 0);
    if (!PyList_Check(list))
    {
        PyErr_SetString(PyExc_ValueError, "Please pass a 2 dimensions list object. 3");
        return nullptr;
    }
    int width = PyList_GET_SIZE(list);
    MatrixXd *p_mat = new MatrixXd(width, height);
    for (int i = 0; i < height; i++)
    {
        PyObject *list = PyList_GET_ITEM(data, i);
        if (!PyList_Check(list))
        {
            PyErr_SetString(PyExc_ValueError, "Please pass a 2 dimensions list object. 3");
            return nullptr;
        }
        int tmp = PyList_GET_SIZE(list);
        if (width != tmp)
        {
            PyErr_SetString(PyExc_ValueError, "Please pass a 2 dimensions list object. Each elements of it must be the same length.");
            return nullptr;
        }
        width = tmp;

        for (int j = 0; j < width; j++)
        {
            PyObject *num = PyList_GET_ITEM(list, j);
            if (!PyFloat_Check(num))
            {
                PyErr_SetString(PyExc_ValueError, "Every elements of the matrix must float.");
                return nullptr;
            }
            (*p_mat)(i, j) = ((PyFloatObject *)num)->ob_fval;
        }
    }

    return ReturnMatrix(p_mat, &MatrixType);
}

static PyMethodDef matrixMethods[] = {
    {"ones", (PyCFunction)PyMatrix_ones, METH_VARARGS | METH_KEYWORDS, "Return a new matrix with initial values one."},
    {"zeros", (PyCFunction)PyMatrix_zeros, METH_VARARGS | METH_KEYWORDS, "Return a new matrix with initial values zero."},
    {"random", (PyCFunction)PyMatrix_random, METH_VARARGS | METH_KEYWORDS, "Return a new matrix with random values"},
    {"matrix", (PyCFunction)PyMatrix_matrix, METH_VARARGS, "Return a new matrix with given values"},
    {nullptr}};

static struct PyModuleDef module = {
    PyModuleDef_HEAD_INIT,
    "matrix",
    "Python interface for Matrix calculation",
    -1,
    matrixMethods};

PyObject *initModule(void)
{
    PyObject *m;
    if (PyType_Ready(&MatrixType) < 0)
        return NULL;

    m = PyModule_Create(&module);
    if (m == NULL)
        return NULL;

    Py_INCREF(&MatrixType);
    if (PyModule_AddObject(m, "Matrix", (PyObject *)&MatrixType) < 0)
    {
        Py_DECREF(&MatrixType);
        Py_DECREF(m);
        return NULL;
    }

    return m;
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 *  @file  TimeOfArrivalExample.cpp
 *  @brief Track a moving object "Time of Arrival" measurements at 4
 * microphones.
 *  @author Frank Dellaert
 *  @author Jay Chakravarty
 *  @date March 2020
 */

#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/expressions.h>
#include <gtsam_unstable/geometry/Event.h>
#include <gtsam_unstable/slam/TOAFactor.h>

#include <boost/format.hpp>

#include <vector>

using namespace std;
using namespace gtsam;

// units
static const double ms = 1e-3;
static const double cm = 1e-2;

// Instantiate functor with speed of sound value
static const TimeOfArrival kTimeOfArrival(330);

/* ************************************************************************* */
// Create microphones
vector<Point3> defineMicrophones() {
  const double height = 0.5;
  vector<Point3> microphones;
  microphones.push_back(Point3(0, 0, height));
  microphones.push_back(Point3(403 * cm, 0, height));
  microphones.push_back(Point3(403 * cm, 403 * cm, height));
  microphones.push_back(Point3(0, 403 * cm, 2 * height));
  return microphones;
}

/* ************************************************************************* */
// Create ground truth trajectory
vector<Event> createTrajectory(size_t n) {
  vector<Event> trajectory;
  double timeOfEvent = 10;
  // simulate emitting a sound every second while moving on straight line
  for (size_t key = 0; key < n; key++) {
    trajectory.push_back(
        Event(timeOfEvent, 245 * cm + key * 1.0, 201.5 * cm, (212 - 45) * cm));
    timeOfEvent += 1;
  }
  return trajectory;
}

/* ************************************************************************* */
// Simulate time-of-arrival measurements for a single event
vector<double> simulateTOA(const vector<Point3>& microphones,
                           const Event& event) {
  size_t K = microphones.size();
  vector<double> simulatedTOA(K);
  for (size_t i = 0; i < K; i++) {
    simulatedTOA[i] = kTimeOfArrival(event, microphones[i]);
  }
  return simulatedTOA;
}

/* ************************************************************************* */
// Simulate time-of-arrival measurements for an entire trajectory
vector<vector<double>> simulateTOA(const vector<Point3>& microphones,
                                   const vector<Event>& trajectory) {
  vector<vector<double>> simulatedTOA;
  for (auto event : trajectory) {
    simulatedTOA.push_back(simulateTOA(microphones, event));
  }
  return simulatedTOA;
}

/* ************************************************************************* */
// create factor graph
NonlinearFactorGraph createGraph(const vector<Point3>& microphones,
                                 const vector<vector<double>>& simulatedTOA) {
  NonlinearFactorGraph graph;

  // Create a noise model for the TOA error
  auto model = noiseModel::Isotropic::Sigma(1, 0.5 * ms);

  size_t K = microphones.size();
  size_t key = 0;
  for (auto toa : simulatedTOA) {
    for (size_t i = 0; i < K; i++) {
      graph.emplace_shared<TOAFactor>(key, microphones[i], toa[i], model);
    }
    key += 1;
  }
  return graph;
}

/* ************************************************************************* */
// create initial estimate for n events
Values createInitialEstimate(size_t n) {
  Values initial;

  Event zero;
  for (size_t key = 0; key < n; key++) {
    initial.insert(key, zero);
  }
  return initial;
}

/* ************************************************************************* */
int main(int argc, char* argv[]) {
  // Create microphones
  auto microphones = defineMicrophones();
  size_t K = microphones.size();
  for (size_t i = 0; i < K; i++) {
    cout << "mic" << i << " = " << microphones[i] << endl;
  }

  // Create a ground truth trajectory
  const size_t n = 5;
  auto groundTruth = createTrajectory(n);

  // Simulate time-of-arrival measurements
  auto simulatedTOA = simulateTOA(microphones, groundTruth);
  for (size_t key = 0; key < n; key++) {
    for (size_t i = 0; i < K; i++) {
      cout << "z_" << key << i << " = " << simulatedTOA[key][i] / ms << " ms"
           << endl;
    }
  }

  // Create factor graph
  auto graph = createGraph(microphones, simulatedTOA);

  // Create initial estimate
  auto initialEstimate = createInitialEstimate(n);
  initialEstimate.print("Initial Estimate:\n");

  // Optimize using Levenberg-Marquardt optimization.
  LevenbergMarquardtParams params;
  params.setAbsoluteErrorTol(1e-10);
  params.setVerbosityLM("SUMMARY");
  LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params);
  Values result = optimizer.optimize();
  result.print("Final Result:\n");
}
/* ************************************************************************* */


#include<iostream>
#include <vector>
#include <tuple>
#include <float.h>
#include <numeric>
#include <Eigen/Dense>
#include "../nanoflann.hpp"
#include "gsec.hpp"
#include "utils.hpp"
#include "icp.hpp"
#include "../SO3.hpp"

using namespace nanoflann;
using namespace Geometry;

//
// Robust ICP
//

icp::Icp::Icp(icp::PointCloud &P, icp::PointCloud &M):
    resultSet(1),
    m_index(dim, std::cref(M), 10),
    p_index(dim, P, 10)
{
    // std::cout << "T: " << this->T << std::endl;
    // std::cout<<"&P = "<<&P<<std::endl;
    this->P = &P;
    // std::cout<<"&this->P = "<<(this->P)<<std::endl;
    this->Np = P.rows();
    this->M = &M;
    //
    this->constructKDTree();
    //
    this->resultSet.init(&(this->m_ret_index), &(this->m_out_dist_sqr));
    this->closestPtsFinder = &icp::Icp::closestpoints;
}

/**
 * if reciprocal is true, reject points based on eps,
 * otherwise based on a mutual closeness
*/
icp::Icp::Icp(icp::PointCloud &P, icp::PointCloud &M, bool reciprocal): icp::Icp(P, M)
{
    this->reciprocal = reciprocal;
    if(this->reciprocal) this->closestPtsFinder = &icp::Icp::closestpointsReciprocal;
}

/**
 * update iterations and distances vector
*/
void icp::Icp::init()
{
    this->iter++;
    this->initdists();
}

/**
 * clear ditances vector content
*/
void icp::Icp::initdists()
{
    // initialize heap
    this->distances.clear();
    // std::cout<< std::distance(distances.begin(), distances.begin()+Npo) << std::endl;
    // std::cout<< mse_cost(distances, xi) << std::endl;
    std::make_heap(this->distances.begin(), this->distances.end(), icp::comparePairs);
}

/**
 * find corresponces in the two pointclouds and compute distances.
 * kdtree is used
*/
void icp::Icp::closestpoints()
{
    // Step 1: Closest Point
    for (size_t i = 0; i < this->Np; i++)
    {
        this->resultSet.init(&(this->m_ret_index), &(this->m_out_dist_sqr));
        this->p_query_pt = {(*this->P)(i,0), (*this->P)(i,1), (*this->P)(i,2)};//{-15.7116, -10.9587, 0.650531};
        //
        this->m_index.index->findNeighbors(
            this->resultSet,
            &this->p_query_pt[0],
            nanoflann::SearchParams(10));

        // wrong pair rejection
        if(icp::rejectWrongPair(*this->M, this->m_ret_index, this->p_index, i)) continue;

        // update heap(distance)
        std::tuple<float, int, int> tmpDist = std::make_tuple(this->m_out_dist_sqr, i, this->m_ret_index);
        this->distances.push_back(tmpDist);
        std::push_heap(this->distances.begin(), this->distances.end(), icp::comparePairs);

    }
}

/**
 * using Reciprocal: Geometric criterion
*/
void icp::Icp::closestpointsReciprocal()
{
    // Step 1: Closest Point
    for (size_t i = 0; i < this->Np; i++)
    {

        this->resultSet.init(&(this->m_ret_index), &(this->m_out_dist_sqr));
        //auto query_ptt = P.row(i);
        this->p_query_pt = {(*this->P)(i,0), (*this->P)(i,1), (*this->P)(i,2)};
        //
        this->m_index.index->findNeighbors(
            this->resultSet,
            &this->p_query_pt[0],
            nanoflann::SearchParams(10));

        std::vector<float> m_query_pt = {(*this->M)(this->m_ret_index,0), (*this->M)(this->m_ret_index,1), (*this->M)(this->m_ret_index,2)};
        // wrong pair rejection
        if(icp::rejectWrongPair(
            m_query_pt, *this->P, this->p_index, i,
            this->eps))
            continue;

        std::tuple<float, int, int> tmpDist = std::make_tuple(this->m_out_dist_sqr, i, this->m_ret_index);
        this->distances.push_back(tmpDist);
        std::push_heap(this->distances.begin(), this->distances.end(), icp::comparePairs);

    }
    std::sort_heap(this->distances.begin(), this->distances.end(),  icp::comparePairs);
    // auto [ dist, pidx, midx ] = this->distances[0];
    // std::cout << "dist: " << dist << std::endl;
}

void icp::Icp::updateNpo()
{
    this->Npo = this->distances.size();//< Npo? distances.size():Npo;
    // std::cout << "Npo = " << Npo << std::endl;
}

/**
 * calculate squared distances and mean squared error (mse)
*/
void icp::Icp::calculateErrors()
{
    this->updateNpo();
    // this->Npo = this->distances.size();
    // Step 2: Squares
    this->Sts = std::accumulate(this->distances.begin(), this->distances.begin()+this->Npo, 0.0, icp::tupleAccumulateOP);
    // MSE
    this->e = this->Sts / this->Npo;

}

bool icp::Icp::convergencetest()
{
    // Step 3: Convergence test
    return (
        this->e <= this->mse_tol ||
        std::abs(this->e - this->e_prev)/this->e <= this->tol ||
        this->iter > this->Niter);// ||
        //std::abs(Sts-Stsprime) <= stsTolerance)

}

void icp::Icp::constructKDTree()
{
    // this->m_index(this->dim, std::cref(this->M), 10);
    // this->m_index.index->buildIndex();
    // // for Geometric criteria: rejecting wrong point pairs
    // this->p_index.index->buildIndex();
}

void icp::Icp::calculateMotion()
{
    // Step 4: Motion Calculation
    // https://igl.ethz.ch/projects/ARAP/svd_rot.pdf
    // select Npo points
    icp::PointCloud A(this->Npo, this->dim), B(this->Npo, this->dim);
    for (size_t i = 0; i < (this->Npo); i++)
    {
        auto [ dist, pidx, midx ] = this->distances[i];
        // std::cout<<"dist = "<<dist<<std::endl;
        A.row(i) = this->P->row(pidx);
        B.row(i) = this->M->row(midx);
    }

    Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> umeyama = Eigen::umeyama(A.transpose(), B.transpose(), false);
    // std::cout << "The matrix umeyama is of size "
    //         << umeyama.rows() << "x" << umeyama.cols() << std::endl;
    // std::cout << "Transformation: " << std::endl << umeyama << std::endl;
    this->R = umeyama.block(0,0,3,3);
    this->t  = umeyama.block(0,3,3,1);

    // std::cout << "rotation error = " <<
    //             std::acos(((
    //                 this->R.transpose().array() * this->Rtotal.array()).matrix().trace() - 1) / 2.0
    //                 ) * 180.f/M_PI <<
    //             std::endl;

    this->Rtotal = this->R * this->Rtotal;
    this->T = umeyama * this->T;
    this->ttotal = this->t + this->ttotal;
    // auto centerA = A.colwise().mean();
    // auto centerB = B.colwise().mean();

    // A.rowwise() -= centerA;
    // B.rowwise() -= centerB;

    // icp::PointCloud H = A.transpose()*B;
    // Eigen::BDCSVD<Eigen::Matrix<float, -1, -1> > svd(H,Eigen::ComputeFullU | Eigen::ComputeFullV);

    // // Rotation
    // this->R = svd.matrixV() * svd.matrixU().transpose();
    // // Translation
    // this->t = centerB.transpose() - this->R * centerA.transpose();
}

void icp::Icp::transform()
{
    // affine transformation
    (*this->P) = (R * (this->P->transpose())).transpose();
    (*this->P).rowwise() += this->t.transpose();
    // t.col(0)
}

void icp::Icp::updateParameters()
{
    // update parameters
    // float* rng = gss(mse_cost, distances, 0.4, 1.0);
    // xi = (rng[0] + rng[1])/ 2.f; //0.5; // 1 -> ICP
    // Npo = xi*Np;

    (this->p_index).index->buildIndex();
    (this->e_prev) = this->e;
    (this->Stsprime) = this->Sts;
}

/**
 * calculate rotaion error using Log(R_gt*R).norm()
*/
float icp::Icp::rotationError(Eigen::Matrix3f R)
{
    // Eigen::Matrix3f Rt = this->Rtotal;
    Eigen::Matrix3f Rt = this->T.block(0,0,3,3);
    Eigen::Matrix3f rotE = R * (Rt).transpose();
    // std::cout << "Log(rotE): " << SO3::Log(rotE).transpose() << std::endl;
    // std::cout << "||Log(rotE)||_2: " << SO3::Log(rotE).norm() << std::endl;
    return SO3::Log(rotE).norm() * 180.f / M_PI;

}

void icp::Icp::run()
{
    do
    {
        // initialize heap
        this->init();

        // Step 1: Closest Point
        // if(this->reciprocal)
        //     this->closestpoints(this->eps);
        // else this->closestpoints();
        (this->*closestPtsFinder)();

        // Step 2: Trimmed Squares
        this->calculateErrors();

        // Step 3: Convergence test
        if(this->convergencetest()) break;

        // Step 4: Motion Calculation
        this->calculateMotion();

        // Step 5: Transformation
        this->transform();

        // Step 6: update parameters
        this->updateParameters();

    } while (true);
}

// getters
size_t icp::Icp::getIter()
{
    return this->iter;
}
size_t icp::Icp::getNpo()
{
    return this->Npo;
}
float icp::Icp::getSts()
{
    return this->Sts;
}
float icp::Icp::getMse()
{
    return this->e;
}

/**
 * return the accumulated rotation matrix
*/
icp::PointCloud icp::Icp::getR()
{
    return this->T.block(0,0,3,3);
    // return this->Rtotal;
}

/**
 * return the accumulated translation vector
*/
Eigen::Vector3f icp::Icp::getT()
{
    return this->T.block(0,3,3,1);
    // return   this->ttotal;
}
//
icp::Icp::~Icp()
{
    this->P = NULL;
    delete this->P;
    this->M = NULL;
    delete this->M;
    // this->closestPtsFinder = NULL;
    // delete this->closestPtsFinder;
}

//
// Trimmed Icp
//
// Constructors
icp::TrIcp::TrIcp(icp::PointCloud& P, icp::PointCloud& M): icp::Icp(P, M)
{
    this->Npo = this->xi * this->Np;
}

icp::TrIcp::TrIcp(icp::PointCloud& P, icp::PointCloud& M, float xi): icp::Icp(P, M)
{
    this->xi = xi;
    this->Npo = xi * this->Np;
}

// public methods
void icp::TrIcp::closestpoints()
{
    // Step 1: Closest Point
    for (size_t i = 0; i < this->Np; i++)
    {
        this->resultSet.init(&(this->m_ret_index), &(this->m_out_dist_sqr));
        this->p_query_pt = {(*this->P)(i,0), (*this->P)(i,1), (*this->P)(i,2)};//{-15.7116, -10.9587, 0.650531};
        //
        this->m_index.index->findNeighbors(
            this->resultSet,
            &this->p_query_pt[0],
            nanoflann::SearchParams(10));

        // update heap(distance)
        std::tuple<float, int, int> tmpDist = std::make_tuple(this->m_out_dist_sqr, i, this->m_ret_index);
        this->distances.push_back(tmpDist);
        std::push_heap(this->distances.begin(), this->distances.end(), icp::comparePairs);

    }
    std::sort_heap(this->distances.begin(), this->distances.end(),  icp::comparePairs);
    // auto [ dist, pidx, midx ] = this->distances[0];
    // std::cout << "dist: " << dist << std::endl;
}

void icp::TrIcp::updateNpo()
{
    if(!this->isUpdatedNpo)
    {
        float* rng = gss(mse_cost, this->distances, 0.4, 1.0);
        this->xi = (rng[0] + rng[1])/ 2.f; //0.5; // 1 -> ICP
        this->Npo = this->xi * this->Np;
        //
        // this->isUpdatedNpo = true;
    }
}

void icp::TrIcp::updateParameters()
{
    (this->e_prev) = this->e;
    (this->Stsprime) = this->Sts;
}

// destructor
icp::TrIcp::~TrIcp()
{
}


/*------------------------------------------------------*/
/* Biological Engineering: Problem Set 03               */
/* main program                                         */
/*                                                      */
/* file name    :   BiologicalEngineering_set3.cpp      */
/* compiler     :   gcc                                 */
/* Student ID   :   1526084                             */
/* author       :   Takaharu Nakajima                   */
/* date         :   2016.11.11                          */
/* memo         :   $B@8BN9)3XBh;02sL\$N(B1.1$B$+$i(B1.3$B$N(B      */
/*                  $B%W%m%0%i%`$r$^$H$a$?$b$N$K$J$j$^$9(B  */
/*                                                      */
/*                  $B0lItJQ?t$NDj5A$N$?$aJQ99$7$?ItJ,(B    */
/*                  ($BNc(B)                                */
/*                  RT0 -> RTa                          */
/*                  $B$3$N$h$&$K(B0, 1...E$B$r(Ba, b...E$B$K(B      */
/*                  $BJQ99$7$^$7$?(B                        */
/*                                                      */
/*------------------------------------------------------*/

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

#define PI 3.1415926

double deg2rad(double degree)
{
    double radian = degree * PI / 180.0f;
    return radian;
}

int main()
{
    double deg1, deg2, deg3, deg4, deg5;
    double rad1, rad2, rad3, rad4, rad5;
    Matrix<double,4,4> RTa, aTb, bTc, cTd, dTe, eTf, fTg;
    Matrix<double,4,4> RTb, RTc, RTd, RTe, RTf, RTg;
    Vector3d RZa, RZb, RZc, RZd, RZe, RZf, RZg;
    Vector3d Rpa, Rpb, Rpc, Rpd, Rpe, Rpf, Rpg;
    Vector3d RpEa, RpEb, RpEc, RpEd, RpEe, RpEf, RpEg;

    /* Input */
    cout << "Angle input" << endl;
    cout << "deg1= ";
    cin >> deg1;
    cout << "deg2= ";
    cin >> deg2;
    cout << "deg3= ";
    cin >> deg3;
    cout << "deg4= ";
    cin >> deg4;
    cout << "deg5= ";
    cin >> deg5;

    /* Convert to radians */
    rad1 = deg2rad(deg1);
    rad2 = deg2rad(deg2);
    rad3 = deg2rad(deg3);
    rad4 = deg2rad(deg4);
    rad5 = deg2rad(deg5);

    /* Link parameters */
    RTa << 1, 0, 0, 0,
           0, 1, 0, 0,
           0, 0, 1, 1,
           0, 0, 0, 1;
    aTb << cos(rad1), -sin(rad1), 0, 0,
           sin(rad1),  cos(rad1), 0, 0,
                   0,          0, 1, 0,
                   0,          0, 0, 1;
    bTc << cos(rad2), -sin(rad2), 0, 0,
                   0,          0, -1, 0,
           sin(rad2),  cos(rad2), 0, 0,
                   0,          0, 0, 1;
    cTd << cos(rad3), -sin(rad3), 0, 1,
           sin(rad3),  cos(rad3), 0, 0,
                   0,          0, 1, 0,
                   0,          0, 0, 1;
    dTe << cos(rad4), -sin(rad4), 0, 1,
           sin(rad4),  cos(rad4), 0, 0,
                   0,          0, 1, 0,
                   0,          0, 0, 1;
    eTf << cos(rad5), -sin(rad5), 0, 0,
                   0,          0, -1, 0,
           sin(rad5),  cos(rad5), 0, 0,
                   0,          0, 0, 1;
    fTg << 1, 0, 0, 0,
           0, 1, 0, 0,
           0, 0, 1, 1,
           0, 0, 0, 1;

    /* Matrix calculation */
    RTb = RTa * aTb;
    RTc = RTb * bTc;
    RTd = RTc * cTd;
    RTe = RTd * dTe;
    RTf = RTe * eTf;
    RTg = RTf * fTg;

    RZa = RTa.block<3,1>(0,2);
    RZb = RTb.block<3,1>(0,2);
    RZc = RTc.block<3,1>(0,2);
    RZd = RTd.block<3,1>(0,2);
    RZe = RTe.block<3,1>(0,2);
    RZf = RTf.block<3,1>(0,2);
    RZg = RTg.block<3,1>(0,2);

    Rpa = RTa.block<3,1>(0,3);
    Rpb = RTb.block<3,1>(0,3);
    Rpc = RTc.block<3,1>(0,3);
    Rpd = RTd.block<3,1>(0,3);
    Rpe = RTe.block<3,1>(0,3);
    Rpf = RTf.block<3,1>(0,3);
    Rpg = RTg.block<3,1>(0,3);

    RpEa = Rpg - Rpa;
    RpEb = Rpg - Rpb;
    RpEc = Rpg - Rpc;
    RpEd = Rpg - Rpd;
    RpEe = Rpg - Rpe;
    RpEf = Rpg - Rpf;
    RpEg = Rpg - Rpg;

    /* Calculation result display */
    /* RT0 ~ 3TE */
    cout << "RT0=" << endl;
    cout << RTa << endl;
    cout << "0T1=" << endl;
    cout << aTb << endl;
    cout << "1T2=" << endl;
    cout << bTc << endl;
    cout << "2T3=" << endl;
    cout << cTd << endl;
    cout << "3T4=" << endl;
    cout << dTe << endl;
    cout << "4T5=" << endl;
    cout << eTf << endl;
    cout << "5T6=" << endl;
    cout << fTg << endl << endl;

    /* RZ0 ~ RZE */
    cout << "RZ0=" << endl;
    cout << RZa << endl;
    cout << "RZ1=" << endl;
    cout << RZb << endl;
    cout << "RZ2=" << endl;
    cout << RZc << endl;
    cout << "RZ3=" << endl;
    cout << RZd << endl;
    cout << "RZ4=" << endl;
    cout << RZe << endl;
    cout << "RZ5=" << endl;
    cout << RZf << endl;
    cout << "RZ6=" << endl;
    cout << RZg << endl;

    /* Rp0 ~ Rp1 */
    cout << "Rp0=" << endl;
    cout << Rpa << endl;
    cout << "Rp1=" << endl;
    cout << Rpb << endl;
    cout << "Rp2=" << endl;
    cout << Rpc << endl;
    cout << "Rp3=" << endl;
    cout << Rpd << endl;
    cout << "Rp4=" << endl;
    cout << Rpe << endl;
    cout << "Rp5=" << endl;
    cout << Rpf << endl;
    cout << "Rp6=" << endl;
    cout << Rpg << endl;

    /* RZ0 x RpE,0 ~ RZE x RpE,E */
    cout << "RZ0 x RpE,0=" << endl;
    cout << RZa.cross(RpEa) << endl;
    cout << "RZ1 x RpE,1=" << endl;
    cout << RZb.cross(RpEb) << endl;
    cout << "RZ2 x RpE,2=" << endl;
    cout << RZc.cross(RpEc) << endl;
    cout << "RZ3 x RpE,3=" << endl;
    cout << RZd.cross(RpEd) << endl;
    cout << "RZ4 x RpE,4=" << endl;
    cout << RZe.cross(RpEe) << endl;
    cout << "RZ5 x RpE,5=" << endl;
    cout << RZf.cross(RpEf) << endl;
    cout << "RZ6 x RpE,6=" << endl;
    cout << RZg.cross(RpEg) << endl;

    return 0;
}


/*
* Copyright (c) by CryptoLab inc.
* This program is licensed under a
* Creative Commons Attribution-NonCommercial 3.0 Unported License.
* You should have received a copy of the license along with this
* work.  If not, see <http://creativecommons.org/licenses/by-nc/3.0/>.
*/

#include "RingMultiplier.h"

#include <NTL/BasicThreadPool.h>
#include <NTL/tools.h>
#include <cmath>
#include <cstdlib>
#include <iterator>

RingMultiplier::RingMultiplier() {

	uint64_t primetest = (1ULL << pbnd) + 1;
	for (long i = 0; i < nprimes; ++i) {
		while(true) {
			primetest += M;
			if(primeTest(primetest)) {
				pVec[i] = primetest;
				break;
			}
		}
	}

	for (long i = 0; i < nprimes; ++i) {
		red_ss_array[i] = _ntl_general_rem_one_struct_build(pVec[i]);
		pInvVec[i] = inv(pVec[i]);
		prVec[i] = (static_cast<unsigned __int128>(1) << kbar2) / pVec[i];
		uint64_t root = findMthRootOfUnity(M, pVec[i]);
		uint64_t rootinv = invMod(root, pVec[i]);
		uint64_t NInv = invMod(N, pVec[i]);
		mulMod(scaledNInv[i], NInv, (1ULL << 32), pVec[i]);
		mulMod(scaledNInv[i], scaledNInv[i], (1ULL << 32), pVec[i]);
		scaledRootPows[i] = new uint64_t[N]();
		scaledRootInvPows[i] = new uint64_t[N]();
		uint64_t power = 1;
		uint64_t powerInv = 1;
		for (long j = 0; j < N; ++j) {
			uint32_t jprime = bitReverse(static_cast<uint32_t>(j)) >> (32 - logN);
			uint64_t rootpow = power;
			mulMod(scaledRootPows[i][jprime], rootpow,(1ULL << 32), pVec[i]);
			mulMod(scaledRootPows[i][jprime], scaledRootPows[i][jprime], (1ULL << 32), pVec[i]);
			uint64_t rootpowInv = powerInv;
			mulMod(scaledRootInvPows[i][jprime], rootpowInv, (1ULL << 32), pVec[i]);
			mulMod(scaledRootInvPows[i][jprime], scaledRootInvPows[i][jprime], (1ULL << 32), pVec[i]);
			mulMod(power, power, root, pVec[i]);
			mulMod(powerInv, powerInv, rootinv, pVec[i]);
		}
	}

	for (long i = 0; i < nprimes; ++i) {
		coeffpinv_array[i] = new mulmod_precon_t[i + 1];
		pProd[i] = (i == 0) ? to_ZZ((long) pVec[i]) : pProd[i - 1] * (long) pVec[i];
		pProdh[i] = pProd[i] / 2;
		pHat[i] = new ZZ[i + 1];
		pHatInvModp[i] = new uint64_t[i + 1];
		for (long j = 0; j < i + 1; ++j) {
			pHat[i][j] = ZZ(1);
			for (long k = 0; k < j; ++k) {
				pHat[i][j] *= (long) pVec[k];
			}
			for (long k = j + 1; k < i + 1; ++k) {
				pHat[i][j] *= (long) pVec[k];
			}
			pHatInvModp[i][j] = to_long(pHat[i][j] % (long) pVec[j]);
			pHatInvModp[i][j] = invMod(pHatInvModp[i][j], pVec[j]);
			coeffpinv_array[i][j] = PrepMulModPrecon(pHatInvModp[i][j], pVec[j]);
		}
	}
}

bool RingMultiplier::primeTest(uint64_t p) {
	if(p < 2) return false;
	if(p != 2 && p % 2 == 0) return false;
	uint64_t s = p - 1;
	while(s % 2 == 0) {
		s /= 2;
	}
	for(long i = 0; i < 200; i++) {
		uint64_t temp1 = rand();
		temp1  = (temp1 << 32) | rand();
		temp1 = temp1 % (p - 1) + 1;
		uint64_t temp2 = s;
		uint64_t mod = powMod(temp1,temp2,p);
		while (temp2 != p - 1 && mod != 1 && mod != p - 1) {
			mulMod(mod, mod, mod, p);
		    temp2 *= 2;
		}
		if (mod != p - 1 && temp2 % 2 == 0) return false;
	}
	return true;
}

void RingMultiplier::NTT(uint64_t* a, long index) {
	long t = N;
	long logt1 = logN + 1;
	uint64_t p = pVec[index];
	uint64_t pInv = pInvVec[index];
	for (long m = 1; m < N; m <<= 1) {
		t >>= 1;
		logt1 -= 1;
		for (long i = 0; i < m; i++) {
			long j1 = i << logt1;
			long j2 = j1 + t - 1;
			uint64_t W = scaledRootPows[index][m + i];
			for (long j = j1; j <= j2; j++) {
				butt(a[j], a[j+t], W, p, pInv);
			}
		}
	}
}

void RingMultiplier::INTT(uint64_t* a, long index) {
	uint64_t p = pVec[index];
	uint64_t pInv = pInvVec[index];
	long t = 1;
	for (long m = N; m > 1; m >>= 1) {
		long j1 = 0;
		long h = m >> 1;
		for (long i = 0; i < h; i++) {
			long j2 = j1 + t - 1;
			uint64_t W = scaledRootInvPows[index][h + i];
			for (long j = j1; j <= j2; j++) {
				ibutt(a[j], a[j+t], W, p, pInv);
			}
			j1 += (t << 1);
		}
		t <<= 1;
	}

	uint64_t NScale = scaledNInv[index];
	for (long i = 0; i < N; i++) {
		idivN(a[i], NScale, p, pInv);
	}
}

//----------------------------------------------------------------------------------
//   FFT
//----------------------------------------------------------------------------------

void RingMultiplier::CRT(uint64_t* rx, ZZ* x, const long np) {
	NTL_EXEC_RANGE(np, first, last);
	for (long i = first; i < last; ++i) {
		uint64_t* rxi = rx + (i << logN);
		uint64_t pi = pVec[i];
		_ntl_general_rem_one_struct* red_ss = red_ss_array[i];
		for (long n = 0; n < N; ++n) {
			rxi[n] = _ntl_general_rem_one_struct_apply(x[n].rep, pi, red_ss);
		}
		NTT(rxi, i);
	}
	NTL_EXEC_RANGE_END;
}

void RingMultiplier::addNTTAndEqual(uint64_t* ra, uint64_t* rb, const long np) {
	for (long i = 0; i < np; ++i) {
		uint64_t* rai = ra + (i << logN);
		uint64_t* rbi = rb + (i << logN);
		uint64_t pi = pVec[i];
		for (long n = 0; n < N; ++n) {
			rai[n] += rbi[n];
			if(rai[n] > pi) rai[n] -= pi;
		}
	}
}

void RingMultiplier::reconstruct(ZZ* x, uint64_t* rx, long np, const ZZ& q) {
	ZZ* pHatnp = pHat[np - 1];
	uint64_t* pHatInvModpnp = pHatInvModp[np - 1];
	mulmod_precon_t* coeffpinv_arraynp = coeffpinv_array[np - 1];
	ZZ& pProdnp = pProd[np - 1];
	ZZ& pProdhnp = pProdh[np - 1];
	NTL_EXEC_RANGE(N, first, last);
	for (long n = first; n < last; ++n) {
		ZZ& acc = x[n];
		QuickAccumBegin(acc, pProdnp.size());
		for (long i = 0; i < np; i++) {
			long p = pVec[i];
			long tt = pHatInvModpnp[i];
			mulmod_precon_t ttpinv = coeffpinv_arraynp[i];
			long s = MulModPrecon(rx[n + (i << logN)], tt, p, ttpinv);
			QuickAccumMulAdd(acc, pHatnp[i], s);
		}
		QuickAccumEnd(acc);
		rem(x[n], x[n], pProdnp);
		if (x[n] > pProdhnp) x[n] -= pProdnp;
		x[n] %= q;
	}
	NTL_EXEC_RANGE_END;
}

void RingMultiplier::mult(ZZ* x, ZZ* a, ZZ* b, long np, const ZZ& mod) {
	uint64_t* ra = new uint64_t[np << logN]();
	uint64_t* rb = new uint64_t[np << logN]();
	uint64_t* rx = new uint64_t[np << logN]();

	NTL_EXEC_RANGE(np, first, last);
	for (long i = first; i < last; ++i) {
		uint64_t* rai = ra + (i << logN);
		uint64_t* rbi = rb + (i << logN);
		uint64_t* rxi = rx + (i << logN);
		uint64_t pi = pVec[i];
		uint64_t pri = prVec[i];
		_ntl_general_rem_one_struct* red_ss = red_ss_array[i];
		for (long n = 0; n < N; ++n) {
			rai[n] = _ntl_general_rem_one_struct_apply(a[n].rep, pi, red_ss);
			rbi[n] = _ntl_general_rem_one_struct_apply(b[n].rep, pi, red_ss);
		}
		NTT(rai, i);
		NTT(rbi, i);
		for (long n = 0; n < N; ++n) {
			mulModBarrett(rxi[n], rai[n], rbi[n], pi, pri);
		}
		INTT(rxi, i);
	}
	NTL_EXEC_RANGE_END;

	reconstruct(x, rx, np, mod);

	delete[] ra;
	delete[] rb;
	delete[] rx;
}

void RingMultiplier::multNTT(ZZ* x, ZZ* a, uint64_t* rb, long np, const ZZ& mod) {
	uint64_t* ra = new uint64_t[np << logN]();
	uint64_t* rx = new uint64_t[np << logN]();
	NTL_EXEC_RANGE(np, first, last);
	for (long i = first; i < last; ++i) {
		uint64_t* rai = ra + (i << logN);
		uint64_t* rbi = rb + (i << logN);
		uint64_t* rxi = rx + (i << logN);
		uint64_t pi = pVec[i];
		uint64_t pri = prVec[i];
		_ntl_general_rem_one_struct* red_ss = red_ss_array[i];
		for (long n = 0; n < N; ++n) {
			rai[n] = _ntl_general_rem_one_struct_apply(a[n].rep, pi, red_ss);
		}
		NTT(rai, i);
		for (long n = 0; n < N; ++n) {
			mulModBarrett(rxi[n], rai[n], rbi[n], pi, pri);
		}
		INTT(rxi, i);
	}
	NTL_EXEC_RANGE_END;

	reconstruct(x, rx, np, mod);

	delete[] ra;
	delete[] rx;
}

void RingMultiplier::multDNTT(ZZ* x, uint64_t* ra, uint64_t* rb, long np, const ZZ& mod) {
	uint64_t* rx = new uint64_t[np << logN]();

	NTL_EXEC_RANGE(np, first, last);
	for (long i = first; i < last; ++i) {
		uint64_t* rai = ra + (i << logN);
		uint64_t* rbi = rb + (i << logN);
		uint64_t* rxi = rx + (i << logN);
		uint64_t pi = pVec[i];
		uint64_t pri = prVec[i];
		for (long n = 0; n < N; ++n) {
			mulModBarrett(rxi[n], rai[n], rbi[n], pi, pri);
		}
		INTT(rxi, i);
	}
	NTL_EXEC_RANGE_END;

	reconstruct(x, rx, np, mod);

	delete[] rx;
}

void RingMultiplier::multAndEqual(ZZ* a, ZZ* b, long np, const ZZ& mod) {
	uint64_t* ra = new uint64_t[np << logN]();
	uint64_t* rb = new uint64_t[np << logN]();

	NTL_EXEC_RANGE(np, first, last);
	for (long i = first; i < last; ++i) {
		uint64_t* rai = ra + (i << logN);
		uint64_t* rbi = rb + (i << logN);
		uint64_t pi = pVec[i];
		uint64_t pri = prVec[i];
		_ntl_general_rem_one_struct* red_ss = red_ss_array[i];
		for (long n = 0; n < N; ++n) {
			rai[n] = _ntl_general_rem_one_struct_apply(a[n].rep, pi, red_ss);
			rbi[n] = _ntl_general_rem_one_struct_apply(b[n].rep, pi, red_ss);
		}
		NTT(rai, i);
		NTT(rbi, i);
		for (long n = 0; n < N; ++n) {
			mulModBarrett(rai[n], rai[n], rbi[n], pi, pri);
		}
		INTT(rai, i);
	}
	NTL_EXEC_RANGE_END;

	ZZ* pHatnp = pHat[np - 1];
	uint64_t* pHatInvModpnp = pHatInvModp[np - 1];

	reconstruct(a, ra, np, mod);

	delete[] ra;
	delete[] rb;
}

void RingMultiplier::multNTTAndEqual(ZZ* a, uint64_t* rb, long np, const ZZ& mod) {
	uint64_t* ra = new uint64_t[np << logN]();

	NTL_EXEC_RANGE(np, first, last);
	for (long i = first; i < last; ++i) {
		uint64_t* rai = ra + (i << logN);
		uint64_t* rbi = rb + (i << logN);
		uint64_t pi = pVec[i];
		uint64_t pri = prVec[i];
		_ntl_general_rem_one_struct* red_ss = red_ss_array[i];
		for (long n = 0; n < N; ++n) {
			rai[n] = _ntl_general_rem_one_struct_apply(a[n].rep, pi, red_ss);
		}
		NTT(rai, i);
		for (long n = 0; n < N; ++n) {
			mulModBarrett(rai[n], rai[n], rbi[n], pi, pri);
		}
		INTT(rai, i);
	}
	NTL_EXEC_RANGE_END;

	ZZ* pHatnp = pHat[np - 1];
	uint64_t* pHatInvModpnp = pHatInvModp[np - 1];

	reconstruct(a, ra, np, mod);

	delete[] ra;
}

void RingMultiplier::square(ZZ* x, ZZ* a, long np, const ZZ& mod) {
	uint64_t* ra = new uint64_t[np << logN]();
	uint64_t* rx = new uint64_t[np << logN]();

	NTL_EXEC_RANGE(np, first, last);
	for (long i = first; i < last; ++i) {
		uint64_t* rai = ra + (i << logN);
		uint64_t* rxi = rx + (i << logN);
		uint64_t pi = pVec[i];
		uint64_t pri = prVec[i];
		_ntl_general_rem_one_struct* red_ss = red_ss_array[i];
		for (long n = 0; n < N; ++n) {
			rai[n] = _ntl_general_rem_one_struct_apply(a[n].rep, pi, red_ss);
		}
		NTT(rai, i);
		for (long n = 0; n < N; ++n) {
			mulModBarrett(rxi[n], rai[n], rai[n], pi, pri);
		}
		INTT(rxi, i);
	}
	NTL_EXEC_RANGE_END;

	ZZ* pHatnp = pHat[np - 1];
	uint64_t* pHatInvModpnp = pHatInvModp[np - 1];

	reconstruct(x, rx, np, mod);

	delete[] ra;
	delete[] rx;
}

void RingMultiplier::squareNTT(ZZ* x, uint64_t* ra, long np, const ZZ& mod) {
	uint64_t* rx = new uint64_t[np << logN]();

	NTL_EXEC_RANGE(np, first, last);
	for (long i = first; i < last; ++i) {
		uint64_t* rai = ra + (i << logN);
		uint64_t* rxi = rx + (i << logN);
		uint64_t pi = pVec[i];
		uint64_t pri = prVec[i];
		for (long n = 0; n < N; ++n) {
			mulModBarrett(rxi[n], rai[n], rai[n], pi, pri);
		}
		INTT(rxi, i);
	}
	NTL_EXEC_RANGE_END;

	reconstruct(x, rx, np, mod);

	delete[] rx;
}

void RingMultiplier::squareAndEqual(ZZ* a, long np, const ZZ& mod) {
	uint64_t* ra = new uint64_t[np << logN]();

	NTL_EXEC_RANGE(np, first, last);
	for (long i = first; i < last; ++i) {
		uint64_t* rai = ra + (i << logN);
		uint64_t pi = pVec[i];
		uint64_t pri = prVec[i];
		_ntl_general_rem_one_struct* red_ss = red_ss_array[i];
		for (long n = 0; n < N; ++n) {
			rai[n] = _ntl_general_rem_one_struct_apply(a[n].rep, pi, red_ss);
		}
		NTT(rai, i);
		for (long n = 0; n < N; ++n) {
			mulModBarrett(rai[n], rai[n], rai[n], pi, pri);
		}
		INTT(rai, i);
	}
	NTL_EXEC_RANGE_END;

	reconstruct(a, ra, np, mod);

	delete[] ra;
}

void RingMultiplier::mulMod(uint64_t &r, uint64_t a, uint64_t b, uint64_t m) {
	unsigned __int128 mul = static_cast<unsigned __int128>(a) * b;
	mul %= static_cast<unsigned __int128>(m);
	r = static_cast<uint64_t>(mul);
}

void RingMultiplier::mulModBarrett(uint64_t& r, uint64_t a, uint64_t b, uint64_t p, uint64_t pr) {
	unsigned __int128 mul = static_cast<unsigned __int128>(a) * b;
	uint64_t abot = static_cast<uint64_t>(mul);
	uint64_t atop = static_cast<uint64_t>(mul >> 64);
	unsigned __int128 tmp = static_cast<unsigned __int128>(abot) * pr;
	tmp >>= 64;
	tmp += static_cast<unsigned __int128>(atop) * pr;
	tmp >>= kbar2 - 64;
	tmp *= p;
	tmp = mul - tmp;
	r = static_cast<uint64_t>(tmp);
	if(r >= p) r -= p;
}

void RingMultiplier::butt(uint64_t& a, uint64_t& b, uint64_t W, uint64_t p, uint64_t pInv) {
	unsigned __int128 U = static_cast<unsigned __int128>(b) * W;
	uint64_t U0 = static_cast<uint64_t>(U);
	uint64_t U1 = U >> 64;
	uint64_t Q = U0 * pInv;
	unsigned __int128 Hx = static_cast<unsigned __int128>(Q) * p;
	uint64_t H = Hx >> 64;
	uint64_t V = U1 < H ? U1 + p - H : U1 - H;
	b = a < V ? a + p - V : a - V;
	a += V;
	if (a > p) a -= p;
}

void RingMultiplier::ibutt(uint64_t& a, uint64_t& b, uint64_t W, uint64_t p, uint64_t pInv) {
	uint64_t T = a < b ? a + p - b : a - b;
	a += b;
	if (a > p) a -= p;
	unsigned __int128 UU = static_cast<unsigned __int128>(T) * W;
	uint64_t U0 = static_cast<uint64_t>(UU);
	uint64_t U1 = UU >> 64;
	uint64_t Q = U0 * pInv;
	unsigned __int128 Hx = static_cast<unsigned __int128>(Q) * p;
	uint64_t H = Hx >> 64;
	b = (U1 < H) ? U1 + p - H : U1 - H;
}

void RingMultiplier::idivN(uint64_t& a, uint64_t NScale, uint64_t p, uint64_t pInv) {
	unsigned __int128 U = static_cast<unsigned __int128>(a) * NScale;
	uint64_t U0 = static_cast<uint64_t>(U);
	uint64_t U1 = U >> 64;
	uint64_t Q = U0 * pInv;
	unsigned __int128 Hx = static_cast<unsigned __int128>(Q) * p;
	uint64_t H = Hx >> 64;
	a = (U1 < H) ? U1 + p - H : U1 - H;
}

uint64_t RingMultiplier::invMod(uint64_t x, uint64_t m) {
	return powMod(x, m - 2, m);
}

uint64_t RingMultiplier::powMod(uint64_t x, uint64_t y, uint64_t modulus) {
	uint64_t res = 1;
	while (y > 0) {
		if (y & 1) {
			mulMod(res, res, x, modulus);
		}
		y = y >> 1;
		mulMod(x, x, x, modulus);
	}
	return res;
}

uint64_t RingMultiplier::inv(uint64_t x) {
	return pow(x, static_cast<uint64_t>(-1));
}

uint64_t RingMultiplier::pow(uint64_t x, uint64_t y) {
	uint64_t res = 1;
	while (y > 0) {
		if (y & 1) {
			res *= x;
		}
		y = y >> 1;
		x *= x;
	}
	return res;
}

uint32_t RingMultiplier::bitReverse(uint32_t x) {
	x = (((x & 0xaaaaaaaa) >> 1) | ((x & 0x55555555) << 1));
	x = (((x & 0xcccccccc) >> 2) | ((x & 0x33333333) << 2));
	x = (((x & 0xf0f0f0f0) >> 4) | ((x & 0x0f0f0f0f) << 4));
	x = (((x & 0xff00ff00) >> 8) | ((x & 0x00ff00ff) << 8));
	return ((x >> 16) | (x << 16));
}

void RingMultiplier::findPrimeFactors(vector<uint64_t> &s, uint64_t number) {
	while (number % 2 == 0) {
		s.push_back(2);
		number /= 2;
	}
	for (uint64_t i = 3; i < sqrt(number); i++) {
		while (number % i == 0) {
			s.push_back(i);
			number /= i;
		}
	}
	if (number > 2) {
		s.push_back(number);
	}
}

uint64_t RingMultiplier::findPrimitiveRoot(uint64_t modulus) {
	vector<uint64_t> s;
	uint64_t phi = modulus - 1;
	findPrimeFactors(s, phi);
	for (uint64_t r = 2; r <= phi; r++) {
		bool flag = false;
		for (auto it = s.begin(); it != s.end(); it++) {
			if (powMod(r, phi / (*it), modulus) == 1) {
				flag = true;
				break;
			}
		}
		if (flag == false) {
			return r;
		}
	}
	return -1;
}

uint64_t RingMultiplier::findMthRootOfUnity(uint64_t M, uint64_t mod) {
    uint64_t res;
    res = findPrimitiveRoot(mod);
    if((mod - 1) % M == 0) {
        uint64_t factor = (mod - 1) / M;
        res = powMod(res, factor, mod);
        return res;
    }
    else {
        return -1;
    }
}


#include <boost_adaptbx/graph/graph_type.hpp>
#include <boost_adaptbx/graph/graph_export_adaptor.hpp>
#include <boost_adaptbx/graph/vertex_map.hpp>

#include <boost_adaptbx/exporting.hpp>

#include <boost/python/module.hpp>
#include <boost/python/list.hpp>
#include <boost/python/tuple.hpp>
#include <boost/python/dict.hpp>
#include <boost/python/def.hpp>
#include <boost/python/extract.hpp>
#include <boost/python/stl_iterator.hpp>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/stoer_wagner_min_cut.hpp>
#include <boost/graph/one_bit_color_map.hpp>
#include <boost/graph/boykov_kolmogorov_max_flow.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/properties.hpp>

#include <boost/mpl/if.hpp>
#include <boost/type_traits.hpp>

#include <iostream>

namespace boost_adaptbx
{

template< typename Graph >
struct minimum_cut_export
{
  typedef boost::graph_traits< Graph > graph_traits;
  typedef typename graph_traits::vertex_iterator vertex_iterator;
  typedef typename graph_traits::edge_iterator edge_iterator;
  typedef vertex_map::index_map< Graph > index_map_type;
  typedef typename index_map_type::property_map_type index_property_map_type;
  typedef boost::one_bit_color_map< index_property_map_type > parity_map_type;
  typedef edge_map::generic_edge_map< Graph, double > edge_map_type;
  typedef typename edge_map_type::property_map_type edge_property_map_type;

  static boost::python::tuple stoer_wagner_minimum_cut(Graph const& graph)
  {
    index_map_type index_map( graph );
    parity_map_type parities = boost::make_one_bit_color_map(
      boost::num_vertices( graph ),
      index_map.get()
      );

    edge_map_type edge_map( graph );
    edge_property_map_type epropmap( edge_map.get() );
    edge_iterator ei, ej;
    double w;

    for( boost::tie( ei, ej ) = boost::edges( graph ); ei != ej; ++ei )
    {
      w = boost::python::extract< double >( boost::get( boost::edge_weight, graph, *ei ) );
      boost::put( epropmap, *ei, w);
    }

    w = boost::stoer_wagner_min_cut(
      graph,
      epropmap,
      boost::parity_map( parities ).
      vertex_index_map( index_map.get() )
      );

    vertex_iterator di, dj;
    boost::python::list result;

    for( boost::tie( di, dj ) = boost::vertices( graph ); di != dj; ++di )
    {
      result.append( bool( boost::get( parities, *di ) ) );
    }

    return boost::python::make_tuple( w, result );
  }

  static void process()
  {
    using namespace boost::python;

    def( "stoer_wagner_min_cut", stoer_wagner_minimum_cut, arg( "graph" ) );
  }
};

struct min_cut_exporter
{
  template< typename Export >
  void operator ()(boost::mpl::identity< Export > myexport) const
  {
    typedef typename Export::first graph_type;
    typedef typename Export::second name_type;

    typedef boost::graph_traits< graph_type > graph_traits;
    typedef typename boost::mpl::if_<
      boost::is_same<
        typename  graph_traits::directed_category,
        boost::undirected_tag
        >,
      minimum_cut_export< graph_type >,
      graph_export_adaptor::no_export< graph_type >
      >::type exporter_type;
    exporter_type::process();
  }
};

template< typename Graph >
struct maximum_flow_export
{
  typedef boost::graph_traits< Graph > graph_traits;
  typedef typename graph_traits::vertex_iterator vertex_iterator;
  typedef typename graph_traits::vertex_descriptor vertex_descriptor;
  typedef typename graph_traits::edge_iterator edge_iterator;
  typedef typename graph_traits::edge_descriptor edge_descriptor;

  typedef vertex_map::index_map< Graph > index_map_type;
  typedef typename index_map_type::property_map_type index_property_map_type;

  typedef edge_map::generic_edge_map< Graph, double > capacity_map_type;
  typedef typename capacity_map_type::property_map_type capacity_property_map_type;

  typedef edge_map::generic_edge_map< Graph, edge_descriptor > reverse_edge_map_type;
  typedef typename reverse_edge_map_type::property_map_type reverse_edge_property_map_type;

  typedef vertex_map::generic_vertex_map< Graph, boost::default_color_type > color_map_type;
  typedef typename color_map_type::property_map_type color_property_map_type;

  typedef graph_export_adaptor::vertex_descriptor_converter< vertex_descriptor > converter;

  static boost::python::tuple boykov_kolmogorov_max_flow(
    Graph const& graph,
    boost::python::dict reverse_edge_map,
    typename converter::type source,
    typename converter::type sink
    )
  {
    using namespace boost;
    using namespace boost::python;

    index_map_type index_map( graph );

    capacity_map_type capmap( graph );
    capacity_property_map_type cappropmap( capmap.get() );
    edge_iterator ei, ej;

    for( boost::tuples::tie( ei, ej ) = edges( graph ); ei != ej; ++ei )
    {
      double weight( extract< double >( get( edge_weight, graph, *ei ) ) );
      boost::put( cappropmap, *ei, weight );
    }

    reverse_edge_map_type revmap( graph );
    reverse_edge_property_map_type revpropmap( revmap.get() );

    stl_input_iterator< object > end;

    for( stl_input_iterator< object > it( reverse_edge_map.iteritems() ); it!=end; ++it)
    {
      edge_descriptor ed = extract< edge_descriptor >( (*it)[0] );
      edge_descriptor red = extract< edge_descriptor >( (*it)[1] );
      put( revpropmap, ed, red );
    }

    color_map_type colormap( graph );
    color_property_map_type colorpropmap( colormap.get() );
    capacity_map_type resicapmap( graph );

    double w = boost::boykov_kolmogorov_max_flow(
        graph,
        cappropmap,
        resicapmap.get(),
        revpropmap,
        colorpropmap,
        index_map.get(),
        converter::backward( source ),
        converter::backward( sink )
        );

    vertex_iterator di, dj;
    boost::python::list result;
    default_color_type const black( color_traits< default_color_type >::black() );
    default_color_type current;

    for( boost::tie( di, dj ) = boost::vertices( graph ); di != dj; ++di )
    {
      current = get( colorpropmap, *di );
      result.append( current == black );
    }

    return boost::python::make_tuple( w, result );
  }

  static void process()
  {
    using namespace boost::python;

    def(
      "boykov_kolmogorov_max_flow",
      boykov_kolmogorov_max_flow,
      ( arg( "graph" ), arg( "reverse_edge_map" ), arg( "source" ), arg( "sink" ) )
      );
  }
};

struct max_flow_exporter
{
  template< typename Export >
  void operator ()(boost::mpl::identity< Export > myexport) const
  {
    typedef typename Export::first graph_type;
    typedef typename Export::second name_type;

    typedef boost::graph_traits< graph_type > graph_traits;
    typedef typename boost::mpl::if_<
      boost::is_same<
        typename  graph_traits::directed_category,
        boost::directed_tag
        >,
      maximum_flow_export< graph_type >,
      graph_export_adaptor::no_export< graph_type >
      >::type exporter_type;
    exporter_type::process();
  }
};

} // namespace boost_adaptbx

BOOST_PYTHON_MODULE(boost_adaptbx_graph_min_cut_max_flow_ext)
{
  boost_adaptbx::exporting::class_list< boost_adaptbx::graph_type::exports >::process(
    boost_adaptbx::min_cut_exporter()
    );
  boost_adaptbx::exporting::class_list< boost_adaptbx::graph_type::exports >::process(
    boost_adaptbx::max_flow_exporter()
    );
}


#pragma once

#include <armadillo>
#include <string>

#include "equationofstate/equationofstatebase.hpp"

/*!
 * \brief The GERG04 class implements the GERG 2004 equation of state.
 *
 * This is a higly accurate, but extremely complex equation of state. It has
 * explicit forms for a lot of properties like heat capacity etc., which are
 * utilized where possible.
 *
 * \warning Note that the composition stored internally in GERG04::X is not in
 * the typical order, but in order C1, C2, C3, iC4, nC4, iC5, nC5, C6, N2, CO2.
 *
 * More info:
 *  - The GERG-2004 Wide-Range Equation of State for Natural Gases and Other Mixtures (O. Kunz and W. Wagner, J. Chem. Eng. Data. 2012, 57, 11, 3032-3091)
 *  - The GERG-2008 Wide-Range Equation of State for Natural Gases and Other Mixtures: An Expansion of GERG-2004 (O. Kunz and W. Wagner, J. Chem. Eng. Data. 2012, 57, 11, 3032-3091)
 */
class GERG04 : public EquationOfStateBase
{
public:
    /*!
     * \brief GERG04 constructor.
     *
     * Initializes the coefficients that only depend on composition.
     *
     * \param composition Gas composition in arma::vec, in the order C1, C2, C3, iC4, nC4, iC5, nC5, C6, N2, CO2.
     */
    explicit GERG04(const arma::vec& composition = Composition::defaultComposition);

    /*!
     * \brief Evaluate the GERG 2004 equation of state at the given pressure and temperature.
     *
     * This is a wrapper around evaluateAllProperties(), which selects the
     * wanted properties from that function. This could perhaps be optimized a
     * bit, since some of the calculations are not required. But the brunt of
     * the cpu time is spent in findDensity() anyway, so there probably isn't
     * that much to save.
     *
     * \param pressure Gas pressure [Pa]
     * \param temperature Gas temperature [K]
     * \return See EquationOfStateBase::evaluate().
     */
    virtual arma::vec evaluate(const double pressure, const double temperature) const override;

    /*!
     * \brief Evaluate all available gas properties at a given pressure and temperature.
     *
     * We have implemented 15 of the explicit equations for different properties
     * given in the GERG 2004 documentation.
     *
     * The method returns an arma::vec containing:
     *  - compressibility factor Z
     *  - partial derivative of Z wrt. temperature at constant pressure \f$\frac{\partial Z}{\partial T}|_p\f$
     *  - partial derivative of Z wrt. pressure at constant temperature \f$\frac{\partial Z}{\partial p}|_T\f$
     *  - partial derivative of Z wrt. temperature at constant density \f$\frac{\partial Z}{\partial T}|_\rho\f$
     *  - entropy S
     *  - internal energy U
     *  - heat capacity at constant volume \f$c_v\f$
     *  - enthalpy H
     *  - heat capacity at constant pressure \f$c_p\f$
     *  - gibbs free energy G
     *  - Joule-Thomson coefficient
     *  - speed of sound
     *  - isothermal throttling coefficient
     *  - density ρ
     *  - isentropic exponent \f$\gamma = \frac{c_p}{c_v}\f$
     *
     * \warning Other than the compressibility factor, the partial derivatives,
     * and the heat capacity, the output of this function has not been tested
     * properly. Feel free to implement tests for the other results.
     *
     * \param pressure Gas pressure [Pa].
     * \param temperature Gas temperature [K].
     * \return An arma::vec containing all the properties. See the detailed description for more details.
     */
    arma::vec evaluateAllProperties(const double pressure, const double temperature) const;

    /*!
     * \brief Calculate compressibility Z at given pressure and temperature.
     * \param pressure Gas pressure [Pa].
     * \param temperature Gas temperature [K].
     * \return Compressibility factor Z [-].
     */
    virtual double calculateCompressibility(const double pressure, const double temperature) const override;

    /*!
     * \brief Set the composition.
     *
     * This calls EquationOfStateBase::setComposition(), updates the non-zero
     * component indices GERG04::m_indices, and calculates the inverse reducing function
     * for mixture density GERG04::rhored and the reducing function for mixture
     * temperature GERG04::tred.
     *
     * \param composition New gas composition.
     * \param force If the composition should be changed even if it's within machine precision of the previous composition.
     * \return True if composition was changed, else false.
     */
    virtual bool setComposition(const arma::vec& composition, const bool force = true) override;

    /*!
     * \brief Find the density of the gas at a given pressure and temperature.
     * \param pressure Gas pressure [Pa].
     * \param temperature Gas temperature [K].
     * \return Gas density [kg/m3]
     */
    double findDensity(const double pressure, const double temperature) const;

    /*!
     * \brief Find the soundspeed in the gas at a given temperature and density.
     * \param temperature Gas temperature [K].
     * \param density Gas density [kg/m3]
     * \return Speed of sound in gas [m/s].
     */
    double findSpeedOfSound(const double temperature, const double density) const;

//    void useBadHeatCapacities(bool useBadHeatCapacities);

//    arma::vec getOutput() const;

    /*!
     * \brief Get the indices of the non-zero components in the composition.
     *
     * \warning Note that the components are stored internally in GERG04 in a
     * different order than the input order and the usual order, so it's not
     * straight-forward to relate this to usual composition.
     *
     * \return The indices of the non-zero components.
     */
    const arma::uvec& indicesOfNonZeroComponents() const { return m_indices; }

private:
    /*!
     * \brief Internal (private) function used in the process of evaluating the GERG 2004 equations.
     *
     * Uses Newton-Raphson to find the density, so will be pretty cpu heavy
     * (depending on the starting point).
     *
     * \param pressure Gas pressure [P].
     * \param temperature Gas Temperature [K].
     * \param density Gas density [kg/m3].
     * \param aroidelta Output argument - left sum in eq. (7.21b) in TM15 (also appears in other equations).
     * \param arijdelta Output argument - the right (double) sum in eq. (7.21b) in TM15 (also appears in other equations).
     * \param aroideltadelta Output argument - the left sum in eq. (7.21c) in TM15 (also appears in other equations).
     * \param arijdeltadelta Output argument - the right (double) sum in eq. (7.21c) in TM15 (also appears in other equations).
     * \return The gas density [kg/m3] as well as many other factors via the output arguments.
     */
    double findDensity(
            const double pressure,
            const double temperature,
            double& density, // output
            double& aroidelta, // output
            double& arijdelta, // output
            double& aroideltadelta, // output
            double& arijdeltadelta // output
            ) const;

    /*!
     * \brief Internal (private) function used in the process of evaluating the GERG 2004 equations.
     * \param tred_temperature Inverse reduced mixture temperature.
     * \param start_rhored Reduced mixture density.
     * \param start_rhored_pow_minusOne pow(reduced mixture density, -1)
     * \param start_rhored_pow_minusTwo pow(reduced mixture density, -2)
     * \param aroidelta Output argument -- left sum in eq. (7.21b) in TM15 (also appears in other equations).
     * \param arijdelta Output argument -- the right (double) sum in eq. (7.21b) in TM15 (also appears in other equations).
     * \param aroideltadelta Output argument -- the left sum in eq. (7.21c) in TM15 (also appears in other equations).
     * \param arijdeltadelta Output argument -- the right (double) sum in eq. (7.21c) in TM15 (also appears in other equations).
     */
    void evaluateAlpha_roi_deltas(
            const double tred_temperature,
            const double start_rhored,
            const double start_rhored_pow_minusOne,
            const double start_rhored_pow_minusTwo,
            double& aroidelta, // output
            double& arijdelta, // output
            double& aroideltadelta, // output
            double& arijdeltadelta // output
            ) const;

    /*!
     * \brief Internal (private) function that updates the non-zero components.
     *
     * This function updates GERG04::m_indices, GERG04::m_firstIndices, and
     * GERG04::m_lastIndices according to the current composition stored in
     * GERG04::X.
     */
    void setNonZeroComponents();

    /*!
     * \brief Set the pressure and temperature independent coefficients.
     *
     * This calculates and sets the pressure and temperature independent factors
     * GERG04::rhored and GERG04::tred, which are stored for efficient'
     * computation.
     */
    void calculateCoefficients();

    const arma::uword N = 10; //!< Number of components
    arma::vec X; //!< Composition in order CH4, N2, CO2, C2H6, C3H8, nC4H10, iC4H10, nC5H12, iC5H12, nC6H14

    //! Critical temperature [K] for components CH4, N2, CO2, C2H6, C3H8, nC4H10, iC4H10, nC5H12, iC5H12, nC6H14
    arma::vec Tc =   {190.56,         126.19,         304.13,         305.32,         369.83,         425.13,         407.82,         469.7,          460.35,         507.82};
    //! Critical density [kg/m3] for components CH4, N2, CO2, C2H6, C3H8, nC4H10, iC4H10, nC5H12, iC5H12, nC6H14
    arma::vec rhoc = {10.14*16.04,    11.18*28.01,    10.62*44.01,    6.87*30.07,     5*44.1,         3.92*58.12,     3.86*58.12,     3.21*72.15,     3.27*72.15,     2.71*86.18};
    //                ch4,            n2,             c02,            c2h6,           c3h8,           nc4h10,         ic4h10,         nc5h12,         ic5h12          nc6h14

    /*!
     * \brief The indices of the non-zero gas fractions (components).
     *
     * \warning Note that the components are stored internally in GERG04 in a
     * different order than the input order and the usual order, so it's not
     * straight-forward to relate this to usual composition.
     */
    arma::uvec m_indices;

    /*!
     * \brief The indices of the two first non-zero gas fractions (components).
     *
     * \warning Note that the components are stored internally in GERG04 in a
     * different order than the input order and the usual order, so it's not
     * straight-forward to relate this to usual composition.
     */
    arma::uvec m_firstIndices;

    /*!
     * \brief The indices of the two last non-zero gas fractions (components).
     *
     * \warning Note that the components are stored internally in GERG04 in a
     * different order than the input order and the usual order, so it's not
     * straight-forward to relate this to usual composition.
     */
    arma::uvec m_lastIndices;

    static arma::mat betav; //!< \f$\beta_{v, ij}\f$ density interaction coefficient, from table A3.8
    static arma::mat betat; //!< \f$\beta_{T, ij}\f$ temperature coefficient, from table A3.8
    static arma::mat gammav; //!< \f$\gamma_{v, ij}\f$ density interaction coefficient, from table A3.8
    static arma::mat gammat; //!< \f$\gamma_{T, ij}\f$ temperature interaction coefficient, from table A3.8

    // Kpol coefficients - used in the first sum in alpha^r_{oi} and derivatives
    static arma::mat noipol; //!< \f$n_{oi, k}\f$, from Table A3.2 in TM15, used in the first sum of alpha^r_{oi}
    static arma::mat doipol; //!< \f$d_{oi, k}\f$, from Table A3.2 in TM15, used in the first sum of alpha^r_{oi}
    static arma::mat toipol; //!< \f$t_{oi, k}\f$, from Table A3.2 in TM15, used in the first sum of alpha^r_{oi}

    // Kexp coefficient - used in the second sum in alpha^r_{oi} and derivatives
    static arma::mat noiexp; //!< \f$n_{oi, k}\f$, from Table A3.3 in TM15, used in the second sum of alpha^r_{oi}
    static arma::mat doiexp; //!< \f$d_{oi, k}\f$, from Table A3.3 in TM15, used in the second sum of alpha^r_{oi}
    static arma::mat coiexp; //!< \f$c_{oi, k}\f$, from Table A3.3 in TM15, used in the second sum of alpha^r_{oi}
    static arma::mat toiexp; //!< \f$t_{oi, k}\f$, from Table A3.3 in TM15, used in the second sum of alpha^r_{oi}

    static arma::mat Fij; //!< \f$F_{ij}\f$ interaction coefficient

    static arma::cube nijpol; //!< \f$n_{ij, k}\f$, from Table A3.7 in TM15, used in the first sum of alpha^r_{ij}
    static arma::cube dijpol; //!< \f$d_{ij, k}\f$, from Table A3.7 in TM15, used in the first sum of alpha^r_{ij}
    static arma::cube tijpol; //!< \f$t_{ij, k}\f$, from Table A3.7 in TM15, used in the first sum of alpha^r_{ij}

    static arma::cube nijexp; //!< \f$n_{ij, k}\f$, from Table A3.7 in TM15, used in the second sum of alpha^r_{ij}
    static arma::cube dijexp; //!< \f$d_{ij, k}\f$, from Table A3.7 in TM15, used in the second sum of alpha^r_{ij}
    static arma::cube tijexp; //!< \f$t_{ij, k}\f$, from Table A3.7 in TM15, used in the second sum of alpha^r_{ij}

    static arma::cube nuijexp; //!< \f$\nu_{ij, k}\f$, from Table A3.7 in TM15, used in the second sum of alpha^r_{ij}
    static arma::cube epijexp; //!< \f$\epsilon_{ij, k}\f$, from Table A3.7 in TM15, used in the second sum of alpha^r_{ij}
    static arma::cube beijexp; //!< \f$\beta_{ij, k}\f$, from Table A3.7 in TM15, used in the second sum of alpha^r_{ij}
    static arma::cube gaijexp; //!< \f$\gamma_{ij, k}\f$, from Table A3.7 in TM15, used in the second sum of alpha^r_{ij}

    // other coefficients
    static arma::mat noik; //!< \f$n^o_{oi, k}\f$, from Table A3.1 in TM15, used in alpha^o_{oi}
    static arma::mat voik; //!< \f$n^o_{oi, k}\f$, from Table A3.1 in TM15, used in alpha^o_{oi}

    // constant terms
    static arma::cube nijpol_times_tijpol; //!< \f$n_{ij, k} \times t_{ij, k}\f$ (GERG04::nijpol x GERG04::tijpol)
    static arma::cube nijpol_times_tijpol_times_tijpol_minus_one; //!<  \f$(n_{ij, k} \times t_{ij, k}) \times (t_{ij, k} - 1)\f$ ((GERG04::nijpol x GERG04::tijpol) x (GERG04::tijpol - 1.0))
    static arma::cube nijexp_times_tijexp; //!< \f$n_{ij, k} \times t_{ij, k}\f$ (GERG04::nijexp x GERG04::tijexp)
    static arma::cube nijexp_times_tijexp_times_tijexp_minus_one; //!< \f$(n_{ij, k} \times t_{ij, k}) \times (t_{ij, k} - 1)\f$ ((GERG04::nijpol x GERG04::tijpol) x (GERG04::tijpol - 1.0))
    static arma::cube nijpol_times_dijpol; //!< \f$n_{ij, k} \times d_{ij, k}\f$ (GERG04::nijpol x GERG04::dijpol)
    static arma::cube nijpol_times_dijpol_times_tijpol; //!< \f$n_{ij, k} \times d_{ij, k} \times t_{ij, k}\f$ (GERG04::nijpol x GERG04::dijpol x GERG04::tijpol)
    static arma::cube nijpol_times_dijpol_times_dijpol_minus_one; //!< \f$n_{ij, k} \times d_{ij, k} \times(d_{ij, k} - 1) \f$ (GERG04::nijpol x GERG04::dijpol x (GERG04::dijpol - 1.0))

    double rhored; //!< Inverse reducing function for mixture density \f$1/\rho_r(\bar x)\f$.
    double tred; //!< Reducing function for mixture temperature \f$T_r(\bar x)\f$.

    double Ra; //!< Gas constant of mixture [J/mol K]
};