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

#define BOOST_UBLAS_TYPE_CHECK_EPSILON (type_traits<real_type>::type_sqrt (boost::math::tools::epsilon <real_type>()))
#define BOOST_UBLAS_TYPE_CHECK_MIN (type_traits<real_type>::type_sqrt ( boost::math::tools::min_value<real_type>()))
#define BOOST_UBLAS_NDEBUG

#include <boost/math/bindings/rr.hpp>
namespace std{
using boost::math::ntl::pow;
} // workaround for spirit parser.
#include <boost/math/tools/remez.hpp>
#include <boost/math/tools/test.hpp>
#include <boost/math/special_functions/binomial.hpp>
#include <boost/spirit/include/classic_core.hpp>
#include <boost/spirit/include/classic_actor.hpp>
#include <boost/lexical_cast.hpp>
#include <iostream>
#include <iomanip>
#include <string>
#include <boost/test/included/test_exec_monitor.hpp> // for test_main

extern boost::math::ntl::RR f(const boost::math::ntl::RR& x, int variant);
extern void show_extra(
   const boost::math::tools::polynomial<boost::math::ntl::RR>& n,
   const boost::math::tools::polynomial<boost::math::ntl::RR>& d,
   const boost::math::ntl::RR& x_offset,
   const boost::math::ntl::RR& y_offset,
   int variant);

using namespace boost::spirit::classic;

boost::math::ntl::RR a(0), b(1);   // range to optimise over
bool rel_error(true);
bool pin(false);
int orderN(3);
int orderD(1);
int target_precision = boost::math::tools::digits<long double>();
int working_precision = target_precision * 2;
bool started(false);
int variant(0);
int skew(0);
int brake(50);
boost::math::ntl::RR x_offset(0), y_offset(0), x_scale(1);
bool auto_offset_y;

boost::shared_ptr<boost::math::tools::remez_minimax<boost::math::ntl::RR> > p_remez;

boost::math::ntl::RR the_function(const boost::math::ntl::RR& val)
{
   return f(x_scale * (val + x_offset), variant) + y_offset;
}

void step_some(unsigned count)
{
   try{
      NTL::RR::SetPrecision(working_precision);
      if(!started)
      {
         //
         // If we have an automatic y-offset calculate it now:
         //
         if(auto_offset_y)
         {
            boost::math::ntl::RR fa, fb, fm;
            fa = f(x_scale * (a + x_offset), variant);
            fb = f(x_scale * (b + x_offset), variant);
            fm = f(x_scale * ((a+b)/2 + x_offset), variant);
            y_offset = -(fa + fb + fm) / 3;
            NTL::RR::SetOutputPrecision(5);
            std::cout << "Setting auto-y-offset to " << y_offset << std::endl;
         }
         //
         // Truncate offsets to float precision:
         //
         x_offset = NTL::RoundToPrecision(x_offset.value(), 20);
         y_offset = NTL::RoundToPrecision(y_offset.value(), 20);
         //
         // Construct new Remez state machine:
         //
         p_remez.reset(new boost::math::tools::remez_minimax<boost::math::ntl::RR>(
            &the_function,
            orderN, orderD,
            a, b,
            pin,
            rel_error,
            skew,
            working_precision));
         std::cout << "Max error in interpolated form: " << std::setprecision(3) << std::scientific << boost::math::tools::real_cast<double>(p_remez->max_error()) << std::endl;
         //
         // Signal that we've started:
         //
         started = true;
      }
      unsigned i;
      for(i = 0; i < count; ++i)
      {
         std::cout << "Stepping..." << std::endl;
         p_remez->set_brake(brake);
         boost::math::ntl::RR r = p_remez->iterate();
         NTL::RR::SetOutputPrecision(3);
         std::cout
            << "Maximum Deviation Found:                     " << std::setprecision(3) << std::scientific << boost::math::tools::real_cast<double>(p_remez->max_error()) << std::endl
            << "Expected Error Term:                         " << std::setprecision(3) << std::scientific << boost::math::tools::real_cast<double>(p_remez->error_term()) << std::endl
            << "Maximum Relative Change in Control Points:   " << std::setprecision(3) << std::scientific << boost::math::tools::real_cast<double>(r) << std::endl;
      }
   }
   catch(const std::exception& e)
   {
      std::cout << "Step failed with exception: " << e.what() << std::endl;
   }
}

void step(const char*, const char*)
{
   step_some(1);
}

void show(const char*, const char*)
{
   NTL::RR::SetPrecision(working_precision);
   if(started)
   {
      boost::math::tools::polynomial<boost::math::ntl::RR> n = p_remez->numerator();
      boost::math::tools::polynomial<boost::math::ntl::RR> d = p_remez->denominator();
      std::vector<boost::math::ntl::RR> cn = n.chebyshev();
      std::vector<boost::math::ntl::RR> cd = d.chebyshev();
      int prec = 2 + (target_precision * 3010LL)/10000;
      std::cout << std::scientific << std::setprecision(prec);
      NTL::RR::SetOutputPrecision(prec);
      boost::numeric::ublas::vector<boost::math::ntl::RR> v = p_remez->zero_points();

      std::cout << "  Zeros = {\n";
      unsigned i;
      for(i = 0; i < v.size(); ++i)
      {
         std::cout << "    " << v[i] << std::endl;
      }
      std::cout << "  }\n";

      v = p_remez->chebyshev_points();
      std::cout << "  Chebeshev Control Points = {\n";
      for(i = 0; i < v.size(); ++i)
      {
         std::cout << "    " << v[i] << std::endl;
      }
      std::cout << "  }\n";

      std::cout << "X offset: " << x_offset << std::endl;
      std::cout << "X scale:  " << x_scale << std::endl;
      std::cout << "Y offset: " << y_offset << std::endl;

      std::cout << "P = {";
      for(i = 0; i < n.size(); ++i)
      {
         std::cout << "    " << n[i] << "L," << std::endl;
      }
      std::cout << "  }\n";

      std::cout << "Q = {";
      for(i = 0; i < d.size(); ++i)
      {
         std::cout << "    " << d[i] << "L," << std::endl;
      }
      std::cout << "  }\n";

      std::cout << "CP = {";
      for(i = 0; i < cn.size(); ++i)
      {
         std::cout << "    " << cn[i] << "L," << std::endl;
      }
      std::cout << "  }\n";

      std::cout << "CQ = {";
      for(i = 0; i < cd.size(); ++i)
      {
         std::cout << "    " << cd[i] << "L," << std::endl;
      }
      std::cout << "  }\n";

      show_extra(n, d, x_offset, y_offset, variant);
   }
   else
   {
      std::cerr << "Nothing to display" << std::endl;
   }
}

void do_graph(unsigned points)
{
   NTL::RR::SetPrecision(working_precision);
   boost::math::ntl::RR step = (b - a) / (points - 1);
   boost::math::ntl::RR x = a;
   while(points > 1)
   {
      NTL::RR::SetOutputPrecision(10);
      std::cout << std::setprecision(10) << std::setw(30) << std::left
         << boost::lexical_cast<std::string>(x) << the_function(x) << std::endl;
      --points;
      x += step;
   }
   std::cout << std::setprecision(10) << std::setw(30) << std::left
      << boost::lexical_cast<std::string>(b) << the_function(b) << std::endl;
}

void graph(const char*, const char*)
{
   do_graph(3);
}

template <class T>
void do_test(T, const char* name)
{
   boost::math::ntl::RR::SetPrecision(working_precision);
   if(started)
   {
      //
      // We want to test the approximation at fixed precision:
      // either float, double or long double.  Begin by getting the
      // polynomials:
      //
      boost::math::tools::polynomial<T> n, d;
      boost::math::tools::polynomial<boost::math::ntl::RR> nr, dr;
      nr = p_remez->numerator();
      dr = p_remez->denominator();
      n = nr;
      d = dr;

      std::vector<boost::math::ntl::RR> cn1, cd1;
      cn1 = nr.chebyshev();
      cd1 = dr.chebyshev();
      std::vector<T> cn, cd;
      for(unsigned i = 0; i < cn1.size(); ++i)
      {
         cn.push_back(boost::math::tools::real_cast<T>(cn1[i]));
      }
      for(unsigned i = 0; i < cd1.size(); ++i)
      {
         cd.push_back(boost::math::tools::real_cast<T>(cd1[i]));
      }
      //
      // We'll test at the Chebeshev control points which is where
      // (in theory) the largest deviation should occur.  For good
      // measure we'll test at the zeros as well:
      //
      boost::numeric::ublas::vector<boost::math::ntl::RR>
         zeros(p_remez->zero_points()),
         cheb(p_remez->chebyshev_points());

      boost::math::ntl::RR max_error(0), cheb_max_error(0);

      //
      // Do the tests at the zeros:
      //
      std::cout << "Starting tests at " << name << " precision...\n";
      std::cout << "Absissa        Error (Poly)   Error (Cheb)\n";
      for(unsigned i = 0; i < zeros.size(); ++i)
      {
         boost::math::ntl::RR true_result = the_function(zeros[i]);
         T absissa = boost::math::tools::real_cast<T>(zeros[i]);
         boost::math::ntl::RR test_result = n.evaluate(absissa) / d.evaluate(absissa);
         boost::math::ntl::RR cheb_result = boost::math::tools::evaluate_chebyshev(cn, absissa) / boost::math::tools::evaluate_chebyshev(cd, absissa);
         boost::math::ntl::RR err, cheb_err;
         if(rel_error)
         {
            err = boost::math::tools::relative_error(test_result, true_result);
            cheb_err = boost::math::tools::relative_error(cheb_result, true_result);
         }
         else
         {
            err = fabs(test_result - true_result);
            cheb_err = fabs(cheb_result - true_result);
         }
         if(err > max_error)
            max_error = err;
         if(cheb_err > cheb_max_error)
            cheb_max_error = cheb_err;
         std::cout << std::setprecision(6) << std::setw(15) << std::left << absissa
            << std::setw(15) << std::left << boost::math::tools::real_cast<T>(err) << boost::math::tools::real_cast<T>(cheb_err) << std::endl;
      }
      //
      // Do the tests at the Chebeshev control points:
      //
      for(unsigned i = 0; i < cheb.size(); ++i)
      {
         boost::math::ntl::RR true_result = the_function(cheb[i]);
         T absissa = boost::math::tools::real_cast<T>(cheb[i]);
         boost::math::ntl::RR test_result = n.evaluate(absissa) / d.evaluate(absissa);
         boost::math::ntl::RR cheb_result = boost::math::tools::evaluate_chebyshev(cn, absissa) / boost::math::tools::evaluate_chebyshev(cd, absissa);
         boost::math::ntl::RR err, cheb_err;
         if(rel_error)
         {
            err = boost::math::tools::relative_error(test_result, true_result);
            cheb_err = boost::math::tools::relative_error(cheb_result, true_result);
         }
         else
         {
            err = fabs(test_result - true_result);
            cheb_err = fabs(cheb_result - true_result);
         }
         if(err > max_error)
            max_error = err;
         std::cout << std::setprecision(6) << std::setw(15) << std::left << absissa
            << std::setw(15) << std::left << boost::math::tools::real_cast<T>(err) <<
            boost::math::tools::real_cast<T>(cheb_err) << std::endl;
      }
      std::string msg = "Max Error found at ";
      msg += name;
      msg += " precision = ";
      msg.append(62 - 17 - msg.size(), ' ');
      std::cout << msg << std::setprecision(6) << "Poly: " << std::setw(20) << std::left
         << boost::math::tools::real_cast<T>(max_error) << "Cheb: " << boost::math::tools::real_cast<T>(cheb_max_error) << std::endl;
   }
   else
   {
      std::cout << "Nothing to test: try converging an approximation first!!!" << std::endl;
   }
}

void test_float(const char*, const char*)
{
   do_test(float(0), "float");
}

void test_double(const char*, const char*)
{
   do_test(double(0), "double");
}

void test_long(const char*, const char*)
{
   do_test((long double)(0), "long double");
}

void test_all(const char*, const char*)
{
   do_test(float(0), "float");
   do_test(double(0), "double");
   do_test((long double)(0), "long double");
}

