//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_basic_types.h>
#include <OpenTissue/collision/gjk/gjk_reduce_triangle.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

#include <cmath>

using namespace OpenTissue;

BOOST_AUTO_TEST_SUITE(opentissue_collision_gjk_reduce_triangle);

BOOST_AUTO_TEST_CASE(case_by_case_test)
{
  typedef OpenTissue::math::BasicMathTypes<double, size_t> math_types;
  typedef math_types::vector3_type                         vector3_type;
  typedef math_types::real_type                            real_type;

  typedef OpenTissue::gjk::Simplex<vector3_type>           simplex_type;

  // Inside face-region new simplex should be ABC
  {
    vector3_type const a = vector3_type(-1.0, -1.0, 0.0);
    vector3_type const b = vector3_type( 1.0, -1.0, 0.0);
    vector3_type const c = vector3_type( 0.5,  0.1, 0.0);

    simplex_type S;

    OpenTissue::gjk::add_point_to_simplex( a, a, a, S);
    OpenTissue::gjk::add_point_to_simplex( b, b, b, S);
    OpenTissue::gjk::add_point_to_simplex( c, c, c, S);

    for(size_t i=1;i<10;++i)
      for(size_t j=1;j<10;++j)
        for(size_t k=1;k<10;++k)
        {
          real_type u = i*0.1;
          real_type v = j*0.1;
          real_type w = k*0.1;

          real_type lgh = u + v +w;
          u /= lgh;
          v /= lgh;
          w /= lgh;

          vector3_type const p = u*a + v*b + w*c;

          OpenTissue::gjk::detail::reduce_triangle( p, S );

          BOOST_CHECK( OpenTissue::gjk::dimension( S ) == 3u );

          int bit_A    = 0;
          size_t idx_A = 0;
          int bit_B    = 0;
          size_t idx_B = 0;
          int bit_C    = 0;
          size_t idx_C = 0;
          OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B , idx_C, bit_C );

          BOOST_CHECK(S.m_bitmask == (bit_A | bit_B | bit_C));
          BOOST_CHECK(S.m_v[idx_A] == a);
          BOOST_CHECK(S.m_a[idx_A] == a);
          BOOST_CHECK(S.m_b[idx_A] == a);

          BOOST_CHECK(S.m_v[idx_B] == b);
          BOOST_CHECK(S.m_a[idx_B] == b);
          BOOST_CHECK(S.m_b[idx_B] == b);

          BOOST_CHECK(S.m_v[idx_C] == c);
          BOOST_CHECK(S.m_a[idx_C] == c);
          BOOST_CHECK(S.m_b[idx_C] == c);

          BOOST_CHECK_CLOSE(S.m_w[idx_A], u, 0.01);
          BOOST_CHECK_CLOSE(S.m_w[idx_B], v, 0.01);
          BOOST_CHECK_CLOSE(S.m_w[idx_C], w, 0.01);

        }
  }

  // First we create a simplex that represents an triangle
  vector3_type const a = vector3_type(-1.0, -1.0, 0.0);
  vector3_type const b = vector3_type( 1.0, -1.0, 0.0);
  vector3_type const c = vector3_type( 0.0,  1.0, 0.0);

  // Inside face-region new simplex should be ABC
  {
    simplex_type S;

    OpenTissue::gjk::add_point_to_simplex( a, a, a, S);
    OpenTissue::gjk::add_point_to_simplex( b, b, b, S);
    OpenTissue::gjk::add_point_to_simplex( c, c, c, S);

    vector3_type const p = vector3_type( 0.0, 0.0,  1.0);

    OpenTissue::gjk::detail::reduce_triangle( p, S );

    BOOST_CHECK( OpenTissue::gjk::dimension( S ) == 3u );

    int bit_A    = 0;
    size_t idx_A = 0;
    int bit_B    = 0;
    size_t idx_B = 0;
    int bit_C    = 0;
    size_t idx_C = 0;
    OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B , idx_C, bit_C );

    BOOST_CHECK(S.m_bitmask == (bit_A | bit_B | bit_C));
    BOOST_CHECK(S.m_v[idx_A] == a);
    BOOST_CHECK(S.m_a[idx_A] == a);
    BOOST_CHECK(S.m_b[idx_A] == a);

    BOOST_CHECK(S.m_v[idx_B] == b);
    BOOST_CHECK(S.m_a[idx_B] == b);
    BOOST_CHECK(S.m_b[idx_B] == b);

    BOOST_CHECK(S.m_v[idx_C] == c);
    BOOST_CHECK(S.m_a[idx_C] == c);
    BOOST_CHECK(S.m_b[idx_C] == c);

    BOOST_CHECK_CLOSE(S.m_w[idx_A], 0.25, 0.01);
    BOOST_CHECK_CLOSE(S.m_w[idx_B], 0.25, 0.01);
    BOOST_CHECK_CLOSE(S.m_w[idx_C], 0.5, 0.01);
  }
  // Inside A voronoi region new simplex should be A
  {
    simplex_type S;

    OpenTissue::gjk::add_point_to_simplex( a, a, a, S);
    OpenTissue::gjk::add_point_to_simplex( b, b, b, S);
    OpenTissue::gjk::add_point_to_simplex( c, c, c, S);

    vector3_type const p = vector3_type( -2.0, -1.0,  1.0);

    OpenTissue::gjk::detail::reduce_triangle( p, S );

    BOOST_CHECK( OpenTissue::gjk::dimension( S ) == 1u );

    int bit_A    = 0;
    size_t idx_A = 0;
    OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A );

    BOOST_CHECK(S.m_bitmask == bit_A);
    BOOST_CHECK(S.m_v[idx_A] == a);
    BOOST_CHECK(S.m_a[idx_A] == a);
    BOOST_CHECK(S.m_b[idx_A] == a);

    BOOST_CHECK_CLOSE(S.m_w[idx_A], 1.0, 0.01);
  }
  // Inside B voronoi region new simplex should be B
  {
    simplex_type S;

    OpenTissue::gjk::add_point_to_simplex( a, a, a, S);
    OpenTissue::gjk::add_point_to_simplex( b, b, b, S);
    OpenTissue::gjk::add_point_to_simplex( c, c, c, S);

    vector3_type const p = vector3_type(  2.0, -1.0,  1.0);

    OpenTissue::gjk::detail::reduce_triangle( p, S );

    BOOST_CHECK( OpenTissue::gjk::dimension( S ) == 1u );

    int bit_A    = 0;
    size_t idx_A = 0;
    OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A );

    BOOST_CHECK(S.m_bitmask == bit_A);
    BOOST_CHECK(S.m_v[idx_A] == b);
    BOOST_CHECK(S.m_a[idx_A] == b);
    BOOST_CHECK(S.m_b[idx_A] == b);

    BOOST_CHECK_CLOSE(S.m_w[idx_A], 1.00, 0.01);
  }
  // Inside C voronoi region new simplex should be C
  {
    simplex_type S;

    OpenTissue::gjk::add_point_to_simplex( a, a, a, S);
    OpenTissue::gjk::add_point_to_simplex( b, b, b, S);
    OpenTissue::gjk::add_point_to_simplex( c, c, c, S);

    vector3_type const p = vector3_type(  0.0, 2.0,  1.0);

    OpenTissue::gjk::detail::reduce_triangle( p, S );

    BOOST_CHECK( OpenTissue::gjk::dimension( S ) == 1u );

    int bit_A    = 0;
    size_t idx_A = 0;
    OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A  );

    BOOST_CHECK(S.m_bitmask == bit_A);
    BOOST_CHECK(S.m_v[idx_A] == c);
    BOOST_CHECK(S.m_a[idx_A] == c);
    BOOST_CHECK(S.m_b[idx_A] == c);

    BOOST_CHECK_CLOSE(S.m_w[idx_A], 1.0, 0.01);
  }
  // Inside AB voronoi region new simplex should be AB
  {
    simplex_type S;

    OpenTissue::gjk::add_point_to_simplex( a, a, a, S);
    OpenTissue::gjk::add_point_to_simplex( b, b, b, S);
    OpenTissue::gjk::add_point_to_simplex( c, c, c, S);

    vector3_type const p = vector3_type( 0.0, -2.0,  1.0);

    OpenTissue::gjk::detail::reduce_triangle( p, S );

    BOOST_CHECK( OpenTissue::gjk::dimension( S ) == 2u );

    int bit_A    = 0;
    size_t idx_A = 0;
    int bit_B    = 0;
    size_t idx_B = 0;
    OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B );

    BOOST_CHECK(S.m_bitmask == (bit_A | bit_B ));
    BOOST_CHECK(S.m_v[idx_A] == a);
    BOOST_CHECK(S.m_a[idx_A] == a);
    BOOST_CHECK(S.m_b[idx_A] == a);

    BOOST_CHECK(S.m_v[idx_B] == b);
    BOOST_CHECK(S.m_a[idx_B] == b);
    BOOST_CHECK(S.m_b[idx_B] == b);

    BOOST_CHECK_CLOSE(S.m_w[idx_A], 0.5, 0.01);
    BOOST_CHECK_CLOSE(S.m_w[idx_B], 0.5, 0.01);
  }
  // Inside BC voronoi region new simplex should be BC
  {
    simplex_type S;

    OpenTissue::gjk::add_point_to_simplex( a, a, a, S);
    OpenTissue::gjk::add_point_to_simplex( b, b, b, S);
    OpenTissue::gjk::add_point_to_simplex( c, c, c, S);

    vector3_type const p = vector3_type( 1.5, 0.5,  1.0);

    OpenTissue::gjk::detail::reduce_triangle( p, S );

    BOOST_CHECK( OpenTissue::gjk::dimension( S ) == 2u );

    int bit_A    = 0;
    size_t idx_A = 0;
    int bit_B    = 0;
    size_t idx_B = 0;
    OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B );

    BOOST_CHECK(S.m_bitmask == (bit_A | bit_B ));
    BOOST_CHECK(S.m_v[idx_A] == b);
    BOOST_CHECK(S.m_a[idx_A] == b);
    BOOST_CHECK(S.m_b[idx_A] == b);

    BOOST_CHECK(S.m_v[idx_B] == c);
    BOOST_CHECK(S.m_a[idx_B] == c);
    BOOST_CHECK(S.m_b[idx_B] == c);

    BOOST_CHECK_CLOSE(S.m_w[idx_A], 0.5, 0.01);
    BOOST_CHECK_CLOSE(S.m_w[idx_B], 0.5, 0.01);
  }
  // Inside AC voronoi region new simplex should be AC
  {
    simplex_type S;

    OpenTissue::gjk::add_point_to_simplex( a, a, a, S);
    OpenTissue::gjk::add_point_to_simplex( b, b, b, S);
    OpenTissue::gjk::add_point_to_simplex( c, c, c, S);

    vector3_type const p = vector3_type( -1.5, 0.5,  1.0);

    OpenTissue::gjk::detail::reduce_triangle( p, S );

    BOOST_CHECK( OpenTissue::gjk::dimension( S ) == 2u );

    int bit_A    = 0;
    size_t idx_A = 0;
    int bit_B    = 0;
    size_t idx_B = 0;
    OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B );

    BOOST_CHECK(S.m_bitmask == (bit_A | bit_B ));
    BOOST_CHECK(S.m_v[idx_A] == a);
    BOOST_CHECK(S.m_a[idx_A] == a);
    BOOST_CHECK(S.m_b[idx_A] == a);

    BOOST_CHECK(S.m_v[idx_B] == c);
    BOOST_CHECK(S.m_a[idx_B] == c);
    BOOST_CHECK(S.m_b[idx_B] == c);

    BOOST_CHECK_CLOSE(S.m_w[idx_A], 0.5, 0.01);
    BOOST_CHECK_CLOSE(S.m_w[idx_B], 0.5, 0.01);
  }

  // On vertex A new simplex should be A
  {
    simplex_type S;

    OpenTissue::gjk::add_point_to_simplex( a, a, a, S);
    OpenTissue::gjk::add_point_to_simplex( b, b, b, S);
    OpenTissue::gjk::add_point_to_simplex( c, c, c, S);

    OpenTissue::gjk::detail::reduce_triangle( a, S );

    BOOST_CHECK( OpenTissue::gjk::dimension( S ) == 1u );

    int bit_A    = 0;
    size_t idx_A = 0;
    OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A );

    BOOST_CHECK(S.m_bitmask == bit_A);
    BOOST_CHECK(S.m_v[idx_A] == a);
    BOOST_CHECK(S.m_a[idx_A] == a);
    BOOST_CHECK(S.m_b[idx_A] == a);

    BOOST_CHECK_CLOSE(S.m_w[idx_A], 1.0, 0.01);
  }
  // On vertex B new simplex should be B
  {
    simplex_type S;

    OpenTissue::gjk::add_point_to_simplex( a, a, a, S);
    OpenTissue::gjk::add_point_to_simplex( b, b, b, S);
    OpenTissue::gjk::add_point_to_simplex( c, c, c, S);

    OpenTissue::gjk::detail::reduce_triangle( b, S );

    BOOST_CHECK( OpenTissue::gjk::dimension( S ) == 1u );

    int bit_A    = 0;
    size_t idx_A = 0;
    OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A );

    BOOST_CHECK(S.m_bitmask == bit_A);
    BOOST_CHECK(S.m_v[idx_A] == b);
    BOOST_CHECK(S.m_a[idx_A] == b);
    BOOST_CHECK(S.m_b[idx_A] == b);

    BOOST_CHECK_CLOSE(S.m_w[idx_A], 1.00, 0.01);
  }
  // On vertex C new simplex should be C
  {
    simplex_type S;

    OpenTissue::gjk::add_point_to_simplex( a, a, a, S);
    OpenTissue::gjk::add_point_to_simplex( b, b, b, S);
    OpenTissue::gjk::add_point_to_simplex( c, c, c, S);

    OpenTissue::gjk::detail::reduce_triangle( c, S );

    BOOST_CHECK( OpenTissue::gjk::dimension( S ) == 1u );

    int bit_A    = 0;
    size_t idx_A = 0;
    OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A  );

    BOOST_CHECK(S.m_bitmask == bit_A);
    BOOST_CHECK(S.m_v[idx_A] == c);
    BOOST_CHECK(S.m_a[idx_A] == c);
    BOOST_CHECK(S.m_b[idx_A] == c);

    BOOST_CHECK_CLOSE(S.m_w[idx_A], 1.0, 0.01);
  }
  // On edge AB new simplex should be AB
  {
    simplex_type S;

    OpenTissue::gjk::add_point_to_simplex( a, a, a, S);
    OpenTissue::gjk::add_point_to_simplex( b, b, b, S);
    OpenTissue::gjk::add_point_to_simplex( c, c, c, S);

    vector3_type const p = vector3_type( 0.0, -1.0,  1.0);

    OpenTissue::gjk::detail::reduce_triangle( p, S );

    BOOST_CHECK( OpenTissue::gjk::dimension( S ) == 2u );

    int bit_A    = 0;
    size_t idx_A = 0;
    int bit_B    = 0;
    size_t idx_B = 0;
    OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B );

    BOOST_CHECK(S.m_bitmask == (bit_A | bit_B ));
    BOOST_CHECK(S.m_v[idx_A] == a);
    BOOST_CHECK(S.m_a[idx_A] == a);
    BOOST_CHECK(S.m_b[idx_A] == a);

    BOOST_CHECK(S.m_v[idx_B] == b);
    BOOST_CHECK(S.m_a[idx_B] == b);
    BOOST_CHECK(S.m_b[idx_B] == b);

    BOOST_CHECK_CLOSE(S.m_w[idx_A], 0.5, 0.01);
    BOOST_CHECK_CLOSE(S.m_w[idx_B], 0.5, 0.01);
  }
  // On edge BC new simplex should be BC
  {
    simplex_type S;

    OpenTissue::gjk::add_point_to_simplex( a, a, a, S);
    OpenTissue::gjk::add_point_to_simplex( b, b, b, S);
    OpenTissue::gjk::add_point_to_simplex( c, c, c, S);

    vector3_type const p = vector3_type( 0.5, 0.0,  1.0);

    OpenTissue::gjk::detail::reduce_triangle( p, S );

    BOOST_CHECK( OpenTissue::gjk::dimension( S ) == 2u );

    int bit_A    = 0;
    size_t idx_A = 0;
    int bit_B    = 0;
    size_t idx_B = 0;
    OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B );

    BOOST_CHECK(S.m_bitmask == (bit_A | bit_B ));
    BOOST_CHECK(S.m_v[idx_A] == b);
    BOOST_CHECK(S.m_a[idx_A] == b);
    BOOST_CHECK(S.m_b[idx_A] == b);

    BOOST_CHECK(S.m_v[idx_B] == c);
    BOOST_CHECK(S.m_a[idx_B] == c);
    BOOST_CHECK(S.m_b[idx_B] == c);

    BOOST_CHECK_CLOSE(S.m_w[idx_A], 0.5, 0.01);
    BOOST_CHECK_CLOSE(S.m_w[idx_B], 0.5, 0.01);
  }
  // On edge AC new simplex should be AC
  {
    simplex_type S;

    OpenTissue::gjk::add_point_to_simplex( a, a, a, S);
    OpenTissue::gjk::add_point_to_simplex( b, b, b, S);
    OpenTissue::gjk::add_point_to_simplex( c, c, c, S);

    vector3_type const p = vector3_type( -0.5, 0.0,  1.0);

    OpenTissue::gjk::detail::reduce_triangle( p, S );

    BOOST_CHECK( OpenTissue::gjk::dimension( S ) == 2u );

    int bit_A    = 0;
    size_t idx_A = 0;
    int bit_B    = 0;
    size_t idx_B = 0;
    OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B );

    BOOST_CHECK(S.m_bitmask == (bit_A | bit_B ));
    BOOST_CHECK(S.m_v[idx_A] == a);
    BOOST_CHECK(S.m_a[idx_A] == a);
    BOOST_CHECK(S.m_b[idx_A] == a);

    BOOST_CHECK(S.m_v[idx_B] == c);
    BOOST_CHECK(S.m_a[idx_B] == c);
    BOOST_CHECK(S.m_b[idx_B] == c);

    BOOST_CHECK_CLOSE(S.m_w[idx_A], 0.5, 0.01);
    BOOST_CHECK_CLOSE(S.m_w[idx_B], 0.5, 0.01);
  }

  // Assymmetric test cases

  { // New simplex should be AB
    simplex_type S;

    OpenTissue::gjk::add_point_to_simplex( a, a, a, S);
    OpenTissue::gjk::add_point_to_simplex( b, b, b, S);
    OpenTissue::gjk::add_point_to_simplex( c, c, c, S);

    vector3_type const p = 0.4*a + 0.6*b;

    OpenTissue::gjk::detail::reduce_triangle( p, S );

    BOOST_CHECK( OpenTissue::gjk::dimension( S ) == 2u );

    int bit_A    = 0;
    size_t idx_A = 0;
    int bit_B    = 0;
    size_t idx_B = 0;
    OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B );

    BOOST_CHECK(S.m_bitmask == (bit_A | bit_B ));
    BOOST_CHECK(S.m_v[idx_A] == a);
    BOOST_CHECK(S.m_a[idx_A] == a);
    BOOST_CHECK(S.m_b[idx_A] == a);

    BOOST_CHECK(S.m_v[idx_B] == b);
    BOOST_CHECK(S.m_a[idx_B] == b);
    BOOST_CHECK(S.m_b[idx_B] == b);

    BOOST_CHECK_CLOSE(S.m_w[idx_A], 0.4, 0.01);
    BOOST_CHECK_CLOSE(S.m_w[idx_B], 0.6, 0.01);
  }
  // New simplex should be BC
  {
    simplex_type S;

    OpenTissue::gjk::add_point_to_simplex( a, a, a, S);
    OpenTissue::gjk::add_point_to_simplex( b, b, b, S);
    OpenTissue::gjk::add_point_to_simplex( c, c, c, S);

    vector3_type const p = 0.4*b + 0.6*c;

    OpenTissue::gjk::detail::reduce_triangle( p, S );

    BOOST_CHECK( OpenTissue::gjk::dimension( S ) == 2u );

    int bit_A    = 0;
    size_t idx_A = 0;
    int bit_B    = 0;
    size_t idx_B = 0;
    OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B );

    BOOST_CHECK(S.m_bitmask == (bit_A | bit_B ));
    BOOST_CHECK(S.m_v[idx_A] == b);
    BOOST_CHECK(S.m_a[idx_A] == b);
    BOOST_CHECK(S.m_b[idx_A] == b);

    BOOST_CHECK(S.m_v[idx_B] == c);
    BOOST_CHECK(S.m_a[idx_B] == c);
    BOOST_CHECK(S.m_b[idx_B] == c);

    BOOST_CHECK_CLOSE(S.m_w[idx_A], 0.4, 0.01);
    BOOST_CHECK_CLOSE(S.m_w[idx_B], 0.6, 0.01);
  }
  // New simplex should be AC
  {
    simplex_type S;

    OpenTissue::gjk::add_point_to_simplex( a, a, a, S);
    OpenTissue::gjk::add_point_to_simplex( b, b, b, S);
    OpenTissue::gjk::add_point_to_simplex( c, c, c, S);

    vector3_type const p = 0.6*a + 0.4*c;

    OpenTissue::gjk::detail::reduce_triangle( p, S );

    BOOST_CHECK( OpenTissue::gjk::dimension( S ) == 2u );

    int bit_A    = 0;
    size_t idx_A = 0;
    int bit_B    = 0;
    size_t idx_B = 0;
    OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B );

    BOOST_CHECK(S.m_bitmask == (bit_A | bit_B ));
    BOOST_CHECK(S.m_v[idx_A] == a);
    BOOST_CHECK(S.m_a[idx_A] == a);
    BOOST_CHECK(S.m_b[idx_A] == a);

    BOOST_CHECK(S.m_v[idx_B] == c);
    BOOST_CHECK(S.m_a[idx_B] == c);
    BOOST_CHECK(S.m_b[idx_B] == c);

    BOOST_CHECK_CLOSE(S.m_w[idx_A], 0.6, 0.01);
    BOOST_CHECK_CLOSE(S.m_w[idx_B], 0.4, 0.01);
  }

  // New simplex should be ABC
  {
    simplex_type S;

    OpenTissue::gjk::add_point_to_simplex( a, a, a, S);
    OpenTissue::gjk::add_point_to_simplex( b, b, b, S);
    OpenTissue::gjk::add_point_to_simplex( c, c, c, S);

    vector3_type const p = 0.1*a + 0.2*b + 0.7*c;

    OpenTissue::gjk::detail::reduce_triangle( p, S );

    BOOST_CHECK( OpenTissue::gjk::dimension( S ) == 3u );

    int bit_A    = 0;
    size_t idx_A = 0;
    int bit_B    = 0;
    size_t idx_B = 0;
    int bit_C    = 0;
    size_t idx_C = 0;
    OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B , idx_C, bit_C );

    BOOST_CHECK(S.m_bitmask == (bit_A | bit_B | bit_C));
    BOOST_CHECK(S.m_v[idx_A] == a);
    BOOST_CHECK(S.m_a[idx_A] == a);
    BOOST_CHECK(S.m_b[idx_A] == a);

    BOOST_CHECK(S.m_v[idx_B] == b);
    BOOST_CHECK(S.m_a[idx_B] == b);
    BOOST_CHECK(S.m_b[idx_B] == b);

    BOOST_CHECK(S.m_v[idx_C] == c);
    BOOST_CHECK(S.m_a[idx_C] == c);
    BOOST_CHECK(S.m_b[idx_C] == c);

    BOOST_CHECK_CLOSE(S.m_w[idx_A], 0.1, 0.01);
    BOOST_CHECK_CLOSE(S.m_w[idx_B], 0.2, 0.01);
    BOOST_CHECK_CLOSE(S.m_w[idx_C], 0.7, 0.01);
  }

}

