// Copyright 2002 Rensselaer Polytechnic Institute

// 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)

//  Authors: Lauren Foutz
//           Scott Hill

/*
  This file implements the functions

  template <class VertexListGraph, class DistanceMatrix,
    class P, class T, class R>
  bool floyd_warshall_initialized_all_pairs_shortest_paths(
    const VertexListGraph& g, DistanceMatrix& d,
    const bgl_named_params<P, T, R>& params)

  AND

  template <class VertexAndEdgeListGraph, class DistanceMatrix,
    class P, class T, class R>
  bool floyd_warshall_all_pairs_shortest_paths(
    const VertexAndEdgeListGraph& g, DistanceMatrix& d,
    const bgl_named_params<P, T, R>& params)
*/

#ifndef BOOST_GRAPH_FLOYD_WARSHALL_HPP
#define BOOST_GRAPH_FLOYD_WARSHALL_HPP

#include <boost/property_map/property_map.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/relax.hpp>
#include <boost/concept/assert.hpp>

namespace boost
{
namespace detail
{
    template < typename T, typename BinaryPredicate >
    T min_with_compare(const T& x, const T& y, const BinaryPredicate& compare)
    {
        if (compare(x, y))
            return x;
        else
            return y;
    }

    template < typename VertexListGraph, typename DistanceMatrix,
        typename BinaryPredicate, typename BinaryFunction, typename Infinity,
        typename Zero >
    bool floyd_warshall_dispatch(const VertexListGraph& g, DistanceMatrix& d,
        const BinaryPredicate& compare, const BinaryFunction& combine,
        const Infinity& inf, const Zero& zero)
    {
        typename graph_traits< VertexListGraph >::vertex_iterator i, lasti, j,
            lastj, k, lastk;

        for (boost::tie(k, lastk) = vertices(g); k != lastk; k++)
            for (boost::tie(i, lasti) = vertices(g); i != lasti; i++)
                if (d[*i][*k] != inf)
                    for (boost::tie(j, lastj) = vertices(g); j != lastj; j++)
                        if (d[*k][*j] != inf)
                            d[*i][*j] = detail::min_with_compare(d[*i][*j],
                                combine(d[*i][*k], d[*k][*j]), compare);

        for (boost::tie(i, lasti) = vertices(g); i != lasti; i++)
            if (compare(d[*i][*i], zero))
                return false;
        return true;
    }
}

template < typename VertexListGraph, typename DistanceMatrix,
    typename BinaryPredicate, typename BinaryFunction, typename Infinity,
    typename Zero >
bool floyd_warshall_initialized_all_pairs_shortest_paths(
    const VertexListGraph& g, DistanceMatrix& d, const BinaryPredicate& compare,
    const BinaryFunction& combine, const Infinity& inf, const Zero& zero)
{
    BOOST_CONCEPT_ASSERT((VertexListGraphConcept< VertexListGraph >));

    return detail::floyd_warshall_dispatch(g, d, compare, combine, inf, zero);
}

template < typename VertexAndEdgeListGraph, typename DistanceMatrix,
    typename WeightMap, typename BinaryPredicate, typename BinaryFunction,
    typename Infinity, typename Zero >
bool floyd_warshall_all_pairs_shortest_paths(const VertexAndEdgeListGraph& g,
    DistanceMatrix& d, const WeightMap& w, const BinaryPredicate& compare,
    const BinaryFunction& combine, const Infinity& inf, const Zero& zero)
{
    BOOST_CONCEPT_ASSERT((VertexListGraphConcept< VertexAndEdgeListGraph >));
    BOOST_CONCEPT_ASSERT((EdgeListGraphConcept< VertexAndEdgeListGraph >));
    BOOST_CONCEPT_ASSERT((IncidenceGraphConcept< VertexAndEdgeListGraph >));

    typename graph_traits< VertexAndEdgeListGraph >::vertex_iterator firstv,
        lastv, firstv2, lastv2;
    typename graph_traits< VertexAndEdgeListGraph >::edge_iterator first, last;

    for (boost::tie(firstv, lastv) = vertices(g); firstv != lastv; firstv++)
        for (boost::tie(firstv2, lastv2) = vertices(g); firstv2 != lastv2;
             firstv2++)
            d[*firstv][*firstv2] = inf;

    for (boost::tie(firstv, lastv) = vertices(g); firstv != lastv; firstv++)
        d[*firstv][*firstv] = zero;

    for (boost::tie(first, last) = edges(g); first != last; first++)
    {
        if (d[source(*first, g)][target(*first, g)] != inf)
        {
            d[source(*first, g)][target(*first, g)]
                = detail::min_with_compare(get(w, *first),
                    d[source(*first, g)][target(*first, g)], compare);
        }
        else
            d[source(*first, g)][target(*first, g)] = get(w, *first);
    }

    bool is_undirected = is_same<
        typename graph_traits< VertexAndEdgeListGraph >::directed_category,
        undirected_tag >::value;
    if (is_undirected)
    {
        for (boost::tie(first, last) = edges(g); first != last; first++)
        {
            if (d[target(*first, g)][source(*first, g)] != inf)
                d[target(*first, g)][source(*first, g)]
                    = detail::min_with_compare(get(w, *first),
                        d[target(*first, g)][source(*first, g)], compare);
            else
                d[target(*first, g)][source(*first, g)] = get(w, *first);
        }
    }

    return detail::floyd_warshall_dispatch(g, d, compare, combine, inf, zero);
}

namespace detail
{
    template < class VertexListGraph, class DistanceMatrix, class WeightMap,
        class P, class T, class R >
    bool floyd_warshall_init_dispatch(const VertexListGraph& g,
        DistanceMatrix& d, WeightMap /*w*/,
        const bgl_named_params< P, T, R >& params)
    {
        typedef typename property_traits< WeightMap >::value_type WM;
        WM inf = choose_param(get_param(params, distance_inf_t()),
            std::numeric_limits< WM >::max BOOST_PREVENT_MACRO_SUBSTITUTION());

        return floyd_warshall_initialized_all_pairs_shortest_paths(g, d,
            choose_param(
                get_param(params, distance_compare_t()), std::less< WM >()),
            choose_param(get_param(params, distance_combine_t()),
                closed_plus< WM >(inf)),
            inf, choose_param(get_param(params, distance_zero_t()), WM()));
    }

    template < class VertexAndEdgeListGraph, class DistanceMatrix,
        class WeightMap, class P, class T, class R >
    bool floyd_warshall_noninit_dispatch(const VertexAndEdgeListGraph& g,
        DistanceMatrix& d, WeightMap w,
        const bgl_named_params< P, T, R >& params)
    {
        typedef typename property_traits< WeightMap >::value_type WM;

        WM inf = choose_param(get_param(params, distance_inf_t()),
            std::numeric_limits< WM >::max BOOST_PREVENT_MACRO_SUBSTITUTION());
        return floyd_warshall_all_pairs_shortest_paths(g, d, w,
            choose_param(
                get_param(params, distance_compare_t()), std::less< WM >()),
            choose_param(get_param(params, distance_combine_t()),
                closed_plus< WM >(inf)),
            inf, choose_param(get_param(params, distance_zero_t()), WM()));
    }

} // namespace detail

template < class VertexListGraph, class DistanceMatrix, class P, class T,
    class R >
bool floyd_warshall_initialized_all_pairs_shortest_paths(
    const VertexListGraph& g, DistanceMatrix& d,
    const bgl_named_params< P, T, R >& params)
{
    return detail::floyd_warshall_init_dispatch(g, d,
        choose_const_pmap(get_param(params, edge_weight), g, edge_weight),
        params);
}

template < class VertexListGraph, class DistanceMatrix >
bool floyd_warshall_initialized_all_pairs_shortest_paths(
    const VertexListGraph& g, DistanceMatrix& d)
{
    bgl_named_params< int, int > params(0);
    return detail::floyd_warshall_init_dispatch(
        g, d, get(edge_weight, g), params);
}

template < class VertexAndEdgeListGraph, class DistanceMatrix, class P, class T,
    class R >
bool floyd_warshall_all_pairs_shortest_paths(const VertexAndEdgeListGraph& g,
    DistanceMatrix& d, const bgl_named_params< P, T, R >& params)
{
    return detail::floyd_warshall_noninit_dispatch(g, d,
        choose_const_pmap(get_param(params, edge_weight), g, edge_weight),
        params);
}

template < class VertexAndEdgeListGraph, class DistanceMatrix >
bool floyd_warshall_all_pairs_shortest_paths(
    const VertexAndEdgeListGraph& g, DistanceMatrix& d)
{
    bgl_named_params< int, int > params(0);
    return detail::floyd_warshall_noninit_dispatch(
        g, d, get(edge_weight, g), params);
}

} // namespace boost

#endif


// Copyright 2018 Apex.AI, Inc.
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

// Copyright 2018 Apex.AI, Inc.
// All rights reserved.
#include <chrono>
#include <Eigen/Cholesky>
#include "kalman_filter/srcf_core.hpp"
#include "kalman_filter/esrcf.hpp"
#include "motion_model/constant_velocity.hpp"
#include "motion_model/parameter_estimator.hpp"

using autoware::prediction::kalman_filter::SrcfCore;
using autoware::prediction::kalman_filter::Esrcf;
using autoware::motion::motion_model::ConstantVelocity;
using autoware::motion::motion_model::ParameterEstimator;
using Eigen::Matrix;
const float TOL = 1.0E-6F;

template<typename T, uint64_t H, uint64_t W>
void print(const Matrix<T, H, W> & A)
{
  std::cerr << "\nPrint Matrix:\n\n";
  for (int i = 0; i < H; ++i) {
    for (int j = 0; j < W; ++j) {
      std::cerr << A(i, j) << ", \t";
    }
    std::cerr << "\n";
  }
}

// example 5.4, in Kalman Filtering Theory and Practice using Matlab, pg 193-194
TEST(srcf_core, univariate)
{
  Matrix<float, 1, 1> F, H, Q, R, x, z, P, B, G;
  // TODO(ltbj): implement memory_test after the completion of #39
  // osrf_testing_tools_cpp::memory_test::start();
  const float log2pi = 1.83787706641F;
  EXPECT_FLOAT_EQ(log2pi, logf(3.14159265359F * 2.0F));
  F << 1;
  H << 1;
  Q << 1;  // sqrt, but 1 so whatever
  R << 2;
  x << 1;
  P << sqrtf(10.0F);
  G << 1;  // no mapping from procss noise subspace to state space
  SrcfCore<1, 1> core;
  // temporal update 0->1
  P(0) = F(0) * P(0);
  B(0) = G(0) * Q(0);
  core.right_lower_triangularize_matrices(P, B);
  EXPECT_LT(fabsf(B(0) - 0.0F), TOL);
  EXPECT_LT(fabsf(P(0) - sqrtf(11.0F)), TOL);
  // observation update 1
  z(0) = 2;
  float S = (H(0) * (P(0) * P(0)) * H(0)) + R(0);
  float err = z(0) - (H(0) * x(0));
  float pdf = -0.5F * (log2pi + logf(S) + (err * err) / S);
  EXPECT_LT(fabsf(core.scalar_update(z(0), R(0), H, P, x) - pdf), TOL) << pdf;
  EXPECT_LT(fabsf(x(0) - 24.0F / 13.0F), TOL);
  EXPECT_LT(fabsf(P(0) - sqrtf(22.0 / 13.0)), TOL);
  // temporal update 1->2
  P(0) = F(0) * P(0);
  B(0) = G(0) * Q(0);
  core.right_lower_triangularize_matrices(P, B);
  EXPECT_LT(fabsf(B(0) - 0.0F), TOL);
  EXPECT_LT(fabsf(P(0) - sqrtf(35.0F / 13.0F)), TOL);
  // observation update 2
  z(0) = 3;
  S = (H(0) * (P(0) * P(0)) * H(0)) + R(0);
  err = z(0) - (H(0) * x(0));
  pdf = -0.5F * (log2pi + logf(S) + (err * err) / S);
  EXPECT_LT(fabsf(core.scalar_update(z(0), R(0), H, P, x) - pdf), TOL) << pdf;
  EXPECT_LT(fabsf(x(0) - 153.0F / 61.0F), TOL);
  EXPECT_LT(fabsf(P(0) - sqrtf(70.0F / 61.0F)), TOL);
  // TODO(ltbj): implement memory_test after the completion of #39
  // osrf_testing_tools_cpp::memory_test::stop();
  // test bad case
  EXPECT_THROW(core.scalar_update(0.0F, 0.0F, H, P, x), std::domain_error);
}

