#include "OccupancyGrid/OccupancyGrid.h"
#include "OccupancyGrid/loadData.h"

#include "gtsam/base/types.h"
#include <boost/program_options.hpp>

using namespace std;
using namespace gtsam;
using namespace occgrid;
namespace po = boost::program_options;

OccupancyGrid loadOccupancyGrid(po::variables_map vm) {
  double resolution = vm["resolution"].as<double>();
  std::string datadirectory = vm["dir"].as<std::string>();

  vector<Pose2> allposes;
  vector<double> allranges;
  vector<uint8_t> allreflectance;
  //double max_dist;
  // loadDataFromTxt(
  //     "Data/SICK_Odometry.txt",
  //     "Data/SICK_Snapshot.txt",
  //     allposes, allranges,
  //     max_dist);

  loadPlayerSim(
      datadirectory + "/laser_pose_all.bin",
      datadirectory + "/laser_range_all.bin",
      datadirectory + "/scan_angles_all.bin",
      datadirectory + "/laser_reflectance_all.bin",
      allposes, allranges, allreflectance);
  double width, height, origin_x, origin_y;
  shiftPoses(allranges, allposes, width, height, origin_x, origin_y);

  std::cout << "Laser spans (" << width << ", " << height << ")\n";
  if (vm.count("width"))
    width = vm["width"].as<double>();
  if (vm.count("height"))
    height = vm["height"].as<double>();

  // Create the occupancy grid data structure
  OccupancyGrid occupancyGrid(width, height, resolution); //default center to middle
  for (size_t i = 0; i < allranges.size(); i++) {
    const Pose2& pose = allposes[i];
    const double range = allranges[i];
    const uint8_t reflectance = allreflectance[i];
    // this is where factors are added into the factor graph
    occupancyGrid.addLaser(pose, range, reflectance); //add laser to grid
  }
  return occupancyGrid;
}


///////////////////////////////////////////////////////////////////////////////
// example::iterator::flatten_iterator.cpp                                   //
//                                                                           //
///////////////////////////////////////////////////////////////////////////////
#include <iostream>
#include <boost/assign/std/vector.hpp>
#include <boost/assign/list_of.hpp>
#include <boost/range.hpp>
#include <vector>
#include <boost/iterator/flatten_iterator.hpp>
#include <boost/range/flatten_range.hpp>
#include <libs/iterator/example/flatten_iterator.h>

void example_flatten_iterator(std::ostream& os){
    os << "->example_flatten_iterator : ";

    using namespace boost;

    typedef int                                    		val_;
    typedef std::vector<val_>                           vals_;
    typedef range_size<vals_>::type                     size_;
	typedef std::vector<vals_>							mat_;

	mat_ mat;
    {
    	using namespace boost::assign;
        mat.push_back(boost::assign::list_of(1)(2)(3)(4)(5));
        mat.push_back(boost::assign::list_of(6)(7)(8)(9)(10));
		mat.push_back(boost::assign::list_of(11)(12)(13)(14)(15));
	}

	{
    	typedef boost::range_iterator<mat_>::type			it_mat_;
		typedef flatten_iterator<it_mat_>					flat_it_;
        typedef flat_it_::difference_type 					diff_;

		flat_it_ b(boost::begin(mat),boost::end(mat));
		flat_it_ e(boost::end(mat),boost::end(mat));

    	int n = std::distance(b,e);
        os << "n=" << n << std::endl;
    	BOOST_ASSERT(
    		n == boost::size(mat[0]) + boost::size(mat[1]) + boost::size(mat[2])
    	);
        flat_it_ i = b;
		int j = 0;
		for(; i!=e; i++, j++){
        	val_ val = *i;
   			BOOST_ASSERT(val == j+1);
            diff_ d = std::distance(i,e);
   			BOOST_ASSERT(d==n-j);
            flat_it_ ii = b;
            std::advance(ii,j);
            BOOST_ASSERT(ii==i);
    	}

    }

    os << "<-" << std::endl;
}


/**
 * @file set_cover_example.cpp
 * @brief
 * @author Piotr Smulewicz
 * @version 1.0
 * @date 2014-02-17
 */
//! [Set Cover Example]
#include <iostream>
#include <vector>
#include <iterator>

#include <boost/range/irange.hpp>

#include "paal/greedy/set_cover/set_cover.hpp"

int main() {
    std::vector<std::vector<int>> set_to_elements = {
        { 1, 2 },
        { 3, 4, 5, 6 },
        { 7, 8, 9, 10, 11, 12, 13, 0 },
        { 1, 3, 5, 7, 9, 11, 13 },
        { 2, 4, 6, 8, 10, 12, 0 }
    };
    std::vector<int> costs = { 1, 1, 1, 1, 1 };
    auto sets = boost::irange(0, 5);
    std::vector<int> result;
    auto element_index = [](int el){return el;};
    auto cost = paal::greedy::set_cover(sets,
                                        [&](int set){return costs[set];},
                                        [&](int set){return set_to_elements[set];},
                                        back_inserter(result),
                                        element_index);
    std::cout << "Cost: " << cost << std::endl;
}
//! [Set Cover Example]


//  (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)

#include <pch.hpp>

#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/results_collector.hpp>
#include <boost/math/special_functions/beta.hpp>
#include <boost/math/distributions/skew_normal.hpp>
#include <boost/math/tools/polynomial.hpp>
#include <boost/math/tools/roots.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/test/results_collector.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/array.hpp>
#include <boost/type_index.hpp>
#include "table_type.hpp"
#include <iostream>
#include <iomanip>

#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/multiprecision/cpp_complex.hpp>

#define BOOST_CHECK_CLOSE_EX(a, b, prec, i) \
   {\
      unsigned int failures = boost::unit_test::results_collector.results( boost::unit_test::framework::current_test_case().p_id ).p_assertions_failed;\
      BOOST_CHECK_CLOSE(a, b, prec); \
      if(failures != boost::unit_test::results_collector.results( boost::unit_test::framework::current_test_case().p_id ).p_assertions_failed)\
      {\
         std::cerr << "Failure was at row " << i << std::endl;\
         std::cerr << std::setprecision(35); \
         std::cerr << "{ " << data[i][0] << " , " << data[i][1] << " , " << data[i][2];\
         std::cerr << " , " << data[i][3] << " , " << data[i][4] << " , " << data[i][5] << " } " << std::endl;\
      }\
   }

//
// Implement various versions of inverse of the incomplete beta
// using different root finding algorithms, and deliberately "bad"
// starting conditions: that way we get all the pathological cases
// we could ever wish for!!!
//

template <class T, class Policy>
struct ibeta_roots_1   // for first order algorithms
{
   ibeta_roots_1(T _a, T _b, T t, bool inv = false, bool neg = false)
      : a(_a), b(_b), target(t), invert(inv), neg(neg) {}

   T operator()(const T& x)
   {
      return boost::math::detail::ibeta_imp(a, b, (neg ? -x : x), Policy(), invert, true) - target;
   }
private:
   T a, b, target;
   bool invert, neg;
};

template <class T, class Policy>
struct ibeta_roots_2   // for second order algorithms
{
   ibeta_roots_2(T _a, T _b, T t, bool inv = false, bool neg = false)
      : a(_a), b(_b), target(t), invert(inv), neg(neg) {}

   boost::math::tuple<T, T> operator()(const T& xx)
   {
      typedef typename boost::math::lanczos::lanczos<T, Policy>::type L;
      T x = neg ? -xx : xx;
      T f = boost::math::detail::ibeta_imp(a, b, x, Policy(), invert, true) - target;
      T f1 = invert ?
         -boost::math::detail::ibeta_power_terms(b, a, 1 - x, x, L(), true, Policy())
               : boost::math::detail::ibeta_power_terms(a, b, x, 1 - x, L(), true, Policy());
      T y = 1 - x;
      if(y == 0)
         y = boost::math::tools::min_value<T>() * 8;
      f1 /= y * x;

      // make sure we don't have a zero derivative:
      if(f1 == 0)
         f1 = (invert ? -1 : 1) * boost::math::tools::min_value<T>() * 64;

      return boost::math::make_tuple(f, neg ? -f1 : f1);
   }
private:
   T a, b, target;
   bool invert, neg;
};

template <class T, class Policy>
struct ibeta_roots_3   // for third order algorithms
{
   ibeta_roots_3(T _a, T _b, T t, bool inv = false, bool neg = false)
      : a(_a), b(_b), target(t), invert(inv), neg(neg) {}

   boost::math::tuple<T, T, T> operator()(const T& xx)
   {
      typedef typename boost::math::lanczos::lanczos<T, Policy>::type L;
      T x = neg ? -xx : xx;
      T f = boost::math::detail::ibeta_imp(a, b, x, Policy(), invert, true) - target;
      T f1 = invert ?
               -boost::math::detail::ibeta_power_terms(b, a, 1 - x, x, L(), true, Policy())
               : boost::math::detail::ibeta_power_terms(a, b, x, 1 - x, L(), true, Policy());
      T y = 1 - x;
      if(y == 0)
         y = boost::math::tools::min_value<T>() * 8;
      f1 /= y * x;
      T f2 = f1 * (-y * a + (b - 2) * x + 1) / (y * x);
      if(invert)
         f2 = -f2;

      // make sure we don't have a zero derivative:
      if(f1 == 0)
         f1 = (invert ? -1 : 1) * boost::math::tools::min_value<T>() * 64;

      if (neg)
      {
         f1 = -f1;
      }

      return boost::math::make_tuple(f, f1, f2);
   }
private:
   T a, b, target;
   bool invert, neg;
};

double inverse_ibeta_bisect(double a, double b, double z)
{
   typedef boost::math::policies::policy<> pol;
   bool invert = false;
   int bits = std::numeric_limits<double>::digits;

   //
   // special cases, we need to have these because there may be other
   // possible answers:
   //
   if(z == 1) return 1;
   if(z == 0) return 0;

   //
   // We need a good estimate of the error in the incomplete beta function
   // so that we don't set the desired precision too high.  Assume that 3-bits
   // are lost each time the arguments increase by a factor of 10:
   //
   using namespace std;
   int bits_lost = static_cast<int>(ceil(log10((std::max)(a, b)) * 3));
   if(bits_lost < 0)
      bits_lost = 3;
   else
      bits_lost += 3;
   int precision = bits - bits_lost;

   double min = 0;
   double max = 1;
   boost::math::tools::eps_tolerance<double> tol(precision);
   return boost::math::tools::bisect(ibeta_roots_1<double, pol>(a, b, z, invert), min, max, tol).first;
}

double inverse_ibeta_bisect_neg(double a, double b, double z)
{
   typedef boost::math::policies::policy<> pol;
   bool invert = false;
   int bits = std::numeric_limits<double>::digits;

   //
   // special cases, we need to have these because there may be other
   // possible answers:
   //
   if(z == 1) return 1;
   if(z == 0) return 0;

   //
   // We need a good estimate of the error in the incomplete beta function
   // so that we don't set the desired precision too high.  Assume that 3-bits
   // are lost each time the arguments increase by a factor of 10:
   //
   using namespace std;
   int bits_lost = static_cast<int>(ceil(log10((std::max)(a, b)) * 3));
   if(bits_lost < 0)
      bits_lost = 3;
   else
      bits_lost += 3;
   int precision = bits - bits_lost;

   double min = -1;
   double max = 0;
   boost::math::tools::eps_tolerance<double> tol(precision);
   return -boost::math::tools::bisect(ibeta_roots_1<double, pol>(a, b, z, invert, true), min, max, tol).first;
}

double inverse_ibeta_newton(double a, double b, double z)
{
   double guess = 0.5;
   bool invert = false;
   int bits = std::numeric_limits<double>::digits;

   //
   // special cases, we need to have these because there may be other
   // possible answers:
   //
   if(z == 1) return 1;
   if(z == 0) return 0;

   //
   // We need a good estimate of the error in the incomplete beta function
   // so that we don't set the desired precision too high.  Assume that 3-bits
   // are lost each time the arguments increase by a factor of 10:
   //
   using namespace std;
   int bits_lost = static_cast<int>(ceil(log10((std::max)(a, b)) * 3));
   if(bits_lost < 0)
      bits_lost = 3;
   else
      bits_lost += 3;
   int precision = bits - bits_lost;

   double min = 0;
   double max = 1;
   return boost::math::tools::newton_raphson_iterate(ibeta_roots_2<double, boost::math::policies::policy<> >(a, b, z, invert), guess, min, max, precision);
}