BOOST_AUTO_TEST_SUITE_END();


#include <iostream>
#include <cstdlib>
#include <cmath>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <string>
#include <boost/multi_array.hpp>

# if not defined MATRIX_CLASS_H
# define MATRIX_CLASS_H

using namespace std;

/* predefined Boost multi-array types */
typedef boost::multi_array<double, 3> boost_array3d_t;
typedef boost::array<boost_array3d_t::index, 3> boost_array3d_ind_t;
typedef boost::multi_array<double, 2> boost_array2d_t;
typedef boost::multi_array<double, 1> boost_array1d_t;

/* print 3D array in Paraview format */
void print_vtk(boost_array3d_t&, string, const int);
void print_vtk(std::vector<double>&, const int, const int, const int, string);

/* print 3D array in standard csv format */
void print_array_slice(boost_array3d_t &arr, string filename, int slice_ind);
void print_array_full(boost_array3d_t &arr, string filename);

double l2_norm(boost_array3d_t& data);
double sum_elements(boost_array3d_t& data);

# endif


//
// Created by a.kiryanenko on 3/10/20.
//

#include <iostream>
#include "../SpuUltraGraphAdapter.h"
#include "../SpuUltraGraphProperty.h"
#include <boost/graph/graphviz.hpp>
#include <boost/graph/dijkstra_shortest_paths_no_color_map.hpp>

