#include "engine.hpp"

#include <algorithm>
#include <numeric>
#include <cmath>
#include <functional> // for std::cref (pass reference of VectorField to odeint)
#include <boost/numeric/odeint/stepper/runge_kutta_cash_karp54.hpp>
#include <boost/numeric/odeint/integrate/integrate_adaptive.hpp>
#include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>
#include <boost/numeric/odeint/stepper/generation.hpp> // make_controlled

#include "variable.hpp"
#include "sde.hpp" // SDE integrator

namespace odeint = boost::numeric::odeint;

typedef odeint::runge_kutta_cash_karp54<RealVec> real_vec_error_stepper_type;

constexpr bool VERBOSE = false; // TODO: better way to handle message printing

const double Engine::dt_max = 1.0;
const double Engine::dt_init = 0.001;
const double Engine::h_init = 0.01; // TODO: can be relaxed with higher-order solvers!

Engine::Engine(const State & s0, const std::vector<Transition*> transitions,
    const Parameters & par) : s0(s0), h(h_init), dt(dt_init), vf(s0, transitions, par) {
  // duplicate transitions
  replace_transitions(transitions); // copy transitions to this->transitions
}

Engine::Engine(const Engine & e) : s0(e.s0), h(e.h), dt(e.dt), vf(e.vf) {
  // copy constructor: duplicate transitions
  replace_transitions(e.transitions);
}

Engine & Engine::operator=(const Engine & e) { // copy assignment constructor
  if ( this != &e ) {
    s0 = e.s0;
    h = e.h;
    dt = e.dt;
    vf = e.vf;
    replace_transitions(e.transitions);
  } // else do nothing
  return *this;
}

void Engine::reset(const State & s0, const Parameters & par) {
  this->s0 = s0;
  // FIXME: reset algorithmic parameters??
  // FIXME: update VectorField?
}

State Engine::evolve_gillespie(State s, double tmax, const Parameters & p, Rng & rng) {
  while ( true ) {
    // choose the next event
    std::vector<double> rates(transitions.size(), 0.0);
    std::transform(transitions.begin(), transitions.end(), rates.begin(),
        [&] (Transition* x) -> double {return x->rate(s, p);});
    double lambda = std::accumulate(rates.begin(), rates.end(), 0.0);
    // check that lambda > 0 (otherwise jump to tmax)
    if ( lambda == 0 ) {
      s.t() = tmax;
      break;
    } // else, lambda > 0
    double time_increment = rng.exponential(lambda);
    // check that the next event takes place before tmax
    if ( std::nextafter(s.t() + time_increment, tmax) >= tmax ) {
      s.t() = tmax;
      break;
    } // else, apply the transition and incerment the time with dt

    Transition* trans = choose_random_transition(transitions, rates, lambda, rng);
    if ( trans == nullptr ) {
      throw std::logic_error("unable to randomly choose transition" + RIGHT_HERE);
    }
    s = trans->apply(s);
    s.t() += time_increment; // manually increase the time
    // TODO: make dt optional argument for Transition::apply?
  }
  return s;
}

State Engine::evolve_tauleap(State s, double tmax, const Parameters & p, Rng & rng) {
  while ( std::nextafter(s.t(), tmax) < tmax ) {
    // choose the next event
    std::vector<double> rates(transitions.size(), 0.0);
    std::transform(transitions.begin(), transitions.end(), rates.begin(),
        [&] (Transition* x) -> double {return x->rate(s, p);});
    double lambda = std::accumulate(rates.begin(), rates.end(), 0.0);
    // sample number of events
    int n = rng.poisson(lambda * dt);
    // do n transitions
    for ( int i = 0; i < n; ++i ) {
      Transition* trans = choose_random_transition(transitions, rates, lambda, rng);
      if ( trans == nullptr ) {
        throw std::logic_error("unable to randomly choose transition" + RIGHT_HERE);
      }
      s = trans->apply(s);
      // re-compute the rates..
      std::transform(transitions.begin(), transitions.end(), rates.begin(),
          [&] (Transition* x) -> double {return x->rate(s, p);});
      lambda = std::accumulate(rates.begin(), rates.end(), 0.0);
    }
    s.t() += dt; // manually increase the time
    // adjust the timestep
    if ( lambda > 0 ) {
      dt = std::min({dt_max, 1/lambda, tmax - s.t()}); // TODO: smoothen, use intermediate rates
    } else {
      dt = std::min(dt_max, tmax - s.t());
    }
  }
  return s;
}

State Engine::evolve_ode(State s, double tmax, const Parameters & p) {
  // make sure that all variables of the state are continuous
  for ( auto & var : s ) {
    var.make_continuous(); // make_continous makes the entire vector continuous
  }
  // initialize a vectorfield with the fully continous state
  vf.update(s, p);
  // integrate the system of ODEs
  if ( tmax > s.t() ) {
    std::tie(s, std::ignore) = integrate(tmax - s.t(), vf, s, h);
  }
  return s;
}

State Engine::evolve_sde(State s, double tmax, const Parameters & p, Rng & rng) {
  // make sure that all variables of the state are continuous
  for ( auto & var : s ) {
    var.make_continuous(); // make_continous makes the entire vector continuous
  }
  // initialize a vectorfield with the fully continous state
  vf.update(s, p);
  // integrate the system of SDEs
  if ( tmax > s.t() ) {
    std::tie(s, std::ignore) = sde_integrate(tmax - s.t(), vf, s, h, rng);
  }
  return s;
}

State Engine::evolve_hybrid(State s, double tmax,
    const Parameters & p, Rng & rng, bool sde) {
  // use THRESHOLD to define a backwards-compatible switcher
  VarTypeSwitcher switcher = [](const State & s, const Parameters & p) -> std::vector<bool> {
    std::vector<bool> ought_discrete(s.flat_size());
    for ( size_t i = 0; i < s.flat_size(); ++i ) {
      const Variable & var = s[i];
      if ( var.is_discrete )
        // keep discrete if below threshold + window/2
        ought_discrete[i] = (var.disc_value < THRESHOLD + WINDOW/2);
      else { // continuous
        // make discrete if below threshold - window/2
        ought_discrete[i] = (var.cont_value < THRESHOLD - WINDOW/2);
      }
    }
    return ought_discrete;
  };
  // now use the general method
  return evolve_hybrid(s, tmax, p, switcher, rng, sde);
}

State Engine::evolve_hybrid(State s, double tmax,
    const Parameters & p, const VarTypeSwitcher & switcher, Rng & rng, bool sde) {
  // make sure that the timestep "fits"
  while ( std::nextafter(s.t(), tmax) < tmax ) {
    /* test if we need to switch Variables from discrete
     * to continuous and vice versa. Do this first to
     * avoid problems with the initially defined state.
     */
    std::vector<bool> ought_discrete = switcher(s, p);
    for ( size_t i = 0; i < s.flat_size(); ++i ) {
      Variable & var = s[i];
      if ( var.is_discrete && !ought_discrete[i] ) {
        var.make_continuous();
        // print message for debugging etc.
        if ( VERBOSE ) {
          std::cout << "# switching from discrete to continuous" << std::endl;
        }
      }
      if ( !var.is_discrete && ought_discrete[i] ) {
        if ( VERBOSE && var.value() < 0 ) {
          std::cerr << "# WARNING: negative var detected: " << var << std::endl;
        }
        var.make_discrete();
        // be careful with the initial timestep...
        dt = std::min(dt, dt_init);
        // print message for debugging etc.
        if ( VERBOSE ) {
          std::cout << "# switching from continuous to discrete" << std::endl;
        }
      }
    }
    // re-assign indices whenever the signature of s has changed
    vf.update(s, p);
    // take a good time step based on instantaneous transition rate
    double lambda = vf.totalTransitionRate();
    if ( lambda > 0 ) {
      dt = std::min(dt_max, 1/lambda);
    } else {
      dt = dt_max;
    }
    double time_increment = std::min(dt, tmax - s.t());
    // integrate the deterministic variables and the loads
    std::map<Transition*, double> loads;
    if ( sde ) {
      // h is passed by ref and modified (TODO adaptive SDE integrator)
      std::tie(s, loads) = sde_integrate(time_increment, vf, s, h, rng);
    } else { // use ODE integrator
      // h is passed by ref and modified
      std::tie(s, loads) = integrate(time_increment, vf, s, h);
    }
    auto op = [] (double c, std::pair<Transition*, double> l) {return c + l.second;};
    double Lambda = std::accumulate(loads.begin(), loads.end(), 0.0, op);
    // sample number of events (0 if Lambda = 0)
    int n = (Lambda > 0 ? rng.poisson(Lambda) : 0); // Lambda = lambda * dt

    // do n transitions
    for ( int i = 0; i < n; ++i ) {
      if ( i > 0 ) {
        // re-compute the rates in order to apply another transition
        for ( auto & [trans, load] : loads ) {
          load = trans->rate(s, p) * dt; // approximate load by rate * dt
        }
        Lambda = std::accumulate(loads.begin(), loads.end(), 0.0, op);
        if ( Lambda == 0 ) {
          // print message for debugging etc.
          if ( VERBOSE ) {
            std::cerr << "# WARNING: sampling more than one event is not possible"
                      << RIGHT_HERE << std::endl;
          }
          break; // break for ( i = 0; i < n; ++i )
        }
      }
      // sample a random transition
      Transition* trans = choose_random_transition(loads, Lambda, rng);
      if ( trans == nullptr ) {
        throw std::logic_error("unable to choose random transition" + RIGHT_HERE);
      } // else..
      State s_prime = trans->apply(s);
      if ( s_prime.isNonNegative() ) {
        s = s_prime;
      } else {
        throw std::logic_error("about to do an illegal transition" + RIGHT_HERE);
      }
    } // for i = 0 ... n-1
  }
  return s;
}

/** Auxiliary integrate function is a wrapper around the Boost odeint methods.
 *
 * The State is "encoded" into a single vector, that can be handled by
 * odeint. After integration, the vector is decoded back into the State
 * object.
 */
std::pair<State, std::map<Transition*, double>>
    integrate(double dt, const VectorField & vf, const State & s, double & h) {
  // TODO: verify that s and vf are compatible
  auto controlled_stepper = odeint::make_controlled(1e-6, 1e-6, real_vec_error_stepper_type());
  // TODO: make the stepper member of engine
  RealVec y = vf.encode(s);

  double t_start = s.t();
  double t_end = s.t() + dt;
  double t_old = s.t(); // used to keep track of the timestep
  double h0 = h; // the initially proposed stepsize
  // observer is used to get h before final step to t_end
  auto observer = [&] (const RealVec & y, double t_new) {
    if ( t_old < t_new && t_new < t_end ) {
      h = t_new - t_old;
      t_old = t_new;
    }
  };
  size_t steps = odeint::integrate_adaptive(controlled_stepper,
      std::cref(vf), y, t_start, t_end, h0,
      observer);
  (void) steps; // TODO: do something with steps??

  State u = vf.decodeState(s.t()+dt, y);
  auto loads = vf.decodeLoads(s.t()+dt, y);
  return std::make_pair(u, loads);
}