double inverse_ibeta_newton_neg(double a, double b, double z)
{
   double guess = 0.5;
   bool invert = false;
   int bits = std::numeric_limits<double>::digits;

   //
   // special cases, we need to have these because there may be other
   // possible answers:
   //
   if(z == 1) return 1;
   if(z == 0) return 0;

   //
   // We need a good estimate of the error in the incomplete beta function
   // so that we don't set the desired precision too high.  Assume that 3-bits
   // are lost each time the arguments increase by a factor of 10:
   //
   using namespace std;
   int bits_lost = static_cast<int>(ceil(log10((std::max)(a, b)) * 3));
   if(bits_lost < 0)
      bits_lost = 3;
   else
      bits_lost += 3;
   int precision = bits - bits_lost;

   double min = -1;
   double max = 0;
   return -boost::math::tools::newton_raphson_iterate(ibeta_roots_2<double, boost::math::policies::policy<> >(a, b, z, invert, true), -guess, min, max, precision);
}

double inverse_ibeta_halley(double a, double b, double z)
{
   double guess = 0.5;
   bool invert = false;
   int bits = std::numeric_limits<double>::digits;

   //
   // special cases, we need to have these because there may be other
   // possible answers:
   //
   if(z == 1) return 1;
   if(z == 0) return 0;

   //
   // We need a good estimate of the error in the incomplete beta function
   // so that we don't set the desired precision too high.  Assume that 3-bits
   // are lost each time the arguments increase by a factor of 10:
   //
   using namespace std;
   int bits_lost = static_cast<int>(ceil(log10((std::max)(a, b)) * 3));
   if(bits_lost < 0)
      bits_lost = 3;
   else
      bits_lost += 3;
   int precision = bits - bits_lost;

   double min = 0;
   double max = 1;
   return boost::math::tools::halley_iterate(ibeta_roots_3<double, boost::math::policies::policy<> >(a, b, z, invert), guess, min, max, precision);
}

double inverse_ibeta_halley_neg(double a, double b, double z)
{
   double guess = -0.5;
   bool invert = false;
   int bits = std::numeric_limits<double>::digits;

   //
   // special cases, we need to have these because there may be other
   // possible answers:
   //
   if(z == 1) return 1;
   if(z == 0) return 0;

   //
   // We need a good estimate of the error in the incomplete beta function
   // so that we don't set the desired precision too high.  Assume that 3-bits
   // are lost each time the arguments increase by a factor of 10:
   //
   using namespace std;
   int bits_lost = static_cast<int>(ceil(log10((std::max)(a, b)) * 3));
   if(bits_lost < 0)
      bits_lost = 3;
   else
      bits_lost += 3;
   int precision = bits - bits_lost;

   double min = -1;
   double max = 0;
   return -boost::math::tools::halley_iterate(ibeta_roots_3<double, boost::math::policies::policy<> >(a, b, z, invert, true), guess, min, max, precision);
}

double inverse_ibeta_schroder(double a, double b, double z)
{
   double guess = 0.5;
   bool invert = false;
   int bits = std::numeric_limits<double>::digits;

   //
   // special cases, we need to have these because there may be other
   // possible answers:
   //
   if(z == 1) return 1;
   if(z == 0) return 0;

   //
   // We need a good estimate of the error in the incomplete beta function
   // so that we don't set the desired precision too high.  Assume that 3-bits
   // are lost each time the arguments increase by a factor of 10:
   //
   using namespace std;
   int bits_lost = static_cast<int>(ceil(log10((std::max)(a, b)) * 3));
   if(bits_lost < 0)
      bits_lost = 3;
   else
      bits_lost += 3;
   int precision = bits - bits_lost;

   double min = 0;
   double max = 1;
   return boost::math::tools::schroder_iterate(ibeta_roots_3<double, boost::math::policies::policy<> >(a, b, z, invert), guess, min, max, precision);
}

template <class Real, class T>
void test_inverses(const T& data)
{
   using namespace std;
   typedef Real                   value_type;

   value_type precision = static_cast<value_type>(ldexp(1.0, 1-boost::math::policies::digits<value_type, boost::math::policies::policy<> >()/2)) * 150;
   if(boost::math::policies::digits<value_type, boost::math::policies::policy<> >() < 50)
      precision = 1;   // 1% or two decimal digits, all we can hope for when the input is truncated

   for(unsigned i = 0; i < data.size(); ++i)
   {
      //
      // These inverse tests are thrown off if the output of the
      // incomplete beta is too close to 1: basically there is insuffient
      // information left in the value we're using as input to the inverse
      // to be able to get back to the original value.
      //
      if(data[i][5] == 0)
      {
         BOOST_CHECK_EQUAL(inverse_ibeta_halley(Real(data[i][0]), Real(data[i][1]), Real(data[i][5])), value_type(0));
         BOOST_CHECK_EQUAL(inverse_ibeta_halley_neg(Real(data[i][0]), Real(data[i][1]), Real(data[i][5])), value_type(0));
         BOOST_CHECK_EQUAL(inverse_ibeta_schroder(Real(data[i][0]), Real(data[i][1]), Real(data[i][5])), value_type(0));
         BOOST_CHECK_EQUAL(inverse_ibeta_newton(Real(data[i][0]), Real(data[i][1]), Real(data[i][5])), value_type(0));
         BOOST_CHECK_EQUAL(inverse_ibeta_newton_neg(Real(data[i][0]), Real(data[i][1]), Real(data[i][5])), value_type(0));
         BOOST_CHECK_EQUAL(inverse_ibeta_bisect(Real(data[i][0]), Real(data[i][1]), Real(data[i][5])), value_type(0));
         BOOST_CHECK_EQUAL(inverse_ibeta_bisect_neg(Real(data[i][0]), Real(data[i][1]), Real(data[i][5])), value_type(0));
      }
      else if((1 - data[i][5] > 0.001)
         && (fabs(data[i][5]) > 2 * boost::math::tools::min_value<value_type>())
         && (fabs(data[i][5]) > 2 * boost::math::tools::min_value<double>()))
      {
         value_type inv = inverse_ibeta_halley(Real(data[i][0]), Real(data[i][1]), Real(data[i][5]));
         BOOST_CHECK_CLOSE_EX(Real(data[i][2]), inv, precision, i);
         inv = inverse_ibeta_halley_neg(Real(data[i][0]), Real(data[i][1]), Real(data[i][5]));
         BOOST_ASSERT(boost::math::isfinite(inv));
         BOOST_CHECK_CLOSE_EX(Real(data[i][2]), inv, precision, i);
         inv = inverse_ibeta_schroder(Real(data[i][0]), Real(data[i][1]), Real(data[i][5]));
         BOOST_CHECK_CLOSE_EX(Real(data[i][2]), inv, precision, i);
         inv = inverse_ibeta_newton(Real(data[i][0]), Real(data[i][1]), Real(data[i][5]));
         BOOST_CHECK_CLOSE_EX(Real(data[i][2]), inv, precision, i);
         inv = inverse_ibeta_newton_neg(Real(data[i][0]), Real(data[i][1]), Real(data[i][5]));
         BOOST_CHECK_CLOSE_EX(Real(data[i][2]), inv, precision, i);
         inv = inverse_ibeta_bisect(Real(data[i][0]), Real(data[i][1]), Real(data[i][5]));
         BOOST_CHECK_CLOSE_EX(Real(data[i][2]), inv, precision, i);
         inv = inverse_ibeta_bisect_neg(Real(data[i][0]), Real(data[i][1]), Real(data[i][5]));
         BOOST_CHECK_CLOSE_EX(Real(data[i][2]), inv, precision, i);
      }
      else if(1 == data[i][5])
      {
         BOOST_CHECK_EQUAL(inverse_ibeta_halley(Real(data[i][0]), Real(data[i][1]), Real(data[i][5])), value_type(1));
         BOOST_CHECK_EQUAL(inverse_ibeta_halley_neg(Real(data[i][0]), Real(data[i][1]), Real(data[i][5])), value_type(1));
         BOOST_CHECK_EQUAL(inverse_ibeta_schroder(Real(data[i][0]), Real(data[i][1]), Real(data[i][5])), value_type(1));
         BOOST_CHECK_EQUAL(inverse_ibeta_newton(Real(data[i][0]), Real(data[i][1]), Real(data[i][5])), value_type(1));
         BOOST_CHECK_EQUAL(inverse_ibeta_newton_neg(Real(data[i][0]), Real(data[i][1]), Real(data[i][5])), value_type(1));
         BOOST_CHECK_EQUAL(inverse_ibeta_bisect(Real(data[i][0]), Real(data[i][1]), Real(data[i][5])), value_type(1));
         BOOST_CHECK_EQUAL(inverse_ibeta_bisect_neg(Real(data[i][0]), Real(data[i][1]), Real(data[i][5])), value_type(1));
      }

   }
}

#ifndef SC_
#define SC_(x) static_cast<typename table_type<T>::type>(BOOST_JOIN(x, L))
#endif

template <class T>
void test_beta(T, const char* /* name */)
{
   //
   // The actual test data is rather verbose, so it's in a separate file
   //
   // The contents are as follows, each row of data contains
   // five items, input value a, input value b, integration limits x, beta(a, b, x) and ibeta(a, b, x):
   //
#  include "ibeta_small_data.ipp"

   test_inverses<T>(ibeta_small_data);

#  include "ibeta_data.ipp"

   test_inverses<T>(ibeta_data);

#  include "ibeta_large_data.ipp"

   test_inverses<T>(ibeta_large_data);
}

#if !defined(BOOST_NO_CXX11_AUTO_DECLARATIONS) && !defined(BOOST_NO_CXX11_UNIFIED_INITIALIZATION_SYNTAX) && !defined(BOOST_NO_CXX11_LAMBDAS)
template <class Complex>
void test_complex_newton()
{
    typedef typename Complex::value_type Real;
    std::cout << "Testing complex Newton's Method on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    using std::abs;
    using std::sqrt;
    using boost::math::tools::complex_newton;
    using boost::math::tools::polynomial;
    using boost::math::constants::half;

    Real tol = std::numeric_limits<Real>::epsilon();
    // p(z) = z^2 + 1, roots: \pm i.
    polynomial<Complex> p{{1,0}, {0, 0}, {1,0}};
    Complex guess{1,1};
    polynomial<Complex> p_prime = p.prime();
    auto f = [&](Complex z) { return std::make_pair<Complex, Complex>(p(z), p_prime(z)); };
    Complex root = complex_newton(f, guess);

    BOOST_CHECK(abs(root.real()) <= tol);
    BOOST_CHECK_CLOSE(root.imag(), 1, tol);

    guess = -guess;
    root = complex_newton(f, guess);
    BOOST_CHECK(abs(root.real()) <= tol);
    BOOST_CHECK_CLOSE(root.imag(), -1, tol);

    // Test that double roots are handled correctly-as correctly as possible.
    // Convergence at a double root is not quadratic.
    // This sets p = (z-i)^2:
    p = polynomial<Complex>({{-1,0}, {0,-2}, {1,0}});
    p_prime = p.prime();
    guess = -guess;
    auto g = [&](Complex z) { return std::make_pair<Complex, Complex>(p(z), p_prime(z)); };
    root = complex_newton(g, guess);
    BOOST_CHECK(abs(root.real()) < 10*sqrt(tol));
    BOOST_CHECK_CLOSE(root.imag(), 1, tol);

    // Test that zero derivatives are handled.
    // p(z) = z^2 + iz + 1
    p = polynomial<Complex>({{1,0}, {0,1}, {1,0}});
    // p'(z) = 2z + i
    p_prime = p.prime();
    guess = Complex(0,-boost::math::constants::half<Real>());
    auto g2 = [&](Complex z) { return std::make_pair<Complex, Complex>(p(z), p_prime(z)); };
    root = complex_newton(g2, guess);

    // Here's the other root, in case code changes cause it to be found:
    //Complex expected_root1{0, half<Real>()*(sqrt(static_cast<Real>(5)) - static_cast<Real>(1))};
    Complex expected_root2{0, -half<Real>()*(sqrt(static_cast<Real>(5)) + static_cast<Real>(1))};

    BOOST_CHECK_CLOSE(expected_root2.imag(),root.imag(), tol);
    BOOST_CHECK(abs(root.real()) < tol);

    // Does a zero root pass the termination criteria?
    p = polynomial<Complex>({{0,0}, {0,0}, {1,0}});
    p_prime = p.prime();
    guess = Complex(0, -boost::math::constants::half<Real>());
    auto g3 = [&](Complex z) { return std::make_pair<Complex, Complex>(p(z), p_prime(z)); };
    root = complex_newton(g3, guess);
    BOOST_CHECK(abs(root.real()) < tol);

    // Does a monstrous root pass?
    Real x = -pow(static_cast<Real>(10), 20);
    p = polynomial<Complex>({{x, x}, {1,0}});
    p_prime = p.prime();
    guess = Complex(0, -boost::math::constants::half<Real>());
    auto g4 = [&](Complex z) { return std::make_pair<Complex, Complex>(p(z), p_prime(z)); };
    root = complex_newton(g4, guess);
    BOOST_CHECK(abs(root.real() + x) < tol);
    BOOST_CHECK(abs(root.imag() + x) < tol);

}