// example 5.5 in Kalman Filtering Theory and Pracice using Matlab, pg 195-196
TEST(srcf_core, multivariate)
{
  Matrix<float, 2, 1> x({1, 2}), z({3, 4});
  Matrix<float, 2, 2> H, C, R;
  H << 0, 2, 3, 0;
  C << 4, 1, 1, 9;
  R << 1, 0, 0, 4;
  Matrix<float, 2, 1> h_row = H.row(0);
  // TODO(ltbj): implement memory_test after the completion of #39
  // osrf_testing_tools_cpp::memory_test::start();
  int info = 1;
  // higher TOL to match matlab precision
  const float TOL2 = 1.0E-4F;
  Eigen::LLT<Eigen::Ref<decltype(C)>> llt(C);

  C(0U, 1U) = 0.0F;
  // cholesky from numpy
  EXPECT_LT(fabsf(C(0, 0) - 2), TOL);
  EXPECT_LT(fabsf(C(1, 0) - 0.5F), TOL);
  EXPECT_LT(fabsf(C(1, 1) - 2.95803989F), TOL);
  // do vector update
  SrcfCore<2, 2> core;
  // observation update 1a
  float likelihood = core.scalar_update(z(0), R(0, 0), H.row(0), C, x);
  //// check intermediate values
  // These values are from the worked example in the book
  EXPECT_LT(fabsf(x(0) - (35.0 / 37.0)), TOL2);
  EXPECT_LT(fabsf(x(1) - (56.0 / 37.0)), TOL2);
  // check C, computed manually with scipy
  EXPECT_LT(fabsf(C(0, 0) - (1.97278785)), TOL2);
  EXPECT_LT(fabsf(C(1, 0) - (0.01369992)), TOL2);
  EXPECT_LT(fabsf(C(1, 1) - (0.49300665)), TOL2);
  // observation update 1b
  likelihood += core.scalar_update(z(1), R(1, 1), H.row(1), C, x);
  //// check final values
  // These values are from the worked example in the book
  EXPECT_LT(fabsf(x(0) - (467.0 / 361.0)), TOL2);
  EXPECT_LT(fabsf(x(1) - (2189.0 / 1444.0)), TOL2);
  // check C
  EXPECT_LT(fabsf(C(0, 0) - (0.63157895)), TOL2);
  EXPECT_LT(fabsf(C(1, 0) - (0.00438596)), TOL2);
  EXPECT_LT(fabsf(C(1, 1) - (0.49300665)), TOL2);
  // check likelihood of exact matrix:
  // S = H * P * H' + R = [37, 6; 6, 40]
  // err = z - (H * x) = [-1; 1]
  // P(err | 0, S) = -5.506280400650966 (from scipy)
  EXPECT_LT(fabsf(likelihood - (-5.506280400650966)), 0.01F) << likelihood;
  // Close enough, < 1% relative error
  // TODO(ltbj): implement memory_test after the completion of #39
  // osrf_testing_tools_cpp::memory_test::stop();
}

// example from Thornton's thesis (see below), pg 28
// This is a case where the conventional kalman filter will fail but the square root will not
// Because the example uses a different factorization paradigm, the answers in thes Thesis
// don't work (they use P = U * U^T, modern standard is P = U^T * U)
/*
s = some big number
e = 1/s = some small number
P = [s^2, 0; 0, s^2]
H = [1, e; 1, 1]
R = [1, 1]
P1 = [
⎡    2        3     ⎤
⎢ 2⋅s       -s      ⎥
⎢──────    ──────   ⎥
⎢ 2         2       ⎥
⎢s  + 2    s  + 2   ⎥
⎢                   ⎥ ~ [2    -s ]
⎢   3     2 ⎛ 2    ⎞⎥   [-s   s^2]
⎢ -s     s ⋅⎝s  + 1⎠⎥
⎢──────  ───────────⎥
⎢ 2          2      ⎥
⎣s  + 2     s  + 2  ⎦
S1 = L1 =
⎡           ________              ⎤
⎢          ╱    2                 ⎥
⎢         ╱    s                  ⎥
⎢  √2⋅   ╱   ──────         0     ⎥
⎢       ╱     2                   ⎥
⎢     ╲╱     s  + 2               ⎥
⎢                                 ⎥
⎢            ________             ⎥ ~ [sqrtf(2)         0             ]
⎢           ╱    2                ⎥   [-sqrf(2)/2 * s   sqrtf(2)/2 * s]
⎢          ╱    s                 ⎥
⎢-√2⋅s⋅   ╱   ──────          ____⎥
⎢        ╱     2             ╱  2 ⎥
⎢      ╲╱     s  + 2    √2⋅╲╱  s  ⎥
⎢─────────────────────  ──────────⎥
⎣          2                2     ⎦

Above were computed exactly using sympy

Similarly, the factors end up being
⎡  _________                       ⎤
⎢╲╱ 2⋅e + 1             0          ⎥
⎢                                  ⎥
⎢                  ________________⎥
⎢                 ╱    2           ⎥
⎢-(3⋅e + 1)      ╱  - e  + 2⋅e + 1 ⎥
⎢───────────    ╱   ────────────── ⎥
⎢  _________  ╲╱       2⋅e + 1     ⎥
⎣╲╱ 2⋅e + 1                        ⎦

for the second update
*/
// TODO(esteve): Test disabled until we fix
// https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/issues/92
TEST(srcf_core, DISABLED_degenerate)
{
  const float eps = 1.0E-6F;
  EXPECT_LT(fabsf((1.0F + (eps * eps)) - 1), TOL);
  const float sigma = 1.0F / eps;
  Matrix<float, 2, 2> H, C;
  H << 1, eps, 1, 1;
  C << sigma * sigma, 0, 0, sigma * sigma;
  Matrix<float, 2, 1> R({1, 1}), x;
  // cholesky on C
  int info = 1;
  Eigen::LLT<Eigen::Ref<decltype(C)>> llt(C);

  C(0, 1) = 0.0F;
  SrcfCore<2, 2> core;
  // TODO(ltbj): implement memory_test after the completion of #39
  // osrf_testing_tools_cpp::memory_test::start();
  // update
  float ll = core.scalar_update(0, R(0), H.row(0), C, x);
  EXPECT_LT(fabsf(C(0, 0) - sqrtf(2)), TOL);
  EXPECT_LT(fabsf(C(1, 0) - (-sqrtf(2) * sigma / 2)), TOL);
  EXPECT_LT(fabsf(C(1, 1) - sqrtf(2) * sigma / 2), TOL);
  ll += core.scalar_update(0, R(1), H.row(1), C, x);
  EXPECT_LT(fabsf(C(0, 0) - (1)), TOL);
  EXPECT_LT(fabsf(C(1, 0) - (-1 - 3 * eps)), 0.1F);  // This one is hard to compute exactly
  EXPECT_LT(fabsf(C(1, 1) - (1)), TOL);
  // S = [sig^2, sig^2; sig^2, 2*sig^2]
  // z = err = x = 0
  EXPECT_LT(fabsf(ll - (-29.468897182338893)), 1.0E-5F) << ll;
  // TODO(ltbj): implement memory_test after the completion of #39
  // osrf_testing_tools_cpp::memory_test::stop();
}

// Example 3.1 in page 66 in NASA JPL paper:
// https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770005172.pdf
// Modified from reference, since they use U*U^T = P, where convention is U^T * U = P
// test covariance temporal update
TEST(srcf_core, propagation)
{
  Matrix<float, 2, 1> GQ({0, 1});  // B = [0; 1], Q = 1
  const float eps = 1.0E-5F;
  const float sigma = 1.0F / eps;
  // sigma ^2 + 1 ~= sigma ^2
  EXPECT_LT(fabsf((sigma * sigma + 1) - (sigma * sigma)), TOL);
  Matrix<float, 2, 2> C;
  C << sigma, 0, sigma, 1;
  SrcfCore<2, 1> core;
  // TODO(ltbj): implement memory_test after the completion of #39
  // osrf_testing_tools_cpp::memory_test::start();
  core.right_lower_triangularize_matrices(C, GQ);
  EXPECT_LT(fabsf(C(1, 1) - (sqrtf(2))), eps) << C(0, 0);
  EXPECT_LT(fabsf(C(1, 0) - (sigma)), eps) << C(1, 0);
  EXPECT_LT(fabsf(C(0, 0) - (sigma)), eps) << C(1, 1);
  // B should be zero'd
  EXPECT_LT(fabsf(GQ(0)), eps) << GQ(0);
  EXPECT_LT(fabsf(GQ(1)), eps) << GQ(1);
  // TODO(ltbj): implement memory_test after the completion of #39
  // osrf_testing_tools_cpp::memory_test::stop();
}

// test other branches of givens rotations
TEST(srcf_core, triangularization)
{
  SrcfCore<3, 1> core;
  Matrix<float, 3, 3> A;
  A <<
    1, 1, 1,
    1, -1, 1,
    1, 1, 0;
  Matrix<float, 3, 3> C(A), D(A), E(A);
  Matrix<float, 3, 1> B;  // zero
  // TODO(ltbj): implement memory_test after the completion of #39
  // osrf_testing_tools_cpp::memory_test::start();
  core.right_lower_triangularize_matrices(A, B);
  // B should remain 0
  EXPECT_LT(fabsf(B(0)), TOL);
  EXPECT_LT(fabsf(B(1)), TOL);
  EXPECT_LT(fabsf(B(2)), TOL);
  // lower triangle of A should be 0
  EXPECT_LT(fabsf(A(0, 1)), TOL);
  EXPECT_LT(fabsf(A(0, 2)), TOL);
  EXPECT_LT(fabsf(A(1, 2)), TOL);
  // Not NAN
  EXPECT_NE(A(0, 0), NAN);
  EXPECT_NE(A(1, 0), NAN);
  EXPECT_NE(A(2, 0), NAN);
  EXPECT_NE(A(1, 1), NAN);
  EXPECT_NE(A(2, 1), NAN);
  EXPECT_NE(A(2, 2), NAN);
  //// yet another case
  E(0, 0) = 0;
  E(2, 0) = -1;
  core.right_lower_triangularize_matrices(E, B);
  // B should remain 0
  EXPECT_LT(fabsf(B(0)), TOL);
  EXPECT_LT(fabsf(B(1)), TOL);
  EXPECT_LT(fabsf(B(2)), TOL);
  // lower triangle of A should be 0
  EXPECT_LT(fabsf(E(0, 1)), TOL);
  EXPECT_LT(fabsf(E(0, 2)), TOL);
  EXPECT_LT(fabsf(E(1, 2)), TOL);
  // Not NAN
  EXPECT_NE(E(0, 0), NAN);
  EXPECT_NE(E(1, 0), NAN);
  EXPECT_NE(E(2, 0), NAN);
  EXPECT_NE(E(1, 1), NAN);
  EXPECT_NE(E(1, 2), NAN);
  EXPECT_NE(E(2, 2), NAN);
  // another arbitrary case
  B(2) = -1;
  C(2, 1) = -1;
  core.right_lower_triangularize_matrices(C, B);
  EXPECT_LT(fabsf(B(0)), TOL);
  EXPECT_LT(fabsf(B(1)), TOL);
  EXPECT_LT(fabsf(B(2)), TOL);
  // lower triangle of A should be 0
  EXPECT_LT(fabsf(C(0, 1)), TOL);
  EXPECT_LT(fabsf(C(0, 2)), TOL);
  EXPECT_LT(fabsf(C(1, 2)), TOL);
  // Not NAN
  EXPECT_NE(C(0, 0), NAN);
  EXPECT_NE(C(1, 0), NAN);
  EXPECT_NE(C(2, 0), NAN);
  EXPECT_NE(C(1, 1), NAN);
  EXPECT_NE(C(1, 2), NAN);
  EXPECT_NE(C(2, 2), NAN);
  // one more case
  B(0) = -1;
  B(1) = -2;
  B(2) = 1;
  D(0, 0) = 0;
  D(1, 1) = 0;
  core.right_lower_triangularize_matrices(D, B);
  EXPECT_LT(fabsf(B(0)), TOL);
  EXPECT_LT(fabsf(B(1)), TOL);
  EXPECT_LT(fabsf(B(2)), TOL);
  // lower triangle of A should be 0
  EXPECT_LT(fabsf(D(0, 1)), TOL);
  EXPECT_LT(fabsf(D(0, 2)), TOL);
  EXPECT_LT(fabsf(D(1, 2)), TOL);
  // Not NAN
  EXPECT_NE(D(0, 0), NAN);
  EXPECT_NE(D(1, 0), NAN);
  EXPECT_NE(D(2, 0), NAN);
  EXPECT_NE(D(1, 1), NAN);
  EXPECT_NE(D(1, 2), NAN);
  EXPECT_NE(D(2, 2), NAN);
  // TODO(ltbj): implement memory_test after the completion of #39
  // osrf_testing_tools_cpp::memory_test::stop();
}

// given a correct initial guess and perfect observations, state
// should not change and covariance should shrink to steady state
TEST(esrcf, convergence)
{
  ConstantVelocity model;
  Matrix<float, 4, 1> R({1, 1, 1, 1});
  // identity
  Matrix<float, 4, 4> GQ;
  GQ <<
    0.1F, 0.0F, 0.0F, 0.0F,
    0.0F, 0.1F, 0.0F, 0.0F,
    0.0F, 0.0F, 0.1F, 0.0F,
    0.0F, 0.0F, 0.0F, 0.1F;

  Matrix<float, 4, 1> x({0, 0, 1, -1});  // all 0's
  // cholesky of: (so there is some covariance wrt hidden state
  // 1   0   0.5 0
  // 0   1   0   0.5
  // 0.5 0   1   0
  // 0   0.5 0   1
  Matrix<float, 4, 4> P;
  P <<
    1, 0, 0, 0,
    0, 1, 0, 0,
    0, 0, 1, 0,
    0, 0, 0, 1;
  // prefit and postfit covariance matrices
  Matrix<float, 4, 4> P_m(P), P_p(P), P_last;
  const Matrix<float, 4, 4> H((Matrix<float, 4, 4>() <<
    1, 0, 0, 0,
    0, 1, 0, 0,
    0, 0, 1, 0,
    0, 0, 0, 1 ).finished());

  Esrcf<4, 4> kf(model, GQ);
  // TODO(ltbj): implement memory_test after the completion of #39
  // osrf_testing_tools_cpp::memory_test::start();
  kf.reset(x, P);
  float last_ll = -std::numeric_limits<float>::max();
  // microseconds_100 = 0.1s
  std::chrono::nanoseconds microseconds_100(100000000LL);
  for (uint32_t idx = 0; idx < 30; ++idx) {
    x(0) += 0.1;
    x(1) -= 0.1;
    kf.temporal_update(microseconds_100);
    // covariance should grow wrt postfit
    EXPECT_GT(kf.get_covariance()(0, 0), P_p(0, 0));
    EXPECT_GT(kf.get_covariance()(1, 1), P_p(1, 1));
    EXPECT_GT(kf.get_covariance()(2, 2), P_p(2, 2));
    EXPECT_GT(kf.get_covariance()(3, 3), P_p(3, 3));
    // covariance should be smaller than last prefit
    if (idx > 0) {
      EXPECT_LE(kf.get_covariance()(0, 0), P_m(0, 0));
      EXPECT_LE(kf.get_covariance()(1, 1), P_m(1, 1));
      EXPECT_LE(kf.get_covariance()(2, 2), P_m(2, 2));
      EXPECT_LE(kf.get_covariance()(3, 3), P_m(3, 3));
    }
    P_m = kf.get_covariance();
    // state should still be 0
    EXPECT_LT(fabsf(model[0] - x(0)), TOL);
    EXPECT_LT(fabsf(model[1] - x(1)), TOL);
    EXPECT_LT(fabsf(model[2] - x(2)), TOL);
    EXPECT_LT(fabsf(model[3] - x(3)), TOL);
    //// exact observation
    const float ll = kf.observation_update(x, H, R);
    // likelihood should improve
    EXPECT_GT(ll, last_ll);
    last_ll = ll;
    // covariance should shrink
    EXPECT_LE(kf.get_covariance()(0, 0), P_m(0, 0));
    EXPECT_LE(kf.get_covariance()(1, 1), P_m(1, 1));
    EXPECT_LE(kf.get_covariance()(2, 2), P_m(2, 2));
    EXPECT_LE(kf.get_covariance()(3, 3), P_m(3, 3));
    // covariance should also be smaller wrt last postfit
    EXPECT_LE(kf.get_covariance()(0, 0), P_p(0, 0));
    EXPECT_LE(kf.get_covariance()(1, 1), P_p(1, 1));
    EXPECT_LE(kf.get_covariance()(2, 2), P_p(2, 2));
    EXPECT_LE(kf.get_covariance()(3, 3), P_p(3, 3));
    P_last = P_p;
    P_p = kf.get_covariance();
    // state should still be 0
    EXPECT_LT(fabsf(model[0] - x(0)), TOL);
    EXPECT_LT(fabsf(model[1] - x(1)), TOL);
    EXPECT_LT(fabsf(model[2] - x(2)), TOL);
    EXPECT_LT(fabsf(model[3] - x(3)), TOL);
  }
  P_last -= P_p;
  float norm = 0.0F;
  for (int i = 0; i < P_last.rows(); ++i) {
    for (int j = 0; j < P_last.cols(); ++j) {
      norm += P_last(i, j) * P_last(i, j);
    }
  }
  EXPECT_LE(sqrtf(norm), 0.0005);
  // TODO(ltbj): implement memory_test after the completion of #39
  // osrf_testing_tools_cpp::memory_test::stop();
}

