//  (C) Copyright Nick Thompson 2018.
//  (C) Copyright Matt Borland 2021.
//  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_BIVARIATE_STATISTICS_HPP
#define BOOST_MATH_STATISTICS_BIVARIATE_STATISTICS_HPP

#include <iterator>
#include <tuple>
#include <type_traits>
#include <stdexcept>
#include <future>
#include <thread>
#include <vector>
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <boost/assert.hpp>

// Support compilers with P0024R2 implemented without linking TBB
// https://en.cppreference.com/w/cpp/compiler_support
#ifndef BOOST_NO_CXX17_HDR_EXECUTION
#include <execution>
#define EXEC_COMPATIBLE
#endif

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

// See Equation III.9 of "Numerically Stable, Single-Pass, Parallel Statistics Algorithms", Bennet et al.
template<typename ReturnType, typename ForwardIterator>
ReturnType means_and_covariance_seq_impl(ForwardIterator u_begin, ForwardIterator u_end, ForwardIterator v_begin, ForwardIterator v_end)
{
    using Real = typename std::tuple_element<0, ReturnType>::type;

    Real cov = 0;
    ForwardIterator u_it = u_begin;
    ForwardIterator v_it = v_begin;
    Real mu_u = *u_it++;
    Real mu_v = *v_it++;
    std::size_t i = 1;

    while(u_it != u_end && v_it != v_end)
    {
        Real u_temp = (*u_it++ - mu_u)/(i+1);
        Real v_temp = *v_it++ - mu_v;
        cov += i*u_temp*v_temp;
        mu_u = mu_u + u_temp;
        mu_v = mu_v + v_temp/(i+1);
        i = i + 1;
    }

    if(u_it != u_end || v_it != v_end)
    {
        throw std::domain_error("The size of each sample set must be the same to compute covariance");
    }

    return std::make_tuple(mu_u, mu_v, cov/i, i);
}

// Numerically stable parallel computation of (co-)variance
// https://dl.acm.org/doi/10.1145/3221269.3223036
template<typename ReturnType, typename ForwardIterator>
ReturnType means_and_covariance_parallel_impl(ForwardIterator u_begin, ForwardIterator u_end, ForwardIterator v_begin, ForwardIterator v_end)
{
    using Real = typename std::tuple_element<0, ReturnType>::type;

    const auto u_elements = std::distance(u_begin, u_end);
    const auto v_elements = std::distance(v_begin, v_end);

    if(u_elements != v_elements)
    {
        throw std::domain_error("The size of each sample set must be the same to compute covariance");
    }

    const unsigned max_concurrency = std::thread::hardware_concurrency() == 0 ? 2u : std::thread::hardware_concurrency();
    unsigned num_threads = 2u;

    // 5.16 comes from benchmarking. See boost/math/reporting/performance/bivariate_statistics_performance.cpp
    // Threading is faster for: 10 + 5.16e-3 N/j <= 5.16e-3N => N >= 10^4j/5.16(j-1).
    const auto parallel_lower_bound = 10e4*max_concurrency/(5.16*(max_concurrency-1));
    const auto parallel_upper_bound = 10e4*2/5.16; // j = 2

    // https://lemire.me/blog/2020/01/30/cost-of-a-thread-in-c-under-linux/
    if(u_elements < parallel_lower_bound)
    {
        return means_and_covariance_seq_impl<ReturnType>(u_begin, u_end, v_begin, v_end);
    }
    else if(u_elements >= parallel_upper_bound)
    {
        num_threads = max_concurrency;
    }
    else
    {
        for(unsigned i = 3; i < max_concurrency; ++i)
        {
            if(parallel_lower_bound < 10e4*i/(5.16*(i-1)))
            {
                num_threads = i;
                break;
            }
        }
    }

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

    ForwardIterator u_it = u_begin;
    ForwardIterator v_it = v_begin;

    for(std::size_t i = 0; i < num_threads - 1; ++i)
    {
        future_manager.emplace_back(std::async(std::launch::async | std::launch::deferred, [u_it, v_it, elements_per_thread]() -> ReturnType
        {
            return means_and_covariance_seq_impl<ReturnType>(u_it, std::next(u_it, elements_per_thread), v_it, std::next(v_it, elements_per_thread));
        }));
        u_it = std::next(u_it, elements_per_thread);
        v_it = std::next(v_it, elements_per_thread);
    }

    future_manager.emplace_back(std::async(std::launch::async | std::launch::deferred, [u_it, u_end, v_it, v_end]() -> ReturnType
    {
        return means_and_covariance_seq_impl<ReturnType>(u_it, u_end, v_it, v_end);
    }));

    ReturnType temp = future_manager[0].get();
    Real mu_u_a = std::get<0>(temp);
    Real mu_v_a = std::get<1>(temp);
    Real cov_a = std::get<2>(temp);
    Real n_a = std::get<3>(temp);

    for(std::size_t i = 1; i < future_manager.size(); ++i)
    {
        temp = future_manager[i].get();
        Real mu_u_b = std::get<0>(temp);
        Real mu_v_b = std::get<1>(temp);
        Real cov_b = std::get<2>(temp);
        Real n_b = std::get<3>(temp);

        const Real n_ab = n_a + n_b;
        const Real delta_u = mu_u_b - mu_u_a;
        const Real delta_v = mu_v_b - mu_v_a;

        cov_a = cov_a + cov_b + (-delta_u)*(-delta_v)*((n_a*n_b)/n_ab);
        mu_u_a = mu_u_a + delta_u*(n_b/n_ab);
        mu_v_a = mu_v_a + delta_v*(n_b/n_ab);
        n_a = n_ab;
    }

    return std::make_tuple(mu_u_a, mu_v_a, cov_a, n_a);
}

template<typename ReturnType, typename ForwardIterator>
ReturnType correlation_coefficient_seq_impl(ForwardIterator u_begin, ForwardIterator u_end, ForwardIterator v_begin, ForwardIterator v_end)
{
    using Real = typename std::tuple_element<0, ReturnType>::type;
    using std::sqrt;

    Real cov = 0;
    ForwardIterator u_it = u_begin;
    ForwardIterator v_it = v_begin;
    Real mu_u = *u_it++;
    Real mu_v = *v_it++;
    Real Qu = 0;
    Real Qv = 0;
    std::size_t i = 1;

    while(u_it != u_end && v_it != v_end)
    {
        Real u_tmp = *u_it++ - mu_u;
        Real v_tmp = *v_it++ - mu_v;
        Qu = Qu + (i*u_tmp*u_tmp)/(i+1);
        Qv = Qv + (i*v_tmp*v_tmp)/(i+1);
        cov += i*u_tmp*v_tmp/(i+1);
        mu_u = mu_u + u_tmp/(i+1);
        mu_v = mu_v + v_tmp/(i+1);
        ++i;
    }

    // If both datasets are constant, then they are perfectly correlated.
    if (Qu == 0 && Qv == 0)
    {
        return std::make_tuple(mu_u, Qu, mu_v, Qv, cov, Real(1), i);
    }
    // If one dataset is constant and the other isn't, then they have no correlation:
    if (Qu == 0 || Qv == 0)
    {
        return std::make_tuple(mu_u, Qu, mu_v, Qv, cov, Real(0), i);
    }

    // Make sure rho in [-1, 1], even in the presence of numerical noise.
    Real rho = cov/sqrt(Qu*Qv);
    if (rho > 1) {
        rho = 1;
    }
    if (rho < -1) {
        rho = -1;
    }

    return std::make_tuple(mu_u, Qu, mu_v, Qv, cov, rho, i);
}