// Polynomials which didn't factorize using Newton's method at first:
void test_daubechies_fails()
{
    std::cout << "Testing failures from Daubechies filter computation.\n";
    using std::abs;
    using std::sqrt;
    using boost::math::tools::complex_newton;
    using boost::math::tools::polynomial;
    using boost::math::constants::half;

    double tol = 500*std::numeric_limits<double>::epsilon();
    polynomial<std::complex<double>> p{{-185961388.136908293,141732493.98435241}, {601080390,0}};
    std::complex<double> guess{1,1};
    polynomial<std::complex<double>> p_prime = p.prime();
    auto f = [&](std::complex<double> z) { return std::make_pair<std::complex<double>, std::complex<double>>(p(z), p_prime(z)); };
    std::complex<double> root = complex_newton(f, guess);

    std::complex<double> expected_root = -p.data()[0]/p.data()[1];
    BOOST_CHECK_CLOSE(expected_root.imag(), root.imag(), tol);
    BOOST_CHECK_CLOSE(expected_root.real(), root.real(), tol);
}
#endif

#if !defined(BOOST_NO_CXX17_IF_CONSTEXPR)
template<class Real>
void test_solve_real_quadratic()
{
    Real tol = std::numeric_limits<Real>::epsilon();
    using boost::math::tools::quadratic_roots;
    auto [x0, x1] = quadratic_roots<Real>(1, 0, -1);
    BOOST_CHECK_CLOSE(x0, Real(-1), tol);
    BOOST_CHECK_CLOSE(x1, Real(1), tol);

    auto p = quadratic_roots<Real>(7, 0, 0);
    BOOST_CHECK_SMALL(p.first, tol);
    BOOST_CHECK_SMALL(p.second, tol);

    // (x-7)^2 = x^2 - 14*x + 49:
    p = quadratic_roots<Real>(1, -14, 49);
    BOOST_CHECK_CLOSE(p.first, Real(7), tol);
    BOOST_CHECK_CLOSE(p.second, Real(7), tol);

    // This test does not pass in multiprecision,
    // due to the fact it does not have an fma:
    if (std::is_floating_point<Real>::value)
    {
        // (x-1)(x-1-eps) = x^2 + (-eps - 2)x + (1)(1+eps)
        Real eps = 2*std::numeric_limits<Real>::epsilon();
        p = quadratic_roots<Real>(256, 256*(-2 - eps), 256*(1 + eps));
        BOOST_CHECK_CLOSE(p.first, Real(1), tol);
        BOOST_CHECK_CLOSE(p.second, Real(1) + eps, tol);
    }

    if (std::is_same<Real, double>::value)
    {
        // Kahan's example: This is the test that demonstrates the necessity of the fma instruction.
        // https://en.wikipedia.org/wiki/Loss_of_significance#Instability_of_the_quadratic_equation
        p = quadratic_roots<Real>(94906265.625, -189812534, 94906268.375);
        BOOST_CHECK_CLOSE_FRACTION(p.first, Real(1), tol);
        BOOST_CHECK_CLOSE_FRACTION(p.second, 1.000000028975958, 4*tol);
    }
}

template<class Z>
void test_solve_int_quadratic()
{
    double tol = std::numeric_limits<double>::epsilon();
    using boost::math::tools::quadratic_roots;
    auto [x0, x1] = quadratic_roots(1, 0, -1);
    BOOST_CHECK_CLOSE(x0, double(-1), tol);
    BOOST_CHECK_CLOSE(x1, double(1), tol);

    auto p = quadratic_roots(7, 0, 0);
    BOOST_CHECK_SMALL(p.first, tol);
    BOOST_CHECK_SMALL(p.second, tol);

    // (x-7)^2 = x^2 - 14*x + 49:
    p = quadratic_roots(1, -14, 49);
    BOOST_CHECK_CLOSE(p.first, double(7), tol);
    BOOST_CHECK_CLOSE(p.second, double(7), tol);
}

template<class Complex>
void test_solve_complex_quadratic()
{
    using Real = typename Complex::value_type;
    Real tol = std::numeric_limits<Real>::epsilon();
    using boost::math::tools::quadratic_roots;
    auto [x0, x1] = quadratic_roots<Complex>({1,0}, {0,0}, {-1,0});
    BOOST_CHECK_CLOSE(x0.real(), Real(-1), tol);
    BOOST_CHECK_CLOSE(x1.real(), Real(1), tol);
    BOOST_CHECK_SMALL(x0.imag(), tol);
    BOOST_CHECK_SMALL(x1.imag(), tol);

    auto p = quadratic_roots<Complex>({7,0}, {0,0}, {0,0});
    BOOST_CHECK_SMALL(p.first.real(), tol);
    BOOST_CHECK_SMALL(p.second.real(), tol);

    // (x-7)^2 = x^2 - 14*x + 49:
    p = quadratic_roots<Complex>({1,0}, {-14,0}, {49,0});
    BOOST_CHECK_CLOSE(p.first.real(), Real(7), tol);
    BOOST_CHECK_CLOSE(p.second.real(), Real(7), tol);

}

#endif

BOOST_AUTO_TEST_CASE( test_main )
{

   test_beta(0.1, "double");

#if !defined(BOOST_NO_CXX11_AUTO_DECLARATIONS) && !defined(BOOST_NO_CXX11_UNIFIED_INITIALIZATION_SYNTAX) && !defined(BOOST_NO_CXX11_LAMBDAS)
   test_complex_newton<std::complex<float>>();
   test_complex_newton<std::complex<double>>();
   test_complex_newton<boost::multiprecision::cpp_complex_100>();
   test_daubechies_fails();
#endif

#if !defined(BOOST_NO_CXX17_IF_CONSTEXPR)
    test_solve_real_quadratic<float>();
    test_solve_real_quadratic<double>();
    test_solve_real_quadratic<long double>();
    test_solve_real_quadratic<boost::multiprecision::cpp_bin_float_50>();

    test_solve_int_quadratic<int>();
    test_solve_complex_quadratic<std::complex<double>>();
#endif

}


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

