#pragma once

#include <boost/functional/hash.hpp>
#include <crab/support/os.hpp>
#include <cstdint>
#include <gmp.h>

// TODO: replace ikos with crab namespace. This class has nothing to
// do with the ikos one. Kept for now for compatibility issues with
// some clients.
namespace ikos {

class z_number {
  friend class q_number;

private:
  mpz_t _n;

  bool fits_sint() const;
  bool fits_slong() const;

public:
  // overloaded typecast operators
  explicit operator int64_t() const;

  z_number();
  z_number(int64_t n);
  z_number(const std::string &s, unsigned base = 10);

  static z_number from_uint64(uint64_t n);
  static z_number from_mpz_t(mpz_t n);
  static z_number from_mpz_srcptr(mpz_srcptr n);

  z_number(const z_number &o);
  z_number(z_number &&o);
  z_number &operator=(const z_number &o);
  z_number &operator=(z_number &&o);

  ~z_number();

  mpz_srcptr get_mpz_t() const { return _n; }

  mpz_ptr get_mpz_t() { return _n; }

  std::string get_str(unsigned base = 10) const;

  std::size_t hash() const;

  bool fits_int64() const;

  z_number operator+(z_number x) const;

  z_number operator*(z_number x) const;

  z_number operator-(z_number x) const;

  z_number operator-() const;

  // signed division
  z_number operator/(z_number x) const;

  // signed remainder
  z_number operator%(z_number x) const;

  z_number &operator+=(z_number x);

  z_number &operator*=(z_number x);

  z_number &operator-=(z_number x);

  z_number &operator/=(z_number x);

  z_number &operator%=(z_number x);

  z_number &operator--();

  z_number &operator++();

  z_number operator++(int);

  z_number operator--(int);

  bool operator==(z_number x) const;

  bool operator!=(z_number x) const;

  bool operator<(z_number x) const;

  bool operator<=(z_number x) const;

  bool operator>(z_number x) const;

  bool operator>=(z_number x) const;

  // bitwise-and
  z_number operator&(z_number x) const;

  // bitwise-or
  z_number operator|(z_number x) const;

  // bitwise-xor
  z_number operator^(z_number x) const;

  // left shift
  z_number operator<<(z_number x) const;

  // arithmetic right shift
  z_number operator>>(z_number x) const;

  z_number fill_ones() const;

  void write(crab::crab_os &o) const;

}; // class z_number

class q_number {

private:
  mpq_t _n;

public:
  q_number();
  q_number(double n);

  q_number(const std::string &s, unsigned base = 10);
  q_number(const z_number &n);
  q_number(const z_number &n, const z_number &d);

  static q_number from_mpq_t(mpq_t n);
  static q_number from_mpz_t(mpz_t n);
  static q_number from_mpq_srcptr(mpq_srcptr q);

  q_number(const q_number &o);
  q_number(q_number &&o);
  q_number &operator=(const q_number &o);
  q_number &operator=(q_number &&o);

  ~q_number();

  mpq_srcptr get_mpq_t() const { return _n; }

  mpq_ptr get_mpq_t() { return _n; }

  double get_double() const;

  std::string get_str(unsigned base = 10) const;

  std::size_t hash() const;

  q_number operator+(q_number x) const;

  q_number operator*(q_number x) const;

  q_number operator-(q_number x) const;

  q_number operator-() const;

  q_number operator/(q_number x) const;

  q_number &operator+=(q_number x);

  q_number &operator*=(q_number x);

  q_number &operator-=(q_number x);

  q_number &operator/=(q_number x);

  q_number &operator--();

  q_number &operator++();

  q_number operator--(int);

  q_number operator++(int);

  bool operator==(q_number x) const;

  bool operator!=(q_number x) const;

  bool operator<(q_number x) const;

  bool operator<=(q_number x) const;

  bool operator>(q_number x) const;

  bool operator>=(q_number x) const;

  z_number numerator() const;

  z_number denominator() const;

  z_number round_to_upper() const;

  z_number round_to_lower() const;

  void write(crab::crab_os &o) const;

}; // class q_number

inline crab::crab_os &operator<<(crab::crab_os &o, const z_number &z) {
  z.write(o);
  return o;
}

inline crab::crab_os &operator<<(crab::crab_os &o, const q_number &q) {
  q.write(o);
  return o;
}

/** for boost::hash_combine **/
inline std::size_t hash_value(const z_number &z) { return z.hash(); }

inline std::size_t hash_value(const q_number &q) { return q.hash(); }
} // namespace ikos

/** for specializations of std::hash **/
namespace std {
template <> struct hash<ikos::z_number> {
  size_t operator()(const ikos::z_number &z) const { return z.hash(); }
};

template <> struct hash<ikos::q_number> {
  size_t operator()(const ikos::q_number &q) const { return q.hash(); }
};
} // namespace std


/**
 * @file laxwendroffscheme.cc
 * @brief NPDE homework "LaxWendroffScheme" code
 * @author Oliver Rietmann
 * @date 29.04.2019
 * @copyright Developed at ETH Zurich
 */

#include "laxwendroffscheme.h"

#include <Eigen/Core>
#include <cmath>

