/**
 * @file det_test.cpp
 * @author Parikshit Ram (pram@cc.gatech.edu)
 *
 * Unit tests for the functions of the class DTree and the utility functions
 * using this class.
 */
#include <mlpack/core.hpp>
#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

// This trick does not work on Windows.  We will have to comment out the tests
// that depend on it.
#ifndef _WIN32
  #define protected public
  #define private public
#endif

#include <mlpack/methods/det/dtree.hpp>
#include <mlpack/methods/det/dt_utils.hpp>

#ifndef _WIN32
  #undef protected
  #undef private
#endif

using namespace mlpack;
using namespace mlpack::det;
using namespace std;

BOOST_AUTO_TEST_SUITE(DETTest);

// Tests for the private functions.  We cannot perform these if we are on
// Windows because we cannot make private functions accessible using the macro
// trick above.
#ifndef _WIN32
BOOST_AUTO_TEST_CASE(TestGetMaxMinVals)
{
  arma::mat testData(3, 5);

  testData << 4 << 5 << 7 << 3 << 5 << arma::endr
           << 5 << 0 << 1 << 7 << 1 << arma::endr
           << 5 << 6 << 7 << 1 << 8 << arma::endr;

  DTree tree(testData);

  BOOST_REQUIRE_EQUAL(tree.maxVals[0], 7);
  BOOST_REQUIRE_EQUAL(tree.minVals[0], 3);
  BOOST_REQUIRE_EQUAL(tree.maxVals[1], 7);
  BOOST_REQUIRE_EQUAL(tree.minVals[1], 0);
  BOOST_REQUIRE_EQUAL(tree.maxVals[2], 8);
  BOOST_REQUIRE_EQUAL(tree.minVals[2], 1);
}

BOOST_AUTO_TEST_CASE(TestComputeNodeError)
{
  arma::vec maxVals("7 7 8");
  arma::vec minVals("3 0 1");

  DTree testDTree(maxVals, minVals, 5);
  double trueNodeError = -log(4.0) - log(7.0) - log(7.0);

  BOOST_REQUIRE_CLOSE((double) testDTree.logNegError, trueNodeError, 1e-10);

  testDTree.start = 3;
  testDTree.end = 5;

  double nodeError = testDTree.LogNegativeError(5);
  trueNodeError = 2 * log(2.0 / 5.0) - log(4.0) - log(7.0) - log(7.0);
  BOOST_REQUIRE_CLOSE(nodeError, trueNodeError, 1e-10);
}

BOOST_AUTO_TEST_CASE(TestWithinRange)
{
  arma::vec maxVals("7 7 8");
  arma::vec minVals("3 0 1");

  DTree testDTree(maxVals, minVals, 5);

  arma::vec testQuery(3);
  testQuery << 4.5 << 2.5 << 2;

  BOOST_REQUIRE_EQUAL(testDTree.WithinRange(testQuery), true);

  testQuery << 8.5 << 2.5 << 2;

  BOOST_REQUIRE_EQUAL(testDTree.WithinRange(testQuery), false);
}

BOOST_AUTO_TEST_CASE(TestFindSplit)
{
  arma::mat testData(3,5);

  testData << 4 << 5 << 7 << 3 << 5 << arma::endr
           << 5 << 0 << 1 << 7 << 1 << arma::endr
           << 5 << 6 << 7 << 1 << 8 << arma::endr;

  DTree testDTree(testData);

  size_t obDim, trueDim;
  double trueLeftError, obLeftError, trueRightError, obRightError,
      obSplit, trueSplit;

  trueDim = 2;
  trueSplit = 5.5;
  trueLeftError = 2 * log(2.0 / 5.0) - (log(7.0) + log(4.0) + log(4.5));
  trueRightError = 2 * log(3.0 / 5.0) - (log(7.0) + log(4.0) + log(2.5));

  testDTree.logVolume = log(7.0) + log(4.0) + log(7.0);
  BOOST_REQUIRE(testDTree.FindSplit(testData, obDim, obSplit, obLeftError,
      obRightError, 1));

  BOOST_REQUIRE(trueDim == obDim);
  BOOST_REQUIRE_CLOSE(trueSplit, obSplit, 1e-10);

  BOOST_REQUIRE_CLOSE(trueLeftError, obLeftError, 1e-10);
  BOOST_REQUIRE_CLOSE(trueRightError, obRightError, 1e-10);
}

BOOST_AUTO_TEST_CASE(TestSplitData)
{
  arma::mat testData(3, 5);

  testData << 4 << 5 << 7 << 3 << 5 << arma::endr
           << 5 << 0 << 1 << 7 << 1 << arma::endr
           << 5 << 6 << 7 << 1 << 8 << arma::endr;

  DTree testDTree(testData);

  arma::Col<size_t> oTest(5);
  oTest << 1 << 2 << 3 << 4 << 5;

  size_t splitDim = 2;
  double trueSplitVal = 5.5;

  size_t splitInd = testDTree.SplitData(testData, splitDim, trueSplitVal,
      oTest);

  BOOST_REQUIRE_EQUAL(splitInd, 2); // 2 points on left side.

  BOOST_REQUIRE_EQUAL(oTest[0], 1);
  BOOST_REQUIRE_EQUAL(oTest[1], 4);
  BOOST_REQUIRE_EQUAL(oTest[2], 3);
  BOOST_REQUIRE_EQUAL(oTest[3], 2);
  BOOST_REQUIRE_EQUAL(oTest[4], 5);
}
#endif

// Tests for the public functions.

BOOST_AUTO_TEST_CASE(TestGrow)
{
  arma::mat testData(3, 5);

  testData << 4 << 5 << 7 << 3 << 5 << arma::endr
           << 5 << 0 << 1 << 7 << 1 << arma::endr
           << 5 << 6 << 7 << 1 << 8 << arma::endr;

  arma::Col<size_t> oTest(5);
  oTest << 0 << 1 << 2 << 3 << 4;

  double rootError, lError, rError, rlError, rrError;

  rootError = -log(4.0) - log(7.0) - log(7.0);

  lError = 2 * log(2.0 / 5.0) - (log(7.0) + log(4.0) + log(4.5));
  rError =  2 * log(3.0 / 5.0) - (log(7.0) + log(4.0) + log(2.5));

  rlError = 2 * log(1.0 / 5.0) - (log(0.5) + log(4.0) + log(2.5));
  rrError = 2 * log(2.0 / 5.0) - (log(6.5) + log(4.0) + log(2.5));

  DTree testDTree(testData);
  double alpha = testDTree.Grow(testData, oTest, false, 2, 1);

  BOOST_REQUIRE_EQUAL(oTest[0], 0);
  BOOST_REQUIRE_EQUAL(oTest[1], 3);
  BOOST_REQUIRE_EQUAL(oTest[2], 1);
  BOOST_REQUIRE_EQUAL(oTest[3], 2);
  BOOST_REQUIRE_EQUAL(oTest[4], 4);

  // Test the structure of the tree.
  BOOST_REQUIRE(testDTree.Left()->Left() == NULL);
  BOOST_REQUIRE(testDTree.Left()->Right() == NULL);
  BOOST_REQUIRE(testDTree.Right()->Left()->Left() == NULL);
  BOOST_REQUIRE(testDTree.Right()->Left()->Right() == NULL);
  BOOST_REQUIRE(testDTree.Right()->Right()->Left() == NULL);
  BOOST_REQUIRE(testDTree.Right()->Right()->Right() == NULL);

  BOOST_REQUIRE(testDTree.SubtreeLeaves() == 3);

  BOOST_REQUIRE(testDTree.SplitDim() == 2);
  BOOST_REQUIRE_CLOSE(testDTree.SplitValue(), 5.5, 1e-5);
  BOOST_REQUIRE(testDTree.Right()->SplitDim() == 1);
  BOOST_REQUIRE_CLOSE(testDTree.Right()->SplitValue(), 0.5, 1e-5);

  // Test node errors for every node (these are private functions).
#ifndef _WIN32
  BOOST_REQUIRE_CLOSE(testDTree.logNegError, rootError, 1e-10);
  BOOST_REQUIRE_CLOSE(testDTree.Left()->logNegError, lError, 1e-10);
  BOOST_REQUIRE_CLOSE(testDTree.Right()->logNegError, rError, 1e-10);
  BOOST_REQUIRE_CLOSE(testDTree.Right()->Left()->logNegError, rlError, 1e-10);
  BOOST_REQUIRE_CLOSE(testDTree.Right()->Right()->logNegError, rrError, 1e-10);
#endif

  // Test alpha.
  double rootAlpha, rAlpha;
  rootAlpha = std::log(-((std::exp(rootError) - (std::exp(lError) +
      std::exp(rlError) + std::exp(rrError))) / 2));
  rAlpha = std::log(-(std::exp(rError) - (std::exp(rlError) +
      std::exp(rrError))));

  BOOST_REQUIRE_CLOSE(alpha, min(rootAlpha, rAlpha), 1e-10);
}

BOOST_AUTO_TEST_CASE(TestPruneAndUpdate)
{
  arma::mat testData(3, 5);

  testData << 4 << 5 << 7 << 3 << 5 << arma::endr
           << 5 << 0 << 1 << 7 << 1 << arma::endr
           << 5 << 6 << 7 << 1 << 8 << arma::endr;

  arma::Col<size_t> oTest(5);
  oTest << 0 << 1 << 2 << 3 << 4;
  DTree testDTree(testData);
  double alpha = testDTree.Grow(testData, oTest, false, 2, 1);
  alpha = testDTree.PruneAndUpdate(alpha, testData.n_cols, false);

  BOOST_REQUIRE_CLOSE(alpha, numeric_limits<double>::max(), 1e-10);
  BOOST_REQUIRE(testDTree.SubtreeLeaves() == 1);

  double rootError = -log(4.0) - log(7.0) - log(7.0);

  BOOST_REQUIRE_CLOSE(testDTree.LogNegError(), rootError, 1e-10);
  BOOST_REQUIRE_CLOSE(testDTree.SubtreeLeavesLogNegError(), rootError, 1e-10);
  BOOST_REQUIRE(testDTree.Left() == NULL);
  BOOST_REQUIRE(testDTree.Right() == NULL);
}

BOOST_AUTO_TEST_CASE(TestComputeValue)
{
  arma::mat testData(3, 5);

  testData << 4 << 5 << 7 << 3 << 5 << arma::endr
           << 5 << 0 << 1 << 7 << 1 << arma::endr
           << 5 << 6 << 7 << 1 << 8 << arma::endr;

  arma::vec q1(3), q2(3), q3(3), q4(3);

  q1 << 4 << 2 << 2;
  q2 << 5 << 0.25 << 6;
  q3 << 5 << 3 << 7;
  q4 << 2 << 3 << 3;

  arma::Col<size_t> oTest(5);
  oTest << 0 << 1 << 2 << 3 << 4;

  DTree testDTree(testData);
  double alpha = testDTree.Grow(testData, oTest, false, 2, 1);

  double d1 = (2.0 / 5.0) / exp(log(4.0) + log(7.0) + log(4.5));
  double d2 = (1.0 / 5.0) / exp(log(4.0) + log(0.5) + log(2.5));
  double d3 = (2.0 / 5.0) / exp(log(4.0) + log(6.5) + log(2.5));

  BOOST_REQUIRE_CLOSE(d1, testDTree.ComputeValue(q1), 1e-10);
  BOOST_REQUIRE_CLOSE(d2, testDTree.ComputeValue(q2), 1e-10);
  BOOST_REQUIRE_CLOSE(d3, testDTree.ComputeValue(q3), 1e-10);
  BOOST_REQUIRE_CLOSE(0.0, testDTree.ComputeValue(q4), 1e-10);

  alpha = testDTree.PruneAndUpdate(alpha, testData.n_cols, false);

  double d = 1.0 / exp(log(4.0) + log(7.0) + log(7.0));

  BOOST_REQUIRE_CLOSE(d, testDTree.ComputeValue(q1), 1e-10);
  BOOST_REQUIRE_CLOSE(d, testDTree.ComputeValue(q2), 1e-10);
  BOOST_REQUIRE_CLOSE(d, testDTree.ComputeValue(q3), 1e-10);
  BOOST_REQUIRE_CLOSE(0.0, testDTree.ComputeValue(q4), 1e-10);
}

BOOST_AUTO_TEST_CASE(TestVariableImportance)
{
  arma::mat testData(3, 5);

  testData << 4 << 5 << 7 << 3 << 5 << arma::endr
           << 5 << 0 << 1 << 7 << 1 << arma::endr
           << 5 << 6 << 7 << 1 << 8 << arma::endr;

  double rootError, lError, rError, rlError, rrError;

  rootError = -1.0 * exp(-log(4.0) - log(7.0) - log(7.0));

  lError = -1.0 * exp(2 * log(2.0 / 5.0) - (log(7.0) + log(4.0) + log(4.5)));
  rError =  -1.0 * exp(2 * log(3.0 / 5.0) - (log(7.0) + log(4.0) + log(2.5)));

  rlError = -1.0 * exp(2 * log(1.0 / 5.0) - (log(0.5) + log(4.0) + log(2.5)));
  rrError = -1.0 * exp(2 * log(2.0 / 5.0) - (log(6.5) + log(4.0) + log(2.5)));

  arma::Col<size_t> oTest(5);
  oTest << 0 << 1 << 2 << 3 << 4;

  DTree testDTree(testData);
  testDTree.Grow(testData, oTest, false, 2, 1);

  arma::vec imps;

  testDTree.ComputeVariableImportance(imps);

  BOOST_REQUIRE_CLOSE((double) 0.0, imps[0], 1e-10);
  BOOST_REQUIRE_CLOSE((double) (rError - (rlError + rrError)), imps[1], 1e-10);
  BOOST_REQUIRE_CLOSE((double) (rootError - (lError + rError)), imps[2], 1e-10);
}

