#pragma once

#include <iostream>
#include <type_traits>
#include <unordered_set>
#include <vector>

#include <boost/heap/fibonacci_heap.hpp>

#include "heap.hpp"
#include "hypergraph.hpp"
#include "cut.hpp"

namespace hypergraphlib {

// A context for vertex ordering calculations
template<typename Heap>
struct OrderingContext {
  // Constructor
  OrderingContext(const std::vector<int> &vertices, size_t capacity) : heap(vertices, capacity) {}

  // Heap for tracking which vertices are most tightly connected to the
  // ordering
  Heap heap;

  std::unordered_map<
      int, boost::heap::fibonacci_heap<std::pair<size_t, int>>::handle_type>
      handles;

  // A mapping of edges to the number of vertices inside the edges that have not
  // been ordered
  std::unordered_map<int, int> edge_to_num_vertices_outside_ordering;

  // Vertices that have been marked as used by the ordering
  std::unordered_set<int> used_vertices;

  // Edges that have been marked as used by the ordering
  std::unordered_set<int> used_edges;
};

/* The method for calculating a vertex ordering for maximum adjacency and tight,
 * and Queyranne orderings is very similar - maintain how "tight" each vertex is
 * and repeatedly add the "tightest" vertex. The only difference is how
 * "tightness" is defined for each ordering. The "_tighten" functions define how
 * each ordering maintains tightness.
 *
 * Each of these methods runs in time linear to the number of edges incident on
 * v.
 */

// See [KW'96] for more details
template<typename HypergraphType>
void maximum_adjacency_ordering_tighten(
    const HypergraphType &hypergraph,
    OrderingContext<typename HypergraphType::Heap> &ctx,
    const int v) {
  // We are adding v to the ordering so far, and need to update the keys for the
  // other vertices
  // Check each edge e incident on v
  for (const int e : hypergraph.edges_incident_on(v)) {
    // If e has already been used, then skip it
    if (ctx.used_edges.find(e) != std::end(ctx.used_edges)) {
      continue;
    }
    // For every vertex u in e that is not v and not already in the ordering,
    // increment its key (it is one edge tighter now)
    // TODO I think vertices need to be removed from the edge's incidence list
    //      to maintain the runtime.
    for (const int u : hypergraph.edges().at(e)) {
      if (ctx.used_vertices.find(u) == std::end(ctx.used_vertices)) {
        if constexpr (std::is_same_v<HypergraphType, Hypergraph>) {
          ctx.heap.increment(u);
        } else {
          ctx.heap.increment(u, edge_weight(hypergraph, e));
        }
      }
    }
    ctx.used_edges.insert(e);
  }
}

// See [MW'00] for more details
template<typename HypergraphType>
void tight_ordering_tighten(
    const HypergraphType &hypergraph,
    OrderingContext<typename HypergraphType::Heap> &ctx,
    const int v) {
  // For every edge e incident on v
  for (const int e : hypergraph.edges_incident_on(v)) {
    // Tighten this edge
    ctx.edge_to_num_vertices_outside_ordering.at(e) -= 1ul;
    // If the edge only has one vertex u left that is outside the ordering,
    // increase the key of u
    if (ctx.edge_to_num_vertices_outside_ordering.at(e) == 1) {
      for (const int u : hypergraph.edges().at(e)) {
        if (ctx.used_vertices.find(u) == std::end(ctx.used_vertices)) {
          if constexpr (std::is_same_v<HypergraphType, Hypergraph>) {
            ctx.heap.increment(u);
          } else {
            ctx.heap.increment(u, edge_weight(hypergraph, e));
          }
        }
      }
    }
  }
}

// See [Q'98] for more details
template<typename HypergraphType>
void queyranne_ordering_tighten(
    const HypergraphType &hypergraph,
    OrderingContext<typename HypergraphType::Heap> &ctx,
    const int v) {
  maximum_adjacency_ordering_tighten(hypergraph, ctx, v);
  tight_ordering_tighten(hypergraph, ctx, v);
}

template<typename HypergraphType>
using tightening_t =
std::add_pointer_t<void(const HypergraphType &, OrderingContext<typename HypergraphType::Heap> &, const int)>;

/* Given a method to update the "tightness" of different vertices, computes a
 * vertex ordering and returns the ordering as well as a list of how tight each
 * vertex was when it was added to the ordering.
 *
 * Time complexity: O(p), where p is the size of the hypergraph, assuming that
 * TIGHTEN runs in time linear to the number of edges incident on the selected
 * vertex.
 */
template<typename HypergraphType, tightening_t<HypergraphType> TIGHTEN>
inline std::pair<std::vector<int>, std::vector<double>>
ordering(const HypergraphType &hypergraph, const int a) {
  std::vector<int> ordering = {a};
  std::vector<double> tightness = {0};
  std::vector<int> vertices_without_a;

  for (const auto v : hypergraph.vertices()) {
    if (v == a) {
      continue;
    }
    vertices_without_a.emplace_back(v);
  }

  // Multiply edges by 2 for Queyranne ordering
  OrderingContext<typename HypergraphType::Heap> ctx(vertices_without_a, 2 * hypergraph.num_edges() + 1);
  for (const auto &[e, vertices] : hypergraph.edges()) {
    ctx.edge_to_num_vertices_outside_ordering.insert({e, vertices.size()});
  }

  const auto tighten = [&hypergraph, &ctx](const int v) {
    ctx.used_vertices.insert(v);
    // It is the responsibility of TIGHTEN to update the context
    TIGHTEN(hypergraph, ctx, v);
  };

  tighten(a);

  while (ordering.size() < hypergraph.num_vertices()) {
    const auto[k, v] = ctx.heap.pop_key_val();
    ordering.emplace_back(v);
    // We need k / 2 instead of just k because this is just used for Queyranne
    // for now
    tightness.push_back(k / 2.0);
    tighten(v);
  }

  return {ordering, tightness};
}

/* Returns a maximum adjacency ordering of vertices, starting with vertex a.
 * Linear in the number of vertices across all hyperedges.
 *
 * Here, tightness for a vertex v is the number of edges that intersect the
 * ordering so far and v.
 *
 * See [KW'96] for more details.
 */
template<typename HypergraphType>
inline std::vector<int> maximum_adjacency_ordering(const HypergraphType &hypergraph,
                                                   const int a) {
  return ordering<HypergraphType, maximum_adjacency_ordering_tighten>(hypergraph, a).first;
}

/* Return a tight ordering of vertices, starting with vertex a. Linear in the
 * number of vertices across all hyperedges.
 *
 * Here, tightness is the number of edges connecting a vertex v to the
 * ordering so far that consist of vertices either in the ordering or v
 * itself.
 *
 * Takes time linear with the size of the hypergraph.
 *
 * See [MW'00] for more details.
 */
template<typename HypergraphType>
inline std::vector<int> tight_ordering(const HypergraphType &hypergraph, const int a) {
  return ordering<HypergraphType, tight_ordering_tighten>(hypergraph, a).first;
}

/* Return a Queyranne ordering of vertices, starting with vertex a.
 *
 * Takes time linear with the size of the hypergraph.
 *
 * See [Q'98] for more details.
 */
template<typename HypergraphType>
inline std::vector<int> queyranne_ordering(const HypergraphType &hypergraph, const int a) {
  return ordering<HypergraphType, queyranne_ordering_tighten>(hypergraph, a).first;
}

template<typename HypergraphType>
inline std::pair<std::vector<int>,
                 std::vector<double>> queyranne_ordering_with_tightness(const HypergraphType &hypergraph, const int a) {
  return ordering<HypergraphType, queyranne_ordering_tighten>(hypergraph, a);
}

template<typename HypergraphType>
using ordering_t = std::add_pointer_t<std::vector<int>(const HypergraphType &, const int)>;

/* Given a hypergraph and a function that orders the vertices, find the min cut
 * by repeatedly finding and contracting pendant pairs.
 *
 * Time complexity: O(np), where n is the number of vertices and p is the size
 * of the hypergraph
 *
 * Ordering should be one of `tight_ordering`, `queyranne_ordering`, or
 * `maximum_adjacency_ordering`.
 */
template<typename HypergraphType, ordering_t<HypergraphType> Ordering, bool ReturnPartitions>
auto vertex_ordering_minimum_cut_start_vertex(HypergraphType &hypergraph,
                                              const int a) -> typename HypergraphCutRet<HypergraphType,
                                                                                ReturnPartitions>::T {
  hypergraph.remove_singleton_and_empty_hyperedges();
  auto min_cut_of_phase = HypergraphCutRet<HypergraphType, ReturnPartitions>::max();
  while (hypergraph.num_vertices() > 1) {
    auto ordering = Ordering(hypergraph, a);
    const auto cut_of_phase = one_vertex_cut<ReturnPartitions>(hypergraph, ordering.back());
    hypergraph = merge_vertices(hypergraph, *(std::end(ordering) - 2),
                                *(std::end(ordering) - 1));
    min_cut_of_phase = std::min(min_cut_of_phase, cut_of_phase);
  }
  return min_cut_of_phase;
}

template<typename HypergraphType, ordering_t<HypergraphType> Ordering, bool ReturnPartitions>
auto vertex_ordering_mincut(HypergraphType &hypergraph) -> typename HypergraphCutRet<HypergraphType,
                                                                                ReturnPartitions>::T {
  const auto a = *std::begin(hypergraph.vertices());
  return vertex_ordering_minimum_cut_start_vertex<HypergraphType, Ordering, ReturnPartitions>(hypergraph, a);
}

template<typename HypergraphType>
inline auto MW_min_cut(HypergraphType &hypergraph) {
  return vertex_ordering_mincut<HypergraphType, tight_ordering, true>(hypergraph);
}
template<typename HypergraphType>
inline auto MW_min_cut_value(HypergraphType &hypergraph) {
  return vertex_ordering_mincut<HypergraphType, tight_ordering, false>(hypergraph);
}

template<typename HypergraphType>
inline auto Q_min_cut(HypergraphType &hypergraph) {
  return vertex_ordering_mincut<HypergraphType, queyranne_ordering, true>(hypergraph);
}
template<typename HypergraphType>
inline auto Q_min_cut_value(HypergraphType &hypergraph) {
  return vertex_ordering_mincut<HypergraphType, queyranne_ordering, false>(hypergraph);
}

template<typename HypergraphType>
inline auto KW_min_cut(HypergraphType &hypergraph) {
  return vertex_ordering_mincut<HypergraphType, maximum_adjacency_ordering, true>(hypergraph);
}
template<typename HypergraphType>
inline auto KW_min_cut_value(HypergraphType &hypergraph) {
  return vertex_ordering_mincut<HypergraphType, maximum_adjacency_ordering, false>(hypergraph);
}

}


#define BOOST_TEST_MODULE example
#include <boost/test/included/unit_test.hpp>

#include <iostream>
#include <vector>
#include <cmath>

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

#include "register_normal.hpp"
#include "gate_nand.hpp"
#include "gate_and.hpp"
#include "gate_nor.hpp"
#include "gate_or.hpp"
#include "gate_xor.hpp"
#include "gate_inv.hpp"
#include "gate_not.hpp"

#define WIDTH 8
#define RANDOM false

register_normal<WIDTH> test_register("Test_Register");
sc_signal<sc_uint<WIDTH> > test_register_input;
sc_signal<sc_uint<WIDTH> > test_register_output;
sc_signal<bool> test_register_enable;
sc_signal<bool> test_register_clock;
sc_signal<bool> test_register_nreset;

NAND<WIDTH> test_nand("Test_NAND_gate");
sc_signal<sc_uint<WIDTH> > test_nand_input;
sc_signal<bool> test_nand_output;

AND<WIDTH> test_and("Test_AND_gate");
sc_signal<sc_uint<WIDTH> > test_and_input;
sc_signal<bool> test_and_output;

NOR<WIDTH> test_nor("Test_NOR_gate");
sc_signal<sc_uint<WIDTH> > test_nor_input;
sc_signal<bool> test_nor_output;

OR<WIDTH> test_or("Test_OR_gate");
sc_signal<sc_uint<WIDTH> > test_or_input;
sc_signal<bool> test_or_output;

XOR<WIDTH> test_xor("Test_XOR_gate");
sc_signal<sc_uint<WIDTH> > test_xor_input;
sc_signal<bool> test_xor_output;

INV<WIDTH> test_inverter("Test_INV_gate");
sc_signal<sc_uint<WIDTH> > test_inverter_input;
sc_signal<sc_uint<WIDTH> > test_inverter_output;

NOT test_not("Test_NOT_gate");
sc_signal<bool> test_not_input;
sc_signal<bool> test_not_output;

bool initialised = false;
void initialise_connections() {
  test_register.Input(test_register_input);
  test_register.Output(test_register_output);
  test_register.Enable(test_register_enable);
  test_register.Clock(test_register_clock);
  test_register.N_Reset(test_register_nreset);

  test_nand.Input(test_nand_input);
  test_nand.Output(test_nand_output);

  test_and.Input(test_and_input);
  test_and.Output(test_and_output);

  test_nor.Input(test_nor_input);
  test_nor.Output(test_nor_output);

  test_or.Input(test_or_input);
  test_or.Output(test_or_output);

  test_xor.Input(test_xor_input);
  test_xor.Output(test_xor_output);

  test_inverter.Input(test_inverter_input);
  test_inverter.Output(test_inverter_output);

  test_not.Input(test_not_input);
  test_not.Output(test_not_output);

  initialised = true;
}

bool PROTO_NOT(const bool &a) {
  return !a;
}

template<const unsigned int N>
bool PROTO_OR(const sc_uint<N> &a) {
  return a != 0;
}

template<const unsigned int N>
bool PROTO_AND(const sc_uint<N> &a) {
  return a == -1;
}

template<const unsigned int N>
bool PROTO_NOR(const sc_uint<N> &a) {
  return PROTO_NOT(PROTO_OR<N>(a));
}

template<const unsigned int N>
bool PROTO_NAND(const sc_uint<N> &a) {
  return PROTO_NOT(PROTO_AND<N>(a));
}

template<const unsigned int N>
bool PROTO_XOR(const sc_uint<N> &a) {
  int numberOfBits = 0;
  sc_uint<N> temp = a;
  for (unsigned int i=0; i<N; i++) {
    numberOfBits += temp & 0x01;
    temp = temp >> 1;
  }

  return numberOfBits == 1;
}

template<const unsigned int N>
sc_uint<N> PROTO_INV(const sc_uint<N> &a) {
  return (1 << N) - 1 - a;
}

template<const unsigned int N>
std::vector<sc_uint<N> > comb_generate_test_vector(const bool &shuffle = RANDOM) {
  const unsigned long long maximumNumber = 1 << N;
  std::vector<sc_uint<N> > toReturn;

  // So no time is wasted resizing the vector
  toReturn.reserve(maximumNumber);

  // Add all of the numbers
  for (unsigned long long i = 0; i<maximumNumber; i++) {
    toReturn.push_back(i);
  }

  // Shuffle if required
  if (shuffle)
    std::random_shuffle (toReturn.begin(), toReturn.end());

  return toReturn;
}