using namespace std;
using namespace SPU_GRAPH;
using namespace boost;

// Класс для печати свойств вершины для graphvis
class vertex_property_writer {
    const SpuUltraGraph &_g;
public:
    vertex_property_writer(SpuUltraGraph &g) : _g(g) {}
    void operator()(std::ostream& out, const SpuUltraGraph::vertex_descriptor &v) const {
        out << "[label=\"" << v << "\"]";
    }
};

// Класс для печати свойств ребра для graphvis
class edge_property_writer {
    const SpuUltraGraph &_g;
public:
    edge_property_writer(SpuUltraGraph &g) : _g(g) {}
    void operator()(std::ostream& out, const SpuUltraGraph::edge_descriptor &e) const {
        out << "[label=\"" << get(edge_weight, _g, e) << "\"]";
    }
};

int main()
{
    SpuUltraGraph graph;

    graph.add_vertex(1);
    graph.add_vertex(2);
    graph.add_vertex(3);
    graph.add_vertex(4);
    graph.add_vertex(5);
    graph.add_vertex(10);

    graph.add_edge(graph.get_free_edge_descriptor(2), 1, 2);
    graph.add_edge(graph.get_free_edge_descriptor(1), 1, 3);
    graph.add_edge(graph.get_free_edge_descriptor(8), 1, 10);
    graph.add_edge(graph.get_free_edge_descriptor(1), 2, 2);
    graph.add_edge(graph.get_free_edge_descriptor(1), 2, 3);
    graph.add_edge(graph.get_free_edge_descriptor(2), 2, 4);
    graph.add_edge(graph.get_free_edge_descriptor(5), 3, 4);
    graph.add_edge(graph.get_free_edge_descriptor(6), 3, 5);
    graph.add_edge(graph.get_free_edge_descriptor(3), 4, 10);
    graph.add_edge(graph.get_free_edge_descriptor(7), 5, 10);

    // С помощью GraphViz создадим svg изображение графа
    vertex_property_writer vpw(graph);
    edge_property_writer epw(graph);
    std::ofstream f("graph.dot");
    boost::write_graphviz(f, graph, vpw, epw);
    f.close();
    system("dot graph.dot -Kcirco -Tsvg -o graph.svg");

    // Создаю свойство предшественник для вершин
    map<SpuUltraGraph::vertex_descriptor, SpuUltraGraph::vertex_descriptor> vertex_to_predecessor;
    associative_property_map<map<SpuUltraGraph::vertex_descriptor, SpuUltraGraph::vertex_descriptor>> predecessor_property_map(vertex_to_predecessor);
    // Создаю свойство расстояние для вершин
    map<SpuUltraGraph::vertex_descriptor, size_t> vertex_to_distance;
    associative_property_map<map<SpuUltraGraph::vertex_descriptor, size_t>> distance_property_map(vertex_to_distance);

    // Выполняю алгоритм дейкстра для подсчета расстояний от вершины #1 до остальных
    dijkstra_shortest_paths_no_color_map(graph, 1, predecessor_map(predecessor_property_map).distance_map(distance_property_map));

    std::cout << "Distances and parents:" << std::endl;
    for (auto v: graph.vertices()) {
        cout << "Vertex " << v << " Distance = " << vertex_to_distance[v] << ", parent = " << vertex_to_predecessor[v] + 1 << endl;
    }
    return 0;
}


// Copyright Oleg Maximenko 2014.
// 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)
//
// See http://github.com/svgpp/svgpp for library home page.

#pragma once

#include <svgpp/definitions.hpp>
#include <boost/math/constants/constants.hpp>

namespace svgpp { namespace traits
{

template<class Src, class Dst, class Number = double>
struct angle_conversion_coefficient
{
  static BOOST_CONSTEXPR Number value()
  {
    return angle_conversion_coefficient<Src, tag::angle_units::deg>::value()
      / angle_conversion_coefficient<Dst, tag::angle_units::deg>::value();
  }
};

template<class Src, class Number>
struct angle_conversion_coefficient<Src, Src, Number>
{ static BOOST_CONSTEXPR Number value() { return static_cast<Number>(1); } };

template<class Number>
struct angle_conversion_coefficient<tag::angle_units::grad, tag::angle_units::deg, Number>
{ static BOOST_CONSTEXPR Number value() { return static_cast<Number>(0.9); } };

template<class Number>
struct angle_conversion_coefficient<tag::angle_units::rad, tag::angle_units::deg, Number>
{ static BOOST_CONSTEXPR Number value() { return boost::math::constants::radian<Number>(); } };

}}


#include <Eigen/Core>
#include <Eigen/Geometry>

#include "mesh/mesh.h"

namespace telef::mesh {
void ColorMesh::applyTransform(Eigen::MatrixXf transform) {
  Eigen::Map<Eigen::Matrix3Xf> v(position.data(), 3, position.size() / 3);
  Eigen::Matrix3Xf result =
      (transform * v.colwise().homogeneous()).colwise().hnormalized();
  position = Eigen::Map<Eigen::VectorXf>{result.data(), result.size()};
}
} // namespace telef::mesh


//
// Copyright 2019 Miral Shah <miralshah2211@gmail.com>
//
// 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)
//

#include <boost/gil/extension/io/jpeg.hpp>
#include <boost/gil/image_processing/threshold.hpp>

using namespace boost::gil;

// Demonstrates thresholding
// Thresholding can either attribute an arbitrary value to pixels whose values are greater than the threshold
// or can truncate the pixel values at an arbitrary maximum.
// In particular, the function threshold_truncate accepts a mode and a direction.
// Passing threshold_truncate_mode::threshold effectively truncates the values to the threshold, whereas
// threshold_truncate_mode::zero sets them to 0.
// The combination of mode and direction controls which pixels are modified:
//     - threshold and regular: truncates the pixels whose values are greater than the threshold
//     - threshold and inverse: truncates the pixels whose values are less than the threshold
//     - zero and regular: zeroes the pixels whose values are less than the threshold
//     - zero and inverse: zeroes the pixels whose values are greater than the threshold
// See also:
// adaptive_threshold.cpp - Adaptive thresholding

int main()
{
    rgb8_image_t img;
    read_image("test.jpg",img, jpeg_tag{});
    rgb8_image_t img_out(img.dimensions());

//    performing binary threshold on each channel of the image
//    if the pixel value is more than 150 than it will be set to 255 else to 0
    boost::gil::threshold_binary(const_view(img), view(img_out), 150, 255);
    write_view("out-threshold-binary.jpg", view(img_out), jpeg_tag{});

//    if the pixel value is more than 150 than it will be set to 150 else no change
    boost::gil::threshold_truncate(const_view(img), view(img_out), 150, threshold_truncate_mode::threshold);
    write_view("out-threshold-binary_inv.jpg", view(img_out), jpeg_tag{});

    return 0;
}


/*
 * Copyright Andrey Semashev 2020
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or copy at
 * https://www.boost.org/LICENSE_1_0.txt)
 */
/*!
 * \file pow2.hpp
 *
 * This header includes all algorithms for operating on integral powers of 2.
 */

#ifndef BOOST_BIT_OPS_POW2_HPP_INCLUDED_
#define BOOST_BIT_OPS_POW2_HPP_INCLUDED_

#include <boost/bit_ops/pow2/bit_ceil.hpp>
#include <boost/bit_ops/pow2/bit_floor.hpp>
#include <boost/bit_ops/pow2/bit_width.hpp>
#include <boost/bit_ops/pow2/is_power_of_2.hpp>
#include <boost/bit_ops/pow2/has_single_bit.hpp>

#endif // BOOST_BIT_OPS_POW2_HPP_INCLUDED_


#include "vdp_sim.hpp"

#include <algorithm>
#include <filesystem>
#include <random>

#include <boost/numeric/odeint.hpp>

const vdp_sim::input_type
vdp_sim::interp1d(const std::vector<time>& u_t,
                const std::vector<input_type>& u,
                time t)
{
    const auto it = std::lower_bound(u_t.cbegin(),
                            u_t.cend(),
                            t);
    if (it==u_t.cend())
        return u.back();
    else if (it==u_t.cbegin())
        return u.front();
    else{
        const auto idx = std::distance(u_t.cbegin(),it);
        input_type result;
        time dt = u_t[idx]-u_t[idx-1];

        for(size_t i=0; i < result.size(); ++i)
            result[i] = u[idx-1][i]
                + ((u[idx][i]-u[idx-1][i])/dt)
                    *(t-u_t[idx-1]);

        return result;
    }
}

void
vdp_sim::generate_data(const std::string& dir, size_t n)
{
    std::random_device rd;
    std::mt19937 gen(rd());
    std::normal_distribution<> noise{0., 1.};
    std::uniform_real_distribution<> initial{-1.,1.};

    boost::numeric::odeint::runge_kutta4<state_type> stepper;
    auto vdp =
        [this](const state_type &x,
                state_type &dxdt,
                time t)
        {
            input_type aux = interp1d(u_t, u, t);

            dxdt[0] = 2.*x[1];
            dxdt[1] = -0.8*x[0] + 2.*x[1]
                    - 10.*x[0]*x[0]*x[1] + aux[0];
        };

    std::filesystem::create_directory(dir);

    auto print_step =
        [this](const state_type& x, const double t)
        {
            data_stream << t << ",";
            for(const auto& e : x)
                data_stream << e << ",";
            data_stream << std::endl;
        };

    for(size_t i=0; i<n; ++i){
        std::generate(u.begin(),
                    u.end(),
                    [&noise,&gen]() {
                        input_type aux;
                        for(auto& e : aux)
                            e = noise(gen);
                        return aux;
                    });

        data_stream.open(dir+"Input_"+std::to_string(i)+".csv");
        for(size_t j=0; j<u.size(); ++j){
            data_stream << u_t[j] << ",";
            for(const auto& e : u[j])
                data_stream << e << ",";
            data_stream << std::endl;
        }
        data_stream.close();

        state_type x0;
        std::generate(x0.begin(),x0.end(),
            [&initial, &gen](){return initial(gen);});

        data_stream.open(
            dir+"Output_"+std::to_string(i)+".csv");
        boost::numeric::odeint::
            integrate_const(stepper,
                            vdp,
                            x0,
                            0.0,
                            period,
                            ts,
                            print_step);
        data_stream.close();
    }
}


// geometry_test.cpp : Defines the entry point for the console application.
//

#include <fstream>
std::ostream& logger()
{
	static std::ofstream instance("geometry_log.hpp");
	return instance;
}

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

#include "private_allocator_tests.hpp"

#include "product_tests.hpp"
//
// //#include "access_time_tests.hpp"
// //#include "linear_algebra_timings.hpp"
//
// #include "lup_decomposition_test.hpp"
#include "proto_expression_tests.hpp"
// //
#include "tensor_tests.hpp"
#include "point_example.hpp"
#include "point_tests.hpp"
#include "matrix_traits_test.hpp"
#include "transform_tests.hpp"
// //#include "numeric_cast_traits_test.hpp"
//
#include "utility_tests.hpp"
#include "point_example_multi_type.hpp"
//
#include "tolerance_comparison_tests.hpp"
//#include "constant_tests.hpp"
#include "distance_tests.hpp"
// #include "bsp_test.hpp"
// #include "member_fusion_adaptor.hpp"
#include "intersection_tests.hpp"
//
// #include "reference_frame_tests.hpp"
//
// //#include "kd_tree_test.hpp"
// // #include "convex_hull_test.hpp"
#include "segment_interval_test.hpp"
// // #include "trapezoidal_decomposition_test.hpp"
#include "eberly_triangle_aabb_intersection_tests.hpp"
// //#include "boolean_operation_bsp_test.hpp"
//
// //#include "compose_matrix_test.hpp"
//
#include "vector_point_arithmetic_tests.hpp"
//
#include "grid_tests.hpp"
#include "mesh_2d_tests.hpp"
// #include "as_tests.hpp"
#include "sorting_tests.hpp"
#include "units_tests.hpp"
 #include "tagged_quantity_tests.hpp"