template <class T>
void do_test_n(T, const char* name, unsigned count)
{
   boost::math::ntl::RR::SetPrecision(working_precision);
   if(started)
   {
      //
      // We want to test the approximation at fixed precision:
      // either float, double or long double.  Begin by getting the
      // polynomials:
      //
      boost::math::tools::polynomial<T> n, d;
      boost::math::tools::polynomial<boost::math::ntl::RR> nr, dr;
      nr = p_remez->numerator();
      dr = p_remez->denominator();
      n = nr;
      d = dr;

      std::vector<boost::math::ntl::RR> cn1, cd1;
      cn1 = nr.chebyshev();
      cd1 = dr.chebyshev();
      std::vector<T> cn, cd;
      for(unsigned i = 0; i < cn1.size(); ++i)
      {
         cn.push_back(boost::math::tools::real_cast<T>(cn1[i]));
      }
      for(unsigned i = 0; i < cd1.size(); ++i)
      {
         cd.push_back(boost::math::tools::real_cast<T>(cd1[i]));
      }

      boost::math::ntl::RR max_error(0), max_cheb_error(0);
      boost::math::ntl::RR step = (b - a) / count;

      //
      // Do the tests at the zeros:
      //
      std::cout << "Starting tests at " << name << " precision...\n";
      std::cout << "Absissa        Error (poly)   Error (Cheb)\n";
      for(boost::math::ntl::RR x = a; x <= b; x += step)
      {
         boost::math::ntl::RR true_result = the_function(x);
         T absissa = boost::math::tools::real_cast<T>(x);
         boost::math::ntl::RR test_result = n.evaluate(absissa) / d.evaluate(absissa);
         boost::math::ntl::RR cheb_result = boost::math::tools::evaluate_chebyshev(cn, absissa) / boost::math::tools::evaluate_chebyshev(cd, absissa);
         boost::math::ntl::RR err, cheb_err;
         if(rel_error)
         {
            err = boost::math::tools::relative_error(test_result, true_result);
            cheb_err = boost::math::tools::relative_error(cheb_result, true_result);
         }
         else
         {
            err = fabs(test_result - true_result);
            cheb_err = fabs(cheb_result - true_result);
         }
         if(err > max_error)
            max_error = err;
         if(cheb_err > max_cheb_error)
            max_cheb_error = cheb_err;
         std::cout << std::setprecision(6) << std::setw(15) << std::left << boost::math::tools::real_cast<double>(absissa)
            << (test_result < true_result ? "-" : "") << std::setw(20) << std::left
            << boost::math::tools::real_cast<double>(err)
            << boost::math::tools::real_cast<double>(cheb_err) << std::endl;
      }
      std::string msg = "Max Error found at ";
      msg += name;
      msg += " precision = ";
      //msg.append(62 - 17 - msg.size(), ' ');
      std::cout << msg << "Poly: " << std::setprecision(6)
         //<< std::setw(15) << std::left
         << boost::math::tools::real_cast<T>(max_error)
         << " Cheb: " << boost::math::tools::real_cast<T>(max_cheb_error) << std::endl;
   }
   else
   {
      std::cout << "Nothing to test: try converging an approximation first!!!" << std::endl;
   }
}

void test_n(unsigned n)
{
   do_test_n(boost::math::ntl::RR(), "boost::math::ntl::RR", n);
}

void test_float_n(unsigned n)
{
   do_test_n(float(0), "float", n);
}

void test_double_n(unsigned n)
{
   do_test_n(double(0), "double", n);
}

void test_long_n(unsigned n)
{
   do_test_n((long double)(0), "long double", n);
}

void rotate(const char*, const char*)
{
   if(p_remez)
   {
      p_remez->rotate();
   }
   else
   {
      std::cerr << "Nothing to rotate" << std::endl;
   }
}

void rescale(const char*, const char*)
{
   if(p_remez)
   {
      p_remez->rescale(a, b);
   }
   else
   {
      std::cerr << "Nothing to rescale" << std::endl;
   }
}

void graph_poly(const char*, const char*)
{
   int i = 50;
   boost::math::ntl::RR::SetPrecision(working_precision);
   if(started)
   {
      //
      // We want to test the approximation at fixed precision:
      // either float, double or long double.  Begin by getting the
      // polynomials:
      //
      boost::math::tools::polynomial<boost::math::ntl::RR> n, d;
      n = p_remez->numerator();
      d = p_remez->denominator();

      boost::math::ntl::RR max_error(0);
      boost::math::ntl::RR step = (b - a) / i;

      std::cout << "Evaluating Numerator...\n";
      boost::math::ntl::RR val;
      for(val = a; val <= b; val += step)
         std::cout << n.evaluate(val) << std::endl;
      std::cout << "Evaluating Denominator...\n";
      for(val = a; val <= b; val += step)
         std::cout << d.evaluate(val) << std::endl;
   }
   else
   {
      std::cout << "Nothing to test: try converging an approximation first!!!" << std::endl;
   }
}

int test_main(int, char* [])
{
   std::string line;
   real_parser<long double/*boost::math::ntl::RR*/ > const rr_p;
   while(std::getline(std::cin, line))
   {
      if(parse(line.c_str(), str_p("quit"), space_p).full)
         return 0;
      if(false == parse(line.c_str(),
         (

            str_p("range")[assign_a(started, false)] && real_p[assign_a(a)] && real_p[assign_a(b)]
      ||
            str_p("relative")[assign_a(started, false)][assign_a(rel_error, true)]
      ||
            str_p("absolute")[assign_a(started, false)][assign_a(rel_error, false)]
      ||
            str_p("pin")[assign_a(started, false)] && str_p("true")[assign_a(pin, true)]
      ||
            str_p("pin")[assign_a(started, false)] && str_p("false")[assign_a(pin, false)]
      ||
            str_p("pin")[assign_a(started, false)] && str_p("1")[assign_a(pin, true)]
      ||
            str_p("pin")[assign_a(started, false)] && str_p("0")[assign_a(pin, false)]
      ||
            str_p("pin")[assign_a(started, false)][assign_a(pin, true)]
      ||
            str_p("order")[assign_a(started, false)] && uint_p[assign_a(orderN)] && uint_p[assign_a(orderD)]
      ||
            str_p("order")[assign_a(started, false)] && uint_p[assign_a(orderN)]
      ||
            str_p("target-precision") && uint_p[assign_a(target_precision)]
      ||
            str_p("working-precision")[assign_a(started, false)] && uint_p[assign_a(working_precision)]
      ||
            str_p("variant")[assign_a(started, false)] && int_p[assign_a(variant)]
      ||
            str_p("skew")[assign_a(started, false)] && int_p[assign_a(skew)]
      ||
            str_p("brake") && int_p[assign_a(brake)]
      ||
            str_p("step") && int_p[&step_some]
      ||
            str_p("step")[&step]
      ||
            str_p("poly")[&graph_poly]
      ||
            str_p("info")[&show]
      ||
            str_p("graph") && uint_p[&do_graph]
      ||
            str_p("graph")[&graph]
      ||
            str_p("x-offset") && real_p[assign_a(x_offset)]
      ||
            str_p("x-scale") && real_p[assign_a(x_scale)]
      ||
            str_p("y-offset") && str_p("auto")[assign_a(auto_offset_y, true)]
      ||
            str_p("y-offset") && real_p[assign_a(y_offset)][assign_a(auto_offset_y, false)]
      ||
            str_p("test") && str_p("float") && uint_p[&test_float_n]
      ||
            str_p("test") && str_p("float")[&test_float]
      ||
            str_p("test") && str_p("double") && uint_p[&test_double_n]
      ||
            str_p("test") && str_p("double")[&test_double]
      ||
            str_p("test") && str_p("long") && uint_p[&test_long_n]
      ||
            str_p("test") && str_p("long")[&test_long]
      ||
            str_p("test") && str_p("all")[&test_all]
      ||
            str_p("test") && uint_p[&test_n]
      ||
            str_p("rotate")[&rotate]
      ||
            str_p("rescale") && real_p[assign_a(a)] && real_p[assign_a(b)] && epsilon_p[&rescale]

         ), space_p).full)
      {
         std::cout << "Unable to parse directive: \"" << line << "\"" << std::endl;
      }
      else
      {
         std::cout << "Variant              = " << variant << std::endl;
         std::cout << "range                = [" << a << "," << b << "]" << std::endl;
         std::cout << "Relative Error       = " << rel_error << std::endl;
         std::cout << "Pin to Origin        = " << pin << std::endl;
         std::cout << "Order (Num/Denom)    = " << orderN << "/" << orderD << std::endl;
         std::cout << "Target Precision     = " << target_precision << std::endl;
         std::cout << "Working Precision    = " << working_precision << std::endl;
         std::cout << "Skew                 = " << skew << std::endl;
         std::cout << "Brake                = " << brake << std::endl;
         std::cout << "X Offset             = " << x_offset << std::endl;
         std::cout << "X scale              = " << x_scale << std::endl;
         std::cout << "Y Offset             = ";
         if(auto_offset_y)
            std::cout << "Auto (";
         std::cout << y_offset;
         if(auto_offset_y)
            std::cout << ")";
         std::cout << std::endl;
     }
   }
   return 0;
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 *  @file  InitializePose3.h
 *  @author Luca Carlone
 *  @author Frank Dellaert
 *  @date   August, 2014
 */

#include <gtsam/slam/InitializePose3.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/timing.h>

#include <boost/math/special_functions.hpp>

using namespace std;

namespace gtsam {
namespace InitializePose3 {

static const Matrix I9 = I_9x9;
static const Vector zero9 = Vector::Zero(9);
static const Matrix zero33 = Z_3x3;

static const Key keyAnchor = symbol('Z', 9999999);

/* ************************************************************************* */
GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) {

  GaussianFactorGraph linearGraph;
  noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9);

  for(const boost::shared_ptr<NonlinearFactor>& factor: g) {
    Matrix3 Rij;

    boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
        boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
    if (pose3Between)
      Rij = pose3Between->measured().rotation().matrix();
    else
      std::cout << "Error in buildLinearOrientationGraph" << std::endl;

    const FastVector<Key>& keys = factor->keys();
    Key key1 = keys[0], key2 = keys[1];
    Matrix M9 = Z_9x9;
    M9.block(0,0,3,3) = Rij;
    M9.block(3,3,3,3) = Rij;
    M9.block(6,6,3,3) = Rij;
    linearGraph.add(key1, -I9, key2, M9, zero9, model);
  }
  // prior on the anchor orientation
  linearGraph.add(keyAnchor, I9, (Vector(9) << 1.0, 0.0, 0.0,/*  */ 0.0, 1.0, 0.0, /*  */ 0.0, 0.0, 1.0).finished(), model);
  return linearGraph;
}

/* ************************************************************************* */
// Transform VectorValues into valid Rot3
Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) {
  gttic(InitializePose3_computeOrientationsChordal);

  Matrix ppm = Z_3x3; // plus plus minus
  ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1;

  Values validRot3;
  for(const VectorValues::value_type& it: relaxedRot3) {
    Key key = it.first;
    if (key != keyAnchor) {
      const Vector& rotVector = it.second;
      Matrix3 rotMat;
      rotMat(0,0) = rotVector(0); rotMat(0,1) = rotVector(1); rotMat(0,2) = rotVector(2);
      rotMat(1,0) = rotVector(3); rotMat(1,1) = rotVector(4); rotMat(1,2) = rotVector(5);
      rotMat(2,0) = rotVector(6); rotMat(2,1) = rotVector(7); rotMat(2,2) = rotVector(8);

      Matrix U, V; Vector s;
      svd(rotMat, U, s, V);
      Matrix3 normalizedRotMat = U * V.transpose();

      //      std::cout << "rotMat \n" << rotMat << std::endl;
      //      std::cout << "U V' \n" << U * V.transpose() << std::endl;
      //      std::cout << "V \n" << V << std::endl;

      if(normalizedRotMat.determinant() < 0)
        normalizedRotMat = U * ppm * V.transpose();

      Rot3 initRot = Rot3(normalizedRotMat);
      validRot3.insert(key, initRot);
    }
  }
  return validRot3;
}

/* ************************************************************************* */
// Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node
NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) {
  gttic(InitializePose3_buildPose3graph);
  NonlinearFactorGraph pose3Graph;

  for(const boost::shared_ptr<NonlinearFactor>& factor: graph) {

    // recast to a between on Pose3
    boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
        boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
    if (pose3Between)
      pose3Graph.add(pose3Between);

    // recast PriorFactor<Pose3> to BetweenFactor<Pose3>
    boost::shared_ptr<PriorFactor<Pose3> > pose3Prior =
        boost::dynamic_pointer_cast<PriorFactor<Pose3> >(factor);
    if (pose3Prior)
      pose3Graph.emplace_shared<BetweenFactor<Pose3> >(keyAnchor, pose3Prior->keys()[0],
              pose3Prior->prior(), pose3Prior->noiseModel());
  }
  return pose3Graph;
}

/* ************************************************************************* */
// Return the orientations of a graph including only BetweenFactors<Pose3>
Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) {
  gttic(InitializePose3_computeOrientationsChordal);

  // regularize measurements and plug everything in a factor graph
  GaussianFactorGraph relaxedGraph = buildLinearOrientationGraph(pose3Graph);

  // Solve the LFG
  VectorValues relaxedRot3 = relaxedGraph.optimize();

  // normalize and compute Rot3
  return normalizeRelaxedRotations(relaxedRot3);
}