BOOST_AUTO_TEST_CASE( test_register_ )
{
  std::vector<sc_uint<WIDTH> > aVec;

  if(!initialised) initialise_connections();
  aVec = comb_generate_test_vector<WIDTH>();

  test_register_clock.write(false);
  sc_start(10,SC_NS);

  test_register_enable.write(true);
  test_register_nreset.write(true);
  test_register_clock.write(false);

  for(std::vector<sc_uint<WIDTH> >::iterator aIt = aVec.begin(); aIt != aVec.end(); ++aIt) {
    // Check the async reset
    test_register_input.write(-1);
    test_register_nreset = 0;
    sc_start(1, SC_NS);
    BOOST_CHECK(test_register_output.read() == 0);
    test_register_nreset = 1;
    sc_start(1,SC_NS);

    // Clock in data
    test_register_input.write(*aIt);
    sc_start(1, SC_NS);
    test_register_clock.write(true);
    sc_start(1, SC_NS);
    test_register_clock.write(false);
    sc_start(1, SC_NS);
    BOOST_CHECK(test_register_output.read() == *aIt);

    // Check enable
    test_register_input.write(0);
    test_register_enable = 0;
    test_register_input.write(*aIt);
    sc_start(1, SC_NS);
    test_register_clock.write(true);
    sc_start(1, SC_NS);
    test_register_enable = 1;
    test_register_clock.write(false);
    sc_start(1, SC_NS);
    BOOST_CHECK(test_register_output.read() == *aIt);

    // TODO check enable and reset at the same time?
  }
}

BOOST_AUTO_TEST_CASE( test_nand_gate )
{
  std::vector<sc_uint<WIDTH> > aVec;

  if (!initialised) initialise_connections();

  aVec = comb_generate_test_vector<WIDTH>();

  sc_start(10, SC_NS);

  for(std::vector<sc_uint<WIDTH> >::iterator aIt = aVec.begin(); aIt != aVec.end(); ++aIt) {
    test_nand_input.write(*aIt);
    sc_start(1, SC_NS);
    BOOST_CHECK(test_nand_output.read() == PROTO_NAND<WIDTH>(*aIt));
  }
  sc_pause();
}

BOOST_AUTO_TEST_CASE( test_and_gate )
{
  std::vector<sc_uint<WIDTH> > aVec;

  if (!initialised) initialise_connections();

  aVec = comb_generate_test_vector<WIDTH>();

  sc_start(10, SC_NS);

  for(std::vector<sc_uint<WIDTH> >::iterator aIt = aVec.begin(); aIt != aVec.end(); ++aIt) {
    test_and_input.write(*aIt);
    sc_start(1, SC_NS);
    BOOST_CHECK(test_and_output.read() == PROTO_AND<WIDTH>(*aIt));
  }
  sc_pause();
}

BOOST_AUTO_TEST_CASE( test_nor_gate )
{
  std::vector<sc_uint<WIDTH> > aVec;

  if (!initialised) initialise_connections();

  aVec = comb_generate_test_vector<WIDTH>();
  sc_start(10, SC_NS);

  for(std::vector<sc_uint<WIDTH> >::iterator aIt = aVec.begin(); aIt != aVec.end(); ++aIt) {
    test_nor_input.write(*aIt);
    sc_start(1, SC_NS);
    BOOST_CHECK(test_nor_output.read() == PROTO_NOR<WIDTH>(*aIt));
  }
  sc_pause();
}

BOOST_AUTO_TEST_CASE( test_or_gate )
{
  std::vector<sc_uint<WIDTH> > aVec;

  if (!initialised) initialise_connections();

  aVec = comb_generate_test_vector<WIDTH>();

  sc_start(10, SC_NS);

  for(std::vector<sc_uint<WIDTH> >::iterator aIt = aVec.begin(); aIt != aVec.end(); ++aIt) {
    test_or_input.write(*aIt);

    sc_start(1, SC_NS);
    BOOST_CHECK(test_or_output.read() == PROTO_OR<WIDTH>(*aIt));
  }
  sc_pause();
}

BOOST_AUTO_TEST_CASE( test_xor_gate )
{
  std::vector<sc_uint<WIDTH> > aVec;

  if (!initialised) initialise_connections();

  aVec = comb_generate_test_vector<WIDTH>();

  sc_start(10, SC_NS);

  for(std::vector<sc_uint<WIDTH> >::iterator aIt = aVec.begin(); aIt != aVec.end(); ++aIt) {
    test_xor_input.write(*aIt);

    sc_start(1, SC_NS);
    BOOST_CHECK(test_xor_output.read() == PROTO_XOR<WIDTH>(*aIt));
  }
  sc_pause();
}

BOOST_AUTO_TEST_CASE( test_inv_gate )
{
  std::vector<sc_uint<WIDTH> > aVec;

  if (!initialised) initialise_connections();

  aVec = comb_generate_test_vector<WIDTH>();

  sc_start(10, SC_NS);

  for(std::vector<sc_uint<WIDTH> >::iterator aIt = aVec.begin(); aIt != aVec.end(); ++aIt) {
    test_inverter_input.write(*aIt);

    sc_start(1, SC_NS);
    BOOST_CHECK(test_inverter_output.read() == PROTO_INV<WIDTH>(*aIt));
  }

  sc_pause();
}

BOOST_AUTO_TEST_CASE( test_not_gate )
{
  std::vector<bool> aVec;

  if (!initialised) initialise_connections();

  aVec.push_back(true);
  aVec.push_back(false);

  sc_start(10, SC_NS);

  for(std::vector<bool>::iterator aIt = aVec.begin(); aIt != aVec.end(); ++aIt) {
    test_not_input.write(*aIt);

    sc_start(1, SC_NS);
    BOOST_CHECK(test_not_output.read() == PROTO_NOT(*aIt));
  }

  sc_pause();
}

int sc_main(int argc, char* argv[]) {
  return 0;
}


//-----------------------------------------------------------------------------
// Copyright (c) 2015-2018 Benjamin Buch
//
// https://github.com/bebuch/mitrax
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt)
//-----------------------------------------------------------------------------
#define BOOST_TEST_MODULE mitrax dim
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

#include <functional>

#include <mitrax/dim.hpp>

namespace utf = boost::unit_test;
using boost::typeindex::type_id;
using boost::typeindex::type_id_runtime;
using namespace mitrax;

#define CP(expr) [&]{ constexpr auto tmp = expr; return tmp; }()

template < typename T >
auto rt_id(T&& v){
	return type_id_runtime(static_cast< T&& >(v));
}

template < typename T >
auto const id = type_id< T >();

template < bool Nct, typename CT, size_t N >
using DimT = dim_type< Nct, CT, CT(N) >;

template < typename CT >
void construct(){
	constexpr auto cct = DimT< true,  CT, 3 >();
	constexpr auto crt = DimT< false, CT, 3 >();
	constexpr auto rrt = DimT< false, CT, 0 >(CT(3));

	BOOST_TEST((rt_id(cct) == id< DimT< true,  CT, 3 > >));
	BOOST_TEST((rt_id(crt) == id< DimT< false, CT, 3 > >));
	BOOST_TEST((rt_id(rrt) == id< DimT< false, CT, 0 > >));

	BOOST_TEST((size_t(cct) == 3));
	BOOST_TEST((size_t(crt) == 3));
	BOOST_TEST((size_t(rrt) == 3));
}

template < typename CT >
void default_construct(){
	constexpr auto cct = DimT< true,  CT, 3 >();
	constexpr auto crt = DimT< false, CT, 3 >();
	constexpr auto rrt = DimT< false, CT, 0 >();

	BOOST_TEST((rt_id(cct) == id< DimT< true,  CT, 3 > >));
	BOOST_TEST((rt_id(crt) == id< DimT< false, CT, 3 > >));
	BOOST_TEST((rt_id(rrt) == id< DimT< false, CT, 0 > >));

	BOOST_TEST((size_t(cct) == 3));
	BOOST_TEST((size_t(crt) == 3));
	BOOST_TEST((size_t(rrt) == 0));
}

void index_literal(){
	BOOST_TEST((rt_id(3_c) == id< c_t >));
	BOOST_TEST((rt_id(3_r) == id< r_t >));
	BOOST_TEST((rt_id(3_d) == id< d_t >));
}

void dim_t_literal(){
	BOOST_TEST((rt_id(3_C) == id< col_t >));
	BOOST_TEST((rt_id(3_R) == id< row_t >));
	BOOST_TEST((rt_id(3_D) == id< dim_t >));
}

void dim_literal(){
	BOOST_TEST((rt_id(3_CS) == id< col< true,  3_C > >));
	BOOST_TEST((rt_id(3_CD) == id< col< false, 3_C > >));

	BOOST_TEST((rt_id(3_RS) == id< row< true,  3_R > >));
	BOOST_TEST((rt_id(3_RD) == id< row< false, 3_R > >));

	BOOST_TEST((rt_id(3_DS) == id< dim< true,  3_D > >));
	BOOST_TEST((rt_id(3_DD) == id< dim< false, 3_D > >));
}

template < typename CT >
void convert2size_t(){
	constexpr auto cct = DimT< true,  CT, 3 >();
	constexpr auto crt = DimT< false, CT, 3 >();
	constexpr auto rrt = DimT< false, CT, 0 >(CT(3));

	BOOST_TEST((rt_id(size_t(cct)) == id< size_t >));
	BOOST_TEST((rt_id(size_t(crt)) == id< size_t >));
	BOOST_TEST((rt_id(size_t(rrt)) == id< size_t >));

	BOOST_TEST((CP(size_t(cct)) == 3));
	BOOST_TEST((CP(size_t(crt)) == 3));
	BOOST_TEST((CP(size_t(rrt)) == 3));
}

template < typename CT >
void convert2dim(){
	constexpr auto cct = DimT< true,  CT, 3 >();
	constexpr auto crt = DimT< false, CT, 3 >();
	constexpr auto rrt = DimT< false, CT, 0 >(CT(3));

	BOOST_TEST((rt_id(cct.as_col()) == id< col< true,  3_C > >));
	BOOST_TEST((rt_id(crt.as_col()) == id< col< false, 3_C > >));
	BOOST_TEST((rt_id(rrt.as_col()) == id< col< false, 0_C > >));

	BOOST_TEST((CP(cct.as_col()) == 3_CS));
	BOOST_TEST((CP(crt.as_col()) == 3_CS));
	BOOST_TEST((CP(rrt.as_col()) == 3_CS));

	BOOST_TEST((rt_id(cct.as_row()) == id< row< true,  3_R > >));
	BOOST_TEST((rt_id(crt.as_row()) == id< row< false, 3_R > >));
	BOOST_TEST((rt_id(rrt.as_row()) == id< row< false, 0_R > >));

	BOOST_TEST((CP(cct.as_row()) == 3_RS));
	BOOST_TEST((CP(crt.as_row()) == 3_RS));
	BOOST_TEST((CP(rrt.as_row()) == 3_RS));

	BOOST_TEST((rt_id(cct.as_dim()) == id< dim< true,  3_D > >));
	BOOST_TEST((rt_id(crt.as_dim()) == id< dim< false, 3_D > >));
	BOOST_TEST((rt_id(rrt.as_dim()) == id< dim< false, 0_D > >));

	BOOST_TEST((CP(cct.as_dim()) == 3_DS));
	BOOST_TEST((CP(crt.as_dim()) == 3_DS));
	BOOST_TEST((CP(rrt.as_dim()) == 3_DS));
}

template < typename CT, typename Op >
void arithmetic(){
	constexpr auto op = Op();
	constexpr auto l = 4;
	constexpr auto r = 3;

	constexpr auto cct1 = DimT< true,  CT, l >();
	constexpr auto crt1 = DimT< false, CT, l >();
	constexpr auto rrt1 = DimT< false, CT, 0 >(CT(l));

	constexpr auto cct2 = DimT< true,  CT, r >();
	constexpr auto crt2 = DimT< false, CT, r >();
	constexpr auto rrt2 = DimT< false, CT, 0 >(CT(r));

	BOOST_TEST((rt_id(CP(op(cct1, cct2))) == id< DimT< true,  CT, op(l, r) > >));
	BOOST_TEST((rt_id(CP(op(cct1, crt2))) == id< DimT< true,  CT, op(l, r) > >));
	BOOST_TEST((rt_id(CP(op(cct1, rrt2))) == id< DimT< false, CT, 0 > >));

	BOOST_TEST((rt_id(CP(op(crt1, cct2))) == id< DimT< true,  CT, op(l, r) > >));
	BOOST_TEST((rt_id(CP(op(crt1, crt2))) == id< DimT< false, CT, op(l, r) > >));
	BOOST_TEST((rt_id(CP(op(crt1, rrt2))) == id< DimT< false, CT, 0 > >));

	BOOST_TEST((rt_id(CP(op(rrt1, cct2))) == id< DimT< false, CT, 0 > >));
	BOOST_TEST((rt_id(CP(op(rrt1, crt2))) == id< DimT< false, CT, 0 > >));
	BOOST_TEST((rt_id(CP(op(rrt1, rrt2))) == id< DimT< false, CT, 0 > >));

	BOOST_TEST((size_t(CP(op(cct1, cct2))) == op(l, r)));
	BOOST_TEST((size_t(CP(op(cct1, crt2))) == op(l, r)));
	BOOST_TEST((size_t(CP(op(cct1, rrt2))) == op(l, r)));

	BOOST_TEST((size_t(CP(op(crt1, cct2))) == op(l, r)));
	BOOST_TEST((size_t(CP(op(crt1, crt2))) == op(l, r)));
	BOOST_TEST((size_t(CP(op(crt1, rrt2))) == op(l, r)));

	BOOST_TEST((size_t(CP(op(rrt1, cct2))) == op(l, r)));
	BOOST_TEST((size_t(CP(op(rrt1, crt2))) == op(l, r)));
	BOOST_TEST((size_t(CP(op(rrt1, rrt2))) == op(l, r)));
}

template < typename CT, typename Op >
void compare(){
	constexpr auto op = Op();
	constexpr auto l = 4;
	constexpr auto r = 3;

	constexpr auto cct1 = DimT< true,  CT, l >();
	constexpr auto crt1 = DimT< false, CT, l >();
	constexpr auto rrt1 = DimT< false, CT, 0 >(CT(l));

	constexpr auto cct2 = DimT< true,  CT, r >();
	constexpr auto crt2 = DimT< false, CT, r >();
	constexpr auto rrt2 = DimT< false, CT, 0 >(CT(r));

	BOOST_TEST((rt_id(CP(op(cct1, cct2))) == id< bool >));
	BOOST_TEST((rt_id(CP(op(cct1, crt2))) == id< bool >));
	BOOST_TEST((rt_id(CP(op(cct1, rrt2))) == id< bool >));

	BOOST_TEST((rt_id(CP(op(crt1, cct2))) == id< bool >));
	BOOST_TEST((rt_id(CP(op(crt1, crt2))) == id< bool >));
	BOOST_TEST((rt_id(CP(op(crt1, rrt2))) == id< bool >));

	BOOST_TEST((rt_id(CP(op(rrt1, cct2))) == id< bool >));
	BOOST_TEST((rt_id(CP(op(rrt1, crt2))) == id< bool >));
	BOOST_TEST((rt_id(CP(op(rrt1, rrt2))) == id< bool >));

	BOOST_TEST((CP(op(cct1, cct2)) == op(l, r)));
	BOOST_TEST((CP(op(cct1, crt2)) == op(l, r)));
	BOOST_TEST((CP(op(cct1, rrt2)) == op(l, r)));

	BOOST_TEST((CP(op(crt1, cct2)) == op(l, r)));
	BOOST_TEST((CP(op(crt1, crt2)) == op(l, r)));
	BOOST_TEST((CP(op(crt1, rrt2)) == op(l, r)));

	BOOST_TEST((CP(op(rrt1, cct2)) == op(l, r)));
	BOOST_TEST((CP(op(rrt1, crt2)) == op(l, r)));
	BOOST_TEST((CP(op(rrt1, rrt2)) == op(l, r)));
}