/** Use an SDE integrator to evolve the State forward in time.
 */
std::pair<State, std::map<Transition*, double>>
    sde_integrate(double dt, const VectorField & vf, const State & s, double & h, Rng & rng) {
  // define the EM stepper
  EulerMaruyamaStepper stepper(rng); // rng is passed by ref
  // TODO: verify that s and vf are compatible
  RealVec y = vf.encode(s);

  double t_start = s.t();
  double t_end = s.t() + dt;
  double h0 = std::min(dt, h);
  size_t steps = odeint::integrate_const(stepper, std::cref(vf),
      y, t_start, t_end, h0);
  (void) steps; // TODO: do something with steps??

  State u = vf.decodeState(t_end, y);
  auto loads = vf.decodeLoads(t_end, y);
  return std::make_pair(u, loads);
}

Transition* choose_random_transition(const std::vector<Transition*> & transitions,
    const std::vector<double> & rates, double lambda, Rng & rng) {
  if ( lambda <= 0 ) {
    std::cerr << "# WARNING: lambda not positive: " << lambda << RIGHT_HERE << std::endl;
    return nullptr;
  }
  double u = 0.0;
  try {
    u = rng.uniform(0, lambda);
  } catch ( const std::exception & ex ) {
    std::cout << ex.what() << RIGHT_HERE << std::endl;
    throw ex;
  }
  auto transit = transitions.begin();
  for ( auto r : rates ) {
    u -= r;
    if ( u < 0 ) {
      break;
    } // else...
    ++transit;
  }
  if ( transit != transitions.end() ) {
    return *transit;
  } else {
    return nullptr; // signals failure
  }
}

Transition* choose_random_transition(const std::map<Transition*, double> & loads,
    double Lambda, Rng & rng) {
  if ( Lambda <= 0 ) {
    std::cerr << "# WARNING: lambda not positive: " << Lambda << RIGHT_HERE << std::endl;
    return nullptr;
  }
  double u = 0.0;
  try { // FIXME: Lambda can be nan!!
    u = rng.uniform(0, Lambda);
  } catch ( const std::exception & ex ) {
    std::cout << ex.what() << RIGHT_HERE << std::endl;
    throw ex;
  }
  //double u = rng.uniform(0, Lambda);
  for ( auto & load : loads ) {
    u -= load.second;
    if ( u < 0 ) {
      return load.first;
    }
  } // for load in loads
  return nullptr; // signals failure
}


#define BOOST_TEST_MODULE srook_constant_sequence_unit_test
#include <boost/test/included/unit_test.hpp>
#include <algorithm>
#include <srook/config/attribute/maybe_unused.hpp>
#include <srook/mpl/constant_sequence/algorithm.hpp>
// #include <srook/mpl/constant_sequence/equ_sequence.hpp> // [[deprecated]]
#include <srook/mpl/constant_sequence/interval_sequence.hpp>
#include <srook/mpl/constant_sequence/iterator/distance.hpp>
#include <srook/mpl/constant_sequence/iterator/index_sequence_iterator.hpp>
#include <srook/mpl/constant_sequence/iterator/make_index_sequence_iterator_range.hpp>
// #include <srook/mpl/constant_sequence/samevalue_sequence.hpp> // [[deprecated]]
#include <vector>

#define INDEX_SEQUENCE(...) std::index_sequence<__VA_ARGS__>
template <class L, class R>
constexpr void static_type_test()
{
    static_assert(std::is_same_v<L, R>);
}

template <std::size_t v>
using pred = std::integral_constant<bool, (v % 2 == 0)>;

template <std::size_t v>
using function = std::integral_constant<std::size_t, (v * 2)>;

