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

#include <hpp/fcl/internal/intersect.h>
#include <hpp/fcl/collision.h>
#include <hpp/fcl/BVH/BVH_model.h>
#include "fcl_resources/config.h"
#include <sstream>

using namespace hpp::fcl;

static FCL_REAL epsilon = 1e-6;

static bool approx(FCL_REAL x, FCL_REAL y)
{
  return std::abs(x - y) < epsilon;
}

BOOST_AUTO_TEST_CASE(projection_test_line)
{
  Vec3f v1(0, 0, 0);
  Vec3f v2(2, 0, 0);

  Vec3f p(1, 0, 0);
  Project::ProjectResult res = Project::projectLine(v1, v2, p);
  BOOST_CHECK(res.encode == 3);
  BOOST_CHECK(approx(res.sqr_distance, 0));
  BOOST_CHECK(approx(res.parameterization[0], 0.5));
  BOOST_CHECK(approx(res.parameterization[1], 0.5));

  p = Vec3f(-1, 0, 0);
  res = Project::projectLine(v1, v2, p);
  BOOST_CHECK(res.encode == 1);
  BOOST_CHECK(approx(res.sqr_distance, 1));
  BOOST_CHECK(approx(res.parameterization[0], 1));
  BOOST_CHECK(approx(res.parameterization[1], 0));

  p = Vec3f(3, 0, 0);
  res = Project::projectLine(v1, v2, p);
  BOOST_CHECK(res.encode == 2);
  BOOST_CHECK(approx(res.sqr_distance, 1));
  BOOST_CHECK(approx(res.parameterization[0], 0));
  BOOST_CHECK(approx(res.parameterization[1], 1));

}

BOOST_AUTO_TEST_CASE(projection_test_triangle)
{
  Vec3f v1(0, 0, 1);
  Vec3f v2(0, 1, 0);
  Vec3f v3(1, 0, 0);

  Vec3f p(1, 1, 1);
  Project::ProjectResult res = Project::projectTriangle(v1, v2, v3, p);
  BOOST_CHECK(res.encode == 7);
  BOOST_CHECK(approx(res.sqr_distance, 4/3.0));
  BOOST_CHECK(approx(res.parameterization[0], 1/3.0));
  BOOST_CHECK(approx(res.parameterization[1], 1/3.0));
  BOOST_CHECK(approx(res.parameterization[2], 1/3.0));

  p = Vec3f(0, 0, 1.5);
  res = Project::projectTriangle(v1, v2, v3, p);
  BOOST_CHECK(res.encode == 1);
  BOOST_CHECK(approx(res.sqr_distance, 0.25));
  BOOST_CHECK(approx(res.parameterization[0], 1));
  BOOST_CHECK(approx(res.parameterization[1], 0));
  BOOST_CHECK(approx(res.parameterization[2], 0));

  p = Vec3f(1.5, 0, 0);
  res = Project::projectTriangle(v1, v2, v3, p);
  BOOST_CHECK(res.encode == 4);
  BOOST_CHECK(approx(res.sqr_distance, 0.25));
  BOOST_CHECK(approx(res.parameterization[0], 0));
  BOOST_CHECK(approx(res.parameterization[1], 0));
  BOOST_CHECK(approx(res.parameterization[2], 1));

  p = Vec3f(0, 1.5, 0);
  res = Project::projectTriangle(v1, v2, v3, p);
  BOOST_CHECK(res.encode == 2);
  BOOST_CHECK(approx(res.sqr_distance, 0.25));
  BOOST_CHECK(approx(res.parameterization[0], 0));
  BOOST_CHECK(approx(res.parameterization[1], 1));
  BOOST_CHECK(approx(res.parameterization[2], 0));

  p = Vec3f(1, 1, 0);
  res = Project::projectTriangle(v1, v2, v3, p);
  BOOST_CHECK(res.encode == 6);
  BOOST_CHECK(approx(res.sqr_distance, 0.5));
  BOOST_CHECK(approx(res.parameterization[0], 0));
  BOOST_CHECK(approx(res.parameterization[1], 0.5));
  BOOST_CHECK(approx(res.parameterization[2], 0.5));

  p = Vec3f(1, 0, 1);
  res = Project::projectTriangle(v1, v2, v3, p);
  BOOST_CHECK(res.encode == 5);
  BOOST_CHECK(approx(res.sqr_distance, 0.5));
  BOOST_CHECK(approx(res.parameterization[0], 0.5));
  BOOST_CHECK(approx(res.parameterization[1], 0));
  BOOST_CHECK(approx(res.parameterization[2], 0.5));

  p = Vec3f(0, 1, 1);
  res = Project::projectTriangle(v1, v2, v3, p);
  BOOST_CHECK(res.encode == 3);
  BOOST_CHECK(approx(res.sqr_distance, 0.5));
  BOOST_CHECK(approx(res.parameterization[0], 0.5));
  BOOST_CHECK(approx(res.parameterization[1], 0.5));
  BOOST_CHECK(approx(res.parameterization[2], 0));
}

BOOST_AUTO_TEST_CASE(projection_test_tetrahedron)
{
  Vec3f v1(0, 0, 1);
  Vec3f v2(0, 1, 0);
  Vec3f v3(1, 0, 0);
  Vec3f v4(1, 1, 1);

  Vec3f p(0.5, 0.5, 0.5);
  Project::ProjectResult res = Project::projectTetrahedra(v1, v2, v3, v4, p);
  BOOST_CHECK(res.encode == 15);
  BOOST_CHECK(approx(res.sqr_distance, 0));
  BOOST_CHECK(approx(res.parameterization[0], 0.25));
  BOOST_CHECK(approx(res.parameterization[1], 0.25));
  BOOST_CHECK(approx(res.parameterization[2], 0.25));
  BOOST_CHECK(approx(res.parameterization[3], 0.25));

  p = Vec3f(0, 0, 0);
  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
  BOOST_CHECK(res.encode == 7);
  BOOST_CHECK(approx(res.sqr_distance, 1/3.0));
  BOOST_CHECK(approx(res.parameterization[0], 1/3.0));
  BOOST_CHECK(approx(res.parameterization[1], 1/3.0));
  BOOST_CHECK(approx(res.parameterization[2], 1/3.0));
  BOOST_CHECK(approx(res.parameterization[3], 0));

  p = Vec3f(0, 1, 1);
  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
  BOOST_CHECK(res.encode == 11);
  BOOST_CHECK(approx(res.sqr_distance, 1/3.0));
  BOOST_CHECK(approx(res.parameterization[0], 1/3.0));
  BOOST_CHECK(approx(res.parameterization[1], 1/3.0));
  BOOST_CHECK(approx(res.parameterization[2], 0));
  BOOST_CHECK(approx(res.parameterization[3], 1/3.0));

  p = Vec3f(1, 1, 0);
  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
  BOOST_CHECK(res.encode == 14);
  BOOST_CHECK(approx(res.sqr_distance, 1/3.0));
  BOOST_CHECK(approx(res.parameterization[0], 0));
  BOOST_CHECK(approx(res.parameterization[1], 1/3.0));
  BOOST_CHECK(approx(res.parameterization[2], 1/3.0));
  BOOST_CHECK(approx(res.parameterization[3], 1/3.0));

  p = Vec3f(1, 0, 1);
  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
  BOOST_CHECK(res.encode == 13);
  BOOST_CHECK(approx(res.sqr_distance, 1/3.0));
  BOOST_CHECK(approx(res.parameterization[0], 1/3.0));
  BOOST_CHECK(approx(res.parameterization[1], 0));
  BOOST_CHECK(approx(res.parameterization[2], 1/3.0));
  BOOST_CHECK(approx(res.parameterization[3], 1/3.0));

  p = Vec3f(1.5, 1.5, 1.5);
  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
  BOOST_CHECK(res.encode == 8);
  BOOST_CHECK(approx(res.sqr_distance, 0.75));
  BOOST_CHECK(approx(res.parameterization[0], 0));
  BOOST_CHECK(approx(res.parameterization[1], 0));
  BOOST_CHECK(approx(res.parameterization[2], 0));
  BOOST_CHECK(approx(res.parameterization[3], 1));

  p = Vec3f(1.5, -0.5, -0.5);
  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
  BOOST_CHECK(res.encode == 4);
  BOOST_CHECK(approx(res.sqr_distance, 0.75));
  BOOST_CHECK(approx(res.parameterization[0], 0));
  BOOST_CHECK(approx(res.parameterization[1], 0));
  BOOST_CHECK(approx(res.parameterization[2], 1));
  BOOST_CHECK(approx(res.parameterization[3], 0));

  p = Vec3f(-0.5, -0.5, 1.5);
  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
  BOOST_CHECK(res.encode == 1);
  BOOST_CHECK(approx(res.sqr_distance, 0.75));
  BOOST_CHECK(approx(res.parameterization[0], 1));
  BOOST_CHECK(approx(res.parameterization[1], 0));
  BOOST_CHECK(approx(res.parameterization[2], 0));
  BOOST_CHECK(approx(res.parameterization[3], 0));

  p = Vec3f(-0.5, 1.5, -0.5);
  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
  BOOST_CHECK(res.encode == 2);
  BOOST_CHECK(approx(res.sqr_distance, 0.75));
  BOOST_CHECK(approx(res.parameterization[0], 0));
  BOOST_CHECK(approx(res.parameterization[1], 1));
  BOOST_CHECK(approx(res.parameterization[2], 0));
  BOOST_CHECK(approx(res.parameterization[3], 0));

  p = Vec3f(0.5, -0.5, 0.5);
  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
  BOOST_CHECK(res.encode == 5);
  BOOST_CHECK(approx(res.sqr_distance, 0.25));
  BOOST_CHECK(approx(res.parameterization[0], 0.5));
  BOOST_CHECK(approx(res.parameterization[1], 0));
  BOOST_CHECK(approx(res.parameterization[2], 0.5));
  BOOST_CHECK(approx(res.parameterization[3], 0));

  p = Vec3f(0.5, 1.5, 0.5);
  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
  BOOST_CHECK(res.encode == 10);
  BOOST_CHECK(approx(res.sqr_distance, 0.25));
  BOOST_CHECK(approx(res.parameterization[0], 0));
  BOOST_CHECK(approx(res.parameterization[1], 0.5));
  BOOST_CHECK(approx(res.parameterization[2], 0));
  BOOST_CHECK(approx(res.parameterization[3], 0.5));

  p = Vec3f(1.5, 0.5, 0.5);
  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
  BOOST_CHECK(res.encode == 12);
  BOOST_CHECK(approx(res.sqr_distance, 0.25));
  BOOST_CHECK(approx(res.parameterization[0], 0));
  BOOST_CHECK(approx(res.parameterization[1], 0));
  BOOST_CHECK(approx(res.parameterization[2], 0.5));
  BOOST_CHECK(approx(res.parameterization[3], 0.5));

  p = Vec3f(-0.5, 0.5, 0.5);
  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
  BOOST_CHECK(res.encode == 3);
  BOOST_CHECK(approx(res.sqr_distance, 0.25));
  BOOST_CHECK(approx(res.parameterization[0], 0.5));
  BOOST_CHECK(approx(res.parameterization[1], 0.5));
  BOOST_CHECK(approx(res.parameterization[2], 0));
  BOOST_CHECK(approx(res.parameterization[3], 0));

  p = Vec3f(0.5, 0.5, 1.5);
  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
  BOOST_CHECK(res.encode == 9);
  BOOST_CHECK(approx(res.sqr_distance, 0.25));
  BOOST_CHECK(approx(res.parameterization[0], 0.5));
  BOOST_CHECK(approx(res.parameterization[1], 0));
  BOOST_CHECK(approx(res.parameterization[2], 0));
  BOOST_CHECK(approx(res.parameterization[3], 0.5));

  p = Vec3f(0.5, 0.5, -0.5);
  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
  BOOST_CHECK(res.encode == 6);
  BOOST_CHECK(approx(res.sqr_distance, 0.25));
  BOOST_CHECK(approx(res.parameterization[0], 0));
  BOOST_CHECK(approx(res.parameterization[1], 0.5));
  BOOST_CHECK(approx(res.parameterization[2], 0.5));
  BOOST_CHECK(approx(res.parameterization[3], 0));

}


#define BOOST_TEST_MODULE "test_static_vector"

#ifdef UNITTEST_FRAMEWORK_LIBRARY_EXIST
#include <boost/test/unit_test.hpp>
#else
#define BOOST_TEST_NO_LIB
#include <boost/test/included/unit_test.hpp>
#endif

#include "../src/Vector.hpp"
template<int N>
using VectorNd = ax::Vector<double, N>;

#include "test_Defs.hpp"
using ax::test::seed;
using ax::test::tolerance;

#include <random>

BOOST_AUTO_TEST_CASE(VectorNd_Constructable)
{
    const VectorNd<10> vec;

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec[i], 0e0);

    const VectorNd<10> vec_1(1e0);

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec_1[i], 1e0);

    const VectorNd<10> vec_cp_0(vec_1);

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec_cp_0[i], 1e0);

    const VectorNd<10> vec_cp_1 = vec_1;

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec_cp_1[i], 1e0);

    const VectorNd<10> vec_2(0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec_2[i], static_cast<double>(i));
}

BOOST_AUTO_TEST_CASE(VectorNd_Add)
{
    const VectorNd<10> vec1(1e0);
    const VectorNd<10> vec2(2e0);
    const VectorNd<10> vec3(vec1 + vec2);

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec3[i], 3e0);

    VectorNd<10> vec4;
    vec4 = vec1 + vec2;

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec4[i], 3e0);

    VectorNd<10> vec5;
    vec5 += vec1;
    vec5 += vec2;

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec5[i], 3e0);
}