template < typename CT >
void get_dim(){
	constexpr auto v = 7;

	constexpr auto d1 = DimT< true,  CT, v >();
	constexpr auto d2 = DimT< false, CT, v >();
	constexpr auto d3 = DimT< false, CT, 0 >(CT(v));

	constexpr auto res01 = get(d1);
	constexpr auto res02 = get(d2);
	constexpr auto res03 = get(d3);
	constexpr auto res04 = get(d1, d1);
	constexpr auto res05 = get(d2, d2);
	constexpr auto res06 = get(d3, d3);
	constexpr auto res07 = get(d1, d2);
	constexpr auto res08 = get(d1, d3);
	constexpr auto res09 = get(d2, d1);
	constexpr auto res10 = get(d2, d3);
	constexpr auto res11 = get(d3, d1);
	constexpr auto res12 = get(d3, d2);
	constexpr auto res13 = get(d1, d1, d1);
	constexpr auto res14 = get(d2, d2, d2);
	constexpr auto res15 = get(d3, d3, d3);
	constexpr auto res16 = get(d1, d2, d1);
	constexpr auto res17 = get(d1, d2, d2);
	constexpr auto res18 = get(d1, d2, d3);
	constexpr auto res19 = get(d1, d3, d1);
	constexpr auto res20 = get(d1, d3, d2);
	constexpr auto res21 = get(d1, d3, d3);
	constexpr auto res22 = get(d2, d1, d1);
	constexpr auto res23 = get(d2, d1, d2);
	constexpr auto res24 = get(d2, d1, d3);
	constexpr auto res25 = get(d2, d3, d1);
	constexpr auto res26 = get(d2, d3, d2);
	constexpr auto res27 = get(d2, d3, d3);
	constexpr auto res28 = get(d3, d1, d1);
	constexpr auto res29 = get(d3, d1, d2);
	constexpr auto res30 = get(d3, d1, d3);
	constexpr auto res31 = get(d3, d2, d1);
	constexpr auto res32 = get(d3, d2, d2);
	constexpr auto res33 = get(d3, d2, d3);

	BOOST_TEST((size_t(res01) == 7));
	BOOST_TEST((size_t(res02) == 7));
	BOOST_TEST((size_t(res03) == 7));
	BOOST_TEST((size_t(res04) == 7));
	BOOST_TEST((size_t(res05) == 7));
	BOOST_TEST((size_t(res06) == 7));
	BOOST_TEST((size_t(res07) == 7));
	BOOST_TEST((size_t(res08) == 7));
	BOOST_TEST((size_t(res09) == 7));
	BOOST_TEST((size_t(res10) == 7));
	BOOST_TEST((size_t(res11) == 7));
	BOOST_TEST((size_t(res12) == 7));
	BOOST_TEST((size_t(res13) == 7));
	BOOST_TEST((size_t(res14) == 7));
	BOOST_TEST((size_t(res15) == 7));
	BOOST_TEST((size_t(res16) == 7));
	BOOST_TEST((size_t(res17) == 7));
	BOOST_TEST((size_t(res18) == 7));
	BOOST_TEST((size_t(res19) == 7));
	BOOST_TEST((size_t(res20) == 7));
	BOOST_TEST((size_t(res21) == 7));
	BOOST_TEST((size_t(res22) == 7));
	BOOST_TEST((size_t(res23) == 7));
	BOOST_TEST((size_t(res24) == 7));
	BOOST_TEST((size_t(res25) == 7));
	BOOST_TEST((size_t(res26) == 7));
	BOOST_TEST((size_t(res27) == 7));
	BOOST_TEST((size_t(res28) == 7));
	BOOST_TEST((size_t(res29) == 7));
	BOOST_TEST((size_t(res30) == 7));
	BOOST_TEST((size_t(res31) == 7));
	BOOST_TEST((size_t(res32) == 7));
	BOOST_TEST((size_t(res33) == 7));

	BOOST_TEST((rt_id(res01) == (id< DimT< true,  CT, 7 > >)));
	BOOST_TEST((rt_id(res02) == (id< DimT< false, CT, 7 > >)));
	BOOST_TEST((rt_id(res03) == (id< DimT< false, CT, 0 > >)));
	BOOST_TEST((rt_id(res04) == (id< DimT< true,  CT, 7 > >)));
	BOOST_TEST((rt_id(res05) == (id< DimT< false, CT, 7 > >)));
	BOOST_TEST((rt_id(res06) == (id< DimT< false, CT, 0 > >)));
	BOOST_TEST((rt_id(res07) == (id< DimT< true,  CT, 7 > >)));
	BOOST_TEST((rt_id(res08) == (id< DimT< true,  CT, 7 > >)));
	BOOST_TEST((rt_id(res09) == (id< DimT< true,  CT, 7 > >)));
	BOOST_TEST((rt_id(res10) == (id< DimT< false, CT, 7 > >)));
	BOOST_TEST((rt_id(res11) == (id< DimT< true,  CT, 7 > >)));
	BOOST_TEST((rt_id(res12) == (id< DimT< false, CT, 7 > >)));
	BOOST_TEST((rt_id(res13) == (id< DimT< true,  CT, 7 > >)));
	BOOST_TEST((rt_id(res14) == (id< DimT< false, CT, 7 > >)));
	BOOST_TEST((rt_id(res15) == (id< DimT< false, CT, 0 > >)));
	BOOST_TEST((rt_id(res16) == (id< DimT< true,  CT, 7 > >)));
	BOOST_TEST((rt_id(res17) == (id< DimT< true,  CT, 7 > >)));
	BOOST_TEST((rt_id(res18) == (id< DimT< true,  CT, 7 > >)));
	BOOST_TEST((rt_id(res19) == (id< DimT< true,  CT, 7 > >)));
	BOOST_TEST((rt_id(res20) == (id< DimT< true,  CT, 7 > >)));
	BOOST_TEST((rt_id(res21) == (id< DimT< true,  CT, 7 > >)));
	BOOST_TEST((rt_id(res22) == (id< DimT< true,  CT, 7 > >)));
	BOOST_TEST((rt_id(res23) == (id< DimT< true,  CT, 7 > >)));
	BOOST_TEST((rt_id(res24) == (id< DimT< true,  CT, 7 > >)));
	BOOST_TEST((rt_id(res25) == (id< DimT< true,  CT, 7 > >)));
	BOOST_TEST((rt_id(res26) == (id< DimT< false, CT, 7 > >)));
	BOOST_TEST((rt_id(res27) == (id< DimT< false, CT, 7 > >)));
	BOOST_TEST((rt_id(res28) == (id< DimT< true,  CT, 7 > >)));
	BOOST_TEST((rt_id(res29) == (id< DimT< true,  CT, 7 > >)));
	BOOST_TEST((rt_id(res30) == (id< DimT< true,  CT, 7 > >)));
	BOOST_TEST((rt_id(res31) == (id< DimT< true,  CT, 7 > >)));
	BOOST_TEST((rt_id(res32) == (id< DimT< false, CT, 7 > >)));
	BOOST_TEST((rt_id(res33) == (id< DimT< false, CT, 7 > >)));
}

template < typename CT >
void add_test(utf::test_suite* ts){
	ts->add(BOOST_TEST_CASE(&construct< CT >));
	ts->add(BOOST_TEST_CASE(&default_construct< CT >));
	ts->add(BOOST_TEST_CASE(&convert2size_t< CT >));
	ts->add(BOOST_TEST_CASE(&convert2dim< CT >));
	ts->add(BOOST_TEST_CASE(&get_dim< CT >));
	ts->add(BOOST_TEST_CASE((&arithmetic< CT, std::plus<> >)));
	ts->add(BOOST_TEST_CASE((&arithmetic< CT, std::minus<> >)));
	ts->add(BOOST_TEST_CASE((&arithmetic< CT, std::multiplies<> >)));
	ts->add(BOOST_TEST_CASE((&arithmetic< CT, std::divides<> >)));
	ts->add(BOOST_TEST_CASE((&arithmetic< CT, std::modulus<> >)));
	ts->add(BOOST_TEST_CASE((&compare< CT, std::equal_to<> >)));
	ts->add(BOOST_TEST_CASE((&compare< CT, std::not_equal_to<> >)));
	ts->add(BOOST_TEST_CASE((&compare< CT, std::less<> >)));
	ts->add(BOOST_TEST_CASE((&compare< CT, std::less_equal<> >)));
	ts->add(BOOST_TEST_CASE((&compare< CT, std::greater<> >)));
	ts->add(BOOST_TEST_CASE((&compare< CT, std::greater_equal<> >)));
}

void make_col(){
	constexpr auto cct = cols< 3_C >();
	constexpr auto crt = cols_rt< 3_C >();
	constexpr auto rrt = cols(3_C);

	BOOST_TEST((rt_id(cct) == id< col< true,  3_C > >));
	BOOST_TEST((rt_id(crt) == id< col< false, 3_C > >));
	BOOST_TEST((rt_id(rrt) == id< col< false, 0_C > >));

	BOOST_TEST((cct == 3_CS));
	BOOST_TEST((crt == 3_CS));
	BOOST_TEST((rrt == 3_CS));
}

void make_row(){
	constexpr auto cct = rows< 3_R >();
	constexpr auto crt = rows_rt< 3_R >();
	constexpr auto rrt = rows(3_R);

	BOOST_TEST((rt_id(cct) == id< row< true,  3_R > >));
	BOOST_TEST((rt_id(crt) == id< row< false, 3_R > >));
	BOOST_TEST((rt_id(rrt) == id< row< false, 0_R > >));

	BOOST_TEST((cct == 3_RS));
	BOOST_TEST((crt == 3_RS));
	BOOST_TEST((rrt == 3_RS));
}

void make_dim(){
	constexpr auto cct = dims< 3_D >();
	constexpr auto crt = dims_rt< 3_D >();
	constexpr auto rrt = dims(3_D);

	BOOST_TEST((rt_id(cct) == id< dim< true,  3_D > >));
	BOOST_TEST((rt_id(crt) == id< dim< false, 3_D > >));
	BOOST_TEST((rt_id(rrt) == id< dim< false, 0_D > >));

	BOOST_TEST((cct == 3_DS));
	BOOST_TEST((crt == 3_DS));
	BOOST_TEST((rrt == 3_DS));
}

void dim_pair_t_default_construct(){
	constexpr auto c = 3_C;
	constexpr auto r = 4_R;

	constexpr auto cc = auto_dim_pair_t< c, r >();
	constexpr auto cr = auto_dim_pair_t< c, 0_R >();
	constexpr auto rc = auto_dim_pair_t< 0_C, r >();
	constexpr auto rr = auto_dim_pair_t< 0_C, 0_R >();

	BOOST_TEST((CP(cc.cols()) == cols(c)));
	BOOST_TEST((CP(cr.cols()) == cols(c)));
	BOOST_TEST((CP(rc.cols()) == 0_CD));
	BOOST_TEST((CP(rr.cols()) == 0_CD));

	BOOST_TEST((CP(cc.rows()) == rows(r)));
	BOOST_TEST((CP(cr.rows()) == 0_RD));
	BOOST_TEST((CP(rc.rows()) == rows(r)));
	BOOST_TEST((CP(rr.rows()) == 0_RD));
}

void dim_pair_t_construct(){
	constexpr auto c = 3_C;
	constexpr auto r = 4_R;

	constexpr auto cc = auto_dim_pair_t< c, r >(cols< c >(), rows< r >());
	constexpr auto cr = auto_dim_pair_t< c, 0_R >(cols< c >(), rows(r));
	constexpr auto rc = auto_dim_pair_t< 0_C, r >(cols(c), rows< r >());
	constexpr auto rr = auto_dim_pair_t< 0_C, 0_R >(cols(c), rows(r));

	BOOST_TEST((CP(cc.cols()) == cols(c)));
	BOOST_TEST((CP(cr.cols()) == cols(c)));
	BOOST_TEST((CP(rc.cols()) == cols(c)));
	BOOST_TEST((CP(rr.cols()) == cols(c)));

	BOOST_TEST((CP(cc.rows()) == rows(r)));
	BOOST_TEST((CP(cr.rows()) == rows(r)));
	BOOST_TEST((CP(rc.rows()) == rows(r)));
	BOOST_TEST((CP(rr.rows()) == rows(r)));
}

