#include "drake/common/polynomial.h"

#include <cmath>
#include <cstddef>
#include <map>
#include <sstream>
#include <stdexcept>
#include <vector>

#include <Eigen/Dense>
#include <gtest/gtest.h>

#include "drake/common/symbolic.h"
#include "drake/common/test_utilities/eigen_matrix_compare.h"
#include "drake/common/test_utilities/expect_no_throw.h"
#include "drake/common/test_utilities/random_polynomial_matrix.h"

using Eigen::VectorXd;
using std::default_random_engine;
using std::map;
using std::runtime_error;
using std::uniform_int_distribution;
using std::uniform_real_distribution;
using std::vector;

namespace drake {
namespace {

using std::pow;

template <typename T>
void testIntegralAndDerivative() {
  VectorXd coefficients = VectorXd::Random(5);
  Polynomial<T> poly(coefficients);

  EXPECT_TRUE(CompareMatrices(poly.GetCoefficients(),
                              poly.Derivative(0).GetCoefficients(), 1e-14,
                              MatrixCompareType::absolute));

  const T x{1.32};
  Polynomial<T> first_derivative = poly.Derivative(1);
  EXPECT_NEAR(poly.EvaluateUnivariate(x, 1),
              first_derivative.EvaluateUnivariate(x), 1e-14);

  Polynomial<T> third_derivative = poly.Derivative(3);
  EXPECT_NEAR(poly.EvaluateUnivariate(x, 3),
              third_derivative.EvaluateUnivariate(x), 1e-14);

  Polynomial<T> third_derivative_check =
      poly.Derivative().Derivative().Derivative();

  EXPECT_TRUE(CompareMatrices(third_derivative.GetCoefficients(),
                              third_derivative_check.GetCoefficients(), 1e-14,
                              MatrixCompareType::absolute));

  Polynomial<T> tenth_derivative = poly.Derivative(10);

  EXPECT_TRUE(CompareMatrices(tenth_derivative.GetCoefficients(),
                              VectorXd::Zero(1), 1e-14,
                              MatrixCompareType::absolute));

  Polynomial<T> integral = poly.Integral(0.0);
  Polynomial<T> poly_back = integral.Derivative();

  EXPECT_TRUE(CompareMatrices(poly_back.GetCoefficients(),
                              poly.GetCoefficients(), 1e-14,
                              MatrixCompareType::absolute));
}

template <typename T>
void testOperators() {
  int max_num_coefficients = 6;
  int num_tests = 10;
  default_random_engine generator;
  std::uniform_int_distribution<> int_distribution(1, max_num_coefficients);
  uniform_real_distribution<double> uniform;

  for (int i = 0; i < num_tests; ++i) {
    VectorXd coeff1 = VectorXd::Random(int_distribution(generator));
    Polynomial<T> poly1(coeff1);

    VectorXd coeff2 = VectorXd::Random(int_distribution(generator));
    Polynomial<T> poly2(coeff2);

    double scalar = uniform(generator);

    Polynomial<T> sum = poly1 + poly2;
    Polynomial<T> difference = poly2 - poly1;
    Polynomial<T> product = poly1 * poly2;
    Polynomial<T> poly1_plus_scalar = poly1 + scalar;
    Polynomial<T> poly1_minus_scalar = poly1 - scalar;
    Polynomial<T> poly1_scaled = poly1 * scalar;
    Polynomial<T> poly1_div = poly1 / scalar;
    Polynomial<T> poly1_times_poly1 = poly1;
    const Polynomial<T> pow_poly1_3{pow(poly1, 3)};
    const Polynomial<T> pow_poly1_4{pow(poly1, 4)};
    const Polynomial<T> pow_poly1_10{pow(poly1, 10)};
    poly1_times_poly1 *= poly1_times_poly1;

    double t = uniform(generator);
    EXPECT_NEAR(sum.EvaluateUnivariate(t),
                poly1.EvaluateUnivariate(t) + poly2.EvaluateUnivariate(t),
                1e-8);
    EXPECT_NEAR(difference.EvaluateUnivariate(t),
                poly2.EvaluateUnivariate(t) - poly1.EvaluateUnivariate(t),
                1e-8);
    EXPECT_NEAR(product.EvaluateUnivariate(t),
                poly1.EvaluateUnivariate(t) * poly2.EvaluateUnivariate(t),
                1e-8);
    EXPECT_NEAR(poly1_plus_scalar.EvaluateUnivariate(t),
                poly1.EvaluateUnivariate(t) + scalar, 1e-8);
    EXPECT_NEAR(poly1_minus_scalar.EvaluateUnivariate(t),
                poly1.EvaluateUnivariate(t) - scalar, 1e-8);
    EXPECT_NEAR(poly1_scaled.EvaluateUnivariate(t),
                poly1.EvaluateUnivariate(t) * scalar, 1e-8);
    EXPECT_NEAR(poly1_div.EvaluateUnivariate(t),
                poly1.EvaluateUnivariate(t) / scalar, 1e-8);
    EXPECT_NEAR(poly1_times_poly1.EvaluateUnivariate(t),
                poly1.EvaluateUnivariate(t) * poly1.EvaluateUnivariate(t),
                1e-8);
    EXPECT_NEAR(pow_poly1_3.EvaluateUnivariate(t),
                pow(poly1.EvaluateUnivariate(t), 3), 1e-8);
    EXPECT_NEAR(pow_poly1_4.EvaluateUnivariate(t),
                pow(poly1.EvaluateUnivariate(t), 4), 1e-8);
    EXPECT_NEAR(pow_poly1_10.EvaluateUnivariate(t),
                pow(poly1.EvaluateUnivariate(t), 10), 1e-8);
    EXPECT_NEAR(pow_poly1_3.EvaluateUnivariate(t) *
                    pow_poly1_4.EvaluateUnivariate(t) *
                    pow_poly1_3.EvaluateUnivariate(t),
                pow_poly1_10.EvaluateUnivariate(t), 1e-8);

    // Check the '==' operator.
    EXPECT_TRUE(poly1 + poly2 == sum);
    EXPECT_FALSE(poly1 == sum);
  }
}

template <typename T>
void testRoots() {
  int max_num_coefficients = 6;
  default_random_engine generator;
  std::uniform_int_distribution<> int_distribution(1, max_num_coefficients);

  int num_tests = 50;
  for (int i = 0; i < num_tests; ++i) {
    VectorXd coeffs = VectorXd::Random(int_distribution(generator));
    Polynomial<T> poly(coeffs);
    auto roots = poly.Roots();
    EXPECT_EQ(roots.rows(), poly.GetDegree());
    for (int k = 0; k < roots.size(); k++) {
      auto value = poly.EvaluateUnivariate(roots[k]);
      EXPECT_NEAR(std::abs(value), 0.0, 1e-8);
    }
  }
}

void testEvalType() {
  int max_num_coefficients = 6;
  default_random_engine generator;
  std::uniform_int_distribution<> int_distribution(1, max_num_coefficients);
  VectorXd coeffs = VectorXd::Random(int_distribution(generator));
  Polynomial<double> poly(coeffs);

  auto valueIntInput = poly.EvaluateUnivariate(1);
  EXPECT_EQ(typeid(decltype(valueIntInput)), typeid(double));

  auto valueComplexInput =
      poly.EvaluateUnivariate(std::complex<double>(1.0, 2.0));
  EXPECT_EQ(typeid(decltype(valueComplexInput)), typeid(std::complex<double>));
}

template <typename T>
void testPolynomialMatrix() {
  int max_matrix_rows_cols = 7;
  int num_coefficients = 6;
  default_random_engine generator;

  uniform_int_distribution<> matrix_size_distribution(1, max_matrix_rows_cols);
  int rows_A = matrix_size_distribution(generator);
  int cols_A = matrix_size_distribution(generator);
  int rows_B = cols_A;
  int cols_B = matrix_size_distribution(generator);

  auto A = test::RandomPolynomialMatrix<T>(num_coefficients,
                                                         rows_A, cols_A);
  auto B = test::RandomPolynomialMatrix<T>(num_coefficients,
                                                         rows_B, cols_B);
  auto C = test::RandomPolynomialMatrix<T>(num_coefficients,
                                                         rows_A, cols_A);
  auto product = A * B;
  auto sum = A + C;

  uniform_real_distribution<double> uniform;
  for (int row = 0; row < A.rows(); ++row) {
    for (int col = 0; col < A.cols(); ++col) {
      double t = uniform(generator);
      EXPECT_NEAR(sum(row, col).evaluateUnivariate(t),
                  A(row, col).evaluateUnivariate(t) +
                  C(row, col).evaluateUnivariate(t), 1e-8);

      double expected_product = 0.0;
      for (int i = 0; i < A.cols(); ++i) {
        expected_product += A(row, i).evaluateUnivariate(t) *
                            B(i, col).evaluateUnivariate(t);
      }
      EXPECT_NEAR(product(row, col).evaluateUnivariate(t),
                  expected_product, 1e-8);
    }
  }

  C.setZero();  // this was a problem before
}

GTEST_TEST(PolynomialTest, IntegralAndDerivative) {
  testIntegralAndDerivative<double>();
}

GTEST_TEST(PolynomialTest, TestMakeMonomialsUnique) {
  Eigen::Vector2d coefficients(1, 2);
  Polynomial<double> poly(coefficients);
  const auto poly_squared = poly * poly;
  EXPECT_EQ(poly_squared.GetNumberOfCoefficients(), 3);
}

GTEST_TEST(PolynomialTest, Operators) { testOperators<double>(); }

GTEST_TEST(PolynomialTest, Roots) { testRoots<double>(); }

GTEST_TEST(PolynomialTest, EvalType) { testEvalType(); }

GTEST_TEST(PolynomialTest, IsAffine) {
  Polynomiald x("x");
  Polynomiald y("y");

  EXPECT_TRUE(x.IsAffine());
  EXPECT_TRUE(y.IsAffine());
  EXPECT_TRUE((2 + x).IsAffine());
  EXPECT_TRUE((2 * x).IsAffine());
  EXPECT_FALSE((x * x).IsAffine());
  EXPECT_FALSE((x * y).IsAffine());
  EXPECT_TRUE((x + y).IsAffine());
  EXPECT_TRUE((2 + x + y).IsAffine());
  EXPECT_TRUE((2 + (2 * x) + y).IsAffine());
  EXPECT_FALSE((2 + (y * x) + y).IsAffine());
}

GTEST_TEST(PolynomialTest, VariableIdGeneration) {
  // Probe the outer edge cases of variable ID generation.

  // There is no documented maximum ID, but empirically it is 2325.  What we
  // really care about here is just that there is some value below which it
  // succeeds and above which it fails.
  static const int kMaxId = 2325;

  DRAKE_EXPECT_NO_THROW(Polynomial<double>("x", kMaxId));
  DRAKE_EXPECT_NO_THROW(Polynomial<double>("zzzz", 1));
  DRAKE_EXPECT_NO_THROW(Polynomial<double>("zzzz", kMaxId));
  EXPECT_THROW(Polynomial<double>("!"),
               std::runtime_error);  // Illegal character.
  EXPECT_THROW(Polynomial<double>("zzzz@"),
               std::runtime_error);  // Illegal length.
  EXPECT_THROW(Polynomial<double>("z", 0),
               std::runtime_error);  // Illegal ID.
  EXPECT_THROW(Polynomial<double>("z", kMaxId + 1),
               std::runtime_error);  // Illegal ID.

  // Test that ID generation round-trips correctly.
  std::stringstream test_stream;
  test_stream << Polynomial<double>("x", 1);
  std::string result;
  test_stream >> result;
  EXPECT_EQ(result, "x1");
}

GTEST_TEST(PolynomialTest, GetVariables) {
  Polynomiald x = Polynomiald("x");
  Polynomiald::VarType x_var = x.GetSimpleVariable();
  Polynomiald y = Polynomiald("y");
  Polynomiald::VarType y_var = y.GetSimpleVariable();
  Polynomiald z = Polynomiald("z");
  Polynomiald::VarType z_var = z.GetSimpleVariable();

  EXPECT_TRUE(x.GetVariables().count(x_var));
  EXPECT_FALSE(x.GetVariables().count(y_var));

  EXPECT_FALSE(Polynomiald().GetVariables().count(x_var));

  EXPECT_TRUE((x + x).GetVariables().count(x_var));

  EXPECT_TRUE((x + y).GetVariables().count(x_var));
  EXPECT_TRUE((x + y).GetVariables().count(y_var));

  EXPECT_TRUE((x * y * y + z).GetVariables().count(x_var));
  EXPECT_TRUE((x * y * y + z).GetVariables().count(y_var));
  EXPECT_TRUE((x * y * y + z).GetVariables().count(z_var));

  EXPECT_FALSE(x.Derivative().GetVariables().count(x_var));
}

GTEST_TEST(PolynomialTest, Simplification) {
  Polynomiald x = Polynomiald("x");
  Polynomiald y = Polynomiald("y");

  {  // Test duplicate monomials.
    std::stringstream test_stream;
    test_stream << ((x * y) + (x * y));
    std::string result;
    test_stream >> result;
    EXPECT_EQ(result, "(2)*x1*y1");
  }

  {  // Test monomials that are duplicates under commutativity.
    std::stringstream test_stream;
    test_stream << ((x * y) + (y * x));
    std::string result;
    test_stream >> result;
    EXPECT_EQ(result, "(2)*x1*y1");
  }
}

GTEST_TEST(PolynomialTest, MonomialFactor) {
  Polynomiald x = Polynomiald("x");
  Polynomiald y = Polynomiald("y");

  // "m_" prefix denotes monomial.
  Polynomiald::Monomial m_one = Polynomiald(1).GetMonomials()[0];
  Polynomiald::Monomial m_two = Polynomiald(2).GetMonomials()[0];
  Polynomiald::Monomial m_x = x.GetMonomials()[0];
  Polynomiald::Monomial m_y = y.GetMonomials()[0];
  Polynomiald::Monomial m_2x = (x * 2).GetMonomials()[0];
  Polynomiald::Monomial m_x2 = (x * x).GetMonomials()[0];
  Polynomiald::Monomial m_x2y = (x * x * y).GetMonomials()[0];

  // Expect failures
  EXPECT_EQ(m_x.Factor(m_y).coefficient, 0);
  EXPECT_EQ(m_x.Factor(m_x2).coefficient, 0);

  // Expect successes
  EXPECT_EQ(m_x.Factor(m_x), m_one);
  EXPECT_EQ(m_2x.Factor(m_x), m_two);
  EXPECT_EQ(m_x2.Factor(m_x), m_x);
  EXPECT_EQ(m_x2y.Factor(m_x2), m_y);
  EXPECT_EQ(m_x2y.Factor(m_y), m_x2);
}

GTEST_TEST(PolynomialTest, MultivariateValue) {
  Polynomiald x = Polynomiald("x");
  Polynomiald y = Polynomiald("y");
  const Polynomiald p{x * x + 2 * x * y + y * y + 2};
  const Polynomiald pow_p_2{pow(p, 2)};
  const Polynomiald pow_p_3{pow(p, 3)};
  const Polynomiald pow_p_7{pow(p, 7)};
  const std::map<Polynomiald::VarType, double> eval_point = {
    {x.GetSimpleVariable(), 1},
    {y.GetSimpleVariable(), 2}};
  EXPECT_EQ((x * x + y).EvaluateMultivariate(eval_point), 3);
  EXPECT_EQ((2 * x * x + y).EvaluateMultivariate(eval_point), 4);
  EXPECT_EQ((x * x + 2 * y).EvaluateMultivariate(eval_point), 5);
  EXPECT_EQ((x * x + x * y).EvaluateMultivariate(eval_point), 3);
  EXPECT_NEAR(pow_p_2.EvaluateMultivariate(eval_point),
              pow(p.EvaluateMultivariate(eval_point), 2), 1e-8);
  EXPECT_NEAR(pow_p_3.EvaluateMultivariate(eval_point),
              pow(p.EvaluateMultivariate(eval_point), 3), 1e-8);
  EXPECT_NEAR(pow_p_7.EvaluateMultivariate(eval_point),
              pow(p.EvaluateMultivariate(eval_point), 7), 1e-8);
  EXPECT_NEAR((pow_p_2 * pow_p_3 * pow_p_2).EvaluateMultivariate(eval_point),
              pow_p_7.EvaluateMultivariate(eval_point), 1e-8);
}

GTEST_TEST(PolynomialTest, Conversion) {
  // Confirm that these conversions compile okay.
  Polynomial<double> x(1.0);
  Polynomial<double> y = 2.0;
  Polynomial<double> z = 3;
}

GTEST_TEST(PolynomialTest, EvaluatePartial) {
  Polynomiald x = Polynomiald("x");
  Polynomiald y = Polynomiald("y");
  Polynomiald dut = (5 * x * x * x) + (3 * x * y) + (2 * y) + 1;

  const std::map<Polynomiald::VarType, double> eval_point_null;
  const std::map<Polynomiald::VarType, double> eval_point_x = {
    {x.GetSimpleVariable(), 7}};
  const std::map<Polynomiald::VarType, double> eval_point_y = {
    {y.GetSimpleVariable(), 11}};
  const std::map<Polynomiald::VarType, double> eval_point_xy = {
    {x.GetSimpleVariable(), 7},
    {y.GetSimpleVariable(), 11}};

  // Test a couple of straightforward explicit cases.
  EXPECT_EQ(dut.EvaluatePartial(eval_point_null).GetMonomials(),
            dut.GetMonomials());
  // TODO(#2216) These fail due to a known drake bug:
#if 0
  EXPECT_EQ(dut.evaluatePartial(eval_point_x).getMonomials(),
            ((23 * y) + 1716).getMonomials());
  EXPECT_EQ(dut.evaluatePartial(eval_point_y).getMonomials(),
            ((5 * x * x * x) + (33 * x) + 23).getMonomials());
#endif

  // Test that every order of partial and then complete evaluation gives the
  // same answer.
  const double expected_result = dut.EvaluateMultivariate(eval_point_xy);
  EXPECT_EQ(
      dut.EvaluatePartial(eval_point_null).EvaluateMultivariate(eval_point_xy),
      expected_result);
  EXPECT_EQ(
      dut.EvaluatePartial(eval_point_xy).EvaluateMultivariate(eval_point_null),
      expected_result);
  EXPECT_EQ(
      dut.EvaluatePartial(eval_point_x).EvaluateMultivariate(eval_point_y),
      expected_result);
  EXPECT_EQ(
      dut.EvaluatePartial(eval_point_y).EvaluateMultivariate(eval_point_x),
      expected_result);

  // Test that zeroing out one term gives a sensible result.
  EXPECT_EQ(dut.EvaluatePartial(
      std::map<Polynomiald::VarType, double>{{x.GetSimpleVariable(), 0}}),
            (2 * y) + 1);
  EXPECT_EQ(dut.EvaluatePartial(
      std::map<Polynomiald::VarType, double>{{y.GetSimpleVariable(), 0}}),
            (5 * x * x * x) + 1);
}

GTEST_TEST(PolynomialTest, FromExpression) {
  using symbolic::Environment;
  using symbolic::Expression;
  using symbolic::Variable;

  const Variable x{"x"};
  const Variable y{"y"};
  const Variable z{"z"};
  Environment env{{x, 1.0}, {y, 2.0}, {z, 3.0}};
  const map<Polynomiald::VarType, double> eval_point{
      {x.get_id(), env[x]}, {y.get_id(), env[y]}, {z.get_id(), env[z]}};

  const Expression e0{42.0};
  const Expression e1{pow(x, 2)};
  const Expression e2{3 + x + y + z};
  const Expression e3{1 + pow(x, 2) + pow(y, 2)};
  const Expression e4{pow(x, 2) * pow(y, 2)};
  const Expression e5{pow(x + y + z, 3)};
  const Expression e6{pow(x + y + z, 3) / 10};
  const Expression e7{-pow(y, 3)};
  const Expression e8{pow(pow(x, 3), 1.0 / 3)};

  EXPECT_NEAR(
      e0.Evaluate(env),
      Polynomial<double>::FromExpression(e0).EvaluateMultivariate(eval_point),
      1e-8);
  EXPECT_NEAR(
      e1.Evaluate(env),
      Polynomial<double>::FromExpression(e1).EvaluateMultivariate(eval_point),
      1e-8);
  EXPECT_NEAR(
      e2.Evaluate(env),
      Polynomial<double>::FromExpression(e2).EvaluateMultivariate(eval_point),
      1e-8);
  EXPECT_NEAR(
      e3.Evaluate(env),
      Polynomial<double>::FromExpression(e3).EvaluateMultivariate(eval_point),
      1e-8);
  EXPECT_NEAR(
      e4.Evaluate(env),
      Polynomial<double>::FromExpression(e4).EvaluateMultivariate(eval_point),
      1e-8);
  EXPECT_NEAR(
      e5.Evaluate(env),
      Polynomial<double>::FromExpression(e5).EvaluateMultivariate(eval_point),
      1e-8);
  EXPECT_NEAR(
      e6.Evaluate(env),
      Polynomial<double>::FromExpression(e6).EvaluateMultivariate(eval_point),
      1e-8);
  EXPECT_NEAR(
      e7.Evaluate(env),
      Polynomial<double>::FromExpression(e7).EvaluateMultivariate(eval_point),
      1e-8);
  EXPECT_NEAR(
      e8.Evaluate(env),
      Polynomial<double>::FromExpression(e8).EvaluateMultivariate(eval_point),
      1e-8);

  const vector<Expression> test_vec{
      log(x),
      abs(x),
      exp(x),
      sqrt(x),
      sin(x),
      cos(x),
      tan(x),
      asin(x),
      acos(x),
      atan(x),
      atan2(x, y),
      sinh(x),
      cosh(x),
      tanh(x),
      min(x, y),
      max(x, y),
      ceil(x),
      floor(x),
      if_then_else(x > y, x, y),
      Expression::NaN(),
      symbolic::uninterpreted_function("uf", {x, y})};
  for (const Expression& e : test_vec) {
    EXPECT_FALSE(e.is_polynomial());
    EXPECT_THROW(Polynomial<double>::FromExpression(e), runtime_error);
  }
}

template <typename T>
void TestScalarType() {
  Eigen::Vector3d coeffs(1., 2., 3.);
  const Polynomial<T> p(coeffs);
  EXPECT_NEAR(ExtractDoubleOrThrow(p.EvaluateUnivariate(0.0)), coeffs(0),
              1e-14);

  EXPECT_THROW(p.Roots(), std::runtime_error);
  EXPECT_TRUE(static_cast<bool>(p.CoefficientsAlmostEqual(p, 1e-14)));

  Polynomial<T> x("x", 1);
  Polynomial<T> y("y", 1);
  const std::map<Polynomiald::VarType, double> eval_point = {
    {x.GetSimpleVariable(), 1},
    {y.GetSimpleVariable(), 2}};
  EXPECT_NEAR(
      ExtractDoubleOrThrow((x * x + y).EvaluateMultivariate(eval_point)), 3,
      1e-14);
}

GTEST_TEST(PolynomialTest, ScalarTypes) {
  TestScalarType<AutoDiffXd>();
  TestScalarType<symbolic::Expression>();

  // Checks that we can create an instance, Polynomial<T>(0). `Scalar(0)` (where
  // Scalar = Polynomial<T>) is a common pattern in Eigen internals and we want
  // to make sure that we can build these instances.
  const Polynomial<double> p_double(0);
  const Polynomial<AutoDiffXd> p_autodiffxd(0);
  const Polynomial<symbolic::Expression> p_symbolic(0);
}

GTEST_TEST(PolynomialTest, CoefficientsAlmostEqualTest) {
  Polynomiald x = Polynomiald("x");
  Polynomiald y = Polynomiald("y");

  EXPECT_FALSE(x.CoefficientsAlmostEqual(y));
  EXPECT_TRUE((x + y).CoefficientsAlmostEqual(y + x));

  EXPECT_TRUE((x + x * x + 3 * pow(x, 3))
                  .CoefficientsAlmostEqual(3 * pow(x, 3) + x + x * x));

  // Test relative and absolute tolerance.
  EXPECT_TRUE(
      (100 * x).CoefficientsAlmostEqual(99 * x, 0.1, ToleranceType::kRelative));
  EXPECT_FALSE(
      (100 * x).CoefficientsAlmostEqual(99 * x, 0.1, ToleranceType::kAbsolute));
  EXPECT_FALSE((0.01 * x).CoefficientsAlmostEqual(0.02 * x, 0.1,
                                                  ToleranceType::kRelative));
  EXPECT_TRUE((0.01 * x).CoefficientsAlmostEqual(0.02 * x, 0.1,
                                                 ToleranceType::kAbsolute));

  // Test missing monomials.
  EXPECT_FALSE((x + y + 0.01 * x * x).CoefficientsAlmostEqual(x + y));
  EXPECT_FALSE((x + y).CoefficientsAlmostEqual(x + y + 0.01 * x * x));
  // Missing monomials is ok only for absolute tolerance.
  EXPECT_TRUE(
      (x + y + 0.01 * x * x)
          .CoefficientsAlmostEqual(x + y, 0.02, ToleranceType::kAbsolute));
  EXPECT_TRUE((x + y).CoefficientsAlmostEqual(x + y + 0.01 * x * x, 0.02,
                                              ToleranceType::kAbsolute));
  EXPECT_FALSE(
      (x + y + 0.01 * x * x)
          .CoefficientsAlmostEqual(x + y, 0.02, ToleranceType::kRelative));
  EXPECT_FALSE((x + y).CoefficientsAlmostEqual(x + y + 0.01 * x * x, 0.02,
                                               ToleranceType::kRelative));
}

GTEST_TEST(PolynomialTest, SubsitutionTest) {
  Polynomiald x = Polynomiald("x");
  Polynomiald y = Polynomiald("y");

  Polynomiald::VarType xvar = x.GetSimpleVariable();
  Polynomiald::VarType yvar = y.GetSimpleVariable();

  EXPECT_TRUE(x.Substitute(xvar, y).CoefficientsAlmostEqual(y));

  Polynomiald p1 = x + 3 * x * x + 3 * y;
  EXPECT_TRUE(
      p1.Substitute(xvar, 1 + x)
          .CoefficientsAlmostEqual(1 + x + 3 * (1 + x) * (1 + x) + 3 * y));
  EXPECT_TRUE(p1.Substitute(yvar, 1 + x)
                  .CoefficientsAlmostEqual(3 + 4 * x + 3 * x * x));
  EXPECT_TRUE(p1.Substitute(xvar, x * x)
                  .CoefficientsAlmostEqual(x * x + 3 * pow(x, 4) + 3 * y));
}

}  // anonymous namespace
}  // namespace drake