/* ************************************************************************* */
// Return the orientations of a graph including only BetweenFactors<Pose3>
Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, const size_t maxIter, const bool setRefFrame) {
  gttic(InitializePose3_computeOrientationsGradient);

  // this works on the inverse rotations, according to Tron&Vidal,2011
  Values inverseRot;
  inverseRot.insert(keyAnchor, Rot3());
  for(const Values::ConstKeyValuePair& key_value: givenGuess) {
    Key key = key_value.key;
    const Pose3& pose = givenGuess.at<Pose3>(key);
    inverseRot.insert(key, pose.rotation().inverse());
  }

  // Create the map of edges incident on each node
  KeyVectorMap adjEdgesMap;
  KeyRotMap factorId2RotMap;

  createSymbolicGraph(adjEdgesMap, factorId2RotMap, pose3Graph);

  // calculate max node degree & allocate gradient
  size_t maxNodeDeg = 0;
  VectorValues grad;
  for(const Values::ConstKeyValuePair& key_value: inverseRot) {
    Key key = key_value.key;
    grad.insert(key,Vector3::Zero());
    size_t currNodeDeg = (adjEdgesMap.at(key)).size();
    if(currNodeDeg > maxNodeDeg)
      maxNodeDeg = currNodeDeg;
  }

  // Create parameters
  double b = 1;
  double f0 = 1/b - (1/b + M_PI) * exp(-b*M_PI);
  double a = (M_PI*M_PI)/(2*f0);
  double rho = 2*a*b;
  double mu_max = maxNodeDeg * rho;
  double stepsize = 2/mu_max; // = 1/(a b dG)

  std::cout <<" b " << b <<" f0 " << f0 <<" a " << a <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl;
  double maxGrad;
  // gradient iterations
  size_t it;
  for(it=0; it < maxIter; it++){
    //////////////////////////////////////////////////////////////////////////
    // compute the gradient at each node
    //std::cout << "it  " << it <<" b " << b <<" f0 " << f0 <<" a " << a
    //   <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl;
    maxGrad = 0;
    for(const Values::ConstKeyValuePair& key_value: inverseRot) {
      Key key = key_value.key;
      //std::cout << "---------------------------key  " << DefaultKeyFormatter(key) << std::endl;
      Vector gradKey = Vector3::Zero();
      // collect the gradient for each edge incident on key
      for(const size_t& factorId: adjEdgesMap.at(key)){
        Rot3 Rij = factorId2RotMap.at(factorId);
        Rot3 Ri = inverseRot.at<Rot3>(key);
        if( key == (pose3Graph.at(factorId))->keys()[0] ){
          Key key1 = (pose3Graph.at(factorId))->keys()[1];
          Rot3 Rj = inverseRot.at<Rot3>(key1);
          gradKey = gradKey + gradientTron(Ri, Rij  * Rj, a, b);
          //std::cout << "key1 " << DefaultKeyFormatter(key1) << " gradientTron(Ri, Rij  * Rj, a, b) \n " << gradientTron(Ri, Rij  * Rj, a, b) << std::endl;
        }else if( key == (pose3Graph.at(factorId))->keys()[1] ){
          Key key0 = (pose3Graph.at(factorId))->keys()[0];
          Rot3 Rj = inverseRot.at<Rot3>(key0);
          gradKey = gradKey + gradientTron(Ri, Rij.between(Rj), a, b);
          //std::cout << "key0 " << DefaultKeyFormatter(key0) << " gradientTron(Ri, Rij.inverse()  * Rj, a, b) \n " << gradientTron(Ri, Rij.between(Rj), a, b) << std::endl;
        }else{
          std::cout << "Error in gradient computation" << std::endl;
        }
      } // end of i-th gradient computation
      grad.at(key) = stepsize *  gradKey;

      double normGradKey = (gradKey).norm();
      //std::cout << "key  " << DefaultKeyFormatter(key) <<" \n grad \n" << grad.at(key) << std::endl;
      if(normGradKey>maxGrad)
        maxGrad = normGradKey;
    } // end of loop over nodes

    //////////////////////////////////////////////////////////////////////////
    // update estimates
    inverseRot = inverseRot.retract(grad);

    //////////////////////////////////////////////////////////////////////////
    // check stopping condition
    if (it>20 && maxGrad < 5e-3)
      break;
  } // enf of gradient iterations

  std::cout << "nr of gradient iterations " << it << "maxGrad " << maxGrad <<  std::endl;

  // Return correct rotations
  const Rot3& Rref = inverseRot.at<Rot3>(keyAnchor); // This will be set to the identity as so far we included no prior
  Values estimateRot;
  for(const Values::ConstKeyValuePair& key_value: inverseRot) {
    Key key = key_value.key;
    if (key != keyAnchor) {
      const Rot3& R = inverseRot.at<Rot3>(key);
      if(setRefFrame)
        estimateRot.insert(key, Rref.compose(R.inverse()));
      else
        estimateRot.insert(key, R.inverse());
    }
  }
  return estimateRot;
}

/* ************************************************************************* */
void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, const NonlinearFactorGraph& pose3Graph){
  size_t factorId = 0;
  for(const boost::shared_ptr<NonlinearFactor>& factor: pose3Graph) {
    boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
        boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
    if (pose3Between){
      Rot3 Rij = pose3Between->measured().rotation();
      factorId2RotMap.insert(pair<Key, Rot3 >(factorId,Rij));

      Key key1 = pose3Between->key1();
      if (adjEdgesMap.find(key1) != adjEdgesMap.end()){  // key is already in
        adjEdgesMap.at(key1).push_back(factorId);
      }else{
        vector<size_t> edge_id;
        edge_id.push_back(factorId);
        adjEdgesMap.insert(pair<Key, vector<size_t> >(key1, edge_id));
      }
      Key key2 = pose3Between->key2();
      if (adjEdgesMap.find(key2) != adjEdgesMap.end()){  // key is already in
        adjEdgesMap.at(key2).push_back(factorId);
      }else{
        vector<size_t> edge_id;
        edge_id.push_back(factorId);
        adjEdgesMap.insert(pair<Key, vector<size_t> >(key2, edge_id));
      }
    }else{
      std::cout << "Error in computeOrientationsGradient" << std::endl;
    }
    factorId++;
  }
}

/* ************************************************************************* */
Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) {
  Vector3 logRot = Rot3::Logmap(R1.between(R2));

  double th = logRot.norm();
  if(th != th){ // the second case means that th = nan (logRot does not work well for +/-pi)
    Rot3 R1pert = R1.compose( Rot3::Expmap(Vector3(0.01, 0.01, 0.01)) ); // some perturbation
    logRot = Rot3::Logmap(R1pert.between(R2));
    th = logRot.norm();
  }
  // exclude small or invalid rotations
  if (th > 1e-5 && th == th){ // nonzero valid rotations
    logRot = logRot / th;
  }else{
    logRot = Vector3::Zero();
    th = 0.0;
  }

  double fdot = a*b*th*exp(-b*th);
  return fdot*logRot;
}

/* ************************************************************************* */
Values initializeOrientations(const NonlinearFactorGraph& graph) {

  // We "extract" the Pose3 subgraph of the original graph: this
  // is done to properly model priors and avoiding operating on a larger graph
  NonlinearFactorGraph pose3Graph = buildPose3graph(graph);

  // Get orientations from relative orientation measurements
  return computeOrientationsChordal(pose3Graph);
}

///* ************************************************************************* */
Values computePoses(NonlinearFactorGraph& pose3graph,  Values& initialRot) {
  gttic(InitializePose3_computePoses);

  // put into Values structure
  Values initialPose;
  for(const Values::ConstKeyValuePair& key_value: initialRot){
    Key key = key_value.key;
    const Rot3& rot = initialRot.at<Rot3>(key);
    Pose3 initializedPose = Pose3(rot, Point3(0, 0, 0));
    initialPose.insert(key, initializedPose);
  }
  // add prior
  noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6);
  initialPose.insert(keyAnchor, Pose3());
  pose3graph.emplace_shared<PriorFactor<Pose3> >(keyAnchor, Pose3(), priorModel);

  // Create optimizer
  GaussNewtonParams params;
  bool singleIter = true;
  if(singleIter){
    params.maxIterations = 1;
  }else{
    std::cout << " \n\n\n\n  performing more than 1 GN iterations \n\n\n" <<std::endl;
    params.setVerbosity("TERMINATION");
  }
  GaussNewtonOptimizer optimizer(pose3graph, initialPose, params);
  Values GNresult = optimizer.optimize();

  // put into Values structure
  Values estimate;
  for(const Values::ConstKeyValuePair& key_value: GNresult) {
    Key key = key_value.key;
    if (key != keyAnchor) {
      const Pose3& pose = GNresult.at<Pose3>(key);
      estimate.insert(key, pose);
    }
  }
  return estimate;
}

/* ************************************************************************* */
Values initialize(const NonlinearFactorGraph& graph) {
  gttic(InitializePose3_initialize);

  // We "extract" the Pose3 subgraph of the original graph: this
  // is done to properly model priors and avoiding operating on a larger graph
  NonlinearFactorGraph pose3Graph = buildPose3graph(graph);

  // Get orientations from relative orientation measurements
  Values valueRot3 = computeOrientationsChordal(pose3Graph);

  // Compute the full poses (1 GN iteration on full poses)
  return computePoses(pose3Graph, valueRot3);
}

/* ************************************************************************* */
Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient) {
  Values initialValues;

  // We "extract" the Pose3 subgraph of the original graph: this
  // is done to properly model priors and avoiding operating on a larger graph
  NonlinearFactorGraph pose3Graph = buildPose3graph(graph);

  // Get orientations from relative orientation measurements
  Values orientations;
  if(useGradient)
    orientations = computeOrientationsGradient(pose3Graph, givenGuess);
  else
    orientations = computeOrientationsChordal(pose3Graph);

//  orientations.print("orientations\n");

  // Compute the full poses (1 GN iteration on full poses)
  return computePoses(pose3Graph, orientations);

  //  for(const Values::ConstKeyValuePair& key_value: orientations) {
  //    Key key = key_value.key;
  //    if (key != keyAnchor) {
  //      const Point3& pos = givenGuess.at<Pose3>(key).translation();
  //      const Rot3& rot = orientations.at<Rot3>(key);
  //      Pose3 initializedPoses = Pose3(rot, pos);
  //      initialValues.insert(key, initializedPoses);
  //    }
  //  }
  //  return initialValues;
}

} // end of namespace lago
} // end of namespace gtsam


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file  Pose3.cpp
 * @brief 3D Pose
 */

#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/concepts.h>
#include <gtsam/base/Lie-inl.h>
#include <boost/foreach.hpp>
#include <iostream>
#include <cmath>

using namespace std;

namespace gtsam {

/** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Pose3);

/** instantiate concept checks */
GTSAM_CONCEPT_POSE_INST(Pose3);

static const Matrix3 I3 = eye(3), Z3 = zeros(3, 3), _I3 = -I3;
static const Matrix6 I6 = eye(6);

/* ************************************************************************* */
Pose3::Pose3(const Pose2& pose2) :
    R_(Rot3::rodriguez(0, 0, pose2.theta())), t_(
        Point3(pose2.x(), pose2.y(), 0)) {
}

/* ************************************************************************* */
// Calculate Adjoint map
// Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)
// Experimental - unit tests of derivatives based on it do not check out yet
Matrix6 Pose3::AdjointMap() const {
  const Matrix3 R = R_.matrix();
  const Vector3 t = t_.vector();
  Matrix3 A = skewSymmetric(t) * R;
  Matrix6 adj;
  adj << R, Z3, A, R;
  return adj;
}

/* ************************************************************************* */
Matrix6 Pose3::adjointMap(const Vector& xi) {
  Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2));
  Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5));
  Matrix6 adj;
  adj << w_hat, Z3, v_hat, w_hat;

  return adj;
}

/* ************************************************************************* */
Vector Pose3::adjoint(const Vector& xi, const Vector& y,
    boost::optional<Matrix&> H) {
  if (H) {
    *H = zeros(6, 6);
    for (int i = 0; i < 6; ++i) {
      Vector dxi = zero(6);
      dxi(i) = 1.0;
      Matrix Gi = adjointMap(dxi);
      (*H).col(i) = Gi * y;
    }
  }
  return adjointMap(xi) * y;
}

/* ************************************************************************* */
Vector Pose3::adjointTranspose(const Vector& xi, const Vector& y,
    boost::optional<Matrix&> H) {
  if (H) {
    *H = zeros(6, 6);
    for (int i = 0; i < 6; ++i) {
      Vector dxi = zero(6);
      dxi(i) = 1.0;
      Matrix GTi = adjointMap(dxi).transpose();
      (*H).col(i) = GTi * y;
    }
  }
  Matrix adjT = adjointMap(xi).transpose();
  return adjointMap(xi).transpose() * y;
}

/* ************************************************************************* */
Matrix6 Pose3::dExpInv_exp(const Vector& xi) {
  // Bernoulli numbers, from Wikipedia
  static const Vector B = (Vector(9) << 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0,
      0.0, 1.0 / 42.0, 0.0, -1.0 / 30);
  static const int N = 5; // order of approximation
  Matrix res = I6;
  Matrix6 ad_i = I6;
  Matrix6 ad_xi = adjointMap(xi);
  double fac = 1.0;
  for (int i = 1; i < N; ++i) {
    ad_i = ad_xi * ad_i;
    fac = fac * i;
    res = res + B(i) / fac * ad_i;
  }
  return res;
}

/* ************************************************************************* */
void Pose3::print(const string& s) const {
  cout << s;
  R_.print("R:\n");
  t_.print("t: ");
}

/* ************************************************************************* */
bool Pose3::equals(const Pose3& pose, double tol) const {
  return R_.equals(pose.R_, tol) && t_.equals(pose.t_, tol);
}

/* ************************************************************************* */
/** Modified from Murray94book version (which assumes w and v normalized?) */
Pose3 Pose3::Expmap(const Vector& xi) {

  // get angular velocity omega and translational velocity v from twist xi
  Point3 w(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5));

  double theta = w.norm();
  if (theta < 1e-10) {
    static const Rot3 I;
    return Pose3(I, v);
  } else {
    Point3 n(w / theta); // axis unit vector
    Rot3 R = Rot3::rodriguez(n.vector(), theta);
    double vn = n.dot(v); // translation parallel to n
    Point3 n_cross_v = n.cross(v); // points towards axis
    Point3 t = (n_cross_v - R * n_cross_v) / theta + vn * n;
    return Pose3(R, t);
  }
}