void dim_pair_t_make(){
	constexpr auto c = 3_C;
	constexpr auto r = 4_R;

	constexpr auto ll = dim_pair(c, r);
	constexpr auto lr = dim_pair(c, rows(r));
	constexpr auto lc = dim_pair(c, rows< r >());
	constexpr auto ld = dim_pair(c, rows_rt< r >());

	constexpr auto rl = dim_pair(cols(c), r);
	constexpr auto rr = dim_pair(cols(c), rows(r));
	constexpr auto rc = dim_pair(cols(c), rows< r >());
	constexpr auto rd = dim_pair(cols(c), rows_rt< r >());

	constexpr auto cl = dim_pair(cols< c >(), r);
	constexpr auto cr = dim_pair(cols< c >(), rows(r));
	constexpr auto cc = dim_pair(cols< c >(), rows< r >());
	constexpr auto cd = dim_pair(cols< c >(), rows_rt< r >());

	constexpr auto dl = dim_pair(cols_rt< c >(), r);
	constexpr auto dr = dim_pair(cols_rt< c >(), rows(r));
	constexpr auto dc = dim_pair(cols_rt< c >(), rows< r >());
	constexpr auto dd = dim_pair(cols_rt< c >(), rows_rt< r >());

	BOOST_TEST((rt_id(ll) == id< dim_pair_t< false, 0_C, false, 0_R > >));
	BOOST_TEST((rt_id(lr) == id< dim_pair_t< false, 0_C, false, 0_R > >));
	BOOST_TEST((rt_id(lc) == id< dim_pair_t< false, 0_C, true,  4_R > >));
	BOOST_TEST((rt_id(ld) == id< dim_pair_t< false, 0_C, false, 4_R > >));

	BOOST_TEST((rt_id(rl) == id< dim_pair_t< false, 0_C, false, 0_R > >));
	BOOST_TEST((rt_id(rr) == id< dim_pair_t< false, 0_C, false, 0_R > >));
	BOOST_TEST((rt_id(rc) == id< dim_pair_t< false, 0_C, true,  4_R > >));
	BOOST_TEST((rt_id(rd) == id< dim_pair_t< false, 0_C, false, 4_R > >));

	BOOST_TEST((rt_id(cl) == id< dim_pair_t< true,  3_C, false, 0_R > >));
	BOOST_TEST((rt_id(cr) == id< dim_pair_t< true,  3_C, false, 0_R > >));
	BOOST_TEST((rt_id(cc) == id< dim_pair_t< true,  3_C, true,  4_R > >));
	BOOST_TEST((rt_id(cd) == id< dim_pair_t< true,  3_C, false, 4_R > >));

	BOOST_TEST((rt_id(dl) == id< dim_pair_t< false, 3_C, false, 0_R > >));
	BOOST_TEST((rt_id(dr) == id< dim_pair_t< false, 3_C, false, 0_R > >));
	BOOST_TEST((rt_id(dc) == id< dim_pair_t< false, 3_C, true,  4_R > >));
	BOOST_TEST((rt_id(dd) == id< dim_pair_t< false, 3_C, false, 4_R > >));

	BOOST_TEST((rt_id(ll.cols()) == id< col< false, 0_C > >));
	BOOST_TEST((rt_id(lr.cols()) == id< col< false, 0_C > >));
	BOOST_TEST((rt_id(lc.cols()) == id< col< false, 0_C > >));
	BOOST_TEST((rt_id(ld.cols()) == id< col< false, 0_C > >));

	BOOST_TEST((rt_id(rl.cols()) == id< col< false, 0_C > >));
	BOOST_TEST((rt_id(rr.cols()) == id< col< false, 0_C > >));
	BOOST_TEST((rt_id(rc.cols()) == id< col< false, 0_C > >));
	BOOST_TEST((rt_id(rd.cols()) == id< col< false, 0_C > >));

	BOOST_TEST((rt_id(cl.cols()) == id< col< true,  3_C > >));
	BOOST_TEST((rt_id(cr.cols()) == id< col< true,  3_C > >));
	BOOST_TEST((rt_id(cc.cols()) == id< col< true,  3_C > >));
	BOOST_TEST((rt_id(cd.cols()) == id< col< true,  3_C > >));

	BOOST_TEST((rt_id(dl.cols()) == id< col< false, 3_C > >));
	BOOST_TEST((rt_id(dr.cols()) == id< col< false, 3_C > >));
	BOOST_TEST((rt_id(dc.cols()) == id< col< false, 3_C > >));
	BOOST_TEST((rt_id(dd.cols()) == id< col< false, 3_C > >));

	BOOST_TEST((rt_id(ll.rows()) == id< row< false, 0_R > >));
	BOOST_TEST((rt_id(lr.rows()) == id< row< false, 0_R > >));
	BOOST_TEST((rt_id(lc.rows()) == id< row< true,  4_R > >));
	BOOST_TEST((rt_id(ld.rows()) == id< row< false, 4_R > >));

	BOOST_TEST((rt_id(rl.rows()) == id< row< false, 0_R > >));
	BOOST_TEST((rt_id(rr.rows()) == id< row< false, 0_R > >));
	BOOST_TEST((rt_id(rc.rows()) == id< row< true,  4_R > >));
	BOOST_TEST((rt_id(rd.rows()) == id< row< false, 4_R > >));

	BOOST_TEST((rt_id(cl.rows()) == id< row< false, 0_R > >));
	BOOST_TEST((rt_id(cr.rows()) == id< row< false, 0_R > >));
	BOOST_TEST((rt_id(cc.rows()) == id< row< true,  4_R > >));
	BOOST_TEST((rt_id(cd.rows()) == id< row< false, 4_R > >));

	BOOST_TEST((rt_id(dl.rows()) == id< row< false, 0_R > >));
	BOOST_TEST((rt_id(dr.rows()) == id< row< false, 0_R > >));
	BOOST_TEST((rt_id(dc.rows()) == id< row< true,  4_R > >));
	BOOST_TEST((rt_id(dd.rows()) == id< row< false, 4_R > >));

	BOOST_TEST((CP(ll.cols()) == 3_CS));
	BOOST_TEST((CP(lr.cols()) == 3_CS));
	BOOST_TEST((CP(lc.cols()) == 3_CS));
	BOOST_TEST((CP(ld.cols()) == 3_CS));

	BOOST_TEST((CP(rl.cols()) == 3_CS));
	BOOST_TEST((CP(rr.cols()) == 3_CS));
	BOOST_TEST((CP(rc.cols()) == 3_CS));
	BOOST_TEST((CP(rd.cols()) == 3_CS));

	BOOST_TEST((CP(cl.cols()) == 3_CS));
	BOOST_TEST((CP(cr.cols()) == 3_CS));
	BOOST_TEST((CP(cc.cols()) == 3_CS));
	BOOST_TEST((CP(cd.cols()) == 3_CS));

	BOOST_TEST((CP(dl.cols()) == 3_CS));
	BOOST_TEST((CP(dr.cols()) == 3_CS));
	BOOST_TEST((CP(dc.cols()) == 3_CS));
	BOOST_TEST((CP(dd.cols()) == 3_CS));

	BOOST_TEST((CP(ll.rows()) == 4_RS));
	BOOST_TEST((CP(lr.rows()) == 4_RS));
	BOOST_TEST((CP(lc.rows()) == 4_RS));
	BOOST_TEST((CP(ld.rows()) == 4_RS));

	BOOST_TEST((CP(rl.rows()) == 4_RS));
	BOOST_TEST((CP(rr.rows()) == 4_RS));
	BOOST_TEST((CP(rc.rows()) == 4_RS));
	BOOST_TEST((CP(rd.rows()) == 4_RS));

	BOOST_TEST((CP(cl.rows()) == 4_RS));
	BOOST_TEST((CP(cr.rows()) == 4_RS));
	BOOST_TEST((CP(cc.rows()) == 4_RS));
	BOOST_TEST((CP(cd.rows()) == 4_RS));

	BOOST_TEST((CP(dl.rows()) == 4_RS));
	BOOST_TEST((CP(dr.rows()) == 4_RS));
	BOOST_TEST((CP(dc.rows()) == 4_RS));
	BOOST_TEST((CP(dd.rows()) == 4_RS));
}

template <
	bool Cct1, col_t C1, bool Rct1, row_t R1,
	bool Cct2, col_t C2, bool Rct2, row_t R2 >
constexpr void dim_compare_test(
	dim_pair_t< Cct1, C1, Rct1, R1 > const& d1,
	dim_pair_t< Cct2, C2, Rct2, R2 > const& d2,
	bool equal_expection
)noexcept{
	BOOST_TEST((rt_id(d1 == d2) == id< bool >));
	BOOST_TEST((d1 == d2) == equal_expection);
	BOOST_TEST(!(d1 == d2) != equal_expection);
}

void dim_pair_t_compare(){
	using namespace ::mitrax::literals;

	constexpr auto cc1 = dim_pair(3_CS,  4_RS );
	constexpr auto cr1 = dim_pair(3_CS,  4_RD);
	constexpr auto rc1 = dim_pair(3_CD, 4_RS );
	constexpr auto rr1 = dim_pair(3_CD, 4_RD);

	constexpr auto cc2 = dim_pair(6_CS,  5_RS );
	constexpr auto cr2 = dim_pair(6_CS,  5_RD);
	constexpr auto rc2 = dim_pair(6_CD, 5_RS );
	constexpr auto rr2 = dim_pair(6_CD, 5_RD);

	constexpr auto cc3 = dim_pair(3_CS,  5_RS );
	constexpr auto cr3 = dim_pair(3_CS,  5_RD);
	constexpr auto rc3 = dim_pair(3_CD, 5_RS );
	constexpr auto rr3 = dim_pair(3_CD, 5_RD);

	constexpr auto cc4 = dim_pair(6_CS,  4_RS );
	constexpr auto cr4 = dim_pair(6_CS,  4_RD);
	constexpr auto rc4 = dim_pair(6_CD, 4_RS );
	constexpr auto rr4 = dim_pair(6_CD, 4_RD);

	dim_compare_test(cc1, cc1, true);
	dim_compare_test(cr1, cc1, true);
	dim_compare_test(rc1, cc1, true);
	dim_compare_test(rr1, cc1, true);

	dim_compare_test(cc1, cr1, true);
	dim_compare_test(cr1, cr1, true);
	dim_compare_test(rc1, cr1, true);
	dim_compare_test(rr1, cr1, true);

	dim_compare_test(cc1, rc1, true);
	dim_compare_test(cr1, rc1, true);
	dim_compare_test(rc1, rc1, true);
	dim_compare_test(rr1, rc1, true);

	dim_compare_test(cc1, rr1, true);
	dim_compare_test(cr1, rr1, true);
	dim_compare_test(rc1, rr1, true);
	dim_compare_test(rr1, rr1, true);

	dim_compare_test(cc1, cc2, false);
	dim_compare_test(cr1, cc2, false);
	dim_compare_test(rc1, cc2, false);
	dim_compare_test(rr1, cc2, false);

	dim_compare_test(cc1, cr2, false);
	dim_compare_test(cr1, cr2, false);
	dim_compare_test(rc1, cr2, false);
	dim_compare_test(rr1, cr2, false);

	dim_compare_test(cc1, rc2, false);
	dim_compare_test(cr1, rc2, false);
	dim_compare_test(rc1, rc2, false);
	dim_compare_test(rr1, rc2, false);

	dim_compare_test(cc1, rr2, false);
	dim_compare_test(cr1, rr2, false);
	dim_compare_test(rc1, rr2, false);
	dim_compare_test(rr1, rr2, false);

	dim_compare_test(cc1, cc3, false);
	dim_compare_test(cr1, cc3, false);
	dim_compare_test(rc1, cc3, false);
	dim_compare_test(rr1, cc3, false);

	dim_compare_test(cc1, cr3, false);
	dim_compare_test(cr1, cr3, false);
	dim_compare_test(rc1, cr3, false);
	dim_compare_test(rr1, cr3, false);

	dim_compare_test(cc1, rc3, false);
	dim_compare_test(cr1, rc3, false);
	dim_compare_test(rc1, rc3, false);
	dim_compare_test(rr1, rc3, false);

	dim_compare_test(cc1, rr3, false);
	dim_compare_test(cr1, rr3, false);
	dim_compare_test(rc1, rr3, false);
	dim_compare_test(rr1, rr3, false);

	dim_compare_test(cc1, cc4, false);
	dim_compare_test(cr1, cc4, false);
	dim_compare_test(rc1, cc4, false);
	dim_compare_test(rr1, cc4, false);

	dim_compare_test(cc1, cr4, false);
	dim_compare_test(cr1, cr4, false);
	dim_compare_test(rc1, cr4, false);
	dim_compare_test(rr1, cr4, false);

	dim_compare_test(cc1, rc4, false);
	dim_compare_test(cr1, rc4, false);
	dim_compare_test(rc1, rc4, false);
	dim_compare_test(rr1, rc4, false);

	dim_compare_test(cc1, rr4, false);
	dim_compare_test(cr1, rr4, false);
	dim_compare_test(rc1, rr4, false);
	dim_compare_test(rr1, rr4, false);
}

template < typename Op >
void dim_pair_t_arithmetic_size_t(){
	constexpr auto op = Op();
	constexpr auto c = 9_C;
	constexpr auto r = 7_R;
	constexpr auto v = 5_D;

	constexpr auto cc = auto_dim_pair_t< c, r >(cols< c >(), rows< r >());
	constexpr auto cr = auto_dim_pair_t< c, 0_R >(cols< c >(), rows(r));
	constexpr auto rc = auto_dim_pair_t< 0_C, r >(cols(c), rows< r >());
	constexpr auto rr = auto_dim_pair_t< 0_C, 0_R >(cols(c), rows(r));

	constexpr auto vcc = dims< v >();
	constexpr auto vcr = dims_rt< v >();
	constexpr auto vrr = dims(v);

	BOOST_TEST((rt_id(CP(op(cc, vcc))) == id< auto_dim_pair_t< op(c, col_t(v)), op(r, row_t(v)) > >));
	BOOST_TEST((rt_id(CP(op(cr, vcc))) == id< auto_dim_pair_t< op(c, col_t(v)), 0_R > >));
	BOOST_TEST((rt_id(CP(op(rc, vcc))) == id< auto_dim_pair_t< 0_C, op(r, row_t(v)) > >));
	BOOST_TEST((rt_id(CP(op(rr, vcc))) == id< auto_dim_pair_t< 0_C, 0_R > >));

	BOOST_TEST((rt_id(CP(op(cc, vcr))) == id< auto_dim_pair_t< op(c, col_t(v)), op(r, row_t(v)) > >));
	BOOST_TEST((rt_id(CP(op(cr, vcr))) == id< auto_dim_pair_t< op(c, col_t(v)), 0_R > >));
	BOOST_TEST((rt_id(CP(op(rc, vcr))) == id< auto_dim_pair_t< 0_C, op(r, row_t(v)) > >));
	BOOST_TEST((rt_id(CP(op(rr, vcr))) == id< auto_dim_pair_t< 0_C, 0_R > >));

	BOOST_TEST((rt_id(CP(op(cc, vrr))) == id< auto_dim_pair_t< 0_C, 0_R > >));
	BOOST_TEST((rt_id(CP(op(cr, vrr))) == id< auto_dim_pair_t< 0_C, 0_R > >));
	BOOST_TEST((rt_id(CP(op(rc, vrr))) == id< auto_dim_pair_t< 0_C, 0_R > >));
	BOOST_TEST((rt_id(CP(op(rr, vrr))) == id< auto_dim_pair_t< 0_C, 0_R > >));

	BOOST_TEST((CP(op(cc, vcc).cols()) == cols(op(c, col_t(v)))));
	BOOST_TEST((CP(op(cr, vcc).cols()) == cols(op(c, col_t(v)))));
	BOOST_TEST((CP(op(rc, vcc).cols()) == cols(op(c, col_t(v)))));
	BOOST_TEST((CP(op(rr, vcc).cols()) == cols(op(c, col_t(v)))));

	BOOST_TEST((CP(op(cc, vcr).cols()) == cols(op(c, col_t(v)))));
	BOOST_TEST((CP(op(cr, vcr).cols()) == cols(op(c, col_t(v)))));
	BOOST_TEST((CP(op(rc, vcr).cols()) == cols(op(c, col_t(v)))));
	BOOST_TEST((CP(op(rr, vcr).cols()) == cols(op(c, col_t(v)))));

	BOOST_TEST((CP(op(cc, vrr).cols()) == cols(op(c, col_t(v)))));
	BOOST_TEST((CP(op(cr, vrr).cols()) == cols(op(c, col_t(v)))));
	BOOST_TEST((CP(op(rc, vrr).cols()) == cols(op(c, col_t(v)))));
	BOOST_TEST((CP(op(rr, vrr).cols()) == cols(op(c, col_t(v)))));

	BOOST_TEST((CP(op(cc, vcc).rows()) == rows(op(r, row_t(v)))));
	BOOST_TEST((CP(op(cr, vcc).rows()) == rows(op(r, row_t(v)))));
	BOOST_TEST((CP(op(rc, vcc).rows()) == rows(op(r, row_t(v)))));
	BOOST_TEST((CP(op(rr, vcc).rows()) == rows(op(r, row_t(v)))));

	BOOST_TEST((CP(op(cc, vcr).rows()) == rows(op(r, row_t(v)))));
	BOOST_TEST((CP(op(cr, vcr).rows()) == rows(op(r, row_t(v)))));
	BOOST_TEST((CP(op(rc, vcr).rows()) == rows(op(r, row_t(v)))));
	BOOST_TEST((CP(op(rr, vcr).rows()) == rows(op(r, row_t(v)))));

	BOOST_TEST((CP(op(cc, vrr).rows()) == rows(op(r, row_t(v)))));
	BOOST_TEST((CP(op(cr, vrr).rows()) == rows(op(r, row_t(v)))));
	BOOST_TEST((CP(op(rc, vrr).rows()) == rows(op(r, row_t(v)))));
	BOOST_TEST((CP(op(rr, vrr).rows()) == rows(op(r, row_t(v)))));
}