BOOST_AUTO_TEST_CASE(VectorNd_Sub)
{
    const VectorNd<10> vec1(3e0);
    const VectorNd<10> vec2(2e0);
    const VectorNd<10> vec3(vec1 - vec2);

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec3[i], 1e0);

    VectorNd<10> vec4;
    vec4 = vec1 - vec2;

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec4[i], 1e0);

    VectorNd<10> vec5(vec1);
    vec5 -= vec2;

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec5[i], 1e0);
}

BOOST_AUTO_TEST_CASE(VectorNd_Scalar_multiple)
{
    const VectorNd<10> vec1(1e0);
    const VectorNd<10> vec2(2e0 * vec1);

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec2[i], 2e0);

    const VectorNd<10> vec3(vec1 * 2e0);

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec3[i], 2e0);

    VectorNd<10> vec4;
    vec4 = vec1 * 2e0;

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec4[i], 2e0);

    vec4 = 2e0 * vec1;

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec4[i], 2e0);

    VectorNd<10> vec5(vec1);
    vec5 *= 2e0;

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec5[i], 2e0);
}

BOOST_AUTO_TEST_CASE(VectorNd_Scalar_division)
{
    const VectorNd<10> vec1(2e0);
    const VectorNd<10> vec2(vec1 / 2e0);

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec2[i], 1e0);

    VectorNd<10> vec3;
    vec3 = vec1 / 2e0;

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec3[i], 1e0);

    VectorNd<10> vec4(vec1);
    vec4 /= 2e0;

    for(std::size_t i=0; i<10; ++i)
        BOOST_CHECK_EQUAL(vec4[i], 1e0);
}

BOOST_AUTO_TEST_CASE(VectorNd_dot_product)
{
    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> randreal(0e0, 1e0);

    for(auto i=0; i<100; ++i)
    {
        std::array<double, 10> v1;
        for(std::size_t i=0; i<10; ++i)
            v1.at(i) = randreal(mt);

        std::array<double, 10> v2;
        for(std::size_t i=0; i<10; ++i)
            v2.at(i) = randreal(mt);

        const VectorNd<10> vec1(v1);
        const VectorNd<10> vec2(v2);

        double dot_product = 0e0;
        for(std::size_t i=0; i<10; ++i)
            dot_product += v1.at(i) * v2.at(i);

        BOOST_CHECK_CLOSE_FRACTION(dot_prod(vec1, vec2), dot_product, tolerance);
    }
}

BOOST_AUTO_TEST_CASE(VectorNd_length)
{
    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> randreal(0e0, 1e0);

    for(auto i=0; i<100; ++i)
    {
        std::array<double, 10> v1;
        for(std::size_t i=0; i<10; ++i)
            v1.at(i) = randreal(mt);

        const VectorNd<10> vec1(v1);

        double lensq = 0e0;
        for(std::size_t i=0; i<10; ++i)
            lensq += v1.at(i) * v1.at(i);

        BOOST_CHECK_CLOSE_FRACTION(len_square(vec1), lensq, tolerance);
        BOOST_CHECK_CLOSE_FRACTION(length(vec1), std::sqrt(lensq), tolerance);

        BOOST_CHECK_CLOSE_FRACTION(len_square(vec1), dot_prod(vec1, vec1), tolerance);
    }
}


#include <NTL/ZZ.h>

NTL_CLIENT

#define CHECK(x) do { if (!(x)) { cerr << "FAIL\n"; return -1; } } while(0)

int main()
{
   ZZ seed;
   RandomLen(seed, 30);
   SetSeed(seed);
   cerr << "\nseed=" << seed << "\n";

   cerr << "\nvalidating RandomLen...";
   for (long i = 1; i < 10000; i++) {
      ZZ x;
      RandomLen(x, i);
      CHECK(x.validate() && NumBits(x) == i);
   }

   cerr << "\nvalidating basic arithmetic...";
   for (long i = 0; i < 200000; i++) {
      long a_len = RandomBnd(8000)+5;
      long b_len = RandomBnd(8000)+5;
      long c_len = RandomBnd(8000)+5;
      long d_len = RandomBnd(8000)+5;

      ZZ a, b, c, d;
      RandomLen(a, a_len);
      RandomLen(b, b_len);
      RandomLen(c, c_len);
      RandomLen(d, d_len);

      ZZ t1, t2;
      t1 = (a-b)*(c-d);
      t2 = a*c - a*d - b*c + b*d;
      CHECK(t1.validate() && t2.validate() && t1 == t2);

      long p = 7919;

      long d1 = rem(t1, p);
      long d2 = MulMod(rem(a-b, p), rem(c-d, p), p);
      CHECK(d1 == d2);
   }

   cerr << "\nvalidating DivRem...";
   for (long i = 0; i < 200000; i++) {
      long b_len = RandomBnd(8000)+5;
      long q_len = RandomBnd(8000)+5;

      ZZ a, b, q, r, q1, r1;
      RandomLen(b, b_len);
      RandomLen(q, q_len);
      RandomBnd(r, b);
      a = b*q + r;

      DivRem(q1, r1, a, b);

      CHECK(q1.validate() && r1.validate() && q == q1 && r == r1);
   }

   cerr << "\nvalidating squaring...";
   for (long i = 0; i < 200000; i++) {
      long a_len = RandomBnd(8000)+5;

      ZZ a, b, a1, c;
      RandomLen(a, a_len);

      sqr(b, a);
      a1 = a;
      mul(c, a, a1);

      CHECK(b.validate() && c.validate() && b == c);
   }

   cerr << "\nvalidating SqrRoot...";
   for (long i = 0; i < 200000; i++) {
      long a_len = RandomBnd(8000)+5;

      ZZ a, b;
      RandomLen(a, a_len);

      SqrRoot(b, a);
      CHECK(b.validate() && b*b <= a && (b+1)*(b+1) > a);
   }

   cerr << "\nvalidating shifts...";
   for (long i = 0; i < 200000; i++) {
      long a_len = RandomBnd(5000)+5;
      long shamt = RandomBnd(a_len+100);

      ZZ a;
      RandomLen(a, a_len);

      ZZ t = ZZ(1);
      for (long k = 0; k < shamt; k++) t += t;

      ZZ xL, xR;
      LeftShift(xL, a, shamt);
      RightShift(xR, a, shamt);

      CHECK(xL.validate() && xR.validate());
      CHECK(xL == a*t && xR == a/t);
   }

   cerr << "\nvalidating Preconditioned Remainder...";
   for (long i = 0; i < 1000000; i++) {
      sp_ZZ_reduce_struct red_struct;

      long p_len = RandomBnd(NTL_SP_NBITS-1)+2;
      long p = RandomLen_long(p_len);

      long a_len = RandomBnd(30000)+5;
      ZZ a;
      RandomLen(a, a_len);

      red_struct.build(p);
      long r1 = red_struct.rem(a);
      long r2 = rem(a, p);

      CHECK(r1 == r2);
   }

   cerr << "\nvalidating MulAddTo...";
   for (long i = 0; i < 1000000; i++) {
      long a_len = RandomBnd(4000)+5;
      long b_len = RandomBnd(4000)+5;
      long c_len = RandomBnd(4000)+5;
      long d_len = RandomBnd(4000)+5;

      ZZ a, b, c, d;
      RandomLen(a, a_len);
      RandomLen(b, b_len);
      RandomLen(c, c_len);
      RandomLen(d, d_len);

      ZZ t1, t2;
      t1 = a-b;
      t2 = c-d;

      long s_len, s;
      s_len = RandomBnd(NTL_NSP_NBITS)+1;
      s = RandomLen_long(s_len);
      if (RandomBnd(2)) s = -s;

      ZZ r1, r2;
      r1 = t1;
      MulAddTo(r1, t2, s);

      r2 = t1 + t2*s;
      CHECK(r1.validate() && r2.validate() && r1 == r2);
   }

   cerr << "\nvalidating GCD...";
   for (long i = 0; i < 100000; i++) {
      long a_len = RandomBnd(4000)+5;
      long b_len = RandomBnd(4000)+5;
      long c_len = RandomBnd(500)+1;

      ZZ a, b, c;
      RandomLen(a, a_len);
      RandomLen(b, b_len);
      RandomLen(c, c_len);

      a *= c;
      b *= c;

      ZZ d, s, t, d1;

      XGCD(d, s, t, a, b);
      GCD(d1, a, b);

      CHECK(d.validate() && s.validate() && t.validate() && d1.validate());
      CHECK(d == d1 && d == a*s + b*t);
      CHECK(divide(a, d) && divide(b, d));
   }

   cerr << "\nvalidating InvMod...";
   for (long i = 0; i < 100000; i++) {
      long n_len = RandomBnd(4000)+5;

      ZZ a, n, x;
      RandomLen(n, n_len);
      RandomBnd(a, n);

      long r = InvModStatus(x, a, n);
      CHECK((r == 0 && (x * a) % n == 1) || (r == 1 && x != 1 && x == GCD(a, n)) );
   }

   cerr << "\nvalidating RatRecon...";

   // This exercises RatRecon using the example from Section 4.6.1
   // in A Computational Introduction to Number Theory

   for (long i = 0; i < 100000; i++) {
      long m_len = RandomBnd(4000)+5;

      ZZ m;
      RandomLen(m, m_len);

      ZZ t;
      RandomBnd(t, m);
      t += 1;

      ZZ s;
      RandomBnd(s, t);

      ZZ bnd = 2*m*m;

      long k = 0;
      ZZ ten_k = ZZ(1);
      while (ten_k <= bnd) { ten_k *= 10; k++; }

      ZZ z = (s*ten_k)/t;

      ZZ a, r, b;
      long res = ReconstructRational(r, b, z, ten_k, m, m);

      CHECK(res == 1);

      a = (b*z - r)/ten_k;
      CHECK(a*t == b*s);
   }

   cerr << "\n";

   return 0;
}


#ifndef INCLUDED_STDDEFX
#include "stddefx.h"
#define INCLUDED_STDDEFX
#endif

#ifndef INCLUDED_BOOST_MATH_SPECIAL_FUNCTIONS_ROUND
#include <boost/math/special_functions/round.hpp>
#define INCLUDED_BOOST_MATH_SPECIAL_FUNCTIONS_ROUND
#endif

#ifndef INCLUDED_GEOM_SIZE
#include "geom_size.h"
#define INCLUDED_GEOM_SIZE
#endif

#ifndef INCLUDED_GEOM_RECTANGLE
#include "geom_rectangle.h"
#define INCLUDED_GEOM_RECTANGLE
#endif

#ifndef INCLUDED_GEOM_UTIL
#include "geom_util.h"
#define INCLUDED_GEOM_UTIL
#endif

/*!
  This function returns a rectangle based on the size of \a maxSize and the
  ratio of \a width and \a height. The new rectangle will have the same aspect
  as \a maxSize, but might be different in size.
*/
geom_Size<int> geom_Util::adjustWidthOrHeight(const geom_Size<int> &maxSize,
                                         double width, double height)
{
  geom_Size<int> adjustedSize(maxSize);

  double widthRatio  = maxSize.getWidth()  / width;
  double heightRatio = maxSize.getHeight() / height;

  widthRatio < heightRatio
    ? adjustedSize.setHeight(static_cast<int>(
                   maxSize.getWidth() * height / width))
    : adjustedSize.setWidth(static_cast<int>(
                   maxSize.getHeight() * width / height));

  return adjustedSize;
}

/*!
  This function returns a rectangle based on the size of \a size and the ratio
  of \a width and \a height. The new rectangle will have the same aspect as
  \a widht and \a height. The size of \a size will be adjusted (enlarged) to
  meet these requirements.
*/
geom::Rectangle<int> geom_Util::reAspect(const geom::Rectangle<int> &size,
                                        double width, double height)
{
  geom::Rectangle<int> newSize = size;
  double widthRatio           = width  / (double)size.width();
  double heightRatio          = height / (double)size.height();

  if(widthRatio < heightRatio)
  {
    int newHeight = boost::math::iround((size.width() * height) / width);
    newSize.setHeight(newHeight);
  }
  else
  {
    int newWidth = boost::math::iround((size.height() * width) / height);
    newSize.setWidth(newWidth);
  }

  return newSize;
}


#pragma once
// matrix_generators.hpp: generators for special matrices
//
// Copyright (C) 2017-2020 Stillwater Supercomputing, Inc.
//
// This file is part of the HPRBLAS project, which is released under an MIT Open Source license.
#include <cstdint>
#include <random>
#include <algorithm>
#include <boost/numeric/mtl/mtl.hpp>
#include <universal/functions/binomial.hpp>