/**
 * These are not yet implemented.
 *
BOOST_AUTO_TEST_CASE(TestTagTree)
{
  MatType testData(3, 5);

  testData << 4 << 5 << 7 << 3 << 5 << arma::endr
            << 5 << 0 << 1 << 7 << 1 << arma::endr
            << 5 << 6 << 7 << 1 << 8 << arma::endr;

  DTree<>* testDTree = new DTree<>(&testData);

  delete testDTree;
}

BOOST_AUTO_TEST_CASE(TestFindBucket)
{
  MatType testData(3, 5);

  testData << 4 << 5 << 7 << 3 << 5 << arma::endr
            << 5 << 0 << 1 << 7 << 1 << arma::endr
            << 5 << 6 << 7 << 1 << 8 << arma::endr;

  DTree<>* testDTree = new DTree<>(&testData);

  delete testDTree;
}

// Test functions in dt_utils.hpp

BOOST_AUTO_TEST_CASE(TestTrainer)
{

}

BOOST_AUTO_TEST_CASE(TestPrintVariableImportance)
{

}

BOOST_AUTO_TEST_CASE(TestPrintLeafMembership)
{

}
*/

BOOST_AUTO_TEST_SUITE_END();


// BLAS level 3
// symmetric matrices, syr2k

#include <stddef.h>
#include <iostream>
#include <complex>
#include <boost/numeric/bindings/blas/level3.hpp>
#include <boost/numeric/bindings/trans.hpp>
#include <boost/numeric/bindings/upper.hpp>
#include <boost/numeric/bindings/lower.hpp>
#include <boost/numeric/bindings/ublas/symmetric.hpp>
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace blas = boost::numeric::bindings::blas;
namespace bindings = boost::numeric::bindings;

using std::cout;
using std::cin;
using std::endl;

typedef double real_t;
typedef std::complex<real_t> cmplx_t;

typedef ublas::matrix<real_t, ublas::column_major> cm_t;
typedef ublas::matrix<real_t, ublas::row_major> rm_t;
typedef ublas::symmetric_adaptor<cm_t, ublas::upper> ucsa_t;
typedef ublas::symmetric_adaptor<cm_t, ublas::lower> lcsa_t;
typedef ublas::symmetric_adaptor<rm_t, ublas::upper> ursa_t;
typedef ublas::symmetric_adaptor<rm_t, ublas::lower> lrsa_t;

typedef ublas::matrix<cmplx_t, ublas::column_major> ccm_t;
typedef ublas::matrix<cmplx_t, ublas::row_major> crm_t;
typedef ublas::symmetric_adaptor<ccm_t, ublas::upper> cucsa_t;
typedef ublas::symmetric_adaptor<ccm_t, ublas::lower> clcsa_t;
typedef ublas::symmetric_adaptor<crm_t, ublas::upper> cursa_t;
typedef ublas::symmetric_adaptor<crm_t, ublas::lower> clrsa_t;

int main (int argc, char **argv) {
  int n = 0, k = 0;
  if (argc > 1) {
    n = atoi(argv [1]);
  }
  if (argc > 2) {
    k = atoi(argv [2]);
  }

  if (n <= 0) {
    cout << "n -> ";
    cin >> n;
  }
  if (k <= 0) {
    cout << "k -> ";
    cin >> k;
  }

  cm_t ac (n, k);
  rm_t ar (n, k);
  init_m (ac, rws1());
  init_m (ar, rws1());
  print_m (ac, "ac");
  cout << endl;
  print_m (ar, "ar");
  cout << endl << endl;

  cm_t bc (n, k);
  rm_t br (n, k);
  init_m (bc, const_val<real_t> (1));
  init_m (br, const_val<real_t> (1));
  print_m (bc, "bc");
  cout << endl;
  print_m (br, "br");
  cout << endl << endl;

  cm_t cmu (n, n);
  cm_t cml (n, n);
  rm_t rmu (n, n);
  rm_t rml (n, n);
  ucsa_t ucsa (cmu);
  lcsa_t lcsa (cml);
  ursa_t ursa (rmu);
  lrsa_t lrsa (rml);

  blas::syr2k (1.0, ac, bc, 0.0, ucsa);
  blas::syr2k (1.0, ac, bc, 0.0, lcsa);
  blas::syr2k (1.0, ar, br, 0.0, ursa);
  blas::syr2k (1.0, ar, br, 0.0, lrsa);

  print_m (ucsa, "ucsa");
  cout << endl;
  print_m (lcsa, "lcsa");
  cout << endl;
  print_m (ursa, "ursa");
  cout << endl;
  print_m (lrsa, "lrsa");
  cout << endl << endl;

  // part 2

  cm_t act (k, n);
  rm_t art (k, n);
  init_m (act, cls1());
  init_m (art, cls1());
  print_m (act, "act");
  cout << endl;
  print_m (art, "art");
  cout << endl << endl;

  cm_t bct (k, n);
  rm_t brt (k, n);
  init_m (bct, rws1());
  init_m (brt, rws1());
  print_m (bct, "bct");
  cout << endl;
  print_m (brt, "brt");
  cout << endl << endl;

  init_m (cmu, const_val<real_t> (0));
  init_m (cml, const_val<real_t> (0));
  init_m (rmu, const_val<real_t> (0));
  init_m (rml, const_val<real_t> (0));

  blas::syr2k (1.0, bindings::trans(act), bct, 0.0, bindings::upper(cmu));
  blas::syr2k (1.0, bindings::trans(act), bct, 0.0, bindings::lower(cml));
  blas::syr2k (1.0, bindings::trans(art), brt, 0.0, bindings::upper(rmu));
  blas::syr2k (1.0, bindings::trans(art), brt, 0.0, bindings::lower(rml));

  print_m (cmu, "cmu");
  cout << endl;
  print_m (cml, "cml");
  cout << endl;
  print_m (rmu, "rmu");
  cout << endl;
  print_m (rml, "rml");
  cout << endl;

  // complex

  const int n1 = 3;
  const int k1 = 2;

  ccm_t cac (n1, k1);
  crm_t car (n1, k1);
  cac(0,0) = car(0,0) = cmplx_t (1., 1.);
  cac(1,0) = car(1,0) = cmplx_t (2., 1.);
  cac(2,0) = car(2,0) = cmplx_t (3., 1.);
  cac(0,1) = car(0,1) = cmplx_t (1., 1.);
  cac(1,1) = car(1,1) = cmplx_t (2., 1.);
  cac(2,1) = car(2,1) = cmplx_t (3., 1.);
  print_m (cac, "cac");
  cout << endl;
  print_m (car, "car");
  cout << endl << endl;

  ccm_t cbc (n1, k1);
  crm_t cbr (n1, k1);
  cbc(0,0) = cbr(0,0) = cmplx_t (0., -1.);
  cbc(1,0) = cbr(1,0) = cmplx_t (0., -1.);
  cbc(2,0) = cbr(2,0) = cmplx_t (0., -1.);
  cbc(0,1) = cbr(0,1) = cmplx_t (0., -1.);
  cbc(1,1) = cbr(1,1) = cmplx_t (0., -1.);
  cbc(2,1) = cbr(2,1) = cmplx_t (0., -1.);
  print_m (cbc, "cbc");
  cout << endl;
  print_m (cbr, "cbr");
  cout << endl << endl;

  ccm_t ccmu (n1, n1);
  ccm_t ccml (n1, n1);
  crm_t crmu (n1, n1);
  crm_t crml (n1, n1);
  cucsa_t cucsa (ccmu);
  clcsa_t clcsa (ccml);
  cursa_t cursa (crmu);
  clrsa_t clrsa (crml);

  blas::syr2k ( 1.0, cac, cbc, 0.0, cucsa);
  blas::syr2k (cmplx_t(1,0), cac, cbc, cmplx_t(0,0), clcsa);
  blas::syr2k (cmplx_t(1,0), car, cbr, cmplx_t(0,0), cursa);
  blas::syr2k (1.0, car, cbr, 0.0, clrsa);

  print_m (cucsa, "cucsa");
  cout << endl;
  print_m (clcsa, "clcsa");
  cout << endl;
  print_m (cursa, "cursa");
  cout << endl;
  print_m (clrsa, "clrsa");
  cout << endl << endl;

}


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/trigonometric/include/functions/asinpi.hpp>

#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/tests/basic.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>

#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/mhalf.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/nan.hpp>

