// Boost.Geometry Index
//
// R-tree R*-tree split algorithm implementation
//
// Copyright (c) 2011-2014 Adam Wulkiewicz, Lodz, Poland.
//
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_INDEX_DETAIL_RTREE_RSTAR_REDISTRIBUTE_ELEMENTS_HPP
#define BOOST_GEOMETRY_INDEX_DETAIL_RTREE_RSTAR_REDISTRIBUTE_ELEMENTS_HPP

#include <boost/geometry/index/detail/algorithms/intersection_content.hpp>
#include <boost/geometry/index/detail/algorithms/union_content.hpp>
#include <boost/geometry/index/detail/algorithms/margin.hpp>

#include <boost/geometry/index/detail/bounded_view.hpp>

#include <boost/geometry/index/detail/rtree/node/node.hpp>
#include <boost/geometry/index/detail/rtree/visitors/insert.hpp>
#include <boost/geometry/index/detail/rtree/visitors/is_leaf.hpp>

namespace geofeatures_boost {} namespace boost = geofeatures_boost; namespace geofeatures_boost { namespace geometry { namespace index {

namespace detail { namespace rtree {

namespace rstar {

template <typename Element, typename Translator, typename Tag, size_t Corner, size_t AxisIndex>
class element_axis_corner_less
{
    typedef typename rtree::element_indexable_type<Element, Translator>::type indexable_type;
    typedef typename geometry::point_type<indexable_type>::type point_type;
    typedef geometry::model::box<point_type> bounds_type;
    typedef index::detail::bounded_view<indexable_type, bounds_type> bounded_view_type;

public:
    element_axis_corner_less(Translator const& tr)
        : m_tr(tr)
    {}

    bool operator()(Element const& e1, Element const& e2) const
    {
        bounded_view_type bounded_ind1(rtree::element_indexable(e1, m_tr));
        bounded_view_type bounded_ind2(rtree::element_indexable(e2, m_tr));

        return geometry::get<Corner, AxisIndex>(bounded_ind1)
            < geometry::get<Corner, AxisIndex>(bounded_ind2);
    }

private:
    Translator const& m_tr;
};

template <typename Element, typename Translator, size_t Corner, size_t AxisIndex>
class element_axis_corner_less<Element, Translator, box_tag, Corner, AxisIndex>
{
public:
    element_axis_corner_less(Translator const& tr)
        : m_tr(tr)
    {}

    bool operator()(Element const& e1, Element const& e2) const
    {
        return geometry::get<Corner, AxisIndex>(rtree::element_indexable(e1, m_tr))
            < geometry::get<Corner, AxisIndex>(rtree::element_indexable(e2, m_tr));
    }

private:
    Translator const& m_tr;
};

template <typename Element, typename Translator, size_t Corner, size_t AxisIndex>
class element_axis_corner_less<Element, Translator, point_tag, Corner, AxisIndex>
{
public:
    element_axis_corner_less(Translator const& tr)
        : m_tr(tr)
    {}

    bool operator()(Element const& e1, Element const& e2) const
    {
        return geometry::get<AxisIndex>(rtree::element_indexable(e1, m_tr))
            < geometry::get<AxisIndex>(rtree::element_indexable(e2, m_tr));
    }

private:
    Translator const& m_tr;
};

template <typename Box, size_t Corner, size_t AxisIndex>
struct choose_split_axis_and_index_for_corner
{
    typedef typename index::detail::default_margin_result<Box>::type margin_type;
    typedef typename index::detail::default_content_result<Box>::type content_type;

    template <typename Elements, typename Parameters, typename Translator>
    static inline void apply(Elements const& elements,
                             size_t & choosen_index,
                             margin_type & sum_of_margins,
                             content_type & smallest_overlap,
                             content_type & smallest_content,
                             Parameters const& parameters,
                             Translator const& translator)
    {
        typedef typename Elements::value_type element_type;
        typedef typename rtree::element_indexable_type<element_type, Translator>::type indexable_type;
        typedef typename tag<indexable_type>::type indexable_tag;

        BOOST_GEOMETRY_INDEX_ASSERT(elements.size() == parameters.get_max_elements() + 1, "wrong number of elements");

        // copy elements
        Elements elements_copy(elements);                                                                       // MAY THROW, STRONG (alloc, copy)

        size_t const index_first = parameters.get_min_elements();
        size_t const index_last = parameters.get_max_elements() - parameters.get_min_elements() + 2;

        // sort elements
        element_axis_corner_less<element_type, Translator, indexable_tag, Corner, AxisIndex> elements_less(translator);
        std::sort(elements_copy.begin(), elements_copy.end(), elements_less);                                   // MAY THROW, BASIC (copy)
//        {
//            typename Elements::iterator f = elements_copy.begin() + index_first;
//            typename Elements::iterator l = elements_copy.begin() + index_last;
//            std::nth_element(elements_copy.begin(), f, elements_copy.end(), elements_less);                   // MAY THROW, BASIC (copy)
//            std::nth_element(f, l, elements_copy.end(), elements_less);                                       // MAY THROW, BASIC (copy)
//            std::sort(f, l, elements_less);                                                                   // MAY THROW, BASIC (copy)
//        }

        // init outputs
        choosen_index = index_first;
        sum_of_margins = 0;
        smallest_overlap = (std::numeric_limits<content_type>::max)();
        smallest_content = (std::numeric_limits<content_type>::max)();

        // calculate sum of margins for all distributions
        for ( size_t i = index_first ; i < index_last ; ++i )
        {
            // TODO - awulkiew: may be optimized - box of group 1 may be initialized with
            // box of min_elems number of elements and expanded for each iteration by another element

            Box box1 = rtree::elements_box<Box>(elements_copy.begin(), elements_copy.begin() + i, translator);
            Box box2 = rtree::elements_box<Box>(elements_copy.begin() + i, elements_copy.end(), translator);

            sum_of_margins += index::detail::comparable_margin(box1) + index::detail::comparable_margin(box2);

            content_type ovl = index::detail::intersection_content(box1, box2);
            content_type con = index::detail::content(box1) + index::detail::content(box2);

            // TODO - shouldn't here be < instead of <= ?
            if ( ovl < smallest_overlap || (ovl == smallest_overlap && con <= smallest_content) )
            {
                choosen_index = i;
                smallest_overlap = ovl;
                smallest_content = con;
            }
        }

        ::geofeatures_boost::ignore_unused_variable_warning(parameters);
    }
};

//template <typename Box, size_t AxisIndex, typename ElementIndexableTag>
//struct choose_split_axis_and_index_for_axis
//{
//    BOOST_MPL_ASSERT_MSG(false, NOT_IMPLEMENTED_FOR_THIS_TAG, (ElementIndexableTag));
//};

template <typename Box, size_t AxisIndex, typename ElementIndexableTag>
struct choose_split_axis_and_index_for_axis
{
    typedef typename index::detail::default_margin_result<Box>::type margin_type;
    typedef typename index::detail::default_content_result<Box>::type content_type;

    template <typename Elements, typename Parameters, typename Translator>
    static inline void apply(Elements const& elements,
                             size_t & choosen_corner,
                             size_t & choosen_index,
                             margin_type & sum_of_margins,
                             content_type & smallest_overlap,
                             content_type & smallest_content,
                             Parameters const& parameters,
                             Translator const& translator)
    {
        size_t index1 = 0;
        margin_type som1 = 0;
        content_type ovl1 = (std::numeric_limits<content_type>::max)();
        content_type con1 = (std::numeric_limits<content_type>::max)();

        choose_split_axis_and_index_for_corner<Box, min_corner, AxisIndex>
            ::apply(elements, index1,
                    som1, ovl1, con1,
                    parameters, translator);                                                                // MAY THROW, STRONG

        size_t index2 = 0;
        margin_type som2 = 0;
        content_type ovl2 = (std::numeric_limits<content_type>::max)();
        content_type con2 = (std::numeric_limits<content_type>::max)();

        choose_split_axis_and_index_for_corner<Box, max_corner, AxisIndex>
            ::apply(elements, index2,
                    som2, ovl2, con2,
                    parameters, translator);                                                                // MAY THROW, STRONG

        sum_of_margins = som1 + som2;

        if ( ovl1 < ovl2 || (ovl1 == ovl2 && con1 <= con2) )
        {
            choosen_corner = min_corner;
            choosen_index = index1;
            smallest_overlap = ovl1;
            smallest_content = con1;
        }
        else
        {
            choosen_corner = max_corner;
            choosen_index = index2;
            smallest_overlap = ovl2;
            smallest_content = con2;
        }
    }
};

template <typename Box, size_t AxisIndex>
struct choose_split_axis_and_index_for_axis<Box, AxisIndex, point_tag>
{
    typedef typename index::detail::default_margin_result<Box>::type margin_type;
    typedef typename index::detail::default_content_result<Box>::type content_type;

    template <typename Elements, typename Parameters, typename Translator>
    static inline void apply(Elements const& elements,
                             size_t & choosen_corner,
                             size_t & choosen_index,
                             margin_type & sum_of_margins,
                             content_type & smallest_overlap,
                             content_type & smallest_content,
                             Parameters const& parameters,
                             Translator const& translator)
    {
        choose_split_axis_and_index_for_corner<Box, min_corner, AxisIndex>
            ::apply(elements, choosen_index,
                    sum_of_margins, smallest_overlap, smallest_content,
                    parameters, translator);                                                                // MAY THROW, STRONG

        choosen_corner = min_corner;
    }
};

template <typename Box, size_t Dimension>
struct choose_split_axis_and_index
{
    BOOST_STATIC_ASSERT(0 < Dimension);

    typedef typename index::detail::default_margin_result<Box>::type margin_type;
    typedef typename index::detail::default_content_result<Box>::type content_type;

    template <typename Elements, typename Parameters, typename Translator>
    static inline void apply(Elements const& elements,
                             size_t & choosen_axis,
                             size_t & choosen_corner,
                             size_t & choosen_index,
                             margin_type & smallest_sum_of_margins,
                             content_type & smallest_overlap,
                             content_type & smallest_content,
                             Parameters const& parameters,
                             Translator const& translator)
    {
        typedef typename rtree::element_indexable_type<typename Elements::value_type, Translator>::type element_indexable_type;

        choose_split_axis_and_index<Box, Dimension - 1>
            ::apply(elements, choosen_axis, choosen_corner, choosen_index,
                    smallest_sum_of_margins, smallest_overlap, smallest_content,
                    parameters, translator);                                                                // MAY THROW, STRONG

        margin_type sum_of_margins = 0;

        size_t corner = min_corner;
        size_t index = 0;

        content_type overlap_val = (std::numeric_limits<content_type>::max)();
        content_type content_val = (std::numeric_limits<content_type>::max)();

        choose_split_axis_and_index_for_axis<
            Box,
            Dimension - 1,
            typename tag<element_indexable_type>::type
        >::apply(elements, corner, index, sum_of_margins, overlap_val, content_val, parameters, translator); // MAY THROW, STRONG

        if ( sum_of_margins < smallest_sum_of_margins )
        {
            choosen_axis = Dimension - 1;
            choosen_corner = corner;
            choosen_index = index;
            smallest_sum_of_margins = sum_of_margins;
            smallest_overlap = overlap_val;
            smallest_content = content_val;
        }
    }
};

template <typename Box>
struct choose_split_axis_and_index<Box, 1>
{
    typedef typename index::detail::default_margin_result<Box>::type margin_type;
    typedef typename index::detail::default_content_result<Box>::type content_type;