// Hidden states should converge to a good value
TEST(esrcf, hidden_state)
{
  ConstantVelocity model;
  Matrix<float, 2, 1> R({0.001F, 0.001F});
  // identity
  Matrix<float, 4, 4> GQ;
  GQ <<
    0.125F, 0.0F, 0.0F, 0.0F,
    0.0F, 0.125F, 0.0F, 0.0F,
    0.5F, 0.0F, 0.1F, 0.0F,
    0.0F, 0.5F, 0.0F, 0.1F
  ;
  const float vx = 1.0F, vy = 1.0F;
  Matrix<float, 4, 1> x({0, 0, 0.0, 0.0});  // all 0's
  // cholesky of: (so there is some covariance wrt hidden state
  // 1   0   0.5 0
  // 0   1   0   0.5
  // 0.5 0   1   0
  // 0   0.5 0   1
  Matrix<float, 4, 4> P;
  P <<
    1.0F, 0.0F, 0.0F, 0.0F,
    0.0F, 1.0F, 0.0F, 0.0F,
    0.0F, 0.0F, 10.0F, 0.0F,
    0.0F, 0.0F, 0.0F, 10.0F
  ;
  // prefit and postfit covariance matrices
  Matrix<float, 4, 4> P_m(P), P_p(P);
  const Matrix<float, 4, 4> P0(P);
  const Matrix<float, 2, 4> H((Matrix<float, 2, 4>() <<
    1, 0, 0, 0,
    0, 1, 0, 0).finished());

  Matrix<float, 2, 1> z({0, 0});
  Esrcf<4, 4> kf(model, GQ, x, P);
  EXPECT_NE(model[ConstantVelocity::States::VELOCITY_X], vx);
  EXPECT_NE(model[ConstantVelocity::States::VELOCITY_Y], vy);
  // microseconds_100 = 0.1s
  const std::chrono::nanoseconds microseconds_100(100000000LL);
  // TODO(ltbj): implement memory_test after the completion of #39
  // osrf_testing_tools_cpp::memory_test::start();
  // velocity error should be shrinking
  float err_u = std::numeric_limits<float>::max();
  float err_v = std::numeric_limits<float>::max();
  float last_ll = -std::numeric_limits<float>::max();
  for (uint32_t idx = 0; idx < 90; ++idx) {
    z(0) += static_cast<float>(microseconds_100.count()) / 1000000000LL * vx;
    z(1) += static_cast<float>(microseconds_100.count()) / 1000000000LL * vy;
    kf.temporal_update(microseconds_100);
    // covariance should grow wrt postfit
    EXPECT_GT(kf.get_covariance()(0, 0), P_p(0, 0)) << idx;
    EXPECT_GT(kf.get_covariance()(1, 1), P_p(1, 1)) << idx;
    // EXPECT_GT(kf.get_covariance()(2, 2), P_p(2, 2)) << idx;
    // EXPECT_GT(kf.get_covariance()(3, 3), P_p(3, 3)) << idx;
    // Analytically, the updated velocity/hidden state covariance is
    // t^2 * (p_v + s^2), where t is the characteristic time step, and s is the characteristic
    // measurement noise. Since the time step is < 1, then variance shrinks
    // Hidden state error should shrink
    float err = fabsf(model[2] - vx);
    EXPECT_LT(err, err_u) << idx;
    err_u = err;
    err = fabsf(model[3] - vy);
    EXPECT_LT(err, err_v) << idx;
    err_v = err;
    if ((err_u < TOL) && (err_v < TOL)) {
      // TODO(ltbj): implement memory_test after the completion of #39
      // osrf_testing_tools_cpp::memory_test::pause();
      std::cout << "Converged at " << idx << "\n";
      // TODO(ltbj): implement memory_test after the completion of #39
      // osrf_testing_tools_cpp::memory_test::resume();
      break;
    }
    P_m = kf.get_covariance();
    //// exact observation
    const float ll = kf.observation_update(z, H, R);
    // likelihood should improve or converge
    EXPECT_GE(ll, last_ll) << idx;
    last_ll = ll;
    // covariance should shrink or converge
    EXPECT_LE(kf.get_covariance()(0, 0), P_m(0, 0)) << idx;
    EXPECT_LE(kf.get_covariance()(1, 1), P_m(1, 1)) << idx;
    EXPECT_LE(kf.get_covariance()(2, 2), P_m(2, 2)) << idx;
    EXPECT_LE(kf.get_covariance()(3, 3), P_m(3, 3)) << idx;
    // postfit covariance should always be smaller than p0
    EXPECT_LT(kf.get_covariance()(0, 0), P0(0, 0)) << idx;
    EXPECT_LT(kf.get_covariance()(1, 1), P0(1, 1)) << idx;
    EXPECT_LT(kf.get_covariance()(2, 2), P0(2, 2)) << idx;
    EXPECT_LT(kf.get_covariance()(3, 3), P0(3, 3)) << idx;
    P_p = kf.get_covariance();
    // state should be very close to observation
    EXPECT_LT(fabsf(model[0] - z(0)), kf.get_covariance()(0, 0)) << idx;
    EXPECT_LT(fabsf(model[1] - z(0)), kf.get_covariance()(1, 1)) << idx;
  }
  // TODO(ltbj): implement memory_test after the completion of #39
  // osrf_testing_tools_cpp::memory_test::stop();
}

TEST(esrcf, imm_mix)
{
  static const uint32_t dim = 2U;
  int info;
  Matrix<float, dim, dim> P1, P2, P, C, GQ;
  Matrix<float, dim, 1> x1, x2, x_mix;
  P1 << 2.0F, 2.0F, 2.0F, 4.0F;
  P2 << 2.0F, 1.0F, 1.0F, 2.0F;
  C = P1;
  Eigen::LLT<Eigen::Ref<decltype(C)>> llt(C);

  C(0U, 1U) = 0.0F;
  const float u1 = 0.75F;
  const float u2 = 0.25F;
  x1 = {-3.0F, 5.0F};
  x2 = {5.0F, -3.0F};
  x_mix = {-1.0F, 3.0F};
  ParameterEstimator<dim> model;
  Esrcf<dim, dim> kf(model, GQ, x1, C);
  // TODO(ltbj): implement memory_test after the completion of #39
  // osrf_testing_tools_cpp::memory_test::start();
  // P1' = u P1 * (x1 - x) (x1 - x)^T
  kf.imm_self_mix(u1, x_mix);
  EXPECT_FLOAT_EQ(model.get_state()(0U), -1.0F);
  EXPECT_FLOAT_EQ(model.get_state()(1U), 3.0F);
  P = P1;
  // dx = {-2, 2} --> dx * dx' = {4, -4; -4, 4}
  P += (Matrix<float, dim, dim>() << 4.0F, -4.0F, -4.0F, 4.0F).finished();
  P *= u1;
  C = P;
  llt.compute(C);
  C(0U, 1U) = 0.0F;
  EXPECT_FLOAT_EQ(kf.get_covariance()(0U, 0U), C(0U, 0U));
  EXPECT_LT(fabsf(kf.get_covariance()(0U, 1U) - C(0U, 1U)), TOL);
  EXPECT_FLOAT_EQ(kf.get_covariance()(1U, 0U), C(1U, 0U));
  EXPECT_FLOAT_EQ(kf.get_covariance()(1U, 1U), C(1U, 1U));
  // Apply update from other model
  C = P2;
  llt.compute(C);

  C(0U, 1U) = 0.0F;
  kf.imm_other_mix(u2, x2, C, dim);
  // inputs should be zero'd after triangularization
  EXPECT_LT(fabsf(C(0U, 0U)), TOL);
  EXPECT_LT(fabsf(C(0U, 1U)), TOL);
  EXPECT_LT(fabsf(C(1U, 0U)), TOL);
  EXPECT_LT(fabsf(C(1U, 1U)), TOL);
  // internal state should remain unchanged
  EXPECT_FLOAT_EQ(model.get_state()(0U), -1.0F);
  EXPECT_FLOAT_EQ(model.get_state()(1U), 3.0F);
  // Compute updated covariance
  // dx2 = {6, -6}
  P2 += (Matrix<float, dim, dim>() << 36.0F, -36.0F, -36.0F, 36.0F).finished();
  P2 *= u2;
  P += P2;
  C = P;
  C(0U, 1U) = 0.0F;
  llt.compute(C);

  EXPECT_FLOAT_EQ(kf.get_covariance()(0U, 0U), C(0U, 0U));
  EXPECT_LT(fabsf(kf.get_covariance()(0U, 1U) - C(0U, 1U)), TOL);
  EXPECT_FLOAT_EQ(kf.get_covariance()(1U, 0U), C(1U, 0U));
  EXPECT_FLOAT_EQ(kf.get_covariance()(1U, 1U), C(1U, 1U));
  // TODO(ltbj): implement memory_test after the completion of #39
  // osrf_testing_tools_cpp::memory_test::stop();
}


#include <eve/function/ellint_3.hpp>
#include <boost/math/special_functions/ellint_3.hpp>
#include <eve/wide.hpp>
#include <iostream>

using wide_ft = eve::wide<float, eve::fixed<4>>;

int main()
{
  wide_ft pf = {0.1f, 0.0f, 0.2f, 3.0f};
  wide_ft qf = {0.1f, 1.5f, 0.2f, 0.5f};
  wide_ft rf = {2.0f, 1.0f, 0.1f, 1.0f};

  std::cout << "---- simd" << '\n'
            << "<- pf                    = " << pf << '\n'
            << "<- qf                    = " << qf << '\n'
            << "<- rf                    = " << rf << '\n'
            << "-> ellint_3(pf, qf, rf) = " << eve::ellint_3(pf, qf, rf) << '\n';

  float xf = 3.0f;
  float yf = 0.5f;
  float zf = 1.0f;

  std::cout << "---- scalar" << '\n'
            << "<- xf                    = " << xf << '\n'
            << "<- yf                    = " << yf << '\n'
            << "<- zf                    = " << zf << '\n'
            << "-> ellint_3(xf, yf, zf) = " << eve::ellint_3(xf, yf, zf) << '\n';

  return 0;
}


// Implement member functions for SimplicialComplexOperators class.
#include "simplicial-complex-operators.h"
#include "geometrycentral/numerical/linear_algebra_utilities.h"
#include <Eigen/Sparse>
#include <cassert>

using namespace geometrycentral;
using namespace geometrycentral::surface;

/*
 * Assign a unique index to each vertex, edge, and face of a mesh.
 * All elements are 0-indexed.
 *
 * Input: None. Access geometry via the member variable <geometry>, and pointer to the mesh via <mesh>.
 * Returns: None.
 */
void SimplicialComplexOperators::assignElementIndices() {

    // Needed to access geometry->vertexIndices, etc. as cached quantities.
    // Not needed if you're just using v->getIndex(), etc.
    geometry->requireVertexIndices();
    geometry->requireEdgeIndices();
    geometry->requireFaceIndices();

    // You can set the index field of a vertex via geometry->vertexIndices[v], where v is a Vertex object (or an
    // integer). Similarly you can do edges and faces via geometry->edgeIndices, geometry->faceIndices, like so:
    size_t idx = 0;
    for (Vertex v : mesh->vertices()) {
        idx = geometry->vertexIndices[v];
    }

    for (Edge e : mesh->edges()) {
        idx = geometry->edgeIndices[e];
    }

    for (Face f : mesh->faces()) {
        idx = geometry->faceIndices[f];
    }

    // You can more easily get the indices of mesh elements using the function getIndex(), albeit less efficiently and
    // technically less safe (although you don't need to worry about it), like so:
    //
    //      v.getIndex()
    //
    // where v can be a Vertex, Edge, Face, Halfedge, etc. For example:

    for (Vertex v : mesh->vertices()) {
        idx = v.getIndex(); // == geometry->vertexIndices[v])
    }

    // Geometry Central already sets the indices for us, though, so this function is just here for demonstration.
    // You don't have to do anything :)
}