#include "point_sequence_tests.hpp"
#include "segment_intersection_tests.hpp"

using namespace geometrix;

typedef point_double_2d point2;
typedef vector_double_2d vector2;
typedef segment_double_2d segment2;
typedef polyline<point2> polyline2;
typedef polygon<point2> polygon2;

double vec_length(vector2 const& v)
{
	using namespace geometrix;
	return sqrt(get<0>(v)*get<0>(v) + get<1>(v)*get<1>(v));
}

double p2p_distance(point2 const& p1, point2 const& p2)
{
	using namespace geometrix;
	return point_point_distance(p1, p2);
}

double p2p_angle(point2 const& p1, point2 const& p2)
{
	using namespace geometrix;
	return angle_from_a_to_b(p1, p2);
}

double vec_angle(vector2 const& v)
{
	return geometrix::vector_angle(v);
}

double seg_length(const segment2& seg)
{
	using namespace geometrix;
	return point_point_distance(seg.get_start(), seg.get_end());
}

double seg2p_distance(const segment2& seg, const point2& p)
{
	using namespace geometrix;
	return point_segment_distance(p, seg);
}

void write_point(const point2& p)
{
	logger() << p << std::endl;
}

void write_vector(const vector2& p)
{
	logger() << p << std::endl;
}

void write_segment(const segment2& s)
{
	logger() << s << std::endl;
}

void write_polygon(const polygon2& p)
{
	logger() << p << std::endl;
}

void write_polyline(const polyline2& p)
{
	logger() << p << std::endl;
}

void write_triangle(const std::array<point2,3>& p)
{
	logger() << polygon2(p.begin(), p.end()) << std::endl;
}

void write_mesh(const geometrix::mesh_2d<double>& mesh)
{
	logger() << "-------start mesh \n";
	for (int i = 0; i < mesh.get_number_triangles(); ++i) {
		write_triangle(mesh.get_triangle_vertices(i));
	}
	logger() << "-------end mesh" << std::endl;
}

void StandardExceptionTranslator(const std::exception& e)
{
	BOOST_TEST_MESSAGE(e.what());
}

boost::unit_test::test_suite* init_unit_test_suite(int, char*[])
{
	boost::unit_test::unit_test_log.set_threshold_level(boost::unit_test::log_messages);
	boost::unit_test::unit_test_monitor.register_exception_translator<std::exception>(&StandardExceptionTranslator);
	boost::unit_test::framework::master_test_suite().p_name.value = "Geometrix Testing Framework";

	//with explicit registration we could specify a test case timeout
	//boost::unit_test::framework::master_test_suite().add( BOOST_TEST_CASE( &infinite_loop ), 0, /* timeout */ 2 );

	return 0;
}


#include <boost/test/unit_test.hpp>

#include "pythagoricien.h"
#include "utilitaires.h"

BOOST_AUTO_TEST_SUITE(test_pythagoricien)

    struct fixture_pythagoricien {
        std::set<std::tuple<size_t, size_t, size_t>> triplets;

        fixture_pythagoricien() {
            triplets = std::set<std::tuple<size_t, size_t, size_t>>
                    {
                            std::make_tuple(3, 4, 5),
                            std::make_tuple(5, 12, 13),
                            std::make_tuple(8, 15, 17),
                            std::make_tuple(7, 24, 25),

                            std::make_tuple(20, 21, 29),
                            std::make_tuple(12, 35, 37),
                            std::make_tuple(9, 40, 41),
                            std::make_tuple(28, 45, 53),

                            std::make_tuple(11, 60, 61),
                            std::make_tuple(16, 63, 65),
                            std::make_tuple(33, 56, 65),
                            std::make_tuple(48, 55, 73),

                            std::make_tuple(13, 84, 85),
                            std::make_tuple(36, 77, 85),
                            std::make_tuple(39, 80, 89),
                            std::make_tuple(65, 72, 97)
                    };
        }
    };

    BOOST_FIXTURE_TEST_CASE(nombres_pythagoricien, fixture_pythagoricien) {
        Pythagoricien pythagoricien(100);
        std::set<std::tuple<size_t, size_t, size_t>> resultat;
        for (auto t: pythagoricien) {
            resultat.insert(t);
        }

        BOOST_CHECK_EQUAL_COLLECTIONS(triplets.begin(), triplets.end(), resultat.begin(), resultat.end());
    }

BOOST_AUTO_TEST_SUITE_END()


#ifndef STAN_MATH_REV_FUN_FFT_HPP
#define STAN_MATH_REV_FUN_FFT_HPP

#include <stan/math/prim/fun/Eigen.hpp>
#include <stan/math/prim/fun/typedefs.hpp>
#include <stan/math/rev/meta.hpp>
#include <stan/math/prim/fun/fft.hpp>
#include <stan/math/prim/fun/to_complex.hpp>
#include <Eigen/Dense>
#include <complex>
#include <type_traits>
#include <vector>

namespace stan {
namespace math {

/**
 * Return the discrete Fourier transform of the specified complex
 * vector.
 *
 * Given an input complex vector `x[0:N-1]` of size `N`, the discrete
 * Fourier transform computes entries of the resulting complex
 * vector `y[0:N-1]` by
 *
 * ```
 * y[n] = SUM_{i < N} x[i] * exp(-n * i * 2 * pi * sqrt(-1) / N)
 * ```
 *
 * The adjoint computation is given by
 * ```
 * adjoint(x) += length(y) * inv_fft(adjoint(y))
 * ```
 *
 * If the input is of size zero, the result is a size zero vector.
 *
 * @tparam V type of complex vector argument
 * @param[in] x vector to transform
 * @return discrete Fourier transform of `x`
 */
template <typename V, require_eigen_vector_vt<is_complex, V>* = nullptr,
          require_var_t<base_type_t<value_type_t<V>>>* = nullptr>
inline plain_type_t<V> fft(const V& x) {
  if (unlikely(x.size() <= 1)) {
    return plain_type_t<V>(x);
  }

  arena_t<V> arena_v = x;
  arena_t<V> res = fft(to_complex(arena_v.real().val(), arena_v.imag().val()));

  reverse_pass_callback([arena_v, res]() mutable {
    auto adj_inv_fft = inv_fft(to_complex(res.real().adj(), res.imag().adj()));
    adj_inv_fft *= res.size();
    arena_v.real().adj() += adj_inv_fft.real();
    arena_v.imag().adj() += adj_inv_fft.imag();
  });

  return plain_type_t<V>(res);
}

/**
 * Return the inverse discrete Fourier transform of the specified
 * complex vector.
 *
 * Given an input complex vector `y[0:N-1]` of size `N`, the inverse
 * discrete Fourier transform computes entries of the resulting
 * complex vector `x[0:N-1]` by
 *
 * ```
 * x[n] = SUM_{i < N} y[i] * exp(n * i * 2 * pi * sqrt(-1) / N)
 * ```
 *
 *  * The adjoint computation is given by
 * ```
 * adjoint(y) += (1 / length(x)) * fft(adjoint(x))
 * ```
 *
 * If the input is of size zero, the result is a size zero vector.
 * The only difference between the discrete DFT and its inverse is
 * the sign of the exponent.
 *
 * @tparam V type of complex vector argument
 * @param[in] y vector to inverse transform
 * @return inverse discrete Fourier transform of `y`
 */
template <typename V, require_eigen_vector_vt<is_complex, V>* = nullptr,
          require_var_t<base_type_t<value_type_t<V>>>* = nullptr>
inline plain_type_t<V> inv_fft(const V& y) {
  if (unlikely(y.size() <= 1)) {
    return plain_type_t<V>(y);
  }

  arena_t<V> arena_v = y;
  arena_t<V> res
      = inv_fft(to_complex(arena_v.real().val(), arena_v.imag().val()));

  reverse_pass_callback([arena_v, res]() mutable {
    auto adj_fft = fft(to_complex(res.real().adj(), res.imag().adj()));
    adj_fft /= res.size();

    arena_v.real().adj() += adj_fft.real();
    arena_v.imag().adj() += adj_fft.imag();
  });
  return plain_type_t<V>(res);
}

}  // namespace math
}  // namespace stan
#endif


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

// Copyright (c) 2011-2012 Barend Gehrels, Amsterdam, the Netherlands.

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

//[c_array
//` Small example showing the combination of an array with a Boost.Geometry algorithm

#include <iostream>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>

BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)

int main()
{
    int a[3] = {1, 2, 3};
    int b[3] = {2, 3, 4};

    std::cout << boost::geometry::distance(a, b) << std::endl;

    return 0;
}

//]

//[c_array_output
/*`
Output:
[pre
1.73205
]
*/
//]


//C System-Headers
//
//C++ System headers
#include <vector>//vector
#include <string>//string
#include <fstream>//iss*
#include <chrono>// timing functions
#include <cmath>//sqrt, abs
#include <iostream>//cout
#include <typeinfo>//typeid
#include <algorithm> // transform, find
#include <functional> // plus/minus
#include <utility>//std::make_pair
//Qt Headers
//
//OpenCV Headers
//
//Boost Headers
#include <boost/algorithm/string.hpp>//split() and is_any_of for parsing .csv files
#include <boost/lexical_cast.hpp>//lexical cast (unsurprisingly)
//Project specific headers
#include "datatransformations.h"

std::vector < data_triple<double> > power_to_data_triples( std::vector< double > power_list, double cavity_length, double min_freq, double max_freq ) {

    uint number_of_points = power_list.size();

    std::vector< data_triple<double> > processed;
    processed.reserve( number_of_points );

    for( uint i = 0; i < number_of_points ; i++ ) {

        double i_f = static_cast<double>(i);
        double num_points_f = static_cast<double>(number_of_points);

        double frequency = (i_f + 1.0)*(max_freq - min_freq)/(num_points_f) + min_freq;
        double power = power_list.at(i);

        processed.push_back( data_triple<double>( round(frequency), cavity_length, power ) );
    }

    return processed;

}

std::vector < float > string_to_power_list( std::string raw_data ) {

    std::istringstream iss (raw_data);

    while (iss) {
        std::string input;
        std::getline(iss, input);

        std::vector<std::string> strs;
        //split on comma delimiter
        boost::split(strs, input, boost::is_any_of(","));

        std::vector< float > processed;
        processed.reserve( strs.size() );

        for( uint i = 0; i < strs.size() ; i ++ ) {
            processed.push_back( boost::lexical_cast<float>( strs[i]) );
        }

        return processed;
    }

}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

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

**/
//==================================================================================================
#ifndef BOOST_SIMD_REDUCTION_HPP_INCLUDED
#define BOOST_SIMD_REDUCTION_HPP_INCLUDED

namespace boost { namespace simd
{
  /*!
    @ingroup group-functions
    @defgroup group-reduction Reduction Functions

    These functions provide algorithms for in-register reduction and prefix-scan operations.
    Those functions are usually only defined for SIMD types but may, in some cases such as sum or
    product, have a valid scalar semantic.
  **/

} }

#include <boost/simd/function/all.hpp>
#include <boost/simd/function/any.hpp>
#include <boost/simd/function/compare_equal.hpp>
#include <boost/simd/function/compare_greater_equal.hpp>
#include <boost/simd/function/compare_greater.hpp>
#include <boost/simd/function/compare_less_equal.hpp>
#include <boost/simd/function/compare_less.hpp>
#include <boost/simd/function/compare_not_equal.hpp>
#include <boost/simd/function/cummax.hpp>
#include <boost/simd/function/cummin.hpp>
#include <boost/simd/function/cumprod.hpp>
#include <boost/simd/function/cumsum.hpp>
#include <boost/simd/function/dot.hpp>
#include <boost/simd/function/hmsb.hpp>
#include <boost/simd/function/is_included_c.hpp>
#include <boost/simd/function/is_included.hpp>
#include <boost/simd/function/maximum.hpp>
#include <boost/simd/function/minimum.hpp>
#include <boost/simd/function/nbtrue.hpp>
#include <boost/simd/function/none.hpp>
#include <boost/simd/function/prod.hpp>
#include <boost/simd/function/sum.hpp>

#endif