    template <typename Elements, typename Parameters, typename Translator>
    static inline void apply(Elements const& elements,
                             size_t & choosen_axis,
                             size_t & choosen_corner,
                             size_t & choosen_index,
                             margin_type & smallest_sum_of_margins,
                             content_type & smallest_overlap,
                             content_type & smallest_content,
                             Parameters const& parameters,
                             Translator const& translator)
    {
        typedef typename rtree::element_indexable_type<typename Elements::value_type, Translator>::type element_indexable_type;

        choosen_axis = 0;

        choose_split_axis_and_index_for_axis<
            Box,
            0,
            typename tag<element_indexable_type>::type
        >::apply(elements, choosen_corner, choosen_index, smallest_sum_of_margins, smallest_overlap, smallest_content, parameters, translator); // MAY THROW
    }
};

template <size_t Corner, size_t Dimension, size_t I = 0>
struct nth_element
{
    BOOST_STATIC_ASSERT(0 < Dimension);
    BOOST_STATIC_ASSERT(I < Dimension);

    template <typename Elements, typename Translator>
    static inline void apply(Elements & elements, const size_t axis, const size_t index, Translator const& tr)
    {
        //BOOST_GEOMETRY_INDEX_ASSERT(axis < Dimension, "unexpected axis value");

        if ( axis != I )
        {
            nth_element<Corner, Dimension, I + 1>::apply(elements, axis, index, tr);                          // MAY THROW, BASIC (copy)
        }
        else
        {
            typedef typename Elements::value_type element_type;
            typedef typename rtree::element_indexable_type<element_type, Translator>::type indexable_type;
            typedef typename tag<indexable_type>::type indexable_tag;

            element_axis_corner_less<element_type, Translator, indexable_tag, Corner, I> less(tr);
            std::nth_element(elements.begin(), elements.begin() + index, elements.end(), less);            // MAY THROW, BASIC (copy)
        }
    }
};

template <size_t Corner, size_t Dimension>
struct nth_element<Corner, Dimension, Dimension>
{
    template <typename Elements, typename Translator>
    static inline void apply(Elements & /*elements*/, const size_t /*axis*/, const size_t /*index*/, Translator const& /*tr*/)
    {}
};

} // namespace rstar

template <typename Value, typename Options, typename Translator, typename Box, typename Allocators>
struct redistribute_elements<Value, Options, Translator, Box, Allocators, rstar_tag>
{
    typedef typename rtree::node<Value, typename Options::parameters_type, Box, Allocators, typename Options::node_tag>::type node;
    typedef typename rtree::internal_node<Value, typename Options::parameters_type, Box, Allocators, typename Options::node_tag>::type internal_node;
    typedef typename rtree::leaf<Value, typename Options::parameters_type, Box, Allocators, typename Options::node_tag>::type leaf;

    typedef typename Options::parameters_type parameters_type;

    static const size_t dimension = geometry::dimension<Box>::value;

    typedef typename index::detail::default_margin_result<Box>::type margin_type;
    typedef typename index::detail::default_content_result<Box>::type content_type;

    template <typename Node>
    static inline void apply(
        Node & n,
        Node & second_node,
        Box & box1,
        Box & box2,
        parameters_type const& parameters,
        Translator const& translator,
        Allocators & allocators)
    {
        typedef typename rtree::elements_type<Node>::type elements_type;
        typedef typename elements_type::value_type element_type;

        elements_type & elements1 = rtree::elements(n);
        elements_type & elements2 = rtree::elements(second_node);

        // copy original elements - use in-memory storage (std::allocator)
        // TODO: move if noexcept
        typedef typename rtree::container_from_elements_type<elements_type, element_type>::type
            container_type;
        container_type elements_copy(elements1.begin(), elements1.end());                               // MAY THROW, STRONG
        container_type elements_backup(elements1.begin(), elements1.end());                             // MAY THROW, STRONG

        size_t split_axis = 0;
        size_t split_corner = 0;
        size_t split_index = parameters.get_min_elements();
        margin_type smallest_sum_of_margins = (std::numeric_limits<margin_type>::max)();
        content_type smallest_overlap = (std::numeric_limits<content_type>::max)();
        content_type smallest_content = (std::numeric_limits<content_type>::max)();

        // NOTE: this function internally copies passed elements
        //       why not pass mutable elements and use the same container for all axes/corners
        //       and again, the same below calling partial_sort/nth_element
        //       It would be even possible to not re-sort/find nth_element if the axis/corner
        //       was found for the last sorting - last combination of axis/corner
        rstar::choose_split_axis_and_index<Box, dimension>
            ::apply(elements_copy,
                    split_axis, split_corner, split_index,
                    smallest_sum_of_margins, smallest_overlap, smallest_content,
                    parameters, translator);                                                            // MAY THROW, STRONG

        // TODO: awulkiew - get rid of following static_casts?
        BOOST_GEOMETRY_INDEX_ASSERT(split_axis < dimension, "unexpected value");
        BOOST_GEOMETRY_INDEX_ASSERT(split_corner == static_cast<size_t>(min_corner) || split_corner == static_cast<size_t>(max_corner), "unexpected value");
        BOOST_GEOMETRY_INDEX_ASSERT(parameters.get_min_elements() <= split_index && split_index <= parameters.get_max_elements() - parameters.get_min_elements() + 1, "unexpected value");

        // TODO: consider using nth_element
        if ( split_corner == static_cast<size_t>(min_corner) )
        {
            rstar::nth_element<min_corner, dimension>
                ::apply(elements_copy, split_axis, split_index, translator);                            // MAY THROW, BASIC (copy)
        }
        else
        {
            rstar::nth_element<max_corner, dimension>
                ::apply(elements_copy, split_axis, split_index, translator);                            // MAY THROW, BASIC (copy)
        }

        BOOST_TRY
        {
            // copy elements to nodes
            elements1.assign(elements_copy.begin(), elements_copy.begin() + split_index);               // MAY THROW, BASIC
            elements2.assign(elements_copy.begin() + split_index, elements_copy.end());                 // MAY THROW, BASIC

            // calculate boxes
            box1 = rtree::elements_box<Box>(elements1.begin(), elements1.end(), translator);
            box2 = rtree::elements_box<Box>(elements2.begin(), elements2.end(), translator);
        }
        BOOST_CATCH(...)
        {
            //elements_copy.clear();
            elements1.clear();
            elements2.clear();

            rtree::destroy_elements<Value, Options, Translator, Box, Allocators>::apply(elements_backup, allocators);
            //elements_backup.clear();

            BOOST_RETHROW                                                                                // RETHROW, BASIC
        }
        BOOST_CATCH_END
    }
};

}} // namespace detail::rtree

}}} // namespace geofeatures_boost::geometry::index

#endif // BOOST_GEOMETRY_INDEX_DETAIL_RTREE_RSTAR_REDISTRIBUTE_ELEMENTS_HPP


#include <ctime>
#include <vector>
#include <string>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <stdexcept>
#include <sys/stat.h>
#include <algorithm>
#include "qm_model.hpp"
#include "properties.hpp"
#include <armadillo>
#include <unordered_map>

ConfigBlockReader QM_Model::model_reader() {
  ConfigBlockReader reader{"qmmodel"};
  reader.add_entry("model", "avoided-crossing");
  return reader;
}

QM_Model::QM_Model(FileHandle& fh,
                   const arma::uvec& in_qmids,
		   arma::mat& in_qm_crd,
		   arma::mat& in_mm_crd,
		   arma::vec& in_mm_chg,
		   const int charge,
		   const int mult,
		   const int excited_states,
                   const int min_state):
  QMInterface(in_qmids, in_qm_crd, in_mm_crd, in_mm_chg, charge, mult, excited_states, min_state)
{

  if (NQM > 1){
    throw std::logic_error("Model systems only work on the x component of a single atom!");
  }

  if (NMM != 0){
    throw std::logic_error("Model systems don't support QM/MM!");
  }

  ConfigBlockReader reader = model_reader();
  reader.parse(fh);

  {
    std::string model_in;
    reader.get_data("model", model_in);

    if (model_in == "avoided-crossing"){
      model= new AvoidedCrossing();
    }
    else if (model_in == "reflective-avoided-crossing"){
      model= new ReflectiveAvoidedCrossing();
    }
    else{
      throw std::runtime_error("Unknown Model: `" + model_in + "`!");
    }
  }
}

void QM_Model::update(void){
  QMInterface::update();
  // always take the x component of the first atom
  model->update(crd_qm(0));
}

void QM_Model::get_properties(PropMap &props){
  for (QMProperty p: props.keys()){
    switch(p){

    case QMProperty::wfoverlap:{
      arma::mat &U = props.get(QMProperty::wfoverlap);
      U = model->overlaps();
      break;
    }

    case QMProperty::nacvector:{
      arma::mat & nac = props.get(QMProperty::nacvector);
      nac.zeros();

      const arma::uvec &idx = *props.get_idx(QMProperty::nacvector);
      size_t A = idx[0]; size_t B = idx[1];

      nac(0) = model->nacs()(A,B);
      break;
    }

    case QMProperty::qmgradient:{
      arma::mat & g_qm = props.get(QMProperty::qmgradient);
      g_qm.zeros();

      arma::uword surface = 0; //assume ground state
      if (props.has_idx(QMProperty::qmgradient)){
	surface = (*props.get_idx(QMProperty::qmgradient))[0];
      }
      g_qm(0) = model->gradients()(surface);

      break;
    }

    case QMProperty::qmgradient_multi:{
      arma::cube & g_qm = props.get(QMProperty::qmgradient_multi);
      g_qm.zeros();

      arma::uvec surfaces = arma::regspace<arma::uvec>(0,excited_states); //assume all
      if (props.has_idx(QMProperty::qmgradient_multi)){ // specific states
	surfaces = *props.get_idx(QMProperty::qmgradient_multi);
      }

      if (g_qm.n_slices != surfaces.n_elem){
	throw std::range_error("Insufficient space for requested gradients!");
      }

      arma::uword i=0;
      for (arma::uword surface: surfaces){
        g_qm.slice(i)(0) = model->gradients()(surface);
        i++;
      }
      break;
    }

    case QMProperty::energies:{
      arma::vec & energies = props.get(QMProperty::energies);
      energies.zeros();

      if (props.has_idx(QMProperty::energies)){
      	energies = model->energies();
      }
      else{
      	energies = model->energies()(0);
      }
      break;
    }

    case QMProperty::mmgradient: //explicit fall-through
    case QMProperty::mmgradient_multi:
      break;

    default:
      throw std::invalid_argument("Unknown QMProperty!");
      break;
    }
  }
}

// updates overlap and, phase matches the new eigen vectors to the old ones.
void HamiltonianDynamics::update_overlap(arma::mat &evec){
  static arma::mat evec_last = evec;
  overlap = evec_last.t() * evec;

  while (arma::any(overlap.diag() < 0)){ // phase match
    arma::uword c = overlap.diag().index_min();
    evec.col(c) *= -1.0;

    overlap = evec_last.t() * evec;
  }

  evec_last = evec;
}

void HamiltonianDynamics::save_state(double x){
  std::vector<double> state;

  state.push_back( x );
  state.push_back( energy(0) );
  state.push_back( energy(1) );
  state.push_back( gradient(0) );
  state.push_back( gradient(1) );
  state.push_back( overlap(0,1) );
  state.push_back( nac(0,1) );

  std::ofstream stream;
  stream.open(state_file, std::ios::out | std::ios::app | std::ios::binary);
  if (!stream){
    throw std::runtime_error("Cannot open data file!");
  }
  else{
    for (auto e: state){
      stream << e << " ";
    }
    stream << std::endl;
  }
}

void HamiltonianDynamics::scan(double a, double b, int N){
  record = true;

  if ((b-a < 0) || (N < 0)){
    throw::std::logic_error("Can only scan on [a,b) in N>0 steps!");
  }

  const double dx = (b-a)/N;

  for (double x = a; x < b; x += dx){
    update(x);
  }

}

