///////////////////////////////////////////////////////////////////
//  Copyright Eduardo Quintana 2021
//  Copyright Janek Kozicki 2021
//  Copyright Christopher Kormanyos 2021
//  Distributed under the Boost Software License,
//  Version 1.0. (See accompanying file LICENSE_1_0.txt
//  or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_FFT_TEST_HELPERS_HPP
  #define BOOST_MATH_FFT_TEST_HELPERS_HPP

#include <boost/math/fft/algorithms.hpp>
  #include <boost/math/fft/dft_api.hpp>

namespace boost { namespace math { namespace fft {

//template<class NativeComplexType, class allocator_t = std::allocator<NativeComplexType>>
//class test_complex_dft_prime_rader
//{
//  /*
//    Special backend for testing the complex_dft_prime_rader implementation
//  */
//public:
//  using value_type      = NativeComplexType;
//  using real_value_type = typename NativeComplexType::value_type;
//  using allocator_type  = allocator_t;
//public:
//  constexpr test_complex_dft_prime_rader(
//    std::size_t n,
//    const allocator_type& that_alloc = allocator_type{})
//    : my_size{n}, alloc{that_alloc}
//  { }
//
//  void resize(std::size_t new_size)
//  {
//    my_size = new_size;
//  }
//  constexpr std::size_t size() const { return my_size; }
//
//  void forward(const NativeComplexType* in, NativeComplexType* out) const
//  {
//    detail::complex_dft_prime_rader(in,in+size(),out,1,alloc);
//  }
//
//  void backward(const NativeComplexType* in, NativeComplexType* out) const
//  {
//    detail::complex_dft_prime_rader(in,in+size(),out,-1,alloc);
//  }
//private:
//  std::size_t my_size;
//  allocator_type alloc;
//};
//
//template<class RingType, class Allocator_t = std::allocator<RingType> >
//using rader_dft = detail::dft< test_complex_dft_prime_rader<RingType,Allocator_t> >;
//
//template<class NativeComplexType, class allocator_t = std::allocator<NativeComplexType>>
//class test_dft_prime_bruteForce
//{
//  /*
//    Special backend for testing the dft_prime_bruteForce implementation
//  */
//public:
//  using value_type      = NativeComplexType;
//  using real_value_type = typename NativeComplexType::value_type;
//  using allocator_type  = allocator_t;
//
//  constexpr test_dft_prime_bruteForce(
//    std::size_t n,
//    const allocator_type& that_alloc = allocator_type{})
//    : my_size{n}, alloc{that_alloc}
//  { }
//
//  void resize(std::size_t new_size)
//  {
//    my_size = new_size;
//  }
//  constexpr std::size_t size() const { return my_size; }
//
//  void forward(const NativeComplexType* in, NativeComplexType* out) const
//  {
//    NativeComplexType w{detail::complex_root_of_unity<NativeComplexType>(size())};
//    detail::dft_prime_bruteForce(in,in+size(),out,w,alloc);
//  }
//
//  void backward(const NativeComplexType* in, NativeComplexType* out) const
//  {
//    NativeComplexType w{detail::complex_inverse_root_of_unity<NativeComplexType>(size())};
//    detail::dft_prime_bruteForce(in,in+size(),out,w,alloc);
//  }
//
//private:
//  std::size_t my_size;
//  allocator_type alloc;
//};
//
//template<class RingType, class Allocator_t = std::allocator<RingType> >
//using bruteForce_dft = detail::dft< test_dft_prime_bruteForce<RingType,Allocator_t> >;
//
//template<class NativeComplexType, class allocator_t = std::allocator<NativeComplexType>>
//class test_complex_dft_prime_bruteForce
//{
//  /*
//    Special backend for testing the complex_dft_prime_bruteForce implementation
//  */
//public:
//  using value_type      = NativeComplexType;
//  using real_value_type = typename NativeComplexType::value_type;
//  using allocator_type  = allocator_t;
//
//  constexpr test_complex_dft_prime_bruteForce(
//    std::size_t n,
//    const allocator_type& that_alloc = allocator_type{})
//    : my_size{n}, alloc{that_alloc}
//  { }
//
//  void resize(std::size_t new_size)
//  {
//    my_size = new_size;
//  }
//  constexpr std::size_t size() const { return my_size; }
//
//  void forward(const NativeComplexType* in, NativeComplexType* out) const
//  {
//    detail::complex_dft_prime_bruteForce(in,in+size(),out,1,alloc);
//  }
//
//  void backward(const NativeComplexType* in, NativeComplexType* out) const
//  {
//    detail::complex_dft_prime_bruteForce(in,in+size(),out,-1,alloc);
//  }
//
//private:
//  std::size_t my_size;
//  allocator_type alloc;
//};
//
//template<class RingType, class Allocator_t = std::allocator<RingType> >
//using bruteForce_cdft = detail::dft< test_complex_dft_prime_bruteForce<RingType,Allocator_t> >;
//
//
//template<class NativeComplexType, class allocator_t = std::allocator<NativeComplexType>>
//class test_dft_composite
//{
//  /*
//    Special backend for testing the dft_composite
//  */
//public:
//  using value_type = NativeComplexType;
//  using real_value_type = typename NativeComplexType::value_type;
//  using allocator_type  = allocator_t ;
//
//  constexpr test_dft_composite(
//    std::size_t n,
//    const allocator_type& that_alloc = allocator_type{}
//    )
//    : my_size{n}, alloc{that_alloc}
//  { }
//
//  void resize(std::size_t new_size)
//  {
//    my_size = new_size;
//  }
//  constexpr std::size_t size() const { return my_size; }
//
//  void forward(const NativeComplexType* in, NativeComplexType* out) const
//  {
//    NativeComplexType w{detail::complex_root_of_unity<NativeComplexType>(size())};
//    detail::dft_composite(in,in+size(),out,w,alloc);
//  }
//
//  void backward(const NativeComplexType* in, NativeComplexType* out) const
//  {
//    NativeComplexType w{detail::complex_inverse_root_of_unity<NativeComplexType>(size())};
//    detail::dft_composite(in,in+size(),out,w,alloc);
//  }
//
//private:
//  std::size_t my_size;
//  allocator_type alloc;
//};
//
//template<class RingType, class Allocator_t = std::allocator<RingType> >
//using composite_dft = detail::dft< test_dft_composite<RingType,Allocator_t> >;
//
//template<class NativeComplexType, class allocator_t = std::allocator<NativeComplexType>>
//class test_complex_dft_composite
//{
//  /*
//    Special backend for testing the complex_dft_composite
//  */
//public:
//  using value_type = NativeComplexType;
//  using real_value_type = typename NativeComplexType::value_type;
//  using allocator_type  = allocator_t ;
//
//  constexpr test_complex_dft_composite(
//    std::size_t n,
//    const allocator_type& that_alloc = allocator_type{})
//    : my_size{n}, alloc{that_alloc}
//  { }
//
//  void resize(std::size_t new_size)
//  {
//    my_size = new_size;
//  }
//  constexpr std::size_t size() const { return my_size; }
//
//  void forward(const NativeComplexType* in, NativeComplexType* out) const
//  {
//    detail::complex_dft_composite(in,in+size(),out,1,alloc);
//  }
//
//  void backward(const NativeComplexType* in, NativeComplexType* out) const
//  {
//    detail::complex_dft_composite(in,in+size(),out,-1,alloc);
//  }
//
//private:
//  std::size_t my_size;
//  allocator_type alloc;
//};
//
//template<class RingType, class Allocator_t = std::allocator<RingType> >
//using composite_cdft = detail::dft< test_complex_dft_composite<RingType,Allocator_t> >;
//
//
//template<class NativeComplexType, class allocator_t = std::allocator<NativeComplexType>>
//class test_dft_power2
//{
//  /*
//    Special backend for testing the dft_power2 implementation
//  */
//public:
//  using value_type = NativeComplexType;
//  using real_value_type = typename NativeComplexType::value_type;
//  using allocator_type  = allocator_t ;
//  constexpr test_dft_power2(
//    std::size_t n,
//    const allocator_type& that_alloc = allocator_type{})
//    : my_size{n}, alloc{that_alloc}
//  { }
//
//  void resize(std::size_t new_size)
//  {
//    my_size = new_size;
//  }
//  constexpr std::size_t size() const { return my_size; }
//
//  void forward(const NativeComplexType* in, NativeComplexType* out) const
//  {
//    NativeComplexType w{detail::complex_inverse_root_of_unity<NativeComplexType>(size())};
//    detail::dft_power2(in,in+size(),out,w);
//  }
//
//  void backward(const NativeComplexType* in, NativeComplexType* out) const
//  {
//    NativeComplexType w{detail::complex_root_of_unity<NativeComplexType>(size())};
//    detail::dft_power2(in,in+size(),out,w);
//  }
//
//private:
//  std::size_t my_size;
//  allocator_type alloc;
//};
//
//template<class RingType, class Allocator_t = std::allocator<RingType> >
//using power2_dft = detail::dft< test_dft_power2<RingType> >;
//
//
//template<class NativeComplexType, class allocator_t = std::allocator<NativeComplexType>>
//class test_complex_dft_power2
//{
//  /*
//    Special backend for testing the complex_dft_power2 implementation
//  */
//public:
//  using value_type = NativeComplexType;
//  using real_value_type = typename NativeComplexType::value_type;
//  using allocator_type  = allocator_t ;
//
//  constexpr test_complex_dft_power2(
//    std::size_t n,
//    const allocator_type& that_alloc = allocator_type{})
//    : my_size{n}, alloc{that_alloc}
//  { }
//
//  void resize(std::size_t new_size)
//  {
//    my_size = new_size;
//  }
//  constexpr std::size_t size() const { return my_size; }
//
//  void forward(const NativeComplexType* in, NativeComplexType* out) const
//  {
//    detail::complex_dft_power2(in,in+size(),out,1);
//  }
//
//  void backward(const NativeComplexType* in, NativeComplexType* out) const
//  {
//    detail::complex_dft_power2(in,in+size(),out,-1);
//  }
//
//private:
//  std::size_t my_size;
//  allocator_type alloc;
//};
//
//template<class RingType, class Allocator_t = std::allocator<RingType> >
//using power2_cdft = detail::dft< test_complex_dft_power2<RingType> >;

namespace my_modulo_lib
{
  template <typename T, T x>
  class field_modulo
  {
   public:
    typedef T integer;
    static constexpr T mod = x;
  };

  /*
      Modular Integers
  */
  template <typename Field>
  class mint;

  template <typename Field>
  std::ostream& operator<<(std::ostream& os, const mint<Field>& A)
  {
    return os << A.x << " (mod " << Field::mod << ")";
  }
  template <typename Field>
  class mint
  {
    typedef typename Field::integer integer;
    integer x;

   public:
    constexpr mint() : x{0} {}

    mint(integer _x) : x{_x}
    {
      x %= Field::mod;
      if (x < 0)
        x += Field::mod;
    }

    mint(const mint<Field>& that) : x{that.x} {}

    mint<Field>& operator=(const mint<Field>& that)
    {
      return x = that.x, *this;
    }

    // explicit operator bool() const { return x == integer{0}; }
    operator integer() const { return x; }

    mint<Field>& operator+=(const mint<Field>& that)
    {
      return x = (x + that.x) % Field::mod, *this;
    }

    mint<Field>& operator*=(const mint<Field>& t)
    {
      // direct multiplication
      x = (x * t.x) % Field::mod;
      return *this;
    }
    bool operator==(const mint<Field>& that) const { return x == that.x; }
    bool operator!=(const mint<Field>& that) const { return x != that.x; }

    mint<Field> inverse() const { return ::boost::math::fft::detail::power(*this, Field::mod - 2); }

    friend std::ostream& operator<<<Field>(std::ostream& os,
                                           const mint<Field>& A);
  };

  template <typename T>
  mint<T> operator+(const mint<T>& A, const mint<T>& B)
  {
    mint<T> C{A};
    return C += B;
  }
  template <typename T>
  mint<T> operator*(const mint<T>& A, const mint<T>& B)
  {
    mint<T> C{A};
    return C *= B;
  }
  template <typename T>
  mint<T>& operator/=(mint<T>& A, const mint<T>& B)
  {
    return A *= B.inverse();
  }
  template <typename T>
  mint<T> operator/(const mint<T>& A, const mint<T>& B)
  {
    mint<T> C{A};
    return C /= B;
  }
}  // namespace my_modulo_lib

}}} // namespace boost::math::fft

#endif // BOOST_MATH_FFT_TEST_HELPERS_HPP


// ALGOLAB BGL Tutorial 3
// Flow example demonstrating
// - breadth first search (BFS) on the residual graph

// Compile and run with one of the following:
// g++ -std=c++11 -O2 bgl_residual_bfs.cpp -o bgl_residual_bfs ./bgl_residual_bfs
// g++ -std=c++11 -O2 -I path/to/boost_1_58_0 bgl_residual_bfs.cpp -o bgl_residual_bfs; ./bgl_residual_bfs