// neural_network.cpp example program using posit arithmetic for a neural network simulation
//
// Copyright (C) 2017-2018 Stillwater Supercomputing, Inc.
//
// This file is part of the universal numbers project, which is released under an MIT Open Source license.
#include "common.hpp"
// include the posit number system
#include <universal/posit/posit>
#define MTL_WITH_INITLIST 1
#include <boost/numeric/mtl/mtl.hpp>

#undef NOW
#ifdef NOW
template<size_t nbits, size_t es>
sw::unum::posit<nbits, es> Sigmoid_(sw::unum::posit<nbits, es>& x, bool derivative = false) {
	if (derivative) return x*(1-x);
	return (1);
}

template<typename Scalar>
Scalar sigmoid(Scalar x) {
	return (1.0f / (1.0f + exp(-x)));
}

template<typename Vector, typename Scalar>
Scalar f_theta(Scalar x, Scalar b, const Vector& V, const Vector& W) {
	double result = b;
	for (int i = 0; i < N; ++i) {
		result += V[i] * sigmoid(c[i] + W[i] * x);
	}
	return result;
}

template<typename Vector, typename Scalar>
void train(Scalar x, Scalar y, Vector& W, Vector& V, Vector& c) {
	for (int i = 0; i < size(W); ++i) {
		W[i] = W[i] - epsilon * 2 * (f_theta(x) - y) * V[i] * x *
			(1 - sigmoid(c[i] + W[i] * x)) * sigmoid(c[i] + W[i] * x);
	}
	for (int i = 0; i < size(V); ++i) {
		V[i] = V[i] - epsilon * 2 * (f_theta(x) - y) * sigmoid(c[i] + W[i] * x);
	}
	b = b - epsilon * 2 * (f_theta(x) - y);
	for (int i = 0; i < size(c); ++i) {
		c[i] = c[i] - epsilon * 2 * (f_theta(x) - y) * V[i] *
			(1 - sigmoid(c[i] + W[i] * x)) * sigmoid(c[i] + W[i] * x);
	}
}