namespace LaxWendroffScheme {

namespace Constant {
constexpr double e = 2.71828182845904523536;
constexpr double pi = 3.14159265358979323846;
}  // namespace Constant

constexpr double Square(double x) { return x * x; }

/**
 * @brief Computes the right-hand side according to the Lax-Wendroff scheme.
 * @param mu mu^(k-1) (i.e. mu at timestep k-1)
 * @param gamma tau / h, where tau = timestep size and h = spatial meshwidth
 * @return mu^(k) (i.e. mu at timestep k)
 */
/* SAM_LISTING_BEGIN_0 */
Eigen::VectorXd LaxWendroffRhs(const Eigen::VectorXd &mu, double gamma) {
  int N = mu.size();
  Eigen::VectorXd result(N);

  //====================
  // Your code goes here
  //====================

  return result;
}

Eigen::VectorXd solveLaxWendroff(const Eigen::VectorXd &u0, double T,
                                 unsigned int M) {
  double gamma = 1.0 / Constant::e;
  Eigen::VectorXd mu = u0;
  // Main timestepping loop
  for (int j = 0; j < M; ++j) mu = LaxWendroffRhs(mu, gamma);
  return mu;
}

/* SAM_LISTING_END_0 */

/* SAM_LISTING_BEGIN_2 */
// Build spatial grid
Eigen::VectorXd getXValues(double T, unsigned int M) {
  double tau = T / M;
  double h = Constant::e * tau;
  int j_max = (int)(std::ceil((3.0 * T + 1.0) / h) + 0.5);
  int j_min = (int)(std::floor(-3.0 * T / h) - 0.5);
  unsigned int N = j_max - j_min + 1;
  return Eigen::VectorXd::LinSpaced(N, j_min * h, j_max * h);
}
Eigen::VectorXd numexpLaxWendroffRP(const Eigen::VectorXi &M) {
  const double T = 1.0;
  const int M_size = M.size();
  Eigen::VectorXd error(M_size);
  // Initial values for the Riemann problem
  auto u_initial = [](double x) { return 0.0 <= x ? 1.0 : 0.0; };
  // Exact solution \prbeqref{eq:solrp} at time $T = 1.0$
  auto u_exact = [](double x) {
    return (x <= 1.0) ? 0.0 : ((Constant::e <= x) ? 1.0 : std::log(x));
  };
  //====================
  // Your code goes here
  //====================
  return error;
}
/* SAM_LISTING_END_2 */

/**
 * @brief Evaluates the discrete function u at position x by linear
 * interpolation
 * @param u descrete function values at spatial positions y
 * @param y vector of same length as u, representing the nodes of u
 * @return best linear interpolation of u at spacial position x
 */
double eval(const Eigen::VectorXd &u, const Eigen::VectorXd &y, double x) {
  int N = y.size();
  double a = y(0);
  double b = y(N - 1);

  if (x <= a) return u(0);
  if (b <= x) return u(N - 1);

  double lambda = (x - a) / (b - a);
  int k0 = (int)(lambda * (N - 1));
  int k1 = k0 + 1;

  lambda = (x - y(k0)) / (y(k1) - y(k0));
  return lambda * u(k1) + (1.0 - lambda) * u(k0);
}

/* SAM_LISTING_BEGIN_9 */
double smoothU0(double x) {
  return (x < 0.0)
             ? 0.0
             : ((1.0 < x) ? 1.0 : Square(std::sin(0.5 * Constant::pi * x)));
}
Eigen::VectorXd referenceSolution(const Eigen::VectorXd &x) {
  double T = 1.0;
  // Reference solution on a very fine mesh
  unsigned int M = 3200;

  Eigen::VectorXd y = getXValues(T, M);
  Eigen::VectorXd u0 = y.unaryExpr(&smoothU0);
  Eigen::VectorXd u = solveLaxWendroff(u0, T, M);
  int N = x.size();
  Eigen::VectorXd u_ref(N);
  // The vector u is larger than u_ref. Use eval() from above the "evaluate" u
  // at the positions x(i) and thus obtain the reference solution u_ref.
  //====================
  // Your code goes here
  //====================
  return u_ref;
}
/* SAM_LISTING_END_9 */

/* SAM_LISTING_BEGIN_1 */
Eigen::VectorXd numexpLaxWendroffSmoothU0(const Eigen::VectorXi &M) {
  const double T = 1.0;
  const int M_size = M.size();
  Eigen::VectorXd error(M_size);

  //====================
  // Your code goes here
  //====================
  return error;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_7 */
Eigen::VectorXd solveGodunov(const Eigen::VectorXd &u0, double T,
                             unsigned int M) {
  double tau = T / M;
  double h = Constant::e * tau;
  unsigned int N = u0.size();
  Eigen::VectorXd mu = u0;

  //====================
  // Your code goes here
  //====================
  return mu;
}

/* SAM_LISTING_END_7 */

/* SAM_LISTING_BEGIN_8 */
Eigen::VectorXd numexpGodunovSmoothU0(const Eigen::VectorXi &M) {
  const double T = 1.0;
  const int M_size = M.size();
  Eigen::VectorXd error(M_size);

  //====================
  // Your code goes here
  //====================
  return error;
}
/* SAM_LISTING_END_8 */

}  // namespace LaxWendroffScheme


/*
 * utilities_trapz_test.cpp Test fixtures for the trapz function
 *
 * Author:                   Tom Clark (thclark @ github)
 *
 * Copyright (c) 2016-9 Octue Ltd. All Rights Reserved.
 *
 */

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

#include "utilities/trapz.h"

using namespace utilities;

class TrapzTest : public ::testing::Test {};

TEST_F(TrapzTest, test_colwise_trapz_1_row) {

    // Test that zeros are returned when trying to colwise integrate an array with 1 row
    Eigen::ArrayXXf integrand(1,4);
    integrand << 1, 2, 6, 9;
    Eigen::ArrayXXf integral_correct(1,4);
    integral_correct<< 0, 0, 0, 0;
    Eigen::ArrayXXf integral = trapz(integrand);
    EXPECT_EQ(integral.matrix(), integral_correct.matrix());

}

TEST_F(TrapzTest, test_colwise_trapz_2_rows) {

    // Test that uniform spaced integration works on an array with 2 rows
    Eigen::ArrayXXf integrand(2,4);
    integrand << 1, 2, 6, 9,
        3, 1, 7, 2;
    Eigen::ArrayXXf integral_correct(1,4);
    integral_correct << 2, 1.5, 6.5, 5.5;
    Eigen::ArrayXXf integral = trapz(integrand);
    EXPECT_EQ(integral.matrix(), integral_correct.matrix());

}

TEST_F(TrapzTest, test_colwise_trapz_3_rows) {

    // Test that uniform spaced integration works on an array with 3 rows
    Eigen::ArrayXXf integrand(3,4);
    integrand << 1, 2, 6, 9,
                 3, 1, 7, 2,
                 4, 8, 3, 1;
    Eigen::ArrayXXf integral_correct(1,4);
    integral_correct << 5.5, 6, 11.5, 7;
    Eigen::ArrayXXf integral = trapz(integrand);
    EXPECT_EQ(integral.matrix(), integral_correct.matrix());

}

TEST_F(TrapzTest, test_colwise_nonuniform_trapz) {

    // Test that non-uniform spacing works with an n x 1 spacing array
    Eigen::ArrayXXf spacing(3, 1);
    spacing << 2,
               5,
               7;
    Eigen::ArrayXXf integrand(3,4);
    integrand << 1, 2, 6, 9,
                 3, 1, 7, 2,
                 4, 8, 3, 1;
    Eigen::ArrayXXf integral_correct(1,4);
    integral_correct << 13, 13.5, 29.5, 19.5;
    Eigen::ArrayXXf integral(1,4);
    trapz(integral, spacing, integrand);
    EXPECT_EQ(integral.matrix(), integral_correct.matrix());

}


/*******
edit_distance: STL and Boost compatible edit distance functions for C++

Copyright (c) 2013 Erik Erlandson

Author:  Erik Erlandson <erikerlandson@yahoo.com>

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

#include <iostream>

// get the edit_distance() function
#include <boost/algorithm/sequence/edit_distance.hpp>
using boost::algorithm::sequence::edit_distance;

int main(int argc, char** argv) {
    char const* str1 = "hello, world.";
    char const* str2 = "Hello World!";

    // Obtain the cost of minimal edit sequence to transform str1 --> str2.
    // The default cost function defines the cost of insertion, deletion and substitution to be 1.  Elements that are equal cost 0.
    // The distance should be 7
    // delete 'h', insert 'H'
    // delete ','
    // delete 'w', insert 'W'
    // delete '.', insert "!"
    unsigned dist = edit_distance(str1, str2);
    std::cout << "The edit distance between \"" << str1 << "\" and \"" << str2 << "\" = " << dist << "\n";

    return 0;
}


/**
 * @file gradientflow_main.cc
 * @brief NPDE homework GradientFlow code
 * @author Unknown, Oliver Rietmann
 * @date 04.04.2021
 * @copyright Developed at ETH Zurich
 */

#include <Eigen/Core>
#include <cmath>
#include <iostream>
#include <vector>

#include "gradientflow.h"

const static Eigen::IOFormat CSVFormat(Eigen::FullPrecision,
                                       Eigen::DontAlignCols, ", ", "\n");

int main() {
  double T = 0.1;
  double lambda = 10.0;
  int N = 10000;
  std::cout << "T = " << T << ", lambda = " << lambda << std::endl;

  Eigen::Vector2d d(1.0, 0.0);
  Eigen::Vector2d y0(1.0, 0.0);
  std::vector<Eigen::VectorXd> Y =
      GradientFlow::solveGradientFlow(d, lambda, y0, T, N);
  std::cout << "Final value (exact): " << Y.back().transpose().format(CSVFormat)
            << std::endl;

  double exact = Y.back()(0);
  Eigen::VectorXi N_list(6);
  N_list << 10, 20, 40, 80, 160, 320;
  std::cout << "Error table:\n";
  std::cout << "N\terror norm" << std::endl;
  for (int i = 0; i < N_list.size(); ++i) {
    std::vector<Eigen::VectorXd> Y =
        GradientFlow::solveGradientFlow(d, lambda, y0, T, N_list(i));
    double approx = Y.back()(0);
    std::cout << N << "\t" << std::abs(approx - exact) << "\t" << std::endl;
  }

  return 0;
}


//=======================================================================
// Copyright (c) 2015
//
// 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)
//=======================================================================
/**
 * @file frequent-directions.cpp
 * @brief frequent_directions binnary
 * @author Tomasz Strozak
 * @version 1.0
 * @date 2015-07-13
 */

#include "paal/sketch/frequent_directions.hpp"
#include "paal/utils/functors.hpp"
#include "paal/utils/irange.hpp"
#include "paal/utils/print_collection.hpp"
#include "paal/utils/read_rows.hpp"
#include "paal/utils/system_message.hpp"

#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/program_options.hpp>

#include <cstdlib>
#include <iostream>
#include <fstream>
#include <string>
#include <vector>

namespace utils = paal::utils;
namespace po = boost::program_options;
using coordinate_t = double;
using matrix_t = boost::numeric::ublas::matrix<coordinate_t>;
using fd_t = paal::frequent_directions<matrix_t>;

struct params {
    size_t m_sketch_rows;
    size_t m_sketch_compress_size;
    //TODO
    //unsigned m_nthread;
    size_t m_row_buffer_size;
    bool m_compress_at_end;
};

void m_main(po::variables_map const &vm, params const &p,
            std::istream &input_stream, std::ostream &output_stream) {
    fd_t fd_sketch;

    std::vector<std::vector<coordinate_t>> row_buffer;
    row_buffer.reserve(p.m_row_buffer_size);

    auto ignore_bad_row = [&](std::string const &bad_line) {
        utils::warning("following line will be ignored cause of bad format: ", bad_line);
        return true;
    };

    std::size_t rows_count;
    std::size_t columns_count;
    if (vm.count("model_in")) {
        std::ifstream ifs(vm["model_in"].as<std::string>());
        boost::archive::binary_iarchive ia(ifs);
        ia >> fd_sketch;
        auto sketch = fd_sketch.get_sketch().first;
        rows_count = sketch.size1();
        columns_count = sketch.size2();
    }
    else {
        paal::read_rows_first_row_size<coordinate_t>
            (input_stream, row_buffer, p.m_row_buffer_size, ignore_bad_row);

        if(row_buffer.empty()) {
            utils::failure("Empty input data");
        }

        rows_count = p.m_sketch_rows;
        columns_count = boost::size(row_buffer.front());
        if(vm.count("sketch_compress_size")) {
            fd_sketch = paal::make_frequent_directions<coordinate_t>(rows_count, columns_count, p.m_sketch_compress_size);
        }
        else {
            fd_sketch = paal::make_frequent_directions<coordinate_t>(rows_count, columns_count);
        }

        fd_sketch.update_range(row_buffer);
    }

    while (input_stream.good()) {
        row_buffer.clear();
        paal::read_rows<coordinate_t>
            (input_stream, row_buffer, columns_count, p.m_row_buffer_size, ignore_bad_row);
        fd_sketch.update_range(row_buffer);
    }

    if (p.m_compress_at_end) {
        fd_sketch.compress();
    }

    auto sketch = fd_sketch.get_sketch().first;
    boost::numeric::ublas::matrix_range<matrix_t> sketch_range (sketch,
         boost::numeric::ublas::range(0, fd_sketch.get_sketch().second),
         boost::numeric::ublas::range(0, columns_count));
    paal::print_matrix(output_stream, sketch_range, " ");
    output_stream << std::endl;

    if (vm.count("model_out")) {
        std::ofstream ofs(vm["model_out"].as<std::string>());
        boost::archive::binary_oarchive oa(ofs);
        oa << fd_sketch;
    }

}

int main(int argc, char** argv) {
    params p{};

    po::options_description desc("Frequent-directions - \n"\
            "suite for a matrix sketching using Singular Value Decomposition\n\nUsage:\n"\
            "This command will read data from standard input and write computed sketch to standard output:\n"\
            "\tfrequent-directions --sketch_rows numer_of_sketch_rows\n\n"\
            "If you want to read data from an input_file and write computed sketch to an output_file you can use following command:\n"\
            "\tfrequent-directions --input input_file --output output_file -r rows\n\n"\
            "If you want to change compress_size and save model you can use following command:\n"\
            "\tfrequent-directions -i input_file -r rows -s compress_size --model_out model\n\n"\
            "Then if you want to use this model and add additional data:\n"\
            "\tfrequent-directions -i input_file --model_in model\n\n"\
            "Options description");

    desc.add_options()
        ("help,h", "help message")
        ("input,i", po::value<std::string>(), "path to the file with input data in csv format with space as delimiter, "\
                "(default read from standart input)")
        ("output,o", po::value<std::string>(), "path to the file with result sketch matrix, only nonzero rows are printed, "\
                "(default write to standart output)")
        ("sketch_rows,r", po::value<std::size_t>(&p.m_sketch_rows), "number of sketch rows")
        ("sketch_compress_size,s", po::value<size_t>(&p.m_sketch_compress_size), "sketch compress size, "\
                "(default is half of number of sketch rows)")
        ("model_in", po::value<std::string>(), "read the sketch model from this file")
        ("model_out", po::value<std::string>(), "write the sketch model to this file")
        ("final_compress", po::value<bool>(&p.m_compress_at_end)->default_value(true),
                "determine if sketch will be compressed after update all data, "\
                "compression in the final phase is necessary to fulfill sketch approximation ratios")
    //TODO
    //    ("nthread,n", po::value<unsigned>(&p.m_nthread)->default_value(std::thread::hardware_concurrency()),
    //          "number of threads (default = number of cores)")
        ("row_buffer_size", po::value<std::size_t>(&p.m_row_buffer_size)->default_value(100000),
                  "size of row buffer (default value = 100000)")
    ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    auto param_is_set_explicitly = [&vm] (const std::string &param_name) {
        return vm.count(param_name) > 0 && !vm[param_name].defaulted();
    };

    if (vm.count("help")) {
        utils::info(desc);
        return EXIT_SUCCESS;
    }

    auto error_with_usage = [&] (const std::string &message) {
        utils::failure(message, "\n", desc);
    };

    if (vm.count("model_in") == 0 && vm.count("sketch_rows") == 0) {
        error_with_usage("Input model sketch or number of sketch rows was not set");
    }

    if (vm.count("model_in")) {
        auto ignored = [&](std::string const & param) {
            if (param_is_set_explicitly(param)) {
                utils::warning("parameter ", param, " was set, but model_in is used, param ", param, " is discarded");
            }
        };
        ignored("sketch_rows");
        ignored("sketch_compress_size");
    }

    if (p.m_row_buffer_size <= 0) {
        error_with_usage("Size of row buffer must be positive");
    }

    std::ifstream ifs;
    if (vm.count("input")) {
        ifs.open(vm["input"].as<std::string>());
    }

    std::ofstream ofs;
    if (vm.count("output")) {
        ofs.open(vm["output"].as<std::string>());
    }

    m_main(vm, p,
           vm.count("input") ? ifs : std::cin,
           vm.count("output") ? ofs : std::cout);

    return EXIT_SUCCESS;
}


// ///////////////////////////////////////////////////
// Roy Burstein and Aaron Osgood-Zimmerman
// August 2017
// Template file for space-time-Z GPR model.
// Used for fitting IHME Geospatial MBG models
// ///////////////////////////////////////////////////

// ///////////////////////////////////////////////////
// NOTES:
// 1. Type `Type` is a special TMB type that should be used for all variables, except `int` can be used as well
// 2. In our nomenclature, Z is a third interaction (ie age) which defaults to AR1
// 3. Requires same space mesh for all time-Z points
// 4. Anything in the density namespace (ie SEPARABLE) returns the negative log likelihood and is thus added to accumulator
//    also, other density function such as dnorm and dbinom return positive log likelihood and are thus subtracted away.
// 5. ref https://github.com/nmmarquez/re_simulations/blob/master/inla/sta.cpp
//        https://github.com/nmmarquez/re_simulations/blob/master/inla/SPDEAR1AR1.R
// ///////////////////////////////////////////////////

// include libraries
#include <TMB.hpp>
#include <Eigen/Sparse>
#include <vector>
using namespace density;
using Eigen::SparseMatrix;

// helper function to make sparse SPDE precision matrix
// Inputs:
//    logkappa: log(kappa) parameter value
//    logtau: log(tau) parameter value
//    M0, M1, M2: these sparse matrices are output from R::INLA::inla.spde2.matern()$param.inla$M*
template<class Type>
SparseMatrix<Type> spde_Q(Type logkappa, Type logtau, SparseMatrix<Type> M0,
                          SparseMatrix<Type> M1, SparseMatrix<Type> M2) {
    SparseMatrix<Type> Q;
    Type kappa2 = exp(2. * logkappa);
    Type kappa4 = kappa2*kappa2;
    Q = pow(exp(logtau), 2.)  * (kappa4*M0 + Type(2.0)*kappa2*M1 + M2);
    return Q;
}

// helper function for detecting NAs in the data supplied from R
template<class Type>
bool isNA(Type x){
  return R_IsNA(asDouble(x));
}

// Robust Inverse Logit that sets min and max values to avoid numerical instability
template<class Type>
Type invlogit_robust(Type x){
  if (x < -20.723){
    x = -20.723; // corresponds to p=1e-9
  } else if ( x > 20.723 ){
    x = 20.723;  // cooresponds to p=1-1e-9
  }
  return 1 / (1 + exp( -1.0 * x ));
}

// AR funtion from neal m
template<class Type>
SparseMatrix<Type> ar_Q(int N, Type rho, Type sigma) {
  SparseMatrix<Type> Q(N,N);
  Q.insert(0,0) = (1.) / pow(sigma, 2.);
  for (size_t n = 1; n < N; n++) {
    Q.insert(n,n) = (1. + pow(rho, 2.)) / pow(sigma, 2.);
    Q.insert(n-1,n) = (-1. * rho) / pow(sigma, 2.);
    Q.insert(n,n-1) = (-1. * rho) / pow(sigma, 2.);
  }
  Q.coeffRef(N-1,N-1) = (1.) / pow(sigma, 2.);
  return Q;
}

// Corresponding list object on the C++ side
template<class Type>
struct option_list {
  int use_priors;
  int adreport_off;
  int nugget;
  int country_random;
  int NID_random;
  int useGP;
  // Way easier to read these in as vectors of integers and then index the first
  option_list(SEXP x){
    use_priors = asVector<int>(getListElement(x,"use_priors"))[0];
    adreport_off = asVector<int>(getListElement(x,"adreport_off"))[0];
    nugget = asVector<int>(getListElement(x,"nugget"))[0];
    country_random = asVector<int>(getListElement(x,"country_random"))[0];
    NID_random = asVector<int>(getListElement(x,"NID_random"))[0];
    useGP = asVector<int>(getListElement(x,"useGP"))[0];
  }
};

// how to read in an object that is used as a prior for standard devs of res
template<class Type>
struct prior_type_sigma {
  std::string name;
  Type par1;
  Type par2;