template < typename Op >
void dim_pair_t_arithmetic(){
	constexpr auto op = Op();
	constexpr auto c1 = 9_C;
	constexpr auto r1 = 15_R;
	constexpr auto c2 = 5_C;
	constexpr auto r2 = 7_R;

	constexpr auto cc1 = auto_dim_pair_t< c1, r1 >(cols< c1 >(), rows< r1 >());
	constexpr auto cr1 = auto_dim_pair_t< c1, 0_R >(cols< c1 >(), rows(r1));
	constexpr auto rc1 = auto_dim_pair_t< 0_C, r1 >(cols(c1), rows< r1 >());
	constexpr auto rr1 = auto_dim_pair_t< 0_C, 0_R >(cols(c1), rows(r1));

	constexpr auto cc2 = auto_dim_pair_t< c2, r2 >(cols< c2 >(), rows< r2 >());
	constexpr auto cr2 = auto_dim_pair_t< c2, 0_R >(cols< c2 >(), rows(r2));
	constexpr auto rc2 = auto_dim_pair_t< 0_C, r2 >(cols(c2), rows< r2 >());
	constexpr auto rr2 = auto_dim_pair_t< 0_C, 0_R >(cols(c2), rows(r2));

	BOOST_TEST((rt_id(CP(op(cc1, cc2))) == id< auto_dim_pair_t< op(c1, c2), op(r1, r2) > >));
	BOOST_TEST((rt_id(CP(op(cc1, cr2))) == id< auto_dim_pair_t< op(c1, c2), 0_R > >));
	BOOST_TEST((rt_id(CP(op(cc1, rc2))) == id< auto_dim_pair_t< 0_C, op(r1, r2) > >));
	BOOST_TEST((rt_id(CP(op(cc1, rr2))) == id< auto_dim_pair_t< 0_C, 0_R > >));

	BOOST_TEST((rt_id(CP(op(cr1, cc2))) == id< auto_dim_pair_t< op(c1, c2), 0_R > >));
	BOOST_TEST((rt_id(CP(op(cr1, cr2))) == id< auto_dim_pair_t< op(c1, c2), 0_R > >));
	BOOST_TEST((rt_id(CP(op(cr1, rc2))) == id< auto_dim_pair_t< 0_C, 0_R > >));
	BOOST_TEST((rt_id(CP(op(cr1, rr2))) == id< auto_dim_pair_t< 0_C, 0_R > >));

	BOOST_TEST((rt_id(CP(op(rc1, cc2))) == id< auto_dim_pair_t< 0_C, op(r1, r2) > >));
	BOOST_TEST((rt_id(CP(op(rc1, cr2))) == id< auto_dim_pair_t< 0_C, 0_R > >));
	BOOST_TEST((rt_id(CP(op(rc1, rc2))) == id< auto_dim_pair_t< 0_C, op(r1, r2) > >));
	BOOST_TEST((rt_id(CP(op(rc1, rr2))) == id< auto_dim_pair_t< 0_C, 0_R > >));

	BOOST_TEST((rt_id(CP(op(rr1, cc2))) == id< auto_dim_pair_t< 0_C, 0_R > >));
	BOOST_TEST((rt_id(CP(op(rr1, cr2))) == id< auto_dim_pair_t< 0_C, 0_R > >));
	BOOST_TEST((rt_id(CP(op(rr1, rc2))) == id< auto_dim_pair_t< 0_C, 0_R > >));
	BOOST_TEST((rt_id(CP(op(rr1, rr2))) == id< auto_dim_pair_t< 0_C, 0_R > >));

	BOOST_TEST((CP(op(cc1, cc2).cols()) == cols(op(c1, c2))));
	BOOST_TEST((CP(op(cc1, cr2).cols()) == cols(op(c1, c2))));
	BOOST_TEST((CP(op(cc1, rc2).cols()) == cols(op(c1, c2))));
	BOOST_TEST((CP(op(cc1, rr2).cols()) == cols(op(c1, c2))));

	BOOST_TEST((CP(op(cr1, cc2).cols()) == cols(op(c1, c2))));
	BOOST_TEST((CP(op(cr1, cr2).cols()) == cols(op(c1, c2))));
	BOOST_TEST((CP(op(cr1, rc2).cols()) == cols(op(c1, c2))));
	BOOST_TEST((CP(op(cr1, rr2).cols()) == cols(op(c1, c2))));

	BOOST_TEST((CP(op(rc1, cc2).cols()) == cols(op(c1, c2))));
	BOOST_TEST((CP(op(rc1, cr2).cols()) == cols(op(c1, c2))));
	BOOST_TEST((CP(op(rc1, rc2).cols()) == cols(op(c1, c2))));
	BOOST_TEST((CP(op(rc1, rr2).cols()) == cols(op(c1, c2))));

	BOOST_TEST((CP(op(rr1, cc2).cols()) == cols(op(c1, c2))));
	BOOST_TEST((CP(op(rr1, cr2).cols()) == cols(op(c1, c2))));
	BOOST_TEST((CP(op(rr1, rc2).cols()) == cols(op(c1, c2))));
	BOOST_TEST((CP(op(rr1, rr2).cols()) == cols(op(c1, c2))));

	BOOST_TEST((CP(op(cc1, cc2).rows()) == rows(op(r1, r2))));
	BOOST_TEST((CP(op(cc1, cr2).rows()) == rows(op(r1, r2))));
	BOOST_TEST((CP(op(cc1, rc2).rows()) == rows(op(r1, r2))));
	BOOST_TEST((CP(op(cc1, rr2).rows()) == rows(op(r1, r2))));

	BOOST_TEST((CP(op(cr1, cc2).rows()) == rows(op(r1, r2))));
	BOOST_TEST((CP(op(cr1, cr2).rows()) == rows(op(r1, r2))));
	BOOST_TEST((CP(op(cr1, rc2).rows()) == rows(op(r1, r2))));
	BOOST_TEST((CP(op(cr1, rr2).rows()) == rows(op(r1, r2))));

	BOOST_TEST((CP(op(rc1, cc2).rows()) == rows(op(r1, r2))));
	BOOST_TEST((CP(op(rc1, cr2).rows()) == rows(op(r1, r2))));
	BOOST_TEST((CP(op(rc1, rc2).rows()) == rows(op(r1, r2))));
	BOOST_TEST((CP(op(rc1, rr2).rows()) == rows(op(r1, r2))));

	BOOST_TEST((CP(op(rr1, cc2).rows()) == rows(op(r1, r2))));
	BOOST_TEST((CP(op(rr1, cr2).rows()) == rows(op(r1, r2))));
	BOOST_TEST((CP(op(rr1, rc2).rows()) == rows(op(r1, r2))));
	BOOST_TEST((CP(op(rr1, rr2).rows()) == rows(op(r1, r2))));
}

void get_dim_pair(){
	constexpr auto c = 3_C;
	constexpr auto r = 4_R;

	constexpr auto d1 = dim_pair(cols< c >(), rows< r >());
	constexpr auto d2 = dim_pair(cols< c >(), rows(r));
	constexpr auto d3 = dim_pair(cols(c), rows< r >());
	constexpr auto d4 = dim_pair(cols(c), rows(r));

	constexpr auto res01 = get(d1);
	constexpr auto res02 = get(d2);
	constexpr auto res03 = get(d3);
	constexpr auto res04 = get(d4);
	constexpr auto res05 = get(d1, d1);
	constexpr auto res06 = get(d1, d2);
	constexpr auto res07 = get(d1, d3);
	constexpr auto res08 = get(d1, d4);
	constexpr auto res09 = get(d2, d1);
	constexpr auto res10 = get(d2, d2);
	constexpr auto res11 = get(d2, d3);
	constexpr auto res12 = get(d2, d4);
	constexpr auto res13 = get(d3, d1);
	constexpr auto res14 = get(d3, d2);
	constexpr auto res15 = get(d3, d3);
	constexpr auto res16 = get(d3, d4);
	constexpr auto res17 = get(d4, d1);
	constexpr auto res18 = get(d4, d2);
	constexpr auto res19 = get(d4, d3);
	constexpr auto res20 = get(d4, d4);
	constexpr auto res21 = get(d1, d1, d1);
	constexpr auto res22 = get(d1, d1, d2);
	constexpr auto res23 = get(d1, d1, d3);
	constexpr auto res24 = get(d1, d1, d4);
	constexpr auto res25 = get(d1, d2, d1);
	constexpr auto res26 = get(d1, d2, d2);
	constexpr auto res27 = get(d1, d2, d3);
	constexpr auto res28 = get(d1, d2, d4);
	constexpr auto res29 = get(d1, d3, d1);
	constexpr auto res30 = get(d1, d3, d2);
	constexpr auto res31 = get(d1, d3, d3);
	constexpr auto res32 = get(d1, d3, d4);
	constexpr auto res33 = get(d1, d4, d1);
	constexpr auto res34 = get(d1, d4, d2);
	constexpr auto res35 = get(d1, d4, d3);
	constexpr auto res36 = get(d1, d4, d4);
	constexpr auto res37 = get(d2, d1, d1);
	constexpr auto res38 = get(d2, d1, d2);
	constexpr auto res39 = get(d2, d1, d3);
	constexpr auto res40 = get(d2, d1, d4);
	constexpr auto res41 = get(d2, d2, d1);
	constexpr auto res42 = get(d2, d2, d2);
	constexpr auto res43 = get(d2, d2, d3);
	constexpr auto res44 = get(d2, d2, d4);
	constexpr auto res45 = get(d2, d3, d1);
	constexpr auto res46 = get(d2, d3, d2);
	constexpr auto res47 = get(d2, d3, d3);
	constexpr auto res48 = get(d2, d3, d4);
	constexpr auto res49 = get(d2, d4, d1);
	constexpr auto res50 = get(d2, d4, d2);
	constexpr auto res51 = get(d2, d4, d3);
	constexpr auto res52 = get(d2, d4, d4);
	constexpr auto res53 = get(d3, d1, d1);
	constexpr auto res54 = get(d3, d1, d2);
	constexpr auto res55 = get(d3, d1, d3);
	constexpr auto res56 = get(d3, d1, d4);
	constexpr auto res57 = get(d3, d2, d1);
	constexpr auto res58 = get(d3, d2, d2);
	constexpr auto res59 = get(d3, d2, d3);
	constexpr auto res60 = get(d3, d2, d4);
	constexpr auto res61 = get(d3, d3, d1);
	constexpr auto res62 = get(d3, d3, d2);
	constexpr auto res63 = get(d3, d3, d3);
	constexpr auto res64 = get(d3, d3, d4);
	constexpr auto res65 = get(d3, d4, d1);
	constexpr auto res66 = get(d3, d4, d2);
	constexpr auto res67 = get(d3, d4, d3);
	constexpr auto res68 = get(d3, d4, d4);
	constexpr auto res69 = get(d4, d1, d1);
	constexpr auto res70 = get(d4, d1, d2);
	constexpr auto res71 = get(d4, d1, d3);
	constexpr auto res72 = get(d4, d1, d4);
	constexpr auto res73 = get(d4, d2, d1);
	constexpr auto res74 = get(d4, d2, d2);
	constexpr auto res75 = get(d4, d2, d3);
	constexpr auto res76 = get(d4, d2, d4);
	constexpr auto res77 = get(d4, d3, d1);
	constexpr auto res78 = get(d4, d3, d2);
	constexpr auto res79 = get(d4, d3, d3);
	constexpr auto res80 = get(d4, d3, d4);
	constexpr auto res81 = get(d4, d4, d1);
	constexpr auto res82 = get(d4, d4, d2);
	constexpr auto res83 = get(d4, d4, d3);
	constexpr auto res84 = get(d4, d4, d4);

	BOOST_TEST((rt_id(res01) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res02) == id< auto_dim_pair_t< c, 0_R > >));
	BOOST_TEST((rt_id(res03) == id< auto_dim_pair_t< 0_C, r > >));
	BOOST_TEST((rt_id(res04) == id< auto_dim_pair_t< 0_C, 0_R > >));
	BOOST_TEST((rt_id(res05) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res06) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res07) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res08) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res09) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res10) == id< auto_dim_pair_t< c, 0_R > >));
	BOOST_TEST((rt_id(res11) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res12) == id< auto_dim_pair_t< c, 0_R > >));
	BOOST_TEST((rt_id(res13) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res14) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res15) == id< auto_dim_pair_t< 0_C, r > >));
	BOOST_TEST((rt_id(res16) == id< auto_dim_pair_t< 0_C, r > >));
	BOOST_TEST((rt_id(res17) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res18) == id< auto_dim_pair_t< c, 0_R > >));
	BOOST_TEST((rt_id(res19) == id< auto_dim_pair_t< 0_C, r > >));
	BOOST_TEST((rt_id(res20) == id< auto_dim_pair_t< 0_C, 0_R > >));
	BOOST_TEST((rt_id(res21) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res22) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res23) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res24) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res25) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res26) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res27) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res28) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res29) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res30) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res31) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res32) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res33) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res34) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res35) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res36) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res37) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res38) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res39) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res40) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res41) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res42) == id< auto_dim_pair_t< c, 0_R > >));
	BOOST_TEST((rt_id(res43) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res44) == id< auto_dim_pair_t< c, 0_R > >));
	BOOST_TEST((rt_id(res45) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res46) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res47) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res48) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res49) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res50) == id< auto_dim_pair_t< c, 0_R > >));
	BOOST_TEST((rt_id(res51) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res52) == id< auto_dim_pair_t< c, 0_R > >));
	BOOST_TEST((rt_id(res53) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res54) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res55) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res56) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res57) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res58) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res59) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res60) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res61) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res62) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res63) == id< auto_dim_pair_t< 0_C, r > >));
	BOOST_TEST((rt_id(res64) == id< auto_dim_pair_t< 0_C, r > >));
	BOOST_TEST((rt_id(res65) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res66) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res67) == id< auto_dim_pair_t< 0_C, r > >));
	BOOST_TEST((rt_id(res68) == id< auto_dim_pair_t< 0_C, r > >));
	BOOST_TEST((rt_id(res69) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res70) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res71) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res72) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res73) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res74) == id< auto_dim_pair_t< c, 0_R > >));
	BOOST_TEST((rt_id(res75) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res76) == id< auto_dim_pair_t< c, 0_R > >));
	BOOST_TEST((rt_id(res77) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res78) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res79) == id< auto_dim_pair_t< 0_C, r > >));
	BOOST_TEST((rt_id(res80) == id< auto_dim_pair_t< 0_C, r > >));
	BOOST_TEST((rt_id(res81) == id< auto_dim_pair_t< c, r > >));
	BOOST_TEST((rt_id(res82) == id< auto_dim_pair_t< c, 0_R > >));
	BOOST_TEST((rt_id(res83) == id< auto_dim_pair_t< 0_C, r > >));
	BOOST_TEST((rt_id(res84) == id< auto_dim_pair_t< 0_C, 0_R > >));

	BOOST_TEST((res01 == dim_pair(c, r)));
	BOOST_TEST((res02 == dim_pair(c, r)));
	BOOST_TEST((res03 == dim_pair(c, r)));
	BOOST_TEST((res04 == dim_pair(c, r)));
	BOOST_TEST((res05 == dim_pair(c, r)));
	BOOST_TEST((res06 == dim_pair(c, r)));
	BOOST_TEST((res07 == dim_pair(c, r)));
	BOOST_TEST((res08 == dim_pair(c, r)));
	BOOST_TEST((res09 == dim_pair(c, r)));
	BOOST_TEST((res10 == dim_pair(c, r)));
	BOOST_TEST((res11 == dim_pair(c, r)));
	BOOST_TEST((res12 == dim_pair(c, r)));
	BOOST_TEST((res13 == dim_pair(c, r)));
	BOOST_TEST((res14 == dim_pair(c, r)));
	BOOST_TEST((res15 == dim_pair(c, r)));
	BOOST_TEST((res16 == dim_pair(c, r)));
	BOOST_TEST((res17 == dim_pair(c, r)));
	BOOST_TEST((res18 == dim_pair(c, r)));
	BOOST_TEST((res19 == dim_pair(c, r)));
	BOOST_TEST((res20 == dim_pair(c, r)));
	BOOST_TEST((res21 == dim_pair(c, r)));
	BOOST_TEST((res22 == dim_pair(c, r)));
	BOOST_TEST((res23 == dim_pair(c, r)));
	BOOST_TEST((res24 == dim_pair(c, r)));
	BOOST_TEST((res25 == dim_pair(c, r)));
	BOOST_TEST((res26 == dim_pair(c, r)));
	BOOST_TEST((res27 == dim_pair(c, r)));
	BOOST_TEST((res28 == dim_pair(c, r)));
	BOOST_TEST((res29 == dim_pair(c, r)));
	BOOST_TEST((res30 == dim_pair(c, r)));
	BOOST_TEST((res31 == dim_pair(c, r)));
	BOOST_TEST((res32 == dim_pair(c, r)));
	BOOST_TEST((res33 == dim_pair(c, r)));
	BOOST_TEST((res34 == dim_pair(c, r)));
	BOOST_TEST((res35 == dim_pair(c, r)));
	BOOST_TEST((res36 == dim_pair(c, r)));
	BOOST_TEST((res37 == dim_pair(c, r)));
	BOOST_TEST((res38 == dim_pair(c, r)));
	BOOST_TEST((res39 == dim_pair(c, r)));
	BOOST_TEST((res40 == dim_pair(c, r)));
	BOOST_TEST((res41 == dim_pair(c, r)));
	BOOST_TEST((res42 == dim_pair(c, r)));
	BOOST_TEST((res43 == dim_pair(c, r)));
	BOOST_TEST((res44 == dim_pair(c, r)));
	BOOST_TEST((res45 == dim_pair(c, r)));
	BOOST_TEST((res46 == dim_pair(c, r)));
	BOOST_TEST((res47 == dim_pair(c, r)));
	BOOST_TEST((res48 == dim_pair(c, r)));
	BOOST_TEST((res49 == dim_pair(c, r)));
	BOOST_TEST((res50 == dim_pair(c, r)));
	BOOST_TEST((res51 == dim_pair(c, r)));
	BOOST_TEST((res52 == dim_pair(c, r)));
	BOOST_TEST((res53 == dim_pair(c, r)));
	BOOST_TEST((res54 == dim_pair(c, r)));
	BOOST_TEST((res55 == dim_pair(c, r)));
	BOOST_TEST((res56 == dim_pair(c, r)));
	BOOST_TEST((res57 == dim_pair(c, r)));
	BOOST_TEST((res58 == dim_pair(c, r)));
	BOOST_TEST((res59 == dim_pair(c, r)));
	BOOST_TEST((res60 == dim_pair(c, r)));
	BOOST_TEST((res61 == dim_pair(c, r)));
	BOOST_TEST((res62 == dim_pair(c, r)));
	BOOST_TEST((res63 == dim_pair(c, r)));
	BOOST_TEST((res64 == dim_pair(c, r)));
	BOOST_TEST((res65 == dim_pair(c, r)));
	BOOST_TEST((res66 == dim_pair(c, r)));
	BOOST_TEST((res67 == dim_pair(c, r)));
	BOOST_TEST((res68 == dim_pair(c, r)));
	BOOST_TEST((res69 == dim_pair(c, r)));
	BOOST_TEST((res70 == dim_pair(c, r)));
	BOOST_TEST((res71 == dim_pair(c, r)));
	BOOST_TEST((res72 == dim_pair(c, r)));
	BOOST_TEST((res73 == dim_pair(c, r)));
	BOOST_TEST((res74 == dim_pair(c, r)));
	BOOST_TEST((res75 == dim_pair(c, r)));
	BOOST_TEST((res76 == dim_pair(c, r)));
	BOOST_TEST((res77 == dim_pair(c, r)));
	BOOST_TEST((res78 == dim_pair(c, r)));
	BOOST_TEST((res79 == dim_pair(c, r)));
	BOOST_TEST((res80 == dim_pair(c, r)));
	BOOST_TEST((res81 == dim_pair(c, r)));
	BOOST_TEST((res82 == dim_pair(c, r)));
	BOOST_TEST((res83 == dim_pair(c, r)));
	BOOST_TEST((res84 == dim_pair(c, r)));
}