NT2_TEST_CASE_TPL ( asinpi_real,  NT2_REAL_TYPES)
{
  using nt2::asinpi;
  using nt2::tag::asinpi_;

  typedef typename nt2::meta::call<asinpi_(T)>::type r_t;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_ULP_EQUAL(asinpi(nt2::Nan<T>()), nt2::Nan<r_t>(), 0.5);
#endif
  NT2_TEST_ULP_EQUAL(asinpi(nt2::Half<T>()), T(1)/6, 0.5);
  NT2_TEST_ULP_EQUAL(asinpi(nt2::Mhalf<T>()), -T(1)/6, 0.5);
  NT2_TEST_ULP_EQUAL(asinpi(nt2::Mone<T>()), -0.5, 0.5);
  NT2_TEST_ULP_EQUAL(asinpi(nt2::One<T>()), 0.5, 0.5);
  NT2_TEST_ULP_EQUAL(asinpi(nt2::Zero<T>()), nt2::Zero<r_t>(), 0.5);
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
/*!
 * \file
**/
#ifndef NT2_COMPLEX_CONSTANTS_I_HPP_INCLUDED
#define NT2_COMPLEX_CONSTANTS_I_HPP_INCLUDED

#include <nt2/include/functor.hpp>
#include <nt2/sdk/complex/imaginary.hpp>
#include <boost/simd/sdk/constant/constant.hpp>

/*!
 * \ingroup nt2_complex
 * \defgroup nt2_complex_i I
 *
 * \par Description
 * Constant I : Imaginary unit.
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/i.hpp>
 * \endcode
 *
 * \alias:
 *    J
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class T>
 *     meta::call<tag::I(as_<T>)>::type
 *     I();
 * }
 * \endcode
 *
 *
 * \param T template parameter of I
 *
 * \return type T value
 *
 *
**/

namespace nt2
{
  namespace tag
  {
    /*!
     * \brief Define the tag I of functor I
     *        in namespace nt2::tag for toolbox complex
    **/
    struct I : ext::constant_<I>
    {
      typedef imaginary<double> default_type;
      typedef ext::constant_<I> parent;
    };
  }

  BOOST_SIMD_CONSTANT_IMPLEMENTATION(nt2::tag::I, I)
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(nt2::tag::I, J)
}

#endif


#include "dijkstra.hpp"
#include "carray.hpp"
#include "thread_local_allocator.hpp"
#include <algorithm>
#include <boost/heap/fibonacci_heap.hpp>
#include <cmath>

using namespace boost::heap;

namespace {
enum class state : unsigned char {
    unexplored,
    fringe,
    settled,
};
}

void sssp::dijkstra(array_slice<const array_slice<const edge>> edges_by_node,
                    array_slice<double> distances,
                    array_slice<size_t> predecessors,
                    array_slice<const bool> prefill_fringe) {
    size_t node_count = edges_by_node.size();

    auto cmp_distance = [&](size_t a, size_t b) { return distances[a] > distances[b]; };
    using distance_queue_t =
        fibonacci_heap<size_t, compare<decltype(cmp_distance)>, allocator<thread_local_allocator<size_t>>>;
    distance_queue_t distance_queue(cmp_distance);
    std::vector<distance_queue_t::handle_type> distance_queue_handles(node_count);

    std::vector<state> states(node_count, state::unexplored);
    if (prefill_fringe.size()) {
        for (size_t n = 0; n < node_count; ++n) {
            if (prefill_fringe[n]) {
                states[n] = state::fringe;
                distance_queue_handles[n] = distance_queue.push(n);
            }
        }
    } else {
        states[0] = state::fringe;
        distance_queue_handles[0] = distance_queue.push(0);
    }

    while (!distance_queue.empty()) {
        size_t node = distance_queue.top();
        distance_queue.pop();
        states[node] = state::settled;

        for (const edge& edge : edges_by_node[node]) {
            if (states[edge.destination] != state::settled &&
                distances[node] + edge.cost < distances[edge.destination]) {
                distances[edge.destination] = distances[node] + edge.cost;
                predecessors[edge.destination] = node;
                if (states[edge.destination] == state::unexplored) {
                    states[edge.destination] = state::fringe;
                    distance_queue_handles[edge.destination] = distance_queue.push(edge.destination);
                } else {
                    distance_queue.update(distance_queue_handles[edge.destination]);
                }
            }
        }
    }
}


#ifndef _jsc_util_interval_hpp_included_
#define _jsc_util_interval_hpp_included_

#include <boost/config.hpp>

#include <math.h>

#include <iostream>
#include <map>
#include <set>
#include <sstream>
#include <vector>

#include <boost/lambda/bind.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/tokenizer.hpp>

#include <jsc/util/log.hpp>

using namespace std;
using namespace boost;
using namespace boost::lambda;

namespace jsc
{
namespace util
{

//! A function which checks whether two intervals overlap.
/*!
 * \return whether [a, b) and [c, d) overlap.
 */
template<class T, class Compare>
bool interval_overlap(T const & a, T const & b, T const & c, T const & d, Compare comp = Compare())
{
	return (!(comp(b, c) || comp(d, a)));
}

//! A function which computes the overlapping length of two intervals.
/*!
 * \return the overlapping length of [a, b) and [c, d).
 */
template<class T, class Compare>
double compute_interval_overlap(T const & a, T const & b, T const & c, T const & d, Compare comp = Compare())
{
	double length = 0;
	if (!(comp(b, c) || comp(d, a)))
	{
		if (comp(a, c))
		{
			if (comp(b, d))
			{
				length = b - c;
			}
			else
			{
				length = d - c;
			}
		}
		else
		{
			if (comp(b, d))
			{
				length = b - a;
			}
			else
			{
				length = d - a;
			}
		}
	}
	return length;
}

//! A class representing a list of intervals.
/*!
 * The intervals in the list are sorted according to their start positions and do not overlap with each other.
 */
template <class T, class Compare = less<T> >
class interval_list
{
private:
	//! The comparator.
	Compare comp;
	//! The internal representation of the list of intervals: [starts[0], end[0]), [starts[1], end[1]), ...
	vector<T> starts;
	//! The internal representation of the list of intervals: [starts[0], end[0]), [starts[1], end[1]), ...
	vector<T> ends;

public:
	//! A constructor.
	interval_list()
		: comp(Compare())
	{
	}

	//! A constructor taking an explicit comparator.
	explicit
	interval_list(Compare const & c)
		: comp(c)
	{
	}

	//! Copy constructor.
	interval_list(interval_list<T, Compare> const & il)
		: comp(il.comp), starts(il.starts), ends(il.ends)
	{
	}

	//! Gets the number of intervals.
	unsigned long get_num_intervals() const
	{
		return starts.size();
	}

	//! Gets the starts vector.
	vector<T> const & get_starts() const
	{
		return starts;
	}

	//! Gets the ends vector.
	vector<T> const & get_ends() const
	{
		return ends;
	}

	//! Counts the number of intervals in the list that overlap with the given interval.
	/*!
	 * \sa count_overlap_il().
	 */
	long count_overlap(T const & start, T const & end) const
	{
		if (!comp(start, end))	/* [start, end) == nil */
		{
			return 0;	// always return 0 in this case
		}

		typename vector<T>::const_iterator s_starts_itr, e_ends_itr;
		s_starts_itr = lower_bound(starts.begin(), starts.end(), start);
		e_ends_itr = lower_bound(ends.begin(), ends.end(), end);

		long s_starts_idx, e_ends_idx;
		s_starts_idx = distance(starts.begin(), s_starts_itr);
		e_ends_idx = distance(ends.begin(), e_ends_itr);
		long i;
		long total_count = 0;
		for (i = max((long)0, s_starts_idx - 1); i <= min((long)starts.size() - 1, e_ends_idx); ++i)
		{
			if (interval_overlap<T, Compare>(starts[i], ends[i], start, end, comp))
			{
				total_count++;
			}
		}

		return total_count;
	}

	//! Counts the number of intervals in the list that overlap with the given interval list.
	/*!
	 * \sa count_overlap().
	 */
	long count_overlap_il(interval_list<T, Compare> const & il) const
	{
		long total_count = 0;
		int n = starts.size();

		for (int i = 0; i < n; ++i)
		{
			if (il.check_overlap(starts[i], ends[i]))
			{
				total_count++;
			}
		}
		return total_count;
	}

	//! Checks whether the interval list overlaps with the interval [start, end).
	bool check_overlap(T const & start, T const & end) const
	{
		if (!comp(start, end))	/* [start, end) == nil */
		{
			return false;	// always return false in this case
		}

		typename vector<T>::const_iterator s_starts_itr, e_ends_itr;
		s_starts_itr = lower_bound(starts.begin(), starts.end(), start);
		e_ends_itr = lower_bound(ends.begin(), ends.end(), end);

		long s_starts_idx, e_ends_idx;
		s_starts_idx = distance(starts.begin(), s_starts_itr);
		e_ends_idx = distance(ends.begin(), e_ends_itr);
		long i;
		for (i = max((long)0, s_starts_idx - 1); i <= min((long)starts.size() - 1, e_ends_idx); ++i)
		{
			if (interval_overlap<T, Compare>(starts[i], ends[i], start, end, comp))
			{
				return true;
			}
		}

		return false;
	}

	static interval_list<T> get_interval_overlap(T const & a, T const & b, T const & c, T const & d, Compare comp = Compare())
	{
		interval_list<T> il;
		if (!(comp(b, c) || comp(d, a)))
		{
			if (comp(a, c))
			{
				if (comp(b, d))
				{
					il.add_interval(c, b);
				}
				else
				{
					il.add_interval(c, d);
				}
			}
			else
			{
				if (comp(b, d))
				{
					il.add_interval(a, b);
				}
				else
				{
					il.add_interval(a, d);
				}
			}
		}
		return il;
	}

	//! Gets the overlap of the interval list with the interval [start, end).
	interval_list<T> get_overlap_il(T const & start, T const & end) const
	{
		interval_list<T> il;
		if (!comp(start, end))	/* [start, end) == nil */
		{
			return il;	// always return 0 in this case
		}

		typename vector<T>::const_iterator s_starts_itr, e_ends_itr;
		s_starts_itr = lower_bound(starts.begin(), starts.end(), start);
		e_ends_itr = lower_bound(ends.begin(), ends.end(), end);

		long s_starts_idx, e_ends_idx;
		s_starts_idx = distance(starts.begin(), s_starts_itr);
		e_ends_idx = distance(ends.begin(), e_ends_itr);
		long i;
		for (i = max((long)0, s_starts_idx - 1); i <= min((long)starts.size() - 1, e_ends_idx); ++i)
		{
			il.add_interval_list(
					get_interval_overlap(starts[i], ends[i], start, end, comp));
		}

		return il;
	}

	//! Get the overlap of the interval list with another interval list.
	interval_list<T> get_overlap_il(interval_list<T, Compare> const & il) const
	{
		interval_list<T> rst_il;
		int n = il.starts.size();
		for (int i = 0; i < n; i++)
		{
			rst_il.add_interval_list(get_overlap_il(il.starts[i], il.ends[i]));
		}
		return rst_il;
	}

	//! Computes the overlap of the interval list with the interval [start, end).
	/*!
	 * \sa compute_overlap_il().
	 */
	double compute_overlap(T const & start, T const & end) const
	{
		if (!comp(start, end))	/* [start, end) == nil */
		{
			return 0;	// always return 0 in this case
		}

		typename vector<T>::const_iterator s_starts_itr, e_ends_itr;
		s_starts_itr = lower_bound(starts.begin(), starts.end(), start);
		e_ends_itr = lower_bound(ends.begin(), ends.end(), end);

		long s_starts_idx, e_ends_idx;
		s_starts_idx = distance(starts.begin(), s_starts_itr);
		e_ends_idx = distance(ends.begin(), e_ends_itr);
		long i;
		double total_length = 0;
		for (i = max((long)0, s_starts_idx - 1); i <= min((long)starts.size() - 1, e_ends_idx); ++i)
		{
			total_length += compute_interval_overlap<T, Compare>(starts[i], ends[i], start, end, comp);
		}

		return total_length;
	}

	bool check_single_overlap(T const & start, T const & end, double const & threshold) const
	{
		bool found = false;
		if (!comp(start, end))	/* [start, end) == nil */
		{
			return false;	// always return false in this case
		}

		typename vector<T>::const_iterator s_starts_itr, e_ends_itr;
		s_starts_itr = lower_bound(starts.begin(), starts.end(), start);
		e_ends_itr = lower_bound(ends.begin(), ends.end(), end);

		long s_starts_idx, e_ends_idx;
		s_starts_idx = distance(starts.begin(), s_starts_itr);
		e_ends_idx = distance(ends.begin(), e_ends_itr);
		long i;
		for (i = max((long)0, s_starts_idx - 1); i <= min((long)starts.size() - 1, e_ends_idx); ++i)
		{
			if (threshold <= compute_interval_overlap<T, Compare>(starts[i], ends[i], start, end, comp)) {
				found = true;
				break;
			}
		}

		return found;
	}

	//! Computes the overlap of the interval list with another interval list.
	/*!
	 * \sa compute_overlap().
	 */
	double compute_overlap_il(interval_list<T, Compare> const & il) const
	{
		double total_length = 0;
		int n = il.starts.size();
		for (int i = 0; i < n; i++)
		{
			total_length += compute_overlap(il.starts[i], il.ends[i]);
		}
		return total_length;
	}

	//! Computes the total length of the intervals in this interval list.
	double compute_total_length() const
	{
		double total_length = 0;
		int n = starts.size();
		for (int i = 0; i < n; i++)
		{
			total_length += ends[i] - starts[i];
		}
		return total_length;
	}

	//! Finds in the interval list the index of the interval which contains the given interval [start, end).
	/*!
	 * \return the index of the interval containing [start, end); otherwise starts.size().
	 * \sa contains_interval(T const & start, T const & end)
	 */
	long idx_containing_interval(T const & start, T const & end) const
	{
		if (!comp(start, end))	/* [start, end) == nil */
		{
			return 0;	// always return 0 in this case
		}

		typename vector<T>::const_iterator s_starts_itr;
		s_starts_itr = lower_bound(starts.begin(), starts.end(), start);

		long s_starts_idx;
		s_starts_idx = distance(starts.begin(), s_starts_itr);
		if (s_starts_idx >= 0 && s_starts_idx < (long)starts.size() &&
				!comp(start, starts[s_starts_idx]) &&
				!comp(ends[s_starts_idx], end))
		{
			return s_starts_idx;
		}
		if (s_starts_idx - 1 >= 0 && s_starts_idx - 1 < (long)starts.size() &&
				!comp(start, starts[s_starts_idx - 1]) &&
				!comp(ends[s_starts_idx - 1], end))
		{
			return s_starts_idx - 1;
		}

		return starts.size();
	}

	//! Checks whether the interval list contains [start, end).
	/*!
	 * \sa idx_containing_interval()
	 */
	bool contains_interval(T const & start, T const & end) const
	{
		if (!comp(start, end))	/* [start, end) == nil */
		{
			return true;
		}

		typename vector<T>::const_iterator s_starts_itr;
		s_starts_itr = lower_bound(starts.begin(), starts.end(), start);

		long s_starts_idx;
		s_starts_idx = distance(starts.begin(), s_starts_itr);
		if (s_starts_idx >= 0 && s_starts_idx < (long)starts.size() &&
				!comp(start, starts[s_starts_idx]) &&
				!comp(ends[s_starts_idx], end))
		{
			return true;
		}
		if (s_starts_idx - 1 >= 0 && s_starts_idx - 1 < (long)starts.size() &&
				!comp(start, starts[s_starts_idx - 1]) &&
				!comp(ends[s_starts_idx - 1], end))
		{
			return true;
		}

		return false;
	}

	//! Fill in gaps if their length is less than the given threshold
	void fill_in_gaps(double const & threshold)
	{
		int n = starts.size();
		if (n == 0)
		{
			return;
		}

		vector<T> new_starts, new_ends;
		T last_end;
		new_starts.push_back(starts[0]);
		last_end = ends[0];
		for (int i = 1; i < n; ++i)
		{
			if (starts[i] - last_end < threshold)
			{
				// fill in the gap
			}
			else
			{
				// copy the last end and current start
				new_ends.push_back(last_end);
				new_starts.push_back(starts[i]);
			}
			// update last end
			last_end = ends[i];
		}
		new_ends.push_back(last_end);
		starts = new_starts;
		ends = new_ends;
	}

	//! Add [start, end) to the interval list.
	/*!
	 * Overlapping intervals in the list will be merged.
	 * \sa add_starts_sizes() and add_interval_list()
	 */
	interval_list<T> & add_interval(T const & start, T const & end)
	{
		if (!comp(start, end))	/* [start, end) == nil */
		{
			return *this;
		}

		typename vector<T>::iterator s_starts_itr, s_ends_itr, e_starts_itr, e_ends_itr;
		s_starts_itr = lower_bound(starts.begin(), starts.end(), start);
		s_ends_itr = lower_bound(ends.begin(), ends.end(), start);
		e_starts_itr = lower_bound(starts.begin(), starts.end(), end);
		e_ends_itr = lower_bound(ends.begin(), ends.end(), end);

		int s_starts_idx, s_ends_idx, e_starts_idx, e_ends_idx;
		s_starts_idx = distance(starts.begin(), s_starts_itr);
		s_ends_idx = distance(ends.begin(), s_ends_itr);
		e_starts_idx = distance(starts.begin(), e_starts_itr);
		e_ends_idx = distance(ends.begin(), e_ends_itr);
		bool s_on_interval = (s_starts_idx - s_ends_idx == 1);
		bool e_on_interval = (e_starts_idx - e_ends_idx == 1);

		typename vector<T>::iterator starts_itr = starts.erase(s_starts_itr, e_starts_itr);
		typename vector<T>::iterator ends_itr = ends.erase(s_ends_itr, e_ends_itr);
		if (s_on_interval && e_on_interval)
		{
		}
		else if (!s_on_interval && !e_on_interval)
		{
			starts.insert(starts_itr, start);
			ends.insert(ends_itr, end);
		}
		else if (s_on_interval && !e_on_interval)
		{
			ends.insert(ends_itr, end);
		}
		else
		{
			starts.insert(starts_itr, start);
		}

		return *this;
	}

	//! Add another interval list to the interval list.
	/*!
	 * \sa add_starts_sizes() and add_interval()
	 */
	void add_interval_list(interval_list<T, Compare> const & il)
	{
		int n = il.starts.size();
		for (int i = 0; i < n; i++)
		{
			add_interval(il.starts[i], il.ends[i]);
		}
	}

	//! Add another interval list to the interval list.
	/*!
	 * \param starts the starts vector of the interval list to be added.
	 * \param sizes  the sizes vector of the interval list to be added.
	 * \sa add_interval_list() and add_interval()
	 */
	void add_starts_sizes(vector<T> const & starts, vector<T> const & sizes)
	{
		int n = starts.size();
		for (int i = 0; i < n; i++)
		{
			add_interval(starts[i], starts[i] + sizes[i]);
		}
	}

	//! Add another interval list to the interval list.
	/*!
	 * \param starts the starts vector of the interval list to be added.
	 * \param ends the ends vector of the interval list to be added.
	 * \sa add_interval_list() and add_interval()
	 */
	void add_starts_ends(vector<T> const & starts, vector<T> const & ends)
	{
		int n = starts.size();
		for (int i = 0; i < n; i++)
		{
			add_interval(starts[i], ends[i]);
		}
	}

	//! Checks whether the total coverage of the interval list overlaps with that of another interval list.
	/*!
	 * coverage = [min(starts), (max(ends))
	 */
	bool coverage_overlap(interval_list<T, Compare> const & il2)
	{
		int n1 = starts.size();
		int n2 = il2.starts.size();
		if (n1 == 0 || n2 == 0)
		{
			return false;
		}
		return interval_overlap<T, Compare>(starts[0], ends[n1 - 1], il2.starts[0], il2.ends[n2 - 1], comp);
	}

	T find_min_uncovered(T start_from) const {
		if (get_num_intervals() == 0) {
			return start_from;
		} else {
			typename vector<T>::const_iterator s_starts_itr;
			s_starts_itr = lower_bound(starts.begin(), starts.end(), start_from);

			long s_starts_idx;
			s_starts_idx = distance(starts.begin(), s_starts_itr);
			long i;
			for (i = max((long)0, (long)s_starts_idx - 1);
					i < (long)get_num_intervals(); ++i)
			{
				if (starts[i] > start_from) {
					return start_from;
				} else if (ends[i] > start_from) {
					start_from = ends[i];
				} else {
					continue;
				}
			}
			return start_from;
		}
	}
};

//! Default printing of an interval_list.
template<class T, class Compare>
ostream & operator << (ostream& os, interval_list<T, Compare> const & il)
{
	int n = il.get_starts().size();
	for (int i = 0; i < n; i++)
	{
		os << "[" << il.get_starts()[i] << "," << il.get_ends()[i] << "), ";
	}
	return os;
}

} /* end of util */

} /* end of jsc */

#endif


// Copyright (C) 2013 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <glog/logging.h>
#include <algorithm>
#include <vector>
#include "gtest/gtest.h"

#include "theia/math/util.h"
#include "theia/util/random.h"
#include "theia/vision/sfm/pose/five_point_relative_pose.h"
#include "theia/vision/sfm/pose/util.h"
#include "theia/test/benchmark.h"

namespace theia {
namespace {
using Eigen::AngleAxisd;
using Eigen::Map;
using Eigen::Matrix3d;
using Eigen::Quaterniond;
using Eigen::Vector2d;
using Eigen::Vector3d;

double RotationAngularDistance(const Matrix3d& rot1, const Matrix3d& rot2) {
  const Matrix3d relative_rotation = rot1.transpose() * rot2;
  return acos((relative_rotation.trace() - 1.0) / 2.0);
}

// Tests that the three point pose works correctly by taking the passed
// points_3d, projecting them to get image one rays, transforming by
// (expected_rotation, expected_translation) to get image two rays and then
// verifying that the ThreePointEssentialMatrix function returns
// (test_rotation, test_translation_direction) among its solutions.
// Noise can be added to the image projections by setting projection_noise.
// The thresholds for rotation and translation similarity can be controlled
// by max_rotation_difference and max_difference_between_translation.
void TestFivePointResultWithNoise(const Vector3d points_3d[5],
                                  const double projection_noise_std_dev,
                                  const Matrix3d& expected_rotation,
                                  const Vector3d& expected_translation,
                                  const double max_rotation_difference,
                                  const double max_angle_between_translation) {
  InitRandomGenerator();

  // Calculates the image points in both views.
  Vector2d view_one_points[5];
  Vector2d view_two_points[5];
  for (int i = 0; i < 5; ++i) {
    const Vector3d proj_3d =
        expected_rotation * points_3d[i] + expected_translation;
    view_one_points[i] = points_3d[i].hnormalized();
    view_two_points[i] = proj_3d.hnormalized();
  }

  if (projection_noise_std_dev) {
    // Adds noise to both of the points.
    for (int i = 0; i < 5; ++i) {
      AddNoiseToProjection(projection_noise_std_dev,
                           &view_one_points[i]);
      AddNoiseToProjection(projection_noise_std_dev,
                           &view_two_points[i]);
    }
  }

  // Calculates the essential matrix, this may return multiple solutions.
  std::vector<Matrix3d> soln_rotations;
  std::vector<Vector3d> soln_translations;
  EXPECT_TRUE(FivePointRelativePose(view_one_points, view_two_points,
                                    &soln_rotations, &soln_translations));
  CHECK_GT(soln_rotations.size(), 0);

  // Among the returned solutions verify that at least one is close to the
  // expected translation and rotation.
  bool matched_transform = false;
  for (int n = 0; n < soln_rotations.size(); ++n) {
    double rotation_difference =
        RotationAngularDistance(expected_rotation, soln_rotations[n]);

    bool matched_rotation = (rotation_difference < max_rotation_difference);

    // The translation is only known up to scale so this verifies that the
    // translations have matching directions.
    double translation_angle_difference =
        fabs(expected_translation.dot(soln_translations[n]) /
             (expected_translation.norm() * soln_translations[n].norm()));
    // Occasionally there is an odd floating point error that causes a nan in
    // the acos below, so we explicitly cap the angle values to be valid.
    translation_angle_difference =
        acos(std::min(1.0, std::max(0.0, translation_angle_difference)));

    bool matched_translation =
        (translation_angle_difference < max_angle_between_translation);

    if (matched_translation && matched_rotation) {
      matched_transform = true;
    }
  }
  EXPECT_TRUE(matched_transform);
}

void BasicTest() {
  // Ground truth essential matrix.
  const Vector3d points_3d[5] = { Vector3d(-1.0, 3.0, 3.0),
                                  Vector3d(1.0, -1.0, 2.0),
                                  Vector3d(3.0, 1.0, 2.5),
                                  Vector3d(-1.0, 1.0, 2.0),
                                  Vector3d(2.0, 1.0, 3.0) };
  const Matrix3d soln_rotation = Quaterniond(
      AngleAxisd(Radians(13.0), Vector3d(0.0, 0.0, 1.0))).toRotationMatrix();
  const Vector3d soln_translation(1.0, 1.0, 1.0);
  const double kNoise = 0.0 / 512.0;
  const double kMaxAllowedRotationDifference = Radians(1e-2);
  const double kMaxAllowedAngleBetweenTranslations = Radians(1e-2);

  TestFivePointResultWithNoise(points_3d,
                               kNoise,
                               soln_rotation,
                               soln_translation,
                               kMaxAllowedRotationDifference,
                               kMaxAllowedAngleBetweenTranslations);
}

TEST(FivePointRelativePose, Basic) {
  BasicTest();
}

BENCHMARK(FivePointRelativePose, BasicBenchmark, 100, 1000) {
  BasicTest();
}

TEST(FivePointRelativePose, NoiseTest) {
  // Ground truth essential matrix.
  const Vector3d points_3d[5] = {Vector3d(-1.0, 3.0, 3.0),
                                 Vector3d(1.0, -1.0, 2.0),
                                 Vector3d(3.0, 1.0, 2.5),
                                 Vector3d(-1.0, 1.0, 2.0),
                                 Vector3d(2.0, 1.0, 3.0)};
  const Matrix3d soln_rotation = Quaterniond(
      AngleAxisd(Radians(13.0), Vector3d(0.0, 0.0, 1.0))).toRotationMatrix();

  const Vector3d soln_translation(1.0, 1.0, 1.0);
  const double kNoise = 1.0 / 512.0;
  const double kMaxAllowedRotationDifference = Radians(1.0);
  const double kMaxAllowedAngleBetweenTranslations = Radians(1.0);

  TestFivePointResultWithNoise(points_3d,
                               kNoise,
                               soln_rotation,
                               soln_translation,
                               kMaxAllowedRotationDifference,
                               kMaxAllowedAngleBetweenTranslations);
}

TEST(FivePointRelativePose, ForwardMotion) {
  // Ground truth essential matrix.
  const Vector3d points_3d[5] = {Vector3d(-1.0, 3.0, 3.0),
                                 Vector3d(1.0, -1.0, 2.0),
                                 Vector3d(3.0, 1.0, 2.0),
                                 Vector3d(-1.0, 1.0, 2.0),
                                 Vector3d(2.0, 1.0, 3.0)};
  const Matrix3d soln_rotation = Quaterniond(
      AngleAxisd(Radians(13.0), Vector3d(0.0, 0.0, 1.0))).toRotationMatrix();

  const Vector3d soln_translation(0.0, 0.0, 1.0);
  const double kNoise = 1.0 / 512.0;
  const double kMaxAllowedRotationDifference = Radians(2.0);
  const double kMaxAllowedAngleBetweenTranslations = Radians(5.0);

  TestFivePointResultWithNoise(points_3d,
                               kNoise,
                               soln_rotation,
                               soln_translation,
                               kMaxAllowedRotationDifference,
                               kMaxAllowedAngleBetweenTranslations);
}

TEST(FivePointRelativePose, NoRotation) {
  // Ground truth essential matrix.
  const Vector3d points_3d[5] = {Vector3d(-1.0, 3.0, 3.0),
                                 Vector3d(1.0, -1.0, 2.0),
                                 Vector3d(3.0, 1.0, 2.0),
                                 Vector3d(-1.0, 1.0, 2.0),
                                 Vector3d(2.0, 1.0, 3.0)};
  const Matrix3d soln_rotation = Quaterniond(
      AngleAxisd(Radians(0.0), Vector3d(0.0, 0.0, 1.0))).toRotationMatrix();

  const Vector3d soln_translation(1.0, 1.0, 1.0);
  const double kNoise = 1.0 / 512.0;
  const double kMaxAllowedRotationDifference = Radians(1.0);
  const double kMaxAllowedAngleBetweenTranslations = Radians(1.0);

  TestFivePointResultWithNoise(points_3d,
                               kNoise,
                               soln_rotation,
                               soln_translation,
                               kMaxAllowedRotationDifference,
                               kMaxAllowedAngleBetweenTranslations);
}

}  // namespace
}  // namespace theia


// shared_ptr::reset example
#include <iostream>
#include <memory>
#include <boost/shared_ptr.hpp>
#include <helper/bounding_box.h>
#include <helper/CommonCV.h>
#include <Eigen/Dense>
#include "../rapidxml/rapidxml.hpp"
#include "../rapidxml/rapidxml_utils.hpp"
#include <assert.h>
#include <string.h>
using namespace std;

using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::Map;
using Eigen::MatrixXf;
using Eigen::Matrix;

using namespace rapidxml;

class A {
public:
  virtual void foo() {
    cout << "A's foo" << endl;
  }
  virtual void foo_invoker() {
    cout << "A's foo_invoker, about to call foo()" << endl;
    foo();
  }
};

class B: public A {
public:
  virtual void foo() {
    cout << "B's foo" << endl;
  }
  virtual void foo_invoker() {
    A::foo_invoker();
  }
};

void doSomethingMat(Mat & mat) {
  Mat M = (Mat_<double>(3,3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);
  M.copyTo(mat);
}

void populateTestEigenMap() {
  // Test Eigen Map
  vector<vector<float> > src;
  for (int i = 0; i < 4; i ++) {
    vector<float> temp;
    for (int j = 0; j < 5; j ++) {
      temp.push_back(j);
    }
    src.push_back(temp);
  }

  Map<Matrix<float,1,5,Eigen::RowMajor> > mf(src[0].data());
  for (int j = 0; j < 5; j ++) {
    cout << mf(0, j) << " ";
  }
  cout << endl;
}

void populateTestEigenFunctions() {
  MatrixXd test(3, 3);
  test <<  4,-1,2, -1,6,0, 2,0,5;

  cout << test << endl;
  cout << "test.col(1).mean():" << test.col(1).mean() << endl;

  Eigen::LLT<MatrixXd> llt(test); // compute the Cholesky decomposition of A
  MatrixXd L = llt.matrixL();

  cout << L << endl;
  cout << L * L.transpose() << endl;

  Eigen::EigenSolver<MatrixXd>eigen_solver(test, true);

  MatrixXd ones = MatrixXd::Ones(3,3);
  Eigen::EigenSolver<MatrixXd> es(ones);
  cout << "The first eigenvector of the 3x3 matrix of ones is:"
      << endl << es.eigenvectors() << endl;
  cout << es.eigenvalues().cols() << endl;

}

void populateTestRapidXml () {
    rapidxml::file<> xmlFile("/home/jimxing/Downloads/Tracking_Sequences/ILSVRC2015/Annotations/VID/train/ILSVRC2015_VID_train_0004/ILSVRC2015_val_00069001/000000.xml"); // Default template is char
    rapidxml::xml_document<> doc;
    doc.parse<0>(xmlFile.data());

    cout << "first node name:" << doc.first_node()->name() << endl;

    assert(strcmp(doc.first_node()->name(), "annotation") == 0);
    xml_node<> *annotation_node = doc.first_node();
    for (xml_node<> *child = annotation_node->first_node(); child; child = child->next_sibling()) {
      // do stuff with child
      cout << child->name() << endl;
      if (strcmp(child->name(), "object") == 0) {
        for (xml_node<> *track_obj_node = child->first_node(); track_obj_node; track_obj_node = track_obj_node->next_sibling()) {
          if (strcmp(track_obj_node->name(), "trackid") == 0) {
            cout << "\t" << "trackid: " << track_obj_node->value() << endl;
          }
          else if (strcmp(track_obj_node->name(), "name") == 0) {
            cout << "\t" << "name: " << track_obj_node->value() << endl;
          }
          else if (strcmp(track_obj_node->name(), "occluded") == 0) {
            cout << "\t" << "occluded: " << track_obj_node->value() << endl;
          }
          else if (strcmp(track_obj_node->name(), "generated") == 0) {
            cout << "\t" << "generated: " << track_obj_node->value() << endl;
          }
          else if (strcmp(track_obj_node->name(), "bndbox") == 0) {
            xml_node<> *xmax_node = track_obj_node->first_node();
            xml_node<> *xmin_node = xmax_node->next_sibling();
            xml_node<> *ymax_node = xmin_node->next_sibling();
            xml_node<> *ymin_node = ymax_node->next_sibling();

            cout << "\t\tx1: " << xmin_node->value() << endl
            << "\t\ty1: " << ymin_node->value() << endl
            << "\t\tx2: " << xmax_node->value() << endl
            << "\t\ty2: " << ymax_node->value() << endl;
          }
        }
      }
    }
}

int main (int argc, char *argv[]) {
  boost::shared_ptr<BoundingBox> sp;  // empty

  sp.reset (new BoundingBox(1,1,1,1));       // takes ownership of pointer
  std::cout << sp->x1_ << '\n';
  std::cout << sp->y1_ << '\n';
  std::cout << sp->x2_ << '\n';
  std::cout << sp->y2_ << '\n';

  boost::shared_ptr<BoundingBox> sp_2;
  sp_2 = sp;

  cout << "before reset, sp_2:" << endl;
  std::cout << sp_2->x1_ << '\n';
  std::cout << sp_2->y1_ << '\n';
  std::cout << sp_2->x2_ << '\n';
  std::cout << sp_2->y2_ << '\n';

  sp.reset (new BoundingBox(3,3,3,3));       // deletes managed object, acquires new pointer
  std::cout << sp->x1_ << '\n';
  std::cout << sp->y1_ << '\n';
  std::cout << sp->x2_ << '\n';
  std::cout << sp->y2_ << '\n';

  cout << "after reset sp, sp_2:" << endl;
  std::cout << sp_2->x1_ << '\n';
  std::cout << sp_2->y1_ << '\n';
  std::cout << sp_2->x2_ << '\n';
  std::cout << sp_2->y2_ << '\n';

//   sp.reset();               // deletes managed object

  vector<int> temp = {1,2,3,4,5,6,7,8,9,10};
  // auto engine = std::default_random_engine{};
  cout << "time(NULL) " << time(NULL) << endl;
  // std::mt19937 engine(time(NULL));
  std::mt19937 engine;
  engine.seed(time(NULL));

  for (int i = 0; i < 5 ; i ++) {
    vector<int> this_temp(temp);
    std::shuffle(this_temp.begin(), this_temp.end(), engine);
    for (int j = 0; j < this_temp.size() ; j ++) {
      cout << this_temp[j] << " ";
    }
    cout << endl;
  }

  cout << std::min(1,2) << endl;

  // TODO: unit test assign &Bbox to a bbox, creates a copy!
  BoundingBox box_a(0, 0, 255, 255);
  BoundingBox &box_b = box_a;

  BoundingBox box_c;
  box_c = box_b;

  box_a.x1_ = 10;
  box_a.y1_ = 10;

  cout << "box_a:" << box_a.x1_ << ", "<< box_a.y1_ << ", " << box_a.x2_ << ", " << box_a.y2_ << endl;
  cout << "box_b:" << box_b.x1_ << ", "<< box_b.y1_ << ", " << box_b.x2_ << ", " << box_b.y2_ << endl;
  cout << "box_c:" << box_c.x1_ << ", "<< box_c.y1_ << ", " << box_c.x2_ << ", " << box_c.y2_ << endl;

  // TODO: unit test dynamic binding for tracker_->Init, which function to call will be based on the dynamic type, virtual table looked up during runtime
  A a;
  B b;
  cout << "a.foo_invoker():" << endl;
  a.foo_invoker();
  cout << "b.foo_invoker():" << endl;
  b.foo_invoker();

  // Test memcpy
  vector<float> src = {1, 2, 3, 4};
  vector<float> dst (src);
  // memcpy(&dst, &src, 4);
  cout << "dst.size(): " << dst.size() << endl;

  for (int i =0 ; i < 4; i ++) { // note that beyond will be undefined
    cout << dst[i] << " ";
  }
  cout << endl;

  Mat temp_mat = (Mat_<double>(3,3) << 0, 0, 0, 0, 0, 0, 0, 0, 0);
  doSomethingMat(temp_mat);
  cout << "after doSomethingMat: \n" << temp_mat << endl;
  temp_mat.at<double>(2,2) = 10;
  cout << "after assign (2, 2): \n" << temp_mat << endl;

  doSomethingMat(temp_mat);
  cout << "after doSomethingMat: \n" << temp_mat << endl;

  // populateTestEigenMap();
  // populateTestEigenFunctions();
  populateTestRapidXml();

  return 0;
}


//
//  Copyright Toon Knapen, Karl Meerbergen
//  Copyright Thomas Klimpel 2008
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_TEST_UBLAS_HEEV_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_TEST_UBLAS_HEEV_HPP

#include "../../blas/test/random.hpp"

#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>

#include <limits>

inline float conj(float v) { return v; }
inline double conj(double v) { return v; }
inline float real(float v) { return v; }
inline double real(double v) { return v; }

// Fill a banded matrix
template <typename M>
void fill_banded(M& m) {
   typedef typename M::size_type  size_type ;
   typedef typename M::value_type value_type ;

   int size = m.size2() ;
   int band = m.upper() ;

   for (int i=0; i<size; ++i) {
      for (int j=std::max(0,i-band); j<i; ++j) m(j,i) = random_value<value_type>();
      m(i,i) = real( random_value<value_type>() );
   }
} // randomize()

// Fill a matrix
template <typename M>
void fill(M& m) {
   typedef typename M::size_type  size_type ;
   typedef typename M::value_type value_type ;

   int size = m.size2() ;

   for (int i=0; i<size; ++i) {
      for (int j=0; j<i; ++j) {
         m(j,i) = random_value<value_type>();
         m(i,j) = conj( m(j,i) ) ;
      }
      m(i,i) = real( random_value<value_type>() );
   }
} // randomize()

template <typename H, typename E, typename Z>
int check_residual(H const& h, E const& e, Z const& z) {
   typedef typename H::value_type value_type ;
   typedef typename E::value_type real_type ;
   real_type safety_factor (1.5);

   // Check eigen decomposition
   int n = h.size1();
   boost::numeric::ublas::matrix<value_type> error( n, n ); error.clear();

   // Copy band matrix in error
   error.assign( h );
   assert( norm_frobenius( error - herm( error ) ) == 0.0 ) ;

   for (int i=0; i<n; ++i) {
      error .minus_assign( outer_prod( column(z, i), e(i) * conj( column(z, i) ) ) ) ;
   }
   return (norm_frobenius( error )
           >= safety_factor*n* norm_2( e ) * std::numeric_limits< real_type >::epsilon() ) ;
} // check_residual()

#endif


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2014 Daniele Panozzo <daniele.panozzo@gmail.com>, Olga Diamanti <olga.diam@gmail.com>
//
// 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 "comb_cross_field.h"

#include <vector>
#include <deque>
#include <Eigen/Geometry>
#include "per_face_normals.h"
#include "is_border_vertex.h"
#include "rotation_matrix_from_directions.h"

#include "triangle_triangle_adjacency.h"

namespace igl {
  template <typename DerivedV, typename DerivedF>
  class Comb
  {
  public:

    const Eigen::PlainObjectBase<DerivedV> &V;
    const Eigen::PlainObjectBase<DerivedF> &F;
    const Eigen::PlainObjectBase<DerivedV> &PD1;
    const Eigen::PlainObjectBase<DerivedV> &PD2;
    Eigen::PlainObjectBase<DerivedV> N;

  private:
    // internal
    Eigen::PlainObjectBase<DerivedF> TT;
    Eigen::PlainObjectBase<DerivedF> TTi;

  private:

    static inline double Sign(double a){return (double)((a>0)?+1:-1);}

  private:

    // returns the 90 deg rotation of a (around n) most similar to target b
    /// a and b should be in the same plane orthogonal to N
    static inline Eigen::Matrix<typename DerivedV::Scalar, 3, 1> K_PI_new(const Eigen::Matrix<typename DerivedV::Scalar, 3, 1>& a,
                                                                   const Eigen::Matrix<typename DerivedV::Scalar, 3, 1>& b,
                                                                   const Eigen::Matrix<typename DerivedV::Scalar, 3, 1>& n)
    {
      Eigen::Matrix<typename DerivedV::Scalar, 3, 1> c = (a.cross(n)).normalized();
      typename DerivedV::Scalar scorea = a.dot(b);
      typename DerivedV::Scalar scorec = c.dot(b);
      if (fabs(scorea)>=fabs(scorec))
        return a*Sign(scorea);
      else
        return c*Sign(scorec);
    }

  public:
    inline Comb(const Eigen::PlainObjectBase<DerivedV> &_V,
         const Eigen::PlainObjectBase<DerivedF> &_F,
         const Eigen::PlainObjectBase<DerivedV> &_PD1,
         const Eigen::PlainObjectBase<DerivedV> &_PD2
         ):
    V(_V),
    F(_F),
    PD1(_PD1),
    PD2(_PD2)
    {
      igl::per_face_normals(V,F,N);
      igl::triangle_triangle_adjacency(V,F,TT,TTi);
    }
    inline void comb(Eigen::PlainObjectBase<DerivedV> &PD1out,
              Eigen::PlainObjectBase<DerivedV> &PD2out)
    {
//      PD1out = PD1;
//      PD2out = PD2;
      PD1out.setZero(F.rows(),3);PD1out<<PD1;
      PD2out.setZero(F.rows(),3);PD2out<<PD2;

      Eigen::VectorXi mark = Eigen::VectorXi::Constant(F.rows(),false);

      std::deque<int> d;

      d.push_back(0);
      mark(0) = true;

      while (!d.empty())
      {
        int f0 = d.at(0);
        d.pop_front();
        for (int k=0; k<3; k++)
        {
          int f1 = TT(f0,k);
          if (f1==-1) continue;
          if (mark(f1)) continue;

          Eigen::Matrix<typename DerivedV::Scalar, 3, 1> dir0    = PD1out.row(f0);
          Eigen::Matrix<typename DerivedV::Scalar, 3, 1> dir1    = PD1out.row(f1);
          Eigen::Matrix<typename DerivedV::Scalar, 3, 1> n0    = N.row(f0);
          Eigen::Matrix<typename DerivedV::Scalar, 3, 1> n1    = N.row(f1);

          Eigen::Matrix<typename DerivedV::Scalar, 3, 1> dir0Rot = igl::rotation_matrix_from_directions(n0, n1)*dir0;
          dir0Rot.normalize();
          Eigen::Matrix<typename DerivedV::Scalar, 3, 1> targD   = K_PI_new(dir1,dir0Rot,n1);

          PD1out.row(f1)  = targD;
          PD2out.row(f1)  = n1.cross(targD).normalized();

          mark(f1) = true;
          d.push_back(f1);

        }
      }

      // everything should be marked
      for (int i=0; i<F.rows(); i++)
      {
        assert(mark(i));
      }
    }

  };
}
template <typename DerivedV, typename DerivedF>
IGL_INLINE void igl::comb_cross_field(const Eigen::PlainObjectBase<DerivedV> &V,
                                      const Eigen::PlainObjectBase<DerivedF> &F,
                                      const Eigen::PlainObjectBase<DerivedV> &PD1,
                                      const Eigen::PlainObjectBase<DerivedV> &PD2,
                                      Eigen::PlainObjectBase<DerivedV> &PD1out,
                                      Eigen::PlainObjectBase<DerivedV> &PD2out)
{
  igl::Comb<DerivedV, DerivedF> cmb(V, F, PD1, PD2);
  cmb.comb(PD1out, PD2out);
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template specialization
template void igl::comb_cross_field<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&);
template void igl::comb_cross_field<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
#endif


#include <iostream>
#include <iomanip>
#include <vector>
#include <boost/random.hpp>
#include <boost/lexical_cast.hpp>
#include <alps/accumulators.hpp>

int main(int argc, char* argv[]){
  alps::accumulators::accumulator_set measurements;
  measurements << alps::accumulators::LogBinningAccumulator<double>("log");
  measurements << alps::accumulators::LogBinningAccumulator<double>("ran");
  boost::mt19937 eng(49135);
  boost::variate_generator<boost::mt19937&, boost::uniform_real<>> random_uniform(eng, boost::uniform_real<>());
  std::vector<double> log_data, ran_data;
  int N = 500;
  if(argc == 2){
    N = boost::lexical_cast<double>(argv[1]);
  }else{
    std::cerr << "usege: ./main number, N = 500 (Default)" << std::endl;
  }
  double x = 0;
  for(int i = 0; i < N; ++i){
    double next = x + random_uniform() - 0.5;
    if(abs(x) < abs(next)){
      double probability = exp((x*x - next*next) * 0.125);
      double dice = random_uniform();
      if(probability > dice){
	x = next;
      }
    }else{
      x = next;
    }
    measurements["log"] << x;
    double ran_cash = random_uniform();
    measurements["ran"] << ran_cash;

    log_data.push_back(x);
    ran_data.push_back(ran_cash);
  }
  alps::accumulators::result_set result(measurements);
  std::cout << "log " << std::setprecision(20) << result["log"] << std::endl;
  std::cout << "ran " <<  result["ran"] << std::endl;
  auto log = result["log"];
  auto ran = result["ran"];

  double vari_log = 0.0;
  double mean_log = 0.0;
  double vari_ran = 0.0;
  double mean_ran = 0.0;
  for(int i = 0; i < ran_data.size(); ++i){
    mean_log += log_data[i];
    mean_ran += ran_data[i];
  }
  mean_log /= log_data.size();
  mean_ran /= ran_data.size();
  for(int i = 0; i < ran_data.size(); i = i+pow(2,floor(log2(N))-7)){
    double bin_log = 0.0;
    double bin_ran = 0.0;
    for(int j = 0; j < pow(2,floor(log2(N))-7); ++j){
      bin_log += log_data[i+j];
      bin_ran += ran_data[i+j];
    }
    bin_log /= pow(2,floor(log2(N))-7);
    bin_ran /= pow(2,floor(log2(N))-7);

    vari_log += (bin_log - mean_log)*(bin_log - mean_log);
    vari_ran += (bin_ran - mean_ran)*(bin_ran - mean_ran);
  }
  double vari_naive = 0.0;
  for(int i = 0; i < ran_data.size(); ++i){
    vari_naive += (log_data[i] - mean_log)*(log_data[i]-mean_log);
  }
  vari_naive /= log_data.size()*(log_data.size()-1);

  vari_log /= log_data.size()/(pow(2,floor(log2(N))-7))*(log_data.size()/(pow(2,floor(log2(N))-7)) - 1);
  vari_ran /= log_data.size()/(pow(2,floor(log2(N))-7))*(log_data.size()/(pow(2,floor(log2(N))-7)) - 1);
  std::cout << "log error: " << sqrt(vari_log) << std::setprecision(20) << ", log tau: " << (vari_log/vari_naive - 1.0)*0.5 << std::endl;
  std::cout << "ran error: " << sqrt(vari_ran) << std::endl;
  std::cout << "naive_error: " << sqrt(vari_naive) << std::endl;
  return 0;
}


#ifndef __GPS_HH
#define __GPS_HH
/********************************* TRICK HEADER *******************************
PURPOSE:
      (Describe the GPS compute unit On Board)
LIBRARY DEPENDENCY:
      ((../src/GPS.cpp))
PROGRAMMERS:
      (((Lai Chun Hsu) () () () ))
*******************************************************************************/
#include "Time_management.hh"
#include <functional>
#include <armadillo>
#include "Transmit_channel.hh"

class GPS_FSW{
    TRICK_INTERFACE(GPS_FSW);

 public:
    GPS_FSW();
    GPS_FSW(const GPS_FSW &other);
    GPS_FSW & operator= (const GPS_FSW &other);

    void setup_state_covariance_matrix(double factp, double pclockb, double pclockf);
    void setup_error_covariance_matrix(double factq, double qclockb, double qclockf);
    void setup_fundamental_dynamic_matrix(double uctime_cor);

    std::function<arma::vec3()> grab_SBIIC;
    std::function<arma::vec3()> grab_VBIIC;
    std::function<arma::vec3()> grab_WBICI;
    std::function<transmit_channel*()> grab_transmit_data;

    void initialize(double int_step);

    arma::vec3 get_SXH();
    arma::vec3 get_VXH();
    arma::vec3 get_CXH();

    std::function<arma::vec3()>  grab_SBEEC;
    std::function<arma::vec3()>  grab_VBEEC;
    std::function<arma::mat33()> grab_TEIC;

    void filter_extrapolation(double int_step);
    void measure(double int_step);

    double ucfreq_error;    /* *o (m)        User clock frequency error */
    double ucfreqm;         /* *o (m/s)      User clock frequency state */

    double std_pos;         /* *o (m)        std deviation of position from p matrix */
    double std_vel;         /* *o (m)        std deviation of velocity from p matrix */
    double std_ucbias;      /* *o (m)        std deviation of user clock bias from p matrix */

    double gps_pos_meas;    /* *o (m)        */
    double gps_vel_meas;    /* *o (m)        */
    double state_pos;    /* *o (m)        */
    double state_vel;    /* *o (m)        */

    double ucfreq_noise;    /* *i (m/s)     User clock frequency error */
    double ucfreq_noise_sigma;  /* *i (--)     User clock frequency error sigma of markov variable */
    double ucfreq_noise_bcor;   /* *i (--)     User clock frequency error bcor of markov variable */
    double ucbias_error;    /* *io (m)      User clock bias error */

    double PR_BIAS[4];      /* *i (m)       Pseudo-range bias GAUSS */
    double PR_NOISE[4];     /* *i (m)       Pseudo-range bias */
    double PR_NOISE_sigma[4];    /* *i (--)       Pseudo-range bias sigma */
    double PR_NOISE_bcor[4];     /* *i (--)       Pseudo-range bias bcor */

    double DR_NOISE[4];     /* *i (m/s)     Delta-range noise */
    double DR_NOISE_sigma[4];    /* *i (--)     Delta-range noise sigma */
    double DR_NOISE_bcor[4];     /* *i (--)     Delta-range noise bcor */

    /* GPS EKF Parameters */
    double ppos;            /* *i (m)       Init 1sig pos values of state cov matrix */
    double pvel;            /* *i (m/s)     Init 1sig vel values of state cov matrix */
    double qpos;            /* *i (m)       1sig pos values of process cov matrix */
    double qvel;            /* *i (m/s)     1sig vel values of process cov matrix */
    double rpos;            /* *i (m)       1sig pos value of meas cov matrix */
    double rvel;            /* *i (m/s)     1sig vel value of meas cov matrix */
    double factr;           /* *i (--)      Factor to modifiy the R-matrix R(1+factr) */

 private:
    time_management * time;

    /* Internal variables */
    bool gps_acq;               /* ** (--)      GPS Signal Acquired? */
    double gps_epoch;           /* ** (s)       GPS update epoch time since launch */
    double time_gps;            /* ** (s)       GPS time passed */

    arma::mat FF;             /* *o (--)      Constant*/
    double _FF[8][8];             /* *o (--)      Constant*/

    arma::mat PHI;            /* *o (--)      Constant*/
    double _PHI[8][8];            /* *o (--)      Constant*/

    arma::mat PP;               /* *o (--)      Covariance Matrix */
    double _PP[8][8];           /* *o (--)      Covariance Matrix */

    arma::mat PP0;
    double _PP0[8][8];

    double factq;               /* *o (--)      Factor to modifiy the Q-matrix Q(1+factq) */
    double qclockb;             /* *o (m)       1sig clock bias error of process cov matrix */
    double qclockf;             /* *o (m/s)     1sig clock freq error of process cov matrix */

    arma::vec SXH;    /* *o (m)        Position state (inertial coor) */
    double   _SXH[3]; /* *o (m)        Position state (inertial coor) */

    arma::vec VXH;    /* *o (m)        Velocity state (inertial coor) */
    double   _VXH[3]; /* *o (m)        Velocity state (inertial coor) */

    arma::vec CXH;    /* *o (--)       CLock state */
    double   _CXH[3]; /* *o (--)       CLock state */

    arma::vec ZZ;     /* *o  (--)      */
    double _ZZ[8];    /* *o  (--)      */

    arma::vec WEII;   /* *o  (r/s)      Earth rate (Vector) */
    double _WEII[3];  /* *o  (r/s)      Earth rate (Vector) */
    /* GPS Outputs */

    double gps_step;  /* *o  (s)      gps time step */
};

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EULER_FUNCTIONS_SCALAR_BETA_HPP_INCLUDED
#define NT2_EULER_FUNCTIONS_SCALAR_BETA_HPP_INCLUDED
#include <nt2/euler/functions/beta.hpp>
#include <boost/math/special_functions.hpp>
#include <nt2/sdk/error/policies.hpp>

/**
 * \ingroup euler_beta
 * \defgroup euler_beta_scalar Notes on the scalar implementation
 *
 * \par Specificities
 *
 *  Some info on \c beta in scalar mode
 **/

namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::beta_, tag::cpu_
                            , (A0)
                            , (scalar_< arithmetic_<A0> >)
                              (scalar_< arithmetic_<A0> >)
                            )
  {

    typedef typename boost::dispatch::meta::as_floating<A0>::type result_type;

    NT2_FUNCTOR_CALL_REPEAT(2)
    {
      return boost::math::beta(result_type(a0), result_type(a1), nt2_policy());
    }
  };
} }