#include "odd_or_even.hpp"
//#include <boost/numeric/meta_math/odd_or_even.hpp>
#include<iostream>
using namespace std;
int main(){
        const long long n=6174;
        cout<<n<<" is "<<(meta_math::odd_or_even<n>::value ? "ODD":"EVEN")<<'\n';
        return 0;
    }


#include <wmtk/TriMesh.h>
#include <wmtk/utils/VectorUtils.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <wmtk/ExecutionScheduler.hpp>
#include "EdgeOperations2d.h"

using namespace Edge2d;
using namespace wmtk;
// get the quadrix in form of an array of 10 floating point numbers
Eigen::MatrixXd compute_Q_f(const EdgeOperations2d& m, const wmtk::TriMesh::Tuple& f_tuple)
{
    auto conn_indices = m.oriented_tri_vertices(f_tuple);
    Eigen::Vector3d A = m.vertex_attrs[conn_indices[0].vid(m)].pos;
    Eigen::Vector3d B = m.vertex_attrs[conn_indices[1].vid(m)].pos;
    Eigen::Vector3d C = m.vertex_attrs[conn_indices[2].vid(m)].pos;

    Eigen::Vector3d n = ((A - B).cross(C - B)).normalized();
    Eigen::Vector4d p;
    p(0) = n(0);
    p(1) = n(1);
    p(2) = n(2);
    p(3) = -n.dot(B);
    return p * p.transpose();
}

Eigen::MatrixXd compute_Q_v(const EdgeOperations2d& m, const TriMesh::Tuple& v_tuple)
{
    auto conn_tris = m.get_one_ring_tris_for_vertex(v_tuple);
    Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(4, 4);
    auto Q_t = [](auto& m, auto& f_tuple) {
        auto conn_indices = m.oriented_tri_vertices(f_tuple);
        Eigen::Vector3d A = m.vertex_attrs[conn_indices[0].vid(m)].pos;
        Eigen::Vector3d B = m.vertex_attrs[conn_indices[1].vid(m)].pos;
        Eigen::Vector3d C = m.vertex_attrs[conn_indices[2].vid(m)].pos;

        Eigen::Vector3d n = ((A - B).cross(C - B)).normalized();
        Eigen::Vector4d p;
        p(0) = n(0);
        p(1) = n(1);
        p(2) = n(2);
        p(3) = -n.dot(B);

        return (p * p.transpose());
    };
    for (auto tri : conn_tris) {
        auto Q_tmp = compute_Q_f(m, tri);
        Q += Q_tmp;
    }
    return Q;
}

double Edge2d::EdgeOperations2d::compute_cost_for_e(const TriMesh::Tuple& v_tuple)
{
    Eigen::MatrixXd Q = compute_Q_v(*this, v_tuple);
    Q += compute_Q_v(*this, v_tuple.switch_vertex(*this));

    Eigen::Vector4d t(0.0, 0.0, 0.0, 1.0);
    Eigen::MatrixXd vQ = Q;
    vQ.row(3) = t;

    Eigen::Vector4d v;
    if (vQ.determinant() < 1e-6) {
        Eigen::Vector3d tmp =
            (vertex_attrs[v_tuple.vid(*this)].pos + vertex_attrs[switch_vertex(v_tuple).vid(*this)].pos) / 2;
        v << tmp, 1.0;
    }

    else
        v = vQ.inverse() * t;

    // wmtk::logger().info("Q is \n {} \n v is \n {}", Q, v);
    Eigen::Vector3d newv = v.head(3);

    return (v.transpose() * Q * v);
}

bool Edge2d::EdgeOperations2d::collapse_qec(int target)
{
    // find the valid pairs (for each vertex)
    size_t vertex_number = vert_capacity();
    auto collect_all_ops = std::vector<std::pair<std::string, Tuple>>();
    for (auto& loc : get_edges()) collect_all_ops.emplace_back("edge_collapse", loc);

    auto executor = wmtk::ExecutePass<EdgeOperations2d, wmtk::ExecutionPolicy::kSeq>();
    executor.renew_neighbor_tuples = [](auto& m, auto op, auto& tris) {
        auto edges = m.new_edges_after(tris);
        auto optup = std::vector<std::pair<std::string, TriMesh::Tuple>>();
        // for (auto& e : edges) optup.emplace_back(op, e);
        return optup;
    };

    executor.priority = [this](auto& m, auto _, auto& e) {
        //     return -(m.vertex_attrs.pos[e.vid(*this)] -
        //     m.vertex_attrs.pos[e.switch_vertex(m).vid(*this)])
        //                 .norm();
        // };

        // wmtk::logger().info(
        //     "{} \n{}\n {}",
        //     vertex_attrs->m_attributes[e.vid(*this)].pos,
        //     vertex_attrs->m_attributes[e.switch_vertex(m).vid(*this)].pos,
        //     compute_cost_for_e(e));
        return -compute_cost_for_e(e);
    };
    executor.is_weight_up_to_date = [&collect_all_ops, this](auto& m, auto& ele) {
        auto& [val, op, e] = ele;

        if (val > 0) return false; // priority is negated.
        double pri = -compute_cost_for_e(e);
        if ((val - pri) < 1e-5) {
            wmtk::logger().info("the priority is different");
            return false;
        }
        for (auto edge : get_edges()) {
            if (pri < -compute_cost_for_e(edge)) {
                wmtk::logger().info("!!!! should not happen !!!!");
                return false;
            }
        }

        return true;
    };
    executor.stopping_criterion_checking_frequency = 1000;
    executor.stopping_criterion = [&target](auto& m) {
        if (m.get_vertices().size() < target) return true;
        return false;
    };

    executor(*this, collect_all_ops);
    return true;
}


#pragma once

#include <Eigen/Dense>

namespace AQSystemSolver {
    template<typename MatrixType=Eigen::MatrixXd, typename VectorType=Eigen::VectorXd, typename RowVectorType=Eigen::RowVectorXd>
    class Tableau;

    template<typename T>
    using TableauType=T;
    //concept TableauType = SM_utils::is_base_of_template<T, Tableau>::value;

    template<typename MatrixType, typename VectorType, typename RowVectorType>
    class Tableau{
    private:
        MatrixType coefficients;
        VectorType constants;
    public:
        void assignRowFromTableau(Eigen::Index lhsRowIndex, const auto& rhsTableau, Eigen::Index rhsRowIndex) {
            coefficients.row(lhsRowIndex)=rhsTableau.getCoefficients().row(rhsRowIndex);
            constants.coeffRef(lhsRowIndex)=rhsTableau.getConstants().coeff(rhsRowIndex);
        }
        void assignRow(Eigen::Index lhsRowIndex, const auto& row, typename std::decay_t<decltype(row)>::Scalar constant) {
            coefficients.row(lhsRowIndex)=row;
            constants.coeffRef(lhsRowIndex)=constant;
        }
        [[nodiscard]] const auto& getCoefficients() const {
            return coefficients;
        }
        [[nodiscard]] const auto& getCoefficient(Eigen::Index row, Eigen::Index column) const {
            return coefficients.coeff(row, column);
        }
        [[nodiscard]] const auto& getConstants() const {
            return constants;
        }
        [[nodiscard]] const auto& getConstant(Eigen::Index i) const {
            return constants.coeff(i);
        }
        [[nodiscard]] auto rows() const {
            return constants.rows();
        }
        [[nodiscard]] auto cols() const {
            return coefficients.cols();
        }
        template<int implicitPower>
        void groupTerm(Eigen::Index row, Eigen::Index column){
            auto& power=coefficients.coeffRef(row, column);
            if(power==implicitPower-1){
                return; //it won't do anything, so we skip it.
            }
            if(power==implicitPower){
                throw std::runtime_error("Power is the same as the implicit power, and we are trying to eliminate a replacement. This is probably a Gibbs Rule violation.");
            }
            const auto factor=1/(implicitPower-power);
            coefficients.row(row)*=factor;
            constants.coeffRef(row)=pow(constants.coeff(row), factor);
            power=0;
        }

        [[nodiscard]] VectorType evalTerms(const RowVectorType& x) const {
            MatrixType terms(coefficients.rows(),coefficients.cols());
            for(Eigen::Index i=0; i<coefficients.rows(); ++i){
                terms.row(i)=pow(x.array(), coefficients.row(i).array());
            }
            return terms.rowwise().prod().array()*constants.array();
        }
        [[nodiscard]] MatrixType evalAddends(VectorType speciesConcentrations) const {
            return coefficients.array().colwise()*speciesConcentrations.array();
        }
        [[nodiscard]] RowVectorType eval(const RowVectorType& x) const {
            return evalAddends(evalTerms(x)).colwise().sum();
        }
        [[nodiscard]] RowVectorType eval(const VectorType& speciesConcentrations) const {
            return evalAddends(speciesConcentrations).colwise().sum();
        }
        [[nodiscard]] RowVectorType eval(const MatrixType& addends) const {
            return addends.colwise().sum();
        }
        void resize(Eigen::Index rows, Eigen::Index cols) {
            coefficients.resize(rows, cols);
            constants.resize(rows);
        }
        void conservativeResize(Eigen::Index rows, Eigen::Index cols) {
            coefficients.conservativeResize(rows, cols);
            constants.conservativeResize(rows);
        }

        template<bool eliminate_column=false>
        void substituteRowAndCol(const /*TableauType*/ auto& replacementTableau, Eigen::Index row, const /*TableauType*/ auto& originalTableau, Eigen::Index col){
            coefficients+=originalTableau.getCoefficients().col(col)*replacementTableau.getCoefficients().row(row);
            if constexpr(eliminate_column){
                coefficients.col(col)-=originalTableau.getCoefficients().col(col);
            }
            constants.array()*=pow(replacementTableau.getConstants().coeff(row), originalTableau.getCoefficients().col(col).array());
        }
        [[nodiscard]] auto reducedCopy(const auto& v1, const auto& v2) const {
            return Tableau{coefficients(v1, v2), constants(v1)};
        }
        [[nodiscard]] auto reducedCopy(const decltype(Eigen::all)& v1, const auto& v2) const {
            return Tableau{coefficients(v1, v2), constants};
        }
        [[nodiscard]] bool operator==(const auto& rhs) const {
            return coefficients==rhs.getCoefficients() && constants==rhs.getConstants();
        }
        Tableau(MatrixType coefficients_, VectorType constants_) : coefficients{std::move(coefficients_)}, constants{std::move(constants_)} {}
        Tableau() = default;
    };
    template<typename MatrixType=Eigen::MatrixXd, typename VectorType=Eigen::VectorXd, typename RowVectorType=Eigen::RowVectorXd>
    class TableauWithTotals : private Tableau<MatrixType, VectorType, RowVectorType>{
    private:
        using parent=Tableau<MatrixType, VectorType, RowVectorType>;
        RowVectorType totals;
    public:
        [[nodiscard]] const auto& getTotals(){
            return totals;
        }
        [[nodiscard]] const auto& getTotal(Eigen::Index col){
            return totals.coeff(col);
        }
        using parent::assignRowFromTableau;
        using parent::assignRow;
        using parent::getCoefficients;
        using parent::getCoefficient;
        using parent::getConstants;
        using parent::getConstant;
        using parent::rows;
        using parent::cols;
        using parent::evalTerms;
        using parent::evalAddends;

        [[nodiscard]] auto reducedCopy(const auto& v1, const auto& v2) const {
            return TableauWithTotals{parent::reducedCopy(v1, v2), totals(v2)};
        }
        [[nodiscard]] auto reducedCopy(const auto& v1, const decltype(Eigen::all)& v2) const {
            return TableauWithTotals{parent::reducedCopy(v1, v2), totals};
        }
        [[nodiscard]] auto reducedCopy(const decltype(Eigen::all)& v1, const auto& v2) const {
            return TableauWithTotals{parent::reducedCopy(v1, v2), totals(v2)};
        }
        template<bool eliminate_column>
        void substituteRowAndCol(const /*TableauType*/ auto& replacementTableau, Eigen::Index row, const TableauWithTotals& originalTableau, Eigen::Index col) {
            static_assert(!eliminate_column);
            parent::template substituteRowAndCol<false>(replacementTableau, row, originalTableau, col);
            totals+=originalTableau.totals.coeff(col)*replacementTableau.getCoefficients().row(row);
        }
        [[nodiscard]] RowVectorType eval(const auto& x) const {
            return parent::eval(x)-totals;
        }
        [[nodiscard]] RowVectorType evalWithoutTotal(const auto& x) const {
            return parent::eval(x);
        }
        void resize(Eigen::Index rows, Eigen::Index cols){
            parent::resize(rows, cols);
            totals.resize(cols);
        }
        void conservativeResize(Eigen::Index rows, Eigen::Index cols){
            parent::conservativeResize(rows, cols);
            totals.conservativeResize(cols);
        }
        [[nodiscard]] bool operator==(const TableauWithTotals& rhs) const {
            return parent::operator==(rhs) && totals==rhs.totals;
        }
        TableauWithTotals(parent tableau_, RowVectorType totals_) : parent{std::move(tableau_)}, totals{std::move(totals_)} {}
        TableauWithTotals() = default;
    };
} // namespace AQSystemSolver