// Multi-Level Perceptron
// one input layer
// one hidden layer with NrOfNeurons.
// A \(sigmoid\\) function as activation function
// one output layer
int main(int argc, char** argv)
try {
	using namespace std;
	using namespace mtl;
	using namespace sw::unum;

	const size_t nbits = 16;
	const size_t es = 1;
	const size_t vecSize = 32;

	using Scalar = float;
	using Matrix = mtl::dense2D<Scalar>;
	using Vector = mtl::dense_vector<Scalar>;
	using Pair   = std::pair<Scalar, Scalar>;
	using VoP    = mtl::dense_vector<Pair>;

#if defined(MTL_WITH_INITLIST) && defined(MTL_WITH_AUTO) && defined(MTL_WITH_RANGEDFOR)
	Matrix X = {
		{0,0,1},
		{0,1,1},
		{1,0,1},
		{1,1,1}
	};
#endif

	constexpr int NrOfTrainingSamples = 20;
	constexpr int NrOfNeurons = 5;
	constexpr float epsilon = 0.05f;
	constexpr int epoch = 50000;

	Vector c(NrOfNeurons), W(NrOfNeurons), V(NrOfNeurons);
	c = W = V = 0;
	Scalar b = 0;

	// fill initial state with random values
	srand((unsigned int)time(NULL));
	for (int i = 0; i < NrOfNeurons; i++) {
		W[i] = Scalar(2 * rand() / RAND_MAX - 1);
		V[i] = Scalar(2 * rand() / RAND_MAX - 1);
		c[i] = Scalar(2 * rand() / RAND_MAX - 1);
	}
	VoP trainingSet(NrOfTrainingSamples);

	for (int i = 0; i < NrOfTrainingSamples; i++) {
		trainingSet[i] = make_pair(i * 2 * m_pi / NrOfTrainingSamples, sin(i * m_2_pi / NrOfTrainingSamples));
	}

	// train for epoch number of cycles
	for (int k = 0; k < epoch; ++k) {
		for (int i = 0; i < NrOfTrainingSamples; i++) {
			auto x = trainingSet[i].first;
			auto y = trainingSet[i].second;
			train(x,y, W, V, c);
		}
		std::cout << k << "\r";
	}

	// Plot the results
	int nrVisualSamples = 1000;
	Vector x(nrVisualSamples);
	Vector y1(nrVisualSamples), y2(nrVisualSamples);

	auto scaling_factor = m_2pi / nrVisualSamples;
	for (int i = 0; i < nrVisualSamples; i++) {
		x[i] =  i * scaling_factor;
		y1[i] = sin(i * scaling_factor);
		y2[i] = f_theta(Scalar(i * scaling_factor), b, V, W);
	}

	FILE * gp = _popen("gnuplot", "w");
	fprintf(gp, "set terminal wxt size 600,400 \n");
	fprintf(gp, "set grid \n");
	fprintf(gp, "set title '%s' \n", "f(x) = sin (x)");
	fprintf(gp, "set style line 1 lt 3 pt 7 ps 0.1 lc rgb 'green' lw 1 \n");
	fprintf(gp, "set style line 2 lt 3 pt 7 ps 0.1 lc rgb 'red' lw 1 \n");
	fprintf(gp, "plot '-' w p ls 1, '-' w p ls 2 \n");

	// Exact f(x) = sin(x) -> Green Graph
	for (int k = 0; k < nrVisualSamples; ++k) {
		fprintf(gp, "%f %f \n", x[k], y1[k]);
	}
	fprintf(gp, "e\n");

	//Neural Network Approximate f(x) = sin(x) -> Red Graph
	for (int k = 0; k < nrVisualSamples; ++k) {
		fprintf(gp, "%f %f \n", x[k], y2[k]);
	}
	fprintf(gp, "e\n");

	fflush(gp);

	return EXIT_SUCCESS;
}
catch (char const* msg) {
	std::cerr << msg << std::endl;
	return EXIT_FAILURE;
}
catch (const posit_arithmetic_exception& err) {
	std::cerr << "Uncaught posit arithmetic exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const quire_exception& err) {
	std::cerr << "Uncaught quire exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const posit_internal_exception& err) {
	std::cerr << "Uncaught posit internal exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (std::runtime_error& err) {
	std::cerr << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (...) {
	std::cerr << "Caught unknown exception" << std::endl;
	return EXIT_FAILURE;
}

#else
int main() {}
#endif


#pragma once
#ifndef CALC_TVALUE_FP_HPP
#define CALC_TVALUE_FP_HPP

//#include <NTL/matrix.h>
#include <NTL/lzz_p.h>
#include <NTL/mat_lzz_p.h>
#include "combination.hpp"

int calc_tvalue(NTL::Mat<NTL::zz_p> array[], int dim, int max_tvalue)
{
    using namespace std;
    using namespace NTL;

    uint32_t s = dim;
    uint32_t m = array[0].NumRows();
    Combination index(s);
    int t = 0;
    for (; t <= max_tvalue; t++) {
        index.reset(m - t);
#if defined(DEBUG)
        cout << "t = " << dec << t << endl;
#endif
        //vector<uint32_t> v;
        bool pass = true;
        for (;;) {
            Mat<zz_p> v;
            v.SetDims(m - t, m);
#if defined(DEBUG)
            index.print(cout);
#endif
            int vrow = 0;
            for (uint32_t i = 0; i < s; ++i) {
                for (int j = 0; j < index[i]; ++j) {
                    v[vrow++] = array[i][j];
                }
            }
            if (gauss(v) != m-t) {
                pass = false;
                break;
            }
            bool hasNext = index.next();
            if (!hasNext) {
                break;
            }
        }
        if (pass) {
            return t;
        }
    }
    return -1;
}

#endif // CALC_TVALUE_FP_HPP


#include <Eigen/Core>
#include <Eigen/Dense>
#include <armadillo>
#include <boost/random.hpp>
#include <boost/math/distributions.hpp>
#include "Sampling_functions.h"

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

//boost::random::mt19937 gen(0);
boost::random::mt19937 gen(time(0));
//distributions
double runif(double lower, double higher)
{
	boost::random::uniform_real_distribution<> dist(lower, higher);
	return dist(gen);
}

double rnorm(double mean, double sd)
{
	boost::random::normal_distribution<> dist(mean, sd);
	return dist(gen);
}

double rgamma(double alpha, double beta)
{

	boost::math::gamma_distribution<> dist(alpha, beta);
	double q = quantile(dist, runif(0,1));

	return(q);
}

double rbeta(double alpha, double beta)
{

	boost::math::beta_distribution<> dist(alpha, beta);
	double q = quantile(dist, runif(0,1));

	return(q);
}

double rinvchisq(double df, double scale)
{

	boost::math::inverse_chi_squared_distribution<> dist(df, scale);
	double q = quantile(dist, runif(0,1));

	return(q);
}
int rbernoulli(double p)
{
	std::bernoulli_distribution dist(p);
	return dist(gen);
}

//switch between armadillo and eigen
Eigen::MatrixXd cast_eigen(arma::mat arma_A)
{
	Eigen::MatrixXd eigen_B = Eigen::Map<Eigen::MatrixXd>(arma_A.memptr(),
			arma_A.n_rows,
			arma_A.n_cols);

	return eigen_B;
}

arma::mat cast_arma(Eigen::MatrixXd& eigen_A)
{
	arma::mat arma_B = arma::mat(eigen_A.data(), eigen_A.rows(), eigen_A.cols(),false, false);
	return arma_B;
}

//sampling functions
double sample_hyper(const MatrixXd& w1_M1_sample, const MatrixXd& WI_m, double b0_m, const VectorXd& mu0_m, int df_m,VectorXd& mu_m,MatrixXd& lambda_m)
{
	//Sample from individual or SNP hyperparams (equation 14)
	int N = w1_M1_sample.rows();
	int num_feat=w1_M1_sample.cols();
	VectorXd u_bar(num_feat);
	MatrixXd S_bar(num_feat,num_feat);

	u_bar = w1_M1_sample.colwise().mean().transpose();
	//covariance to correlation
	S_bar = (w1_M1_sample.transpose()*w1_M1_sample)/N;

	//Gaussian-Wishard sampling
	//1.sample lambda from wishard distribution (lambda_m or lambda_u)
	//2.sample mu (mu_m or mu_u) from multivariate normal distribution with mean mu_0 (mu_temp) and variance (lambda*Lambda)^-1

	//Wishard-covariance matrix
	MatrixXd WI_post(num_feat,num_feat);
	WI_post = WI_m.inverse() + N*S_bar+((N*b0_m)/(b0_m+N))*((mu0_m - u_bar)*(mu0_m - u_bar).transpose());

	//WI_post = (WI_post + WI_post.transpose())/2;

	WI_post=WI_post.inverse();

	//Wishard-degrees of freedom
	int df_mpost = df_m+N;

	//Wishard draw, using the armadillo function. So I cast to an armadillo matrix, draw wishart then cast back to eigen matrix.
	//should just code the wishart function.
	mat arma_lambda =cast_arma(WI_post);
	mat W = wishrnd(arma_lambda, df_mpost);
	lambda_m=cast_eigen(W);

	//multivariate normal mean
	VectorXd mu_temp(num_feat);
	mu_temp = (b0_m*mu0_m + N*u_bar)/(b0_m+N);

	//Multivariate normal with mean mu_temp and cholesky decomposed covariance lam
	MatrixXd lam = (((b0_m+N)*lambda_m).inverse());

	lam=lam.llt().matrixU();
	lam.transposeInPlace();

	MatrixXd normV(num_feat,1);
	for (int i=0;i<num_feat;i++){
		normV(i,0)=rnorm(0,1);
	}

	mu_m = lam*normV+mu_temp;

	return 0;
}

double sample_ind (double Xglobalmean,const MatrixXd& w1_M1_sample, MatrixXd& w1_P1_sample, const MatrixXd& X,int num_p,int num_feat,const MatrixXd& lambda_u,const VectorXd& mu_u,const VectorXd& alpha)
{
	//random shuffling of individuals.
	std::vector<int> I;
	for (int i=0; i<num_p; ++i) {
		I.push_back(i);
	}

	std::random_shuffle(I.begin(), I.end());

	// Gibbs updates over individual latent vectors given hyperparams.
	// Infer posterior distribution over individual latent vectors (equation 11).
	MatrixXd covarprod = w1_P1_sample.transpose()*w1_P1_sample;
	MatrixXd lambda_mu_prod=lambda_u*mu_u;

	for (int i=0;i<num_p;i++){
		//observed data row
		VectorXd rr = X.row(I[i]).array();
		//equation 12
		MatrixXd covar = alpha[i]*covarprod+lambda_u;
		covar=covar.inverse();
		//equation 13
		VectorXd mean_u = covar * (alpha[i]*(w1_M1_sample.transpose()*rr)+lambda_mu_prod);
		//multivariate normal with mean mean_u and cholesky decomposed variance lam
		MatrixXd lam = covar.llt().matrixU();
		lam.transposeInPlace();

		MatrixXd normV(num_feat,1);
		for (int j=0;j<num_feat;j++){
			normV(j,0)=rnorm(0,1);
		}

		w1_P1_sample.row(I[i]) = (lam*normV+mean_u).transpose();

	}

	return 0;
}

double sample_SNP (double Xglobalmean,const MatrixXd& w1_P1_sample, MatrixXd& w1_M1_sample, const MatrixXd& X,int num_m,int num_feat,const MatrixXd& lambda_m,const VectorXd& mu_m,double alpha)
{
	//random shuffling of markers.
	std::vector<int> I;
	for (int i=0; i<num_m; ++i) {
		I.push_back(i);
	}

	std::random_shuffle(I.begin(), I.end());
	//equation 12
	MatrixXd covar = ((alpha*(w1_P1_sample.transpose()*w1_P1_sample)+lambda_m));
	covar=covar.inverse();
	MatrixXd lam = covar.llt().matrixU();
	lam.transposeInPlace();
	MatrixXd lambda_mu_prod=lambda_m*mu_m;

	for (int i=0;i<num_m;i++){
		//observed data column
		VectorXd rr = X.col(I[i]).array();

		//equation 13
		VectorXd mean_m = covar * (alpha*(w1_P1_sample.transpose()*rr)+lambda_mu_prod);
		//multivariate normal with mean mean_m and cholesky decomposed variance lam
		MatrixXd normV(num_feat,1);
		for (int j=0;j<num_feat;j++){
			normV(j,0)=rnorm(0,1);
		}
		w1_M1_sample.row(I[i]) = (lam*normV+mean_m).transpose();
	}

	return 0;
}

double sample_ind_missing (double Xglobalmean,const MatrixXd& w1_M1_sample, MatrixXd& w1_P1_sample,const MatrixXd& Indicator, const MatrixXd& X,int num_p,int num_m,int num_feat,const MatrixXd& lambda_u,const VectorXd& mu_u,const VectorXd& alpha)
{
	//random shuffling of individuals.
	std::vector<int> I;
	for (int i=0; i<num_p; ++i) {
		I.push_back(i);
	}

	std::random_shuffle(I.begin(), I.end());

	// Gibbs updates over individual latent vectors given hyperparams.
	// Infer posterior distribution over individual latent vectors (equation 11).

	for (int i=0;i<num_p;i++){
		//observed data row
		VectorXd rr = X.row(I[i]).array();
		//equation 12
		MatrixXd covarprod(num_feat,num_feat);
		VectorXd obsprod(num_feat);
		MatrixXd lambda_mu_prod=lambda_u*mu_u;

		covarprod.setZero();
		obsprod.setZero();
		for (int j=0;j<num_m;j++){
			covarprod=covarprod + (w1_M1_sample.row(j).transpose()*w1_M1_sample.row(j))*Indicator(i,j);
			obsprod=obsprod + (w1_M1_sample.row(j).transpose()*rr[j])*Indicator(i,j);
		}

		MatrixXd covar = alpha[i]*covarprod+lambda_u;
		covar=covar.inverse();
		//equation 13
		VectorXd mean_u = covar * (alpha[i]*obsprod+lambda_mu_prod);
		//multivariate normal with mean mean_u and cholesky decomposed variance lam
		MatrixXd lam = covar.llt().matrixU();
		lam.transposeInPlace();

		MatrixXd normV(num_feat,1);
		for (int j=0;j<num_feat;j++){
			normV(j,0)=rnorm(0,1);
		}

		w1_P1_sample.row(I[i]) = (lam*normV+mean_u).transpose();

	}

	return 0;
}

double sample_SNP_missing (double Xglobalmean,const MatrixXd& w1_P1_sample, MatrixXd& w1_M1_sample, const MatrixXd& Indicator,const MatrixXd& X,int num_p,int num_m,int num_feat,const MatrixXd& lambda_m,const VectorXd& mu_m,double alpha)
{
	//random shuffling of markers.
	std::vector<int> I;
	for (int i=0; i<num_m; ++i) {
		I.push_back(i);
	}

	std::random_shuffle(I.begin(), I.end());

	for (int i=0;i<num_m;i++){
		//observed data column
		VectorXd rr = X.col(I[i]).array();
		//equation 12
		MatrixXd covarprod(num_feat,num_feat);
		VectorXd obsprod(num_feat);
		MatrixXd lambda_mu_prod=lambda_m*mu_m;

		covarprod.setZero();
		obsprod.setZero();
		for (int j=0;j<num_p;j++){
			covarprod=covarprod + (w1_P1_sample.row(j).transpose()*w1_P1_sample.row(j))*Indicator(j,i);
			obsprod=obsprod + (w1_P1_sample.row(j).transpose()*rr[j])*Indicator(j,i);
		}

		MatrixXd covar = alpha*covarprod+lambda_m;
		covar=covar.inverse();

		//equation 13
		VectorXd mean_m= covar * (alpha*obsprod+lambda_mu_prod);
		//multivariate normal with mean mean_m and cholesky decomposed variance lam
		MatrixXd lam = covar.llt().matrixU();
		lam.transposeInPlace();

		MatrixXd normV(num_feat,1);
		for (int j=0;j<num_feat;j++){
			normV(j,0)=rnorm(0,1);
		}
		w1_M1_sample.row(I[i]) = (lam*normV+mean_m).transpose();
	}

	return 0;
}

double sample_ind_SNP (MatrixXd& w1_M1_sample, MatrixXd& w1_P1_sample, const MatrixXd& X,int num_p,int num_m,int num_feat,const MatrixXd& lambda_u,const VectorXd& mu_u,const MatrixXd& lambda_m,const VectorXd& mu_m,double alpha)
{
	//random shuffling of individuals.
	std::vector<int> I;
	for (int i=0; i<num_p; ++i) {
		I.push_back(i);
	}
	std::vector<int> J;
	for (int i=0; i<num_m; ++i) {
		J.push_back(i);
	}

	std::random_shuffle(I.begin(), I.end());
	std::random_shuffle(J.begin(), J.end());

	// Gibbs updates over individual latent vectors given hyperparams.
	// Infer posterior distribution over individual latent vectors (equation 11).
	int upd_row=0;
	int upd_col=0;
	int i=0;
	while (i<num_p+num_m){

		if(upd_row<num_p){
			//observed data row
			VectorXd rr = X.row(I[upd_row]);
			//equation 12
			MatrixXd covar = ((alpha*(w1_M1_sample.transpose()*w1_M1_sample)+lambda_u));
			covar=covar.inverse();
			//equation 13
			VectorXd mean_u = covar * (alpha*w1_M1_sample.transpose()*rr+lambda_u*mu_u);
			//multivariate normal with mean mean_u and cholesky decomposed variance lam
			MatrixXd lam = covar.llt().matrixU();
			lam.transposeInPlace();

			MatrixXd normV(num_feat,1);
			for (int j=0;j<num_feat;j++){
				normV(j,0)=rnorm(0,1);
			}

			w1_P1_sample.row(I[upd_row]) = (lam*normV+mean_u).transpose();
			upd_row=upd_row+1;
			i=i+1;
		}

		if(upd_col<num_m){
			//observed data column
			VectorXd rr = X.col(J[upd_col]);
			//equation 12
			MatrixXd covar = ((alpha*(w1_P1_sample.transpose()*w1_P1_sample)+lambda_m));
			covar=covar.inverse();
			//equation 13
			VectorXd mean_m = covar * (alpha*w1_P1_sample.transpose()*rr+lambda_m*mu_m);
			//multivariate normal with mean mean_m and cholesky decomposed variance lam
			MatrixXd lam = covar.llt().matrixU();
			lam.transposeInPlace();

			MatrixXd normV(num_feat,1);
			for (int j=0;j<num_feat;j++){
				normV(j,0)=rnorm(0,1);
			}
			w1_M1_sample.row(J[upd_col]) = (lam*normV+mean_m).transpose();
			upd_col=upd_col+1;
			i=i+1;
		}

	}

	return 0;
}

double sample_residual_variance_gamma(const MatrixXd& epsilon)
{
	int N=epsilon.rows()*epsilon.cols();
	double res=epsilon.squaredNorm();
	double sigma2=1/rgamma((N-1)/2,1/(0.5*res));
	//cout<<"residual info"<<endl;
	//cout<<N<<"\t"<<res<<"\t"<<sigma2<<endl;
	return sigma2;
}

double sample_residual_row_variance_gamma(const MatrixXd& epsilon, VectorXd& alpha)
{
	int N=epsilon.rows();
	int M=epsilon.cols();
	double res=0;
	for (int i=0;i<N;i++){
	res=epsilon.row(i).squaredNorm();
	alpha[i]=1/rgamma((M-1)/2,1/(0.5*res));
	}

	return 0;
}


#include <iostream>
#include <ctime>
#include <Eigen/Core>  // Eigen 部分
#include <Eigen/Dense> // 稠密矩阵的代数运算
using namespace std;

#define MATRIX_SIZE 50

/********************************************
 * 本程序演示Eigen的基本类型的使用
 * *****************************************/

int main(int argc, char** argv)
{
    Eigen::Matrix<float,2,3> matrix_23;
    Eigen::Vector3d v_3d;
    Eigen::Matrix<float,3,1> vd_3d;

    Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero();
    Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> matrix_dynamic;
    Eigen::MatrixXd matrix_x;

    matrix_23 << 1, 2, 3, 4, 5, 6;
    cout<< matrix_23<<endl;

    for(int i=0; i<2; i++)
    {
        for(int j=0; j<3; j++)
            cout<<matrix_23(i,j)<<"\t";
        cout<<endl;
    }

    v_3d << 3, 2, 1;
    vd_3d << 4, 5, 6;
    Eigen::Matrix<double,2,1> result = matrix_23.cast<double>() * v_3d;
    cout << result << endl;

    // Eigen::Matrix<double,2,1> result2 = matrix_23 * vd_3d;
    // cout << result2 << endl;

    // 四则运算直接用+-*/
    // 下面为其他矩阵运算
    matrix_33  = Eigen::Matrix3d::Random();
    cout << matrix_33 << endl;

    cout << matrix_33.transpose() << endl;
    cout << matrix_33.sum() << endl;
    cout << matrix_33.trace() << endl;
    cout << 10*matrix_33 <<endl;
    cout << matrix_33.inverse() << endl;
    cout << matrix_33.determinant() << endl;

    // 特征值
    //实对称矩阵可以保证对角化成功
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver
        (matrix_33.transpose()*matrix_33);
    cout << "Eigen values = \n"<< eigen_solver.eigenvalues()<<endl;
    cout << "Eigen vectors = \n"<< eigen_solver.eigenvectors()<<endl;

    // 解方程
    // 求解matrix_NN * x = v_Nd这个方程
    // N 由前面的宏定义
    // 直接求解最直接,但是求逆运算量大

    Eigen::Matrix<double,MATRIX_SIZE,MATRIX_SIZE> matrix_NN;
    matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE);
    Eigen::Matrix<double,MATRIX_SIZE,1> v_Nd;
    v_Nd = Eigen::MatrixXd::Random(MATRIX_SIZE,1);

    clock_t time_stt = clock(); // start

    // 直接求逆
    Eigen::Matrix<double,MATRIX_SIZE,1> x = matrix_NN.inverse()*v_Nd;
    cout << "time use in normal inverse is " << 1000*(clock()-time_stt)/
        (double)CLOCKS_PER_SEC << "ms" <<endl;

    // 通常用矩阵分解的方式求
    time_stt = clock();
    x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
    cout << "time use in Qr decomposition is " << 1000*(clock()-time_stt)/
        (double)CLOCKS_PER_SEC << "ms" <<endl;

    return 0;

}