#endif


//
// Created by Hamza El-Kebir on 2/25/22.
//

#ifndef LSEXAMPLE_FUZZYPIDEXAMPLE_HPP
#define LSEXAMPLE_FUZZYPIDEXAMPLE_HPP

#include <Lodestar/blocks/std/GainBlock.hpp>
#include <Lodestar/blocks/std/SumBlock.hpp>
#include <Lodestar/blocks/std/DelayBlock.hpp>
#include <Lodestar/blocks/std/FunctionBlock.hpp>
#include <Lodestar/blocks/BlockUtilities.hpp>
#include <Lodestar/blocks/aux/Executor.hpp>

#include <Eigen/Dense>
#include <chrono>

#include "fuzzyPID.h"

using namespace ls::blocks::std;
using namespace ls::blocks;

class FuzzyPIDExample {
public:
    static void example()
    {
        using TType = float;
        using TScalar = float;

        TScalar T = 1e-3;
        TScalar K, Kp, Kd, Ki, KuPD, KuI;

        GainBlock<TType, TScalar> gainK{K}, gainKp{Kp}, gainKd{Kd}, gainKi{Ki}, gainKuPD{KuPD}, gainKuI{KuI}, gainTinv1{1.f/T}, gainTinv2{1.f/T};
        DelayBlock<TType> delay1, delay2, delay3, delay4, delay5;
        SumBlock<TType, 2> sum1, sum2, sum3, sum4, sum5, sum6, sum7, fuzzyPDSum;

        sum1.setOperators(decltype(sum1)::Plus, decltype(sum1)::Minus);
        sum2.setOperators(decltype(sum2)::Plus, decltype(sum2)::Minus);
        sum5.setOperators(decltype(sum5)::Plus, decltype(sum5)::Minus);

        auto &sp = sum1.i<0>();
        auto &e = sum1.o<0>();
        auto &d = gainTinv2.o<0>();
        auto &r = gainTinv1.o<0>();
        auto &duPD = fuzzyPDSum.o<0>();
        auto &duI = sum4.o<0>();
        auto &uPD = sum5.o<0>();
        auto &uI = sum6.o<0>();
        auto &uPID = sum7.o<0>();

        connect(e, delay1.i<0>());
        connect(delay1.o<0>(), gainKi.i<0>());
        connect(gainKi.o<0>(), sum4.i<0>());

        connect(e, delay2.i<0>());
        connect(delay2.o<0>(), sum3.i<0>());
        connect(e, sum3.i<1>());
        connect(sum3.o<0>(), gainTinv2.i<0>());
        connect(d, gainKp.i<0>());

        connect(e, sum2.i<0>());
        connect(e, delay3.i<0>());
        connect(delay3.o<0>(), sum2.i<1>());
        connect(sum2.o<0>(), gainTinv1.i<0>());

        connect(r, gainK.i<0>());
        connect(r, gainKd.i<0>());
        connect(gainK.o<0>(), sum4.i<1>());

        connect(gainKp.o<0>(), fuzzyPDSum.i<0>());
        connect(gainKd.o<0>(), fuzzyPDSum.i<1>());
        connect(duPD, gainKuPD.i<0>());
        connect(gainKuPD.o<0>(), sum5.i<0>());
        connect(uPD, delay4.i<0>());
        connect(delay4.o<0>(), sum5.i<1>());

        connect(sum4.o<0>(), gainKuI.i<0>());
        connect(gainKuI.o<0>(), sum6.i<0>());
        connect(uI, delay5.i<0>());
        connect(delay5.o<0>(), sum6.i<1>());

        connect(uPD, sum7.i<0>());
        connect(uI, sum7.i<1>());

        BlockPack bp{gainK, gainKp, gainKd, gainKi, gainKuPD, gainKuI, gainTinv1, gainTinv2, delay1, delay2, delay3, delay4, delay5, sum1, sum2, sum3, sum4, sum5, sum6, sum7, fuzzyPDSum};

        aux::Executor ex{bp};
        ex.resolveExecutionOrder();

        for (int i = 0; i < 1000; i++) {
            ::std::chrono::steady_clock::time_point begin = ::std::chrono::steady_clock::now();
            ex.trigger();
            ::std::chrono::steady_clock::time_point end = ::std::chrono::steady_clock::now();
            ::std::cout << "0 " << ::std::chrono::duration_cast<::std::chrono::nanoseconds>(end - begin).count() << ::std::endl;
        }
    }