#include <gtest/gtest.h>

#include "scheme/numeric/util.hh"
#include "scheme/io/dump_pdb_atom.hh"

#include "scheme/numeric/geom_4d.hh"

#include <Eigen/Dense>
#include <random>

#include "scheme/util/Timer.hh"

#include <fstream>

namespace scheme { namespace nest { namespace pmap {

using namespace Eigen;
using std::cout;
using std::endl;

TEST( geom_4d, quat_half_cell ){
	ASSERT_EQ( -0.0, 0.0 );
	ASSERT_EQ( numeric::to_half_cell(Eigen::Quaterniond( -1, 1, 1, 1 ) ).w(), 1.0 );
	ASSERT_EQ( numeric::to_half_cell(Eigen::Quaterniond(  0,-1, 1, 1 ) ).x(), 1.0 );
	ASSERT_EQ( numeric::to_half_cell(Eigen::Quaterniond(  0, 0,-1, 1 ) ).y(), 1.0 );
	ASSERT_EQ( numeric::to_half_cell(Eigen::Quaterniond(  0, 0, 0,-1 ) ).z(), 1.0 );
}

TEST( geom_4d , tetracontoctachoron_cell_lookup )
{
	typedef double Float;
	typedef uint64_t Index;
	typedef Eigen::Matrix<Float,4,1> V4;

	Map<Matrix<Float,48,4,RowMajor>const> t24( numeric::get_raw_48cell<Float>() );
	// ASSERT_EQ( V4(0,0,0,-1), t24.row(7).transpose() );
	// for(int i = 0; i < 48; ++i) ASSERT_FLOAT_EQ( t24.row(i).norm(), 1.0 );

	// std::ofstream out("test.pdb");
	// for(int i = 0; i < 48; ++i){
	// 	if( t24.row(i).block(0,0,1,3).norm() < 0.9 )
	// 		io::dump_pdb_atom(out,i,10*t24.row(i));
	// }
	// out.close();

	std::mt19937 mt((unsigned int)time(0));
	std::normal_distribution<> rnorm;
	std::uniform_real_distribution<> runif;

	int NITER = 200*1000;
	#ifdef SCHEME_BENCHMARK
		NITER *= 50;
	#endif

	std::vector<V4> samp(NITER);
	std::vector<Index> cell(NITER),cell2(NITER);

	for(int i = 0; i < NITER; ++i){
		V4 quat(rnorm(mt),rnorm(mt),rnorm(mt),rnorm(mt));
		samp[i] = quat.normalized();
	}
		// V4 const quat_pos = quat.cwiseAbs();

	util::Timer<> naive;
	for(int i = 0; i < NITER; ++i){
		// (t24*samp[i]).cwiseAbs().maxCoeff(&cell[i]);
		(t24*samp[i]).maxCoeff(&cell[i]);
	}
	cout << "bt24 naive rate:  " << (Float)NITER / naive.elapsed_nano() << endl;

	util::Timer<> clever;
	for(int i = 0; i < NITER; ++i){
		numeric::get_cell_48cell( samp[i], cell2[i] );
		// this is slower !?!
		// Float mx = std::max(std::max(hyperface_dist,corner_dist),edge_dist);
		// cell2[i] = hyperface_dist==mx ? facecell : (corner_dist==mx ? cornercell+8 : edgecell+24);

	}
	cout << "bt24 clever rate: " << (double)NITER / clever.elapsed_nano() << endl;

	for(int i = 0; i < NITER; ++i){
		if( cell[i] != cell2[i] ){
			ASSERT_FLOAT_EQ(
			     t24.row(cell [i]).dot(samp[i]) ,
			     t24.row(cell2[i]).dot(samp[i]) );
		} else {
			ASSERT_EQ( cell[i], cell2[i] );
		}
	}

}

TEST( geom_4d , tetracontoctachoron_half_cell_lookup )
{
	typedef double Float;
	typedef uint64_t Index;
	typedef Eigen::Matrix<Float,4,1> V4;

	Map<Matrix<Float,24,4,RowMajor>const> t24h( numeric::get_raw_48cell_half<Float>() );

	for(int i = 0; i < 24; ++i){
		size_t cell;
		V4 tmp = t24h.row(i);
		numeric::get_cell_48cell_half( tmp, cell );
		ASSERT_EQ( cell, i );
		// if( i > 11 ) continue;
		tmp = -tmp;
		numeric::get_cell_48cell_half( tmp, cell );
		ASSERT_EQ( cell, i );
	}

	// ASSERT_EQ( V4(0,0,0,-1), t24h.row(7).transpose() );
	// for(int i = 0; i < 48; ++i) ASSERT_FLOAT_EQ( t24h.row(i).norm(), 1.0 );

	// std::ofstream out("test.pdb");
	// for(int i = 0; i < 24; ++i){
	// 	if( t24h.row(i).block(0,0,1,3).norm() < 0.9 )
	// 		io::dump_pdb_atom(out,i,10*t24h.row(i));
	// }
	// out.close();

	std::mt19937 mt((unsigned int)time(0));
	std::normal_distribution<> rnorm;
	std::uniform_real_distribution<> runif;

	int NITER = 200*1000;
	#ifdef SCHEME_BENCHMARK
		NITER *= 50;
	#endif

	std::vector<V4> samp(NITER);
	std::vector<Index> cell(NITER),cell2(NITER);

	for(int i = 0; i < NITER; ++i){
		V4 quat(rnorm(mt),rnorm(mt),rnorm(mt),rnorm(mt));
		samp[i] = quat.normalized();
	}
		// V4 const quat_pos = quat.cwiseAbs();

	util::Timer<> naive;
	for(int i = 0; i < NITER; ++i){
		// (t24h*samp[i]).cwiseAbs().maxCoeff(&cell[i]);
		(t24h*samp[i]).cwiseAbs().maxCoeff(&cell[i]);
	}
	cout << "hbt24 naive rate:  " << (Float)NITER / naive.elapsed_nano() << endl;

	util::Timer<> clever;
	for(int i = 0; i < NITER; ++i){
		numeric::get_cell_48cell_half( samp[i], cell2[i] );
	}
	cout << "hbt24 clever rate: " << (double)NITER / clever.elapsed_nano() << endl;

	for(int i = 0; i < NITER; ++i){
		// if( cell[i] > 11 ) continue;
		// if( cell[i] < 12 ) ASSERT_EQ( cell[i], cell2[i] );
		if( cell[i] != cell2[i] ){
			ASSERT_FLOAT_EQ(
			     t24h.row(cell [i]).dot(samp[i]) ,
			     t24h.row(cell2[i]).dot(samp[i]) );
		} else {
			ASSERT_EQ( cell[i], cell2[i] );
		}
	}

}

}}}


// (C) Copyright Jeremy Siek 2001.
// 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)
//
// Sample output:
//  mask =      101010101010
//  Enter a 12-bit bitset in binary: 100110101101
//  x =        100110101101
//  As ulong:  2477
//  And with mask: 100010101000
//  Or with mask:  101110101111

#include <iostream>
#include <boost/dynamic_bitset.hpp>

int main(int, char*[]) {
  const boost::dynamic_bitset<> mask(12, 2730ul);
  std::cout << "mask = " << mask << std::endl;

  boost::dynamic_bitset<> x(12);
  std::cout << "x.size()=" << x.size() << std::endl;

  std::cout << "Enter a 12-bit bitset in binary: " << std::flush;
  if (std::cin >> x) {
    std::cout << "input number:     " << x << std::endl;
    std::cout << "As unsigned long: " << x.to_ulong() << std::endl;
    std::cout << "And with mask:    " << (x & mask) << std::endl;
    std::cout << "Or with mask:     " << (x | mask) << std::endl;
    std::cout << "Shifted left:     " << (x << 1) << std::endl;
    std::cout << "Shifted right:    " << (x >> 1) << std::endl;
  }
  return EXIT_SUCCESS;
}


#include <iostream>
#include <vector>
#include <list>
#include "gtest/gtest.h"
#include <Eigen/Dense>
#include "hmm.h"

using namespace std;
using namespace rtHMM;

#include "hmm_example/hmm_matrices.h"

/* TODO:
 *
 * The following things still need test cases:
 *
 *   - if the predecessors are created correctly
 *   - if resetting a transition works correctly
 *   - if tied observation distributions are created correctly
 *
 */

TEST(hmm_constructor_tests, test_num_states)
{
    const size_t num_states = 3;
    hmm my_hmm(num_states);
    ASSERT_EQ(my_hmm.num_states(), num_states);
}

template<typename T, typename HMM_T>
void check_prior(const T& prior, const HMM_T& my_hmm)
{
    size_t i = 0;
    for (auto p : prior) {
        ASSERT_DOUBLE_EQ(my_hmm.prior(i), p);
        ++i;
    }
}

TEST(hmm_constructor_tests, test_prior_vector)
{
    const vector<double> prior{PRIOR};

    hmm my_hmm(prior);
    ASSERT_EQ(my_hmm.num_states(), prior.size());
    check_prior(prior, my_hmm);
}

TEST(hmm_constructor_tests, test_prior_list)
{
    const list<double> prior{PRIOR};

    hmm my_hmm(prior);
    ASSERT_EQ(my_hmm.num_states(), prior.size());
    check_prior(prior, my_hmm);
}

class hmm_test : public ::testing::Test {
    protected:
        virtual void SetUp() {

            dense_vector prior(STATE_COUNT);
            prior << PRIOR;
            dense_matrix trans(STATE_COUNT, STATE_COUNT);
            trans << TRANSITION;
            dense_matrix obs(STATE_COUNT, ALPHABET_SIZE);
            obs << OBSERVATION;

            simple_hmm = new hmm(prior, trans, obs);
        }

        virtual void TearDown() {
            delete simple_hmm;
        }

        hmm* simple_hmm;
};

TEST_F(hmm_test, prior_probability_test)
{
    double correct_prior_probs[] = {PRIOR};

    for (size_t i = 0; i < simple_hmm->num_states(); ++i) {
        ASSERT_DOUBLE_EQ(correct_prior_probs[i], simple_hmm->prior(i));
    }
}

TEST_F(hmm_test, successors_test)
{
    double correct_trans_probs[] = {TRANSITION};

    for (size_t i = 0; i < simple_hmm->num_states(); ++i) {
        size_t k = 0;
        auto& successors = simple_hmm->successors(i);

        for (size_t j = 0; j < simple_hmm->num_states(); ++j) {
            double correct_p = correct_trans_probs[i * STATE_COUNT + j];
            if (correct_p > 0.0) {
                auto& succ = successors[k];
                ASSERT_EQ(succ.state_id, j);
                ASSERT_DOUBLE_EQ(succ.probability, correct_p); ++k;
            }
        }
    }
}

TEST_F(hmm_test, observation_probability_test)
{
    double correct_observation_probs[] = {OBSERVATION};
    size_t obs[] = ALPHABET;

    for (size_t state_id = 0; state_id < simple_hmm->num_states(); ++state_id) {
        for (size_t obs_id = 0; obs_id < ALPHABET_SIZE; ++obs_id) {
            auto& dist = simple_hmm->observation_distribution(state_id);
            double correct_p = correct_observation_probs[state_id * ALPHABET_SIZE + obs_id];
            ASSERT_DOUBLE_EQ(correct_p, dist->probability(obs[obs_id]));
        }
    }
}


#ifndef MOCHIMOCHI_NHERD_HPP_
#define MOCHIMOCHI_NHERD_HPP_

#include <Eigen/Dense>
#include <boost/serialization/serialization.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/split_member.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <fstream>
#include <functional>
#include "../../functions/enumerate.hpp"