ReflectiveAvoidedCrossing::ReflectiveAvoidedCrossing(void){
  energy.set_size(2);
  gradient.set_size(2);
  nac.set_size(2,2);
  overlap.set_size(2,2);
}

void ReflectiveAvoidedCrossing::update(double x){
  double e2 = A*std::tanh(B*(x+7));
  double e1 = e2 + 2*A*std::tanh(B*x) + 2*A;

  double v = C * std::exp(-1.0*(x+7)*(x+7));

  arma::mat H = {{e1, v },
                 {v, -e2}};

  arma::vec eval;
  arma::mat evec;
  eig_sym(eval, evec, H);

  update_overlap(evec);

  energy = eval;

  // compute gradients
  {
    double de2 = A*B / std::pow(std::cosh(B*(x+7)),2) ;
    double de1 = A*B / std::pow(std::cosh(B*x)    ,2) + de2 ;
    double dv = -C * 2 * (x+7) *  std::exp(-1.0*(x+7)*(x+7));

    // gradient = 0.5*((de1 - de2) +
    //                 ((de2-de1)*(e2-e1) + 2*(e1*de2 + e2*de1) + 4*v*dv) /
    //                 ((e2-e1) + 2 * energy));

    double gl = 0.5 * (de1 - de2);
    double gr = 0.5 * ((de2-de1)*(e2-e1) + 2*(e1*de2 + e2*de1) + 4*v*dv) /
      std::sqrt((e2-e1)*(e2-e1) + 4*e1*e2+4*v*v);

    gradient(0) = gl - gr;
    gradient(1) = gl + gr;
  }

  // compute NAC
  nac = evec.t() * arma::diagmat(gradient) * evec;
  nac.diag().zeros();
  {
    double gap = energy(1) - energy(0);
    nac(0,1) /=  gap;
    nac(1,0) /= -gap;
  }

  if(record){
    save_state(x);
  }
}

AvoidedCrossing::AvoidedCrossing(void){
  energy.set_size(2);
  gradient.set_size(2);
  nac.set_size(2,2);
  overlap.set_size(2,2);
}

void AvoidedCrossing::update(double x){
  double xs = x+7;

  double e = A * std::tanh(B*xs);
  double v = C * std::exp(-1.0*xs*xs);

  arma::mat H = {{e,  v},
                 {v, -e}};

  arma::vec eval;
  arma::mat evec;
  eig_sym(eval, evec, H);

  update_overlap(evec);

  energy = eval;

  // compute gradients
  double f;
  {
    double sech = 1.0 / std::cosh(B*xs);
    f = e*A*B*sech*sech + -2.0*xs*v*v;
  }
  gradient = f / energy;

  // compute NAC
  nac = evec.t() * arma::diagmat(gradient) * evec;
  nac.diag().zeros();
  {
    double gap = energy(1) - energy(0);
    nac(0,1) /=  gap;
    nac(1,0) /= -gap;
  }

  // save a bunch of things
  if(record)
  {
    save_state(x);
  }
}


// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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
// HOLDER 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.

#include "solver_slam2d_linear.h"

#include <Eigen/Core>

#include "g2o/core/sparse_block_matrix.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/hyper_dijkstra.h"

#include "g2o/types/slam2d/edge_se2.h"

#include "g2o/stuff/misc.h"
#include "g2o/stuff/scoped_pointer.h"

#include "g2o/solvers/csparse/linear_solver_csparse.h"

#include "g2o/core/solver.h"

using namespace std;

namespace g2o {

  /**
   * \brief compute the initial guess of theta while travelling along the MST
   */
  class ThetaTreeAction : public HyperDijkstra::TreeAction
  {
    public:
      ThetaTreeAction(double* theta) : HyperDijkstra::TreeAction(), _thetaGuess(theta) {}
      virtual double perform(HyperGraph::Vertex* v, HyperGraph::Vertex* vParent, HyperGraph::Edge* e)
      {
        if (! vParent)
          return 0.;
        EdgeSE2* odom    = static_cast<EdgeSE2*>(e);
        VertexSE2* from  = static_cast<VertexSE2*>(vParent);
        VertexSE2* to    = static_cast<VertexSE2*>(v);
        assert(to->hessianIndex() >= 0);
        double fromTheta = from->hessianIndex() < 0 ? 0. : _thetaGuess[from->hessianIndex()];
        bool direct      = odom->vertices()[0] == from;
        if (direct)
          _thetaGuess[to->hessianIndex()] = fromTheta + odom->measurement().rotation().angle();
        else
          _thetaGuess[to->hessianIndex()] = fromTheta - odom->measurement().rotation().angle();
        return 1.;
      }
    protected:
      double* _thetaGuess;
  };

  SolverSLAM2DLinear::SolverSLAM2DLinear(std::unique_ptr<Solver> solver)
    : OptimizationAlgorithmGaussNewton(std::move(solver))
  {}

  SolverSLAM2DLinear::~SolverSLAM2DLinear()
  {}

  OptimizationAlgorithm::SolverResult SolverSLAM2DLinear::solve(int iteration, bool online)
  {
    if (iteration == 0) {
      bool status = solveOrientation();
      if (! status)
        return OptimizationAlgorithm::Fail;
    }

    return OptimizationAlgorithmGaussNewton::solve(iteration, online);
  }

  bool SolverSLAM2DLinear::solveOrientation()
  {
    assert(_optimizer->indexMapping().size() + 1 == _optimizer->vertices().size() && "Needs to operate on full graph");
    assert(_optimizer->vertex(0)->fixed() && "Graph is not fixed by vertex 0");
    VectorXD b, x; // will be used for theta and x/y update
    b.setZero(_optimizer->indexMapping().size());
    x.setZero(_optimizer->indexMapping().size());

    typedef Eigen::Matrix<double, 1, 1, Eigen::ColMajor> ScalarMatrix;

    ScopedArray<int> blockIndeces(new int[_optimizer->indexMapping().size()]);
    for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i)
      blockIndeces[i] = i+1;

    SparseBlockMatrix<ScalarMatrix> H(blockIndeces.get(), blockIndeces.get(), _optimizer->indexMapping().size(), _optimizer->indexMapping().size());

    // building the structure, diagonal for each active vertex
    for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
      OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
      int poseIdx = v->hessianIndex();
      ScalarMatrix* m = H.block(poseIdx, poseIdx, true);
      m->setZero();
    }

    HyperGraph::VertexSet fixedSet;

    // off diagonal for each edge
    for (SparseOptimizer::EdgeContainer::const_iterator it = _optimizer->activeEdges().begin(); it != _optimizer->activeEdges().end(); ++it) {
#    ifndef NDEBUG
      EdgeSE2* e = dynamic_cast<EdgeSE2*>(*it);
      assert(e && "Active edges contain non-odometry edge"); //
#    else
      EdgeSE2* e = static_cast<EdgeSE2*>(*it);
#    endif
      OptimizableGraph::Vertex* from = static_cast<OptimizableGraph::Vertex*>(e->vertices()[0]);
      OptimizableGraph::Vertex* to   = static_cast<OptimizableGraph::Vertex*>(e->vertices()[1]);

      int ind1 = from->hessianIndex();
      int ind2 = to->hessianIndex();
      if (ind1 == -1 || ind2 == -1) {
        if (ind1 == -1) fixedSet.insert(from); // collect the fixed vertices
        if (ind2 == -1) fixedSet.insert(to);
        continue;
      }

      bool transposedBlock = ind1 > ind2;
      if (transposedBlock){ // make sure, we allocate the upper triangle block
        std::swap(ind1, ind2);
      }

      ScalarMatrix* m = H.block(ind1, ind2, true);
      m->setZero();
    }

    // walk along the Minimal Spanning Tree to compute the guess for the robot orientation
    assert(fixedSet.size() == 1);
    VertexSE2* root = static_cast<VertexSE2*>(*fixedSet.begin());
    VectorXD thetaGuess;
    thetaGuess.setZero(_optimizer->indexMapping().size());
    UniformCostFunction uniformCost;
    HyperDijkstra hyperDijkstra(_optimizer);
    hyperDijkstra.shortestPaths(root, &uniformCost);

    HyperDijkstra::computeTree(hyperDijkstra.adjacencyMap());
    ThetaTreeAction thetaTreeAction(thetaGuess.data());
    HyperDijkstra::visitAdjacencyMap(hyperDijkstra.adjacencyMap(), &thetaTreeAction);

    // construct for the orientation
    for (SparseOptimizer::EdgeContainer::const_iterator it = _optimizer->activeEdges().begin(); it != _optimizer->activeEdges().end(); ++it) {
      EdgeSE2* e = static_cast<EdgeSE2*>(*it);
      VertexSE2* from = static_cast<VertexSE2*>(e->vertices()[0]);
      VertexSE2* to   = static_cast<VertexSE2*>(e->vertices()[1]);

      double omega = e->information()(2,2);

      double fromThetaGuess = from->hessianIndex() < 0 ? 0. : thetaGuess[from->hessianIndex()];
      double toThetaGuess   = to->hessianIndex() < 0 ? 0. : thetaGuess[to->hessianIndex()];
      double error          = normalize_theta(-e->measurement().rotation().angle() + toThetaGuess - fromThetaGuess);

      bool fromNotFixed = !(from->fixed());
      bool toNotFixed   = !(to->fixed());

      if (fromNotFixed || toNotFixed) {
        double omega_r = - omega * error;
        if (fromNotFixed) {
          b(from->hessianIndex()) -= omega_r;
          (*H.block(from->hessianIndex(), from->hessianIndex()))(0,0) += omega;
          if (toNotFixed) {
            if (from->hessianIndex() > to->hessianIndex())
              (*H.block(to->hessianIndex(), from->hessianIndex()))(0,0) -= omega;
            else
              (*H.block(from->hessianIndex(), to->hessianIndex()))(0,0) -= omega;
          }
        }
        if (toNotFixed ) {
          b(to->hessianIndex()) += omega_r;
          (*H.block(to->hessianIndex(), to->hessianIndex()))(0,0) += omega;
        }
      }
    }

    // solve orientation
    typedef LinearSolverCSparse<ScalarMatrix> SystemSolver;
    SystemSolver linearSystemSolver;
    linearSystemSolver.init();
    bool ok = linearSystemSolver.solve(H, x.data(), b.data());
    if (!ok) {
      cerr << __PRETTY_FUNCTION__ << "Failure while solving linear system" << endl;
      return false;
    }

    // update the orientation of the 2D poses and set translation to 0, GN shall solve that
    root->setToOrigin();
    for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
      VertexSE2* v = static_cast<VertexSE2*>(_optimizer->indexMapping()[i]);
      int poseIdx = v->hessianIndex();
      SE2 poseUpdate(0, 0, normalize_theta(thetaGuess(poseIdx) + x(poseIdx)));
      v->setEstimate(poseUpdate);
    }

    return true;
  }

} // end namespace


#include <boost/multiprecision/cpp_int.hpp>
#include <peersafe/crypto/LibSnark.h>
#include <peersafe/precompiled/PreContractRegister.h>
#include <peersafe/basics/TypeTransform.h>
#include <ripple/protocol/digest.h>
#include <ripple/protocol/CommonKey.h>
#include <ripple/basics/Blob.h>
#include <ripple/basics/Slice.h>
using namespace std;