/*
 * Construct the unsigned vertex-edge adjacency matrix A0.
 *
 * Input:
 * Returns: The sparse vertex-edge adjacency matrix which gets stored in the global variable A0.
 */
SparseMatrix<size_t> SimplicialComplexOperators::buildVertexEdgeAdjacencyMatrix() const {

    geometry->requireVertexIndices();
    geometry->requireEdgeIndices();
    geometry->requireFaceIndices();
    // Note: You can build an Eigen sparse matrix from triplets, then return it as a Geometry Central SparseMatrix.
    // See <https://eigen.tuxfamily.org/dox/group__TutorialSparse.html> for documentation.

    // initialize eigensparse matrix
    SparseMatrix<size_t> mat(mesh->nEdges(),mesh->nVertices());

    for (Edge e : mesh->edges()){
        // we compute by edge since we know there will only be 2 per row
        // enter sparse matrix values one by one:
        size_t i1 = geometry->vertexIndices[e.firstVertex()];
        size_t i2 = geometry->vertexIndices[e.secondVertex()];
        size_t e_i = geometry->edgeIndices[e];

        mat.insert(e_i,i1)=1;
        mat.insert(e_i,i2)=1;

    }

    return mat;
}

/*
 * Construct the unsigned face-edge adjacency matrix A1.
 *
 * Input:
 * Returns: The sparse face-edge adjacency matrix which gets stored in the global variable A1.
 */
SparseMatrix<size_t> SimplicialComplexOperators::buildFaceEdgeAdjacencyMatrix() const {

    geometry->requireVertexIndices();
    geometry->requireEdgeIndices();
    geometry->requireFaceIndices();

    // initialize eigensparse matrix
    SparseMatrix<size_t> mat(mesh->nFaces(),mesh->nEdges());

    for (Halfedge he : mesh->halfedges()){

        // only count interior edges
        if(he.isInterior()){
        // enter sparse matrix values one by one:
        size_t e = geometry->edgeIndices[he.edge()];

        size_t f = geometry->faceIndices[he.face()];

        mat.insert(f,e)=1;
        }

    }
    return mat;
}

/*
 * Construct a vector encoding the vertices in the selected subset of simplices.
 *
 * Input: Selected subset of simplices.
 * Returns: Vector of length |V|, where |V| = # of vertices in the mesh.
 */
Vector<size_t> SimplicialComplexOperators::buildVertexVector(const MeshSubset& subset) const {

    Vector<size_t> vertices= Vector<size_t>::Zero(mesh->nVertices(),1);

    std::set<size_t>::iterator it;

    for (it = subset.vertices.begin(); it!= subset.vertices.end(); it++){
        vertices[*it]=1;
    }

    return vertices;
}

/*
 * Construct a vector encoding the edges in the selected subset of simplices.
 *
 * Input: Selected subset of simplices.
 * Returns: Vector of length |E|, where |E| = # of edges in mesh.
 */
Vector<size_t> SimplicialComplexOperators::buildEdgeVector(const MeshSubset& subset) const {

    Vector<size_t> edges= Vector<size_t>::Zero(mesh->nEdges(),1);

    std::set<size_t>::iterator it;

    for (it = subset.edges.begin(); it!= subset.edges.end(); it++){
         edges[*it]=1;
    }

    return edges;
}

/*
 * Construct a vector encoding the faces in the selected subset of simplices.
 *
 * Input: Selected subset of simplices.
 * Returns: Vector of length |F|, where |F| = # of faces in mesh.
 */
Vector<size_t> SimplicialComplexOperators::buildFaceVector(const MeshSubset& subset) const {

    Vector<size_t> faces= Vector<size_t>::Zero(mesh->nFaces(),1);

    std::set<size_t>::iterator it;

    for (it = subset.faces.begin(); it!= subset.faces.end(); it++){
         faces[*it]=1;
    }

    return faces;
}

/*
 * Compute the simplicial star St(S) of the selected subset of simplices.
 *
 * Input: A MeshSubset object containing the indices of the currently active vertices, edges, and faces, respectively.
 * Returns: The star of the given subset.
 */
MeshSubset SimplicialComplexOperators::star(const MeshSubset& subset) const {

    MeshSubset star = subset;

    Vector<size_t> E0;
    Vector<size_t> F0;
    Vector<size_t> F1;

    std::set<size_t> set_E0;
    std::set<size_t> set_F0;
    std::set<size_t> set_F1;

    Vector<size_t> sub_verts = buildVertexVector(subset);
    Vector<size_t> sub_edges = buildEdgeVector(subset);
    Vector<size_t> sub_faces = buildFaceVector(subset);

    // Compute all connected vertices in the vectors
    E0 = A0 * sub_verts;
    F0 = A1 * A0 * sub_verts;
    F1 = A1 * sub_edges;

    // fill edge sets
    for(int i =0; i<E0.rows(); ++i){
        if(E0[i] != 0){
            set_E0.insert(i);
        }
    }
    // fill face sets
    for(int i =0; i<F0.rows(); ++i){
        if(F0[i] != 0){
            set_F0.insert(i);
        }
        if(F1[i] != 0){
            set_F1.insert(i);
        }
    }

    star.addEdges(set_E0);
    star.addFaces(set_F0);
    star.addFaces(set_F1);

    return star;
}

/*
 * Compute the closure Cl(S) of the selected subset of simplices.
 *
 * Input: A MeshSubset object containing the indices of the currently active vertices, edges, and faces, respectively.
 * Returns: The closure of the given subset.
 */
MeshSubset SimplicialComplexOperators::closure(const MeshSubset& subset) const {

    // final returned subset
    MeshSubset closure = subset;

    // vectors for computation
    Vector<size_t> V0;
    Vector<size_t> E1;
    Vector<size_t> V1;

    // sets for storing indices
    std::set<size_t> set_V0;
    std::set<size_t> set_E1;
    std::set<size_t> set_V1;

    // Only need the subset edges and faces to compute the closure
    Vector<size_t> sub_edges = buildEdgeVector(subset);
    Vector<size_t> sub_faces = buildFaceVector(subset);

    // Add vertices that touch edges
    V0 = A0.transpose()*sub_edges;
    for(int i =0; i<V0.rows(); ++i){
        if(V0[i] != 0){
            set_V0.insert(i);
        }
    }
    closure.addVertices(set_V0);

    // Add edges that touch faces
    E1 = A1.transpose()*sub_faces;
    for(int i =0; i<E1.rows(); i++){
        if(E1[i] != 0){
            set_E1.insert(i);
        }
    }
    closure.addEdges(set_E1);

    // Add edges that touch faces
    V1 =A0.transpose()*A1.transpose()*sub_faces;
    for(int i =0; i<V1.rows(); i++){
        if(V1[i] != 0){
            set_V1.insert(i);
        }
    }
    closure.addVertices(set_V1);

    return closure;
    }

/*
 * Compute the link Lk(S) of the selected subset of simplices.
 *
 * Input: A MeshSubset object containing the indices of the currently active vertices, edges, and faces, respectively.
 * Returns: The link of the given subset.
 */
MeshSubset SimplicialComplexOperators::link(const MeshSubset& subset) const {

    // define Cl(St(subset)) and St(Cl(subset))
    MeshSubset ClSt = closure(star(subset));
    MeshSubset StCl = star(closure(subset));

    // to remove StCl from ClSt we pick out the set of faces, edges and vertices of StCl
    std::set<size_t> v = StCl.vertices;
    std::set<size_t> e = StCl.edges;
    std::set<size_t> f = StCl.faces;

    // initialize link as Cl(St(subset))
    MeshSubset link = ClSt;

    link.deleteVertices(v);
    link.deleteEdges(e);
    link.deleteFaces(f);

    return link;
}

/*
 * Return true if the selected subset is a simplicial complex, false otherwise.
 *
 * Input: A MeshSubset object containing the indices of the currently active vertices, edges, and faces, respectively.
 * Returns: True if given subset is a simplicial complex, false otherwise.
 */
bool SimplicialComplexOperators::isComplex(const MeshSubset& subset) const {

    MeshSubset Cl;
    Cl = closure(subset);

    // a potentially faster method that only compares sizes rather than each elt
    size_t v0 = subset.vertices.size();
    size_t e0 = subset.edges.size();
    size_t f0 = subset.faces.size();
    size_t v1 = Cl.vertices.size();
    size_t e1 = Cl.edges.size();
    size_t f1 = Cl.faces.size();

    return (v0==v1 && e0==e1 && f0==f1);
    //return closure==subset;

}

/*
 * Check if the given subset S is a pure simplicial complex. If so, return the degree of the complex. Otherwise, return
 * -1.
 *
 * Input: A MeshSubset object containing the indices of the currently active vertices, edges, and faces, respectively.
 * Returns: int representing the degree of the given complex (-1 if not pure)
 */
int SimplicialComplexOperators::isPureComplex(const MeshSubset& subset) const {

    if(isComplex(subset)){
        Vector<size_t> sub_verts = buildVertexVector(subset);
        Vector<size_t> sub_edges = buildEdgeVector(subset);
        Vector<size_t> sub_faces = buildFaceVector(subset);

        if(subset.faces.size()!=0){
            // make sure every edge and vertex is part of a face
            Vector<size_t> new_edges = A1.transpose() * sub_faces;
            for(int i =0; i<new_edges.rows(); i++){
                if(new_edges[i] != 0){
                    new_edges[i]=1;
                }
            }
            Vector<size_t> new_verts = A0.transpose() * A1.transpose() * sub_faces;
            for(int i =0; i<new_verts.rows(); i++){
                if(new_verts[i] != 0){
                    new_verts[i]=1;
                }
            }

            if(new_edges == sub_edges && new_verts == sub_verts){
                return 2;
            }
            else{
                return -1;
            }
        }

        else if(subset.edges.size()!=0){
            // make sure every vertex is part of an edge
            Vector<size_t> new_verts = A0.transpose() * sub_edges;
            for(int i =0; i<new_verts.rows(); i++){
                if(new_verts[i] != 0){
                    new_verts[i]=1;
                }
            }

            if(new_verts == sub_verts){
                return 1;
            }
            else{
                return -1;
            }
        }

        // if only vertices we are done
        else{
            return 0;
        }
    }
    return -1;
}

/*
 * Compute the set of simplices contained in the boundary bd(S) of the selected subset S of simplices.
 *
 * Input: A MeshSubset object containing the indices of the currently active vertices, edges, and faces, respectively.
 * Returns: The boundary of the given subset.
 */
MeshSubset SimplicialComplexOperators::boundary(const MeshSubset& subset) const {

    MeshSubset boundary;

    // find simplicial complex degree
    int deg = isPureComplex(subset);

    // if not pure complex, return empty placeholder
    // (or error?)
    if(deg == -1){
        return boundary;
    }

    // if subset is points, return empty placeholder
    else if(deg == 0){
        return boundary;
    }

    // 1-simplicial complex, return points connected to one edge
    else if(deg ==1){
        // only vertices that touch 1 edge
        // first find vertex connection vector
        Vector<size_t> edges = buildEdgeVector(subset);
        Vector<size_t> vertices = A0.transpose()*edges;
        std::set<size_t> int_verts_set;

        // fill bd_edges_set and turn edges into boundary edges
        for(int i =0; i<vertices.rows(); i++){
            if(vertices[i] == 1){
                int_verts_set.insert(i);
            }
        }
        boundary.addVertices(int_verts_set);
    }

    // since boundaries are simplicial complexes of degree one lower:
    // we can only find the boundary edges, then take the closure at the end
    else if(deg ==2){
        // first find edges connected to one face
        // then take the closure
        Vector<size_t> faces = buildFaceVector(subset);
        Vector<size_t> edges = A1.transpose()*faces;
        std::set<size_t> int_edges_set;
        MeshSubset bdry_only_edges;

        for(int i =0; i<edges.rows(); i++){
            if(edges[i] == 1){
                int_edges_set.insert(i);
            }
        }
        bdry_only_edges.addEdges(int_edges_set);

        boundary = closure(bdry_only_edges);

    }

    return boundary;
    }


/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2013, Rice University
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Rice University nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Bryant Gipson, Mark Moll */

#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/geometric/planners/rrt/RRT.h>
#include <ompl/geometric/planners/kpiece/KPIECE1.h>
#include <ompl/geometric/planners/est/EST.h>
#include <ompl/geometric/planners/prm/PRM.h>
#include <ompl/geometric/planners/stride/STRIDE.h>
#include <ompl/tools/benchmark/Benchmark.h>

#include <boost/math/constants/constants.hpp>
#include <boost/format.hpp>
#include <fstream>

unsigned ndim = 6;
const double edgeWidth = 0.1;

// Only states near some edges of a hypercube are valid. The valid edges form a
// narrow passage from (0,...,0) to (1,...,1). A state s is valid if there exists
// a k s.t. (a) 0<=s[k]<=1, (b) for all i<k s[i]<=edgeWidth, and (c) for all i>k
// s[i]>=1-edgewidth.
bool isStateValid(const ompl::base::State *state)
{
    const ompl::base::RealVectorStateSpace::StateType *s
        = static_cast<const ompl::base::RealVectorStateSpace::StateType*>(state);
    bool foundMaxDim = false;

    for (int i = ndim - 1; i >= 0; i--)
        if (!foundMaxDim)
        {
            if ((*s)[i] > edgeWidth)
                foundMaxDim = true;
        }
        else if ((*s)[i] < (1. - edgeWidth))
            return false;
        return true;
}