  prior_type_sigma(SEXP x){
    name = CHAR(STRING_ELT(getListElement(x,"type"), 0));
    par1 = asVector<float>(getListElement(x,"par1"))[0];
    par2 = asVector<float>(getListElement(x,"par2"))[0];
  }
};

// how to read in an object that is used as a prior for Matern hyperparameters
template<class Type>
struct prior_type_matern {
  std::string name;
  Type par1a; //mean logtau / rho0
  Type par1b; //prec logtau / alpha_rho
  Type par2a; //mean logkappa / sigma0
  Type par2b; //prec logkappa / alpha_sigma

  prior_type_matern(SEXP x){
    name = CHAR(STRING_ELT(getListElement(x,"type"), 0));
    par1a = asVector<float>(getListElement(x,"par1"))[0];
    par1b = asVector<float>(getListElement(x,"par1"))[1];
    par2a = asVector<float>(getListElement(x,"par2"))[0];
    par2b = asVector<float>(getListElement(x,"par2"))[1];
  }
};

// evaluate a prior for sigma using the read in object
template<class Type>
Type eval_prior_sigma(prior_type_sigma<Type> prior, Type log_sigma){
  Type penalty;
  // transform log sigma to log tau space to match INLA prior specification
  // NOTE: log tau --> x=log sigma -->
  //    transform: log tau = -2x --> Jacobian: |J| = 2
  // https://becarioprecario.bitbucket.io/inla-gitbook/ch-priors.html#sec:priors
  Type tau = pow(exp(log_sigma), -2.);
  Type logtau = log(tau);

  if(prior.name == "pc.prec") {
    Type lambda = - log(prior.par2) / prior.par1;
    penalty = -lambda * exp(-logtau/Type(2.0)) - logtau/Type(2.0);
  }
  else if(prior.name == "normal") {
    // prior.par2 needs to be in precision space such as in INLA
    // https://inla.r-inla-download.org/r-inla.org/doc/prior/gaussian.pdf
    penalty = dnorm(logtau, prior.par1, pow(prior.par2, -.5), true);
  }
  else { //loggamma
    // prior.par2 gamma function for TMB uses shape and scale
    // https://kaskr.github.io/adcomp/group__R__style__distribution.html#gab0e2205710a698ad6a0ed39e0652c9a3
    // INLA uses shape and rate so we need to transforma
    // https://inla.r-inla-download.org/r-inla.org/doc/prior/prior-loggamma.pdf
    penalty = dlgamma(logtau, prior.par1, 1./prior.par2, true);
  }

  return penalty;
}

// evaluate priors for matern using the read in object
template<class Type>
Type eval_prior_matern(prior_type_matern<Type> prior, Type logtau, Type logkappa){
  Type penalty;

  if(prior.name == "pc") {
    Type d = 2.;
    Type lambda1 = -log(prior.par1b) * pow(prior.par1a, d/2.);
    Type lambda2 = -log(prior.par2b) / prior.par2a;
    Type range   = sqrt(8.0) / exp(logkappa);
    Type sigma   = 1.0 / sqrt(4.0 * 3.14159265359 * exp(2.0 * logtau) * exp(2.0 * logkappa));

    penalty = (-d/2. - 1.) * log(range) - lambda1 * pow(range, -d/2.) - lambda2 * sigma;
    // Note: (rho, sigma) --> (x=log kappa, y=log tau) -->
    //  transforms: rho = sqrt(8)/e^x & sigma = 1/(sqrt(4pi)*e^x*e^y)
    //  --> Jacobian: |J| propto e^(-y -2x)
    Type jacobian = - logtau - 2.0*logkappa;
    penalty += jacobian;
  }
  else { //normal
    penalty = dnorm(logtau, prior.par1a, 1/sqrt(prior.par1b), true) +
      dnorm(logkappa, prior.par2a, 1/sqrt(prior.par2b), true);
  }

  return penalty;
}

// Constrain alpha values
template<class Type>
vector<Type> constrain_pars(vector<Type> alpha, vector<int> constraints){
  int K = alpha.size();
  vector<Type> alpha_c(K);

  for(int k = 0; k < K; k++){
    if(constraints[k] == 1){
      alpha_c[k] = exp(alpha[k]);
    }
    if(constraints[k] == -1){
      alpha_c[k] = -1. * exp(alpha[k]);
    }
    if(constraints[k] == 0){
      alpha_c[k] = alpha[k];
    }
  }

  return alpha_c;
}

// objective function (ie the likelihood function for the model), returns the evaluated negative log likelihood
template<class Type>
Type objective_function<Type>::operator() ()
{

  // ////////////////////////////////////////////////////////////////////////////
  // INPUTS
  // ////////////////////////////////////////////////////////////////////////////
  DATA_INTEGER(flag); // flag=0 => only prior

  // Indices
  DATA_INTEGER(num_i);       // number of datapts in space-time-Z (aka STZ)
  DATA_INTEGER(num_s);       // number of mesh pts in space mesh
  DATA_INTEGER(num_t);       // number of time periods
  DATA_INTEGER(num_z);       // number of Z groups

  // Data (each, excpect for X_ij is a vector of length num_i)
  DATA_VECTOR(y_i);          // obs successes per binomial experiment at point i (aka cluster)
  DATA_VECTOR(n_i);          // trials per cluster
  DATA_IVECTOR(t_i);         // time period of the data point
  DATA_IVECTOR(c_re_i);      // country identifiers
  DATA_IVECTOR(nid_re_i);    // NID identifiers
  DATA_IVECTOR(w_i);         // weights for observations
  DATA_MATRIX(X_ij);         // covariate design matrix (num_i by number of fixed effects matrix)
  DATA_IVECTOR(fconstraints); // constraints of fixed effects

  // instructions for likelihood asessment
  DATA_VECTOR(lik_gaussian_i); // data likelihood for each row
  DATA_VECTOR(lik_binomial_i); // data likelihood for each row
  DATA_VECTOR(sd_i);           // crossalked standard deviation (set to zero if non-existant)

  // SPDE objects
  DATA_SPARSE_MATRIX(M0);    // used to make gmrf precision
  DATA_SPARSE_MATRIX(M1);    // used to make gmrf precision
  DATA_SPARSE_MATRIX(M2);    // used to make gmrf precision
  DATA_SPARSE_MATRIX(Aproj); // used to project from spatial mesh to data locations

  // Boolean vector of options to be used to select different models/modelling
  //   options: see above
  DATA_STRUCT(options, option_list);

  // Prior specifications
  DATA_STRUCT(prior_log_nugget_sigma, prior_type_sigma);
  DATA_STRUCT(prior_log_cre_sigma, prior_type_sigma);
  DATA_STRUCT(prior_log_nidre_sigma, prior_type_sigma);
  DATA_STRUCT(prior_matern, prior_type_matern);

  // Parameters
  PARAMETER_VECTOR(alpha_j);   // fixed effect coefs, including intercept as first index
  PARAMETER(logtau);           // log of INLA tau param (precision of space-time covariance mat)
  PARAMETER(logkappa);         // log of INLA kappa - related to spatial correlation and range
  PARAMETER(trho);             // temporal autocorrelation parameter for AR1, natural scale
  PARAMETER(zrho);             // Z autocorrelation parameter for AR1, natural scale
  PARAMETER(log_nugget_sigma); // log of the standard deviation of the normal error nugget term
  PARAMETER(log_cre_sigma);    // log of the standard deviation of the country random effect (later do as vec if using Random SLOPE TODO)
  PARAMETER(log_nidre_sigma);  // log of the standard deviation of the nid random effect
  PARAMETER(log_gauss_sigma);  // log of sigma for any gaussian observations

  // Random effects
  PARAMETER_ARRAY(Epsilon_stz);  // Random effects for each STZ mesh location. Should be 3D array of dimensions num_s by num_t by num_z
  PARAMETER_VECTOR(nug_i);       // Random effects of the nugget
  PARAMETER_VECTOR(cntry_re);    // Random effects values for country intercept
  PARAMETER_VECTOR(nid_re);      // Random effects values for nid intercept

  printf("Epsilon_stz size: %ld \n", Epsilon_stz.size());

  // ////////////////////////////////////////////////////////////////////////////
  // LIKELIHOOD
  // ////////////////////////////////////////////////////////////////////////////

  // Define the joint-negative log-likelihood as a parallel_accumulator
  // this allows us to add or subtract numbers to the object in parallel
  // parallel_accumulator<Type> jnll(this);
  Type jnll = 0;

  // print parallel info
  max_parallel_regions = omp_get_max_threads();

  // Make spatial precision matrix
  SparseMatrix<Type> Q_ss   = spde_Q(logkappa, logtau, M0, M1, M2);
  printf("Q_ss size: %ld \n", Q_ss.size());

  // Make transformations of some of our parameters
  Type range         = sqrt(8.0) / exp(logkappa);
  Type sigma         = 1.0 / sqrt(4.0 * 3.14159265359 * exp(2.0 * logtau) * exp(2.0 * logkappa));
  Type trho_trans    = (exp(trho) - 1) / (exp(trho) + 1); // TRANSOFRM from -inf, inf to -1, 1.. //log((1.1 + trho) / (1.1 - trho));
  Type zrho_trans    = (exp(zrho) - 1) / (exp(zrho) + 1); //TRANSOFRM from -inf, inf to -1, 1.. // log((1.1 + zrho) / (1.1 - zrho));
  Type nugget_sigma  = exp(log_nugget_sigma);
  Type cre_sigma     = exp(log_cre_sigma);
  Type nidre_sigma   = exp(log_nidre_sigma);
  Type gauss_sigma   = exp(log_gauss_sigma);

  // Define objects for derived values
  vector<Type> fe_i(num_i);                         // main effect X_ij %*% t(alpha_j)
  vector<Type> epsilon_stz(num_s * num_t * num_z);  // Epsilon_stz unlisted into a vector for easier matrix multiplication
  vector<Type> projepsilon_i(num_i);                // value of gmrf at data points
  vector<Type> prob_i(num_i);                       // Logit estimated prob for each point i

  // Latent field/Random effect contribution to likelihood.
  // Possibilities of Kronecker include: S, ST, SZ, and STZ
  if (num_t == 1 & num_z == 1)  {
    printf("GP FOR SPACE  ONLY \n");
    PARALLEL_REGION jnll += GMRF(Q_ss,false)(epsilon_stz);
  } else if(num_t > 1 & num_z == 1) {
    printf("GP FOR SPACE-TIME \n");
    PARALLEL_REGION jnll += SEPARABLE(AR1(trho_trans),GMRF(Q_ss,false))(Epsilon_stz);
  } else if (num_t == 1 & num_z > 1) {
    printf("GP FOR SPACE-Z \n");
    PARALLEL_REGION jnll += SEPARABLE(AR1(zrho_trans),GMRF(Q_ss,false))(Epsilon_stz);
  } else if (num_t > 1 & num_z > 1) {
    printf("GP FOR SPACE-TIME-Z \n");
    PARALLEL_REGION jnll += SEPARABLE(AR1(zrho_trans),SEPARABLE(AR1(trho_trans),GMRF(Q_ss,false)))(Epsilon_stz);
  }

  // nugget contribution to the likelihood
  if(options.nugget == 1){
    printf("Nugget \n");
    for (int i = 0; i < num_i; i++){
      // binomial models with sd_i, the additional variance gets put into the nugget.
      // for gaussian models nuggets seem unidentifiable so, the sd_i is in the data likelihood
      PARALLEL_REGION jnll -= dnorm(nug_i(i), Type(0.0), sqrt( pow(sd_i(i),2) + pow(nugget_sigma,2) ), true);
    }
  }

  // country random intercept
  if(options.country_random == 1){
    printf("Country RE \n");
    for(int i=0; i<cntry_re.size(); i++){
      PARALLEL_REGION jnll -= dnorm(cntry_re(i), Type(0.0), cre_sigma, true);
    }
  }

  // nid random intercept
  if(options.NID_random == 1){
    printf("NID RE \n");
    for(int i=0; i<nid_re.size(); i++){
      PARALLEL_REGION jnll -= dnorm(nid_re(i), Type(0.0), nidre_sigma, true);
    }
  }

  // Transform GMRFs and make vector form
  printf("Transform GMRF \n");
  for(int s = 0; s < num_s; s++){
    for(int t = 0; t < num_t; t++){
      if(num_z == 1) {
        epsilon_stz[(s + num_s * t )] = Epsilon_stz(s,t);
      } else {
        for(int z = 0; z < num_z; z++){
          epsilon_stz[(s + num_s * t + num_s * num_t * z)] = Epsilon_stz(s,t,z);
        }
      }
    }
  }

  // Project from mesh points to data points in order to eval likelihood at each data point
  printf("Project Epsilon \n");
  projepsilon_i = Aproj * epsilon_stz.matrix();

  // evaluate fixed effects for alpha_j values
  vector<Type> calpha_j = constrain_pars(alpha_j, fconstraints);
  fe_i = X_ij * calpha_j.matrix();

  // Return un-normalized density on request
  if (flag == 0) return jnll;

  // Prior contribution to likelihood. Values are defaulted.
  // Only run if options.use_priors==1
  if(options.use_priors == 1) {
    PARALLEL_REGION jnll -= eval_prior_matern(prior_matern, logtau, logkappa);
    if(num_t > 1) {
      // N(0,2.58^2) prior on log((1+rho)/(1-rho))
      PARALLEL_REGION jnll -= dnorm(trho, Type(0.0), Type(2.58), true);
    }
    if(num_z > 1) {
      // N(0,2.58^2) prior on log((1+rho)/(1-rho))
      PARALLEL_REGION jnll -= dnorm(zrho, Type(0.0), Type(2.58), true);
    }
    for(int j = 0; j < alpha_j.size(); j++){
      // N(0,3) prior for fixed effects.
      PARALLEL_REGION jnll -= dnorm(alpha_j(j), Type(0.0), Type(3.0), true);
    }
    // if using nugget (option in 3rd index)
    if(options.nugget == 1){
      PARALLEL_REGION jnll -= eval_prior_sigma(prior_log_nugget_sigma, log_nugget_sigma);
    }
    // if using country (option in 4th index)
    if(options.country_random == 1){
      PARALLEL_REGION jnll -= eval_prior_sigma(prior_log_cre_sigma, log_cre_sigma);
    }
    // if using nid (option in 5th index)
    if(options.NID_random == 1){
      PARALLEL_REGION jnll -= eval_prior_sigma(prior_log_nidre_sigma, log_nidre_sigma);
    }
  }

  // Likelihood contribution from each datapoint i
  printf("Data likelihood \n");
  for (int i = 0; i < num_i; i++){

    // mean model
    if(options.useGP==1){
      prob_i(i) = fe_i(i) + projepsilon_i(i) + nug_i(i) + cntry_re(c_re_i(i)) + nid_re(nid_re_i(i));
    } else {
      prob_i(i) = fe_i(i) +  nug_i(i) + cntry_re(c_re_i(i)) + nid_re(nid_re_i(i));
    }

    if(!isNA(y_i(i))){

      if(lik_binomial_i(i) == 1){
        PARALLEL_REGION jnll -= dbinom( y_i(i), n_i(i), invlogit_robust(prob_i(i)), true ) * w_i(i);
      }
      if(lik_gaussian_i(i) == 1){
        // this includes any crosswalked sd (sd_i), other variance (gauss_sigma), and is scaled by sample size (n_i)
        PARALLEL_REGION jnll -= dnorm( y_i(i), prob_i(i),  sqrt( (pow(sd_i(i),2) + (1/n_i(i) * pow(gauss_sigma,2)) ) ), true ) * w_i(i);
      }

    }
  }

  // Report estimates
  if(options.adreport_off == 0){
    ADREPORT(alpha_j);
  }

  return jnll;
}


#include "main.h"
#include <Eigen/MPRealSupport>
#include <Eigen/LU>
#include <Eigen/Eigenvalues>
#include <sstream>

using namespace mpfr;
using namespace std;
using namespace Eigen;

void test_mpreal_support()
{
  // set precision to 256 bits (double has only 53 bits)
  mpreal::set_default_prec(256);
  typedef Matrix<mpreal,Eigen::Dynamic,Eigen::Dynamic> MatrixXmp;

  std::cerr << "epsilon =         " << NumTraits<mpreal>::epsilon() << "\n";
  std::cerr << "dummy_precision = " << NumTraits<mpreal>::dummy_precision() << "\n";
  std::cerr << "highest =         " << NumTraits<mpreal>::highest() << "\n";
  std::cerr << "lowest =          " << NumTraits<mpreal>::lowest() << "\n";

  for(int i = 0; i < g_repeat; i++) {
    int s = Eigen::internal::random<int>(1,100);
    MatrixXmp A = MatrixXmp::Random(s,s);
    MatrixXmp B = MatrixXmp::Random(s,s);
    MatrixXmp S = A.adjoint() * A;
    MatrixXmp X;

    // Basic stuffs
    VERIFY_IS_APPROX(A.real(), A);
    VERIFY(Eigen::internal::isApprox(A.array().abs2().sum(), A.squaredNorm()));
    VERIFY_IS_APPROX(A.array().exp(),         exp(A.array()));
    VERIFY_IS_APPROX(A.array().abs2().sqrt(), A.array().abs());
    VERIFY_IS_APPROX(A.array().sin(),         sin(A.array()));
    VERIFY_IS_APPROX(A.array().cos(),         cos(A.array()));

    // Cholesky
    X = S.selfadjointView<Lower>().llt().solve(B);
    VERIFY_IS_APPROX((S.selfadjointView<Lower>()*X).eval(),B);

    // partial LU
    X = A.lu().solve(B);
    VERIFY_IS_APPROX((A*X).eval(),B);

    // symmetric eigenvalues
    SelfAdjointEigenSolver<MatrixXmp> eig(S);
    VERIFY_IS_EQUAL(eig.info(), Success);
    VERIFY_IS_APPROX((S.selfadjointView<Lower>() * eig.eigenvectors()),
                      eig.eigenvectors() * eig.eigenvalues().asDiagonal());
  }

  {
    MatrixXmp A(8,3); A.setRandom();
    // test output (interesting things happen in this code)
    std::stringstream stream;
    stream << A;
  }
}

extern "C" {
#include "mpreal/dlmalloc.c"
}
#include "mpreal/mpreal.cpp"


//  (C) Copyright Nick Thompson 2018
//  (C) Copyright Matt Borland 2020
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_STATISTICS_UNIVARIATE_STATISTICS_DETAIL_SINGLE_PASS_HPP
#define BOOST_MATH_STATISTICS_UNIVARIATE_STATISTICS_DETAIL_SINGLE_PASS_HPP

#include <boost/math/tools/config.hpp>
#include <boost/math/tools/assert.hpp>
#include <tuple>
#include <iterator>
#include <type_traits>
#include <cmath>
#include <algorithm>
#include <valarray>
#include <stdexcept>
#include <functional>
#include <vector>

#ifdef BOOST_HAS_THREADS
#include <future>
#include <thread>
#endif

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

template<typename ReturnType, typename ForwardIterator>
ReturnType mean_sequential_impl(ForwardIterator first, ForwardIterator last)
{
    const std::size_t elements {static_cast<std::size_t>(std::distance(first, last))};
    std::valarray<ReturnType> mu {0, 0, 0, 0};
    std::valarray<ReturnType> temp {0, 0, 0, 0};
    ReturnType i {1};
    const ForwardIterator end {std::next(first, elements - (elements % 4))};
    ForwardIterator it {first};

    while(it != end)
    {
        const ReturnType inv {ReturnType(1) / i};
        temp = {static_cast<ReturnType>(*it++), static_cast<ReturnType>(*it++), static_cast<ReturnType>(*it++), static_cast<ReturnType>(*it++)};
        temp -= mu;
        mu += (temp *= inv);
        i += 1;
    }

    const ReturnType num1 {ReturnType(elements - (elements % 4))/ReturnType(4)};
    const ReturnType num2 {num1 + ReturnType(elements % 4)};

    while(it != last)
    {
        mu[3] += (*it-mu[3])/i;
        i += 1;
        ++it;
    }

    return (num1 * std::valarray<ReturnType>(mu[std::slice(0,3,1)]).sum() + num2 * mu[3]) / ReturnType(elements);
}

// Higham, Accuracy and Stability, equation 1.6a and 1.6b:
// Calculates Mean, M2, and variance
template<typename ReturnType, typename ForwardIterator>
ReturnType variance_sequential_impl(ForwardIterator first, ForwardIterator last)
{
    using Real = typename std::tuple_element<0, ReturnType>::type;

    Real M = *first;
    Real Q = 0;
    Real k = 2;
    Real M2 = 0;
    std::size_t n = 1;

    for(auto it = std::next(first); it != last; ++it)
    {
        Real tmp = (*it - M) / k;
        Real delta_1 = *it - M;
        Q += k*(k-1)*tmp*tmp;
        M += tmp;
        k += 1;
        Real delta_2 = *it - M;
        M2 += delta_1 * delta_2;
        ++n;
    }

    return std::make_tuple(M, M2, Q/(k-1), Real(n));
}

// https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance#Higher-order_statistics
template<typename ReturnType, typename ForwardIterator>
ReturnType first_four_moments_sequential_impl(ForwardIterator first, ForwardIterator last)
{
    using Real = typename std::tuple_element<0, ReturnType>::type;
    using Size = typename std::tuple_element<4, ReturnType>::type;

    Real M1 = *first;
    Real M2 = 0;
    Real M3 = 0;
    Real M4 = 0;
    Size n = 2;
    for (auto it = std::next(first); it != last; ++it)
    {
        Real delta21 = *it - M1;
        Real tmp = delta21/n;
        M4 = M4 + tmp*(tmp*tmp*delta21*((n-1)*(n*n-3*n+3)) + 6*tmp*M2 - 4*M3);
        M3 = M3 + tmp*((n-1)*(n-2)*delta21*tmp - 3*M2);
        M2 = M2 + tmp*(n-1)*delta21;
        M1 = M1 + tmp;
        n += 1;
    }

    return std::make_tuple(M1, M2, M3, M4, n-1);
}

#ifdef BOOST_HAS_THREADS

// https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance#Higher-order_statistics
// EQN 3.1: https://www.osti.gov/servlets/purl/1426900
template<typename ReturnType, typename ForwardIterator>
ReturnType first_four_moments_parallel_impl(ForwardIterator first, ForwardIterator last)
{
    using Real = typename std::tuple_element<0, ReturnType>::type;

    const auto elements = std::distance(first, last);
    const unsigned max_concurrency = std::thread::hardware_concurrency() == 0 ? 2u : std::thread::hardware_concurrency();
    unsigned num_threads = 2u;

    // Threading is faster for: 10 + 5.13e-3 N/j <= 5.13e-3N => N >= 10^4j/5.13(j-1).
    const auto parallel_lower_bound = 10e4*max_concurrency/(5.13*(max_concurrency-1));
    const auto parallel_upper_bound = 10e4*2/5.13; // j = 2

    // https://lemire.me/blog/2020/01/30/cost-of-a-thread-in-c-under-linux/
    if(elements < parallel_lower_bound)
    {
        return detail::first_four_moments_sequential_impl<ReturnType>(first, last);
    }
    else if(elements >= parallel_upper_bound)
    {
        num_threads = max_concurrency;
    }
    else
    {
        for(unsigned i = 3; i < max_concurrency; ++i)
        {
            if(parallel_lower_bound < 10e4*i/(5.13*(i-1)))
            {
                num_threads = i;
                break;
            }
        }
    }

    std::vector<std::future<ReturnType>> future_manager;
    const auto elements_per_thread = std::ceil(static_cast<double>(elements) / num_threads);

    auto it = first;
    for(std::size_t i {}; i < num_threads - 1; ++i)
    {
        future_manager.emplace_back(std::async(std::launch::async | std::launch::deferred, [it, elements_per_thread]() -> ReturnType
        {
            return first_four_moments_sequential_impl<ReturnType>(it, std::next(it, elements_per_thread));
        }));
        it = std::next(it, elements_per_thread);
    }

    future_manager.emplace_back(std::async(std::launch::async | std::launch::deferred, [it, last]() -> ReturnType
    {
        return first_four_moments_sequential_impl<ReturnType>(it, last);
    }));

    auto temp = future_manager[0].get();
    Real M1_a = std::get<0>(temp);
    Real M2_a = std::get<1>(temp);
    Real M3_a = std::get<2>(temp);
    Real M4_a = std::get<3>(temp);
    Real range_a = std::get<4>(temp);

    for(std::size_t i = 1; i < future_manager.size(); ++i)
    {
        temp = future_manager[i].get();
        Real M1_b = std::get<0>(temp);
        Real M2_b = std::get<1>(temp);
        Real M3_b = std::get<2>(temp);
        Real M4_b = std::get<3>(temp);
        Real range_b = std::get<4>(temp);

        const Real n_ab = range_a + range_b;
        const Real delta = M1_b - M1_a;

        M1_a = (range_a * M1_a + range_b * M1_b) / n_ab;
        M2_a = M2_a + M2_b + delta * delta * (range_a * range_b / n_ab);
        M3_a = M3_a + M3_b + (delta * delta * delta) * range_a * range_b * (range_a - range_b) / (n_ab * n_ab)
               + Real(3) * delta * (range_a * M2_b - range_b * M2_a) / n_ab;
        M4_a = M4_a + M4_b + (delta * delta * delta * delta) * range_a * range_b * (range_a * range_a - range_a * range_b + range_b * range_b) / (n_ab * n_ab * n_ab)
               + Real(6) * delta * delta * (range_a * range_a * M2_b + range_b * range_b * M2_a) / (n_ab * n_ab)
               + Real(4) * delta * (range_a * M3_b - range_b * M3_a) / n_ab;
        range_a = n_ab;
    }

    return std::make_tuple(M1_a, M2_a, M3_a, M4_a, elements);
}

#endif // BOOST_HAS_THREADS

// Follows equation 1.5 of:
// https://prod.sandia.gov/techlib-noauth/access-control.cgi/2008/086212.pdf
template<typename ReturnType, typename ForwardIterator>
ReturnType skewness_sequential_impl(ForwardIterator first, ForwardIterator last)
{
    using std::sqrt;
    BOOST_MATH_ASSERT_MSG(first != last, "At least one sample is required to compute skewness.");

    ReturnType M1 = *first;
    ReturnType M2 = 0;
    ReturnType M3 = 0;
    ReturnType n = 2;

    for (auto it = std::next(first); it != last; ++it)
    {
        ReturnType delta21 = *it - M1;
        ReturnType tmp = delta21/n;
        M3 += tmp*((n-1)*(n-2)*delta21*tmp - 3*M2);
        M2 += tmp*(n-1)*delta21;
        M1 += tmp;
        n += 1;
    }

    ReturnType var = M2/(n-1);

    if (var == 0)
    {
        // The limit is technically undefined, but the interpretation here is clear:
        // A constant dataset has no skewness.
        return ReturnType(0);
    }

    ReturnType skew = M3/(M2*sqrt(var));
    return skew;
}

template<typename ReturnType, typename ForwardIterator>
ReturnType gini_coefficient_sequential_impl(ForwardIterator first, ForwardIterator last)
{
    ReturnType i = 1;
    ReturnType num = 0;
    ReturnType denom = 0;

    for(auto it = first; it != last; ++it)
    {
        num += *it*i;
        denom += *it;
        ++i;
    }

    // If the l1 norm is zero, all elements are zero, so every element is the same.
    if(denom == 0)
    {
        return ReturnType(0);
    }
    else
    {
        return ((2*num)/denom - i)/(i-1);
    }
}

template<typename ReturnType, typename ForwardIterator>
ReturnType gini_range_fraction(ForwardIterator first, ForwardIterator last, std::size_t starting_index)
{
    using Real = typename std::tuple_element<0, ReturnType>::type;

    std::size_t i = starting_index + 1;
    Real num = 0;
    Real denom = 0;

    for(auto it = first; it != last; ++it)
    {
        num += *it*i;
        denom += *it;
        ++i;
    }

    return std::make_tuple(num, denom, i);
}

#ifdef BOOST_HAS_THREADS

template<typename ReturnType, typename ExecutionPolicy, typename ForwardIterator>
ReturnType gini_coefficient_parallel_impl(ExecutionPolicy&&, ForwardIterator first, ForwardIterator last)
{
    using range_tuple = std::tuple<ReturnType, ReturnType, std::size_t>;

    const auto elements = std::distance(first, last);
    const unsigned max_concurrency = std::thread::hardware_concurrency() == 0 ? 2u : std::thread::hardware_concurrency();
    unsigned num_threads = 2u;

    // Threading is faster for: 10 + 10.12e-3 N/j <= 10.12e-3N => N >= 10^4j/10.12(j-1).
    const auto parallel_lower_bound = 10e4*max_concurrency/(10.12*(max_concurrency-1));
    const auto parallel_upper_bound = 10e4*2/10.12; // j = 2

    // https://lemire.me/blog/2020/01/30/cost-of-a-thread-in-c-under-linux/
    if(elements < parallel_lower_bound)
    {
        return gini_coefficient_sequential_impl<ReturnType>(first, last);
    }
    else if(elements >= parallel_upper_bound)
    {
        num_threads = max_concurrency;
    }
    else
    {
        for(unsigned i = 3; i < max_concurrency; ++i)
        {
            if(parallel_lower_bound < 10e4*i/(10.12*(i-1)))
            {
                num_threads = i;
                break;
            }
        }
    }

    std::vector<std::future<range_tuple>> future_manager;
    const auto elements_per_thread = std::ceil(static_cast<double>(elements) / num_threads);

    auto it = first;
    for(std::size_t i {}; i < num_threads - 1; ++i)
    {
        future_manager.emplace_back(std::async(std::launch::async | std::launch::deferred, [it, elements_per_thread, i]() -> range_tuple
        {
            return gini_range_fraction<range_tuple>(it, std::next(it, elements_per_thread), i*elements_per_thread);
        }));
        it = std::next(it, elements_per_thread);
    }

    future_manager.emplace_back(std::async(std::launch::async | std::launch::deferred, [it, last, num_threads, elements_per_thread]() -> range_tuple
    {
        return gini_range_fraction<range_tuple>(it, last, (num_threads - 1)*elements_per_thread);
    }));

    ReturnType num = 0;
    ReturnType denom = 0;

    for(std::size_t i = 0; i < future_manager.size(); ++i)
    {
        auto temp = future_manager[i].get();
        num += std::get<0>(temp);
        denom += std::get<1>(temp);
    }

    // If the l1 norm is zero, all elements are zero, so every element is the same.
    if(denom == 0)
    {
        return ReturnType(0);
    }
    else
    {
        return ((2*num)/denom - elements)/(elements-1);
    }
}

#endif // BOOST_HAS_THREADS

template<typename ForwardIterator, typename OutputIterator>
OutputIterator mode_impl(ForwardIterator first, ForwardIterator last, OutputIterator output)
{
    using Z = typename std::iterator_traits<ForwardIterator>::value_type;
    using Size = typename std::iterator_traits<ForwardIterator>::difference_type;

    std::vector<Z> modes {};
    modes.reserve(16);
    Size max_counter {0};

    while(first != last)
    {
        Size current_count {0};
        ForwardIterator end_it {first};
        while(end_it != last && *end_it == *first)
        {
            ++current_count;
            ++end_it;
        }

        if(current_count > max_counter)
        {
            modes.resize(1);
            modes[0] = *first;
            max_counter = current_count;
        }

        else if(current_count == max_counter)
        {
            modes.emplace_back(*first);
        }

        first = end_it;
    }

    return std::move(modes.begin(), modes.end(), output);
}
}}}}