BOOST_AUTO_TEST_CASE(test_op_reportings)
{
    // generating sequence
    using geometric_progression = srook::make_interval_sequence<srook::plus, 10, 10, 5>;
    static_type_test<geometric_progression, INDEX_SEQUENCE(10, 20, 30, 40, 50)>();

    using no_geometric_progression = srook::constant_sequence::insert_t<std::index_sequence<40, 40, 40>, geometric_progression::size() / 2, geometric_progression>;
    static_type_test<no_geometric_progression, INDEX_SEQUENCE(10, 20, 40, 40, 40, 30, 40, 50)>();

    // using samevalue_sequence = srook::constant_sequence::make_samevalue_sequence<4, 42>; // [[deprecated]]
    // static_type_test<samevalue_sequence, INDEX_SEQUENCE(42, 42, 42, 42)>();

    // using equ_sequence = srook::constant_sequence::make_equ_sequence<5>; // [[deprecated]]
    // static_type_test<equ_sequence, INDEX_SEQUENCE(0, 1, 2, 2, 3, 3, 3, 4, 4, 4, 4, 5, 5, 5, 5, 5)>();

    // constant sequence iterator
    using constant_sequence_iterator =
	srook::index_sequence_iterator::index_sequence_iterator<geometric_progression>;
    BOOST_TEST((constant_sequence_iterator::value == 10));

    using one_increment = srook::index_sequence_iterator::increment<constant_sequence_iterator>;
    BOOST_TEST((one_increment::value == 20));

    using one_decrement = srook::index_sequence_iterator::decrement<one_increment>;
    BOOST_TEST((one_decrement::value == 10));

    static_type_test<
	srook::constant_sequence::iter_swap_L<
	    srook::index_sequence_iterator::increment<one_increment>,
	    srook::index_sequence_iterator::increment<constant_sequence_iterator>>,
	INDEX_SEQUENCE(10, 20, 20, 40, 50)>();

    constexpr std::size_t distance_result =
	srook::index_sequence_iterator::distance_v<one_increment, srook::index_sequence_iterator::increment<one_increment>>;
    BOOST_TEST((distance_result == 1));

    using make_index_sequence_iterator_range_result =
	srook::index_sequence_iterator::make_index_sequence_iterator_range_t<
	    one_increment,
	    srook::index_sequence_iterator::increment<one_increment>>;
    static_type_test<make_index_sequence_iterator_range_result, INDEX_SEQUENCE(20, 30)>();

    // algorithms
    constexpr std::size_t at_result = srook::constant_sequence::at_v<2, geometric_progression>;
    BOOST_TEST((at_result == 30));

    constexpr bool binary_search_result1 = srook::constant_sequence::binary_search_v<30, geometric_progression>;
    constexpr bool binary_search_result2 = srook::constant_sequence::binary_search_v<42, geometric_progression>;
    BOOST_TEST((binary_search_result1 and !binary_search_result2));

    using concat_result = srook::constant_sequence::concat_t<geometric_progression, std::index_sequence<42, 42>>;
    static_type_test<concat_result, INDEX_SEQUENCE(10, 20, 30, 40, 50, 42, 42)>();

    constexpr std::size_t count_result = srook::constant_sequence::count_v<40, no_geometric_progression>;
    BOOST_TEST((4 == count_result));

    constexpr std::size_t count_if_result = srook::constant_sequence::count_if_v<pred, std::make_index_sequence<10>>;
    BOOST_TEST((5 == count_if_result));

    using no_geometric_progression2 = no_geometric_progression;
    constexpr bool equal_result1 = srook::constant_sequence::equal_v<no_geometric_progression, no_geometric_progression2>;
    constexpr bool equal_result2 = srook::constant_sequence::equal_v<no_geometric_progression, std::index_sequence<1>>;
    BOOST_TEST((equal_result1 and !equal_result2));

    constexpr auto equal_range_result = std::move(srook::constant_sequence::equal_range_v<40, no_geometric_progression>);
    BOOST_TEST((equal_range_result.first == 2 and equal_range_result.second == 7));

    using erase_result = srook::constant_sequence::erase_t<40, no_geometric_progression>;
    static_type_test<erase_result, INDEX_SEQUENCE(10, 20, 40, 40, 30, 40, 50)>();

    using erase_all_result = srook::constant_sequence::erase_all_t<40, no_geometric_progression>;
    static_type_test<erase_all_result, INDEX_SEQUENCE(10, 20, 30, 50)>();

    using erase_duplicate_result = srook::constant_sequence::erase_duplicate_t<no_geometric_progression>;
    static_type_test<erase_duplicate_result, INDEX_SEQUENCE(10, 20, 40, 30, 50)>();

    using erase_if_result = srook::constant_sequence::erase_if_t<pred, std::make_index_sequence<10>>;
    static_type_test<erase_if_result, INDEX_SEQUENCE(0, 2, 4, 6, 8)>();

    using erase_index_result = srook::constant_sequence::erase_index_t<INDEX_SEQUENCE(10, 20), INDEX_SEQUENCE(10, 20, 30, 40)>;
    static_type_test<erase_index_result, INDEX_SEQUENCE(30, 40)>();

    using erase_n_result = srook::constant_sequence::erase_n_t<3, geometric_progression>;
    static_type_test<erase_n_result, INDEX_SEQUENCE(10, 20, 30, 50)>();

    using filter_result = srook::constant_sequence::filter_t<pred, std::make_index_sequence<10>>;
    static_type_test<filter_result, INDEX_SEQUENCE(0, 2, 4, 6, 8)>();

    constexpr int find_result = srook::constant_sequence::find_v<30, geometric_progression>;
    BOOST_TEST((find_result == 2));

    constexpr int find_end_result = srook::constant_sequence::find_end_v<INDEX_SEQUENCE(5, 6), INDEX_SEQUENCE(1, 2, 3, 4, 5, 6, 7, 8, 5, 6, 9)>;
    BOOST_TEST((find_end_result == 8));

    SROOK_ATTRIBUTE_UNUSED constexpr int find_if_result1 = srook::constant_sequence::find_if_v<pred, INDEX_SEQUENCE(1, 1, 1, 1, 1, 2, 1)>;
    // constexpr int find_if_result2 = srook::constant_sequence::find_if_v<pred, srook::constant_sequence::make_samevalue_sequence<4, 1>>; // [[deprecated]]
    // BOOST_TEST((find_if_result1 == 5 and find_if_result2 == -1));

    constexpr int find_if_not_result1 = srook::constant_sequence::find_if_not_v<pred, INDEX_SEQUENCE(2, 2, 2, 2, 1)>;
    constexpr int find_if_not_result2 = srook::constant_sequence::find_if_not_v<pred, geometric_progression>;
    BOOST_TEST((find_if_not_result1 == 4 and find_if_not_result2 == -1));

    using find_index_result = srook::constant_sequence::find_index_t<2, INDEX_SEQUENCE(1, 2, 2, 2, 3, 4)>;
    static_type_test<find_index_result, INDEX_SEQUENCE(1, 2, 3)>();

    using find_if_index_result = srook::constant_sequence::find_if_index_t<pred, INDEX_SEQUENCE(1, 2, 3, 4, 5)>;
    static_type_test<find_if_index_result, INDEX_SEQUENCE(1, 3)>();

    constexpr std::size_t first_result = srook::constant_sequence::first_v<geometric_progression>;
    BOOST_TEST((first_result == 10));

    using for_each_result = srook::constant_sequence::for_each_t<function, geometric_progression>;
    static_type_test<for_each_result, INDEX_SEQUENCE(20, 40, 60, 80, 100)>();

    constexpr bool includes_result1 = srook::constant_sequence::includes_v<geometric_progression, INDEX_SEQUENCE(20, 30, 40)>;
    constexpr bool includes_result2 = srook::constant_sequence::includes_v<geometric_progression, std::make_index_sequence<3>>;
    BOOST_TEST((includes_result1 and !includes_result2));

    using inplace_merge_result = srook::constant_sequence::inplace_merge_t<geometric_progression, INDEX_SEQUENCE(1, 2, 3, 10, 20)>;
    static_type_test<inplace_merge_result, INDEX_SEQUENCE(1, 2, 3, 10, 10, 20, 20, 30, 40, 50)>();

    constexpr bool is_all_less_result = srook::constant_sequence::is_all_less_v<INDEX_SEQUENCE(1, 2), INDEX_SEQUENCE(10, 20)>;
    constexpr bool is_all_greater_result = srook::constant_sequence::is_all_greater_v<INDEX_SEQUENCE(10, 20), INDEX_SEQUENCE(1, 2)>;
    BOOST_TEST((is_all_less_result and is_all_greater_result));

    constexpr bool is_heap_result1 = srook::constant_sequence::is_heap_v<INDEX_SEQUENCE(100, 19, 36, 17, 3, 25, 1, 2, 7)>;
    constexpr bool is_heap_result2 = srook::constant_sequence::is_heap_v<INDEX_SEQUENCE(1, 2, 3, 17, 19, 36, 7, 25, 100), srook::constant_sequence::less>;
    BOOST_TEST((is_heap_result1 and is_heap_result2));

    constexpr bool is_partitioned_result1 = srook::constant_sequence::is_partitioned_v<pred, INDEX_SEQUENCE(1, 3, 5, 7, 9, 2, 4, 6)>;
    constexpr bool is_partitioned_result2 = srook::constant_sequence::is_partitioned_v<pred, INDEX_SEQUENCE(1, 2, 3)>;
    BOOST_TEST((is_partitioned_result1 and !is_partitioned_result2));

    constexpr bool is_permutation_result = srook::constant_sequence::is_permutation_v<INDEX_SEQUENCE(10, 20, 30), INDEX_SEQUENCE(20, 10, 30)>;
    BOOST_TEST((is_permutation_result));

    constexpr int search_result = srook::constant_sequence::search_v<INDEX_SEQUENCE(40, 50), geometric_progression>;
    BOOST_TEST((search_result == 3));

    constexpr int search_n_result = srook::constant_sequence::search_n_v<INDEX_SEQUENCE(1, 2, 3, 2, 2, 3, 1, 2, 3, 3, 3, 4, 1, 2, 3), 3, 3>;
    BOOST_TEST((search_n_result == 8));

    constexpr bool is_sorted_result1 = srook::constant_sequence::is_sorted_v<geometric_progression>;
    constexpr bool is_sorted_result2 = srook::constant_sequence::is_sorted_v<srook::constant_sequence::reverse_t<geometric_progression>, srook::constant_sequence::greater>;
    BOOST_TEST((is_sorted_result1 and is_sorted_result2));

    constexpr std::size_t is_sorted_until_result1 = srook::constant_sequence::is_sorted_until_v<INDEX_SEQUENCE(2, 3, 4, 1, 5, 6, 7), srook::constant_sequence::less>;
    constexpr std::size_t is_sorted_until_result2 = srook::constant_sequence::is_sorted_until_v<INDEX_SEQUENCE(6, 5, 4, 7), srook::constant_sequence::greater>;
    BOOST_TEST((is_sorted_until_result1 == 3 and is_sorted_until_result2 == 3));

    constexpr std::size_t last_result = srook::constant_sequence::last_v<geometric_progression>;
    BOOST_TEST((last_result == 50));

    constexpr std::size_t length_result = srook::constant_sequence::length_v<geometric_progression>;
    BOOST_TEST((length_result == 5));

    constexpr int lower_bound_result1 = srook::constant_sequence::lower_bound_v<40, no_geometric_progression>;
    constexpr int lower_bound_result2 = srook::constant_sequence::lower_bound_v<100, no_geometric_progression>;
    BOOST_TEST(((lower_bound_result1 == 2) and (lower_bound_result2 == -1)));

    using nth_element_result = srook::constant_sequence::nth_element_t<3, INDEX_SEQUENCE(20, 10, 40, 30, 50)>;
    constexpr std::size_t nth_min_point =
	srook::constant_sequence::find_v<srook::constant_sequence::nth_min_v<3, nth_element_result>, nth_element_result>;
    BOOST_TEST((
	srook::constant_sequence::is_permutation_v<
	    srook::constant_sequence::partial_head_t<nth_min_point + 1, nth_element_result>,
	    INDEX_SEQUENCE(10, 20, 30)> and
	srook::constant_sequence::is_permutation_v<
	    srook::constant_sequence::partial_tail_t<nth_min_point + 1, nth_element_result>,
	    INDEX_SEQUENCE(40, 50)>));

    constexpr std::size_t nth_max_result = srook::constant_sequence::nth_max_v<3, geometric_progression>;
    constexpr std::size_t nth_min_result = srook::constant_sequence::nth_min_v<3, geometric_progression>;
    BOOST_TEST((nth_max_result == 30 and nth_min_result == 30));

    constexpr bool none_of_result = srook::constant_sequence::none_of_v<pred, INDEX_SEQUENCE(1, 3, 5, 7)>;
    BOOST_TEST(none_of_result);

    constexpr std::size_t min_result = srook::constant_sequence::min_v<geometric_progression>;
    BOOST_TEST((min_result == 10));

    constexpr std::size_t max_result = srook::constant_sequence::max_v<geometric_progression>;
    BOOST_TEST((max_result == 50));

    using merge_result = srook::constant_sequence::merge_t<geometric_progression, INDEX_SEQUENCE(30, 40, 50, 70)>;
    static_type_test<merge_result, INDEX_SEQUENCE(10, 20, 30, 30, 40, 40, 50, 50, 70)>();

    constexpr int mismatch_result1 = srook::constant_sequence::mismatch_v<geometric_progression, INDEX_SEQUENCE(10, 20, 30, 60)>;
    constexpr int mismatch_result2 = srook::constant_sequence::mismatch_v<geometric_progression, geometric_progression>;
    BOOST_TEST((mismatch_result1 == 3 and mismatch_result2 == -1));

    using partial_head_result = srook::constant_sequence::partial_head_t<geometric_progression::size() / 2, geometric_progression>;
    static_type_test<partial_head_result, INDEX_SEQUENCE(10, 20)>();

    using partial_tail_result = srook::constant_sequence::partial_tail_t<geometric_progression::size() / 2, geometric_progression>;
    static_type_test<partial_tail_result, INDEX_SEQUENCE(30, 40, 50)>();

    using prev_rotate_result = srook::constant_sequence::prev_rotate_t<geometric_progression>;
    static_type_test<prev_rotate_result, INDEX_SEQUENCE(20, 30, 40, 50, 10)>();

    using pop_back_result = srook::constant_sequence::pop_back_t<geometric_progression>;
    static_type_test<pop_back_result, INDEX_SEQUENCE(10, 20, 30, 40)>();

    using pop_front_result = srook::constant_sequence::pop_front_t<geometric_progression>;
    static_type_test<pop_front_result, INDEX_SEQUENCE(20, 30, 40, 50)>();

    using replace_result = srook::constant_sequence::replace_t<40, 42, no_geometric_progression>;
    static_type_test<replace_result, INDEX_SEQUENCE(10, 20, 42, 40, 40, 30, 40, 50)>();

    using replace_all_result = srook::constant_sequence::replace_all_t<40, 42, no_geometric_progression>;
    static_type_test<replace_all_result, INDEX_SEQUENCE(10, 20, 42, 42, 42, 30, 42, 50)>();

    using reverse_result = srook::constant_sequence::reverse_t<geometric_progression>;
    static_type_test<reverse_result, INDEX_SEQUENCE(50, 40, 30, 20, 10)>();

    using rotate_result = srook::constant_sequence::rotate_t<geometric_progression>;
    static_type_test<rotate_result, INDEX_SEQUENCE(50, 10, 20, 30, 40)>();

    using set_difference_result = srook::constant_sequence::set_difference_t<INDEX_SEQUENCE(1, 2, 3, 4, 5), INDEX_SEQUENCE(1, 2, 5, 6, 7)>;
    static_type_test<set_difference_result, INDEX_SEQUENCE(3, 4, 6, 7)>();

    using set_intersection_result = srook::constant_sequence::set_intersection_t<INDEX_SEQUENCE(1, 2, 3, 4, 5), INDEX_SEQUENCE(1, 2, 5, 6, 7)>;
    static_type_test<set_intersection_result, INDEX_SEQUENCE(1, 2, 5)>();

    constexpr std::size_t size_result = srook::constant_sequence::size<geometric_progression>;
    BOOST_TEST((size_result == srook::constant_sequence::length_v<geometric_progression>));

    using random_sequence = INDEX_SEQUENCE(30, 20, 42, 19, 22, 2, 8, 45, 10);
    using sort_less_result = srook::constant_sequence::sort_t<random_sequence>;
    static_type_test<sort_less_result, INDEX_SEQUENCE(2, 8, 10, 19, 20, 22, 30, 42, 45)>();

    constexpr std::size_t summation_result = srook::constant_sequence::summation_v<geometric_progression>;
    BOOST_TEST((summation_result == 110));

    using swap_L_result = srook::constant_sequence::swap_L<3, geometric_progression, random_sequence>;
    using swap_R_result = srook::constant_sequence::swap_R<3, geometric_progression, random_sequence>;
    static_type_test<swap_L_result, INDEX_SEQUENCE(10, 20, 30, 19, 50)>();
    static_type_test<swap_R_result, INDEX_SEQUENCE(30, 20, 42, 40, 22, 2, 8, 45, 10)>();

    using swap_at_L_result = srook::constant_sequence::swap_at_L<2, geometric_progression, 4, random_sequence>;
    using swap_at_R_result = srook::constant_sequence::swap_at_R<2, geometric_progression, 4, random_sequence>;
    static_type_test<swap_at_L_result, INDEX_SEQUENCE(10, 20, 22, 40, 50)>();
    static_type_test<swap_at_R_result, INDEX_SEQUENCE(30, 20, 42, 19, 30, 2, 8, 45, 10)>();

    // const auto test_range = std::move(srook::constant_sequence::transfer<std::vector, geometric_progression>::value);
    // decltype(test_range) comparison{10, 20, 30, 40, 50};
    // BOOST_TEST(std::equal(test_range.begin(), test_range.end(), comparison.begin()));

    const auto test_array = std::move(srook::constant_sequence::transfer_array<geometric_progression>::value);
    decltype(test_array) comparison_ar{{10, 20, 30, 40, 50}};
    BOOST_TEST(std::equal(test_array.begin(), test_array.end(), comparison_ar.begin()));

    constexpr int upper_bound_result1 = srook::constant_sequence::upper_bound_v<40, no_geometric_progression>;
    constexpr int upper_bound_result2 = srook::constant_sequence::upper_bound_v<100, no_geometric_progression>;
    BOOST_TEST(((upper_bound_result1 == 7) and (upper_bound_result2 == -1)));
}