int main(int argc, char** argv, char** /*envp*/){
	auto ts = BOOST_TEST_SUITE("suite_dim");

	add_test< col_t >(ts);
	add_test< row_t >(ts);
	add_test< dim_t >(ts);
	ts->add(BOOST_TEST_CASE(&index_literal));
	ts->add(BOOST_TEST_CASE(&dim_t_literal));
	ts->add(BOOST_TEST_CASE(&dim_literal));
	ts->add(BOOST_TEST_CASE(&make_col));
	ts->add(BOOST_TEST_CASE(&make_row));
	ts->add(BOOST_TEST_CASE(&make_dim));
	ts->add(BOOST_TEST_CASE(&dim_pair_t_default_construct));
	ts->add(BOOST_TEST_CASE(&dim_pair_t_construct));
	ts->add(BOOST_TEST_CASE(&dim_pair_t_make));
	ts->add(BOOST_TEST_CASE(&dim_pair_t_compare));
	ts->add(BOOST_TEST_CASE(&dim_pair_t_arithmetic_size_t< std::multiplies<> >));
	ts->add(BOOST_TEST_CASE(&dim_pair_t_arithmetic_size_t< std::divides<> >));
	ts->add(BOOST_TEST_CASE(&dim_pair_t_arithmetic_size_t< std::modulus<> >));
	ts->add(BOOST_TEST_CASE(&dim_pair_t_arithmetic< std::plus<> >));
	ts->add(BOOST_TEST_CASE(&dim_pair_t_arithmetic< std::minus<> >));
	ts->add(BOOST_TEST_CASE(&dim_pair_t_arithmetic< std::multiplies<> >));
	ts->add(BOOST_TEST_CASE(&dim_pair_t_arithmetic< std::divides<> >));
	ts->add(BOOST_TEST_CASE(&dim_pair_t_arithmetic< std::modulus<> >));
	ts->add(BOOST_TEST_CASE(&get_dim_pair));

	utf::framework::master_test_suite().add(ts);

	return utf::unit_test_main(&init_unit_test, argc, argv);
}


#include "iisph.hpp"
#include "spatial/2d/neighborhood_spatial_hashing.hpp"
#include "kernel_debrun_spiky.hpp"
#include "kernel_poly6.hpp"
#include "kernel_poly_viscosity.hpp"
#include "generic/eigen.hpp"
#include "generic/is_finite.hpp"

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

namespace GooBalls {

namespace d2 {

namespace Physics {

using namespace Spatial;

IISPH::IISPH(){
    // Decent defaults
    m_kernelDensity = std::make_unique<Poly6>();
    m_kernelPressure = std::make_unique<DebrunSpiky>();
    m_kernelViscosity = std::make_unique<PolyViscosity>();
}

void IISPH::densityKernel(std::unique_ptr<Kernel>&& k) {
    m_kernelDensity = std::move(k);
}
void IISPH::pressureKernel(std::unique_ptr<Kernel>&& k) {
    m_kernelPressure = std::move(k);
}
void IISPH::viscosityKernel(std::unique_ptr<Kernel>&& k) {
    m_kernelViscosity = std::move(k);
}

void IISPH::computeTotalForce(Scene& scene, TimeStep dt){
    if(scene.fluid.get() == nullptr){
        return;
    }
    prepareFluid(scene);
    prepareBoundary(scene);
    assert(m_kernelDensity.get() != nullptr);
    assert(m_kernelPressure.get() != nullptr);
    assert(m_kernelViscosity.get() != nullptr);
    const Float h = scene.fluid->h();
    m_kernelDensity->setH(h);
    m_kernelPressure->setH(h);
    m_kernelViscosity->setH(h);
    auto& pos = scene.fluid->particles_position();
    assert(is_finite(pos));
    Float K = scene.fluid->stiffnessConstant(); // gas constant dependent on temperature, TODO: correct value?

    // p_i = k rho0 / gamma ((rho_i/rho0)^gamma - 1)
    // F^p_i = m_i sum_j m_j ( p_i / rho_i^2 + p_j / rho_j^2) \nabbla W_ij
    // v^adv_i = v_i + dt * F^adv_i / m_i
    // rho^adv_i = rho_i + dt * sum_j m_j v^adv_ij \nabbla W_ij
    // dt^2 sum_j (m_j/m_i F^p_i - F^p_j) * \nabbla W_ij = rho0 - rho^adv_i
    // sum_j a_ij p_j = b_i = rho_0 - rho^adv_i

    scene.fluid->fluid_neighborhood->inRange(pos, h);
    //computeFluidPressure(scene);
    predictAdvection(scene, dt, *m_kernelDensity);
    pressureSolve(scene, dt, *m_kernelDensity);
    computeMomentumPreservingPressureForce(scene, *m_kernelDensity);
    //computeStandardPressureForce(scene, *m_kernelDensity);

    FPressure = FPressure.array().min(100*K).max(-100*K);
    FViscosity = FViscosity.array().min(100*K).max(-100*K);
    assert(is_finite(FPressure));
    assert(is_finite(FViscosity));
    assert(is_finite(FSurface));
    scene.fluid->particles_total_force() = FPressure + FGravity + FViscosity;
}

void IISPH::predictAdvection(Scene& scene, TimeStep dt, const Kernel& kernel) {
    const auto& pos = scene.fluid->particles_position();
    const auto& vs = scene.fluid->particles_velocity();
    const auto& ps = scene.fluid->particles_pressure();
    const auto& ms = scene.fluid->particles_mass();
    const auto& rho = scene.fluid->particles_density();
    const auto& fluid_index = scene.fluid->fluid_neighborhood->indexes();
    int PN = vs.rows();
    // for all particle i do
    //     compute rho_i(t) = sum_j m_j W_ij
    computeFluidDensity(scene, *m_kernelDensity);
    //     compute F^adv_i = F^SurfaceTension_i + F^Gravity_i + F^Visco_i + ...
    computeGravityForce(scene);
    computeStandardViscosityForce(scene, *m_kernelViscosity);
    // TODO: computeSurfaceTensionForce();
    const auto Fadv = FGravity + FViscosity;
    //     predict v^adv_i = v_i + dt F^adv_i / m_i
    Coordinates2d da = Fadv.array().colwise() / ms.array();
    Coordinates2d Vadv = vs + dt*da;
    //     d_ii = - dt^2 sum_j m_j/rho_i^2 \nabbla W_ij
    dii.resize(PN, 2);
    auto dt2 = dt*dt;
    for(int i = 0; i < PN; ++i){
        const auto& index = fluid_index[i];
        Coordinates2d diis(index.size(), 2);
        Coordinates2d jpos;
        Coordinates2d wGrad;
        pickRows(pos, index, jpos);
        auto xij = -(jpos.rowwise() - pos.row(i));
        kernel.compute(xij, nullptr, &wGrad, nullptr);
        for(size_t j = 0; j < index.size(); ++j){
            int jj = index[j];
            diis.row(j) = ms[jj] * wGrad.row(j);
        }
        dii.row(i) = - dt2/(rho[i]*rho[i]) * diis.colwise().sum();
    }

    // for all particle i do
    // rho^adv_i = rho_i + dt sum_j m_j * v^adv_ij * \nabbla W_ij
    rhoAdv = rho;
    // initializing pi is a bit of an art
    // these are possibilities, last chosen by paper:
    // p^0_i = 0
    // p^0_i = p_i(t-dt).
    // p^0_i = 0.5 p_i(t - dt)
    p0 = 0.5* ps;
    //p0.setZero(PN, 1);
    aii.resize(PN, 1);
    for(int i = 0; i < PN; ++i){
        const auto& index = fluid_index[i];
        Coordinates1d rhos(index.size(), 1);
        Coordinates2d jpos;
        Coordinates2d wGrad;
        pickRows(pos, index, jpos);
        auto xij = -(jpos.rowwise() - pos.row(i));
        kernel.compute(xij, nullptr, &wGrad, nullptr);
        for(size_t j = 0; j < index.size(); ++j){
            int jj = index[j];
            rhos[j] = ms[jj] * (Vadv.row(i) - Vadv.row(jj)).dot(wGrad.row(j));
        }
        rhoAdv[i] += dt*rhos.sum();
        // compute a_ii = sum_j m_j (d_ii - d_ji) nabbla W_ij
        Coordinates1d aiis (index.size(), 1);
        for(size_t j = 0; j < index.size(); ++j){
            int jj = index[j];
            // d_ij = - dt^2 * m_j / rho_j^2 \nabbla W_ij
            TranslationVector dji = dt2 * ms[i] / (rho[i]*rho[i]) * wGrad.row(j);
            aiis[j] = ms[jj] * (dii.row(i) - dji).dot(wGrad.row(j));
        }
        aii[i] = aiis.sum();
    }
}

void IISPH::pressureSolve(Scene& scene, TimeStep dt, const Kernel& kernel) {
    // rho^l_avg = 1/n * sum_i rho^l_i
    // l = 0
    // while rho^l_avg - rho0 > eta OR l < 2 do
    // for all particle i do:
    //    sum_j d_ij p^l_j = dt^2 sum_j - m_j / rho_j^2 p^l_j \nabbla _Wij
    // for all particle i do:
    //    comptue p^(l+1)_i = (1 - omega) p^l_i + omega/a_ii ( rho0 - rho^adv_i - sum_j m_j (sum_j d_ij p^l_j - d_jj p^l_j - sum_{k!= i} d_ji p^l_k ) \nabbla W_ij)
    //    p_i(t) = p_i^l
    // l = l + 1
    const auto& ms = scene.fluid->particles_mass();
    const auto& rho = scene.fluid->particles_density();
    const auto& pos = scene.fluid->particles_position();
    auto& ps = scene.fluid->particles_pressure();
    auto rho0 = scene.fluid->rest_density();
    auto PN = pos.rows();
    const auto& fluid_index = scene.fluid->fluid_neighborhood->indexes();
    Float eta = 0.01*rho0;
    Coordinates1d p1;
    Coordinates1d rhol;
    Coordinates2d dp; // sum_j dij p^l_j, for given i
    p1.resize(PN, 1);
    rhol.setZero(PN, 1);
    dp.resize(PN, 2);
    Float omega = 0.5; // relaxation factor
    int l = 0;
    do {
        //    sum_j d_ij p^l_j = - dt^2 sum_j m_j / rho_j^2 p^l_j \nabbla _Wij
        for(int i = 0; i < PN; ++i){
            const auto& index = fluid_index[i];
            Coordinates2d jpos;
            pickRows(pos, index, jpos);
            Coordinates2d wGrad;
            Coordinates2d xij = -(jpos.rowwise() - pos.row(i));
            kernel.compute(xij, nullptr, &wGrad, nullptr);
            Coordinates2d dps(index.size(), 2);
            for(size_t j = 0; j < index.size(); ++j){
                int jj = index[j];
                dps.row(j) = ms[jj] / (rho[jj] * rho[jj]) * p0[jj] * wGrad.row(j);
            }
            dp.row(i) = -dt*dt * dps.colwise().sum();
        }
        //    comptue p^(l+1)_i = (1 - omega) p^l_i + omega/a_ii (rho0 - rho^adv_i - A_i)
        //         A_i = sum_j m_j (dp_i - d_jj p^l_j - dpp_j ) \nabbla W_ij
        //             dp_i = sum_j d_ij p^l_j
        //             dpp_j = sum_{k!= i} d_jk p^l_k = sum_k d_jk p_k^l - d_ji p_i^l
        //             sum_k d_jk p_k^l = dp.row(j)
        //             d_ij = -dt^2 m_j / rho_j^2 \nabbla W_ij
        // compute A_j and store in p1[i]
        for(int i = 0; i < PN; ++i){
            const auto& indexI = fluid_index[i];
            Coordinates2d jpos;
            pickRows(pos, indexI, jpos);
            Coordinates2d wGradI;
            Coordinates2d xij = -(jpos.rowwise() - pos.row(i));
            kernel.compute(xij, nullptr, &wGradI, nullptr);
            Coordinates1d Ais(indexI.size());
            Coordinates2d wGradJ;
            for(size_t j = 0; j < indexI.size(); ++j){
                int jj = indexI[j];
                kernel.compute(-xij.row(j), nullptr, &wGradJ, nullptr);
                TranslationVector dppj = dp.row(jj) - (-dt*dt*ms[i]/(rho[i]*rho[i]) * wGradJ * p0[i]);
                Ais[j] = ms[jj]*(dp.row(i) - dii.row(jj)*p0[jj] - dppj).dot(wGradI.row(j));
            }
            p1[i] = Ais.sum();
        }
        p1 = (1.0 - omega) * p0.array() + omega/aii.array() * (rho0 - rhoAdv.array() - p1.array());
        std::swap(p0, p1);
        ps = p0;
        //std::cout << p0.mean() << "\n";
        computeMomentumPreservingPressureForce(scene, *m_kernelDensity);
        Coordinates2d dv = dt*(FPressure.array().colwise() / ms.array());
        assert(is_finite(dv));
        for(int i = 0; i < PN; ++i){
            const auto& index = fluid_index[i];
            Coordinates2d jpos;
            pickRows(pos, index, jpos);
            Coordinates2d wGrad;
            Coordinates2d xij = -(jpos.rowwise() - pos.row(i));
            kernel.compute(xij, nullptr, &wGrad, nullptr);
            assert(is_finite(wGrad));
            rhol[i] = 0.0;
            for(size_t j = 0; j < index.size(); ++j){
                int jj = index[j];
                rhol[i] += ms[jj] * (dv.row(i) - dv.row(jj)).dot(wGrad.row(j));
            }
            rhol[i] *= dt;
        }
        l++;
        assert(is_finite(ps));
        assert(is_finite(rhol));
        if(l > 400){
            BOOST_LOG_TRIVIAL(info) << "reached maximum number of iterations with rhol=" << rhol.maxCoeff() << " avg: " << rhol.mean() << ", eta: " << eta;
            break;
        }
    } while (rhol.mean() > eta || l < 2);
    //    p_i(t) = p_i^(l+1)
}

void IISPH::advance(Scene& scene, TimeStep dt){
    if(scene.fluid.get() == nullptr){
        return;
    }
    if(scene.fluid->particles_position().rows() == 0){
        return;
    }
    computeTotalForce(scene, dt);
    Coordinates2d a;
    // a_i = f_i / rho_i
    const auto& rho = scene.fluid->particles_density();
    const auto& Ftotal = scene.fluid->particles_total_force();
    a.resize(rho.rows(), 2);
    a.col(0) = Ftotal.col(0).array() / rho.array();
    a.col(1) = Ftotal.col(1).array() / rho.array();
    auto& pos = scene.fluid->particles_position();
    auto& vs = scene.fluid->particles_velocity();
    vs = vs + dt * a;
    scene.room.restrictFluid(* scene.fluid);
    pos = pos + dt * vs;
    limitVelocity(scene);
}

} // Physics

} // d2

} // GooBalls