// Numerically stable parallel computation of (co-)variance:
// https://dl.acm.org/doi/10.1145/3221269.3223036
//
// Parallel computation of variance:
// http://i.stanford.edu/pub/cstr/reports/cs/tr/79/773/CS-TR-79-773.pdf
template<typename ReturnType, typename ForwardIterator>
ReturnType correlation_coefficient_parallel_impl(ForwardIterator u_begin, ForwardIterator u_end, ForwardIterator v_begin, ForwardIterator v_end)
{
    using Real = typename std::tuple_element<0, ReturnType>::type;

    const auto u_elements = std::distance(u_begin, u_end);
    const auto v_elements = std::distance(v_begin, v_end);

    if(u_elements != v_elements)
    {
        throw std::domain_error("The size of each sample set must be the same to compute covariance");
    }

    const unsigned max_concurrency = std::thread::hardware_concurrency() == 0 ? 2u : std::thread::hardware_concurrency();
    unsigned num_threads = 2u;

    // 3.25 comes from benchmarking. See boost/math/reporting/performance/bivariate_statistics_performance.cpp
    // Threading is faster for: 10 + 3.25e-3 N/j <= 3.25e-3N => N >= 10^4j/3.25(j-1).
    const auto parallel_lower_bound = 10e4*max_concurrency/(3.25*(max_concurrency-1));
    const auto parallel_upper_bound = 10e4*2/3.25; // j = 2

    // https://lemire.me/blog/2020/01/30/cost-of-a-thread-in-c-under-linux/
    if(u_elements < parallel_lower_bound)
    {
        return correlation_coefficient_seq_impl<ReturnType>(u_begin, u_end, v_begin, v_end);
    }
    else if(u_elements >= parallel_upper_bound)
    {
        num_threads = max_concurrency;
    }
    else
    {
        for(unsigned i = 3; i < max_concurrency; ++i)
        {
            if(parallel_lower_bound < 10e4*i/(3.25*(i-1)))
            {
                num_threads = i;
                break;
            }
        }
    }

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

    ForwardIterator u_it = u_begin;
    ForwardIterator v_it = v_begin;

    for(std::size_t i = 0; i < num_threads - 1; ++i)
    {
        future_manager.emplace_back(std::async(std::launch::async | std::launch::deferred, [u_it, v_it, elements_per_thread]() -> ReturnType
        {
            return correlation_coefficient_seq_impl<ReturnType>(u_it, std::next(u_it, elements_per_thread), v_it, std::next(v_it, elements_per_thread));
        }));
        u_it = std::next(u_it, elements_per_thread);
        v_it = std::next(v_it, elements_per_thread);
    }

    future_manager.emplace_back(std::async(std::launch::async | std::launch::deferred, [u_it, u_end, v_it, v_end]() -> ReturnType
    {
        return correlation_coefficient_seq_impl<ReturnType>(u_it, u_end, v_it, v_end);
    }));

    ReturnType temp = future_manager[0].get();
    Real mu_u_a = std::get<0>(temp);
    Real Qu_a = std::get<1>(temp);
    Real mu_v_a = std::get<2>(temp);
    Real Qv_a = std::get<3>(temp);
    Real cov_a = std::get<4>(temp);
    Real n_a = std::get<6>(temp);

    for(std::size_t i = 1; i < future_manager.size(); ++i)
    {
        temp = future_manager[i].get();
        Real mu_u_b = std::get<0>(temp);
        Real Qu_b = std::get<1>(temp);
        Real mu_v_b = std::get<2>(temp);
        Real Qv_b = std::get<3>(temp);
        Real cov_b = std::get<4>(temp);
        Real n_b = std::get<6>(temp);

        const Real n_ab = n_a + n_b;
        const Real delta_u = mu_u_b - mu_u_a;
        const Real delta_v = mu_v_b - mu_v_a;

        cov_a = cov_a + cov_b + (-delta_u)*(-delta_v)*((n_a*n_b)/n_ab);
        mu_u_a = mu_u_a + delta_u*(n_b/n_ab);
        mu_v_a = mu_v_a + delta_v*(n_b/n_ab);
        Qu_a = Qu_a + Qu_b + delta_u*delta_u*((n_a*n_b)/n_ab);
        Qv_b = Qv_a + Qv_b + delta_v*delta_v*((n_a*n_b)/n_ab);
        n_a = n_ab;
    }

    // If both datasets are constant, then they are perfectly correlated.
    if (Qu_a == 0 && Qv_a == 0)
    {
        return std::make_tuple(mu_u_a, Qu_a, mu_v_a, Qv_a, cov_a, Real(1), n_a);
    }
    // If one dataset is constant and the other isn't, then they have no correlation:
    if (Qu_a == 0 || Qv_a == 0)
    {
        return std::make_tuple(mu_u_a, Qu_a, mu_v_a, Qv_a, cov_a, Real(0), n_a);
    }

    // Make sure rho in [-1, 1], even in the presence of numerical noise.
    Real rho = cov_a/sqrt(Qu_a*Qv_a);
    if (rho > 1) {
        rho = 1;
    }
    if (rho < -1) {
        rho = -1;
    }

    return std::make_tuple(mu_u_a, Qu_a, mu_v_a, Qv_a, cov_a, rho, n_a);
}

} // namespace detail

#ifdef EXEC_COMPATIBLE

template<typename ExecutionPolicy, typename Container, typename Real = typename Container::value_type>
inline auto means_and_covariance(ExecutionPolicy&& exec, Container const & u, Container const & v)
{
    if constexpr (std::is_same_v<std::remove_reference_t<decltype(exec)>, decltype(std::execution::seq)>)
    {
        if constexpr (std::is_integral_v<Real>)
        {
            using ReturnType = std::tuple<double, double, double, double>;
            ReturnType temp = detail::means_and_covariance_seq_impl<ReturnType>(std::begin(u), std::end(u), std::begin(v), std::end(v));
            return std::make_tuple(std::get<0>(temp), std::get<1>(temp), std::get<2>(temp));
        }
        else
        {
            using ReturnType = std::tuple<Real, Real, Real, Real>;
            ReturnType temp = detail::means_and_covariance_seq_impl<ReturnType>(std::begin(u), std::end(u), std::begin(v), std::end(v));
            return std::make_tuple(std::get<0>(temp), std::get<1>(temp), std::get<2>(temp));
        }
    }
    else
    {
        if constexpr (std::is_integral_v<Real>)
        {
            using ReturnType = std::tuple<double, double, double, double>;
            ReturnType temp = detail::means_and_covariance_parallel_impl<ReturnType>(std::begin(u), std::end(u), std::begin(v), std::end(v));
            return std::make_tuple(std::get<0>(temp), std::get<1>(temp), std::get<2>(temp));
        }
        else
        {
            using ReturnType = std::tuple<Real, Real, Real, Real>;
            ReturnType temp = detail::means_and_covariance_parallel_impl<ReturnType>(std::begin(u), std::end(u), std::begin(v), std::end(v));
            return std::make_tuple(std::get<0>(temp), std::get<1>(temp), std::get<2>(temp));
        }
    }
}

template<typename Container>
inline auto means_and_covariance(Container const & u, Container const & v)
{
    return means_and_covariance(std::execution::seq, u, v);
}

template<typename ExecutionPolicy, typename Container>
inline auto covariance(ExecutionPolicy&& exec, Container const & u, Container const & v)
{
    return std::get<2>(means_and_covariance(exec, u, v));
}

template<typename Container>
inline auto covariance(Container const & u, Container const & v)
{
    return covariance(std::execution::seq, u, v);
}

template<typename ExecutionPolicy, typename Container, typename Real = typename Container::value_type>
inline auto correlation_coefficient(ExecutionPolicy&& exec, Container const & u, Container const & v)
{
    if constexpr (std::is_same_v<std::remove_reference_t<decltype(exec)>, decltype(std::execution::seq)>)
    {
        if constexpr (std::is_integral_v<Real>)
        {
            using ReturnType = std::tuple<double, double, double, double, double, double, double>;
            return std::get<5>(detail::correlation_coefficient_seq_impl<ReturnType>(std::begin(u), std::end(u), std::begin(v), std::end(v)));
        }
        else
        {
            using ReturnType = std::tuple<Real, Real, Real, Real, Real, Real, Real>;
            return std::get<5>(detail::correlation_coefficient_seq_impl<ReturnType>(std::begin(u), std::end(u), std::begin(v), std::end(v)));
        }
    }
    else
    {
        if constexpr (std::is_integral_v<Real>)
        {
            using ReturnType = std::tuple<double, double, double, double, double, double, double>;
            return std::get<5>(detail::correlation_coefficient_parallel_impl<ReturnType>(std::begin(u), std::end(u), std::begin(v), std::end(v)));
        }
        else
        {
            using ReturnType = std::tuple<Real, Real, Real, Real, Real, Real, Real>;
            return std::get<5>(detail::correlation_coefficient_parallel_impl<ReturnType>(std::begin(u), std::end(u), std::begin(v), std::end(v)));
        }
    }
}