#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

int main(int, char**)
{
    using namespace mtl;

    double           array[][3]= {{1., 0., 3.}, {4., 0., 6.}, {7., 0., 9.}, {0., 0., 0.}};
    dense2D<double>  A(array), B2, B3;

    // Creating a compression (reordering) matrix from a vector (or an array respectively)
    int non_zero_rows[]= {0, 1, 2}, non_zero_columns[]= {0, 2};
    // To be sure, we give the number of columns for a consistent size!
    mat::traits::reorder<>::type RR= mat::reorder(non_zero_rows, 4), RC= mat::reorder(non_zero_columns);
    std::cout << "A =\n" << A << "\nRR =\n" << RR << "\nRC =\n" << RC;

    // Compress rows
    B2= RR * A;
    std::cout << "\nRR * A, i.e. compress row of A =\n" << B2;

    // Compress columns
    B3= B2 * trans(RC);
    std::cout << "\nB2 * trans(RC), i.e. compress columns of B2 =\n" << B3;

    // Decompress rows
    dense2D<double>  C1(trans(RR) * B3);
    std::cout << "\ntrans(RR) * B3, i.e. row decompression of B3 =\n" << C1;

    // Decompress columns
    dense2D<double>  C2(C1 * RC);
    std::cout << "\nC1 * RC, i.e. column decompression of C1 (should be A) =\n" << C2;

    return 0;
}


#include <iostream>
#include <tuple>
#include <Eigen/Dense>
#include "linear_algebra_addon.hpp"
using namespace Eigen;
using namespace std;

MatrixXcd bl_cocg_rq(const MatrixXcd& A, const MatrixXcd& B, const double& tol, const int& itermax)
{
// only for complex-symmetric matrix
// Gu et al 2016, arXiv, Block variants of COCG and COCR methods
// for solving complex symmetric linear systems with multiple right-hand sides

  double Bnorm= B.norm();
  MatrixXcd X= MatrixXcd::Zero(B.rows(),B.cols()); // Initial guess of X (zeros)

  MatrixXcd Q;
  MatrixXcd xi;
  tie(Q,xi)= qr_reduced(B-A*X);
  MatrixXcd S= Q;

  for(int k= 0; k < itermax; ++k){
      MatrixXcd AS= A*S;
      MatrixXcd alpha= (S.transpose()*AS).fullPivLu().solve(Q.transpose()*Q);
      X= X+S*alpha*xi;
      MatrixXcd Qnew;
      MatrixXcd tau;
      tie(Qnew,tau)= qr_reduced(Q-AS*alpha);
      xi= tau*xi;
      double err= xi.norm()/Bnorm;
      cout << "bl_cocg_rq: " << "iter= " << k << " relative err= " << err << endl;
      if(err < tol) break;
      MatrixXcd beta= (Q.transpose()*Q).fullPivLu().solve(tau.transpose()*Qnew.transpose()*Qnew);
      Q= Qnew;
      S= Q+S*beta;
  }

  if((A*X-B).norm()/Bnorm > 10*tol){
      cerr << "bl_cocg_rq did not converge to solution within error tolerance !" << endl;
     // exit(EXIT_FAILURE);
  }

  return X;
}


#include <iostream>
#include <cassert>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;
using namespace mtl;

typedef dense2D<double> Matrix;
// using Matrix= dense2D<double>; // C++11

dense_vector<double> unit_vector(unsigned k, unsigned n)
{
    dense_vector<double>  e_k(n, 0.0);
    e_k[k]= 1.0;

    // for (unsigned i= 0; i < n; ++i)
    // 	   e_k[k]= static_cast<double>(i == k);
    // for (unsigned i= 0; i < n; ++i)
    // 	   e_k[k]= i == k;

    return e_k;
}

Matrix inverse_upper(const Matrix& U)
{
    const unsigned n= num_rows(U);
    Matrix Inv(n, n);
    Inv= 0.0;
    for (unsigned k= 0; k < n; k++) {
	irange  r(0,k+1);
	Inv[r][k]= upper_trisolve(U[r][r], unit_vector(k, k+1));
    }
    return Inv;
}

Matrix inverse_lower(const Matrix& L)
{
    Matrix T(trans(L));
    return Matrix(trans(inverse_upper(T)));
}

Matrix inverse(const Matrix& A)
{
    const unsigned n= num_rows(A);
    if (num_cols(A) != n)
	throw "Matrix must be square";

    Matrix Inv(n, n);

    Matrix LU(A); // Kopie von A
    dense_vector<unsigned>  Pv(n);
    lu(LU, Pv);
    compressed2D<unsigned> P(permutation(Pv));
    Matrix I(mat::identity(n, n)), L(I + strict_lower(LU)), U(upper(LU));
    Inv= inverse_upper(U) * inverse_lower(L) * P;

    return Inv;
}