#endif // BOOST_MATH_STATISTICS_UNIVARIATE_STATISTICS_DETAIL_SINGLE_PASS_HPP


//
// Created by lei on 4/21/19.
//

#include "tgo.hpp"
#include "topography.hpp"

#include <armadillo>
#include <nlopt.hpp>
#include <fmt/format.h>
#include <functional>

using namespace std::placeholders;

OptimizeResult::OptimizeResult(const arma::vec &xl,
                               const arma::vec &fl,
                               double x, double f) :
        xl(xl), funl(fl), x(x), f(f) {}

TGO *TGO::pThis = nullptr;

TGO::TGO(std::function<double(double)> functor,
         int nsample, int nk,
         double xmin, double xmax, double xtol) :
        functor_(std::move(functor)),
        nsample_(nsample),
        nk_(nk),
        xmin_(xmin),
        xmax_(xmax),
        xtol_(xtol) {
    pThis = this;
}

double TGO::func_nlopt(const std::vector<double> &x,
                       std::vector<double> &grad,
                       void *f_data) {
    if (pThis == nullptr) {
        exit(-1);
    }
    double fev = pThis->functor_(x[0]);
    if (!grad.empty()) {
        grad[0] = approx_grad(x[0], fev, epsilon_);
    }
    return fev;
}