template<typename Container, typename Real = typename Container::value_type>
inline auto correlation_coefficient(Container const & u, Container const & v)
{
    return correlation_coefficient(std::execution::seq, u, v);
}

#else // C++11 bindings

template<typename Container, typename Real = typename Container::value_type, typename std::enable_if<std::is_integral<Real>::value, bool>::type = true>
inline auto means_and_covariance(Container const & u, Container const & v) -> std::tuple<double, double, double>
{
    using ReturnType = std::tuple<double, double, double, double>;
    ReturnType temp = detail::means_and_covariance_seq_impl<ReturnType>(std::begin(u), std::end(u), std::begin(v), std::end(v));
    return std::make_tuple(std::get<0>(temp), std::get<1>(temp), std::get<2>(temp));
}

template<typename Container, typename Real = typename Container::value_type, typename std::enable_if<!std::is_integral<Real>::value, bool>::type = true>
inline auto means_and_covariance(Container const & u, Container const & v) -> std::tuple<Real, Real, Real>
{
    using ReturnType = std::tuple<Real, Real, Real, Real>;
    ReturnType temp = detail::means_and_covariance_seq_impl<ReturnType>(std::begin(u), std::end(u), std::begin(v), std::end(v));
    return std::make_tuple(std::get<0>(temp), std::get<1>(temp), std::get<2>(temp));
}

template<typename Container, typename Real = typename Container::value_type, typename std::enable_if<std::is_integral<Real>::value, bool>::type = true>
inline double covariance(Container const & u, Container const & v)
{
    using ReturnType = std::tuple<double, double, double, double>;
    return std::get<2>(detail::means_and_covariance_seq_impl<ReturnType>(std::begin(u), std::end(u), std::begin(v), std::end(v)));
}

template<typename Container, typename Real = typename Container::value_type, typename std::enable_if<!std::is_integral<Real>::value, bool>::type = true>
inline Real covariance(Container const & u, Container const & v)
{
    using ReturnType = std::tuple<Real, Real, Real, Real>;
    return std::get<2>(detail::means_and_covariance_seq_impl<ReturnType>(std::begin(u), std::end(u), std::begin(v), std::end(v)));
}

template<typename Container, typename Real = typename Container::value_type, typename std::enable_if<std::is_integral<Real>::value, bool>::type = true>
inline double correlation_coefficient(Container const & u, Container const & v)
{
    using ReturnType = std::tuple<double, double, double, double, double, double, double>;
    return std::get<5>(detail::correlation_coefficient_seq_impl<ReturnType>(std::begin(u), std::end(u), std::begin(v), std::end(v)));
}

template<typename Container, typename Real = typename Container::value_type, typename std::enable_if<!std::is_integral<Real>::value, bool>::type = true>
inline Real correlation_coefficient(Container const & u, Container const & v)
{
    using ReturnType = std::tuple<Real, Real, Real, Real, Real, Real, Real>;
    return std::get<5>(detail::correlation_coefficient_seq_impl<ReturnType>(std::begin(u), std::end(u), std::begin(v), std::end(v)));
}

#endif

}}} // namespace boost::math::statistics

#endif


#include <CGAL/Simple_cartesian.h>
#include <CGAL/Surface_mesh.h>
#include <CGAL/Surface_mesh_simplification/Policies/Edge_collapse/Edge_profile.h>
#include <boost/tuple/tuple.hpp>
#include <boost/tuple/tuple_comparison.hpp>
#include <fstream>

typedef CGAL::Simple_cartesian<double> K;
typedef CGAL::Surface_mesh<K::Point_3> Mesh;
typedef CGAL::Surface_mesh_simplification::Edge_profile<Mesh> Profile;

void naive_all_triangles(Mesh::Halfedge_index h, Mesh& m, std::set<Mesh::Face_index>& triangles)
{
  for(Mesh::Halfedge_index hh : CGAL::halfedges_around_source(h, m))
  {
    if(!is_border(hh, m))
      triangles.insert(face(hh,m));
  }
  for(Mesh::Halfedge_index hh : CGAL::halfedges_around_target(h, m))
  {
    if(!is_border(hh, m))
      triangles.insert(face(hh,m));
  }
}

void naive_link_vertices(Mesh::Halfedge_index h, Mesh& m,
                         const std::set<Mesh::Face_index>& triangles,
                         std::set<Mesh::Vertex_index>& link_vertices)
{
  for(Mesh::Face_index f : triangles)
  {
    for(Mesh::Halfedge_index h : CGAL::halfedges_around_face(halfedge(f, m), m))
    {
      link_vertices.insert(target(h, m));
    }
  }
  link_vertices.erase(source(h, m));
  link_vertices.erase(target(h, m));
}

struct A{};

boost::tuple<Mesh::Vertex_index, Mesh::Vertex_index, Mesh::Vertex_index>
make_canonical_tuple(Mesh::Vertex_index v1, Mesh::Vertex_index v2, Mesh::Vertex_index v3)
{
  Mesh::Vertex_index vs[3]={v1, v2, v3};
  std::sort(&vs[0], &vs[0]+3);
  return boost::make_tuple(vs[0], vs[1], vs[2]);
}

boost::tuple<Mesh::Vertex_index, Mesh::Vertex_index, Mesh::Vertex_index>
make_canonical_tuple(const Profile::Triangle& t)
{
  return make_canonical_tuple(t.v0, t.v1, t.v2);
}

boost::tuple<Mesh::Vertex_index, Mesh::Vertex_index, Mesh::Vertex_index>
make_canonical_tuple(Mesh::Face_index f, Mesh& m)
{
  Mesh::Halfedge_index h=halfedge(f, m);
  return make_canonical_tuple(source(h,m), target(h,m), target(next(h, m), m));
}

void test(const char* fname)
{
  Mesh m;
  std::ifstream input(fname);
  assert(!input.fail());
  input >> m;
  assert(num_vertices(m)!=0);
  A a;

  for(Mesh::Halfedge_index h : halfedges(m))
  {
    std::set<Mesh::Face_index> triangles;
    naive_all_triangles(h, m, triangles);

    Profile profile(h, m, K(), a, get(boost::vertex_point, m), a, true);

    if(CGAL::Euler::does_satisfy_link_condition(edge(h, m), m))
    {
      std::set<Mesh::Vertex_index> link_vertices;
      naive_link_vertices(h, m, triangles, link_vertices);
      assert(link_vertices.size()==profile.link().size());
      assert(std::set<Mesh::Vertex_index>(profile.link().begin(),
                                           profile.link().end()).size()
              == link_vertices.size());

      for(const Mesh::Vertex_index& v : profile.link())
      {
        assert(link_vertices.count(v) == 1);
      }
    }

    assert(triangles.size() == profile.triangles().size());
    std::set<boost::tuple<Mesh::Vertex_index, Mesh::Vertex_index, Mesh::Vertex_index> > triple_set;
    for(const Profile::Triangle& t : profile.triangles())
    {
      triple_set.insert(make_canonical_tuple(t));
    }
    for(Mesh::Face_index f : triangles)
    {
      assert(triple_set.count(make_canonical_tuple(f, m)) == 1);
    }
  }
}