class NHERD {
private :
  const std::size_t kDim;
  const double kC;
  const int kDiagonal;

private :
  Eigen::VectorXd _covariances;
  Eigen::VectorXd _means;

private :
  std::function<double(double, double, double)> _compute_covariance;

public :
  NHERD(const std::size_t dim, const double C, const int diagonal = 0)
    : kDim(dim),
      kC(C),
      kDiagonal(diagonal),
      _covariances(Eigen::VectorXd::Ones(kDim)),
      _means(Eigen::VectorXd::Zero(kDim)) {

    static_assert(std::numeric_limits<decltype(dim)>::max() > 0, "Dimension Error. (Dimension > 0)");
    static_assert(std::numeric_limits<decltype(C)>::max() > 0, "Hyper Parameter Error. (C > 0)");

    // int diagonal : switching the diagonal covariance
    // 0 : Full covariance
    // 1 : Exact covariance
    // 2 : Project covariance
    // 3 : Drop covariance
    switch(kDiagonal) {
    case 0 :
      _compute_covariance = [=](const auto covariance, const auto confidence, const auto value) {
        const auto v = covariance * value;
        return covariance - (v * v * (kC * kC * confidence + 2 * kC) / std::pow((1.0 + kC * confidence), 2));
      };
      break;
    case 1 :
      _compute_covariance = [=](const auto covariance, const auto confidence, const auto value) {
        return covariance / std::pow(1.0 + kC * value * value * covariance, 2);
      };
      break;
    case 2 :
      _compute_covariance = [=](const auto covariance, const auto confidence, const auto value) {
        return 1.0 / ((1.0 / covariance) + (2 * kC + kC * kC * confidence) * value * value);
      };
      break;
    case 3 :
      _compute_covariance = [=](const auto covariance, const auto confidence, const auto value) {
        const auto v = (std::pow(covariance * value, 2) * (kC * kC * confidence + 2 * kC) / std::pow(1.0 + kC * confidence, 2));
        return covariance - v;
      };
      break;
    default:
      std::runtime_error("Error in switching the diagonal covariance.");
    }

  }

  virtual ~NHERD() { }

private :

  double suffer_loss(const double margin, const int label) const {
    return margin * label;
  }

  double compute_margin(const Eigen::VectorXd& x) const {
    return _means.dot(x);
  }

  double compute_confidence(const Eigen::VectorXd& feature) const {
    auto confidence = 0.0;
    functions::enumerate(feature.data(), feature.data() + feature.size(), 0,
                         [&](const int index, const double value) {
                           confidence += _covariances[index] * value * value;
                         });
    return confidence;
  }

public :

  bool update(const Eigen::VectorXd& feature, const int label) {
    const auto margin = compute_margin(feature);

    if (suffer_loss(margin, label) >= 1.0) { return false; }

    const auto confidence = compute_confidence(feature);
    const auto alpha = std::max(0.0, 1.0 - label * margin) / (confidence + 1 / kC) ;

    functions::enumerate(feature.data(), feature.data() + feature.size(), 0,
                       [&](const std::size_t index, const double value) {
                         _means[index] += alpha * label * _covariances[index] * value;
                         _covariances[index] = _compute_covariance(_covariances[index], confidence, value);
                       });
    return true;
  }

  int predict(const Eigen::VectorXd& x) const {
    return compute_margin(x) > 0.0 ? 1 : -1;
  }

  Eigen::VectorXd get_means(void) const {
    return _means;
  }

  void save(const std::string& filename) {
    std::ofstream ofs(filename);
    assert(ofs);
    boost::archive::text_oarchive oa(ofs);
    oa << *this;
    ofs.close();
  }

  void load(const std::string& filename) {
    std::ifstream ifs(filename);
    assert(ifs);
    boost::archive::text_iarchive ia(ifs);
    ia >> *this;
    ifs.close();
  }

private :
  friend class boost::serialization::access;
  BOOST_SERIALIZATION_SPLIT_MEMBER();
  template <class Archive>
  void save(Archive& ar, const unsigned int version) const {
    std::vector<double> covariances_vector(_covariances.data(), _covariances.data() + _covariances.size());
    std::vector<double> means_vector(_means.data(), _means.data() + _means.size());
    ar & boost::serialization::make_nvp("covariances", covariances_vector);
    ar & boost::serialization::make_nvp("means", means_vector);
    ar & boost::serialization::make_nvp("dimension", const_cast<std::size_t&>(kDim));
    ar & boost::serialization::make_nvp("C", const_cast<double&>(kC));
  }

  template <class Archive>
  void load(Archive& ar, const unsigned int version) {
    std::vector<double> covariances_vector;
    std::vector<double> means_vector;
    ar & boost::serialization::make_nvp("covariances", covariances_vector);
    ar & boost::serialization::make_nvp("means", means_vector);
    ar & boost::serialization::make_nvp("dimension", const_cast<std::size_t&>(kDim));
    ar & boost::serialization::make_nvp("C", const_cast<double&>(kC));
    _covariances = Eigen::Map<Eigen::VectorXd>(&covariances_vector[0], covariances_vector.size());
    _means = Eigen::Map<Eigen::VectorXd>(&means_vector[0], means_vector.size());
  }
};

#endif //MOCHIMOCHI_NHERD_HPP_


//  justanhduc
// Oct 2020

#include <Eigen/Core>
#include <cmath>
#include <ctime>

#include "TSDFVolume.hpp"
#include "GPURaycaster.hpp"
#include "opencv2/opencv.hpp"

#define MAX_PATH 260  // for linux

int main() {
    using namespace Eigen;

    const int dim_x = 101, dim_y = 139, dim_z = 106;
    const int n_max_frame = 30;
    const int width = 1024;
    const int height = 1024;
    const int n_samples = 3;  // for anti-aliasing. 1 == no anti-aliasing

    GPURaycaster r{width, height};
    Vector3f light_source{0, 3, 3};
    auto eye = Vector3f{0.5, 0, 2};  // view point
    Camera cam((float) width / 2, (float) height / 2, (float) (width - 1) / 2,
             (float) (height - 1) / 2);
    cam.move_to(eye);
    cam.look_at(0.5, 0.5, 0.5);

    TSDFVolume volume(dim_x, dim_y, dim_z, dim_x * .01, dim_y * .01, dim_z * .01);
    volume.set_truncation_distance(.1);  // set low threshold as the step size may be large

    char stmp[MAX_PATH];
    float ftemp[8];
    auto total_time = 0.;
    for (int i = 0; i < n_max_frame; i++) {
        // reading tsdf files
        auto *tsdf = new float[dim_x * dim_y * dim_z * n_max_frame];
        sprintf(stmp, "../volume/_tsdf_multi_%03d.bin", i);  // for linux
        FILE *fp;
        fp = fopen(stmp, "rb");  // for linux
        if (fp == nullptr) {
            printf("Cannot read file %s\n", stmp);
            exit(-1);
        }
        fread(ftemp, sizeof(float), 8, fp);
        fread(tsdf, dim_x * dim_y * dim_z, sizeof(float), fp);
        fclose(fp);

        // start
        auto start = std::time(nullptr);
        // set tsdf to volume
        volume.set_distance_data(tsdf);

        // define containers for normals and vertices
        Eigen::Matrix<float, 3, Eigen::Dynamic> vertices;
        Eigen::Matrix<float, 3, Eigen::Dynamic> normals;

        // raycast + shading + rendering
        auto *scene = new uint8_t[height * width];
        r.render_with_shading(volume, cam, vertices, normals, light_source, n_samples, scene);

        // done
        total_time += std::difftime(std::time(nullptr), start);

        // display
        cv::Mat frame(height, width, 0, scene);
        cv::imshow("Frame", frame);
        auto c = (char) cv::waitKey(1);
        if (c == 27)
            break;
    }

    std::cout << "FPS: " << 1 / (total_time / n_max_frame) << std::endl;
    return 0;
}


#include "quadrotor_simulator/Quadrotor.h"
#include <Eigen/Geometry>

int main(int argc, char **argv)
{
  double dt = 0.001;
  QuadrotorSimulator::Quadrotor quad;
  QuadrotorSimulator::Quadrotor::State state = quad.getState();

  const double m = quad.getMass();
  const double g = quad.getGravity();
  const double kf = quad.getPropellerThrustCoefficient();

  const double hover_rpm = std::sqrt(m*g/(4*kf));
  std::cerr << "hover rpm: " << hover_rpm << std::endl;
  state.motor_rpm = Eigen::Array4d(hover_rpm, hover_rpm, hover_rpm, hover_rpm);
  quad.setState(state);

  double thrust = m*g;
  double rpm = std::sqrt(thrust/(4*kf));
  quad.setInput(rpm, rpm, rpm, rpm);

  struct timespec ts_start, ts1, ts2, ts_sleep, ts_end;
  clock_gettime(CLOCK_MONOTONIC, &ts1);
  int64_t time_taken = 0;

  double KP = 8.0;
  double KD = 2.5;
  const double z_des = 0.5;
  clock_gettime(CLOCK_MONOTONIC, &ts_start);
  for(int i = 0; i < 6000; i++)
  {
    state = quad.getState();
    thrust = m*g + KP*(z_des - state.x(2)) + KD*(0 - state.v(2));
    rpm = std::sqrt(thrust/(4*kf));
    if( i < 3000)
      quad.setExternalForce(Eigen::Vector3d(0, 0, -KP*z_des));
    else
      quad.setExternalForce(Eigen::Vector3d(0, 0, 0));
    quad.setInput(rpm, rpm, rpm, rpm);
    quad.step(dt);
    Eigen::Vector3d euler = state.R.eulerAngles(2,1,0);
    std::cout << i*dt << ", " << state.x(2) << ", " << euler(0) << ", " << euler(1) << ", " << euler(2) << ", " <<
        state.omega(0) << ", " << state.omega(1) << ", " << state.omega(2) << ", " << state.motor_rpm(0) << std::endl;

    clock_gettime(CLOCK_MONOTONIC, &ts2);
    time_taken += ((ts2.tv_sec-ts1.tv_sec)*1000000000UL + (ts2.tv_nsec-ts1.tv_nsec));
    int64_t time_sleep = i*dt/2*1e9 - time_taken;
    clock_gettime(CLOCK_MONOTONIC, &ts1);
    if(time_sleep > 0)
    {
      ts_sleep.tv_sec = time_sleep/1000000000UL;
      ts_sleep.tv_nsec = time_sleep - ts_sleep.tv_sec*1000000000UL;
      //nanosleep(&ts_sleep, NULL);
    }
  }
  clock_gettime(CLOCK_MONOTONIC, &ts_end);
  std::cerr << "Time: " << (ts_end.tv_sec-ts_start.tv_sec)*1e6 + (ts_end.tv_nsec-ts_start.tv_nsec)/1e3 << " usec" <<std::endl;

  return 0;
}


#include "cuNDArray_math.h"
#include "cuPartialDerivativeOperator.h"
#include "osLALMSolver.h"
#include "cuSolverUtils.h"
#include <boost/program_options.hpp>
#include "hoNDArray_fileio.h"
#include "cuNlcgSolver.h"
#include "identityOperator.h"
using namespace Gadgetron;
typedef cuNDArray<float> ARRAY_TYPE;
typedef float REAL;
typedef std::vector<std::vector<boost::shared_ptr<linearOperator<cuNDArray<float >>>>> RG;

	void denoise(ARRAY_TYPE& x, ARRAY_TYPE& s, REAL scaling,REAL avg_lambda, int inner_iterations, RG& regularization_groups){
		std::cout << "scaling " << scaling << std::endl;
		REAL tau=1e-4;
		REAL gam=0.35/(scaling*avg_lambda);
		REAL sigma = 1;
		REAL alpha = 0;
		ARRAY_TYPE g(x.get_dimensions());

		for (auto it = 0u; it < inner_iterations; it++){
			clear(&g);

			for (auto & reg_group : regularization_groups){
				std::vector<ARRAY_TYPE> datas(reg_group.size());
				REAL val = 0;
				for (auto i = 0u; i < reg_group.size(); i++){
					datas[i] = ARRAY_TYPE(reg_group[i]->get_codomain_dimensions());
					reg_group[i]->mult_M(&x,&datas[i]);
					datas[i] *= sigma*reg_group[i]->get_weight()/avg_lambda;
				}
				//updateFgroup is the resolvent operators on the group
				updateFgroup(datas,alpha,sigma);

				for (auto i = 0u; i < reg_group.size(); i++){
					datas[i] *= reg_group[i]->get_weight()/avg_lambda;
					reg_group[i]->mult_MH(&datas[i],&g,true);

				}

			}
			//updateG is the resolvent operator on the |x-s| part of the optimization
	axpy(-tau,&g,&x);
			g = s;

			axpy(tau/(scaling*avg_lambda),&g,&x);

			//g = precon;
			fill(&g,1.0f);

			reciprocal_inplace(&g);
			g *= tau/(scaling*avg_lambda);
			g += REAL(1);
			x /= g;
			//x *= 1/(1+tau/(scaling*avg_lambda));
			REAL theta = 1/std::sqrt(1+2*gam*tau);
			tau *= theta;
			sigma /= theta;
		}

	};