// Includes
// ========
// STL includes
#include <iostream>
#include <algorithm>
#include <vector>
#include <queue>
// BGL includes
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/cycle_canceling.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/graph/successive_shortest_path_nonnegative_weights.hpp>
#include <boost/graph/find_flow_cost.hpp>

// Graph Type with nested interior edge properties for Cost Flow Algorithms
typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> traits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
    boost::property<boost::edge_capacity_t, long,
        boost::property<boost::edge_residual_capacity_t, long,
            boost::property<boost::edge_reverse_t, traits::edge_descriptor,
                boost::property <boost::edge_weight_t, long> > > > > graph; // new! weightmap corresponds to costs

typedef boost::graph_traits<graph>::edge_descriptor             edge_desc;
typedef boost::graph_traits<graph>::out_edge_iterator           out_edge_it; // Iterator

// Custom edge adder class
class edge_adder {
 graph &G;

 public:
  explicit edge_adder(graph &G) : G(G) {}
  void add_edge(int from, int to, long capacity, long cost) {
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    auto w_map = boost::get(boost::edge_weight, G); // new!
    const edge_desc e = boost::add_edge(from, to, G).first;
    const edge_desc rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0; // reverse edge has no capacity!
    r_map[e] = rev_e;
    r_map[rev_e] = e;
    w_map[e] = cost;   // new assign cost
    w_map[rev_e] = -cost;   // new negative cost
  }
};

// Main
void testcase() {
  // build graph
  int n, m, s;
  std::cin >> n >> m >> s;
  graph G(n + m + s);
  edge_adder adder(G);
  // auto rc_map = boost::get(boost::edge_residual_capacity, G);
  const int v_source = boost::add_vertex(G);
  const int v_sink = boost::add_vertex(G);

  auto c_map = boost::get(boost::edge_capacity, G);
  auto rc_map = boost::get(boost::edge_residual_capacity, G);

  for(int i = 0; i < s; i++) {
    int l;
    std::cin >> l;
    adder.add_edge(v_source, i, l, 0); // no cost
  }

  for(int i = 0; i < m; i++) {
    int si;
    std::cin >> si;
    adder.add_edge(si - 1, s + i, 1, 0); // no cost
  }

  std::vector<int> costs(n * m);

  for(int i = 0; i < n; i++) {
    for(int j = 0; j < m; j++) {
      int bij;
      std::cin >> bij;
      costs[i * m + j] = bij;
      // adder.add_edge(s + j, s + m + i, 1, -bij); // no cost
    }
    adder.add_edge(s + m + i, v_sink, 1, 0); // 1 purchase per buyer, no cost
  }

  int max_cost = *std::max_element(costs.begin(), costs.end());
  for(int i = 0; i < n; i++) {
    for(int j = 0; j < m; j++) {
      int bij = costs[i * m + j];
      adder.add_edge(s + j, s + m + i, 1, -bij + max_cost); // no cost
    }
  }

  // Find a min cut via maxflow
  boost::successive_shortest_path_nonnegative_weights(G, v_source, v_sink);
  int cost = boost::find_flow_cost(G);

  // Iterate over all edges leaving the source to sum up the flow values.
  int s_flow = 0;
  out_edge_it e, eend;
  for(boost::tie(e, eend) = boost::out_edges(boost::vertex(v_source,G), G); e != eend; ++e) {
      // std::cout << "edge from " << boost::source(*e, G) << " to " << boost::target(*e, G)
      //     << " with capacity " << c_map[*e] << " and residual capacity " << rc_map[*e] << "\n";
      s_flow += c_map[*e] - rc_map[*e];
  }
  std::cout << s_flow << " "; // 5
  std::cout << -cost + (s_flow * max_cost) << "\n";

}

int main() {
  std::ios_base::sync_with_stdio(false);
  std::size_t t;
  for (std::cin >> t; t > 0; --t) testcase();
  return 0;
}


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// This file is part of the Boost Graph Library
//
// You should have received a copy of the License Agreement for the
// Boost Graph Library along with the software; see the file LICENSE.
// If not, contact Office of Research, Indiana University,
// Bloomington, IN 47405.
//
// Permission to modify the code and to distribute the code is
// granted, provided the text of this NOTICE is retained, a notice if
// the code was modified is included with the above COPYRIGHT NOTICE
// and with the COPYRIGHT NOTICE in the LICENSE file, and that the
// LICENSE file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//=======================================================================
#include <vector>
#include <string>
#include <iostream>
#include <boost/graph/topological_sort.hpp>
#include <boost/graph/stanford_graph.hpp>

int
main()
{
  using namespace boost;
  const int n_vertices = 7;
  Graph *sgb_g = gb_new_graph(n_vertices);

  const char *tasks[] = {
    "pick up kids from school",
    "buy groceries (and snacks)",
    "get cash at ATM",
    "drop off kids at soccer practice",
    "cook dinner",
    "pick up kids from soccer",
    "eat dinner"
  };
  const int n_tasks = sizeof(tasks) / sizeof(char *);

  gb_new_arc(sgb_g->vertices + 0, sgb_g->vertices + 3, 0);
  gb_new_arc(sgb_g->vertices + 1, sgb_g->vertices + 3, 0);
  gb_new_arc(sgb_g->vertices + 1, sgb_g->vertices + 4, 0);
  gb_new_arc(sgb_g->vertices + 2, sgb_g->vertices + 1, 0);
  gb_new_arc(sgb_g->vertices + 3, sgb_g->vertices + 5, 0);
  gb_new_arc(sgb_g->vertices + 4, sgb_g->vertices + 6, 0);
  gb_new_arc(sgb_g->vertices + 5, sgb_g->vertices + 6, 0);

  typedef graph_traits < Graph * >::vertex_descriptor vertex_t;
  std::vector < vertex_t > topo_order;
  topological_sort(sgb_g, std::back_inserter(topo_order),
                   vertex_index_map(get(vertex_index, sgb_g)));
  int n = 1;
  for (std::vector < vertex_t >::reverse_iterator i = topo_order.rbegin();
       i != topo_order.rend(); ++i, ++n)
    std::cout << n << ": " << tasks[get(vertex_index, sgb_g)[*i]] << std::endl;

  gb_recycle(sgb_g);
  return EXIT_SUCCESS;
}


#include <iostream>
#include <cstdint> // Includes integer types
#include <vector>
#include <string>

#include <boost/safe_numerics/safe_integer.hpp>

// variables8.cpp
// Make me compile! Go to the folder hint if you want a hint :)

// We sometimes encourage you to keep trying things on a given exercise,
// even after you already figured it out.

// Try to find the constant values matching the expected results.
// This exercises showcases integer overflows. This occurs when the
// selected datatype is not large enough to hold the value.
// See https://www.learncpp.com/cpp-tutorial/fixed-width-integers-and-size-t/
// See: https://www.learncpp.com/cpp-tutorial/unsigned-integers-and-why-to-avoid-them/

using namespace boost::safe_numerics;

std::vector<std::string> words;

//Hint: Use safe<> type from the safe_numerics (see more details in hint file)
uint32_t len; // This global variable keeps track of how may words should be accessed

// init_words is called one before test_danger_loop
void init_words(){
    words = { "Hello", "World", "!"};
    len = words.size(); // Only those 3 words should be accessed
}

const std::string secret_key = "Encrypted S3cr3t!";

std::string test_danger_loop(uint32_t  query_idx) { // Use safe<> type
    // Tip: if you cannot find the problem replace all uint32_t with
    // safe<uint32_t>, from the boost::safe_numerics library
    // The safe<uint32_t> will throw an exception to help you find the bug

    words.push_back(secret_key); // Copies a secret at the end of the vector

    std::string response;
    if (len - query_idx > 0) { // Bound checking for protecting secret
        response = words.at(query_idx);
    }
    return response;
}

#include <catch2/catch.hpp>

TEST_CASE("integer_signedness") {
    // Initialize data
    const std::vector<std::string> expected_words { "Hello", "World", "!"};
    init_words();

    // Run loop
    for (size_t i = 0; i < expected_words.size(); i++)
    {
        std::cout << "Next index to query " << i << ", this is word: " << words[i] << "\n";
        REQUIRE(test_danger_loop(i) == expected_words.at(i));
    }

    std::cout << "Next index to query is dangerous :" << 4 << ", this is word:" << words[4] << "\n";
    REQUIRE(test_danger_loop(len + 1) == "");
}


#include <cslibs_boost_geometry/algorithms.h>

#include <gtest/gtest.h>
#include <boost/geometry/algorithms/area.hpp>

#include "points_and_lines.hpp"

using namespace cslibs_boost_geometry::algorithms;
using namespace cslibs_boost_geometry::types;
using namespace cslibs_boost_geometry::test_samples;

TEST(TestCSLibsBoostGeometry, testPolygons)
{
    Polygon2d test_result;
    circularPolygonApproximation(Point2d(0.0,0.0),
                                 1.0,
                                 rad(1.0),
                                 test_result);

    Polygon2d::ring_type &outer_ring = test_result.outer();

    EXPECT_EQ(outer_ring.size(), 361);
    EXPECT_TRUE(equal(outer_ring.front(), Point2d(1.0, 0.0), 1e-9));
    EXPECT_TRUE(equal(outer_ring.back(),  Point2d(1.0, 0.0), 1e-9));
    EXPECT_TRUE(outer_ring[0].x() > outer_ring[1].x());
    EXPECT_TRUE(outer_ring[0].y() > outer_ring[1].y());
    EXPECT_TRUE(outer_ring[0].x() > outer_ring[2].x());
    EXPECT_TRUE(outer_ring[0].y() > outer_ring[2].y());
    EXPECT_TRUE(outer_ring[0].x() > outer_ring[outer_ring.size() - 3].x());
    EXPECT_TRUE(outer_ring[0].y() < outer_ring[outer_ring.size() - 3].y());

    EXPECT_TRUE(equal(outer_ring[90],  Point2d(0.0, -1.0), 1e-9));
    EXPECT_TRUE(equal(outer_ring[180], Point2d(-1.0, 0.0), 1e-9));
    EXPECT_TRUE(equal(outer_ring[270], Point2d( 0.0, 1.0), 1e-9));
}

TEST(TestCSLibsBoostGeometry, testWithin)
{
    Polygon2d test_result;
    circularPolygonApproximation(Point2d(0.0,0.0),
                                 1.0,
                                 rad(1.0),
                                 test_result);
    EXPECT_TRUE(withinExcl<Point2d>(point_a, test_result));
}

TEST(TestCSLibsBoostGeometry, testIntersection)
{
    Polygon2d test_result;
    circularPolygonApproximation(Point2d(0.0,0.0),
                                 1.0,
                                 rad(1.0),
                                 test_result);
    EXPECT_FALSE(intersects<Point2d>(line_e, test_result));
    EXPECT_TRUE(intersects<Point2d>(line_b, test_result));
}

TEST(TestCSLibsBoostGeometry, testTouching)
{
    Polygon2d test_result;
    circularPolygonApproximation(Point2d(0.0,0.0),
                                 1.0,
                                 rad(1.0),
                                 test_result);
    EXPECT_TRUE(touches<Point2d>(line_b, test_result));
    EXPECT_FALSE(touches<Point2d>(line_e, test_result));
    EXPECT_FALSE(touches<Point2d>(line_f, test_result));
}

int main(int argc, char *argv[])
{
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}


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

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

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <string>

#include <geometry_test_common.hpp>

#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/algorithms/equals.hpp>
#include <boost/geometry/algorithms/make.hpp>
#include <boost/geometry/extensions/algorithms/distance_info.hpp>
#include <boost/geometry/io/wkt/wkt.hpp>

template <typename Result>
void check_distance_info(Result const& result,
                          std::string const& expected_pp,
                          bool expected_on_segment,
                          double expected_projected_distance,
                          double expected_real_distance,
                          double expected_fraction)
{
    if (! expected_pp.empty())
    {
        std::ostringstream out;
        out << bg::wkt(result.projected_point1);
        std::string wkt_projected = out.str();

        BOOST_CHECK_EQUAL(wkt_projected, expected_pp);
    }
    BOOST_CHECK_EQUAL(result.on_segment, expected_on_segment);
    BOOST_CHECK_CLOSE(result.fraction1, expected_fraction, 0.001);
    BOOST_CHECK_CLOSE(result.projected_distance1, expected_projected_distance, 0.001);
    BOOST_CHECK_CLOSE(result.real_distance, expected_real_distance, 0.001);
}

template <typename Geometry1, typename Geometry2>
void test_distance_info(Geometry1 const& geometry1, Geometry2 const& geometry2,
                          std::string const& expected_pp,
                          bool expected_on_segment,
                          double expected_projected_distance,
                          double expected_real_distance,
                          double expected_fraction)
{
    typename bg::distance_info_result<typename bg::point_type<Geometry1>::type> result, reversed_result;
    bg::distance_info(geometry1, geometry2, result);
    check_distance_info(result,
                expected_pp, expected_on_segment,
                expected_projected_distance, expected_real_distance,
                expected_fraction);

    // Check reversed version too.
    std::string reversed_expected_pp = expected_pp;
    if (boost::is_same<typename bg::tag<Geometry1>::type, bg::point_tag>::value
        && boost::is_same<typename bg::tag<Geometry2>::type, bg::point_tag>::value
        )
    {
        // For point-point, we cannot check projected-point again, it is also the other one.
        reversed_expected_pp.clear();
    }
    bg::distance_info(geometry2, geometry1, reversed_result);
    check_distance_info(reversed_result,
                reversed_expected_pp,
                expected_on_segment,
                expected_projected_distance, expected_real_distance,
                expected_fraction);
}