#ifndef RLSS_OCCUPANCY_GRID_HPP
#define RLSS_OCCUPANCY_GRID_HPP

#include <limits>
#include <unordered_set>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <absl/strings/str_cat.h>
#include <queue>
#include <Eigen/StdVector>
#include <boost/functional/hash/hash.hpp>
#include <rlss/internal/Util.hpp>
#include <rlss/CollisionShapes/CollisionShape.hpp>
#include <qp_wrappers/problem.hpp>
#include <qp_wrappers/cplex.hpp>

namespace rlss {

template<typename T, unsigned int DIM>
class OccupancyGridIterator;

template<typename T, unsigned int DIM>
class OccupancyGridDistanceIterator;

template<typename T, unsigned int DIM>
class OccupancyGrid {
public:
    using VectorDIM = rlss::internal::VectorDIM<T, DIM>;
    using VectorlliDIM = rlss::internal::VectorDIM<long long int, DIM>;
    using AlignedBox = Eigen::AlignedBox<T, DIM>;
    using StdVectorVectorDIM = rlss::internal::StdVectorVectorDIM<T, DIM>;
    using Ellipsoid = rlss::Ellipsoid<T, DIM>;

    using Coordinate = VectorDIM;
    using Index = VectorlliDIM;

    using iterator = OccupancyGridIterator<T, DIM>;
    using distance_iterator = OccupancyGridDistanceIterator<T, DIM>;

    // implement hash table to store unique keys of the obstacle voxel coordinates,
    // and uses hash function based on vector coordinate to get container idx
    using UnorderedIndexSet
            = std::unordered_set<
                    Index,
                    rlss::internal::VectorDIMHasher<long long int, DIM>
            >;

    // step size is the resolution of the obstacle grid
    OccupancyGrid(Coordinate step_size): m_step_size(step_size) {

    }

    /*
    * gets the cell index that coordinate is in
    */
    Index getIndex(const Coordinate& coord) const {
        Index idx;
        for(unsigned int d = 0; d < DIM; d++) {
            idx(d) = std::llround((coord(d) / m_step_size(d)) - 0.5);
        }
        return idx;
    }

    /*
    * get the center coordinate of the cell with the given index
    */
    Coordinate getCenter(const Index& idx) const {
        Coordinate center;
        for(unsigned int d = 0; d < DIM; d++) {
            center(d) = (idx(d) + 0.5) * m_step_size(d);
        }
        return center;
    }

    /*
     * Get neighbors of the index
     */
    static std::vector<Index> getNeighbors(const Index& idx) {
        std::vector<Index> res;
        Index dir = Index::Zero();

        for(unsigned int d = 0; d < DIM; d++) {
            for(const long long int& i : {-1, 1}) {
                dir(d) = i;
                res.push_back(idx + dir);
            }
            dir(d) = 0;
        }

        return res;
    }

    /*
     * Get neighbors of the index that the coordinate is in
     */
    std::vector<Index> getNeighbors(const Coordinate& coord) const {
        return this->getNeighbors(this->getIndex(coord));
    }

    /*
    * get the center coordinate of the cell that contains the given coordinate
    */
    Coordinate getCenter(const Coordinate& coord) const {
        return this->getCenter(this->getIndex(coord));
    }

    // remove occupied voxel from the grid
    void removeOccupancy(const Index& idx) {
        m_grid.erase(idx);
    }

    void removeOccupancy(const Coordinate& coord) {
        this->removeOccupancy(this->getIndex(coord));
    }

    // insert into grid (unordered set container)
    void setOccupancy(const Index& idx) {
        m_grid.insert(idx);
    }

    void setOccupancy(const Coordinate& coord) {
        this->setOccupancy(this->getIndex(coord));
    }

    // function takes in two arguments - minimum and maximum voxel coordinates
    // to be inserted into the obs grid
    std::vector<Index> fillOccupancy(const Index& min, const Index& max) {
        for(unsigned int d = 0; d < DIM; d++) {
            if(min(d) > max(d)) {
                throw std::domain_error(
                    absl::StrCat(
                        "minimum index is bigger than index coordinate",
                        " at dimension ",
                        d,
                        "min(d): ",
                        min(d),
                        " max(d): ",
                        max(d)
                    )
                );
            }
        }

        std::vector<Index> filled_indexes;

        std::queue<Index> occupied_indexes;
        UnorderedIndexSet visited;
        occupied_indexes.push(min);
        visited.insert(min);
        while(!occupied_indexes.empty()) {
            Index& occ = occupied_indexes.front();
            if(!this->isOccupied(occ)) {
                // if not inside grid, insert into grid and append to filled_indexes vector
                filled_indexes.push_back(occ);
                this->setOccupancy(occ);
            }

            // iteratively search the all nbr voxels, and append to occupied indexes vector
            // if it has yet to be visited, to be inserted into occ grid until max index
            for(unsigned int d = 0; d < DIM; d++) {
                occ(d)++;
                if(occ(d) <= max(d) && visited.find(occ) == visited.end()) {
                    occupied_indexes.push(occ);
                    visited.insert(occ);
                }
                occ(d)--;
            }
            occupied_indexes.pop();
        }

        return filled_indexes;
    }

    std::vector<Index> fillOccupancy(
            const Coordinate& min, const Coordinate& max) {
        return this->fillOccupancy(this->getIndex(min), this->getIndex(max));
    }

    // check if voxel is occupied
    bool isOccupied(const Index& idx) const {
        AlignedBox idxBox = this->toBox(idx);

        for(const auto& bbox: m_temporary_obstacles) {
            if(bbox.intersects(idxBox)) {
                return true;
            }
        }

        return m_grid.find(idx) != m_grid.end();
    }

    bool isOccupied(const Coordinate& coord) const {
        return this->isOccupied(this->getIndex(coord));
    }

    bool isOccupied(const AlignedBox& box) const {
        // checks if the AlignedBox region contains any temporary obstacles
        for(const auto& bbox: m_temporary_obstacles) {
            if(bbox.intersects(box))
                return true;
        }

        Index min = this->getIndex(box.min());
        Index max = this->getIndex(box.max());

        UnorderedIndexSet visited;
        std::queue<Index> indexes;
        indexes.push(min);
        visited.insert(min);

        // iteratively search through all voxels within the box to check if it contains
        // any obstacles
        while(!indexes.empty()) {
            Index& occ = indexes.front();
            if(m_grid.find(occ) != m_grid.end())
                return true;

            for(unsigned int d = 0; d < DIM; d++) {
                occ(d)++;
                if(occ(d) <= max(d) && visited.find(occ) == visited.end()) {
                    indexes.push(occ);
                    visited.insert(occ);
                }
                occ(d)--;
            }
            indexes.pop();
        }

        return false;

    }

    // add obstacle based on bounding box coordinates
    void addObstacle(const AlignedBox& box) {
        this->fillOccupancy(box.min(), box.max());
    }

    // add obstacle that is the convex hull of points in pts
    void addObstacle(const StdVectorVectorDIM& pts) {
        using Row = rlss::internal::Row<T>;

        if(pts.empty())
            return;

        // get box region of the convex hull of points inside pts vector
        AlignedBox box(pts[0], pts[0]);
        for(const auto& pt: pts) {
            box.extend(pt);
        }

        Index min = this->getIndex(box.min());
        Index max = this->getIndex(box.max());

        QPWrappers::CPLEX::Engine<T> cplex;
        cplex.setFeasibilityTolerance(1e-9);

        std::queue<Index> q;
        UnorderedIndexSet visited;
        q.push(min);
        visited.insert(min);
        int idx = 0;
        while(!q.empty()) {
            Index& fr = q.front();
            AlignedBox box = this->toBox(fr);

            // set variable limits
            QPWrappers::Problem<T> problem(DIM + pts.size());
            for(unsigned int d = 0; d < DIM; d++) {
                problem.set_var_limits(d, box.min()(d), box.max()(d));
            }
            for(std::size_t i = 0; i < pts.size(); i++) {
                problem.set_var_limits(
                        DIM + i, 0, std::numeric_limits<T>::max());
            }

            // initialize coeffcients to 1, 0 for first DIM cofficients
            Row coeff(DIM + pts.size());
            coeff.setZero();
            for(std::size_t i = 0; i < pts.size(); i++) {
                coeff(DIM + i) = 1;
            }

            problem.add_constraint(coeff, 1, 1);

            // set coefficients to -1 for first DIM coefficients
            for(unsigned int d = 0; d < DIM; d++) {
                coeff.setZero();
                coeff(d) = -1;
                // set coefficients of point coordinate
                for(std::size_t i = 0; i < pts.size(); i++) {
                    coeff(DIM + i) = pts[i](d);
                }
                // add constraint for each dimension
                problem.add_constraint(coeff, 0, 0);
            }

            // if LP solution is optimal, set box occupancy
            typename QPWrappers::Problem<T>::Vector result;
            auto ret = cplex.init(problem, result);
            if(ret == QPWrappers::OptReturnType::Optimal) {
                this->setOccupancy(fr);
            }

            // iterate and append nbr voxels if it is still unvisited to q
            for(unsigned int d = 0; d < DIM; d++) {
                fr(d)++;
                if(fr(d) <= max(d) && visited.find(fr) == visited.end()) {
                    q.push(fr);
                    visited.insert(fr);
                }
                fr(d)--;
            }

            q.pop();
        }
    }