namespace ripple {

PrecompiledRegistrar* PrecompiledRegistrar::s_this = nullptr;

PrecompiledExecutor const& PrecompiledRegistrar::executor(std::string const& _name)
{
    if (!get()->m_execs.count(_name))
        BOOST_THROW_EXCEPTION(ExecutorNotFound());
    return get()->m_execs[_name];
}

PrecompiledPricer const& PrecompiledRegistrar::pricer(std::string const& _name)
{
    if (!get()->m_pricers.count(_name))
        BOOST_THROW_EXCEPTION(PricerNotFound());
    return get()->m_pricers[_name];
}

namespace
{

// // Big-endian to/from host endian conversion functions.

// /// Converts a templated integer value to the big-endian byte-stream represented on a templated collection.
// /// The size of the collection object will be unchanged. If it is too small, it will not represent the
// /// value properly, if too big then the additional elements will be zeroed out.
// /// @a Out will typically be either std::string or bytes.
// /// @a T will typically by unsigned, u160, u256 or bigint.
// template <class T, class Out>
// inline void toBigEndian(T _val, Out& o_out)
// {
// 	static_assert(std::is_same<eth::bigint, T>::value || !std::numeric_limits<T>::is_signed, "only unsigned types or bigint supported"); //bigint does not carry sign bit on shift
// 	for (auto i = o_out.size(); i != 0; _val >>= 8, i--)
// 	{
// 		T v = _val & (T)0xff;
// 		o_out[i - 1] = (typename Out::value_type)(uint8_t)v;
// 	}
// }

// /// Converts a big-endian byte-stream represented on a templated collection to a templated integer value.
// /// @a _In will typically be either std::string or bytes.
// /// @a T will typically by unsigned, u160, u256 or bigint.
// template <class T, class _In>
// inline T fromBigEndian(_In const& _bytes)
// {
// 	T ret = (T)0;
// 	for (auto i: _bytes)
// 		ret = (T)((ret << 8) | (eth::byte)(typename std::make_unsigned<decltype(i)>::type)i);
// 	return ret;
// }

int64_t linearPricer(unsigned _base, unsigned _word, eth::bytesConstRef _in)
{
    int64_t const s = _in.size();
    int64_t const b = _base;
    int64_t const w = _word;
    return b + (s + 31) / 32 * w;
}

// ETH_REGISTER_PRECOMPILED_PRICER(ecrecover)
// (eth::bytesConstRef /*_in*/, ChainOperationParams const& /*_chainParams*/, int64_t const& /*_blockNumber*/)
// {
//     return 3000;
// }

// ETH_REGISTER_PRECOMPILED(ecrecover)(eth::bytesConstRef _in)
// {
//     struct
//     {
//         h256 hash;
//         h256 v;
//         h256 r;
//         h256 s;
//     } in;

//     memcpy(&in, _in.data(), min(_in.size(), sizeof(in)));

//     h256 ret;
//     u256 v = (u256)in.v;
//     if (v >= 27 && v <= 28)
//     {
//         SignatureStruct sig(in.r, in.s, (byte)((int)v - 27));
//         if (sig.isValid())
//         {
//             try
//             {
//                 if (Public rec = recover(sig, in.hash))
//                 {
//                     ret = dev::sha3(rec);
//                     memset(ret.data(), 0, 12);
//                     return {true, ret.asBytes()};
//                 }
//             }
//             catch (...) {}
//         }
//     }
//     return {true, {}};
// }

ETH_REGISTER_PRECOMPILED_PRICER(sha256)
(eth::bytesConstRef _in, int64_t const& /*_blockNumber*/)
{
    return linearPricer(60, 12, _in);
}

ETH_REGISTER_PRECOMPILED(sha256)(eth::bytesConstRef _in)
{
    auto hashRet = sha512Half<CommonKey::sha>(Slice(_in.data(), _in.size()));
    return {true, Blob(hashRet.begin(), hashRet.end())};
}

ETH_REGISTER_PRECOMPILED_PRICER(sm3)
(eth::bytesConstRef _in, int64_t const& /*_blockNumber*/)
{
    return linearPricer(60, 12, _in);
}

ETH_REGISTER_PRECOMPILED(sm3)(eth::bytesConstRef _in)
{
    auto hashRet = sha512Half<CommonKey::sm3>(Slice(_in.data(), _in.size()));
    return {true, Blob(hashRet.begin(), hashRet.end())};
}

ETH_REGISTER_PRECOMPILED_PRICER(ripemd160)
(eth::bytesConstRef _in, int64_t const& /*_blockNumber*/)
{
    return linearPricer(600, 120, _in);
}

ETH_REGISTER_PRECOMPILED(ripemd160)(eth::bytesConstRef _in)
{
    // return {true, h256(dev::ripemd160(_in), h256::AlignRight).asBytes()};
    ripemd160_hasher rh;
    rh(_in.data(), _in.size());
    auto rhRet = ripemd160_hasher::result_type(rh);
    return {true, Blob(rhRet.begin(), rhRet.end())};
}

ETH_REGISTER_PRECOMPILED_PRICER(identity)
(eth::bytesConstRef _in, int64_t const& /*_blockNumber*/)
{
    return linearPricer(15, 3, _in);
}

ETH_REGISTER_PRECOMPILED(identity)(eth::bytesConstRef _in)
{
    return {true, _in.toBytes()};
}

ETH_REGISTER_PRECOMPILED_PRICER(enbase58)
(eth::bytesConstRef _in, int64_t const& /*_blockNumber*/)
{
    return linearPricer(60, 12, _in);
}

ETH_REGISTER_PRECOMPILED(enbase58)(eth::bytesConstRef _in/*, std::uint8_t tokenType = (std::uint8_t)TokenType::AccountID*/)
{
    auto enRet = base58EncodeToken(TokenType::AccountID, _in.data(), _in.size());
    return { true, Blob(enRet.begin(), enRet.end()) };
}

ETH_REGISTER_PRECOMPILED_PRICER(debase58)
(eth::bytesConstRef _in, int64_t const& /*_blockNumber*/)
{
    return linearPricer(60, 12, _in);
}

ETH_REGISTER_PRECOMPILED(debase58)(eth::bytesConstRef _in/*, std::uint8_t tokenType = (std::uint8_t)TokenType::AccountID*/)
{
    auto enRet = decodeBase58Token(std::string((const char*)_in.data(), _in.size()), TokenType::AccountID);
    return { true, Blob(enRet.begin(), enRet.end()) };
}

// Parse _count bytes of _in starting with _begin offset as big endian int.
// If there's not enough bytes in _in, consider it infinitely right-padded with zeroes.
eth::bigint parseBigEndianRightPadded(eth::bytesConstRef _in, eth::bigint const& _begin, eth::bigint const& _count)
{
    if (_begin > _in.count())
        return 0;
    assert(_count <= numeric_limits<size_t>::max() / 8); // Otherwise, the return value would not fit in the memory.

    size_t const begin{_begin};
    size_t const count{_count};

    // crop _in, not going beyond its size
    eth::bytesConstRef cropped = _in.cropped(begin, min(count, _in.count() - begin));

    eth::bigint ret = ripple::fromBigEndian<eth::bigint>(cropped);
    // shift as if we had right-padding zeroes
    assert(count - cropped.count() <= numeric_limits<size_t>::max() / 8);
    ret <<= 8 * (count - cropped.count());

    return ret;
}

 ETH_REGISTER_PRECOMPILED(modexp)(eth::bytesConstRef _in)
 {
     eth::bigint const baseLength(parseBigEndianRightPadded(_in, 0, 32));
     eth::bigint const expLength(parseBigEndianRightPadded(_in, 32, 32));
     eth::bigint const modLength(parseBigEndianRightPadded(_in, 64, 32));
     assert(modLength <= numeric_limits<size_t>::max() / 8); // Otherwise gas should be too expensive.
     assert(baseLength <= numeric_limits<size_t>::max() / 8); // Otherwise, gas should be too expensive.
     if (modLength == 0 && baseLength == 0)
         return {true, eth::bytes{}}; // This is a special case where expLength can be very big.
     assert(expLength <= numeric_limits<size_t>::max() / 8);

     eth::bigint const base(parseBigEndianRightPadded(_in, 96, baseLength));
     eth::bigint const exp(parseBigEndianRightPadded(_in, 96 + baseLength, expLength));
     eth::bigint const mod(parseBigEndianRightPadded(_in, 96 + baseLength + expLength, modLength));

     eth::bigint const result = mod != 0 ? boost::multiprecision::powm(base, exp, mod) : eth::bigint{0};

     size_t const retLength(modLength);
     eth::bytes ret(retLength);
     ripple::toBigEndian(result, ret);

     return {true, ret};
 }

 namespace
 {
     int64_t expLengthAdjust(int64_t const& _expOffset, int64_t const& _expLength, eth::bytesConstRef _in)
     {
         if (_expLength <= 32)
         {
             eth::bigint const exp(parseBigEndianRightPadded(_in, _expOffset, _expLength));
             return exp ? boost::multiprecision::msb(exp) : 0;
         }
         else
         {
             eth::bigint const expFirstWord(parseBigEndianRightPadded(_in, _expOffset, 32));
             size_t const highestBit(expFirstWord ? boost::multiprecision::msb(expFirstWord) : 0);
             return 8 * (_expLength - 32) + highestBit;
         }
     }

     int64_t multComplexity(int64_t const& _x)
     {
         if (_x <= 64)
             return _x * _x;
         if (_x <= 1024)
             return (_x * _x) / 4 + 96 * _x - 3072;
         else
             return (_x * _x) / 16 + 480 * _x - 199680;
     }
 }

 ETH_REGISTER_PRECOMPILED_PRICER(modexp)(eth::bytesConstRef _in, int64_t const&)
 {
     int64_t const baseLength(parseBigEndianRightPadded(_in, 0, 32));
     int64_t const expLength(parseBigEndianRightPadded(_in, 32, 32));
     int64_t const modLength(parseBigEndianRightPadded(_in, 64, 32));

     int64_t const maxLength(max(modLength, baseLength));
     int64_t const adjustedExpLength(expLengthAdjust(baseLength + 96, expLength, _in));

     return multComplexity(maxLength) * max<int64_t>(adjustedExpLength, 1) / 20;
 }

 ETH_REGISTER_PRECOMPILED(alt_bn128_G1_add)(eth::bytesConstRef _in)
 {
     return peersafe::alt_bn128_G1_add(_in);
 }

 ETH_REGISTER_PRECOMPILED_PRICER(alt_bn128_G1_add)
 (eth::bytesConstRef /*_in*/, int64_t const& _blockNumber)
 {
     return 150;
     // return _blockNumber < _chainParams.istanbulForkBlock ? 500 : 150;
 }

 ETH_REGISTER_PRECOMPILED(alt_bn128_G1_mul)(eth::bytesConstRef _in)
 {
     return  peersafe::alt_bn128_G1_mul(_in);
 }

 ETH_REGISTER_PRECOMPILED_PRICER(alt_bn128_G1_mul)
 (eth::bytesConstRef /*_in*/, int64_t const& _blockNumber)
 {
     return 6000;
     // return _blockNumber < _chainParams.istanbulForkBlock ? 40000 : 6000;
 }

 ETH_REGISTER_PRECOMPILED(alt_bn128_pairing_product)(eth::bytesConstRef _in)
 {
     return  peersafe::alt_bn128_pairing_product(_in);
 }