#include <boost/simd/include/functor.hpp>
#include <boost/simd/constant/register.hpp>
#include <boost/simd/constant/hierarchy.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
   /*!
     @brief Sqrtvalmax generic tag

     Represents the Sqrtvalmax constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    struct Sqrtvalmax : ext::pure_constant_<Sqrtvalmax>
    {
      typedef double default_type;
      typedef ext::pure_constant_<Sqrtvalmax> parent;

      /// INTERNAL ONLY
      template<class Target, class Dummy=void>
      struct  apply
            : meta::int_c < typename Target::type
                          , typename Target::
                            type( (typename Target::type(1)
                                  << (sizeof(typename Target::type)*CHAR_BIT/2))-1
                                )
                          >
      {};
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_Sqrtvalmax( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };

    /// INTERNAL ONLY
    template<class T, class Dummy>
    struct  Sqrtvalmax::apply< boost::dispatch::meta::single_<T>,Dummy>
          : meta::single_<0x5f800000> {};

    /// INTERNAL ONLY
    template<class T, class Dummy>
    struct  Sqrtvalmax::apply<boost::dispatch::meta::double_<T>,Dummy>
          : meta::double_<0x5ff0000000000001ll> {};

    /// INTERNAL ONLY
    template<class T, class Dummy>
    struct  Sqrtvalmax::apply<boost::dispatch::meta::int8_<T>,Dummy>
          : meta::int_c<T, 11> {};

    /// INTERNAL ONLY
    template<class T, class Dummy>
    struct  Sqrtvalmax::apply<boost::dispatch::meta::int16_<T>,Dummy>
          : meta::int_c<T, 181> {};

    /// INTERNAL ONLY
    template<class T, class Dummy>
    struct  Sqrtvalmax::apply<boost::dispatch::meta::int32_<T>,Dummy>
          : meta::int_c<T, 46340> {};

    /// INTERNAL ONLY
    template<class T, class Dummy>
    struct  Sqrtvalmax::apply<boost::dispatch::meta::int64_<T>,Dummy>
          : meta::int_c<T, 3037000499ll> {};
  }
  namespace ext
  {
   template<class Site>
   BOOST_FORCEINLINE generic_dispatcher<tag::Sqrtvalmax, Site> dispatching_Sqrtvalmax(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
   {
     return generic_dispatcher<tag::Sqrtvalmax, Site>();
   }
   template<class... Args>
   struct impl_Sqrtvalmax;
  }
  /*!
    Generates the square root of the greatest finite representable value

    @par Semantic:

    @code
    T r = Sqrtvalmax<T>();
    @endcode

    is similar to:

    @code
    T r =  sqrt(Valmax<T>();
    @endcode
  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Sqrtvalmax, Sqrtvalmax)
} }

#include <boost/simd/constant/common.hpp>

#endif


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

#include <boost/simd/arithmetic/functions/sqrt.hpp>
#include <boost/simd/include/functions/scalar/is_gez.hpp>
#include <boost/simd/sdk/math.hpp>
#include <boost/dispatch/attributes.hpp>
#include <boost/assert.hpp>
#include <cmath>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( sqrt_, tag::cpu_
                                    , (A0)
                                    , (scalar_< double_<A0> >)
                                    )
  {
    typedef typename boost::dispatch::meta::as_floating<A0>::type result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return ::sqrt(a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( sqrt_, tag::cpu_
                                    , (A0)
                                    , (scalar_< single_<A0> >)
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      #ifdef BOOST_SIMD_HAS_SQRTF
      return ::sqrtf(a0);
      #else
      return std::sqrt(a0);
      #endif
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( sqrt_, tag::cpu_
                                    , (A0)
                                    , (scalar_< integer_<A0> >)
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      BOOST_ASSERT_MSG(is_gez(a0), "sqrt input is negative");

      return A0(sqrt(double(a0)));
    }
  };

} } }

#endif


#include "optimiser.hpp"
#include "logger.hpp"
#include "scf.hpp"
#include "atom.hpp"
#include "integrals.hpp"
#include <vector>
#include <Eigen/Core>
#include <iomanip>

double rfo(Vector& dx, Vector& grad, Matrix& hessian, double alpha, double stepsize) {

	Vector step;
	std::pair<double, double> results = rfo_prime(step, grad, hessian, alpha);
	Vector m_dy;
	double m_expect;
	double step_norm = step.norm();
	if (step_norm > stepsize) {
		double v = alpha;
		int niter = 0;
		double ndy_last = 0.0;

		double m_ndy = step_norm;
		m_dy = step;
		m_expect = results.first;

		while(niter < 100) {
			v += (1-step_norm / stepsize) * step_norm / results.second;
			results = rfo_prime(step, grad, hessian, v);
			step_norm = step.norm();

			if (fabs(step_norm - stepsize)/stepsize < 0.001) {
				m_dy = step;
				m_expect = results.first;
				break;
			}
			else if (niter > 10 && fabs(ndy_last - step_norm)/step_norm < 0.001) {
				m_dy = step;
				m_expect = results.first;
				break;
			}

			niter++;
			ndy_last = step_norm;
			if (step_norm < m_ndy) {
				m_ndy = step_norm;
				m_dy = step;
				m_expect = results.first;
			}
		}
	} else {
		m_dy = step;
		m_expect = results.first;
	}

	dx = m_dy;
	return m_expect;
}

double trust_newton(Vector& dx, Vector& grad, Matrix& hessian, double stepsize) {
	EigenSolver hsolve(hessian);
	Vector gtilde = hsolve.eigenvectors().transpose() * grad;
	Vector step = Vector::Zero(hessian.rows());
	for (int i = 0; i < hessian.rows(); i++) {
		double eival = hsolve.eigenvalues()[i];
		if (fabs(eival) > 1e-12)
			step[i] = - gtilde[i] / eival;
	}

	dx = hsolve.eigenvectors() * step;

	Matrix ident = Matrix::Identity(hessian.rows(), hessian.cols());
	if (dx.norm() > stepsize) {
		double eimin = hsolve.eigenvalues()[0];
		double factor = eimin < 0 ? 1.0 : 0.0;
		double mu = (factor + 0.9)*eimin;

		Matrix hshift = hsolve.eigenvalues().asDiagonal();
		for (int i = 0; i < hshift.rows(); i++) {
			double hii = hshift(i, i) - mu;
			hshift(i, i) = 1.0 / (hii * hii);
		}

		double delta = gtilde.transpose() * hshift * gtilde;
		delta = sqrt(delta) - stepsize;

		for (int i = 1; i < 9; i++) {
			double new_mu = (factor + 0.1) * i * eimin;

			for (int i = 0; i < hshift.rows(); i++) {
				double hii = hsolve.eigenvalues()[i] - new_mu;
				hshift(i, i) = 1.0 / (hii * hii);
			}

			double new_delta = gtilde.transpose() * hshift * gtilde;
			new_delta = sqrt(new_delta) - stepsize;

			if (fabs(new_delta) < fabs(delta)) {
				mu = new_mu;
				delta = new_delta;
			}
		}

		for (int i = 0; i < hessian.rows(); i++) {
			double eival = hsolve.eigenvalues()[i];
			if (fabs(eival - mu) > 1e-12)
				step[i] = - gtilde[i] / (eival - mu);
			else step[i] = 0.0;
		}

	}

	dx = hsolve.eigenvectors() * step;
	double expect = -grad.dot(dx) + dx.transpose() * hessian * dx;

	return expect;
}

std::pair<double, double> rfo_prime(Vector& dy, Vector &grad, Matrix& hessian, double alpha) {

	int ndim = hessian.rows();
	int eival = 2;

	Matrix augH = Matrix::Zero(ndim+1, ndim+1);
	augH.block(0, 0, ndim, ndim) = hessian;
	augH.block(ndim, 0, 1, ndim) = -grad.transpose();
	augH.block(0, ndim, ndim, 1) = -grad;

	Matrix B = alpha * Matrix::Identity(ndim+1, ndim+1);
	B(ndim, ndim) = 1.0;

	GeneralizedEigenSolver solver(augH, B);
	double lmin = solver.eigenvalues()[eival];
	Vector step = solver.eigenvectors().block(0, eival, ndim, 1);
	std::cout << solver.eigenvectors() << std::endl << std::endl;
	double vmin = solver.eigenvectors()(ndim, eival);
	if (fabs(vmin) > 1e-12)
		step /= vmin;

	double nu = alpha * lmin;
	EigenSolver hsolve(hessian);
	const Matrix& hvec = hsolve.eigenvectors();
	const Vector& heig = hsolve.eigenvalues();
	double dyprime2 = 0.0;
	double dy2 = 0.0;
	double dotp, denom;
	for (int i = 0; i < hessian.cols(); i++) {
		dotp = hvec.col(i).dot(grad);
		dotp *= dotp;
		denom = heig[i] - nu;
		dyprime2 += dotp / (denom * denom * denom);
		dy2 += dotp / (denom * denom);
	}
	double expect = 1 + alpha * step.dot(step);
	dyprime2 *= 2.0 * lmin / expect;
	expect *= 0.5 * lmin;
	double dyprime = 0.5 * dyprime2 / sqrt(dy2);

	dy = step;
	std::pair<double, double> results = {expect, dyprime};
	return results;

}

void RHFOptimiser::optimise() {
	Logger& log = mol->control->log;
	log.title("GEOMETRY OPTIMIZATION");
	log.initIterationOpt();
	int MAXITER = cmd.get_option<int>("maxiter");
	double CONVERGE = cmd.get_option<double>("gradconverge");

	Vector dx = Vector::Zero(3*mol->getNActiveAtoms());

	bool converged = false;
	Vector grad;
	Matrix hessian;
	calc.iter = 0;
	double trust = cmd.get_option<double>("trust");
	double trust_ratio;
	double expect = 0.0;
	double step_norm = 0.0;
	while (!converged && calc.iter < MAXITER) {
		calc(dx, grad, hessian);
		if (calc.iter > 1) {

			trust_ratio = calc.delta_e / expect;

			double stepnorm = dx.norm();
			if (trust_ratio > 0.75 && 1.25*stepnorm > trust) trust *= 2.0;
			else if (trust_ratio < 0.25) trust = 0.25*stepnorm;
		}

		mol->control->log.optg_dump(calc.iter, grad, dx, mol,
								hessian, trust, calc.delta_e, calc.grad_norm,
								step_norm, calc.energy, expect);

		converged = (calc.grad_norm < CONVERGE) || (fabs(calc.delta_e) < CONVERGE / 1000.0);
		if (!converged) {
			expect = trust_newton(dx, grad, hessian, trust);
			step_norm = dx.norm();
		}
	}

	if (calc.iter < MAXITER) {
		log.print("Converged geometry:\n");
		for (int i = 0; i <mol->getNAtoms(); i++) log.print(mol->getAtom(i));
		log.result("HF Energy", calc.energy, "Hartree");

		if (cmd.get_option<bool>("freq")) frequencies(hessian);

	} else {
		log.result("Geometry optimisation failed to converge.");
		log.print("Last trial geometry\n");
		for (int i = 0; i <mol->getNAtoms(); i++) log.print(mol->getAtom(i));
	}
}

void RHFOptimiser::frequencies(Matrix& hessian) {

	int natom = hessian.rows();

	// form mass-weighted hessian
	int row = 0;
	double mi, mj;
	Matrix mass_hessian = hessian;

	std::vector<int> activeAtoms = mol->getActiveList();
	for (int i : activeAtoms) {
		mi = mol->getAtom(i).getMass();
		mi = sqrt(mi);

		for (int xyz = 0; xyz < 3; xyz++) {

			int col = 0;
			for (int j : activeAtoms) {
				mj = mol->getAtom(j).getMass();
				mj = sqrt(mj);

				for (int abc = 0; abc < 3; abc++) {
					mass_hessian(row, col) /= mi * mj;
					col++;
				}
			}

			row++;
		}
	}

	EigenSolver hsolve(mass_hessian);
	Vector freqs = hsolve.eigenvalues();
	Matrix modes = hsolve.eigenvectors();

	int nimaginary = 0;
	for (int i = 0; i < hessian.rows(); i++) {
		if (freqs[i] < -1e-4) {
			freqs[i] = -sqrt(-freqs[i]);
			nimaginary++;
		} else if (freqs[i] < 1e-4) {
			freqs[i] = 0.0;
		} else freqs[i] = sqrt(freqs[i]);
		freqs[i] /= (2.0 * M_PI);
		freqs[i] *= Logger::TOWAVENUMBERS;
	}

	mol->control->log.frequencies(freqs, modes, cmd.get_option<bool>("modes"));

}

void RHFOptimiser::exponents() {

	int nexp = mol->getBasis().getNExps();

	Logger& log = mol->control->log;
	log.title("EXPONENT OPTIMIZATION");
	log.initIterationOpt();
	int MAXITER = cmd.get_option<int>("maxiter");
	double CONVERGE = cmd.get_option<double>("gradconverge");
	bool withmp2 = cmd.get_option<bool>("mp2");

	std::string active = cmd.get_option<std::string>("active");
	std::vector<int> activex;
	if (active == "all") {
		for (int i = 0; i < nexp; i++) activex.push_back(i);
	} else {
		size_t pos = active.find(',');
		std::string token;
		while (pos != std::string::npos) {
			token = active.substr(0, pos);
			activex.push_back(std::stoi(token)-1);
			active.erase(0, pos+1);
			pos = active.find(',');
		}
		activex.push_back(std::stoi(active) - 1);

		nexp = activex.size();
	}

	int orbital = cmd.get_option<int>("orbital");
	bool momap = cmd.get_option<bool>("momap");
	Vector mocoeffs;

	bool converged = false;
	Matrix xhessian;
	Vector xgrad;
	Vector dxx = Vector::Zero(nexp);
	int iter = 0;
	double trust = cmd.get_option<double>("trust");
	double trust_ratio, delta_e, grad_norm;
	double energy = 0.0;
	double expect = 0.0;
	double step_norm = 0.0;
	while (!converged && iter < MAXITER) {

		IntegralEngine ints(mol, false);
		Fock f(cmd, ints, mol);
		SCF hf(cmd, mol, f);
		hf.rhf(false);
		delta_e = hf.getEnergy() - energy;
		energy = hf.getEnergy();

		if (withmp2) {
			MP2 mp2obj(f);
			mp2obj.tensormp2(false);
			energy += mp2obj.getEnergy();
			delta_e += mp2obj.getEnergy();
		}

		xgrad = f.compute_xgrad(energy, xhessian, activex, cmd);
		grad_norm = xgrad.norm();
		if (momap)
			mocoeffs = f.getCP().col(orbital);

		if (iter > 1) {

			trust_ratio = delta_e / expect;

			step_norm = dxx.norm();
			if (trust_ratio > 0.75 && 1.25*step_norm > trust) trust *= 2.0;
			else if (trust_ratio < 0.25) trust = 0.25*step_norm;
		}

		mol->control->log.optx_dump(iter++, xgrad, dxx, mol,
								xhessian, trust, delta_e, grad_norm,
								step_norm, energy, expect, activex);

		converged = (grad_norm < CONVERGE) || (fabs(delta_e) < CONVERGE / 1000.0);
		if (!converged) {
			expect = trust_newton(dxx, xgrad, xhessian, trust);

			double currexp, newexp;
			int ctr = 0;
			for (int i : activex) {
				currexp = mol->getBasis().getExp(i);
				newexp = currexp + dxx[ctr++];
				newexp = newexp <= 0.0 ? currexp / 10.0 : newexp;
				mol->getBasis().setExp(i, newexp);
			}
		}

	}

	if (calc.iter < MAXITER) {
		log.print("Converged exponents:\n");

		for (int i : activex)
			log.print(std::to_string(i) + ": " + std::to_string(mol->getBasis().getExp(i)));

		log.result("HF Energy", energy, "Hartree");

		if (momap) {
			std::string mapfile = cmd.get_option<std::string>("mapfile");
			log.mo_map(mocoeffs, mol, cmd.get_option<int>("fineness"), mapfile);
		}

	} else {
		log.result("Exponent optimisation failed to converge.");
		log.print("Last trial exponents\n");

		for (int i : activex)
			log.print(std::to_string(i) + ": " + std::to_string(mol->getBasis().getExp(i)));
	}

}

double RHFCalculator::operator()(const Vector& dx, Vector& grad, Matrix& hessian) {

	int offset = 0;
	int natoms = mol->getNAtoms();

	std::vector<int> activeAtoms = mol->getActiveList();
	for (int i : activeAtoms) {
		Atom& a = mol->getAtom(i);
		a.translate(dx[offset], dx[offset+1], dx[offset+2]);
		offset += 3;
	}
	mol->updateBasisPositions();
	mol->calcEnuc();

	IntegralEngine ints(mol, false);
	Fock f(cmd, ints, mol);
	SCF hf(cmd, mol, f);
	hf.rhf(false);
	energy = hf.getEnergy();

	std::vector<Atom> atomlist;
	for (int i = 0; i < natoms; i++) atomlist.push_back(mol->getAtom(i));

	//f.compute_forces(atomlist, mol->getNel()/2);
	f.compute_hessian(atomlist, mol->getNel()/2);
	Matrix g = f.getForces().transpose();
	Vector full_grad = Eigen::Map<Vector>(g.data(), g.cols()*g.rows());

	//f.compute_hessian_numeric(atomlist, mol->getNel()/2, cmd);
	if (natoms == activeAtoms.size()) {
		hessian = f.getHessian();
		grad = full_grad;
	} else {
		Matrix& full_hessian = f.getHessian();

		// Restrict to only active atoms
		int nactive = activeAtoms.size();
		grad = Vector::Zero(3*nactive);
		Matrix temphessian = Matrix::Zero(3*nactive, 3*natoms);
		int row = 0;
		for (int i : activeAtoms) {
			grad.segment(row, 3) = full_grad.segment(3*i, 3);
			temphessian.block(row, 0, 3, 3*natoms) = full_hessian.block(3*i, 0, 3, 3*natoms);
			row += 3;
		}

		row = 0;
		hessian = Matrix::Zero(3*nactive, 3*nactive);
		for (int j : activeAtoms) {
			hessian.block(0, row, 3*nactive, 3) = temphessian.block(0, 3*j, 3*nactive, 3);
			row += 3;
		}

	}

	delta_e = energy - old_e;
	grad_norm = grad.norm();
	old_e = energy;
	iter++;

	return energy;

}


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

/* Author: Elizabeth Fudge */