double TGO::approx_grad(double x, double f, double epsilon) {
    if (pThis == nullptr) {
        exit(-1);
    }
    double fev = pThis->functor_(x + epsilon);
    double grad = (fev - f) / epsilon;
    return grad;
}

OptimizeResult TGO::optimize() {
    arma::vec x = arma::linspace(xmin_, xmax_, nsample_);
    arma::vec fev(nsample_);
    for (auto i = 0; i < nsample_; ++i) {
        fev(i) = functor_(x(i));
    }
    Topography topo(x, fev, nk_);
    std::vector<double> min_pool = topo.minimize_pool();
    std::vector<double> x_local, f_local;

    nlopt::opt opt(nlopt::LD_SLSQP, 1);
    std::vector<double> lb{xmin_}, ub{xmax_};
    opt.set_min_objective(func_nlopt, nullptr);
    opt.set_lower_bounds(lb);
    opt.set_upper_bounds(ub);
    opt.set_xtol_abs(xtol_);
    for (auto x0 : min_pool) {
        std::vector<double> xl{x0};
        double fl = 1.0e20;
        try {
            nlopt::result result = opt.optimize(xl, fl);
        } catch (std::runtime_error &err) {
            fmt::print("{}\n", err.what());
        }
        x_local.push_back(xl[0]);
        f_local.push_back(fl);
    }
    int num_minimal = x_local.size();
    arma::vec xl(&x_local[0], num_minimal, false, true);
    arma::vec fl(&f_local[0], num_minimal, false, true);
    int ind_best = arma::index_min(fl);
    double x_best = xl(ind_best);
    double f_best = fl(ind_best);
    OptimizeResult result(xl, fl, x_best, f_best);
    return result;
}


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/big/big_types.h>
#include <OpenTissue/core/math/optimization/optimization_agglomerate_vector.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