int main (int argc, char* argv[])
{
    const unsigned size= 3;
    const double   eps= 0.001; // C++11: constexpr double   eps= 0.001;
    Matrix   A(size, size);
    A= 4, 1, 2,
       1, 5, 3,
       2, 6, 9;

    cout << "A ist\n" << A;

    Matrix LU(A); // Kopie von A
    dense_vector<unsigned>  Pv(size);
    lu(LU, Pv);
    Matrix P(permutation(Pv));

    cout << "LU ist\n" << LU;
    cout << "Permutation vector is " << Pv << "\nPermutationsmatrix ist\n" << P;

    Matrix I(mat::identity(size, size)), L(I + strict_lower(LU)), U(upper(LU));
    cout << "L ist\n" << L << "U ist\n" << U;

    Matrix UI(inverse_upper(U));
    cout << "Inverse von U ist\n" << UI << "UI * U ist\n" << Matrix(UI * U);
    assert(one_norm(Matrix(UI * U - I)) < eps);

    Matrix LI(inverse_lower(L));
    cout << "Inverse von L ist\n" << LI << "LI * L ist\n" << Matrix(LI * L);
    assert(one_norm(Matrix(LI * L - I)) < eps);

    Matrix AI(UI * LI * P);
    cout << "Inverse von A ist\n" << AI << "AI * A ist\n" << Matrix(AI * A);
    assert(one_norm(Matrix(AI * A - I)) < eps);

    Matrix A_inverse(inverse(A));
    cout << "Inverse von A ist\n" << A_inverse << "A_inverse * A ist\n" << Matrix(A_inverse * A);
    assert(one_norm(Matrix(A_inverse * A - I)) < eps);

    return 0 ;
}


///////////////////////////////////////////////////////////////////////////////
// math::logit_shift.hpp                                                     //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_MATH_FUNCTION_LOGIT_SHIFT_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_MATH_FUNCTION_LOGIT_SHIFT_HPP_ER_2009
#include <boost/statistics/detail/math/function/log_shift.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace math{

    // The t>0 is an adjustment which preventsa numerical error at p = 0 and
    // p = 1 causes numerical error
    template<typename T>
    T logit_shift(const T& p,const T& t){
        // log(p/(1-p)) = log(p)-log(1-p)
        return log_shift(p,t) - boost::math::log1p(t - p);
    }

}// math
}// detail
}// statistics
}// boost

#endif


#include "triangulation.h"

#include "defines.h"

#include <Eigen/SVD>

// Используем DLT метод, составляем систему уравнений. Система похожа на систему для гомографии, там пары уравнений получались из выражений вида x (cross) Hx = 0, а здесь будет x (cross) PX = 0
// (см. Hartley & Zisserman p.312)
cv::Vec4d phg::triangulatePoint(const cv::Matx34d *Ps, const cv::Vec3d *ms, int count)
{
    using mat = Eigen::MatrixXd;
    using vec = Eigen::VectorXd;

    mat A(2 * count, 4);

    for (int i = 0; i < count; ++i) {

        const matrix34d &P = Ps[i];
        const vector3d &m = ms[i];

        double x = m[0];
        double y = m[1];
        double z = m[2];

        auto p0 = P.row(0);
        auto p1 = P.row(1);
        auto p2 = P.row(2);

        auto row0 = x * p2 - z * p0;
        auto row1 = y * p2 - z * p1;

        A.row(i * 2 + 0) << row0(0), row0(1), row0(2), row0(3);
        A.row(i * 2 + 1) << row1(0), row1(1), row1(2), row1(3);
    }

    Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::VectorXd null_space = svd.matrixV().col(3);

    vector4d result;
    for (int i = 0; i < 4; ++i) {
        result(i) = null_space(i);
    }

    return result;
}


//=============================================================================
// Daniel J. Greenhoe
// normed linear space R^2
//=============================================================================
//=====================================
// headers
//=====================================
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <vector>
#include <complex>
#include <Eigen/Dense>
#include "r1.h"
#include "r2.h"
#include "r2op.h"

//-----------------------------------------------------------------------------
//! List value of opair
//! \param[in]  str1 : string to print before the sequence
//! \param[in]  str2 : string to print after the sequence
//! \param[out] ptr  : output string
//-----------------------------------------------------------------------------
void opair::list(const char *str1, const char *str2, FILE *ptr) const
{
  if( strlen(str1)>0 ) fprintf(ptr,"%s",str1);
  fprintf(ptr,"(%9.6lf,%9.6lf)",getx(),gety());
  if( strlen(str2)>0 ) fprintf(ptr,"%s",str2);
}

//-----------------------------------------------------------------------------
//! \brief polar rotation coordinate <theta> of opair point (x,y)
//! \returns Return value is in the half open interval [0:2pi),
//!          -1 on error
//-----------------------------------------------------------------------------
double vectR2::theta(void) const
{
  const double x = getx();
  const double y = gety();
  if(x==0){
    if(     y == 0 ) return -1           ;
    else if(y > 0  ) return  1 * M_PI / 2;
    else             return  3 * M_PI / 2;
  }
  if(y==0){
    if(     x>0    ) return 0;
    else             return M_PI;
  }
  if( x>0 && y>0 ) return        atan( y/x); // 1st quadrant
  if( x>0 && y<0 ) return 2*M_PI-atan(-y/x); // 4th quadrant
  if( x<0 && y>0 ) return   M_PI-atan(-y/x); // 2nd quadrant
  if( x<0 && y<0 ) return   M_PI+atan( y/x); // 3rd quadrant
  else             return -1;
}

//-----------------------------------------------------------------------------
//! \brief Operator to rotate vector R2 [x,y] counter-clockwise by <phi> radians
//! \param[in]  phi: angle to rotate vector by
//-----------------------------------------------------------------------------
void vectR2::operator&=(const double phi)
{
  vectR2  p(getx(),gety());
  p = p & phi;
  put(p.getx(),p.gety());
}

//=====================================
// seqR2
//=====================================
//-----------------------------------------------------------------------------
//! \brief Constructor initializing seqR1 to 0s
//! \param[in]  M: Length of sequence
//-----------------------------------------------------------------------------
seqR2::seqR2(const long M)
{
  N  = M;
  xy = new vectR2[N];
  clear();
}

//-----------------------------------------------------------------------------
//! \brief constructor initializing seqR1 to u
//! \param[in]  M: Length of sequence
//! \param[in]  u: Initialization value
//-----------------------------------------------------------------------------
seqR2::seqR2(long M, double u)
{
  long n;
  N=M;
  xy = new vectR2[N];
  for(n=0; n<N; n++)
  {
    xy[n].put(u,u);
  }
}

//-----------------------------------------------------------------------------
//! \brief Fill the sequence with a value 0
//-----------------------------------------------------------------------------
void seqR2::clear(void)
{
  for( long n=0; n<N; n++ ) xy[n].clear();
}

//-----------------------------------------------------------------------------
//! \brief Fill the sequence with a value <u>
//-----------------------------------------------------------------------------
void seqR2::fill(double u)
{
  long n;
  for(n=0; n<N; n++) xy[n].put(u,u);
}

//-----------------------------------------------------------------------------
//! \brief Fill the sequence with (x_0, x_1, x_2, ...)
//!        where x_n = x_{n-1} + (dx,dy)
//-----------------------------------------------------------------------------
void seqR2::inc(double x0, double y0,double dx, double dy)
{
  for( long n=0; n<N; n++ )
  {
    xy[n].put(x0,y0);
    x0 += dx;
    y0 += dy;
  }
}

//-----------------------------------------------------------------------------
//! \brief Put an order pair (u,v) into the sequence x at location n
//-----------------------------------------------------------------------------
int seqR2::put(long n, double u, double v)
{
  int retval=0;
  if(n>=N){
    fprintf(stderr,"n=%ld larger than seqR1 size N=%ld\n",n,N);
    retval=-1;
    }
  else xy[n].put(u,v);
  return retval;
}

//-----------------------------------------------------------------------------
//! \brief Put an R2 vector xyz into the sequence x at location n
//-----------------------------------------------------------------------------
int seqR2::put(long n, vectR2 xya)
{
  int retval=0;
  if(n>=N){
    fprintf(stderr,"n=%ld larger than seqR1 size N=%ld\n",n,N);
    retval=-1;
    }
  else xy[n]=xya;
  return retval;
}

//-----------------------------------------------------------------------------
//! Get a single value from the sequence x at location n
//-----------------------------------------------------------------------------
vectR2 seqR2::get(const long n) const
{
  vectR2 xya(0,0);
  if(n<N)xya=xy[n];
  else   fprintf(stderr,"n=%ld larger than seqR1 size N=%ld\n",n,N);
  return xya;
}

//-----------------------------------------------------------------------------
//! \brief Get a single value from the sequence x element x at location n
//-----------------------------------------------------------------------------
double seqR2::getx(const long n) const
{
  double u=0;
  if(n<N)u=xy[n].getx();
  else   fprintf(stderr,"n=%ld larger than x seqR1 size N=%ld\n",n,N);
  return u;
}

//-----------------------------------------------------------------------------
//! \brief Get a single value from the sequence x element y at location n
//-----------------------------------------------------------------------------
double seqR2::gety(const long n) const
{
  double u=0;
  if(n<N)u=xy[n].gety();
  else   fprintf(stderr,"n=%ld larger than y seqR1 size N=%ld\n",n,N);
  return u;
}

//-----------------------------------------------------------------------------
//! \brief List contents of sequence
//-----------------------------------------------------------------------------
void seqR2::list(const long start, const long end, const char *str1, const char *str2, FILE *ptr) const
{
  long n,m;
  vectR2 x;
  if(strlen(str1)>0)fprintf(ptr,"%s",str1);
  for(n=start,m=1; n<=end; n++,m++){
    x=get(n);
    fprintf(ptr,"  ");
    x.list(ptr);
    if(m%3==0)fprintf(ptr,"\n");
    }
  if(strlen(str2)>0)fprintf(ptr,"%s",str2);
}

//-----------------------------------------------------------------------------
//! \brief List contents of seqC1 using 1 digit per element
//-----------------------------------------------------------------------------
void seqR2::list1(void) const
{
  long n,m;
  for(n=0,m=1; n<N; n++,m++){
    printf("(%2.0lf,%2.0lf)   ",getx(n),gety(n));
    if(m%5==0)printf("\n");
    }
}

//-----------------------------------------------------------------------------
//! \brief List contents of seqC1 using 1 digit per element
//-----------------------------------------------------------------------------
void seqR2::list1(long start, long end) const
{
  long n,m;
  for(n=start,m=1; n<=end; n++,m++){
    printf("(%2.0lf,%2.0lf)   ",getx(n),gety(n));
    if(m%50==0)printf("\n");
    else if(m%10==0)printf(" ");
    }
}

//-----------------------------------------------------------------------------
//! \brief Return the largest pair of values in the sequence as measured by norm()
//-----------------------------------------------------------------------------
double seqR2::norm(const long n) const
{
  vectR2 xya=get(n);
  return xya.norm();
}

//-----------------------------------------------------------------------------
//! \brief Return the largest pair of values in the sequence as measured by norm()
//-----------------------------------------------------------------------------
vectR2 seqR2::max(const int verbose) const
{
  long n;
  double maxnorm=0;
  long   maxn=0;
  vectR2  maxpair;
  for(n=0; n<N; n++)if(norm(n)>maxnorm){maxnorm=norm(n); maxn=n;}
  maxpair=get(maxn);
  if(verbose)
  {
    for(n=0; n<N; n++)
    {
      if(norm(n)>=(maxnorm*0.999))
        printf("max=(%lf,%lf) at n=%ld\n",maxpair.getx(),maxpair.gety(),n);
    }
  }
  return maxpair;
}

//=====================================
// external operations
//=====================================
//-----------------------------------------------------------------------------
//! \brief Operator: return p+q
//-----------------------------------------------------------------------------
vectR2 operator+(const vectR2 p, const vectR2 q)
{
  const Eigen::Map< const Eigen::Vector2d > a( p.getdata() );
  const Eigen::Map< const Eigen::Vector2d > b( q.getdata() );
  const Eigen::Vector2d c = a + b;
  const vectR2 r( c(0), c(1) );
  return r;
}

//-----------------------------------------------------------------------------
//! \brief Operator: return p-q
//-----------------------------------------------------------------------------
vectR2 operator-(const vectR2 p, const vectR2 q)
{
  const Eigen::Map< const Eigen::Vector2d > a( p.getdata() );
  const Eigen::Map< const Eigen::Vector2d > b( q.getdata() );
  const Eigen::Vector2d c = a - b;
  const vectR2 r( c(0), c(1) );
  return r;
}

//-----------------------------------------------------------------------------
//! \brief Operator: return -p
//-----------------------------------------------------------------------------
vectR2 operator-(const vectR2 p)
{
  const Eigen::Map< const Eigen::Vector2d > a( p.getdata() );
  const Eigen::Vector2d b = -a;
  const vectR2 c( b(0), b(1) );
  return c;
}

//-----------------------------------------------------------------------------
//!  \brief operator: return <p> rotated counter-clockwise by <phi> radians
//!  \details
//!    https://stackoverflow.com/questions/17036818/
//!    https://eigen.tuxfamily.org/dox/group__TopicStorageOrders.html
//!    https://stackoverflow.com/questions/28722899/
//-----------------------------------------------------------------------------
vectR2 operator&(const vectR2 p, const double phi)
{
  double cosphi, sinphi;
  if(phi==0){ cosphi = 1;        sinphi = 0;        }
  else      { cosphi = cos(phi); sinphi = sin(phi); }
  const std::vector<double> rr = { cosphi, -sinphi,
                                   sinphi,  cosphi  };
  const Eigen::Matrix<double, 2, 2, Eigen::RowMajor> R(rr.data());
//printf("     _                    _\n");
//printf("R = | %9.6lf %9.6lf  |\n", R(0,0), R(0,1) );
//printf("    |_%9.6lf %9.6lf _|\n", R(1,0), R(1,1) );
//const Eigen::Map< const Eigen::Vector2d > x( p.pairxy.data() );
  const Eigen::Map< const Eigen::Vector2d > x( p.getdata() );
  const Eigen::Vector2d y = R * x;
  const vectR2 q( y(0), y(1) );
  return q;
}

