//------------------------------------------------------------------------------
// \file Functor_tests.cpp
//------------------------------------------------------------------------------
#include "Algebra/Categories/Functor.h"

#include <boost/test/unit_test.hpp>
#include <cmath>

using Categories::Functors::Details::object_map;
using std::cos;
using std::sin;
using std::sqrt;

BOOST_AUTO_TEST_SUITE(Categories)
BOOST_AUTO_TEST_SUITE(Functors)
BOOST_AUTO_TEST_SUITE(Functors_tests)

BOOST_AUTO_TEST_SUITE(Details)

BOOST_AUTO_TEST_SUITE(ObjectMap)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(MapAsTemplateParameter)
{
  const auto result = object_map<double, double, &sin>(M_PI_4);
  BOOST_TEST(result == 1.0 / sqrt(2.0));
}

BOOST_AUTO_TEST_SUITE_END() // ObjectMap

BOOST_AUTO_TEST_SUITE_END() // Details

BOOST_AUTO_TEST_SUITE_END() // Functors_tests
BOOST_AUTO_TEST_SUITE_END() // Functors
BOOST_AUTO_TEST_SUITE_END() // Categories


/**
 * @file tests/activation_functions_test.cpp
 * @author Marcus Edel
 * @author Dhawal Arora
 *
 * Tests for the various activation functions.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>

#include <mlpack/methods/ann/layer/layer.hpp>
#include <mlpack/methods/ann/activation_functions/logistic_function.hpp>
#include <mlpack/methods/ann/activation_functions/identity_function.hpp>
#include <mlpack/methods/ann/activation_functions/softsign_function.hpp>
#include <mlpack/methods/ann/activation_functions/tanh_function.hpp>
#include <mlpack/methods/ann/activation_functions/rectifier_function.hpp>
#include <mlpack/methods/ann/activation_functions/softplus_function.hpp>
#include <mlpack/methods/ann/activation_functions/swish_function.hpp>
#include <mlpack/methods/ann/activation_functions/hard_sigmoid_function.hpp>
#include <mlpack/methods/ann/activation_functions/mish_function.hpp>
#include <mlpack/methods/ann/activation_functions/lisht_function.hpp>
#include <mlpack/methods/ann/activation_functions/gelu_function.hpp>
#include <mlpack/methods/ann/activation_functions/elliot_function.hpp>
#include <mlpack/methods/ann/activation_functions/elish_function.hpp>
#include <mlpack/methods/ann/activation_functions/inverse_quadratic_function.hpp>
#include <mlpack/methods/ann/activation_functions/quadratic_function.hpp>
#include <mlpack/methods/ann/activation_functions/multi_quadratic_function.hpp>
#include <mlpack/methods/ann/activation_functions/spline_function.hpp>
#include <mlpack/methods/ann/activation_functions/poisson1_function.hpp>
#include <mlpack/methods/ann/activation_functions/gaussian_function.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack;
using namespace mlpack::ann;

BOOST_AUTO_TEST_SUITE(ActivationFunctionsTest);

// Generate dataset for activation function tests.
const arma::colvec activationData("-2 3.2 4.5 -100.2 1 -1 2 0");

/**
 * Implementation of the activation function test.
 *
 * @param input Input data used for evaluating the activation function.
 * @param target Target data used to evaluate the activation.
 *
 * @tparam ActivationFunction Activation function used for the check.
 */
template<class ActivationFunction>
void CheckActivationCorrect(const arma::colvec input,
                            const arma::colvec target)
{
  // Test the activation function using a single value as input.
  for (size_t i = 0; i < target.n_elem; ++i)
  {
    BOOST_REQUIRE_CLOSE(ActivationFunction::Fn(input.at(i)),
        target.at(i), 1e-3);
  }

  // Test the activation function using the entire vector as input.
  arma::colvec activations;
  ActivationFunction::Fn(input, activations);
  for (size_t i = 0; i < activations.n_elem; ++i)
  {
    BOOST_REQUIRE_CLOSE(activations.at(i), target.at(i), 1e-3);
  }
}

/**
 * Implementation of the activation function derivative test.
 *
 * @param input Input data used for evaluating the activation function.
 * @param target Target data used to evaluate the activation.
 *
 * @tparam ActivationFunction Activation function used for the check.
 */
template<class ActivationFunction>
void CheckDerivativeCorrect(const arma::colvec input,
                            const arma::colvec target)
{
  // Test the calculation of the derivatives using a single value as input.
  for (size_t i = 0; i < target.n_elem; ++i)
  {
    BOOST_REQUIRE_CLOSE(ActivationFunction::Deriv(input.at(i)),
        target.at(i), 1e-3);
  }

  // Test the calculation of the derivatives using the entire vector as input.
  arma::colvec derivatives;
  ActivationFunction::Deriv(input, derivatives);
  for (size_t i = 0; i < derivatives.n_elem; ++i)
  {
    BOOST_REQUIRE_CLOSE(derivatives.at(i), target.at(i), 1e-3);
  }
}

/**
 * Implementation of the activation function inverse test.
 *
 * @param input Input data used for evaluating the activation function.
 * @param target Target data used to evaluate the activation.
 *
 * @tparam ActivationFunction Activation function used for the check.
 */
template<class ActivationFunction>
void CheckInverseCorrect(const arma::colvec input)
{
    // Test the calculation of the inverse using a single value as input.
  for (size_t i = 0; i < input.n_elem; ++i)
  {
    BOOST_REQUIRE_CLOSE(ActivationFunction::Inv(ActivationFunction::Fn(
        input.at(i))), input.at(i), 1e-3);
  }

  // Test the calculation of the inverse using the entire vector as input.
  arma::colvec activations;
  ActivationFunction::Fn(input, activations);
  ActivationFunction::Inv(activations, activations);

  for (size_t i = 0; i < input.n_elem; ++i)
  {
    BOOST_REQUIRE_CLOSE(activations.at(i), input.at(i), 1e-3);
  }
}

/**
 * Implementation of the HardTanH activation function test. The function is
 * implemented as a HardTanH Layer in hard_tanh.hpp
 *
 * @param input Input data used for evaluating the HardTanH activation function.
 * @param target Target data used to evaluate the HardTanH activation.
 */
void CheckHardTanHActivationCorrect(const arma::colvec input,
                                    const arma::colvec target)
{
  HardTanH<> htf;

  // Test the activation function using the entire vector as input.
  arma::colvec activations;
  htf.Forward(input, activations);
  for (size_t i = 0; i < activations.n_elem; ++i)
  {
    BOOST_REQUIRE_CLOSE(activations.at(i), target.at(i), 1e-3);
  }
}

/**
 * Implementation of the HardTanH activation function derivative test. The
 * derivative is implemented as HardTanH Layer in hard_tanh.hpp
 *
 * @param input Input data used for evaluating the HardTanH activation
 * function.
 * @param target Target data used to evaluate the HardTanH activation.
 */
void CheckHardTanHDerivativeCorrect(const arma::colvec input,
                                    const arma::colvec target)
{
  HardTanH<> htf;

  // Test the calculation of the derivatives using the entire vector as input.
  arma::colvec derivatives;

  // This error vector will be set to 1 to get the derivatives.
  arma::colvec error = arma::ones<arma::colvec>(input.n_elem);
  htf.Backward(input, error, derivatives);

  for (size_t i = 0; i < derivatives.n_elem; ++i)
  {
    BOOST_REQUIRE_CLOSE(derivatives.at(i), target.at(i), 1e-3);
  }
}

/**
 * Implementation of the LeakyReLU activation function test. The function is
 * implemented as LeakyReLU layer in the file leaky_relu.hpp
 *
 * @param input Input data used for evaluating the LeakyReLU activation
 * function.
 * @param target Target data used to evaluate the LeakyReLU activation.
 */
void CheckLeakyReLUActivationCorrect(const arma::colvec input,
                                     const arma::colvec target)
{
  LeakyReLU<> lrf;

  // Test the activation function using the entire vector as input.
  arma::colvec activations;
  lrf.Forward(input, activations);
  for (size_t i = 0; i < activations.n_elem; ++i)
  {
    BOOST_REQUIRE_CLOSE(activations.at(i), target.at(i), 1e-3);
  }
}

/**
 * Implementation of the LeakyReLU activation function derivative test.
 * The derivative function is implemented as LeakyReLU layer in the file
 * leaky_relu_layer.hpp
 *
 * @param input Input data used for evaluating the LeakyReLU activation
 * function.
 * @param target Target data used to evaluate the LeakyReLU activation.
 */
void CheckLeakyReLUDerivativeCorrect(const arma::colvec input,
                                     const arma::colvec target)
{
  LeakyReLU<> lrf;

  // Test the calculation of the derivatives using the entire vector as input.
  arma::colvec derivatives;

  // This error vector will be set to 1 to get the derivatives.
  arma::colvec error = arma::ones<arma::colvec>(input.n_elem);
  lrf.Backward(input, error, derivatives);
  for (size_t i = 0; i < derivatives.n_elem; ++i)
  {
    BOOST_REQUIRE_CLOSE(derivatives.at(i), target.at(i), 1e-3);
  }
}

/**
 * Implementation of the ELU activation function test. The function is
 * implemented as ELU layer in the file elu.hpp
 *
 * @param input Input data used for evaluating the ELU activation function.
 * @param target Target data used to evaluate the ELU activation.
 */
void CheckELUActivationCorrect(const arma::colvec input,
                               const arma::colvec target)
{
  // Initialize ELU object with alpha = 1.0.
  ELU<> lrf(1.0);

  // Test the activation function using the entire vector as input.
  arma::colvec activations;
  lrf.Forward(input, activations);
  for (size_t i = 0; i < activations.n_elem; ++i)
  {
    BOOST_REQUIRE_CLOSE(activations.at(i), target.at(i), 1e-3);
  }
}

/**
 * Implementation of the ELU activation function derivative test. The function
 * is implemented as ELU layer in the file elu.hpp
 *
 * @param input Input data used for evaluating the ELU activation function.
 * @param target Target data used to evaluate the ELU activation.
 */
void CheckELUDerivativeCorrect(const arma::colvec input,
                               const arma::colvec target)
{
  // Initialize ELU object with alpha = 1.0.
  ELU<> lrf(1.0);

  // Test the calculation of the derivatives using the entire vector as input.
  arma::colvec derivatives, activations;

  // This error vector will be set to 1 to get the derivatives.
  arma::colvec error = arma::ones<arma::colvec>(input.n_elem);
  lrf.Forward(input, activations);
  lrf.Backward(activations, error, derivatives);
  for (size_t i = 0; i < derivatives.n_elem; ++i)
  {
    BOOST_REQUIRE_CLOSE(derivatives.at(i), target.at(i), 1e-3);
  }
}

/**
 * Implementation of the PReLU activation function test. The function
 * is implemented as PReLU layer in the file parametric_relu.hpp.
 *
 * @param input Input data used for evaluating the PReLU activation
 *   function.
 * @param target Target data used to evaluate the PReLU activation.
 */
void CheckPReLUActivationCorrect(const arma::colvec input,
                                 const arma::colvec target)
{
  PReLU<> prelu;

  // Test the activation function using the entire vector as input.
  arma::colvec activations;
  prelu.Forward(input, activations);
  for (size_t i = 0; i < activations.n_elem; ++i)
  {
    BOOST_REQUIRE_CLOSE(activations.at(i), target.at(i), 1e-3);
  }
}

/**
 * Implementation of the PReLU activation function derivative test.
 * The function is implemented as PReLU layer in the file
 * parametric_relu.hpp
 *
 * @param input Input data used for evaluating the PReLU activation
 *   function.
 * @param target Target data used to evaluate the PReLU activation.
 */
void CheckPReLUDerivativeCorrect(const arma::colvec input,
                                 const arma::colvec target)
{
  PReLU<> prelu;

  // Test the calculation of the derivatives using the entire vector as input.
  arma::colvec derivatives;

  // This error vector will be set to 1 to get the derivatives.
  arma::colvec error = arma::ones<arma::colvec>(input.n_elem);
  prelu.Backward(input, error, derivatives);
  for (size_t i = 0; i < derivatives.n_elem; ++i)
  {
    BOOST_REQUIRE_CLOSE(derivatives.at(i), target.at(i), 1e-3);
  }
}

/**
 * Implementation of the PReLU activation function gradient test.
 * The function is implemented as PReLU layer in the file
 * parametric_relu.hpp
 *
 * @param input Input data used for evaluating the PReLU activation
 *   function.
 * @param target Target data used to evaluate the PReLU gradient.
 */