namespace sw {
namespace hprblas {

// fill a dense matrix with random values between [lowerbound, upperbound]
template <typename Matrix>
void uniform_rand(Matrix& A, double lowerbound = 0.0, double upperbound = 1.0)
{
	// Use random_device to generate a seed for Mersenne twister engine.
	std::random_device rd{};
	// Use Mersenne twister engine to generate pseudo-random numbers.
	std::mt19937 engine{ rd() };
	// "Filter" MT engine's output to generate pseudo-random double values,
	// **uniformly distributed** on the closed interval [lowerbound, upperbound].
	// (Note that the range is [inclusive, inclusive].)
	std::uniform_real_distribution<double> dist{ lowerbound, upperbound };
	// Pattern to generate pseudo-random number.
	// double rnd_value = dist(engine);

	typedef typename mtl::Collection<Matrix>::value_type    value_type;
	typedef typename mtl::Collection<Matrix>::size_type     size_type;

	// inserters add to the elements, so we need to set the value to 0 before we begin
	A = 0.0;
	{ // extra block unfortunately needed for VS2013
		// Create inserter for matrix m
		mtl::mat::inserter<Matrix> ins(A, num_cols(A));

		// generate and insert random values in A
		for (size_type r = 0; r < num_rows(A); r++) {
			for (size_type c = 0; c < num_cols(A); c++) {
				ins[r][c] << value_type(dist(engine));
			}
		}
		// Destructor of ins sets final state of m
	}
}

// create a dense matrix with random values between [lowerbound, upperbound]
template<typename Scalar>
mtl::mat::dense2D<Scalar> uniform_rand(size_t m, size_t n, double lowerbound = 0.0, double upperbound = 1.0) {
	mtl::mat::dense2D<Scalar> A(m, n);
	uniform_rand(A, lowerbound, upperbound);
	return A;
}

// fill a dense MTL matrix with random values between [lowerbound, upperbound]
template <typename Matrix>
void uniform_rand_sorted(Matrix& A, double lowerbound = 0.0, double upperbound = 1.0)
{
	// Use random_device to generate a seed for Mersenne twister engine.
	std::random_device rd{};
	// Use Mersenne twister engine to generate pseudo-random numbers.
	std::mt19937 engine{ rd() };
	// "Filter" MT engine's output to generate pseudo-random double values,
	// **uniformly distributed** on the closed interval [lowerbound, upperbound].
	// (Note that the range is [inclusive, inclusive].)
	std::uniform_real_distribution<double> dist{ lowerbound, upperbound };
	// Pattern to generate pseudo-random number.
	// double rnd_value = dist(engine);

	typedef typename mtl::Collection<Matrix>::value_type    value_type;
	typedef typename mtl::Collection<Matrix>::size_type     size_type;

	// generate a good set of randoms
	std::vector<value_type> v(size(A));
	for (size_type r = 0; r < num_rows(A); ++r) {
		for (size_type c = 0; c < num_cols(A); ++c) {
			v.push_back(value_type(dist(engine)));
		}
	}
	// sort them so that we have better control over the scale of each element in a row vector
	sort(v.begin(), v.end(), std::greater<value_type>());

	// for each row minus the last column, calculate the sum of elements without rounding
	sw::unum::posit<value_type::nbits, value_type::es> one(1), p;
	for (size_type r = 0; r < num_rows(A); ++r) {
		sw::unum::quire<value_type::nbits, value_type::es> q1, q2;
		size_type lastElement = num_cols(A) - 1;
		for (size_type c = 0; c < lastElement; ++c) {
			q1 += sw::unum::quire_mul(one, v[r*num_cols(A) + c]);
		}
		// truncate the value in the quire
		convert(q1.to_value(), p);
		// calculate the difference between the truncated and the non-truncated quire values
		q2 = p;
//				std::cout << "q1 :" << q1 << std::endl;
//				std::cout << "q2 :" << q2 << std::endl;
		q2 -= q1;
		convert(q2.to_value(), p);
//				std::cout << "Residual is: " << double(p) << std::endl;
//		if (p.iszero()) p = one;
		v[r*num_cols(A) + lastElement] = p;
	}

	// inserters add to the elements, so we need to set the value to 0 before we begin
	A = 0;
	{ // extra block unfortunately needed for VS2013
		// Create inserter for matrix m
		mtl::mat::inserter<Matrix> ins(A, num_cols(A));

		// insert sorted values in A
		size_type i = 0;
		for (size_type r = 0; r < num_rows(A); r++) {
			for (size_type c = 0; c < num_cols(A); c++) {
				ins[r][c] << v[i++];
			}
		}
		// Destructor of ins sets final state of m
	}
}

// fill a dense MTL matrix with diagonally dominant random values between [lowerbound, upperbound]
template<typename Matrix>
void uniform_rand_diagonally_dominant(Matrix& A, double lowerbound = 0.0, double upperbound = 1.0) {
	// generate off-diagonal entries, calculate the sum, scale to upperbound, and set diagonal entry to slightly larger

	// Use random_device to generate a seed for Mersenne twister engine.
	std::random_device rd{};
	// Use Mersenne twister engine to generate pseudo-random numbers.
	std::mt19937 engine{ rd() };
	// "Filter" MT engine's output to generate pseudo-random double values,
	// **uniformly distributed** on the closed interval [lowerbound, upperbound].
	// (Note that the range is [inclusive, inclusive].)
	std::uniform_real_distribution<double> dist{ lowerbound, upperbound };
	// Pattern to generate pseudo-random number.
	// double rnd_value = dist(engine);

	typedef typename mtl::Collection<Matrix>::value_type    value_type;
	typedef typename mtl::Collection<Matrix>::size_type     size_type;

	// no need to null A as each element is assigned explicitely
	for (size_type r = 0; r < num_rows(A); ++r) {
		// generate a random vector of N
		size_type N = num_cols(A);
		mtl::dense_vector<value_type> v(N);
		for (size_type c = 0; c < N; ++c) {
			v[c] = value_type(dist(engine));
		}
		// add the sum of the other row elements to the diagonal
		for (size_type c = 0; c < N; ++c) {
			if (r != c) v[r] += v[c];
		}
		value_type factor = v[r] / upperbound;
		for (size_type c = 0; c < N; ++c) {
			A[r][c] = v[c] / factor;
		}
	}
}

// Random Orthogonal Matrices

/*
Standard methods for generating random orthogonal matrices with Haar distribution
are based on the method of Heiberger (1978). With this method, an (n x n) matrix A is
first generated with entries xij ~ Normal(0; 1). Then a QR factorization (X = QR) is computed.
This method provides a random Q with correct distribution.
*/
template<typename Matrix>
void uniform_random_orthogonal_Heiberger(Matrix& Q) {
	// Use random_device to generate a seed for Mersenne twister engine.
	std::random_device rd{};
	// Use Mersenne twister engine to generate pseudo-random numbers.
	std::mt19937 engine{ rd() };
	// "Filter" MT engine's output to generate pseudo-random double values,
	// **uniformly distributed** on the closed interval [lowerbound, upperbound].
	// (Note that the range is [inclusive, inclusive].)
	std::uniform_real_distribution<double> dist{ 0.0, 1.0 };
	// Pattern to generate pseudo-random number.
	// double rnd_value = dist(engine);

	typedef typename mtl::Collection<Matrix>::value_type    value_type;
	typedef typename mtl::Collection<Matrix>::size_type     size_type;
	Matrix A(mtl::mat::num_rows(Q), mtl::mat::num_cols(Q));
	Matrix R(mtl::mat::num_rows(Q), mtl::mat::num_cols(Q));

	// fill X with elements from Normal(0,1)
	// inserters add to the elements, so we need to set the value to 0 before we begin
	A = 0.0;
	{ // extra block unfortunately needed for VS2013
		// Create inserter for matrix A
		mtl::mat::inserter<Matrix> ins(A, num_cols(A));

		// insert random values in A
		for (size_type r = 0; r < num_rows(A); r++) {
			for (size_type c = 0; c < num_cols(A); c++) {
				ins[r][c] << value_type(dist(engine));
			}
		}
		// Destructor of ins sets final state of A
	}
	// QR method to generate orthonormal matrix Q
	mtl::mat::qr(A, Q, R);
	std::cout << A << std::endl;
	std::cout << Q << std::endl;
	std::cout << R << std::endl;
}
/*
C.3: Generating a Random Matrix with Specified Eigenvalues
Generate random orthogonal matrix G. W. Stewart (1980).

start RandOrthog(n);
A = I(n); // identity matrix
d = j(n, 1, 0);
d[n] = sgn(RndNormal(1, 1)); // +/- 1
do k = n - 1 to 1 by - 1;
	// generate random Householder transformation
	x = RndNormal(n - k + 1, 1); // column vector from N(0,1)
	s = sqrt(x[##]); // norm(x)
	sgn = sgn(x[1]);
	s = sgn*s;
	d[k] = -sgn;
	x[1] = x[1] + s;
	beta = s*x[1];
	// apply the Householder transformation to A
	y = x`*A[k:n, ];
	A[k:n, ] = A[k:n, ] - x*(y / beta);
end;
A = d # A; // change signs of i_th row when d[i]=-1
return(A);
finish;

// helper functions
// return matrix of same size as A with
// m[i,j]= {  1 if A[i,j]>=0
//         { -1 if A[i,j]< 0
// Similar to the SIGN function, except SIGN(0)=0
start sgn(A);
	return(choose(A >= 0, 1, -1));
finish;

// return (r x c) matrix of standard normal variates
start RndNormal(r, c);
	x = j(r, c);
	call randgen(x, "Normal");
	return(x);
finish;

// The following statements call the RANDORTHOG function to generate a random 4x4 orthogonal matrix.
call randseed(1);
Q = RandOrthog(4);
print(Q);
*/
template<typename Matrix>
void uniform_rand_orthogonal(Matrix& A) {

}

//
// fill a dense upper-triangular matrix with elements: [i][j] = 0.5 + (1 + j - i)*0.5
template <typename Matrix>
void fill_U(Matrix& A, double lowerbound = 0.0, double upperbound = 1.0)
{
	using namespace mtl;
	typedef typename Collection<Matrix>::value_type    value_type;
	typedef typename Collection<Matrix>::size_type     size_type;

	// inserters add to the elements, so we need to set the value to 0 before we begin
	A = 0.0;
	{ // extra block unfortunately needed for VS2013
		// Create inserter for matrix m
		mat::inserter<Matrix> ins(A, num_cols(A));

		// generate and insert random values in A
		for (size_type r = 0; r < num_rows(A); r++) {
			for (size_type c = r; c < num_cols(A); c++) {
				ins[r][c] << value_type(0.5) + value_type(1 + c - r)*value_type(0.5);
				ins[r][c] << value_type(1.0);
			}
		}
		// Destructor of ins sets final state of m
	}
}

// fill a dense lower-triangular matrix with elements: [i][j] = 0.5 + (1 + i - j)*0.5
template <typename Matrix>
void fill_L(Matrix& A, double lowerbound = 0.0, double upperbound = 1.0)
{
	using namespace mtl;
	typedef typename Collection<Matrix>::value_type    value_type;
	typedef typename Collection<Matrix>::size_type     size_type;

	// inserters add to the elements, so we need to set the value to 0 before we begin
	A = 0.0;
	{ // extra block unfortunately needed for VS2013
		// Create inserter for matrix m
		mat::inserter<Matrix> ins(A, num_cols(A));

		// generate and insert random values in A
		for (size_type r = 0; r < num_rows(A); r++) {
			for (size_type c = 0; c <= r; c++) {
				ins[r][c] << value_type(0.5) + value_type(1 + r - c)*value_type(0.5);
				ins[r][c] << value_type(1.0);

			}
		}
		// Destructor of ins sets final state of m
	}
}

///////////////////////////////////////////////////////////////////////////////////////////
/// Hilbert matrices

// Greatest Common Divisor of two numbers, a and b
template<typename IntegerType>
IntegerType gcd(IntegerType a, IntegerType b)
{
	if (b == IntegerType(0))	return a;
	return gcd(b, a % b);
}

// Least Common Multiple of n numbers
template<typename IntegerType>
IntegerType findlcm(const std::vector<IntegerType>& v)
{
	IntegerType lcm = v[0];
	for (size_t i = 1; i < v.size(); i++) {
		lcm = (v[i] * lcm) / gcd(v[i], lcm);
	}
	return lcm;
}

// Generate the scaling factor of a Hilbert matrix so that its elements are representable
// that is, no infinite expensions of rationals, such as 1/3, 1/10, etc.
template<typename IntegerType>
IntegerType HilbertScalingFactor(IntegerType N) {
	std::vector<IntegerType> coef;
	for (IntegerType i = 2; i <= N; ++i) coef.push_back(i);
	for (IntegerType j = 2; j <= N; ++j) coef.push_back(N + j - IntegerType(1));
	return findlcm(coef);
}

// Generate a scaled/unscaled Hilbert matrix depending on the bScale parameter
template<typename Scalar>
size_t GenerateHilbertMatrix(mtl::mat::dense2D<Scalar>& M, bool bScale = true) {
	assert(M.num_rows() == M.num_cols());
	size_t N = M.num_rows();
	size_t lcm = HilbertScalingFactor(N); // always calculate the Least Common Multiplier
	Scalar scale = bScale ? Scalar(lcm) : Scalar(1);
	for (int i = 1; i <= N; ++i) {
		for (int j = 1; j <= N; ++j) {
			M[i - 1][j - 1] = scale / Scalar(i + j - 1);
		}
	}
	return lcm;
}

template<typename Scalar>
void GenerateHilbertMatrixInverse(mtl::mat::dense2D<Scalar>& m, Scalar scale = Scalar(1.0)) {
	assert(m.num_rows() == m.num_cols());
	size_t N = m.num_rows();
	for (int i = 1; i <= N; ++i) {
		for (int j = 1; j <= N; ++j) {
			Scalar sign = ((i + j) % 2) ? Scalar(-1) : Scalar(1);
			Scalar factor1 = Scalar(i + j - 1);
			Scalar factor2 = Scalar(sw::function::binomial<uint64_t>(N + i - 1, N - j));
			Scalar factor3 = Scalar(sw::function::binomial<uint64_t>(N + j - 1, N - i));
			Scalar factor4 = Scalar(sw::function::binomial<uint64_t>(i + j - 2, i - 1));
			m[i - 1][j - 1] = Scalar(sign * factor1 * factor2 * factor3 * factor4 * factor4);
			/* for tracing dynamic range failures
			std::cout << "element " << i << "," << j << std::endl;
			std::cout << "sign    " << sign << std::endl;
			std::cout << "factor1 " << factor1 << std::endl;
			std::cout << "factor2 " << factor2 << std::endl;
			std::cout << "factor3 " << factor3 << std::endl;
			std::cout << "factor4 " << factor4 << std::endl;
			*/
		}
	}
}

// isEqual compares to matrices
template<typename Matrix>
bool isEqual(const Matrix& lhs, const Matrix& rhs) {
	size_t r = lhs.num_rows();
	size_t c = lhs.num_cols();
	for (size_t i = 0; i < r; ++i) {
		for (size_t j = 0; j < c; ++j) {
			if (lhs[i][j] != rhs[i][j]) return false;
		}
	}
	return true;
}

} // namespace hprblas
} // namespace sw