 ETH_REGISTER_PRECOMPILED_PRICER(alt_bn128_pairing_product)
 (eth::bytesConstRef _in, int64_t const& _blockNumber)
 {
     auto const k = _in.size() / 192;
     return 45000 + k * 34000;
     // return _blockNumber < _chainParams.istanbulForkBlock ? 100000 + k * 80000 : 45000 + k * 34000;
 }

// ETH_REGISTER_PRECOMPILED(blake2_compression)(eth::bytesConstRef _in)
// {
//     static constexpr size_t roundsSize = 4;
//     static constexpr size_t stateVectorSize = 8 * 8;
//     static constexpr size_t messageBlockSize = 16 * 8;
//     static constexpr size_t offsetCounterSize = 8;
//     static constexpr size_t finalBlockIndicatorSize = 1;
//     static constexpr size_t totalInputSize = roundsSize + stateVectorSize + messageBlockSize +
//                                              2 * offsetCounterSize + finalBlockIndicatorSize;

//     if (_in.size() != totalInputSize)
//         return {false, {}};

//     auto const rounds = fromBigEndian<uint32_t>(_in.cropped(0, roundsSize));
//     auto const stateVector = _in.cropped(roundsSize, stateVectorSize);
//     auto const messageBlockVector = _in.cropped(roundsSize + stateVectorSize, messageBlockSize);
//     auto const offsetCounter0 =
//         _in.cropped(roundsSize + stateVectorSize + messageBlockSize, offsetCounterSize);
//     auto const offsetCounter1 = _in.cropped(
//         roundsSize + stateVectorSize + messageBlockSize + offsetCounterSize, offsetCounterSize);
//     uint8_t const finalBlockIndicator =
//         _in[roundsSize + stateVectorSize + messageBlockSize + 2 * offsetCounterSize];

//     if (finalBlockIndicator != 0 && finalBlockIndicator != 1)
//         return {false, {}};

//     return {true, dev::crypto::blake2FCompression(rounds, stateVector, offsetCounter0,
//                       offsetCounter1, finalBlockIndicator, messageBlockVector)};
// }

// ETH_REGISTER_PRECOMPILED_PRICER(blake2_compression)
// (eth::bytesConstRef _in, int64_t const&)
// {
//     auto const rounds = fromBigEndian<uint32_t>(_in.cropped(0, 4));
//     return rounds;
// }
}

}


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2015 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#include "graph.hh"
#include "graph_filtering.hh"
#include "graph_properties.hh"
#include "graph_selectors.hh"

#include <boost/python.hpp>

#include <boost/graph/johnson_all_pairs_shortest.hpp>
#include <boost/graph/floyd_warshall_shortest.hpp>

using namespace std;
using namespace boost;
using namespace graph_tool;

struct do_all_pairs_search
{
    template <class Graph, class VertexIndexMap, class DistMap, class WeightMap>
    void operator()(const Graph& g, VertexIndexMap vertex_index,
                    DistMap dist_map, WeightMap weight, bool dense) const
    {
        typedef typename property_traits<DistMap>::value_type::value_type
            dist_t;

        int i, N = num_vertices(g);
        #pragma omp parallel for default(shared) private(i) schedule(runtime) if (N > 100)
        for (i = 0; i < N; ++i)
        {
            dist_map[i].clear();
            dist_map[i].resize(num_vertices(g), 0);
        }

        if (dense)
        {
            floyd_warshall_all_pairs_shortest_paths
                (g, dist_map,
                 weight_map(ConvertedPropertyMap<WeightMap,dist_t>(weight)).
                 vertex_index_map(vertex_index));
        }
        else
        {
            johnson_all_pairs_shortest_paths
                (g, dist_map,
                 weight_map(ConvertedPropertyMap<WeightMap,dist_t>(weight)).
                 vertex_index_map(vertex_index));
        }
    }
};

void get_all_dists(GraphInterface& gi, boost::any dist_map, boost::any weight,
                   bool dense)
{
    typedef ConstantPropertyMap<size_t,GraphInterface::edge_t> cweight_map_t;

    if (weight.empty())
        weight = boost::any(cweight_map_t(1));

    run_action<>()
        (gi, std::bind(do_all_pairs_search(), placeholders::_1,
                       gi.GetVertexIndex(), placeholders::_2, placeholders::_3,
                       dense),
         vertex_scalar_vector_properties(),
         mpl::push_back<edge_scalar_properties,cweight_map_t>::type())
        (dist_map, weight);
}

void export_all_dists()
{
    python::def("get_all_dists", &get_all_dists);
};


//    Copyright (C) Dimitrios Michail 2019 - 2021.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          https://www.boost.org/LICENSE_1_0.txt)

#include <iostream>
#include <fstream>
#include <map>
#include <list>

#include <boost/config.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/program_options.hpp>
#include <boost/timer/timer.hpp>
#include <boost/thread.hpp>

#include <parmcb/parmcb.hpp>
#include <parmcb/util.hpp>
#include <parmcb/detail/cycles.hpp>
#include <parmcb/sptrees.hpp>

using namespace boost;
namespace po = boost::program_options;

#define USAGE "Computes statistics on FVS and ISO cycle collections given a weighted undirected graph in DIMACS format."

template<class Graph, class WeightMap>
class CandidateCycleBuilder {
public:
    typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
    typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
    typedef typename boost::property_traits<WeightMap>::value_type WeightType;

    CandidateCycleBuilder(const Graph &g, const WeightMap &weight_map) :
            g(g), weight_map(weight_map) {
    }

    std::tuple<std::set<Edge>, WeightType> operator()(const std::vector<parmcb::SPTree<Graph, WeightMap>> &trees,
            const parmcb::CandidateCycle<Graph, WeightMap> &c) const {

        std::shared_ptr<parmcb::SPNode<Graph, WeightMap>> v = trees[c.tree()].node(boost::source(c.edge(), g));
        std::shared_ptr<parmcb::SPNode<Graph, WeightMap>> u = trees[c.tree()].node(boost::target(c.edge(), g));

        Edge e = c.edge();
        bool valid = true;
        WeightType cycle_weight = boost::get(weight_map, e);
        std::set<Edge> result;
        result.insert(e);

        // first part
        Vertex w = boost::source(c.edge(), g);
        std::shared_ptr<parmcb::SPNode<Graph, WeightMap>> ws = trees[c.tree()].node(w);
        while (ws->has_pred()) {
            Edge a = ws->pred();
            if (result.insert(a).second == false) {
                valid = false;
                break;
            }
            cycle_weight += boost::get(weight_map, a);
            w = boost::opposite(a, w, g);
            ws = trees[c.tree()].node(w);
        }

        if (!valid) {
            return std::make_tuple(std::set<Edge> { }, 0.0);
        }

        // second part
        w = boost::target(c.edge(), g);
        ws = trees[c.tree()].node(w);
        while (ws->has_pred()) {
            Edge a = ws->pred();
            if (result.insert(a).second == false) {
                valid = false;
                break;
            }
            cycle_weight += boost::get(weight_map, a);
            w = boost::opposite(a, w, g);
            ws = trees[c.tree()].node(w);
        }

        if (!valid) {
            return std::make_tuple(std::set<Edge> { }, 0.0);
        }

        return std::make_tuple(result, cycle_weight);
    }

private:
    const Graph &g;
    const WeightMap &weight_map;
};

template<class Graph, class WeightMap, class CyclesBuilder>
void print_tree_stats(const char *name, const Graph &g, WeightMap weight_map) {
    std::vector<parmcb::SPTree<Graph, WeightMap>> trees;
    std::vector<parmcb::CandidateCycle<Graph, WeightMap>> cycles;
    CyclesBuilder cycles_builder;
    cycles_builder(g, weight_map, trees, cycles);
    std::cout << name << " cycles: " << cycles.size() << std::endl;

    CandidateCycleBuilder<Graph, WeightMap> candidate_cycle_builder(g, weight_map);

    typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
    typedef typename boost::property_traits<WeightMap>::value_type WeightType;

    double total_weight = 0.0;
    long total_card = 0;
    for (parmcb::CandidateCycle<Graph, WeightMap> c : cycles) {
        std::tuple<std::set<Edge>, WeightType> cc = candidate_cycle_builder(trees, c);

        total_weight += std::get<1>(cc);
        total_card += std::get<0>(cc).size();
    }

    std::cout << name << " total cycles weight: " << total_weight << std::endl;
    std::cout << name << " total cycles cardinality: " << total_card << std::endl;
}

template <class Graph, class WeightMap>
void print_all_tree_stats(const Graph& g, WeightMap weight_map) {
    print_tree_stats<Graph, WeightMap, parmcb::detail::FVSCyclesBuilder<Graph, WeightMap>>("FVS", g, weight_map);
    print_tree_stats<Graph, WeightMap, parmcb::detail::ISOCyclesBuilder<Graph, WeightMap>>("ISO", g, weight_map);
    print_tree_stats<Graph, WeightMap, parmcb::detail::HortonCyclesBuilder<Graph, WeightMap>>("HORTON", g, weight_map);
}

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

    po::variables_map vm;
    try {
        po::options_description desc(USAGE);
        // @formatter:off
        desc.add_options()
                ("help,h", "Help")
                ("verbose,v", po::value<bool>()->default_value(false)->implicit_value(true), "Verbose")
                ("input-file,I",po::value<std::string>(), "Input filename");
        // @formatter:on
        po::positional_options_description pos_desc;
        pos_desc.add("input-file", 1);
        po::command_line_parser parser { argc, argv };
        parser.options(desc).positional(pos_desc).allow_unregistered();
        po::parsed_options parsed_options = parser.run();
        po::store(parsed_options, vm);

        if (vm.count("help")) {
            std::cout << desc << std::endl;
            return EXIT_SUCCESS;
        }
        po::notify(vm);

        if (!vm.count("input-file")) {
            std::cerr << "Input file missing. See usage by calling with -h ." << std::endl;
            exit(EXIT_FAILURE);
        }

    } catch (const po::error &ex) {
        std::cerr << "Invalid arguments:" << ex.what() << std::endl;
        return EXIT_FAILURE;
    }

    typedef adjacency_list<vecS, vecS, undirectedS, no_property, property<edge_weight_t, double> > graph_t;

    // create graph
    graph_t graph;
    FILE *fp = fopen(vm["input-file"].as<std::string>().c_str(), "r");
    if (fp == NULL) {
        std::cerr << "Failed to open input file." << std::endl;
        exit(EXIT_FAILURE);
    }
    parmcb::read_dimacs_from_file(fp, graph);
    fclose(fp);

    if (parmcb::has_loops(graph)) {
        std::cerr << "Graph has loops, aborting.." << std::endl;
        return EXIT_FAILURE;
    }
    if (parmcb::has_multiple_edges(graph)) {
        std::cerr << "Graph has multiple edges, aborting.." << std::endl;
        return EXIT_FAILURE;
    }
    if (parmcb::has_non_positive_weights(graph, get(boost::edge_weight, graph))) {
        std::cerr << "Graph has negative or zero weight edges, aborting.." << std::endl;
        return EXIT_FAILURE;
    }

    std::cout << "graph n: " << num_vertices(graph) << std::endl;
    std::cout << "graph m: " << num_edges(graph) << std::endl;
    std::cout << "graph n*m: " << num_vertices(graph)*num_edges(graph) << std::endl;
    std::cout << std::flush;

    print_all_tree_stats(graph, get(boost::edge_weight, graph));

    return EXIT_SUCCESS;
}