void CheckPReLUGradientCorrect(const arma::colvec input,
                               const arma::colvec target)
{
  PReLU<> prelu;

  // Test the calculation of the derivatives using the entire vector as input.
  arma::colvec gradient;

  // This error vector will be set to 1 to get the gradient.
  arma::colvec error = arma::ones<arma::colvec>(input.n_elem);
  prelu.Gradient(input, error, gradient);
  BOOST_REQUIRE_EQUAL(gradient.n_rows, 1);
  BOOST_REQUIRE_EQUAL(gradient.n_cols, 1);
  BOOST_REQUIRE_CLOSE(gradient(0), target(0), 1e-3);
}

/**
 * Implementation of the Hard Shrink activation function test. The function is
 * implemented as Hard Shrink layer in the file hardshrink.hpp
 *
 * @param input Input data used for evaluating the Hard Shrink activation function.
 * @param target Target data used to evaluate the Hard Shrink activation.
 */
void CheckHardShrinkActivationCorrect(const arma::colvec input,
                                      const arma::colvec target)
{
  HardShrink<> hardshrink;

  // Test the activation function using the entire vector as input.
  arma::colvec activations;
  hardshrink.Forward(input, activations);
  for (size_t i = 0; i < activations.n_elem; ++i)
  {
    BOOST_REQUIRE_CLOSE(activations.at(i), target.at(i), 1e-3);
  }
}

/**
 * Implementation of the HardShrink activation function derivative test.
 * The derivative function is implemented as HardShrink layer in the file
 * hardshrink.hpp
 *
 * @param input Input data used for evaluating the HardShrink activation
 * function.
 * @param target Target data used to evaluate the HardShrink activation.
 */
void CheckHardShrinkDerivativeCorrect(const arma::colvec input,
                                      const arma::colvec target)
{
  HardShrink<> hardshrink;

  // Test the calculation of the derivatives using the entire vector as input.
  arma::colvec derivatives;

  // This error vector will be set to 1 to get the derivatives.
  arma::colvec error = arma::ones<arma::colvec>(input.n_elem);
  hardshrink.Backward(input, error, derivatives);
  for (size_t i = 0; i < derivatives.n_elem; ++i)
  {
    BOOST_REQUIRE_CLOSE(derivatives.at(i), target.at(i), 1e-3);
  }
}

/**
 * Implementation of the Soft Shrink activation function test. The function is
 * implemented as Soft Shrink layer in the file softshrink.hpp.
 *
 * @param input Input data used for evaluating the Soft Shrink activation
 * function.
 * @param target Target data used to evaluate the Soft Shrink activation.
 */
void CheckSoftShrinkActivationCorrect(const arma::colvec input,
                                      const arma::colvec target)
{
  SoftShrink<> softshrink;

  // Test the activation function using the entire vector as input.
  arma::colvec activations;
  softshrink.Forward(input, activations);
  for (size_t i = 0; i < activations.n_elem; ++i)
  {
    BOOST_REQUIRE_CLOSE(activations.at(i), target.at(i), 1e-3);
  }
}

/**
 * Implementation of the Soft Shrink activation function derivative test.
 * The derivative function is implemented as Soft Shrink layer in the file
 * softshrink.hpp
 *
 * @param input Input data used for evaluating the Soft Shrink activation
 * function.
 * @param target Target data used to evaluate the Soft Shrink activation.
 */
void CheckSoftShrinkDerivativeCorrect(const arma::colvec input,
                                      const arma::colvec target)
{
  SoftShrink<> softshrink;

  // Test the calculation of the derivatives using the entire vector as input.
  arma::colvec derivatives;

  // This error vector will be set to 1 to get the derivatives.
  arma::colvec error = arma::ones<arma::colvec>(input.n_elem);
  softshrink.Backward(input, error, derivatives);
  for (size_t i = 0; i < derivatives.n_elem; ++i)
  {
    BOOST_REQUIRE_CLOSE(derivatives.at(i), target.at(i), 1e-3);
  }
}

/**
 * Simple SELU activation test to check whether the mean and variance remain
 * invariant after passing normalized inputs through the function.
 */
BOOST_AUTO_TEST_CASE(SELUFunctionNormalizedTest)
{
  arma::mat input = arma::randn<arma::mat>(1000, 1);

  arma::mat output;

  SELU selu;

  selu.Forward(input, output);

  BOOST_REQUIRE_LE(arma::as_scalar(arma::abs(arma::mean(input) -
      arma::mean(output))), 0.1);

  BOOST_REQUIRE_LE(arma::as_scalar(arma::abs(arma::var(input) -
      arma::var(output))), 0.1);
}

/**
 * Simple SELU activation test to check whether the mean and variance
 * vary significantly after passing unnormalized inputs through the function.
 */
BOOST_AUTO_TEST_CASE(SELUFunctionUnnormalizedTest)
{
  const arma::colvec input("5.96402758 0.9966824 0.99975321 1 \
                            7.76159416 -0.76159416 0.96402758 8");

  arma::mat output;

  SELU selu;

  selu.Forward(input, output);

  BOOST_REQUIRE_GE(arma::as_scalar(arma::abs(arma::mean(input) -
      arma::mean(output))), 0.1);

  BOOST_REQUIRE_GE(arma::as_scalar(arma::abs(arma::var(input) -
      arma::var(output))), 0.1);
}

/**
 * Simple SELU derivative test to check whether the derivatives
 * produced by the activation function are correct.
 *
 */
BOOST_AUTO_TEST_CASE(SELUFunctionDerivativeTest)
{
  arma::mat input = arma::ones<arma::mat>(1000, 1);

  arma::mat error = arma::ones<arma::mat>(input.n_elem, 1);

  arma::mat derivatives, activations;

  SELU selu;

  selu.Forward(input, activations);
  selu.Backward(activations, error, derivatives);

  BOOST_REQUIRE_LE(arma::as_scalar(arma::abs(arma::mean(derivatives) -
      selu.Lambda())), 10e-4);

  input.fill(-1);

  selu.Forward(input, activations);
  selu.Backward(activations, error, derivatives);

  BOOST_REQUIRE_LE(arma::as_scalar(arma::abs(arma::mean(derivatives) -
      selu.Lambda() * selu.Alpha() - arma::mean(activations))), 10e-4);
}

/**
 * Implementation of the CELU activation function test. The function is
 * implemented as CELU layer in the file celu.hpp.
 *
 * @param input Input data used for evaluating the CELU activation function.
 * @param target Target data used to evaluate the CELU activation.
 */
void CheckCELUActivationCorrect(const arma::colvec input,
                                const arma::colvec target)
{
  // Initialize CELU object with alpha = 1.0.
  CELU<> lrf(1.0);

  // Test the activation function using the entire vector as input.
  arma::colvec activations;
  lrf.Forward(input, activations);
  for (size_t i = 0; i < activations.n_elem; ++i)
  {
    BOOST_REQUIRE_CLOSE(activations.at(i), target.at(i), 1e-3);
  }
}

/**
 * Implementation of the CELU activation function derivative test. The function
 * is implemented as CELU layer in the file celu.hpp.
 *
 * @param input Input data used for evaluating the CELU activation function.
 * @param target Target data used to evaluate the CELU activation.
 */
void CheckCELUDerivativeCorrect(const arma::colvec input,
                                const arma::colvec target)
{
  // Initialize CELU object with alpha = 1.0.
  CELU<> lrf(1.0);

  // Test the calculation of the derivatives using the entire vector as input.
  arma::colvec derivatives, activations;

  // This error vector will be set to 1 to get the derivatives.
  arma::colvec error = arma::ones<arma::colvec>(input.n_elem);
  lrf.Forward(input, activations);
  lrf.Backward(activations, error, derivatives);
  for (size_t i = 0; i < derivatives.n_elem; ++i)
  {
    BOOST_REQUIRE_CLOSE(derivatives.at(i), target.at(i), 1e-3);
  }
}

/**
 * Basic test of the tanh function.
 */
BOOST_AUTO_TEST_CASE(TanhFunctionTest)
{
  const arma::colvec desiredActivations("-0.96402758 0.9966824 0.99975321 -1 \
                                         0.76159416 -0.76159416 0.96402758 0");

  const arma::colvec desiredDerivatives("0.07065082 0.00662419 0.00049352 0 \
                                         0.41997434 0.41997434 0.07065082 1");

  CheckActivationCorrect<TanhFunction>(activationData, desiredActivations);
  CheckDerivativeCorrect<TanhFunction>(desiredActivations, desiredDerivatives);
  CheckInverseCorrect<TanhFunction>(desiredActivations);
}

/**
 * Basic test of the logistic function.
 */
BOOST_AUTO_TEST_CASE(LogisticFunctionTest)
{
  const arma::colvec desiredActivations("1.19202922e-01 9.60834277e-01 \
                                         9.89013057e-01 3.04574e-44 \
                                         7.31058579e-01 2.68941421e-01 \
                                         8.80797078e-01 0.5");

  const arma::colvec desiredDerivatives("0.10499359 0.03763177 0.01086623 \
                                         3.04574e-44 0.19661193 0.19661193 \
                                         0.10499359 0.25");

  CheckActivationCorrect<LogisticFunction>(activationData, desiredActivations);
  CheckDerivativeCorrect<LogisticFunction>(desiredActivations,
                                           desiredDerivatives);
  CheckInverseCorrect<LogisticFunction>(activationData);
}

/**
 * Basic test of the softsign function.
 */
BOOST_AUTO_TEST_CASE(SoftsignFunctionTest)
{
  const arma::colvec desiredActivations("-0.66666667 0.76190476 0.81818182 \
                                         -0.99011858 0.5 -0.5 0.66666667 0");

  const arma::colvec desiredDerivatives("0.11111111 0.05668934 0.03305785 \
                                         9.7642e-05 0.25 0.25 0.11111111 1");

  CheckActivationCorrect<SoftsignFunction>(activationData, desiredActivations);
  CheckDerivativeCorrect<SoftsignFunction>(desiredActivations,
                                           desiredDerivatives);
  CheckInverseCorrect<SoftsignFunction>(desiredActivations);
}

/**
 * Basic test of the identity function.
 */
BOOST_AUTO_TEST_CASE(IdentityFunctionTest)
{
  const arma::colvec desiredDerivatives = arma::ones<arma::colvec>(
      activationData.n_elem);

  CheckActivationCorrect<IdentityFunction>(activationData, activationData);
  CheckDerivativeCorrect<IdentityFunction>(activationData, desiredDerivatives);
}

/**
 * Basic test of the rectifier function.
 */
BOOST_AUTO_TEST_CASE(RectifierFunctionTest)
{
  const arma::colvec desiredActivations("0 3.2 4.5 0 1 0 2 0");

  const arma::colvec desiredDerivatives("0 1 1 0 1 0 1 0");

  CheckActivationCorrect<RectifierFunction>(activationData, desiredActivations);
  CheckDerivativeCorrect<RectifierFunction>(desiredActivations,
                                            desiredDerivatives);
}

/**
 * Basic test of the LeakyReLU function.
 */
BOOST_AUTO_TEST_CASE(LeakyReLUFunctionTest)
{
  const arma::colvec desiredActivations("-0.06 3.2 4.5 -3.006 \
                                         1 -0.03 2 0");

  const arma::colvec desiredDerivatives("0.03 1 1 0.03 \
                                         1 0.03 1 1");

  CheckLeakyReLUActivationCorrect(activationData, desiredActivations);
  CheckLeakyReLUDerivativeCorrect(desiredActivations, desiredDerivatives);
}

/**
 * Basic test of the HardTanH function.
 */
BOOST_AUTO_TEST_CASE(HardTanHFunctionTest)
{
  const arma::colvec desiredActivations("-1 1 1 -1 \
                                         1 -1 1 0");

  const arma::colvec desiredDerivatives("0 0 0 0 \
                                         1 1 0 1");

  CheckHardTanHActivationCorrect(activationData, desiredActivations);
  CheckHardTanHDerivativeCorrect(activationData, desiredDerivatives);
}

/**
 * Basic test of the ELU function.
 */
BOOST_AUTO_TEST_CASE(ELUFunctionTest)
{
  const arma::colvec desiredActivations("-0.86466471 3.2 4.5 -1.0 \
                                         1 -0.63212055 2 0");

  const arma::colvec desiredDerivatives("0.13533529 1 1 0 \
                                         1 0.36787945 1 1");

  CheckELUActivationCorrect(activationData, desiredActivations);
  CheckELUDerivativeCorrect(activationData, desiredDerivatives);
}

/**
 * Basic test of the softplus function.
 */