#ifndef STAN_MATH_PRIM_MAT_FUN_MDIVIDE_RIGHT_HPP
#define STAN_MATH_PRIM_MAT_FUN_MDIVIDE_RIGHT_HPP

#include <boost/math/tools/promotion.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/mat/fun/mdivide_left.hpp>
#include <stan/math/prim/mat/fun/transpose.hpp>
#include <stan/math/prim/mat/err/check_multiplicable.hpp>
#include <stan/math/prim/mat/err/check_square.hpp>

namespace stan {
  namespace math {

    /**
     * Returns the solution of the system Ax=b.
     * @param A Matrix.
     * @param b Right hand side matrix or vector.
     * @return x = b A^-1, solution of the linear system.
     * @throws std::domain_error if A is not square or the rows of b don't
     * match the size of A.
     */
    template <typename T1, typename T2, int R1, int C1, int R2, int C2>
    inline
    Eigen::Matrix<typename boost::math::tools::promote_args<T1, T2>::type,
                  R1, C2>
    mdivide_right(const Eigen::Matrix<T1, R1, C1> &b,
                  const Eigen::Matrix<T2, R2, C2> &A) {
      check_square("mdivide_right", "A", A);
      check_multiplicable("mdivide_right", "b", b, "A", A);
      // FIXME: This is nice and general but likely slow.
      return transpose(mdivide_left(transpose(A), transpose(b)));
      //      return promote_common<Eigen::Matrix<T1, R2, C2>,
      //                            Eigen::Matrix<T2, R2, C2> >(A)
      //        .transpose()
      //        .lu()
      //        .solve(promote_common<Eigen::Matrix<T1, R1, C1>,
      //                              Eigen::Matrix<T2, R1, C1> >(b)
      //               .transpose())
      //        .transpose();
    }

  }
}
#endif


/*
  Copyright (c) 2018-2019 Alexander A. Ganin. All rights reserved.
  Twitter: @alxga. Website: alexganin.com.
  Licensed under the MIT License.
  See LICENSE file in the project root for full license information.
*/

#include "stdafx.h"
#include "Utils/rand01.h"

#ifdef HAVE_BOOST

#include <boost/random.hpp>
#include <boost/random/uniform_01.hpp>
using namespace boost;
using namespace boost::random;

static double boost_rand_0_1()
{
  static mt19937 eng(((unsigned int) time(NULL)) * rand());
  static uniform_01<> dist;
  static variate_generator<mt19937 &, uniform_01<>> var(eng, dist);
  return var();
}

#endif

// Minimal random number generator of Park and Miller with Bays-Durham shuffle and added
// safeguards. Returns a uniform random deviate between 0.0 and 1.0 (exclusive of the endpoint
// values). Call with idum a negative integer to initialize; thereafter, do not alter idum between
// successive deviates in a sequence. RNMX should approximate the largest floating value that is
// less than 1

#define IA 16807
#define IM 2147483647
#define AM (1.0/IM)
#define IQ 127773
#define IR 2836
#define NTAB 32
#define NDIV (1+(IM-1)/NTAB)
#define EPS 1.2e-7
#define RNMX (1.0-EPS)

static double park_miller_rand_0_1(long *idum)
{
  int j;
  long k;
  static long iy = 0;
  static long iv[NTAB];
  double temp;

  if (*idum <= 0 || !iy)
  {
    // Initialize
    if (-(*idum) < 1)
      *idum = 1; // Be sure to prevent idum = 0
    else
      *idum = -(*idum);

    for (j = NTAB + 7; j >= 0; j--)
    { // Load the shuffle table (after 8 warm-ups)
      k = (*idum) / IQ;
      *idum = IA * (*idum - k * IQ) - IR * k;
      if (*idum < 0) *idum += IM;
      if (j < NTAB) iv[j] = *idum;
    }
    iy = iv[0];
  }

  k = (*idum) / IQ;  // Start here when not initializing
  *idum = IA * (*idum - k * IQ) - IR*k; // Compute idum = (IA * idum) % IM without overlows by Schrage's method
  if (*idum < 0)
    *idum += IM;

  j = iy / NDIV;  // Will be in the range 0 ... NTAB-1
  iy = iv[j]; // Output previously stored value and refill the  shuffle table
  iv[j] = *idum;
  if ((temp = AM * iy) > RNMX)
    return RNMX; // Because users don't expect endpoint values
  else
    return temp;
}

double Rand01::generate()
{
  // return boost_rand_0_1();
  static long seed = -rand();
  return park_miller_rand_0_1(&seed);
}


#include <skylark.hpp>
#include <boost/mpi.hpp>
#include <elemental.hpp>
#include <iostream>
#include "../base/QR.hpp"
#include <cfloat>
#include <vector>

/** Aliases for matrix types */
typedef elem::DistMatrix<double> dist_matrix_t;
typedef elem::Matrix<double> matrix_t;
typedef elem::DistMatrix<double, elem::VR, elem::STAR> vr_star_dist_matrix_t;
typedef elem::DistMatrix<double, elem::STAR, elem::STAR> star_star_matrix_t;
typedef skylark::sketch::JLT_t<dist_matrix_t, dist_matrix_t> sketch_transform_t;

using namespace std;

int main(int argc, char* argv[]) {

   /** Initialize MPI  */
#ifdef SKYLARK_HAVE_OPENMP
    int provided;
    MPI_Init_thread(&argc, &argv, MPI_THREAD_MULTIPLE, &provided);
#endif
    boost::mpi::environment env(argc, argv);
    boost::mpi::communicator world;
    MPI_Comm mpi_world(world);
    elem::Grid grid(mpi_world);

    /** Initialize Elemental */
    elem::Initialize (argc, argv);

    /** Initialize context */
    skylark::base::context_t context(0);

    /** Declare matrices */
    dist_matrix_t A(grid), B(grid), C(grid);
    elem::Uniform(B, 5000, 100);
    skylark::base::qr::Explicit(B);

    elem::Uniform(C, 100, 100);
    skylark::base::qr::Explicit(C);

    //star_star_matrix_t S(100,100);
    dist_matrix_t S(100,100);
    elem::Zero(S);

    vector<double> diag(100);

    for( int j=0; j<100; ++j )
    {
	diag[j] = exp(-j)*100;
        std::cout << exp(-j) *100 << "\n";
    }

    elem::Diagonal(S, diag);
    dist_matrix_t tmp(grid);

    elem::Gemm(elem::NORMAL, elem::NORMAL, double(1), B, S, tmp);
    elem::Gemm(elem::NORMAL, elem::ADJOINT, double(1), tmp, C, A);

    dist_matrix_t U(grid), V(grid);
    vr_star_dist_matrix_t S1;

    dist_matrix_t A1(A);
    elem::SVD(A1,S1,V);

    elem::Print(S1, "S1");

    /** Declare matrices */
    dist_matrix_t A2(A);
    dist_matrix_t U1(grid), V1(grid);
    vr_star_dist_matrix_t S2;

    int sketch_size	=	50;
    int target_rank	=	10;

    skylark::nla::rand_svd_params_t params(sketch_size-target_rank);

    skylark::nla::randsvd_t<skylark::sketch::JLT_t> rand_svd;
    rand_svd(A2, target_rank, U1, S2, V1, params, context);

    elem::Print(S2, "S2");

    elem::Finalize();
    return 0;
}


#include <boost/math/constants/constants.hpp>
#include "servicecontainer.h"

double compile_time_expression_structure::serviceContainer::degrees(double rad)
{
    constexpr double pi = boost::math::constants::pi<double>();
    return rad * 180.0 / pi;
}

double compile_time_expression_structure::serviceContainer::radians(double deg)
{
    constexpr double pi = boost::math::constants::pi<double>();
    return deg * pi / 180.0;
}

// const std::map<compile_time_expression_structure::constantId, double>
// compile_time_expression_structure::serviceContainer::global_constants
// {
//     {constantId::pi, boost::math::constants::pi<double>()},
//     {constantId::e, boost::math::constants::e<double>()}
// };
//
// const std::map<std::string, compile_time_expression_structure::constantId>
// compile_time_expression_structure::serviceContainer::global_constants_id
// {
//     {"pi", constantId::pi},
//     {"e", constantId::e}
// };
//
//
// const std::map<std::string, compile_time_expression_structure::variableId>
// compile_time_expression_structure::serviceContainer::global_variables_id
// {
//     {"x", variableId::x},
//     {"y", variableId::y},
//     {"r", variableId::r}
// };


/*
 * 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 cauchy_distribution.hpp
 * \date August 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#pragma once

#include <Eigen/Dense>

#include <fl/util/types.hpp>
#include <fl/distribution/t_distribution.hpp>

namespace fl
{

/**
 * \ingroup distributions
 *
 * \brief CauchyDistribution represents a multivariate cauchy distribution. It
 * is a special case of student's t-distribution and is equal to
 * \f$t_1(\mu, \Sigma)\f$.
 */
template <typename Variate>
class CauchyDistribution
    : public TDistribution<Variate>
{
public:
    /**
     * Creates a dynamic or fixed size multivariate cauchy distribution
     *
     * \param dimension Dimension of the distribution. The default is defined by
     *                  the dimension of the variable type \em Vector. If the
     *                  size of the Variate at compile time is fixed, this will
     *                  be adapted. For dynamic-sized Variable the dimension is
     *                  initialized to 0.
     */
    explicit CauchyDistribution(int dim = DimensionOf<Variate>())
        : TDistribution<Variate>(Real(1), dim)
    { }
};

}


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

#include "stdafx.h"
#include <vector>
#include <boost/array.hpp>
#include <assert.h>
#include <iostream>

//you need boost to compile
#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/shared_ptr.hpp>

#pragma region ServiceFunctions
template<typename Collection>
__inline void normalizeL2(Collection& collection)
{
	typedef typename Collection::value_type value_type;
	value_type summ = std::accumulate(collection.begin(), collection.end(), (value_type)0, [](value_type & a, value_type b)->value_type{a = a + sqr(b); return a; });
	summ = sqrt(summ);
	if (summ < std::numeric_limits<value_type>::epsilon())
		return;
	std::for_each(collection.begin(), collection.end(), [summ](value_type& val)
	{
		val /= summ;
	});
}

template<typename OutArg, typename VecIn1, typename VecIn2>
__inline OutArg dotProduct(const VecIn1& a, const VecIn2& b)
{
	OutArg prod = 0;
	for (int i = 0; i < (int)a.size(); i++)
	{
		prod += static_cast<OutArg>(a[i]) * static_cast<OutArg>(b[i]);
	}
	return prod;
}
template<typename Type>
__inline Type sqr(Type t)
{
	return t * t;
}
double selectRandom(double a, double b, int range)
{
	return a + (b - a) *(rand() % range) / (range - 1);
}

struct NormalRandomGenerator
{
public:
	typedef boost::normal_distribution<double> NormalDistribution;
	typedef boost::variate_generator<boost::mt19937&, NormalDistribution > VarGen;
	typedef boost::shared_ptr<VarGen> VarGenPtr;
public:
	NormalRandomGenerator() :
		m_normalDist(0.0, 1.0)
	{
		m_varNor.reset(new VarGen(m_rng, m_normalDist));
	}
	void setParams(double  middle, double sigma)
	{
		m_normalDist = boost::normal_distribution<double>(middle, sigma);
		m_varNor.reset(new VarGen(m_rng, m_normalDist));
	}
	double generate()
	{

		return (*m_varNor)();
	}
private:
	NormalDistribution m_normalDist;
	VarGenPtr m_varNor;
	boost::mt19937 m_rng;
};
#pragma endregion ServiceFunctions

template<int dim>
struct FastIterativePcaSelector
{
	typedef boost::array<float, dim> Element;
	typedef std::vector<Element> Elements;
	typedef std::vector<float> Weights;

	Element getFirstPC(const Elements& elements, const Weights& weights, int numIterations, const Elements& prevBazis)
	{
		//sds we maximze projections on space with size 2
		std::vector<Element> currentVecs(2);
		for (int i1 = 0; i1 < 2; i1++)
		{
			if (i1 == 0)
				currentVecs[i1] = selectRandomVec(prevBazis);
			else
			{
				currentVecs[i1] = getRandomOrthogonalVec(currentVecs[0], prevBazis);
			}
		}
		double bestSigma = 0;
		for (int i1 = 0; i1 < numIterations; i1++)
		{
			Element currentApproximation = getApproximation(currentVecs.data(), elements, weights);
			Element newVec = getRandomOrthogonalVec(currentApproximation, prevBazis);
			currentVecs[0] = currentApproximation;
			currentVecs[1] = newVec;
			double sigma = calcSigma(elements, weights, currentApproximation);
			//stop criterion -- stabilization
			if (abs(sigma - bestSigma) < sigma * 0.00000001)
				break;

			std::cout << "num component " << prevBazis.size() << " " << "current sigma " << sigma << std::endl;
		}
		return currentVecs[0];
	}
	static double calcSigma(const Elements& elements, const Weights& weights, const Element& element)
	{
		double sigma = 0;
		for (int i1 = 0; i1 < (int)elements.size(); i1++)
		{
			sigma += sqr(dotProduct<float>(elements[i1], element))* weights[i1];
		}
		return sigma;
	}
private:

	static Element selectRandomVec(const Elements& prevBazis)
	{
		//select random vector, orthogonal to previous got bazis
		Element result;
		float dp = 0;

		//try new vector until it has some orthogonal part to previous bazis
		do
		{
			for (int i1 = 0; i1 < (int)result.size(); i1++)
			{
				result[i1] = (float)selectRandom(-1.0f, 1.0f, 1000);
			}
			for (int i1 = 0; i1 < (int)prevBazis.size(); i1++)
			{
				float dp = dotProduct<float>(result, prevBazis[i1]);
				for (int i2 = 0; i2 < (int)result.size(); i2++)
				{
					result[i2] = result[i2] - prevBazis[i1][i2] * dp;
				}
			}
			normalizeL2(result);
			dp = dotProduct<float>(result, result);
		} while (dp < 0.99);
		return result;
	}
	static Element getRandomOrthogonalVec(const Element& vec, const Elements& prevBazis)
	{

		Element result;
		float dp = 0;
		//try new vector until it has some orthogonal part to previous part and current best vector (vec)
		do
		{
			result = selectRandomVec(prevBazis);
			dp = dotProduct<float>(result, vec);
			for (int i1 = 0; i1 < (int)result.size(); i1++)
			{
				result[i1] = result[i1] - vec[i1] * dp;
			}
			normalizeL2(result);
			dp = dotProduct<float>(result, result);
		} while (dp < 0.5f);
		return result;
	}

	Element getApproximation(const Element* approximationStack, const Elements& elements, const Weights& weights)
	{
		/*
		Maximize decomposition to R2 space spanned on two vectors .
		2 Plum -- you could use int D(w) * H(w) dw instead
		*/

		//fill covariation matrix
		double matrix[4] = { 0., 0., 0., 0. };
		for (int i0 = 0; i0 < (int)weights.size(); i0++)
		{
			float dps[2] = { dotProduct<float>(approximationStack[0], elements[i0]), dotProduct<float>(approximationStack[1], elements[i0]) };
			for (int i1 = 0; i1 < 2; i1++)
			{
				for (int i2 = 0; i2 < 2; i2++)
				{
					matrix[i1 * 2 + i2] += dps[i1] * dps[i2] * weights[i0];
				}
			}
		}

		//calc eigen vectors/values
		double offset = (matrix[0] + matrix[3]);
		double d = std::max(sqr(matrix[0] - matrix[3]) + 4 * (matrix[1] * matrix[2]), 0.);
		d = sqrt(d);
		double eV = offset < 0 ? offset / 2.0 - d / 2.0 : offset / 2.0 + d / 2.0;
		float coeffs[2];
		if (abs(matrix[0] - eV) > abs(matrix[3] - eV))
		{
			coeffs[0] = static_cast<float>(matrix[1] / (matrix[0] - eV));
			coeffs[1] = -1.f;
		}
		else
		{
			coeffs[0] = -1.f;
			coeffs[1] = static_cast<float>(matrix[2] / (matrix[3] - eV));
		}
		//make decomposition
		Element result;
		for (int i1 = 0; i1 < (int)approximationStack[0].size(); i1++)
		{
			result[i1] = approximationStack[0][i1] * coeffs[0] + approximationStack[1][i1] * coeffs[1];
		}
		normalizeL2(result);
		return result;
	}

};
//test function shows, how to use
struct FastIterativePcaSelectorTest
{
	static const int numDimensions = 10000;
	typedef FastIterativePcaSelector<numDimensions> FiPC;
	typedef FiPC::Element Element;
	typedef FiPC::Elements Elements;
	void test()
	{
		std::vector<double> sigmas(numDimensions);
		std::vector<boost::shared_ptr<NormalRandomGenerator> > normalDistributions(numDimensions);
		//generate some random distribution
		for (int i1 = 0; i1 < (int)sigmas.size(); i1++)
		{
			sigmas[i1] = 0.2 + (rand() & 0xff)*(200.0 - 0.2) / 255.0;
			normalDistributions[i1].reset(new NormalRandomGenerator());
			normalDistributions[i1]->setParams(0, sigmas[i1]);
		}
		int maxElt = std::max_element(sigmas.begin(), sigmas.end()) - sigmas.begin();
		int numElements = 200;
		std::vector<Element> elements(numElements);
		//assign all weights to 1
		std::vector<float> weights(numElements);
		for (int i1 = 0; i1 < (int)elements.size(); i1++)
		{
			for (int i2 = 0; i2 < numDimensions; i2++)
			{
				elements[i1][i2] = (float)normalDistributions[i2]->generate();
			}
			weights[i1] = 1.0;
		}
		//calc reference sigma along main axis
		Element bestElt;
		std::fill(bestElt.begin(), bestElt.end(), 0.f);
		bestElt[maxElt] = 1.0;

		double bestSigma = FiPC().calcSigma(elements, weights, bestElt);
		std::cout << bestSigma << std::endl;
		Elements bazis;
		Element middle;
		std::fill(middle.begin(), middle.end(), 1.0f);
		normalizeL2(middle);
		//insert middle in bazis, to maximize only zero mean vectors
		bazis.push_back(middle);
		static const int numComponents = 10;
		boost::shared_ptr<FiPC> fipcPtr(new FiPC());
		{
			for (int i1 = 0; i1 < numComponents; i1++)
			{
				Element comp = fipcPtr->getFirstPC(elements, weights, numDimensions, bazis);
				bazis.push_back(comp);
			}
			int j = 0;
		}

	}
private:

};

int _tmain(int argc, _TCHAR* argv[])
{
	FastIterativePcaSelectorTest().test();
	return 0;
}


#define PY_ARRAY_UNIQUE_SYMBOL superimg_PyArray_API
#define NO_IMPORT_ARRAY

#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include <vigra/numpy_array.hxx>
#include <vigra/numpy_array_converters.hxx>

#include "seglib/cgp2d/cgp2d.hxx"
#include "seglib/cgp2d/cgp2d_python.hxx"
#include "seglib/cgp2d/objectives/high_level.hxx"
#include "seglib/distances/distance.hxx"

namespace cgp2d {

namespace python = boost::python;

template<class HLO>
void setRegionFeatures(
    HLO &  hlo ,
    vigra::NumpyArray<2,float> cell2Features
){
    hlo.setRegionFeatures(cell2Features);
}

template<class HLO>
void setPrimalLabels(
    HLO &  hlo ,
    vigra::NumpyArray<1,LabelType> labels
){
    hlo.setPrimalLabels(labels);
}

template<class HLO>
float betweenClusterDistance(
    HLO &  hlo ,
    const std::string distance,
    const double gamma
){
    if(distance==std::string("squaredNorm")){
        dist::ChiSquared<double> distFunctor;
        return hlo.betweenClusterDistance(distFunctor,gamma);
    }
    else if(distance==std::string("chi2")){
        dist::SquaredNorm<double> distFunctor;
        return hlo.betweenClusterDistance(distFunctor,gamma);
    }
    else{
        CGP_ASSERT_OP(false,==,true);
    }
}

template<class HLO>
vigra::NumpyAnyArray writeBackMergedFeatures(
    const HLO &  hlo ,
    vigra::NumpyArray<2, float > res = vigra::NumpyArray<2,float >()
){
    res.reshapeIfEmpty(hlo.features().shape());
    hlo.writeBackMergedFeatures(res);
    return res;
}

void export_hl_objective()
{
    using namespace python;

    docstring_options doc_options(true, true, false);

    ////////////////////////////////////////
    // Region Graph
    ////////////////////////////////////////
    // basic types
    // tgrid and input image type
    typedef Cgp<CoordinateType,LabelType> CgpType;
    typedef CgpType::TopologicalGridType TopologicalGridType;

    typedef HighLevelObjective<CgpType,float> HlOjective;

    python::class_<HlOjective>("HighLevelObjective",python::init<const CgpType & >()
            [with_custodian_and_ward<1 /*custodian == self*/, 2 /*ward == const CgpType& */>()] )
        .def("setRegionFeatures", vigra::registerConverters(&setRegionFeatures<HlOjective>)  )
        .def("setPrimalLabels",   vigra::registerConverters(&setPrimalLabels<HlOjective>)  )
        .def("mergeFeatures",&HlOjective::mergeFeatures)
        .def("withinClusterDistance",&HlOjective::withinClusterDistance)
        .def("betweenClusterDistance",&betweenClusterDistance<HlOjective>)
        .def("writeBackMergedFeatures",vigra::registerConverters(&writeBackMergedFeatures<HlOjective>),
            (
                arg("res")=python::object()
            )
        )
    ;

}

} // namespace vigra


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2009 Mathieu Gautier <mathieu.gautier@cea.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/SVD>
#include "AnnoyingScalar.h"

template<typename T> T bounded_acos(T v)
{
  using std::acos;
  using std::min;
  using std::max;
  return acos((max)(T(-1),(min)(v,T(1))));
}

template<typename QuatType> void check_slerp(const QuatType& q0, const QuatType& q1)
{
  using std::abs;
  typedef typename QuatType::Scalar Scalar;
  typedef AngleAxis<Scalar> AA;

  Scalar largeEps = test_precision<Scalar>();

  Scalar theta_tot = AA(q1*q0.inverse()).angle();
  if(theta_tot>Scalar(EIGEN_PI))
    theta_tot = Scalar(2.)*Scalar(EIGEN_PI)-theta_tot;
  for(Scalar t=0; t<=Scalar(1.001); t+=Scalar(0.1))
  {
    QuatType q = q0.slerp(t,q1);
    Scalar theta = AA(q*q0.inverse()).angle();
    VERIFY(abs(q.norm() - 1) < largeEps);
    if(theta_tot==0)  VERIFY(theta_tot==0);
    else              VERIFY(abs(theta - t * theta_tot) < largeEps);
  }
}

template<typename Scalar, int Options> void quaternion(void)
{
  /* this test covers the following files:
     Quaternion.h
  */
  using std::abs;
  typedef Matrix<Scalar,3,1> Vector3;
  typedef Matrix<Scalar,3,3> Matrix3;
  typedef Quaternion<Scalar,Options> Quaternionx;
  typedef AngleAxis<Scalar> AngleAxisx;

  Scalar largeEps = test_precision<Scalar>();
  if (internal::is_same<Scalar,float>::value)
    largeEps = Scalar(1e-3);

  Scalar eps = internal::random<Scalar>() * Scalar(1e-2);

  Vector3 v0 = Vector3::Random(),
          v1 = Vector3::Random(),
          v2 = Vector3::Random(),
          v3 = Vector3::Random();

  Scalar  a = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI)),
          b = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI));

  // Quaternion: Identity(), setIdentity();
  Quaternionx q1, q2;
  q2.setIdentity();
  VERIFY_IS_APPROX(Quaternionx(Quaternionx::Identity()).coeffs(), q2.coeffs());
  q1.coeffs().setRandom();
  VERIFY_IS_APPROX(q1.coeffs(), (q1*q2).coeffs());

  // concatenation
  q1 *= q2;

  q1 = AngleAxisx(a, v0.normalized());
  q2 = AngleAxisx(a, v1.normalized());

  // angular distance
  Scalar refangle = abs(AngleAxisx(q1.inverse()*q2).angle());
  if (refangle>Scalar(EIGEN_PI))
    refangle = Scalar(2)*Scalar(EIGEN_PI) - refangle;

  if((q1.coeffs()-q2.coeffs()).norm() > Scalar(10)*largeEps)
  {
    VERIFY_IS_MUCH_SMALLER_THAN(abs(q1.angularDistance(q2) - refangle), Scalar(1));
  }

  // rotation matrix conversion
  VERIFY_IS_APPROX(q1 * v2, q1.toRotationMatrix() * v2);
  VERIFY_IS_APPROX(q1 * q2 * v2,
    q1.toRotationMatrix() * q2.toRotationMatrix() * v2);

  VERIFY(  (q2*q1).isApprox(q1*q2, largeEps)
        || !(q2 * q1 * v2).isApprox(q1.toRotationMatrix() * q2.toRotationMatrix() * v2));

  q2 = q1.toRotationMatrix();
  VERIFY_IS_APPROX(q1*v1,q2*v1);

  Matrix3 rot1(q1);
  VERIFY_IS_APPROX(q1*v1,rot1*v1);
  Quaternionx q3(rot1.transpose()*rot1);
  VERIFY_IS_APPROX(q3*v1,v1);

  // angle-axis conversion
  AngleAxisx aa = AngleAxisx(q1);
  VERIFY_IS_APPROX(q1 * v1, Quaternionx(aa) * v1);

  // Do not execute the test if the rotation angle is almost zero, or
  // the rotation axis and v1 are almost parallel.
  if (abs(aa.angle()) > Scalar(5)*test_precision<Scalar>()
      && (aa.axis() - v1.normalized()).norm() < Scalar(1.99)
      && (aa.axis() + v1.normalized()).norm() < Scalar(1.99))
  {
    VERIFY_IS_NOT_APPROX(q1 * v1, Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1);
  }

  // from two vector creation
  VERIFY_IS_APPROX( v2.normalized(),(q2.setFromTwoVectors(v1, v2)*v1).normalized());
  VERIFY_IS_APPROX( v1.normalized(),(q2.setFromTwoVectors(v1, v1)*v1).normalized());
  VERIFY_IS_APPROX(-v1.normalized(),(q2.setFromTwoVectors(v1,-v1)*v1).normalized());
  if (internal::is_same<Scalar,double>::value)
  {
    v3 = (v1.array()+eps).matrix();
    VERIFY_IS_APPROX( v3.normalized(),(q2.setFromTwoVectors(v1, v3)*v1).normalized());
    VERIFY_IS_APPROX(-v3.normalized(),(q2.setFromTwoVectors(v1,-v3)*v1).normalized());
  }

  // from two vector creation static function
  VERIFY_IS_APPROX( v2.normalized(),(Quaternionx::FromTwoVectors(v1, v2)*v1).normalized());
  VERIFY_IS_APPROX( v1.normalized(),(Quaternionx::FromTwoVectors(v1, v1)*v1).normalized());
  VERIFY_IS_APPROX(-v1.normalized(),(Quaternionx::FromTwoVectors(v1,-v1)*v1).normalized());
  if (internal::is_same<Scalar,double>::value)
  {
    v3 = (v1.array()+eps).matrix();
    VERIFY_IS_APPROX( v3.normalized(),(Quaternionx::FromTwoVectors(v1, v3)*v1).normalized());
    VERIFY_IS_APPROX(-v3.normalized(),(Quaternionx::FromTwoVectors(v1,-v3)*v1).normalized());
  }

  // inverse and conjugate
  VERIFY_IS_APPROX(q1 * (q1.inverse() * v1), v1);
  VERIFY_IS_APPROX(q1 * (q1.conjugate() * v1), v1);

  // test casting
  Quaternion<float> q1f = q1.template cast<float>();
  VERIFY_IS_APPROX(q1f.template cast<Scalar>(),q1);
  Quaternion<double> q1d = q1.template cast<double>();
  VERIFY_IS_APPROX(q1d.template cast<Scalar>(),q1);

  // test bug 369 - improper alignment.
  Quaternionx *q = new Quaternionx;
  delete q;

  q1 = Quaternionx::UnitRandom();
  q2 = Quaternionx::UnitRandom();
  check_slerp(q1,q2);

  q1 = AngleAxisx(b, v1.normalized());
  q2 = AngleAxisx(b+Scalar(EIGEN_PI), v1.normalized());
  check_slerp(q1,q2);

  q1 = AngleAxisx(b,  v1.normalized());
  q2 = AngleAxisx(-b, -v1.normalized());
  check_slerp(q1,q2);

  q1 = Quaternionx::UnitRandom();
  q2.coeffs() = -q1.coeffs();
  check_slerp(q1,q2);
}