template <typename Geometry1>
void test_distance_info(std::string const& wkt, std::string const& wkt_point,
                          std::string const& expected_pp,
                          bool expected_on_segment,
                          double expected_projected_distance,
                          double expected_real_distance,
                          double expected_fraction)
{
    Geometry1 geometry1;
    typename bg::point_type<Geometry1>::type point;
    bg::read_wkt(wkt, geometry1);
    bg::read_wkt(wkt_point, point);

    test_distance_info(geometry1, point, expected_pp, expected_on_segment,
                expected_projected_distance, expected_real_distance,
                expected_fraction);
}

template <typename P>
void test_2d()
{
    test_distance_info<bg::model::segment<P> >("LINESTRING(2 0,4 0)", "POINT(3 2)", "POINT(3 0)", true, 2.0, 2.0, 0.5);
    test_distance_info<bg::model::segment<P> >("LINESTRING(2 0,4 0)", "POINT(2 0)", "POINT(2 0)", true, 0.0, 0.0, 0.0);
    test_distance_info<bg::model::segment<P> >("LINESTRING(2 0,4 0)", "POINT(4 0)", "POINT(4 0)", true, 0.0, 0.0, 1.0);
    test_distance_info<bg::model::segment<P> >("LINESTRING(2 0,4 0)", "POINT(5 2)", "POINT(5 0)", false, 2.0, sqrt(5.0), 1.5);
    test_distance_info<bg::model::segment<P> >("LINESTRING(2 0,4 0)", "POINT(0 2)", "POINT(0 0)", false, 2.0, sqrt(8.0), -1.0);

    // Degenerated segment
    test_distance_info<bg::model::segment<P> >("LINESTRING(2 0,2 0)", "POINT(4 0)", "POINT(2 0)", false, 2.0, 2.0, 0.0);

    // Linestring
    test_distance_info<bg::model::linestring<P> >("LINESTRING(2 0,4 0)", "POINT(3 2)", "POINT(3 0)", true, 2.0, 2.0, 0.5);

    // Point-point
    test_distance_info<P>("Point(1 1)", "POINT(2 2)", "POINT(2 2)", false, sqrt(2.0), sqrt(2.0), 0.0);
}

template <typename P>
void test_3d()
{
    test_distance_info<bg::model::segment<P> >("LINESTRING(0 0 0,5 5 5)", "POINT(2 3 4)", "POINT(3 3 3)", true, sqrt(2.0), sqrt(2.0), 0.6);
}

int test_main(int, char* [])
{
    test_2d<bg::model::d2::point_xy<int> >();
    test_2d<bg::model::d2::point_xy<float> >();
    test_2d<bg::model::d2::point_xy<double> >();

    test_3d<bg::model::point<double, 3, bg::cs::cartesian> >();

    return 0;
}


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file gaussian_filter_test_suite.hpp
 * \date June 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#pragma once

#include <gtest/gtest.h>
#include "../typecast.hpp"

#include <Eigen/Dense>

#include <cmath>
#include <iostream>

#include <fl/util/profiling.hpp>
#include <fl/util/math/linear_algebra.hpp>
#include <fl/filter/filter_interface.hpp>

#include <fl/model/transition/linear_transition.hpp>
#include <fl/model/sensor/linear_gaussian_sensor.hpp>
#include <fl/model/sensor/linear_decorrelated_gaussian_sensor.hpp>

template <typename TestType>
class GaussianFilterTest
    : public ::testing::Test
{
protected:
    typedef typename TestType::Parameter Configuration;

    enum: signed int
    {
        StateDim = Configuration::StateDim,
        InputDim = Configuration::InputDim,
        ObsrvDim = Configuration::ObsrvDim,

        StateSize = fl::TestSize<StateDim, TestType>::Value,
        InputSize = fl::TestSize<InputDim, TestType>::Value,
        ObsrvSize = fl::TestSize<ObsrvDim, TestType>::Value,

        FilterIterations = Configuration::Iterations
    };

    enum ModelSetup
    {
        Random,
        Identity
    };

    typedef Eigen::Matrix<fl::Real, StateSize, 1> State;
    typedef Eigen::Matrix<fl::Real, InputSize, 1> Input;
    typedef Eigen::Matrix<fl::Real, ObsrvSize, 1> Obsrv;

    GaussianFilterTest()
        : predict_steps_(30),
          predict_update_steps_(FilterIterations)
    { }

    struct ModelFactory
    {
        // Sizes will be 1 of the test type is static and it will fall back to
        // -1 for dynamic size tests. This may be used as an expansion factor
        // using fl::ExpandSizes<MySize, Sizes>::Value. If Sizes is equal to 1
        // (Static) then ExpandSizes will simply multiply MySizes by 1 and
        // everything is defined statically. On the other hand, if the Sizes
        // is -1, fl::ExpandSizes will fallback to -1 indicating that the test
        // type is dynamic.
        enum : signed int { Sizes = fl::TestSize<1, TestType>::Value };

        typedef fl::LinearTransition<
                    State, Input
                > LinearTransition;

        typedef fl::LinearGaussianSensor<
                    Obsrv, State
                > LinearObservation;

        LinearTransition create_linear_state_model()
        {
            auto model = LinearTransition(StateDim, InputDim);

            auto A = model.create_dynamics_matrix();
            auto Q = model.create_noise_matrix();

            switch (setup)
            {
            case Random:
                A.setRandom();
                Q.setRandom();
                break;

            case Identity:
                A.setIdentity();
                Q.setIdentity();
                break;
            }

            model.dynamics_matrix(A);
            model.noise_matrix(Q);

            return model;
        }

        LinearObservation create_sensor()
        {
            auto model = LinearObservation(ObsrvDim, StateDim);

            auto H = model.create_sensor_matrix();
            auto R = model.create_noise_matrix();

            switch (setup)
            {
            case Random:
                H.setRandom();
                R.setRandom();
                break;

            case Identity:
                H.setIdentity();
                R.setIdentity();
                break;
            }

            model.sensor_matrix(H);
            model.noise_matrix(R);

            return model;
        }

        ModelSetup setup;
    };

    typedef typename Configuration::template FilterDefinition<
                ModelFactory
            >::Type Filter;

    Filter create_filter(ModelSetup setup = Identity) const
    {
        return Configuration::create_filter(ModelFactory{setup});
    }

    typename fl::Traits<Filter>::Input zero_input(const Filter& filter)
    {
        return fl::Traits<Filter>::Input::Zero(
            filter.transition().input_dimension());
    }

    typename fl::Traits<Filter>::Obsrv rand_obsrv(const Filter& filter)
    {
        return fl::Traits<Filter>::Obsrv::Random(
            filter.sensor().obsrv_dimension());
    }

protected:
    int predict_steps_;
    int predict_update_steps_;
};

TYPED_TEST_CASE_P(GaussianFilterTest);

//TYPED_TEST_P(GaussianFilterTest, init_predict)
//{
//    typedef TestFixture This;

//    auto filter = This::create_filter();
//    auto belief = filter.create_belief();

//    EXPECT_TRUE(belief.mean().isZero());
//    EXPECT_TRUE(belief.covariance().isIdentity());

//    std::cout << filter.name() << std::endl;
//    std::cout << filter.description() << std::endl;

//    filter.predict(belief, This::zero_input(), belief);

//    auto Q = filter.transition().noise_covariance();

//    EXPECT_TRUE(belief.mean().isZero());
//    EXPECT_TRUE(fl::are_similar(belief.covariance(), 2. * Q));
//}

TYPED_TEST_P(GaussianFilterTest, predict_then_update)
{
    typedef TestFixture This;

    auto filter = This::create_filter(This::Identity);

    PV(filter.name());

    auto belief = filter.create_belief();

    EXPECT_TRUE(belief.covariance().ldlt().isPositive());

    for (int i = 0; i < This::predict_update_steps_; ++i)
    {
        PF(i);
//        PV(belief.mean());
//        PV(belief.covariance());

        filter.predict(belief, This::zero_input(filter), belief);

//        if (!belief.covariance().ldlt().isPositive())
//        {
//            PV(belief.mean());
//            PV(belief.covariance());
//        }

        ASSERT_TRUE(belief.covariance().ldlt().isPositive());

        filter.update(belief, This::rand_obsrv(filter), belief);

//        if (!belief.covariance().ldlt().isPositive())
//        {
//            PV(belief.mean());
//            PV(belief.covariance());
//        }

        ASSERT_TRUE(belief.covariance().ldlt().isPositive());
    }

    PV(belief.mean());
    PV(belief.covariance());
}

TYPED_TEST_P(GaussianFilterTest, predict_loop)
{
    typedef TestFixture This;

    auto filter = This::create_filter(This::Identity);
    auto belief = filter.create_belief();

    EXPECT_TRUE(belief.covariance().ldlt().isPositive());

    for (int i = 0; i < This::predict_steps_; ++i)
    {
        filter.predict(belief, This::zero_input(filter), belief);
    }

    EXPECT_TRUE(belief.covariance().ldlt().isPositive());
}

REGISTER_TYPED_TEST_CASE_P(GaussianFilterTest,
                           //init_predict,
                           predict_then_update,
                           predict_loop);


//          Copyright Carl Philipp Reh 2009 - 2016.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <fcppt/math/bresenham.hpp>
#include <fcppt/math/vector/comparison.hpp>
#include <fcppt/math/vector/object_impl.hpp>
#include <fcppt/math/vector/output.hpp>
#include <fcppt/math/vector/static.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/test/unit_test.hpp>
#include <vector>
#include <fcppt/config/external_end.hpp>

// TODO: Add a test for bresenham_thick as well

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	math_bresenham
)
{
FCPPT_PP_POP_WARNING

	typedef
	fcppt::math::vector::static_<
		int,
		2
	>
	int2_vector;

	typedef
	std::vector<
		int2_vector
	>
	int2_vector_sequence;

	int2_vector_sequence result;

	fcppt::math::bresenham(
		int2_vector(
			0,
			0
		),
		int2_vector(
			10,
			4
		),
		[
			&result
		](
			int2_vector const _vec
		)
		{
			result.push_back(
				_vec
			);

			return
				true;
		}
	);

	BOOST_REQUIRE_EQUAL(
		result.size(),
		11u
	);

	BOOST_CHECK_EQUAL(
		result[0],
		int2_vector(
			0,
			0
		)
	);

	BOOST_CHECK_EQUAL(
		result[1],
		int2_vector(
			1,
			0
		)
	);

	BOOST_CHECK_EQUAL(
		result[2],
		int2_vector(
			2,
			1
		)
	);

	BOOST_CHECK_EQUAL(
		result[3],
		int2_vector(
			3,
			1
		)
	);

	BOOST_CHECK_EQUAL(
		result[4],
		int2_vector(
			4,
			2
		)
	);

	BOOST_CHECK_EQUAL(
		result[5],
		int2_vector(
			5,
			2
		)
	);

	BOOST_CHECK_EQUAL(
		result[6],
		int2_vector(
			6,
			2
		)
	);

	BOOST_CHECK_EQUAL(
		result[7],
		int2_vector(
			7,
			3
		)
	);

	BOOST_CHECK_EQUAL(
		result[8],
		int2_vector(
			8,
			3
		)
	);

	BOOST_CHECK_EQUAL(
		result[9],
		int2_vector(
			9,
			4
		)
	);

	BOOST_CHECK_EQUAL(
		result[10],
		int2_vector(
			10,
			4
		)
	);
}


#include "CalibrationHelpers.h"

#include <iostream>
#include <fstream>
#include <string>
#include <Eigen/Dense>