//-----------------------------------------------------------------------------
//! \brief Return the angle theta in radians between the two vectors induced by
//!        the points <p> and <q> in the plane R^2
//! \returns On SUCCESS return theta in the closed interval [0:M_PI];
//!          On ERROR   exit with value EXIT_FAILURE
//-----------------------------------------------------------------------------
double pqtheta(const vectR2 p, const vectR2 q)
{
  const double rp=p.mag(), rq=q.mag();
  double y;
  if(rp==0) return -1;
  if(rq==0) return -2;
  y = (p^q)/(rp*rq);
  if( y > +1.0 )
  {
    if(y-1 < 1e-13 ) y = 1.0;
    else{
      fprintf(stderr,"\nERROR using pqtheta(vectR2 p, vectR2 q): (p^q)/(rp*rq)=%.12lf>+1\n",y);
      exit(EXIT_FAILURE);
      }
  }
  if(y<(-1.0))
  {
    if(y+1> -1e-13 ) y = -1.0;
    else{
      fprintf(stderr,"\nERROR using pqtheta(vectR2 p, vectR2 q): (p^q)/(rp*rq)=%.12lf<-1\n",y);
      exit(EXIT_FAILURE);
      }
  }
  const double theta = acos(y);
  return theta;
}


#include <cmath>
#include "../matplotlibcpp.h"
namespace plt = matplotlibcpp;

#ifdef WITH_EIGEN
  #include <Eigen/Dense>
#endif

typedef std::vector<std::vector<double>> nested_vectors;

template <typename Matrix>
void get_data(Matrix& x, Matrix& y, Matrix& z) {
  // check the sizes
  const unsigned ncols = x.cols(),
                 nrows = x.rows();
  assert(ncols == y.cols() && ncols == z.cols());
  assert(nrows == y.rows() && nrows == z.rows());

  // write data
  const double a = -5,
               b = 5;
  const double col_incr = (b - a) / ncols,
               row_incr = (b - a) / nrows;
  for (unsigned i = 0; i < nrows; ++i) {
    for (unsigned j = 0; j < ncols; ++j) {
      x(i, j) = i;
      y(i, j) = j;
      z(i, j) = ::std::sin(::std::hypot(a + i * col_incr, a + j * row_incr));
    }
  }
}

template <>
void get_data(nested_vectors& x, nested_vectors& y, nested_vectors& z) {
    for (double i = -5; i <= 5;  i += 0.25) {
        std::vector<double> x_row, y_row, z_row;
        for (double j = -5; j <= 5; j += 0.25) {
            x_row.push_back(i);
            y_row.push_back(j);
            z_row.push_back(::std::sin(::std::hypot(i, j)));
        }
        x.push_back(x_row);
        y.push_back(y_row);
        z.push_back(z_row);
    }
}

int main() {
    std::vector<std::vector<double>> x, y, z;
    get_data(x, y, z);
    plt::plot_surface(x, y, z);
    plt::show();

    #ifdef WITH_EIGEN
      const unsigned n = 100;  // resolution of hypot function
      Eigen::MatrixXd X(n,n), Y(n,n), Z(n,n);
      get_data(X, Y, Z);
      plt::plot_surface(X, Y,   Z);
      plt::show();
    #endif
}


#include <Eigen/SVD> // Eigen is required

template<int R, int C>
static inline void pseudoInverse(float A[], float B[])
{
	typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor | Eigen::AutoAlign, R, C> t_A;

	t_A mA(R, C);

	for (int i = 0; i < R; i++)
	{
		for (int j = 0; j < C; j++)
		{
			mA(i, j) = A[i + j * R];
		}
	}

	Eigen::JacobiSVD<t_A, Eigen::ColPivHouseholderQRPreconditioner> svd(mA, Eigen::ComputeThinU | Eigen::ComputeThinV);

	auto U = svd.matrixU();
	auto S = svd.singularValues();
	auto V = svd.matrixV();

	float Vt[C * C];

	for (int i = 0; i < C; i++)
	{
		float s = S(i);

		if (s != 0.f)
			s = 1.f / s;

		for (int j = 0; j < C; j++)
		{
			Vt[j * C + i] = V(j, i) * s;
		}
	}

	for (int i = 0; i < C; i++)
	{
		for (int j = 0; j < R; j++)
		{
			float v = 0.f;

			for (int k = 0; k < C; k++)
			{
				v += Vt[i * C + k] * U(j, k);
			}

			B[i * R + j] = v;
		}
	}
}

void svd_routine_49_2(float A[], float B[])
{
	pseudoInverse<49, 2>(A, B);
}

void svd_routine_121_8(float A[], float B[])
{
	pseudoInverse<121, 8>(A, B);
}


/**
 * @author      : adamlamson (adamlamson@LamsonMacbookPro)
 * @file        : two_step_prob
 * @created     : Wednesday Jan 29, 2020 14:05:35 MST
 */

#ifndef TWO_STEP_PROB_HPP

#define TWO_STEP_PROB_HPP
#include <boost/math/tools/roots.hpp>
#include <cassert>
#include <cmath>

// Solve the for the time that maximizes the probability of two events occuring
template <class T>
class MaxTimeOfTwoStepProbFunctor {
  public:
    // Constructor just stores values find root of function.
    MaxTimeOfTwoStepProbFunctor(T const &rate1, T const &rate2,
                                T const &total_time)
        : k_ab(rate1), k_bc(rate2), tot_time(total_time) {}

    T operator()(T const &t) {
        T fx = k_ab * (exp(k_bc * (tot_time - t)) - 1.) -
               k_bc * (exp(k_ab * t) - 1.);
        return fx;
    }

  private:
    T k_ab;
    T k_bc;
    T tot_time;
};

template <class T>
T max_time_of_two_step_prob(T const &rate1, T const &rate2,
                            T const &total_time) {

    T guess = .5 * total_time;
    T factor = 2; // How big steps to take when searching.

    const boost::uintmax_t maxit = 20; // Limit to maximum iterations.
    boost::uintmax_t it =
        maxit; // Initally our chosen max iterations, but updated with actual.
    bool is_rising =
        false; // So if guess is too low, then try increasing guess.
    int digits = std::numeric_limits<T>::digits; // Maximum possible binary
                                                 // digits accuracy for type T.
    // Some fraction of digits is used to control how accurate o try to make the
    // result.
    int get_digits =
        digits - 3; // We have to have a non-zero interval at each step, so
                    // maximum accuracy is digits - 1.  But we also have to
                    // allow for inaccuracy in f(x), otherwise the last few
                    // iterations just thrash around.
    boost::math::tools::eps_tolerance<T> rel_tol(
        get_digits); // Set the tolerance.
    std::pair<T, T> t = boost::math::tools::bracket_and_solve_root(
        MaxTimeOfTwoStepProbFunctor<T>(rate1, rate2, total_time), guess, factor,
        is_rising, rel_tol, it);
    return t.first + (t.second - t.first) * .5;
};

template <class T>
T two_step_prob(T const &rate1, T const &rate2, T const &total_time,
                T const &t) {
    assert(total_time >= t);
    T prob = (1. - exp(-1. * rate1 * t)) *
             (1. - exp(-1. * rate2 * (total_time - t)));
    return prob;
};

template <class T>
T two_step_max_prob(T const &rate1, T const &rate2, T const &total_time) {
    T t_max = max_time_of_two_step_prob(rate1, rate2, total_time);
    T prob = two_step_prob(rate1, rate2, total_time, t_max);
    return prob;
};

#endif /* end of include guard TWO_STEP_PROB_HPP */


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

using Eigen::MatrixXd;
typedef Eigen::Array<double,Eigen::Dynamic,Eigen::Dynamic> ArrayXXd;
typedef Eigen::Array<double,Eigen::Dynamic,1> ArrayXd;
int main(int argc, char const *argv[]) {
  Eigen::Matrix<double,2,2> Q;
  Q(0,0) = 2.0;
  Q(1,0) = 3.0;
  Q(0,1) = 4.0;
  Q(1,1) = 5.0;
  ArrayXXd A_a = Q.array();
  std::cout << A_a.inverse() << std::endl;
  /* code */
  return 0;
}


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "../util/DistanceFuncs.hpp"
#include "../util/FluidEigenMappings.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>
#include <Eigen/SVD>
#include <cassert>
#include <cmath>

namespace fluid {
namespace algorithm {

class MDS
{
public:
  using MatrixXd = Eigen::MatrixXd;
  using VectorXd = Eigen::VectorXd;

  void process(RealMatrixView in, RealMatrixView out, index distance, index k)
  {
    using namespace Eigen;
    using namespace _impl;
    auto     dist = static_cast<DistanceFuncs::Distance>(distance);
    MatrixXd input = asEigen<Matrix>(in);
    index    n = input.rows();
    MatrixXd D = MatrixXd::Zero(n, n);
    MatrixXd I = MatrixXd::Identity(n, n);
    MatrixXd ones = MatrixXd::Ones(n, n);
    for (index i = 0; i < n; i++)
    {
      for (index j = 0; j < n; j++)
      { D(i, j) = DistanceFuncs::map()[dist](input.row(i), input.row(j)); }
    }
    MatrixXd J = I - ones / n;
    D = -0.5 * J * D * J;
    BDCSVD<MatrixXd> svd(D, ComputeThinV | ComputeThinU);
    MatrixXd         U = svd.matrixU();
    ArrayXd          s = svd.singularValues().segment(0, k);
    MatrixXd         result =
        U.block(0, 0, U.rows(), k).array().rowwise() * s.transpose();
    out <<= asFluid(result);
  }
};
}// namespace algorithm
}// namespace fluid


#pragma once
#include <iostream>
#include <random>
#include <cmath>
#include <Eigen/Dense>
#include <Eigen/Geometry>

#define EPSILON 1e-6f
#define DELTA 1e-4f
#define PI 3.14159265358979323846f
#define PI_INV 0.31830988618f

namespace mathext {
    std::default_random_engine random_generator;

    std::vector<float> unif(float a, float b, int N = 1)
    {
        std::vector<float> res;
        std::uniform_real_distribution<float> dis(a, b);

        for (int i = 0; i < N; i++) {
            res.push_back(dis(random_generator));
        }
        return res;
    }

    Eigen::Vector2f disk(float r)
    {
        std::vector<float> u = unif(0.0f, 1.0f, 2);

        return Eigen::Vector2f(r * sqrt(u[0]), 2 * PI * u[1]);
    }

    Eigen::Matrix3f rot_from_two_vectors(Eigen::Vector3f src, Eigen::Vector3f des)
    {
        return Eigen::Quaternionf::FromTwoVectors(src, des).toRotationMatrix();
    }

    Eigen::Matrix3f rot_align_normal(Eigen::Vector3f n)
    {
        return rot_from_two_vectors(Eigen::Vector3f(0.0f, 0.0f, 1.0f), n);
    }

    Eigen::Vector3f reflect(Eigen::Vector3f v1, Eigen::Vector3f n)
    {
        return 2 * n.dot(v1) * n - v1;
    }

    Eigen::Vector3f refract(Eigen::Vector3f v1, Eigen::Vector3f n, float ior = 1.0f)
    {
        float cos_theta_i = v1.dot(n);
        float eta = 1 / ior;
        if (cos_theta_i < 0) {
            eta = 1 / eta;
            n *= -1;
            cos_theta_i *= -1;
        }
        float c = sqrt(1 - eta * eta * (1 - cos_theta_i * cos_theta_i));
        return c < 0 ? Eigen::Vector3f(0, 0, 0) : (eta * (-v1) + (eta * cos_theta_i - c) * n);
    }

    float power_heuristic(int n_f, float p_f, int n_g, float p_g, float beta = 2)
    {
        float c_f = (float)n_f * p_f, c_g = (float)n_g * p_g;
        return pow(c_f, beta) / (pow(c_f, beta) + pow(c_g, beta));
    }
}


#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

int main(int argc, char **argv)
{
  // exploit namespaces to shorten code
  using namespace boost::numeric::ublas;
  using std::cout;
  using std::endl;

  // declare three 3x3 matrices of complex<long double> elements
  matrix<std::complex<long double> > m(3, 3), n(3, 3), o(3, 3);

  // iterate over 3x3 matrix entries
  // r : row index
  // c : column index
  for (unsigned r = 0; r < m.size1(); r++) {
    for (unsigned c = 0; c < m.size2(); c++) {
      // enumerated matrix entries
      m(r,c) = 3 * r + c;

      // complex numbers designating rows and cols
      n(r,c) = r + c * 1i;

      // elementwise square of n
      o(r,c) = std::pow(n(r,c), 2);
    }
  }

  // print to screen as demonstration
  cout << "m:" << endl;
  cout << m << endl;
  cout << endl << "n:" << endl;
  cout << n << endl;
  cout << endl << "o:" << endl;
  cout << n << endl;
  cout << endl << "m + n:" << endl;
  cout << m + n << endl;
  cout << endl << "m * n:" << endl;
  cout << prod(m, n) << endl;
  cout << endl << "n * n - o:" << endl;
  cout << prod(n, n) - o << endl;
}


//####### Test module for physical constants ##################

//Define Module name
 #define BOOST_TEST_MODULE "phys/constants"

#include <cmath>

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

#include "phys_constants.h"
#include "math_constants.h"

using namespace picsar::multi_physics::phys;
using namespace picsar::multi_physics::math;

//Tolerance for double precision calculations
const double double_tolerance = 1.0e-14;

//Tolerance for single precision calculations
const float float_tolerance = 1.0e-7;

//Templated tolerance
template <typename T>
T constexpr tolerance()
{
    if(std::is_same<T,float>::value)
        return float_tolerance;
    else
        return double_tolerance;
}

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

// ***Test physical constants