void addPlanner(ompl::tools::Benchmark& benchmark, ompl::base::PlannerPtr planner, double range)
{
    ompl::base::ParamSet& params = planner->params();
    if (params.hasParam(std::string("range")))
        params.setParam(std::string("range"), boost::lexical_cast<std::string>(range));
    benchmark.addPlanner(planner);
}

int main(int argc, char **argv)
{
    if(argc > 1)
        ndim = boost::lexical_cast<size_t>(argv[1]);

    double range = edgeWidth * 0.5;
    ompl::base::StateSpacePtr space(new ompl::base::RealVectorStateSpace(ndim));
    ompl::base::RealVectorBounds bounds(ndim);
    ompl::geometric::SimpleSetup ss(space);
    ompl::base::ScopedState<> start(space), goal(space);

    bounds.setLow(0.);
    bounds.setHigh(1.);
    space->as<ompl::base::RealVectorStateSpace>()->setBounds(bounds);
    ss.setStateValidityChecker(&isStateValid);
    ss.getSpaceInformation()->setStateValidityCheckingResolution(0.001);
    for(unsigned int i = 0; i < ndim; ++i)
    {
        start[i] = 0.;
        goal[i] = 1.;
    }
    ss.setStartAndGoalStates(start, goal);

    // by default, use the Benchmark class
    double runtime_limit = 1000, memory_limit = 4096;
    int run_count = 20;
    ompl::tools::Benchmark::Request request(runtime_limit, memory_limit, run_count);
    ompl::tools::Benchmark b(ss, "HyperCube");
    b.addExperimentParameter("num_dims", "INTEGER", boost::lexical_cast<std::string>(ndim));

    addPlanner(b, ompl::base::PlannerPtr(new ompl::geometric::STRIDE(ss.getSpaceInformation())), range);
    addPlanner(b, ompl::base::PlannerPtr(new ompl::geometric::EST(ss.getSpaceInformation())), range);
    addPlanner(b, ompl::base::PlannerPtr(new ompl::geometric::KPIECE1(ss.getSpaceInformation())), range);
    addPlanner(b, ompl::base::PlannerPtr(new ompl::geometric::RRT(ss.getSpaceInformation())), range);
    addPlanner(b, ompl::base::PlannerPtr(new ompl::geometric::PRM(ss.getSpaceInformation())), range);
    b.benchmark(request);
    b.saveResultsToFile(boost::str(boost::format("hypercube_%i.log") % ndim).c_str());

    exit(0);
}


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;
int main() {
    cpp_int s, p; cin >> s >> p;
    for (cpp_int i = 1; i <= sqrt(p); i++) {
        if (p % i == 0) {
            if (i + p / i == s) {
                cout << "Yes" << endl;
                return 0;
            }
        }
    }
    cout << "No" << endl;
    return 0;
}


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2014-2015 Rokko Developers https://github.com/t-sakashita/rokko
*
* 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)
*
*****************************************************************************/

#include <rokko/utility/sort_eigenpairs.hpp>

#include <algorithm>
#include <boost/iterator/counting_iterator.hpp>

#define BOOST_TEST_MODULE test_sort_eigenpairs
#ifndef BOOST_TEST_DYN_LINK
#include <boost/test/included/unit_test.hpp>
#else
#include <boost/test/unit_test.hpp>
#endif

#define make_test(major, ascending) \
  int num = 10;\
  std::vector<int> index;\
  std::copy(boost::counting_iterator<int>(0),\
            boost::counting_iterator<int>(num),\
            back_inserter(index));\
  std::random_shuffle(index.begin(), index.end());\
  rokko::localized_vector<double> eigvals(num), eigvals_sorted(num);   \
  rokko::localized_matrix<double, major > eigvecs(num, num), eigvecs_sorted(num, num); \
  for(int i=0; i<num; ++i){\
    eigvals(i) = 1.0*index[i];\
    for(int j=0; j<num; ++j){\
      if(eigvecs.is_row_major()){\
        eigvecs(i,j) = eigvals(i);\
      }else{\
        eigvecs(j,i) = eigvals(i);\
      }\
    }\
  }\
  rokko::sort_eigenpairs(eigvals, eigvecs, eigvals_sorted, eigvecs_sorted, ascending);\
  for(int i=0; i<num; ++i){\
    std::cout << "dim: " << i << std::endl;\
    double e = ascending ? 1.0 * i : 1.0*(num-i-1);\
    if(eigvecs.is_row_major()){\
      BOOST_CHECK_EQUAL( eigvals_sorted(i), e);\
      BOOST_CHECK_EQUAL( eigvecs_sorted(i,0), e);\
      BOOST_CHECK_EQUAL( eigvecs_sorted(i,1), e);\
    }else{\
      BOOST_CHECK_EQUAL( eigvals_sorted(i), e);\
      BOOST_CHECK_EQUAL( eigvecs_sorted(0,i), e);\
      BOOST_CHECK_EQUAL( eigvecs_sorted(1,i), e);\
    }\
  }

BOOST_AUTO_TEST_CASE(test_sort_eigenpairs) {
  {
    std::cout << "row_major, ascending order\n";
    make_test(rokko::matrix_row_major, true)
  }
  {
    std::cout << "row_major, descending order\n";
    make_test(rokko::matrix_row_major, false)
  }
  {
    std::cout << "col_major, ascending order\n";
    make_test(rokko::matrix_col_major, true)
  }
  {
    std::cout << "col_major, descending order\n";
    make_test(rokko::matrix_col_major, false)
  }
}

#undef make_test


// Copyright John Maddock 2006, 2007.
// Copyright Paul A. Bristow 2007.

//  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_STATS_CAUCHY_HPP
#define BOOST_STATS_CAUCHY_HPP

#ifdef _MSC_VER
#pragma warning(push)
#pragma warning(disable : 4127) // conditional expression is constant
#endif

#include <boost/math/distributions/fwd.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/distributions/complement.hpp>
#include <boost/math/distributions/detail/common_error_handling.hpp>
#include <boost/config/no_tr1/cmath.hpp>

#include <utility>

namespace boost{ namespace math
{

template <class RealType, class Policy>
class cauchy_distribution;

namespace detail
{

template <class RealType, class Policy>
RealType cdf_imp(const cauchy_distribution<RealType, Policy>& dist, const RealType& x, bool complement)
{
   //
   // This calculates the cdf of the Cauchy distribution and/or its complement.
   //
   // The usual formula for the Cauchy cdf is:
   //
   // cdf = 0.5 + atan(x)/pi
   //
   // But that suffers from cancellation error as x -> -INF.
   //
   // Recall that for x < 0:
   //
   // atan(x) = -pi/2 - atan(1/x)
   //
   // Substituting into the above we get:
   //
   // CDF = -atan(1/x)  ; x < 0
   //
   // So the proceedure is to calculate the cdf for -fabs(x)
   // using the above formula, and then subtract from 1 when required
   // to get the result.
   //
   BOOST_MATH_STD_USING // for ADL of std functions
   static const char* function = "boost::math::cdf(cauchy<%1%>&, %1%)";
   RealType result = 0;
   RealType location = dist.location();
   RealType scale = dist.scale();
   if(false == detail::check_location(function, location, &result, Policy()))
   {
     return result;
   }
   if(false == detail::check_scale(function, scale, &result, Policy()))
   {
      return result;
   }
   if(std::numeric_limits<RealType>::has_infinity && x == std::numeric_limits<RealType>::infinity())
   { // cdf +infinity is unity.
     return static_cast<RealType>((complement) ? 0 : 1);
   }
   if(std::numeric_limits<RealType>::has_infinity && x == -std::numeric_limits<RealType>::infinity())
   { // cdf -infinity is zero.
     return static_cast<RealType>((complement) ? 1 : 0);
   }
   if(false == detail::check_x(function, x, &result, Policy()))
   { // Catches x == NaN
      return result;
   }
   RealType mx = -fabs((x - location) / scale); // scale is > 0
   if(mx > -tools::epsilon<RealType>() / 8)
   {  // special case first: x extremely close to location.
      return 0.5;
   }
   result = -atan(1 / mx) / constants::pi<RealType>();
   return (((x > location) != complement) ? 1 - result : result);
} // cdf

template <class RealType, class Policy>
RealType quantile_imp(
      const cauchy_distribution<RealType, Policy>& dist,
      const RealType& p,
      bool complement)
{
   // This routine implements the quantile for the Cauchy distribution,
   // the value p may be the probability, or its complement if complement=true.
   //
   // The procedure first performs argument reduction on p to avoid error
   // when calculating the tangent, then calulates the distance from the
   // mid-point of the distribution.  This is either added or subtracted
   // from the location parameter depending on whether `complement` is true.
   //
   static const char* function = "boost::math::quantile(cauchy<%1%>&, %1%)";
   BOOST_MATH_STD_USING // for ADL of std functions

   RealType result = 0;
   RealType location = dist.location();
   RealType scale = dist.scale();
   if(false == detail::check_location(function, location, &result, Policy()))
   {
     return result;
   }
   if(false == detail::check_scale(function, scale, &result, Policy()))
   {
      return result;
   }
   if(false == detail::check_probability(function, p, &result, Policy()))
   {
      return result;
   }
   // Special cases:
   if(p == 1)
   {
      return (complement ? -1 : 1) * policies::raise_overflow_error<RealType>(function, 0, Policy());
   }
   if(p == 0)
   {
      return (complement ? 1 : -1) * policies::raise_overflow_error<RealType>(function, 0, Policy());
   }

   RealType P = p - floor(p);   // argument reduction of p:
   if(P > 0.5)
   {
      P = P - 1;
   }
   if(P == 0.5)   // special case:
   {
      return location;
   }
   result = -scale / tan(constants::pi<RealType>() * P);
   return complement ? RealType(location - result) : RealType(location + result);
} // quantile

} // namespace detail

template <class RealType = double, class Policy = policies::policy<> >
class cauchy_distribution
{
public:
   typedef RealType value_type;
   typedef Policy policy_type;

   cauchy_distribution(RealType location = 0, RealType scale = 1)
      : m_a(location), m_hg(scale)
   {
    static const char* function = "boost::math::cauchy_distribution<%1%>::cauchy_distribution";
     RealType result;
     detail::check_location(function, location, &result, Policy());
     detail::check_scale(function, scale, &result, Policy());
   } // cauchy_distribution

   RealType location()const
   {
      return m_a;
   }
   RealType scale()const
   {
      return m_hg;
   }

private:
   RealType m_a;    // The location, this is the median of the distribution.
   RealType m_hg;   // The scale )or shape), this is the half width at half height.
};

typedef cauchy_distribution<double> cauchy;

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> range(const cauchy_distribution<RealType, Policy>&)
{ // Range of permissible values for random variable x.
  if (std::numeric_limits<RealType>::has_infinity)
  {
     return std::pair<RealType, RealType>(-std::numeric_limits<RealType>::infinity(), std::numeric_limits<RealType>::infinity()); // - to + infinity.
  }
  else
  { // Can only use max_value.
   using boost::math::tools::max_value;
   return std::pair<RealType, RealType>(-max_value<RealType>(), max_value<RealType>()); // - to + max.
  }
}

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> support(const cauchy_distribution<RealType, Policy>& )
{ // Range of supported values for random variable x.
   // This is range where cdf rises from 0 to 1, and outside it, the pdf is zero.
  if (std::numeric_limits<RealType>::has_infinity)
  {
     return std::pair<RealType, RealType>(-std::numeric_limits<RealType>::infinity(), std::numeric_limits<RealType>::infinity()); // - to + infinity.
  }
  else
  { // Can only use max_value.
     using boost::math::tools::max_value;
     return std::pair<RealType, RealType>(-tools::max_value<RealType>(), max_value<RealType>()); // - to + max.
  }
}

template <class RealType, class Policy>
inline RealType pdf(const cauchy_distribution<RealType, Policy>& dist, const RealType& x)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

   static const char* function = "boost::math::pdf(cauchy<%1%>&, %1%)";
   RealType result = 0;
   RealType location = dist.location();
   RealType scale = dist.scale();
   if(false == detail::check_scale("boost::math::pdf(cauchy<%1%>&, %1%)", scale, &result, Policy()))
   {
      return result;
   }
   if(false == detail::check_location("boost::math::pdf(cauchy<%1%>&, %1%)", location, &result, Policy()))
   {
      return result;
   }
   if((boost::math::isinf)(x))
   {
     return 0; // pdf + and - infinity is zero.
   }
   // These produce MSVC 4127 warnings, so the above used instead.
   //if(std::numeric_limits<RealType>::has_infinity && abs(x) == std::numeric_limits<RealType>::infinity())
   //{ // pdf + and - infinity is zero.
   //  return 0;
   //}

   if(false == detail::check_x(function, x, &result, Policy()))
   { // Catches x = NaN
      return result;
   }