/* ************************************************************************* */
Vector6 Pose3::Logmap(const Pose3& p) {
  Vector3 w = Rot3::Logmap(p.rotation()), T = p.translation().vector();
  double t = w.norm();
  if (t < 1e-10) {
    Vector6 log;
    log << w, T;
    return log;
  } else {
    Matrix3 W = skewSymmetric(w / t);
    // Formula from Agrawal06iros, equation (14)
    // simplified with Mathematica, and multiplying in T to avoid matrix math
    double Tan = tan(0.5 * t);
    Vector3 WT = W * T;
    Vector3 u = T - (0.5 * t) * WT + (1 - t / (2. * Tan)) * (W * WT);
    Vector6 log;
    log << w, u;
    return log;
  }
}

/* ************************************************************************* */
Pose3 Pose3::retractFirstOrder(const Vector& xi) const {
  Vector3 omega(sub(xi, 0, 3));
  Point3 v(sub(xi, 3, 6));
  Rot3 R = R_.retract(omega); // R is done exactly
  Point3 t = t_ + R_ * v; // First order t approximation
  return Pose3(R, t);
}

/* ************************************************************************* */
// Different versions of retract
Pose3 Pose3::retract(const Vector& xi, Pose3::CoordinatesMode mode) const {
  if (mode == Pose3::EXPMAP) {
    // Lie group exponential map, traces out geodesic
    return compose(Expmap(xi));
  } else if (mode == Pose3::FIRST_ORDER) {
    // First order
    return retractFirstOrder(xi);
  } else {
    // Point3 t = t_.retract(v.vector()); // Incorrect version retracts t independently
    // Point3 t = t_ + R_ * (v+Point3(omega).cross(v)/2); // Second order t approximation
    assert(false);
    exit(1);
  }
}

/* ************************************************************************* */
// different versions of localCoordinates
Vector6 Pose3::localCoordinates(const Pose3& T,
    Pose3::CoordinatesMode mode) const {
  if (mode == Pose3::EXPMAP) {
    // Lie group logarithm map, exact inverse of exponential map
    return Logmap(between(T));
  } else if (mode == Pose3::FIRST_ORDER) {
    // R is always done exactly in all three retract versions below
    Vector3 omega = R_.localCoordinates(T.rotation());

    // Incorrect version
    // Independently computes the logmap of the translation and rotation
    // Vector v = t_.localCoordinates(T.translation());

    // Correct first order t inverse
    Point3 d = R_.unrotate(T.translation() - t_);

    // TODO: correct second order t inverse
    Vector6 local;
    local << omega(0), omega(1), omega(2), d.x(), d.y(), d.z();
    return local;
  } else {
    assert(false);
    exit(1);
  }
}

/* ************************************************************************* */
Matrix4 Pose3::matrix() const {
  const Matrix3 R = R_.matrix();
  const Vector3 T = t_.vector();
  Eigen::Matrix<double, 1, 4> A14;
  A14 << 0.0, 0.0, 0.0, 1.0;
  Matrix4 mat;
  mat << R, T, A14;
  return mat;
}

/* ************************************************************************* */
Pose3 Pose3::transform_to(const Pose3& pose) const {
  Rot3 cRv = R_ * Rot3(pose.R_.inverse());
  Point3 t = pose.transform_to(t_);
  return Pose3(cRv, t);
}

/* ************************************************************************* */
Point3 Pose3::transform_from(const Point3& p, boost::optional<Matrix&> Dpose,
    boost::optional<Matrix&> Dpoint) const {
  if (Dpose) {
    const Matrix R = R_.matrix();
    Matrix DR = R * skewSymmetric(-p.x(), -p.y(), -p.z());
    Dpose->resize(3, 6);
    (*Dpose) << DR, R;
  }
  if (Dpoint)
    *Dpoint = R_.matrix();
  return R_ * p + t_;
}

/* ************************************************************************* */
Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix&> Dpose,
    boost::optional<Matrix&> Dpoint) const {
  const Point3 result = R_.unrotate(p - t_);
  if (Dpose) {
    const Point3& q = result;
    Matrix DR = skewSymmetric(q.x(), q.y(), q.z());
    Dpose->resize(3, 6);
    (*Dpose) << DR, _I3;
  }
  if (Dpoint)
    *Dpoint = R_.transpose();
  return result;
}

/* ************************************************************************* */
Pose3 Pose3::compose(const Pose3& p2, boost::optional<Matrix&> H1,
    boost::optional<Matrix&> H2) const {
  if (H1)
    *H1 = p2.inverse().AdjointMap();
  if (H2)
    *H2 = I6;
  return (*this) * p2;
}

/* ************************************************************************* */
Pose3 Pose3::inverse(boost::optional<Matrix&> H1) const {
  if (H1)
    *H1 = -AdjointMap();
  Rot3 Rt = R_.inverse();
  return Pose3(Rt, Rt * (-t_));
}

/* ************************************************************************* */
// between = compose(p2,inverse(p1));
Pose3 Pose3::between(const Pose3& p2, boost::optional<Matrix&> H1,
    boost::optional<Matrix&> H2) const {
  Pose3 result = inverse() * p2;
  if (H1)
    *H1 = -result.inverse().AdjointMap();
  if (H2)
    *H2 = I6;
  return result;
}

/* ************************************************************************* */
double Pose3::range(const Point3& point, boost::optional<Matrix&> H1,
    boost::optional<Matrix&> H2) const {
  if (!H1 && !H2)
    return transform_to(point).norm();
  Point3 d = transform_to(point, H1, H2);
  double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z, n = sqrt(
      d2);
  Matrix D_result_d = (Matrix(1, 3) << x / n, y / n, z / n);
  if (H1)
    *H1 = D_result_d * (*H1);
  if (H2)
    *H2 = D_result_d * (*H2);
  return n;
}

/* ************************************************************************* */
double Pose3::range(const Pose3& point, boost::optional<Matrix&> H1,
    boost::optional<Matrix&> H2) const {
  double r = range(point.translation(), H1, H2);
  if (H2) {
    Matrix H2_ = *H2 * point.rotation().matrix();
    *H2 = zeros(1, 6);
    insertSub(*H2, H2_, 0, 3);
  }
  return r;
}

/* ************************************************************************* */
boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
  const size_t n = pairs.size();
  if (n < 3)
    return boost::none; // we need at least three pairs

  // calculate centroids
  Vector cp = zero(3), cq = zero(3);
  BOOST_FOREACH(const Point3Pair& pair, pairs){
  cp += pair.first.vector();
  cq += pair.second.vector();
}
  double f = 1.0 / n;
  cp *= f;
  cq *= f;

  // Add to form H matrix
  Matrix H = zeros(3, 3);
  BOOST_FOREACH(const Point3Pair& pair, pairs){
  Vector dp = pair.first.vector() - cp;
  Vector dq = pair.second.vector() - cq;
  H += dp * dq.transpose();
}

// Compute SVD
  Matrix U, V;
  Vector S;
  svd(H, U, S, V);

  // Recover transform with correction from Eggert97machinevisionandapplications
  Matrix UVtranspose = U * V.transpose();
  Matrix detWeighting = eye(3, 3);
  detWeighting(2, 2) = UVtranspose.determinant();
  Rot3 R(Matrix(V * detWeighting * U.transpose()));
  Point3 t = Point3(cq) - R * Point3(cp);
  return Pose3(R, t);
}

/* ************************************************************************* */
std::ostream &operator<<(std::ostream &os, const Pose3& pose) {
  os << pose.rotation() << "\n" << pose.translation() << endl;
  return os;
}

} // namespace gtsam


/*cppimport
<%
cfg['include_dirs'] = ['../..','../extern']
cfg['compiler_args'] = ['-std=c++17', '-w']
cfg['dependencies'] = ['xbin.hpp', '../util/assertions.hpp',
'../util/global_rng.hpp']

cfg['parallel'] = False
setup_pybind11(cfg)
%>
*/

#include <Eigen/Geometry>

#include <random>
#include <unordered_set>

#include "rpxdock/util/Timer.hpp"
#include "rpxdock/util/assertions.hpp"
#include "rpxdock/util/global_rng.hpp"
#include "rpxdock/util/types.hpp"
#include "rpxdock/xbin/xbin.hpp"

#include <pybind11/pybind11.h>
namespace py = pybind11;

namespace rpxdock {
using namespace util;

namespace xbin {
namespace test {

template <class F, int M, int O>
void rand_xform(std::mt19937& rng, Eigen::Transform<F, 3, M, O>& x,
                float max_cart = 512.0f) {
  std::uniform_real_distribution<F> runif;
  std::normal_distribution<F> rnorm;
  Eigen::Quaternion<F> qrand(rnorm(rng), rnorm(rng), rnorm(rng), rnorm(rng));
  qrand.normalize();
  x.linear() = qrand.matrix();
  x.translation() = V3<F>(runif(rng) * max_cart - max_cart / 2.0,
                          runif(rng) * max_cart - max_cart / 2.0,
                          runif(rng) * max_cart - max_cart / 2.0);
}

template <class F>
X3<F> rand_xform(F max_cart = 512.0) {
  X3<F> x;
  rand_xform(global_rng(), x, max_cart);
  return x;
}

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

typedef Eigen::Transform<double, 3, Eigen::AffineCompact> Xform;
// typedef Eigen::Affine3d Xform;

template <template <class X> class XformHash>
int get_num_ori_cells(int ori_nside, double& xcov) {
  std::mt19937 rng((unsigned int)time(0) + 7693487);
  XformHash<Xform> xh(1.0, ori_nside, 512.0);
  int n_ori_bins;
  {
    std::unordered_set<size_t> idx_seen;
    // idx_seen.set_empty_key(std::numeric_limits<uint64_t>::max());

    int NSAMP = std::max(1000000, 500 * ori_nside * ori_nside * ori_nside);
    Xform x;
    for (int i = 0; i < NSAMP; ++i) {
      rand_xform(rng, x, 512.0);
      x.translation()[0] = x.translation()[1] = x.translation()[2] = 0;
      idx_seen.insert(xh.get_key(x));
    }
    n_ori_bins = (int)idx_seen.size();
    xcov = (double)NSAMP / n_ori_bins;
  }
  return n_ori_bins;
}

template <template <class X> class XformHash>
bool xform_hash_perf_test(double cart_resl, double ang_resl,
                          int const N2 = 100 * 1000, unsigned int seed = 0) {
  std::mt19937 rng((unsigned int)time(0) + seed);

  double time_key = 0.0, time_cen = 0.0;

  XformHash<Xform> xh(cart_resl, ang_resl, 512.0);
  ang_resl = xh.ori_resl();
  double cart_resl2 = cart_resl * cart_resl;
  double ang_resl2 = ang_resl * ang_resl;

  std::vector<Xform> samples(N2), centers(N2);

  for (int i = 0; i < N2; ++i) rand_xform(rng, samples[i], 512.0);

  util::Timer tk;
  std::vector<uint64_t> keys(N2);
  for (int i = 0; i < N2; ++i) {
    keys[i] = xh.get_key(samples[i]);
    // centers[i] = xh.get_center( keys[i] );
    // cout << endl;
  }
  time_key += (double)tk.elapsed_nano();

  util::Timer tc;
  for (int i = 0; i < N2; ++i) centers[i] = xh.get_center(keys[i]);
  time_cen += (double)tc.elapsed_nano();

  std::unordered_set<size_t> idx_seen;
  // idx_seen.set_empty_key(std::numeric_limits<uint64_t>::max());
  for (int i = 0; i < N2; ++i) idx_seen.insert(keys[i]);

  double covrad = 0, max_dt = 0, max_da = 0;
  for (int i = 0; i < N2; ++i) {
    Xform l = centers[i].inverse() * samples[i];
    double dt = l.translation().norm();
    Eigen::Matrix3d m;
    for (int k = 0; k < 9; ++k) m.data()[k] = l.data()[k];
    // cout << m << endl;
    // cout << l.rotation() << endl;
    double da = Eigen::AngleAxisd(m).angle() * 180.0 / M_PI;
    // double da = Eigen::AngleAxisd(l.rotation()).angle()*180.0/M_PI;
    double err = sqrt(da * da / ang_resl2 * cart_resl2 + dt * dt);
    covrad = fmax(covrad, err);
    max_dt = fmax(max_dt, dt);
    max_da = fmax(max_da, da);
  }
  if (max_dt > cart_resl * 1.1 || max_dt < cart_resl * 0.8)
    std::cout << "TEST FAIL cart: " << cart_resl << " " << max_dt << std::endl;
  if (max_da > ang_resl * 1.1 || max_da < ang_resl * 0.8)
    std::cout << "TEST FAIL ang: " << ang_resl << " " << max_da << " "
              << xh.get_ori_resl(xh.ori_nside_) << std::endl;
  ASSERT_GT(max_dt, cart_resl * 0.8)
  ASSERT_LT(max_dt, cart_resl * 1.1)
  ASSERT_GT(max_da, ang_resl * 0.8)
  ASSERT_LT(max_da, ang_resl * 1.1)

  double tot_cell_vol = covrad * covrad * covrad * covrad * covrad * covrad *
                        xh.approx_nori() / (cart_resl * cart_resl * cart_resl);
  // printf(
  // " %5.3f/%5.1f cr %5.3f dt %5.3f da %6.3f x2k: %7.3fns k2x: %7.3fns "
  // "%9.3f "
  // "%7lu\n",
  // cart_resl, ang_resl, covrad, max_dt / cart_resl, max_da / ang_resl,
  // time_key / N2, time_cen / N2, tot_cell_vol, xh.approx_nori());

  // cout << " rate " << N1*N2/time_key << "  " << N1*N2/time_cen << endl;
  return true;
}

bool TEST_XformHash_XformHash_bt24_BCC6() {
  unsigned int s = 0;
  int N = 10 * 1000;
  bool pass = true;
  // cout << "  bt24_BCC6";
  pass &= xform_hash_perf_test<XformHash_bt24_BCC6>(4.00, 30.0, N, ++s);
  // cout << "  bt24_BCC6";
  pass &= xform_hash_perf_test<XformHash_bt24_BCC6>(2.00, 20.0, N, ++s);
  // cout << "  bt24_BCC6";
  pass &= xform_hash_perf_test<XformHash_bt24_BCC6>(1.00, 15.0, N, ++s);
  // cout << "  bt24_BCC6";
  pass &= xform_hash_perf_test<XformHash_bt24_BCC6>(0.50, 10.0, N, ++s);
  // cout << "  bt24_BCC6";
  pass &= xform_hash_perf_test<XformHash_bt24_BCC6>(0.25, 5.0, N, ++s);
  // cout << "  bt24_BCC6";
  pass &= xform_hash_perf_test<XformHash_bt24_BCC6>(0.11, 3.3, N, ++s);
  return pass;
}

PYBIND11_MODULE(xbin_test, m) {
  m.def("TEST_XformHash_XformHash_bt24_BCC6",
        &TEST_XformHash_XformHash_bt24_BCC6);
}

}  // namespace test
}  // namespace xbin
}  // namespace rpxdock