int main(int argc, char** argv)
{
  for(int i=1; i<argc; ++i)
  {
    std::cout << "Testing " << argv[i] << "\n";
    test(argv[i]);
  }
  if(argc==1)
  {
    std::cout << "No file provided, nothing tested\n";
  }

  return 0;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/bitwise/include/functions/ctz.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/tests/exceptions.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/signmask.hpp>
#include <boost/simd/include/constants/nbmantissabits.hpp>
#include <boost/simd/include/constants/nan.hpp>
#include <boost/simd/include/constants/inf.hpp>
#include <boost/simd/include/constants/minf.hpp>
#include <boost/simd/sdk/config.hpp>

NT2_TEST_CASE_TPL ( ctz_real,  BOOST_SIMD_REAL_TYPES)
{
  using boost::simd::ctz;
  using boost::simd::tag::ctz_;
  typedef typename boost::dispatch::meta::call<ctz_(T)>::type r_t;
  typedef typename boost::dispatch::meta::as_integer<T>::type wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_EQUAL(ctz(boost::simd::Inf<T>()), r_t(boost::simd::Nbmantissabits<T>()));
  NT2_TEST_EQUAL(ctz(boost::simd::Minf<T>()), r_t(boost::simd::Nbmantissabits<T>()));
  NT2_TEST_EQUAL(ctz(boost::simd::Nan<T>()), r_t(boost::simd::Zero<r_t>()));
#endif
  NT2_TEST_ASSERT(ctz(boost::simd::Zero<T>()));
  NT2_TEST_EQUAL(ctz(boost::simd::Signmask<T>()), r_t(sizeof(T)*8-1));
} // end of test for real_

NT2_TEST_CASE_TPL ( ctz_signed_int,  BOOST_SIMD_INTEGRAL_SIGNED_TYPES)
{
  using boost::simd::ctz;
  using boost::simd::tag::ctz_;
  using boost::simd::Nbmantissabits;
  using boost::simd::Signmask;

  typedef typename boost::dispatch::meta::call<ctz_(T)>::type r_t;

  for(std::size_t j=0; j< (sizeof(T)*CHAR_BIT-1); j++)
  {
    // Test 01111 ... 10000b
    T value = ~T(0) & ~((T(1)<<j)-1);
    NT2_TEST_EQUAL(ctz( value ), r_t(j));
    NT2_TEST_EQUAL(ctz( T(-value) ), r_t(j));
  }

  NT2_TEST_EQUAL(ctz(Signmask<T>()) , r_t(sizeof(T)*CHAR_BIT-1) );
}

NT2_TEST_CASE_TPL( ctz_unsigned_integer, BOOST_SIMD_UNSIGNED_TYPES )
{
  using boost::simd::ctz;
  using boost::simd::tag::ctz_;

  typedef typename boost::dispatch::meta::call<ctz_(T)>::type r_t;
  typedef typename boost::dispatch::meta::as_integer<T>::type wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
  for(std::size_t j=0; j< sizeof(T)*CHAR_BIT; j++)
  {
    // Test 1111 ... 10000b
    T value = ~T(0) & ~((T(1)<<j)-1);
    NT2_TEST_EQUAL(ctz( value ), r_t(j));
  }
 }


/**
 * @file tree_test.cpp
 *
 * Tests for tree-building methods.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>
#include <mlpack/core/tree/bounds.hpp>
#include <mlpack/core/metrics/lmetric.hpp>
#include <mlpack/methods/neighbor_search/neighbor_search.hpp>
#include <mlpack/core/tree/binary_space_tree.hpp>

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

using namespace mlpack;
using namespace mlpack::math;
using namespace mlpack::tree;
using namespace mlpack::neighbor;
using namespace mlpack::metric;
using namespace mlpack::bound;

BOOST_AUTO_TEST_SUITE(VantagePointTreeTest);

BOOST_AUTO_TEST_CASE(VPTreeTraitsTest)
{
  typedef VPTree<EuclideanDistance, EmptyStatistic, arma::mat> TreeType;

  bool b = TreeTraits<TreeType>::HasOverlappingChildren;
  BOOST_REQUIRE_EQUAL(b, true);
  b = TreeTraits<TreeType>::FirstPointIsCentroid;
  BOOST_REQUIRE_EQUAL(b, false);
  b = TreeTraits<TreeType>::HasSelfChildren;
  BOOST_REQUIRE_EQUAL(b, false);
  b = TreeTraits<TreeType>::RearrangesDataset;
  BOOST_REQUIRE_EQUAL(b, true);
  b = TreeTraits<TreeType>::BinaryTree;
  BOOST_REQUIRE_EQUAL(b, true);
}

BOOST_AUTO_TEST_CASE(HollowBallBoundTest)
{
  HollowBallBound<EuclideanDistance> b(2, 4, arma::vec("1.0 2.0 3.0 4.0 5.0"));

  BOOST_REQUIRE_EQUAL(b.Contains(arma::vec("1.0 2.0 3.0 7.0 5.0")), true);

  BOOST_REQUIRE_EQUAL(b.Contains(arma::vec("1.0 2.0 3.0 9.0 5.0")), false);

  BOOST_REQUIRE_EQUAL(b.Contains(arma::vec("1.0 2.0 3.0 5.0 5.0")), false);

  HollowBallBound<EuclideanDistance> b2(0.5, 1,
      arma::vec("1.0 2.0 3.0 7.0 5.0"));
  BOOST_REQUIRE_EQUAL(b.Contains(b2), true);

  b2 = HollowBallBound<EuclideanDistance>(2.5, 3.5,
      arma::vec("1.0 2.0 3.0 4.5 5.0"));
  BOOST_REQUIRE_EQUAL(b.Contains(b2), true);

  b2 = HollowBallBound<EuclideanDistance>(2.0, 3.5,
      arma::vec("1.0 2.0 3.0 4.5 5.0"));
  BOOST_REQUIRE_EQUAL(b.Contains(b2), false);

  BOOST_REQUIRE_CLOSE(b.MinDistance(arma::vec("1.0 2.0 8.0 4.0 5.0")), 1.0,
      1e-5);
  BOOST_REQUIRE_CLOSE(b.MinDistance(arma::vec("1.0 2.0 4.0 4.0 5.0")), 1.0,
      1e-5);
  BOOST_REQUIRE_CLOSE(b.MinDistance(arma::vec("1.0 2.0 3.0 4.0 5.0")), 2.0,
      1e-5);
  BOOST_REQUIRE_CLOSE(b.MinDistance(arma::vec("1.0 2.0 5.0 4.0 5.0")), 0.0,
      1e-5);
  BOOST_REQUIRE_CLOSE(b.MinDistance(arma::vec("5.0 2.0 3.0 4.0 5.0")), 0.0,
      1e-5);
  BOOST_REQUIRE_CLOSE(b.MinDistance(arma::vec("3.0 2.0 3.0 4.0 5.0")), 0.0,
      1e-5);

  BOOST_REQUIRE_CLOSE(b.MaxDistance(arma::vec("1.0 2.0 4.0 4.0 5.0")), 5.0,
      1e-5);
  BOOST_REQUIRE_CLOSE(b.MaxDistance(arma::vec("1.0 2.0 8.0 4.0 5.0")), 9.0,
      1e-5);
  BOOST_REQUIRE_CLOSE(b.MaxDistance(arma::vec("1.0 2.0 3.0 4.0 5.0")), 4.0,
      1e-5);

  b2 = HollowBallBound<EuclideanDistance>(3, 4,
      arma::vec("1.0 2.0 3.0 5.0 5.0"));
  BOOST_REQUIRE_CLOSE(b.MinDistance(b2), 0.0, 1e-5);

  b2 = HollowBallBound<EuclideanDistance>(1, 2,
      arma::vec("1.0 2.0 3.0 4.0 5.0"));
  BOOST_REQUIRE_CLOSE(b.MinDistance(b2), 0.0, 1e-5);

  b2 = HollowBallBound<EuclideanDistance>(0.5, 1.0,
      arma::vec("1.0 2.5 3.0 4.0 5.0"));
  BOOST_REQUIRE_CLOSE(b.MinDistance(b2), 0.5, 1e-5);

  b2 = HollowBallBound<EuclideanDistance>(0.5, 1.0,
      arma::vec("1.0 8.0 3.0 4.0 5.0"));
  BOOST_REQUIRE_CLOSE(b.MinDistance(b2), 1.0, 1e-5);

  b2 = HollowBallBound<EuclideanDistance>(0.5, 2.0,
      arma::vec("1.0 8.0 3.0 4.0 5.0"));
  BOOST_REQUIRE_CLOSE(b.MinDistance(b2), 0.0, 1e-5);

  b2 = HollowBallBound<EuclideanDistance>(0.5, 2.0,
      arma::vec("1.0 8.0 3.0 4.0 5.0"));
  BOOST_REQUIRE_CLOSE(b.MaxDistance(b2), 12.0, 1e-5);

  b2 = HollowBallBound<EuclideanDistance>(0.5, 2.0,
      arma::vec("1.0 3.0 3.0 4.0 5.0"));
  BOOST_REQUIRE_CLOSE(b.MaxDistance(b2), 7.0, 1e-5);

  HollowBallBound<EuclideanDistance> b1 = b;
  b2 = HollowBallBound<EuclideanDistance>(1.0, 2.0,
      arma::vec("1.0 2.5 3.0 4.0 5.0"));

  b1 |= b2;
  BOOST_REQUIRE_CLOSE(b1.InnerRadius(), 0.5, 1e-5);

  b1 = b;
  b2 = HollowBallBound<EuclideanDistance>(0.5, 2.0,
      arma::vec("1.0 3.0 3.0 4.0 5.0"));
  b1 |= b2;
  BOOST_REQUIRE_CLOSE(b1.InnerRadius(), 0.0, 1e-5);

  b1 = b;
  b2 = HollowBallBound<EuclideanDistance>(0.5, 4.0,
      arma::vec("1.0 3.0 3.0 4.0 5.0"));
  b1 |= b2;
  BOOST_REQUIRE_CLOSE(b1.OuterRadius(), 5.0, 1e-5);
}

template<typename TreeType>
void CheckBound(TreeType& tree)
{
  typedef typename TreeType::ElemType ElemType;
  if (tree.IsLeaf())
  {
    // Ensure that the bound contains all descendant points.
    for (size_t i = 0; i < tree.NumPoints(); i++)
    {
      ElemType dist = tree.Bound().Metric().Evaluate(tree.Bound().Center(),
          tree.Dataset().col(tree.Point(i)));
      ElemType hollowDist = tree.Bound().Metric().Evaluate(
          tree.Bound().HollowCenter(),
          tree.Dataset().col(tree.Point(i)));

      BOOST_REQUIRE_LE(tree.Bound().InnerRadius(), hollowDist  *
          (1.0 + 10.0 * std::numeric_limits<ElemType>::epsilon()));

      BOOST_REQUIRE_LE(dist, tree.Bound().OuterRadius() *
          (1.0 + 10.0 * std::numeric_limits<ElemType>::epsilon()));
    }
  }
  else
  {
    // Ensure that the bound contains all descendant points.
    for (size_t i = 0; i < tree.NumDescendants(); i++)
    {
      ElemType dist = tree.Bound().Metric().Evaluate(tree.Bound().Center(),
          tree.Dataset().col(tree.Descendant(i)));
      ElemType hollowDist = tree.Bound().Metric().Evaluate(
          tree.Bound().HollowCenter(),
          tree.Dataset().col(tree.Descendant(i)));

      BOOST_REQUIRE_LE(tree.Bound().InnerRadius(), hollowDist  *
          (1.0 + 10.0 * std::numeric_limits<ElemType>::epsilon()));

      BOOST_REQUIRE_LE(dist, tree.Bound().OuterRadius() *
          (1.0 + 10.0 * std::numeric_limits<ElemType>::epsilon()));
    }

    CheckBound(*tree.Left());
    CheckBound(*tree.Right());
  }
}

BOOST_AUTO_TEST_CASE(VPTreeBoundTest)
{
  typedef VPTree<EuclideanDistance, EmptyStatistic, arma::mat> TreeType;

  arma::mat dataset(8, 1000);
  dataset.randu();

  TreeType tree(dataset);
  CheckBound(tree);
}

BOOST_AUTO_TEST_CASE(VPTreeTest)
{
  typedef VPTree<EuclideanDistance, EmptyStatistic, arma::mat> TreeType;

  size_t maxRuns = 10; // Ten total tests.
  size_t pointIncrements = 1000; // Range is from 2000 points to 11000.

  // We use the default leaf size of 20.
  for (size_t run = 0; run < maxRuns; run++)
  {
    size_t dimensions = run + 2;
    size_t maxPoints = (run + 1) * pointIncrements;

    size_t size = maxPoints;
    arma::mat dataset = arma::mat(dimensions, size);
    arma::mat datacopy; // Used to test mappings.

    // Mappings for post-sort verification of data.
    std::vector<size_t> newToOld;
    std::vector<size_t> oldToNew;

    // Generate data.
    dataset.randu();

    // Build the tree itself.
    TreeType root(dataset, newToOld, oldToNew);
    const arma::mat& treeset = root.Dataset();

    // Ensure the size of the tree is correct.
    BOOST_REQUIRE_EQUAL(root.NumDescendants(), size);

    // Check the forward and backward mappings for correctness.
    for(size_t i = 0; i < size; i++)
    {
      for(size_t j = 0; j < dimensions; j++)
      {
        BOOST_REQUIRE_EQUAL(treeset(j, i), dataset(j, newToOld[i]));
        BOOST_REQUIRE_EQUAL(treeset(j, oldToNew[i]), dataset(j, i));
      }
    }
  }
}

BOOST_AUTO_TEST_CASE(SingleTreeTraverserTest)
{
  arma::mat dataset;
  dataset.randu(8, 1000); // 1000 points in 8 dimensions.
  arma::Mat<size_t> neighbors1;
  arma::mat distances1;
  arma::Mat<size_t> neighbors2;
  arma::mat distances2;

  // Nearest neighbor search with the VP tree.
  NeighborSearch<NearestNeighborSort, metric::LMetric<2, true>, arma::mat,
      VPTree> knn1(dataset, SINGLE_TREE_MODE);

  knn1.Search(5, neighbors1, distances1);

  // Nearest neighbor search the naive way.
  KNN knn2(dataset, NAIVE_MODE);

  knn2.Search(5, neighbors2, distances2);

  for (size_t i = 0; i < neighbors1.size(); i++)
  {
    BOOST_REQUIRE_EQUAL(neighbors1[i], neighbors2[i]);
    BOOST_REQUIRE_EQUAL(distances1[i], distances2[i]);
  }
}

BOOST_AUTO_TEST_CASE(DualTreeTraverserTest)
{
  arma::mat dataset;
  dataset.randu(8, 1000); // 1000 points in 8 dimensions.
  arma::Mat<size_t> neighbors1;
  arma::mat distances1;
  arma::Mat<size_t> neighbors2;
  arma::mat distances2;

  // Nearest neighbor search with the VP tree.
  NeighborSearch<NearestNeighborSort, metric::LMetric<2, true>, arma::mat,
      VPTree> knn1(dataset, DUAL_TREE_MODE);

  knn1.Search(5, neighbors1, distances1);

  // Nearest neighbor search the naive way.
  KNN knn2(dataset, NAIVE_MODE);

  knn2.Search(5, neighbors2, distances2);

  for (size_t i = 0; i < neighbors1.size(); i++)
  {
    BOOST_REQUIRE_EQUAL(neighbors1[i], neighbors2[i]);
    BOOST_REQUIRE_EQUAL(distances1[i], distances2[i]);
  }
}

BOOST_AUTO_TEST_SUITE_END();


#include <boost/math/distributions/students_t.hpp>


// Copyright (C) 2013 Massachusetts Institute of Technology, Lincoln Laboratory
// License: Boost Software License   See LICENSE.txt for the full license.
// Authors: Davis E. King (davis@dlib.net)

#include "cca_morph.h"
#include <map>
#include <dlib/matrix.h>
#include <dlib/statistics.h>
#include <mitie/approximate_substring_set.h>
#include <mitie/total_word_feature_extractor.h>

using namespace std;
using namespace dlib;
using namespace mitie;

typedef std::vector<std::pair<dlib::uint32, float> > sparse_vector_type;

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

sparse_vector_type dense_to_sparse (
    const matrix<float,0,1>& vect
)
{
    sparse_vector_type res(vect.size());
    for (long i = 0; i < vect.size(); ++i)
        res[i] = std::make_pair(i, vect(i));
    return res;
}

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

void learn_morphological_dimension_reduction (
    const approximate_substring_set& substrs,
    const std::map<std::string, matrix<float,0,1> >& word_vectors,
    const long num_correlations,
    matrix<float>& morph_trans
)
{
    std::map<std::string, matrix<float,0,1> >::const_iterator i;

    std::vector<sparse_vector_type> L, R;

    sparse_vector_type temp;
    std::vector<dlib::uint16> hits;

    cout << "building morphological vectors" << endl;
    for (i = word_vectors.begin(); i != word_vectors.end(); ++i)
    {
        L.push_back(dense_to_sparse(i->second));
        substrs.find_substrings(i->first, hits);
        temp.clear();
        for (unsigned long i = 0; i < hits.size(); ++i)
            temp.push_back(make_pair(hits[i],1));
        make_sparse_vector_inplace(temp);
        R.push_back(temp);
    }
    cout << "L.size(): " << L.size() << endl;
    cout << "R.size(): " << R.size() << endl;

    cout << "Now running CCA on word <-> morphology..." << endl;
    matrix<float> Ltrans;
    cout << "correlations: " << trans(cca(L, R, Ltrans, morph_trans, num_correlations, 1000, 2)) << endl;
    //print_true_correlations(L, R, Ltrans, morph_trans);
}

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

void cca_morph(const dlib::command_line_parser& parser)
{
    const long num_morph_correlations = 90;

    std::ifstream fin("word_vects.dat", ios::binary);
    std::map<std::string, matrix<float,0,1> > word_vectors;
    deserialize(word_vectors, fin);
    cout << "num word vectors loaded: " << word_vectors.size() << endl;
    cout << "got word vectors, now learn how they correlate with morphological features." << endl;

    fin.close();
    fin.open("substring_set.dat", ios::binary);
    approximate_substring_set substring_set;
    deserialize(substring_set, fin);

    matrix<float> morph_trans;
    learn_morphological_dimension_reduction(substring_set, word_vectors, num_morph_correlations, morph_trans);

    // morph_trans should have a row for every possible output from substring_set.  But
    // since we work with sparse vectors and some outputs might not have been observed in
    // learn_morphological_dimension_reduction() we need to make sure this is the case.
    if (morph_trans.nr() != substring_set.max_substring_id()+1)
    {
        matrix<float> temp(substring_set.max_substring_id()+1, morph_trans.nc());
        set_subm(temp, get_rect(morph_trans)) = morph_trans;
        temp.swap(morph_trans);
    }

    word_morphology_feature_extractor fe(substring_set, morph_trans);
    cout << "morphological feature dimensionality: "<< fe.get_num_dimensions() << endl;

    ofstream fout("word_morph_feature_extractor.dat", ios::binary);
    serialize(fe, fout);

    total_word_feature_extractor tfe(word_vectors, fe);
    cout << "total word feature dimensionality: "<< tfe.get_num_dimensions() << endl;
    serialize("total_word_feature_extractor.dat") << "mitie::total_word_feature_extractor" << tfe;
}

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


#pragma once
#include <vector>
#include <boost/serialization/access.hpp>

namespace snn::internal
{
    struct binaryClassification
    {
        float truePositive{};
        float trueNegative{};
        float falsePositive{};
        float falseNegative{};
        float totalError{};

        bool operator==(const binaryClassification&) const
        {
            return true;
        };

        template <typename Archive>
        void serialize(Archive& ar, unsigned)
        {
            ar & truePositive;
            ar & trueNegative;
            ar & falsePositive;
            ar & falseNegative;
        }
    };

    class StatisticAnalysis
    {
    private:
        friend class boost::serialization::access;
        template <class Archive>
        void serialize(Archive& ar, unsigned version);

        std::vector<binaryClassification> clusters;
        float numberOfDataWellClassified;
        float numberOfDataMisclassified;

        float globalClusteringRate = -1.0f;
        float weightedClusteringRate = -1.0f;
        float f1Score = -1.0f;
        float meanAbsoluteError = -1.0f;
        float rootMeanSquaredError = -1.0f;

        float globalClusteringRateMax = -1.0f;
        float weightedClusteringRateMax = -1.0f;
        float f1ScoreMax = -1.0f;
        float meanAbsoluteErrorMin = -1.0f;
        float rootMeanSquaredErrorMin = -1.0f;

        [[nodiscard]] float computeGlobalClusteringRate() const;
        [[nodiscard]] float computeWeightedClusteringRate() const;
        [[nodiscard]] float computeF1Score() const;
        [[nodiscard]] float computeMeanAbsoluteError() const;
        [[nodiscard]] float computeRootMeanSquaredError() const;

    protected:
        StatisticAnalysis() = default;
        StatisticAnalysis(const StatisticAnalysis&) = default;
        virtual ~StatisticAnalysis() = default;

        void initialize(int numberOfCluster);

        void setResultsAsNan();

        void evaluateOnceForRegression(const std::vector<float>& outputs,
                                       const std::vector<float>& desiredOutputs,
                                       float precision);
        void evaluateOnceForMultipleClassification(const std::vector<float>& outputs,
                                                   const std::vector<float>& desiredOutputs,
                                                   float separator);
        void evaluateOnceForClassification(const std::vector<float>& outputs,
                                           int classNumber,
                                           float separator);

        void startTesting();
        void stopTesting();

        bool globalClusteringRateIsBetterThanPreviously = false;
        bool weightedClusteringRateIsBetterThanPreviously = false;
        bool f1ScoreIsBetterThanPreviously = false;
        bool meanAbsoluteErrorIsBetterThanPreviously = false;
        bool rootMeanSquaredErrorIsBetterThanPreviously = false;

    public:
        float getGlobalClusteringRate() const;
        float getWeightedClusteringRate() const;
        float getF1Score() const;
        float getMeanAbsoluteError() const;
        float getRootMeanSquaredError() const;

        float getGlobalClusteringRateMax() const;
        float getWeightedClusteringRateMax() const;
        float getF1ScoreMax() const;
        float getMeanAbsoluteErrorMin() const;
        float getRootMeanSquaredErrorMin() const;

        bool operator==(const StatisticAnalysis& sa) const;
        bool operator!=(const StatisticAnalysis& sa) const;
    };

    template <class Archive>
    void StatisticAnalysis::serialize(Archive& ar, unsigned)
    {
        ar & this->clusters;
        ar & this->numberOfDataWellClassified;
        ar & this->numberOfDataMisclassified;
        ar & this->globalClusteringRate;
        ar & this->weightedClusteringRate;
        ar & this->f1Score;
        ar & this->meanAbsoluteError;
        ar & this->rootMeanSquaredError;
        ar & this->globalClusteringRateMax;
        ar & this->weightedClusteringRateMax;
        ar & this->f1ScoreMax;
        ar & this->meanAbsoluteErrorMin;
        ar & this->rootMeanSquaredErrorMin;
        ar & this->globalClusteringRateIsBetterThanPreviously;
        ar & this->weightedClusteringRateIsBetterThanPreviously;
        ar & this->f1ScoreIsBetterThanPreviously;
        ar & this->meanAbsoluteErrorIsBetterThanPreviously;
        ar & this->rootMeanSquaredErrorIsBetterThanPreviously;
    }
}


/*
 * Copyright 2016-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 * Copyright 2020 ANYbotics AG
 */

#include <iostream>
#include <numeric>

#include <Eigen/Core>

#include <gtest/gtest.h>

#include "copra/solvers/all.h"

#include "time_invariant_systems.h"

TEST_F(Problem, QuadProgTest) {  // NOLINT
    copra::QuadProgDenseSolver qpQuadProg;

    qpQuadProg.SI_problem(nrvars, nreqs, nrineqs);
    ASSERT_TRUE(qpQuadProg.SI_solve(Q, c, Aeq, beq, Aineq, bineq, XL, XU));

    ASSERT_EQ(qpQuadProg.SI_fail(), 0);
}

TEST_F(Problem, qpOASESTest) {  // NOLINT
  copra::qpOASESSolver qpOasesSolver;

  qpOasesSolver.SI_problem(nrvars, nreqs, nrineqs);
  ASSERT_TRUE(qpOasesSolver.SI_solve(Q, c, Aeq, beq, Aineq, bineq, XL, XU));

  ASSERT_EQ(qpOasesSolver.SI_fail(), 0);
}

TEST_F(Problem, QLDOnQuadProgTest) {  // NOLINT
    copra::QLDSolver qpQLD;
    copra::QuadProgDenseSolver qpQuadProg;

    qpQLD.SI_problem(nrvars, nreqs, nrineqs);
    qpQuadProg.SI_problem(nrvars, nreqs, nrineqs);
    ASSERT_TRUE(qpQLD.SI_solve(Q, c, Aeq, beq, Aineq, bineq, XL, XU));
    ASSERT_TRUE(qpQuadProg.SI_solve(Q, c, Aeq, beq, Aineq, bineq, XL, XU));

    Eigen::VectorXd resQLD = qpQLD.SI_result();
    Eigen::VectorXd resQuadProg = qpQuadProg.SI_result();
    EXPECT_TRUE(resQuadProg.isApprox(resQLD));
    ASSERT_EQ(qpQLD.SI_fail(), 0);
    ASSERT_EQ(qpQuadProg.SI_fail(), 0);
}

#ifdef EIGEN_LSSOL_FOUND
TEST_F(Problem, LSSOLOnQuadProgTest) {  // NOLINT
    copra::QuadProgDenseSolver qpQuadProg;
    copra::LSSOLSolver qpLSSOL;

    qpQuadProg.SI_problem(nrvars, nreqs, nrineqs);
    qpLSSOL.SI_problem(nrvars, nreqs, nrineqs);
    ASSERT_TRUE(qpQuadProg.SI_solve(Q, c, Aeq, beq, Aineq, bineq, XL, XU));
    ASSERT_TRUE(qpLSSOL.SI_solve(Q, c, Aeq, beq, Aineq, bineq, XL, XU));

    Eigen::VectorXd resQuadProg = qpQuadProg.SI_result();
    Eigen::VectorXd resLSSOL = qpLSSOL.SI_result();
    EXPECT_TRUE(resLSSOL.isApprox(resQuadProg));
    ASSERT_EQ(qpLSSOL.SI_fail(), 0);
}
#endif

#ifdef EIGEN_GUROBI_FOUND
TEST_F(Problem, GUROBIOnQuadProgTest) {  // NOLINT
    copra::QuadProgDenseSolver qpQuadProg;
    copra::GUROBISolver qpGUROBI;

    qpQuadProg.SI_problem(nrvars, nreqs, nrineqs);
    qpGUROBI.SI_problem(nrvars, nreqs, nrineqs);
    ASSERT_TRUE(qpQuadProg.SI_solve(Q, c, Aeq, beq, Aineq, bineq, XL, XU));
    ASSERT_TRUE(qpGUROBI.SI_solve(Q, c, Aeq, beq, Aineq, bineq, XL, XU));

    Eigen::VectorXd resQuadProg = qpQuadProg.SI_result();
    Eigen::VectorXd resGUROBI = qpGUROBI.SI_result();
    EXPECT_TRUE(resGUROBI.isApprox(resQuadProg, 1e-6));
    ASSERT_EQ(qpGUROBI.SI_fail(), GRB_OPTIMAL);
}
#endif

#ifdef EIGEN_OSQP_FOUND
TEST_F(Problem, OSQPOnQuadProgTest) {  // NOLINT
    copra::QuadProgDenseSolver qpQuadProg;
    copra::OSQPSolver osqp;

    qpQuadProg.SI_problem(nrvars, nreqs, nrineqs);
    osqp.SI_problem(nrvars, nreqs, nrineqs);
    ASSERT_TRUE(qpQuadProg.SI_solve(Q, c, Aeq, beq, Aineq, bineq, XL, XU));
    ASSERT_TRUE(osqp.SI_solve(Q, c, Aeq, beq, Aineq, bineq, XL, XU));

    Eigen::VectorXd resQuadProg = qpQuadProg.SI_result();
    Eigen::VectorXd resOSQP = osqp.SI_result();
    EXPECT_TRUE(resOSQP.isApprox(resQuadProg, 1e-6));
    ASSERT_EQ(osqp.SI_fail(), 1);
}
#endif


// Copyright PinaPL
//
// weights.hpp
// PinaPL
//

#ifndef WEIGHTS_HPP
#define WEIGHTS_HPP

#include <Eigen/Dense>

class Weights {
 public:
    int input_size;
    int output_size;
    Weights(int input_size, int output_size);
    ~Weights();
    void apply_gradient(double lambda);

    //   Information :
    // weight_in means the weight matrix applied to the new input
    // weight_st means the weight matrix applied to the previous cell OUT

    Eigen::MatrixXd weight_in_forget_gate;                  // Wf
    Eigen::MatrixXd weight_in_input_gate;                   // Wi
    Eigen::MatrixXd weight_in_input_block;                  // Wz
    Eigen::MatrixXd weight_in_output_gate;                  // Wo

    Eigen::MatrixXd weight_st_forget_gate;                  // Rf
    Eigen::MatrixXd weight_st_input_gate;                   // Ri
    Eigen::MatrixXd weight_st_input_block;                  // Rz
    Eigen::MatrixXd weight_st_output_gate;                  // Ro

    Eigen::MatrixXd bias_forget_gate;                       // Bf
    Eigen::MatrixXd bias_input_gate;                        // Bi
    Eigen::MatrixXd bias_input_block;                       // Bz
    Eigen::MatrixXd bias_output_gate;                       // Bo

    //   Information :
    // weight_in means the weight matrix applied to the new INPUT
    // weight_st means the weight matrix applied to the previous cell OUT

    Eigen::MatrixXd delta_weight_in_forget_gate;           // dWf
    Eigen::MatrixXd delta_weight_in_input_gate;            // dWi
    Eigen::MatrixXd delta_weight_in_input_block;           // dWz
    Eigen::MatrixXd delta_weight_in_output_gate;           // dWo

    Eigen::MatrixXd delta_weight_st_forget_gate;           // dRf
    Eigen::MatrixXd delta_weight_st_input_gate;            // dRi
    Eigen::MatrixXd delta_weight_st_input_block;           // dRz
    Eigen::MatrixXd delta_weight_st_output_gate;           // dRo

    Eigen::MatrixXd delta_bias_forget_gate;                // Bf
    Eigen::MatrixXd delta_bias_input_gate;                 // Bi
    Eigen::MatrixXd delta_bias_input_block;                // Bz
    Eigen::MatrixXd delta_bias_output_gate;                // Bo
};
#endif


#include "ekf.h"

#include <ros/ros.h>
#include <ros/console.h>
#include <sensor_msgs/Imu.h>
#include <Eigen/Eigen>
#include <random>

using namespace std;
using namespace Eigen;

ros::Publisher imu_noise_pub;
//add imu noise
double imu_noise = 0.05;
Vector3d noise;
std::default_random_engine generator;
std::normal_distribution<double> distribution_imu(0, imu_noise);

void imu_callback(const sensor_msgs::Imu::ConstPtr &msg)
{
    // add imu noise
    noise = Vector3d(distribution_imu(generator), distribution_imu(generator), distribution_imu(generator));

    sensor_msgs::Imu imu_noise;
    imu_noise = *msg;

    imu_noise.linear_acceleration.x += noise(0);
    imu_noise.linear_acceleration.y += noise(1);
    imu_noise.linear_acceleration.z += noise(2);

    imu_noise_pub.publish(imu_noise);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "ekf");
    ros::NodeHandle n("~");
    ros::Subscriber s1 = n.subscribe("imu", 1000, imu_callback, ros::TransportHints().tcpNoDelay());
    imu_noise_pub = n.advertise<sensor_msgs::Imu>("/djiros/imu_noise", 1000);

    n.getParam("imu_noise", imu_noise);

    cout << "imu_noise: " << imu_noise << endl;

    ros::spin();
}