    // add obstacle from ellipsoid
    void addObstacle(const Ellipsoid& ell) {
        // get bounding box region of ellipsoid
        AlignedBox bbox = ell.boundingBox();

        Index min = getIndex(bbox.min());
        Index max = getIndex(bbox.max());

        std::queue<Index> q;
        UnorderedIndexSet visited;
        q.push(min);
        visited.insert(min);
        int idx = 0;

        // iterate through all voxels in the bounding box. if the voxel is within
        // ellipsoid region, set to occupied
        while(!q.empty()) {
            Index& fr = q.front();
            AlignedBox box = this->toBox(fr);

            if(ell.intersects(box)) {
                this->setOccupancy(fr);
            }

            for(unsigned int d = 0; d < DIM; d++) {
                fr(d)++;
                if(fr(d) <= max(d) && visited.find(fr) == visited.end()) {
                    q.push(fr);
                    visited.insert(fr);
                }
                fr(d)--;
            }

            q.pop();
        }
    }

    void addTemporaryObstacle(const AlignedBox& box) {
        m_temporary_obstacles.push_back(box);
    }

    void clearTemporaryObstacles() {
        m_temporary_obstacles.clear();
    }

    // get voxel in AlignedBox type
    AlignedBox toBox(const Index& idx) const {
        Coordinate center = this->getCenter(idx);
        return AlignedBox(center - 0.5 * m_step_size,
                          center + 0.5 * m_step_size);
    }

    // get voxel in AlignedBox
    AlignedBox toBox(const Coordinate& coord) const {
        return this->toBox(this->getIndex(coord));
    }

    // iterator methods
    iterator begin() const {
        return iterator(*this, 0, m_grid.begin());
    }

    iterator end() const {
        return iterator(*this, m_temporary_obstacles.size(), m_grid.end());
    }

    distance_iterator begin(const AlignedBox& box, T max_distance) const {
        return distance_iterator(*this, 0, m_grid.begin(), box, max_distance);
    }

    distance_iterator end(const AlignedBox& box, T max_distance) const {
        return distance_iterator(*this, m_temporary_obstacles.size(),
                m_grid.end(), box, max_distance);
    }

    std::size_t size() const {
        return m_grid.size();
    }

    const UnorderedIndexSet& getIndexSet() const {
        return m_grid;
    }

    friend OccupancyGridIterator<T, DIM>;
    friend OccupancyGridDistanceIterator<T, DIM>;

private:
    Coordinate m_step_size;
    UnorderedIndexSet m_grid;

    std::vector<AlignedBox> m_temporary_obstacles;
}; // class OccupancyGrid

/*
 * Iterator for occupied alignedboxes such that the distance of alignedboxes
 * to the given box in the constructor is at most max_distance_to_max
 */
template<typename T, unsigned int DIM>
class OccupancyGridDistanceIterator {
public:
    using _OccupancyGrid = OccupancyGrid<T, DIM>;
    using UnorderedIndexSet = typename _OccupancyGrid::UnorderedIndexSet;
    using AlignedBox = typename _OccupancyGrid::AlignedBox;

    OccupancyGridDistanceIterator(
            const _OccupancyGrid& g,
            std::size_t temp_obs_idx,
            typename UnorderedIndexSet::const_iterator occ_idx_itr,
            const AlignedBox& bx,
            T max_distance_to_box
    ):
            grid(g),
            temporary_obstacles_idx(temp_obs_idx),
            occupied_idx_iterator(occ_idx_itr),
            box(bx),
            max_distance(max_distance_to_box)
    {
        if((temporary_obstacles_idx < grid.m_temporary_obstacles.size()
              || occupied_idx_iterator != grid.m_grid.end())
              && box.exteriorDistance(*(*this)) > max_distance_to_box) {
            operator++();
        }
    }

    explicit OccupancyGridDistanceIterator(
            const _OccupancyGrid& g,
            const AlignedBox& bx,
            T max_distance_to_box):
            rlss::OccupancyGridIterator<T, DIM>(
                    g,
                    0,
                    grid.m_grid.begin(),
                    bx,
                    max_distance_to_box)
    {
    }

    AlignedBox operator*() const {
        if(temporary_obstacles_idx < grid.m_temporary_obstacles.size()) {
//            debug_message("hop");
            return grid.m_temporary_obstacles[temporary_obstacles_idx];
        } else {
//            debug_message("ba");
            return grid.toBox(*occupied_idx_iterator);
        }
    }

    OccupancyGridDistanceIterator<T,DIM>& operator++() {
        do {
            if (temporary_obstacles_idx < grid.m_temporary_obstacles.size()) {
                temporary_obstacles_idx++;
            } else {
                occupied_idx_iterator++;
            }
        } while((temporary_obstacles_idx < grid.m_temporary_obstacles.size()
                 || occupied_idx_iterator != grid.m_grid.end())
                && box.exteriorDistance(*(*this)) > max_distance);

        return *this;
    }

    bool operator==(const OccupancyGridDistanceIterator<T, DIM>& rhs) const {
        return rhs.temporary_obstacles_idx == this->temporary_obstacles_idx
               && rhs.occupied_idx_iterator == this->occupied_idx_iterator;
    }
    bool operator!=(const OccupancyGridDistanceIterator<T, DIM>& rhs) const {
        return !operator==(rhs);
    }

private:

    const _OccupancyGrid& grid;
    AlignedBox box;
    T max_distance;
    std::size_t temporary_obstacles_idx;
    typename UnorderedIndexSet::const_iterator occupied_idx_iterator;
}; // class OccupancyGridDistanceIterator

template<typename T, unsigned int DIM>
class OccupancyGridIterator {
public:
    using _OccupancyGrid = OccupancyGrid<T, DIM>;
    using UnorderedIndexSet = typename _OccupancyGrid::UnorderedIndexSet;
    using AlignedBox = typename _OccupancyGrid::AlignedBox;

    OccupancyGridIterator(
            const _OccupancyGrid& g,
            std::size_t temp_obs_idx,
            typename UnorderedIndexSet::const_iterator occ_idx_itr
    ):
        grid(g),
        temporary_obstacles_idx(temp_obs_idx),
        occupied_idx_iterator(occ_idx_itr)
    {

    }

    explicit OccupancyGridIterator(const _OccupancyGrid& g):
        rlss::OccupancyGridIterator<T, DIM>(
                g,
                0,
                grid.m_grid.begin())
    {
    }

    AlignedBox operator*() const {
        if(temporary_obstacles_idx < grid.m_temporary_obstacles.size()) {
//            debug_message("hop");
            return grid.m_temporary_obstacles[temporary_obstacles_idx];
        } else {
//            debug_message("ba");
            return grid.toBox(*occupied_idx_iterator);
        }
    }

    OccupancyGridIterator<T,DIM>& operator++() {
        if(temporary_obstacles_idx < grid.m_temporary_obstacles.size()) {
            temporary_obstacles_idx++;
        } else {
            occupied_idx_iterator++;
        }
        return *this;
    }

    bool operator==(const OccupancyGridIterator<T, DIM>& rhs) const {
        return rhs.temporary_obstacles_idx == this->temporary_obstacles_idx
            && rhs.occupied_idx_iterator == this->occupied_idx_iterator;
    }
    bool operator!=(const OccupancyGridIterator<T, DIM>& rhs) const {
        return !operator==(rhs);
    }

private:

    const _OccupancyGrid& grid;
    std::size_t temporary_obstacles_idx;
    typename UnorderedIndexSet::const_iterator occupied_idx_iterator;
};

namespace internal {

// function to determine if segment is valid or not.
template<typename T, unsigned int DIM>
bool segmentValid(
    const OccupancyGrid<T, DIM>& grid,
    const AlignedBox<T, DIM>& workspace,
    const AlignedBox<T, DIM>& from_box,
    const AlignedBox<T, DIM>& to_box
) {
    using AlignedBox = AlignedBox<T, DIM>;

    AlignedBox to_box_copy = to_box;
    to_box_copy.extend(from_box);

    // returns true if workspace contains both to box and from box regions, and
    // to box and from box must not contain any obstacles or temporary obstacles
    return workspace.contains(to_box_copy) && !grid.isOccupied(to_box_copy);
}

template<typename T, unsigned int DIM>
bool segmentValid(
    const OccupancyGrid<T, DIM>& grid,
    const AlignedBox<T, DIM>& workspace,
    const AlignedBox<T, DIM>& from_box,
    const VectorDIM<T, DIM>& to,
    std::shared_ptr<CollisionShape<T, DIM>> collision_shape
) {
    using AlignedBox = AlignedBox<T, DIM>;
    using OccupancyGrid = OccupancyGrid<T, DIM>;

    // get to box region using collision shape bounding box method
    AlignedBox to_box = collision_shape->boundingBox(to);

    return segmentValid<T, DIM>(
            grid, workspace, from_box, to_box);
}

template<typename T, unsigned int DIM>
bool segmentValid(
        const OccupancyGrid<T, DIM>& grid,
        const AlignedBox<T, DIM>& workspace,
        const AlignedBox<T, DIM>& from_box,
        const typename OccupancyGrid<T, DIM>::Index& to,
        std::shared_ptr<CollisionShape<T, DIM>> collision_shape
) {
    auto to_center = grid.getCenter(to);
    return segmentValid<T, DIM>(
            grid, workspace, from_box, to_center, collision_shape);
}

template<typename T, unsigned int DIM>
bool segmentValid(
    const OccupancyGrid<T, DIM>& grid,
    const AlignedBox<T, DIM>& workspace,
    const VectorDIM<T, DIM>& from,
    const VectorDIM<T, DIM>& to,
    std::shared_ptr<CollisionShape<T, DIM>> collision_shape)
{
    using AlignedBox = AlignedBox<T, DIM>;
    using OccupancyGrid = OccupancyGrid<T, DIM>;

    AlignedBox from_box = collision_shape->boundingBox(from);

    return segmentValid<T, DIM>(
            grid, workspace, from_box, to, collision_shape);
}

template<typename T, unsigned int DIM>
bool segmentValid(
    const OccupancyGrid<T, DIM>& grid,
    const AlignedBox<T, DIM>& workspace,
    const VectorDIM<T, DIM>& from,
    const typename OccupancyGrid<T, DIM>::Index& to,
    std::shared_ptr<CollisionShape<T, DIM>> collision_shape
) {
    auto to_center = grid.getCenter(to);
    return segmentValid<T, DIM>(
            grid, workspace, from, to_center, collision_shape);
}

template<typename T, unsigned int DIM>
bool segmentValid(
    const OccupancyGrid<T, DIM>& grid,
    const AlignedBox<T, DIM>& workspace,
    const typename OccupancyGrid<T, DIM>::Index& from,
    const typename OccupancyGrid<T, DIM>::Index& to,
    std::shared_ptr<CollisionShape<T, DIM>> collision_shape
) {
    auto from_center = grid.getCenter(from);
    auto to_center = grid.getCenter(to);
    return segmentValid<T, DIM>(
            grid, workspace, from_center, to_center, collision_shape);
}

}

} // namespace rlss