BOOST_AUTO_TEST_CASE(SoftplusFunctionTest)
{
  const arma::colvec activationData("-2 3.2 4.5 -100.2 1 -1 2 0 1000 10000");

  const arma::colvec desiredActivations("0.12692801 3.23995333 4.51104774 \
                                         0 1.31326168 0.31326168 2.12692801 \
                                         0.69314718 1000 10000");

  const arma::colvec desiredDerivatives("0.53168946 0.96231041 0.98913245 \
                                         0.5 0.78805844 0.57768119 0.89349302\
                                         0.66666666 1 1");

  CheckActivationCorrect<SoftplusFunction>(activationData, desiredActivations);
  CheckDerivativeCorrect<SoftplusFunction>(desiredActivations,
                                           desiredDerivatives);
  CheckInverseCorrect<SoftplusFunction>(desiredActivations);
}

/**
 * Basic test of the PReLU function.
 */
BOOST_AUTO_TEST_CASE(PReLUFunctionTest)
{
  const arma::colvec desiredActivations("-0.06 3.2 4.5 -3.006 \
                                         1 -0.03 2 0");

  const arma::colvec desiredDerivatives("0.03 1 1 0.03 \
                                         1 0.03 1 1");
  const arma::colvec desiredGradient("-103.2");

  CheckPReLUActivationCorrect(activationData, desiredActivations);
  CheckPReLUDerivativeCorrect(desiredActivations, desiredDerivatives);
  CheckPReLUGradientCorrect(activationData, desiredGradient);
}

/**
 * Basic test of the CReLU function.
 */
BOOST_AUTO_TEST_CASE(CReLUFunctionTest)
{
  const arma::colvec desiredActivations("0 3.2 4.5 0 \
                                         1 0 2 0 2 0 0 \
                                         100.2 0 1 0 0");

  const arma::colvec desiredDerivatives("0 0 0 0 \
                                         0 0 0 0");
  CReLU<> crelu;
  // Test the activation function using the entire vector as input.
  arma::colvec activations;
  crelu.Forward(activationData, activations);
  arma::colvec derivatives;
  // This error vector will be set to 1 to get the derivatives.
  arma::colvec error = arma::ones<arma::colvec>(desiredActivations.n_elem);
  crelu.Backward(desiredActivations, error, derivatives);
  for (size_t i = 0; i < activations.n_elem; ++i)
  {
    BOOST_REQUIRE_CLOSE(activations.at(i), desiredActivations.at(i), 1e-3);
  }
  for (size_t i = 0; i < derivatives.n_elem; ++i)
  {
    BOOST_REQUIRE_CLOSE(derivatives.at(i), desiredDerivatives.at(i), 1e-3);
  }
}

/**
 * Basic test of the swish function.
 */
BOOST_AUTO_TEST_CASE(SwishFunctionTest)
{
  // Hand-calculated values using Python interpreter.
  const arma::colvec desiredActivations("-0.238405 3.07466 4.45055 \
                                         -3.05183208657e-42 0.731058 -0.26894 \
                                         1.76159 0");

  const arma::colvec desiredDerivatives("0.3819171 1.0856295 1.039218 \
                                         0.5 0.83540367 0.3671335 1.073787\
                                         0.5");

  CheckActivationCorrect<SwishFunction>(activationData, desiredActivations);
  CheckDerivativeCorrect<SwishFunction>(desiredActivations,
                                        desiredDerivatives);
}

/**
 * Basic test of the hard sigmoid function.
 */
BOOST_AUTO_TEST_CASE(HardSigmoidFunctionTest)
{
  // Hand-calculated values using Python interpreter.
  const arma::colvec desiredActivations("0.1 1 1 \
                                         0 0.7 0.3 \
                                         0.9 0.5");

  const arma::colvec desiredDerivatives("0.2 0.0 0.0 \
                                         0.0 0.2 0.2 0.2\
                                         0.2");

  CheckActivationCorrect<HardSigmoidFunction>(activationData,
                                              desiredActivations);
  CheckDerivativeCorrect<HardSigmoidFunction>(desiredActivations,
                                              desiredDerivatives);
}

/**
 * Basic test of the Mish function.
 */
BOOST_AUTO_TEST_CASE(MishFunctionTest)
{
  // Calculated using tfa.activations.mish().
  // where tfa is tensorflow_addons.
  const arma::colvec desiredActivations("-0.25250152 3.1901977 \
                                         4.498914 -3.05183208e-42 0.86509836 \
                                         -0.30340138 1.943959 0");

  const arma::colvec desiredDerivatives("0.4382387  1.0159768849 \
                                         1.0019108 0.6 \
                                         1.0192586  0.40639898 \
                                         1.0725079  0.6");

  CheckActivationCorrect<MishFunction>(activationData,
                                       desiredActivations);
  CheckDerivativeCorrect<MishFunction>(desiredActivations,
                                       desiredDerivatives);
}

/**
 * Basic test of the LiSHT function.
 */
BOOST_AUTO_TEST_CASE(LiSHTFunctionTest)
{
  // Calculated using tfa.activations.LiSHT().
  // where tfa is tensorflow_addons.
  const arma::colvec desiredActivations("1.928055 3.189384 \
                                         4.4988894 100.2 0.7615942 \
                                         0.7615942 1.9280552 0");

  const arma::colvec desiredDerivatives("1.1150033 1.0181904 \
                                         1.001978 1.0 \
                                         1.0896928 1.0896928 \
                                         1.1150033 0.0");

  CheckActivationCorrect<LiSHTFunction>(activationData,
                                        desiredActivations);
  CheckDerivativeCorrect<LiSHTFunction>(desiredActivations,
                                        desiredDerivatives);
}

/**
 * Basic test of the GELU function.
 */
BOOST_AUTO_TEST_CASE(GELUFunctionTest)
{
  // Calculated using torch.nn.gelu().
  const arma::colvec desiredActivations("-0.0454023 3.1981304 \
                                         4.5 -0.0 0.84119199 \
                                         -0.158808 1.954597694 0.0");

  const arma::colvec desiredDerivatives("0.4637992 1.0065302 \
                                         1.0000293 0.5 1.03513446 \
                                         0.37435387 1.090984 0.5");

  CheckActivationCorrect<GELUFunction>(activationData,
                                       desiredActivations);
  CheckDerivativeCorrect<GELUFunction>(desiredActivations,
                                       desiredDerivatives);
}

/**
 * Basic test of the Hard Shrink function.
 */
BOOST_AUTO_TEST_CASE(HardShrinkFunctionTest)
{
  const arma::colvec desiredActivations("-2 3.2 4.5 -100.2 1 -1 2 0");

  const arma::colvec desiredDerivatives("1 1 1 1 1 1 1 0");

  CheckHardShrinkActivationCorrect(activationData,
                                   desiredActivations);
  CheckHardShrinkDerivativeCorrect(desiredActivations,
                                   desiredDerivatives);
}

/**
 * Basic test of the Elliot function.
 */
BOOST_AUTO_TEST_CASE(ElliotFunctionTest)
{
  // Calculated using PyTorch tensor.
  const arma::colvec desiredActivations("-0.66666667 0.76190476 0.81818182 \
                                         -0.99011858 0.5 -0.5 \
                                          0.66666667 0.0 ");

  const arma::colvec desiredDerivatives("0.36 0.32213294 0.3025 \
                                         0.25248879 0.44444444 \
                                         0.44444444 0.36 1.0 ");

  CheckActivationCorrect<ElliotFunction>(activationData,
                                         desiredActivations);
  CheckDerivativeCorrect<ElliotFunction>(desiredActivations,
                                         desiredDerivatives);
}

/**
 * Basic test of the EliSH function.
 */
BOOST_AUTO_TEST_CASE(ElishFunctionTest)
{
  // Manually-calculated using python-numpy module.
  const arma::colvec desiredActivations("-0.10307056 3.0746696 4.4505587 \
                                         -3.0457406e-44 0.731058578 \
                                         -0.1700034 1.76159415 0.0 ");

  const arma::colvec desiredDerivatives("0.4033889 1.0856292 \
                                         1.03921798 0.5 0.83540389 \
                                         0.34725726 1.07378804 0.5");

  CheckActivationCorrect<ElishFunction>(activationData,
                                        desiredActivations);
  CheckDerivativeCorrect<ElishFunction>(desiredActivations,
                                        desiredDerivatives);
}

/**
 * Basic test of the Soft Shrink function.
 */
BOOST_AUTO_TEST_CASE(SoftShrinkFunctionTest)
{
  const arma::colvec desiredActivations("-1.5 2.7 4 -99.7 0.5 -0.5 1.5 0");

  const arma::colvec desiredDerivatives("1 1 1 1 1 1 1 0");

  CheckSoftShrinkActivationCorrect(activationData,
                                   desiredActivations);
  CheckSoftShrinkDerivativeCorrect(desiredActivations,
                                   desiredDerivatives);
}

/**
 * Basic test of the CELU activation function.
 */
BOOST_AUTO_TEST_CASE(CELUFunctionTest)
{
  const arma::colvec desiredActivations("-0.86466472 3.2 4.5 \
                                         -1 1 -0.63212056 2 0");

  const arma::colvec desiredDerivatives("0.42119275 1 1 \
                                         0.36787944 1 \
                                         0.5314636 1 1");

  CheckCELUActivationCorrect(activationData, desiredActivations);
  CheckCELUDerivativeCorrect(desiredActivations, desiredDerivatives);
}

/**
 * Basic test of the inverse quadratic function.
 */
BOOST_AUTO_TEST_CASE(InverseQuadraticFunctionTest)
{
  // Hand-calculated values.
  const arma::colvec desiredActivations("0.2 0.088968 0.0470588 \
                                         9.95913e-05 0.5 0.5 \
                                         0.2 1");

  const arma::colvec desiredDerivatives("-0.369822 -0.175152 -0.0937021 \
                                         -0.000199183 -0.64 -0.64 -0.369822\
                                         -0.5");

  CheckActivationCorrect<InvQuadFunction>(activationData, desiredActivations);
  CheckDerivativeCorrect<InvQuadFunction>(desiredActivations,
                                          desiredDerivatives);
}

/**
 * Basic test of the quadratic function.
 */
BOOST_AUTO_TEST_CASE(QuadraticFunctionTest)
{
  // Hand-calculated values.
  const arma::colvec desiredActivations("4 10.24 20.25 \
                                         10040 1 1 \
                                         4 0");

  const arma::colvec desiredDerivatives("8 20.48 40.50 \
                                         20080 2 2 \
                                         8 0");

  CheckActivationCorrect<QuadraticFunction>(activationData, desiredActivations);
  CheckDerivativeCorrect<QuadraticFunction>(desiredActivations,
                                            desiredDerivatives);
}

/**
 * Basic test of the Spline function.
 */
BOOST_AUTO_TEST_CASE(SplineFunctionTest)
{
  const arma::colvec activationData1("2 3.2 4.5 100.2 1 1 2 0");

  // Hand-calculated values.
  const arma::colvec desiredActivations("4.39445 14.6953 34.5211 \
                                         46355.9 0.693147 0.693147 \
                                         4.39445 0");

  const arma::colvec desiredDerivatives("18.3923 94.6819 280.03866 \
                                         1042462.1078 1.0137702 1.0137702 \
                                         18.3923 0");

  CheckActivationCorrect<SplineFunction>(activationData1, desiredActivations);
  CheckDerivativeCorrect<SplineFunction>(desiredActivations,
                                         desiredDerivatives);
}

/**
 * Basic test of the multi quadratic function.
 */
BOOST_AUTO_TEST_CASE(MultiquadFunctionTest)
{
  // Hand-calculated values.
  const arma::colvec desiredActivations("2.23607 3.35261 4.60977 \
                                         100.205 1.41421 1.41421 \
                                         2.23607 1");

  const arma::colvec desiredDerivatives("0.912871 0.95828 0.97727 \
                                         0.99995 0.816496 0.816496 \
                                         0.912871 0.707107");

  CheckActivationCorrect<MultiQuadFunction>(activationData, desiredActivations);
  CheckDerivativeCorrect<MultiQuadFunction>(desiredActivations,
                                            desiredDerivatives);
}

/**
 * Basic test of the Poisson one function.
 */
BOOST_AUTO_TEST_CASE(Poisson1FunctionTest)
{
  const arma::colvec activationData1("-2 3.2 4.5 5 1 -1 2 0");

  // Hand-calculated values.
  const arma::colvec desiredActivations("-22.1672 0.0896768 0.0388815 \
                                         0.0269518 0 -5.43656 \
                                         0.135335 -1");

  const arma::colvec desiredDerivatives("1.02404e+11 1.74647 1.88633 \
                                         1.92058 2 1707.81 \
                                         1.62864 8.15485");

  CheckActivationCorrect<Poisson1Function>(activationData1, desiredActivations);
  CheckDerivativeCorrect<Poisson1Function>(desiredActivations,
                                           desiredDerivatives);
}