   RealType xs = (x - location) / scale;
   result = 1 / (constants::pi<RealType>() * scale * (1 + xs * xs));
   return result;
} // pdf

template <class RealType, class Policy>
inline RealType cdf(const cauchy_distribution<RealType, Policy>& dist, const RealType& x)
{
   return detail::cdf_imp(dist, x, false);
} // cdf

template <class RealType, class Policy>
inline RealType quantile(const cauchy_distribution<RealType, Policy>& dist, const RealType& p)
{
   return detail::quantile_imp(dist, p, false);
} // quantile

template <class RealType, class Policy>
inline RealType cdf(const complemented2_type<cauchy_distribution<RealType, Policy>, RealType>& c)
{
   return detail::cdf_imp(c.dist, c.param, true);
} //  cdf complement

template <class RealType, class Policy>
inline RealType quantile(const complemented2_type<cauchy_distribution<RealType, Policy>, RealType>& c)
{
   return detail::quantile_imp(c.dist, c.param, true);
} // quantile complement

template <class RealType, class Policy>
inline RealType mean(const cauchy_distribution<RealType, Policy>&)
{  // There is no mean:
   typedef typename Policy::assert_undefined_type assert_type;
   BOOST_STATIC_ASSERT(assert_type::value == 0);

   return policies::raise_domain_error<RealType>(
      "boost::math::mean(cauchy<%1%>&)",
      "The Cauchy distribution does not have a mean: "
      "the only possible return value is %1%.",
      std::numeric_limits<RealType>::quiet_NaN(), Policy());
}

template <class RealType, class Policy>
inline RealType variance(const cauchy_distribution<RealType, Policy>& /*dist*/)
{
   // There is no variance:
   typedef typename Policy::assert_undefined_type assert_type;
   BOOST_STATIC_ASSERT(assert_type::value == 0);

   return policies::raise_domain_error<RealType>(
      "boost::math::variance(cauchy<%1%>&)",
      "The Cauchy distribution does not have a variance: "
      "the only possible return value is %1%.",
      std::numeric_limits<RealType>::quiet_NaN(), Policy());
}

template <class RealType, class Policy>
inline RealType mode(const cauchy_distribution<RealType, Policy>& dist)
{
   return dist.location();
}

template <class RealType, class Policy>
inline RealType median(const cauchy_distribution<RealType, Policy>& dist)
{
   return dist.location();
}
template <class RealType, class Policy>
inline RealType skewness(const cauchy_distribution<RealType, Policy>& /*dist*/)
{
   // There is no skewness:
   typedef typename Policy::assert_undefined_type assert_type;
   BOOST_STATIC_ASSERT(assert_type::value == 0);

   return policies::raise_domain_error<RealType>(
      "boost::math::skewness(cauchy<%1%>&)",
      "The Cauchy distribution does not have a skewness: "
      "the only possible return value is %1%.",
      std::numeric_limits<RealType>::quiet_NaN(), Policy()); // infinity?
}

template <class RealType, class Policy>
inline RealType kurtosis(const cauchy_distribution<RealType, Policy>& /*dist*/)
{
   // There is no kurtosis:
   typedef typename Policy::assert_undefined_type assert_type;
   BOOST_STATIC_ASSERT(assert_type::value == 0);

   return policies::raise_domain_error<RealType>(
      "boost::math::kurtosis(cauchy<%1%>&)",
      "The Cauchy distribution does not have a kurtosis: "
      "the only possible return value is %1%.",
      std::numeric_limits<RealType>::quiet_NaN(), Policy());
}

template <class RealType, class Policy>
inline RealType kurtosis_excess(const cauchy_distribution<RealType, Policy>& /*dist*/)
{
   // There is no kurtosis excess:
   typedef typename Policy::assert_undefined_type assert_type;
   BOOST_STATIC_ASSERT(assert_type::value == 0);

   return policies::raise_domain_error<RealType>(
      "boost::math::kurtosis_excess(cauchy<%1%>&)",
      "The Cauchy distribution does not have a kurtosis: "
      "the only possible return value is %1%.",
      std::numeric_limits<RealType>::quiet_NaN(), Policy());
}

} // namespace math
} // namespace boost

#ifdef _MSC_VER
#pragma warning(pop)
#endif

// This include must be at the end, *after* the accessors
// for this distribution have been defined, in order to
// keep compilers that support two-phase lookup happy.
#include <boost/math/distributions/detail/derived_accessors.hpp>

#endif // BOOST_STATS_CAUCHY_HPP


//
// Created by mathis on 28/02/2020.
//

#ifndef DAFT_ENGINE_ADAPTERS_HPP
#define DAFT_ENGINE_ADAPTERS_HPP

//#include <Eigen/Eigen>
#include <Eigen/Eigen>
#include <glm/glm.hpp>

template <int m, int n>
inline glm::mat<m, n, float> toGlm( const Eigen::Matrix<float, m, n>& mat ) {
    glm::mat<m, n, float> ret;
    for ( int i = 0; i < m; ++i )
    {
        for ( int j = 0; j < n; ++j )
        {
            ret[j][i] = mat( i, j );
        }
    }
    return ret;
}

template <int m>
inline glm::vec<m, float> toGlm( const Eigen::Vector<float, m>& vec ) {
    glm::vec<m, float> ret;
    for ( int i = 0; i < m; ++i )
    {
        ret[i] = vec( i );
    }
    return ret;
}

template <int m, int n>
inline Eigen::Matrix<float, m, n> toEigen( const glm::mat<m, n, float>& mat ) {
    Eigen::Matrix<float, m, n> ret;
    for ( int i = 0; i < m; ++i )
    {
        for ( int j = 0; j < n; ++j )
        {
            ret( i, j ) = mat[j][i];
        }
    }
    return ret;
}

template <int m>
inline Eigen::Vector<float, m> toEigen( const glm::vec<m, float>& vec ) {
    Eigen::Vector<float, m> ret;
    for ( int i = 0; i < m; ++i )
    {
        ret( i ) = vec[i];
    }
    return ret;
}

#endif // DAFT_ENGINE_ADAPTERS_HPP


/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2020 Scientific Computing and Imaging Institute,
   University of Utah.

   Permission is hereby granted, free of charge, to any person obtaining a
   copy of this software and associated documentation files (the "Software"),
   to deal in the Software without restriction, including without limitation
   the rights to use, copy, modify, merge, publish, distribute, sublicense,
   and/or sell copies of the Software, and to permit persons to whom the
   Software is furnished to do so, subject to the following conditions:

   The above copyright notice and this permission notice shall be included
   in all copies or substantial portions of the Software.

   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
   OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
   THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
   FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
   DEALINGS IN THE SOFTWARE.
*/

#include <gtest/gtest.h>
#include <gmock/gmock.h>

#include <fstream>
#include <Core/Algorithms/Math/ParallelAlgebra/ParallelLinearAlgebra.h>
#include <Core/Datatypes/DenseMatrix.h>
#include <Core/Datatypes/DenseColumnMatrix.h>
#include <Core/Datatypes/SparseRowMatrix.h>
#include <Core/Datatypes/MatrixComparison.h>
#include <Core/Datatypes/MatrixTypeConversions.h>
#include <Core/Datatypes/MatrixIO.h>
#include <Testing/Utils/MatrixTestUtilities.h>
#include <boost/thread/thread.hpp>

using namespace SCIRun::Core::Datatypes;
using namespace SCIRun::Core::Algorithms::Math;
using namespace SCIRun::Core::Algorithms;
using namespace SCIRun::TestUtils;
using namespace SCIRun;
using namespace ::testing;

namespace
{
  const int size = 1000;
  SparseRowMatrixHandle matrix1()
  {
    SparseRowMatrixHandle m(boost::make_shared<SparseRowMatrix>(size,size));
    m->insert(0,0) = 1;
    m->insert(1,2) = -1;
    m->insert(size-1,size-1) = 2;
    return m;
  }

  DenseColumnMatrixHandle vector1()
  {
    DenseColumnMatrixHandle v(boost::make_shared<DenseColumnMatrix>(size));
	  v->setZero();
//    *v << 1, 2, 4;
	  (*v)[0] = 1;
  	(*v)[1] = 2;
	  (*v)[2] = 4;
    (*v)[size-1] = -1;
    return v;
  }

  DenseColumnMatrixHandle vector2()
  {
    DenseColumnMatrixHandle v(boost::make_shared<DenseColumnMatrix>(size));
    v->setZero();
    //*v << -1, -2, -4;
	  (*v)[0] = -1;
	  (*v)[1] = -2;
	  (*v)[2] = -4;
    (*v)[300] = -300;
    (*v)[size/2] = -6;
    (*v)[size/2 + 1] = -7;
    (*v)[600] = -600;
    (*v)[size-1] = 1;
    return v;
  }

  DenseColumnMatrixHandle vector3()
  {
    DenseColumnMatrixHandle v(boost::make_shared<DenseColumnMatrix>(size));
    v->setZero();
   // *v << 0, 1, 0;
	  (*v)[0] = 0;
	  (*v)[1] = 1;
	  (*v)[2] = 0;
    (*v)[size-1] = -7;
    return v;
  }

  SolverInputs getDummySystem()
  {
    SolverInputs system;
    system.A = matrix1();
    system.b = vector1();
    system.x = vector2();
    system.x0 = vector3();
    return system;
  }

  const int SINGLE_THREADED_TEST_NUMPROCS = 1;
  const int SINGLE_THREADED_TEST_PROC_INDEX = 0;
}

TEST(ParallelLinearAlgebraTests, CanCreateEmptyParallelVector)
{
  ParallelLinearAlgebraSharedData data(getDummySystem(), SINGLE_THREADED_TEST_NUMPROCS);
  ParallelLinearAlgebra pla(data, SINGLE_THREADED_TEST_PROC_INDEX);

  ParallelLinearAlgebra::ParallelVector v;
  EXPECT_TRUE(pla.new_vector(v));

  EXPECT_EQ(size, v.size_);
}

TEST(ParallelLinearAlgebraTests, CanCreateParallelVectorFromVectorAsShallowReference)
{
  ParallelLinearAlgebraSharedData data(getDummySystem(), 1);
  ParallelLinearAlgebra pla(data, 0);

  ParallelLinearAlgebra::ParallelVector v;
  auto v1 = vector1();
  EXPECT_TRUE(pla.add_vector(v1, v));

  EXPECT_EQ(v1->nrows(), v.size_);
  for (size_t i = 0; i < size; ++i)
    EXPECT_EQ((*v1)[i], v.data_[i]);

  EXPECT_EQ(0, (*v1)[100]);
  v.data_[100]++;
  EXPECT_EQ(1, (*v1)[100]);
}

TEST(ParallelLinearAlgebraTests, CanCopyParallelSparseMatrixAsShallowReference)
{
  ParallelLinearAlgebraSharedData data(getDummySystem(), 1);
  ParallelLinearAlgebra pla(data, 0);

  ParallelLinearAlgebra::ParallelMatrix m;
  auto m1 = matrix1();
  EXPECT_TRUE(pla.add_matrix(m1, m));

  EXPECT_EQ(m1->nrows(), m.m_);
  EXPECT_EQ(m1->ncols(), m.n_);
  EXPECT_EQ(m1->nonZeros(), m.nnz_);
  EXPECT_EQ(m1->coeff(1,2), m.data_[1]);

  EXPECT_EQ(1, m1->coeff(0,0));
  m.data_[0]++;
  EXPECT_EQ(2, m1->coeff(0,0));
}

TEST(ParallelLinearAlgebraTests, CanCopyContentsOfVector)
{
  ParallelLinearAlgebraSharedData data(getDummySystem(), 1);
  ParallelLinearAlgebra pla(data, 0);

  ParallelLinearAlgebra::ParallelVector v1;
  pla.new_vector(v1);

  ParallelLinearAlgebra::ParallelVector v2;
  auto vec2 = vector2();
  pla.add_vector(vec2, v2);

  pla.copy(v2, v1);

  EXPECT_EQ(v1.size_, v2.size_);
  for (size_t i = 0; i < size; ++i)
  {
    EXPECT_EQ(v2.data_[i], v1.data_[i]);
  }
  v1.data_[7]++;
  EXPECT_NE(v1.data_[7], v2.data_[7]);
}

struct Copy
{
  Copy(ParallelLinearAlgebraSharedData& data, ParallelLinearAlgebra::ParallelVector& v1,
    ParallelLinearAlgebra::ParallelVector& v2, int proc, DenseColumnMatrixHandle vec2copy) :
    data_(data), proc_(proc), v1_(v1), v2_(v2), vec2copy_(vec2copy) {}

  ParallelLinearAlgebraSharedData& data_;
  int proc_;
  ParallelLinearAlgebra::ParallelVector& v1_;
  ParallelLinearAlgebra::ParallelVector& v2_;
  DenseColumnMatrixHandle vec2copy_;

  void operator()()
  {
    ParallelLinearAlgebra pla(data_, proc_);
    pla.new_vector(v1_);
    pla.add_vector(vec2copy_, v2_);
    pla.copy(v2_, v1_);
  }
};

TEST(ParallelLinearAlgebraTests, CanCopyContentsOfVectorMulti)
{
	const int NUM_THREADS = 2;
  ParallelLinearAlgebraSharedData data(getDummySystem(), NUM_THREADS);

  ParallelLinearAlgebra::ParallelVector v1, v2;

  auto vec2copy = vector2();
  {
	  Copy c0(data, v1, v2, 0, vec2copy);
	  Copy c1(data, v1, v2, 1, vec2copy);

	  boost::thread t1 = boost::thread(boost::ref(c0));
	  boost::thread t2 = boost::thread(boost::ref(c1));
	  t1.join();
	  t2.join();
  }

  EXPECT_EQ(v1.size_, v2.size_);
  for (size_t i = 0; i < size; ++i)
  {
    EXPECT_EQ(v2.data_[i], v1.data_[i]);
  }
  v1.data_[7]++;
  EXPECT_NE(v1.data_[7], v2.data_[7]);
}

TEST(ParallelArithmeticTests, CanComputeMaxOfVector)
{
  ParallelLinearAlgebraSharedData data(getDummySystem(), SINGLE_THREADED_TEST_NUMPROCS);
  ParallelLinearAlgebra pla(data, SINGLE_THREADED_TEST_PROC_INDEX);

  ParallelLinearAlgebra::ParallelVector v1;
  auto vec1 = vector1();
  pla.add_vector(vec1, v1);
  double max1 = pla.max(v1);
  ParallelLinearAlgebra::ParallelVector v2;
  auto vec2 = vector2();
  pla.add_vector(vec2, v2);
  double max2 = pla.max(v2);

  EXPECT_EQ(4, max1);
  EXPECT_EQ(1, max2);
}

TEST(ParallelArithmeticTests, CanTakeAbsoluteValueOfDiagonal)
{
  ParallelLinearAlgebraSharedData data(getDummySystem(),1);
  ParallelLinearAlgebra pla(data,0);

  ParallelLinearAlgebra::ParallelVector v1;
  auto vec1 = vector1();
  vec1->setZero();
  pla.add_vector(vec1, v1);

  ParallelLinearAlgebra::ParallelMatrix m1;
  auto mat1 = matrix1();
  pla.add_matrix(mat1 , m1);

  pla.absdiag(m1, v1);
  for (size_t i = 0; i < size; ++i)
  {
	  EXPECT_GE(v1.data_[i],0);
  }
  EXPECT_EQ(1,v1.data_[0]);
  EXPECT_EQ(0,v1.data_[2]);
  EXPECT_EQ(2,v1.data_[size-1]);
}