#include <ompl/base/goals/GoalState.h>
#include <ompl/base/spaces/SE2StateSpace.h>
#include <ompl/base/spaces/DiscreteStateSpace.h>
#include <ompl/control/spaces/RealVectorControlSpace.h>
#include <ompl/control/SimpleSetup.h>
#include <ompl/config.h>
#include <iostream>
#include <limits>
#include <boost/math/constants/constants.hpp>
#include <ompl/control/planners/kpiece/KPIECE1.h>
#include <ompl/control/planners/rrt/RRT.h>

namespace ob = ompl::base;
namespace oc = ompl::control;

class MyProjection : public ob::ProjectionEvaluator
{
public:
	MyProjection(const ob::StateSpacePtr &space) : ob::ProjectionEvaluator(space)
	{
	}
	virtual unsigned int getDimension(void) const
	{
		return 2;
	}
	virtual void defaultCellSizes(void)
	{
		cellSizes_.resize(2);
		cellSizes_[0] = 1;
		cellSizes_[1] = 1;
	}
	virtual void project(const ob::State *state, Eigen::Ref<Eigen::VectorXd> projection) const
	{
		const double *values = state->as<ob::RealVectorStateSpace::StateType>()->values;
		projection(0) = values[0];
		projection(1) = values[1];
	}
};

void propagate(const oc::SpaceInformation *si, const ob::State *state,
	const oc::Control* control, const double duration, ob::State *result)
{
	static double timeStep = .01;
	int nsteps = ceil(duration / timeStep);
	double dt = duration / nsteps;
	const double *u = control->as<oc::RealVectorControlSpace::ControlType>()->values;

	ob::CompoundStateSpace::StateType& s = *result->as<ob::CompoundStateSpace::StateType>();
	ob::SE2StateSpace::StateType& se2 = *s.as<ob::SE2StateSpace::StateType>(0);
	ob::RealVectorStateSpace::StateType& velocity = *s.as<ob::RealVectorStateSpace::StateType>(1);
	ob::DiscreteStateSpace::StateType& gear = *s.as<ob::DiscreteStateSpace::StateType>(2);

	si->getStateSpace()->copyState(result, state);
	for (int i = 0; i<nsteps; i++)
	{
		se2.setX(se2.getX() + dt * velocity.values[0] * cos(se2.getYaw()));
		se2.setY(se2.getY() + dt * velocity.values[0] * sin(se2.getYaw()));
		se2.setYaw(se2.getYaw() + dt * u[0]);
		velocity.values[0] = velocity.values[0] + dt * (u[1] * gear.value);

		// 'guards' - conditions to change gears
		if (gear.value > 0)
		{
			if (gear.value < 3 && velocity.values[0] > 10 * (gear.value + 1))
				gear.value++;
			else if (gear.value > 1 && velocity.values[0] < 10 * gear.value)
				gear.value--;
		}
		if (!si->satisfiesBounds(result))
			return;
	}
}

// The free space consists of two narrow corridors connected at right angle.
// To make the turn, the car will have to downshift.
bool isStateValid(const oc::SpaceInformation *si, const ob::State *state)
{
	const auto *se2 =
		state->as<ob::CompoundState>()->as<ob::SE2StateSpace::StateType>(0);
	return si->satisfiesBounds(state) && (se2->getX() < -80. || se2->getY() > 80.);
}

int main(int /*argc*/, char** /*argv*/)
{
	// plan for hybrid car in SE(2) with discrete gears
	auto SE2(std::make_shared<ob::SE2StateSpace>());
	auto velocity(std::make_shared<ob::RealVectorStateSpace>(1));
	// set the range for gears: [-1,3] inclusive
	auto gear(std::make_shared<ob::DiscreteStateSpace>(-1, 3));
	ob::StateSpacePtr stateSpace = SE2 + velocity + gear;

	// set the bounds for the R^2 part of SE(2)
	ob::RealVectorBounds bounds(2);
	bounds.setLow(-100);
	bounds.setHigh(100);
	SE2->setBounds(bounds);

	// set the bounds for the velocity
	ob::RealVectorBounds velocityBound(1);
	velocityBound.setLow(0);
	velocityBound.setHigh(60);
	velocity->setBounds(velocityBound);

	// create start and goal states
	ob::ScopedState<> start(stateSpace);
	ob::ScopedState<> goal(stateSpace);

	// Both start and goal are states with high velocity with the car in third gear.
	// However, to make the turn, the car cannot stay in third gear and will have to
	// shift to first gear.
	start[0] = start[1] = -90.; // position
	start[2] = boost::math::constants::pi<double>() / 2; // orientation
	start[3] = 40.; // velocity
	start->as<ob::CompoundState>()->as<ob::DiscreteStateSpace::StateType>(2)->value = 3; // gear

	goal[0] = goal[1] = 90.; // position
	goal[2] = 0.; // orientation
	goal[3] = 40.; // velocity
	goal->as<ob::CompoundState>()->as<ob::DiscreteStateSpace::StateType>(2)->value = 3; // gear

	oc::ControlSpacePtr cmanifold(std::make_shared<oc::RealVectorControlSpace>(stateSpace, 2));

	// set the bounds for the control manifold
	ob::RealVectorBounds cbounds(2);
	// bounds for steering input
	cbounds.setLow(0, -1.);
	cbounds.setHigh(0, 1.);
	// bounds for brake/gas input
	cbounds.setLow(1, -20.);
	cbounds.setHigh(1, 20.);
	cmanifold->as<oc::RealVectorControlSpace>()->setBounds(cbounds);

	oc::SimpleSetup setup(cmanifold);
	const oc::SpaceInformation *si = setup.getSpaceInformation().get();
	setup.setStartAndGoalStates(start, goal, 5.);
	setup.setStateValidityChecker([si](const ob::State *state)
	{
		return isStateValid(si, state);
	});
	setup.setStatePropagator([si](const ob::State *state, const oc::Control* control,
		const double duration, ob::State *result)
	{
		propagate(si, state, control, duration, result);
	});
	setup.getSpaceInformation()->setPropagationStepSize(.1);
	setup.getSpaceInformation()->setMinMaxControlDuration(2, 3);
	// set the default projection and planner
	setup.getStateSpace()->registerDefaultProjection(ob::ProjectionEvaluatorPtr(new MyProjection(setup.getStateSpace())));
	setup.setPlanner(std::make_shared<oc::RRT>(setup.getSpaceInformation()));
	// try to solve the problem
	if (setup.solve(100))
	{
		// print the (approximate) solution path: print states along the path
		// and controls required to get from one state to the next
		oc::PathControl& path(setup.getSolutionPath());

		// print out full state on solution path
		// (format: x, y, theta, v, u0, u1, dt)
		for (unsigned int i = 0; i<path.getStateCount(); ++i)
		{
			const ob::State* state = path.getState(i);
			const auto *se2 =
				state->as<ob::CompoundState>()->as<ob::SE2StateSpace::StateType>(0);
			const auto *velocity =
				state->as<ob::CompoundState>()->as<ob::RealVectorStateSpace::StateType>(1);
			const auto *gear =
				state->as<ob::CompoundState>()->as<ob::DiscreteStateSpace::StateType>(2);
			std::cout << se2->getX() << ' ' << se2->getY() << ' ' << se2->getYaw()
				<< ' ' << velocity->values[0] << ' ' << gear->value << ' ';
			if (i == 0)
				// null controls applied for zero seconds to get to start state
				std::cout << "0 0 0";
			else
			{
				// print controls and control duration needed to get from state i-1 to state i
				const double* u =
					path.getControl(i - 1)->as<oc::RealVectorControlSpace::ControlType>()->values;
				std::cout << u[0] << ' ' << u[1] << ' ' << path.getControlDuration(i - 1);
			}
			std::cout << std::endl;
		}
		if (!setup.haveExactSolutionPath())
		{
			std::cout << "Solution is approximate. Distance to actual goal is " <<
				setup.getProblemDefinition()->getSolutionDifference() << std::endl;
		}
	}

	return 0;
}


/*******************************************************************************
* Copyright 2020 Jose Manuel Fajardo
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
*     http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/

#pragma once

#include <iostream>
#include <algorithm>
#include <Eigen/Dense>
#include <math.h>
#include <vector>

#include <dart/dart.hpp>
#include <dart/gui/osg/osg.hpp>
#include <dart/utils/urdf/urdf.hpp>
#include <dart/utils/utils.hpp>

namespace osc_controller {

/*!
 * Class containing the Effort Task Algorithms
 */
class OSC_Controller {
public:
	/*!
	 * Constructor.
	 */
	OSC_Controller();

	/*!
	 * Destructor.
	 */
	virtual ~OSC_Controller();

    /*!
	 * Functions to change the gains of the controller
	 */
	void changeCartesianPositionGains(double Pos_X_stiffness, double Pos_Y_stiffness, double Pos_Z_stiffness,
																		double Pos_X_damping, double Pos_Y_damping, double Pos_Z_damping);

	void changeCartesianOrientationGains(double Ori_X_stiffness, double Ori_Y_stiffness, double Ori_Z_stiffness,
																			double Ori_X_damping, double Ori_Y_damping, double Ori_Z_damping);

    void changeJointTaskGains(double Mob_base_P_Gain, double Mob_base_D_Gain, double Manipulator_P_Gain, double Manipulator_D_Gain);

    /*!
	 * Function to change the higher threshold for singularities
	 */
    void changeHighThSing(double new_high_thr);

    /*!
	 * Function to change the lower threshold for singularities
	 */
    void changeLowThSing(double new_low_thr);

    /***********************************************************************************/
    /***********************************************************************************/
    // Joint Tasks

    /*!
     * Effort Task
     * Hold/ Achieve a Joint Configuration
    */
    void AchieveJointConf(  Eigen::VectorXd q_desired,
                            Eigen::MatrixXd M,
                            Eigen::VectorXd C_t,
                            Eigen::VectorXd g_t,
                            dart::dynamics::SkeletonPtr mRobot,
                            dart::dynamics::BodyNode* mEndEffector,
                            Eigen::VectorXd *tau_total,
                            Eigen::MatrixXd *Null_space_iter);

    /***********************************************************************************/
    /***********************************************************************************/
    // Joint limit avoidance tasks

    /*!
     * Effort Task
     * Avoid Joint Limits
    */
    void AvoidJointLimitsPotentials(Eigen::MatrixXd M,
                                    Eigen::VectorXd C_t,
                                    Eigen::VectorXd g_t,
                                    dart::dynamics::SkeletonPtr mRobot,
                                    dart::dynamics::BodyNode* mEndEffector,
                                    Eigen::VectorXd *tau_total,
                                    Eigen::MatrixXd *Null_space_iter);

    void AvoidJointLimitsIntermValue(Eigen::MatrixXd M,
                                    Eigen::VectorXd C_t,
                                    Eigen::VectorXd g_t,
                                    dart::dynamics::SkeletonPtr mRobot,
                                    dart::dynamics::BodyNode* mEndEffector,
                                    Eigen::VectorXd *tau_total,
                                    Eigen::MatrixXd *Null_space_iter);

    void updateSJSConstraints(dart::dynamics::SkeletonPtr mRobot, double sampling_time);

    void checkSJSConstraints(Eigen::VectorXd Joint_Acceleration,
                            dart::dynamics::BodyNode* mEndEffector,
                            bool* flag_sjs);

    void updateSJSConstraintTask(Eigen::VectorXd Current_joint_accel,
                                bool* flag_sjs,
                                Eigen::MatrixXd *Jacobian_constr,
                                Eigen::VectorXd *Constr_Task_Acceleration);

    void SaturationJointSpace(Eigen::MatrixXd Jacobian,
                              Eigen::VectorXd desired_accel,
                              Eigen::MatrixXd M,
                              Eigen::VectorXd C_t,
                              Eigen::VectorXd g_t,
                              dart::dynamics::SkeletonPtr mRobot,
                              dart::dynamics::BodyNode* mEndEffector,
                              Eigen::VectorXd *tau_total,
                              Eigen::MatrixXd *Null_space_iter);

    /***********************************************************************************/
    /***********************************************************************************/
    // Position tasks

    /*******************************************************/
    // Mobile manipulator tasks

    /*!
     * Effort Task
     * Cartesian Impedance for Position
    */
    void CartesianImpedance(  Eigen::Vector3d mTargetPos,
                              Eigen::Vector3d mTargetVel,
                              Eigen::Vector3d mTargetAccel,
                              double *svd_position,
                              int cycle,
                              Eigen::VectorXd tau_ext,
                              Eigen::MatrixXd M,
                              Eigen::VectorXd C_t,
                              Eigen::VectorXd g_t,
                              dart::dynamics::SkeletonPtr mRobot,
                              dart::dynamics::BodyNode* mEndEffector,
                              Eigen::VectorXd *tau_total,
                              Eigen::VectorXd *tau_ns,
                              Eigen::MatrixXd *Null_space_iter);