/**
 * Basic test of the Gaussian activation function.
 */
BOOST_AUTO_TEST_CASE(GaussianFunctionTest)
{
  const arma::colvec desiredActivations("0.018315639 0.000035713 \
                                         1.6052280551856116e-09 \
                                         0 0.367879441 0.367879441 \
                                         0.018315639 1");

  const arma::colvec desiredDerivatives("-0.036618991635992616 \
                                         -0.0000714259999 \
                                         -0.0000000032104561 \
                                         0 -0.6426287436 \
                                         -0.642628743680 \
                                         -0.03661899163 \
                                         -0.73575888234");

  CheckActivationCorrect<GaussianFunction>(activationData,
                                           desiredActivations);
  CheckDerivativeCorrect<GaussianFunction>(desiredActivations,
                                           desiredDerivatives);
}

BOOST_AUTO_TEST_SUITE_END();


//####### Test module for the overload of cmath functions ##################

//Define Module name
 #define BOOST_TEST_MODULE "math/cmath_overload"

#include <cmath>
#include <vector>

//Include Boost unit tests library & library for floating point comparison
#include <boost/test/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>

#define PXRMP_PREVENT_USE_STD_FOR_MATH
#include "cmath_overloads.hpp"

using namespace picsar::multi_physics::math;

// ------------- Tests --------------

// ***Test cmath overloads

template<typename RealType>
void test_case_cmath_overloads()
{
    const auto vals = std::vector<RealType>{1.0e-30, 1.0e-20, 1.0e-10, 1.0,
     3.141459, 1.0e10, 1.0e20, 1.0e30};

     for (const auto val : vals){
        BOOST_CHECK_EQUAL(m_sqrt(val), std::sqrt(val));
        BOOST_CHECK_EQUAL(m_cbrt(val), std::cbrt(val));
        BOOST_CHECK_EQUAL(m_exp(val), std::exp(val));
        BOOST_CHECK_EQUAL(m_exp(-val), std::exp(-val));
        BOOST_CHECK_EQUAL(m_log(val), std::log(val));
        BOOST_CHECK_EQUAL(m_tanh(val), std::tanh(val));
        BOOST_CHECK_EQUAL(m_tanh(-val), std::tanh(-val));
     }

    const auto vals_floor_fabs = std::vector<RealType>{0.0, 0.001, 0.3, 0.7,
         1.11, 1.5, 1.55, 20.9, 100.56, 1000.24};

    for (const auto val : vals_floor_fabs){
        BOOST_CHECK_EQUAL(m_floor(val), std::floor(val));
        BOOST_CHECK_EQUAL(m_floor(-val), std::floor(-val));

        BOOST_CHECK_EQUAL(m_fabs(val), std::fabs(val));
        BOOST_CHECK_EQUAL(m_fabs(-val), std::fabs(-val));
    }
}

BOOST_AUTO_TEST_CASE( picsar_cmath_overloads )
{
    test_case_cmath_overloads<double>();
    test_case_cmath_overloads<float>();
}

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


//
// Created by Vlad Argunov on 03/01/2022.
//

#include "Simulation.h"

#include <Eigen/Dense>
#include <random>
#include <cmath>
#include <tuple>

Eigen::VectorXd Simulation::generate_uniform_variable(int n) {
        std::random_device rd;
        std::mt19937 gen(rd());
        Eigen::VectorXd uniform_numbers;
        uniform_numbers.resize(n);
        std::uniform_real_distribution<double> uniform(0.0, 1.0);
        for (int i = 0; i < n; ++i) {
            uniform_numbers(i) = uniform(gen);
        }
        return uniform_numbers;
}

Eigen::VectorXd Simulation::generate_normal_variable(int n, double mu, double sigma) {
    std::random_device rd;
    std::mt19937 gen(rd());
    Eigen::VectorXd normal_numbers;
    normal_numbers.resize(n);
    std::normal_distribution<> normal{mu,sigma};
    for (int i = 0; i < n; ++i) {
        normal_numbers(i) = normal(gen);
    }
    return normal_numbers;
}

Eigen::MatrixXd Simulation::generate_gbm(int n_processes, double tn, double stock_price, double mu, double sigma, int n_steps) {
//    Generates the matrix of Geometric brownian motions for each row and a time step for each column
    double time_step = tn / n_steps;
    Eigen::MatrixXd simulated_processes;
    Eigen::Index n_rows = n_processes;
    Eigen::Index n_cols = n_steps + 1;
    simulated_processes.resize(n_rows, n_cols);
    simulated_processes.leftCols(1) = Eigen::MatrixXd::Constant(n_rows, 1, stock_price);

    Eigen::VectorXd normal_rv;
    normal_rv.resize(n_processes);
    for (int col = 0; col < n_cols - 1; ++col) {
        normal_rv = generate_normal_variable(n_processes, 0, 1);
        for (int row = 0; row < n_rows; ++row) {
            simulated_processes(row, col + 1) = simulated_processes(row, col)
                    * exp(mu - 0.5 * sigma * sigma * time_step + sigma * sqrt(time_step) * normal_rv(row) );
        }

    }

    return simulated_processes;
}

std::tuple<Eigen::MatrixXd , Eigen::MatrixXd>
Simulation::generate_heston_process(int n_processes, double tn, double stock_price, double mu, double variance,
                                    double kappa, double theta, double sigma, double rho, int n_steps) {
    //    Generates the matrix of Heston processes for each row and a time step for each column
    // Here variance is the starting value_matrix of the volatility, theta is the long-term level,
    // kappa is the speed of convergence, and sigma is the volatility of volatility
    double time_step = tn / n_steps;
    Eigen::MatrixXd simulated_stock_processes, simulated_var_process;
    Eigen::Index n_rows = n_processes;
    Eigen::Index n_cols = n_steps + 1;

    simulated_stock_processes.resize(n_rows, n_cols);
    simulated_var_process.resize(n_rows, n_cols);

    simulated_stock_processes.leftCols(1) = Eigen::MatrixXd::Constant(n_rows, 1, stock_price);
    simulated_var_process.leftCols(1) = Eigen::MatrixXd::Constant(n_rows, 1, variance);

    Eigen::VectorXd normal_rv1, normal_rv2;
    normal_rv1.resize(n_processes);
    normal_rv2.resize(n_processes);
    for (int col = 0; col < n_cols - 1; ++col) {
        normal_rv1 = generate_normal_variable(n_processes, 0, 1);
        for (int row = 0; row < n_rows; ++row) {
            simulated_var_process(row, col + 1) = simulated_var_process(row, col) +
                                                  kappa * (theta - simulated_var_process(row, col)) * time_step
                                                  + sigma * sqrt(simulated_var_process(row, col) * time_step)
                                                  * (rho * normal_rv1(row) + sqrt(1 - rho * rho) * normal_rv2(row));

            simulated_stock_processes(row, col + 1) = simulated_stock_processes(row, col) * (1 + mu * time_step)
                    + simulated_stock_processes(row, col) * sqrt(simulated_var_process(row, col) * time_step) * normal_rv1(row);
        }

    }
    return {simulated_stock_processes, simulated_var_process};
}


#include <iostream>

#include <Eigen/Dense>

#include "LSLOpt/BFGS.hpp"

struct Parabola {
    double value(const Eigen::VectorXd& x)
    {
      return x.dot(x);
    }

    Eigen::VectorXd gradient(const Eigen::VectorXd& x)
    {
      return 2 * x;
    }

    double initial_step_length(const Eigen::VectorXd& x, const Eigen::VectorXd& p)
    {
      double max_change = p.array().abs().maxCoeff();

      return 0.2 / max_change;
    }

    double change_acceptable(const Eigen::VectorXd& x, const Eigen::VectorXd& xp) {
      return (xp - x).array().abs().maxCoeff() - (0.2 + 1e-6);
    }
};

int main()
{
  Parabola parabola;
  Eigen::VectorXd x0 = Eigen::VectorXd::Constant(2, 1.0);
  LSLOpt::OptimizationParameters<double> params
      = LSLOpt::getOptimizationParameters<double>();
  LSLOpt::OstreamOutput output{LSLOpt::OutputLevel::Status, std::cerr};

  auto result = LSLOpt::lsl_bfgs(parabola, x0, params, output);

  std::cout << "STATUS " << result.status << std::endl;
  std::cout << "F      " << result.function_value << std::endl;
  std::cout << "X      " << result.x << std::endl;

  return 0;
}


#include "NMatrixOperations.h"

#include <Eigen/Dense>
#include <stdexcept>
#include <assert.h>
#include <string.h>
#include <vector>
#include <QDebug>

#define print(something) {std::stringstream ss; ss << something; qDebug() << ss.str().c_str(); }

// This was based on https://ef.gy/linear-algebra:normal-vectors-in-higher-dimensional-spaces
Eigen::VectorXd getNormalVector(Eigen::MatrixXd vectors) {
	assert(vectors.rows() == vectors.cols() + 1);
	const unsigned int N = vectors.rows();

	Eigen::MatrixXd pM = vectors.transpose();
	pM.conservativeResize(pM.rows() + 1, Eigen::NoChange);
	pM.row(N - 1) = Eigen::VectorXd::Zero(N);
	Eigen::MatrixXd baseVectors = Eigen::MatrixXd::Identity(N, N);

	Eigen::VectorXd result = Eigen::VectorXd::Zero(N);

	int signal = 1;
	for (unsigned int i = 0; i < N; i++) {
		Eigen::MatrixXd pS(N - 1, N - 1);

		for (unsigned int j = 0; j < (N - 1); j++) {
			pS.block(j, 0, 1, i) = pM.block(j, 0, 1, i);
			pS.block(j, i, 1, N - i - 1) = pM.block(j, i + 1, 1, N - i - 1);
		}

		result += signal * baseVectors.row(i) * pS.determinant();
		signal *= -1;
	}

	return result;
}

Eigen::MatrixXd translateMatrixN(int N, Eigen::VectorXd point) {
	assert(N > 0);

	Eigen::MatrixXd m = Eigen::MatrixXd::Identity(N + 1, N + 1);
	m.topRightCorner(N, 1) = point;
	return m;
}

// This was based on https://ef.gy/linear-algebra:perspective-projections
// However, he makes this in a different way than every other LookAt I could find
// Therefor it has been silightly adjusted to conform to the others
Eigen::MatrixXd lookAtMatrixN(const int N, Eigen::VectorXd from, Eigen::VectorXd to, Eigen::MatrixXd ups) {
	assert(N > 2);

	//print("LookAt Double Calculation:");
	//print("FROM:");
	//print(from.transpose());
	//print("TO:");
	//print(to.transpose());
	//print("UPS");
	//print(ups);

	Eigen::MatrixXd m(N, N);

	m.rightCols(1) = (to - from).normalized();

	int numLoops = 0;
	for (int currentColumn = N - 2; currentColumn > 0; currentColumn--) {
		Eigen::MatrixXd vectorsToCross(N, N - 1);
		int currentColumnOnVectorsToCross = 1;

		//First, cross product all ups, in order
		vectorsToCross.col(0) = ups.col(numLoops);
		for (int i = 1; i < currentColumn; i++) {
			vectorsToCross.col(currentColumnOnVectorsToCross) = ups.col(numLoops + i);
			currentColumnOnVectorsToCross++;
		}

		numLoops++;
		for (int i = 0; i < numLoops; i++) {
			vectorsToCross.col(currentColumnOnVectorsToCross) = m.col(currentColumn + i + 1);
			currentColumnOnVectorsToCross++;
		}

		//print("vectorsToCross\n" << vectorsToCross << "\n");

		auto normal = getNormalVector(vectorsToCross);
		//print("Normal:\n" << normal << "\n");
		//print("Normal Normalized:\n" << normal.normalized() << "\n");

		m.col(currentColumn) = normal.normalized();

		//print("Current Matrix:\n" << m << "\n");
	}

	m.col(0) = getNormalVector(m.rightCols(N - 1)).normalized();

	//print("m");
	//print(m);

	Eigen::MatrixXd temp = Eigen::MatrixXd::Identity(N + 1, N + 1);
	//m.leftCols(N - 1).rowwise().reverseInPlace();
	temp.topLeftCorner(N, N) = m;

	return temp.transpose();
}