struct absdiag
{
   absdiag(ParallelLinearAlgebraSharedData& data, ParallelLinearAlgebra::ParallelMatrix& m1,
     ParallelLinearAlgebra::ParallelVector& v2, int proc, DenseColumnMatrixHandle dcmHandle,
     SparseRowMatrixHandle srmHandle) : data_(data), proc_(proc), m1_(m1), v2_(v2),
     dcmHandle_(dcmHandle), srmHandle_(srmHandle) {}

  ParallelLinearAlgebraSharedData& data_;
  int proc_;
  ParallelLinearAlgebra::ParallelMatrix& m1_;
  ParallelLinearAlgebra::ParallelVector& v2_;
  DenseColumnMatrixHandle dcmHandle_;
  SparseRowMatrixHandle srmHandle_;

  void operator()()
  {
    ParallelLinearAlgebra pla(data_, proc_);
    pla.add_matrix(srmHandle_, m1_);

    pla.new_vector(v2_);
    pla.add_vector(dcmHandle_, v2_);

    pla.absdiag(m1_, v2_);
  }
};

TEST(ParallelArithmeticTests, CanTakeAbsoluteValueOfDiagonalMulti)
{
  const int NUM_THREADS = 2;
  ParallelLinearAlgebraSharedData data(getDummySystem(), NUM_THREADS);

  ParallelLinearAlgebra::ParallelVector v2;
  ParallelLinearAlgebra::ParallelMatrix m1;

  auto vec2 = vector2();
  auto mat1 = matrix1();

  {
	  absdiag diag0(data, m1, v2, 0, vec2, mat1);
	  absdiag diag1(data, m1, v2, 1, vec2, mat1);

	  boost::thread t1 = boost::thread(boost::ref(diag0));
	  boost::thread t2 = boost::thread(boost::ref(diag1));
	  t1.join();
	  t2.join();
  }

  EXPECT_EQ( v2.size_, size );
  for (size_t i = 0; i < size; ++i)
  {
    EXPECT_GE(v2.data_[i] , 0);
  }
}

struct max
{
  max(ParallelLinearAlgebraSharedData& data, ParallelLinearAlgebra::ParallelVector& v1,
     int proc, DenseColumnMatrixHandle dcmHandle) :
      data_(data), proc_(proc), v1_(v1), dcmHandle_(dcmHandle) {}

  ParallelLinearAlgebraSharedData& data_;
  int proc_;
  ParallelLinearAlgebra::ParallelVector& v1_;
  DenseColumnMatrixHandle dcmHandle_;
  double maxResult_{0};

  void operator()()
  {
    ParallelLinearAlgebra pla(data_, proc_);
    pla.new_vector(v1_);
    pla.add_vector(dcmHandle_, v1_);
    maxResult_ = pla.max(v1_);
  }
};

TEST(ParallelArithmeticTests, CanComputeMaxOfVectorMulti)
{
  /// @todo: multi thread
  ParallelLinearAlgebraSharedData data(getDummySystem(), 2);

  ParallelLinearAlgebra::ParallelVector v1;
  double maxDouble_;

  auto vec2 = vector2();
  {
	  max max0(data, v1, 0, vec2);
	  max max1(data, v1, 1, vec2);

	  boost::thread t1 = boost::thread(boost::ref(max0));
	  boost::thread t2 = boost::thread(boost::ref(max1));
	  t1.join();
	  t2.join();
    maxDouble_ = max0.maxResult_;
  }
  EXPECT_EQ(1, maxDouble_);
}

//Find what error is acceptable for the float comparison
TEST(ParallelArithmeticTests, CanInvertElementsOfVectorWithAbsoluteValueThreshold)
{
	ParallelLinearAlgebraSharedData data(getDummySystem(),1);
	ParallelLinearAlgebra pla(data, 0);

  DenseColumnMatrixHandle v(boost::make_shared<DenseColumnMatrix>(size));
	v->setZero();

  ParallelLinearAlgebra::ParallelVector dummyResult;
	auto dcmDummy = v;
	pla.add_vector(dcmDummy, dummyResult);

  //test vector 1
  ParallelLinearAlgebra::ParallelVector v1;
	auto vec1 = vector1();
	pla.add_vector(vec1, v1);
  pla.absthreshold_invert(v1, dummyResult, 1);
	EXPECT_EQ(dummyResult.data_[0],1);
	EXPECT_DOUBLE_EQ(dummyResult.data_[1], 0.5);
	EXPECT_DOUBLE_EQ(dummyResult.data_[2], 0.25);
	EXPECT_EQ(dummyResult.data_[size-1], 1);

  //test vector 2
  pla.zeros(dummyResult);
	ParallelLinearAlgebra::ParallelVector v2;
	auto vec2 = vector2();
	pla.add_vector(vec2, v2);
  pla.absthreshold_invert(v2, dummyResult, 1);

  EXPECT_EQ(1,dummyResult.data_[0]);
	EXPECT_DOUBLE_EQ(-0.5,dummyResult.data_[1]);
	EXPECT_DOUBLE_EQ(-0.25,dummyResult.data_[2]);
	EXPECT_EQ(1,dummyResult.data_[size-1]);

  //test vector 3
  pla.zeros(dummyResult);
  ParallelLinearAlgebra::ParallelVector v3;
	auto vec3 = vector3();
	pla.add_vector(vec3, v3);
  pla.absthreshold_invert(v3, dummyResult, 1);

  EXPECT_EQ(1,dummyResult.data_[0]);
	EXPECT_EQ(1,dummyResult.data_[1]);
	EXPECT_EQ(1,dummyResult.data_[2]);
	EXPECT_NEAR(-0.1429, dummyResult.data_[size-1], 0.001);
}

struct absthreshold_inv
{
   absthreshold_inv(ParallelLinearAlgebraSharedData& data, ParallelLinearAlgebra::ParallelVector& v1,
     ParallelLinearAlgebra::ParallelVector& v2, int proc, DenseColumnMatrixHandle dcmHandle) :
    data_(data), proc_(proc), v1_(v1), v2_(v2), dcmHandle_(dcmHandle) {}

  ParallelLinearAlgebraSharedData& data_;
  int proc_;
  ParallelLinearAlgebra::ParallelVector& v1_;
  ParallelLinearAlgebra::ParallelVector& v2_;
  DenseColumnMatrixHandle dcmHandle_;

  void operator()()
  {
    ParallelLinearAlgebra pla(data_, proc_);

    pla.new_vector(v1_);
    pla.add_vector(dcmHandle_, v1_);

    pla.new_vector(v2_);

    pla.absthreshold_invert(v1_, v2_, 1);
  }
};

TEST(ParallelArithmeticTests, CanInvertElementsOfVectorWithAbsoluteValueThresholdMulti)
{
  const int NUM_THREADS = 2;
  ParallelLinearAlgebraSharedData data(getDummySystem(), NUM_THREADS);

  ParallelLinearAlgebra::ParallelVector v1;
  ParallelLinearAlgebra::ParallelVector v2;

  auto vec2 = vector2();
  {
	  absthreshold_inv absthreshold_inv0(data, v1, v2, 0, vec2);
	  absthreshold_inv absthreshold_inv1(data, v1, v2, 1, vec2);

	  boost::thread t1 = boost::thread(boost::ref(absthreshold_inv0));
	  boost::thread t2 = boost::thread(boost::ref(absthreshold_inv1));
	  t1.join();
	  t2.join();
  }

  //test vector 1
	EXPECT_EQ(1,v2.data_[0]);
	EXPECT_NEAR(-0.5,v2.data_[1],0.001);
	EXPECT_NEAR(-0.25,v2.data_[2],0.001);
  EXPECT_NEAR(-0.0033,v2.data_[300],0.001);
  EXPECT_NEAR(-0.1667,v2.data_[size/2],0.001);
  EXPECT_NEAR(-0.1429,v2.data_[size/2 + 1],0.001);
  EXPECT_NEAR(-0.0017,v2.data_[600],0.001);
	EXPECT_EQ(1,v2.data_[size-1]);

}

TEST(ParallelArithmeticTests, CanInvertElementsOfVectorWithAbsoluteValueThresholdMulti8Threads)
{
  const int NUM_THREADS = 8;
  ParallelLinearAlgebraSharedData data(getDummySystem(), NUM_THREADS);

  ParallelLinearAlgebra::ParallelVector v1;
  ParallelLinearAlgebra::ParallelVector v2;

  auto vec2 = vector1();
  std::vector<boost::shared_ptr<absthreshold_inv>> workers;
  boost::thread_group threads;
  {
    for (int i = 0; i < NUM_THREADS; ++i)
      workers.push_back(boost::make_shared<absthreshold_inv>(data, v1, v2, i, vec2));

    for (int i = 0; i < NUM_THREADS; ++i)
      threads.create_thread(boost::ref(*workers[i]));

    threads.join_all();
  }

  //test vector 1
	EXPECT_EQ(1, v2.data_[0]);
	EXPECT_DOUBLE_EQ(0.5, v2.data_[1]);
	EXPECT_DOUBLE_EQ(0.25,v2.data_[2] );
  EXPECT_DOUBLE_EQ(1, v2.data_[300]);
  EXPECT_DOUBLE_EQ(1, v2.data_[size/2]);
  EXPECT_DOUBLE_EQ(1, v2.data_[size/2 + 1]);
  EXPECT_DOUBLE_EQ(1, v2.data_[600]);
	EXPECT_EQ(1, v2.data_[size-1]);

}

TEST(ParallelLinearAlgebraTests, CanFillVectorWithOnes)
{
  ParallelLinearAlgebraSharedData data(getDummySystem(),1);
  ParallelLinearAlgebra pla(data,0);

  ParallelLinearAlgebra::ParallelVector v1;
  auto vec1 = vector1();
  pla.add_vector(vec1, v1);

  pla.ones(v1);

  for (size_t i = 0; i < size; ++i)
  {
    EXPECT_EQ(1,v1.data_[i]);
  }
}

TEST(ParallelArithmeticTests, CanMultiplyMatrixByVector)
{
  ParallelLinearAlgebraSharedData data(getDummySystem(),1);
  ParallelLinearAlgebra pla(data,0);

  ParallelLinearAlgebra::ParallelVector v1;
  auto vec1 = vector1();
  pla.add_vector(vec1, v1);

  ParallelLinearAlgebra::ParallelVector v2;
  auto vec2 = vector2();
  pla.add_vector(vec2, v2);
  pla.zeros(v2);

  ParallelLinearAlgebra::ParallelMatrix m1;
  auto mat1 = matrix1();
  pla.add_matrix(mat1 , m1);

  pla.mult(m1,v1,v2);

  EXPECT_EQ(1,v2.data_[0]);
  EXPECT_EQ(-4,v2.data_[1]);
  EXPECT_EQ(-2,v2.data_[size-1]);
}

struct mv_Multiply
{
  mv_Multiply(ParallelLinearAlgebraSharedData& data, ParallelLinearAlgebra::ParallelMatrix& m1,
    ParallelLinearAlgebra::ParallelVector& v2, ParallelLinearAlgebra::ParallelVector& vR,
    int proc, DenseColumnMatrixHandle dcmHandle, SparseRowMatrixHandle srmHandle) :
    data_(data), proc_(proc), m1_(m1), v2_(v2), vR_(vR), dcmHandle_(dcmHandle), srmHandle_(srmHandle) {}

  ParallelLinearAlgebraSharedData& data_;
  int proc_;
  ParallelLinearAlgebra::ParallelMatrix& m1_;
  ParallelLinearAlgebra::ParallelVector& v2_;
  ParallelLinearAlgebra::ParallelVector& vR_;
  DenseColumnMatrixHandle dcmHandle_;
  SparseRowMatrixHandle srmHandle_;

  void operator()()
  {
    ParallelLinearAlgebra pla(data_, proc_);

    pla.add_matrix(srmHandle_, m1_);

    pla.new_vector(v2_);
    pla.add_vector(dcmHandle_,v2_);

    pla.new_vector(vR_);
    pla.add_vector(dcmHandle_,vR_);

    pla.mult(m1_, v2_, vR_);
  }
};

TEST(ParallelArithmeticTests, CanMultiplyMatrixByVectorMulti)
{
  ParallelLinearAlgebraSharedData data(getDummySystem(), 2);

  ParallelLinearAlgebra::ParallelMatrix m1;
  ParallelLinearAlgebra::ParallelVector v2;
  ParallelLinearAlgebra::ParallelVector vR;

  auto vecR = vector2();
  auto mat1 = matrix1();
  {
	  mv_Multiply mult_0(data, m1, v2, vR, 0, vecR, mat1);
	  mv_Multiply mult_1(data, m1, v2, vR, 1, vecR, mat1);

	  boost::thread t1 = boost::thread(boost::ref(mult_0));
	  boost::thread t2 = boost::thread(boost::ref(mult_1));
	  t1.join();
	  t2.join();
  }

  EXPECT_EQ(-1,vR.data_[0]);
  EXPECT_EQ(4,vR.data_[1]);
  EXPECT_EQ(0,vR.data_[2]);
  EXPECT_EQ(2,vR.data_[size-1]);
}