template<typename Scalar> void mapQuaternion(void){
  typedef Map<Quaternion<Scalar>, Aligned> MQuaternionA;
  typedef Map<const Quaternion<Scalar>, Aligned> MCQuaternionA;
  typedef Map<Quaternion<Scalar> > MQuaternionUA;
  typedef Map<const Quaternion<Scalar> > MCQuaternionUA;
  typedef Quaternion<Scalar> Quaternionx;
  typedef Matrix<Scalar,3,1> Vector3;
  typedef AngleAxis<Scalar> AngleAxisx;

  Vector3 v0 = Vector3::Random(),
          v1 = Vector3::Random();
  Scalar  a = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI));

  EIGEN_ALIGN_MAX Scalar array1[4];
  EIGEN_ALIGN_MAX Scalar array2[4];
  EIGEN_ALIGN_MAX Scalar array3[4+1];
  Scalar* array3unaligned = array3+1;

  MQuaternionA    mq1(array1);
  MCQuaternionA   mcq1(array1);
  MQuaternionA    mq2(array2);
  MQuaternionUA   mq3(array3unaligned);
  MCQuaternionUA  mcq3(array3unaligned);

//  std::cerr << array1 << " " << array2 << " " << array3 << "\n";
  mq1 = AngleAxisx(a, v0.normalized());
  mq2 = mq1;
  mq3 = mq1;

  Quaternionx q1 = mq1;
  Quaternionx q2 = mq2;
  Quaternionx q3 = mq3;
  Quaternionx q4 = MCQuaternionUA(array3unaligned);

  VERIFY_IS_APPROX(q1.coeffs(), q2.coeffs());
  VERIFY_IS_APPROX(q1.coeffs(), q3.coeffs());
  VERIFY_IS_APPROX(q4.coeffs(), q3.coeffs());
  #ifdef EIGEN_VECTORIZE
  if(internal::packet_traits<Scalar>::Vectorizable)
    VERIFY_RAISES_ASSERT((MQuaternionA(array3unaligned)));
  #endif

  VERIFY_IS_APPROX(mq1 * (mq1.inverse() * v1), v1);
  VERIFY_IS_APPROX(mq1 * (mq1.conjugate() * v1), v1);

  VERIFY_IS_APPROX(mcq1 * (mcq1.inverse() * v1), v1);
  VERIFY_IS_APPROX(mcq1 * (mcq1.conjugate() * v1), v1);

  VERIFY_IS_APPROX(mq3 * (mq3.inverse() * v1), v1);
  VERIFY_IS_APPROX(mq3 * (mq3.conjugate() * v1), v1);

  VERIFY_IS_APPROX(mcq3 * (mcq3.inverse() * v1), v1);
  VERIFY_IS_APPROX(mcq3 * (mcq3.conjugate() * v1), v1);

  VERIFY_IS_APPROX(mq1*mq2, q1*q2);
  VERIFY_IS_APPROX(mq3*mq2, q3*q2);
  VERIFY_IS_APPROX(mcq1*mq2, q1*q2);
  VERIFY_IS_APPROX(mcq3*mq2, q3*q2);

  // Bug 1461, compilation issue with Map<const Quat>::w(), and other reference/constness checks:
  VERIFY_IS_APPROX(mcq3.coeffs().x() + mcq3.coeffs().y() + mcq3.coeffs().z() + mcq3.coeffs().w(), mcq3.coeffs().sum());
  VERIFY_IS_APPROX(mcq3.x() + mcq3.y() + mcq3.z() + mcq3.w(), mcq3.coeffs().sum());
  mq3.w() = 1;
  const Quaternionx& cq3(q3);
  VERIFY( &cq3.x() == &q3.x() );
  const MQuaternionUA& cmq3(mq3);
  VERIFY( &cmq3.x() == &mq3.x() );
  // FIXME the following should be ok. The problem is that currently the LValueBit flag
  // is used to determine whether we can return a coeff by reference or not, which is not enough for Map<const ...>.
  //const MCQuaternionUA& cmcq3(mcq3);
  //VERIFY( &cmcq3.x() == &mcq3.x() );
}

template<typename Scalar> void quaternionAlignment(void){
  typedef Quaternion<Scalar,AutoAlign> QuaternionA;
  typedef Quaternion<Scalar,DontAlign> QuaternionUA;

  EIGEN_ALIGN_MAX Scalar array1[4];
  EIGEN_ALIGN_MAX Scalar array2[4];
  EIGEN_ALIGN_MAX Scalar array3[4+1];
  Scalar* arrayunaligned = array3+1;

  QuaternionA *q1 = ::new(reinterpret_cast<void*>(array1)) QuaternionA;
  QuaternionUA *q2 = ::new(reinterpret_cast<void*>(array2)) QuaternionUA;
  QuaternionUA *q3 = ::new(reinterpret_cast<void*>(arrayunaligned)) QuaternionUA;

  q1->coeffs().setRandom();
  *q2 = *q1;
  *q3 = *q1;

  VERIFY_IS_APPROX(q1->coeffs(), q2->coeffs());
  VERIFY_IS_APPROX(q1->coeffs(), q3->coeffs());
  #if defined(EIGEN_VECTORIZE) && EIGEN_MAX_STATIC_ALIGN_BYTES>0
  if(internal::packet_traits<Scalar>::Vectorizable && internal::packet_traits<Scalar>::size<=4)
    VERIFY_RAISES_ASSERT((::new(reinterpret_cast<void*>(arrayunaligned)) QuaternionA));
  #endif
}

template<typename PlainObjectType> void check_const_correctness(const PlainObjectType&)
{
  // there's a lot that we can't test here while still having this test compile!
  // the only possible approach would be to run a script trying to compile stuff and checking that it fails.
  // CMake can help with that.

  // verify that map-to-const don't have LvalueBit
  typedef typename internal::add_const<PlainObjectType>::type ConstPlainObjectType;
  VERIFY( !(internal::traits<Map<ConstPlainObjectType> >::Flags & LvalueBit) );
  VERIFY( !(internal::traits<Map<ConstPlainObjectType, Aligned> >::Flags & LvalueBit) );
  VERIFY( !(Map<ConstPlainObjectType>::Flags & LvalueBit) );
  VERIFY( !(Map<ConstPlainObjectType, Aligned>::Flags & LvalueBit) );
}

#if EIGEN_HAS_RVALUE_REFERENCES

// Regression for bug 1573
struct MovableClass {
  MovableClass() = default;
  MovableClass(const MovableClass&) = default;
  MovableClass(MovableClass&&) noexcept = default;
  MovableClass& operator=(const MovableClass&) = default;
  MovableClass& operator=(MovableClass&&) = default;
  Quaternionf m_quat;
};

#endif

EIGEN_DECLARE_TEST(geo_quaternion)
{
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1(( quaternion<float,AutoAlign>() ));
    CALL_SUBTEST_1( check_const_correctness(Quaternionf()) );
    CALL_SUBTEST_1(( quaternion<float,DontAlign>() ));
    CALL_SUBTEST_1(( quaternionAlignment<float>() ));
    CALL_SUBTEST_1( mapQuaternion<float>() );

    CALL_SUBTEST_2(( quaternion<double,AutoAlign>() ));
    CALL_SUBTEST_2( check_const_correctness(Quaterniond()) );
    CALL_SUBTEST_2(( quaternion<double,DontAlign>() ));
    CALL_SUBTEST_2(( quaternionAlignment<double>() ));
    CALL_SUBTEST_2( mapQuaternion<double>() );

    AnnoyingScalar::dont_throw = true;
    CALL_SUBTEST_3(( quaternion<AnnoyingScalar,AutoAlign>() ));
  }
}


/** @file planar.cc
 * @author David F. Gleich
 * @date 2008-09-29
 * @copyright Stanford University, 2008
 * Planar graph algorithm wrappers
 */

/** History
 *  2008-09-29: Initial coding
 */

#include "include/matlab_bgl.h"

#include <yasmic/undir_simple_csr_matrix_as_graph.hpp>
#include <yasmic/simple_csr_matrix_as_graph.hpp>
#include <yasmic/iterator_utility.hpp>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/planar_canonical_ordering.hpp>
//#include <boost/graph/chrobak_payne_drawing.hpp>
#include <yasmic/boost_mod/chrobak_payne_drawing.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>
#include <boost/graph/is_kuratowski_subgraph.hpp>
#include <boost/graph/make_connected.hpp>
#include <boost/graph/make_biconnected_planar.hpp>
#include <boost/graph/make_maximal_planar.hpp>
#include <boost/graph/simple_point.hpp>
//#include <boost/graph/is_straight_line_drawing.hpp>
#include <yasmic/boost_mod/is_straight_line_drawing.hpp>
#include <boost/graph/filtered_graph.hpp>

#include <vector>
#include <iostream>
#include <algorithm>

#include <math.h>

#include "libmbgl_util.hpp"

/** Copy an edge iterator to a pair of arrays
 */
template <typename Graph, typename Iterator>
mbglIndex copy_to_ij(Graph& g, Iterator oi,
                   Iterator oi_end, mbglIndex* i, mbglIndex* j)
{
  using namespace boost;

  mbglIndex ei;
  for (ei= 0; oi != oi_end; ++oi, ++ei) {
    typename graph_traits<Graph>::edge_descriptor e = *oi;
    i[ei] = source(e,g);
    j[ei] = target(e,g);
  }

  return (ei);
}

/** Translate the boost embedding information to the libmbgl embedding output
 */
template <typename Graph, typename PlanarEmbedding>
void copy_embedding(Graph& g, PlanarEmbedding e, mbglIndex *eip, mbglIndex *eie)
{
  using namespace boost;
  typename graph_traits<Graph>::vertex_iterator vi, viend;
  mbglIndex nedges= num_edges(g);
  mbglIndex curi= 0;
  mbglIndex nullv= graph_traits<Graph>::null_vertex();
  mbglIndex oldv= nullv;
  for (boost::tie(vi,viend)=vertices(g); vi!=viend; ++vi) {
    if (oldv!=nullv) { assert(*vi==oldv+1); }
    assert(*vi<num_vertices(g));
    eip[*vi] = curi;
    typename property_traits<PlanarEmbedding>::value_type::const_iterator
      ei=e[*vi].begin(), eiend=e[*vi].end();
    for (; ei!=eiend; ++ei) {
      assert(source(*ei,g) == *vi || target(*ei,g) == *vi);
      assert(curi<nedges);
      if (source(*ei,g) == *vi) {
        eie[curi] = target(*ei,g);
      } else if (target(*ei,g) == *vi) {
        eie[curi] = source(*ei,g);
      } else {
        assert(source(*ei,g) == *vi || target(*ei,g) == *vi);
      }
      ++curi;
    }
    oldv = *vi;
  }
  eip[num_vertices(g)] = curi;
}

/** Test if a graph is planar, compute a planar embedding, or get a Kuratowski
 * subgraph
 *
 * @param nverts the number of vertices in the graph
 * @param ja the connectivity for each vertex
 * @param ia the row connectivity points into ja
 * @param is_planar set to 0 if the graph is not planar, else the graph is
 *   planar
 * @param i the source vertex of any edge in the Kuratowski subgraph,
 *   length max(3*nverts-6,6)
 * @param j the dest vertex of any edge in the Kuratowski subgraph,
 *   length max(3*nverts-6,6)
 * @param nedges set to the number of edges in i and j actually used
 * @param eip the embedding information edge pointer to eie, length nverts+1
 *   eip[eie[v]] to eip[eie[v+1]] gives the embedding order for vertex v
 * @param eie the embedding information destination list, length ia[nverts]
 *   see eip for a description.
 */