    /*!
     * Effort Task
     * Hold/ Achieve a Cartesian Position
    */
    void AchieveCartesian(  Eigen::Vector3d mTargetPos,
                            Eigen::Vector3d mTargetVel,
                            Eigen::Vector3d mTargetAccel,
                            double *svd_position,
                            int cycle,
                            Eigen::MatrixXd M,
                            Eigen::VectorXd C_t,
                            Eigen::VectorXd g_t,
                            dart::dynamics::SkeletonPtr mRobot,
                            dart::dynamics::BodyNode* mEndEffector,
                            Eigen::VectorXd *tau_total,
                            Eigen::VectorXd *tau_ns,
                            Eigen::MatrixXd *Null_space_iter);

    /*!
     * Effort Task
     * Go to a desired position with constant vel
    */
    void AchieveCartesianConstVel(  Eigen::Vector3d mTarget,
                                    double *svd_position,
                                    int cycle,
                                    Eigen::MatrixXd M,
                                    Eigen::VectorXd C_t,
                                    Eigen::VectorXd g_t,
                                    dart::dynamics::SkeletonPtr mRobot,
                                    dart::dynamics::BodyNode* mEndEffector,
                                    Eigen::VectorXd *tau_total,
                                    Eigen::VectorXd *tau_ns,
                                    Eigen::MatrixXd *Null_space_iter);

    /*******************************************************/
    // Mobile robot tasks

    /*!
     * Effort Task
     * Tasks using only the DOF of the mobile robot
    */

    void AchieveCartesianMobilRob( Eigen::Vector3d mTargetPos,
                                    Eigen::Vector3d mTargetVel,
                                    Eigen::Vector3d mTargetAccel,
                                    double *svd_position,
                                    Eigen::MatrixXd M,
                                    Eigen::VectorXd C_t,
                                    Eigen::VectorXd g_t,
                                    dart::dynamics::SkeletonPtr mRobot,
                                    dart::dynamics::BodyNode* mEndEffector,
                                    Eigen::VectorXd *tau_total,
                                    Eigen::MatrixXd *Null_space_iter);

    void AchieveCartesianMobilRobConstVel(  Eigen::Vector3d mTargetPos,
                                            double *svd_position,
                                            Eigen::MatrixXd M,
                                            Eigen::VectorXd C_t,
                                            Eigen::VectorXd g_t,
                                            dart::dynamics::SkeletonPtr mRobot,
                                            dart::dynamics::BodyNode* mEndEffector,
                                            Eigen::VectorXd *tau_total,
                                            Eigen::MatrixXd *Null_space_iter);

    /*******************************************************/
    // Manipulator tasks

    /*!
     * Effort Task
     * Tasks using only the DOF of the manipulator
    */

    void AchieveCartesianManipulator(Eigen::Vector3d mTargetPos,
                                    Eigen::Vector3d mTargetVel,
                                    Eigen::Vector3d mTargetAccel,
                                    double *svd_position,
                                    int cycle,
                                    Eigen::MatrixXd M,
                                    Eigen::VectorXd C_t,
                                    Eigen::VectorXd g_t,
                                    dart::dynamics::SkeletonPtr mRobot,
                                    dart::dynamics::BodyNode* mEndEffector,
                                    Eigen::VectorXd *tau_total,
                                    Eigen::VectorXd *tau_ns,
                                    Eigen::MatrixXd *Null_space_iter);

    void AchieveCartManipulatorConstVel(Eigen::Vector3d mTarget,
                                        double *svd_position,
                                        int cycle,
                                        Eigen::MatrixXd M,
                                        Eigen::VectorXd C_t,
                                        Eigen::VectorXd g_t,
                                        dart::dynamics::SkeletonPtr mRobot,
                                        dart::dynamics::BodyNode* mEndEffector,
                                        Eigen::VectorXd *tau_total,
                                        Eigen::VectorXd *tau_ns,
                                        Eigen::MatrixXd *Null_space_iter);

    void AchievePosZ( Eigen::Vector3d mTargetPos,
                      Eigen::Vector3d mTargetVel,
                      Eigen::Vector3d mTargetAccel,
                      double *svd_position,
                      int cycle,
                      Eigen::MatrixXd M,
                      Eigen::VectorXd C_t,
                      Eigen::VectorXd g_t,
                      dart::dynamics::SkeletonPtr mRobot,
                      dart::dynamics::BodyNode* mEndEffector,
                      Eigen::VectorXd *tau_total,
                      Eigen::VectorXd *tau_ns,
                      Eigen::MatrixXd *Null_space_iter);

    void AchievePosZConstVel( Eigen::Vector3d mTarget,
                              double *svd_position,
                              int cycle,
                              Eigen::MatrixXd M,
                              Eigen::VectorXd C_t,
                              Eigen::VectorXd g_t,
                              dart::dynamics::SkeletonPtr mRobot,
                              dart::dynamics::BodyNode* mEndEffector,
                              Eigen::VectorXd *tau_total,
                              Eigen::VectorXd *tau_ns,
                              Eigen::MatrixXd *Null_space_iter);

    /***********************************************************************************/
    /***********************************************************************************/
    // Orientation tasks

    /*******************************************************/
    // Mobile manipulator tasks

    /*!
     * Effort Task
     * Orientation Impedance
    */
    void OrientationImpedance(Eigen::Matrix3d rot_mat_desired,
                              Eigen::Vector3d mTargetVel,
                              Eigen::Vector3d mTargetAccel,
                              double *svd_orientation,
                              int cycle,
                              Eigen::VectorXd tau_ext,
                              Eigen::MatrixXd M,
                              Eigen::VectorXd C_t,
                              Eigen::VectorXd g_t,
                              dart::dynamics::SkeletonPtr mRobot,
                              dart::dynamics::BodyNode* mEndEffector,
                              Eigen::VectorXd *tau_total,
                              Eigen::VectorXd *tau_ns,
                              Eigen::MatrixXd *Null_space_iter);

    /*!
     * Effort Task
     * Hold/ Achieve an Orientation
    */
    void AchieveOrientation(Eigen::Matrix3d rot_mat_desired,
                            Eigen::Vector3d mTargetVel,
                            Eigen::Vector3d mTargetAccel,
                            double *svd_orientation,
                            int cycle,
                            Eigen::MatrixXd M,
                            Eigen::VectorXd C_t,
                            Eigen::VectorXd g_t,
                            dart::dynamics::SkeletonPtr mRobot,
                            dart::dynamics::BodyNode* mEndEffector,
                            Eigen::VectorXd *tau_total,
                            Eigen::VectorXd *tau_ns,
                            Eigen::MatrixXd *Null_space_iter);

    /*!
     * Effort Task
     * Go to a desired orientation with constant vel
    */
    void AchieveOrientationConstVel(Eigen::Matrix3d rot_mat_desired,
                                    double *svd_orientation,
                                    int cycle,
                                    Eigen::MatrixXd M,
                                    Eigen::VectorXd C_t,
                                    Eigen::VectorXd g_t,
                                    dart::dynamics::SkeletonPtr mRobot,
                                    dart::dynamics::BodyNode* mEndEffector,
                                    Eigen::VectorXd *tau_total,
                                    Eigen::VectorXd *tau_ns,
                                    Eigen::MatrixXd *Null_space_iter);

    /*******************************************************/
    // Manipulator tasks

    void AchieveOriManipulator( Eigen::Matrix3d rot_mat_desired,
                                Eigen::Vector3d mTargetVel,
                                Eigen::Vector3d mTargetAccel,
                                double *svd_orientation,
                                int cycle,
                                Eigen::MatrixXd M,
                                Eigen::VectorXd C_t,
                                Eigen::VectorXd g_t,
                                dart::dynamics::SkeletonPtr mRobot,
                                dart::dynamics::BodyNode* mEndEffector,
                                Eigen::VectorXd *tau_total,
                                Eigen::VectorXd *tau_ns,
                                Eigen::MatrixXd *Null_space_iter);

    void AchieveOriManipulatorConstVel( Eigen::Matrix3d rot_mat_desired,
                                        double *svd_orientation,
                                        int cycle,
                                        Eigen::MatrixXd M,
                                        Eigen::VectorXd C_t,
                                        Eigen::VectorXd g_t,
                                        dart::dynamics::SkeletonPtr mRobot,
                                        dart::dynamics::BodyNode* mEndEffector,
                                        Eigen::VectorXd *tau_total,
                                        Eigen::VectorXd *tau_ns,
                                        Eigen::MatrixXd *Null_space_iter);

    /*******************************************************/
    // Error Functions

    Eigen::Vector3d ErrorAngleAxis1(Eigen::Matrix3d rot_mat_desired,
                                    dart::dynamics::SkeletonPtr mRobot,
                                    dart::dynamics::BodyNode* mEndEffector);

    Eigen::Vector3d ErrorAngleAxis2(Eigen::Matrix3d rot_mat_desired,
                                    dart::dynamics::SkeletonPtr mRobot,
                                    dart::dynamics::BodyNode* mEndEffector);

    Eigen::Vector3d ErrorQuaternion1(Eigen::Matrix3d rot_mat_desired,
                                    dart::dynamics::SkeletonPtr mRobot,
                                    dart::dynamics::BodyNode* mEndEffector);

    Eigen::Vector3d ErrorQuaternion2(Eigen::Matrix3d rot_mat_desired,
                                    dart::dynamics::SkeletonPtr mRobot,
                                    dart::dynamics::BodyNode* mEndEffector);

    Eigen::Vector3d ErrorQuaternion3(Eigen::Matrix3d rot_mat_desired,
                                    dart::dynamics::SkeletonPtr mRobot,
                                    dart::dynamics::BodyNode* mEndEffector);

    /***********************************************************************************/
    /***********************************************************************************/
    // Whole body controller functions

    Eigen::VectorXd admittance_controller(Eigen::VectorXd torque_out);

    /***********************************************************************************/
    /***********************************************************************************/
    // Miscellaneous functions

    Eigen::MatrixXd calcInertiaMatrix(Eigen::MatrixXd Alpha_inv, double* min_svd);

    void calcInertiaMatrixHandling( Eigen::MatrixXd Alpha_inv,
                                    double* min_svd,
                                    double* act_param,
                                    Eigen::MatrixXd *Alpha_ns,
                                    Eigen::MatrixXd *Alpha_s,
                                    Eigen::MatrixXd *Alpha_s_dummy);

    Eigen::MatrixXd calcDampingMatrix(Eigen::MatrixXd Alpha,
                                      Eigen::MatrixXd Stiffness,
                                      Eigen::MatrixXd DampingCoeff);

    /***********************************************************************************/
    /***********************************************************************************/
    // Public variables

    // Variables for hierarchy control (WBC and OSC)
    bool compensate_topdown;
    bool compensate_jtspace;
    bool augmented_projections;

    // Variables for tasks
    double max_lineal_vel_;
    double max_angular_vel_;

    // Variable to select the method for orientacion error calculation
    int ori_error_mode;

    // Variables for singularity handling method
    int singularity_handling_method;

    double singularity_thres_high_pos_;
    double singularity_thres_low_pos_;

    double singularity_thres_high_ori_;
    double singularity_thres_low_ori_;

    // Variables for joint limit avoidance methods
    int joint_limit_handling_method;

    Eigen::VectorXd Lower_limits;
    Eigen::VectorXd Upper_limits;

    Eigen::VectorXd joint_margin_;

    double eta_firas_;

    // Variables for intermediate value
    int   interm_alg_update_null;
    Eigen::VectorXd joint_limit_buffer;
    double gain_limit_avoidance;
    double scale_null_space;

    // Variables for SJS
    Eigen::VectorXd Max_constraint_accel;
    Eigen::VectorXd Min_constraint_accel;

    Eigen::VectorXd Max_joint_accel;
    Eigen::VectorXd Min_joint_accel;

    Eigen::VectorXd Max_joint_vel;
    Eigen::VectorXd Min_joint_vel;

    Eigen::VectorXd Max_joint_pos;
    Eigen::VectorXd Min_joint_pos;

private:

    // Gain matrices for tasks in operational and joint spaces
    Eigen::MatrixXd kp_cartesian_;
    Eigen::MatrixXd kd_cartesian_;
    Eigen::MatrixXd kp_joints_;
    Eigen::MatrixXd kd_joints_;

    // Variables for singularity handling method
    double singularity_thres_high_;
    double singularity_thres_low_;