    static void exampleSL()
    {
        fuzzyPIDModelClass fPID;
        fPID.initialize();
        fPID.fuzzyPID_U.Input = 0;
        fPID.fuzzyPID_U.Input1 = 0;

        for (int i = 0; i < 1000; i++) {
            ::std::chrono::steady_clock::time_point begin = ::std::chrono::steady_clock::now();
            fPID.step();
            ::std::chrono::steady_clock::time_point end = ::std::chrono::steady_clock::now();
            ::std::cout << "1 " << ::std::chrono::duration_cast<::std::chrono::nanoseconds>(end - begin).count() << ::std::endl;
        }
    }

    static void example7D()
    {
        using TType = float;
        using TScalar = float;

        TScalar T = 1e-3;
        TScalar K, Kp, Kd, Ki, KuPD, KuI;

        GainBlock<TType, TScalar> gainK{K}, gainKp{Kp}, gainKd{Kd}, gainKi{Ki}, gainKuPD{KuPD}, gainKuI{KuI}, gainTinv1{1.f/T}, gainTinv2{1.f/T};
        DelayBlock<TType> delay1, delay2, delay3, delay4, delay5;
        SumBlock<TType, 2> sum1, sum2, sum3, sum4, sum5, sum6, sum7, fuzzyPDSum;

        sum1.setOperators(decltype(sum1)::Plus, decltype(sum1)::Minus);
        sum2.setOperators(decltype(sum2)::Plus, decltype(sum2)::Minus);
        sum5.setOperators(decltype(sum5)::Plus, decltype(sum5)::Minus);

        auto &sp = sum1.i<0>();
        auto &e = sum1.o<0>();
        auto &d = gainTinv2.o<0>();
        auto &r = gainTinv1.o<0>();
        auto &duPD = fuzzyPDSum.o<0>();
        auto &duI = sum4.o<0>();
        auto &uPD = sum5.o<0>();
        auto &uI = sum6.o<0>();
        auto &uPID = sum7.o<0>();

        connect(e, delay1.i<0>());
        connect(delay1.o<0>(), gainKi.i<0>());
        connect(gainKi.o<0>(), sum4.i<0>());

        connect(e, delay2.i<0>());
        connect(delay2.o<0>(), sum3.i<0>());
        connect(e, sum3.i<1>());
        connect(sum3.o<0>(), gainTinv2.i<0>());
        connect(d, gainKp.i<0>());

        connect(e, sum2.i<0>());
        connect(e, delay3.i<0>());
        connect(delay3.o<0>(), sum2.i<1>());
        connect(sum2.o<0>(), gainTinv1.i<0>());

        connect(r, gainK.i<0>());
        connect(r, gainKd.i<0>());
        connect(gainK.o<0>(), sum4.i<1>());

        connect(gainKp.o<0>(), fuzzyPDSum.i<0>());
        connect(gainKd.o<0>(), fuzzyPDSum.i<1>());
        connect(duPD, gainKuPD.i<0>());
        connect(gainKuPD.o<0>(), sum5.i<0>());
        connect(uPD, delay4.i<0>());
        connect(delay4.o<0>(), sum5.i<1>());

        connect(sum4.o<0>(), gainKuI.i<0>());
        connect(gainKuI.o<0>(), sum6.i<0>());
        connect(uI, delay5.i<0>());
        connect(delay5.o<0>(), sum6.i<1>());

        connect(uPD, sum7.i<0>());
        connect(uI, sum7.i<1>());
    }
};