/* -*-C++-*-
   (c) Copyright 1993-2005, Hewlett-Packard Development Company, LP

   See the file named COPYING for license details
*/

/** @file
    Simple statistics functions for single variables
*/

#include <stdlib.h>
#include <cstdio>
#include <string>
#include <float.h>
#include <string.h>

#include <boost/format.hpp>

#include <Lintel/AssertBoost.hpp>
#include <Lintel/Stats.hpp>
#include <Lintel/Double.hpp>

///////////////////////////////////////////////////////////////////////////
// Functions specific to the Statistics base class
///////////////////////////////////////////////////////////////////////////

StatsBase::StatsBase() : reset_count(0), is_assigned(true) {
    reset();
    reset_count = 0; // this is really is the first time
};

StatsBase::~StatsBase() {
    DEBUG_SINVARIANT(checkInvariants());
    is_assigned = false;
};

bool StatsBase::checkInvariants() const {
    return is_assigned;
}

void StatsBase::reset() {
    DEBUG_SINVARIANT(checkInvariants());
    reset_count++;
}

double StatsBase::stddev() const {
    DEBUG_SINVARIANT(checkInvariants());
    double sigsq = variance();

    if (sigsq <= 0.0) {
	return 0.0;
    }

    DEBUG_SINVARIANT(sigsq > 0.0);
    return sqrt(sigsq);
}