/* -*- c++ -*- */
/*
 * Copyright 2003,2008,2011,2012 Free Software Foundation, Inc.
 *
 * This file is part of GNU Radio
 *
 * GNU Radio is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3, or (at your option)
 * any later version.
 *
 * GNU Radio is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with GNU Radio; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#include <gnuradio/fft/fft.h>
#include <gnuradio/sys_paths.h>
#include <gnuradio/gr_complex.h>
#include <volk/volk.h>
#include <fftw3.h>

#ifdef _WIN32 //http://www.fftw.org/install/windows.html#DLLwisdom
static void my_fftw_write_char(char c, void *f) { fputc(c, (FILE *) f); }
#define fftw_export_wisdom_to_file(f) fftw_export_wisdom(my_fftw_write_char, (void*) (f))
#define fftwf_export_wisdom_to_file(f) fftwf_export_wisdom(my_fftw_write_char, (void*) (f))
#define fftwl_export_wisdom_to_file(f) fftwl_export_wisdom(my_fftw_write_char, (void*) (f))

static int my_fftw_read_char(void *f) { return fgetc((FILE *) f); }
#define fftw_import_wisdom_from_file(f) fftw_import_wisdom(my_fftw_read_char, (void*) (f))
#define fftwf_import_wisdom_from_file(f) fftwf_import_wisdom(my_fftw_read_char, (void*) (f))
#define fftwl_import_wisdom_from_file(f) fftwl_import_wisdom(my_fftw_read_char, (void*) (f))
#include <fcntl.h>
#include <io.h>
#define O_NOCTTY 0
#define O_NONBLOCK 0
#endif //_WIN32

#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <cassert>
#include <stdexcept>

#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/path.hpp>
#include <boost/interprocess/sync/file_lock.hpp>
namespace fs = boost::filesystem;

namespace gr {
  namespace fft {
    static boost::mutex wisdom_thread_mutex;
    boost::interprocess::file_lock wisdom_lock;
    static bool wisdom_lock_init_done = false; // Modify while holding 'wisdom_thread_mutex'

    gr_complex *
    malloc_complex(int size)
    {
      return (gr_complex*) volk_malloc (sizeof (gr_complex)*size, volk_get_alignment ());
    }

    float *
    malloc_float(int size)
    {
      return (float*) volk_malloc (sizeof (float)*size, volk_get_alignment ());
    }

    double *
    malloc_double(int size)
    {
      return (double*) volk_malloc (sizeof (double)*size, volk_get_alignment ());
    }

    void
    free(void *b)
    {
      volk_free(b);
    }

    boost::mutex &
    planner::mutex()
    {
      static boost::mutex  s_planning_mutex;

      return s_planning_mutex;
    }

    static std::string
    wisdom_filename()
    {
      static fs::path path;
      path = fs::path(gr::appdata_path()) / ".gr_fftw_wisdom";
      return path.string();
    }

    static void
    wisdom_lock_init()
    {
      if (wisdom_lock_init_done)
        return;

      const std::string wisdom_lock_file = wisdom_filename() + ".lock";
      // std::cerr << "Creating FFTW wisdom lockfile: " << wisdom_lock_file << std::endl;
      int fd = open(wisdom_lock_file.c_str(),
        O_WRONLY | O_CREAT | O_NOCTTY | O_NONBLOCK,
        0666);
      if (fd < 0) {
        throw std::runtime_error("Failed to create FFTW wisdom lockfile: " + wisdom_lock_file);
      }
      close(fd);
      wisdom_lock = boost::interprocess::file_lock(wisdom_lock_file.c_str());
      wisdom_lock_init_done = true;
    }

    static void
    lock_wisdom()
    {
      wisdom_thread_mutex.lock();
      wisdom_lock_init();
      wisdom_lock.lock();
    }

    static void
    unlock_wisdom()
    {
      // Assumes 'lock_wisdom' has already been called (i.e. this file_lock is valid)
      wisdom_lock.unlock();
      wisdom_thread_mutex.unlock();
    }

    static void
    import_wisdom()
    {
      const std::string filename = wisdom_filename ();
      FILE *fp = fopen (filename.c_str(), "r");
      if (fp != 0){
        int r = fftwf_import_wisdom_from_file (fp);
        fclose (fp);
        if (!r){
          fprintf (stderr, "gr::fft: can't import wisdom from %s\n", filename.c_str());
        }
      }
    }

    static void
    config_threading(int nthreads)
    {
      static int fftw_threads_inited = 0;

#ifdef FFTW3F_THREADS
      if (fftw_threads_inited == 0)
    {
      fftw_threads_inited = 1;
      fftwf_init_threads();
    }

      fftwf_plan_with_nthreads(nthreads);
#endif
    }

    static void
    export_wisdom()
    {
      const std::string filename = wisdom_filename ();
      FILE *fp = fopen (filename.c_str(), "w");
      if (fp != 0){
        fftwf_export_wisdom_to_file (fp);
        fclose (fp);
      }
      else {
        fprintf (stderr, "fft_impl_fftw: ");
        perror (filename.c_str());
      }
    }

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

    fft_complex::fft_complex(int fft_size, bool forward, int nthreads)
    {
      // Hold global mutex during plan construction and destruction.
      planner::scoped_lock lock(planner::mutex());

      assert (sizeof (fftwf_complex) == sizeof (gr_complex));

      if (fft_size <= 0){
        throw std::out_of_range ("fft_impl_fftw: invalid fft_size");
      }

      d_fft_size = fft_size;
      d_inbuf = (gr_complex *) volk_malloc (sizeof (gr_complex) * inbuf_length (), volk_get_alignment ());
      if (d_inbuf == 0) {
        throw std::runtime_error ("volk_malloc");
      }
      d_outbuf = (gr_complex *) volk_malloc (sizeof (gr_complex) * outbuf_length (), volk_get_alignment ());
      if (d_outbuf == 0) {
        volk_free (d_inbuf);
        throw std::runtime_error ("volk_malloc");
      }

      d_nthreads = nthreads;
      config_threading(nthreads);
      lock_wisdom();
      import_wisdom();	// load prior wisdom from disk

      d_plan = fftwf_plan_dft_1d (fft_size,
                  reinterpret_cast<fftwf_complex *>(d_inbuf),
                  reinterpret_cast<fftwf_complex *>(d_outbuf),
                  forward ? FFTW_FORWARD : FFTW_BACKWARD,
                  FFTW_MEASURE);

      if (d_plan == NULL) {
        fprintf(stderr, "gr::fft: error creating plan\n");
        throw std::runtime_error ("fftwf_plan_dft_1d failed");
      }
      export_wisdom();	// store new wisdom to disk
      unlock_wisdom();
    }

    fft_complex::~fft_complex()
    {
      // Hold global mutex during plan construction and destruction.
      planner::scoped_lock lock(planner::mutex());

      fftwf_destroy_plan ((fftwf_plan) d_plan);
      volk_free (d_inbuf);
      volk_free (d_outbuf);
    }

    void
    fft_complex::set_nthreads(int n)
    {
      if (n <= 0) {
        throw std::out_of_range ("gr::fft: invalid number of threads");
      }
      d_nthreads = n;

#ifdef FFTW3F_THREADS
      fftwf_plan_with_nthreads(d_nthreads);
#endif
    }

    void
    fft_complex::execute()
    {
      fftwf_execute((fftwf_plan) d_plan);
    }

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

    fft_real_fwd::fft_real_fwd (int fft_size, int nthreads)
    {
      // Hold global mutex during plan construction and destruction.
      planner::scoped_lock lock(planner::mutex());

      assert (sizeof (fftwf_complex) == sizeof (gr_complex));

      if (fft_size <= 0) {
        throw std::out_of_range ("gr::fft: invalid fft_size");
      }

      d_fft_size = fft_size;
      d_inbuf = (float *) volk_malloc (sizeof (float) * inbuf_length (), volk_get_alignment ());
      if (d_inbuf == 0) {
        throw std::runtime_error ("volk_malloc");
      }

      d_outbuf = (gr_complex *) volk_malloc (sizeof (gr_complex) * outbuf_length (), volk_get_alignment ());
      if (d_outbuf == 0) {
        volk_free (d_inbuf);
        throw std::runtime_error ("volk_malloc");
      }

      d_nthreads = nthreads;
      config_threading(nthreads);
      lock_wisdom();
      import_wisdom();	// load prior wisdom from disk

      d_plan = fftwf_plan_dft_r2c_1d (fft_size,
                      d_inbuf,
                      reinterpret_cast<fftwf_complex *>(d_outbuf),
                      FFTW_MEASURE);

      if (d_plan == NULL) {
        fprintf(stderr, "gr::fft::fft_real_fwd: error creating plan\n");
        throw std::runtime_error ("fftwf_plan_dft_r2c_1d failed");
      }
      export_wisdom();	// store new wisdom to disk
      unlock_wisdom();
    }

    fft_real_fwd::~fft_real_fwd()
    {
      // Hold global mutex during plan construction and destruction.
      planner::scoped_lock lock(planner::mutex());

      fftwf_destroy_plan ((fftwf_plan) d_plan);
      volk_free (d_inbuf);
      volk_free (d_outbuf);
    }

    void
    fft_real_fwd::set_nthreads(int n)
    {
      if (n <= 0) {
        throw std::out_of_range ("gr::fft::fft_real_fwd::set_nthreads: invalid number of threads");
      }
      d_nthreads = n;

#ifdef FFTW3F_THREADS
      fftwf_plan_with_nthreads(d_nthreads);
#endif
    }

    void
    fft_real_fwd::execute()
    {
      fftwf_execute ((fftwf_plan) d_plan);
    }

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

    fft_real_rev::fft_real_rev(int fft_size, int nthreads)
    {
      // Hold global mutex during plan construction and destruction.
      planner::scoped_lock lock(planner::mutex());

      assert (sizeof (fftwf_complex) == sizeof (gr_complex));

      if (fft_size <= 0) {
        throw std::out_of_range ("gr::fft::fft_real_rev: invalid fft_size");
      }

      d_fft_size = fft_size;
      d_inbuf = (gr_complex *) volk_malloc (sizeof (gr_complex) * inbuf_length (), volk_get_alignment ());
      if (d_inbuf == 0) {
        throw std::runtime_error ("volk_malloc");
      }

      d_outbuf = (float *) volk_malloc (sizeof (float) * outbuf_length (), volk_get_alignment ());
      if (d_outbuf == 0) {
        volk_free (d_inbuf);
        throw std::runtime_error ("volk_malloc");
      }

      d_nthreads = nthreads;
      config_threading(nthreads);
      lock_wisdom();
      import_wisdom();	// load prior wisdom from disk

      // FIXME If there's ever a chance that the planning functions
      // will be called in multiple threads, we've got to ensure single
      // threaded access.  They are not thread-safe.
      d_plan = fftwf_plan_dft_c2r_1d (fft_size,
                      reinterpret_cast<fftwf_complex *>(d_inbuf),
                      d_outbuf,
                      FFTW_MEASURE);

      if (d_plan == NULL) {
        fprintf(stderr, "gr::fft::fft_real_rev: error creating plan\n");
        throw std::runtime_error ("fftwf_plan_dft_c2r_1d failed");
      }
      export_wisdom ();	// store new wisdom to disk
      unlock_wisdom();
    }

    fft_real_rev::~fft_real_rev ()
    {
      // Hold global mutex during plan construction and destruction.
      planner::scoped_lock lock(planner::mutex());

      fftwf_destroy_plan ((fftwf_plan) d_plan);
      volk_free (d_inbuf);
      volk_free (d_outbuf);
    }

    void
    fft_real_rev::set_nthreads(int n)
    {
      if (n <= 0) {
        throw std::out_of_range ("gr::fft::fft_real_rev::set_nthreads: invalid number of threads");
      }
      d_nthreads = n;

#ifdef FFTW3F_THREADS
      fftwf_plan_with_nthreads(d_nthreads);
#endif
    }

    void
    fft_real_rev::execute ()
    {
      fftwf_execute ((fftwf_plan) d_plan);
    }

  } /* namespace fft */
} /* namespace gr */


// This file is part of LatticeTester.
//
// LatticeTester
// Copyright (C) 2012-2018  Pierre L'Ecuyer and Universite de Montreal
//
// 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.

/*
 * The purpose of this example is to compare the time
 *  of each reduction method. We compute several
 * random matrix and apply algorithms. The parameters of this
 * analysis are defined at the beginning of this file,
 * after include part. The output is a graphic generated by R.
 * The User need R distribution and the header RInside.h
 */

// Include Header
#include <iostream>
#include <map>
#include <fstream>
#include <iterator>
#include <string>
#include <sstream>
#include <iomanip>
#include <time.h>
#include <sys/types.h>
#include <sys/stat.h>