namespace romocc
{

std::vector<Transform3d> invert_matrices(std::vector<Transform3d> matrices)
{
    std::vector<Transform3d> invertedMatrices;

    for(int i = 0; i<matrices.size(); i++)
    {
        invertedMatrices.push_back(matrices.at(i).inverse());
    }
    return invertedMatrices;
}

double compute_average(std::vector<double> const &v)
{
    double sum = 0;
    for(int i=0; i<v.size(); i++)
        sum += v[i];
    return sum/v.size();
}

double compute_variance(std::vector<double> const &v, double mean)
{
    double sum = 0.0;
    double temp =0.0;
    double var =0.0;

    for ( int j =0; j <= v.size()-1; j++)
    {
        temp = std::pow(v[j]-mean,2);
        sum += temp;
    }

    return var = sum/(v.size()-2);
}

std::vector<double> compute_linspace(double a, double b, int n)
{
    std::vector<double> array;
    double step = (b-a) / (n-1);

    while(a <= b) {
        array.push_back(a);
        a += step;
    }
    return array;
}

double sgn(double val)
{
    return (double(0) < val) - (val < double(0));
}

Transform3d load_calibration_file(std::string filepath){
    auto calMat = Eigen::Affine3d::Identity();
    std::ifstream inFile;
    inFile.open(filepath);

    if(inFile.is_open())
    {
        for(int row = 0; row < calMat.rows(); row++){
            for(int col = 0; col < calMat.cols(); col++)
            {
                double item = 0;
                inFile >> item;
                calMat(row, col) = item;
            }
        }
        inFile.close();
    }
    return calMat;
}

void save_calibration_file(std::string path, Eigen::Affine3d calMat) {
    std::ofstream outFile;
    outFile.open(path);

    if(outFile.is_open()){
        outFile << calMat.matrix();
        outFile.close();
    }
}

}


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
///
///          Distributed under the Boost Software License, Version 1.0
///                 See accompanying file LICENSE.txt or copy at
///                     http://www.boost.org/LICENSE_1_0.txt
//////////////////////////////////////////////////////////////////////////////
#ifndef NT2_TOOLBOX_REDUCTION_FUNCTION_SIMD_COMMON_PROD_HPP_INCLUDED
#define NT2_TOOLBOX_REDUCTION_FUNCTION_SIMD_COMMON_PROD_HPP_INCLUDED
#include <nt2/sdk/constant/digits.hpp>
#include <boost/fusion/tuple.hpp>
#include <boost/fusion/include/fold.hpp>
#include <nt2/sdk/meta/strip.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type  is arithmetic_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::prod_, tag::cpu_,
                       (A0)(X),
                       ((simd_<arithmetic_<A0>,X>))
                      );

namespace nt2 { namespace ext
{
  template<class X, class Dummy>
  struct call<tag::prod_(tag::simd_<tag::arithmetic_, X> ),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)>
    {
      typedef typename meta::scalar_of<A0>::type                base;
      typedef typename std::tr1::result_of<meta::arithmetic(base)>::type type;
    };

    NT2_FUNCTOR_CALL(1)
    {
      typedef typename NT2_RETURN_TYPE(1)::type     type;
      return boost::fusion::fold(a0,One<type>(),functor<tag::multiplies_>());
    }

  };
} }

#endif
// modified by jt the 05/01/2011


// ------------------------------------------------------------
// Copyright (c) Microsoft Corporation.  All rights reserved.
// Licensed under the MIT License (MIT). See License.txt in the repo root for license information.
// ------------------------------------------------------------

#include "stdafx.h"
#include "RandomDistribution.h"

#include <boost/test/unit_test.hpp>
#include "Common/boost-taef.h"

namespace PlacementAndLoadBalancingUnitTest
{
    using namespace std;
    using namespace Common;
    using namespace Reliability::LoadBalancingComponent;

    class TestRandomDistribution
    {
    protected:
        TestRandomDistribution()
        {
            BOOST_REQUIRE(ClassSetup());
        }
        TEST_CLASS_SETUP(ClassSetup);
        uint32_t seed_;
    };

    BOOST_FIXTURE_TEST_SUITE(RandomDistributionTestSuite, TestRandomDistribution)

    BOOST_AUTO_TEST_CASE(AllOnesDistributionTest)
    {
        RandomDistribution rd(seed_);
        DistributionMap distribution =
            rd.GenerateDistribution(10000, RandomDistribution::Enum::AllOnes);

        VERIFY_ARE_EQUAL(1u, distribution.size());
        VERIFY_ARE_EQUAL(10000u, rd.GetNumberOfSamples(distribution));

        VERIFY_ARE_EQUAL(10000u, distribution[1]);
    }

    BOOST_AUTO_TEST_CASE(UniformDistributionTest)
    {
        RandomDistribution rd(seed_);
        DistributionMap distribution =
            rd.GenerateDistribution(10000, RandomDistribution::Enum::Uniform, -50, 49);

        VERIFY_ARE_EQUAL(100u, distribution.size());
        VERIFY_ARE_EQUAL(10000u, rd.GetNumberOfSamples(distribution));

        for (int i = -50; i <= 49; i++)
        {
            VERIFY_ARE_EQUAL(100u, distribution[i]);
        }
    }

    BOOST_AUTO_TEST_CASE(ExponentialDistributionTest)
    {
        RandomDistribution rd(seed_);
        DistributionMap distribution =
            rd.GenerateDistribution(10000, RandomDistribution::Enum::Exponential, -50, 50);

        VERIFY_ARE_EQUAL(10000u, rd.GetNumberOfSamples(distribution));
    }

    BOOST_AUTO_TEST_CASE(GaussianDistributionTest)
    {
        RandomDistribution rd(seed_);
        DistributionMap distribution =
            rd.GenerateDistribution(10000, RandomDistribution::Enum::Gaussian, -50, 50);

        VERIFY_ARE_EQUAL(10000u, rd.GetNumberOfSamples(distribution));
    }

    BOOST_AUTO_TEST_CASE(RandomSeedTest)
    {
        RandomDistribution rd1(100);
        RandomDistribution rd2(200);
        RandomDistribution rd3(200);

        DistributionMap d1_g = rd1.GenerateDistribution(10000, RandomDistribution::Enum::Gaussian, -50, 49);
        DistributionMap d2_g = rd2.GenerateDistribution(10000, RandomDistribution::Enum::Gaussian, -50, 49);
        DistributionMap d3_g = rd3.GenerateDistribution(10000, RandomDistribution::Enum::Gaussian, -50, 49);

        VERIFY_ARE_EQUAL(d2_g, d3_g);
        VERIFY_IS_TRUE(d1_g != d2_g);
        VERIFY_IS_TRUE(d1_g != d3_g);

        DistributionMap d1_e = rd1.GenerateDistribution(10000, RandomDistribution::Enum::Exponential, -50, 49);
        DistributionMap d2_e = rd2.GenerateDistribution(10000, RandomDistribution::Enum::Exponential, -50, 49);
        DistributionMap d3_e = rd3.GenerateDistribution(10000, RandomDistribution::Enum::Exponential, -50, 49);

        VERIFY_ARE_EQUAL(d2_e, d3_e);
        VERIFY_IS_TRUE(d1_e != d2_e);
        VERIFY_IS_TRUE(d1_e != d3_e);
    }

    BOOST_AUTO_TEST_SUITE_END()

    bool TestRandomDistribution::ClassSetup()
    {
        seed_ = 500;
        return TRUE;
    }
}


#include "RBGL.hpp"
#include "Basic2DMatrix.hpp"
#include <boost/graph/depth_first_search.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/topological_sort.hpp>
#include <boost/graph/strong_components.hpp>
#include <boost/graph/edge_connectivity.hpp>
#include <boost/graph/biconnected_components.hpp>
#include <boost/graph/sequential_vertex_coloring.hpp>

/* need a template with C++ linkage for BFS */
/* adapted from Siek's bfs-example.cpp */

template < typename TimeMap > class bfs_time_visitor
    : public boost::default_bfs_visitor {
    typedef typename boost::property_traits < TimeMap >::value_type T;
public:
    bfs_time_visitor(TimeMap tmap, T & t):m_timemap(tmap), m_time(t) { }
    template < typename Vertex, typename Graph >
    void discover_vertex(Vertex u, const Graph & g) const
    {
        put(m_timemap, u, m_time++);
    }
    TimeMap m_timemap;
    T & m_time;
};

template < typename TimeMap > class dfs_time_visitor
    : public boost::default_dfs_visitor {
    typedef typename boost::property_traits < TimeMap >::value_type T;
public:
    dfs_time_visitor(TimeMap dmap, TimeMap fmap, T & t)
            :  m_dtimemap(dmap), m_ftimemap(fmap), m_time(t) {
    }
    template < typename Vertex, typename Graph >
    void discover_vertex(Vertex u, const Graph & g) const
    {
        put(m_dtimemap, u, m_time++);
    }
    template < typename Vertex, typename Graph >
    void finish_vertex(Vertex u, const Graph & g) const
    {
        put(m_ftimemap, u, m_time++);
    }
    TimeMap m_dtimemap;
    TimeMap m_ftimemap;
    T & m_time;
};