#endif // RLSS_OCCUPANCY_GRID_HPP


#include "modulo.hpp"
#include "ast.hpp"
#include "parsers.hpp"
#include "types.hpp"
#include "utility/itoa.hpp"
#include <boost/variant.hpp>
#include <cmath>
#include <stdexcept>
#include <algorithm>
namespace HT
{
    void modulo(PASTNode astnode, ParsersHelper& ph)
    {
        auto myParserHelper(ph);
        if (astnode->ch.size()!=3)
          throw std::runtime_error("modulo should have exact 2 parameters");
        auto & thirdCh = *astnode->ch.rbegin();
        auto & secondCh = *(++astnode->ch.begin());
        ph.parse(secondCh);
        ph.parse(thirdCh);
        if (secondCh->token.tokenType!=Complex || !boost::get<ComplexType>(secondCh->token.info).isInt())
          throw std::runtime_error("The arguments of modulo should be integer");

        if (thirdCh->token.tokenType!=Complex || !boost::get<ComplexType>(thirdCh->token.info).isInt())
          throw std::runtime_error("The arguments of modulo should be integer");

        astnode->type = Simple;
        astnode->token.tokenType = Complex;
        auto cast =boost::get<ComplexType>(secondCh->token.info);
        auto third = boost::get<ComplexType>(thirdCh->token.info);
        if (cast.toInt().isZero()) astnode->token.info = ComplexType(0);
        else
        {
            astnode->token.info = ComplexType(cast.toInt()  \
                        %
                        third.toInt()) ;
            if (!cast.exact() || !third.exact())
              astnode->token.info = boost::get<ComplexType>(astnode->token.info).toinexact();

            if (!cast.toInt().getSign() && !third.toInt().getSign())
              astnode->token.info = - boost::get<ComplexType>(astnode->token.info);
            else
              if (cast.toInt().getSign() ^ third.toInt().getSign())
              {
                  auto rat = boost::get<ComplexType>(astnode->token.info).toInt();
                  if (!rat.isZero())
                  {
                  rat -= third.toInt().setSign(true);
                  if (third.toInt().getSign())
                    astnode->token.info = ComplexType(-rat);
                  else
                    astnode->token.info = ComplexType(rat);
                  }
              }
        }
        astnode->type = Simple;
        astnode->token.tokenType = Complex;
        astnode->remove();
    }
}


/*********************************************************************************
 *  OKVIS - Open Keyframe-based Visual-Inertial SLAM
 *  Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich
 *
 *  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 Autonomous Systems Lab / ETH Zurich 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.
 *
 *  Created on: Feb 3, 2015
 *      Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
 *********************************************************************************/

/**
 * @file implementation/RadialTangentialDistortion.hpp
 * @brief Header implementation file for the RadialTangentialDistortion class.
 * @author Stefan Leutenegger
 */

#include <Eigen/LU>
#include <iostream>

/// \brief vio Main namespace of this package.
namespace vio {
/// \brief cameras Namespace for camera-related functionality.
namespace cameras {

// The default constructor with all zero ki
RadialTangentialDistortion::RadialTangentialDistortion()
    : k1_(0.0), k2_(0.0), p1_(0.0), p2_(0.0) {
  parameters_.setZero();
}

// Constructor initialising ki
RadialTangentialDistortion::RadialTangentialDistortion(float k1, float k2,
                                                       float p1, float p2) {
  parameters_[0] = k1;
  parameters_[1] = k2;
  parameters_[2] = p1;
  parameters_[3] = p2;
  k1_ = k1;
  k2_ = k2;
  p1_ = p1;
  p2_ = p2;
}

bool RadialTangentialDistortion::setParameters(
    const Eigen::VectorXd& parameters) {
  if (parameters.cols() != NumDistortionIntrinsics) {
    return false;
  }
  parameters_ = parameters.cast<float>();
  k1_ = parameters[0];
  k2_ = parameters[1];
  p1_ = parameters[2];
  p2_ = parameters[3];
  return true;
}

bool RadialTangentialDistortion::distort(
    const Eigen::Vector2d& pointUndistorted,
    Eigen::Vector2d* pointDistorted) const {
  // just compute the distorted point
  const float u0 = pointUndistorted[0];
  const float u1 = pointUndistorted[1];
  const float mx_u = u0 * u0;
  const float my_u = u1 * u1;
  const float mxy_u = u0 * u1;
  const float rho_u = mx_u + my_u;
  const float rad_dist_u = k1_ * rho_u + k2_ * rho_u * rho_u;
  (*pointDistorted)[0] =
      u0 + u0 * rad_dist_u + 2.0 * p1_ * mxy_u + p2_ * (rho_u + 2.0 * mx_u);
  (*pointDistorted)[1] =
      u1 + u1 * rad_dist_u + 2.0 * p2_ * mxy_u + p1_ * (rho_u + 2.0 * my_u);
  return true;
}
bool RadialTangentialDistortion::distort(
    const Eigen::Vector2f& pointUndistorted,
    Eigen::Vector2f* pointDistorted) const {
  // just compute the distorted point
  const float u0 = pointUndistorted[0];
  const float u1 = pointUndistorted[1];
  const float mx_u = u0 * u0;
  const float my_u = u1 * u1;
  const float mxy_u = u0 * u1;
  const float rho_u = mx_u + my_u;
  const float rad_dist_u = k1_ * rho_u + k2_ * rho_u * rho_u;
  (*pointDistorted)[0] =
      u0 + u0 * rad_dist_u + 2.0 * p1_ * mxy_u + p2_ * (rho_u + 2.0 * mx_u);
  (*pointDistorted)[1] =
      u1 + u1 * rad_dist_u + 2.0 * p2_ * mxy_u + p1_ * (rho_u + 2.0 * my_u);
  return true;
}

bool RadialTangentialDistortion::distort(
    const Eigen::Vector2d& pointUndistorted, Eigen::Vector2d* pointDistorted,
    Eigen::Matrix2d* pointJacobian, Eigen::Matrix2Xd* parameterJacobian) const {
  // first compute the distorted point
  const float u0 = pointUndistorted[0];
  const float u1 = pointUndistorted[1];
  const float mx_u = u0 * u0;
  const float my_u = u1 * u1;
  const float mxy_u = u0 * u1;
  const float rho_u = mx_u + my_u;
  const float rad_dist_u = k1_ * rho_u + k2_ * rho_u * rho_u;
  (*pointDistorted)[0] =
      u0 + u0 * rad_dist_u + 2.0 * p1_ * mxy_u + p2_ * (rho_u + 2.0 * mx_u);
  (*pointDistorted)[1] =
      u1 + u1 * rad_dist_u + 2.0 * p2_ * mxy_u + p1_ * (rho_u + 2.0 * my_u);

  // next the Jacobian w.r.t. changes on the undistorted point
  Eigen::Matrix2d& J = *pointJacobian;
  J(0, 0) = 1 + rad_dist_u + k1_ * 2.0 * mx_u + k2_ * rho_u * 4 * mx_u +
            2.0 * p1_ * u1 + 6 * p2_ * u0;
  J(1, 0) = k1_ * 2.0 * u0 * u1 + k2_ * 4 * rho_u * u0 * u1 + p1_ * 2.0 * u0 +
            2.0 * p2_ * u1;
  J(0, 1) = J(1, 0);
  J(1, 1) = 1 + rad_dist_u + k1_ * 2.0 * my_u + k2_ * rho_u * 4 * my_u +
            6 * p1_ * u1 + 2.0 * p2_ * u0;

  if (parameterJacobian) {
    // the Jacobian w.r.t. intrinsics parameters
    Eigen::Matrix2Xd& J2 = *parameterJacobian;
    J2.resize(2, NumDistortionIntrinsics);
    const float r2 = rho_u;
    const float r4 = r2 * r2;

    //[ u0*(u0^2 + u1^2), u0*(u0^2 + u1^2)^2,       2*u0*u1, 3*u0^2 + u1^2]
    //[ u1*(u0^2 + u1^2), u1*(u0^2 + u1^2)^2, u0^2 + 3*u1^2,       2*u0*u1]

    J2(0, 0) = u0 * r2;
    J2(0, 1) = u0 * r4;
    J2(0, 2) = 2.0 * u0 * u1;
    J2(0, 3) = r2 + 2.0 * u0 * u0;

    J2(1, 0) = u1 * r2;
    J2(1, 1) = u1 * r4;
    J2(1, 2) = r2 + 2.0 * u1 * u1;
    J2(1, 3) = 2.0 * u0 * u1;
  }
  return true;
}
bool RadialTangentialDistortion::distort(
    const Eigen::Vector2f& pointUndistorted, Eigen::Vector2f* pointDistorted,
    Eigen::Matrix2f* pointJacobian, Eigen::Matrix2Xf* parameterJacobian) const {
  // first compute the distorted point
  const float u0 = pointUndistorted[0];
  const float u1 = pointUndistorted[1];
  const float mx_u = u0 * u0;
  const float my_u = u1 * u1;
  const float mxy_u = u0 * u1;
  const float rho_u = mx_u + my_u;
  const float rad_dist_u = k1_ * rho_u + k2_ * rho_u * rho_u;
  (*pointDistorted)[0] =
      u0 + u0 * rad_dist_u + 2.0 * p1_ * mxy_u + p2_ * (rho_u + 2.0 * mx_u);
  (*pointDistorted)[1] =
      u1 + u1 * rad_dist_u + 2.0 * p2_ * mxy_u + p1_ * (rho_u + 2.0 * my_u);

  // next the Jacobian w.r.t. changes on the undistorted point
  Eigen::Matrix2f& J = *pointJacobian;
  J(0, 0) = 1 + rad_dist_u + k1_ * 2.0 * mx_u + k2_ * rho_u * 4 * mx_u +
            2.0 * p1_ * u1 + 6 * p2_ * u0;
  J(1, 0) = k1_ * 2.0 * u0 * u1 + k2_ * 4 * rho_u * u0 * u1 + p1_ * 2.0 * u0 +
            2.0 * p2_ * u1;
  J(0, 1) = J(1, 0);
  J(1, 1) = 1 + rad_dist_u + k1_ * 2.0 * my_u + k2_ * rho_u * 4 * my_u +
            6 * p1_ * u1 + 2.0 * p2_ * u0;

  if (parameterJacobian) {
    // the Jacobian w.r.t. intrinsics parameters
    Eigen::Matrix2Xf& J2 = *parameterJacobian;
    J2.resize(2, NumDistortionIntrinsics);
    const float r2 = rho_u;
    const float r4 = r2 * r2;

    //[ u0*(u0^2 + u1^2), u0*(u0^2 + u1^2)^2,       2*u0*u1, 3*u0^2 + u1^2]
    //[ u1*(u0^2 + u1^2), u1*(u0^2 + u1^2)^2, u0^2 + 3*u1^2,       2*u0*u1]

    J2(0, 0) = u0 * r2;
    J2(0, 1) = u0 * r4;
    J2(0, 2) = 2.0 * u0 * u1;
    J2(0, 3) = r2 + 2.0 * u0 * u0;

    J2(1, 0) = u1 * r2;
    J2(1, 1) = u1 * r4;
    J2(1, 2) = r2 + 2.0 * u1 * u1;
    J2(1, 3) = 2.0 * u0 * u1;
  }
  return true;
}

bool RadialTangentialDistortion::distortWithExternalParameters(
    const Eigen::Vector2d& pointUndistorted, const Eigen::VectorXd& parameters,
    Eigen::Vector2d* pointDistorted, Eigen::Matrix2d* pointJacobian,
    Eigen::Matrix2Xd* parameterJacobian) const {
  const float k1 = parameters[0];
  const float k2 = parameters[1];
  const float p1 = parameters[2];
  const float p2 = parameters[3];
  // first compute the distorted point
  const float u0 = pointUndistorted[0];
  const float u1 = pointUndistorted[1];
  const float mx_u = u0 * u0;
  const float my_u = u1 * u1;
  const float mxy_u = u0 * u1;
  const float rho_u = mx_u + my_u;
  const float rad_dist_u = k1 * rho_u + k2 * rho_u * rho_u;
  (*pointDistorted)[0] =
      u0 + u0 * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho_u + 2.0 * mx_u);
  (*pointDistorted)[1] =
      u1 + u1 * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho_u + 2.0 * my_u);