// Include LatticeTester Header
#include "latticetester/Util.h"
#include "latticetester/Const.h"
#include "latticetester/IntFactor.h"
#include "latticetester/IntLatticeBasis.h"
#include "latticetester/Reducer.h"
#include "latticetester/ParamReader.h"
#include "latticetester/Config.h"
#include "latticetester/LatticeAnalysis.h"

// Include NTL Header
#include <NTL/tools.h>
#include <NTL/ctools.h>
#include <NTL/ZZ.h>
#include <NTL/ZZ_p.h>
#include "NTL/vec_ZZ.h"
#include "NTL/vec_ZZ_p.h"
#include <NTL/vec_vec_ZZ.h>
#include <NTL/vec_vec_ZZ_p.h>
#include <NTL/mat_ZZ.h>
#include <NTL/matrix.h>
#include <NTL/LLL.h>

using namespace std;
using namespace LatticeTester;

int ZZRR(int argc, char* argv[]) {
  LatticeAnalysis<NTL::ZZ, NTL::ZZ, NTL::RR, NTL::RR> latAnalysis;
  struct stat buf; // properties of a file or directory
  int status = 0;

  for (int j = 2; j < argc; j++) {
    // Do the test for each data file or directory on the command line

    stat(argv[j], &buf);
    if (0 != S_ISDIR(buf.st_mode)) // directory
      status |= latAnalysis.doTestFromDirectory (argv[j]);
    else {
      string dataname(argv[j]);
      dataname.append(".dat");
      stat(dataname.c_str(), &buf);
      status |= latAnalysis.doTestFromInputFile (argv[j]);
    }
  }
  return 1;
}
int ZZDD(int argc, char* argv[]) {
  LatticeAnalysis<NTL::ZZ, NTL::ZZ, double, double> latAnalysis;
  struct stat buf; // properties of a file or directory
  int status = 0;

  for (int j = 2; j < argc; j++) {
    // Do the test for each data file or directory on the command line

    stat(argv[j], &buf);
    if (0 != S_ISDIR(buf.st_mode)) // directory
      status |= latAnalysis.doTestFromDirectory (argv[j]);
    else {
      string dataname(argv[j]);
      dataname.append(".dat");
      stat(dataname.c_str(), &buf);
      status |= latAnalysis.doTestFromInputFile (argv[j]);
    }
  }
  return 1;
}
int LLDD(int argc, char* argv[]) {
  LatticeAnalysis<std::int64_t, std::int64_t, double, double> latAnalysis;
  struct stat buf; // properties of a file or directory
  int status = 0;

  for (int j = 2; j < argc; j++) {
    // Do the test for each data file or directory on the command line

    stat(argv[j], &buf);
    if (0 != S_ISDIR(buf.st_mode)) // directory
      status |= latAnalysis.doTestFromDirectory (argv[j]);
    else {
      string dataname(argv[j]);
      dataname.append(".dat");
      stat(dataname.c_str(), &buf);
      status |= latAnalysis.doTestFromInputFile (argv[j]);
    }
  }
  return 1;
}
//==============================================================================

int main (int argc, char *argv[])
{
  std::clock_t start_t = std::clock();
  if (argc < 2) {
    cerr << "\n*** Usage:\n   "
      << argv[0] << " data_file1 data_file2 ...." << endl
      << "or\n   "
      << argv[0] << " dir1 dir2 ...." << endl
      << endl;
    return -1;
  } else if (argc < 3) {
    cerr << "Must specify types\n";
    return -1;
  }
  //cout << "ARGC " << argc << endl;
  if (!strcmp(argv[1], "ZZRR")) {
    ZZRR(argc, argv);
    std::clock_t end_t = std::clock();
    cout << "Time: " << (double)(end_t-start_t)/CLOCKS_PER_SEC << " sec\n";
  } else if (!strcmp(argv[1], "ZZDD")) {
    ZZDD(argc, argv);
    std::clock_t end_t = std::clock();
    cout << "Time: " << (double)(end_t-start_t)/CLOCKS_PER_SEC << " sec\n";
  } else if (!strcmp(argv[1], "LLDD")) {
    LLDD(argc, argv);
    std::clock_t end_t = std::clock();
    cout << "Time: " << (double)(end_t-start_t)/CLOCKS_PER_SEC << " sec\n";
  } else {
    cerr << "No valid type name has been specified\n";
  }

  return 0;
}


// Boost.Polygon library detail/voronoi_ctypes.hpp header file

//          Copyright Andrii Sydorchuk 2010-2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

// See http://www.boost.org for updates, documentation, and revision history.

#ifndef BOOST_POLYGON_DETAIL_VORONOI_CTYPES
#define BOOST_POLYGON_DETAIL_VORONOI_CTYPES

#include <boost/cstdint.hpp>

#include <cmath>
#include <cstring>
#include <utility>
#include <vector>

namespace boost {
namespace polygon {
namespace detail {

typedef boost::int32_t int32;
typedef boost::int64_t int64;
typedef boost::uint32_t uint32;
typedef boost::uint64_t uint64;
typedef double fpt64;

// If two floating-point numbers in the same format are ordered (x < y),
// then they are ordered the same way when their bits are reinterpreted as
// sign-magnitude integers. Values are considered to be almost equal if
// their integer bits reinterpretations differ in not more than maxUlps units.
template <typename _fpt>
struct ulp_comparison;

template <>
struct ulp_comparison<fpt64> {
  enum Result {
    LESS = -1,
    EQUAL = 0,
    MORE = 1
  };

  Result operator()(fpt64 a, fpt64 b, unsigned int maxUlps) const {
    uint64 ll_a, ll_b;

    // Reinterpret double bits as 64-bit signed integer.
    std::memcpy(&ll_a, &a, sizeof(fpt64));
    std::memcpy(&ll_b, &b, sizeof(fpt64));

    // Positive 0.0 is integer zero. Negative 0.0 is 0x8000000000000000.
    // Map negative zero to an integer zero representation - making it
    // identical to positive zero - the smallest negative number is
    // represented by negative one, and downwards from there.
    if (ll_a < 0x8000000000000000ULL)
      ll_a = 0x8000000000000000ULL - ll_a;
    if (ll_b < 0x8000000000000000ULL)
      ll_b = 0x8000000000000000ULL - ll_b;

    // Compare 64-bit signed integer representations of input values.
    // Difference in 1 Ulp is equivalent to a relative error of between
    // 1/4,000,000,000,000,000 and 1/8,000,000,000,000,000.
    if (ll_a > ll_b)
      return (ll_a - ll_b <= maxUlps) ? EQUAL : LESS;
    return (ll_b - ll_a <= maxUlps) ? EQUAL : MORE;
  }
};

template <typename _fpt>
struct extened_exponent_fpt_traits;

template <>
struct extened_exponent_fpt_traits<fpt64> {
 public:
  typedef int exp_type;
  enum {
    MAX_SIGNIFICANT_EXP_DIF = 54
  };
};

// Floating point type wrapper. Allows to extend exponent boundaries to the
// integer type range. This class does not handle division by zero, subnormal
// numbers or NaNs.
template <typename _fpt, typename _traits = extened_exponent_fpt_traits<_fpt> >
class extended_exponent_fpt {
 public:
  typedef _fpt fpt_type;
  typedef typename _traits::exp_type exp_type;

  explicit extended_exponent_fpt(fpt_type val) {
    val_ = std::frexp(val, &exp_);
  }

  extended_exponent_fpt(fpt_type val, exp_type exp) {
    val_ = std::frexp(val, &exp_);
    exp_ += exp;
  }

  bool is_pos() const {
    return val_ > 0;
  }

  bool is_neg() const {
    return val_ < 0;
  }

  bool is_zero() const {
    return val_ == 0;
  }

  extended_exponent_fpt operator-() const {
    return extended_exponent_fpt(-val_, exp_);
  }

  extended_exponent_fpt operator+(const extended_exponent_fpt& that) const {
    if (this->val_ == 0.0 ||
        that.exp_ > this->exp_ + _traits::MAX_SIGNIFICANT_EXP_DIF) {
      return that;
    }
    if (that.val_ == 0.0 ||
        this->exp_ > that.exp_ + _traits::MAX_SIGNIFICANT_EXP_DIF) {
      return *this;
    }
    if (this->exp_ >= that.exp_) {
      exp_type exp_dif = this->exp_ - that.exp_;
      fpt_type val = std::ldexp(this->val_, exp_dif) + that.val_;
      return extended_exponent_fpt(val, that.exp_);
    } else {
      exp_type exp_dif = that.exp_ - this->exp_;
      fpt_type val = std::ldexp(that.val_, exp_dif) + this->val_;
      return extended_exponent_fpt(val, this->exp_);
    }
  }

  extended_exponent_fpt operator-(const extended_exponent_fpt& that) const {
    if (this->val_ == 0.0 ||
        that.exp_ > this->exp_ + _traits::MAX_SIGNIFICANT_EXP_DIF) {
      return extended_exponent_fpt(-that.val_, that.exp_);
    }
    if (that.val_ == 0.0 ||
        this->exp_ > that.exp_ + _traits::MAX_SIGNIFICANT_EXP_DIF) {
      return *this;
    }
    if (this->exp_ >= that.exp_) {
      exp_type exp_dif = this->exp_ - that.exp_;
      fpt_type val = std::ldexp(this->val_, exp_dif) - that.val_;
      return extended_exponent_fpt(val, that.exp_);
    } else {
      exp_type exp_dif = that.exp_ - this->exp_;
      fpt_type val = std::ldexp(-that.val_, exp_dif) + this->val_;
      return extended_exponent_fpt(val, this->exp_);
    }
  }

  extended_exponent_fpt operator*(const extended_exponent_fpt& that) const {
    fpt_type val = this->val_ * that.val_;
    exp_type exp = this->exp_ + that.exp_;
    return extended_exponent_fpt(val, exp);
  }

  extended_exponent_fpt operator/(const extended_exponent_fpt& that) const {
    fpt_type val = this->val_ / that.val_;
    exp_type exp = this->exp_ - that.exp_;
    return extended_exponent_fpt(val, exp);
  }

  extended_exponent_fpt& operator+=(const extended_exponent_fpt& that) {
    return *this = *this + that;
  }

  extended_exponent_fpt& operator-=(const extended_exponent_fpt& that) {
    return *this = *this - that;
  }

  extended_exponent_fpt& operator*=(const extended_exponent_fpt& that) {
    return *this = *this * that;
  }

  extended_exponent_fpt& operator/=(const extended_exponent_fpt& that) {
    return *this = *this / that;
  }

  extended_exponent_fpt sqrt() const {
    fpt_type val = val_;
    exp_type exp = exp_;
    if (exp & 1) {
      val *= 2.0;
      --exp;
    }
    return extended_exponent_fpt(std::sqrt(val), exp >> 1);
  }

  fpt_type d() const {
    return std::ldexp(val_, exp_);
  }

 private:
  fpt_type val_;
  exp_type exp_;
};
typedef extended_exponent_fpt<double> efpt64;

template <typename _fpt>
extended_exponent_fpt<_fpt> get_sqrt(const extended_exponent_fpt<_fpt>& that) {
  return that.sqrt();
}

template <typename _fpt>
bool is_pos(const extended_exponent_fpt<_fpt>& that) {
  return that.is_pos();
}

template <typename _fpt>
bool is_neg(const extended_exponent_fpt<_fpt>& that) {
  return that.is_neg();
}

template <typename _fpt>
bool is_zero(const extended_exponent_fpt<_fpt>& that) {
  return that.is_zero();
}

// Very efficient stack allocated big integer class.
// Supports next set of arithmetic operations: +, -, *.
template<std::size_t N>
class extended_int {
 public:
  extended_int() {}

