#pragma once

#include <Discregrid/mesh/triangle_mesh.hpp>

namespace std {
	template <> struct hash<Eigen::Vector3d>
	{
		std::size_t operator()(Eigen::Vector3d const& x) const
		{
			std::size_t seed = 0;
			std::hash<double> hasher;
			seed ^= hasher(x[0]) + 0x9e3779b9 + (seed<<6) + (seed>>2);
			seed ^= hasher(x[1]) + 0x9e3779b9 + (seed<<6) + (seed>>2);
			seed ^= hasher(x[2]) + 0x9e3779b9 + (seed<<6) + (seed>>2);
			return seed;
		}
	};

	template <> struct less<Eigen::Vector3d>
	{
		bool operator()(Eigen::Vector3d const& left, Eigen::Vector3d const& right) const
		{
			for (auto i = 0u; i < 3u; ++i)
			{
				if (left(i) < right(i))
					return true;
				else if (left(i) > right(i))
					return false;
			}
			return false;
		}
	};
}
#include <Discregrid/utility/lru_cache.hpp>

#include <Discregrid/acceleration/bounding_sphere_hierarchy.hpp>

#include <array>
#include <vector>
#include <set>
#include <map>
#include <unordered_map>

#include <Eigen/Dense>

namespace Discregrid
{

enum class NearestEntity;
class TriangleMesh;
class Halfedge;
class MeshDistance
{

	struct Candidate
	{
		bool operator<(Candidate const& other) const { return b < other.b; }
		unsigned int node_index;
		double b, w;
	};

public:

	MeshDistance(TriangleMesh const& mesh, bool precompute_normals = true);

	// Returns the shortest unsigned distance from a given point x to
	// the stored mesh.
	// Thread-safe function.
	double distance(Eigen::Vector3d const& x, Eigen::Vector3d* nearest_point = nullptr,
		unsigned int* nearest_face = nullptr, NearestEntity* ne = nullptr) const;

	// Requires a closed two-manifold mesh as input data.
	// Thread-safe function.
	double signedDistance(Eigen::Vector3d const& x) const;
	double signedDistanceCached(Eigen::Vector3d const& x) const;

	double unsignedDistance(Eigen::Vector3d const& x) const;
	double unsignedDistanceCached(Eigen::Vector3d const& x) const;

private:

	Eigen::Vector3d vertex_normal(unsigned int v) const;
	Eigen::Vector3d edge_normal(Halfedge const& h) const;
	Eigen::Vector3d face_normal(unsigned int f) const;

	void callback(unsigned int node_index, TriangleMeshBSH const& bsh,
		Eigen::Vector3d const& x,
		double& dist) const;

	bool predicate(unsigned int node_index, TriangleMeshBSH const& bsh,
		Eigen::Vector3d const& x, double& dist) const;

private:

	TriangleMesh const& m_mesh;
	TriangleMeshBSH m_bsh;

	using FunctionValueCache = LRUCache<Eigen::Vector3d, double>;
	mutable std::vector<TriangleMeshBSH::TraversalQueue> m_queues;
	mutable std::vector<unsigned int> m_nearest_face;
	mutable std::vector<FunctionValueCache> m_cache;
	mutable std::vector<FunctionValueCache> m_ucache;

	std::vector<Eigen::Vector3d> m_face_normals;
	std::vector<Eigen::Vector3d> m_vertex_normals;
	bool m_precomputed_normals;
};

}


#include <alglib/graph/directed_graph.h>
#include <alglib/graph/graph_property.h>
#include <bits/stdc++.h>

using namespace std;
using namespace alglib::graph;

int main() {

    typedef directed_graph<string> GraphT;

    // A social network of people
    GraphT graph;

    graph.add_vertex("A");
    graph.add_vertex("B");
    graph.add_vertex("C");
    graph.add_vertex("D");

    // Property on each vertex: let us say the age of each person
    vertex_property<GraphT, int> P(graph);

    P["A"] = 10;
    P["B"] = 20;
    P["C"] = 30;
    P["D"] = 40;

    // make all property one
    P.bulk_update(graph.vbegin(), graph.vend(), 100);

    //! P["E"] = 50; // Causes exception to be thrown

    for(auto it = graph.vbegin(); it != graph.vend(); it++)
        cout << "Age of person: " << *it << " is: " << P(*it) << endl;

}


/* ************************************************************************
* Copyright 2015 Advanced Micro Devices, 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.
* ************************************************************************ */

/*! \file
* \brief sparse_matrix_fill.hpp provides functionality to fill CSR matrix values with
*  random powers of 2 or integer values.
*
*/

#pragma once
#ifndef _SPARSE_MATRIX_FILL_HXX
#define _SPARSE_MATRIX_FILL_HXX

#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/generator_iterator.hpp>

template <typename T>
class clsparse_matrix_fill {
    typedef boost::minstd_rand base_generator_type;
public:
    clsparse_matrix_fill(int seed, int lR, int hR)
    {
        generator = new base_generator_type(seed);
        minR = lR;
        maxR = hR;
    }//
    ~clsparse_matrix_fill()
    {
        delete generator;
    }

    void fillMtxTwoPowers(T* A, size_t sz)
    {
        for (size_t i = 0; i < sz; i++)
        {
            int tmp = roll_die();
            A[i] = static_cast<T>(1 << tmp);
        }
    }

    void fillMtxIntegers(T* A, size_t sz)
    {
        for (size_t i = 0; i < sz; i++)
        {
            A[i] = static_cast<T>(roll_die());
        }
    }

    void fillMtxOnes(T* A, size_t sz)
    {
        for (size_t i = 0; i < sz; i++)
        {
            A[i] = static_cast<T>(1);
        }
    }

private:
    base_generator_type *generator;
    int minR;  // Range [minR, maxR]
    int maxR;
    int roll_die()
    {
        boost::random::uniform_int_distribution<> dist(minR, maxR);

        //generator.seed(static_cast<unsigned int>(std::time(0)));
        return dist(*generator);
    }
};

#endif // _SPARSE_MATRIX_FILL_HXX


#include "worldengine/plates.h"
#include "worldengine/generation.h"
#include "worldengine/world.h"

#include <chrono>
#include <random>

#include <boost/log/trivial.hpp>
#include <boost/random.hpp>

#include <platecapi.hpp>

namespace WorldEngine
{

/**
 * @brief Create a new world based on an initial plates simulation
 * @param name World name
 * @param width Width in pixels
 * @param height Height in pixels
 * @param seed Random seed value
 * @param temps A list of six temperatures
 * @param humids A list of seven humidity values
 * @param gammaCurve Gamma value for temperature and precipitation on gamma
 * correction curve
 * @param curveOffset Adjustment value for temperature and precipitation gamma
 * correction curve
 * @param numPlates Number of plates
 * @param oceanLevel The elevation representing the ocean level
 * @param step Generation steps to perform
 * @return A new world
 */
static std::shared_ptr<World>
PlatesSimulation(const std::string&        name,
                 uint32_t                  width,
                 uint32_t                  height,
                 uint32_t                  seed,
                 const std::vector<float>& temps       = DEFAULT_TEMPS,
                 const std::vector<float>& humids      = DEFAULT_HUMIDS,
                 float                     gammaCurve  = DEFAULT_GAMMA_CURVE,
                 float                     curveOffset = DEFAULT_CURVE_OFFSET,
                 uint32_t                  numPlates   = DEFAULT_NUM_PLATES,
                 float                     oceanLevel  = DEFAULT_OCEAN_LEVEL,
                 const Step&               step        = DEFAULT_STEP);

std::shared_ptr<World> WorldGen(const std::string&        name,
                                uint32_t                  width,
                                uint32_t                  height,
                                uint32_t                  seed,
                                const std::vector<float>& temps,
                                const std::vector<float>& humids,
                                float                     gammaCurve,
                                float                     curveOffset,
                                uint32_t                  numPlates,
                                float                     oceanLevel,
                                const Step&               step,
                                bool                      fadeBorders)
{
   std::chrono::steady_clock::time_point startTime;
   std::chrono::steady_clock::time_point endTime;

   startTime = std::chrono::steady_clock::now();

   std::shared_ptr<World> world = PlatesSimulation(name,
                                                   width,
                                                   height,
                                                   seed,
                                                   temps,
                                                   humids,
                                                   gammaCurve,
                                                   curveOffset,
                                                   numPlates,
                                                   oceanLevel,
                                                   step);

   CenterLand(*world);

   endTime = std::chrono::steady_clock::now();
   auto elapsedTime =
      std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime)
         .count();

   BOOST_LOG_TRIVIAL(debug)
      << "WorldGen(): setElevation, setPlates, centerLand complete. "
      << "Elapsed time " << elapsedTime << "ms.";

   startTime = std::chrono::steady_clock::now();

   std::mt19937                                      generator(seed);
   boost::random::uniform_int_distribution<uint32_t> distribution(0,
                                                                  UINT32_MAX);
   AddNoiseToElevation(*world, distribution(generator));

   endTime = std::chrono::steady_clock::now();
   elapsedTime =
      std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime)
         .count();

   BOOST_LOG_TRIVIAL(debug) << "WorldGen(): elevation noise added. "
                            << "Elapsed time " << elapsedTime << "ms.";

   startTime = std::chrono::steady_clock::now();

   if (fadeBorders)
      PlaceOceansAtMapBorders(*world);
   InitializeOceanAndThresholds(*world);

   endTime = std::chrono::steady_clock::now();
   elapsedTime =
      std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime)
         .count();

   BOOST_LOG_TRIVIAL(debug) << "WorldGen(): oceans initialized. "
                            << "Elapsed time " << elapsedTime << "ms.";

   GenerateWorld(*world, step, distribution(generator));

   return world;
}

void* GeneratePlatesSimulation(float**    heightmap,
                               uint32_t** platesmap,
                               long       seed,
                               uint32_t   width,
                               uint32_t   height,
                               float      seaLevel,
                               uint32_t   erosionPeriod,
                               float      foldingRatio,
                               uint32_t   aggrOverlapAbs,
                               float      aggrOverlapRel,
                               uint32_t   cycleCount,
                               uint32_t   numPlates)
{
   std::chrono::steady_clock::time_point startTime;
   std::chrono::steady_clock::time_point endTime;

   startTime = std::chrono::steady_clock::now();

   void* p = platec_api_create(seed,
                               width,
                               height,
                               seaLevel,
                               erosionPeriod,
                               foldingRatio,
                               aggrOverlapAbs,
                               aggrOverlapRel,
                               cycleCount,
                               numPlates);

   // Note: To rescale the world's heightmap to roughly Earth's scale, multiply
   // by 2000

   while (!platec_api_is_finished(p))
   {
      platec_api_step(p);
   }

   *heightmap = platec_api_get_heightmap(p);
   *platesmap = platec_api_get_platesmap(p);

   endTime = std::chrono::steady_clock::now();
   auto elapsedTime =
      std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime)
         .count();

   BOOST_LOG_TRIVIAL(debug) << "GeneratePlatesSimulation() complete. "
                            << "Elapsed time " << elapsedTime << "ms.";

   return p;
}