double StatsBase::relconf95() const {
    DEBUG_SINVARIANT(checkInvariants());
    return conf95()/mean();
}

///////////////////////////////////////////////////////////////////////////
// Functions related to the Stats singe-variable statistic class
///////////////////////////////////////////////////////////////////////////

// Create a new one
//
Stats::Stats() : StatsBase() {
  reset();
};

Stats::~Stats()	{};

void Stats::reset() {
    DEBUG_SINVARIANT(checkInvariants());
    StatsBase::reset();
    number = 0;
    sum = 0.0;
    sumsq = 0.0;
    min_value = Double::Inf;
    max_value = -Double::Inf;
}

void Stats::add(const double value) {
    DEBUG_INVARIANT(value == value, "You tried to add a NaN to the stats object.");

    ++number;
    sum += value;
    sumsq += value*value;

    if (value < min_value) {
	min_value = value;
    }
    if (value > max_value) {
	max_value = value;
    }
}

void Stats::add(const Stats &stat) {
    number += stat.number;
    sum += stat.sum;
    sumsq += stat.sumsq;

    if (stat.min_value < min_value) {
	min_value = stat.min_value;
    }
    if (stat.max_value > max_value) {
	max_value = stat.max_value;
    }
}

void Stats::addTimeSeq(const double value, const double) {
    this->add(value);
}