template<typename RealType>
void test_case_const_phys()
{
    const auto exp_electron_mass =
        static_cast<RealType>(9.1093837015e-31);
    const auto exp_elementary_charge =
        static_cast<RealType>(1.602176634e-19);
    const auto exp_light_speed =
        static_cast<RealType>(299792458.);
    const auto exp_reduced_plank =
        static_cast<RealType>(1.054571817e-34);
    const auto exp_vacuum_permittivity =
        static_cast<RealType>(8.8541878128e-12);
    const auto exp_vacuum_permeability =
        static_cast<RealType>(1.25663706212e-6);
    const auto exp_fine_structure =
        static_cast<RealType>(0.0072973525693);
    const auto exp_eV =
        static_cast<RealType>(1.602176634e-19);
    const auto exp_KeV =
        static_cast<RealType>(1.602176634e-16);
    const auto exp_MeV =
        static_cast<RealType>(1.602176634e-13);
    const auto exp_GeV =
        static_cast<RealType>(1.602176634e-10);
    const auto exp_sqrt_4_pi_fine_structure =
            static_cast<RealType>(sqrt(
                4.0*pi<double>*fine_structure<double>));
    const auto exp_classical_electron_radius =
        static_cast<RealType>(2.81794032620493e-15);
    const auto exp_schwinger_field =
        static_cast<RealType>(1.32328547494817e18);
    const auto exp_tau_e =
        static_cast<RealType>(9.39963715232933e-24);

    BOOST_CHECK_SMALL(
        (electron_mass<RealType>-exp_electron_mass)/exp_electron_mass,
        tolerance<RealType>());

    BOOST_CHECK_SMALL(
        (elementary_charge<RealType>-exp_elementary_charge)/exp_elementary_charge,
        tolerance<RealType>());

    BOOST_CHECK_SMALL(
        (light_speed<RealType>-exp_light_speed)/exp_light_speed,
        tolerance<RealType>());

    BOOST_CHECK_SMALL(
        (reduced_plank<RealType>-exp_reduced_plank)/exp_reduced_plank,
        tolerance<RealType>());

    BOOST_CHECK_SMALL(
        (vacuum_permittivity<RealType>-exp_vacuum_permittivity)/exp_vacuum_permittivity,
        tolerance<RealType>());

    BOOST_CHECK_SMALL(
        (vacuum_permeability<RealType>-exp_vacuum_permeability)/exp_vacuum_permeability,
        tolerance<RealType>());

    BOOST_CHECK_SMALL(
        (fine_structure<RealType>-exp_fine_structure)/exp_fine_structure,
        tolerance<RealType>());

    BOOST_CHECK_SMALL(
        (eV<RealType>-exp_eV)/exp_eV,
        tolerance<RealType>());

    BOOST_CHECK_SMALL(
        (KeV<RealType>-exp_KeV)/exp_KeV,
        tolerance<RealType>());

    BOOST_CHECK_SMALL(
        (MeV<RealType>-exp_MeV)/exp_MeV,
        tolerance<RealType>());

    BOOST_CHECK_SMALL(
        (GeV<RealType>-exp_GeV)/exp_GeV,
        tolerance<RealType>());

    BOOST_CHECK_SMALL(
            (sqrt_4_pi_fine_structure<RealType>-exp_sqrt_4_pi_fine_structure)/
            exp_sqrt_4_pi_fine_structure, tolerance<RealType>());

    BOOST_CHECK_SMALL(
        (classical_electron_radius<RealType>-exp_classical_electron_radius)/exp_classical_electron_radius,
        tolerance<RealType>());

    BOOST_CHECK_SMALL(
        (schwinger_field<RealType>-exp_schwinger_field)/exp_schwinger_field,
        tolerance<RealType>());

    BOOST_CHECK_SMALL(
        (tau_e<RealType>-exp_tau_e)/exp_tau_e,
        tolerance<RealType>());
}

BOOST_AUTO_TEST_CASE( picsar_const_phys )
{
    test_case_const_phys<double>();
    test_case_const_phys<float>();
}

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


#pragma once
#include "coordinate_transform.hpp"
#include "grad_shape.hpp"
#include "integrate.hpp"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <functional>

//----------------H1Begin----------------
//! Computes the H^1 differences between
//! u1 (considered as coefficients for Quadratic FEM) and u2grad.
double computeH1Difference(const Eigen::MatrixXd &vertices,
                           const Eigen::MatrixXi &dofs,
                           const Eigen::VectorXd &u1,
                           const std::function<Eigen::Vector2d(double, double)> &u2grad) {
	const int numberOfElements = dofs.rows();

	double error = 0;
	for (int i = 0; i < numberOfElements; ++i) {
		auto &idSet = dofs.row(i);

		const auto &a = vertices.row(idSet(0));
		const auto &b = vertices.row(idSet(1));
		const auto &c = vertices.row(idSet(2));

		auto            coordinateTransform = makeCoordinateTransform(b - a, c - a);
		auto            volumeFactor        = std::abs(coordinateTransform.determinant());
		Eigen::Matrix2d elementMap          = coordinateTransform.inverse().transpose();

		// (write your solution here)
		auto f = [&](double x, double y) -> double {
			Eigen::Vector2d transformedPoint = coordinateTransform * Eigen::Vector2d(x, y) + a.transpose();

			Eigen::Vector2d approximate_grad = Eigen::Vector2d(0, 0);
			for (int j = 0; j < 6; ++j) {
				approximate_grad += u1(idSet(j)) * elementMap * gradientShapefun(j, x, y);
			}
			Eigen::Vector2d diff = u2grad(transformedPoint.x(), transformedPoint.y()) - approximate_grad;

			return diff.dot(diff) * volumeFactor;
		};

		error += integrate(f);
	}

	return std::sqrt(error);
}
//----------------H1End----------------


// 2.62     Nov 10, 05  hyperparameter autosearch: observations weighted by inverse stddev
// 3.02     Sep 01, 11  watch for denormal or infinite limits during autosearch
// MAS      Jan 02, 15  changed first step direction if hyperparameter is very small

#define  _USE_MATH_DEFINES
#include <cmath>
#include <stdexcept>
#include <limits>
#include <iostream>

#include <Eigen/Dense>
#include <Eigen/LU>

//#include "tnt_array2d.h"
//#include "tnt_array2d_utils.h"
//#include "jama_lu.h"
//
//#include "logging.h"

#include "HParSearch.h"

//observations weighted by inverse stddev
QuadrCoefs QuadrLogFit( const map<double,UniModalSearch::MS> & y_by_x )
{
    unsigned n = y_by_x.size();

    //prepare for case weights: inverse of stddev; beware of zero stddev
    double minstddev=numeric_limits<double>::max();
    for (map<double,UniModalSearch::MS>::const_iterator itr=y_by_x.begin(); itr!=y_by_x.end(); itr++) {
        if (0 < itr->second.s && itr->second.s < minstddev) {
        	minstddev = itr->second.s;
        }
    }
    const double zeroadjust = minstddev<numeric_limits<double>::max() ? 100.0/minstddev //non-zeroes present
        : 1.0; //all weights will be equal

    Eigen::MatrixXd X( n, 3 ); //nRows, nCols
    Eigen::MatrixXd XTW( 3, n ); //nRows, nCols
    Eigen::VectorXd Y( n );
    int i = 0;
    for (map<double,UniModalSearch::MS>::const_iterator itr=y_by_x.begin(); itr!=y_by_x.end();
        itr++, i++) {
        double weight = itr->second.s>0 ? 1/itr->second.s : zeroadjust;
        X(i,0) = XTW(0,i) = 1.0;
        X(i,1) = XTW(1,i) = log( itr->first );
        X(i,2) = XTW(2,i) = log( itr->first ) * log( itr->first );
        for (int j=0; j < 3; j++) { //for no weighting, just skip this
            XTW(j,i) *= weight;
        }
        Y(i) = itr->second.m;
    }

    Eigen::MatrixXd XTX =  XTW * X;
    Eigen::VectorXd XTY = XTW * Y;
    Eigen::VectorXd b_hat = XTX.fullPivLu().solve(XTY);

    // TODO Check numerical accuracy and throw error

    QuadrCoefs ret;
    ret.c0 = b_hat(0);
    ret.c1 = b_hat(1);
    ret.c2 = b_hat(2);
    return ret;
}

StepValue UniModalSearch::step() //recommend: do/not next step, and the next x value
{
    StepValue ret(true,0,0);
    switch( y_by_x.size() ) {
    case 0: ret.second = 1; break;
    case 1:
        if (y_by_x.begin()->first < m_first_cut)
            ret.second = y_by_x.begin()->first * m_stdstep;
        else
            ret.second = y_by_x.begin()->first / m_stdstep;
            //we divide here because step with more penalty is safer numerically
        break;
    case 2:
        if( y_by_x.begin()->second.m > y_by_x.rbegin()->second.m )
            ret.second = y_by_x.begin()->first / m_stdstep;
        else
            ret.second = y_by_x.rbegin()->first * m_stdstep;
        break;
    default: // 3 or more
		if( y_by_x.begin()->first==best->first ) {  //max is at the left - move to the left
            ret.second = best->first / m_stdstep;
			if( !(best->first > numeric_limits<double>::denorm_min()) ) { //inf or nan
				ret.first = false;
			    ret.expected = best->second.m;
			}
		}else if( y_by_x.rbegin()->first==best->first ) { //max is at the right - move to the right
            ret.second = best->first * m_stdstep;
			if( !(best->first < numeric_limits<double>::infinity()) ) { //inf or nan
				ret.first = false;
			    ret.expected = best->second.m;
			}
		}else { //max is 'bracketed'
            QuadrCoefs coefs = QuadrLogFit( y_by_x );
            double log_argmax = - coefs.c1 / coefs.c2 / 2;
            double expected_max = - coefs.c1*coefs.c1 / coefs.c2 / 4 + coefs.c0;

			double maxval = best->second.m;
            if( 0==maxval )  ret.first=false;
            else if( (expected_max-maxval)/fabs(maxval) < m_stop_by_y )  ret.first=false;
            else if( fabs(log_argmax-log(best->first)) < m_stop_by_x )  ret.first=false;
            //double deriv = 2*coefs.c2*best->first + coefs.c1;    cout<<"\nderiv  "<<deriv;
            //ret.first = fabs(deriv) > stop_by_y;

            ret.second = exp( log_argmax );
            ret.expected = expected_max;
            //map<double,double>::const_iterator left = best; left--;
            //map<double,double>::const_iterator right = best; right++;
//            std::cout
//            //Log(6)
//            <<"\nSearch step "<<ret.second<<" stop_by_y "<<((expected_max-maxval)/fabs(maxval))
//                <<" stop_by_x "<<(fabs(log_argmax-log(best->first))) << endl;
        }
    }
    return ret;
}

//void UniModalSearch::dump(std::ostream& stream) const {
//    int i = 0;
//    for (map<double,UniModalSearch::MS>::const_iterator itr=y_by_x.begin(); itr!=y_by_x.end();
//        itr++, i++) {
//    	stream << "search[ " << itr->first << " ] = " << itr->second << std::endl;
//    }
//}

std::ostream& operator<< (std::ostream& stream, const UniModalSearch& search) {
//	search.dump(stream);

    for (map<double,UniModalSearch::MS>::const_iterator itr=search.y_by_x.begin(); itr!=search.y_by_x.end();
        itr++) {
    	stream << "search[ " << itr->first << " ] = " << itr->second.m
    			<< "(" << itr->second.s << ")" << std::endl;
    }

	return stream;
}

#ifdef UNITTEST_
#include <iostream>
void main() {
    map<double,double> y_by_x;
    //example llkl.d001145.b prepared in R
    y_by_x[ 2.000000e+04 ] = -408.199;
    y_by_x[ 2.002884e+03 ] = -328.581;
    y_by_x[ 2.000000e+02 ] = -281.788;
    y_by_x[ 2.002884e+01 ] = -229.966;
    y_by_x[ 2.000000e+00 ] = -184.908;
    y_by_x[ 2.002884e-01 ] = -150.792;
    y_by_x[ 2.000000e-02 ] = -132.081;
    y_by_x[ 2.002884e-03 ] = -127.708;
    y_by_x[ 2.000000e-04 ] = -146.397;
    y_by_x[ 2.002884e-05 ] = -183.208;
    QuadrCoefs c = QuadrLogFit( y_by_x );
    //R results:
    // (Intercept)     logvar2      logvar
    // -171.030466   -1.186039  -12.662184
    cout<<"c0="<<c.c0<<"  c1="<<c.c1<<"  c2="<<c.c2<<endl;

    UniModalSearch s;
    for( map<double,double>::const_iterator itr=y_by_x.begin(); itr!=y_by_x.end(); itr++ )
        s.tried( itr->first, itr->second );
    //R result: 0.004805411
    cout<<"step="<<s.step().second<<endl;
}
#endif //UNITTEST_

/*
    Copyright (c) 2002, 2003, 2004, 2005, 2006, 2007, Rutgers University, New Brunswick, NJ, USA.

    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.

    Except as contained in this notice, the name(s) of the above
    copyright holders, DIMACS, and the software authors shall not be used
    in advertising or otherwise to promote the sale, use or other
    dealings in this Software without prior written authorization.
*/


#include <vector> /* std::vector */
#include <utility> /* std::pair */
#include <algorithm> /* std::lower_bound, std::sort */

#include "XYParser.hpp"

#include <boost/range/combine.hpp> /* boost::combine */
template<typename U = double>
class Interpolator {
    std::vector<std::pair<U, U>> _table;
    const U INF = static_cast<U>(1e100);

    U _lower_bound = -INF;
    U _upper_bound = INF;

    void init(const std::vector<U> &x, const std::vector<U> &y)
    {
        // taken from here: https://stackoverflow.com/a/8513803/2289030
        for(auto tup : boost::combine(x, y)) {
            U x, y;
            boost::tie(x, y) = tup;

            _table.push_back(std::make_pair(x, y));
        }

        std::sort(_table.begin(), _table.end());
    }

    void init(const std::vector<U> &x, const std::vector<U> &y, std::pair<U, U> bounds)
    {
        init(x, y);
        set_bounds(bounds);
    }

    void copy(const Interpolator &other)
    {
        _table = other._table;
        _lower_bound = other._lower_bound;
        _upper_bound = other._upper_bound;
    }
public:
    U get(U x)
    {
        // taken from here: https://stackoverflow.com/a/11675205/2289030
        // Assumes that "table" is sorted by .first

        // Check if x is out of bound
        if (x > _table.back().first) return _upper_bound;
        if (x < _table[0].first) return _lower_bound;

        typename std::vector<std::pair<U, U>>::iterator it, it2;
        // INFINITY is defined in math.h in the glibc implementation
        it = std::lower_bound(_table.begin(), _table.end(), std::make_pair(x, -INF));
        // Corner case
        if (it == _table.begin()) return it->second;
        it2 = it;
        --it2;
        return it2->second + ((it->second - it2->second)
                                *(x - it2->first)/(it->first - it2->first));
    }

    void set_bounds(const std::pair<U, U> &bounds)
    {
        _lower_bound = bounds.first;
        _upper_bound = bounds.second;
    }