/*-----------------------------------------------------------------------------+
Copyright (c) 2007-2009: Joachim Faulhaber
+------------------------------------------------------------------------------+
Copyright (c) 1999-2006: Cortex Software GmbH, Kantstrasse 57, Berlin
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/

/*-----------------------------------------------------------------------------+
itvset_shell.cpp provides  a simple test shells for interval sets.
The shell also gives you a good idea how interval container are working.
+-----------------------------------------------------------------------------*/
#include <iostream>

#include <boost/icl/split_interval_set.hpp>
#include <boost/icl/split_interval_map.hpp>

using namespace std;
using namespace boost;
using namespace boost::icl;

void instructions()
{
    cout << "+++++ Test shell for interval set +++++\n";
    cout << "Type: q e or 0  to quit\n";
    cout << "Type: +         for insertions\n";
    cout << "Type: -         for subtraction\n";
    cout << "Type: j         to join contiguous intervals\n";
    cout << "Type: s         to compute total size\n";
}

void wrongInput()
{
    cout << "Wrong Input ------------------\n";
    instructions();
}

template <class SetTV>
void setTestShell()
{
    SetTV m1;

    try {
        char cmd = 'b';
        typename SetTV::domain_type lwb = typename SetTV::domain_type();
        typename SetTV::domain_type upb = typename SetTV::domain_type();

        instructions();

        for(;;)
        {
            cout << "> ";
            cin >> cmd ;

            switch(cmd)
            {
            case 'q':
            case 'e':
            case '0': cout << "good bye\n"; return;
            case '+':
                {
                    cout << "input: lwb upb >> ";
                    cin >> lwb >> upb;
                    typename SetTV::interval_type itv
                        = typename SetTV::interval_type(lwb,upb);
                    // SetTV::IntervalTD itv = rightOpenInterval(lwb,upb);
                    m1.insert(itv);

                    cout << "+" << itv << " =" << endl;
                    cout << "{" << m1 << "}" << endl;

                }
                break;
            case '-':
                {
                    cout << "input: lwb upb >> ";
                    cin >> lwb >> upb;
                    typename SetTV::interval_type itv
                        = typename SetTV::interval_type(lwb,upb);
                    // m1.subtract(itv);
                    SetTV tmp;
                    tmp.insert(itv);
                    m1 -= tmp;

                    cout << "-" << itv << " =" << endl;
                    cout << "{" << m1 << "}" << endl;

                }
                break;
            case 'j':
                {
                    icl::join(m1);
                    cout << "{" << m1 << "}" << endl;
                }
                break;
            case 's':
                {
                    cout << "size = " << m1.size() << endl;
                }
                break;

            default: wrongInput();
            }
        }

    }
    catch (exception& e)
    {
        cout << "itvset_shell: exception caught: " << endl
             << e.what() << endl;
    }
    catch (...)
    {
        cout << "itvset_shell: unknown exception caught" << endl;
    }
}

int main()
{
    cout << ">>Interval Container Library: Test itvset_shell.cpp <<\n";
    cout << "------------------------------------------------------\n";
    setTestShell< interval_set<int> >();

    return 0;
}


/*
 * @file
 * @author University of Warwick
 * @version 1.0
 *
 * @section LICENSE
 *
 * @section DESCRIPTION
 *
 * Unit Tests for the concrete methods of the Triangle class
 */

#define BOOST_TEST_MODULE Triangle
#include <boost/test/unit_test.hpp>
#include <boost/test/output_test_stream.hpp>
#include <stdexcept>

#include "Triangle.h"
#include "EuclideanPoint.h"

using namespace cupcfd::geometry::shapes;
namespace euc = cupcfd::geometry::euclidean;
namespace utf = boost::unit_test;

// === Constructor ===
// Test 1: Constructor: 3 Defined Points - 2D
BOOST_AUTO_TEST_CASE(constructor_test1)
{
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(4.0, 12.0);
	euc::EuclideanPoint<double,2> p3(3.3, 15.0);

	Triangle<double,2> shape(p1, p2, p3);

	BOOST_CHECK_EQUAL(shape.a.cmp[0], p1.cmp[0]);
	BOOST_CHECK_EQUAL(shape.a.cmp[1], p1.cmp[1]);
	BOOST_CHECK_EQUAL(shape.b.cmp[0], p2.cmp[0]);
	BOOST_CHECK_EQUAL(shape.b.cmp[1], p2.cmp[1]);
	BOOST_CHECK_EQUAL(shape.c.cmp[0], p3.cmp[0]);
	BOOST_CHECK_EQUAL(shape.c.cmp[1], p3.cmp[1]);
}

// Test 2: Constructor: 3 Defined Points - 3D
BOOST_AUTO_TEST_CASE(constructor_test2)
{
	euc::EuclideanPoint<double,3> p1(3.0, 4.0, 5.0);
	euc::EuclideanPoint<double,3> p2(4.0, 12.0, 20.0);
	euc::EuclideanPoint<double,3> p3(3.3, 15.0, 21.0);

	Triangle<double,3> shape(p1, p2, p3);
	BOOST_CHECK_EQUAL(shape.a.cmp[0], p1.cmp[0]);
	BOOST_CHECK_EQUAL(shape.a.cmp[1], p1.cmp[1]);
	BOOST_CHECK_EQUAL(shape.a.cmp[2], p1.cmp[2]);
	BOOST_CHECK_EQUAL(shape.b.cmp[0], p2.cmp[0]);
	BOOST_CHECK_EQUAL(shape.b.cmp[1], p2.cmp[1]);
	BOOST_CHECK_EQUAL(shape.b.cmp[2], p2.cmp[2]);
	BOOST_CHECK_EQUAL(shape.c.cmp[0], p3.cmp[0]);
	BOOST_CHECK_EQUAL(shape.c.cmp[1], p3.cmp[1]);
	BOOST_CHECK_EQUAL(shape.c.cmp[2], p3.cmp[2]);
}

// === isPointInsideBarycentric (static, 2D) ===
BOOST_AUTO_TEST_CASE(isPointInsideBarycentric_static_test1)
{
	// Setup
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(4.0, 12.0);
	euc::EuclideanPoint<double,2> p3(3.3, 15.0);

	// Test and Check
	euc::EuclideanPoint<double,2> p4(3.1, 6.0);

	bool inside = Triangle<double,2>::isPointInsideBarycentric(p1,p2,p3,p4);
	BOOST_CHECK_EQUAL(inside, true);
}

// Test 2: Test a point on top of vertex a - 2D
BOOST_AUTO_TEST_CASE(isPointInsideBarycentric_static_test2)
{
	// Setup
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(4.0, 12.0);
	euc::EuclideanPoint<double,2> p3(3.3, 15.0);

	euc::EuclideanPoint<double,2> p4 = p1;

	// Test and Check
	bool inside = Triangle<double,2>::isPointInsideBarycentric(p1,p2,p3,p4);
	BOOST_CHECK_EQUAL(inside, true);
}

// Test 3: Test a point on top of vertex b - 2D
BOOST_AUTO_TEST_CASE(isPointInsideBarycentric_static_test3)
{
	// Setup
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(4.0, 12.0);
	euc::EuclideanPoint<double,2> p3(3.3, 15.0);

	euc::EuclideanPoint<double,2> p4 = p2;

	// Test and Check
	bool inside = Triangle<double,2>::isPointInsideBarycentric(p1,p2,p3,p4);
	BOOST_CHECK_EQUAL(inside, true);
}

// Test 4: Test a point on top of vertex c - 2D
BOOST_AUTO_TEST_CASE(isPointInsideBarycentric_static_test4)
{
	// Setup
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(4.0, 12.0);
	euc::EuclideanPoint<double,2> p3(3.3, 15.0);

	euc::EuclideanPoint<double,2> p4 = p3;

	// Test and Check
	bool inside = Triangle<double,2>::isPointInsideBarycentric(p1,p2,p3,p4);
	BOOST_CHECK_EQUAL(inside, true);
}

// Test 5: Test a point on edge ab - 2D
BOOST_AUTO_TEST_CASE(isPointInsideBarycentric_static_test5)
{
	// Setup
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(4.0, 12.0);
	euc::EuclideanPoint<double,2> p3(3.3, 15.0);

	// Test and Check
	euc::EuclideanPoint<double,2> p4(3.5, 8.0);

	bool inside = Triangle<double,2>::isPointInsideBarycentric(p1,p2,p3,p4);
	BOOST_CHECK_EQUAL(inside, true);
}

// Test 6: Test a point on edge ac - 2D
BOOST_AUTO_TEST_CASE(isPointInsideBarycentric_static_test6)
{
	// Setup
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(4.0, 12.0);
	euc::EuclideanPoint<double,2> p3(3.3, 15.0);

	// Test and Check
	euc::EuclideanPoint<double,2> p4(3.15, 9.5);

	bool inside = Triangle<double,2>::isPointInsideBarycentric(p1,p2,p3,p4);
	BOOST_CHECK_EQUAL(inside, true);
}

// Test 7: Test a point on edge bc - 2D
BOOST_AUTO_TEST_CASE(isPointInsideBarycentric_static_test7)
{
	// Setup
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(4.0, 12.0);
	euc::EuclideanPoint<double,2> p3(3.3, 15.0);

	// Test and Check
	euc::EuclideanPoint<double,2> p4(3.65, 13.5);

	bool inside = Triangle<double,2>::isPointInsideBarycentric(p1,p2,p3,p4);
	BOOST_CHECK_EQUAL(inside, true);
}

// Test 8: Test a point outside the Triangle - 2D
BOOST_AUTO_TEST_CASE(isPointInsideBarycentric_static_test8)
{
	// Setup
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(4.0, 12.0);
	euc::EuclideanPoint<double,2> p3(3.3, 15.0);

	// Test and Check
	euc::EuclideanPoint<double,2> p4(1.0, 1.34);

	bool inside = Triangle<double,2>::isPointInsideBarycentric(p1,p2,p3,p4);
	BOOST_CHECK_EQUAL(inside, false);
}

// Test 9: Test a point inside when a.cmp[1] == c.cmp[1] - 2D
BOOST_AUTO_TEST_CASE(isPointInsideBarycentric_static_test9)
{
	// Setup
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(3.15, 12.0);
	euc::EuclideanPoint<double,2> p3(3.3, 4.0);

	// Test and Check
	euc::EuclideanPoint<double,2> p4(3.15, 4.2);

	bool inside = Triangle<double,2>::isPointInsideBarycentric(p1,p2,p3,p4);
	BOOST_CHECK_EQUAL(inside, true);
}

// === heronsFormula ===
// Test 1: Test the area is computed correctly - 2D
BOOST_AUTO_TEST_CASE(heronsFormula_test1, * utf::tolerance(0.00001))
{
	// Setup
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(3.15, 12.0);
	euc::EuclideanPoint<double,2> p3(3.3, 4.0);

	// Test and Check
	double area = Triangle<double,2>::heronsFormula(p1, p2, p3);
	BOOST_TEST(area == 1.2);
}

// Test 2: Test the area is computed correctly - 2D
BOOST_AUTO_TEST_CASE(heronsFormula_test2, * utf::tolerance(0.00001))
{
	// Setup
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(6.0, 4.0);
	euc::EuclideanPoint<double,2> p3(3.0, 12.0);

	// Test and Check
	double area = Triangle<double,2>::heronsFormula(p1, p2, p3);
	BOOST_TEST(area == 12.0);
}