extern "C"
{
    SEXP BGL_tsort_D(SEXP num_verts_in, SEXP num_edges_in, SEXP R_edges_in)
    {
        // tsortbCG -- for bioConductor graph objects

        using namespace boost;
        typedef graph_traits < Graph_dd >::edge_descriptor Edge;
        typedef graph_traits < Graph_dd >::vertex_descriptor Vertex;
        Graph_dd g(num_verts_in, num_edges_in, R_edges_in);

        typedef property_map<Graph_dd, vertex_color_t>::type Color;
        graph_traits<Graph_dd>::vertex_iterator viter, viter_end;

        typedef std::list<Vertex> tsOrder;
        tsOrder tsord;
        SEXP tsout;

        PROTECT(tsout = NEW_NUMERIC(INTEGER(num_verts_in)[0]));

        try {
            topological_sort(g, std::front_inserter(tsord));

            int j = 0;
            for (tsOrder::iterator i = tsord.begin();
                    i != tsord.end(); ++i)
            {
                REAL(tsout)[j] = (double) *i;
                j++;
            }
        }
        catch ( not_a_dag )
        {
            warning("not a DAG.\n");
            for (int j = 0 ; j < INTEGER(num_verts_in)[0]; j++)
                REAL(tsout)[j] = 0.0;
        }
        UNPROTECT(1);

        return(tsout);
    } // end BGL_tsort_D

    SEXP BGL_bfs_D(SEXP num_verts_in, SEXP num_edges_in,
                   SEXP R_edges_in, SEXP R_weights_in, SEXP init_ind)
    {
        using namespace boost;

        typedef graph_traits < Graph_dd >::edge_descriptor Edge;
        typedef graph_traits < Graph_dd >::vertex_descriptor Vertex;
        Graph_dd g(num_verts_in, num_edges_in, R_edges_in, R_weights_in);

        typedef graph_traits < Graph_dd >::vertices_size_type size_type;

        const int N = INTEGER(num_verts_in)[0];
        // Typedefs
        typedef size_type* Iiter;

        // discover time properties
        std::vector < size_type > dtime(num_vertices(g));

        size_type time = 0;
        bfs_time_visitor < size_type * >vis(&dtime[0], time);
        breadth_first_search(g, vertex((int)INTEGER(init_ind)[0], g), visitor(vis));

        // use std::sort to order the vertices by their discover time
        std::vector < size_type > discover_order(N);
        integer_range < size_type > r(0, N);
        std::copy(r.begin(), r.end(), discover_order.begin());
        std::sort(discover_order.begin(), discover_order.end(),
                  indirect_cmp < Iiter, std::less < size_type > >(&dtime[0]));

        SEXP disc;
        PROTECT(disc = allocVector(INTSXP,N));

        int i;
        for (i = 0; i < N; ++i)
        {
            INTEGER(disc)[i] = discover_order[i];
        }

        UNPROTECT(1);
        return(disc);
    }

    SEXP BGL_dfs_D(SEXP num_verts_in, SEXP num_edges_in, SEXP R_edges_in,
                   SEXP R_weights_in)
    {
        using namespace boost;

        typedef graph_traits < Graph_dd >::edge_descriptor Edge;
        typedef graph_traits < Graph_dd >::vertex_descriptor Vertex;
        Graph_dd g(num_verts_in, num_edges_in, R_edges_in, R_weights_in);

        typedef graph_traits < Graph_dd >::vertices_size_type size_type;

        const int N = INTEGER(num_verts_in)[0];
        // Typedefs
        typedef size_type* Iiter;

        // discover time and finish time properties
        std::vector < size_type > dtime(num_vertices(g));
        std::vector < size_type > ftime(num_vertices(g));
        size_type t = 0;
        dfs_time_visitor < size_type * >vis(&dtime[0], &ftime[0], t);

        depth_first_search(g, visitor(vis));

        // use std::sort to order the vertices by their discover time
        std::vector < size_type > discover_order(N);
        integer_range < size_type > r(0, N);
        std::copy(r.begin(), r.end(), discover_order.begin());
        std::sort(discover_order.begin(), discover_order.end(),
                  indirect_cmp < Iiter, std::less < size_type > >(&dtime[0]));
        std::vector < size_type > finish_order(N);
        std::copy(r.begin(), r.end(), finish_order.begin());
        std::sort(finish_order.begin(), finish_order.end(),
                  indirect_cmp < Iiter, std::less < size_type > >(&ftime[0]));

        SEXP ansList;
        PROTECT(ansList = allocVector(VECSXP,2));
        SEXP disc;
        PROTECT(disc = allocVector(INTSXP,N));
        SEXP fin;
        PROTECT(fin = allocVector(INTSXP,N));

        int i;
        for (i = 0; i < N; ++i)
        {
            INTEGER(disc)[i] = discover_order[i];
            INTEGER(fin)[i] = finish_order[i];
        }

        SET_VECTOR_ELT(ansList,0,disc);
        SET_VECTOR_ELT(ansList,1,fin);
        UNPROTECT(3);
        return(ansList);
    }

    SEXP BGL_connected_components_U (SEXP num_verts_in,
                                     SEXP num_edges_in, SEXP R_edges_in,
				     SEXP R_weights_in )
    {
        using namespace boost;
        SEXP outvec;

        typedef graph_traits < Graph_ud >::edge_descriptor Edge;
        typedef graph_traits < Graph_ud >::vertex_descriptor Vertex;
        Graph_ud g(num_verts_in, num_edges_in, R_edges_in, R_weights_in);

        int nvert = INTEGER(num_verts_in)[0] ;

        std::vector<int> component(num_vertices(g));
        connected_components(g, &component[0]);

        std::vector<int>::size_type k;

        PROTECT(outvec = allocVector(REALSXP,nvert));

        for (k = 0; k < component.size(); k++ )
            REAL(outvec)[k] = component[k];

        UNPROTECT(1);
        return(outvec);
    }

    SEXP BGL_strong_components_D (SEXP num_verts_in,
                                  SEXP num_edges_in, SEXP R_edges_in,
                                  SEXP R_weights_in )
    {
        using namespace boost;
        SEXP outvec;

        typedef graph_traits < Graph_dd >::edge_descriptor Edge;
        typedef graph_traits < Graph_dd >::vertex_descriptor Vertex;
        Graph_dd g(num_verts_in, num_edges_in, R_edges_in, R_weights_in);

        int nvert = INTEGER(num_verts_in)[0] ;

        std::vector<int> component(num_vertices(g));
        strong_components(g, &component[0]);

        std::vector<int>::size_type k;

        PROTECT(outvec = allocVector(REALSXP,nvert));

        for (k = 0; k < component.size(); k++ )
            REAL(outvec)[k] = component[k];

        UNPROTECT(1);
        return(outvec);
    }

    SEXP BGL_biconnected_components_U (SEXP num_verts_in,
                                  SEXP num_edges_in, SEXP R_edges_in,
                                  SEXP R_weights_in )
    {
        using namespace boost;
        SEXP outvec;

        typedef graph_traits < Graph_ud >::edge_descriptor Edge;
        typedef graph_traits < Graph_ud >::vertex_descriptor Vertex;
        Graph_ud g(num_verts_in, num_edges_in, R_edges_in, R_weights_in);

        int ne = INTEGER(num_edges_in)[0] ;

        // this is a bit cheating: use "edge_weight_t" for "edge_component_t"
        property_map < Graph_ud, edge_weight_t >::type
              component = get(edge_weight, g);
        graph_traits < Graph_ud >::edge_iterator ei, ei_end;
        for (tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
            component[*ei] = (int)-1;
        int num_comps = biconnected_components(g, component);

        SEXP ansList, eList, nc;
        PROTECT(ansList = allocVector(VECSXP,3));
        PROTECT(nc = NEW_INTEGER(1));
        PROTECT(eList = allocMatrix(INTSXP, 2, ne));
        PROTECT(outvec = allocMatrix(INTSXP, 1, ne));

        INTEGER(nc)[0] = num_comps;
        int ke = 0;
        int k = 0;
        for (tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
        {
            INTEGER(eList)[ke++] = (int)source(*ei, g);
            INTEGER(eList)[ke++] = (int)target(*ei, g);
            INTEGER(outvec)[k++] = (int)component[*ei];
        }

        SET_VECTOR_ELT(ansList,0,nc);
        SET_VECTOR_ELT(ansList,1,eList);
        SET_VECTOR_ELT(ansList,2,outvec);
        UNPROTECT(4);
        return(ansList);
    }

    SEXP BGL_articulation_points_U (SEXP num_verts_in,
                                  SEXP num_edges_in, SEXP R_edges_in,
                                  SEXP R_weights_in )
    {
        using namespace boost;

        typedef graph_traits < Graph_ud >::edge_descriptor Edge;
        typedef graph_traits < Graph_ud >::vertex_descriptor Vertex;
        Graph_ud g(num_verts_in, num_edges_in, R_edges_in, R_weights_in);

        std::vector<Vertex> art_points;
        articulation_points(g, std::back_inserter(art_points));

        SEXP outvec;
        PROTECT(outvec = allocVector(INTSXP,art_points.size()));

        for (unsigned int k = 0; k < art_points.size(); k++ )
            INTEGER(outvec)[k] = art_points[k];

        UNPROTECT(1);
        return(outvec);
    }

    SEXP BGL_edge_connectivity_U (SEXP num_verts_in,
                                  SEXP num_edges_in, SEXP R_edges_in,
                                  SEXP R_weights_in )
    {
        using namespace boost;
        SEXP ansList, conn, edTmp;

        typedef graph_traits < Graph_ud >::edge_descriptor Edge;
        typedef graph_traits < Graph_ud >::vertex_descriptor Vertex;
        Graph_ud g(num_verts_in, num_edges_in, R_edges_in, R_weights_in);

        typedef graph_traits<Graph_ud>::degree_size_type dst;
        std::vector<Edge> disconnecting_set;
        std::vector<Edge>::iterator ei;
        dst c = edge_connectivity( g, std::back_inserter(disconnecting_set) );

        PROTECT(conn = NEW_NUMERIC(1));
        REAL(conn)[0] = (double)c;

        SEXP eList;
        PROTECT(ansList = allocVector(VECSXP,2));

        PROTECT(eList = allocVector(VECSXP,(int)c));

        SET_VECTOR_ELT(ansList,0,conn);

        int sind = 0;
        for (ei = disconnecting_set.begin(); ei != disconnecting_set.end();
                ++ei)
        {
            PROTECT(edTmp = NEW_NUMERIC(2));
            REAL(edTmp)[0] = (double)source(*ei,g);
            REAL(edTmp)[1] = (double)target(*ei,g);
            SET_VECTOR_ELT(eList,sind,edTmp);
            sind=sind+1;
            UNPROTECT(1);
        }

        SET_VECTOR_ELT(ansList,1,eList);
        UNPROTECT(3);
        return(ansList);
    }

    SEXP BGL_sequential_vertex_coloring (SEXP num_verts_in,
    		SEXP num_edges_in, SEXP R_edges_in)
    {
        using namespace boost;

        typedef graph_traits < Graph_ud >::edge_descriptor Edge;
        typedef graph_traits < Graph_ud >::vertex_descriptor Vertex;
        typedef graph_traits < Graph_ud >::vertices_size_type Vertex_Size_Type;
        typedef property_map < Graph_ud, vertex_index_t >::const_type vertex_index_map;

        Graph_ud g(num_verts_in, num_edges_in, R_edges_in);

        std::vector<Vertex_Size_Type> color_vec(num_vertices(g));
        iterator_property_map < Vertex_Size_Type*, vertex_index_map >
             color(&color_vec.front(), get(vertex_index, g));
        Vertex_Size_Type n = sequential_vertex_coloring( g, color );

        SEXP ansList, nc, cList;
        PROTECT(ansList = allocVector(VECSXP,2));
        PROTECT(nc = NEW_INTEGER(1));
        PROTECT(cList = allocVector(INTSXP, num_vertices(g)));
        INTEGER(nc)[0] = (int)n;

        Graph_ud::vertex_iterator vi, v_end;
        int i = 0;
        for (tie(vi, v_end) = vertices(g); vi != v_end; ++vi)
        {
            INTEGER(cList)[i++] = color_vec[*vi];
        }

        SET_VECTOR_ELT(ansList, 0, nc);
        SET_VECTOR_ELT(ansList, 1, cList);
        UNPROTECT(3);
        return(ansList);
    }

    SEXP BGL_astar_search_D (SEXP num_verts_in,
    		SEXP num_edges_in, SEXP R_edges_in )
    {
        // TODO: fill in
    	using namespace boost;

        SEXP ansList;
        PROTECT(ansList = NEW_INTEGER(1));
        INTEGER(ansList)[0] = 0;
        UNPROTECT(1);
        return(ansList);
    }
}


/**
 *  sim_carAbst.cpp
 *
 *  To simulate the vehicle example using abstraction-based control.
 *
 *  Authors: Yinan Li, Zhibing Sun, Jun Liu
 *  Created: May 27, 2020
 *
 *  Hybrid Systems Group, University of Waterloo.
 */

#include <iostream>
#include <fstream>
#include <sstream>
#include <cstdlib>

#include <string>
#include <math.h>
#include <boost/numeric/odeint.hpp>

#include "src/abstraction.hpp"
#include "src/DBAparser.h"
#include "src/bsolver.hpp"
#include "src/hdf5io.h"

/* define dynamics */
struct car_dynamics {
	rocs::Rn u;
	car_dynamics (const rocs::Rn param): u (param) {}

	void operator() (rocs::Rn &x, rocs::Rn &dxdt, double t) const
	{
	      double alpha = std::atan(std::tan(u[1])/2.0);
	      dxdt[0] = u[0]*std::cos(alpha+x[2])/std::cos(alpha);
	      dxdt[1] = u[0]*std::sin(alpha+x[2])/std::cos(alpha);
	      dxdt[2] = u[0]*std::tan(u[1]);
	}
};

int main(int argc, char *argv[])
{
    /**
     * Default arguments
     */
    std::string specfile{"dba1.txt"};
    double eta[]{0.2, 0.2, 0.2};

    /* Input arguments:
     * sim_abst specfile precision(e.g. 0.2 0.2 0.2)
     */
    if (argc > 2 && argc < 5) {
	std::cout << "Improper number of arguments. Input arguments are:\n";
	std::cout << "./sim_abst specfile precision(e.g. 0.2 0.2 0.2)\n";
	std::exit(1);
    }
    if(argc > 1) {
	specfile = std::string(argv[1]);
    }
    if (argc == 5) {
	for(int i = 2; i < 5; ++i)
	    eta[i-2] = std::atof(argv[i]);
    }
    std::vector<std::string> tokens;
    boost::split(tokens, specfile, boost::is_any_of("."));
    std::string ctlrfile = "controller_" + tokens[0] + "_";
    for(int i = 0; i < 3; ++i) {
	std::stringstream ss;
	ss << std::setprecision(1);
	ss << eta[i];
	ctlrfile += ss.str();
	if (i < 2)
	    ctlrfile += "-";
    }
    ctlrfile += ".h5";

    /**
     * Load specification
     **/
    std::cout << "\nReading the specification...\n";
    rocs::UintSmall nAP = 0, nNodes = 0, q0 = 0;
    std::vector<rocs::UintSmall> acc;
    std::vector<std::vector<rocs::UintSmall>> arrayM;
    if (!rocs::read_spec(specfile, nNodes, nAP, q0, arrayM, acc))
	std::exit(1);
    boost::dynamic_bitset<> isacc(nNodes, false);
    for (rocs::UintSmall i = 0; i < acc.size(); ++i)
	isacc[acc[i]] = true;

    /**
     * Setup the motion planning workspace
     **/
    /* set the state space */
    const int x_dim = 3;
    const double theta = 3.5;
    double xlb[] = {0, 0, -theta};
    double xub[] = {10, 10, theta};
    /* set the control values */
    const int u_dim = 2;
    double ulb[] = {-1.0, -1.0};
    double uub[] = {1.0, 1.0};
    double mu[] = {0.3, 0.3};
    /* Generate grids */
    rocs::grid x_grid(x_dim,eta,xlb,xub);
    x_grid.gridding();
    rocs::grid u_grid(u_dim,mu,ulb,uub);
    std::cout << "Number of discrete states: " << x_grid._nv << "\n";
    std::cout << "Number of discrete inputs: " << u_grid._nv << "\n";

    /**
     * Load global controller
     **/
    std::cout << "\nLoading the controller from " << ctlrfile << "...\n";
    std::vector<long long> w_x0, encode3;
    std::vector<NODE_POST> nts_ctrlr;
    std::vector<CTRL> ctrl;
    std::vector<int> q_prime;
    rocs::h5FileHandler planRdr(ctlrfile, H5F_ACC_RDONLY);
    planRdr.read_discrete_controller(w_x0, encode3, nts_ctrlr, ctrl, q_prime);

    /**
     * Simulation
     **/
    const double tsim = 0.3;  //simulation time step
    /* Open files for writing results and logs */
    std::string simfile = "sim_traj_"+tokens[0]+".txt";
    std::ofstream ctlrWtr(simfile);
    if(!ctlrWtr.is_open())
	ctlrWtr.open(simfile, std::ios::out);
	/********** Logging **********/
    std::ofstream logger;
    logger.open("logs_"+tokens[0]+".txt", std::ios::out);
    /********** Logging **********/

    /* ode solver */
    const double dt = 0.001; //integration step size for odeint
    boost::numeric::odeint::runge_kutta_cash_karp54<rocs::Rn> rk45;

    /* Initial condition */
    rocs::Rn x{3, 2, M_PI/2.};
    rocs::Rn u{0, 0};
    long long x_index = x_grid.val_to_id(x);
    std::vector<long long>::iterator p1;
    p1 = std::lower_bound(w_x0.begin(), w_x0.end(), x_index);
    if(*p1 != x_index) {
	std::cout << x[0] <<  " "  << x[1] << " " << x[2];
	std::cout << " is not in the winning set, change a new initial state." << std::endl;
	return 1;
    }

    float tpat = 0;
    int max_num_achieve_acc=5, max_num_iteration=500; //3000000;
    rocs::UintSmall q;
    int i, j;
    std::vector<CTRL>::iterator p7;
    NODE_POST p5;
    std::cout << "\nLaunching simulation...\n";
    for(q = q0, i = j = 0; i<max_num_achieve_acc && j<max_num_iteration; ++j) {
	/* Determine the control u by the product state (x_index, q) */
	p5 = nts_ctrlr[encode3[x_index]];
	p7 = std::lower_bound(ctrl.begin()+p5.pos, ctrl.begin()+p5.pos+p5.num_a,
			      q, [](const CTRL &item, const int val) {
				     return item.q < val;});
	if(p7->q != q) {
	    std::cout << j << "th iteration: " << "Reach accepting state " << i << " times.\n";
	    std::cout << "Automaton state: " << q << '\n';
	    std::cout << "Position in ctrl list: " << p5.pos << ", number of actions: " << p5.num_a << '\n';
	    std::cout << "Error in ctrl" << std::endl;
	    break;
	}
	u_grid.id_to_val(u, p7->u);

	/* Print to screen */
	std::cout << "# " << j << ":\n";
	std::cout << "x: [" << x[0] <<  ','  << x[1] << ',' << x[2] << "]\n";
	std::cout << "Nominal control: " << p7->u << '('
		  << '[' << u[0] << ',' << u[1] << "])\n";
	std::cout << "current dba state: " << q << "\n";

	/* Write to txt file */
	// ctlrWtr << j << ':';
	for(int d = 0; d < x_dim; ++d) {
	    ctlrWtr << x[d];
	    if(d<x_dim-1) {
		ctlrWtr << ',';
	    }
	}
	ctlrWtr << ';';
	for(int d = 0; d < u_dim; ++d) {
	    ctlrWtr << u[d];
	    if(d<u_dim-1) {
		ctlrWtr << ',';
	    }
	}
	ctlrWtr << '\n';

	/* Integrate the dynamics */
	boost::numeric::odeint::integrate_const(rk45, car_dynamics(u), x, 0.0, tsim, dt);
	if(x[2] > M_PI) //convert angle into [-pi, pi]
	    x[2] -= 2*M_PI;
	if(x[2] < -M_PI)
	    x[2] += 2*M_PI;

	/********** Logging **********/
    logger << "x,q,u=" << x_index << ' ' << q << ' ' << p7->u << "; encode3[x]=" << encode3[x_index];
	logger << "\np5=" << p5.num_a << ' ' << p5.label << ' ' << p5.pos;
	logger << "\np7=";
	for(std::vector<CTRL>::iterator p = ctrl.begin()+p5.pos;
		p!=ctrl.begin()+p5.pos+p5.num_a; ++p) {
		logger << p->q << ' ' << p->u << ';';
	}
	logger << "\n\n";
    /********** Logging **********/

	/* Update automaton state q */
	x_index = x_grid.val_to_id(x);
	q = q_prime[p5.label*nNodes + q];
	if(isacc[q])
	    std::cout << "******** achieve acc \'" << q << "\' " << ++i << " time ********" << std::endl;
    }

    ctlrWtr.close();
	logger.close();

    return 0;
}


#define BOOST_TEST_MODULE DikinEllipsoidCalculatorTestSuite
#define BOOST_TEST_DYN_LINK

#include <boost/test/included/unit_test.hpp>
#include <Eigen/Core>
#include <hops/hops.hpp>

BOOST_AUTO_TEST_SUITE(DikinEllipsoidCalculator)

    BOOST_AUTO_TEST_CASE(Cube) {
        const long rows = 6;
        const long cols = 3;
        Eigen::MatrixXd A(rows, cols);
        A << 1, 0, 0,
                0, 1, 0,
                0, 0, 1,
                -1, 0, 0,
                0, -1, 0,
                0, 0, -1;
        Eigen::VectorXd b(rows);
        b << 1, 1, 1, 1, 1, 1;

        hops::DikinEllipsoidCalculator dikinEllipsoidCalculator(A, b);
        Eigen::VectorXd interiorPoint(cols);
        for (size_t i = 0; i < cols; ++i) {
            interiorPoint(i) = 0;
        }

        auto actualDikinEllipsoid = dikinEllipsoidCalculator.computeDikinEllipsoid(interiorPoint);

        BOOST_CHECK(actualDikinEllipsoid == 2 * Eigen::MatrixXd::Identity(cols, cols));
    }

    BOOST_AUTO_TEST_CASE(CholeskyOfCube) {
        const long rows = 6;
        const long cols = 3;
        Eigen::MatrixXd expectedDikinEllipsoid = 2 * Eigen::MatrixXd::Identity(cols, cols);

        Eigen::MatrixXd A(rows, cols);
        A << 1, 0, 0,
                0, 1, 0,
                0, 0, 1,
                -1, 0, 0,
                0, -1, 0,
                0, 0, -1;
        Eigen::VectorXd b(rows);
        b << 1, 1, 1, 1, 1, 1;

        hops::DikinEllipsoidCalculator dikinEllipsoidCalculator(A, b);
        Eigen::VectorXd interiorPoint(cols);
        for (size_t i = 0; i < cols; ++i) {
            interiorPoint(i) = 0;
        }

        auto[choleskyWasSuccessful, actualDikinEllipsoidLowerFactor] = dikinEllipsoidCalculator.computeCholeskyFactorOfDikinEllipsoid(
                interiorPoint);
        Eigen::MatrixXd actualDikinEllipsoid =
                actualDikinEllipsoidLowerFactor * actualDikinEllipsoidLowerFactor.transpose();

        BOOST_CHECK(choleskyWasSuccessful);
        BOOST_CHECK(((actualDikinEllipsoid - expectedDikinEllipsoid).array() < 1e-15).all());
    }

    BOOST_AUTO_TEST_CASE(Simplex) {
        const long rows = 4;
        const long cols = 3;

        Eigen::MatrixXd expectedDikinEllipsoid(cols, cols);
        expectedDikinEllipsoid << 3.35012345679, 2.56, 2.56,
                2.56, 3.35012345679, 2.56,
                2.56, 2.56, 3.35012345679;

        Eigen::MatrixXd A(rows, cols);
        A << 1, 1, 1,
                -1, 0, 0,
                0, -1, 0,
                0, 0, -1;
        Eigen::VectorXd b(rows);
        b << 1, 1, 1, 1;

        hops::DikinEllipsoidCalculator dikinEllipsoidCalculator(A, b);
        Eigen::VectorXd interiorPoint(cols);
        for (size_t i = 0; i < cols; ++i) {
            interiorPoint(i) = 1. / 8;
        }

        auto actualDikinEllipsoid = dikinEllipsoidCalculator.computeDikinEllipsoid(interiorPoint);

        BOOST_CHECK(((actualDikinEllipsoid - expectedDikinEllipsoid).array() < 1e-12).all());
    }

    BOOST_AUTO_TEST_CASE(CholeskyOfSimplex) {
        const long rows = 4;
        const long cols = 3;

        Eigen::MatrixXd expectedDikinEllipsoid(cols, cols);
        expectedDikinEllipsoid << 3.35012345679, 2.56, 2.56,
                2.56, 3.35012345679, 2.56,
                2.56, 2.56, 3.35012345679;

        Eigen::MatrixXd A(rows, cols);
        A << 1, 1, 1,
                -1, 0, 0,
                0, -1, 0,
                0, 0, -1;
        Eigen::VectorXd b(rows);
        b << 1, 1, 1, 1;

        hops::DikinEllipsoidCalculator dikinEllipsoidCalculator(A, b);
        Eigen::VectorXd interiorPoint(cols);
        for (size_t i = 0; i < cols; ++i) {
            interiorPoint(i) = 1. / 8;
        }

        auto[choleskyWasSuccessful, actualDikinEllipsoidLowerFactor] = dikinEllipsoidCalculator.computeCholeskyFactorOfDikinEllipsoid(
                interiorPoint);
        Eigen::MatrixXd actualDikinEllipsoid =
                actualDikinEllipsoidLowerFactor * actualDikinEllipsoidLowerFactor.transpose();

        BOOST_CHECK(choleskyWasSuccessful);
        BOOST_CHECK(((actualDikinEllipsoid - expectedDikinEllipsoid).array() < 1e-12).all());
    }

BOOST_AUTO_TEST_SUITE_END()


/*
 *  Distributed under our modified Boost Software License.
 *  Version 1.0 (see accompanying file LICENSE)
 */
/**
 *  @file       Robot.hpp
 *  @author     Lydia Zoghbi, Ari Kupferberg
 *  @copyright  Copyright ARL 2019
 *  @date       10/15/2019
 *  @version    2.0
 *
 *  @brief      Header file for constructing a Robot class.
 *
 */

#ifndef INCLUDE_ROBOT_HPP_
#define INCLUDE_ROBOT_HPP_

#include <vector>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/math/constants/constants.hpp>
#include <RobotPosition.hpp>
#include <RobotPath.hpp>
#include <Point.hpp>
#include <PathPlanner.hpp>

/**
 *  @brief      Class for creating a Robot object containing useful information about the system
 */

class Robot {
 private:
   int numberLinks = 6;
   Point initialEEPosition;
   std::vector<double> initialJointAngles = {0, 0, 0, 0, 0, 0};
   std::vector<RobotPath> path;
    std::vector<std::vector<double>> dhParams{
     {770, M_PI/2.0, 750, 0},
     {1050, 0, 0, 0},
     {200, M_PI/2.0, 0, 0},
     {0, -M_PI/2.0, 1705, 0},
     {0, M_PI/2.0, 0, 0},
     {0, 0, 325, 0}};

 public:

  /**
   *  @brief    Constructor for class Robot
   *  @param	Point of the robot's end effector position
   *  @return	Instance of robot
   */
   explicit Robot(const Point& startingPos);

  /**
   *  @brief     Compute the intermediate transformation between two DH frames
   *  @param	 DH Table as matrix double
   *  @return	'A' transformation matrix
   */
   boost::numeric::ublas::matrix<double> computeATransform(std::vector<double> dhRow);

  /**
   *  @brief    Computing the forward kinematics for the Robot
   *  @param	Vector of Robot's joint angles as double
   *  @return	Vector of Point objects depicting Robot's joint positions
   */
   std::vector<Point> computeFk(std::vector<double> jointAngles);

  /**
   *  @brief    Computing a set of transformation matrices
   *  @param	Vector of Robot's joint angles
   *  @return	Vector of matrices representing the transformation from each joint frame to the base frame
   */
   std::vector<boost::numeric::ublas::matrix<double>> computeTransformationMatrices(std::vector<double> jointAngles);

  /**
   *  @brief    Computing the geometric Jacobian matrix
   *  @param	Robot's current position for performing a cross product
   *  @param    Vector of transformation matrices
   *  @return	The Jacobian matrix
   */
   boost::numeric::ublas::matrix<double> computeJacobian(RobotPosition robotPosition, std::vector<boost::numeric::ublas::matrix<double>> tTransforms);

  /**
   *  @brief    Computing the inverse kinematics for Robot's generated path
   *  @param	The end effector's target position
   *  @param    The environment to check for collisions
   *  @return	Vector of RobotPosition object containing Points of joint positions
   */
   std::vector<RobotPosition> computeIK(Point targetPoint, Environment environment);

  /**
   *  @brief    Computes cross product of the two input vectors
   *  @param	Vector to compute cross product of (1 of 2)
   *  @param	Vector to compute cross product of (2 of 2)
   *  @return	Vector of the resultant cross product
   */
   boost::numeric::ublas::vector<double> crossProduct(boost::numeric::ublas::vector<double> vector1, boost::numeric::ublas::vector<double> vector2);

  /**
   *  @brief    Computes the Moore-Penrose pseudoinverse
   *  @param	Input matrix to find the pseudoinverse of
   *  @return	Pseudoinverse matrix result
   */
   boost::numeric::ublas::matrix<double> penroseInverseMatrix(boost::numeric::ublas::matrix<double> mat);

};

#endif // INCLUDE_ROBOT_HPP_


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

#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/point_cloud_color_handlers.h>

using namespace std ;
using namespace Eigen ;

int
main (int argc, char** argv)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointXYZ zero;
	zero.x = zero.y = zero.z = 0.0;

	cloud_ptr->width  = 5;
	cloud_ptr->height = 1;
	cloud_ptr->points.resize (cloud_ptr->width * cloud_ptr->height);

	// Fill in the cloud data
	for (size_t i = 0; i < cloud_ptr->points.size(); ++i)
	{
		cloud_ptr->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
		cloud_ptr->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
		cloud_ptr->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
	}
	cout << "cloud_ptr data" << endl;
	for (size_t i = 0; i < cloud_ptr->points.size (); ++i)
	{
		cout << cloud_ptr->points[i].x << " " << cloud_ptr->points[i].y << " " << cloud_ptr->points[i].z << " " << endl;
	}
	std::cerr << "cloud_ptr has: " << cloud_ptr->points.size () << " data points." << std::endl;

	Eigen::Matrix<double, 5, 3> p_matrix;
	for (size_t i = 0; i < cloud_ptr->points.size(); ++i)
	{
		p_matrix(i,0) = cloud_ptr->points[i].x;
		p_matrix(i,1) = cloud_ptr->points[i].y;
		p_matrix(i,2) = cloud_ptr->points[i].z;
	}

	cout << "p_matrix" << endl;
	cout << p_matrix << endl;

//	Transform<float, 3, Affine> t = Transform<float, 3, Affine>::
//	Identity();
//	t.scale(0.8f);
//	t.rotate(AngleAxisf(0.25f * M_PI, Vector3f::UnitX()));
//	t.translate(Vector3f(1.5, 10.2, -5.1));

	pcl::visualization::PCLVisualizer viser("cloud_cluster");
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color1(cloud_ptr, 150, 150, 0);
//	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color2(cloud_projected, 0, 150, 200);
//	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color3(cloud_filtered, 150, 0, 200);
	viser.addPointCloud (cloud_ptr, single_color1, "cloud_ptr");

	ostringstream out;
	for (size_t i = 0; i < cloud_ptr->points.size (); ++i)
	{
		out << "point: " << i;
		viser.addArrow(cloud_ptr->points[i], zero, i * 10, i * 30, i * 40, false, out.str());
	}
//	viser.addPointCloud (cloud_projected, single_color2, "cloud_projected");
//	viser.addPointCloud (cloud_filtered, single_color3, "cloud_filtered");
	viser.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 20, "cloud_ptr");
	viser.addCoordinateSystem (1.0);
	viser.spin ();
	1;

	return (0);
}