    U interpolate(U x)
    {
        return get(x);
    }

    Interpolator(const std::vector<U> &x, const std::vector<U> &y)
    {
        init(x, y);
    }

    Interpolator(const std::vector<U> &x, const std::vector<U> &y
            , std::pair<U,U> bounds)
    {
        init(x, y, bounds);
    }

    Interpolator(const std::string &filename)
    {
        std::vector<U> x, y;
        load_xy_data<U>(filename, x, y);

        init(x, y);
    }

    Interpolator() {};

    Interpolator(const Interpolator &other)
    {
        copy(other);
    }

    Interpolator& operator=(const Interpolator &rhs)
    {
        copy(rhs);
        return *this;
    }

};


#include <iomanip>
#include <memory>
#include <vector>

#include <Eigen/Core>
#include <aslam/common/memory.h>
#include <eigen-checks/gtest.h>
#include <glog/logging.h>
#include <gtest/gtest.h>

#include "maplab-common/global-coordinate-tools.h"
#include "maplab-common/test/testing-entrypoint.h"
#include "maplab-common/test/testing-predicates.h"

namespace common {

TEST(GlobalCoordinateToolsTest, testLlhToEcefAndBack) {
  // Zurich coordinates in longitude, latitude, height.
  const double latitude_degree = 47.3666700;
  const double longitude_degree = 8.5500000;
  // Height above WGS84 ellipsoid.
  const double height_meter = 429.0;
  const Eigen::Vector3d llh(latitude_degree, longitude_degree, height_meter);

  // Conversion from LLH to ECEF coordinates.
  Eigen::Vector3d ecef;

  common::llhToEcef(llh, &ecef);

  // Conversion from ECEF back to LLH coordinates using iterative method.
  Eigen::Vector3d llh_back_transformed_iterative;
  common::ecefToLlhIterative(ecef, &llh_back_transformed_iterative);
  EXPECT_NEAR_EIGEN(llh, llh_back_transformed_iterative, 1.0e-5);

  // Conversion from ECEF back to LLH coordinates using closed method.
  Eigen::Vector3d llh_back_transformed_closed;
  common::ecefToLlh(ecef, &llh_back_transformed_closed);
  EXPECT_NEAR_EIGEN(llh, llh_back_transformed_closed, 1.0e-5);
}

TEST(GlobalCoordinateToolsTest, testEcefToNedAndBack) {
  // Zurich coordinates in longitude, latitude, height.
  const double latitude_degree = 47.3666700;
  const double longitude_degree = 8.5500000;
  // Height above WGS84 ellipsoid.
  const double height_meter = 429.0;
  const Eigen::Vector3d llh(latitude_degree, longitude_degree, height_meter);

  // Conversion from LLH to ECEF coordinates.
  Eigen::Vector3d ecef;
  common::llhToEcef(llh, &ecef);

  // Conversion from ECEF to NED coordinates.
  const Eigen::Vector3d offset(10.0, 50.0, 20.0);
  const Eigen::Vector3d origin_ecef(ecef + offset);
  Eigen::Vector3d north_east_down;
  common::ecefToNed(ecef, origin_ecef, &north_east_down);

  // Conversion from NED back to ECEF coordinates.
  Eigen::Vector3d ecef_back_transformed;
  common::nedToEcef(north_east_down, origin_ecef, &ecef_back_transformed);

  EXPECT_NEAR_EIGEN(ecef, ecef_back_transformed, 1.0e-12);
}
}  // namespace common

MAPLAB_UNITTEST_ENTRYPOINT


#include <iostream>
#include <armadillo>
#define PRINT 1

using namespace std;
using namespace arma;

int main()
{
  // User input
  arma::mat input;
  input << 1 << 3 << 2 << 4 << 0 << 0 << 0 << 0 << 0 << 0 << 0 << 0 << 0 << 0 << 0 << 0 << endr
        << 5 << 7 << 6 << 8 << 0 << 0 << 0 << 0 << 0 << 0 << 0 << 0 << 0 << 0 << 0 << 0 << endr;
  input = input.t();

  size_t size = 4; // input channels
  size_t upscale_factor = 2;
  size_t height = 2;
  size_t width = 2;

  // Forward()
  size_t batchSize = input.n_cols;
  size_t size_out = size / std::pow(upscale_factor, 2); // output channels
  size_t output_height = height * upscale_factor;
  size_t output_width = width * upscale_factor;

  arma::mat output;
  output.zeros(output_height * output_width * size_out, batchSize );

  for(size_t n = 0; n < batchSize; n++)
  {
    arma::mat inputImage = input.col(n);
    arma::mat outputImage = output.col(n);
    arma::cube inputTemp(const_cast<arma::mat&>(inputImage).memptr(), height, width, size, false, false);
    arma::cube outputTemp(const_cast<arma::mat&>(outputImage).memptr(), output_height, output_width, size_out, false, false);

    for (size_t c = 0; c < size_out ; c++)
    {
      for (size_t h = 0; h < output_height; h++)
      {
        for (size_t w = 0; w < output_width; w++)
        {
          size_t height_index = h / upscale_factor;
          size_t width_index = w / upscale_factor;
          size_t channel_index = (upscale_factor * (h % upscale_factor)) + (w % upscale_factor) + (c * std::pow(upscale_factor, 2));
          outputTemp(w, h, c) = inputTemp(width_index, height_index, channel_index);
        }
      }
    }

    output.col(n) = outputImage;

    if(PRINT)
    {
      cout << "Image " << n << " calculations: " << endl;
      cout << "-----------------------------------" << endl;
      cout << "-----------------------------------" << endl;
      cout << "INPUT for Pixel Shuffle: " << endl;
      cout << inputTemp << endl;
      cout << "-----------------------------------" << endl;
      cout << "OUTPUT from Pixel Shuffle: " << endl;
      cout << outputTemp << endl;
      cout << "-----------------------------------" << endl;
    }

  }

  // this is gy for Pixel Shuffle layer simulated as a tensor filled with an arbitrary sequence of values.
  arma::mat gy;
  gy << 1 << 5 << 9 << 13 << 2 << 6 << 10 << 14 << 3 << 7 << 11 << 15 << 4 << 8 << 12 << 16 << endr
    << 17 << 21 << 25 << 29 << 18 << 22 << 26 << 30 << 19 << 23 << 27 << 31 << 20 << 24 << 28 << 32 << endr;
  gy = gy.t();

  // Backward()
  arma::mat g;
  g.zeros(arma::size(input));

  for(size_t n = 0; n < batchSize; n++)
  {
    arma::mat gyImage = gy.col(n);
    arma::mat gImage = g.col(n);
    arma::cube gyTemp(const_cast<arma::mat&>(gyImage).memptr(), output_height, output_width, size_out, false, false);
    arma::cube gTemp(const_cast<arma::mat&>(gImage).memptr(), height, width, size, false, false);

    for (size_t c = 0; c < size_out ; c++)
    {
      for (size_t h = 0; h < output_height; h++)
      {
        for (size_t w = 0; w < output_width; w++)
        {
          size_t height_index = h / upscale_factor;
          size_t width_index = w / upscale_factor;
          size_t channel_index = (upscale_factor * (h % upscale_factor)) + (w % upscale_factor) + (c * std::pow(upscale_factor, 2));
          gTemp(width_index, height_index, channel_index) = gyTemp(w, h, c);
        }
      }
    }

    g.col(n) = gImage;

    if (PRINT)
    {
      cout << "Image " << n << " calculations: " << endl;
      cout << "-----------------------------------" << endl;
      cout << "Hypothetical gy for Pixel Shuffle: " << endl;
      cout << gyTemp << endl;
      cout << "-----------------------------------" << endl;
      cout << "g for Pixel Shuffle: " << endl;
      cout << gTemp << endl;
      cout << "-----------------------------------" << endl;
    }
  }

  return 0;
}


#include <iostream>
#include <Eigen/Dense>
#include <cmath>
#include <string>
#include <fstream>
#include <bits/stdc++.h>

template <typename T>
void print(T x)
{
    std::ofstream output;
    output.open("output/output.txt", std::ios_base::app); // std::ofstream::trunc);
    std::cout << x << std::endl;
    output.close();
}

double trapezRule(Eigen::VectorXd function(Eigen::VectorXd), double x_min, double x_max, double h)
{
    double result = 100;
    double old_result = 0;
    int iterator = 0;

    do
    {
        h = h / 2;
        iterator++;
        Eigen::VectorXd deltaX = Eigen::VectorXd::LinSpaced((x_max - x_min) / h + 1, x_min, x_max);
        Eigen::VectorXd f = function(deltaX);
        Eigen::VectorXd result_vec = f * h;
        old_result = result;
        result = 0.5 * (result_vec(0) + result_vec(result_vec.size() - 1)) + result_vec(Eigen::seq(1, result_vec.size() - 2)).sum();

    } while ((abs(result - old_result) > 1e-6) & (iterator <= 30));
    print("Iterationen:");
    print(iterator);
    return result;
}

Eigen::VectorXd f1a(Eigen::VectorXd x)
{
    return Eigen::exp(x.array()) / x.array();
}

Eigen::VectorXd f1b(Eigen::VectorXd x)
{
    return Eigen::exp(-x.array()) / Eigen::sqrt(x.array());
}

Eigen::VectorXd f1c(Eigen::VectorXd x)
{
    return Eigen::sin(x.array()) / x.array(); //sin(x) / x;
}

double f3a(double x)
{
    return (x * x) - 2;
}

double bisection(double function(double), double a, double b, double c)
{
    if ((a < b) & (b < c) & (function(b) < function(a)) & (function(b) < function(c)))
    {
        int iteration = 0;
        do
        {
            iteration++;

            if (abs(b - a) > abs(c - b))
            {
                double a_new = a;
                double b_new = (a + b) / 2;
                double c_new = b;
                if (function(b_new) < function(c_new))
                {
                    a = a_new;
                    b = b_new;
                    c = c_new;
                }
                else
                {
                    a = b_new;
                    b = c_new;
                    c = c;
                }
            }
            else
            {
                double a_new = b;
                double b_new = (b + c) / 2;
                double c_new = c;
                if (function(b_new) < function(a_new))
                {
                    a = a_new;
                    b = b_new;
                    c = c_new;
                }
                else
                {
                    a = a;
                    b = a_new;
                    c = b_new;
                }
            }
        } while ((abs(a - c) > 1e-9) & (iteration < 100));
        print("Iterationen: ");
        print(iteration);
        print("a - c = ");
        print(abs(a - c));
    }
    else
    {
        print("First requirement of bisection is not fullfiled.");
    }
    return a;
}

double fDot(double function(double), double x0)
{
    double deltaX = (x0 + 1) / 1000;
    // Eigen::VectorXd deltaX = Eigen::VectorXd::LinSpaced((x(x.size()-1) -x(0) ) / diff_x + 1, x(0), x(x.size()-1));
    return (function(x0 + deltaX) - function(x0 - deltaX)) / (2 * deltaX);
}

double fDDot(double function(double), double x0)
{
    double deltaX = (x0 + 1) / 1000;
    // Eigen::VectorXd deltaX = Eigen::VectorXd::LinSpaced((x(x.size()-1) -x(0) ) / diff_x + 1, x(0), x(x.size()-1));
    return (function(x0 + deltaX) - 2 * function(x0) + function(x0 - deltaX)) / (deltaX * deltaX);
}

double newton(double function(double), double x0)
{
    double x_new = x0;
    double x_old = 0;
    double iteration = 0;
    do
    {
        iteration++;
        x_old = x_new;
        x_new = x_old - fDot(function, x_old) / fDDot(function, x_old);
    } while ((abs(x_new - x_old) > 1e-9) & (iteration < 100));
    print("Iterationen: ");
    print(iteration);
    print("x_new - x_old = ");
    print(abs(x_new - x_old));
    return x_new;
}

int main()
{
    for (int m = 3; m < 5; m++)
    {
        int j = pow(2, m);
        int N = 10;
        int l = N;
        std::complex<double> complex_(0.0, 1.0);
        Eigen::VectorXcd F_j(j);
        Eigen::VectorXcd f_l(l);
        Eigen::MatrixXcd omega(j, l);

        for (int i = 0; i < l; i++)
        {
            f_l(i) = std::sqrt(1 + i);
        }

        for (int i = 0; i < l; i++)
        {
            for (int k = 0; k < j; k++)
            {
                omega(k, i) = std::exp(k * i / N * 2 * M_PI * complex_);
            }
        }

        F_j = omega * f_l;
        Eigen::VectorXd F_j_real = F_j.real();
        print(F_j_real);
    }

    // // Ex. 1
    // // a)

    // print("Exercise a): ");
    // double zero = 1e-6;
    // print("Zero is:");
    // print(zero);
    // double i1 = trapezRule(f1a, -1, -zero, 0.5) + trapezRule(f1a, zero, 1, 0.5);
    // print("Result I1: ");
    // print(i1);

    // // b) see WolframAlpha for x_max
    // print("Exercise b): ");
    // double infty = 100.;
    // print("Infinity is:");
    // print(infty);
    // double epsilon = 0.001;
    // print("Epsilon is:");
    // print(epsilon);
    // zero = 1e-11;
    // print("Zero is:");
    // print(zero);
    // double i2 = trapezRule(f1b, zero, epsilon, 0.0005) + trapezRule(f1b, epsilon, infty, 0.5);
    // print("Result I2: ");
    // print(i2);

    // // c)
    // print("Exercise c): ");
    // infty = 1000000.;
    // print("Infinity is:");
    // print(infty);
    // zero = 1e-6;
    // print("Zero is:");
    // print(zero);
    // double i3 = trapezRule(f1c, -infty, -zero, 0.5) + trapezRule(f1c, zero, infty, 0.5);
    // print("Result I3: ");
    // print(i3);

    // // Ex. 3
    // // a)
    // double a = -0.5;
    // double b = -0.1;
    // double c = 2.0;
    // double result_bisec = bisection(f3a, a, b, c);

    // // b)
    // double x0 = 1;
    // double result_newton = newton(f3a, x0);
    // print("Bisec - Newton:");
    // print(abs(result_bisec - result_newton));

    return 0;
}