TEST(ParallelArithmeticTests, CanSubtractVectors)
{
  ParallelLinearAlgebraSharedData data(getDummySystem(),1);
  ParallelLinearAlgebra pla(data,0);

  ParallelLinearAlgebra::ParallelVector v1;
  auto vec1 = vector1();
  pla.add_vector(vec1, v1);

  ParallelLinearAlgebra::ParallelVector v2;
  auto vec2 = vector2();
  pla.add_vector(vec2, v2);

  ParallelLinearAlgebra::ParallelVector v3;
  auto vec3 = vector3();
  pla.add_vector(vec3, v3);
  pla.zeros(v3);

  pla.sub(v1, v2, v3);
  EXPECT_EQ(v3.data_[0],2);
  EXPECT_EQ(v3.data_[1],4);
  EXPECT_EQ(v3.data_[2],8);
  EXPECT_EQ(v3.data_[3],0);
  EXPECT_EQ(v3.data_[size-1],-2);
}

struct subtract
{
  subtract(ParallelLinearAlgebraSharedData& data, ParallelLinearAlgebra::ParallelVector& v1,
    ParallelLinearAlgebra::ParallelVector& v2, ParallelLinearAlgebra::ParallelVector& vR,
    int proc, DenseColumnMatrixHandle dcmHandle, DenseColumnMatrixHandle dcmHandle2) :
    data_(data), proc_(proc), v1_(v1), v2_(v2), vR_(vR), dcmHandle_(dcmHandle), dcmHandle2_(dcmHandle2) {}

  ParallelLinearAlgebraSharedData& data_;
  int proc_;
  ParallelLinearAlgebra::ParallelVector& v1_;
  ParallelLinearAlgebra::ParallelVector& v2_;
  ParallelLinearAlgebra::ParallelVector& vR_;
  DenseColumnMatrixHandle dcmHandle_;
  DenseColumnMatrixHandle dcmHandle2_;

  void operator()()
  {
    ParallelLinearAlgebra pla(data_, proc_);

    pla.new_vector(v1_);
    pla.add_vector(dcmHandle_, v1_);

    pla.new_vector(v2_);
    pla.add_vector(dcmHandle2_,v2_);

    pla.new_vector(vR_);

    pla.ones(vR_);

    pla.sub(v1_, v2_, vR_);
  }
};

TEST(ParallelArithmeticTests, CanSubtractVectorsMulti)
{
  ParallelLinearAlgebraSharedData data(getDummySystem(),2);

  ParallelLinearAlgebra::ParallelVector v1;
  ParallelLinearAlgebra::ParallelVector v2;
  ParallelLinearAlgebra::ParallelVector vR;

  auto vec2 = vector2();
  auto vec1 = vector1();
    {
	    subtract sub_0(data, v1, v2, vR, 0, vec1, vec2);
	    subtract sub_1(data, v1, v2, vR, 1, vec1, vec2);

	    boost::thread t1 = boost::thread(boost::ref(sub_0));
	    boost::thread t2 = boost::thread(boost::ref(sub_1));
	    t1.join();
	    t2.join();
    }

  EXPECT_EQ(2,  vR.data_[0]);
  EXPECT_EQ(4,  vR.data_[1]);
  EXPECT_EQ(8,  vR.data_[2]);
  EXPECT_EQ(0,  vR.data_[3]);
  EXPECT_EQ(-2, vR.data_[size-1]);
}
//
TEST(ParallelArithmeticTests, CanCompute2Norm)
{
  ParallelLinearAlgebraSharedData data(getDummySystem(),1);
  ParallelLinearAlgebra pla(data,0);

  ParallelLinearAlgebra::ParallelVector v1;
  auto vec1 = vector1();
  pla.add_vector(vec1,v1);

  ParallelLinearAlgebra::ParallelVector v2;
  auto vec2 = vector2();
  pla.add_vector(vec2,v2);

  ParallelLinearAlgebra::ParallelVector v3;
  auto vec3 = vector3();
  pla.add_vector(vec3,v3);

  EXPECT_NEAR(4.6904,pla.norm(v1),0.001);
  EXPECT_NEAR(670.900,pla.norm(v2),0.001);
  EXPECT_NEAR(7.0711,pla.norm(v3),0.001);
}

struct norm
{
  norm(ParallelLinearAlgebraSharedData& data, ParallelLinearAlgebra::ParallelVector& v1,
    ParallelLinearAlgebra::ParallelVector& v2, ParallelLinearAlgebra::ParallelVector& v3,
    int proc, DenseColumnMatrixHandle dcmHandle1, DenseColumnMatrixHandle dcmHandle2
    , DenseColumnMatrixHandle dcmHandle3) :
    data_(data), proc_(proc), v1_(v1), v2_(v2), v3_(v3), dcmHandle1_(dcmHandle1),
    dcmHandle2_(dcmHandle2), dcmHandle3_(dcmHandle3) {}

  ParallelLinearAlgebraSharedData& data_;
  int proc_;
  ParallelLinearAlgebra::ParallelVector& v1_;
  ParallelLinearAlgebra::ParallelVector& v2_;
  ParallelLinearAlgebra::ParallelVector& v3_;
  DenseColumnMatrixHandle dcmHandle1_;
  DenseColumnMatrixHandle dcmHandle2_;
  DenseColumnMatrixHandle dcmHandle3_;
  double v1Norm_;
  double v2Norm_;
  double v3Norm_;

  void operator()()
  {
    ParallelLinearAlgebra pla(data_, proc_);

    pla.new_vector(v1_);
    pla.add_vector(dcmHandle1_,v1_);

    pla.new_vector(v2_);
    pla.add_vector(dcmHandle2_,v2_);

    pla.new_vector(v3_);
    pla.add_vector(dcmHandle3_,v3_);

    v1Norm_ = pla.norm(v1_);
    v2Norm_ = pla.norm(v2_);
    v3Norm_ = pla.norm(v3_);
  }
};

TEST(ParallelArithmeticTests, CanCompute2NormMulti)
{
  ParallelLinearAlgebraSharedData data(getDummySystem(), 2);

  ParallelLinearAlgebra::ParallelVector v1;
  ParallelLinearAlgebra::ParallelVector v2;
  ParallelLinearAlgebra::ParallelVector v3;
  double v1Norm_Result;
  double v2Norm_Result;
  double v3Norm_Result;

  auto vec1 = vector1();
  auto vec2 = vector2();
  auto vec3 = vector3();
  {
	  norm norm_0(data, v1, v2, v3, 0, vec1, vec2, vec3);
	  norm norm_1(data, v1, v2, v3, 1, vec1, vec2, vec3);

	  boost::thread t1 = boost::thread(boost::ref(norm_0));
	  boost::thread t2 = boost::thread(boost::ref(norm_1));
	  t1.join();
	  t2.join();

    v1Norm_Result = norm_0.v1Norm_;
    v2Norm_Result = norm_0.v2Norm_;
    v3Norm_Result = norm_0.v3Norm_;
  }

  EXPECT_NEAR(4.6904,v1Norm_Result,0.001);
  EXPECT_NEAR(670.900,v2Norm_Result,0.001);
  EXPECT_NEAR(7.0711,v3Norm_Result,0.001);
}

TEST(ParallelArithmeticTests, CanMultiplyVectorsComponentWise)
{
  ParallelLinearAlgebraSharedData data(getDummySystem(),1);
  ParallelLinearAlgebra pla(data,0);

  ParallelLinearAlgebra::ParallelVector v1;
  auto vec1 = vector1();
  pla.add_vector(vec1,v1);

  ParallelLinearAlgebra::ParallelVector v2;
  auto vec2 = vector2();
  pla.add_vector(vec2,v2);

  ParallelLinearAlgebra::ParallelVector v3;
  auto vec3 = vector3();
  pla.add_vector(vec3,v3);
  pla.zeros(v3);

  pla.mult(v1,v2,v3);
  EXPECT_EQ(v3.data_[0],-1);
  EXPECT_EQ(v3.data_[1],-4);
  EXPECT_EQ(v3.data_[2],-16);
  EXPECT_EQ(v3.data_[size-1],-1);

  pla.zeros(v2);
  auto resetV3 = vector3();
  pla.add_vector(resetV3,v3);
  pla.mult(v1,v3,v2);

  EXPECT_EQ(0,v2.data_[0]);
  EXPECT_EQ(2,v2.data_[1]);
  EXPECT_EQ(0,v2.data_[2]);
  EXPECT_EQ(7,v2.data_[size-1]);
}
struct multVectors
{
  multVectors(ParallelLinearAlgebraSharedData& data, ParallelLinearAlgebra::ParallelVector& v1,
    ParallelLinearAlgebra::ParallelVector& v2, ParallelLinearAlgebra::ParallelVector& v3,
    int proc, DenseColumnMatrixHandle dcmHandle1, DenseColumnMatrixHandle dcmHandle2) :
    data_(data), proc_(proc), v1_(v1), v2_(v2), v3_(v3),
      dcmHandle1_(dcmHandle1), dcmHandle2_(dcmHandle2) {}

  ParallelLinearAlgebraSharedData& data_;
  int proc_;
  ParallelLinearAlgebra::ParallelVector& v1_;
  ParallelLinearAlgebra::ParallelVector& v2_;
  ParallelLinearAlgebra::ParallelVector& v3_;
  DenseColumnMatrixHandle dcmHandle1_;
  DenseColumnMatrixHandle dcmHandle2_;

  void operator()()
  {
    ParallelLinearAlgebra pla(data_, proc_);

    pla.new_vector(v1_);
    pla.add_vector(dcmHandle1_, v1_);

    pla.new_vector(v2_);
    pla.add_vector(dcmHandle2_,v2_);

    pla.new_vector(v3_);

    pla.mult(v1_,v2_,v3_);
  }
};
/// @todo: by intern
TEST(ParallelArithmeticTests, CanMultiplyVectorsComponentWiseMulti)
{
  ParallelLinearAlgebraSharedData data(getDummySystem(), 2);

  ParallelLinearAlgebra::ParallelVector v1;
  ParallelLinearAlgebra::ParallelVector v2;
  ParallelLinearAlgebra::ParallelVector v3;

  auto vec1 = vector1();
  auto vec2 = vector2();
  {
	  multVectors mult_0(data, v1, v2, v3, 0, vec1, vec2);
	  multVectors mult_1(data, v1, v2, v3, 1, vec1, vec2);

	  boost::thread t1 = boost::thread(boost::ref(mult_0));
	  boost::thread t2 = boost::thread(boost::ref(mult_1));
	  t1.join();
	  t2.join();
  }

  EXPECT_EQ(-1  , v3.data_[0]);
  EXPECT_EQ(-4  , v3.data_[1]);
  EXPECT_EQ(-16 , v3.data_[2]);
  EXPECT_EQ(-1  , v3.data_[size-1]);
}

TEST(ParallelArithmeticTests, CanComputeDotProduct)
{
  ParallelLinearAlgebraSharedData data(getDummySystem(),1);
  ParallelLinearAlgebra pla(data,0);

  ParallelLinearAlgebra::ParallelVector v1;
  auto vec1 = vector1();
  pla.add_vector(vec1,v1);

  ParallelLinearAlgebra::ParallelVector v2;
  auto vec2 = vector2();
  pla.add_vector(vec2,v2);

  ParallelLinearAlgebra::ParallelVector v3;
  auto vec3 = vector3();
  pla.add_vector(vec3,v3);

  EXPECT_EQ(-22 , pla.dot(v1,v2));
  EXPECT_EQ(-9 , pla.dot(v2,v3));
  EXPECT_EQ(9 , pla.dot(v1,v3));
}

struct dotMult
{
  dotMult(ParallelLinearAlgebraSharedData& data, ParallelLinearAlgebra::ParallelVector& v1,
    ParallelLinearAlgebra::ParallelVector& v2, ParallelLinearAlgebra::ParallelVector& v3,
    int proc, DenseColumnMatrixHandle dcmHandle, DenseColumnMatrixHandle dcmHandle2,
    DenseColumnMatrixHandle dcmHandle3) :
      data_(data), proc_(proc), v1_(v1), v2_(v2), v3_(v3),
      dcmHandle_(dcmHandle), dcmHandle2_(dcmHandle2), dcmHandle3_(dcmHandle3) {}

  ParallelLinearAlgebraSharedData& data_;
  int proc_;
  ParallelLinearAlgebra::ParallelVector& v1_;
  ParallelLinearAlgebra::ParallelVector& v2_;
  ParallelLinearAlgebra::ParallelVector& v3_;
  DenseColumnMatrixHandle dcmHandle_;
  DenseColumnMatrixHandle dcmHandle2_;
  DenseColumnMatrixHandle dcmHandle3_;
  double v12_;
  double v23_;
  double v13_;

  void operator()()
  {
    ParallelLinearAlgebra pla(data_, proc_);

    pla.new_vector(v1_);
    pla.add_vector(dcmHandle_, v1_);

    pla.new_vector(v2_);
    pla.add_vector(dcmHandle2_,v2_);

    pla.new_vector(v3_);
    pla.add_vector(dcmHandle3_,v3_);

    v12_ = pla.dot(v1_,v2_);
    v23_ = pla.dot(v2_,v3_);
    v13_ = pla.dot(v1_,v3_);
  }
};

TEST(ParallelArithmeticTests, CanComputeDotProductMulti)
{
  ParallelLinearAlgebraSharedData data(getDummySystem(), 2);

  ParallelLinearAlgebra::ParallelVector v1;
  ParallelLinearAlgebra::ParallelVector v2;
  ParallelLinearAlgebra::ParallelVector v3;
  double v12;
  double v23;
  double v13;

  auto vec1 = vector1();
  auto vec2 = vector2();
  auto vec3 = vector3();
  {
	  dotMult dotMult_0(data, v1, v2, v3, 0, vec1, vec2, vec3);
	  dotMult dotMult_1(data, v1, v2, v3, 1, vec1, vec2, vec3);

	  boost::thread t1 = boost::thread(boost::ref(dotMult_0));
	  boost::thread t2 = boost::thread(boost::ref(dotMult_1));
	  t1.join();
	  t2.join();
    v12 = dotMult_0.v12_;
    v23 = dotMult_0.v23_;
    v13 = dotMult_0.v13_;
  }

  EXPECT_EQ(-22 , v12);
  EXPECT_EQ(-9 , v23);
  EXPECT_EQ(9 , v13);
}