///////////////////////////////////////////////////////////////////////////////
// importance_sampling::maximal_finite_sums.hpp                              //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_IMPORTANCE_SAMPLING_WEIGHTS_MAXIMAL_FINITE_SUMS_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_IMPORTANCE_SAMPLING_WEIGHTS_MAXIMAL_FINITE_SUMS_HPP_ER_2009
#include <boost/iterator/iterator_traits.hpp>
#include <boost/math/special_functions/fpclassify.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace importance_sampling{

    // Breaks down the summation of *i, i in [b,e) into maximal finite amounts,
    // each of which are copied to the output iterator i
    //
    // Note: Primarily for use by find_scale_to_finite_sum
    template<typename InIt,typename OutIt>
    OutIt maximal_finite_sums(InIt b, InIt e, OutIt i)
    {
        typedef typename iterator_value<InIt>::type value_type;
        value_type sum = static_cast<value_type>(0);
        while(b!=e){
            value_type d = *b;
            if( boost::math::isinf( sum + d ) ){
                *i = sum;
                sum = d;
                ++i;
            }else{
                sum += d;
            }
            ++b;
        }
        return i;
    };

}// importance_weights
}// detail
}// statistics
}// boost

#endif


/*
 *  Copyright Nick Thompson, 2019
 *  Use, modification and distribution are subject to the
 *  Boost Software License, Version 1.0. (See accompanying file
 *  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
 *
 *  Exactly the same as barycentric_rational.hpp, but delivers values in $\mathbb{R}^n$.
 *  In some sense this is trivial, since each component of the vector is computed in exactly the same
 *  as would be computed by barycentric_rational.hpp. But this is a bit more efficient and convenient.
 */