// Copyright (C) 2018 The Regents of the University of California (Regents).
// 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 Regents or University of California 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Victor Fragoso (victor.fragoso@mail.wvu.edu)

#include "theia/sfm/pose/upnp.h"

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <glog/logging.h>

#include <algorithm>
#include <complex>
#include <utility>
#include <vector>

#include "theia/alignment/alignment.h"
#include "theia/math/util.h"
#include "theia/sfm/pose/build_upnp_action_matrix.h"
#include "theia/sfm/pose/build_upnp_action_matrix_using_symmetry.h"

namespace theia {

namespace {
typedef Eigen::Matrix<double, 3, 10> Matrix3x10d;
typedef Eigen::Matrix<double, 8, 8> Matrix8d;
typedef Eigen::Matrix<std::complex<double>, 8, 8> Matrix8cd;
typedef Eigen::Matrix<double, 10, 10> Matrix10d;
typedef Eigen::Matrix<double, 16, 16> Matrix16d;
typedef Eigen::Matrix<std::complex<double>, 16, 16> Matrix16cd;
typedef Eigen::Matrix<double, 10, 1> Vector10d;

const int kNumMaxRotations = 16;
const int kNumMaxRotationsExploitingSymmetry = 8;
const int kNumMinCorrespondences = 4;

// Helper structure to simplify function argument list.
struct InputDatum {
  InputDatum(const std::vector<Eigen::Vector3d>& _ray_origins,
             const std::vector<Eigen::Vector3d>& _ray_directions,
             const std::vector<Eigen::Vector3d>& _world_points) :
      ray_origins(_ray_origins),
      ray_directions(_ray_directions),
      world_points(_world_points) {}
  ~InputDatum() = default;