// Accessor functions

double Stats::mean() const {
    DEBUG_SINVARIANT(checkInvariants());
    if (number == 0) {
	return 0.0;
    } else {
	return double(sum)/double(number);
    }
};

double Stats::variance() const {
    DEBUG_SINVARIANT(checkInvariants());
    if (number == 0) return 0.0;
    double m = mean();
    return double(sumsq)/double(number) - m*m;
}

// TODO-future: Add in an optional invariant that checks statistical validity,
// i.e., that number>30. Consider also adding a generic conf function that
// takes a z-constant as input so that we can easily add more/different conf
// intervals. Could also add an interface that checks if a specific value is
// within some specified confidence interval.
// Potential z constants of interest:
// conf90 z= 1.645
// conf95 z= 1.960
// conf99 z= 2.576
double Stats::conf95() const {
    DEBUG_SINVARIANT(checkInvariants());
    if (number == 0) return DBL_MAX; // **** Should really be NaN if count==0
    DEBUG_SINVARIANT(number > 0); // **** Could be number > 30 for statistical
                                  // **** validity.
    return 1.96*stddev()/sqrt((double)number);
}

// Summarize contents as a string

std::string Stats::debugString() const {
    DEBUG_SINVARIANT(checkInvariants());

    if (count() == 0) {
	return "count 0";
    } else {
	return str(boost::format("count %d mean %G stddev %G var %G 95%%conf %G rel95%%conf %G"
				 " min %G max %G") % count() % mean() % stddev() % variance()
		   % conf95() % relconf95() % min() % max());
    }
};