    // Gains for WBC
    double admittance_linear_damping;
    double admittance_angular_damping;
    Eigen::MatrixXd admittance_desired_inertia;

};

} /* namespace */


#include "mesh.h"
#include "arap.h"
#include "redArap.h"
#include "elastic.h"
#include "solver.h"
#include "redSolver.h"
#include <Eigen/Core>
#include <iostream>
#include <igl/readOFF.h>
#include <igl/readMESH.h>
#include <igl/readOBJ.h>
#include <igl/writeOBJ.h>
#include <igl/opengl/glfw/Viewer.h>

using namespace LBFGSpp;
using json = nlohmann::json;
using namespace Eigen;
using namespace std;
json j_input;

std::vector<int> getMaxVerts_Axis_Tolerance(MatrixXd& mV, int dim, double tolerance=1e-5){
    auto maxX = mV.col(dim).maxCoeff();
    std::vector<int> maxV;
    for(unsigned int ii=0; ii<mV.rows(); ++ii) {

        if(fabs(mV(ii,dim) - maxX) < tolerance) {
            maxV.push_back(ii);
        }
    }
    return maxV;
}

std::vector<int> getMinVerts_Axis_Tolerance(MatrixXd& mV, int dim, double tolerance=1e-5){
    auto maxX = mV.col(dim).minCoeff();
    std::vector<int> maxV;
    for(unsigned int ii=0; ii<mV.rows(); ++ii) {

        if(fabs(mV(ii,dim) - maxX) < tolerance) {
            maxV.push_back(ii);
        }
    }
    return maxV;
}
void getMaxTets_Axis_Tolerance(std::vector<int>& ibones, MatrixXd& mV, MatrixXi& mT, double dim, double tolerance = 1-5){
	auto maxX = mV.col(dim).maxCoeff();
	for(int i=0; i< mT.rows(); i++){
		Vector3d centre = (mV.row(mT.row(i)[0])+ mV.row(mT.row(i)[1]) + mV.row(mT.row(i)[2])+ mV.row(mT.row(i)[3]))/4.0;
		if (fabs(centre[dim] - maxX)< tolerance){
			ibones.push_back(i);
		}
	}
}

void getMinTets_Axis_Tolerance(std::vector<int>& ibones, MatrixXd& mV, MatrixXi& mT, double dim, double tolerance = 1-5){
	auto maxX = mV.col(dim).minCoeff();
	for(int i=0; i< mT.rows(); i++){
		Vector3d centre = (mV.row(mT.row(i)[0])+ mV.row(mT.row(i)[1]) + mV.row(mT.row(i)[2])+ mV.row(mT.row(i)[3]))/4.0;
		if (fabs(centre[dim] - maxX)< tolerance){
			ibones.push_back(i);
		}
	}
}

int main()
{
	std::cout<<"-----Configs-------"<<std::endl;
    json j_config_parameters;
    std::ifstream i("../input/input.json");
    i >> j_input;

    if(j_input["reduced"]){
        std::cout<<"Use Reduced code"<<endl;
        exit(0);
    }

    MatrixXd V;
    MatrixXi T;
    MatrixXi F;
    igl::readMESH(j_input["mesh_file"], V, T, F);
    cout<<"V size: "<<V.rows()<<endl;
    cout<<"T size: "<<T.rows()<<endl;
    cout<<"F size: "<<F.rows()<<endl;

    std::vector<int> fix = getMaxVerts_Axis_Tolerance(V, 1);
    std::sort (fix.begin(), fix.end());
    std::vector<int> mov = {};//getMinVerts_Axis_Tolerance(V, 1);
    std::sort (mov.begin(), mov.end());
    std::vector<VectorXi> bones = {};
    // getMaxTets_Axis_Tolerance(bones, V, T, 1, 3);
    // getMinTets_Axis_Tolerance(bones, V, T, 1, 3);

    std::cout<<"-----Mesh-------"<<std::endl;
    VectorXi muscle1;
    MatrixXd Uvec;
    std::vector<VectorXi> muscles = {muscle1};
    Mesh* mesh = new Mesh(T, V, fix, mov,bones, muscles, Uvec, j_input);
    std::cout<<"-----Neo-------"<<std::endl;
    Elastic* neo = new Elastic(*mesh);

    std::cout<<"-----ARAP-----"<<std::endl;
    Arap* arap = new Arap(*mesh);

    std::cout<<"-----Solver-------"<<std::endl;
    int DIM = mesh->red_s().size();
    Rosenbrock f(DIM, mesh, arap, neo, j_input, true);

    LBFGSParam<double> param;
    param.epsilon = 1e-1;
    // param.max_iterations = 1000;
    // param.past = 2;
    // param.m = 5;
    param.linesearch = LBFGSpp::LBFGS_LINESEARCH_BACKTRACKING_ARMIJO;
    LBFGSSolver<double> solver(param);

    // for(int i=0; i<5; i++){
    //     MatrixXd newV = mesh->continuousV();
    //     string datafile = j_input["data"];
    //     igl::writeOBJ(datafile+"test"+to_string(i)+".obj",newV,F);

    //     double fx =0;
    //     VectorXd ns = mesh->N().transpose()*mesh->red_s();
    //     int niter = solver.minimize(f, ns, fx);
    //     cout<<"End BFGS"<<", "<<niter<<endl;
    //     VectorXd reds = mesh->N()*ns + mesh->AN()*mesh->AN().transpose()*mesh->red_s();
    //     for(int i=0; i<reds.size(); i++){
    //         mesh->red_s()[i] = reds[i];
    //     }

    //     neo->changeFiberMag(1.5);
    // }
    // exit(0);

	igl::opengl::glfw::Viewer viewer;
    std::cout<<"-----Display-------"<<std::endl;
    MatrixXd Colors = MatrixXd::Random(100,3); // 3x3 Matrix filled with random numbers between (-1,1)
    Colors = (Colors + MatrixXd::Constant(100,3,1.))*(1-1e-6)/2.; // add 1 to the matrix to have values between 0 and 2; multiply with range/2
    Colors = (Colors + MatrixXd::Constant(100,3,1e-6)); //set LO as the lower bound (offset)
    viewer.callback_pre_draw = [&](igl::opengl::glfw::Viewer & viewer)
    {
        if(viewer.core.is_animating)
        {

    	}
        return false;
    };

    viewer.callback_key_down = [&](igl::opengl::glfw::Viewer & viewer, unsigned char key, int modifiers)
    {   std::cout<<"Key down, "<<key<<std::endl;
        viewer.data().clear();

        // //----------------
        if(key=='A'){
            cout<<"here"<<endl;
            neo->changeFiberMag(2);
        }

        if(key==' '){
      		VectorXd& dx = mesh->dx();
		    for(int i=0; i<mov.size(); i++){
		        dx[3*mov[i]+1] -= 3;
		    }

            // for(int i=0; i<mesh->red_s().size()/6; i++){
            //     mesh->red_s()[6*i+1] += 0.1;
            //     mesh->red_s()[6*i+0] += 0.1;
            // }
            // arap->minimize(*mesh);
            // double fx =0;
            // VectorXd ns = mesh->N().transpose()*mesh->red_s();
            // int niter = solver.minimize(f, ns, fx);
            // VectorXd reds = mesh->N()*ns + mesh->AN()*mesh->AN().transpose()*mesh->red_s();

            double fx =0;
            VectorXd reds = mesh->red_s();
            int niter = solver.minimize(f, reds, fx);
            for(int i=0; i<reds.size(); i++){
                mesh->red_s()[i] = reds[i];
            }
            std::cout<<"niter "<<niter<<std::endl;
        }

        //----------------
        //Draw continuous mesh
        MatrixXd newV = mesh->continuousV();
        viewer.data().set_mesh(newV, F);

        //Draw disc mesh
        std::cout<<std::endl;
        MatrixXd& discV = mesh->discontinuousV();
        MatrixXi& discT = mesh->discontinuousT();
        for(int i=0; i<discT.rows(); i++){
            Vector4i e = discT.row(i);
            // std::cout<<discT.row(i)<<std::endl<<std::endl;
            // std::cout<<discV(Eigen::placeholders::all, discT.row(i))<<std::endl;
            Matrix<double, 1,3> p0 = discV.row(e[0]);
            Matrix<double, 1,3> p1 = discV.row(e[1]);
            Matrix<double, 1,3> p2 = discV.row(e[2]);
            Matrix<double, 1,3> p3 = discV.row(e[3]);

            viewer.data().add_edges(p0,p1,Eigen::RowVector3d(1,0,1));
            viewer.data().add_edges(p0,p2,Eigen::RowVector3d(1,0,1));
            viewer.data().add_edges(p0,p3,Eigen::RowVector3d(1,0,1));
            viewer.data().add_edges(p1,p2,Eigen::RowVector3d(1,0,1));
            viewer.data().add_edges(p1,p3,Eigen::RowVector3d(1,0,1));
            viewer.data().add_edges(p2,p3,Eigen::RowVector3d(1,0,1));
        }
        //Draw fixed and moving points
        for(int i=0; i<fix.size(); i++){
            viewer.data().add_points(mesh->V().row(fix[i]),Eigen::RowVector3d(1,0,0));
        }
        for(int i=0; i<mov.size(); i++){
            viewer.data().add_points(newV.row(mov[i]),Eigen::RowVector3d(0,1,0));
        }
        // for(int c=0; c<mesh->red_w().size()/3; c++){
        //     std::vector<int> cluster_elem = mesh->r_cluster_elem_map()[c];
        //     for(int e=0; e<cluster_elem.size(); e++){
        //         viewer.data().add_points(newV.row(mesh->T().row(cluster_elem[e])[0]), Colors.row(c));
        //         viewer.data().add_points(newV.row(mesh->T().row(cluster_elem[e])[1]), Colors.row(c));
        //         viewer.data().add_points(newV.row(mesh->T().row(cluster_elem[e])[2]), Colors.row(c));
        //         viewer.data().add_points(newV.row(mesh->T().row(cluster_elem[e])[3]), Colors.row(c));
        //     }
        // }

        return false;
    };

	viewer.data().set_mesh(V,F);
    viewer.data().show_lines = true;
    viewer.data().invert_normals = true;
    viewer.core.is_animating = false;
    viewer.data().face_based = true;
    // viewer.data.set_colors(C);
    viewer.launch();

    return EXIT_SUCCESS;

    return 0;
}


// Copyright (c) Prevail Verifier contributors.
// SPDX-License-Identifier: MIT
#pragma once

#include <climits>
#include <iostream>
#include <sstream>
#include <string>

#include <boost/functional/hash.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <utility>

#include "debug.hpp"
using boost::multiprecision::cpp_int;

namespace crab {

class z_number final {
  private:
    cpp_int _n{nullptr};

  public:
    z_number() = default;
    explicit z_number(cpp_int n) : _n(std::move(n)) {}
    explicit z_number(const std::string& s) { _n = cpp_int(s); }

    z_number(signed long long int n) { _n = n; }
    z_number(unsigned long long int n) { _n = n; }
    z_number(int n) { _n = n; }
    z_number(unsigned int n) { _n = n; }
    z_number(long n) { _n = n; }

    // overloaded typecast operators
    explicit operator long() const {
        if (!fits_slong()) {
            CRAB_ERROR("z_number ", _n.str(), " does not fit into a signed long integer");
        } else {
            return (long)_n;
        }
    }

    explicit operator int() const {
        if (!fits_sint()) {
            CRAB_ERROR("z_number ", _n.str(), " does not fit into a signed integer");
        } else {
            return (int)_n;
        }
    }

    explicit operator cpp_int() const { return _n; }

    [[nodiscard]] std::size_t hash() const {
        boost::hash<std::string> hasher;
        return hasher(_n.str());
    }

    [[nodiscard]] bool fits_sint() const {
        return ((_n >= INT_MIN) && (_n <= INT_MAX));
    }

    [[nodiscard]] bool fits_slong() const {
        return ((_n >= LONG_MIN) && (_n <= LONG_MAX));
    }

    z_number operator+(const z_number& x) const {
        return z_number(_n + x._n);
    }

    z_number operator+(int x) const { return operator+(z_number(x)); }

    z_number operator*(const z_number& x) const { return z_number(_n * x._n); }

    z_number operator*(int x) const { return operator*(z_number(x)); }

    z_number operator-(const z_number& x) const { return z_number(_n - x._n); }

    z_number operator-(int x) const { return operator-(z_number(x)); }

    z_number operator-() const { return z_number(-_n); }