void PlatecApiDestroy(void* p)
{
   platec_api_destroy(p);
}

static std::shared_ptr<World> PlatesSimulation(const std::string&        name,
                                               uint32_t                  width,
                                               uint32_t                  height,
                                               uint32_t                  seed,
                                               const std::vector<float>& temps,
                                               const std::vector<float>& humids,
                                               float       gammaCurve,
                                               float       curveOffset,
                                               uint32_t    numPlates,
                                               float       oceanLevel,
                                               const Step& step)
{
   float*    heightmap;
   uint32_t* platesmap;

   void* p = GeneratePlatesSimulation(&heightmap,
                                      &platesmap,
                                      seed,
                                      width,
                                      height,
                                      DEFAULT_SEA_LEVEL,
                                      DEFAULT_EROSION_PERIOD,
                                      DEFAULT_FOLDING_RATIO,
                                      DEFAULT_AGGR_OVERLAP_ABS,
                                      DEFAULT_AGGR_OVERLAP_REL,
                                      DEFAULT_CYCLE_COUNT,
                                      numPlates);

   std::shared_ptr<World> world = std::shared_ptr<World>(
      new World(name,
                Size(width, height),
                seed,
                GenerationParameters(numPlates, oceanLevel, step),
                temps,
                humids,
                gammaCurve,
                curveOffset));

   world->SetElevationData(heightmap);
   world->SetPlatesData(platesmap);

   PlatecApiDestroy(p);

   return world;
}

} // namespace WorldEngine


/* CirKit: A circuit toolkit
 * Copyright (C) 2009-2015  University of Bremen
 * Copyright (C) 2015-2017  EPFL
 *
 * 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.
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE truth_table

#include <cmath>
#include <vector>

#include <boost/assign/std/vector.hpp>
#include <boost/range/combine.hpp>
#include <boost/range/counting_range.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/output_test_stream.hpp>

#include <reversible/circuit.hpp>
#include <reversible/truth_table.hpp>
#include <reversible/functions/circuit_to_truth_table.hpp>
#include <reversible/simulation/simple_simulation.hpp>
#include <reversible/synthesis/reed_muller_synthesis.hpp>
#include <reversible/synthesis/transformation_based_synthesis.hpp>
#include <reversible/synthesis/transposition_based_synthesis.hpp>

using namespace cirkit;

void add_entries_from_permutation( binary_truth_table& spec, const std::vector<unsigned>& permutation )
{
  using boost::combine;
  using boost::counting_range;
  using boost::get;

  auto n = (unsigned)ceil( log( permutation.size() ) / log( 2 ) );

  for ( const auto& i : combine( counting_range( 0u, (unsigned)permutation.size() ),
                                 permutation ) )
  {
    spec.add_entry( number_to_truth_table_cube( get<0>( i ), n ),
                    number_to_truth_table_cube( get<1>( i ), n ) );
  }
}

BOOST_AUTO_TEST_CASE(simple)
{
  using boost::test_tools::output_test_stream;

  using namespace boost::assign;

  // Construct truth table from benchmark
  std::vector<unsigned> permutation;
  permutation += 7u,0u,1u,3u,4u,2u,6u,5u; // 3_17 benchmark
  binary_truth_table spec;
  add_entries_from_permutation( spec, permutation );

  // Synthesize and compare with different methods
  std::vector<truth_table_synthesis_func> funcs;
  funcs += transformation_based_synthesis_func(),reed_muller_synthesis_func(),transposition_based_synthesis_func();

  output_test_stream output;
  output << spec;

  for ( const auto& f : funcs )
  {
    std::stringstream new_stream;

    circuit circ;
    binary_truth_table new_spec;

    f( circ, spec );
    circuit_to_truth_table( circ, new_spec, simple_simulation_func() );

    new_stream << new_spec;

    BOOST_CHECK( output.is_equal( new_stream.str(), false ) );
  }
}

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


/*
 * GraphBLAS Template Library (GBTL), Version 3.0
 *
 * Copyright 2020 Carnegie Mellon University, Battelle Memorial Institute, and
 * Authors.
 *
 * THIS MATERIAL WAS PREPARED AS AN ACCOUNT OF WORK SPONSORED BY AN AGENCY OF
 * THE UNITED STATES GOVERNMENT.  NEITHER THE UNITED STATES GOVERNMENT NOR THE
 * UNITED STATES DEPARTMENT OF ENERGY, NOR THE UNITED STATES DEPARTMENT OF
 * DEFENSE, NOR CARNEGIE MELLON UNIVERSITY, NOR BATTELLE, NOR ANY OF THEIR
 * EMPLOYEES, NOR ANY JURISDICTION OR ORGANIZATION THAT HAS COOPERATED IN THE
 * DEVELOPMENT OF THESE MATERIALS, MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR
 * ASSUMES ANY LEGAL LIABILITY OR RESPONSIBILITY FOR THE ACCURACY, COMPLETENESS,
 * OR USEFULNESS OR ANY INFORMATION, APPARATUS, PRODUCT, SOFTWARE, OR PROCESS
 * DISCLOSED, OR REPRESENTS THAT ITS USE WOULD NOT INFRINGE PRIVATELY OWNED
 * RIGHTS.
 *
 * Released under a BSD-style license, please see LICENSE file or contact
 * permission@sei.cmu.edu for full terms.
 *
 * [DISTRIBUTION STATEMENT A] This material has been approved for public release
 * and unlimited distribution.  Please see Copyright notice for non-US
 * Government use and distribution.
 *
 * This Software includes and/or makes use of the following Third-Party Software
 * subject to its own license:
 *
 * 1. Boost Unit Test Framework
 * (https://www.boost.org/doc/libs/1_45_0/libs/test/doc/html/utf.html)
 * Copyright 2001 Boost software license, Gennadiy Rozental.
 *
 * DM20-0442
 */

#define GRAPHBLAS_LOGGING_LEVEL 0

#include <functional>
#include <iostream>
#include <vector>

#include <graphblas/graphblas.hpp>

using namespace grb;

#define BOOST_TEST_MAIN
#define BOOST_TEST_MODULE reduce_test_suite

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

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

/// @todo This is a merge of two test files for reduce.  Many are redundant.