Eigen::MatrixXd perspectiveMatrixN(const int N, double eye_radians_angle, double nearPlane, double farPlane, double aspectRatio) {
	assert(N > 2);

	if (N == 3) {
		Eigen::MatrixXd m = Eigen::MatrixXd::Zero(N + 1, N + 1);

		double f_tan = 1 / tan(eye_radians_angle / 2);
		m(0, 0) = f_tan / aspectRatio;
		m(1, 1) = f_tan;
		m(2, 2) = (nearPlane + farPlane) / (nearPlane - farPlane);
		m(2, 3) = -1.f;
		m(3, 2) = 2 * (nearPlane*farPlane) / (nearPlane - farPlane);

		return m.transpose();
	}
	else {
		Eigen::MatrixXd m = Eigen::MatrixXd::Identity(N + 1, N + 1);

		double f_tan = 1 / tan(eye_radians_angle / 2);
		m = m*f_tan;
		m(N - 1, N - 1) = 1;
		m(N, N) = 1;

		return m;
	}
}

// N > 3
Eigen::MatrixXd viewMatrixN(
	const int N,
	Eigen::VectorXd from,
	Eigen::VectorXd to,
	Eigen::MatrixXd ups,
	double eyeRadiansAngle,
	double nearPlane,
	double farPlane,
	double aspectRatio) {

	auto tr = translateMatrixN(N, -from);

	auto la = lookAtMatrixN(N, from, to, ups);

	auto pm = perspectiveMatrixN(N, eyeRadiansAngle, nearPlane, farPlane, aspectRatio);

	Eigen::MatrixXd result = pm * la * tr;

	//Axis direction correction
	Eigen::MatrixXd aux = Eigen::MatrixXd::Identity(N + 1, N + 1);
	//X correction
	aux(0, 0) = -1;
	//Z correction
	aux(2, 2) = N != 4 ? -1 : 1;
	result = aux * result;

	//Z correction

	return result;
}

//This assumes that point is an N+1 dimensional vector and that point(N) == 1
Eigen::VectorXd projectPointLosingDimension(Eigen::VectorXd point, Eigen::MatrixXd m) {
	//std::cout << "point\n" << point << "\n";

	Eigen::VectorXd pointWith1(point.rows() + 1);
	pointWith1.topRightCorner(point.rows(), 1) = point;
	pointWith1(point.rows()) = 1;

	//std::cout << "pointWith1\n" << pointWith1 << "\n";

	Eigen::VectorXd v = m * pointWith1;
	//std::cout << "v\n" << v << "\n";
	v = v / v(v.rows() - 2, 0);
	//std::cout << "v\n" << v << "\n";
	auto result = v.topLeftCorner(v.rows() - 1, 1);
	//std::cout << "result\n" << result << "\n";

	return result;
}

//This assumes that each point is an N+1 dimensional column in a matrix and that point(N) == 1
Eigen::MatrixXd projectPointsLosingDimension(Eigen::MatrixXd points, Eigen::MatrixXd m, bool usePerspective) {
	//std::cout << "point\n" << points << "\n";

	Eigen::MatrixXd pointWith1(points.rows() + 1, points.cols());
	pointWith1.topRightCorner(points.rows(), points.cols()) = points;
	pointWith1.row(points.rows()) = Eigen::VectorXd::Ones(points.cols());

	//std::cout << "pointWith1\n" << pointWith1 << "\n";

	Eigen::MatrixXd v = m * pointWith1;
	//std::cout << "v\n" << v << "\n";

	if (usePerspective) {
		for (int i = 0; i < v.cols(); i++) {
			if (v.col(i)(v.rows() - 2, 0) == 0 || v.col(i)(v.rows() - 2, 0) == -0) {
				v.col(i) = v.col(i) / (0.000001);
			} else {
				v.col(i) = v.col(i) / v.col(i)(v.rows() - 2, 0);
			}
		}
	}else{
		for (int i = 0; i < v.cols(); i++) {
			if (v.col(i)(v.rows() - 1, 0) == 0 || v.col(i)(v.rows() - 1, 0) == -0) {
				v.col(i) = v.col(i) / (0.000001);
			}
			else {
				v.col(i) = v.col(i) / v.col(i)(v.rows() - 1, 0);
			}
		}
	}

	Eigen::MatrixXd result = v.topRows(v.rows() - 2);

	return result;
}

Eigen::MatrixXd rotateMatrixN(const unsigned int N, unsigned int axis1, unsigned int axis2, double radians_angle) {
	assert(axis1 != axis2);

	Eigen::MatrixXd rot_aa = Eigen::MatrixXd::Identity(N, N);

	rot_aa(axis1, axis1) = cos(radians_angle);
	rot_aa(axis1, axis2) = sin(radians_angle);
	rot_aa(axis2, axis1) = -sin(radians_angle);
	rot_aa(axis2, axis2) = cos(radians_angle);

	return rot_aa.transpose();
}

std::string generateNDimensionalShader(int n) {
	std::stringstream ss;
	ss << "#version 330 core\nlayout (location = 0) in double[" << n << "] position;\n";

	for (int i = n; i >= 3; i--) {
		ss << "uniform double m" << i << "[" << ((i + 1) * (i + 1)) << "];\n";
	}

	ss << "\nvoid main() {\n";

	ss << "	double[" << (n + 1) << "] positionWithOne;\n";
	ss << "	for (int i = 0; i < " << n << "; i++){\n";
	ss << "		positionWithOne[i] = position[i];\n";
	ss << "	}\n";
	ss << "	positionWithOne[" << n << "] = 1;\n\n";

	ss << "	double[" << (n + 1) << "] newPos" << n << ";\n";

	for (int i = n; i >= 3; i--) {
		ss << "	for (int i = 0; i <= " << i << "; i++) {\n";
		ss << "		double newVal = 0;\n";
		ss << "		for (int j = 0; j <= " << i << "; j++){\n";
		ss << "			newVal += m" << i << "[j * " << i << " + i] * positionWithOne[j];\n";
		ss << "		}\n";
		ss << "		newPos" << i << "[i] = newVal;\n";
		ss << "	}\n";
		ss << "	\n";
		if (i != 3) {
			ss << "	for (int i = 0; i < " << i << "; i++) {\n";
			ss << "		positionWithOne[i] = newPos" << i << "[i] / newPos" << i << "[" << (i - 1) << "];\n";
			ss << "	}\n";
			ss << "	\n";
			ss << "	double[" << i << "] newPos" << i - 1 << ";\n";
		}
	}

	ss << "	gl_Position.x = newPos3[0]/newPos3[3];\n";
	ss << "	gl_Position.y = newPos3[1]/newPos3[3];\n";
	ss << "	gl_Position.z = newPos3[2]/newPos3[3];\n";
	ss << "	gl_Position.w = newPos3[3]/newPos3[3];\n";
	ss << "}\n";

	std::string str = ss.str();
	return str;
}

Eigen::MatrixXd makeVectorsHomogeneous(Eigen::MatrixXd points) {
	points.conservativeResize(points.rows() + 1, Eigen::NoChange);
	points.row(points.rows() - 1) = Eigen::VectorXd::Ones(points.cols());

	return points;
}

Eigen::MatrixXd makeMatrixHomogeneous(Eigen::MatrixXd matrix) {
	matrix.conservativeResize(matrix.rows() + 1, matrix.cols() + 1);
	matrix.row(matrix.rows() - 1) = Eigen::VectorXd::Zero(matrix.cols());
	matrix.col(matrix.cols() - 1) = Eigen::VectorXd::Zero(matrix.rows());
	matrix(matrix.rows() - 1, matrix.cols() - 1) = 1;

	return matrix;
}

#define print(something) {std::stringstream ss; ss << something; qDebug() << ss.str().c_str(); }