    z_number operator/(const z_number& x) const {
        if (x._n.is_zero()) {
            CRAB_ERROR("z_number: division by zero [1]");
        } else {
            return z_number(_n / x._n);
        }
    }

    z_number operator/(int x) const { return operator/(z_number(x)); }

    z_number operator%(const z_number& x) const {
        if (x._n.is_zero()) {
            CRAB_ERROR("z_number: division by zero [2]");
        } else {
            return z_number(_n % x._n);
        }
    }

    z_number operator%(int x) const { return operator%(z_number(x)); }

    z_number& operator+=(const z_number& x) {
        _n += x._n;
        return *this;
    }

    z_number& operator+=(int x) { return operator+=(z_number(x)); }

    z_number& operator*=(const z_number& x) {
        _n *= x._n;
        return *this;
    }

    z_number& operator*=(int x) { return operator*=(z_number(x)); }

    z_number& operator-=(const z_number& x) {
        _n -= x._n;
        return *this;
    }

    z_number& operator-=(int x) { return operator-=(z_number(x)); }

    z_number& operator/=(const z_number& x) {
        if (x._n.is_zero()) {
            CRAB_ERROR("z_number: division by zero [3]");
        } else {
            _n /= x._n;
            return *this;
        }
    }

    z_number& operator/=(int x) { return operator/=(z_number(x)); }

    z_number& operator%=(const z_number& x) {
        if (x._n.is_zero()) {
            CRAB_ERROR("z_number: division by zero [4]");
        } else {
            _n %= x._n;
            return *this;
        }
    }

    z_number& operator%=(int x) { return operator%=(z_number(x)); }

    z_number& operator--() & {
        _n--;
        return *this;
    }

    z_number& operator++() & {
        _n++;
        return *this;
    }

    z_number operator++(int) & {
        z_number r(*this);
        ++(*this);
        return r;
    }

    z_number operator--(int) & {
        z_number r(*this);
        --(*this);
        return r;
    }

    bool operator==(const z_number& x) const {
        return (_n == x._n);
    }

    bool operator==(int x) const { return operator==(z_number(x)); }

    bool operator!=(const z_number& x) const { return (_n != x._n); }

    bool operator!=(int x) const { return operator!=(z_number(x)); }

    bool operator<(const z_number& x) const { return (_n < x._n); }

    bool operator<(int x) const { return operator<(z_number(x)); }

    bool operator<=(const z_number& x) const { return (_n <= x._n); }

    bool operator<=(int x) const { return operator<=(z_number(x)); }

    bool operator>(const z_number& x) const { return (_n > x._n); }

    bool operator>(int x) const { return operator>(z_number(x)); }

    bool operator>=(const z_number& x) const { return (_n >= x._n); }

    bool operator>=(int x) const { return operator>=(z_number(x)); }

    z_number operator&(const z_number& x) const { return z_number(_n & x._n); }

    z_number operator&(int x) const { return z_number(_n & x); }

    z_number operator|(const z_number& x) const { return z_number(_n | x._n); }

    z_number operator|(int x) const { return z_number(_n | x); }

    z_number operator^(const z_number& x) const { return z_number(_n ^ x._n); }

    z_number operator^(int x) const { return z_number(_n ^ x); }

    z_number operator<<(z_number x) const {
        if (!x.fits_sint()) {
            CRAB_ERROR("z_number ", x._n.str(), " does not fit into an int");
        }
        return z_number(_n << (int)x);
    }

    z_number operator<<(int x) const { return z_number(_n << x); }

    z_number operator>>(z_number x) const {
        if (!x.fits_sint()) {
            CRAB_ERROR("z_number ", x._n.str(), " does not fit into an int");
        }
        return z_number(_n >> (int)x);
    }

    z_number operator>>(int x) const { return z_number(_n >> x); }

    [[nodiscard]] z_number fill_ones() const {
        if (_n.is_zero()) {
            return z_number((signed long long)0);
        }

        z_number result;
        for (result = 1; result < *this; result = result * 2 + 1)
            ;
        return result;
    }

    friend std::ostream& operator<<(std::ostream& o, const z_number& z) {
        return o << z._n.str();
    }

}; // class z_number

using number_t = z_number;

inline std::size_t hash_value(const z_number& z) { return z.hash(); }

} // namespace crab


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Exact_predicates_exact_constructions_kernel.h>

#include <cassert>
#include <vector>
#include <fstream>
#include <boost/tuple/tuple.hpp>

#include <CGAL/Polyhedron_incremental_builder_3.h>
#include <CGAL/Polyhedron_3.h>

#include <CGAL/Polygon_mesh_processing/triangulate_hole.h>

typedef CGAL::Exact_predicates_inexact_constructions_kernel  Epic;
typedef CGAL::Exact_predicates_exact_constructions_kernel  Epec;

// to debug visually, construct polyhedron from patch
template<class HDS, class K>
class Polyhedron_builder : public CGAL::Modifier_base<HDS> {
  typedef typename K::Point_3 Point_3;
public:
  Polyhedron_builder(std::vector<boost::tuple<int, int, int> >* triangles,
    std::vector<Point_3>* polyline)
    : triangles(triangles), polyline(polyline)
  { }

  void operator()(HDS& hds) {
    CGAL::Polyhedron_incremental_builder_3<HDS> B(hds, true);
    B.begin_surface(polyline->size() -1, triangles->size());

    for(typename std::vector<Point_3>::iterator it = polyline->begin();
      it != --polyline->end(); ++it) {
        B.add_vertex(*it);
    }

    for(typename std::vector<boost::tuple<int, int, int> >::iterator it = triangles->begin();
      it != triangles->end(); ++it) {
        B.begin_facet();
        B.add_vertex_to_facet(it->get<0>());
        B.add_vertex_to_facet(it->get<1>());
        B.add_vertex_to_facet(it->get<2>());
        B.end_facet();
    }

    B.end_surface();
  }

private:
  std::vector<boost::tuple<int, int, int> >* triangles;
  std::vector<Point_3>* polyline;
};

template <typename K>
struct Main {

typedef typename  K::Point_3                  Point_3;
  typedef CGAL::Polyhedron_3<K>       Polyhedron;

// it reads .polylines.txt (there should be one polyline with last point repeated)
void read_polyline_one_line(const char* file_name, std::vector<Point_3>& points) {
  std::ifstream stream(file_name);
  if(!stream) { assert(false); }

  int count;
  if(!(stream >> count)) { assert(false); }
  while(count-- > 0) {
    Point_3 p;
    if(!(stream >> p)) { assert(false); }
    points.push_back(p);
  }
}

// last point should be repeated
void read_polyline_with_extra_points(
  const char* file_name,
  std::vector<Point_3>& points,
  std::vector<Point_3>& extras)
{
  std::ifstream stream(file_name);
  if(!stream) { assert(false); }

  for(int i =0; i < 2; ++i) {
    int count;
    if(!(stream >> count)) { assert(false); }
    while(count-- > 0) {
      Point_3 p;
      if(!(stream >> p)) { assert(false); }
      i == 0 ? points.push_back(p) : extras.push_back(p);
    }
  }
}

void check_triangles(std::vector<Point_3>& points, std::vector<boost::tuple<int, int, int> >& tris) {
  if(points.size() - 3 != tris.size()) {
    std::cerr << "  Error: there should be n-2 triangles in generated patch." << std::endl;
    assert(false);
  }

  const int max_index = static_cast<int>(points.size())-1;
  for(std::vector<boost::tuple<int, int, int> >::iterator it = tris.begin(); it != tris.end(); ++it) {
    if(it->get<0>() == it->get<1>() ||
      it->get<0>() == it->get<2>() ||
      it->get<1>() == it->get<2>() )
    {
      std::cerr << "Error: indices of triangles should be all different." << std::endl;
      assert(false);
    }

    if(it->get<0>() >= max_index ||
      it->get<1>() >= max_index ||
      it->get<2>() >= max_index )
    {
      std::cerr << "  Error: max possible index check failed." << std::endl;
      assert(false);
    }
  }
}

void check_constructed_polyhedron(const char* file_name,
  std::vector<boost::tuple<int, int, int> >* triangles,
  std::vector<Point_3>* polyline,
  const bool save_poly)
{
  Polyhedron poly;
  Polyhedron_builder<typename Polyhedron::HalfedgeDS,K> patch_builder(triangles, polyline);
  poly.delegate(patch_builder);

  if(!poly.is_valid()) {
    std::cerr << "  Error: constructed patch does not constitute a valid polyhedron." << std::endl;
    assert(false);
  }

  if (!save_poly)
    return;

  std::string out_file_name;
  out_file_name.append(file_name).append(".off");
  std::ofstream out(out_file_name.c_str());
  out << poly; out.close();
}

void test_1(const char* file_name, bool use_DT, bool save_output) {
  std::cerr << "test_1 + useDT: " << use_DT << std::endl;
  std::cerr << "  File: "<< file_name  << std::endl;
  std::vector<Point_3> points; // this will contain n and +1 repeated point
  read_polyline_one_line(file_name, points);

  std::vector<boost::tuple<int, int, int> > tris;
  CGAL::Polygon_mesh_processing::triangulate_hole_polyline(
    points, std::back_inserter(tris),
    CGAL::Polygon_mesh_processing::parameters::use_delaunay_triangulation(use_DT)
        .use_2d_constrained_delaunay_triangulation(true));

  check_triangles(points, tris);
  check_constructed_polyhedron(file_name, &tris, &points, save_output);

  std::cerr << "  Done!" << std::endl;
}

void test_2(const char* file_name, bool use_DT, bool save_output) {
  std::cerr << "test_2 + useDT: " << use_DT << std::endl;
  std::cerr << "  File: "<< file_name  << std::endl;
  std::vector<Point_3> points; // this will contain n and +1 repeated point
  std::vector<Point_3> extras;
  read_polyline_with_extra_points(file_name, points, extras);

  std::vector<boost::tuple<int, int, int> > tris;
  CGAL::Polygon_mesh_processing::triangulate_hole_polyline(
    points, extras, std::back_inserter(tris),
    CGAL::Polygon_mesh_processing::parameters::use_delaunay_triangulation(use_DT)
        .use_2d_constrained_delaunay_triangulation(true));

  check_triangles(points, tris);
  check_constructed_polyhedron(file_name, &tris, &points, save_output);

  std::cerr << "  Done!" << std::endl;
}

void test_should_be_no_output(const char* file_name, bool use_DT) {
  std::cerr << "test_should_be_no_output + useDT: " <<use_DT<< std::endl;
  std::cerr << "  File: "<< file_name  << std::endl;
  std::vector<Point_3> points; // this will contain n and +1 repeated point
  read_polyline_one_line(file_name, points);

  std::vector<boost::tuple<int, int, int> > tris;
  CGAL::Polygon_mesh_processing::triangulate_hole_polyline(
    points, std::back_inserter(tris),
    CGAL::Polygon_mesh_processing::parameters::use_delaunay_triangulation(use_DT)
        .use_2d_constrained_delaunay_triangulation(true));

  if(!tris.empty()) {
    std::cerr << "  Error: patch should be empty" << std::endl;
    assert(false);
  }
  std::cerr << "  Done!" << std::endl;
}

 Main() {
  std::vector<std::string> input_files_1;
  input_files_1.push_back("data/triangle.polylines.txt");
  input_files_1.push_back("data/quad.polylines.txt");
  input_files_1.push_back("data/U.polylines.txt");
  input_files_1.push_back("data/planar.polylines.txt");

  for(std::vector<std::string>::iterator it = input_files_1.begin(); it != input_files_1.end(); ++it) {
    test_1(it->c_str(), true, false);
    test_1(it->c_str(), false, false);
  }

  std::vector<std::string> input_files_2;
  input_files_2.push_back("data/hole1.txt");
  input_files_2.push_back("data/hole2.txt");
  input_files_2.push_back("data/hole3.txt");
  input_files_2.push_back("data/hole4.txt");

  for(std::vector<std::string>::iterator it = input_files_2.begin(); it != input_files_2.end(); ++it) {
    if(it != input_files_2.begin())
    { test_2(it->c_str(), true, false); } // to skip hole1.txt (DT does not include all border edges)
    test_2(it->c_str(), false, false);
  }

  test_should_be_no_output("data/collinear.polylines.txt", true);
  test_should_be_no_output("data/collinear.polylines.txt", false);
  test_should_be_no_output("data/null_edge.polylines.txt", true);
  test_should_be_no_output("data/null_edge.polylines.txt", false);
  std::cerr << "All Done!" << std::endl;
 }

};

int main()
{
  Main<Epic> m;
  Main<Epec> m2;
  return 0;
}