BOOST_AUTO_TEST_SUITE(opentissue_math_big_agglomerate_vector);

BOOST_AUTO_TEST_CASE(test_case)
{
  typedef ublas::vector<size_t>       idx_vector_type;
  typedef ublas::vector<double>       vector_type;

  idx_vector_type new2old;
  idx_vector_type old2new;

  old2new.resize(10,false);
  new2old.resize(10,false);

  old2new( 2 ) = 0 ;
  old2new( 4 ) = 1 ;
  old2new( 6 ) = 2 ;
  old2new( 9 ) = 3 ;
  old2new( 1 ) = 4 ;
  old2new( 3 ) = 5 ;
  old2new( 8 ) = 6 ;
  old2new( 0 ) = 7 ;
  old2new( 5 ) = 8 ;
  old2new( 7 ) = 9 ;

  new2old( 0 ) = 2 ;
  new2old( 1 ) = 4 ;
  new2old( 2 ) = 6 ;
  new2old( 3 ) = 9 ;
  new2old( 4 ) = 1 ;
  new2old( 5 ) = 3 ;
  new2old( 6 ) = 8 ;
  new2old( 7 ) = 0 ;
  new2old( 8 ) = 5 ;
  new2old( 9 ) = 7 ;

  vector_type x_a;
  vector_type x_b;
  vector_type x;
  x.resize(10,false);

  x_a.resize(4,false);
  x_b.resize(6,false);

  x_a(0) = 1.0;
  x_a(1) = 2.0;
  x_a(2) = 3.0;
  x_a(3) = 4.0;

  x_b(0) = 5.0;
  x_b(1) = 6.0;
  x_b(2) = 7.0;
  x_b(3) = 8.0;
  x_b(4) = 9.0;
  x_b(5) = 10.0;

  x.clear();

  OpenTissue::math::optimization::agglomerate_vector( x_a, x_b, new2old, x);

  double tol = 0.01;

  BOOST_CHECK_CLOSE( double( x( 2 ) ), double( x_a(0) ), tol );
  BOOST_CHECK_CLOSE( double( x( 4 ) ), double( x_a(1) ), tol );
  BOOST_CHECK_CLOSE( double( x( 6 ) ), double( x_a(2) ), tol );
  BOOST_CHECK_CLOSE( double( x( 9 ) ), double( x_a(3) ), tol );
  BOOST_CHECK_CLOSE( double( x( 1 ) ), double( x_b(0) ), tol );
  BOOST_CHECK_CLOSE( double( x( 3 ) ), double( x_b(1) ), tol );
  BOOST_CHECK_CLOSE( double( x( 8 ) ), double( x_b(2) ), tol );
  BOOST_CHECK_CLOSE( double( x( 0 ) ), double( x_b(3) ), tol );
  BOOST_CHECK_CLOSE( double( x( 5 ) ), double( x_b(4) ), tol );
  BOOST_CHECK_CLOSE( double( x( 7 ) ), double( x_b(5) ), tol );
}

BOOST_AUTO_TEST_SUITE_END();


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_basic_types.h>
#include <OpenTissue/collision/gjk/gjk_voronoi_simplex_solver_policy.h>
#include <OpenTissue/collision/gjk/gjk_compute_closest_points.h>
#include <OpenTissue/collision/gjk/gjk_support_functors.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

#include <cmath>

using namespace OpenTissue;

BOOST_AUTO_TEST_SUITE(opentissue_collision_gjk_compute_closest_points);

BOOST_AUTO_TEST_CASE(case_by_case_testing)
{
  typedef OpenTissue::math::BasicMathTypes<double, size_t> math_types;

  typedef math_types::quaternion_type                      quaternion_type;
  typedef math_types::vector3_type                         vector3_type;
  typedef math_types::real_type                            real_type;
  typedef math_types::coordsys_type                        transformation_type;
  typedef math_types::value_traits                         value_traits;

  OpenTissue::gjk::VoronoiSimplexSolverPolicy const simplex_solver_policy = OpenTissue::gjk::VoronoiSimplexSolverPolicy();

  OpenTissue::gjk::Sphere<math_types> const supportA;
  OpenTissue::gjk::Sphere<math_types> const supportB;

  size_t    const max_iterations       = 100u;
  real_type const absolute_tolerance   = boost::numeric_cast<real_type>(10e-6);
  real_type const relative_tolerance   = boost::numeric_cast<real_type>(10e-6);
  real_type const stagnation_tolerance = boost::numeric_cast<real_type>(10e-15);

  // Two unit-spheres placed ontop of each other
  {
    transformation_type transformA;
    transformation_type transformB;

    vector3_type a;
    vector3_type b;
    size_t iterations     = 0u;
    size_t status         = 0u;
    real_type distance    = value_traits::infinity();

    transformA.T().clear();
    transformA.Q().identity();
    transformB.T().clear();
    transformB.Q().identity();

    OpenTissue::gjk::compute_closest_points(
      transformA
      , supportA
      , transformB
      , supportB
      , a
      , b
      , distance
      , iterations
      , status
      , absolute_tolerance
      , relative_tolerance
      , stagnation_tolerance
      , max_iterations
      , simplex_solver_policy
      );

    real_type true_distance = length( transformA.T() - transformB.T() ) - value_traits::two();

    std::cout << "\tstatus = " << OpenTissue::gjk::get_status_message(status) << std::endl;
    std::cout << "\tdistance = " << distance << std::endl;
    std::cout << "\titerations = " << iterations << std::endl;
    std::cout << "\ttrue distance = " << true_distance << std::endl;

  }
  // Two unit-spheres overlapping but both placed on the x-axis
  {
    transformation_type transformA;
    transformation_type transformB;

    vector3_type a;
    vector3_type b;
    size_t iterations     = 0u;
    size_t status         = 0u;
    real_type distance    = value_traits::infinity();

    transformA.T().clear();
    transformA.Q().identity();
    transformA.T()(0) = -1.5;
    transformB.T().clear();
    transformB.Q().identity();

    OpenTissue::gjk::compute_closest_points(
      transformA
      , supportA
      , transformB
      , supportB
      , a
      , b
      , distance
      , iterations
      , status
      , absolute_tolerance
      , relative_tolerance
      , stagnation_tolerance
      , max_iterations
      , simplex_solver_policy
      );

    real_type true_distance = length( transformA.T() - transformB.T() ) - value_traits::two();

    std::cout << "\tstatus = " << OpenTissue::gjk::get_status_message(status) << std::endl;
    std::cout << "\tdistance = " << distance << std::endl;
    std::cout << "\titerations = " << iterations << std::endl;
    std::cout << "\ttrue distance = " << true_distance << std::endl;
  }
  // Two unit-spheres exactly touching in one point (= one intersection point) and but both placed on the x-axis
  {
    transformation_type transformA;
    transformation_type transformB;

    vector3_type a;
    vector3_type b;
    size_t iterations     = 0u;
    size_t status         = 0u;
    real_type distance    = value_traits::infinity();

    transformA.T().clear();
    transformA.Q().identity();
    transformA.T()(0) = -2.0;
    transformB.T().clear();
    transformB.Q().identity();

    OpenTissue::gjk::compute_closest_points(
      transformA
      , supportA
      , transformB
      , supportB
      , a
      , b
      , distance
      , iterations
      , status
      , absolute_tolerance
      , relative_tolerance
      , stagnation_tolerance
      , max_iterations
      , simplex_solver_policy
      );

    real_type true_distance = length( transformA.T() - transformB.T() ) - value_traits::two();

    std::cout << "\tstatus = " << OpenTissue::gjk::get_status_message(status) << std::endl;
    std::cout << "\tdistance = " << distance << std::endl;
    std::cout << "\titerations = " << iterations << std::endl;
    std::cout << "\ttrue distance = " << true_distance << std::endl;

  }
  // Two unit-spheres non-overlapping but both placed on the x-axis
  {
    transformation_type transformA;
    transformation_type transformB;

    vector3_type a;
    vector3_type b;
    size_t iterations     = 0u;
    size_t status         = 0u;
    real_type distance    = value_traits::infinity();

    transformA.T().clear();
    transformA.Q().identity();
    transformA.T()(0) = -2.5;
    transformB.T().clear();
    transformB.Q().identity();

    OpenTissue::gjk::compute_closest_points(
      transformA
      , supportA
      , transformB
      , supportB
      , a
      , b
      , distance
      , iterations
      , status
      , absolute_tolerance
      , relative_tolerance
      , stagnation_tolerance
      , max_iterations
      , simplex_solver_policy
      );

    real_type true_distance = length( transformA.T() - transformB.T() ) - value_traits::two();

    std::cout << "\tstatus = " << OpenTissue::gjk::get_status_message(status) << std::endl;
    std::cout << "\tdistance = " << distance << std::endl;
    std::cout << "\titerations = " << iterations << std::endl;
    std::cout << "\ttrue distance = " << true_distance << std::endl;
  }

}