#endif //LSEXAMPLE_FUZZYPIDEXAMPLE_HPP


#include <NTL/GF2X.h>
#include "xsadd.h"
#include "xsadd.c" // to check static functions
#include <stdint.h>
#include <sstream>
#include <UnitTest++.h>
#include <random>

using namespace NTL;
using namespace std;

char buff[200];

bool eq(f2_polynomial& a, f2_polynomial& b)
{
    for (int i = 0; i < POLYNOMIAL_ARRAY_SIZE; i++) {
	if (a.ar[i] != b.ar[i]) {
	    return false;
	}
    }
    return true;
}

void tof2(f2_polynomial& a, GF2X& b)
{
    clear(&a);
    for (int i = 0; i < POLYNOMIAL_ARRAY_SIZE; i++) {
	for (int j = 0; j < 32; j++) {
	    uint32_t mask = 1 << j;
	    if (IsOne(coeff(b, i * 32 + j))) {
		a.ar[i] |= mask;
	    }
	}
    }
}

void touz(uz *r, ZZ& x)
{
    ZZ work = x;
    uz_clear(r);
    for (int i = 0; i < UZ_ARRAY_SIZE; i++) {
	for (int j = 0; j < 16; j++) {
	    uint16_t mask = 1 << j;
	    if (IsOdd(work)) {
		r->ar[i] |= mask;
	    }
	    work = work >> 1;
	}
    }
}