// Test 3: Test the area is computed correctly - 3D
BOOST_AUTO_TEST_CASE(areaHeronsFormula_test3,  * utf::tolerance(0.00001))
{
	// Setup
	euc::EuclideanPoint<double,3> p1(3.0, 4.0, 8.0);
	euc::EuclideanPoint<double,3> p2(3.15, 12.0, 14.0);
	euc::EuclideanPoint<double,3> p3(3.3, 4.0, 9.0);

	Triangle<double,3> shape(p1, p2, p3);

	// Test and Check
	double area = Triangle<double,3>::heronsFormula(p1, p2, p3);
	BOOST_TEST(area == 4.25683);
}

// Test 4: Test the area is computed correctly - 3D
BOOST_AUTO_TEST_CASE(areaHeronsFormula_test4,  * utf::tolerance(0.00001))
{
	// Setup
	euc::EuclideanPoint<double,3> p1(3.0, 4.0, 8.9);
	euc::EuclideanPoint<double,3> p2(6.0, 4.0, 7.6);
	euc::EuclideanPoint<double,3> p3(3.0, 12.0, 15.4);

	Triangle<double,3> shape(p1, p2, p3);

	// Test and Check
	double area = Triangle<double,3>::heronsFormula(p1, p2, p3);
	BOOST_TEST(area == 16.3126);
}

// === getArea ===
// Test 1: Test the area is computed correctly - 2D
BOOST_AUTO_TEST_CASE(getArea_test1, * utf::tolerance(0.00001))
{
	// Setup
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(3.15, 12.0);
	euc::EuclideanPoint<double,2> p3(3.3, 4.0);

	// Test and Check
	Triangle<double,2> shape(p1, p2, p3);
	double area = shape.getArea();
	BOOST_TEST(area == 1.2);
}

// Test 2: Test the area is computed correctly - 2D
BOOST_AUTO_TEST_CASE(getArea_test2, * utf::tolerance(0.00001))
{
	// Setup
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(6.0, 4.0);
	euc::EuclideanPoint<double,2> p3(3.0, 12.0);

	// Test and Check
	Triangle<double,2> shape(p1, p2, p3);
	double area = shape.getArea();
	BOOST_TEST(area == 12.0);
}

// Test 3: Test the area is computed correctly - 3D
BOOST_AUTO_TEST_CASE(getArea_test3,  * utf::tolerance(0.00001))
{
	// Setup
	euc::EuclideanPoint<double,3> p1(3.0, 4.0, 8.0);
	euc::EuclideanPoint<double,3> p2(3.15, 12.0, 14.0);
	euc::EuclideanPoint<double,3> p3(3.3, 4.0, 9.0);

	// Test and Check
	Triangle<double,3> shape(p1, p2, p3);
	double area = shape.getArea();
	BOOST_TEST(area == 4.25683);
}

// Test 4: Test the area is computed correctly - 3D
BOOST_AUTO_TEST_CASE(getArea_test4,  * utf::tolerance(0.00001))
{
	// Setup
	euc::EuclideanPoint<double,3> p1(3.0, 4.0, 8.9);
	euc::EuclideanPoint<double,3> p2(6.0, 4.0, 7.6);
	euc::EuclideanPoint<double,3> p3(3.0, 12.0, 15.4);

	// Test and Check
	Triangle<double,3> shape(p1, p2, p3);
	double area = shape.getArea();
	BOOST_TEST(area == 16.3126);
}

// === getNormal ===
// Test 1: Compute the correct normal, and check that the direction is as expected for anticlockwise
// vertices from origin
BOOST_AUTO_TEST_CASE(getNormal_test1,  * utf::tolerance(0.00001))
{
	euc::EuclideanPoint<double,3> p1(0.0, 0.0, 2.0);
	euc::EuclideanPoint<double,3> p2(5.0, 0.0, 2.0);
	euc::EuclideanPoint<double,3> p3(0.0, 5.0, 2.0);

	Triangle<double,3> shape(p1, p2, p3);

	euc::EuclideanVector<double,3> normalCmp(0.0,0.0,1.0);
	euc::EuclideanVector<double,3> normal = shape.getNormal();

	BOOST_TEST(normal.cmp[0] == normalCmp[0]);
	BOOST_TEST(normal.cmp[1] == normalCmp[1]);
	BOOST_TEST(normal.cmp[2] == normalCmp[2]);
}

// Test 2: Compute the correct normal, and check that the direction is as expected for clockwise vertices
// from origin
BOOST_AUTO_TEST_CASE(getNormal_test2,  * utf::tolerance(0.00001))
{
	euc::EuclideanPoint<double,3> p1(0.0, 5.0, 2.0);
	euc::EuclideanPoint<double,3> p2(5.0, 0.0, 2.0);
	euc::EuclideanPoint<double,3> p3(0.0, 0.0, 2.0);

	Triangle<double,3> shape(p1, p2, p3);

	euc::EuclideanVector<double,3> normalCmp(0.0,0.0,-1.0);
	euc::EuclideanVector<double,3> normal = shape.getNormal();

	BOOST_TEST(normal.cmp[0] == normalCmp[0]);
	BOOST_TEST(normal.cmp[1] == normalCmp[1]);
	BOOST_TEST(normal.cmp[2] == normalCmp[2]);
}