#ifndef BOOST_MATH_INTERPOLATORS_VECTOR_BARYCENTRIC_RATIONAL_HPP
#define BOOST_MATH_INTERPOLATORS_VECTOR_BARYCENTRIC_RATIONAL_HPP

#include <memory>
#include <boost/math/interpolators/detail/vector_barycentric_rational_detail.hpp>

namespace boost{ namespace math{ namespace interpolators{

template<class TimeContainer, class SpaceContainer>
class vector_barycentric_rational
{
public:
    using Real = typename TimeContainer::value_type;
    using Point = typename SpaceContainer::value_type;
    vector_barycentric_rational(TimeContainer&& times, SpaceContainer&& points, size_t approximation_order = 3);

    void operator()(Point& x, Real t) const;

    // I have validated using google benchmark that returning a value is no more expensive populating it,
    // at least for Eigen vectors with known size at compile-time.
    // This is kinda a weird thing to discover since it goes against the advice of basically every high-performance computing book.
    Point operator()(Real t) const {
        Point p;
        this->operator()(p, t);
        return p;
    }

    void prime(Point& dxdt, Real t) const {
        Point x;
        m_imp->eval_with_prime(x, dxdt, t);
    }

    Point prime(Real t) const {
        Point p;
        this->prime(p, t);
        return p;
    }

    void eval_with_prime(Point& x, Point& dxdt, Real t) const {
        m_imp->eval_with_prime(x, dxdt, t);
        return;
    }

    std::pair<Point, Point> eval_with_prime(Real t) const {
        Point x;
        Point dxdt;
        m_imp->eval_with_prime(x, dxdt, t);
        return {x, dxdt};
    }

private:
    std::shared_ptr<detail::vector_barycentric_rational_imp<TimeContainer, SpaceContainer>> m_imp;
};

template <class TimeContainer, class SpaceContainer>
vector_barycentric_rational<TimeContainer, SpaceContainer>::vector_barycentric_rational(TimeContainer&& times, SpaceContainer&& points, size_t approximation_order):
 m_imp(std::make_shared<detail::vector_barycentric_rational_imp<TimeContainer, SpaceContainer>>(std::move(times), std::move(points), approximation_order))
{
    return;
}

template <class TimeContainer, class SpaceContainer>
void vector_barycentric_rational<TimeContainer, SpaceContainer>::operator()(typename SpaceContainer::value_type& p, typename TimeContainer::value_type t) const
{
    m_imp->operator()(p, t);
    return;
}

}}}
#endif


/*
Copyright 2014 Rogier van Dalen.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/

/** \file
Test lexicographical.hpp.
*/

#define BOOST_TEST_MODULE test_math_lexicographical_fast
#include "utility/test/boost_unit_test.hpp"

#include "math/lexicographical.hpp"

#include <string>
#include <vector>

#include <boost/mpl/assert.hpp>

#include "math/sequence.hpp"
#include "math/arithmetic_magma.hpp"
#include "math/cost.hpp"

#include "./make_lexicographical.hpp"

using range::first;
using range::second;

BOOST_AUTO_TEST_SUITE (test_suite_lexicographical)

BOOST_AUTO_TEST_CASE (test_lexicographical_static) {
    // Basic requirements for the component types.
    BOOST_MPL_ASSERT ((math::is::monoid <math::callable::choose, cost>));
    BOOST_MPL_ASSERT ((math::is::monoid <math::callable::choose,
        math::sequence <char>>));

    BOOST_MPL_ASSERT ((math::is::semiring <
        math::left, math::callable::times, math::callable::plus,
        lexicographical>));

    BOOST_MPL_ASSERT ((math::is::distributive <
        math::left, math::callable::times, math::callable::plus,
        lexicographical>));
    BOOST_MPL_ASSERT ((math::is::distributive <
        math::right, math::callable::times, math::callable::plus,
        lexicographical>));
    BOOST_MPL_ASSERT ((math::is::distributive <
        math::either, math::callable::times, math::callable::plus,
        lexicographical>));

    BOOST_MPL_ASSERT ((math::is::distributive <
        math::left, math::callable::times, math::callable::choose,
        lexicographical>));
    BOOST_MPL_ASSERT ((math::is::distributive <
        math::left, math::callable::times, math::callable::choose, cost>));
    BOOST_MPL_ASSERT ((math::is::distributive <
        math::left, math::callable::times, math::callable::choose,
        math::sequence <char>>));

    BOOST_MPL_ASSERT ((math::is::distributive <
        math::left, math::callable::times, math::callable::choose,
        lexicographical>));

    {
        BOOST_MPL_ASSERT ((math::is::associative <math::callable::times,
            math::lexicographical <math::over <math::cost <float>>>>));
        BOOST_MPL_ASSERT ((math::is::associative <math::callable::times,
            math::lexicographical <math::over <math::sequence <char>>>>));
    }
    {
        BOOST_MPL_ASSERT ((math::is::commutative <math::callable::times,
            math::lexicographical <math::over <math::cost <float>>>>));
        BOOST_MPL_ASSERT_NOT ((math::is::commutative <math::callable::times,
            math::lexicographical <math::over <math::sequence <char>>>>));
    }
    typedef math::lexicographical <math::over <math::cost <float>>>
        cost;
    typedef math::lexicographical <math::over <math::sequence <char>>>
        sequence;
    {
        BOOST_MPL_ASSERT ((math::is::approximate <
            math::callable::times (cost, cost)>));
        BOOST_MPL_ASSERT_NOT ((math::is::approximate <
            math::callable::times (sequence, sequence)>));
        BOOST_MPL_ASSERT_NOT ((math::is::approximate <
            math::callable::plus (cost, cost)>));
        BOOST_MPL_ASSERT_NOT ((math::is::approximate <
            math::callable::plus (sequence, sequence)>));
    }
    {
        BOOST_MPL_ASSERT_NOT ((math::is::idempotent <
            math::callable::times (cost, cost)>));
        BOOST_MPL_ASSERT_NOT ((math::is::idempotent <
            math::callable::times (sequence, sequence)>));
        BOOST_MPL_ASSERT ((math::is::idempotent <
            math::callable::plus (cost, cost)>));
        BOOST_MPL_ASSERT ((math::is::idempotent <
            math::callable::plus (sequence, sequence)>));
    }
    {
        BOOST_MPL_ASSERT_NOT ((math::is::path_operation <
            math::callable::times (cost, cost)>));
        BOOST_MPL_ASSERT_NOT ((math::is::path_operation <
            math::callable::times (sequence, sequence)>));
        BOOST_MPL_ASSERT ((math::is::path_operation <
            math::callable::plus (cost, cost)>));
        BOOST_MPL_ASSERT ((math::is::path_operation <
            math::callable::plus (sequence, sequence)>));
    }
}

BOOST_AUTO_TEST_CASE (test_lexicographical_spot) {
    std::string ab = "ab";
    std::string abc = "abc";
    std::string abd = "abd";
    std::string c = "c";
    std::string d = "d";

    lexicographical ab4 = make_lexicographical (4, ab);
    BOOST_CHECK_EQUAL (first (ab4.components()).value(), 4.f);
    BOOST_CHECK_EQUAL (first (second (ab4.components()).symbols()), 'a');
    BOOST_CHECK_EQUAL (second (second (ab4.components()).symbols()), 'b');

    lexicographical c7 = make_lexicographical (7, c);
    lexicographical abc11 = make_lexicographical (11, abc);

    {
        auto ab4_2 = math::make_lexicographical (
            math::cost <float> (4), math::sequence <char> (std::string ("ab")));
        static_assert (
            std::is_same <decltype (ab4_2), lexicographical>::value, "");

        BOOST_CHECK (ab4_2 == ab4);
    }
    {
        auto components = range::make_tuple (
            math::cost <float> (4), math::sequence <char> (std::string ("ab")));
        auto ab4_2 = math::make_lexicographical_over (components);
        static_assert (
            std::is_same <decltype (ab4_2), lexicographical>::value, "");

        BOOST_CHECK (ab4_2 == ab4);
    }

    BOOST_CHECK_EQUAL (ab4 * c7, abc11);
    BOOST_CHECK_EQUAL (ab4 * math::one <lexicographical>(), ab4);
    // divide is not implemented.
    // BOOST_CHECK_EQUAL (math::divide <math::left> (abc11, ab4), c7);

    BOOST_CHECK_EQUAL (ab4 + abc11, ab4);
    BOOST_CHECK_EQUAL (ab4 + math::zero <lexicographical>(), ab4);

    BOOST_CHECK (math::equal (
        make_empty_lexicographical (0), make_lexicographical (0, "")));
    BOOST_CHECK (math::equal (
        make_empty_lexicographical (7), make_lexicographical (7, "")));
    BOOST_CHECK (!math::equal (
        make_empty_lexicographical (7), make_lexicographical (0, "")));
    BOOST_CHECK (!math::equal (
        make_empty_lexicographical (7), make_lexicographical (6, "")));

    BOOST_CHECK (math::equal (
        make_single_lexicographical (0, 'a'),
        make_lexicographical (0, "a")));
    BOOST_CHECK (math::equal (
        make_single_lexicographical (7, 'b'),
        make_lexicographical (7, "b")));
    BOOST_CHECK (!math::equal (
        make_single_lexicographical (7, 'a'),
        make_lexicographical (6, "a")));
    BOOST_CHECK (!math::equal (
        make_single_lexicographical (7, 'a'),
        make_lexicographical (7, "b")));
}

BOOST_AUTO_TEST_SUITE_END()


#include <jfs/differential_ops/grid_diff2d.h>

#include <Eigen/Eigen>