void uztostring16(char * str, uz * x)
{
    int p = 0;
    int first = 1;
    for (int i = UZ_ARRAY_SIZE - 1; i >= 0; i--) {
	if (first) {
	    if (x->ar[i] != 0) {
		first = 0;
		sprintf(str, "%x", x->ar[i]);
		p = strlen(str);
	    }
	} else {
	    sprintf(str + p, "%04x", x->ar[i]);
	    p = p + 4;
	}
    }
    if (first) {
	strcpy(str, "0");
    }
}

void toGF2X(GF2X& result, f2_polynomial * x)
{
    clear(result);
    int index = 0;
    for (int i = 0; i < POLYNOMIAL_ARRAY_SIZE; i++) {
	uint32_t mask;
	mask = 1;
	for (int j = 0; j < 32; j++) {
	    if (mask & x->ar[i]) {
		SetCoeff(result, index, 1);
	    } else {
		SetCoeff(result, index, 0);
	    }
	    index++;
	    mask = mask << 1;
	}
    }
}

void set(ZZ& step, const char * step_str)
{
    stringstream ss;
    ss << step_str;
    ss >> step;
}

void set(GF2X& ch, const char * characteristic_polynomial)
{
    f2_polynomial x;
    strtopolynomial(&x, characteristic_polynomial);
    toGF2X(ch, &x);
}

SUITE(F2POLYNOMIAL) {
    TEST(ADD)
    {
	GF2X p;
	GF2X q;
	GF2X r;
	f2_polynomial p2;
	f2_polynomial q2;
	f2_polynomial r2;
	for (int i = 0; i < 100; i++) {
	    random(p, POLYNOMIAL_ARRAY_SIZE * 32 - 1);
	    random(q, POLYNOMIAL_ARRAY_SIZE * 32 - 1);
	    r = p + q;
	    tof2(p2, p);
	    tof2(q2, q);
	    tof2(r2, r);
	    add(&p2, &q2);
	    CHECK(eq(p2, r2));
	}
    }
    TEST(MUL)
    {
	GF2X p;
	GF2X q;
	GF2X r;
	f2_polynomial p2;
	f2_polynomial q2;
	f2_polynomial r2;
	for (int i = 0; i < 100; i++) {
	    random(p, POLYNOMIAL_ARRAY_SIZE * 16 - 1);
	    random(q, POLYNOMIAL_ARRAY_SIZE * 16 - 1);
	    r = p * q;
	    tof2(p2, p);
	    tof2(q2, q);
	    tof2(r2, r);
	    mul(&p2, &q2);
	    CHECK(eq(p2, r2));
	    if (!eq(p2, r2)) {
		printf("deg p = %ld\n", deg(p));
		printf("deg q = %ld\n", deg(q));
		printf("deg r = %ld\n", deg(r));
		break;
	    }
	}
    }
    TEST(MOD)
    {
	GF2X p;
	GF2X q;
	GF2X r;
	f2_polynomial p2;
	f2_polynomial q2;
	f2_polynomial r2;
	for (int i = 0; i < 100; i++) {
	    random(p, POLYNOMIAL_ARRAY_SIZE * 32 - 1);
	    random(q, POLYNOMIAL_ARRAY_SIZE * 32 - 1);
	    r = p % q;
	    tof2(p2, p);
	    tof2(q2, q);
	    tof2(r2, r);
	    mod(&p2, &q2);
	    CHECK(eq(p2, r2));
	    if (!eq(p2, r2)) {
		printf("deg p = %ld\n", deg(p));
		printf("deg q = %ld\n", deg(q));
		printf("deg r = %ld\n", deg(r));
		break;
	    }
	}
    }
    TEST(SQUARE)
    {
	GF2X p;
	GF2X r;
	f2_polynomial p2;
	f2_polynomial r2;
	for (int i = 0; i < 100; i++) {
	    random(p, POLYNOMIAL_ARRAY_SIZE * 16 - 1);
	    r = p * p;
	    tof2(p2, p);
	    tof2(r2, r);
	    square(&p2);
	    CHECK(eq(p2, r2));
	    if (!eq(p2, r2)) {
		printf("deg p = %ld\n", deg(p));
		printf("deg r = %ld\n", deg(r));
		break;
	    }
	}
    }
    TEST(POWERMOD)
    {
	GF2X p;
	GF2X q;
	ZZ x;
	GF2X r;
	f2_polynomial p2;
	f2_polynomial q2;
	uz x2;
	f2_polynomial r2;
	f2_polynomial r3;
	for (int i = 0; i < 100; i++) {
	    random(p, POLYNOMIAL_ARRAY_SIZE * 16 - 1);
	    random(q, POLYNOMIAL_ARRAY_SIZE * 16 - 1);
	    p = p % q;
	    RandomBits(x, 10);
	    r = PowerMod(p, x, q);
	    tof2(p2, p);
	    tof2(q2, q);
	    tof2(r2, r);
	    touz(&x2, x);
	    polynomial_power_mod(&r3, &p2, &x2, &q2);
	    CHECK(eq(r2, r3));
	    if (!eq(r2, r3)) {
		printf("deg p = %ld\n", deg(p));
		printf("deg q = %ld\n", deg(q));
		printf("deg r = %ld\n", deg(r));
		fflush(stdout);
		cout << "x:" << hex << x << endl;
		uztostring16(buff, &x2);
		printf("x2:%s\n", buff);
		break;
	    }
	}
    }
    TEST(POWERMOD2)
    {
	ZZ step;
	const char * step_str = "48594547533363904080606289765";
	//const char * step_str = "48594547533363904080606289766";
	set(step, step_str);
	GF2X ch;
	set(ch, characteristic_polynomial);
	GF2X q;
	PowerXMod(q, step, ch);
	f2_polynomial p2;
	f2_polynomial q2;
	uz step2;
	clear(&p2);
	p2.ar[0] = 2;
	touz(&step2, step);
	f2_polynomial ch2;
	strtopolynomial(&ch2, characteristic_polynomial);
	f2_polynomial result;
	polynomial_power_mod(&q2, &p2, &step2, &ch2);
	tof2(result, q);
	CHECK(eq(result, q2));
	if (!eq(result, q2)) {
	    cout << "powermod2 error" << endl;
	    polynomialtostr(buff, &result);
	    cout << "result:" << buff << endl;
	    polynomialtostr(buff, &q2);
	    cout << "q2:    " << buff << endl;
	}
    }
}

void str16(char * buff, const ZZ& a)
{
    if (IsZero(a)) {
	strcpy(buff, "0");
	return;
    }
    const char tbl[] = {'0','1','2','3','4','5','6','7','8','9',
			'a','b','c','d','e','f'};
    stringstream ss;
    ZZ b = a;
    ZZ c;
    while(!IsZero(b)) {
	c = b % 16;
	long t = trunc_long(c, 100);
	ss << tbl[t % 16];
	b = b / 16;
    }
    string str;
    ss >> str;
    string result(str.rbegin(), str.rend());
    strncpy(buff, result.c_str(), 200);
}

bool eq(uz& a, uz& b)
{
    for (int i = 0; i < UZ_ARRAY_SIZE; i++) {
	if (a.ar[i] != b.ar[i]) {
	    return false;
	}
    }
    return true;
}

void touz(uz& a, const ZZ& x)
{
    ZZ work = x;
    for (int i = 0; i < UZ_ARRAY_SIZE; i++) {
	a.ar[i] = 0;
    }
    for (int i = 0; i < UZ_ARRAY_SIZE; i++) {
	for (int j = 0; j < 16; j++) {
	    uint16_t mask = 1 << j;
	    if (IsOdd(work)) {
		a.ar[i] |= mask;
	    }
	    work = work >> 1;
	}
    }
}