int boyer_myrvold_planarity_test(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia,
    int *is_planar,
    mbglIndex *i, mbglIndex *j, mbglIndex* nedges, /* kuratowski subgraph output */
    mbglIndex *eip, mbglIndex *eie)
{
  using namespace yasmic;
  using namespace boost;

  typedef simple_csr_matrix<mbglIndex,double> crs_graph;
  crs_graph g(nverts, nverts, ia[nverts], ia, ja, NULL);

  //Define the storage type for the planar embedding
  typedef std::vector< std::vector< graph_traits<crs_graph>::edge_descriptor > >
    embedding_storage_t;
  typedef boost::iterator_property_map
    < embedding_storage_t::iterator,
      property_map<crs_graph, vertex_index_t>::type
    >
    embedding_t;

  bool planar=false;
  if ((i == NULL || j == NULL || nedges == NULL) &&
      (eip == NULL || eip == NULL)) {
    // just test for a planar graph
    planar= boyer_myrvold_planarity_test(g);
  } else if (eip == NULL || eie == NULL ) {
    // just get the kuratowski subgraph
    std::vector<graph_traits<crs_graph>::edge_descriptor>
      kuratowski_edges;
    planar= boyer_myrvold_planarity_test(
        boyer_myrvold_params::graph = g,
        boyer_myrvold_params::kuratowski_subgraph =
          std::back_inserter(kuratowski_edges)
        );
    if (planar == false) {
      *nedges =
        copy_to_ij(g, kuratowski_edges.begin(), kuratowski_edges.end(), i, j);
    } else { *nedges = 0; }
  } else if (i == NULL || j == NULL || nedges == NULL) {
    embedding_storage_t embedding_storage(num_vertices(g));
    embedding_t embedding(embedding_storage.begin(), get(vertex_index,g));
    planar= boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g,
                                   boyer_myrvold_params::embedding = embedding
                                   );
    if (planar == true) {
      copy_embedding(g, embedding, eip, eie);
    } else {
      memset(eip, 0, sizeof(mbglIndex)*(nverts+1));
    }
  } else {
    // get the kuratowski subgraph and the planar embedding
    std::vector<graph_traits<crs_graph>::edge_descriptor>
        kuratowski_edges;
    embedding_storage_t embedding_storage(num_vertices(g));
    embedding_t embedding(embedding_storage.begin(), get(vertex_index,g));
    planar= boyer_myrvold_planarity_test(
        boyer_myrvold_params::graph = g,
        boyer_myrvold_params::embedding = embedding,
        boyer_myrvold_params::kuratowski_subgraph =
                 std::back_inserter(kuratowski_edges)
        );
    if (planar == false) {
      *nedges =
        copy_to_ij(g, kuratowski_edges.begin(), kuratowski_edges.end(), i, j);
    } else { *nedges = 0; }
    if (planar == true) {
      copy_embedding(g, embedding, eip, eie);
    } else {
      memset(eip, 0, sizeof(mbglIndex)*(nverts+1));
    }
  }
  if (is_planar) {
    if (planar) { *is_planar = 1; }
    else { *is_planar = 0; }
  }
  return (0);
}

/** An edge filter that only returns the edges in an upper-triangular part
 */
template <typename Graph>
struct upper_triangle_edge_filter {
  upper_triangle_edge_filter() : _g(NULL) {}
  upper_triangle_edge_filter(const Graph& g_) : _g(&g_) {}
  template <typename Edge>
  bool operator()(const Edge& e) const {
    return (boost::source(e,*_g) < boost::target(e,*_g));
  }
  const Graph* _g;
};

/** Test is a graph is Kuratowski (i.e. contracts to K_3,3 or K_5)
 * This function only uses the upper triangular set of edges.  It will
 * silently report incorrect ouput if there are too many parallel edges,
 * so beware.
 * @param nverts the number of vertices in the graph
 * @param ja the connectivity for each vertex
 * @param ia the row connectivity points into ja
 * @param is_ksubgraph set to 1 if the graph is Kuratowski
 */
int is_kuratowski_subgraph(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia,
    int *is_ksubgraph)
{
  using namespace boost;
  using namespace yasmic;

  typedef simple_csr_matrix<mbglIndex,double> crs_graph;
  typedef upper_triangle_edge_filter<crs_graph> filter;
  typedef filtered_graph<crs_graph, filter> fgraph;
  assert(is_ksubgraph);
  crs_graph g(nverts, nverts, ia[nverts], ia, ja, NULL);
  filter f(g);
  fgraph fg(g,f);
  graph_traits<fgraph>::edge_iterator ei, eiend;
  boost::tie(ei,eiend)= edges(fg);
  bool is_k= is_kuratowski_subgraph(g, ei, eiend, get(vertex_index,g));
  if (is_k) { *is_ksubgraph = 1; } else { *is_ksubgraph = 0; }
  return (0);
}

/** Test if a given set of positions is a straight line drawing of the graph.
 * This function uses a bucket-sort, and so large positions may cause out
 * of memory errors.
 *
 * @param nverts the number of vertices in the graph
 * @param ja the connectivity for each vertex
 * @param ia the row connectivity points into ja
 * @param X The set of positions.  These are rounded to size_t variables and so
 *   all the elements of X must be positive.
 * @param is_sldrawing set to 1 if the positions are a straight line drawing,
 *   otherwise set to 0
 */
int is_straight_line_drawing(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia,
    double *X, int *is_sldrawing)
{
  using namespace boost;
  using namespace yasmic;

  typedef simple_csr_matrix<mbglIndex,double> crs_graph;
  crs_graph g(nverts, nverts, ia[nverts], ia, ja, NULL);
  assert(is_sldrawing);
  // copy the layout from positions
  std::vector<simple_point<size_t> > position_vec(nverts);
  mbglIndex n = num_vertices(g);
  for (mbglIndex i = 0; i<n; i++) {
    if (X[i+0*n] < 0 || X[i+1*n] < 0) {
      return -11;
    }
    position_vec[i].x = (size_t)floor(X[i+0*n]);
    position_vec[i].y = (size_t)floor(X[i+1*n]);
  }
  bool is_sl= is_straight_line_drawing(g,
      make_iterator_property_map(position_vec.begin(),get(vertex_index,g)),
      get(vertex_index,g));
  if (is_sl) { *is_sldrawing = 1; } else { *is_sldrawing = 0; }
  return (0);
}

/** Copy a crs_graph to a mutable boost graph
 */
template <typename Graph>
void copy_crs_to_graph(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia,
    Graph& g)
{
  using namespace boost;
  using namespace yasmic;
  typedef simple_csr_matrix<mbglIndex,double> crs_graph;
  crs_graph g_in(nverts, nverts, ia[nverts], ia, ja, NULL);
  //typename property_map<Graph, vertex_index_t>::type vi(get(vertex_index, g));
  //for (mbglIndex i=0; i<nverts; i++) { vi[i]=i; }
  typename graph_traits<crs_graph>::vertex_iterator vi, vi_end;
  typename graph_traits<crs_graph>::out_edge_iterator ei, ei_end;
  for (tie(vi, vi_end) = vertices(g_in); vi != vi_end; ++vi) {
    for (tie(ei, ei_end) = out_edges(*vi, g_in); ei != ei_end; ++ei) {
      if (source(*ei,g_in) < target(*ei,g_in)) {
        add_edge(source(*ei,g_in),target(*ei,g_in),g);
      }
    }
  }
}

/** Add an edge and record all edges we added
 * EdgeSrcOutIterator - the type of the out iterator for the source vertex
 *   in the new edge pair
 * EdgeDstOutIterator - the type of the out iterator for the destination vertex
 *   in the new edge pair
 * TODO Make this function update the edge_index parameter too!  This will avoid
 * O(E) work in a few cases.
 */
template <typename EdgeSrcOutIterator, typename EdgeDstOutIterator>
struct record_add_edge_visitor {
  EdgeSrcOutIterator soi, soi_end;
  EdgeDstOutIterator doi, doi_end;
  mbglIndex* count; // incremented for each additional edge

  record_add_edge_visitor(EdgeSrcOutIterator soi_, EdgeSrcOutIterator soi_end_,
      EdgeDstOutIterator doi_, EdgeDstOutIterator doi_end_, mbglIndex* count_)
  : soi(soi_), soi_end(soi_end_), doi(doi_), doi_end(doi_end_), count(count_)
  {}

  template <typename Graph, typename Vertex>
  void visit_vertex_pair(Vertex u, Vertex v, Graph& g) {
    add_edge(u,v,g);
    assert(soi != soi_end);
    assert(doi != doi_end);
    *soi = u;
    *doi = v;
    ++doi;
    ++soi;
    (*count)++;
  }
};

/** Helper to abstract the details of the triangulation */
template <typename Graph, typename RecordAddEdgeVisitor>
int triangulate_bgl_graph(
  Graph& g, int make_conn, int make_biconnected, int make_maximal,
  RecordAddEdgeVisitor add_edge_visitor)
{
  using namespace boost;
  typedef std::vector< std::vector<
            typename graph_traits<Graph>::edge_descriptor > >
  embedding_storage_t;
  typedef boost::iterator_property_map
    < typename embedding_storage_t::iterator,
      typename property_map<Graph, vertex_index_t>::type
    >
    embedding_t;

  if (make_conn) {
    make_connected(g, get(vertex_index,g), add_edge_visitor);
  }
  if (make_biconnected || make_maximal) {
    typename property_map<Graph, edge_index_t>::type e_index = get(edge_index, g);
    typename graph_traits<Graph>::edges_size_type edge_count = 0;
    typename graph_traits<Graph>::edge_iterator ei, ei_end;

    // compute a planar embedding
    embedding_storage_t embedding_storage(num_vertices(g));
    embedding_t embedding(embedding_storage.begin(), get(vertex_index,g));

    if (make_biconnected) {
      // compute the edge index
      edge_count = 0;
      for(tie(ei, ei_end) = edges(g); ei != ei_end; ++ei) {
        put(e_index, *ei, edge_count++);
      }
      // compute a planar embedding
      if (boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g,
                                       boyer_myrvold_params::embedding =
                                           embedding
                                       )
          ) {
        make_biconnected_planar(g, embedding, get(edge_index,g),
            add_edge_visitor);
      } else {
        return 1;
      }
    }
    if (make_maximal) {
      // compute the edge index
      edge_count = 0;
      for(tie(ei, ei_end) = edges(g); ei != ei_end; ++ei) {
        put(e_index, *ei, edge_count++);
      }
      // compute a planar embedding
      if (boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g,
                                       boyer_myrvold_params::embedding =
                                           embedding
                                       )
          ) {
        make_maximal_planar(g, embedding, get(vertex_index,g),
            get(edge_index,g), add_edge_visitor);
      } else {
        return 1;
      }
    }
  }
  return 0;
}

/** Compute extra edges that are needed for a straight line embedding.
 *
 * @param nverts the number of vertices in the graph
 * @param ja the connectivity for each vertex
 * @param ia the row connectivity points into ja
 * @param make_connected if set (!=0), compute edges to make a single
 *   connected component
 * @param make_biconnected If set (!=0), compute edges to make a single
 *   biconnected component (requires a planar input graph).  If only this
 *   parameter is set, then we assume the graph is already connected.
 * @param make_maximal If set (!=0), compute edges to make the maximal
 *   planar graph (requires a planar input graph). If only this
 *   parameter is set, then we assume the graph is already biconnected.
 * @param i source of all edges added in the triangulation,
 *   length max(3*nverts-6,6)
 * @param j dest of all edges added in the triangulation,
 *   length max(3*nverts-6,6)
 * @param nedges the number of edges in i and j actually used
 */
int triangulate_graph(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia,
    int make_connected, int make_biconnected, int make_maximal,
    mbglIndex *i, mbglIndex *j, mbglIndex* nedges /* extra edges */)
{
  using namespace boost;
  typedef adjacency_list
        < vecS, vecS, undirectedS, no_property,
        property<edge_index_t, mbglIndex> > graph;
  assert(i != NULL); assert(j != NULL); assert(nedges != NULL);
  *nedges = 0;
  size_t endedges= nverts>4 ? 3*nverts-6 : 6;
  record_add_edge_visitor<mbglIndex*,mbglIndex*> add_edge_visitor(
      i, i+endedges, j, j+endedges, nedges);
  graph g(nverts);
  copy_crs_to_graph(nverts, ja, ia, g);
  return triangulate_bgl_graph(
      g, make_connected, make_biconnected, make_maximal, add_edge_visitor);
}

/** Abstract some details between the csr_graph and the adjacency_list graph
 * for the straight line drawing
 */
template <typename Graph>
int chrobak_payne_straight_line_drawing_on_maximal_graph(const Graph& g,
    int just_ordering,
    mbglIndex *p, /* ordering permutation */
    mbglDegreeType *X)
{
  using namespace boost;
  typedef std::vector< std::vector<
            typename graph_traits<Graph>::edge_descriptor > >
    embedding_storage_t;
  typedef boost::iterator_property_map
    < typename embedding_storage_t::iterator,
      typename property_map<Graph, vertex_index_t>::type
    >
    embedding_t;

  embedding_storage_t embedding_storage(num_vertices(g));
  embedding_t embedding(embedding_storage.begin(), get(vertex_index,g));

  if (boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g,
                                   boyer_myrvold_params::embedding =
                                      embedding
                                  )
     ) {
   // do nothing, the graph is planar
  } else {
   return 1;
  }
  // TODO convert this to use p directly
  mbglIndex n = num_vertices(g);
  assert(n == 0 || p);
  std::vector< typename graph_traits<Graph>::vertex_descriptor > ordering;
  planar_canonical_ordering(g, embedding,
     std::back_inserter(ordering), get(vertex_index,g));

  if (just_ordering == 0) {
    typedef simple_point<size_t> coord_t;
    typedef std::vector< coord_t > straight_line_drawing_storage_t;
    typedef boost::iterator_property_map
       < straight_line_drawing_storage_t::iterator,
         typename property_map<Graph, vertex_index_t>::type
       >
       straight_line_drawing_t;

    assert(n == 0 || X);
    // compute the straight line embedding
    straight_line_drawing_storage_t straight_line_drawing_storage
     (num_vertices(g));
    straight_line_drawing_t straight_line_drawing
     (straight_line_drawing_storage.begin(),
      get(vertex_index,g)
      );
    if (n == 2) {
      straight_line_drawing_storage[0].x = 0;
      straight_line_drawing_storage[0].y = 0;
      straight_line_drawing_storage[1].x = 1;
      straight_line_drawing_storage[1].y = 0;
    } else {
      chrobak_payne_straight_line_drawing(g,
                                         embedding,
                                         ordering.begin(),
                                         ordering.end(),
                                         straight_line_drawing
                                         );
    }
    // copy all the data
    for (mbglIndex i= 0; i<n; i++) {
      X[i+0*n] = straight_line_drawing_storage[i].x;
      X[i+1*n] = straight_line_drawing_storage[i].y;
    }
  }
  assert((ordering.end() - ordering.begin()) == (ptrdiff_t)n);
  for (mbglIndex i= 0; i<n; i++) {
    p[i] = ordering[i];
  }
  return 0;
}