//****************************************************************************
// reduce matrix to vector
//****************************************************************************

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_reduce_matvec_bad_dimensions)
{
    std::vector<std::vector<double> > m3x3_dense = {{12, 1, 3},
                                                    {0,  0, 0},
                                                    {7,  0, 9}};

    std::vector<std::vector<double> > m6x4_dense = {{5, 0, 1, 0},
                                                    {6, 7, 0, 0},
                                                    {6, 7, 0, 0},
                                                    {0, 5, 0, 0},
                                                    {6, 7, 0, 0},
                                                    {0, 0, 0, 0}};

    Matrix<double, DirectedMatrixTag> mA(m3x3_dense, 0.);
    Matrix<double, DirectedMatrixTag> mB(m6x4_dense, 0.);

    Vector<double, SparseTag> w4(4);

    // incompatible output vector dimensions
    BOOST_CHECK_THROW(
        (reduce(w4,
                NoMask(),
                NoAccumulate(),
                Plus<double>(), mA)),
        DimensionException);

    BOOST_CHECK_THROW(
        (reduce(w4,
                NoMask(),
                NoAccumulate(),
                Plus<double>(), mB)),
        DimensionException);

    // vector-scalar version - no bad dimensions

    // matrix-scalar version - no bad dimensions
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(reduce_matvec_test_bad_dimension)
{
    // Build some matrices.
    IndexArrayType i_A    = {0, 0, 1, 1, 1, 2, 2, 2, 3, 3};
    IndexArrayType j_A    = {0, 1, 0, 1, 2, 1, 2, 3, 2, 3};
    std::vector<double> v_A = {1, 1, 1, 2, 2, 1, 3, 5, 3, 4};
    Matrix<double, DirectedMatrixTag> A(4, 4);
    A.build(i_A, j_A, v_A);

    IndexArrayType i_C    = {0, 1, 2};
    std::vector<double> v_C = {1, 1, 1};
    Vector<double> C(3);
    C.build(i_C, v_C);

    BOOST_CHECK_THROW(
        (reduce(C, NoMask(), NoAccumulate(),
                PlusMonoid<double>(), A)),
        DimensionException);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_reduce_matvec_mask_replace_bad_dimensions)
{
    std::vector<std::vector<double> > m3x3_dense = {{12, 1, 3},
                                                    {0,  0, 0},
                                                    {7,  0, 9}};
    std::vector<double> u3_dense = {1, 1, 0};
    std::vector<double> u4_dense = {0, 0, 13, 1};

    Matrix<double, DirectedMatrixTag> mA(m3x3_dense, 0.);

    Vector<double, SparseTag> u3(u3_dense, 0.);
    Vector<double, SparseTag> m4(u4_dense, 0.);
    Vector<double, SparseTag> w3(3);

    // incompatible mask matrix dimensions
    BOOST_CHECK_THROW(
        (reduce(w3,
                m4,
                NoAccumulate(),
                Plus<double>(), mA,
                REPLACE)),
        DimensionException);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_reduce_matvec_mask_merge_bad_dimensions)
{
    std::vector<std::vector<double> > m3x3_dense = {{12, 1, 3},
                                                    {0,  0, 0},
                                                    {7,  0, 9}};
    std::vector<double> v3_ones = {1, 1, 1};
    std::vector<double> u4_dense = {0, 0, 13, 1};

    Matrix<double, DirectedMatrixTag> mA(m3x3_dense, 0.);
    Vector<double, SparseTag> m4(u4_dense, 0.);
    Vector<double, SparseTag> w3(v3_ones, 0.);

    // incompatible mask matrix dimensions
    BOOST_CHECK_THROW(
        (reduce(w3,
                m4,
                NoAccumulate(),
                Plus<double>(), mA)),
        DimensionException);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(reduce_matvec_test_transpose_bad_dimensions)
{
    // Build some matrices.
    IndexArrayType i_A    = {0, 0, 1, 1, 1, 2, 2, 2, 3, 3};
    IndexArrayType j_A    = {0, 1, 0, 1, 2, 1, 2, 3, 2, 3};
    std::vector<double> v_A = {1, 1, 1, 2, 2, 1, 3, 5, 3, 4};
    Matrix<double, DirectedMatrixTag> A(4, 4);
    A.build(i_A, j_A, v_A);

    IndexArrayType j_C    = {0, 1, 2, 3, 4};
    std::vector<double> v_C = {1, 1, 1, 1, 1};
    Vector<double> C(5);
    C.build(j_C, v_C);

    BOOST_CHECK_THROW(
        (reduce(C, NoMask(), NoAccumulate(),
                PlusMonoid<double>(), transpose(A))),
        DimensionException);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_reduce_matvec_scmp_replace_bad_dimensions)
{
    std::vector<std::vector<double> > m3x3_dense = {{12, 1, 3},
                                                    {0,  0, 0},
                                                    {7,  0, 9}};
    std::vector<double> u4_dense = {0, 0, 13, 1};

    Matrix<double, DirectedMatrixTag> mA(m3x3_dense, 0.);
    Vector<double, SparseTag> m4(u4_dense, 0.);
    Vector<double, SparseTag> w3(3);

    // incompatible mask matrix dimensions
    BOOST_CHECK_THROW(
        (reduce(w3,
                complement(m4),
                NoAccumulate(),
                Plus<double>(), mA,
                REPLACE)),
        DimensionException);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_reduce_matvec_scmp_merge_bad_dimensions)
{
    std::vector<std::vector<double> > m3x3_dense = {{12, 1, 3},
                                                    {0,  0, 0},
                                                    {7,  0, 9}};
    std::vector<double> u4_dense = {0, 0, 13, 1};
    std::vector<double> v3_ones = {1, 1, 1};

    Matrix<double, DirectedMatrixTag> mA(m3x3_dense, 0.);

    Vector<double, SparseTag> m4(u4_dense, 0.);
    Vector<double, SparseTag> w3(v3_ones, 0.);

    // incompatible mask matrix dimensions
    BOOST_CHECK_THROW(
        (reduce(w3,
                complement(m4),
                NoAccumulate(),
                Plus<double>(), mA)),
        DimensionException);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(reduce_matvec_test_noaccum)
{
    // Build some matrices.
    IndexArrayType i_A    = {0, 0, 1, 1, 1, 2, 2, 2, 3, 3};
    IndexArrayType j_A    = {0, 1, 0, 1, 2, 1, 2, 3, 2, 3};
    std::vector<double> v_A = {1, 1, 1, 2, 2, 1, 3, 5, 3, 4};
    Matrix<double, DirectedMatrixTag> A(4, 4);
    A.build(i_A, j_A, v_A);

    IndexArrayType i_C    = {0, 1, 2, 3};
    std::vector<double> v_C = {1, 1, 1, 1};
    Vector<double> C(4);
    C.build(i_C, v_C);

    IndexArrayType i_answer    = {0, 1, 2, 3};
    std::vector<double> v_answer = {2, 5, 9, 7};
    Vector<double> answer(4);
    answer.build(i_answer, v_answer);

    reduce(C, NoMask(), NoAccumulate(),
           PlusMonoid<double>(), A);
    BOOST_CHECK_EQUAL(C, answer);

    reduce(C, NoMask(), NoAccumulate(),
           Plus<double>(), A);
    BOOST_CHECK_EQUAL(C, answer);

    reduce(C, NoMask(), NoAccumulate(),
           add_monoid(ArithmeticSemiring<double>()), A);
    BOOST_CHECK_EQUAL(C, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_reduce_matvec_reg)
{
    std::vector<std::vector<double> > m3x3_dense = {{12, 1, 3},
                                                    {0,  0, 0},
                                                    {7,  0, 9}};

    std::vector<std::vector<double> > m6x4_dense = {{5, 0, 1, 0},
                                                    {6, 7, 0, 0},
                                                    {6, 7, 0, 0},
                                                    {0, 5, 0, 0},
                                                    {6, 7, 0, 0},
                                                    {0, 0, 0, 0}};

    std::vector<double> ans3x3_dense  = {16, 0, 16};
    std::vector<double> ans6x4_dense = {6, 13, 13, 5, 13, 0};

    // Matrix to vector reduction
    Matrix<double, DirectedMatrixTag> mA(m3x3_dense, 0.);
    Matrix<double, DirectedMatrixTag> mB(m6x4_dense, 0.);
    Matrix<double, DirectedMatrixTag> empty6x4(6, 4);
    Vector<double> result3(3);
    Vector<double> result6(6);
    Vector<double> empty_result6(6);
    Vector<double> ansA(ans3x3_dense, 0.);
    Vector<double> ansB(ans6x4_dense, 0.);

    reduce(result3,
           NoMask(),
           NoAccumulate(),
           Plus<double>(), mA);
    BOOST_CHECK_EQUAL(result3, ansA);

    reduce(result6,
           NoMask(),
           NoAccumulate(),
           Plus<double>(), mB);
    BOOST_CHECK_EQUAL(result6, ansB);

    reduce(empty_result6,
           NoMask(),
           NoAccumulate(),
           Plus<double>(), empty6x4);
    BOOST_CHECK_EQUAL(empty_result6.nvals(), 0);

}

//****************************************************************************
BOOST_AUTO_TEST_CASE(reduce_matvec_test_nomask_accum)
{
    // Build some matrices.
    IndexArrayType i_A    = {0, 0, 1, 1, 1, 2, 2, 2, 3, 3};
    IndexArrayType j_A    = {0, 1, 0, 1, 2, 1, 2, 3, 2, 3};
    std::vector<double> v_A = {1, 1, 1, 2, 2, 1, 3, 5, 3, 4};
    Matrix<double, DirectedMatrixTag> A(4, 4);
    A.build(i_A, j_A, v_A);

    IndexArrayType i_C    = {0, 1, 2, 3};
    std::vector<double> v_C = {1, 1, 1, 1};
    Vector<double> C(4);
    C.build(i_C, v_C);

    IndexArrayType i_answer    = {0, 1, 2, 3};
    std::vector<double> v_answer = {3, 6, 10, 8};
    Vector<double> answer(4);
    answer.build(i_answer, v_answer);

    reduce(C, NoMask(), Plus<double>(),
           PlusMonoid<double>(), A);

    BOOST_CHECK_EQUAL(C, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_reduce_matvec_mask_replace_second_accum)
{
    std::vector<std::vector<double> > m3x3_dense = {{12, 1, 3},
                                                    {0,  0, 0},
                                                    {7,  0, 9}};
    std::vector<double> u3_dense = {1, 1, 0};
    std::vector<double> v3_ones = {1, 1, 1};

    Matrix<double, DirectedMatrixTag> mA(m3x3_dense, 0.);
    Vector<double, SparseTag> m3(u3_dense, 0.);
    BOOST_CHECK_EQUAL(m3.nvals(), 2);

    {
        Vector<double> result(v3_ones, 0.);

        std::vector<double> ans = {16, 1, 0};
        Vector<double> answer(ans, 0.);

        reduce(result,
               m3,
               Second<double>(),
               Plus<double>(), mA,
               REPLACE);

        BOOST_CHECK_EQUAL(result.nvals(), 2);
        BOOST_CHECK_EQUAL(result, answer);
    }

    {
        Vector<double> result(v3_ones, 0.);

        std::vector<double> ans = {16, 0, 0};
        Vector<double> answer(ans, 0.);

        reduce(result,
               m3,
               NoAccumulate(),
               Plus<double>(), mA,
               REPLACE);

        BOOST_CHECK_EQUAL(result.nvals(), 1);
        BOOST_CHECK_EQUAL(result, answer);
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_reduce_matvec_mask_replace_stored_zero)
{
    // tests a computed and stored zero
    // tests a stored zero in the mask

    // Build some matrices.
    std::vector<std::vector<int> > A_dense = {{1, 1, 0, 0},
                                              {1, 2, 2, 0},
                                              {0, 2, 3, 3},
                                              {0, 0, 3, 4}};
    std::vector<int> mask_dense =  { 1, 1, 1, 0};
    std::vector<int> ones_dense =  { 1, 1, 1, 1};
    Matrix<int, DirectedMatrixTag> A(A_dense, 0);
    Vector<int> mask(mask_dense, 0);
    Vector<int> result(ones_dense, 0);

    BOOST_CHECK_EQUAL(mask.nvals(), 3);  // [ 1 1 1 - ]
    mask.setElement(3, 0);
    BOOST_CHECK_EQUAL(mask.nvals(), 4);  // [ 1 1 1 0 ]

    int const NIL(666);
    std::vector<int> ans = { 2, 5, 8,  NIL};
    Vector<int> answer(ans, NIL);

    reduce(result,
           mask,
           NoAccumulate(),
           Plus<double>(), A,
           REPLACE);
    BOOST_CHECK_EQUAL(result.nvals(), 3);
    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_reduce_matvec_mask_replace_transpose)
{
    // Build some matrices.
    std::vector<std::vector<int> > A_dense = {{1, 1, 0, 0, 1},
                                              {-1, 2, 2, 0, 1},
                                              {0, 2, 3, 3, 1},
                                              {0, 0, 3, 4, 1}};

    std::vector<int> mask_dense =  { 1, 0, 1, 0, 1};
    std::vector<int> ones_dense =  { 1, 1, 1, 1, 1};
    Matrix<int, DirectedMatrixTag> A(A_dense, 0);
    Vector<int> mask(mask_dense, 0);
    Vector<int> result(ones_dense, 0);

    int const NIL(666);
    std::vector<int> ans = { 0, NIL, 8,  NIL, 4};
    Vector<int> answer(ans, NIL);

    reduce(result,
           mask,
           NoAccumulate(),
           Plus<double>(), transpose(A),
           REPLACE);

    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_reduce_matvec_mask_merge)
{
    std::vector<std::vector<double> > m3x3_dense = {{12, 1, 3},
                                                    {0,  0, 0},
                                                    {7,  0, 9}};
    std::vector<double> u3_dense = {1, 1, 0};
    std::vector<double> v3_ones = {1, 1, 1};

    Matrix<double, DirectedMatrixTag> mA(m3x3_dense, 0.);
    Vector<double, SparseTag> m3(u3_dense, 0.); // [1 1 -]
    BOOST_CHECK_EQUAL(m3.nvals(), 2);

    {
        Vector<double> result(v3_ones, 0.);

        std::vector<double> ans = {16, 1, 1};
        Vector<double> answer(ans, 0.);

        reduce(result,
               m3,
               Second<double>(),
               Plus<double>(), mA);

        BOOST_CHECK_EQUAL(result.nvals(), 3);
        BOOST_CHECK_EQUAL(result, answer);
    }

    {
        Vector<double> result(v3_ones, 0.);

        std::vector<double> ans = {16, 0, 1};
        Vector<double> answer(ans, 0.);

        reduce(result,
               m3,
               NoAccumulate(),
               Plus<double>(), mA);

        BOOST_CHECK_EQUAL(result.nvals(), 2);
        BOOST_CHECK_EQUAL(result, answer);
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_reduce_matvec_mask_merge_stored_zero)
{
    std::vector<std::vector<double> > m3x3_dense = {{12, 1, 3},
                                                    {0,  0, 0},
                                                    {7,  0, 9}};
    std::vector<double> u3_dense = {1, 1, 0};
    std::vector<double> v3_ones = {1, 1, 1};

    Matrix<double, DirectedMatrixTag> mA(m3x3_dense, 0.);

    Vector<double, SparseTag> u3(u3_dense, 0.);
    Vector<double, SparseTag> m3(u3_dense); // [1 1 0]
    BOOST_CHECK_EQUAL(m3.nvals(), 3);

    {
        Vector<double> result(v3_ones, 0.);

        std::vector<double> ans = {16, 1, 1};
        Vector<double> answer(ans, 0.);

        reduce(result,
               m3,
               Second<double>(),
               Plus<double>(), mA);

        BOOST_CHECK_EQUAL(result.nvals(), 3);
        BOOST_CHECK_EQUAL(result, answer);
    }

    {
        Vector<double> result(v3_ones, 0.);

        std::vector<double> ans = {16, 0, 1};
        Vector<double> answer(ans, 0.);

        reduce(result,
               m3,
               NoAccumulate(),
               Plus<double>(), mA);

        BOOST_CHECK_EQUAL(result.nvals(), 2);
        BOOST_CHECK_EQUAL(result, answer);
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_reduce_matvec_mask_transpose)
{
    std::vector<std::vector<double> > m3x3_dense = {{12, 1, 3},
                                                    {0,  0, 0},
                                                    {7,  0, 9}};
    std::vector<double> u3_dense = {1, 1, 0};
    std::vector<double> v3_ones = {1, 1, 1};

    Matrix<double, DirectedMatrixTag> mA(m3x3_dense, 0.);
    Vector<double, SparseTag> m3(u3_dense, 0.); // [1 1 -]
    Vector<double> result(v3_ones, 0.);

    BOOST_CHECK_EQUAL(m3.nvals(), 2);

    std::vector<double> ans = {19, 1, 1};
    Vector<double> answer(ans, 0.);

    reduce(result,
           m3,
           NoAccumulate(),
           Plus<double>(), transpose(mA));

    BOOST_CHECK_EQUAL(result.nvals(), 3);
    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(reduce_matvec_test_nomask_noaccum)
{
    // Build some matrices.
    IndexArrayType i_A    = {0, 0, 1, 1, 1, 2, 2, 2, 3, 3};
    IndexArrayType j_A    = {0, 1, 0, 1, 2, 1, 2, 3, 2, 3};
    std::vector<double> v_A = {1, 1, 1, 2, 2, 1, 3, 5, 3, 4};
    Matrix<double, DirectedMatrixTag> A(4, 4);
    A.build(i_A, j_A, v_A);

    IndexArrayType j_C    = {0, 1, 2, 3};
    std::vector<double> v_C = {1, 1, 1, 1};
    Vector<double> C(4);
    C.build(j_C, v_C);

    IndexArrayType j_answer    = {0, 1, 2, 3};
    std::vector<double> v_answer = {2, 4, 8, 9};
    Vector<double> answer(4);
    answer.build(j_answer, v_answer);

    //col_reduce(A, C);
    reduce(C, NoMask(), NoAccumulate(),
           PlusMonoid<double>(), transpose(A));

    BOOST_CHECK_EQUAL(C, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(reduce_matvec_test_transpose_nomask_noaccum)
{
    // Build some matrices.
    IndexArrayType i_A    = {0, 0, 1, 1, 1, 2, 2, 2, 3, 3};
    IndexArrayType j_A    = {0, 1, 0, 1, 2, 1, 2, 3, 2, 3};
    std::vector<double> v_A = {1, 1, 1, 2, 2, 1, 3, 5, 3, 4};
    Matrix<double, DirectedMatrixTag> A(4, 4);
    A.build(i_A, j_A, v_A);

    IndexArrayType j_C    = {0, 1, 2, 3};
    std::vector<double> v_C = {1, 1, 1, 1};
    Vector<double> C(4);
    C.build(j_C, v_C);

    IndexArrayType j_answer    = {0, 1, 2, 3};
    std::vector<double> v_answer = {2, 4, 8, 9};
    Vector<double> answer(4);
    answer.build(j_answer, v_answer);

    //col_reduce(A, C);
    reduce(C, NoMask(), NoAccumulate(),
           PlusMonoid<double>(), transpose(A));

    BOOST_CHECK_EQUAL(C, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(reduce_matvec_test_transpose_nomask_accum)
{
    // Build some matrices.
    IndexArrayType i_A    = {0, 0, 1, 1, 1, 2, 2, 2, 3, 3};
    IndexArrayType j_A    = {0, 1, 0, 1, 2, 1, 2, 3, 2, 3};
    std::vector<double> v_A = {1, 1, 1, 2, 2, 1, 3, 5, 3, 4};
    Matrix<double, DirectedMatrixTag> A(4, 4);
    A.build(i_A, j_A, v_A);

    IndexArrayType j_C    = {0, 1, 2, 3};
    std::vector<double> v_C = {1, 1, 1, 1};
    Vector<double> C(4);
    C.build(j_C, v_C);

    IndexArrayType j_answer    = {0, 1, 2, 3};
    std::vector<double> v_answer = {3, 5, 9, 10};
    Vector<double> answer(4);
    answer.build(j_answer, v_answer);

    reduce(C, NoMask(), Plus<double>(),
           PlusMonoid<double>(), transpose(A));

    BOOST_CHECK_EQUAL(C, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(reduce_matvec_test_transpose_masked_accum)
{
    // Build some matrices.
    IndexArrayType i_A    = {0, 0, 1, 1, 1, 2, 2, 2, 3, 3};
    IndexArrayType j_A    = {0, 1, 0, 1, 2, 1, 2, 3, 2, 3};
    std::vector<double> v_A = {1, 1, 1, 2, 2, 1, 3, 5, 3, 4};
    Matrix<double, DirectedMatrixTag> A(4, 4);
    A.build(i_A, j_A, v_A);

    IndexArrayType j_C    = {0, 1, 2, 3};
    std::vector<double> v_C = {1, 1, 1, 1};
    Vector<double> C(4);
    C.build(j_C, v_C);

    IndexArrayType j_mask    = {0, 1, 2, 3};
    std::vector<double> v_mask = {1, 0, 1, 0};
    Vector<bool> mask(4);
    mask.build(j_mask, v_mask);

    IndexArrayType j_answer    = {0, 1, 2, 3};
    std::vector<double> v_answer = {2, 1, 8, 1};
    Vector<double> answer(4);
    answer.build(j_answer, v_answer);

    //colReduceMasked(A, C, mask);
    reduce(C, mask, NoAccumulate(),
           PlusMonoid<double>(), transpose(A));

    BOOST_CHECK_EQUAL(C, answer);
}

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

BOOST_AUTO_TEST_CASE(reduce_matvec_test_masked_noaccum)
{
    // Build some matrices.
    IndexArrayType i_A    = {0, 0, 1, 1, 1, 2, 2, 2, 3, 3};
    IndexArrayType j_A    = {0, 1, 0, 1, 2, 1, 2, 3, 2, 3};
    std::vector<double> v_A = {1, 1, 1, 2, 2, 1, 3, 5, 3, 4};
    Matrix<double, DirectedMatrixTag> A(4, 4);
    A.build(i_A, j_A, v_A);

    IndexArrayType i_C    = {0, 1, 2, 3};
    std::vector<double> v_C = {1, 1, 1, 1};
    Vector<double> C(4);
    C.build(i_C, v_C);

    IndexArrayType i_mask    = {0, 1, 2, 3};
    std::vector<double> v_mask = {1, 0, 1, 0};
    Vector<bool> mask(4);
    mask.build(i_mask, v_mask);

    IndexArrayType i_answer    = {0, 1, 2, 3};
    std::vector<double> v_answer = {2, 1, 9, 1};
    Vector<double> answer(4);
    answer.build(i_answer, v_answer);

    reduce(C, mask, NoAccumulate(),
           PlusMonoid<double>(), A);

    BOOST_CHECK_EQUAL(C, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_reduce_matvec_transpose_scmp_noaccum)
{
    std::vector<std::vector<double> > m3x3_dense = {{12, 1, 3},
                                                    {0,  0, 0},
                                                    {7,  0, 9}};
    std::vector<double> u3_dense = {1, 1, 0};
    std::vector<double> v3_ones = {1, 1, 1};

    Matrix<double, DirectedMatrixTag> mA(m3x3_dense, 0.);

    Vector<double, SparseTag> u3(u3_dense, 0.); // [1 1 -]
    Vector<double, SparseTag> m3(u3_dense, 0.); // [1 1 -]
    Vector<double> result(v3_ones, 0.);

    BOOST_CHECK_EQUAL(m3.nvals(), 2);

    std::vector<double> ans = {1, 1, 12};
    Vector<double> answer(ans, 0.);

    reduce(result,
           complement(m3),
           NoAccumulate(),
           Plus<double>(), transpose(mA));

    BOOST_CHECK_EQUAL(result.nvals(), 3);
    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_reduce_matvec_nomask_noaccum_stored_zero_result)
{
    // Build some matrices.
    std::vector<std::vector<int> > A = {{1, -1, 0, 0},
                                        {-1, -2, 2, 0},
                                        {0, -2, -3, 3},
                                        {0, 0, 0, 0}};
    Matrix<int, DirectedMatrixTag> mA(A, 0);
    Vector<int> result(4);

    // use a different sentinel value so that stored zeros are preserved.
    int const NIL(666);
    std::vector<int> ans = {  0,  -1, -2, NIL};
    Vector<int> answer(ans, NIL);

    reduce(result,
           NoMask(),
           NoAccumulate(), // Second<int>(),
           Plus<int>(), mA);
    BOOST_CHECK_EQUAL(result, answer);
    BOOST_CHECK_EQUAL(result.nvals(), 3);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_reduce_matvec_transpose_nomask_noaccum)
{
    std::vector<std::vector<double> > m3x3_dense = {{12, 1, 3},
                                                    {0,  0, 0},
                                                    {7,  0, 9}};

    std::vector<std::vector<double> > m3x4_dense = {{5, 0, 1, 0},
                                                    {6, 7, 0, 0},
                                                    {6, 7, 0, 0}};

    std::vector<double> u3_dense = {1, 1, 0};
    std::vector<double> u4_dense = {0, 0, 13, 1};

    Matrix<double, DirectedMatrixTag> mA(m3x3_dense, 0.);
    Matrix<double, DirectedMatrixTag> mB(m3x4_dense, 0.);
    Vector<double> u3(u3_dense, 0.);
    Vector<double> u4(u4_dense, 0.);
    Vector<double> result3(3);
    Vector<double> result4(4);

    std::vector<double> ansAT_dense = {19, 1, 12};
    std::vector<double> ansBT_dense = {17, 14, 1, 0};

    Vector<double> ansA(ansAT_dense, 0.);
    Vector<double> ansB(ansBT_dense, 0.);

    reduce(result3,
           NoMask(),
           NoAccumulate(),
           Plus<double>(),
           transpose(mA));
    BOOST_CHECK_EQUAL(result3, ansA);

    reduce(result4,
           NoMask(),
           NoAccumulate(),
           Plus<double>(),
           transpose(mB));
    BOOST_CHECK_EQUAL(result4, ansB);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_reduce_matvec_scmp_replace)
{
    std::vector<std::vector<double> > m3x3_dense = {{12, 1, 3},
                                                    {0,  0, 0},
                                                    {7,  0, 9}};
    std::vector<double> v3_ones = {1, 1, 1};

    Matrix<double, DirectedMatrixTag> mA(m3x3_dense, 0.);
    std::vector<double> scmp_mask_dense = {0., 0., 1.};
    Vector<double, SparseTag> m3(scmp_mask_dense, 0.);
    BOOST_CHECK_EQUAL(m3.nvals(), 1);

    {
        Vector<double> result(v3_ones, 0.);

        std::vector<double> ans = {16, 1, 0};
        Vector<double> answer(ans, 0.);

        reduce(result,
               complement(m3),
               Second<double>(),
               Plus<double>(), mA,
               REPLACE);

        BOOST_CHECK_EQUAL(result.nvals(), 2);
        BOOST_CHECK_EQUAL(result, answer);
    }

    {
        Vector<double> result(v3_ones, 0.);

        std::vector<double> ans = {16, 0, 0};
        Vector<double> answer(ans, 0.);

        reduce(result,
               complement(m3),
               NoAccumulate(),
               Plus<double>(), mA,
               REPLACE);

        BOOST_CHECK_EQUAL(result.nvals(), 1);
        BOOST_CHECK_EQUAL(result, answer);
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_reduce_matvec_scmp_replace_stored_zero)
{
    // tests a computed and stored zero
    // tests a stored zero in the mask
    // Build some matrices.
    std::vector<std::vector<int> > A_dense = {{1,-1, 0, 0},
                                              {1, 2, 2, 0},
                                              {0, 2, 3, 3},
                                              {0, 0, 3, 4}};
    std::vector<int> scmp_mask_dense =  { 0, 0, 0, 1 };
    std::vector<int> ones_dense =  { 1, 1, 1, 1};
    Matrix<int, DirectedMatrixTag> A(A_dense, 0);
    Vector<int> mask(scmp_mask_dense); //, 0);
    Vector<int> result(ones_dense, 0);

    BOOST_CHECK_EQUAL(mask.nvals(), 4);  // [ 0 0 0 1 ]

    int const NIL(666);
    std::vector<int> ans = { 0, 5, 8,  NIL};
    Vector<int> answer(ans, NIL);

    reduce(result,
           complement(mask),
           NoAccumulate(),
           Plus<double>(), A,
           REPLACE);
    BOOST_CHECK_EQUAL(result.nvals(), 3);
    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_reduce_matvec_scmp_replace_transpose)
{
    // Build some matrices.
    std::vector<std::vector<int> > A_dense = {{1, 1, 0, 0, 1},
                                              {-1, 2, 2, 0, 1},
                                              {0, 2, 3, 3, 1},
                                              {0, 0, 3, 4, 1}};
    std::vector<int> mask_dense =  { 0, 1, 0, 1, 0};
    std::vector<int> ones_dense =  { 1, 1, 1, 1, 1};
    Matrix<int, DirectedMatrixTag> A(A_dense, 0);
    Vector<int> mask(mask_dense, 0);
    Vector<int> result(ones_dense, 0);

    int const NIL(666);
    std::vector<int> ans = { 0, NIL, 8,  NIL, 4};
    Vector<int> answer(ans, NIL);

    reduce(result,
           complement(mask),
           NoAccumulate(),
           Plus<double>(), transpose(A),
           REPLACE);

    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_reduce_matvec_scmp_merge)
{
    std::vector<std::vector<double> > m3x3_dense = {{12, 1, 3},
                                                    {0,  0, 0},
                                                    {7,  0, 9}};
    std::vector<double> u3_dense = {1, 1, 0};
    std::vector<double> v3_ones = {1, 1, 1};

    Matrix<double, DirectedMatrixTag> mA(m3x3_dense, 0.);
    Vector<double, SparseTag> m3(u3_dense, 0.); // [1 1 -]
    Vector<double> result(v3_ones, 0.);

    BOOST_CHECK_EQUAL(m3.nvals(), 2);

    std::vector<double> ans = {1, 1, 16}; // if accum is NULL then {1, 0, 7};
    Vector<double> answer(ans, 0.);

    reduce(result,
           complement(m3),
           NoAccumulate(),
           Plus<double>(), mA);

    BOOST_CHECK_EQUAL(result.nvals(), 3); //2);
    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_reduce_matvec_scmp_merge_stored_zero)
{
    std::vector<std::vector<double> > m3x3_dense = {{12, 1, 3},
                                                    {0,  0, 0},
                                                    {7,  0, 9}};
    std::vector<double> u3_dense = {1, 1, 0};
    std::vector<double> v3_ones = {1, 1, 1};

    Matrix<double, DirectedMatrixTag> mA(m3x3_dense, 0.);
    Vector<double, SparseTag> m3(u3_dense); // [1 1 0]
    Vector<double> result(v3_ones, 0.);

    BOOST_CHECK_EQUAL(m3.nvals(), 3);

    std::vector<double> ans = {1, 1, 16};
    Vector<double> answer(ans, 0.);

    reduce(result,
           complement(m3),
           NoAccumulate(),
           Plus<double>(), mA);

    BOOST_CHECK_EQUAL(result.nvals(), 3); //2);
    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
// reduce vector to scalar
//****************************************************************************

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_reduce_vecscalar_reg)
{
    std::vector<double> ans3x3_dense  = {16, 0, 16};

    Vector<double> empty_result6(6);
    Vector<double> ansA(ans3x3_dense, 0.);

    // Vector to scalar reduction
    double val = 22;
    reduce(val,
           NoAccumulate(),
           PlusMonoid<double>(), empty_result6);
    BOOST_CHECK_EQUAL(val, PlusMonoid<double>().identity());

    val = 22;
    reduce(val,
           NoAccumulate(),
           PlusMonoid<double>(), ansA);
    BOOST_CHECK_EQUAL(val, 32.0);

    val = 22;
    reduce(val,
           NoAccumulate(),
           add_monoid(ArithmeticSemiring<double>()), ansA);
    BOOST_CHECK_EQUAL(val, 32.0);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_reduce_vecscalar_noaccum)
{
    // | - 2 3 3 |

    // Build some sparse vectors.
    std::vector<double> v = {0, 2, 3, 3};
    Vector<double> u(v, 0.0);

    {
        double val = 99.;
        reduce(val,
               NoAccumulate(),
               PlusMonoid<double>(),
               u);
        BOOST_CHECK_EQUAL(val, 8.0);
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_reduce_vecscalar_noaccum_empty)
{
    // | - 2 3 3 |

    // Build some sparse vectors.
    Vector<double> u(4);

    {
        double val = 99.;
        reduce(val,
               NoAccumulate(),
               PlusMonoid<double>(),
               u);
        BOOST_CHECK_EQUAL(val, 0.0);
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_reduce_vecscalar_accum)
{
    // | - 2 3 3 |

    // Build some sparse vectors.
    std::vector<double> v = {0, 2, 3, 3};
    Vector<double> u(v, 0.0);

    {
        double val = 99.;
        reduce(val,
               Plus<double>(),
               PlusMonoid<double>(),
               u);
        BOOST_CHECK_EQUAL(val, 107.0);
    }

    {
        double val = 99.;
        reduce(val,
               Second<double>(),
               PlusMonoid<double>(),
               u);
        BOOST_CHECK_EQUAL(val, 8.0);
    }
}

//****************************************************************************
// reduce matrix to scalar
//****************************************************************************

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_reduce_matscalar_noaccum)
{
    std::vector<std::vector<double> > m6x4_dense = {{5, 0, 1, 0},
                                                    {6, 7, 0, 0},
                                                    {6, 7, 0, 0},
                                                    {0, 5, 0, 0},
                                                    {6, 7, 0, 0},
                                                    {0, 0, 0, 0}};

    Matrix<double, DirectedMatrixTag> mB(m6x4_dense, 0.);

    // Matrix to scalar reduction
    double val = 22;
    reduce(val,
           NoAccumulate(),
           PlusMonoid<double>(), mB);
    BOOST_CHECK_EQUAL(val, 50.0);

    val = 22;
    reduce(val,
           NoAccumulate(),
           add_monoid(ArithmeticSemiring<double>()), mB);
    BOOST_CHECK_EQUAL(val, 50.0);
}

//****************************************************************************
//
// Transpose not supported
//
// BOOST_AUTO_TEST_CASE(test_reduce_matscalar_noaccum_transpose)
// {
//     std::vector<std::vector<double> > m6x4_dense = {{5, 0, 1, 0},
//                                                     {6, 7, 0, 0},
//                                                     {6, 7, 0, 0},
//                                                     {0, 5, 0, 0},
//                                                     {6, 7, 0, 0},
//                                                     {0, 0, 0, 0}};
//
//     Matrix<double, DirectedMatrixTag> mB(m6x4_dense, 0.);
//     // Matrix to scalar reduction
//     double val = 22;
//     reduce(val,
//            NoAccumulate(),
//            PlusMonoid<double>(), transpose(mB));
//     BOOST_CHECK_EQUAL(val, 50.0);
// }

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_reduce_matscalar_noaccum_empty)
{
    Matrix<double, DirectedMatrixTag> empty6x4(6, 4);

    // Matrix to scalar reduction
    double val = 22;
    reduce(val,
           NoAccumulate(),
           PlusMonoid<double>(), empty6x4);
    BOOST_CHECK_EQUAL(val, PlusMonoid<double>().identity());
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_reduce_matscalar_accum)
{
    std::vector<std::vector<double> > m6x4_dense = {{5, 0, 1, 0},
                                                    {6, 7, 0, 0},
                                                    {6, 7, 0, 0},
                                                    {0, 5, 0, 0},
                                                    {6, 7, 0, 0},
                                                    {0, 0, 0, 0}};

    Matrix<double, DirectedMatrixTag> mB(m6x4_dense, 0.);

    // Matrix to scalar reduction
    double val = 22;
    reduce(val,
           Plus<double>(),
           PlusMonoid<double>(), mB);
    BOOST_CHECK_EQUAL(val, 72.0);
}

//****************************************************************************
//
// Transpose not supported
//
// BOOST_AUTO_TEST_CASE(test_reduce_matscalar_accum_transpose)
// {
//     std::vector<std::vector<double> > m6x4_dense = {{5, 0, 1, 0},
//                                                     {6, 7, 0, 0},
//                                                     {6, 7, 0, 0},
//                                                     {0, 5, 0, 0},
//                                                     {6, 7, 0, 0},
//                                                     {0, 0, 0, 0}};
//     Matrix<double, DirectedMatrixTag> mB(m6x4_dense, 0.);
//     // Matrix to scalar reduction
//     double val = 22;
//     reduce(val,
//            NoAccumulate(),
//            PlusMonoid<double>(), transpose(mB));
//     BOOST_CHECK_EQUAL(val, 77.0);
// }

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_reduce_matscalar_accum_empty)
{
    Matrix<double, DirectedMatrixTag> empty6x4(6, 4);

    // Matrix to scalar reduction
    double val = 22;
    reduce(val,
           Plus<double>(),
           PlusMonoid<double>(), empty6x4);
    BOOST_CHECK_EQUAL(val, 22.0);
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#include <Eigen/Dense>
#include <memory>
#include <QQuickPaintedItem>
#include <QtQuick>
#include <QtWidgets>
#include <rrt/2dplane/GridStateSpace.hpp>
#include <rrt/BiRRT.hpp>

/**
 * This widget creates an RRT tree for searching a 2d space and draws it.
 * It has methods (slots) for stepping and resetting tree growth.
 * You can also draw and erase obstacles by clicking and dragging.
 *
 * This class is used within QML to show and interact with the rrt. The
 * Q_PROPERTY macros in here allow properties to be accessible within qml. The
 * slots declared below allow methods to be called within qml.
 */
class RRTWidget : public QQuickPaintedItem {
    Q_OBJECT

public:
    RRTWidget();

    Q_PROPERTY(int iterations READ iterations NOTIFY signal_stepped)
    int iterations() const { return _biRRT->iterationCount(); }

    Q_PROPERTY(double stepSize READ stepSize WRITE setStepSize)
    void setStepSize(double step);
    double stepSize() const { return _biRRT->stepSize(); }

    Q_PROPERTY(double goalBias READ goalBias WRITE setGoalBias)
    void setGoalBias(double bias);  //  bias is from 0 to 1
    double goalBias() const { return _biRRT->goalBias(); }

    Q_PROPERTY(double waypointBias READ waypointBias WRITE setWaypointBias)
    void setWaypointBias(double bias);  //  bias is from 0 to 1
    double waypointBias() const { return _biRRT->waypointBias(); }

    Q_PROPERTY(bool ascEnabled READ ascEnabled WRITE setASCEnabled)
    void setASCEnabled(bool enabled);
    double ascEnabled() const { return _biRRT->isASCEnabled(); }

public Q_SLOTS:
    void saveObstacles();
    void loadObstacles();
    void run();
    void stop();
    void reset();
    void clearObstacles();
    void step();
    void stepBig();

    // called on a timer interval by run()
    void _run_step();

Q_SIGNALS:
    void signal_stepped();

protected:
    void paint(QPainter* p) override;
    void drawTree(QPainter& painter, const RRT::Tree<Eigen::Vector2d>& rrt,
                  const RRT::Node<Eigen::Vector2d>* solutionNode = NULL,
                  QColor treeColor = Qt::blue, QColor solutionColor = Qt::red);
    void drawTerminalState(QPainter& painter, const Eigen::Vector2d& pos,
                           const Eigen::Vector2d& vel, const QColor& color);

    QPointF pointFromNode(const RRT::Node<Eigen::Vector2d>* n);

    void _step(int numTimes);

    void mousePressEvent(QMouseEvent* event);
    void mouseMoveEvent(QMouseEvent* event);
    void mouseReleaseEvent(QMouseEvent* event);

    static bool mouseInGrabbingRange(QMouseEvent* event,
                                     const Eigen::Vector2d& pt);

private:
    std::shared_ptr<RRT::GridStateSpace> _stateSpace;
    std::unique_ptr<RRT::BiRRT<Eigen::Vector2d>> _biRRT;

    Eigen::Vector2d _startVel, _goalVel;

    //  if you click down on an obstacle, you enter erase mode
    //  if you click down where there's no obstacle, you enter draw mode
    bool _editingObstacles, _erasingObstacles;

    enum {
        DraggingNone = 0,
        DraggingStart,
        DraggingGoal,
        DraggingStartVel,
        DraggingGoalVel
    } _draggingItem;

    int _waypointCacheMaxSize;

    std::vector<Eigen::Vector2d> _previousSolution;

    QTimer* _runTimer;
};


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is 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_GEOMETRY_ALGORITHMS_DETAIL_DISJOINT_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISJOINT_HPP

// Note: contrary to most files, the geometry::detail::disjoint namespace
// is partly implemented in a separate file, to avoid circular references
// disjoint -> get_turns -> disjoint

#include <cstddef>

#include <boost/range.hpp>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/reverse_dispatch.hpp>

#include <boost/geometry/algorithms/covered_by.hpp>

#include <boost/geometry/util/math.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace disjoint
{

struct disjoint_interrupt_policy
{
    static bool const enabled = true;
    bool has_intersections;

    inline disjoint_interrupt_policy()
        : has_intersections(false)
    {}

    template <typename Range>
    inline bool apply(Range const& range)
    {
        // If there is any IP in the range, it is NOT disjoint
        if (boost::size(range) > 0)
        {
            has_intersections = true;
            return true;
        }
        return false;
    }
};

template
<
    typename Point1, typename Point2,
    std::size_t Dimension, std::size_t DimensionCount
>
struct point_point
{
    static inline bool apply(Point1 const& p1, Point2 const& p2)
    {
        if (! geometry::math::equals(get<Dimension>(p1), get<Dimension>(p2)))
        {
            return true;
        }
        return point_point
            <
                Point1, Point2,
                Dimension + 1, DimensionCount
            >::apply(p1, p2);
    }
};

template <typename Point1, typename Point2, std::size_t DimensionCount>
struct point_point<Point1, Point2, DimensionCount, DimensionCount>
{
    static inline bool apply(Point1 const& , Point2 const& )
    {
        return false;
    }
};

template
<
    typename Point, typename Box,
    std::size_t Dimension, std::size_t DimensionCount
>
struct point_box
{
    static inline bool apply(Point const& point, Box const& box)
    {
        if (get<Dimension>(point) < get<min_corner, Dimension>(box)
            || get<Dimension>(point) > get<max_corner, Dimension>(box))
        {
            return true;
        }
        return point_box
            <
                Point, Box,
                Dimension + 1, DimensionCount
            >::apply(point, box);
    }
};

template <typename Point, typename Box, std::size_t DimensionCount>
struct point_box<Point, Box, DimensionCount, DimensionCount>
{
    static inline bool apply(Point const& , Box const& )
    {
        return false;
    }
};

template
<
    typename Box1, typename Box2,
    std::size_t Dimension, std::size_t DimensionCount
>
struct box_box
{
    static inline bool apply(Box1 const& box1, Box2 const& box2)
    {
        if (get<max_corner, Dimension>(box1) < get<min_corner, Dimension>(box2))
        {
            return true;
        }
        if (get<min_corner, Dimension>(box1) > get<max_corner, Dimension>(box2))
        {
            return true;
        }
        return box_box
            <
                Box1, Box2,
                Dimension + 1, DimensionCount
            >::apply(box1, box2);
    }
};

template <typename Box1, typename Box2, std::size_t DimensionCount>
struct box_box<Box1, Box2, DimensionCount, DimensionCount>
{
    static inline bool apply(Box1 const& , Box2 const& )
    {
        return false;
    }
};

// Segment - Box intersection
// Based on Ray-AABB intersection
// http://www.siggraph.org/education/materials/HyperGraph/raytrace/rtinter3.htm

// TODO - later maybe move to strategy::intersects and add a policy to conditionally extract intersection points

template <typename Point, typename Box, size_t I>
struct segment_box_intersection_dim
{
    //BOOST_STATIC_ASSERT(I < dimension<Box>::value);
    //BOOST_STATIC_ASSERT(I < dimension<Point>::value);
    //BOOST_STATIC_ASSERT(dimension<Point>::value == dimension<Box>::value);

    typedef typename coordinate_type<Point>::type point_coordinate;

    template <typename RelativeDistance> static inline
    bool apply(Point const& p0, Point const& p1, Box const& b, RelativeDistance & t_near, RelativeDistance & t_far)
    {
        //// WARNING! - RelativeDistance must be IEEE float for this to work (division by 0)
        //BOOST_STATIC_ASSERT(boost::is_float<RelativeDistance>::value);
        //// Ray origin is in segment point 0
        //RelativeDistance ray_d = geometry::get<I>(p1) - geometry::get<I>(p0);
        //RelativeDistance tn = ( geometry::get<min_corner, I>(b) - geometry::get<I>(p0) ) / ray_d;
        //RelativeDistance tf = ( geometry::get<max_corner, I>(b) - geometry::get<I>(p0) ) / ray_d;

        // TODO - should we support also unsigned integers?
        BOOST_STATIC_ASSERT(!boost::is_unsigned<point_coordinate>::value);
        point_coordinate ray_d = geometry::get<I>(p1) - geometry::get<I>(p0);
        RelativeDistance tn, tf;
        if ( is_zero(ray_d) )
        {
            tn = dist_div_by_zero<RelativeDistance>(geometry::get<min_corner, I>(b) - geometry::get<I>(p0));
            tf = dist_div_by_zero<RelativeDistance>(geometry::get<max_corner, I>(b) - geometry::get<I>(p0));
        }
        else
        {
            tn = static_cast<RelativeDistance>(geometry::get<min_corner, I>(b) - geometry::get<I>(p0)) / ray_d;
            tf = static_cast<RelativeDistance>(geometry::get<max_corner, I>(b) - geometry::get<I>(p0)) / ray_d;
        }

        if ( tf < tn )
            ::std::swap(tn, tf);

        if ( t_near < tn )
            t_near = tn;
        if ( tf < t_far )
            t_far = tf;

        return 0 <= t_far && t_near <= t_far && t_near <= 1;
    }

    template <typename R, typename T> static inline
    R dist_div_by_zero(T const& val)
    {
        if ( is_zero(val) )
            return 0;
        else if ( val < 0 )
            return -(::std::numeric_limits<R>::max)();
        else
            return (::std::numeric_limits<R>::max)();
    }

    template <typename T> static inline
    bool is_zero(T const& val)
    {
        // ray_d == 0 is here because eps of rational<int> is 0 which isn't < than 0
        return val == 0 || math::abs(val) < ::std::numeric_limits<T>::epsilon();
    }
};

template <typename Point, typename Box, size_t CurrentDimension>
struct segment_box_intersection_impl
{
    BOOST_STATIC_ASSERT(0 < CurrentDimension);

    typedef segment_box_intersection_dim<Point, Box, CurrentDimension - 1> for_dim;

    template <typename RelativeDistance>
    static inline bool apply(Point const& p0, Point const& p1, Box const& b,
                             RelativeDistance & t_near, RelativeDistance & t_far)
    {
        return segment_box_intersection_impl<Point, Box, CurrentDimension - 1>::apply(p0, p1, b, t_near, t_far)
            && for_dim::apply(p0, p1, b, t_near, t_far);
    }
};

template <typename Point, typename Box>
struct segment_box_intersection_impl<Point, Box, 1>
{
    typedef segment_box_intersection_dim<Point, Box, 0> for_dim;

    template <typename RelativeDistance>
    static inline bool apply(Point const& p0, Point const& p1, Box const& b,
                             RelativeDistance & t_near, RelativeDistance & t_far)
    {
        return for_dim::apply(p0, p1, b, t_near, t_far);
    }
};

template <typename Point, typename Box>
struct segment_box_intersection
{
    typedef segment_box_intersection_impl<Point, Box, dimension<Box>::value> impl;

    static inline bool apply(Point const& p0, Point const& p1, Box const& b)
    {
        typedef
        typename geometry::promote_floating_point<
            typename geometry::select_most_precise<
                typename coordinate_type<Point>::type,
                typename coordinate_type<Box>::type
            >::type
        >::type relative_distance_type;

        relative_distance_type t_near = -(::std::numeric_limits<relative_distance_type>::max)();
        relative_distance_type t_far = (::std::numeric_limits<relative_distance_type>::max)();

        // relative_distance = 0 < t_near ? t_near : 0;

        return impl::apply(p0, p1, b, t_near, t_far);
    }
};

template
<
    typename Geometry1, typename Geometry2
>
struct reverse_covered_by
{
    static inline bool apply(Geometry1 const& geometry1, Geometry2 const& geometry2)
    {
        return ! geometry::covered_by(geometry1, geometry2);
    }
};

/*!
    \brief Internal utility function to detect of boxes are disjoint
    \note Is used from other algorithms, declared separately
        to avoid circular references
 */
template <typename Box1, typename Box2>
inline bool disjoint_box_box(Box1 const& box1, Box2 const& box2)
{
    return box_box
        <
            Box1, Box2,
            0, dimension<Box1>::type::value
        >::apply(box1, box2);
}

/*!
    \brief Internal utility function to detect of points are disjoint
    \note To avoid circular references
 */
template <typename Point1, typename Point2>
inline bool disjoint_point_point(Point1 const& point1, Point2 const& point2)
{
    return point_point
        <
            Point1, Point2,
            0, dimension<Point1>::type::value
        >::apply(point1, point2);
}

}} // namespace detail::disjoint
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace equals
{

/*!
    \brief Internal utility function to detect of points are disjoint
    \note To avoid circular references
 */
template <typename Point1, typename Point2>
inline bool equals_point_point(Point1 const& point1, Point2 const& point2)
{
    return ! detail::disjoint::disjoint_point_point(point1, point2);
}

}} // namespace detail::equals
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISJOINT_HPP


#define BOOST_TEST_MODULE "test_pulling_force_interaction"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/core/BoundaryCondition.hpp>
#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/forcefield/external/PullingForceInteraction.hpp>
#include <random>

BOOST_AUTO_TEST_CASE(PullingForce)
{
    mjolnir::LoggerManager::set_default_logger(
            "test_pulling_force_interaction.log");

    using traits_type      = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type        = traits_type::real_type;
    using coordinate_type  = traits_type::coordinate_type;
    using boundary_type    = traits_type::boundary_type;
    using system_type      = mjolnir::System<traits_type>;
    using interaction_type = mjolnir::PullingForceInteraction<traits_type>;

    interaction_type interaction(
            std::vector<std::pair<std::size_t, coordinate_type>>{
                {0, coordinate_type( 0.1,  0.2,  0.3)},
                {2, coordinate_type(-0.3, -0.4, -0.5)}
            });

    system_type sys(4, boundary_type{});

    for(std::size_t i=0; i<sys.size(); ++i)
    {
        sys.mass(i)  = 1.0;
        sys.rmass(i) = 1.0;

        sys.position(i) = coordinate_type( 0.0,  0.0,  0.0);
        sys.velocity(i) = coordinate_type( 0.0,  0.0,  0.0);
        sys.force   (i) = coordinate_type( 0.0,  0.0,  0.0);

        sys.name (i) = "X";
        sys.group(i) = "NONE";
    }

    interaction.calc_force(sys);

    const real_type tol = 1e-6;

    BOOST_TEST( 0.1 == mjolnir::math::X(sys.force(0)), boost::test_tools::tolerance(tol));
    BOOST_TEST( 0.2 == mjolnir::math::Y(sys.force(0)), boost::test_tools::tolerance(tol));
    BOOST_TEST( 0.3 == mjolnir::math::Z(sys.force(0)), boost::test_tools::tolerance(tol));

    BOOST_TEST( 0.0 == mjolnir::math::X(sys.force(1)), boost::test_tools::tolerance(tol));
    BOOST_TEST( 0.0 == mjolnir::math::Y(sys.force(1)), boost::test_tools::tolerance(tol));
    BOOST_TEST( 0.0 == mjolnir::math::Z(sys.force(1)), boost::test_tools::tolerance(tol));

    BOOST_TEST(-0.3 == mjolnir::math::X(sys.force(2)), boost::test_tools::tolerance(tol));
    BOOST_TEST(-0.4 == mjolnir::math::Y(sys.force(2)), boost::test_tools::tolerance(tol));
    BOOST_TEST(-0.5 == mjolnir::math::Z(sys.force(2)), boost::test_tools::tolerance(tol));

    BOOST_TEST( 0.0 == mjolnir::math::X(sys.force(3)), boost::test_tools::tolerance(tol));
    BOOST_TEST( 0.0 == mjolnir::math::Y(sys.force(3)), boost::test_tools::tolerance(tol));
    BOOST_TEST( 0.0 == mjolnir::math::Z(sys.force(3)), boost::test_tools::tolerance(tol));
}


//==================================================================================================
/**
  Copyright 2016 Numscale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_AVX2_SIMD_FUNCTION_IS_GREATER_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_AVX2_SIMD_FUNCTION_IS_GREATER_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/meta/as_logical.hpp>
#include <boost/simd/constant/signmask.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;

   BOOST_DISPATCH_OVERLOAD( is_greater_
                          , (typename A0)
                          , bs::avx2_
                          , bs::pack_<bd::unsigned_<A0>, bs::avx_>
                          , bs::pack_<bd::unsigned_<A0>, bs::avx_>
                          )
   {
      using result = bs::as_logical_t<A0>;
      BOOST_FORCEINLINE result operator()( const A0& a0, const A0& a1) const BOOST_NOEXCEPT
      {
        using stype =  bd::as_integer_t<A0, signed>;
        auto const m = Signmask<stype>();
        return bitwise_cast<result>((bitwise_cast<stype>(a0) - m) > (bitwise_cast<stype>(a1) - m));
      }
   };

   BOOST_DISPATCH_OVERLOAD( is_greater_
                          , (typename A0)
                          , bs::avx2_
                          , bs::pack_< bd::int64_<A0>, bs::avx_>
                          , bs::pack_< bd::int64_<A0>, bs::avx_>
                          )
   {
      BOOST_FORCEINLINE bs::as_logical_t<A0>
      operator()(A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
      {
        return _mm256_cmpgt_epi64(a0,a1);
      }
   };

   BOOST_DISPATCH_OVERLOAD( is_greater_
                          , (typename A0)
                          , bs::avx2_
                          , bs::pack_< bd::int32_<A0>, bs::avx_>
                          , bs::pack_< bd::int32_<A0>, bs::avx_>
                          )
   {
      BOOST_FORCEINLINE bs::as_logical_t<A0>
      operator()(A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
      {
        return _mm256_cmpgt_epi32(a0,a1);
      }
   };

   BOOST_DISPATCH_OVERLOAD( is_greater_
                          , (typename A0)
                          , bs::avx2_
                          , bs::pack_< bd::int16_<A0>, bs::avx_>
                          , bs::pack_< bd::int16_<A0>, bs::avx_>
                          )
   {
      BOOST_FORCEINLINE bs::as_logical_t<A0>
      operator()(A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
      {
        return _mm256_cmpgt_epi16(a0,a1);
      }
   };

   BOOST_DISPATCH_OVERLOAD( is_greater_
                          , (typename A0)
                          , bs::avx2_
                          , bs::pack_< bd::int8_<A0>, bs::avx_>
                          , bs::pack_< bd::int8_<A0>, bs::avx_>
                          )
   {
      BOOST_FORCEINLINE bs::as_logical_t<A0>
      operator()(A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
      {
        return _mm256_cmpgt_epi8(a0,a1);
      }
   };
} } }

#endif


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

Copyright (c) 2013 Erik Erlandson

Author:  Erik Erlandson <erikerlandson@yahoo.com>

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

#if !defined(BOOST_ALGORITHM_SEQUENCE_DETAIL_EDIT_DISTANCE_HPP)
#define BOOST_ALGORITHM_SEQUENCE_DETAIL_EDIT_DISTANCE_HPP

// this requires me to link against boost system lib, which disappoints me,
// since it prevents my algorithms from being pure-template.
// maybe figure out how to use a pure-template pool allocator later
#include <boost/pool/object_pool.hpp>

#include <boost/heap/skew_heap.hpp>

#include <boost/range/metafunctions.hpp>

#include <boost/algorithm/sequence/detail/edit_distance_types.hpp>
#include <boost/algorithm/sequence/detail/edit_distance_script.hpp>

namespace boost {
namespace algorithm {
namespace sequence {
namespace detail {

using boost::distance;
using boost::range_iterator;

using boost::make_tuple;
using boost::enable_if;
using boost::is_same;
using boost::mpl::and_;
using std::iterator_traits;
using std::random_access_iterator_tag;

template <typename ForwardRange1, typename ForwardRange2, typename Output, typename Cost, typename Equal, typename AllowSub, typename MaxCost>
struct edit_cost_struct<ForwardRange1, ForwardRange2, Output, Cost, Equal, AllowSub, MaxCost,
                        typename enable_if<and_<is_same<Output, none>,
                                                not_<and_<range_category<ForwardRange1, ForwardRange2, random_access_iterator_tag>,
                                                          is_same<Cost, unit_cost>,
                                                          is_same<AllowSub, boost::false_type> > > > >::type> {
typedef typename cost_type<Cost, typename boost::range_value<ForwardRange1>::type>::type cost_t;
typedef typename range_iterator<ForwardRange1 const>::type itr1_t;
typedef typename range_iterator<ForwardRange2 const>::type itr2_t;
typedef path_head<itr1_t, itr2_t, cost_t> head_t;
typedef typename head_t::pos1_type pos1_t;
typedef typename head_t::pos2_type pos2_t;

cost_t max_cost_fallback(max_cost_checker<MaxCost, cost_t, head_t>& max_cost_check, bool max_cost_exception, const itr1_t end1, const itr2_t end2, const Cost& cost, const Equal& equal, sub_checker<AllowSub, Cost, cost_t, int> const& allow_sub) const {
    if (max_cost_exception) throw max_edit_cost_exception();

    head_t* h;
    max_cost_check.get(h);

    pos1_t j1 = h->pos1;
    pos2_t j2 = h->pos2;
    cost_t C = h->cost;
    while (true) {
        if (j1 == end1) {
            if (j2 == end2) {
                return C;
            } else {
                C += cost.insertion(*j2);
                ++j2;
            }
        } else {
            if (j2 == end2) {
                C += cost.deletion(*j1);
                ++j1;
            } else {
                C += (equal(*j1, *j2)) ? 0
                                       : ((allow_sub()) ? std::min(allow_sub.substitution(cost, *j1, *j2), (cost.deletion(*j1)+cost.insertion(*j2)))
                                                        : (cost.deletion(*j1)+cost.insertion(*j2))) ;
                ++j1;  ++j2;
            }
        }
    }
    return C;
}

// Default is generic edit distance algorithm based on a Dijkstra Single Source Shortest Path approach
typename cost_type<Cost, typename boost::range_value<ForwardRange1>::type>::type
operator()(ForwardRange1 const& seq1, ForwardRange2 const& seq2, none&, const Cost& cost, const Equal& equal, const AllowSub& allowsub, const MaxCost& max_cost, const bool max_cost_exception) const {

    head_t* const hnull = static_cast<head_t*>(NULL);

    const itr1_t end1 = boost::end(seq1);
    const itr2_t end2 = boost::end(seq2);
    pos1_t beg1;  beg1.beg(boost::begin(seq1));
    pos2_t beg2;  beg2.beg(boost::begin(seq2));

    // pool allocator for path nodes
    boost::object_pool<head_t> pool;

    // priority queue for path nodes
    boost::heap::skew_heap<head_t*, boost::heap::compare<heap_lessthan<pos1_t, pos2_t> > > heap(heap_lessthan<pos1_t, pos2_t>(beg1, beg2));

    sub_checker<AllowSub, Cost, cost_t, int> allow_sub(allowsub);

    max_cost_checker<MaxCost, cost_t, head_t> max_cost_check(max_cost, beg1, beg2);

    // keep track of nodes in the edit graph that have been visited
    typedef boost::unordered_set<head_t*, visited_hash<pos1_t,pos2_t>, visited_equal> visited_t;
    visited_t visited(31, visited_hash<pos1_t,pos2_t>(beg1,beg2));

    // kick off graph path frontier with initial node:
    heap.push(construct(pool, visited, beg1, beg2, cost_t(0)));

    // update frontier from least-cost node at each iteration, until we hit sequence end
    while (true) {
        head_t* h = heap.top();
        heap.pop();

        if (max_cost_check(h->cost)) {
            return max_cost_fallback(max_cost_check, max_cost_exception, end1, end2, cost, equal, allow_sub);
        }
        max_cost_check.update(h);

        if (h->pos1 == end1) {
            // if we are at end of both sequences, then we have our final cost:
            if (h->pos2 == end2) return h->cost;
            // sequence 1 is at end, so only consider insertion from seq2
            pos2_t p2 = h->pos2;
            head_t* t = construct(pool, visited, h->pos1, ++p2, h->cost + cost.insertion(*(h->pos2)));
            if (t != hnull) heap.push(t);
        } else if (h->pos2 == end2) {
            // sequence 2 is at end, so only consider deletion from seq1
            pos1_t p1 = h->pos1;
            head_t* t = construct(pool, visited, ++p1, h->pos2, h->cost + cost.deletion(*(h->pos1)));
            if (t != hnull) heap.push(t);
        } else {
            // interior of both sequences: consider insertion deletion and sub/eql:
            pos1_t p1 = h->pos1;  ++p1;
            pos1_t p1p = h->pos1;
            pos2_t p2 = h->pos2;  ++p2;
            pos2_t p2p = h->pos2;
            while (true) {
                const bool eq = equal(*p1p, *p2p);
                if (!eq  ||  p1 == end1  ||  p2 == end2) {
                    head_t* t;
                    if (allow_sub() || eq) {
                        t = construct(pool, visited, p1, p2, h->cost + ((eq) ? 0 : allow_sub.substitution(cost, *p1p, *p2p)));
                        if (t != hnull) heap.push(t);
                    }
                    t = construct(pool, visited, p1p, p2, h->cost + cost.insertion(*p2p));
                    if (t != hnull) heap.push(t);
                    t = construct(pool, visited, p1, p2p, h->cost + cost.deletion(*p1p));
                    if (t != hnull) heap.push(t);
                    break;
                }
                ++p1;  ++p2;  ++p1p;  ++p2p;
            }
        }
    }

    // control should not reach here
    BOOST_ASSERT(false);
    return 0;
}

}; // edit_cost_struct

template <typename Range1, typename Range2, typename Output, typename Equal, typename MaxCost>
struct edit_cost_struct<Range1, Range2, Output, unit_cost, Equal, boost::false_type, MaxCost,
                        typename enable_if<and_<is_same<Output, none>,
                                                range_category<Range1, Range2, random_access_iterator_tag> > >::type> {

typedef typename range_iterator<Range1 const>::type itr1_t;
typedef typename range_iterator<Range2 const>::type itr2_t;

typedef std::vector<int>::difference_type diff_type;
typedef std::vector<int>::size_type size_type;

typedef typename std::vector<diff_type>::iterator itrv_t;
typedef max_cost_checker_myers<MaxCost, diff_type, diff_type> max_cost_type;

std::string dump(const itr1_t& S1, const size_type& len1) const {
    std::string r;
    for (int j = 0; j < len1;  ++j) r += S1[j];
    return r;
}

template <typename Vec, typename Itr>
inline void expand(Vec& V_data, Itr& Vf, Itr& Vr, size_type& R, const diff_type& D, const diff_type& delta) const {
    size_type Rp = R + (R>>1);
    V_data.resize(2 + 4*Rp);
    Vf = V_data.begin() + R;
    Vr = V_data.begin() + (3*R+1) - delta;

    Itr Vp = V_data.begin() + (3*Rp+1) - delta;
    for (diff_type j=D+delta;  j >= -D+delta;  --j) Vp[j] = Vr[j];
    Vr = Vp;

    Vp = V_data.begin() + Rp;
    for (diff_type j=D;  j >= -D;  --j) Vp[j] = Vf[j];
    Vf = Vp;

    R = Rp;
}

diff_type max_cost_fallback(max_cost_type& max_cost_check, const bool max_cost_exception, const Equal& equal,
                            const itr1_t& S1, const size_type& L1, const itr2_t& S2, const size_type& L2,
                            const itrv_t& Vf, const itrv_t& Vr, const diff_type& delta, const diff_type& D) const {
    if (max_cost_exception) throw max_edit_cost_exception();

    for (diff_type k = -D;  k <= D;  k += 2) {
        max_cost_check.update(k, Vf, Vr, delta, L1, L2, D);
    }

    diff_type r1b, r2b, r1e, r2e;

    diff_type C = 0;
    diff_type k;
    remainder::kind kind;
    max_cost_check.get(k, kind);
    switch (kind) {
    case remainder::forward: {
            r1b = Vf[k];
            r2b = r1b-k;
            r1e = L1;
            r2e = L2;
            C = D;
        }; break;

        case remainder::reverse: {
            r1b = 0;
            r2b = 0;
            r1e = Vr[k];
            r2e = r1e-k;
            C = D;
        }; break;

        case remainder::bidirectional: {
            r1b = Vf[k];
            r2b = r1b-k;
            r1e = Vr[k];
            r2e = r1e-k;
            C = 2*D;
        }; break;

        default: BOOST_ASSERT(false);
    }

    // this is the part we bailed on due to hitting the maximum
    diff_type j1 = r1b;
    diff_type j2 = r2b;
    while (true) {
        if (j1 >= r1e) {
            if (j2 >= r2e) {
                break;
            } else {
                C += 1;
                ++j2;
            }
        } else {
            if (j2 >= r2e) {
                C += 1;
                ++j1;
            } else {
                if (!equal(S1[j1], S2[j2])) C += 2;
                ++j1;
                ++j2;
            }
        }
    }

    return C;
}

// If we are using unit cost for ins/del, with no substitution,
// and if our sequences support random-access,
// *then* we can invoke the efficient and elegant Myers algorithm:
//     An O(ND) Difference Algorithm and its Variations
//     by Eugene W. Myers
//     Dept of Computer Science, University of Arizona
typename cost_type<unit_cost, typename boost::range_value<Range1>::type>::type
operator()(Range1 const& seq1_, Range2 const& seq2_, none&, const unit_cost&, const Equal& equal, const boost::false_type&, const MaxCost& max_cost, const bool max_cost_exception) const {
    itr1_t seq1 = boost::begin(seq1_);
    itr2_t seq2 = boost::begin(seq2_);
    size_type len1 = distance(seq1_);
    size_type len2 = distance(seq2_);

    // identify any equal suffix and/or prefix
    diff_type eqb = 0;
    for (;  eqb < std::min(len1, len2);  ++eqb) if (!equal(seq1[eqb],seq2[eqb])) break;
    diff_type eqe = len1-1;
    for (diff_type j2 = len2-1;  eqe > eqb && j2 > eqb;  --eqe,--j2) if (!equal(seq1[eqe],seq2[j2])) break;
    eqe = len1-1-eqe;

    // sub-strings with equal suffix and/or prefix stripped
    const itr1_t S1 = seq1 + eqb;
    const diff_type L1 = len1-(eqb+eqe);
    const itr2_t S2 = seq2 + eqb;
    const diff_type L2 = len2-(eqb+eqe);

    // either or both strings are empty:
    if (L1 <= 0) return L2;
    if (L2 <= 0) return L1;

    const diff_type delta = L1-L2;
    const bool delta_even = delta%2 == 0;

    size_type R = 10;
    std::vector<diff_type> V_data(2*(1 + 2*R));
    itrv_t Vf = V_data.begin()+R;
    itrv_t Vr = V_data.begin()+(3*R+1)-delta;

    max_cost_type max_cost_check(max_cost);

    diff_type D = 0;
    Vf[1] = 0;
    Vr[-1+delta] = L1;
    while (true) {
        // advance forward-path diagonals:
        for (diff_type k = -D;  k <= D;  k += 2) {
            diff_type j1 = (k == -D  ||  (k != D  &&  Vf[k-1] < Vf[k+1]))  ?  Vf[k+1]  :  1+Vf[k-1];
            diff_type j2 = j1-k;
            if (!delta_even  &&  (k-delta) >= -(D-1)  &&  (k-delta) <= (D-1)) {
                diff_type r1 = Vr[k];
                diff_type r2 = Vr[k]-k;
                if ((j1-j2) == (r1-r2)  &&  j1 >= r1) return 2*D-1;
            }
            while (j1 < L1  &&  j2 < L2  &&  equal(S1[j1], S2[j2])) { ++j1;  ++j2; }
            Vf[k] = j1;
        }

        // advance the reverse-path diagonals:
        for (diff_type k = -D+delta;  k <= D+delta;  k += 2) {
            diff_type j1 = (k == D+delta  ||  (k != -D+delta  &&  Vr[k-1] < Vr[k+1]))  ?  Vr[k-1]  :  Vr[k+1]-1;
            diff_type j2 = j1-k;
            if (delta_even  &&  k >= -D  &&  k <= D) {
                diff_type f1 = Vf[k];
                diff_type f2 = Vf[k]-k;
                if ((j1-j2) == (f1-f2)  &&  f1 >= j1) return 2*D;
            }
            while (j1 > 0  &&  j2 > 0  &&  equal(S1[j1-1], S2[j2-1])) { --j1;  --j2; }
            Vr[k] = j1;
        }

        if (max_cost_check((delta_even) ? (2*D+2) : (2*D+1))) {
            return max_cost_fallback(max_cost_check, max_cost_exception, equal,
                                     S1, L1, S2, L2,
                                     Vf, Vr, delta, D);
        }

        // expand the working vector as needed
        if (D >= R) expand(V_data, Vf, Vr, R, D, delta);
        ++D;
    }

    // control should not reach here
    BOOST_ASSERT(false);
    return 0;
}

}; // edit_cost_struct

template <typename Range1, typename Range2, typename Output, typename Cost, typename Equal, typename AllowSub, typename MaxCost>
inline
typename cost_type<Cost, typename boost::range_value<Range1>::type>::type
edit_cost_impl(Range1 const& seq1, Range2 const& seq2, Output& output, const Cost& cost, const Equal& equal, const AllowSub& allow_sub, const MaxCost& max_cost, const bool max_cost_exception) {
    // specialize the most appropriate implementation for the given parameters
    return edit_cost_struct<Range1, Range2, Output, Cost, Equal, AllowSub, MaxCost>()(seq1, seq2, output, cost, equal, allow_sub, max_cost, max_cost_exception);
}

}}}}

#endif