Eigen::MatrixXd rotateShapeToLowerDimension(Eigen::MatrixXd m) {
	const int D = m.rows();

	// First we move the first vertex of the shape to the origin
	m = translateMatrixN(D, -m.col(0).cast<double>()).cast<double>() * makeVectorsHomogeneous(m);
	m.conservativeResize(m.rows() - 1, Eigen::NoChange);

	//Then, we find a new base created from the points of the polytope
	//And also a vector that is simply the last dimension
	Eigen::MatrixXd newBase = Eigen::MatrixXd::Zero(D, D);
	int currentSizeBase = 1;;
	{
		Eigen::VectorXd aux = Eigen::VectorXd::Zero(D);
		aux(D - 1) = 1;
		newBase.col(0) = aux;

		for (int i = 0; i < m.cols() && currentSizeBase < 2; i++) {
			for (int j = i + 1; j < m.cols() && currentSizeBase < 2; j++) {
				Eigen::VectorXd newVectorOfBase = m.col(j) - m.col(i);
				newVectorOfBase.normalize();
				if (newVectorOfBase(0) < 0) {
					newVectorOfBase = newVectorOfBase * -1;
				}

				if (newVectorOfBase != newBase.col(0)) {
					newBase.col(currentSizeBase) = newVectorOfBase;
					currentSizeBase++;
				}

			}
		}
	}

	for (int i = 0; i < m.cols() && currentSizeBase < D; i++) {
		for (int j = i + 1; j < m.cols() && currentSizeBase < D; j++) {
			Eigen::VectorXd newVectorOfBase = m.col(j) - m.col(i);
			newVectorOfBase.normalize();
			if (newVectorOfBase(0) < 0) {
				newVectorOfBase = newVectorOfBase * -1;
			}

			Eigen::VectorXd x = newBase.colPivHouseholderQr().solve(newVectorOfBase);

			Eigen::VectorXd check = (newBase * x) - newVectorOfBase;
			double error = 0;
			for (int k = 0; k < x.size(); k++) {
				error += check(k) * check(k);
			}

			if (error > 1e-5f) {
				newBase.col(currentSizeBase) = newVectorOfBase;
				currentSizeBase++;
			}
		}
	}

	if (currentSizeBase != newBase.cols()) {
		for (int i = 0; i < newBase.rows(); i++) {
			if (newBase.row(i).isZero(1e-10)) {
				m.row(i) = m.row(m.rows() - 1);
				return m.topRows(D - 1);
			}
		}
	} else {
		//We set the coordinate we want to exclude as the last one
		for (int i = 0; i < D - 1; i++) {
			Eigen::VectorXd aux = newBase.col(i);
			newBase.col(i) = newBase.col(i + 1);
			newBase.col(i + 1) = aux;
		}

		//We change the base
		Eigen::MatrixXd transformationMatrix = newBase.inverse();
		Eigen::MatrixXd newVertices = transformationMatrix * m;

		//Return only the top rows, excluding the last one
		return newVertices.topRows(D - 1);
	}

	return Eigen::MatrixXd(0, 0);
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_CONSTANT_CONSTANTS_GOLD_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_CONSTANTS_GOLD_HPP_INCLUDED

#include <boost/simd/include/functor.hpp>
#include <boost/simd/constant/register.hpp>
#include <boost/simd/constant/hierarchy.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
   /*!
     @brief Gold generic tag

     Represents the Gold constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    BOOST_SIMD_CONSTANT_REGISTER( Gold,double,1
                                , 0x3FCF1BBD,0x3FF9E3779B97F4A8ULL
                                )
  }
  namespace ext
  {
   template<class Site, class... Ts>
   BOOST_FORCEINLINE generic_dispatcher<tag::Gold, Site> dispatching_Gold(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
   {
     return generic_dispatcher<tag::Gold, Site>();
   }
   template<class... Args>
   struct impl_Gold;
  }
  /*!
    Generates the golden ratio that is \f$\phi = \frac{1+\sqrt5}{2}\f$

    @par Semantic:

    @code
    T r = Gold<T>();
    @endcode

    is similar for floating types to:

    @code
    T r = (1+sqrt(5))/2;
    @endcode

  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Gold, Gold)
} }

#include <boost/simd/constant/common.hpp>

#endif


#include "math_unit_test.hpp"
#include <boost/math/fft/multiprecision_complex.hpp>
#include <boost/math/fft/bsl_backend.hpp>
#if defined(__GNUC__)
#include <boost/math/fft/fftw_backend.hpp>
#include <boost/math/fft/gsl_backend.hpp>
#endif
#include <boost/math/fft/algorithms.hpp>
#include <boost/math/constants/constants.hpp>
#ifdef BOOST_MATH_USE_FLOAT128
#include <boost/multiprecision/complex128.hpp>
#endif
#include <boost/multiprecision/cpp_complex.hpp>
// TODO:
//#include <boost/multiprecision/mpfr.hpp>
//#include <boost/multiprecision/mpc.hpp>
#include <boost/random.hpp>

#include "fft_test_helpers.hpp"

#include <type_traits>
#include <vector>
#include <limits>

using namespace boost::math::fft;

template<class T>
void convolution_brute_force(
  const T* first1, const T* last1,
  const T* first2,
  T* out)
{
  long N = std::distance(first1,last1);
  for(long i=0;i<N;++i)
  {
    T sum{0};
    for(int j=0;j<N;++j)
    {
      sum += first1[j] * first2[(i-j+N) % N];
    }
    out[i] = sum;
  }
}

template<class T>
void dft_forward_bruteForce(
  const T* in_beg, const T* in_end,
  T* out)
{
  ::boost::math::fft::detail::complex_dft_prime_bruteForce(
    in_beg,in_end,out,
    1,
    std::allocator<T>{});
}

template<template<class ...Args> class backend_t, class T>
void test_directly(unsigned int N, int tolerance)
{
  using Complex = boost::multiprecision::complex<T>;
  const T tol = tolerance*std::numeric_limits<T>::epsilon();

  // ...
  boost::random::mt19937 rng;
  boost::random::uniform_real_distribution<T> U(0.0,1.0);
  {
    std::vector<Complex> A(N),B(N),C(N);

    for(auto& x: A)
    {
        x.real( U(rng) );
        x.imag( U(rng) );
    }
    backend_t<Complex> plan(N);
    plan.forward(A.begin(),A.end(),B.begin());
    dft_forward_bruteForce(A.data(),A.data()+N,C.data());

    T diff{0.0};

    for(size_t i=0;i<N;++i)
    {
        using std::norm;
        diff += norm(B[i]-C[i]);
    }
    using std::sqrt;
    diff = sqrt(diff)/N;
    CHECK_MOLLIFIED_CLOSE(T{0.0},diff,tol);
  }
}

template<class backend_t>
void test_convolution(unsigned int N, int tolerance)
{
  using Complex = typename backend_t::value_type;
  using T = typename Complex::value_type;

  // using Complex = boost::multiprecision::complex<T>;
  const T tol = tolerance*std::numeric_limits<T>::epsilon();

  // ...
  boost::random::mt19937 rng;
  boost::random::uniform_real_distribution<T> U(0.0,1.0);
  {
    std::vector<Complex> A(N),B(N),C(N);

    for(auto& x: A)
    {
        x.real( U(rng) );
        x.imag( U(rng) );
    }
    for(auto& x: B)
    {
        x.real( U(rng) );
        x.imag( U(rng) );
    }
    convolution_brute_force(A.data(),A.data()+N,B.data(),C.data());

    std::vector<Complex> C_candidate;
    transform<backend_t>::convolution(A.begin(),A.end(),B.begin(),std::back_inserter(C_candidate));

    T diff{0.0};

    for(size_t i=0;i<N;++i)
    {
        using std::norm;
        diff += norm(C[i]-C_candidate[i]);
    }
    using std::sqrt;
    diff = sqrt(diff)/N;
    CHECK_MOLLIFIED_CLOSE(T{0.0},diff,tol);
  }
}

template<class Backend>
void test_fixed_transforms(int tolerance)
{
  // using Complex = boost::multiprecision::complex<T>;
  using Complex = typename Backend::value_type;
  using real_value_type = typename Complex::value_type;
  const real_value_type tol = tolerance*std::numeric_limits<real_value_type>::epsilon();
  {
    std::vector< Complex > A{1.0},B(1);
    Backend plan(A.size());
    plan.forward(A.data(),A.data()+A.size(),B.data());
    CHECK_MOLLIFIED_CLOSE(real_value_type{1.0},B[0].real(),0);
    CHECK_MOLLIFIED_CLOSE(real_value_type{0.0},B[0].imag(),0);
  }
  {
    std::vector< Complex > A{1.0,1.0},B(2);
    Backend plan(A.size());
    plan.forward(A.data(),A.data()+A.size(),B.data());
    CHECK_MOLLIFIED_CLOSE(real_value_type{2.0},B[0].real(),tol);
    CHECK_MOLLIFIED_CLOSE(real_value_type{0.0},B[0].imag(),tol);

    CHECK_MOLLIFIED_CLOSE(real_value_type{0.0},B[1].real(),tol);
    CHECK_MOLLIFIED_CLOSE(real_value_type{0.0},B[1].imag(),tol);
  }
  {
    std::vector< Complex > A{1.0,1.0,1.0},B(3);
    Backend plan(A.size());
    plan.forward(A.data(),A.data()+A.size(),B.data());
    CHECK_MOLLIFIED_CLOSE(real_value_type{3.0},B[0].real(),tol);
    CHECK_MOLLIFIED_CLOSE(real_value_type{0.0},B[0].imag(),tol);

    CHECK_MOLLIFIED_CLOSE(
        real_value_type{0.0},B[1].real(),tol);
    CHECK_MOLLIFIED_CLOSE(
        real_value_type{0.0},B[1].imag(),tol);

    CHECK_MOLLIFIED_CLOSE(
        real_value_type{0.0},B[2].real(),tol);
    CHECK_MOLLIFIED_CLOSE(
        real_value_type{0.0},B[2].imag(),tol);
  }
  {
    std::vector< Complex > A{1.0,1.0,1.0};
    Backend plan(A.size());
    plan.forward(A.data(),A.data()+A.size(),A.data());
    CHECK_MOLLIFIED_CLOSE(real_value_type{3.0},A[0].real(),tol);
    CHECK_MOLLIFIED_CLOSE(real_value_type{0.0},A[0].imag(),tol);

    CHECK_MOLLIFIED_CLOSE(
        real_value_type{0.0},A[1].real(),tol);
    CHECK_MOLLIFIED_CLOSE(
        real_value_type{0.0},A[1].imag(),tol);

    CHECK_MOLLIFIED_CLOSE(
        real_value_type{0.0},A[2].real(),tol);
    CHECK_MOLLIFIED_CLOSE(
        real_value_type{0.0},A[2].imag(),tol);
  }
}

template<class Backend>
void test_inverse(int N, int tolerance)
{
  using Complex = typename Backend::value_type;
  using real_value_type = typename Complex::value_type;
  const real_value_type tol = tolerance*std::numeric_limits<real_value_type>::epsilon();

  boost::random::mt19937 rng;
  boost::random::uniform_real_distribution<real_value_type> U(0.0,1.0);
  {
    std::vector<Complex> A(N),B(N),C(N);

    for(auto& x: A)
    {
        x.real( U(rng) );
        x.imag( U(rng) );
    }
    Backend plan(N);
    plan.forward(A.data(),A.data()+A.size(),B.data());
    plan.backward(B.data(),B.data()+B.size(),C.data());

    const real_value_type inverse_N = real_value_type{1.0}/N;
    for(auto &x : C)
      x *= inverse_N;

    real_value_type diff{0.0};

    for(size_t i=0;i<A.size();++i)
    {
        using std::norm;
        diff += norm(A[i]-C[i]);
    }
    using std::sqrt;
    diff = sqrt(diff)*inverse_N;
    CHECK_MOLLIFIED_CLOSE(real_value_type{0.0},diff,tol);
  }
}

#if defined(__GNUC__)
template<class T>
using complex_fftw_dft = fftw_dft< boost::multiprecision::complex<T> >;

template<class T>
using complex_gsl_dft = gsl_dft< boost::multiprecision::complex<T> >;
#endif

template<class T>
using complex_bsl_dft = bsl_dft< boost::multiprecision::complex<T> >;

//template<class T>
//using complex_rader_dft = rader_dft< boost::multiprecision::complex<T>  >;
//
//template<class T>
//using complex_bruteForce_dft = bruteForce_dft< boost::multiprecision::complex<T>  >;
//
//template<class T>
//using complex_bruteForce_cdft = bruteForce_cdft< boost::multiprecision::complex<T>  >;
//
//template<class T>
//using complex_composite_dft = composite_dft< boost::multiprecision::complex<T>  >;
//
//template<class T>
//using complex_composite_cdft = composite_cdft< boost::multiprecision::complex<T>  >;
//
//template<class T>
//using complex_power2_dft = power2_dft< boost::multiprecision::complex<T>  >;
//
//template<class T>
//using complex_power2_cdft = power2_cdft< boost::multiprecision::complex<T>  >;

int main()
{
#if defined(__GNUC__)
  test_fixed_transforms<complex_fftw_dft<float>>(1);
  test_fixed_transforms<complex_fftw_dft<double>>(1);
  test_fixed_transforms<complex_fftw_dft<long double>>(1);
#endif

#if defined(BOOST_MATH_USE_FLOAT128)
  test_fixed_transforms<complex_fftw_dft<boost::multiprecision::float128>>(1);
#endif

#if defined(__GNUC__)
  test_fixed_transforms<complex_gsl_dft<double>>(1);
#endif

//  test_fixed_transforms<complex_bruteForce_dft<float> >(4);
//  test_fixed_transforms<complex_bruteForce_dft<double> >(4);
//  test_fixed_transforms<complex_bruteForce_dft<long double> >(4);

//  test_fixed_transforms<complex_bruteForce_cdft<float> >(4);
//  test_fixed_transforms<complex_bruteForce_cdft<double> >(4);
//  test_fixed_transforms<complex_bruteForce_cdft<long double> >(4);
//
//
//  test_fixed_transforms<complex_composite_dft<float>>(4);
//  test_fixed_transforms<complex_composite_dft<double>>(4);
//  test_fixed_transforms<complex_composite_dft<long double>>(4);
//
//  test_fixed_transforms<complex_composite_cdft<float>>(4);
//  test_fixed_transforms<complex_composite_cdft<double>>(4);
//  test_fixed_transforms<complex_composite_cdft<long double>>(4);

  test_fixed_transforms<complex_bsl_dft<float>>(2);
  test_fixed_transforms<complex_bsl_dft<double>>(2);
  test_fixed_transforms<complex_bsl_dft<long double>>(2);
#ifdef BOOST_MATH_USE_FLOAT128
  test_fixed_transforms<complex_bsl_dft< boost::multiprecision::float128 >>(1);
#endif
  test_fixed_transforms<complex_bsl_dft< boost::multiprecision::cpp_bin_float_50>>(2);
  test_fixed_transforms<complex_bsl_dft< boost::multiprecision::cpp_bin_float_100 >>(2);
  test_fixed_transforms<complex_bsl_dft< boost::multiprecision::cpp_bin_float_quad >>(2);
  // TODO:
  //test_fixed_transforms<complex_bsl_dft< boost::multiprecision::mpfr_float_100 >>(1);

  for(int i=1;i<=(1<<10); i*=2)
  {
#if defined(__GNUC__)
    test_directly<fftw_dft,double>(i,i*8);
    test_directly<gsl_dft,double>(i,i*8);
#endif
    test_directly<bsl_dft,double>(i,i*8);

#if defined(__GNUC__)
    test_inverse<complex_fftw_dft<float>>(i,1);
    test_inverse<complex_fftw_dft<double>>(i,1);
    test_inverse<complex_fftw_dft<long double>>(i,1);
#endif
#if defined(BOOST_MATH_USE_FLOAT128)
    test_inverse<complex_fftw_dft<boost::multiprecision::float128>>(i,1);
#endif
#if defined(__GNUC__)
    test_inverse<complex_gsl_dft<double>>(i,1);
#endif
    test_inverse<complex_bsl_dft<float>>(i,1);
    test_inverse<complex_bsl_dft<double>>(i,1);
    test_inverse<complex_bsl_dft<long double>>(i,1);
#if defined(BOOST_MATH_USE_FLOAT128)
    test_inverse<complex_bsl_dft<boost::multiprecision::float128>>(i,1);
#endif
    test_inverse<complex_bsl_dft<boost::multiprecision::cpp_bin_float_50>>(i,1);

//    test_inverse<complex_power2_dft<float>>(i,32);
//    test_inverse<complex_power2_dft<double>>(i,32);
//    test_inverse<complex_power2_dft<long double>>(i,32);
//    test_inverse<complex_power2_dft<boost::multiprecision::cpp_bin_float_50>>(i,32);
//#ifdef BOOST_MATH_USE_FLOAT128
//    test_inverse<complex_power2_dft<boost::multiprecision::float128>>(i,32);
//#endif
//
//    test_inverse<complex_power2_cdft<float>>(i,1);
//    test_inverse<complex_power2_cdft<double>>(i,1);
//    test_inverse<complex_power2_cdft<long double>>(i,1);
//    test_inverse<complex_power2_cdft<boost::multiprecision::cpp_bin_float_50>>(i,1);
//#ifdef BOOST_MATH_USE_FLOAT128
//    test_inverse<complex_power2_cdft<boost::multiprecision::float128>>(i,1);
//#endif
  }
  for(int i=1;i<=1000; i*=10)
  {
#if defined(__GNUC__)
    test_directly<fftw_dft,double>(i,i*8);
    test_directly<gsl_dft,double>(i,i*8);
#endif
    test_directly<bsl_dft,double>(i,i*8);

#if defined(__GNUC__)
    test_inverse<complex_fftw_dft<float>>(i,1);
    test_inverse<complex_fftw_dft<double>>(i,1);
    test_inverse<complex_fftw_dft<long double>>(i,1);
#endif
#if defined(BOOST_MATH_USE_FLOAT128)
    test_inverse<complex_fftw_dft<boost::multiprecision::float128>>(i,1);
#endif
#if defined(__GNUC__)
    test_inverse<complex_gsl_dft<double>>(i,1);
#endif
    test_inverse<complex_bsl_dft<float>>(i,1);
    test_inverse<complex_bsl_dft<double>>(i,1);
    test_inverse<complex_bsl_dft<long double>>(i,1);
#if defined(BOOST_MATH_USE_FLOAT128)
    test_inverse<complex_bsl_dft<boost::multiprecision::float128>>(i,1);
#endif
    test_inverse<complex_bsl_dft<boost::multiprecision::cpp_bin_float_50>>(i,1);
  }
  for(auto i : std::vector<int>{2,3,5,7,11,13,17,23,29,31})
  {
#if defined(__GNUC__)
    test_directly<fftw_dft,double>(i,i*8);
    test_directly<gsl_dft,double>(i,i*8);
#endif
    test_directly<bsl_dft,double>(i,i*8);

#if defined(__GNUC__)
    test_inverse<complex_fftw_dft<float>>(i,1);
    test_inverse<complex_fftw_dft<double>>(i,1);
    test_inverse<complex_fftw_dft<long double>>(i,1);
#endif
#if defined(BOOST_MATH_USE_FLOAT128)
    test_inverse<complex_fftw_dft<boost::multiprecision::float128>>(i,1);
#endif
#if defined(__GNUC__)
    test_inverse<complex_gsl_dft<double>>(i,1);
#endif
    test_inverse<complex_bsl_dft<float>>(i,2);
    test_inverse<complex_bsl_dft<double>>(i,2);
    test_inverse<complex_bsl_dft<long double>>(i,2);
#if defined(BOOST_MATH_USE_FLOAT128)
    test_inverse<complex_bsl_dft<boost::multiprecision::float128>>(i,2);
#endif
    test_inverse<complex_bsl_dft<boost::multiprecision::cpp_bin_float_50>>(i,2);

//    if(i>2)
//    {
//      test_inverse<complex_rader_dft<float> >(i,2);
//      test_inverse<complex_rader_dft<double> >(i,2);
//      test_inverse<complex_rader_dft<long double> >(i,2);
//#ifdef BOOST_MATH_USE_FLOAT128
//      test_inverse<complex_rader_dft<boost::multiprecision::float128> >(i,2);
//#endif
//      test_inverse<complex_rader_dft<boost::multiprecision::cpp_bin_float_50> >(i,2);
//    }
  }

  for(int i=1;i<=100;++i)
  {
#if defined(__GNUC__)
    test_directly<fftw_dft,double>(i,i*8);
    test_directly<gsl_dft,double>(i,i*8);
#endif
    test_directly<bsl_dft,double>(i,i*8);

#if defined(__GNUC__)
    test_inverse<complex_fftw_dft<float>>(i,1);
    test_inverse<complex_fftw_dft<double>>(i,1);
    test_inverse<complex_fftw_dft<long double>>(i,1);
#endif
#if defined(BOOST_MATH_USE_FLOAT128)
    test_inverse<complex_fftw_dft<boost::multiprecision::float128>>(i,1);
#endif
#if defined(__GNUC__)
    test_inverse<complex_gsl_dft<double>>(i,1);
#endif
    test_inverse<complex_bsl_dft<float>>(i,2);
    test_inverse<complex_bsl_dft<double>>(i,2);
    test_inverse<complex_bsl_dft<long double>>(i,2);
#if defined(BOOST_MATH_USE_FLOAT128)
    test_inverse<complex_bsl_dft<boost::multiprecision::float128>>(i,2);
#endif
    test_inverse<complex_bsl_dft<boost::multiprecision::cpp_bin_float_50>>(i,2);

//    if(i<=20)
//    {
//      test_inverse<complex_bruteForce_dft<float> >(i,i*8);
//      test_inverse<complex_bruteForce_dft<double> >(i,i*8);
//      test_inverse<complex_bruteForce_dft<long double> >(i,i*8);
//      test_inverse<complex_bruteForce_dft<boost::multiprecision::cpp_bin_float_50> >(i,i*8);
//#ifdef BOOST_MATH_USE_FLOAT128
//      test_inverse<complex_bruteForce_dft<boost::multiprecision::float128> >(i,i*8);
//#endif
//
//      test_inverse<complex_bruteForce_cdft<float> >(i,i*8);
//      test_inverse<complex_bruteForce_cdft<double> >(i,i*8);
//      test_inverse<complex_bruteForce_cdft<long double> >(i,i*8);
//      test_inverse<complex_bruteForce_cdft<boost::multiprecision::cpp_bin_float_50> >(i,i*8);
//#ifdef BOOST_MATH_USE_FLOAT128
//      test_inverse<complex_bruteForce_cdft<boost::multiprecision::float128> >(i,i*8);
//#endif
//    }

#if defined(__GNUC__)
    test_convolution<fftw_dft<std::complex<double>>>(i,i*8);
    test_convolution<gsl_dft<std::complex<double>>>(i,i*8);
#endif
    test_convolution<bsl_dft<std::complex<double>>>(i,i*8);

//    test_inverse<complex_composite_dft<float>>(i,i*8);
//    test_inverse<complex_composite_dft<double>>(i,i*8);
//    test_inverse<complex_composite_dft<long double>>(i,i*8);
//
//    test_inverse<complex_composite_cdft<float>>(i,2);
//    test_inverse<complex_composite_cdft<double>>(i,2);
//    test_inverse<complex_composite_cdft<long double>>(i,2);
  }
  // TODO: can we print a useful compilation error message for the following
  // illegal case?
  // dft<std::complex<int>> P(3);
  return boost::math::test::report_errors();
}


//
//#include <cstdlib>
//#include <iostream>
//#include <Eigen/Dense>
//#include <Eigen/Eigenvalues>

#include <iostream>
#include <cstdlib>
#include <cstring>
#include <random>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

extern "C"
{
	double randValue(double max, double min) {
			return min + ((rand() / (double)RAND_MAX) * (max - min));
	}

	__declspec(dllexport) void cleanModel(double * model)
	{
		free(model);
	}

	__declspec(dllexport) double * lineaire_model(int nbInput)
	{
		double * model = (double *)malloc((nbInput + 1) * sizeof(double));
		model[0] = 1;

		for (int i = 1; i <= nbInput; ++i)
		{
			model[i] = randValue(1, -1);
		}
		return model;
	}

	__declspec(dllexport) double classifyRegression(double* model, double* input, int inputSize)
	{
		double sum = model[0];
		for (int i = 0; i < inputSize; ++i)
		{
			sum += model[i + 1] * input[i];
		}
		return sum;
	}

	__declspec(dllexport) void regression(double* model, double* valueSend, int size, int inputSize, double* waitValue)
	{
		Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> entry(size, inputSize + 1);
		Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> waited(size, 1);

		for (int i = 0; i < size; ++i)
		{
			waited(i, 0) = *waitValue;
			++waitValue;
		}

		for (int i = 0; i < size; ++i)
		{
			entry(i, 0) = 1.0;
			for (int j = 1; j < inputSize + 1; ++j)
			{
				entry(i, j) = *valueSend;
				++valueSend;
			}
		}

		Eigen::MatrixXd transposedMatrix = entry.transpose();
		Eigen::MatrixXd inverseMatrix = (transposedMatrix*entry).inverse();
		Eigen::MatrixXd final = inverseMatrix*transposedMatrix;
		Eigen::MatrixXd weight = final*waited;

		for (int i = 0; i < weight.rows(); ++i)
		{
			*model = weight(i, 0);
			++model;
		}
	}
}


#include<iostream>
#include<vector>
#include<fstream>
#include <boost/math/special_functions/gamma.hpp>

const int X=0;
const int O2=1;
const int G=2;
const int Xy=3;
const int A=4;
const int B=5;

void get_rhs(std::vector<double>& rhs,std::vector<double> solnvec,double t,int nvars)
{
  // ------- MODEL CONSTANTS -------
  double y_xs = 0.009;
  double y_as = 1.01;
  double y_bs = 0.88;
  double y_os = 0.0467;

  double x_max = 11;
  double qs_max = 17;
  double o2_max = 0.214;

  double K_e = 0.0214;
  double K_s = 31;

  double alpha_s = 3;
  double beta_s = 12;
  double alpha_e = 1;
  double beta_e = 1e3;

  double kLa = 5;

  // calculate chi_i
  // A word of explanation about the form of the gamma incomplete call. This function
  // is usually expressed as g(a, x), where a and x are obligate positive. Therefore, we need
  // to restrict x to be no lower than 0. Additionally, we need to make sure we don't hit a
  // divide by zero error, so we condition use of the gamma function on the denominator being
  // non-zero.
  double chi_s = 1;
  double chi_e = 1;

  if (solnvec[Xy]>1e-8) {
    double sRatio = std::max(solnvec[G]/(solnvec[Xy]), 0.0);
    chi_s = boost::math::gamma_p(alpha_s, beta_s*sRatio);
  }

  if (solnvec[A]>1e-8) {
    double eRatio = std::max(solnvec[O2]/(solnvec[A]), 0.0);
    chi_e = boost::math::gamma_p(alpha_e, beta_e*eRatio);
  }

  double chi_p = 0.3;

  // calculate q_s
  double F_s = (solnvec[G] + solnvec[Xy])/(solnvec[G] + solnvec[Xy] + K_s);
  double F_e = (solnvec[O2] + solnvec[A]/beta_e)/(solnvec[O2] + solnvec[A]/beta_e + K_e);
  double q_s = qs_max*F_s*F_e;

  // calculate intermediate rates
  double rar = chi_p*y_as*q_s*solnvec[X];
  double rbr = (1-chi_p)*y_bs*q_s*solnvec[X];

  double our = -chi_e*y_os*q_s*solnvec[X];
  double otr = kLa *(o2_max - solnvec[O2]);
  double rae = -(1-chi_e)*y_as*q_s*solnvec[X];
  double rbe = -rae;

  // calculate final rates
  rhs[X] = y_xs*q_s*solnvec[X]*(1 - solnvec[X]/x_max);
  //rhs[O2] = our+otr;
  rhs[02] = 0;
  rhs[G] = -chi_s     *q_s*solnvec[X];
  rhs[Xy] = -(1-chi_s) *q_s*solnvec[X];
  rhs[A] = rar+rae;
  rhs[B] = rbr+rbe;

}

double get_our(std::vector<double> solnvec,double t,int nvars)
{
  // ------- MODEL CONSTANTS -------
  double y_os = 0.0467;

  double x_max = 11;
  double qs_max = 17;
  double o2_max = 0.214;

  double K_e = 0.0214;
  double K_s = 31;

  double alpha_s = 3;
  double beta_s = 12;
  double alpha_e = 1;
  double beta_e = 1e3;

  // calculate chi_i
  double chi_s = 1;
  double chi_e = 1;

  if (solnvec[Xy]>1e-8) {
    double sRatio = std::max(solnvec[G]/(solnvec[Xy]), 0.0);
    chi_s = boost::math::gamma_p(alpha_s, beta_s*sRatio);
  }

  if (solnvec[A]>1e-8) {
    double eRatio = std::max(solnvec[O2]/(solnvec[A]), 0.0);
    chi_e = boost::math::gamma_p(alpha_e, beta_e*eRatio);
  }

  double chi_p = 0.3;

  // calculate q_s
  double F_s = (solnvec[G] + solnvec[Xy])/(solnvec[G] + solnvec[Xy] + K_s);
  double F_e = (solnvec[O2] + solnvec[A]/beta_e)/(solnvec[O2] + solnvec[A]/beta_e + K_e);
  double q_s = qs_max*F_s*F_e;

  double our = -chi_e*y_os*q_s*solnvec[X];

  return our;
}

void advance(std::vector<double>& solnvec,int nvars,double t_now,double t_adv,double dt)
{
    double current_time=t_now;
    double final_time=t_now+t_adv;

    std::vector<double> rhs(nvars);
    std::vector<double> solnvec_n(nvars);

    while(current_time < final_time)
    {
        current_time += dt;

        //at current time level n
        solnvec_n=solnvec;

        //Doing RK23

        //stage 1
        get_rhs(rhs,solnvec,current_time,nvars);
        for(int i=0;i<nvars;i++)
        {
            solnvec[i] = solnvec_n[i] + 0.5*rhs[i]*dt;
        }

        //stage 2
        get_rhs(rhs,solnvec,current_time,nvars);
        for(int i=0;i<nvars;i++)
        {
            solnvec[i] = solnvec_n[i] + rhs[i]*dt;
        }
    }
}

int main()
{
    int nvars=6;
    std::vector<double> solnvec(nvars);

    double final_time=24.0;
    double advance_time=0.5;
    double current_time=0.0;
    double dt=advance_time/50.0;

    std::ofstream outfile("timehist.dat");

    //set initial conditions
    solnvec[X]  = 0.5;
    solnvec[O2] = 0.214;
    solnvec[G]  = 500.0;
    solnvec[Xy] = 250.0;
    solnvec[A]  = 0.0;
    solnvec[B]  = 0.0;

    //write initial condition
    outfile<<current_time<<"\t";
    for(int i=0;i<nvars;i++)
    {
        outfile<<solnvec[i]<<"\t";
    }
    outfile<<"\n";

    while(current_time < final_time)
    {
        current_time += advance_time;

        advance(solnvec,nvars,current_time,advance_time,dt);

        outfile<<current_time<<"\t";
        for(int i=0;i<nvars;i++)
        {
            outfile<<solnvec[i]<<"\t";
        }
        outfile<<"\n";
    }

    outfile.close();
    return(0);
}


/*
 * MIT License
 *
 * Copyright (c) 2020 Robert Grupp
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include "xregPairedPointRegi3D3D.h"

#include <Eigen/Eigenvalues>

#include "xregAssert.h"
#include "xregRotUtils.h"
#include "xregPointCloudUtils.h"
#include "xregLandmarkMapUtils.h"

xreg::FrameTransform
xreg::PairedPointRegi3D3D(const Pt3List& pts1, const Pt3List& pts2, const CoordScalar scale)
{
  const size_type num_pts = pts1.size();
  xregASSERT(num_pts == pts2.size());

  FrameTransform xform = FrameTransform::Identity();

  // These calls are threaded
  const Pt3 cent1 = ComputeCentroid(pts1);
  const Pt3 cent2 = ComputeCentroid(pts2);

  // These calls are threaded
  const Pt3List pts1_prime = OffsetPoints(-cent1, pts1);
  const Pt3List pts2_prime = OffsetPoints(-cent2, pts2);

  CoordScalar s = scale;
  if (s <= 0)
  {
    s = 0;

    const CoordScalar s1 = SumOfNormsSquared(pts1_prime);  // threaded

    if (std::abs(s1) > 1.0e-6)
    {
      const CoordScalar s2 = SumOfNormsSquared(pts2_prime);  // threaded

      s = std::sqrt(s2 / s1);
    }
  }

  CoordScalar S[9];

  CoordScalar* S_dst = S;
  for (size_type i = 0; i < 3; ++i)
  {
    for (size_type j = 0; j < 3; ++j, ++S_dst)
    {
      // This call is threaded
      *S_dst = InnerProductAboutDimsOfPts(pts1_prime, pts2_prime, i, j);
    }
  }

  const CoordScalar S_xx = S[0];
  const CoordScalar S_xy = S[1];
  const CoordScalar S_xz = S[2];
  const CoordScalar S_yx = S[3];
  const CoordScalar S_yy = S[4];
  const CoordScalar S_yz = S[5];
  const CoordScalar S_zx = S[6];
  const CoordScalar S_zy = S[7];
  const CoordScalar S_zz = S[8];

  Mat4x4 N_matrix;
  N_matrix(0,0) = S_xx + S_yy + S_zz;
  N_matrix(0,1) = S_yz - S_zy;
  N_matrix(0,2) = S_zx - S_xz;
  N_matrix(0,3) = S_xy - S_yx;
  N_matrix(1,0) = N_matrix(0,1);
  N_matrix(1,1) = S_xx - S_yy - S_zz;
  N_matrix(1,2) = S_xy + S_yx;
  N_matrix(1,3) = S_zx + S_xz;
  N_matrix(2,0) = N_matrix(0,2);
  N_matrix(2,1) = N_matrix(1,2);
  N_matrix(2,2) = -S_xx + S_yy - S_zz;
  N_matrix(2,3) = S_yz + S_zy;
  N_matrix(3,0) = N_matrix(0,3);
  N_matrix(3,1) = N_matrix(1,3);
  N_matrix(3,2) = N_matrix(2,3);
  N_matrix(3,3) = -S_xx - S_yy + S_zz;

  Eigen::EigenSolver<Mat4x4> eig_dec(N_matrix, true);

  size_type max_index = 0;
  eig_dec.eigenvalues().real().maxCoeff(&max_index);
  xform.linear().matrix() = QuatToRotMat(eig_dec.eigenvectors().col(max_index).real());

  xform.linear().matrix() *= s;

  xform.matrix().block(0,3,3,1) = cent2 - (xform.linear() * cent1);

  return xform;
}

xreg::FrameTransform
xreg::PairedPointRegi3D3D(const LandMap3& pts1, const LandMap3& pts2, const CoordScalar scale)
{
  const auto corr_lists = CreateCorrespondencePointLists(pts1, pts2);

  return PairedPointRegi3D3D(std::get<0>(corr_lists), std::get<1>(corr_lists), scale);
}


//  (C) Copyright Nick Thompson 2021.
//  (C) Copyright Matt Borland 2022.
//  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)

#include <cmath>
#include <cstdint>
#include <array>
#include <complex>
#include <tuple>
#include <iostream>
#include <vector>
#include <limits>
#include <boost/math/tools/color_maps.hpp>

#if !__has_include("lodepng.h")
 #error "lodepng.h is required to run this example."
#endif
#include "lodepng.h"
#include <iostream>
#include <string>
#include <vector>

// In lodepng, the vector is expected to be row major, with the top row
// specified first. Note that this is a bit confusing sometimes as it's more
// natural to let y increase moving *up*.
unsigned write_png(const std::string &filename,
                   const std::vector<std::uint8_t> &img, std::size_t width,
                   std::size_t height) {
  unsigned error = lodepng::encode(filename, img, width, height,
                                   LodePNGColorType::LCT_RGBA, 8);
  if (error) {
    std::cerr << "Error encoding png: " << lodepng_error_text(error) << "\n";
  }
  return error;
}

// Computes ab - cd.
// See: https://pharr.org/matt/blog/2019/11/03/difference-of-floats.html
template <typename Real>
inline Real difference_of_products(Real a, Real b, Real c, Real d)
{
    Real cd = c * d;
    Real err = std::fma(-c, d, cd);
    Real dop = std::fma(a, b, -cd);
    return dop + err;
}

template<typename Real>
auto fifth_roots(std::complex<Real> z)
{
    std::complex<Real> v = std::pow(z,4);
    std::complex<Real> dw = Real(5)*v;
    std::complex<Real> w = v*z - Real(1);
    return std::make_pair(w, dw);
}

template<typename Real>
auto g(std::complex<Real> z)
{
    std::complex<Real> z2 = z*z;
    std::complex<Real> z3 = z*z2;
    std::complex<Real> z4 = z2*z2;
    std::complex<Real> w = z4*(z4 + Real(15)) - Real(16);
    std::complex<Real> dw = Real(4)*z3*(Real(2)*z4 + Real(15));
    return std::make_pair(w, dw);
}

template<typename Real>
std::complex<Real> complex_newton(std::function<std::pair<std::complex<Real>,std::complex<Real>>(std::complex<Real>)> f, std::complex<Real> z)
{
    // f(x(1+e)) = f(x) + exf'(x)
    bool close = false;
    do
    {
        auto [y, dy] = f(z);
        z -= y/dy;
        close = (abs(y) <= 1.4*std::numeric_limits<Real>::epsilon()*abs(z*dy));
    } while(!close);
    return z;
}

template<typename Real>
class plane_pixel_map
{
public:
    plane_pixel_map(int64_t image_width, int64_t image_height, Real xmin, Real ymin)
    {
        image_width_ = image_width;
        image_height_ = image_height;
        xmin_ = xmin;
        ymin_ = ymin;
    }

    std::complex<Real> to_complex(int64_t i, int64_t j) const {
        Real x = xmin_ + 2*abs(xmin_)*Real(i)/Real(image_width_ - 1);
        Real y = ymin_ + 2*abs(ymin_)*Real(j)/Real(image_height_ - 1);
        return std::complex<Real>(x,y);
    }

    std::pair<int64_t, int64_t> to_pixel(std::complex<Real> z) const {
        Real x = z.real();
        Real y = z.imag();
        Real ii = (image_width_ - 1)*(x - xmin_)/(2*abs(xmin_));
        Real jj = (image_height_ - 1)*(y - ymin_)/(2*abs(ymin_));

        return std::make_pair(std::round(ii), std::round(jj));
    }

private:
    int64_t image_width_;
    int64_t image_height_;
    Real xmin_;
    Real ymin_;
};

int main(int argc, char** argv)
{
    using Real = double;
    using boost::math::tools::viridis;
    using std::sqrt;

    std::function<std::array<Real, 3>(Real)> color_map = viridis<Real>;
    std::string requested_color_map = "viridis";
    if (argc == 2) {
       requested_color_map = std::string(argv[1]);
       if (requested_color_map == "smooth_cool_warm") {
          color_map = boost::math::tools::smooth_cool_warm<Real>;
       }
       else if (requested_color_map == "plasma") {
          color_map = boost::math::tools::plasma<Real>;
       }
       else if (requested_color_map == "black_body") {
          color_map = boost::math::tools::black_body<Real>;
       }
       else if (requested_color_map == "inferno") {
          color_map = boost::math::tools::inferno<Real>;
       }
       else if (requested_color_map == "kindlmann") {
          color_map = boost::math::tools::kindlmann<Real>;
       }
       else if (requested_color_map == "extended_kindlmann") {
          color_map = boost::math::tools::extended_kindlmann<Real>;
       }
       else {
          std::cerr << "Could not recognize color map " << argv[1] << ".";
          return 1;
       }
    }
    constexpr int64_t image_width = 1024;
    constexpr int64_t image_height = 1024;
    constexpr const Real two_pi = 6.28318530718;

    std::vector<std::uint8_t> img(4*image_width*image_height, 0);
    plane_pixel_map<Real> map(image_width, image_height, Real(-2), Real(-2));

    for (int64_t j = 0; j < image_height; ++j)
    {
        std::cout << "j = " << j << "\n";
        for (int64_t i = 0; i < image_width; ++i)
        {
            std::complex<Real> z0 = map.to_complex(i,j);
            auto rt = complex_newton<Real>(g<Real>, z0);
            // The root is one of exp(2*pi*ij/5). Therefore, it can be classified by angle.
            Real theta = std::atan2(rt.imag(), rt.real());
            // Now theta in [-pi,pi]. Get it into [0,2pi]:
            if (theta < 0) {
                theta += two_pi;
            }
            theta /= two_pi;
            if (std::isnan(theta)) {
                std::cerr << "Theta is a nan!\n";
            }
            auto c = boost::math::tools::to_8bit_rgba(color_map(theta));
            int64_t idx = 4 * image_width * (image_height - 1 - j) + 4 * i;
            img[idx + 0] = c[0];
            img[idx + 1] = c[1];
            img[idx + 2] = c[2];
            img[idx + 3] = c[3];
        }
    }

    std::array<std::complex<Real>, 8> roots;
    roots[0] = -Real(1);
    roots[1] = Real(1);
    roots[2] = {Real(0), Real(1)};
    roots[3] = {Real(0), -Real(1)};
    roots[4] = {sqrt(Real(2)), sqrt(Real(2))};
    roots[5] = {sqrt(Real(2)), -sqrt(Real(2))};
    roots[6] = {-sqrt(Real(2)), -sqrt(Real(2))};
    roots[7] = {-sqrt(Real(2)), sqrt(Real(2))};

    for (int64_t k = 0; k < 8; ++k)
    {
        auto [ic, jc] = map.to_pixel(roots[k]);

        int64_t r = 7;
        for (int64_t i = ic - r; i < ic + r; ++i)
        {
            for (int64_t j = jc - r; j < jc + r; ++j)
            {
                if ((i-ic)*(i-ic) + (j-jc)*(j-jc) > r*r)
                {
                    continue;
                }
                int64_t idx = 4 * image_width * (image_height - 1 - j) + 4 * i;
                img[idx + 0] = 0;
                img[idx + 1] = 0;
                img[idx + 2] = 0;
                img[idx + 3] = 0xff;
            }
        }
    }

    // Requires lodepng.h
    // See: https://github.com/lvandeve/lodepng for download and compilation instructions
    write_png(requested_color_map + "_newton_fractal.png", img, image_width, image_height);
}