BOOST_AUTO_TEST_CASE(random_test)
{
  typedef OpenTissue::math::BasicMathTypes<double, size_t> math_types;

  typedef math_types::quaternion_type                      quaternion_type;
  typedef math_types::vector3_type                         vector3_type;
  typedef math_types::real_type                            real_type;
  typedef math_types::coordsys_type                        transformation_type;
  typedef math_types::value_traits                         value_traits;

  typedef OpenTissue::gjk::Simplex<vector3_type>           simplex_type;

  OpenTissue::gjk::VoronoiSimplexSolverPolicy const simplex_solver_policy = OpenTissue::gjk::VoronoiSimplexSolverPolicy();

  OpenTissue::gjk::Sphere<math_types> const supportA;
  OpenTissue::gjk::Sphere<math_types> const supportB;

  size_t    const max_iterations       = 100u;
  real_type const absolute_tolerance   = boost::numeric_cast<real_type>(10e-6);
  real_type const relative_tolerance   = boost::numeric_cast<real_type>(10e-10);
  real_type const stagnation_tolerance = boost::numeric_cast<real_type>(0.0);

  for(size_t i=0;i<100u;++i)
  {
    transformation_type transformA;
    transformation_type transformB;

    quaternion_type Q;
    vector3_type a;
    vector3_type b;
    size_t iterations     = 0u;
    size_t status         = 0u;
    real_type distance    = value_traits::infinity();

    OpenTissue::math::random( transformA.T(), -2.0, 2.0 );
    OpenTissue::math::random( transformB.T(), -2.0, 2.0 );
    Q.random();
    transformA.Q() = OpenTissue::math::unit ( Q );
    Q.random();
    transformB.Q() = OpenTissue::math::unit ( Q );

    OpenTissue::gjk::compute_closest_points(
      transformA
      , supportA
      , transformB
      , supportB
      , a
      , b
      , distance
      , iterations
      , status
      , absolute_tolerance
      , relative_tolerance
      , stagnation_tolerance
      , max_iterations
      , simplex_solver_policy
      );

    real_type true_distance = length( transformA.T() - transformB.T() ) - value_traits::two();

    if(  true_distance > absolute_tolerance )
    {
      BOOST_CHECK_CLOSE( true_distance, distance, 0.05 );

      BOOST_CHECK( status != OpenTissue::gjk::ABSOLUTE_CONVERGENCE );            // Would indicate penetration
      BOOST_CHECK( status != OpenTissue::gjk::INTERSECTION );                    // Would indicate penetration
      BOOST_CHECK( status != OpenTissue::gjk::ITERATING );                       // Would indicate internal error in GJK
      BOOST_CHECK( status != OpenTissue::gjk::EXCEEDED_MAX_ITERATIONS_LIMIT );   // Would indicate internal error in GJK
      BOOST_CHECK( status != OpenTissue::gjk::NON_DESCEND_DIRECTION );           // Would indicate internal error in GJK
      //BOOST_CHECK( status != OpenTissue::gjk::RELATIVE_CONVERGENCE );          // Would indicate convergence to positive distance
      //BOOST_CHECK( status != OpenTissue::gjk::SIMPLEX_EXPANSION_FAILED );      // Would indicate convergence to positive distance
      //BOOST_CHECK( status != OpenTissue::gjk::STAGNATION );                    // Would indicate convergence to positive distance
      //BOOST_CHECK( status != OpenTissue::gjk::LOWER_ERROR_BOUND_CONVERGENCE ); // Would indicate convergence to positive distance
    }
    else
    {
      BOOST_CHECK( 0.0 <= distance );
      BOOST_CHECK( distance < absolute_tolerance );

      BOOST_CHECK( status != OpenTissue::gjk::STAGNATION );                     // Can only occur in case of positive distance
      BOOST_CHECK( status != OpenTissue::gjk::LOWER_ERROR_BOUND_CONVERGENCE );  // Can only occur in case of positive distance
      BOOST_CHECK( status != OpenTissue::gjk::RELATIVE_CONVERGENCE );           // Can only occur in case of positive distance
      BOOST_CHECK( status != OpenTissue::gjk::SIMPLEX_EXPANSION_FAILED );       // Can only occur in case of positive distance
      BOOST_CHECK( status != OpenTissue::gjk::ITERATING );                      // Would indicate internal error in GJK
      BOOST_CHECK( status != OpenTissue::gjk::EXCEEDED_MAX_ITERATIONS_LIMIT );  // Would indicate internal error in GJK
      BOOST_CHECK( status != OpenTissue::gjk::NON_DESCEND_DIRECTION );          // Would indicate internal error in GJK
      //BOOST_CHECK( status != OpenTissue::gjk::ABSOLUTE_CONVERGENCE );         // Indicates penetration
      //BOOST_CHECK( status != OpenTissue::gjk::INTERSECTION );                 // Indicates penetration
    }

    OpenTissue::gjk::compute_closest_points(
      transformA
      , supportA
      , transformB
      , supportB
      , a
      , b
      , distance
      , status
      );

    if(  true_distance > absolute_tolerance )
    {
      BOOST_CHECK_CLOSE( true_distance, distance, 0.05 );

      BOOST_CHECK( status != OpenTissue::gjk::ABSOLUTE_CONVERGENCE );            // Would indicate penetration
      BOOST_CHECK( status != OpenTissue::gjk::INTERSECTION );                    // Would indicate penetration
      BOOST_CHECK( status != OpenTissue::gjk::ITERATING );                       // Would indicate internal error in GJK
      BOOST_CHECK( status != OpenTissue::gjk::EXCEEDED_MAX_ITERATIONS_LIMIT );   // Would indicate internal error in GJK
      BOOST_CHECK( status != OpenTissue::gjk::NON_DESCEND_DIRECTION );           // Would indicate internal error in GJK
      //BOOST_CHECK( status != OpenTissue::gjk::RELATIVE_CONVERGENCE );          // Would indicate convergence to positive distance
      //BOOST_CHECK( status != OpenTissue::gjk::SIMPLEX_EXPANSION_FAILED );      // Would indicate convergence to positive distance
      //BOOST_CHECK( status != OpenTissue::gjk::STAGNATION );                    // Would indicate convergence to positive distance
      //BOOST_CHECK( status != OpenTissue::gjk::LOWER_ERROR_BOUND_CONVERGENCE ); // Would indicate convergence to positive distance
    }
    else
    {
      BOOST_CHECK( 0.0 <= distance );
      BOOST_CHECK( distance < absolute_tolerance );

      BOOST_CHECK( status != OpenTissue::gjk::STAGNATION );                     // Can only occur in case of positive distance
      BOOST_CHECK( status != OpenTissue::gjk::LOWER_ERROR_BOUND_CONVERGENCE );  // Can only occur in case of positive distance
      BOOST_CHECK( status != OpenTissue::gjk::RELATIVE_CONVERGENCE );           // Can only occur in case of positive distance
      BOOST_CHECK( status != OpenTissue::gjk::SIMPLEX_EXPANSION_FAILED );       // Can only occur in case of positive distance
      BOOST_CHECK( status != OpenTissue::gjk::ITERATING );                      // Would indicate internal error in GJK
      BOOST_CHECK( status != OpenTissue::gjk::EXCEEDED_MAX_ITERATIONS_LIMIT );  // Would indicate internal error in GJK
      BOOST_CHECK( status != OpenTissue::gjk::NON_DESCEND_DIRECTION );          // Would indicate internal error in GJK
      //BOOST_CHECK( status != OpenTissue::gjk::ABSOLUTE_CONVERGENCE );         // Indicates penetration
      //BOOST_CHECK( status != OpenTissue::gjk::INTERSECTION );                 // Indicates penetration
    }
  }
}

BOOST_AUTO_TEST_CASE(cylinder_test)
{
  typedef OpenTissue::math::BasicMathTypes<double, size_t> math_types;

  typedef math_types::quaternion_type                      quaternion_type;
  typedef math_types::vector3_type                         vector3_type;
  typedef math_types::real_type                            real_type;
  typedef math_types::coordsys_type                        transformation_type;
  typedef math_types::value_traits                         value_traits;

  typedef OpenTissue::gjk::Simplex<vector3_type>           simplex_type;

  OpenTissue::gjk::VoronoiSimplexSolverPolicy const simplex_solver_policy = OpenTissue::gjk::VoronoiSimplexSolverPolicy();

  // We let the cylinder move around the sphere in a fixed distance. Thus
  // we always know the closest point by construction.
  OpenTissue::gjk::Sphere<math_types> const supportA;
  OpenTissue::gjk::Cylinder<math_types> const supportB;

  size_t    const max_iterations       = 1000u;
  real_type const absolute_tolerance   = boost::numeric_cast<real_type>(0.0);
  real_type const relative_tolerance   = boost::numeric_cast<real_type>(10e-10); // Don't be to over agressive!
  real_type const stagnation_tolerance = boost::numeric_cast<real_type>(0.0);

  real_type rad_x = value_traits::zero();
  real_type rad_z = value_traits::zero();

  for(size_t i=0;i<100u;++i)
  {
    transformation_type transformA;
    transformation_type transformB;

    quaternion_type Qx,Qz;
    vector3_type a;
    vector3_type b;
    size_t iterations     = 0u;
    size_t status         = 0u;
    real_type distance    = value_traits::infinity();

    // Place sphere at origin
    transformA.T().clear();
    transformA.Q().identity();

    // Place cylinder a fixed distance away from sphere
    transformB.T().clear();
    transformB.Q().identity();

    // Minimum distance between sphere and cylinder is fixed by design

    // The unrotated cloest points
    vector3_type tst_a = vector3_type(1.0, 0.0, 0.0);
    vector3_type tst_b = vector3_type(1.1, 0.0, 0.0);

    // Apply some rotation to the cylinder, which does not alter the closest points
    Qx.Rx( rad_x );
    Qz.Rz( rad_z );
    rad_x += value_traits::pi()/100.0;
    rad_z += value_traits::pi()/100.0;

    transformB.Q() = prod( Qz, prod( Qx , transformB.Q() ));
    transformB.T() = Qz.rotate( vector3_type( 2.1, 0.0, 0.0 ) );

    // Update closest points with rotation
    tst_a = Qz.rotate(tst_a);
    tst_b = Qz.rotate(tst_b);

    OpenTissue::gjk::compute_closest_points(
      transformA
      , supportA
      , transformB
      , supportB
      , a
      , b
      , distance
      , iterations
      , status
      , absolute_tolerance
      , relative_tolerance
      , stagnation_tolerance
      , max_iterations
      , simplex_solver_policy
      );

    // Test if closest distance was correct
    BOOST_CHECK_CLOSE( 0.1, distance, 0.01 );

    // Test if status code was as expected
      BOOST_CHECK( status != OpenTissue::gjk::ABSOLUTE_CONVERGENCE );            // Would indicate penetration
      BOOST_CHECK( status != OpenTissue::gjk::INTERSECTION );                    // Would indicate penetration
      BOOST_CHECK( status != OpenTissue::gjk::ITERATING );                       // Would indicate internal error in GJK
      BOOST_CHECK( status != OpenTissue::gjk::EXCEEDED_MAX_ITERATIONS_LIMIT );   // Would indicate internal error in GJK
      BOOST_CHECK( status != OpenTissue::gjk::NON_DESCEND_DIRECTION );           // Would indicate internal error in GJK
      //BOOST_CHECK( status != OpenTissue::gjk::RELATIVE_CONVERGENCE );          // Would indicate convergence to positive distance
      //BOOST_CHECK( status != OpenTissue::gjk::SIMPLEX_EXPANSION_FAILED );      // Would indicate convergence to positive distance
      //BOOST_CHECK( status != OpenTissue::gjk::STAGNATION );                    // Would indicate convergence to positive distance
      //BOOST_CHECK( status != OpenTissue::gjk::LOWER_ERROR_BOUND_CONVERGENCE ); // Would indicate convergence to positive distance

    // Test if closest points make sense
    BOOST_CHECK( fabs(a(0) - tst_a(0)) < 10e-6 );
    BOOST_CHECK( fabs(a(1) - tst_a(1)) < 10e-6 );
    BOOST_CHECK( fabs(a(2) - tst_a(2)) < 10e-6 );

    BOOST_CHECK( fabs(b(0) - tst_b(0)) < 10e-6 );
    BOOST_CHECK( fabs(b(1) - tst_b(1)) < 10e-6 );
    BOOST_CHECK( fabs(b(2) - tst_b(2)) < 10e-6 );
  }

}