namespace po = boost::program_options;

int main(int argc, char** argv){
	std::string filename;
	std::string outputFile;
	int iterations;
	float tv_weight;
	po::options_description desc("Allowed options");

	desc.add_options()
    				("help", "produce help message")
    				("filename,f",po::value<std::string>(&filename)->default_value("lena.real"),"input filename")
    				("output,o", po::value<std::string>(&outputFile)->default_value("reconstruction.real"), "Output filename")
    				("TV",po::value<float>(&tv_weight)->default_value(0),"Total variation weight")
    				("iterations,i",po::value<int>(&iterations)->default_value(10),"Denoising iterations")
    				;

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

	auto img = cuNDArray<float>(*read_nd_array<float>(filename.c_str()));
	img *= 1e-5f;
	auto dims = *img.get_dimensions();
	auto Dx = boost::make_shared<cuPartialDerivativeOperator<float,2>>(0);
	Dx->set_weight(tv_weight);
	Dx->set_domain_dimensions(&dims);
	Dx->set_codomain_dimensions(&dims);
	auto Dy = boost::make_shared<cuPartialDerivativeOperator<float,2>>(1);
	Dy->set_weight(tv_weight);
	Dy->set_domain_dimensions(&dims);
	Dy->set_codomain_dimensions(&dims);

	std::vector<boost::shared_ptr<linearOperator<cuNDArray<float>>>> reg_group({Dx,Dy});

	cuNDArray<float> out(img);

	RG group({reg_group});

	denoise(out,img,1.0f,tv_weight,iterations,group);

	/*
	cuNlcgSolver<float> solver;
	auto id = boost::make_shared<identityOperator<cuNDArray<float>>>();
	id->set_codomain_dimensions(&dims);
	id->set_domain_dimensions(&dims);
	solver.set_encoding_operator(id);

	solver.add_regularization_group_operator(Dx);
	solver.add_regularization_group_operator(Dy);
	solver.add_group(1);

	solver.set_max_iterations(iterations);
	auto out = solver.solve(&img);
*/
	write_nd_array(out.to_host().get(),outputFile.c_str());

}


// -*- mode: C++; c-indent-level: 4; c-basic-offset: 4; indent-tabs-mode: nil; -*-

#include <RcppArmadillo.h>
#include <random>
#include <boost/math/distributions/normal.hpp>
#include "myrng.h"

using namespace Rcpp;
using namespace RcppArmadillo;
using namespace boost::math;
// [[Rcpp::depends(RcppArmadillo)]]
// [[Rcpp::depends(BH)]]
// [[Rcpp::plugins(cpp11)]]

// accesses myrng.h to make a random uniform variable
// [[Rcpp::export]]
arma::colvec crunif(unsigned int n, unsigned int seed) {
    Uniform unif(seed);
    arma::colvec out(n, arma::fill::none);
    for(auto i = 0; i < n; i++) {
        out[i] = unif.draw();
    }
    return out;
}

// Pseudocode 2 of Finley et al. 2019
// [[Rcpp::export]]
Rcpp::List csolve_for_A_and_D(arma::sp_mat& cov_cur,
                              Rcpp::List& neighbor_list) {
    int s = cov_cur.n_rows;
    arma::sp_mat A = arma::sp_mat(s, s);
    arma::sp_mat D = arma::sp_mat(s, s);
    arma::uvec N;
    int sizen;
    D[0] = cov_cur[0];

    for(auto i = 0; i < (s-1); i++) {
        // minus 1 because 0 vs 1-based indexing
        N = as<arma::uvec>(neighbor_list[i+1]) - 1;
        sizen = N.size()-1;
        A.row(i+1).cols(N[0], N[sizen]) = arma::solve(arma::mat(cov_cur.cols(N[0], N[sizen]).rows(N[0], N[sizen])), arma::colvec(cov_cur.col(i+1).rows(N[0], N[sizen]))).t();
        D.row(i+1).col(i+1) = cov_cur.col(i+1).row(i+1) - dot(arma::rowvec(cov_cur.cols(N[0], N[sizen]).row(i+1)), A.cols(N[0], N[sizen]).row(i+1));
    }
    return Rcpp::List::create(
        Rcpp::Named("A") = A,
        Rcpp::Named("D") = D);
}

// [[Rcpp::export]]
double csparse_quadratic_form_symm(arma::colvec& u,
                                      arma::sp_mat& A,
                                      arma::colvec& D,
                                      Rcpp::List& neighbor_list) {
    double tmp = pow(u[0], 2) / D[0];
    int n = u.n_elem;
    arma::uvec N;
    int sizen;
    for(auto i = 1; i < n; i++) {
        N = as<arma::uvec>(neighbor_list[i]) - 1;
        sizen = N.size()-1;
        tmp += pow(u[i] - arma::dot(A.cols(N[0], N[sizen]).row(i), u.elem(N).t()), 2) / D[i];
    }
    return(tmp);
}

// [[Rcpp::export]]
double csparse_quadratic_form_asymm(arma::colvec u,
                                    arma::colvec v,
                                    arma::sp_mat A,
                                    arma::colvec D,
                                    Rcpp::List neighbor_list) {
    double tmp = u[0] * v[0] / D[0];
    int n = u.n_elem;
    arma::uvec N;
    int sizen;
    for(auto i = 1; i < n; i++) {
        N = as<arma::uvec>(neighbor_list[i]) - 1;
        sizen = N.size()-1;
        tmp += (u[i] - arma::dot(A.cols(N[0], N[sizen]).row(i), u.elem(N).t())) *
            (v[i] - arma::dot(A.cols(N[0], N[sizen]).row(i), v.elem(N).t())) / D[i];
    }
    return(tmp);
}

// [[Rcpp::export]]
Rcpp::List csolve_for_B_and_b(arma::mat& y,
                              arma::mat& X,
                              arma::sp_mat& A,
                              arma::colvec& D,
                              Rcpp::List& neighbor_list,
                              arma::mat& precision_beta) {
    int p = X.n_cols;
    arma::colvec b(p, arma::fill::ones);
    arma::mat B(p, p, arma::fill::ones);
    arma::colvec ybar = mean(y, 1);

    for(auto i = 0; i < p; i++) {
        b[i] = csparse_quadratic_form_asymm(X.col(i), ybar, A, D, neighbor_list);
        for(auto j = 0; j < p; j++) {
            B.col(j).row(i) = csparse_quadratic_form_asymm(X.col(i), X.col(j), A, D, neighbor_list);
        }
    }

    return(Rcpp::List::create(
            Rcpp::Named("B") = B + precision_beta,
            Rcpp::Named("b") = b)
               );
}

// [[Rcpp::export]]
arma::mat cmake_one_pred_sparse(Rcpp::List& neighbor_list,
                                arma::mat& y,
                                arma::mat& s,
                                arma::mat& X,
                                arma::mat& cond_cov,
                                int BOOT,
                                unsigned int SEED) {
    int n = y.n_rows;
    arma::mat L;

    arma::mat uniform_rv;
    uniform_rv = arma::reshape(crunif(n * BOOT, SEED), n, BOOT);
    arma::mat z(n, BOOT, arma::fill::none);
    arma::mat preds(n, BOOT, arma::fill::none);
    normal standard_normal;

    for(auto i = 0; i < n; i++) {
        for(auto j = 0; j < BOOT; j++) {
            z(i,j) = quantile(standard_normal, uniform_rv(i,j));
        }
    }

    arma::uvec Nidx;
    int sizen;
    preds.row(0) = sqrt(cond_cov[0]) * z.row(0);
    for(auto i = 1; i < n; i++) {
        Nidx = as<arma::uvec>(neighbor_list[i]) - 1;
        sizen = Nidx.size()-1;
        L = arma::chol(cond_cov.cols(Nidx[0], Nidx[sizen]).rows(Nidx[0], Nidx[sizen]), "upper");
        preds.row(i) = L.col(L.n_cols-1).t() * (z.rows(Nidx[0], Nidx[sizen]));
    }

    return(preds);
}

// [[Rcpp::export]]
Rcpp::List csolve_for_A_and_D_2d(arma::sp_mat& cov_cur,
                                 Rcpp::List& neighbor_list) {
    int s = cov_cur.n_rows;
    arma::mat cov_cur_dense = arma::mat(cov_cur);

    arma::mat A = arma::mat(s, s);
    arma::sp_mat D = arma::sp_mat(s, s);
    arma::uvec N;

    D[0] = cov_cur[0];
    arma::uvec v = { 1 };
    for(auto i = 0; i < (s-1); i++) {
        // minus 1 because 0 vs 1-based indexing
        N = as<arma::uvec>(neighbor_list[i+1]) - 1;
        A.submat(v, N) = arma::solve(cov_cur_dense.submat(N, N), arma::colvec(cov_cur_dense.submat(N, v))).t();
        D.row(i+1).col(i+1) = cov_cur_dense.col(i+1).row(i+1) - dot(arma::rowvec(cov_cur_dense.submat(v, N)), A.submat(v, N));
        v = v+1;
    }

    return Rcpp::List::create(
        Rcpp::Named("A") = arma::sp_mat(A),
        Rcpp::Named("D") = D);
}

// [[Rcpp::export]]
double csparse_quadratic_form_symm_2d(arma::colvec& u,
                                   arma::sp_mat& A,
                                   arma::colvec& D,
                                   Rcpp::List& neighbor_list) {
    double tmp = pow(u[0], 2) / D[0];
    int n = u.n_elem;
    arma::uvec N;
    int sizen;

    arma::mat A_dense = arma::mat(A);
    arma::uvec v = { 1 };
    for(auto i = 1; i < n; i++) {
        N = as<arma::uvec>(neighbor_list[i]) - 1;
        sizen = N.size()-1;
        tmp += pow(u[i] - arma::dot(A_dense.submat(v, N), u.elem(N).t()), 2) / D[i];
        v = v+1;
    }
    return(tmp);
}

// [[Rcpp::export]]
double csparse_quadratic_form_asymm_2d(arma::colvec u,
                                    arma::colvec v,
                                    arma::sp_mat A,
                                    arma::colvec D,
                                    Rcpp::List neighbor_list) {
    double tmp = u[0] * v[0] / D[0];
    int n = u.n_elem;
    arma::uvec N;
    int sizen;

    arma::mat A_dense = arma::mat(A);
    arma::uvec vv = { 1 };
    for(auto i = 1; i < n; i++) {
        N = as<arma::uvec>(neighbor_list[i]) - 1;
        sizen = N.size()-1;
        tmp += (u[i] - arma::dot(A_dense.submat(vv, N), u.elem(N).t())) *
            (v[i] - arma::dot(A_dense.submat(vv, N), v.elem(N).t())) / D[i];
        vv = vv+1;
    }
    return(tmp);
}

// [[Rcpp::export]]
Rcpp::List csolve_for_B_and_b_2d(arma::mat& y,
                              arma::mat& X,
                              arma::sp_mat& A,
                              arma::colvec& D,
                              Rcpp::List& neighbor_list,
                              arma::mat& precision_beta) {
    int p = X.n_cols;
    arma::colvec b(p, arma::fill::ones);
    arma::mat B(p, p, arma::fill::ones);
    arma::colvec ybar = mean(y, 1);

    for(auto i = 0; i < p; i++) {
        b[i] = csparse_quadratic_form_asymm_2d(X.col(i), ybar, A, D, neighbor_list);
        for(auto j = 0; j < p; j++) {
            B.col(j).row(i) = csparse_quadratic_form_asymm_2d(X.col(i), X.col(j), A, D, neighbor_list);
        }
    }

    return(Rcpp::List::create(
            Rcpp::Named("B") = B + precision_beta,
            Rcpp::Named("b") = b)
    );
}