/** Compute a straight line embedding of a graph or a canonical planar
 * ordering
 *
 * This function must copy the graph unless is_maximal is set.
 *
 * @param nverts the number of vertices in the graph
 * @param ja the connectivity for each vertex
 * @param ia the row connectivity points into ja
 * @param just_ordering if set (!=0), then just compute the ordering permutation
 * @param is_maximal if set (!=0), assume the input is a maximal planar graph
 * @param i source edges for the maximal planar graph, length 3n-6
 * @param j dest edges for the maximal planar graph, length 3n-6
 * @param nedges the number of entries in i,j actually used, length 1
 * @param p the ordering permutation, length nverts
 * @param X the positions, length 2*nverts
 */
int chrobak_payne_straight_line_drawing(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia,
    int just_ordering, int is_maximal,
    mbglIndex *i, mbglIndex *j, mbglIndex* nedges, /* extra edges */
    mbglIndex *p, /* ordering permutation */
    mbglDegreeType *X)
{
  using namespace boost;
  using namespace yasmic;
  if (is_maximal) {
    // trust the user that the graph is maximal, this means we
    // can skip the copy
    typedef simple_csr_matrix<mbglIndex,double> crs_graph;
    crs_graph g(nverts, nverts, ia[nverts], ia, ja, NULL);
    if (nedges) { *nedges = 0; } // already maximal
    return chrobak_payne_straight_line_drawing_on_maximal_graph(g,
        just_ordering, p, X);
  } else {
    typedef adjacency_list
      < vecS, vecS, undirectedS, no_property,
      property<edge_index_t, mbglIndex> > graph;
    graph g(nverts);
    copy_crs_to_graph(nverts, ja, ia, g);
    int rval=0;
    if (i != NULL && j != NULL && nedges != NULL) {
      *nedges = 0;
      size_t endedges= nverts>4 ? 3*nverts-6 : 6;
      record_add_edge_visitor<mbglIndex*,mbglIndex*> add_edge_visitor(
            i, i+endedges, j, j+endedges, nedges);
      rval= triangulate_bgl_graph(g, 1, 1, 1, add_edge_visitor);
    } else {
      mbglIndex cval1= 0;
      mbglIndex extra_edges= 0;
      typedef yasmic::constant_iterator<mbglIndex> dummy_iterator;
      dummy_iterator di(&cval1);
      record_add_edge_visitor<dummy_iterator,dummy_iterator> add_edge_visitor(
            di, di, di, di, &extra_edges);
      rval= triangulate_bgl_graph(g, 1, 1, 1, add_edge_visitor);
    }
    if (rval != 0) { return rval; } // return on error
    return chrobak_payne_straight_line_drawing_on_maximal_graph(g,
            just_ordering, p, X);
  }
}


#include <CGAL/boost/graph/alpha_expansion_graphcut.h>
#include <boost/graph/adjacency_list.hpp>

struct Vertex_property
{
  int label;
  std::vector<double> cost;
};

struct Edge_property
{
  double weight;
};

using Graph = boost::adjacency_list <boost::setS,
                                     boost::vecS,
                                     boost::undirectedS,
                                     Vertex_property,
                                     Edge_property>;
using GT = boost::graph_traits<Graph>;
using vertex_descriptor = GT::vertex_descriptor;
using edge_descriptor = GT::edge_descriptor;

int main()
{
  std::array<char, 3> labels = { 'X', ' ', 'O' };

  std::array<std::array<int, 6>, 5> input
    = { { { 0, 2, 0, 1, 1, 1 },
          { 0, 0, 1, 0, 1, 2 },
          { 2, 0, 1, 1, 2, 2 },
          { 0, 1, 1, 2, 2, 0 },
          { 1, 1, 2, 0, 2, 2 } } };

  std::array<std::array<vertex_descriptor, 6>, 5> vertices;

  // Init vertices from values
  Graph g;
  for (std::size_t i = 0; i < input.size(); ++ i)
    for (std::size_t j = 0; j < input[i].size(); ++ j)
    {
      vertices[i][j] = boost::add_vertex(g);
      g[vertices[i][j]].label = input[i][j];

      // Cost of assigning this vertex to any label is positive except
      // for current label which is 0 (favor init solution)
      g[vertices[i][j]].cost.resize(3, 1);
      g[vertices[i][j]].cost[std::size_t(input[i][j])] = 0;
    }

  // Display input values
  std::cerr << "Input:" << std::endl;
  for (std::size_t i = 0; i < vertices.size(); ++ i)
  {
    for (std::size_t j = 0; j < vertices[i].size(); ++ j)
      std::cerr << labels[std::size_t(g[vertices[i][j]].label)];
    std::cerr << std::endl;
  }

  // Init adjacency
  double weight = 0.5;
  for (std::size_t i = 0; i < vertices.size(); ++ i)
    for (std::size_t j = 0; j < vertices[i].size(); ++ j)
    {
      // Neighbor vertices are connected
      if (i < vertices.size() - 1)
      {
        edge_descriptor ed = boost::add_edge (vertices[i][j], vertices[i+1][j], g).first;
        g[ed].weight = weight;
      }
      if (j < vertices[i].size() - 1)
      {
        edge_descriptor ed = boost::add_edge (vertices[i][j], vertices[i][j+1], g).first;
        g[ed].weight = weight;
      }
    }

  std::cerr << std::endl << "Alpha expansion..." << std::endl << std::endl;
  CGAL::alpha_expansion_graphcut (g,
                                  get (&Edge_property::weight, g),
                                  get (&Vertex_property::cost, g),
                                  get (&Vertex_property::label, g),
                                  CGAL::parameters::vertex_index_map (get (boost::vertex_index, g)));

  // Display output graph
  std::cerr << "Output:" << std::endl;
  for (std::size_t i = 0; i < vertices.size(); ++ i)
  {
    for (std::size_t j = 0; j < vertices[i].size(); ++ j)
      std::cerr << labels[std::size_t(g[vertices[i][j]].label)];
    std::cerr << std::endl;
  }

  return 0;
}


#include <Engine/MeshEdit/MinSurf.h>

#include <Engine/Primitive/TriMesh.h>

#include <Eigen/Sparse>

using namespace Ubpa;

using namespace std;
using namespace Eigen;

MinSurf::MinSurf(Ptr<TriMesh> triMesh)
	: heMesh(make_shared<HEMesh<V>>())
{
	Init(triMesh);
}

void MinSurf::Clear() {
	heMesh->Clear();
	triMesh = nullptr;
}

bool MinSurf::Init(Ptr<TriMesh> triMesh) {
	Clear();

	if (triMesh == nullptr)
		return true;

	if (triMesh->GetType() == TriMesh::INVALID) {
		printf("ERROR::MinSurf::Init:\n"
			"\t""trimesh is invalid\n");
		return false;
	}

	// init half-edge structure
	size_t nV = triMesh->GetPositions().size();
	vector<vector<size_t>> triangles;
	triangles.reserve(triMesh->GetTriangles().size());
	for (auto triangle : triMesh->GetTriangles())
		triangles.push_back({ triangle->idx[0], triangle->idx[1], triangle->idx[2] });
	heMesh->Reserve(nV);
	heMesh->Init(triangles);

	if (!heMesh->IsTriMesh() || !heMesh->HaveBoundary()) {
		printf("ERROR::MinSurf::Init:\n"
			"\t""trimesh is not a triangle mesh or hasn't a boundaries\n");
		heMesh->Clear();
		return false;
	}

	// triangle mesh's positions ->  half-edge structure's positions
	for (int i = 0; i < nV; i++) {
		auto v = heMesh->Vertices().at(i);
		v->pos = triMesh->GetPositions()[i].cast_to<vecf3>();
	}

	this->triMesh = triMesh;
	return true;
}

bool MinSurf::Run() {
	if (heMesh->IsEmpty() || !triMesh) {
		printf("ERROR::MinSurf::Run\n"
			"\t""heMesh->IsEmpty() || !triMesh\n");
		return false;
	}

	Minimize();

	// half-edge structure -> triangle mesh
	size_t nV = heMesh->NumVertices();
	size_t nF = heMesh->NumPolygons();
	vector<pointf3> positions;
	vector<unsigned> indice;
	positions.reserve(nV);
	indice.reserve(3 * nF);
	for (auto v : heMesh->Vertices())
		positions.push_back(v->pos.cast_to<pointf3>());
	for (auto f : heMesh->Polygons()) { // f is triangle
		for (auto v : f->BoundaryVertice()) // vertices of the triangle
			indice.push_back(static_cast<unsigned>(heMesh->Index(v)));
	}

	triMesh->Init(indice, positions);

	return true;
}

void MinSurf::Minimize() {
	// TODO
	auto bound = heMesh->Boundaries();
	vector<int> bound_idx;
	//get the index of the boundary
	for (auto& ver : bound)
	{
		for (auto p_v : ver)
		{
			bound_idx.push_back(heMesh->Index(p_v->Pair()->End()));
		}
	}
	//sort
	bound_idx.erase(std::unique(bound_idx.begin(), bound_idx.end()), bound_idx.end());
	std::sort(bound_idx.begin(), bound_idx.end());
	const auto& v = heMesh->Vertices();

	const auto mat_size = v.size() - bound_idx.size();
	SparseMatrix<double> mat(mat_size, mat_size);
	//set matrix
	MatrixX3d right = Eigen::MatrixX3d::Zero(mat_size, 3);

	using std::cout;
	//get the matrix
	for (auto vert : v)
	{
		int index = find_idx(bound_idx, heMesh->Index(vert));
		if (index != -1) //if the vertex is not in the boundary
		{
			auto adj_v = vert->AdjVertices(); //get the adjvertexs
			double degree = adj_v.size();
			if (index >= mat_size)
			{
				cout << "index=" << index << "mat_size=" << mat_size << endl;
				system("pause");
			}
			else
			{
				mat.coeffRef(index, index) ++;
			}
			for (auto v : adj_v)
			{
				int row_idx = find_idx(bound_idx, heMesh->Index(v));
				if (row_idx == -1)
				{
					for (int i = 0; i < 3; i++)
					{
						right(index, i) += v->pos[i] / degree;
					}
				}
				else
				{
					if (index >= mat_size)
					{
						cout << "index:" << index << endl;
						system("pause");
					}
					else	if (row_idx >= mat_size)
					{
						cout << "row_index:" << row_idx << endl;
						system("pause");
					}
					else
					{
						mat.coeffRef(index, row_idx) -= 1 / degree;
					}
				}
			}
		}
	}
	//solve matrix
	SparseLU<SparseMatrix<double>> solver;
	solver.compute(mat);

	if (solver.info() != Success)
	{
		cerr << "Unable to decompose the matrix" << endl;
	}

	auto result = solver.solve(right);
	//set vertex
	for (auto vert : v)
	{
		int idx = find_idx(bound_idx, heMesh->Index(vert));
		if (idx != -1)
		{
			for (int i = 0; i < 3; i++)
			{
				vert->pos[i] = result(idx, i);
			}
		}
	}
}

//return the index of the vertex in the matrix above
const int MinSurf::find_idx(std::vector<int>& vec, int idx)
{
	int N = vec.size();
	if (idx < vec[0]) return idx;
	if (idx > vec[N - 1]) return idx - N ;
	for (int i = 0; i < N; i++)
	{
		if (idx == vec[i]) return -1;
		if (idx < vec[i])
		{
			return idx - i;
		}
	}
}


#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/array.hpp>
#include <array>
#include <utility>
#include <iostream>

int main()
{
  enum { topLeft, topRight, bottomRight, bottomLeft };

  std::array<std::pair<int, int>, 4> edges{{
    std::make_pair(topLeft, topRight),
    std::make_pair(topRight, bottomRight),
    std::make_pair(bottomRight, bottomLeft),
    std::make_pair(bottomLeft, topLeft)
  }};

  struct edge_properties
  {
    int weight;
  };

  typedef boost::adjacency_list<boost::listS, boost::vecS,
    boost::undirectedS, boost::no_property,
    edge_properties> graph;

  boost::array<edge_properties, 4> props{{2, 1, 1, 1}};

  graph g{edges.begin(), edges.end(), props.begin(), 4};

  boost::array<int, 4> directions;
  boost::dijkstra_shortest_paths(g, bottomRight,
    boost::predecessor_map(directions.begin()).
    weight_map(boost::get(&edge_properties::weight, g)));

  int p = topLeft;
  while (p != bottomRight)
  {
    std::cout << p << '\n';
    p = directions[p];
  }
  std::cout << p << '\n';
}


///////////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::distributions::location_scale::log_unnormalized_pdf.hpp //
//                                                                               //
//  (C) Copyright 2009 Erwann Rogard                                             //
//  Use, modification and distribution are subject to the                        //
//  Boost Software License, Version 1.0. (See accompanying file                  //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)             //
///////////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_LOCATION_SCALE_LOG_UNNORMALIZED_PDF_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_LOCATION_SCALE_LOG_UNNORMALIZED_PDF_HPP_ER_2009
#include <boost/concept/assert.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/location_scale/location_scale.hpp>
#include <boost/statistics/detail/distribution_common/meta/value.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace toolkit{

    template<typename Z,typename T>
    T
    log_unnormalized_pdf(
        const distribution::toolkit::location_scale_distribution<Z>& d,
        const T& x
    ){
        typedef distribution::toolkit::location_scale_distribution<Z> dist_;

        T z = (x-d.mu())/d.sigma();
        T result = log_unnormalized_pdf(d.z(),z);
        // -log ( sigma ) is a constant so it is ignored
        return result;
    }

}// toolkit
}// distribution
}// detail
}// statistics
}// boost

#endif