  const std::vector<Eigen::Vector3d>& ray_origins;
  const std::vector<Eigen::Vector3d>& ray_directions;
  const std::vector<Eigen::Vector3d>& world_points;
};

// Computes the H Matrix (see Eq. (6)) and the outer products of the ray
// directions, since these are used to compute matrix V (Eq. (5)).
inline Eigen::Matrix3d ComputeHMatrixAndRayDirectionsOuterProducts(
    const InputDatum& input_datum,
    std::vector<Eigen::Matrix3d>* outer_products) {
  const std::vector<Eigen::Vector3d>& ray_directions =
      input_datum.ray_directions;
  CHECK_NOTNULL(outer_products)->reserve(ray_directions.size());
  Eigen::Matrix3d h_inverse;
  h_inverse.setZero();
  for (const Eigen::Vector3d& ray : ray_directions) {
    outer_products->emplace_back(ray * ray.transpose());
    h_inverse -= outer_products->back();
  }
  h_inverse += ray_directions.size() * Eigen::Matrix3d::Identity();
  return h_inverse.inverse();
}

inline Matrix3x10d LeftMultiply(const Eigen::Vector3d& point) {
  Matrix3x10d phi_mat;
  // Row 0.
  phi_mat(0, 0) = point.x();
  phi_mat(0, 1) = point.x();
  phi_mat(0, 2) = -point.x();
  phi_mat(0, 3) = -point.x();
  phi_mat(0, 4) = 0.0;
  phi_mat(0, 5) = 2 * point.z();
  phi_mat(0, 6) = -2 * point.y();
  phi_mat(0, 7) = 2 * point.y();
  phi_mat(0, 8) = 2 * point.z();
  phi_mat(0, 9) = 0.0;

  // Row 1.
  phi_mat(1, 0) = point.y();
  phi_mat(1, 1) = -point.y();
  phi_mat(1, 2) = point.y();
  phi_mat(1, 3) = -point.y();
  phi_mat(1, 4) = -2.0 * point.z();
  phi_mat(1, 5) = 0.0;
  phi_mat(1, 6) = 2 * point.x();
  phi_mat(1, 7) = 2 * point.x();
  phi_mat(1, 8) = 0.0;
  phi_mat(1, 9) = 2 * point.z();

  // Row 3.
  phi_mat(2, 0) = point.z();
  phi_mat(2, 1) = -point.z();
  phi_mat(2, 2) = -point.z();
  phi_mat(2, 3) = point.z();
  phi_mat(2, 4) = 2.0 * point.y();
  phi_mat(2, 5) = -2.0 * point.x();
  phi_mat(2, 6) = 0.0;
  phi_mat(2, 7) = 0.0;
  phi_mat(2, 8) = 2.0 * point.x();
  phi_mat(2, 9) = 2.0 * point.y();
  return phi_mat;
}

inline std::vector<Eigen::Matrix3d> ComputeHelperMatrices(
    const InputDatum& input_datum,
    const std::vector<Eigen::Matrix3d>& outer_products,
    const Eigen::Matrix3d& h_matrix,
    Matrix3x10d* g_matrix,
    Eigen::Vector3d* j_matrix) {
  const std::vector<Eigen::Vector3d>& world_points = input_datum.world_points;
  const std::vector<Eigen::Vector3d>& ray_origins = input_datum.ray_origins;
  CHECK_EQ(ray_origins.size(), outer_products.size());
  CHECK_NOTNULL(g_matrix)->setZero();
  CHECK_NOTNULL(j_matrix)->setZero();
  const Eigen::Matrix3d identity = Eigen::Matrix3d::Identity();
  std::vector<Eigen::Matrix3d> v_matrices(
      world_points.size(), Eigen::Matrix3d::Zero());
  for (int i = 0; i < ray_origins.size(); ++i) {
    const Eigen::Matrix3d& outer_product = outer_products[i];
    // Computation following Eq. (5).
    v_matrices[i] = h_matrix * (outer_product - identity);
    const Eigen::Matrix3d& v_matrix = v_matrices[i];
    // Compute the left multiplication matrix or Phi matrix in the paper.
    const Matrix3x10d left_multiply_mat = LeftMultiply(world_points[i]);
    *j_matrix += v_matrix * ray_origins[i];
    *g_matrix += v_matrix * left_multiply_mat;
  }
  return v_matrices;
}

// Computes the block matrices that compose the M matrix in Eq. 17. These
// blocks are:
// quadratic_penalty_matrix = \sum A_i^T * A_i,
// linear_penalty_vector = \sum A_i^T * b_i ,
// gamma = \sum b_i^T * b_i.
void ComputeCostParameters(
    const InputDatum& input_datum,
    const std::vector<Eigen::Matrix3d>& outer_products,
    const Matrix3x10d& g_matrix,
    const Eigen::Vector3d& j_matrix,
    Upnp::CostParameters* cost_params) {
  const Eigen::Matrix3d identity = Eigen::Matrix3d::Identity();
  const std::vector<Eigen::Vector3d>& world_points = input_datum.world_points;
  const std::vector<Eigen::Vector3d>& ray_origins = input_datum.ray_origins;
  Matrix10d& a_matrix = CHECK_NOTNULL(cost_params)->quadratic_penalty_mat;
  Vector10d& b_vector = cost_params->linear_penalty_vector;
  double& gamma = cost_params->gamma;

  // Gamma is the sum of the dot products of b_matrices.
  for (int i = 0; i < world_points.size(); ++i) {
    // Compute the left multiplication matrix or Phi matrix in the paper.
    const Matrix3x10d left_multiply_mat = LeftMultiply(world_points[i]);
    const Eigen::Matrix3d outer_prod_minus_identity =
        outer_products[i] - identity;

    // Compute the i-th a_matrix.
    const Matrix3x10d temp_a_mat =
        outer_prod_minus_identity * (left_multiply_mat + g_matrix);
    a_matrix += temp_a_mat.transpose() * temp_a_mat;

    // Compute the i-th b_vector.
    const Eigen::Vector3d temp_b_mat =
        -outer_prod_minus_identity * (ray_origins[i] + j_matrix);
    b_vector += temp_a_mat.transpose() * temp_b_mat;

    // Compute the i-th gamma.
    gamma += temp_b_mat.squaredNorm();
  }
}

// Constructs the vector s as indicated in Eq. 12.
inline Vector10d ComputeRotationVector(const Eigen::Quaterniond& rotation) {
  Vector10d rotation_vector;
  // Set the values of the rotation vector.
  rotation_vector[0] = rotation.w() * rotation.w();
  rotation_vector[1] = rotation.x() * rotation.x();
  rotation_vector[2] = rotation.y() * rotation.y();
  rotation_vector[3] = rotation.z() * rotation.z();
  rotation_vector[4] = rotation.w() * rotation.x();
  rotation_vector[5] = rotation.w() * rotation.y();
  rotation_vector[6] = rotation.w() * rotation.z();
  rotation_vector[7] = rotation.x() * rotation.y();
  rotation_vector[8] = rotation.x() * rotation.z();
  rotation_vector[9] = rotation.y() * rotation.z();
  return rotation_vector;
}

Eigen::Vector3d ComputeTranslation(
    const InputDatum& input_datum,
    const Eigen::Quaterniond& rotation,
    const std::vector<Eigen::Matrix3d>& v_matrices) {
  Eigen::Vector3d translation = Eigen::Vector3d::Zero();
  for (int i = 0; i < input_datum.world_points.size(); ++i) {
    translation +=
        v_matrices[i] *
        (rotation * input_datum.world_points[i] - input_datum.ray_origins[i]);
  }
  return translation;
}

std::vector<Eigen::Vector3d> ComputeTranslations(
    const InputDatum& input_datum,
    const std::vector<Eigen::Quaterniond>& rotations,
    const std::vector<Eigen::Matrix3d>& v_matrices) {
  std::vector<Eigen::Vector3d> translations(rotations.size());
  const int num_points = input_datum.world_points.size();
  const int num_rotations = rotations.size();
  for (int i = 0; i < num_rotations; ++i) {
    translations[i] = ComputeTranslation(input_datum, rotations[i], v_matrices);
  }
  return translations;
}

std::vector<double> ComputeCostsAndRankSolutions(
    const Upnp::CostParameters& cost_params,
    std::vector<Eigen::Quaterniond>* solution_rotations,
    std::vector<Eigen::Vector3d>* solution_translations) {
  std::vector<double> costs(solution_rotations->size(), 0.0);
  std::vector<std::pair<int, double>> indexes_and_costs;
  indexes_and_costs.reserve(costs.size());
  // 1. Compute the costs of the solutions.
  for (int i = 0; i < costs.size(); ++i) {
    costs[i] = Upnp::EvaluateCost(cost_params, solution_rotations->at(i));
    indexes_and_costs.emplace_back(i, costs[i]);
  }

  // 2. Sort the costs such that the best rotation (i.e., lowest error) is the
  // first solution.
  std::sort(indexes_and_costs.begin(), indexes_and_costs.end(),
            [](const std::pair<int, double>& lhs,
               const std::pair<int, double>& rhs) {
              return lhs.second < rhs.second;
            });

  // 3. Rank the solutions.
  std::vector<Eigen::Quaterniond> ranked_rotations(costs.size());
  std::vector<Eigen::Vector3d> ranked_translations(costs.size());
  for (int i = 0; i < costs.size(); ++i) {
    costs[i] = indexes_and_costs[i].second;
    ranked_rotations[i] = solution_rotations->at(indexes_and_costs[i].first);
    ranked_translations[i] =
        solution_translations->at(indexes_and_costs[i].first);
  }

  *solution_rotations = std::move(ranked_rotations);
  *solution_translations = std::move(ranked_translations);

  return costs;
}

void DiscardBadSolutions(const InputDatum& input_datum,
                         std::vector<Eigen::Quaterniond>* solution_rotations,
                         std::vector<Eigen::Vector3d>* solution_translations) {
  CHECK_EQ(CHECK_NOTNULL(solution_rotations)->size(),
           CHECK_NOTNULL(solution_translations)->size());
  std::vector<Eigen::Quaterniond> final_rotations;
  std::vector<Eigen::Vector3d> final_translations;
  final_rotations.reserve(solution_rotations->size());
  final_translations.reserve(solution_translations->size());

  // Useful aliases.
  const std::vector<Eigen::Vector3d>& world_points = input_datum.world_points;
  const std::vector<Eigen::Vector3d>& ray_origins = input_datum.ray_origins;
  const std::vector<Eigen::Vector3d>& ray_directions =
      input_datum.ray_directions;

  // For every computed solution, check that points are in front of camera.
  for (int i = 0; i < solution_rotations->size(); ++i) {
    const Eigen::Quaterniond& soln_rotation = solution_rotations->at(i);
    const Eigen::Vector3d& soln_translation = solution_translations->at(i);

    // Check that all points are in front of the camera. Discard the solution
    // if this is not the case.
    bool all_points_in_front_of_camera = true;

    for (int j = 0; j < world_points.size(); ++j) {
      const Eigen::Vector3d transformed_point =
          soln_rotation * world_points[j] + soln_translation - ray_origins[j];

      // Find the rotation that puts the image ray at [0, 0, 1] i.e. looking
      // straightforward from the camera.
      const Eigen::Quaterniond unrot =
          Eigen::Quaterniond::FromTwoVectors(ray_directions[j],
                                             Eigen::Vector3d::UnitZ());

      // Rotate the transformed point and check if the z coordinate is
      // negative. This will indicate if the point is projected behind the
      // camera.
      const Eigen::Vector3d rotated_projection = unrot * transformed_point;
      if (rotated_projection.z() < 0) {
        all_points_in_front_of_camera = false;
        break;
      }
    }

    if (all_points_in_front_of_camera) {
      final_rotations.emplace_back(soln_rotation);
      final_translations.emplace_back(soln_translation);
    }
  }

  // Set the final solutions.
  std::swap(*solution_rotations, final_rotations);
  std::swap(*solution_translations, final_translations);
}

// The observed pattern is that duplicate rotations appear consequtively in the
// vector, i.e., rotation[i] == rotation[i + 1] is common.
std::vector<Eigen::Quaterniond> RemoveDuplicateRotations(
    const std::vector<Eigen::Quaterniond>& candidate_rotations) {
  const double kAngleThreshold = DegToRad(0.1);
  std::vector<Eigen::Quaterniond> rotations;
  rotations.reserve(candidate_rotations.size());

  // If no rotations then return empty vector.
  if (candidate_rotations.empty()) {
    return rotations;
  }

  for (int i = 0; i < candidate_rotations.size(); ++i) {
    bool duplicate_rotation = false;
    const Eigen::Quaterniond& candidate_rotation = candidate_rotations[i];
    for (int j = rotations.size() - 1; j >= 0; --j) {
      if (candidate_rotation.angularDistance(rotations[j]) < kAngleThreshold) {
        duplicate_rotation = true;
        break;
      }
    }
    if (!duplicate_rotation) {
      rotations.push_back(candidate_rotation);
    }
  }
  return rotations;
}

}  // namespace

inline std::vector<Eigen::Quaterniond>
Upnp::ComputeRotations(const int num_correspondences) {
  // Build the action matrix.
  std::vector<Eigen::Quaterniond> candidate_rotations;
  if (use_minimal_template_ && num_correspondences <= kNumMinCorrespondences) {
    candidate_rotations = SolveForRotationsFromMinimalSample();
  }
  candidate_rotations = SolveForRotationsFromNonMinimalSample();
  // Remove duplicate solutions.
  return RemoveDuplicateRotations(candidate_rotations);
}

double Upnp::EvaluateCost(const Upnp::CostParameters& parameters,
                          const Eigen::Quaterniond& rotation) {
  // Compute the quaternion vector.
  const Vector10d rotation_vector = ComputeRotationVector(rotation);
  return (rotation_vector.transpose() *
          parameters.quadratic_penalty_mat * rotation_vector +
          2.0 * parameters.linear_penalty_vector.transpose() *
          rotation_vector)(0, 0) + parameters.gamma;
}

double Upnp::ComputeResidual(const Eigen::Vector3d& ray_origin,
                             const Eigen::Vector3d& ray_direction,
                             const Eigen::Vector3d& world_point,
                             const Eigen::Quaterniond& rotation,
                             const Eigen::Vector3d& translation) {
  const Eigen::Quaterniond unrot =
      Eigen::Quaterniond::FromTwoVectors(ray_direction,
                                         Eigen::Vector3d::UnitZ());
  const Eigen::Vector3d reprojected_point =
      rotation * world_point + translation - ray_origin;
  const Eigen::Vector3d unrot_reprojected_point = unrot * reprojected_point;
  const Eigen::Vector3d unrot_ray_direction = unrot * ray_direction;
  return (unrot_reprojected_point.hnormalized() -
          unrot_ray_direction.hnormalized()).norm();
}

std::vector<Eigen::Matrix3d> Upnp::ComputeCostParameters(
    const std::vector<Eigen::Vector3d>& ray_origins,
    const std::vector<Eigen::Vector3d>& ray_directions,
    const std::vector<Eigen::Vector3d>& world_points) {
  const InputDatum input_datum(ray_origins, ray_directions, world_points);
  // 1. Compute the H matrix and the outer products of the ray directions.
  std::vector<Eigen::Matrix3d> outer_products;
  const Eigen::Matrix3d h_matrix =
      ComputeHMatrixAndRayDirectionsOuterProducts(input_datum, &outer_products);

  // 2. Compute matrices J and G from page 132 or 6-th page in the paper.
  Matrix3x10d g_matrix;
  Eigen::Vector3d j_matrix;
  const std::vector<Eigen::Matrix3d> v_matrices =
      ComputeHelperMatrices(input_datum,
                            outer_products,
                            h_matrix,
                            &g_matrix,
                            &j_matrix);

  // 3. Compute matrix the block-matrix of matrix M from Eq. 17.
  theia::ComputeCostParameters(input_datum,
                               outer_products,
                               g_matrix,
                               j_matrix,
                               &cost_params_);

  return v_matrices;
}

std::vector<Eigen::Quaterniond> Upnp::SolveForRotationsFromNonMinimalSample() {
  std::vector<Eigen::Quaterniond> rotations(kNumMaxRotationsExploitingSymmetry);
  // Build action matrix.
  const Matrix8d action_matrix = BuildActionMatrixUsingSymmetry(
      cost_params_.quadratic_penalty_mat,
      cost_params_.linear_penalty_vector,
      &non_minimal_sample_template_matrix_);

  const Eigen::EigenSolver<Matrix8d> eigen_solver(action_matrix);
  const Matrix8cd eigen_vectors = eigen_solver.eigenvectors();

  for (int i = 0; i < rotations.size(); ++i) {
    // According to the original implementation, the complex solutions
    // can be good, in particular when the number of correspondences is really
    // low. The solutions simply ignore the imaginary part.
    rotations[i] = Eigen::Quaterniond(eigen_vectors(4, i).real(),
                                      eigen_vectors(5, i).real(),
                                      eigen_vectors(6, i).real(),
                                      eigen_vectors(7, i).real()).normalized();
  }

  return rotations;
}

std::vector<Eigen::Quaterniond> Upnp::SolveForRotationsFromMinimalSample() {
  std::vector<Eigen::Quaterniond> rotations(kNumMaxRotations);
  // Build action matrix.
  const Matrix16d action_matrix =
      BuildActionMatrix(cost_params_.quadratic_penalty_mat,
                        cost_params_.linear_penalty_vector,
                        &minimal_sample_template_matrix_);

  const Eigen::EigenSolver<Matrix16d> eigen_solver(action_matrix, true);
  const Matrix16cd eigen_vectors = eigen_solver.eigenvectors();

  for (int i = 0; i < rotations.size(); ++i) {
    // According to the original implementation, the complex solutions
    // can be good, in particular when the number of correspondences is really
    // low. The solutions simply ignore the imaginary part.
    Eigen::Vector4d quaternion(eigen_vectors(11, i).real(),
                               eigen_vectors(12, i).real(),
                               eigen_vectors(13, i).real(),
                               eigen_vectors(14, i).real());

    if (quaternion[0] < 0.0) {
      quaternion *= -1.0;
    }

    rotations[i] = Eigen::Quaterniond(quaternion[0],
                                      quaternion[1],
                                      quaternion[2],
                                      quaternion[3]).normalized();
  }

  return rotations;
}

bool Upnp::EstimatePose(const std::vector<Eigen::Vector3d>& ray_origins,
                        const std::vector<Eigen::Vector3d>& ray_directions,
                        const std::vector<Eigen::Vector3d>& world_points,
                        std::vector<Eigen::Quaterniond>* solution_rotations,
                        std::vector<Eigen::Vector3d>* solution_translations,
                        std::vector<double>* solution_costs) {
  CHECK_NOTNULL(solution_rotations)->clear();
  CHECK_NOTNULL(solution_translations)->clear();
  CHECK_EQ(ray_origins.size(), ray_directions.size());
  CHECK_EQ(world_points.size(), ray_directions.size());

  // Compute Upnp cost parameters.
  const InputDatum input_datum(ray_origins, ray_directions, world_points);
  const std::vector<Eigen::Matrix3d> v_matrices =
      ComputeCostParameters(ray_origins, ray_directions, world_points);

  // Compute rotations.
  *solution_rotations = ComputeRotations(world_points.size());

  // Compute translation.
  *solution_translations =
      ComputeTranslations(input_datum, *solution_rotations, v_matrices);

  // Discard solutions that have points behind the camera.
  DiscardBadSolutions(input_datum, solution_rotations, solution_translations);

  if (solution_costs) {
    *solution_costs = ComputeCostsAndRankSolutions(cost_params_,
                                                   solution_rotations,
                                                   solution_translations);
  }

  return !solution_rotations->empty();
}

Upnp::CostParameters Upnp(const std::vector<Eigen::Vector3d>& ray_origins,
                          const std::vector<Eigen::Vector3d>& ray_directions,
                          const std::vector<Eigen::Vector3d>& world_points,
                          std::vector<Eigen::Quaterniond>* solution_rotations,
                          std::vector<Eigen::Vector3d>* solution_translations) {
  class Upnp estimator;
  CHECK(estimator.EstimatePose(ray_origins,
                               ray_directions,
                               world_points,
                               solution_rotations,
                               solution_translations))
      << "Could not estimate pose";
  return estimator.cost_params();
}

Upnp::CostParameters Upnp(const std::vector<Eigen::Vector2d>& normalized_pixels,
                          const std::vector<Eigen::Vector3d>& world_points,
                          std::vector<Eigen::Quaterniond>* solution_rotations,
                          std::vector<Eigen::Vector3d>* solution_translations) {
  CHECK_EQ(normalized_pixels.size(), world_points.size());
  std::vector<Eigen::Vector3d> ray_directions(world_points.size());
  std::vector<Eigen::Vector3d> ray_origins(world_points.size());

  // Compute the ray directions and origins from the normalized pixels.
  for (int i = 0; i < world_points.size(); ++i) {
    ray_directions[i] = normalized_pixels[i].homogeneous().normalized();
    ray_origins[i].setZero();
  }

  return Upnp(ray_origins, ray_directions, world_points,
              solution_rotations, solution_translations);
}

}  // namespace theia


#include <iostream>
#include <string>
#include <armadillo>
#include <time.h>
#include "ga.h"

using namespace std;
using namespace arma;

//------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------
mat ga_eval(mat DNAS, vec fitness, double alpha, double reg, double temperature){

  int DNASrows = DNAS.n_rows;
  int DNAScols = DNAS.n_cols;
//  fitness *= 1/max(abs(fitness));

  double myMax=max(max(abs(DNAS)));

	double randBuff=0;
  double aveDNAS=sum(sum(abs(DNAS),0)) /DNASrows/DNAScols;

	double mutRat=0.1*temperature;//*DNAScols;
	double mutAmmount=0.001*aveDNAS*temperature;

  double bigMutRat=0.001*temperature;//*DNAScols;
	double bigMutAmmount=0.005*aveDNAS*temperature;

  double veryBigMutRat=0.0001*temperature;//*DNAScols;
	double verBigMutAmmount=1.0*aveDNAS*temperature;

	int decision[2*DNASrows];
	int myindex;

  rowvec baby = zeros<rowvec>(DNAScols);
	vec orderedV(DNASrows);

	mat orderedDNAS(DNASrows,DNAScols);
	mat newDNAS(DNASrows,DNAScols);
  // without this activation, higher fitness the better.
 fitness.save("fitness.save",raw_ascii);
	fitness=-fitness - reg*sum(DNAS%DNAS,1);
 vec regVec = reg*sum(DNAS%DNAS,1);
 regVec.save("regVec.save",raw_ascii);

	uvec q = sort_index(fitness,"descend");

  for(int j=0;j < DNASrows; j++){
     orderedDNAS.row(j)= DNAS.row(q(j));
	   orderedV(j) = fitness(q(j));
	}

 fitness.save("fitness2.save",raw_ascii);
 q.save("q.save",raw_ascii);
 vec y = exp(alpha*orderedV)/sum(exp(alpha*orderedV));
 y.save("y.save",raw_ascii);
 vec z = cumsum(y);
 z.save("z.save",raw_ascii);

 y.save("distri.dat",raw_ascii);
 z.save("cumsum.dat",raw_ascii);

  for(int j=0;j < 2*DNASrows; j++){
    myindex = index_min(abs(z - (double)(rand() + 1)/(RAND_MAX))  );
    decision[j] = myindex;
  }

    //BEGIN MATING RITUAL
	  for(int i=0; i < 2*DNAS.n_rows-1; i = i + 2){
		//cout << "THE I" << (i)/2 << endl;
       baby =  orderedDNAS.row(decision[i]);
	   //MIXING
	  for(int j=0; j <DNAScols; j++){
       if((double)(rand() + 1)/(RAND_MAX) < 0.5) baby(j) = orderedDNAS.row(decision[i+1])(j);
	  }
	  //SMALL MUTATION
	  for(int j=0; j <DNAScols; j++){
       randBuff = (double)(rand() + 1)/(RAND_MAX);
       if((double)(rand() + 1)/(RAND_MAX) < mutRat) baby(j) = baby(j) + baby(j) * mutAmmount*(2*randBuff-1);
	  }
	  //LARGE MUTATION
	  for(int j=0; j <DNAScols; j++){
       randBuff = (double)(rand() + 1)/(RAND_MAX);
       if((double)(rand() + 1)/(RAND_MAX) < bigMutRat) baby(j) = baby(j) + baby(j) * bigMutAmmount*(2*randBuff-1);
	  }
	  for(int j=0; j <DNAScols; j++){
       randBuff = (double)(rand() + 1)/(RAND_MAX);
       if((double)(rand() + 1)/(RAND_MAX) < veryBigMutRat) baby(j) = baby(j) + baby(j) * verBigMutAmmount*(2*randBuff-1);
	  }

	newDNAS.row(i/2) = baby;
	}

	return newDNAS;
}
//------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------