void Stats::printRome(int depth, std::ostream &out) const {
    DEBUG_SINVARIANT(checkInvariants());

    std::string spaces;
    for(int i = 0; i < depth; i++) {
	spaces += " ";
    }

    out << spaces << "{ count " << countll() << " }\n";
    if (count() > 0) {
	out << spaces << "{ min " << min() << " }\n";
	out << spaces << "{ max " << max() << " }\n";
	out << spaces << "{ mean " << mean() << " }\n";
	out << spaces << "{ stddev " << stddev() << " }\n";
	out << spaces << "{ variance " << variance() << " }\n";
	out << spaces << "{ conf95 " << conf95() << " }\n";
	out << spaces << "{ total " << total() << " }\n";
	out << spaces << "{ total_sq " << total_sq() << " }\n";
    }
}

void Stats::printTabular(int depth, std::ostream &out) const {
  DEBUG_SINVARIANT(checkInvariants());

  std::string spaces;
  for(int i = 0; i < depth; i++) {
    spaces += " ";
  }

  out << spaces << "count " << countll() << "\n";
  if (count() > 0) {
    out << spaces << "min " << min() << "\n";
    out << spaces << "max " << max() << "\n";
    out << spaces << "mean " << mean() << "\n";
    out << spaces << "stddev " << stddev() << "\n";
    out << spaces << "variance " << variance() << "\n";
    out << spaces << "conf95 " << conf95() << "\n";
    out << spaces << "total " << total() << "\n";
    out << spaces << "total_sq " << total_sq() << "\n";
  }
  // This is kind of a hack, but I had problems when the printout of
  // Stats changed for an empty list.
  else
    {
      out << spaces << "min " << min() << "\n";
      out << spaces << "max " << max() << "\n";
      out << spaces << "mean " << 0 << "\n";
      out << spaces << "stddev " << 0 << "\n";
      out << spaces << "variance " << 0 << "\n";
      out << spaces << "conf95 " << 0 << "\n";
      out << spaces << "total " << total() << "\n";
      out << spaces << "total_sq " << total_sq() << "\n";
    }
}

void Stats::printText(std::ostream &out) const {
    out << boost::format("count=%d, mean=%.8g, stddev=%.8g, min=%.8g, max=%.8g\n")
	% countll() % mean() % stddev() % min() % max();
}

Stats *Stats::another_new() const {
    return new Stats();
}


// The MIT License
// (c) 2019 Daniel Williams

#ifndef _GRID_LAYOUT_HPP_
#define _GRID_LAYOUT_HPP_

#include <limits>
#include <memory>
#include <string>

#include <Eigen/Geometry>

#include <nlohmann/json/json_fwd.hpp>

using Eigen::Vector2d;

namespace terrain::geometry {

typedef uint64_t index_t;

///! \brief Layout is used to encapsulate common logic about how to layout a square grid
class Layout {
public:

    constexpr Layout();
    // constexpr Layout()
    //     : precision(1), x(0), y(0), width(1) {}

    constexpr Layout(const double _precision, const double _x, const double _y, const double _width);

    bool contains(const Eigen::Vector2d& at) const;

    bool operator!=(const Layout& other) const;
    bool operator==(const Layout& other) const;

    // definitely _not_ constexpr ;)
    Layout(nlohmann::json& doc);

    void clear();

    const Vector2d get_anchor() const;
    inline Vector2d get_center() const { return {x,y}; }
    inline size_t get_dimension() const { return dimension; }
    inline double get_half_width() const { return half_width; }
    inline uint8_t get_padding() const { return padding; }
    inline double get_precision() const { return precision; }
    inline size_t get_size() const { return size; }
    double get_x() const { return x; }
    double get_x_max() const;
    double get_x_min() const;
    double get_y() const { return y; }
    double get_y_max() const;
    double get_y_min() const;
    inline size_t get_width() const { return width; }

    ///! \brief hashes x,y ... into a simple row-major indexing
    constexpr index_t rhash( const Eigen::Vector2d& p) const { return rhash( p[0], p[1]); }
    constexpr index_t rhash( const double x, const double y) const;
    constexpr index_t rhash( const uint32_t i, const uint32_t j) const;

    ///! \brief hashes x,y ... by a Z-Order Curve
    ///! [1] http://en.wikipedia.org/wiki/Z-Order_curve
    constexpr index_t zhash( const Eigen::Vector2d& p) const { return zhash( p[0], p[1]); }
    constexpr index_t zhash( const double x, const double y) const;
    constexpr index_t zhash( const uint32_t i, const uint32_t j) const;

    ///! \brief factory method for creating from a json document
    static std::unique_ptr<Layout> make_from_json(nlohmann::json& doc);

    double constrain_x( const double x) const;
    double constrain_y( const double y) const;

    nlohmann::json to_json() const;

    std::string to_string() const;

// constants
public:
    // used for comparisons
    constexpr static double epsilon = 1e-6;
    constexpr static size_t index_bit_size = 64;
    constexpr static size_t maximum_supported_dimension = std::numeric_limits<uint32_t>::max();
    constexpr static double minimum_supported_precision = 1.;

private:
    constexpr uint64_t interleave( const uint32_t input) const;

    ///! \brief snaps this precision to match the next-power-of-2 dimension that covers the precision
    ///!
    ///! Note:  Precision <= 1.0
    constexpr double snap_precision(const double precision);
    constexpr double snap_width( const double _width);

    constexpr uint8_t calculate_padding( const double dimension);

private:  // primary variables
    double precision;

    double width;

    // Center Coordinates:
    // =====
    // these are stored as the raw doubles, (instead of Eigen::Vector2d) because ...
    //    (1) the layout can compress the layout,
    //    (2) allow the use of constexpr constructors
    double x;
    double y;

private: // secondary / cached variables
    size_t dimension;
    double half_width;
    uint8_t padding;  // left-pad the z-index with this many zeros.  Ranges from 0-64... which fits into a byte.
    size_t size;
};

#include "layout.inl"

} // namespace terrain::geometry

#endif // #ifdef _GRID_LAYOUT_HPP_