  // next the Jacobian w.r.t. changes on the undistorted point
  Eigen::Matrix2d& J = *pointJacobian;
  J(0, 0) = 1 + rad_dist_u + k1 * 2.0 * mx_u + k2 * rho_u * 4 * mx_u +
            2.0 * p1 * u1 + 6 * p2 * u0;
  J(1, 0) = k1 * 2.0 * u0 * u1 + k2 * 4 * rho_u * u0 * u1 + p1 * 2.0 * u0 +
            2.0 * p2 * u1;
  J(0, 1) = J(1, 0);
  J(1, 1) = 1 + rad_dist_u + k1 * 2.0 * my_u + k2 * rho_u * 4 * my_u +
            6 * p1 * u1 + 2.0 * p2 * u0;

  if (parameterJacobian) {
    // the Jacobian w.r.t. intrinsics parameters
    Eigen::Matrix2Xd& J2 = *parameterJacobian;
    J2.resize(2, NumDistortionIntrinsics);
    const float r2 = rho_u;
    const float r4 = r2 * r2;

    //[ u0*(u0^2 + u1^2), u0*(u0^2 + u1^2)^2,       2*u0*u1, 3*u0^2 + u1^2]
    //[ u1*(u0^2 + u1^2), u1*(u0^2 + u1^2)^2, u0^2 + 3*u1^2,       2*u0*u1]

    J2(0, 0) = u0 * r2;
    J2(0, 1) = u0 * r4;
    J2(0, 2) = 2.0 * u0 * u1;
    J2(0, 3) = r2 + 2.0 * u0 * u0;

    J2(1, 0) = u1 * r2;
    J2(1, 1) = u1 * r4;
    J2(1, 2) = r2 + 2.0 * u1 * u1;
    J2(1, 3) = 2.0 * u0 * u1;
  }
  return true;
}
bool RadialTangentialDistortion::undistort(
    const Eigen::Vector2d& pointDistorted,
    Eigen::Vector2d* pointUndistorted) const {
  // this is expensive: we solve with Gauss-Newton...
  Eigen::Vector2d x_bar = pointDistorted;  // initialise at distorted point
  const int n = 5;                         // just 5 iterations max.
  Eigen::Matrix2d E;                       // error Jacobian

  bool success = false;
  for (int i = 0; i < n; i++) {
    Eigen::Vector2d x_tmp;

    distort(x_bar, &x_tmp, &E);

    Eigen::Vector2d e(pointDistorted - x_tmp);
    Eigen::Matrix2d E2 = (E.transpose() * E);
    Eigen::Vector2d du = E2.inverse() * E.transpose() * e;

    x_bar += du;

    const double chi2 = e.dot(e);
    if (chi2 < 1e-4) {
      success = true;
    }
    if (chi2 < 1e-15) {
      success = true;
      break;
    }
  }
  *pointUndistorted = x_bar;

  if (!success) {
    // std::cout<<(E.transpose() * E)<<std::endl;
  }
  return success;
}

bool RadialTangentialDistortion::undistort(
    const Eigen::Vector2f& pointDistorted,
    Eigen::Vector2f* pointUndistorted) const {
  // this is expensive: we solve with Gauss-Newton...
  Eigen::Vector2f x_bar = pointDistorted;  // initialise at distorted point
  const int n = 5;                         // just 5 iterations max.
  Eigen::Matrix2f E;                       // error Jacobian

  bool success = false;
  for (int i = 0; i < n; i++) {
    Eigen::Vector2f x_tmp;

    distort(x_bar, &x_tmp, &E);

    Eigen::Vector2f e(pointDistorted - x_tmp);
    Eigen::Matrix2f E2 = (E.transpose() * E);
    Eigen::Vector2f du = E2.inverse() * E.transpose() * e;

    x_bar += du;

    const double chi2 = e.dot(e);
    if (chi2 < 1e-4) {
      success = true;
    }
    if (chi2 < 1e-15) {
      success = true;
      break;
    }
  }
  *pointUndistorted = x_bar;

  if (!success) {
    // std::cout<<(E.transpose() * E)<<std::endl;
  }
  return success;
}

bool RadialTangentialDistortion::undistort(
    const Eigen::Vector2d& pointDistorted, Eigen::Vector2d* pointUndistorted,
    Eigen::Matrix2d* pointJacobian) const {
  // this is expensive: we solve with Gauss-Newton...
  Eigen::Vector2d x_bar = pointDistorted;  // initialise at distorted point
  const int n = 5;                         // just 5 iterations max.
  Eigen::Matrix2d E;                       // error Jacobian

  bool success = false;
  for (int i = 0; i < n; i++) {
    Eigen::Vector2d x_tmp;

    distort(x_bar, &x_tmp, &E);

    Eigen::Vector2d e(pointDistorted - x_tmp);
    Eigen::Vector2d dx = (E.transpose() * E).inverse() * E.transpose() * e;

    x_bar += dx;

    const double chi2 = e.dot(e);
    if (chi2 < 1e-4) {
      success = true;
    }
    if (chi2 < 1e-15) {
      success = true;
      break;
    }
  }
  *pointUndistorted = x_bar;

  // the Jacobian of the inverse map is simply the inverse Jacobian.
  *pointJacobian = E.inverse();

  return success;
}

bool RadialTangentialDistortion::undistort(
    const Eigen::Vector2f& pointDistorted, Eigen::Vector2f* pointUndistorted,
    Eigen::Matrix2f* pointJacobian) const {
  Eigen::Vector2f x_bar = pointDistorted;  // initialize at distorted point
  const int n = 5;                         // Max 5 iterations
  Eigen::Matrix2f E;                       // error Jacobian

  bool success = false;
  for (int i = 0; i < n; ++i) {
    Eigen::Vector2f x_tmp;
    distort(x_bar, &x_tmp, &E);

    Eigen::Vector2f e(pointDistorted - x_tmp);
    Eigen::Vector2f dx = (E.transpose() * E).inverse() * E.transpose() * e;

    x_bar += dx;

    const float chi2 = e.dot(e);
    if (chi2 < 1e-4) {
      success = true;
    }
    if (chi2 < 1e-8) {
      success = true;
      break;
    }
  }
  *pointUndistorted = x_bar;

  // the Jacobian of the inverse map is simply the inverse Jacobian.
  *pointJacobian = E.inverse();

  return success;
}

}  // namespace cameras
}  // namespace vio


#include <mandoline/mesh3.hpp>
#include <mtao/cmdline_parser.hpp>
#include <mtao/logging/logger.hpp>

#include <Eigen/Dense>
using namespace mandoline;

template <typename Matrix, typename Vector>
struct SparseLDLT
{
    typedef typename Matrix::Scalar Scalar;
    SparseLDLT() {}
    SparseLDLT(const Matrix & A)
    {
        // L=tril(A);
        L=A.template triangularView<Eigen::StrictlyLower>();//Don't copy the diagonal
        for(int i=0; i<L.rows(); ++i)
        {
            if(L.coeff(i,i)!=0)
                L.coeffRef(i,i)=0;
        }
        Dinv=D=A.diagonal();

        // for k=1:size(L,2)
        for(int k=0; k<A.rows(); ++k)//k is the column that we're infecting the remaining columns with
        {//L(:,k)

            //Solidify the current column values
            //==================================
            if(D(k)==0) continue;
            if(Dinv(k)<0.25*D(k))//If D has shrunk too much since it started
                Dinv(k)=1/D(k);
            else
                Dinv(k)=1/Dinv(k);
            L.innerVector(k) *= Dinv(k);

            //Add k terms to all of the following columns
            //===========================================
            for(typename Matrix::InnerIterator it(L,k); it; ++it)// -L(i,k)*D(k)*L(j,k)
            {
                int j = it.row();//j>k
                if(j<=k) continue;
                Scalar missing=0;
                Scalar multiplier=it.value();//L(j,k)*D(k)

                typename Matrix::InnerIterator k_it(L,k);
                typename Matrix::InnerIterator j_it(L,j);
                //move down teh column of L(:,k) to collect missing elements in the match with A(:,j)
                //i=k_it.row()

                while (k_it && k_it.row()<j){//L(i,k)
                    while(j_it)//L(i,j) occasionally
                    {
                        if(j_it.row() < k_it.row())
                            ++j_it;
                        else if(j_it.row() == k_it.row())//L(i,k) are L(i,j) are nonzero
                            break;
                        else
                        {
                            missing += k_it.value();//L(i,k) will fill something not in L(i,j)
                            break;
                        }
                    }
                    ++k_it;
                }

                if(k_it && j_it.row() == j)
                {
                    Dinv(j) -= it.value() * multiplier;
                }

                typename Matrix::InnerIterator j_it2(L,j);
                while(k_it && j_it2)
                {
                    if(j_it2.row() < k_it.row())
                        ++j_it2;
                    else if(j_it2.row() == k_it.row())//L(i,k) and L(i,j) are both nonzero, -=L(i,k)*L(j,k)*D(k)
                    {
                        j_it2.valueRef() -= multiplier * k_it.value() ;//k_it.value()=L(i,k)
                        ++j_it2;
                        ++k_it;
                    }
                    else
                    {
                        missing+=k_it.value();
                        ++k_it;
                    }
                }

                while(k_it)
                {
                    missing+=k_it.value();
                    ++k_it;
                }
                Dinv(j)-=0.97*missing*multiplier;
            }
        }

        /*
           std::cout << L << std::endl;
           */

    }
    void solve(const Vector & b, Vector & x)
    {
        x = L.template triangularView<Eigen::UnitLower>().solve(b);
        x.noalias() = x.cwiseProduct(Dinv);//safe beacuse it's a dot
        L.transpose().template triangularView<Eigen::UnitUpper>().solveInPlace(x);
    }
    Matrix getA()
    {
        Matrix
                A = L.template triangularView<Eigen::UnitLower>();
        A = A * D.asDiagonal();
        A = A * L.template triangularView<Eigen::UnitLower>().transpose();

        return A;
    }
private:
    Matrix L;
    Vector D,Dinv;
};