// Test 3: Test correct normal for a 2D triangle translated into a 3D space with a Z component of 0.
BOOST_AUTO_TEST_CASE(getNormal_test3,  * utf::tolerance(0.00001))
{

}


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2012-2015 Rokko Developers https://github.com/t-sakashita/rokko
*
* Distributed under the Boost Software License, Version 1.0. (See accompanying
* file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*
*****************************************************************************/

#include <iostream>
#include <boost/lexical_cast.hpp>
#include <rokko/localized_matrix.hpp>
#include <rokko/utility/frank_matrix.hpp>
#include <rokko/utility/timer.hpp>

int main(int argc, char **argv) {
  int dim;
  if (argc > 1) {
    dim = boost::lexical_cast<int>(argv[1]);
  } else {
    std::cin >> dim;
  }

  rokko::timer timer;
  timer.registrate(1, "generate");
  timer.registrate(2, "eigenvalue");

  rokko::global_timer::registrate(1, "generate");
  rokko::global_timer::registrate(2, "eigenvalue");
  timer.start(1);
  rokko::global_timer::start(1);
  rokko::localized_matrix<double, rokko::matrix_col_major> mat(dim, dim);
  rokko::frank_matrix::generate(mat);
  std::cout << "dimension = " << dim << std::endl;
  std::cout << "[elements of frank matrix]" << std::endl;
  std::cout << mat << std::endl;
  timer.stop(1);
  rokko::global_timer::stop(1);

  timer.start(2);
  rokko::global_timer::start(2);
  std::cout << "[eigenvalues of frank matrix]" << std::endl;
  double sum = 0;
  for (int i = 0; i < dim; ++i) {
    double ev = rokko::frank_matrix::eigenvalue(dim, i);
    sum += ev;
    std::cout << ev << "  ";
  }
  std::cout << std::endl;
  timer.stop(2);
  rokko::global_timer::stop(2);

  std::cout << "[sum of eigenvalues of frank matrix]" << std::endl;
  std::cout << sum << std::endl;

  timer.summarize();
  rokko::global_timer::summarize();
}


//
// Copyright 2016 Pixar
//
// Licensed under the Apache License, Version 2.0 (the "Apache License")
// with the following modification; you may not use this file except in
// compliance with the Apache License and the following modification to it:
// Section 6. Trademarks. is deleted and replaced with:
//
// 6. Trademarks. This License does not grant permission to use the trade
//    names, trademarks, service marks, or product names of the Licensor
//    and its affiliates, except as required to comply with Section 4(c) of
//    the License and to reproduce the content of the NOTICE file.
//
// You may obtain a copy of the Apache License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the Apache License with the above modification is
// distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
// KIND, either express or implied. See the Apache License for the specific
// language governing permissions and limitations under the Apache License.
//
#include <boost/python/def.hpp>

#include "pxr/pxr.h"
#include "pxr/base/gf/homogeneous.h"

using namespace boost::python;

PXR_NAMESPACE_USING_DIRECTIVE

void wrapHomogeneous()
{
    def("GetHomogenized", (GfVec4d (*)(const GfVec4d &)) GfGetHomogenized);
    def("GetHomogenized", (GfVec4f (*)(const GfVec4f &)) GfGetHomogenized);

    def("HomogeneousCross", (GfVec4d (*)(const GfVec4d &, const GfVec4d &)) GfHomogeneousCross);
    def("HomogeneousCross", (GfVec4f (*)(const GfVec4f &, const GfVec4f &)) GfHomogeneousCross);

    def("Project", (GfVec3d (*)(const GfVec4d &)) GfProject);
    def("Project", (GfVec3f (*)(const GfVec4f &)) GfProject);
}


#include <cmath>
#include <iostream>
#include <stdio.h>
#include <vector>
#include <sys/time.h>
#include <chrono>
#include <sys/resource.h>   // check the memory usage
#include <stdio.h>
#include <thread>
#include <fstream>
#include <sstream>

#include <NTL/RR.h>
#include <NTL/xdouble.h>
#include <NTL/ZZ.h>
#include <NTL/ZZX.h>
#include <NTL/BasicThreadPool.h>

#include "../src/CZZ.h"
#include "../src/Params.h"
#include "../src/PubKey.h"
#include "../src/Scheme.h"
#include "../src/SchemeAlgo.h"
#include "../src/SecKey.h"
#include "../src/TestScheme.h"
#include "../src/TimeUtils.h"
#include "../src/Ring2Utils.h"
#include "../src/StringUtils.h"
#include "../src/EvaluatorUtils.h"

#include "Database.h"
#include "LRtest.h"
#include "HELR.h"

using namespace NTL;
using namespace std;

int main(int Argc, char** Argv) {

    if(Argc != 3){
        cout << "-------------------------------------------------------------" << endl;
        cerr << "Enter the File and degree of approximation \t"  << "(e.g. $test edin.txt 3) \n ";
    }

    char* filename  =  Argv[1];
    int polydeg = atoi(Argv[2]);  // degree of approximation polynomial

    dMat  zData;
    dMat* zTest = new dMat[5];
    dMat* zTrain = new dMat[5];

    int nLine= readData(zData, filename);

    cout << "Sample the learning and test data ..." << endl;
    cvRandomSamplingData(zTrain, zTest, zData, filename);

    //----------------------------------------------------------------
    // Parameters for Logistic regression
    //----------------------------------------------------------------

    long logN= 17;
    long logp= 28;
    long logl= 10;
    long logq, cBit1, cBit2;
    int max_iter;

    struct LRpar LRparams;
    ReadLRparams(LRparams, max_iter, zTrain[0], polydeg, logp);

    SetNumThreads(LRparams.dim1);
    //SetNumThreads(4);

    switch(polydeg){
        case 3:
            cBit1=  (LRparams.logn - LRparams.log2polyscale);          // 1st iteration
            cBit2 =  (3*logp+ LRparams. logn - LRparams.log2polyscale);  // 2nd~ iteration
            logq = cBit1 + (LRparams.max_iter-1)*(cBit2)+ logp + logl;                  // max-bitlength we need
            break;

        case 7:
            cBit1=  (LRparams.logn - LRparams.log2polyscale);          // 1st iteration
            cBit2=  (4*logp+ LRparams.logn - LRparams.log2polyscale);
            logq= cBit1 + (LRparams.max_iter-1)*(cBit2)+ logp + logl;
            break;
    }

    cout << "Data dimension with dummy vectors: " << LRparams.dim1 << ", Number of lines: " << nLine << endl;

    cout << "-------------------------------------------------------------" << endl;
    cout << "Key Generation ... (logN,logp,logq, nslots)= (" ;
    cout << logN << "," << logp << "," << logq << "," << LRparams.nslots << ")" <<endl;

    auto start= chrono::steady_clock::now();

    Params params(logN, logq);
    SecKey secretKey(params);
    PubKey publicKey(params, secretKey);
    SchemeAux schemeaux(logN);
    Scheme scheme(params, publicKey, schemeaux);
    SchemeAlgo algo(scheme);

    auto end = std::chrono::steady_clock::now();
    auto diff = end - start;
    cout << "KeyGen time= " << chrono::duration <double, milli> (diff).count()/1000.0 << " s" << endl;

    LogReg LR(scheme, secretKey, LRparams);
    dMat HEtheta_list;

    ofstream fout;
    fout.open("beta.txt");

    for(int k = 0; k < 1; ++k){

        cout << "-------------------------------------------------------------" << endl;
        cout << k << "th Data Encryption ... " << endl;

        struct rusage usage;

        start= chrono::steady_clock::now();

        Cipher* zTrainCipher = new Cipher[LRparams.dim1];

        LR.EncryptData(zTrainCipher, zTrain[k]);

        end = std::chrono::steady_clock::now();
        diff = end - start;
        cout << "Enc time= "  << chrono::duration <double, milli> (diff).count()/1000.0 << "(s), " ;

        int ret = getrusage(RUSAGE_SELF,&usage);
        cout<< "Mem: " << usage.ru_maxrss/(1024)  << "(MB)" << endl;

        cout << "-------------------------------------------------------------" << endl;
        cout << "HE Logistic Regression ... "  << endl;

        Cipher* thetaCipher= new Cipher[LRparams.dim1];

        start= chrono::steady_clock::now();

        LR.HElogreg(thetaCipher, zTrainCipher, zTrain[k]);

        end = std::chrono::steady_clock::now();
        diff = end - start;
        cout << "Eval time= "  << chrono::duration <double, milli> (diff).count()/1000.0 << "(s), " ;

        ret = getrusage(RUSAGE_SELF,&usage);
        cout<< "Mem: " << usage.ru_maxrss/(1024)  << "(MB)" << endl;

        cout << "-------------------------------------------------------------" << endl;
        cout << "Decryption ... "  << endl;

        dVec HEtheta(LRparams.dim1, 0.0);

        CZZ* dtheta = new CZZ[LRparams.dim1];

        for(int i=0; i< LRparams.dim1; ++i){
            dtheta[i] = (scheme.decrypt(secretKey, thetaCipher[i]))[0];

            conv(HEtheta[i], dtheta[i].r);
            HEtheta[i] = scaledown(HEtheta[i], LRparams.logp);
            cout << "[" << HEtheta[i] << "] " ;
        }
        cout << ": enc " << endl;

        getAUC(HEtheta, zTest[k]);
        HEtheta_list.push_back(HEtheta);

        cout << "-------------------------------------------------------------" << endl;
        cout << "Compare with unenc LR " << endl;
        dVec mtheta(LRparams.dim1, 0.0);

        for(int i= 0; i< LRparams.max_iter; i++){
            LR_poly(mtheta, zTrain[k], LRparams);
        }

        for(int i= 0; i< LRparams.dim1; i++)
            cout << "[" << mtheta[i] << "] " ;
        cout << ": unenc " << endl;

        getAUC(mtheta, zTest[k]);

        cout << "MSE (HELR/non-HELR): " << getMSE(HEtheta, mtheta) << endl;

        cout << "-------------------------------------------------------------" << endl;
        cout << "Compare with sigmoid LR " << endl;
        dVec mtheta_sig(LRparams.dim1, 0.0);

        for(int i= 0; i< LRparams.max_iter; i++){
            LR_sigmoid(mtheta_sig, zTrain[k], LRparams);
        }

        for(int i= 0; i< LRparams.dim1; i++)
            cout << "[" << mtheta_sig[i] << "] " ;
        cout << ": unenc " << endl;

        getAUC(mtheta_sig, zTest[k]);

        cout << "MSE (HELR/non-HELR): " << getMSE(HEtheta, mtheta_sig) << endl;

    }

    //! write the beta results in the text file
    fout << "-------------------------------------------------------------" << endl;
    fout << "[" << endl;
    for(int i = 0; i < LRparams.dim1; ++i){
        for(int k = 0; k < 4; ++k){
            fout << HEtheta_list[k][i] << "," ;
        }
        fout << HEtheta_list[4][i] << ";" << endl;
    }
    fout << "];" << endl;
    fout.close();

    delete[] zTest;
    delete[] zTrain;

    return 0;
}


#ifndef LARGE
#define LARGE

#include <Eigen/Dense>
#include <algorithm>
#include <iostream>

#include "MiniDnn/layer.h"
#include "MiniDnn/layer/conv.h"
#include "MiniDnn/layer/fully_connected.h"
#include "MiniDnn/layer/ave_pooling.h"
#include "MiniDnn/layer/max_pooling.h"
#include "MiniDnn/layer/relu.h"
#include "MiniDnn/layer/sigmoid.h"
#include "MiniDnn/layer/softmax.h"
#include "MiniDnn/loss.h"
#include "MiniDnn/loss/mse_loss.h"
#include "MiniDnn/loss/cross_entropy_loss.h"
#include "MiniDnn/mnist.h"
#include "MiniDnn/network.h"
#include "MiniDnn/optimizer.h"
#include "MiniDnn/optimizer/sgd.h"
#include "Enclave.h"
#include "Enclave_t.h"  /* print_string */

using namespace std;

# define SIZE_CONTROLL 72
# define CATEGORY 2

class RandData {
public:
    Matrix train_data;
    Matrix train_labels;

    RandData(int size) {
        train_data = Matrix::Random(SIZE_CONTROLL * SIZE_CONTROLL, size);
        train_labels = Matrix::Ones(1, size);
    }
};

void ecall_ml_large() {
    // data
//    MNIST dataset("/Users/rc/Study/Projects/mini-dnn/data/mnist/");
//    dataset.read();
//    int n_train = dataset.train_data.cols();
//    int dim_in = dataset.train_data.rows();
//    std::cout << "mnist train number: " << n_train << std::endl;
//    std::cout << "mnist test number: " << dataset.test_labels.cols() << std::endl;
    RandData dataset(10240);
//    dataset.read();
    int n_train = dataset.train_data.cols();
    int dim_in = dataset.train_data.rows();
    // dnn
    Network dnn;

    // std::chrono::time_point<std::chrono::high_resolution_clock> start, end;
    // std::chrono::duration<double> elapsed;

    //------------------------------------------------------------

    Layer *fc_fc1 = new FullyConnected(SIZE_CONTROLL * SIZE_CONTROLL, SIZE_CONTROLL * SIZE_CONTROLL);
    Layer *fc_relu1 = new ReLU;

    dnn.add_layer(fc_fc1);
    dnn.add_layer(fc_relu1);

    Layer *fc_fc2 = new FullyConnected(SIZE_CONTROLL * SIZE_CONTROLL, CATEGORY);
    Layer *fc_relu2 = new Softmax;

    dnn.add_layer(fc_fc2);
    dnn.add_layer(fc_relu2);

    // loss
    Loss *loss = new CrossEntropy;
    dnn.add_loss(loss);
    // train & test
    SGD opt(0.001, 5e-4, 0.9, true);
    // SGD opt(0.001);
    const int n_epoch = 3;
    // const int batch_size = 3072;
    for (int batch_size=8192; batch_size<=10240; batch_size+=1024){
        printf("batch_size=%d\n", batch_size);
        for (int epoch = 0; epoch < n_epoch; epoch++) {
            shuffle_data(dataset.train_data, dataset.train_labels);
            for (int start_idx = 0, rounds=0; start_idx < n_train && rounds < 1; start_idx += batch_size) {
                int ith_batch = start_idx / batch_size;
                Matrix x_batch = dataset.train_data.block(0, start_idx, dim_in,
                                                          std::min(batch_size, n_train - start_idx));
                Matrix label_batch = dataset.train_labels.block(0, start_idx, 1,
                                                                std::min(batch_size, n_train - start_idx));
                Matrix target_batch = one_hot_encode(label_batch, CATEGORY);

                // start = std::chrono::high_resolution_clock::now();
                ocall_start_clock();
                dnn.forward(x_batch);
                ocall_end_clock("Forward: %f  ");
                // end = std::chrono::high_resolution_clock::now();
                // elapsed = end - start;
                // std::cout << "Forward: " << elapsed.count() << std::endl;

                // start = std::chrono::high_resolution_clock::now();
                ocall_start_clock();
                dnn.backward(x_batch, target_batch);
                // end = std::chrono::high_resolution_clock::now();
                // elapsed = end - start;
                // std::cout << "Backward: " << elapsed.count() << std::endl;

                // optimize
                // dnn.update(opt);
                ocall_end_clock("Backward: %f\n");
                rounds++;
            }

        }
    }
}

#endif


//------------------------------------------------------------------------------
/// \file StateMonad_tests.cpp
/// \author Ernest Yeung
//------------------------------------------------------------------------------
#include "Categories/Monads/OldStateMonad.h"

#include <boost/test/unit_test.hpp>
#include <cmath>
#include <type_traits> // std::underlying_type
#include <utility> // std::pair

BOOST_AUTO_TEST_SUITE(Categories)
BOOST_AUTO_TEST_SUITE(Monads)
BOOST_AUTO_TEST_SUITE(OldStateMonad_tests)

using Categories::Monads::OldStateMonad::StateObject;
using Categories::Monads::OldStateMonad::StateObjectAsPair;
using Categories::Monads::OldStateMonad::multiplication_component;
using Categories::Monads::OldStateMonad::unit;

struct TestInputs
{
  double x_;
  bool yes_;
};

enum class TestEnumState: char
{
  a = 'a',
  b = 'b',
  c = 'c'
};

struct TestStateAsEnum
{
  TestEnumState state_;

  explicit TestStateAsEnum(const TestEnumState state):
    state_{state}
  {}

  explicit TestStateAsEnum():
    state_{TestEnumState::a}
  {}
};

template <typename X, typename Function>
StateObject<X, TestEnumState> test_T_morphism(
  const StateObject<X, TestEnumState> tx, Function f)
{
  const auto y = f(tx.inputs());

  TestEnumState state;

  if (y == 0.0)
  {
    state = TestEnumState::a;
  }
  else if (y > 0.0)
  {
    state = TestEnumState::b;
  }
  else
  {
    state = TestEnumState::c;
  }

  return StateObject<X, TestEnumState>{y, state};
}

BOOST_AUTO_TEST_SUITE(CartesianProductDataType)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(TestStatesDefaultConstructs)
{
  const TestEnumState enum_state {};

  BOOST_TEST(
    static_cast<std::underlying_type_t<TestEnumState>>(enum_state) == 0);

  TestStateAsEnum state {};
  BOOST_TEST(
    static_cast<std::underlying_type_t<TestEnumState>>(state.state_) == 'a');
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(TestEnumStateConstructs)
{
  TestEnumState enum_state {TestEnumState::b};

  BOOST_TEST(
    static_cast<std::underlying_type_t<TestEnumState>>(enum_state) == 'b');

  enum_state = TestEnumState::c;

  BOOST_TEST(
    static_cast<std::underlying_type_t<TestEnumState>>(enum_state) == 'c');

  TestStateAsEnum state {TestEnumState::b};
  BOOST_TEST(
    static_cast<std::underlying_type_t<TestEnumState>>(state.state_) == 'b');
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(StateObjectConstructs)
{
  const StateObject<double, TestEnumState> state_object {
    42.0,
    TestEnumState::c};

  BOOST_TEST(state_object.inputs() == 42.0);
  BOOST_TEST((state_object.state() == TestEnumState::c));
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(StateObjectDefaultConstructs)
{
  {
    const StateObject<double, TestEnumState> state_object {42.0};

    BOOST_TEST(state_object.inputs() == 42.0);
    BOOST_TEST((static_cast<std::underlying_type_t<TestEnumState>>(
      state_object.state()) == 0));
  }
  {
    const StateObject<double, TestStateAsEnum> state_object {42.0};

    BOOST_TEST(state_object.inputs() == 42.0);
    BOOST_TEST((static_cast<std::underlying_type_t<TestEnumState>>(
      state_object.state().state_) == 'a'));
  }
}

BOOST_AUTO_TEST_SUITE_END() // CartesianProductDataType

BOOST_AUTO_TEST_SUITE(UnitComponent)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(UnitCanReturnStateObject)
{
  const StateObject<double, TestEnumState> tx {
    unit<double, TestEnumState>(6.022)};
  BOOST_TEST(tx.inputs() == 6.022);
  BOOST_TEST(
    (static_cast<std::underlying_type_t<TestEnumState>>(tx.state()) == 0));
}

BOOST_AUTO_TEST_SUITE_END() // UnitComponent

BOOST_AUTO_TEST_SUITE(MultiplicationComponent)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(MultiplicationComponentReturnsStateObject)
{
  const std::pair<StateObject<double, TestEnumState>, TestEnumState> ttx {
    StateObject<double, TestEnumState>{6.674, TestEnumState::c},
    TestEnumState::b};

  const auto result = multiplication_component(ttx);

  BOOST_TEST(result.inputs() == 6.674);
  BOOST_TEST((
    static_cast<std::underlying_type_t<TestEnumState>>(result.state()) == 'c'));
}

BOOST_AUTO_TEST_SUITE_END() // MultiplicationComponent

BOOST_AUTO_TEST_SUITE(TMorphisms)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(TestTMorphismTransitionsState)
{
  {
    const StateObject<double, TestEnumState> tx {0.0};
    const auto result = test_T_morphism(tx, sin);
    BOOST_TEST(result.inputs() == 0.0);
    BOOST_TEST((
      static_cast<std::underlying_type_t<TestEnumState>>(result.state()) ==
        'a'));
  }
  {
    const StateObject<double, TestEnumState> tx {0.707};
    const auto result = test_T_morphism(tx, sin);
    BOOST_TEST(result.inputs() == 0.64955575555642242);
    BOOST_TEST((
      static_cast<std::underlying_type_t<TestEnumState>>(result.state()) ==
        'b'));
  }
  {
    const StateObject<double, TestEnumState> tx {-0.107};
    const auto result = test_T_morphism(tx, sin);
    BOOST_TEST(result.inputs() == -0.10679594301412187);
    BOOST_TEST((
      static_cast<std::underlying_type_t<TestEnumState>>(result.state()) ==
        'c'));
  }
}

BOOST_AUTO_TEST_SUITE_END() // TMorphisms

BOOST_AUTO_TEST_SUITE_END() // StateMonad_tests
BOOST_AUTO_TEST_SUITE_END() // Monads
BOOST_AUTO_TEST_SUITE_END() // Categories


/*******************************************************************
 Module: BigInt unit test

 Author: Rafael Sá Menezes

 Date: December 2019

 Test Plan:
   - Constructors
   - Assignments
   - Comparator
   - Math Operations
 \*******************************************************************/

#define BOOST_TEST_MODULE "Big Int"

#include <big-int/bigint.hh>
#include <boost/test/included/unit_test.hpp>
namespace utf = boost::unit_test;

namespace
{
const char *as_string(BigInt const &obj, std::vector<char> &vec)
{
  return obj.as_string(vec.data(), vec.size());
}

void check_bigint_str(BigInt const &obj, const char *expected, bool is_correct)
{
  std::vector<char> bigint_str(obj.digits());
  const char *actual = as_string(obj, bigint_str);
  if(is_correct)
    BOOST_TEST(expected == actual);
  else
    BOOST_TEST(expected != actual);
}

template <class T>
struct BigIntHelper
{
  BigInt obj;

  BigIntHelper()
  {
  }
  explicit BigIntHelper(T val) : obj(val)
  {
  }
  BigIntHelper(char const *val, BigInt::onedig_t base) : obj(val, base)
  {
  }
  void check_value(const char *expected, bool is_correct = true)
  {
    check_bigint_str(obj, expected, is_correct);
  }
};
} // namespace

#define binary_op_test(FIRST_OPERATOR, SECOND_OPERATOR, BIN_OP)                \
  BigInt obj(FIRST_OPERATOR);                                                  \
  int expected = FIRST_OPERATOR BIN_OP SECOND_OPERATOR;                        \
  int actual = obj BIN_OP SECOND_OPERATOR;                                     \
  BOOST_TEST(expected == actual);

#define math_test(FIRST_OPERATOR, SECOND_OPERATOR, BIN_OP)                     \
  BigInt obj(FIRST_OPERATOR);                                                  \
  int expected_result = FIRST_OPERATOR BIN_OP SECOND_OPERATOR;                 \
  BigInt actual_result = obj BIN_OP SECOND_OPERATOR;                           \
  bool expected_equals_actual = actual_result == expected_result;              \
  BOOST_TEST(expected_equals_actual);

#define math_test_signed_generator(NAME, OP)                                   \
  BOOST_AUTO_TEST_CASE(signed_1_##NAME){math_test(-255, -300, OP)};            \
  BOOST_AUTO_TEST_CASE(signed_2_##NAME){math_test(-255, 300, OP)};             \
  BOOST_AUTO_TEST_CASE(signed_3_##NAME){math_test(-255, 255, OP)};             \
  BOOST_AUTO_TEST_CASE(signed_4_##NAME){math_test(-255, 230, OP)};

// ******************** TESTS ********************

// ** Constructors
// Check whether the object is initialized correctly

BOOST_AUTO_TEST_SUITE(constructors)
BOOST_AUTO_TEST_CASE(null_constructor_ok)
{
  BigIntHelper<int> obj;
  obj.check_value(NULL);
}
BOOST_AUTO_TEST_CASE(null_constructor_fail)
{
  BigIntHelper<int> obj;
  obj.check_value("32", false);
}

BOOST_AUTO_TEST_CASE(signed_constructor_ok)
{
  const int input = -42;
  BigIntHelper<int> obj(input);
  obj.check_value("-42");
}
BOOST_AUTO_TEST_CASE(signed_constructor_fail)
{
  const int input = -42;
  BigIntHelper<int> obj(input);
  obj.check_value("32", false);
}

BOOST_AUTO_TEST_CASE(unsigned_constructor_ok)
{
  const unsigned input = 398;
  BigIntHelper<int> obj(input);
  obj.check_value("398");
}
BOOST_AUTO_TEST_CASE(unsigned_constructor_fail)
{
  const unsigned input = 0;
  BigIntHelper<int> obj(input);
  obj.check_value("398", false);
}

BOOST_AUTO_TEST_CASE(string_constructor_ok_1)
{
  const BigInt::onedig_t base = 10;
  const char *input = "42";
  BigIntHelper<int> obj(input, base);
  obj.check_value("42");
}
BOOST_AUTO_TEST_CASE(string_constructor_fail_1)
{
  const BigInt::onedig_t base = 10;
  const char *input = "42";
  BigIntHelper<int> obj(input, base);
  obj.check_value("79", false);
}

BOOST_AUTO_TEST_CASE(string_constructor_ok_2)
{
  const BigInt::onedig_t base = 16;
  const char *input = "FF";
  BigIntHelper<int> obj(input, base);
  obj.check_value("255");
}
BOOST_AUTO_TEST_CASE(string_constructor_fail_2)
{
  const BigInt::onedig_t base = 16;
  const char *input = "A";
  BigIntHelper<int> obj(input, base);
  obj.check_value("100", false);
}

BOOST_AUTO_TEST_CASE(string_constructor_ok_3)
{
  const BigInt::onedig_t base = 10;
  const char *input = "12345678987654321234567890";
  BigIntHelper<int> obj(input, base);
  obj.check_value("12345678987654321234567890");
}
BOOST_AUTO_TEST_CASE(string_constructor_fail_3)
{
  const BigInt::onedig_t base = 10;
  const char *input = "12345678987654321234567890";
  BigIntHelper<int> obj(input, base);
  obj.check_value("1234567898765432123456789", false);
}

BOOST_AUTO_TEST_CASE(bigint_constructor_ok)
{
  BigInt input(42);
  BigIntHelper<BigInt> obj(input);
  obj.check_value("42");
}
BOOST_AUTO_TEST_CASE(bigint_constructor_fail)
{
  BigInt input(42);
  BigIntHelper<BigInt> obj(input);
  obj.check_value(NULL, false);
}
BOOST_AUTO_TEST_SUITE_END()

// ** Assignment
// Check whether the object is initialized correctly after an assignement

BOOST_AUTO_TEST_SUITE(assignment)
BOOST_AUTO_TEST_CASE(signed_assignment_ok)
{
  BigIntHelper<unsigned> obj(42);
  obj.obj = -9090;
  obj.check_value("-9090");
}
BOOST_AUTO_TEST_CASE(signed_assignment_fail)
{
  BigIntHelper<int> obj(-42);
  obj.obj = -9090;
  obj.check_value("-42", false);
}

BOOST_AUTO_TEST_CASE(unsigned_assignment_ok)
{
  BigIntHelper<int> obj(-255);
  unsigned value = 400000;
  obj.obj = value;
  obj.check_value("400000");
}
BOOST_AUTO_TEST_CASE(unsigned_assignment_fail)
{
  BigIntHelper<int> obj(40000);
  obj.obj = 9090;
  obj.check_value("40000", false);
}

BOOST_AUTO_TEST_CASE(bigint_assignment_ok)
{
  BigIntHelper<int> obj(-255);
  BigInt value(42);
  obj.obj = value;
  obj.check_value("42");
}
BOOST_AUTO_TEST_CASE(bigint_assignment_fail)
{
  BigInt value(42);
  BigIntHelper<BigInt> obj(value);
  obj.obj = 9090;
  obj.check_value("42", false);
}

BOOST_AUTO_TEST_CASE(string_assignment_ok_1)
{
  const char *input = "12345678987654321234567890";
  BigIntHelper<int> obj(-255);
  obj.obj = input;
  obj.check_value(input);
}

BOOST_AUTO_TEST_CASE(string_assignment_ok_2)
{
  BigIntHelper<int> obj(-255);
  obj.obj = "255";
  obj.check_value("255");
}

BOOST_AUTO_TEST_CASE(string_assignment_ok_3)
{
  BigIntHelper<int> obj(255);
  obj.obj = "-255";
  obj.check_value("-255");
}

BOOST_AUTO_TEST_CASE(string_assignment_ok_4)
{
  const BigInt::onedig_t base = 10;
  const char *input = "12345678987654321234567890";
  BigIntHelper<char> obj(input, base);
  obj.obj = input;
  obj.check_value(input);
}

BOOST_AUTO_TEST_CASE(string_assignment_fail_1)
{
  const BigInt::onedig_t base = 10;
  const char *input = "12345678987654321234567890";
  BigIntHelper<char> obj(input, base);
  obj.obj = 255;
  obj.check_value(input, false);
}

BOOST_AUTO_TEST_CASE(string_assignment_fail_2)
{
  const BigInt::onedig_t base = 10;
  const char *input = "255";
  BigIntHelper<char> obj(input, base);
  obj.obj = -255;
  obj.check_value("-255255", false);
}
BOOST_AUTO_TEST_SUITE_END()

// ** Comparator
// Check whether comparations are working

BOOST_AUTO_TEST_SUITE(compare)

BOOST_AUTO_TEST_CASE(signed_cmp_lesser_ok_1){binary_op_test(-4567, -300, <)}

BOOST_AUTO_TEST_CASE(signed_cmp_lesser_ok_2){binary_op_test(-1235, -2000, <)}

BOOST_AUTO_TEST_CASE(signed_cmp_greater_ok_1){binary_op_test(-255, -300, >)}

BOOST_AUTO_TEST_CASE(signed_cmp_greater_ok_2){binary_op_test(-255, -200, >)}

BOOST_AUTO_TEST_CASE(signed_cmp_lesser_equal_ok_1){
  binary_op_test(-4567, -300, <=)}

BOOST_AUTO_TEST_CASE(signed_cmp_lesser_equal_ok_2){
  binary_op_test(-4567, -4567, <=)}

BOOST_AUTO_TEST_CASE(signed_cmp_lesser_equal_ok_3){
  binary_op_test(-1235, -2000, <=)}

BOOST_AUTO_TEST_CASE(signed_cmp_greater_equal_ok_1){
  binary_op_test(-300, -4567, >=)}

BOOST_AUTO_TEST_CASE(signed_cmp_greater_equal_ok_2){
  binary_op_test(-4567, -4567, >=)}

BOOST_AUTO_TEST_CASE(signed_cmp_greater_equal_ok_3){
  binary_op_test(-4567, -4566, >=)}

BOOST_AUTO_TEST_CASE(signed_cmp_equal_ok_1){binary_op_test(-255, -255, ==)}

BOOST_AUTO_TEST_CASE(signed_cmp_equal_ok_2){binary_op_test(-255, 0, ==)}

BOOST_AUTO_TEST_CASE(unsigned_cmp_lesser_ok_1){
  binary_op_test(0, (unsigned)200, <)}

BOOST_AUTO_TEST_CASE(unsigned_cmp_lesser_ok_2){
  binary_op_test(200, (unsigned)0, <)}

BOOST_AUTO_TEST_CASE(unsigned_cmp_greater_ok_1){
  binary_op_test(300, (unsigned)200, >)}

BOOST_AUTO_TEST_CASE(unsigned_cmp_greater_ok_2){
  binary_op_test(0, (unsigned)200, >)}

BOOST_AUTO_TEST_CASE(unsigned_cmp_lesser_equal_ok_1){
  binary_op_test(0, (unsigned)200, <=)}

BOOST_AUTO_TEST_CASE(unsigned_cmp_lesser_equal_ok_2){
  binary_op_test(200, (unsigned)0, <=)}

BOOST_AUTO_TEST_CASE(unsigned_cmp_lesser_equal_ok_4){
  binary_op_test(20, (unsigned)20, <=)}

BOOST_AUTO_TEST_CASE(unsigned_cmp_greater_equal_ok_1){
  binary_op_test(300, (unsigned)0, >=)}

BOOST_AUTO_TEST_CASE(unsigned_cmp_greater_equal_ok_3){
  binary_op_test(400, (unsigned)600, >=)}

BOOST_AUTO_TEST_CASE(unsigned_cmp_greater_equal_ok_4){
  binary_op_test(400, (unsigned)400, >=)}

BOOST_AUTO_TEST_CASE(unsigned_cmp_equal_ok_1){
  binary_op_test(255, (unsigned)255, ==)}

BOOST_AUTO_TEST_CASE(unsigned_cmp_equal_ok_2){
  binary_op_test(-255, (unsigned)255, ==)}

BOOST_AUTO_TEST_SUITE_END();

// ** Math Operations
// Check whether math operations are working

BOOST_AUTO_TEST_SUITE(math);

math_test_signed_generator(addition, +);
math_test_signed_generator(subtraction, -);
math_test_signed_generator(multiplication, *);
math_test_signed_generator(division, /);
math_test_signed_generator(mod, %);

BOOST_AUTO_TEST_CASE(floor_pow_ok_1){
  BigInt obj(31);
  unsigned expected = 4;
  unsigned actual = obj.floorPow2();
  BOOST_TEST(expected == actual);
};

BOOST_AUTO_TEST_CASE(floor_pow_ok_2){
  BigInt obj(-31);
  unsigned expected = 4;
  unsigned actual = obj.floorPow2();
  BOOST_TEST(expected == actual);
}

BOOST_AUTO_TEST_CASE(floor_pow_ok_3){
  BigInt obj(32);
  unsigned expected = 5;
  unsigned actual = obj.floorPow2();
  BOOST_TEST(expected == actual);
}

BOOST_AUTO_TEST_CASE(floor_pow_ok_4){
  BigInt obj(-32);
  unsigned expected = 5;
  unsigned actual = obj.floorPow2();
  BOOST_TEST(expected == actual);
}

BOOST_AUTO_TEST_CASE(set_pow_ok_1){
  BigInt obj;
  obj.setPower2(5);
  unsigned expected = 32;
  int expected_is_equal_to_actual = obj == expected;
  BOOST_TEST(expected_is_equal_to_actual);
};

BOOST_AUTO_TEST_CASE(set_pow_ok_2){
  BigInt obj;
  obj.setPower2(0);
  unsigned expected = 1;
  int expected_is_equal_to_actual = obj == expected;
  BOOST_TEST(expected_is_equal_to_actual);
};

BOOST_AUTO_TEST_SUITE_END()

#undef binary_op_test
#undef math_test