  extended_int(int32 that) {
    if (that > 0) {
      this->chunks_[0] = that;
      this->count_ = 1;
    } else if (that < 0) {
      this->chunks_[0] = -that;
      this->count_ = -1;
    } else {
      this->count_ = 0;
    }
  }

  extended_int(int64 that) {
    if (that > 0) {
      this->chunks_[0] = static_cast<uint32>(that);
      this->chunks_[1] = that >> 32;
      this->count_ = this->chunks_[1] ? 2 : 1;
    } else if (that < 0) {
      that = -that;
      this->chunks_[0] = static_cast<uint32>(that);
      this->chunks_[1] = that >> 32;
      this->count_ = this->chunks_[1] ? -2 : -1;
    } else {
      this->count_ = 0;
    }
  }

  extended_int(const std::vector<uint32>& chunks, bool plus = true) {
    this->count_ = static_cast<int32>((std::min)(N, chunks.size()));
    for (int i = 0; i < this->count_; ++i)
      this->chunks_[i] = chunks[chunks.size() - i - 1];
    if (!plus)
      this->count_ = -this->count_;
  }

  template<std::size_t M>
  extended_int(const extended_int<M>& that) {
    this->count_ = that.count();
    std::memcpy(this->chunks_, that.chunks(), that.size() * sizeof(uint32));
  }

  extended_int& operator=(int32 that) {
    if (that > 0) {
      this->chunks_[0] = that;
      this->count_ = 1;
    } else if (that < 0) {
      this->chunks_[0] = -that;
      this->count_ = -1;
    } else {
      this->count_ = 0;
    }
    return *this;
  }

  extended_int& operator=(int64 that) {
    if (that > 0) {
      this->chunks_[0] = static_cast<uint32>(that);
      this->chunks_[1] = that >> 32;
      this->count_ = this->chunks_[1] ? 2 : 1;
    } else if (that < 0) {
      that = -that;
      this->chunks_[0] = static_cast<uint32>(that);
      this->chunks_[1] = that >> 32;
      this->count_ = this->chunks_[1] ? -2 : -1;
    } else {
      this->count_ = 0;
    }
    return *this;
  }

  template<std::size_t M>
  extended_int& operator=(const extended_int<M>& that) {
    this->count_ = that.count();
    std::memcpy(this->chunks_, that.chunks(), that.size() * sizeof(uint32));
    return *this;
  }

  bool is_pos() const {
    return this->count_ > 0;
  }

  bool is_neg() const {
    return this->count_ < 0;
  }

  bool is_zero() const {
    return this->count_ == 0;
  }

  bool operator==(const extended_int& that) const {
    if (this->count_ != that.count())
      return false;
    for (std::size_t i = 0; i < this->size(); ++i)
      if (this->chunks_[i] != that.chunks()[i])
        return false;
    return true;
  }

  bool operator!=(const extended_int& that) const {
    return !(*this == that);
  }

  bool operator<(const extended_int& that) const {
    if (this->count_ != that.count())
      return this->count_ < that.count();
    std::size_t i = this->size();
    if (!i)
      return false;
    do {
      --i;
      if (this->chunks_[i] != that.chunks()[i])
        return (this->chunks_[i] < that.chunks()[i]) ^ (this->count_ < 0);
    } while (i);
    return false;
  }

  bool operator>(const extended_int& that) const {
    return that < *this;
  }

  bool operator<=(const extended_int& that) const {
    return !(that < *this);
  }

  bool operator>=(const extended_int& that) const {
    return !(*this < that);
  }

  extended_int operator-() const {
    extended_int ret_val = *this;
    ret_val.neg();
    return ret_val;
  }

  void neg() {
    this->count_ = -this->count_;
  }

  extended_int operator+(const extended_int& that) const {
    extended_int ret_val;
    ret_val.add(*this, that);
    return ret_val;
  }

  void add(const extended_int& e1, const extended_int& e2) {
    if (!e1.count()) {
      *this = e2;
      return;
    }
    if (!e2.count()) {
      *this = e1;
      return;
    }
    if ((e1.count() > 0) ^ (e2.count() > 0)) {
      dif(e1.chunks(), e1.size(), e2.chunks(), e2.size());
    } else {
      add(e1.chunks(), e1.size(), e2.chunks(), e2.size());
    }
    if (e1.count() < 0)
      this->count_ = -this->count_;
  }

  extended_int operator-(const extended_int& that) const {
    extended_int ret_val;
    ret_val.dif(*this, that);
    return ret_val;
  }

  void dif(const extended_int& e1, const extended_int& e2) {
    if (!e1.count()) {
      *this = e2;
      this->count_ = -this->count_;
      return;
    }
    if (!e2.count()) {
      *this = e1;
      return;
    }
    if ((e1.count() > 0) ^ (e2.count() > 0)) {
      add(e1.chunks(), e1.size(), e2.chunks(), e2.size());
    } else {
      dif(e1.chunks(), e1.size(), e2.chunks(), e2.size());
    }
    if (e1.count() < 0)
      this->count_ = -this->count_;
  }

  extended_int operator*(int32 that) const {
    extended_int temp(that);
    return (*this) * temp;
  }

  extended_int operator*(int64 that) const {
    extended_int temp(that);
    return (*this) * temp;
  }

  extended_int operator*(const extended_int& that) const {
    extended_int ret_val;
    ret_val.mul(*this, that);
    return ret_val;
  }

  void mul(const extended_int& e1, const extended_int& e2) {
    if (!e1.count() || !e2.count()) {
      this->count_ = 0;
      return;
    }
    mul(e1.chunks(), e1.size(), e2.chunks(), e2.size());
    if ((e1.count() > 0) ^ (e2.count() > 0))
      this->count_ = -this->count_;
  }

  const uint32* chunks() const {
    return chunks_;
  }

  int32 count() const {
    return count_;
  }

  std::size_t size() const {
    return (std::abs)(count_);
  }

  std::pair<fpt64, int> p() const {
    std::pair<fpt64, int> ret_val(0, 0);
    std::size_t sz = this->size();
    if (!sz) {
      return ret_val;
    } else {
      if (sz == 1) {
        ret_val.first = static_cast<fpt64>(this->chunks_[0]);
      } else if (sz == 2) {
        ret_val.first = static_cast<fpt64>(this->chunks_[1]) *
                        static_cast<fpt64>(0x100000000LL) +
                        static_cast<fpt64>(this->chunks_[0]);
      } else {
        for (std::size_t i = 1; i <= 3; ++i) {
          ret_val.first *= static_cast<fpt64>(0x100000000LL);
          ret_val.first += static_cast<fpt64>(this->chunks_[sz - i]);
        }
        ret_val.second = (sz - 3) << 5;
      }
    }
    if (this->count_ < 0)
      ret_val.first = -ret_val.first;
    return ret_val;
  }

  fpt64 d() const {
    std::pair<fpt64, int> p = this->p();
    return std::ldexp(p.first, p.second);
  }

 private:
  void add(const uint32* c1, std::size_t sz1,
           const uint32* c2, std::size_t sz2) {
    if (sz1 < sz2) {
      add(c2, sz2, c1, sz1);
      return;
    }
    this->count_ = sz1;
    uint64 temp = 0;
    for (std::size_t i = 0; i < sz2; ++i) {
      temp += static_cast<uint64>(c1[i]) + static_cast<uint64>(c2[i]);
      this->chunks_[i] = static_cast<uint32>(temp);
      temp >>= 32;
    }
    for (std::size_t i = sz2; i < sz1; ++i) {
      temp += static_cast<uint64>(c1[i]);
      this->chunks_[i] = static_cast<uint32>(temp);
      temp >>= 32;
    }
    if (temp && (this->count_ != N)) {
      this->chunks_[this->count_] = static_cast<uint32>(temp);
      ++this->count_;
    }
  }

  void dif(const uint32* c1, std::size_t sz1,
           const uint32* c2, std::size_t sz2,
           bool rec = false) {
    if (sz1 < sz2) {
      dif(c2, sz2, c1, sz1, true);
      this->count_ = -this->count_;
      return;
    } else if ((sz1 == sz2) && !rec) {
      do {
        --sz1;
        if (c1[sz1] < c2[sz1]) {
          ++sz1;
          dif(c2, sz1, c1, sz1, true);
          this->count_ = -this->count_;
          return;
        } else if (c1[sz1] > c2[sz1]) {
          ++sz1;
          break;
        }
      } while (sz1);
      if (!sz1) {
        this->count_ = 0;
        return;
      }
      sz2 = sz1;
    }
    this->count_ = sz1-1;
    bool flag = false;
    for (std::size_t i = 0; i < sz2; ++i) {
      this->chunks_[i] = c1[i] - c2[i] - (flag?1:0);
      flag = (c1[i] < c2[i]) || ((c1[i] == c2[i]) && flag);
    }
    for (std::size_t i = sz2; i < sz1; ++i) {
      this->chunks_[i] = c1[i] - (flag?1:0);
      flag = !c1[i] && flag;
    }
    if (this->chunks_[this->count_])
      ++this->count_;
  }

  void mul(const uint32* c1, std::size_t sz1,
           const uint32* c2, std::size_t sz2) {
    uint64 cur = 0, nxt, tmp;
    this->count_ = static_cast<int32>((std::min)(N, sz1 + sz2 - 1));
    for (std::size_t shift = 0; shift < static_cast<std::size_t>(this->count_);
         ++shift) {
      nxt = 0;
      for (std::size_t first = 0; first <= shift; ++first) {
        if (first >= sz1)
          break;
        std::size_t second = shift - first;
        if (second >= sz2)
          continue;
        tmp = static_cast<uint64>(c1[first]) * static_cast<uint64>(c2[second]);
        cur += static_cast<uint32>(tmp);
        nxt += tmp >> 32;
      }
      this->chunks_[shift] = static_cast<uint32>(cur);
      cur = nxt + (cur >> 32);
    }
    if (cur && (this->count_ != N)) {
      this->chunks_[this->count_] = static_cast<uint32>(cur);
      ++this->count_;
    }
  }

  uint32 chunks_[N];
  int32 count_;
};

template <std::size_t N>
bool is_pos(const extended_int<N>& that) {
  return that.count() > 0;
}

template <std::size_t N>
bool is_neg(const extended_int<N>& that) {
  return that.count() < 0;
}

template <std::size_t N>
bool is_zero(const extended_int<N>& that) {
  return !that.count();
}

struct type_converter_fpt {
  template <typename T>
  fpt64 operator()(const T& that) const {
    return static_cast<fpt64>(that);
  }

  template <std::size_t N>
  fpt64 operator()(const extended_int<N>& that) const {
    return that.d();
  }

  fpt64 operator()(const extended_exponent_fpt<fpt64>& that) const {
    return that.d();
  }
};

struct type_converter_efpt {
  template <std::size_t N>
  extended_exponent_fpt<fpt64> operator()(const extended_int<N>& that) const {
    std::pair<fpt64, int> p = that.p();
    return extended_exponent_fpt<fpt64>(p.first, p.second);
  }
};

// Voronoi coordinate type traits make it possible to extend algorithm
// input coordinate range to any user provided integer type and algorithm
// output coordinate range to any ieee-754 like floating point type.
template <typename T>
struct voronoi_ctype_traits;

template <>
struct voronoi_ctype_traits<int32> {
  typedef int32 int_type;
  typedef int64 int_x2_type;
  typedef uint64 uint_x2_type;
  typedef extended_int<64> big_int_type;
  typedef fpt64 fpt_type;
  typedef extended_exponent_fpt<fpt_type> efpt_type;
  typedef ulp_comparison<fpt_type> ulp_cmp_type;
  typedef type_converter_fpt to_fpt_converter_type;
  typedef type_converter_efpt to_efpt_converter_type;
};
}  // detail
}  // polygon
}  // boost

#endif  // BOOST_POLYGON_DETAIL_VORONOI_CTYPES