template <typename MatrixType, typename VectorType, typename Preconditioner>
struct PreconditionedConjugateGradient
{
    typedef MatrixType Matrix;
    typedef VectorType Vector;
    typedef typename Vector::Scalar Scalar;
    PreconditionedConjugateGradient(const Matrix & A): A(A)
    {
        precond = Preconditioner(A);
    }
    auto solve(const Vector& b) {
        auto x = b.eval();
        x.setZero();
        Vector r = b-A*x;
        Vector z;
        precond.solve(r,z);
        Vector p = z;
        Vector Ap = A*p;
        Scalar rdz = r.dot(z);
        Scalar alpha, beta;
        auto error = [&]() { return r.template lpNorm<Eigen::Infinity>(); };

        uint iterations = 0;
        while(++iterations < 10 &&
                error() > epsilon)
        {
            alpha = (rdz)/(p.dot(Ap));
            x+=alpha * p;
            r-=alpha * Ap;
            precond.solve(r,z);
            beta=1/rdz;
            rdz = r.dot(z);
            beta*=rdz;
            p=z+beta*p;
            Ap=A*p;
        }
        return x;
    }
private:
    const Matrix & A;
    Preconditioner precond;

    Scalar epsilon = 1e-5;

};

template <typename Matrix, typename Vector>
auto ldlt_pcg_solver(const Matrix & A, const Vector& b)
{
    return PreconditionedConjugateGradient<Matrix,Vector, SparseLDLT<Matrix, mtao::Vector<typename Vector::Scalar, Vector::RowsAtCompileTime>>>(A);
    //auto solver = IterativeLinearSolver<PreconditionedConjugateGradientCapsule<Matrix,Vector, Preconditioner> >(A.rows(), 1e-5);
}
template <typename Matrix, typename Vector>
auto ldlt_pcg_solve(const Matrix & A, const Vector & b)
{
    auto solver = ldlt_pcg_solver(A,b);
    //auto solver = IterativeLinearSolver<PreconditionedConjugateGradientCapsule<Matrix,Vector, Preconditioner> >(A.rows(), 1e-5);
    return solver.solve(b);
}

using namespace mtao::logging;

double max_eigenvalue(const Eigen::SparseMatrix<double>& A) {
    mtao::VecXd xold = mtao::VecXd::Random(A.rows()).normalized();
    mtao::VecXd x = (A * xold).normalized();
    while((x - xold).norm() > 1e-5) {
        xold = x;
        x = (A * xold).normalized();
        std::cout << "Max:" << (x-xold).norm() << std::endl;
    }
    return (A*x).norm();
}

double min_eigenvalue(const Eigen::SparseMatrix<double>& A) {
    mtao::VecXd xold = mtao::VecXd::Random(A.rows()).normalized();
    std::cout << "Preparing solver" << std::endl;
    auto solver = ldlt_pcg_solver(A,xold);
    std::cout << "Done" << std::endl;

    mtao::VecXd x = (solver.solve(xold)).normalized();
    while((x - xold).norm() > 1e-5) {
        xold = x;
        x = (solver.solve(xold)).normalized();
        std::cout << "Min:" << (x-xold).norm() << std::endl;
    }
    return (A*x).norm();
}

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

    auto&& log = make_logger("profiler",mtao::logging::Level::All);
    mtao::CommandLineParser clp;
    clp.parse(argc, argv);

    if(clp.args().size() < 1) {
        fatal() << "No input mesh filename!";
        return {};
    }

    std::string input_cutmesh = clp.arg(0);

    CutCellMesh<3> ccm = CutCellMesh<3>::from_proto(input_cutmesh);

    auto B = ccm.boundary();
    mtao::VecXd DM = ccm.mesh_face_mask();
    B = DM.asDiagonal() * B;
    DM = mtao::VecXd::Ones(ccm.cell_size());
    for(auto&& [i,c]: mtao::iterator::enumerate(ccm.cells())) {
    }
    B = B * DM.asDiagonal();

    mtao::VecXd DH2 = ccm.dual_hodge2();
    Eigen::SparseMatrix<double> L = B.transpose() * DH2.asDiagonal() * B;

    double M = max_eigenvalue(L);
    double m = min_eigenvalue(L);

    std::cout << m << " < " << M  << std::endl;
    std::cout << "condition number: " << (M / m) << std::endl;
}


#ifndef __COUNTER_HPP__
#define __COUNTER_HPP__

#include <fstream>
#include <algorithm>
// #include <boost/unordered_map.hpp>
#include <unordered_map>
using namespace std;
//using namespace boost;

template <class T>
bool asc_sort_pair(T& pair1, T& pair2)
{
    return pair1.first > pair2.first;
}

template <class T>
class Histogram {
public:
    void Add(const T& val)
    {
        typename unordered_map<T, int>::iterator it =
                counts__.find(val);
        if (it == counts__.end()) {
            counts__.insert(pair<T, int>(val, 1));
        }
        else {
            it->second += 1;
        }
        return;
    }

    void GetTopK(int k, vector<pair<T, int> >& values)
    {
        if (counts__.size() <= k) {
            // for (auto t:counts__) {
            for (int i = 0, N = values.size(); i < N; i++) {
                const pair<T, int>& t = values[i];
                values.push_back(pair<T, int>(t.first, t.second));
            }
            return;
        }

        vector<pair<int, T> > scratch(counts__.size());
        // for (auto entry:counts__) {
        for (typename unordered_map<T, int>::const_iterator entry = counts__.begin();
                entry != counts__.end();
                entry++) {
            scratch.push_back(pair<int, T>(entry->second,
                    entry->first));
        }

        partial_sort(scratch.begin(), scratch.begin() + k,
                scratch.end(), asc_sort_pair<pair<int, T> >);

        for (int i = 0; i < k; i++) {
            pair<int, T> tmp = scratch[i];
            values.push_back(pair<T, int>(tmp.second, tmp.first));
        }
        return;
    }

    void Size()
    {
        return counts__.size();
    }

private:
    unordered_map<T, int> counts__;
};

/**
 * Use KeyGenerator if you need to map values to unique ids and also need
 * to retrive the value using the id.
 * If you always need a value to id mapping, use IdGenerator.
 */
template <class T>
class KeyGenerator {
public:
    typedef typename std::map<T, int>::iterator IdMapIterator;
    KeyGenerator() : key_count__(0) {}

    bool Exists(const T& entry)
    {
        IdMapIterator it = id_map__.find(entry);
        return (it != id_map__.end());
    }

    int GetKey(const T& entry)
    {
        int id;
        IdMapIterator it = id_map__.find(entry);
        if (it == id_map__.end()) {
            id = id_map__.size();
            id_map__.insert(std::pair<T, int>(entry, id));
            value_map__.insert(std::pair<int, T>(id, entry));
        }
        else {
            id = it->second;
        }
        return id;
    }

    void GetValue(int id, T& val) const
    {
        typedef typename std::map<int, T>::const_iterator ValueMapIterator;
        ValueMapIterator it = value_map__.find(id);
        val = it->second;
        return;
    }

    int size() const
    {
        return id_map__.size();
    }

private:
    std::map<T, int> id_map__;
    std::map<int, T> value_map__;
    int key_count__;
};

template <class T>
class IdGenerator {
public:
    typedef typename unordered_map<T, int>::iterator iterator;
    typedef typename unordered_map<T, int>::const_iterator const_iterator;
    IdGenerator() {}

    int GetKey(const T& entry)
    {
        int id;
        iterator it = id_map__.find(entry);
        if (it == id_map__.end()) {
            id = id_map__.size();
            id_map__.insert(std::pair<T, int>(entry, id));
        }
        else {
            id = it->second;
        }
        return id;
    }

    int size() const
    {
        return id_map__.size();
    }

    const_iterator begin() const
    {
        return id_map__.begin();
    }

    const_iterator end() const
    {
        return id_map__.end();
    }

    void load(string path)
    {
        ifstream ifs(path.c_str(), ios_base::in);
        if (ifs.is_open() == false) {
            cout << "Failed to open=" << path << endl;
            _exit(1);
            return;
        }

        T key;
        int value, num_pairs(0);
        std::string line;

        while (ifs.good()) {
            getline(ifs, line);
            if (line.size() == 0) {
                continue;
            }
            if (line[0] == '#') {
                continue;
            }
            vector<std::string> tokens;
            std::stringstream strm(line);

            strm >> key >> value;
            id_map__.insert(pair<T, int>(key, value));
            num_pairs++;
        }
        ifs.close();
        cout << "Loaded IdGenerator with " << num_pairs
             << " entries from " << path << endl;
        return;
    }

    void save(string out_path)
    {
        ofstream ofs(out_path.c_str(), ios_base::out);
        for (const_iterator it = id_map__.begin();
                it != id_map__.end(); it++) {
            ofs << it->first << " " << it->second << endl;
        }
        ofs.close();
        return;
    }

private:
    // map<T, int> id_map__;
    unordered_map<T, int> id_map__;
};

template <class T>
class Counter {
public:
    typedef typename unordered_map<T, int>::iterator iterator;
    typedef typename unordered_map<T, int>::const_iterator const_iterator;
    int Add(const T& entry, int count = 1)
    {
        // typedef typename map<T, int>::iterator CounterIterator;
        iterator it = count_table__.find(entry);
        if (it == count_table__.end()) {
            count_table__.insert(pair<T, int>(entry, count));
        } else {
            count = it->second + 1;
            it->second = count;
        }
        return count;
    }

    int Count(const T& entry) const
    {
        // typedef typename map<int, T>::const_iterator CounterIterator;
        const_iterator it = count_table__.find(entry);
        return it->second;
    }

    int size() { return count_table__.size(); }

    void reset()
    {
        count_table__.clear();
    }

    const_iterator begin() const
    {
        return count_table__.begin();
    }

    const_iterator end() const
    {
        return count_table__.end();
    }

private:
    unordered_map<T, int> count_table__;
};

#endif