BOOST_AUTO_TEST_SUITE_END();


// Standard
#include <iostream> // cout, endl
#include <cmath> // exp, abs
#include <vector>
// Thirdparties
#include <Eigen/Dense> // Eigen
// Lib
#include "s0s/euler.h" // s0s
// Simple
#include "func.h" // Func

constexpr unsigned int DIM = 10;
using TypeScalar = double;
using TypeVector = Eigen::Matrix<TypeScalar, DIM, 1>;
template<typename ...Args>
using TypeView = Eigen::Map<Args...>;

using TypeFunction = Func<TypeVector>;

int main () {
    // Parameters
    TypeScalar dt = 1e-4;
    TypeScalar tMax = 1e0;
    std::size_t nt = tMax / dt;
    TypeFunction f;
    s0s::SolverEuler<TypeVector, TypeView> solver;
    // Init
    TypeVector x = TypeVector::Constant(1.0);
    TypeScalar t = 0.0;
    // computation
    for(unsigned int i = 0; i < nt; i++) {
        solver(f, x.data(), x.size(), t, dt);
        t += dt;
    }
    // out
    std::cout << "\n";
    std::cout << "Solver solved exp(" << 0.0 << " -> " << tMax << ") = " << std::endl;
    std::cout << "\n";
    std::cout << x.transpose() << "\n";
    std::cout << "\n";
    std::cout << std::endl;
}


//System includes
#include <iostream>
#include <chrono>

//Eigen includes
#include <Eigen/Sparse>
#include <Eigen/Core>
#include <unsupported/Eigen/Splines>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Interpolation_traits_2.h>
#include <CGAL/natural_neighbor_coordinates_2.h>
#include <CGAL/interpolation_functions.h>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Delaunay_triangulation_2<K> Delaunay_triangulation;
typedef CGAL::Interpolation_traits_2<K> Traits;
typedef K::FT Coord_type;
typedef K::Point_2 Point;

int main(){

    typedef Eigen::Spline<double, 1, 1> spline_type;
    typedef typename spline_type::PointType point_type;
    typedef typename spline_type::ControlPointVectorType cpv_type;

    const Eigen::VectorXd xvals = (Eigen::VectorXd(9) << 0, 0, 0, 1, 1, 1, 2, 2, 2).finished();
    const Eigen::VectorXd yvals = (Eigen::VectorXd(9) << 0, 1, 2, 0, 1, 2, 0, 1, 2).finished();
    cpv_type nodes(2,9);
    nodes.row(0)=xvals;
    nodes.row(1)=yvals;

    const Eigen::VectorXd zvals = xvals.array().square()+yvals.array().square();

    const spline_type spline = Eigen::SplineFitting<spline_type>::Interpolate( zvals.transpose(), 1, xvals.transpose());

    Delaunay_triangulation T;

    std::map<Point, Coord_type, K::Less_xy_2> function_values;

    typedef CGAL::Data_access< std::map<Point, Coord_type, K::Less_xy_2 > > Value_access;

    Coord_type a(0.25), bx(1.3), by(-0.7);

    for (int y=0 ; y<3 ; y++)
	for (int x=0 ; x<3 ; x++){
	    K::Point_2 p(x,y);
	    T.insert(p);
	    function_values.insert(std::make_pair(p,a + bx* x+ by*y));
	}

    //coordinate computation
    K::Point_2 p(1.3,0.34);

    std::vector< std::pair< Point, Coord_type > > coords;

    Coord_type norm = CGAL::natural_neighbor_coordinates_2 (T, p,std::back_inserter(coords)).second;
    Coord_type res = CGAL::linear_interpolation(coords.begin(), coords.end(), norm, Value_access(function_values));

    std::cout << " Tested interpolation on " << p << " interpolation: " << res << " exact: " << a + bx* p.x()+ by* p.y()<< std::endl;
    std::cout << "done" << std::endl;

     return 0;
}


#include <state_estimation/filters/ukf.h>
#include <state_estimation/utilities/logging.h>
#include <Eigen/Dense>

namespace state_estimation {

UKF::UKF(system_models::NonlinearSystemModel* system_model)
    : FilterBase::FilterBase(system_model) {
    initializeSigmaPointParameters();
}

UKF::UKF(system_models::NonlinearSystemModel* system_model, const Eigen::VectorXd& x,
         const Eigen::MatrixXd& cov, double timestamp)
    : FilterBase::FilterBase(system_model, x, cov, timestamp) {
    initializeSigmaPointParameters();
}

void UKF::setSigmaPointParameters(double alpha, double kappa, double beta) {
    uint32_t n = system_model_->g().size();
    num_sigma_pts_ = 2 * system_model_->stateSize() + 1;

    // Compute our lambda value
    lambda_ = pow(alpha, 2) * (n + kappa) - n;

    // Compute our weight vectors
    w_mean_.resize(num_sigma_pts_);
    w_cov_.resize(num_sigma_pts_);

    const double init_w = 0.5 / (n + lambda_);
    w_mean_ = Eigen::VectorXd::Constant(num_sigma_pts_, init_w);
    w_cov_ = Eigen::VectorXd::Constant(num_sigma_pts_, init_w);

    w_mean_(0) = lambda_ / (n + lambda_);
    w_cov_(0) = w_mean_(0) + 1 - pow(alpha, 2) + beta;

#ifdef DEBUG_STATE_ESTIMATION
    std::cout << "UKF sigma point initialization" << std::endl
              << "alpha=" << alpha << std::endl
              << "kappa=" << kappa << std::endl
              << "lambda=" << lambda_ << std::endl
              << "Initialized mean weights to [" << w_mean_.transpose() << "]" << std::endl
              << "Initialized covariance weights to [" << w_cov_.transpose() << "]" << std::endl;
#endif
}

void UKF::initializeSigmaPointParameters() {
    setSigmaPointParameters(0.001, 0, 2);
}

void UKF::myPredict(const Eigen::VectorXd& u, double dt) {
    // Generate the sigma points and run them through the system model
    const Eigen::MatrixXd sigma_offset =
        ((system_model_->stateSize() + lambda_) * filter_state_.covariance).llt().matrixL();
    Eigen::MatrixXd sigma_pts(system_model_->stateSize(), num_sigma_pts_);

    system_model_->update(filter_state_.x, u, dt);
    Eigen::VectorXd x = system_model_->g();
    sigma_pts.col(0) = system_model_->g();

    for (uint32_t i = 0; i < system_model_->stateSize(); ++i) {
        const uint32_t i_high = i + 1;
        const uint32_t i_low = i + 1 + system_model_->stateSize();

        const Eigen::VectorXd x_high =
            system_model_->addVectors(filter_state_.x, sigma_offset.col(i));
        system_model_->update(x_high, u, dt);
        sigma_pts.col(i_high) = system_model_->g();

        const Eigen::VectorXd x_low =
            system_model_->subtractVectors(filter_state_.x, sigma_offset.col(i));
        system_model_->update(x_low, u, dt);
        sigma_pts.col(i_low) = system_model_->g();
    }

    // Compute the weighted mean
    filter_state_.x = system_model_->weightedSum(w_mean_, sigma_pts);

    // Compute the weighted covariance
    filter_state_.covariance =
        system_model_->P() * system_model_->Rp() * system_model_->P().transpose() +
        system_model_->V() * system_model_->Rc() * system_model_->V().transpose();
    for (uint32_t i = 0; i < num_sigma_pts_; ++i) {
        const Eigen::VectorXd dx =
            system_model_->subtractVectors(sigma_pts.col(i), filter_state_.x);
        filter_state_.covariance += w_cov_(i) * dx * dx.transpose();
    }

#ifdef DEBUG_STATE_ESTIMATION
    std::cout << "UKF predicition update:" << std::endl
              << "Sigma offsets=" << std::endl
              << printMatrix(sigma_offset) << std::endl
              << "Sigma points=" << std::endl
              << printMatrix(sigma_pts) << std::endl
              << "P=" << std::endl
              << printMatrix(system_model_->P()) << std::endl
              << "V=" << std::endl
              << printMatrix(system_model_->V()) << std::endl
              << "x=" << printMatrix(filter_state_.x) << std::endl
              << "Covariance=" << std::endl
              << printMatrix(filter_state_.covariance) << std::endl;
#endif
}

void UKF::myCorrect(const Eigen::VectorXd& z, measurement_models::NonlinearMeasurementModel* model,
                    double dt) {
    // Generate the sigma points and run them through the measurement model
    Eigen::MatrixXd sigma_offset =
        ((system_model_->stateSize() + lambda_) * filter_state_.covariance).llt().matrixL();
    Eigen::MatrixXd sigma_pts(system_model_->stateSize(), num_sigma_pts_);
    Eigen::MatrixXd observed_sigma_pts(model->measurementSize(), num_sigma_pts_);

    sigma_pts.col(0) = filter_state_.x;
    model->update(filter_state_.x, dt);
    observed_sigma_pts.col(0) = model->h();

    for (uint32_t i = 0; i < system_model_->stateSize(); ++i) {
        const uint32_t i_high = i + 1;
        const uint32_t i_low = i + 1 + system_model_->stateSize();

        sigma_pts.col(i_high) = system_model_->addVectors(filter_state_.x, sigma_offset.col(i));
        model->update(sigma_pts.col(i_high), dt);
        observed_sigma_pts.col(i_high) = model->h();

        sigma_pts.col(i_low) = system_model_->subtractVectors(filter_state_.x, sigma_offset.col(i));
        model->update(sigma_pts.col(i_low), dt);
        observed_sigma_pts.col(i_low) = model->h();
    }

    // Compute the weighted mean for the predicted measurement
    Eigen::VectorXd z_pred = model->weightedSum(w_mean_, observed_sigma_pts);

    // Compute the gain
    Eigen::MatrixXd S = model->covariance();
    for (uint32_t i = 0; i < num_sigma_pts_; ++i) {
        const Eigen::VectorXd dz = model->subtractVectors(observed_sigma_pts.col(i), z_pred);
        S += w_cov_(i) * dz * dz.transpose();
    }

    Eigen::MatrixXd cross_covariance =
        Eigen::MatrixXd::Zero(model->stateSize(), model->measurementSize());
    for (uint32_t i = 0; i < num_sigma_pts_; ++i) {
        const Eigen::VectorXd dx =
            system_model_->subtractVectors(sigma_pts.col(i), sigma_pts.col(0));
        const Eigen::VectorXd dz = model->subtractVectors(observed_sigma_pts.col(i), z_pred);

        cross_covariance += w_cov_(i) * dx * dz.transpose();
    }

    const Eigen::MatrixXd K = cross_covariance * S.inverse();

    // Perform the mean and covariance updates
    const Eigen::VectorXd dx = K * model->subtractVectors(z, z_pred);
    filter_state_.x = system_model_->addVectors(filter_state_.x, dx);
    filter_state_.covariance -= K * S * K.transpose();

#ifdef DEBUG_STATE_ESTIMATION
    std::cout << "UKF measurement update:" << std::endl
              << "Sigma offsets=" << std::endl
              << printMatrix(sigma_offset) << std::endl
              << "Sigma points=" << std::endl
              << printMatrix(sigma_pts) << std::endl
              << "Observed sigma points=" << std::endl
              << printMatrix(observed_sigma_pts) << std::endl
              << "z_pred=" << printMatrix(z_pred) << std::endl
              << "Q=" << std::endl
              << printMatrix(model->covariance()) << std::endl
              << "S=" << std::endl
              << printMatrix(S) << std::endl
              << "Cross Covariance=" << std::endl
              << printMatrix(cross_covariance) << std::endl
              << "K=" << std::endl
              << printMatrix(K) << std::endl
              << "x=" << printMatrix(filter_state_.x) << std::endl
              << "Covariance=" << std::endl
              << printMatrix(filter_state_.covariance) << std::endl;
#endif
}

}  // namespace state_estimation