namespace jfs {

template <class SparseMatrix>
JFS_INLINE void gridDiff2D<SparseMatrix>::Laplace(const grid2D* grid, SparseMatrix &dst, unsigned int dims, unsigned int fields)
{
    auto btype = grid->bound_type_;
    auto L = grid->L;
    auto N = grid->N;
    auto D = grid->D;

    typedef Eigen::Triplet<float> T;
    std::vector<T> tripletList;
    tripletList.reserve(N*N*5*dims);

    for (int idx = 0; idx < dims*fields*N*N; idx++)
    {
        int idx_tmp = idx;
        int j = idx_tmp / (N*dims*fields);
        idx_tmp -= j * (N*dims*fields);
        int i = idx_tmp / (dims*fields);
        idx_tmp -= i * (dims*fields);
        int f = idx_tmp / dims;
        idx_tmp -= f * dims;
        int d = idx_tmp;

        int iMat, jMat;

        iMat = N*fields*dims*j + fields*dims*i + dims*f + d;
        jMat = iMat;
        tripletList.push_back(T(iMat,jMat,-4.f));

        int i_tmp = i;
        for (int offset = -1; offset < 2; offset+=2)
        {
            i = i_tmp + offset;
            if ( (i == -1 || i == N) && btype == ZERO)
                continue;
            else if ( i == -1 )
                i = (N-2);
            else if ( i == N )
                i = 1;
            jMat = N*fields*dims*j + fields*dims*i + dims*f + d;
            tripletList.push_back(T(iMat,jMat,1.f));
        }
        i = i_tmp;

        int j_tmp = j;
        for (int offset = -1; offset < 2; offset+=2)
        {
            j = j_tmp + offset;
            if ( (j == -1 || j == N) && btype == ZERO)
                continue;
            else if ( j == -1 )
                j = (N-2);
            else if ( j == N )
                j = 1;
            jMat = N*fields*dims*j + fields*dims*i + dims*f + d;
            tripletList.push_back(T(iMat,jMat,1.f));
        }
        j = j_tmp;
    }

    dst = SparseMatrix(N*N*dims*fields,N*N*dims*fields);
    dst.setFromTriplets(tripletList.begin(), tripletList.end());
    dst = 1.f/(D*D) * dst;
}

template <class SparseMatrix>
JFS_INLINE void gridDiff2D<SparseMatrix>::div(const grid2D* grid, SparseMatrix &dst, unsigned int fields)
{
    auto btype = grid->bound_type_;
    auto L = grid->L;
    auto N = grid->N;
    auto D = grid->D;

    typedef Eigen::Triplet<float> T;
    std::vector<T> tripletList;
    tripletList.reserve(N*N*2*2);

    int dims = 2;

    for (int idx = 0; idx < dims*fields*N*N; idx++)
    {
        int idx_tmp = idx;
        int j = idx_tmp / (N*dims*fields);
        idx_tmp -= j * (N*dims*fields);
        int i = idx_tmp / (dims*fields);
        idx_tmp -= i * (dims*fields);
        int f = idx_tmp / dims;
        idx_tmp -= f * dims;
        int d = idx_tmp;

        int iMat, jMat;

        iMat = N*fields*j + fields*i + f;

        int i_tmp = i;
        for (int offset = -1; offset < 2 && d == 0; offset+=2)
        {
            i = i_tmp + offset;
            if ( (i == -1 || i == N) && btype == ZERO)
                continue;
            else if ( i == -1 )
                i = (N-2);
            else if ( i == N )
                i = 1;
            jMat = N*fields*dims*j + fields*dims*i + dims*f + d;
            tripletList.push_back(T(iMat,jMat,(float) offset));
        }
        i = i_tmp;

        int j_tmp = j;
        for (int offset = -1; offset < 2 && d == 1; offset+=2)
        {
            j = j_tmp + offset;
            if ( (j == -1 || j == N) && btype == ZERO)
                continue;
            else if ( j == -1 )
                j = (N-2);
            else if ( j == N )
                j = 1;
            jMat = N*fields*dims*j + fields*dims*i + dims*f + d;
            tripletList.push_back(T(iMat,jMat,(float) offset));
        }
        j = j_tmp;
    }

    dst = SparseMatrix(N*N*fields,N*N*2*fields);
    dst.setFromTriplets(tripletList.begin(), tripletList.end());
    dst = 1.f/(2*D) * dst;
}

template <class SparseMatrix>
JFS_INLINE void gridDiff2D<SparseMatrix>::grad(const grid2D* grid, SparseMatrix &dst, unsigned int fields)
{
    auto btype = grid->bound_type_;
    auto L = grid->L;
    auto N = grid->N;
    auto D = grid->D;

    typedef Eigen::Triplet<float> T;
    std::vector<T> tripletList;
    tripletList.reserve(N*N*2*2);

    int dims = 2;

    for (int idx = 0; idx < dims*fields*N*N; idx++)
    {
        int idx_tmp = idx;
        int j = idx_tmp / (N*dims*fields);
        idx_tmp -= j * (N*dims*fields);
        int i = idx_tmp / (dims*fields);
        idx_tmp -= i * (dims*fields);
        int f = idx_tmp / dims;
        idx_tmp -= f * dims;
        int d = idx_tmp;

        int iMat, jMat;

        iMat = N*fields*dims*j + fields*dims*i + dims*f + d;

        int i_tmp = i;
        for (int offset = -1; offset < 2 && d == 0; offset+=2)
        {
            i = i_tmp + offset;
            if ( (i == -1 || i == N) && btype == ZERO)
                continue;
            else if ( i == -1 )
                i = (N-2);
            else if ( i == N )
                i = 1;
            jMat = N*fields*j + fields*i + f;
            tripletList.push_back(T(iMat,jMat,(float) offset));
        }
        i = i_tmp;

        int j_tmp = j;
        for (int offset = -1; offset < 2 && d == 1; offset+=2)
        {
            j = j_tmp + offset;
            if ( (j == -1 || j == N) && btype == ZERO)
                continue;
            else if ( j == -1 )
                j = (N-2);
            else if ( j == N )
                j = 1;
            jMat = N*fields*j + fields*i + f;
            tripletList.push_back(T(iMat,jMat,(float) offset));
        }
        j = j_tmp;
    }

    dst = SparseMatrix(N*N*dims*fields,N*N*fields);
    dst.setFromTriplets(tripletList.begin(), tripletList.end());
    dst = 1.f/(2*D) * dst;
}

// explicit instantiation of templates
#ifdef JFS_STATIC
template class gridDiff2D<Eigen::SparseMatrix<float, Eigen::ColMajor>>;
template class gridDiff2D<Eigen::SparseMatrix<float, Eigen::RowMajor>>;
#endif

} // namespace jfs


#pragma once

#include "enum.hpp"
#include "cc_trade_stream.hpp"
#include "utils.hpp"
#include "trade_data.hpp"

#include <boost/log/trivial.hpp>

#include <concepts>
#include <map>

namespace profitview
{

template<std::floating_point Float = double, std::integral Int = int>
class CcDamped : public CcTradeStream<Float, Int>
{
public:
    CcDamped(
        const std::string trade_stream_name,
        OrderExecutor* executor,
        Int lookback,
        Float reversion_level,
        Float base_quantity,
        Float damping,
        const std::string& csv_name = "Damped.csv")
        : CcTradeStream<Float, Int>(trade_stream_name, executor, csv_name)
        , lookback_{lookback}
        , reversion_level_{reversion_level}
        , base_quantity_{base_quantity}
        , damping_{damping}
    {}

    void onStreamedTrade(TradeData const& trade_data) override
    {
        trade_data.print();

        auto& [prices, mean_reached, initial_mean, initial_stdev]{price_structure_[trade_data.symbol]};

        prices.emplace_back(trade_data.price);

        if (not mean_reached and prices.size() > lookback_)
        {
            initial_mean = util::ma(prices);
            initial_stdev = util::stdev(prices, initial_mean, lookback_);
            BOOST_LOG_TRIVIAL(info) << "Initial mean: " << initial_mean << std::endl << std::endl;
            mean_reached = true;
        }
        else if (mean_reached)
        {
            auto mean{util::ma(prices)};
            BOOST_LOG_TRIVIAL(info) << "MA: " << mean << std::endl << std::endl;

            auto const& damping_factor{damping_ * initial_stdev};

            // Version 1: chopping the tops/bottoms off outliers
            auto const& cut_damped{prices | std::ranges::views::transform([&mean, &damping_factor](auto price) -> auto {
                                       return std::abs(price - mean) > damping_factor
                                                ? boost::math::sign(price - mean) * damping_factor + mean
                                                : price;
                                   })};

            // Version 2: excluding outliers
            auto excluded_damped{prices | std::ranges::views::filter([&mean, &damping_factor](auto price) -> auto {
                                     return std::abs(price - mean) < damping_factor;
                                 })};

            // Using Version 2 this time:
            auto std_reversion{reversion_level_ * util::stdev(excluded_damped, mean, lookback_)};
            BOOST_LOG_TRIVIAL(info) << "Std Reversion: " << std_reversion << std::endl << std::endl;

            prices.pop_front();

            bool
                sell_signal{trade_data.price > mean + std_reversion},
                buy_signal {trade_data.price < mean - std_reversion};

            if (sell_signal)
            {
                this->new_order(trade_data.symbol, Side::Sell, base_quantity_, OrderType::Market);
            }
            else if (buy_signal)
            {
                this->new_order(trade_data.symbol, Side::Buy, base_quantity_, OrderType::Market);
            }

            this->writeCsv(
                trade_data.symbol,
                trade_data.price,
                toString(trade_data.side).data(),
                trade_data.size,
                trade_data.source,
                trade_data.time,
                mean,
                std_reversion,
                buy_signal ? "Buy" : (sell_signal ? "Sell" : "No trade"));
        }
    }

    struct Data
    {
        std::deque<Float> prices;
        bool mean_reached;
        Float initial_mean, initial_stdev;
    };

private:
    const Int lookback_;
    const Float reversion_level_;
    const Float base_quantity_;
    const Float damping_;

    std::map<std::string, Data> price_structure_;
};

}    // namespace profitview


#define BOOST_TEST_MODULE TS test
#include <boost/test/unit_test.hpp>
#include "Sum.h"
#include "Variable.h"
#include "System.h"

using namespace omnn::math;
using namespace boost::unit_test;

template<typename T>
T bits_in_use(T v){
    T bits = 0;
    while (v) {
        ++bits;
        v = v >> 1;
    }
    return bits;
}

/// Control theory generalization hints are here: finds best moves/goods for single dimension
BOOST_AUTO_TEST_CASE(TS_1d) {
    //    System s;
    //    s.MakeTotalEqu(true);
    // y=f(x), where x is code of sequence
    Variable time, price;
    int points[] = {10, 1, 30, 5};
    auto sz = std::size(points);
    auto encodeBits = bits_in_use(sz);
    int mask = (1 << encodeBits) - 1;

    auto sys = 0_v;

    // points math data
    Variable x;
    auto ExtractMove = [&](int i) {
        auto targetId = x.And((i + 1) * encodeBits, mask << (i * encodeBits)).shr(i * encodeBits);
        return targetId;
    };
    // last move back to start
    auto statement = ExtractMove(sz - 1).Equals(0);
    //    s << statement;
    sys += statement.sq();
    // moves uniqueness
    for (int i = 1; i < sz; ++i) {
        statement = ExtractMove(i).NotEquals(
            ExtractMove(i - 1)); // FIXME: NotEquals behaviour changed. It was 0 for a!=b and 1 for a==b.  then it
                                 // became  /=(a-b) like if a=b then its a division by zero which must exclude branches,
                                 // but does it or it just breaks expression
                                 //        s << statement;
        sys += statement.sq();
    }
    // generate function of getting point value by its index
    auto ValueByIdx = [&](Valuable i) {
        Variable id;
        static auto MathDataForm = [&]() {
            auto data = 1_v;
            Variable val;
            for (int i = 0; i < sz; ++i) {
                data.logic_or(id.Equals(i).logic_and(val.Equals(points[i])));
            }
            return data(val);
        };

        static auto data = MathDataForm();

        auto localData = data;
        localData.Eval(id, i);
        return localData;
    };
    // move len square
    auto MoveLenSq = [&](int i) {
        assert(i);
        auto prev = ExtractMove(i - 1);
        auto target = ExtractMove(i);
        auto diff = ValueByIdx(prev) - ValueByIdx(target);
        return diff ^ 2;
    };
    auto SumSqLens = [&]() {
        auto sum = 0_v;
        for (int i = 1; i < sz; ++i)
            sum += MoveLenSq(i);
        return sum;
    };
    auto sumSqLens = SumSqLens();
}

BOOST_AUTO_TEST_CASE(TS_2d
                     ,*disabled()
                     )
{
	Variable time, price;
    std::pair<Valuable,Valuable> points[] = {
		{0,0},
		{1,5},
		{3,8},
	};
	auto bits = 2;

	Variable xpath, ypath;
    Variable xmove[std::size(points)], ymove[std::size(points)];
//
//    auto unix = xpath.equals((xmove[0] << (2 * 2)) + (xmove[1] << 2) + xmove[2]);
//    auto uniy = ypath.equals((xmove[0] << (2 * 2)) + (xmove[1] << 2) + xmove[2]);

//    auto xext = unix(

IMPLEMENT
}