SUITE(UZ) {
    TEST(STRTOUZ0)
    {
	ZZ x;
	uz x2;
	uz r;
	x = 0;
	str16(buff, x);
	touz(x2, x);
	string16touz(&r, buff);
	CHECK(eq(x2, r));
	if (!eq(x2, r)) {
	    cout << "s: " << buff << endl;
	}
    }
    TEST(STRTOUZ)
    {
	ZZ x;
	uz x2;
	uz r;
	for (int i = 0; i < 100; i++) {
	    RandomBits(x, UZ_ARRAY_SIZE * 16 - 1);
	    str16(buff, x);
	    touz(x2, x);
	    string16touz(&r, buff);
	    CHECK(eq(x2, r));
	    if (!eq(x2, r)) {
		cout << "s: " << buff << endl;
		break;
	    }
	}
    }
    TEST(UZTOSTR)
    {
	ZZ x;
	uz x2;
	uz r;
	for (int i = 0; i < 100; i++) {
	    RandomBits(x, UZ_ARRAY_SIZE * 16 - 1);
	    touz(x2, x);
	    uztostring16(buff, &x2);
	    string16touz(&r, buff);
	    CHECK(eq(x2, r));
	    if (!eq(x2, r)) {
		cout << "s: " << buff << endl;
		break;
	    }
	}
    }
    TEST(MUL0)
    {
	ZZ x;
	ZZ y;
	ZZ r;
	uz x2;
	uz y2;
	uz r2;
	uz r3;
	for (int i = 0; i < 100; i++) {
	    RandomBits(x, UZ_ARRAY_SIZE * 16 - 1);
	    y = 0;
	    //RandomBits(y, UZ_ARRAY_SIZE * 16 - 1);
	    r = x * y;
	    touz(x2, x);
	    touz(y2, y);
	    touz(r2, r);
	    uz_mul(&r3, &x2, &y2);
	    CHECK(eq(r3, r2));
	    if (!eq(r3, r2)) {
		cout << "x: " << hex << x;
		cout << "y: " << hex << y;
		cout << "r: " << hex << r;
		break;
	    }
	}
    }
    TEST(MUL)
    {
	ZZ x;
	ZZ y;
	ZZ r;
	uz x2;
	uz y2;
	uz r2;
	uz r3;
	for (int i = 0; i < 100; i++) {
	    RandomBits(x, UZ_ARRAY_SIZE * 16 - 1);
	    RandomBits(y, UZ_ARRAY_SIZE * 16 - 1);
	    r = x * y;
	    touz(x2, x);
	    touz(y2, y);
	    touz(r2, r);
	    uz_mul(&r3, &x2, &y2);
	    CHECK(eq(r3, r2));
	    if (!eq(r3, r2)) {
		cout << "x: " << hex << x;
		cout << "y: " << hex << y;
		cout << "r: " << hex << r;
		break;
	    }
	}
    }
}

void tostr(char * str, GF2X& poly)
{
    f2_polynomial x;
    tof2(x, poly);
    polynomialtostr(str, &x);
}

SUITE(JUMPPOL)
{
    TEST(CALCULATE)
    {
	/* xsadd_calculate_jump_polynomial */
	mt19937 mt(1);
	ZZ step;
	ZZ step3;
	uint32_t mul_step;
	uz step2;
	char base_string[200];
	char jump_string[200];
	char jump_string2[200];
	GF2X xpoly;
	GF2X xchar;
	f2_polynomial characteristic;
	strtopolynomial(&characteristic, characteristic_polynomial);
	toGF2X(xchar, &characteristic);
	for (int i = 0; i < 100; i++) {
	    //cout << "test calculate no." << dec << i << endl;
	    mul_step = mt();
	    RandomBits(step, 65);
	    touz(step2, step);
	    uztostring16(base_string, &step2);
	    xsadd_calculate_jump_polynomial(jump_string,
					    mul_step,
					    base_string);
	    step3 = step * mul_step;
	    //cout << "step: " << step << endl;
	    PowerXMod(xpoly, step3, xchar);
	    //cout << "xpoly: " << xpoly << endl;
	    //cout << "deg(xpoly): " << dec << deg(xpoly) << endl;
	    tostr(jump_string2, xpoly);
	    //cout << "jump_string:" << jump_string << endl;
	    //cout << "jump_string2:" << jump_string2 << endl;
	    CHECK(strncmp(jump_string2, jump_string, 200) == 0);
	    //cout << "mul_step :" << dec << mul_step << endl;
	    //cout << "base_string:" << base_string << endl;
	    if (strcmp(jump_string2, jump_string) != 0) {
		cout << "jump_string: " << jump_string << endl;
		cout << "jump_string2:" << jump_string2 << endl;
		cout << "i = " << dec << i << endl;
		cout << "mul_step :" << dec << mul_step << endl;
		cout << "base_string:" << base_string << endl;
		cout << "step: " << step << endl;
		uztostring16(buff, &step2);
		cout << "step2:" << buff << endl;
		cout << "step3 = jump_step:" << step3 << endl;
		uz tmp;
		uint32touz(&tmp, mul_step);
		uz tmp2;
		uz_mul(&tmp2, &step2, &tmp);
		uztostring16(buff, &tmp2);
		cout << "step3uz:" << buff << endl;
		break;
	    }
	}
    }
}


// (C) 2014 Arek Olek

#pragma once

#include <algorithm>
#include <functional>
#include <utility>
#include <vector>

#include <boost/iterator/function_input_iterator.hpp>
#include <boost/function_output_iterator.hpp>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>
#include <boost/graph/planar_canonical_ordering.hpp>
#include <boost/graph/chrobak_payne_drawing.hpp>
#include <boost/graph/make_connected.hpp>
#include <boost/graph/make_biconnected_planar.hpp>
#include <boost/graph/make_maximal_planar.hpp>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include <CGAL/Point_set_2.h>
#include <CGAL/squared_distance_2.h>

#include "range.hpp"

template<class Graph>
unsigned num_internal(Graph const & G) {
  unsigned internal = 0;
  for (auto v : range(vertices(G)))
    internal += out_degree(v, G) > 1;
  return internal;
}

template<class Graph>
unsigned upper_bound(Graph const & G) {
  return std::min(num_internal(G), (unsigned) num_vertices(G) - 2);
}

template<class Graph>
bool is_connected(Graph const & G) {
  std::vector<int> component(num_vertices(G));
  return connected_components(G, &component[0]) == 1;
}

template<class Tree, class Graph>
bool is_subgraph(Tree const & T, Graph const & G) {
  for (auto e : range(edges(T)))
    if(!edge(source(e, T), target(e, T), G).second)
      return false;
  return true;
}

template<class G>
void add_edge_no_dup(typename boost::graph_traits<G>::vertex_descriptor v,
                     typename boost::graph_traits<G>::vertex_descriptor u,
                     G& g) {
  if (!edge(v, u, g).second) add_edge(v, u, g);
}

template<class Input, class Output>
void copy_edges(const Input& in, Output& out) {
  for (auto e : range(edges(in)))
    add_edge(source(e, in), target(e, in), out);
}

template<class Input, class Output, class Generator>
void copy_edges_shuffled(const Input& in, Output& out, Generator& generator) {
  auto p = shuffled(range_iterator(0, num_vertices(in)), generator);
  for (auto e : shuffled(edges(in), generator))
    add_edge(p[source(e, in)], p[target(e, in)], out);
}

template<class Graph>
void add_spider(Graph& G, unsigned legs) {
  unsigned n = num_vertices(G);
  unsigned cutoff = ceil((double)n / legs);
  for (unsigned i = 0; i < n - 1; ++i)
    add_edge_no_dup(i % cutoff == 0 ? 0 : i, i + 1, G);
}

template<class Graph, class Generator>
void add_edges_uniform(Graph& G, double p, Generator& generator, bool mst) {
  unsigned n = num_vertices(G);
  double connectedness = 20.0 / n;
  if (mst && p < connectedness) {
    typedef boost::property<boost::edge_weight_t, double> Weight;
    typedef boost::adjacency_list<boost::hash_setS, boost::vecS, boost::undirectedS, boost::no_property, Weight> WeightedGraph;
    typedef boost::graph_traits<WeightedGraph>::edge_descriptor WeightedEdge;
    std::uniform_real_distribution<> distribution(0, 1);
    auto trial = std::bind(distribution, std::ref(generator));
    WeightedGraph g;
    for (unsigned i = 0; i < n; ++i) {
      for (unsigned j = i + 1; j < n; ++j) {
        auto w = trial();
        if (w < connectedness) add_edge(i, j, w, g);
        if (w < p) add_edge_no_dup(i, j, G);
      }
    }
    std::vector<WeightedEdge> t;
    kruskal_minimum_spanning_tree(g, std::back_inserter(t));
    for (auto e : t)
      add_edge_no_dup(source(e, g), target(e, g), G);
  } else {
    std::bernoulli_distribution distribution(p);
    auto trial = std::bind(distribution, std::ref(generator));
    for (unsigned i = 0; i < n; ++i) {
      for (unsigned j = i + 1; j < n; ++j) {
        if (trial()) add_edge_no_dup(i, j, G);
      }
    }
  }
}

class Geometric {
  CGAL::Exact_predicates_inexact_constructions_kernel typedef Kernel;
  CGAL::Triangulation_vertex_base_with_info_2<unsigned, Kernel> typedef Vertex_struct;
  CGAL::Triangulation_data_structure_2<Vertex_struct> typedef Triangulation_struct;
  CGAL::Point_set_2<Kernel, Triangulation_struct> typedef Delaunay;
  Delaunay::Point typedef Point;

  Delaunay D;

 public:
  template<class Generator>
  Geometric(int n, Generator& generator) {
    // Choose random points in square [0, 1) x [0, 1)
    std::uniform_real_distribution<> distribution(0, 1);
    auto r = std::bind(distribution, std::ref(generator));
    // Generate index for every point
    int counter = 0;
    std::function<std::pair<Point, int>()> f = [&]() {return std::make_pair(Point(r(), r()), counter++);};
    // Initialize triangulation with n points
    D = Delaunay(boost::make_function_input_iterator(f, 0), boost::make_function_input_iterator(f, n));
  }

  template<class Graph>
  void add_random_geometric(Graph& G, double d) {
    CGAL::Circle_2<Kernel> typedef Circle;
    Delaunay::Vertex_handle typedef Vertex_handle;
    // Query each point for other points lying in the circle
    for (auto v : range(D.finite_vertices_begin(), D.finite_vertices_end()))
      D.range_search(Circle(v.point(), d * d), boost::make_function_output_iterator([&](Vertex_handle u) {
        if (v.info() < u->info()) add_edge_no_dup(v.info(), u->info(), G);
      }));
  }

  template<class Graph>
  void add_mst(Graph& G) {
    typedef boost::property<boost::edge_weight_t, Kernel::FT> Weight;
    typedef boost::adjacency_list<boost::hash_setS, boost::vecS, boost::undirectedS, boost::no_property, Weight> WeightedGraph;
    typedef boost::graph_traits<WeightedGraph>::edge_descriptor WeightedEdge;

    Delaunay::Finite_vertices_iterator vit;
    Delaunay::Vertex_circulator vc, done;

    WeightedGraph g;

    for (vit = D.finite_vertices_begin(); vit != D.finite_vertices_end(); ++vit) {
      unsigned s = vit->info();
      done = vc = vit->incident_vertices();
      if (vc != 0) do {
        if (D.is_infinite(vc)) continue;
        unsigned d = vc->info();
        add_edge(s, d, CGAL::squared_distance(vit->point(), vc->point()), g);
      } while (++vc != done);
    }

    std::vector<WeightedEdge> mst;
    kruskal_minimum_spanning_tree(g, std::back_inserter(mst));
    for (auto e : mst)
      add_edge_no_dup(source(e, g), target(e, g), G);
  }
};

template<class G>
bool is_planar(G const & gIn) {
  return boyer_myrvold_planarity_test(gIn);
}

//a class to hold the coordinates of the straight line embedding
struct coord_t {
  std::size_t x;
  std::size_t y;
};

template<class G>
std::vector<coord_t> straight_line_drawing(G const & gIn) {
  using namespace boost;

  typedef adjacency_list<vecS, vecS, undirectedS, property<vertex_index_t, int>, property<edge_index_t, int> > Graph;

  Graph g;
  copy_edges(gIn, g);

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

  make_connected(g);

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

  boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g, boyer_myrvold_params::embedding = embedding);

  //Initialize the interior edge index
  property_map<Graph, edge_index_t>::type e_index = get(edge_index, g);
  graph_traits<Graph>::edges_size_type edge_count = 0;
  graph_traits<Graph>::edge_iterator ei, ei_end;
  for (boost::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
    put(e_index, *ei, edge_count++);

  make_biconnected_planar(g, embedding);

  // Re-initialize the edge index, since we just added a few edges
  edge_count = 0;
  for (boost::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
    put(e_index, *ei, edge_count++);

  //Test for planarity again; compute the planar embedding as a side-effect
  boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g, boyer_myrvold_params::embedding = embedding);

  make_maximal_planar(g, embedding);

  //Test for planarity again; compute the planar embedding as a side-effect
  boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g, boyer_myrvold_params::embedding = embedding);

  // Find a canonical ordering
  std::vector<typename graph_traits<Graph>::vertex_descriptor> ordering;
  planar_canonical_ordering(g, embedding, std::back_inserter(ordering));

  //Set up a property map to hold the mapping from vertices to coord_t's
  typedef std::vector<coord_t> drawing_storage_t;
  typedef boost::iterator_property_map<drawing_storage_t::iterator, property_map<Graph, vertex_index_t>::type> drawing_t;

  drawing_storage_t drawing_storage(num_vertices(g));
  drawing_t drawing(drawing_storage.begin(), get(vertex_index, g));

  // Compute the straight line drawing
  chrobak_payne_straight_line_drawing(g, embedding, ordering.begin(), ordering.end(), drawing);

  return drawing_storage;
}