/*-----------------------------------------------------------------------------+
Copyright (c) 2010-2010: Joachim Faulhaber
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#ifndef BOOST_ICL_CONCEPT_ELEMENT_MAP_HPP_JOFA_100921
#define BOOST_ICL_CONCEPT_ELEMENT_MAP_HPP_JOFA_100921

#include <boost/mpl/and.hpp>
#include <boost/mpl/not.hpp>
#include <boost/icl/detail/on_absorbtion.hpp>
#include <boost/icl/type_traits/is_total.hpp>
#include <boost/icl/type_traits/absorbs_identities.hpp>
#include <boost/icl/type_traits/is_associative_element_container.hpp>
#include <boost/icl/type_traits/is_combinable.hpp>

#include <boost/icl/concept/map_value.hpp>
#include <boost/icl/detail/map_algo.hpp>

namespace boost{ namespace icl
{

//NOTE: Some forward declarations are needed by some compilers.
template<class Type, class Predicate>
typename enable_if<is_associative_element_container<Type>, Type>::type&
erase_if(const Predicate& pred, Type& object);

//==============================================================================
//= Containedness<ElementMap>
//==============================================================================
//------------------------------------------------------------------------------
//- bool within(c P&, c T&) T:{m} P:{b} fragment_types
//------------------------------------------------------------------------------
/** Checks if a key-value pair is in the map */
template<class Type>
typename enable_if<is_element_map<Type>, bool>::type
within(const typename Type::element_type& value_pair, const Type& super)
{
    typedef typename Type::const_iterator const_iterator;
    const_iterator found_ = super.find(value_pair.first);
    return found_ != super.end() && (*found_).second == value_pair.second;
}

//------------------------------------------------------------------------------
//- bool contains(c T&, c P&) T:{m} P:{b} fragment_types
//------------------------------------------------------------------------------
template<class Type>
typename enable_if<is_element_map<Type>, bool>::type
contains(const Type& super, const typename Type::element_type& value_pair)
{
    return icl::within(value_pair, super);
}

//==============================================================================
//= Equivalences and Orderings<ElementMap>
//==============================================================================

/** Protonic equality is equality on all elements that do not carry an identity element as content. */
template<class Type>
inline typename enable_if<is_element_map<Type>, bool>::type
is_distinct_equal(const Type& lhs, const Type& rhs)
{
    return Map::lexicographical_distinct_equal(lhs, rhs);
}

//==============================================================================
//= Addition<ElementMap>
//==============================================================================
/** \c add inserts \c value_pair into the map if it's key does
    not exist in the map.
    If \c value_pairs's key value exists in the map, it's data
    value is added to the data value already found in the map. */
template <class Type>
typename enable_if<is_element_map<Type>, Type>::type&
add(Type& object, const typename Type::value_type& value_pair)
{
    return object.add(value_pair);
}

/** \c add add \c value_pair into the map using \c prior as a hint to
    insert \c value_pair after the position \c prior is pointing to. */
template <class Type>
typename enable_if<is_element_map<Type>, typename Type::iterator>::type
add(Type& object, typename Type::iterator prior,
    const typename Type::value_type& value_pair)
{
    return object.add(prior, value_pair);
}

//==============================================================================
//= Erasure
//==============================================================================
//------------------------------------------------------------------------------
//- T& erase(T&, c P&) T:{m} P:{b} fragment_type
//------------------------------------------------------------------------------
template <class Type>
typename enable_if<is_element_map<Type>, typename Type::size_type>::type
erase(Type& object, const typename Type::element_type& value_pair)
{
    typedef typename Type::size_type             size_type;
    typedef typename Type::iterator              iterator;
    typedef typename Type::on_identity_absorbtion on_identity_absorbtion;

    if(on_identity_absorbtion::is_absorbable(value_pair.second))
        return identity_element<size_type>::value();

    iterator it_ = object.find(value_pair.first);
    if(it_ != object.end() && value_pair.second == (*it_).second)
    {
        object.erase(it_);
        return unit_element<size_type>::value();
    }

    return identity_element<size_type>::value();
}

template<class Type>
typename enable_if<is_element_map<Type>, Type>::type&
erase(Type& object, const typename Type::set_type& erasure)
{
    typedef typename Type::set_type set_type;
    ICL_const_FORALL(typename set_type, elem_, erasure)
        icl::erase(object, *elem_);

    return object;
}

//==============================================================================
//= Subtraction
//==============================================================================
//------------------------------------------------------------------------------
//- T& subtract(T&, c P&) T:{m} P:{b} fragment_type
//------------------------------------------------------------------------------
template <class Type>
inline typename enable_if<is_element_map<Type>, Type>::type&
subtract(Type& object, const typename Type::element_type& operand)
{
    return object.subtract(operand);
}

//------------------------------------------------------------------------------
//- T& subtract(T&, c P&) T:{m} P:{e} key_type
//------------------------------------------------------------------------------
template <class Type>
typename enable_if<is_element_map<Type>, Type>::type&
subtract(Type& object, const typename Type::domain_type& key_value)
{
    return icl::erase(object, key_value);
}

//------------------------------------------------------------------------------
//- T& subtract(T&, c P&) T:{m} P:{s} set key_type
//------------------------------------------------------------------------------
template <class Type>
inline typename enable_if<is_element_map<Type>, Type>::type&
operator -= (Type& object, const typename Type::set_type& operand)
{
    typedef typename Type::set_type           set_type;
    typedef typename set_type::const_iterator co_iterator;
    typedef typename Type::iterator           iterator;

    co_iterator common_lwb_, common_upb_;
    if(!Set::common_range(common_lwb_, common_upb_, operand, object))
        return object;

    co_iterator it_ = common_lwb_;
    iterator    common_;

    while(it_ != common_upb_)
        object.erase(*it_++);

    return object;
}

template <class Type>
inline typename enable_if<is_element_map<Type>, Type>::type
operator - (Type object, const typename Type::set_type& subtrahend)
{
    return object -= subtrahend;
}

//==============================================================================
//= Selective Update<ElementMap>
//==============================================================================
//------------------------------------------------------------------------------
//- T& set_at(T&, c P&) T:{m} P:{b}
//------------------------------------------------------------------------------
template<class Type>
inline typename enable_if<is_element_map<Type>, Type>::type&
set_at(Type& object, const typename Type::element_type& operand)
{
    typedef typename Type::iterator         iterator;
    typedef typename Type::codomain_combine codomain_combine;
    typedef on_absorbtion<Type,codomain_combine,absorbs_identities<Type>::value>
                                            on_identity_absorbtion;

    if(!on_identity_absorbtion::is_absorbable(operand.second))
    {
        std::pair<iterator,bool> insertion = object.insert(operand);
        if(!insertion.second)
            insertion->second = operand.second;
    }
    return object;
}

//==============================================================================
//= Intersection
//==============================================================================
template<class Type>
inline typename enable_if<is_element_map<Type>, void>::type
add_intersection(Type& section, const Type&               object,
                       const typename Type::element_type& operand)
{
    object.add_intersection(section, operand);
}

template<class Type>
inline typename enable_if<is_element_map<Type>, void>::type
add_intersection(Type& section, const Type& object, const Type& operand)
{
    ICL_const_FORALL(typename Type, it_, operand)
        icl::add_intersection(section, object, *it_);
}

//------------------------------------------------------------------------------
//- T& op &=(T&, c P&) T:{m} P:{b m} fragment_types
//------------------------------------------------------------------------------

template<class Type>
inline typename enable_if<mpl::and_<is_element_map<Type>, is_total<Type> >, Type>::type&
operator &=(Type& object, const typename Type::element_type& operand)
{
    object.add(operand);
    return object;
}

template<class Type>
inline typename enable_if<mpl::and_<is_element_map<Type>, mpl::not_<is_total<Type> > >, Type>::type&
operator &=(Type& object, const typename Type::element_type& operand)
{
    Type section;
    icl::add_intersection(section, object, operand);
    object.swap(section);
    return object;
}

template<class Type>
inline typename enable_if<is_element_map<Type>, Type>::type
operator & (Type object, const typename Type::element_type& operand)
{
    return object &= operand;
}

template<class Type>
inline typename enable_if<is_element_map<Type>, Type>::type
operator & (const typename Type::element_type& operand, Type object)
{
    return object &= operand;
}

template<class Type>
inline typename enable_if<mpl::and_<is_element_map<Type>, is_total<Type> >, Type>::type&
operator &=(Type& object, const Type& operand)
{
    object += operand;
    return object;
}

template<class Type>
inline typename enable_if<mpl::and_<is_element_map<Type>, mpl::not_<is_total<Type> > >, Type>::type&
operator &=(Type& object, const Type& operand)
{
    Type section;
    icl::add_intersection(section, object, operand);
    object.swap(section);
    return object;
}

template<class Type>
inline typename enable_if<is_element_map<Type>, Type>::type
operator & (Type object, const typename Type::key_object_type& operand)
{
    return object &= operand;
}

template<class Type>
inline typename enable_if<is_element_map<Type>, Type>::type
operator & (const typename Type::key_object_type& operand, Type object)
{
    return object &= operand;
}

//==============================================================================
//= Intersection<ElementMap> bool intersects(x,y)
//==============================================================================
template<class Type, class CoType>
inline typename enable_if< mpl::and_< is_element_map<Type>
                                    , is_total<Type>      >
                         , bool>::type
intersects(const Type&, const CoType&)
{
    return true;
}

template<class Type>
inline typename enable_if< mpl::and_< is_element_map<Type>
                                    , mpl::not_<is_total<Type> > >
                         , bool>::type
intersects(const Type& object, const typename Type::domain_type& operand)
{
    return icl::contains(object, operand);
}

template<class Type>
inline typename enable_if< mpl::and_< is_element_map<Type>
                                    , mpl::not_<is_total<Type> > >
                         , bool>::type
intersects(const Type& object, const typename Type::set_type& operand)
{
    if(object.iterative_size() < operand.iterative_size())
        return Map::intersects(object, operand);
    else
        return Map::intersects(operand, object);
}

template<class Type>
inline typename enable_if< mpl::and_< is_element_map<Type>
                                    , mpl::not_<is_total<Type> > >
                         , bool>::type
intersects(const Type& object, const typename Type::element_type& operand)
{
    Type intersection;
    icl::add_intersection(intersection, object, operand);
    return !intersection.empty();
}

template<class Type>
inline typename enable_if< mpl::and_< is_element_map<Type>
                                    , mpl::not_<is_total<Type> > >
                         , bool>::type
intersects(const Type& object, const Type& operand)
{
    if(object.iterative_size() < operand.iterative_size())
        return Map::intersects(object, operand);
    else
        return Map::intersects(operand, object);
}

//==============================================================================
//= Symmetric difference
//==============================================================================
template<class Type>
inline typename enable_if<is_element_map<Type>, Type>::type&
flip(Type& object, const typename Type::element_type& operand)
{
    return object.flip(operand);
}

template<class Type, class CoType>
inline typename enable_if< mpl::and_< is_element_map<Type>
                                    , is_total<Type>
                                    , absorbs_identities<Type> >
                         , Type>::type&
operator ^= (Type& object, const CoType&)
{
    icl::clear(object);
    return object;
}

template<class Type>
inline typename enable_if< mpl::and_< is_element_map<Type>
                                    , is_total<Type>
                                    , mpl::not_<absorbs_identities<Type> > >
                         , Type>::type&
operator ^= (Type& object, const typename Type::element_type& operand)
{
    return object.flip(operand);
}

template<class Type>
inline typename enable_if< mpl::and_< is_element_map<Type>
                                    , is_total<Type>
                                    , mpl::not_<absorbs_identities<Type> > >
                         , Type>::type&
operator ^= (Type& object, const Type& operand)
{
    ICL_const_FORALL(typename Type, it_, operand)
        icl::flip(object, *it_);

    ICL_FORALL(typename Type, it2_, object)
        (*it2_).second = identity_element<typename Type::codomain_type>::value();

    return object;
}

template<class Type>
inline typename enable_if< mpl::and_< is_element_map<Type>
                                    , mpl::not_<is_total<Type> > >
                         , Type>::type&
operator ^= (Type& object, const typename Type::element_type& operand)
{
    return icl::flip(object, operand);
}

template<class Type>
inline typename enable_if< mpl::and_< is_element_map<Type>
                                    , mpl::not_<is_total<Type> > >
                         , Type>::type&
operator ^= (Type& object, const Type& operand)
{
    typedef typename Type::const_iterator const_iterator;
    const_iterator it_ = operand.begin();
    while(it_ != operand.end())
        icl::flip(object, *it_++);

    return object;
}

//==============================================================================
//= Set selection
//==============================================================================
template<class Type>
inline typename enable_if<is_element_map<Type>,
                          typename Type::set_type>::type&
domain(typename Type::set_type& domain_set, const Type& object)
{
    typename Type::set_type::iterator prior_ = domain_set.end();
    typename Type::const_iterator it_ = object.begin();
    while(it_ != object.end())
        prior_ = domain_set.insert(prior_, (*it_++).first);

    return domain_set;
}

//==============================================================================
//= Neutron absorbtion
//==============================================================================
template<class Type>
inline typename enable_if<mpl::and_< is_element_map<Type>
                                   , absorbs_identities<Type> >, Type>::type&
absorb_identities(Type& object)
{
    typedef typename Type::element_type element_type;
    return icl::erase_if(content_is_identity_element<element_type>(), object);
}

template<class Type>
inline typename enable_if<mpl::and_< is_element_map<Type>
                                   , mpl::not_<absorbs_identities<Type> > >
                         , Type>::type&
absorb_identities(Type&){}

//==============================================================================
//= Streaming<ElementMap>
//==============================================================================
template<class CharType, class CharTraits, class Type>
inline typename enable_if<is_element_map<Type>, std::basic_ostream<CharType, CharTraits> >::type&
operator << (std::basic_ostream<CharType, CharTraits>& stream, const Type& object)
{
    stream << "{";
    ICL_const_FORALL(typename Type, it, object)
        stream << "(" << it->first << "->" << it->second << ")";

    return stream << "}";
}

}} // namespace boost icl

#endif


// Boost.Geometry Index
// Unit Test

// Copyright (c) 2011-2013 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_TEST_UNION_CONTENT_HPP
#define BOOST_GEOMETRY_INDEX_TEST_UNION_CONTENT_HPP

#include <geometry_index_test_common.hpp>

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

template <typename Geometry>
void test_union_content(Geometry const& geometry1, Geometry const& geometry2,
            typename bgi::detail::default_content_result<Geometry>::type expected_value)
{
    typename bgi::detail::default_content_result<Geometry>::type value = bgi::detail::union_content(geometry1, geometry2);

#ifdef GEOMETRY_TEST_DEBUG
    std::ostringstream out;
    out << typeid(typename bg::coordinate_type<Geometry>::type).name()
        << " "
        << typeid(typename bgi::detail::default_content_result<Geometry>::type).name()
        << " "
        << "union_content : " << value
        << std::endl;
    std::cout << out.str();
#endif

    BOOST_CHECK_CLOSE(value, expected_value, 0.0001);
}

template <typename Geometry>
void test_geometry(std::string const& wkt1, std::string const& wkt2,
            typename bgi::detail::default_content_result<Geometry>::type expected_value)
{
    Geometry geometry1, geometry2;
    bg::read_wkt(wkt1, geometry1);
    bg::read_wkt(wkt2, geometry2);
    test_union_content(geometry1, geometry2, expected_value);
}

#endif


#include <armadillo>

void pointer_fun(const double * charges, size_t natoms) {
    std::cout << "charges:" << std::endl;
    for (size_t i = 0; i < natoms; i++) {
        const double charge = charges[i];
        std::cout << " " << charge << std::endl;
    }
}

int main() {

    const size_t ncharges = 10;
    int * charges = new int[ncharges];
    for (size_t i = 0; i < ncharges; i++) {
        charges[i] = i;
    }
    arma::Col<int> icharges(charges, ncharges, false, true);
    icharges.print("icharges");
    arma::vec dcharges = arma::conv_to<arma::vec>::from(icharges);
    dcharges.print("dcharges");
    pointer_fun(dcharges.memptr(), dcharges.n_elem);
    for (size_t i = 0; i < dcharges.n_elem; i++) {
        // no bounds check
        dcharges[i] = i + (i / 10.0);
    }
    pointer_fun(dcharges.memptr(), dcharges.n_elem);
    delete[] charges;

    return 0;

}


/** \file
 *  \brief    Implementation of the DAEDALUS algorithm.
 *  \details  DAEDALUS algorithm designs an origami figure with two helices
 *            per edge. Minimum edge length has to be at least 31bp, for which
 *            it uses single crossovers. Otherwise a DX-motif is employed.
 *  \author   Elisa de Llano <elisa.dellano.fl@ait.ac.at>
 */

#ifndef DASDAEDALUS_H
#define DASDAEDALUS_H

#include <vector>
#include <string>
#include <cmath>
#include <map>
#include <algorithm>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>
#include <boost/graph/copy.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/graphviz.hpp>
#include "DASPolyhedron.hpp"
#include "DASCreator.hpp"
#include "ADNNanorobot.hpp"
#include "ADNVectorMath.hpp"
#include "ADNConstants.hpp"

const double BP_RISE = ADNConstants::BP_RISE * 10;  // angstroms
const double SS_PADDING = 10;  //angstroms

struct DOTNode {
  DOTNode() = default;
  ~DOTNode() = default;
  DOTNode(const DOTNode& other);
  DOTNode& operator=(const DOTNode& other);

  std::vector<double> coordinates_;
  int id_;

  DASVertex* vertex_; // if it corresponds with a vertex
  DASEdge* edge_; // if it lies in an edge

  bool pseudo_ = false; // if it is a pseudo-node
};

struct DOTLink {
  DOTLink() = default;
  DOTLink(const DOTLink& other);
  ~DOTLink();
  DOTLink& operator=(const DOTLink& other);

  int id_;
  DOTNode* source_;
  DOTNode* target_;

  DASHalfEdge* halfEdge_;
  bool directed_;
  bool split_ = false;

  ADNPointer<ADNBaseSegment> firstBase_;
  ADNPointer<ADNBaseSegment> lastBase_;

  int bp_ = 0; // number of base pairs
};

typedef std::vector<DOTLink*> LinkList;
typedef std::map<int, DOTNode*> NodeList;
typedef std::pair<SBPosition3, SBPosition3> PositionPair;
typedef std::map<ADNPointer<ADNBaseSegment>, PositionPair> BSPositions;

struct VertexProperty {
  /** Structure to store vertex properties.
   *  \param node object associated with this vertex.
   *  \param index assigned to the vertex when created.
   *  \param original index (for split edges).
   *  \param x coordinate in 2D space (for Schlegel diagrams).
   *  \param y coordinate in 2D space (for Schlegel diagrams).
   */
  DOTNode* node_;
  int index;
  //int pseudoindex;
  //int original_index;
  float x;
  float y;
};

struct EdgeProperty {
  DOTLink* link_;
  int weight_ = 1;
};

/** \enum Daedalus::CrossoverType
 *  \brief The possible crossover types in DASDaedalus.
 */
enum CrossoverType {
  kSingleXO,
  kDoubleXO,
};

//typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
//  VertexProperty, boost::property<boost::edge_weight_t, int>> UndirectedGraph;
typedef boost::adjacency_list < boost::vecS, boost::vecS, boost::undirectedS, VertexProperty, EdgeProperty> UndirectedGraph;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::bidirectionalS, VertexProperty, EdgeProperty> DirectedGraph;
template<typename T>
using Edge = typename boost::graph_traits<T>::edge_descriptor;
template<typename T>
using Node = typename boost::graph_traits<T>::vertex_descriptor;
template<typename T>
using NodeIt = typename boost::graph_traits<T>::vertex_iterator;
template<typename T>
using EdgeIt = typename boost::graph_traits<T>::edge_iterator;
typedef std::vector<Edge<UndirectedGraph>> Path;

class DASDaedalus {
public:
  /** Destructor
   */
  ~DASDaedalus();
  /**
   *  \param ANTNanorobot object with a mesh.
   *  \output Origami object.
   */
  ADNPointer<ADNPart> ApplyAlgorithm(std::string seq, std::string filename, bool center = true);
  /**
  *  \param ANTNanorobot object with a mesh as ANTPolyhedron.
  *  \output Origami object.
  */
  ADNPointer<ADNPart> ApplyAlgorithm(std::string seq, DASPolyhedron & polyhedron, bool center = true, bool editor = false);
  /** Setters
   */
  void SetMinEdgeLength(int l);
  void SetVerticesPositions(ADNPointer<ADNPart> origami, DASPolyhedron &fig, bool center = true);
  /**
  * Calculates the size in integer multiples of 10.5bp
  */
  static int CalculateEdgeSize(SBQuantity::length nmLength);
  /** Initializes the edge map object.
 *
 *  We generate now the base objects to make sure we don't forget any position.
 *  \param origami ANTNanorobot object
 */
  void InitEdgeMap(ADNPointer<ADNPart> origami, DASPolyhedron &fig);
  //! Generate geometric model
  void InitEdgeMap2(ADNPointer<ADNPart> origami, DASPolyhedron &fig);

  void SetEdgeBps(int min_edge_bp, ADNPointer<ADNPart> part, DASPolyhedron &fig);
protected:
private:
  /** Minimum edge length
   */
  int min_edge_length_ = 42; // default 42 bp
  /** Edge staple spanning size
   */
  int edge_staple_span_ = 21; // bp
  /** Vertex staple span at the beginning of an edge
   */
  int vertex_staple_span_start_= 10;
  /** Vertex staple span at the end of an edge
   */
  int vertex_staple_span_end_= 11;
  /** Size of the single crossover staples
   */
  int crossover_staple_span_ = 10;
  /** Crossover staple split
   */
  int crossover_split_ = 16;
  /** Number of Poly-T in vertex staples
   */
  int num_poly_t_ = 5;
  /** Map ids with Node objects
  */
  std::map<int, DOTNode*> nodes_;
  /** map with the length of edges */
  EdgeBps bpLengths_;
  /** List with links
  */
  LinkList linkGraph_;
  std::map<int, ADNPointer<ADNSingleStrand>> chains_;
  std::map<DASHalfEdge*, ADNPointer<ADNBaseSegment>> firstBasesHe_;
  std::map<int, int> bsPairs_;
  BSPositions positionsBBSC_;
  /** Generates an undirected graph from figure from current nodes and links
   *  \return an UndirectedGraph object
   */
  UndirectedGraph GenerateUndirectedGraph();
  /** Create directed graph
   *  \param undirectedGraph
   *  \param map with position of bases
   *  \return DirectedGraph, direction is chosen randomly.
   */
  DirectedGraph GenerateDirectedGraph(const UndirectedGraph &u_graph, ADNPointer<ADNPart> origami);
  /** Computes the MST from an undirected graph
   *  \param UndirectedGraph object
   *  \return a vector with the edges in the order they are accessed
   */
  std::vector<DOTLink*> MinimumSpanningTree(const UndirectedGraph &u_graph);
  /** Get the Eulerian Circuit from a cyclic graph
   *  \param the cyclic graph
   *  \return a vector with the nodes in order of visitation
   */
  std::vector<DOTNode*> GetEulerianCircuit(UndirectedGraph g);
  /** Get the neighbors from a node in a graph
   *  \param the node.
   *  \param the graph.
   *  \return a vector with the nodes connected to the input node.
   */
  std::vector<Node<UndirectedGraph>> GetNeighbours(Node<UndirectedGraph> v, UndirectedGraph g);
  /** Split edges not in MST.
   *  Edges not belonging to MST are split in half to allow scaffold crossovers.
   *  \param vector with MST
   *  \param target object
   */
  void SplitEdges(const std::vector<DOTLink*> &mst, ADNPointer<ADNPart> origami, DASPolyhedron &figure);
  /** Adds Pseudo-nodes so the A-trail can be routed.
   *  \param the target polyhedron
   */
  void AddPseudoNodes(DASPolyhedron &figure);
  /** Length of the routing
   *  \param Length of every edge
   *  \return the total length of the routing in base pairs.
   */
  int RoutingLength(EdgeBps &lengths);

  void CreateEdgeStaples(ADNPointer<ADNPart> origami);
  void CreateVertexStaples(ADNPointer<ADNPart> origami, DASPolyhedron &figure);
  /** Wrapper to add an edge to a graph if it doesn't exist
   *  \param id of the source node
   *  \param id of the target node
   *  \param graph either directed or undirected
   */
  template<typename T>
  void AddEdge(int id_source, int id_target, DOTLink* link, T &graph);
  /** Returns the crossover type.
   *  \param the length of the edge with the crossover.
   *  \return the type of crossover.
   *
   *  If min_edge_length_ = 31, the crossover may fall on a 31bp edge and using
   *  two 21bp staples is not possible.
   */
  CrossoverType GetCrossoverType(int edge_length);
  /** Get the length of an edge pair. Includes split edges.
   *  \param the edge pair.
   *  \param the edge map.
   */
  void CreateLinkGraphFromMesh(ADNPointer<ADNPart> p, DASPolyhedron &figure);
  void AddNode(DOTNode* node);
  void RemoveLink(DOTLink* link);
  DOTLink* AddLink(std::pair<DOTNode*, DOTNode*> ab, int bp, DASPolyhedron &fig);
  DOTNode* GetNodeById(int id) const;
  DOTLink* GetLinkByNodes(DOTNode* v, DOTNode* w) const;
  void RouteScaffold(ADNPointer<ADNPart> part, ADNPointer<ADNSingleStrand> scaff, std::string seq, int routing_length);
  ADNPointer<ADNBaseSegment> AdvanceBaseSegment(ADNPointer<ADNBaseSegment> bs, int pos);
  ADNPointer<ADNBaseSegment> MoveBackBaseSegment(ADNPointer<ADNBaseSegment> bs, int pos);
  ADNPointer<ADNNucleotide> AdvanceNucleotide(ADNPointer<ADNNucleotide> nt, int pos);
  ADNPointer<ADNSingleStrand> CreateEdgeChain(ADNPointer<ADNPart> origami, ADNPointer<ADNBaseSegment> bs, int c_id, int pos_span, int neg_span);
  ADNPointer<ADNSingleStrand> CreateVertexChain(ADNPointer<ADNPart> part, int c_id, std::vector<DASHalfEdge*>ps, EdgeBps &lengths);
  void LogEdgeMap(ADNPointer<ADNPart> origami);
  static SBVector3 SBCrossProduct(SBVector3 v, SBVector3 w);
  double SBInnerProduct(SBVector3 v, SBVector3 w);
  void LogLinkGraph();
  template <typename T>
  static void OutputGraph(T g, std::string filename);
  SBVector3 GetPolygonNorm(DASPolygon* face);
  ADNPointer<ADNBaseSegment> FindBaseSegmentPair(ADNPointer<ADNPart> origami, ADNPointer<ADNBaseSegment> bs);
  std::map<DASHalfEdge*, SBPosition3> GetVertexPositions(DASPolyhedron &fig);
};

#endif


// Copyright (c) Microsoft Corporation.
// Licensed under the MIT License.

/** File containing all non-primitive creation methods for Mesh (see
 * primitives.cpp) */

#include "mesh.h"

#include "transforms.h"
#include "util.h"

#include <Eigen/Geometry>
#include <array>
#include <climits>
#include <exception>
#include <map>
#include <utility>

namespace scenepic
{
  Vector compute_triangle_normal(
    const Vector& pos0, const Vector& pos1, const Vector& pos2)
  {
    Vector p01 = pos1 - pos0;
    Vector p02 = pos2 - pos0;
    Vector normal = p01.cross(p02);
    return normal.normalized();
  }

  Mesh::Mesh(const Color& shared_color, const std::string& texture_id)
  : Mesh("")
  {
    this->shared_color(shared_color);
    this->texture_id(texture_id);
  }

  Mesh::Mesh(const std::string& mesh_id)
  : m_mesh_id(mesh_id),
    m_shared_color(Color::None()),
    m_texture_id(""),
    m_layer_id(""),
    m_camera_space(false),
    m_double_sided(false),
    m_is_billboard(false),
    m_is_label(false),
    m_nn_texture(true),
    m_use_texture_alpha(false),
    m_vertices(VertexBuffer::Zero(0, 6)),
    m_triangles(TriangleBuffer::Zero(0, 3)),
    m_lines(LineBuffer::Zero(0, 2))
  {
    bool vertex_colors = m_shared_color.is_none();
    bool vertex_uvs = !m_texture_id.empty();
    if (vertex_colors && vertex_uvs)
    {
      throw std::invalid_argument(
        "Cannot have both vertex colors and vertex UVs");
    }
    else if (vertex_colors)
    {
      m_vertices = VertexBuffer::Zero(0, 9);
    }
    else if (vertex_uvs)
    {
      m_vertices = VertexBuffer::Zero(0, 8);
    }
  }

  std::uint32_t Mesh::count_vertices() const
  {
    return static_cast<std::uint32_t>(m_vertices.rows());
  }

  Vector Mesh::center_of_mass() const
  {
    return m_vertices.leftCols(3).rowwise().mean();
  }

  void Mesh::reverse_triangle_order()
  {
    m_triangles.col(1).swap(m_triangles.col(2));
    m_vertices.block(0, 3, this->count_vertices(), 3) *= -1;
  }

  void Mesh::apply_transform(const Transform& transform)
  {
    WorldVectorMatrix positions(this->count_vertices(), 4);
    positions.leftCols(3) = m_vertices.leftCols(3);
    positions.col(3).setConstant(1.0);
    positions *= transform.transpose();
    m_vertices.leftCols(3) = positions.leftCols(3);

    auto norm_transform = transform.topLeftCorner(3, 3).inverse();
    this->vertex_normals() *= norm_transform;
    this->vertex_normals().rowwise().normalize();
  }

  void Mesh::apply_rotation(const Transform& transform)
  {
    auto rotation = transform.topLeftCorner(3, 3).transpose();
    this->vertex_positions() *= rotation;
    this->vertex_normals() *= rotation;
  }

  Mesh Mesh::get_transformed(const Transform& transform)
  {
    Mesh mesh = *this;
    mesh.apply_transform(transform);
    return mesh;
  }

  Mesh Mesh::get_rotated(const Transform& transform)
  {
    Mesh mesh = *this;
    mesh.apply_rotation(transform);
    return mesh;
  }

  void Mesh::append_mesh(const Mesh& mesh)
  {
    auto vert_offset = this->count_vertices();
    append_matrix(m_vertices, mesh.m_vertices);
    TriangleBuffer triangles = mesh.m_triangles.array() + vert_offset;
    LineBuffer lines = mesh.m_lines.array() + vert_offset;
    append_matrix(m_triangles, triangles);
    append_matrix(m_lines, lines);
  }

  void Mesh::add_triangle(
    const Color& color,
    const Vector& p0,
    const Vector& p1,
    const Vector& p2,
    const Vector& n,
    bool fill_triangles,
    bool add_wireframe,
    const UV& uv0,
    const UV& uv1,
    const UV& uv2)
  {
    this->check_instances();
    this->check_color(color);
    Vector normal = n;
    if (normal == VectorNone())
    {
      normal = compute_triangle_normal(p0, p1, p2);
    }

    std::uint32_t i0, i1, i2;
    if (m_texture_id.empty())
    {
      i0 = this->append_vertex(p0, normal, color);
      i1 = this->append_vertex(p1, normal, color);
      i2 = this->append_vertex(p2, normal, color);
    }
    else
    {
      i0 = this->append_vertex(p0, normal, uv0);
      i1 = this->append_vertex(p1, normal, uv1);
      i2 = this->append_vertex(p2, normal, uv2);
    }

    if (fill_triangles)
    {
      this->append_triangle(i0, i1, i2);
    }

    if (add_wireframe)
    {
      this->append_line(i0, i1);
      this->append_line(i1, i2);
      this->append_line(i2, i0);
    }
  }

  void Mesh::add_quad(
    const Color& color,
    const Vector& p0,
    const Vector& p1,
    const Vector& p2,
    const Vector& p3,
    const Vector& n,
    bool fill_triangles,
    bool add_wireframe,
    const Transform& transform,
    const UV& uv0,
    const UV& uv1,
    const UV& uv2,
    const UV& uv3)
  {
    this->check_instances();
    this->check_color(color);
    Vector normal = n;
    if (normal == VectorNone())
    {
      normal = compute_triangle_normal(p0, p1, p2);
    }

    Mesh m = Mesh("").shared_color(m_shared_color).texture_id(m_texture_id);

    std::uint32_t i0, i1, i2, i3;
    if (m_texture_id.empty())
    {
      i0 = m.append_vertex(p0, normal, color);
      i1 = m.append_vertex(p1, normal, color);
      i2 = m.append_vertex(p2, normal, color);
      i3 = m.append_vertex(p3, normal, color);
    }
    else
    {
      i0 = m.append_vertex(p0, normal, uv0);
      i1 = m.append_vertex(p1, normal, uv1);
      i2 = m.append_vertex(p2, normal, uv2);
      i3 = m.append_vertex(p3, normal, uv3);
    }

    if (fill_triangles)
    {
      m.append_triangle(i0, i1, i2);
      m.append_triangle(i0, i2, i3);
    }

    if (add_wireframe)
    {
      m.append_line(i0, i1);
      m.append_line(i1, i2);
      m.append_line(i2, i3);
      m.append_line(i3, i0);
    }

    if (!transform.isIdentity())
    {
      m.apply_transform(transform);
    }

    this->append_mesh(m);
  }

  void Mesh::add_image(
    const Vector& origin,
    const Vector& x_axis,
    const Vector& y_axis,
    const Vector& n,
    const UV& uv0,
    const UV& uv1,
    const UV& uv2,
    const UV& uv3,
    bool double_sided,
    const Transform& transform)
  {
    this->check_instances();
    if (m_texture_id.length() == 0)
    {
      throw std::logic_error("Must set mesh's texture_id property first");
    }

    Vector p0 = origin;
    Vector p1 = p0 + x_axis;
    Vector p2 = p1 + y_axis;
    Vector p3 = p0 + y_axis;
    Vector normal = n;
    if (normal == VectorNone())
    {
      normal = compute_triangle_normal(p0, p1, p2);
    }

    Mesh m = Mesh("").texture_id(m_texture_id);

    // Front face
    auto i0 = m.append_vertex(p0, normal, uv0);
    auto i1 = m.append_vertex(p1, normal, uv1);
    auto i2 = m.append_vertex(p2, normal, uv2);
    auto i3 = m.append_vertex(p3, normal, uv3);
    m.append_triangle(i0, i1, i2);
    m.append_triangle(i0, i2, i3);

    // Back face
    if (double_sided)
    {
      i0 = m.append_vertex(p0, -normal, uv0);
      i1 = m.append_vertex(p1, -normal, uv1);
      i2 = m.append_vertex(p2, -normal, uv2);
      i3 = m.append_vertex(p3, -normal, uv3);
      m.append_triangle(i0, i2, i1);
      m.append_triangle(i0, i3, i2);
    }

    if (!transform.isIdentity())
    {
      m.apply_transform(transform);
    }

    this->append_mesh(m);
  }

  void Mesh::add_mesh(
    const std::shared_ptr<MeshInfo>& mesh_info,
    const Transform& transform,
    bool reverse_triangle_order,
    bool fill_triangles,
    bool add_wireframe)
  {
    if (mesh_info->has_normals())
    {
      this->add_mesh_with_normals(
        mesh_info->position_buffer(),
        mesh_info->normal_buffer(),
        mesh_info->triangle_buffer(),
        mesh_info->color_buffer(),
        mesh_info->uv_buffer(),
        transform,
        reverse_triangle_order,
        fill_triangles,
        add_wireframe);
    }
    else
    {
      this->add_mesh_without_normals(
        mesh_info->position_buffer(),
        mesh_info->triangle_buffer(),
        mesh_info->color_buffer(),
        mesh_info->uv_buffer(),
        transform,
        reverse_triangle_order,
        fill_triangles,
        add_wireframe);
    }
  }

  VectorBuffer Mesh::compute_normals(
    const ConstVectorBufferRef& vertices,
    const ConstTriangleBufferRef& triangles,
    bool reverse_triangle_order)
  {
    // Calculate per-face normals for the entire mesh, and normalize them
    VectorBuffer per_face_normals(triangles.rows(), 3);
    for (auto index = 0; index < triangles.rows(); ++index)
    {
      const Triangle& triangle = triangles.row(index);
      const Vector& p0 = vertices.row(triangle(0));
      const Vector& p1 = vertices.row(triangle(1));
      const Vector& p2 = vertices.row(triangle(2));
      per_face_normals.row(index) = (p1 - p0).cross(p2 - p0);
    }

    per_face_normals.rowwise().normalize();

    if (reverse_triangle_order)
    {
      per_face_normals *= -1;
    }

    // For each triangle, add that triangle's normal to each vertex in the
    // triangle
    VectorBuffer normals = VectorBuffer::Zero(vertices.rows(), 3);
    for (auto index = 0; index < triangles.rows(); ++index)
    {
      const Triangle& triangle = triangles.row(index);
      const Vector& normal = per_face_normals.row(index);
      normals.row(triangle(0)) += normal;
      normals.row(triangle(1)) += normal;
      normals.row(triangle(2)) += normal;
    }

    normals.rowwise().normalize();

    return normals;
  }

  void Mesh::add_mesh_without_normals(
    const ConstVectorBufferRef& vertices,
    const ConstTriangleBufferRef& triangles,
    const ConstColorBufferRef& colors,
    const ConstUVBufferRef& uvs,
    const Transform& transform,
    bool reverse_triangle_order,
    bool fill_triangles,
    bool add_wireframe)
  {
    this->check_instances();

    auto normals = Mesh::compute_normals(vertices, triangles);

    this->add_mesh_with_normals(
      vertices,
      normals,
      triangles,
      colors,
      uvs,
      transform,
      reverse_triangle_order,
      fill_triangles,
      add_wireframe);
  }

  void Mesh::add_mesh_with_normals(
    const ConstVectorBufferRef& vertices,
    const ConstVectorBufferRef& normals,
    const ConstTriangleBufferRef& triangles,
    const ConstColorBufferRef& colors,
    const ConstUVBufferRef& uvs,
    const Transform& transform,
    bool reverse_triangle_order,
    bool fill_triangles,
    bool add_wireframe)
  {
    assert(vertices.rows() == normals.rows());
    this->check_instances();

    bool has_texture = m_texture_id.length() > 0;
    bool has_vertex_colors = m_shared_color.is_none();
    bool has_uvs = uvs.rows() > 0;

    if (!has_texture && has_uvs)
    {
      throw std::invalid_argument(
        "Must create Mesh with texture_id in order to use uvs");
    }

    if (has_vertex_colors && has_uvs)
    {
      throw std::invalid_argument(
        "The use of vertex colors and uvs together is not supported");
    }

    if (has_vertex_colors && colors.rows() == 0)
    {
      throw std::invalid_argument(
        "Per-vertex colors must be provided unless the mesh has a single "
        "color "
        "or texture map");
    }

    if (has_vertex_colors && colors.rows() != vertices.rows())
    {
      throw std::invalid_argument("Expecting per-vertex colors");
    }

    if (has_uvs && (uvs.rows() != vertices.rows()))
    {
      throw std::invalid_argument("Expecting per-vertex uvs stored in uvs");
    }

    Mesh m = Mesh("").shared_color(m_shared_color).texture_id(m_texture_id);
    if (has_uvs)
    {
      m.m_vertices = VertexBuffer(vertices.rows(), 8);
      m.vertex_positions() = vertices;
      m.vertex_normals() = normals;
      m.vertex_uvs() = uvs;
    }
    else if (has_vertex_colors)
    {
      m.m_vertices = VertexBuffer(vertices.rows(), 9);
      m.vertex_positions() = vertices;
      m.vertex_normals() = normals;
      m.vertex_colors() = colors;
    }
    else
    {
      m.m_vertices = VertexBuffer(vertices.rows(), 6);
      m.vertex_positions() = vertices;
      m.vertex_normals() = normals;
    }

    if (add_wireframe)
    {
      m.m_lines = LineBuffer(triangles.rows() * 3, 2);
      m.m_lines.topRows(triangles.rows()) = triangles.leftCols(2);
      m.m_lines.block(triangles.rows(), 0, triangles.rows(), 2) =
        triangles.block(0, 1, triangles.rows(), 2);
      m.m_lines.block(2 * triangles.rows(), 0, triangles.rows(), 1) =
        triangles.col(0);
      m.m_lines.block(2 * triangles.rows(), 1, triangles.rows(), 1) =
        triangles.col(2);
    }

    if (fill_triangles)
    {
      m.m_triangles = triangles;
      if (reverse_triangle_order)
      {
        m.m_triangles.col(1).swap(m.m_triangles.col(2));
      }
    }

    if (!transform.isIdentity())
    {
      m.apply_transform(transform);
    }

    this->append_mesh(m);
  }

  void Mesh::add_lines(
    const ConstVertexBufferRef& start_points,
    const ConstVertexBufferRef& end_points,
    const Color& color,
    const Transform& transform)
  {
    assert(start_points.rows() == end_points.rows());
    assert(start_points.cols() == end_points.cols());

    bool per_point_color = m_shared_color.is_none() && color.is_none();
    if (per_point_color && start_points.cols() != 6)
    {
      throw std::invalid_argument(
        "Expecting either single-color mesh, or shared color for whole set "
        "of "
        "lines, or per-point color stored in start_points and end_points");
    }

    Mesh m = Mesh("").shared_color(m_shared_color).texture_id(m_texture_id);
    const std::uint32_t num_lines =
      static_cast<const std::uint32_t>(start_points.rows());

    if (per_point_color)
    {
      m.m_vertices = VertexBuffer(num_lines * 2, 9);
      m.vertex_colors().topRows(num_lines) = start_points.rightCols(3);
      m.vertex_colors().bottomRows(num_lines) = end_points.rightCols(3);
    }
    else if (!color.is_none())
    {
      m.m_vertices = VertexBuffer(num_lines * 2, 9);
      m.vertex_colors().col(0).fill(color.r());
      m.vertex_colors().col(1).fill(color.g());
      m.vertex_colors().col(2).fill(color.b());
    }
    else
    {
      m.m_vertices = VertexBuffer(num_lines * 2, 6);
    }

    m.vertex_positions().topRows(num_lines) = start_points.leftCols(3);
    m.vertex_positions().bottomRows(num_lines) = end_points.leftCols(3);
    m.vertex_normals().col(0).fill(1);
    m.vertex_normals().col(1).fill(0);
    m.vertex_normals().col(2).fill(0);

    typedef Eigen::Array<std::uint32_t, Eigen::Dynamic, 1> LineInit;
    m.m_lines = LineBuffer(num_lines, 2);
    for (std::uint32_t row = 0; row < num_lines; ++row)
    {
      m.m_lines(row, 0) = row;
      m.m_lines(row, 1) = row + num_lines;
    }

    if (!transform.isIdentity())
    {
      m.apply_transform(transform);
    }

    this->append_mesh(m);
  }

  void Mesh::enable_instancing(
    const ConstVectorBufferRef& positions,
    const ConstQuaternionBufferRef& rotations,
    const ConstColorBufferRef& colors)
  {
    if (m_instance_buffer.rows())
    {
      std::cerr << "WARNING: multiple calls to enable_instancing will replace "
                   "existing instance_buffers."
                << std::endl;
    }

    m_instance_buffer_has_rotations = false;
    m_instance_buffer_has_colors = false;
    if (rotations.size() && colors.size())
    {
      assert(rotations.rows() == positions.rows());
      assert(colors.rows() == positions.rows());
      m_instance_buffer_has_rotations = true;
      m_instance_buffer_has_colors = true;
      m_instance_buffer = InstanceBuffer(positions.rows(), 10);
      m_instance_buffer.leftCols(3) = positions;
      m_instance_buffer.block(0, 3, positions.rows(), 4) = rotations;
      m_instance_buffer.rightCols(3) = colors;
    }
    else if (rotations.size())
    {
      assert(rotations.rows() == positions.rows());
      m_instance_buffer_has_rotations = true;
      m_instance_buffer = InstanceBuffer(positions.rows(), 7);
      m_instance_buffer.leftCols(3) = positions;
      m_instance_buffer.rightCols(4) = rotations;
    }
    else if (colors.size())
    {
      assert(colors.rows() == positions.rows());
      m_instance_buffer_has_colors = true;
      m_instance_buffer = InstanceBuffer(positions.rows(), 6);
      m_instance_buffer.leftCols(3) = positions;
      m_instance_buffer.rightCols(3) = colors;
    }
    else
    {
      m_instance_buffer = positions;
    }
  }

  JsonValue Mesh::definition_to_json() const
  {
    std::string data_type;
    JsonValue obj;

    obj["VertexBuffer"] = matrix_to_json(m_vertices);

    if (m_vertices.rows() < 0xFFFF)
    {
      TriangleShortBuffer triangles = m_triangles.cast<std::uint16_t>();
      LineShortBuffer lines = m_lines.cast<std::uint16_t>();
      obj["IndexBufferType"] = "UInt16";
      obj["TriangleBuffer"] = matrix_to_json(triangles);
      obj["LineBuffer"] = matrix_to_json(lines);
    }
    else
    {
      obj["IndexBufferType"] = "UInt32";
      obj["TriangleBuffer"] = matrix_to_json(m_triangles);
      obj["LineBuffer"] = matrix_to_json(m_lines);
    }

    if (!m_shared_color.is_none())
    {
      obj["PrimitiveType"] = "SingleColorMesh";
      obj["Color"] = matrix_to_json(m_shared_color);
    }
    else
    {
      obj["PrimitiveType"] = "MultiColorMesh";
    }

    if (m_texture_id.length() > 0)
    {
      obj["TextureId"] = m_texture_id;
      obj["NearestNeighborTexture"] = m_nn_texture;
      obj["UseTextureAlpha"] = m_use_texture_alpha;
    }

    if (m_instance_buffer.rows() > 0)
    {
      obj["InstanceBuffer"] = matrix_to_json(m_instance_buffer);
      obj["InstanceBufferHasRotations"] = m_instance_buffer_has_rotations;
      obj["InstanceBufferHasColors"] = m_instance_buffer_has_colors;
    }

    return obj;
  }

  JsonValue Mesh::to_json() const
  {
    JsonValue root;
    root["CommandType"] = "DefineMesh";
    root["MeshId"] = m_mesh_id;
    if (m_layer_id.empty())
    {
      root["LayerId"] = JsonValue::nullSingleton();
    }
    else
    {
      root["LayerId"] = m_layer_id;
    }
    root["DoubleSided"] = m_double_sided;
    root["Definition"] = this->definition_to_json();
    root["CameraSpace"] = m_camera_space;
    root["IsBillboard"] = m_is_billboard;
    root["IsLabel"] = m_is_label;
    return root;
  }

  std::uint32_t Mesh::append_vertex(
    const Vector& pos, const Vector& normal, const Color& color)
  {
    if (color.is_none())
    {
      return this->append_vertex(pos, normal);
    }

    std::uint32_t index = this->count_vertices();
    Vertex vertex(1, 9);
    vertex.segment(0, 3) = pos;
    vertex.segment(3, 3) = normal.normalized();
    vertex.segment(6, 3) = color;
    append_row(m_vertices, vertex);

    return index;
  }

  std::uint32_t
  Mesh::append_vertex(const Vector& pos, const Vector& normal, UV texture_uv)
  {
    std::uint32_t index = this->count_vertices();
    Vertex vertex(1, 8);
    vertex.segment(0, 3) = pos;
    vertex.segment(3, 3) = normal.normalized();
    vertex.segment(6, 2) = texture_uv;
    append_row(m_vertices, vertex);
    return index;
  }

  std::uint32_t Mesh::append_vertex(const Vector& pos, const Vector& normal)
  {
    std::uint32_t index = this->count_vertices();
    Vertex vertex(1, 6);
    vertex.segment(0, 3) = pos;
    vertex.segment(3, 3) = normal.normalized();
    append_row(m_vertices, vertex);
    return index;
  }

  void Mesh::append_triangle(
    std::uint32_t index0, std::uint32_t index1, std::uint32_t index2)
  {
    append_row(m_triangles, Triangle(index0, index1, index2));
  }

  void Mesh::append_line(std::uint32_t index0, std::uint32_t index1)
  {
    append_row(m_lines, Line(index0, index1));
  }

  bool Mesh::is_instanced() const
  {
    return m_instance_buffer.rows() > 0;
  }

  void Mesh::check_instances()
  {
    if (this->is_instanced())
    {
      std::cerr
        << "WARNING: Editing Mesh after calling enable_instancing"
        << "(used for point/line clouds) can lead to unexpected results."
        << std::endl;
    }
  }

  void Mesh::check_color(const Color& color)
  {
    if (color.is_none() && m_shared_color.is_none())
    {
      throw std::invalid_argument("Expected a vertex color");
    }
  }

  const Color& Mesh::shared_color() const
  {
    return m_shared_color;
  }

  Mesh& Mesh::shared_color(const Color& shared_color)
  {
    if (m_shared_color.is_none() && !shared_color.is_none())
    {
      // remove per-vertex color
      m_vertices = m_vertices.leftCols(6);
    }
    else if (!m_shared_color.is_none() && shared_color.is_none())
    {
      // attempt to add per-vertex color.
      if (this->count_vertices() == 0)
      {
        m_vertices = VertexBuffer::Zero(0, 9);
      }
      else
      {
        // this behavior is undefined if vertices exist
        throw std::invalid_argument(
          "Cannot add per-vertex color to a non-empty shared color mesh");
      }
    }

    m_shared_color = shared_color;
    return *this;
  }

  const std::string& Mesh::texture_id() const
  {
    return m_texture_id;
  }

  Mesh& Mesh::texture_id(const std::string& texture_id)
  {
    m_texture_id = texture_id;
    if (!texture_id.empty())
    {
      // convert this to a UV mesh
      if (this->count_vertices() == 0)
      {
        m_shared_color = Color(1, 1, 1);
        m_vertices = VertexBuffer::Zero(0, 8);
      }
      else
      {
        // behavior not defined if vertices exist
        throw std::invalid_argument(
          "Cannot convert a per-vertex color mesh to a UV mesh");
      }
    }
    else
    {
      if (!m_shared_color.is_none())
      {
        // Remove the texture coordinates
        m_vertices = m_vertices.leftCols(6);
      }
    }

    return *this;
  }

  const std::string& Mesh::mesh_id() const
  {
    return m_mesh_id;
  }

  const std::string& Mesh::layer_id() const
  {
    return m_layer_id;
  }

  Mesh& Mesh::layer_id(const std::string& layer_id)
  {
    m_layer_id = layer_id;
    return *this;
  }

  bool Mesh::double_sided() const
  {
    return m_double_sided;
  }

  Mesh& Mesh::double_sided(bool double_sided)
  {
    m_double_sided = double_sided;
    return *this;
  }

  bool Mesh::camera_space() const
  {
    return m_camera_space;
  }

  Mesh& Mesh::camera_space(bool camera_space)
  {
    m_camera_space = camera_space;
    return *this;
  }

  bool Mesh::nn_texture() const
  {
    return m_nn_texture;
  }

  Mesh& Mesh::nn_texture(bool nn_texture)
  {
    m_nn_texture = nn_texture;
    return *this;
  }

  bool Mesh::use_texture_alpha() const
  {
    return m_use_texture_alpha;
  }

  Mesh& Mesh::use_texture_alpha(bool use_texture_alpha)
  {
    m_use_texture_alpha = use_texture_alpha;
    return *this;
  }

  bool Mesh::is_billboard() const
  {
    return m_is_billboard;
  }

  Mesh& Mesh::is_billboard(bool is_billboard)
  {
    m_is_billboard = is_billboard;
    return *this;
  }

  bool Mesh::is_label() const
  {
    return m_is_label;
  }

  Mesh& Mesh::is_label(bool is_label)
  {
    m_is_label = is_label;
    return *this;
  }

  const ConstTriangleBufferRef Mesh::triangles() const
  {
    return ConstTriangleBufferRef(m_triangles);
  }

  VertexBlock Mesh::vertex_positions()
  {
    return m_vertices.leftCols(3);
  }

  Mesh& Mesh::vertex_positions(const VertexBlock& vertices)
  {
    this->vertex_positions() = vertices;
    return *this;
  }

  const ConstVertexBlock Mesh::vertex_positions() const
  {
    return m_vertices.leftCols(3);
  }

  VertexBlock Mesh::vertex_normals()
  {
    return m_vertices.block(0, 3, this->count_vertices(), 3);
  }

  Mesh& Mesh::vertex_normals(const VertexBlock& normals)
  {
    this->vertex_normals() = normals;
    return *this;
  }

  const ConstVertexBlock Mesh::vertex_normals() const
  {
    return m_vertices.block(0, 3, this->count_vertices(), 3);
  }

  VertexBlock Mesh::vertex_colors()
  {
    return m_vertices.rightCols(3);
  }

  Mesh& Mesh::vertex_colors(const VertexBlock& colors)
  {
    this->vertex_colors() = colors;
    return *this;
  }

  const ConstVertexBlock Mesh::vertex_colors() const
  {
    return m_vertices.rightCols(3);
  }

  VertexBlock Mesh::vertex_uvs()
  {
    return m_vertices.rightCols(2);
  }

  Mesh& Mesh::vertex_uvs(const VertexBlock& uvs)
  {
    this->vertex_uvs() = uvs;
    return *this;
  }

  const ConstVertexBlock Mesh::vertex_uvs() const
  {
    return m_vertices.rightCols(2);
  }

  VertexBufferRef Mesh::vertex_buffer()
  {
    return VertexBufferRef(m_vertices);
  }

  InstanceBufferRef Mesh::instance_buffer()
  {
    return InstanceBufferRef(m_instance_buffer);
  }

  std::string Mesh::to_string() const
  {
    return this->to_json().to_string();
  }

} // namespace scenepic


/*!
@file
Forward declares `boost::hana::Group`.

@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_HANA_GROUP_GROUP_HPP
#define BOOST_HANA_GROUP_GROUP_HPP

#include <boost/hana/group/detail/group_fwd.hpp>

#include <boost/hana/core/typeclass.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/monoid/monoid.hpp>

namespace boost { namespace hana {
    //! Minimal complete definition: `minus`
    template <typename G1, typename G2>
    struct Group::minus_mcd {
        template <typename X>
        static constexpr auto negate_impl(X x) {
            return minus(zero<G1>, x);
        }
    };
}} // end namespace boost::hana

#endif // !BOOST_HANA_GROUP_GROUP_HPP

#ifndef BOOST_HANA_GROUP_GROUP_MORE_HPP
#define BOOST_HANA_GROUP_GROUP_MORE_HPP

#include <boost/hana/core/is_a.hpp>

namespace boost { namespace hana {
    //! Instance of `Group` for objects with numeric data types.
    //!
    //! Any two objects whose data types form a `Monoid` and can be subtracted
    //! with the usual `operator-` naturally form an additive group, with
    //! the group subtraction being that usual `operator-`.
    template <typename X, typename Y>
    struct Group::instance<X, Y, when<
        are<Monoid, X, Y>() && is_valid<decltype((void)(*(X*)0 - *(Y*)0))>
    >>
        : Group::minus_mcd<X, Y>
    {
        static constexpr auto minus_impl(X x, Y y) {
            return x - y;
        }
    };
}}

#endif


//
// Copyright (c) 2011-2012 Krzysztof Jusiak (krzysztof at jusiak dot net)
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
#ifndef QFSM_BACK_DETAIL_EXECUTION_STATE_HPP
#define QFSM_BACK_DETAIL_EXECUTION_STATE_HPP

#include <boost/mpl/at.hpp>
#include <boost/mpl/find.hpp>
#include "QFsm/Back/Aux/State.hpp"

namespace QFsm
{
namespace Back
{
namespace Detail
{
namespace Execution
{

typedef int State;

} // namespace Execution
} // namespace Detail

namespace Aux
{
template<typename TFsm>
class State<Back::Detail::Execution::State, TFsm>
{
public:
    static void init(Back::Detail::Execution::State& p_state, const boost::mpl::int_<0>&)
    {
        p_state = boost::mpl::find<typename TFsm::States, typename TFsm::InitialState>::type::pos::value;
    }

    template<int region> static void init(Back::Detail::Execution::State& p_state, const boost::mpl::int_<region>&)
    {
        typedef typename boost::mpl::at_c<typename TFsm::InitialState, region - 1>::type state;
        p_state = boost::mpl::find<typename TFsm::States, state>::type::pos::value;
    }

    template<typename TState> static bool equals(const Back::Detail::Execution::State& p_state)
    {
        return static_cast<int>(boost::mpl::find<typename TFsm::States, TState>::type::pos::value) == p_state;
    }
};
} // namespace Aux

} // namespace Back
} // namespace QFsm

#endif


#include <iostream>
#include <ros/ros.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Int8.h>
#include <Eigen/Dense>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <nav_msgs/Odometry.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>

#include <moving_avg.h>
#include <PID.h>

using namespace Eigen;
using std::string;

class Controller
{
    private:
        ros::NodeHandle nh;
        ros::Publisher local_pos_pub, vel_pub;
        ros::Subscriber state_sub, target_found_sub, rtag_ekf_sub,rtag_quad_sub, quad_odom_sub;
        ros::Subscriber rtag_ekf_vel_sub, land_permit_sub, true_quad_odom_sub;
        ros::Subscriber moving_avg_sub, user_input_sub;

        //mavros service clients for arming and setting modes
        ros::ServiceClient arming_client, set_mode_client;

        mavros_msgs::State current_state;
        mavros_msgs::SetMode set_mode;
        mavros_msgs::CommandBool arm_cmd;

        //
        geometry_msgs::PoseStamped pose;

        //apriltag found
        bool target_found;
        bool land_permit;
        //Position vectors
        float r_x; // x tag wrt quad
        float r_y; // y tag wrt quad
        float r_z; // z tag wrt quad -> Don't really need this can use actual quad to true ground
        // Quaternion orientation vectors
        float r_qx;
        float r_qy;
        float r_qz;
        float r_qw;

        //kalman filter estimate
        float kf_x;
        float kf_y;
        float kf_vel_x;
        float kf_vel_y;
        //quad odom

        //Odometry of quad with offset because Airsim does not like to play nice
        float odom_x;
        float odom_y;
        float odom_z;
        float * curr_z_ptr = nullptr;

        float true_odom_z; // this is the true odometry

        float pre_error_x;
        float pre_error_y;

        float pre_ierror_x;
        float pre_ierror_y;

        float kp;
        float ki;
        float kd;
        float dt;

        int decision_case;
        int landing_decision_case;

        //initial pose commands
        float init_x = 0.0;
        float init_y = 3.0;
        float init_z = 4.0;

        bool avg_stabilize;
        bool begin_land;

        int user_input = 0;
        //class pid
        //PID(float kp, float ki, float kd, float dt, float target, float current);

    public:
        Controller()
        {
            //subscribers
            state_sub = nh.subscribe<mavros_msgs::State>
                    ("mavros/state", 10, &Controller::state_cb, this);
            target_found_sub = nh.subscribe<std_msgs::Bool>
                    ("target_found", 10, &Controller::target_found_cb,this);
            rtag_quad_sub = nh.subscribe<geometry_msgs::PoseStamped>
                    ("tag/pose", 10, &Controller::rtagquad_cb,this);
            rtag_ekf_sub = nh.subscribe<geometry_msgs::PoseStamped>
                    ("kf_tag/pose", 10, &Controller::kftag_cb,this);
            quad_odom_sub = nh.subscribe<geometry_msgs::PoseStamped>
                ("mavros/offset_local_position/pose",15, &Controller::quad_odom_callback, this);
            true_quad_odom_sub = nh.subscribe<geometry_msgs::PoseStamped>
                            ("mavros/local_position/pose",15, &Controller::true_quad_odom_callback, this);
            rtag_ekf_vel_sub = nh.subscribe<geometry_msgs::TwistStamped>
                ("kf_tag/vel", 10, &Controller::kftagvel_cb,this);
            land_permit_sub = nh.subscribe<std_msgs::Bool>
                    ("precland", 10, &Controller::land_permit_cb,this);
            moving_avg_sub = nh.subscribe<std_msgs::Bool>
                    ("stabilize_tag", 10, &Controller::moving_avg_cb,this);

            user_input_sub = nh.subscribe<std_msgs::Int8>
                            ("user_control", 10, &Controller::usercontrol_cb,this);

            local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
                    ("mavros/setpoint_position/local", 10);
            vel_pub = nh.advertise<geometry_msgs::TwistStamped>
                    ("/mavros/setpoint_velocity/cmd_vel", 10);

            //services
            arming_client = nh.serviceClient<mavros_msgs::CommandBool>
                ("mavros/cmd/arming");
            set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
                    ("mavros/set_mode");

            //the setpoint publishing rate MUST be faster than 2Hz
            ros::Rate rate(20.0);

            // wait for FCU connection
            while(ros::ok() && !current_state.connected){
                ros::spinOnce();
                rate.sleep();
            }

            //send initial points
            pose.pose.position.x = init_x;
            pose.pose.position.y = init_y;
            pose.pose.position.z = init_z;

            //send a few setpoints before starting
            for(int i = 100; ros::ok() && i > 0; --i){
                local_pos_pub.publish(pose);
                ros::spinOnce();
                rate.sleep();
            }

            init_vals();
            set_mode.request.custom_mode = "OFFBOARD";
            arm_cmd.request.value = true;

            ros::Time last_request = ros::Time::now();
            while(ros::ok())
            {
                setmode_arm(last_request, "OFFBOARD", arm_cmd);
                float curr_z;

                //PID gains
                PID pid_x(kp, ki, kd, dt, kf_x, odom_x);
                PID pid_y(kp, ki, kd, dt, kf_y, odom_y);

                float p_x = pid_x.getPID();
                float p_y = pid_y.getPID();
                Eigen::Vector2d gain(p_x, p_y);

                switch(user_input)
                {
                    case 0:
                        go_home();
                        break ;
                    case 1: // Track
                        curr_z = go_follow(gain, 4.0);
                        break;
                    case 2: //we can land
                        ROS_INFO("landing");
                        begin_land_protocol(gain);
                    case 3: //Land WHere you are at
                        //ROS_INFO("stabilizing");
                        curr_z_ptr = &curr_z;
                        curr_z = go_follow(gain, 4.0);
                        break;
                }

                ros::spinOnce();
                rate.sleep();
            }
        }

    void init_vals()
    {
        //set target found and landing permit to false initially
        target_found = false;
        land_permit = false;
        avg_stabilize = false;
        begin_land = false;

        //rotation matrix
        r_x = 0.0;
        r_y = 0.0;
        r_z = 0.0;
        r_qx = 0.0;
        r_qy = 0.0;
        r_qz = 0.0;
        r_qw = 0.0;

        //kalman filter
        kf_x = 0.0;
        kf_y = 0.0;
        kf_vel_x = 0.0;
        kf_vel_y = 0.0;

        //odometry of quad
        odom_x = 0.0;
        odom_y = 0.0;
        odom_z = 0.0;

        true_odom_z = 0.0;

        //intial error estimates
        pre_error_x = 0.0;
        pre_error_y = 0.0;

        pre_ierror_x = 0.0;
        pre_ierror_y = 0.0;

        //set this into a text/config file
        kp = 0.45;
        ki = 1E-3;
        kd = 0.0;
        dt = 0.1;

        //set decision case to go stay where they are at initially
        user_input = 0;
        landing_decision_case = 1;
    }

    //recieve state of quad
    void state_cb(const mavros_msgs::State::ConstPtr& msg)
    {
        current_state = *msg;
    }

    void usercontrol_cb(const std_msgs::Int8::ConstPtr& msg)
    {
        user_input = msg->data;
    }

    void target_found_cb(const std_msgs::Bool::ConstPtr& msg)
    {
        target_found = msg->data;
        //std::cout << "target_found:" << target_found << std::endl;
    }

    void land_permit_cb(const std_msgs::Bool::ConstPtr& msg)
    {
        land_permit = msg->data;
        //std::cout << "target_found:" << target_found << std::endl;
    }

    void rtagquad_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
    {
        r_z = msg->pose.position.x;
        r_y = msg->pose.position.y;
        r_z = msg->pose.position.z;

        r_qx = msg->pose.orientation.x;
        r_qy = msg->pose.orientation.y;
        r_qz = msg->pose.orientation.z;
        r_qw = msg->pose.orientation.w;
    }

    void kftag_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
    {
        kf_x = msg->pose.position.x;
        kf_y = msg->pose.position.y;
    }

    void quad_odom_callback(const geometry_msgs::PoseStamped::ConstPtr& msg)
    {
        odom_x = msg->pose.position.x;
        odom_y = msg->pose.position.y;
        odom_z = msg->pose.position.z;

    }

    void true_quad_odom_callback(const geometry_msgs::PoseStamped::ConstPtr& msg)
    {
        true_odom_z = msg->pose.position.z;
    }

    void kftagvel_cb(const geometry_msgs::TwistStamped::ConstPtr& msg)
    {
        kf_vel_x = msg->twist.linear.x;
        kf_vel_y = msg->twist.linear.y;
    }

    void moving_avg_cb(const std_msgs::Bool::ConstPtr& msg)
    {
        avg_stabilize = msg->data;
    }

    void setmode_arm(ros::Time last_request,const std::string& mode_input,
    mavros_msgs::CommandBool arm_cmd)
    {
        if( current_state.mode != mode_input &&
            (ros::Time::now() - last_request > ros::Duration(5.0))){
            if( set_mode_client.call(set_mode) &&
                set_mode.response.mode_sent){
                //ROS_INFO("Offboard enabled");
            }
            last_request = ros::Time::now();
        } else {
            if( !current_state.armed &&
                (ros::Time::now() - last_request > ros::Duration(5.0))){
                if( arming_client.call(arm_cmd) &&
                    arm_cmd.response.success){
                    //ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();
            }
        }
    }

    //Pointer functions -> probably put in a seperate library like how ardupilot does it
    float go_follow(Eigen::Vector2d gain, float z_cmd)
    {
        //ROS_INFO("following");
        pose.pose.position.x = odom_x - gain[0];
        pose.pose.position.y = odom_y - gain[1];
        pose.pose.position.z = z_cmd; // just testing the loiter
        local_pos_pub.publish(pose);
        //ros::spinOnce();

        return odom_z;
    }

    // this function is too long
    void begin_land_protocol(Eigen::Vector2d gain )
    {
        while (ros::ok()){
            PID pid_x(kp, ki, kd, dt, kf_x, odom_x);
            PID pid_y(kp, ki, kd, dt, kf_y, odom_y);

            float p_x = pid_x.getPID();
            float p_y = pid_y.getPID();
            Eigen::Vector2d gain(p_x, p_y);
            check_landing_cases();
            ros::Rate rate(20.0);
            switch(landing_decision_case)
            {
                case 1: //drop slowly to landing target
                    ROS_INFO("dropping down slowly");
                    pre_land_protocol(gain, 1E-4);
                    break;
                case 2:
                    land_disarm_protcol();
                    break;
                case 3:
                    already_landed();
                    break;
                case 4:
                    stabilize(gain, odom_z);
                    break;
            }
            local_pos_pub.publish(pose);
            ros::spinOnce();
            rate.sleep();
        }
    }

    void already_landed()
    {
        ROS_INFO("I have landed");
    }

    void go_home()
    {
        pose.pose.position.x = init_x;
        pose.pose.position.y = init_y;
        pose.pose.position.z = init_z;
        local_pos_pub.publish(pose);
    }

    void go_find()
    {
        printf("I'm lost");
    }

    void check_landing_cases()
    {
        // should be a hashtable or struct
        const float pre_land = 1.0; //this is an offset from the actual height
        const float z_land = 0.4; //need to set this as offset
        float tol = 0.1;

        if ((odom_z < pre_land) && (odom_z > z_land))
        {
            landing_decision_case = 1; // drop down slowly
        }
        else if ((odom_z < z_land))
        {
            landing_decision_case = 2; // begin landing
        }
        else if (current_state.armed == false)
        {
            landing_decision_case = 3; // we landed already
        }
        else
        {
            landing_decision_case = 4; // just stabilize
        }
    }

    void pre_land_protocol(Eigen::Vector2d gain, float drop_down_val)
    {
        pose.pose.position.x = odom_x - gain[0];
        pose.pose.position.y = odom_y - gain[1]; // pretty much keep at where we are
        pose.pose.position.z = odom_z - drop_down_val; //keep it at this general area
    }

    void stabilize(Eigen::Vector2d gain, float current_z)
    {
        pose.pose.position.x = odom_x - gain[0];
        pose.pose.position.y = odom_y - gain[1]; // pretty much keep at where we are
        pose.pose.position.z = current_z; //keep it at this general area
        local_pos_pub.publish(pose);
        ros::spinOnce();
    }

    void land_disarm_protcol()
    {   // need to clean this up or check if you're out of sight...
        while(ros::ok())
        {
            //ROS_INFO("Beginning Land");
            ros::Rate rate(20.0);

            //set to prelanding hover/ hover to around
            set_mode.request.custom_mode = "AUTO.LAND";
            arm_cmd.request.value = false;
            ros::Time last_request = ros::Time::now();
            //had to set
            while(ros::ok())
            {
                setmode_arm(last_request, "AUTO.LAND", arm_cmd);
                ros::spinOnce();
                rate.sleep();
            }
        }
    }
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "offboard_tracking");
    Controller control;

    return 0;
}


/*-----------------------------------------------------------------------------+
Copyright (c) 2010-2010: Joachim Faulhaber
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#ifndef BOOST_ICL_CONCEPT_JOINABLE_HPP_JOFA_100920
#define BOOST_ICL_CONCEPT_JOINABLE_HPP_JOFA_100920

#include <boost/icl/type_traits/is_interval_container.hpp>
#include <boost/icl/concept/interval.hpp>

namespace boost{ namespace icl
{

namespace segmental
{
    template<class Type>
    typename enable_if<is_interval_set<Type>, bool>::type
    is_joinable(typename Type::iterator it_, typename Type::iterator next_, Type* = 0)
    {
        return touches(*it_, *next_);
    }

    template<class Type>
    typename enable_if<is_interval_map<Type>, bool>::type
    is_joinable(typename Type::iterator it_, typename Type::iterator next_, Type* = 0)
    {
        return touches((*it_).first, (*next_).first)
            && (*it_).second == (*next_).second    ;
    }
}

}} // namespace boost icl

#endif


#pragma once

#include <boost/multiprecision/cpp_int.hpp>

#include <istream>
#include <ostream>

namespace base
{

template<typename T>
class BigInteger
{
  public:
    BigInteger() = default;

    template<typename N>
    BigInteger(const N& big_int);

    BigInteger(const BigInteger&) = default;

    BigInteger& operator=(const BigInteger&) = default;

    BigInteger(BigInteger&&) = default;

    BigInteger& operator=(BigInteger&&) = default;

    ~BigInteger() = default;

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

    BigInteger<T> operator+(const BigInteger<T>& other) const;

    BigInteger<T> operator-(const BigInteger<T>& other) const;

    BigInteger<T> operator*(const BigInteger<T>& other) const;

    BigInteger<T> operator/(const BigInteger<T>& other) const;

    BigInteger<T>& operator+=(const BigInteger<T>& other);

    BigInteger<T>& operator-=(const BigInteger<T>& other);

    BigInteger<T>& operator*=(const BigInteger<T>& other);

    BigInteger<T>& operator/=(const BigInteger<T>& other);

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

    bool operator!=(const BigInteger<T>& other) const;

    bool operator==(const BigInteger<T>& other) const;

    bool operator>(const BigInteger<T>& other) const;

    bool operator<(const BigInteger<T>& other) const;

    bool operator>=(const BigInteger<T>& other) const;

    bool operator<=(const BigInteger<T>& other) const;

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

    BigInteger<T>& operator++();

    BigInteger<T> operator++(int);

    BigInteger<T>& operator--();

    BigInteger<T> operator--(int);

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

    std::string toString() const noexcept;

  private:
    boost::multiprecision::number<T> _number;
};

template<typename T>
std::ostream& operator<<(std::ostream& output, const BigInteger<T>& big_int);

template<typename T>
std::istream& operator>>(std::istream& input, BigInteger<T>& big_int);

using Uint256 = BigInteger<
  boost::multiprecision::backends::cpp_int_backend<256,
                                                   256,
                                                   boost::multiprecision::cpp_integer_type::unsigned_magnitude,
                                                   boost::multiprecision::cpp_int_check_type::checked,
                                                   void>>;

using Uint512 = BigInteger<
  boost::multiprecision::backends::cpp_int_backend<512,
                                                   512,
                                                   boost::multiprecision::cpp_integer_type::unsigned_magnitude,
                                                   boost::multiprecision::cpp_int_check_type::checked,
                                                   void>>;
} // namespace base

#include "big_integer.tpp"


/*
Copyright 2017 InitialDLab

Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
of the Software, and to permit persons to whom the Software is furnished to do
so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/
#include <iostream>
#include <vector>
#include <fstream>
#include <random>
#include <limits>
#include <algorithm>
#include <cstdio>
#include <cassert>
#include <functional>

#include <boost/lexical_cast.hpp>
#include <boost/timer/timer.hpp>

#include <boost/date_time/posix_time/posix_time_types.hpp>

#include "mongo_types.h"
#include "rtree/rtree.h"
#include "level_sampling/level_sampling.h"

//using entry = mongo_types::entry;
//using sample_entry = mongo_types::sample_entry;
//using point = mongo_types::point3d;
//using box = mongo_types::box3d;
//using rtree_t = rtree::rtree<
//    entry,
//    sample_entry,
//    box,
//    256, // node sample count
//    256,  // leaf size
//    16   // internal size
//>;

template<typename Iter>
void generate_entries(size_t element_count, Iter iter)
{
    std::default_random_engine rng;

    std::uniform_int_distribution<unsigned int> lat_dist(0, 180L * 10000000);
    std::uniform_int_distribution<unsigned int> lon_dist(0, 360L * 10000000);
    std::uniform_int_distribution<int> timestamp_dist(0, std::numeric_limits<int>::max());

    static const char * id_list = "0123456789abcdef";
    std::uniform_int_distribution<int> id_dist(0, 15);
    char id_buffer[2 * mongo_types::OID::oid_len + 1];
    id_buffer[2*mongo_types::OID::oid_len] = 0;

    for(size_t i = 0; i < element_count; ++i)
    {
        for(int j = 0; j < 2 * mongo_types::OID::oid_len; ++j)
        {
            id_buffer[j] = id_list[id_dist(rng)];
        }

        *iter++ = mongo_types::entry(
            ((double)(lat_dist(rng))) / 10000000.0 - 90,
            ((double)(lon_dist(rng))) / 10000000.0 - 180,
            timestamp_dist(rng),
            id_buffer
        );
    }
}

void generate(size_t element_count, std::string const& filename)
{
    std::vector<mongo_types::entry> entries;
    entries.reserve(element_count);

    generate_entries(element_count, std::back_inserter(entries));

    {
        std::cerr << "bbox: ";
        mongo_types::box3d bbox;
        boost::geometry::assign_inverse(bbox);
        for(auto & v : entries)
            boost::geometry::expand(bbox, v.get_point());
        dump_box(std::cerr, bbox);
        std::cerr << std::endl;
    }

    using entry = mongo_types::entry;
    using sample_entry = mongo_types::sample_entry;
    using point = mongo_types::point3d;
    using box = mongo_types::box3d;

    rtree::IOLayersParameters parameters;

    parameters.max_top_layer_io_node_count = 1024;
    constexpr size_t NODE_SAMPLE_SIZE = 256;
    constexpr size_t MAX_FANOUT = 16;
    rtree::rtree<
        entry,
        sample_entry,
        box,
        NODE_SAMPLE_SIZE,
        MAX_FANOUT
    >::build_io_layers(entries.begin(), entries.end(), filename + "-rtree");

    parameters.max_top_layer_io_node_count *= NODE_SAMPLE_SIZE;
    parameters.max_top_layer_io_node_count /= MAX_FANOUT;

    level_sampling::level_sampling<
        entry,
        sample_entry,
        box,
        MAX_FANOUT
    >::build(entries.begin(), entries.end(), filename + "-ls", parameters);
}

void test()
{
    using entry = mongo_types::entry;
    using sample_entry = mongo_types::sample_entry;
    using point = mongo_types::point3d;
    using box = mongo_types::box3d;

    rtree::IOLayersParameters parameters;

    constexpr size_t NODE_SAMPLE_SIZE = 256;
    constexpr size_t MAX_FANOUT = 16;

    parameters.max_top_layer_io_node_count *= NODE_SAMPLE_SIZE;
    parameters.max_top_layer_io_node_count /= MAX_FANOUT;

    std::function<entry(const std::string&)> GeoLife_converter =
        [](const std::string& str)
    {
        float x_val;
        float y_val;
        int   time;
        char OIDvalue[32];

        int ret = sscanf(str.c_str(), "%24s %f %f %d", OIDvalue, &x_val, &y_val, &time);

        return mongo_types::entry(x_val, y_val, time, OIDvalue);
    };

    level_sampling::level_sampling<
        entry,
        sample_entry,
        box,
        MAX_FANOUT
    >::build("raw/GeoLifeDataNormalized.txt",
            "test/1",
            GeoLife_converter,
            1024*1024*1024,
            parameters);
}

int main(int argc, char ** argv)
{
    //// THIS IS A TEMP MAIN FUNCTION FOR TESTING PURPOSES FOR BULK LOADING WITHOUT NEED FOR THE ENTIRE
    //// DATA SET TO FIT INTO MAIN MEMORY.
    //std::string inputfilename  = "/uusoc/scratch/magpie2/robertc/data/GeoLifeDataAdded.txt";
    //std::string outputfilename = "output";

    //using entry = mongo_types::entry;
    //using sample_entry = mongo_types::sample_entry;
    //using point = mongo_types::point3d;
    //using box = mongo_types::box3d;

    //boost::posix_time::ptime epoch = boost::posix_time::from_iso_string("1970-01-01T00:00:00Z");

    ////std::function<entry(const std::string&)> convert =
    ////    [](const std::string& str){
    ////    float x_val = 0.0f;
    ////    float y_val = 0.0f;
    ////    int   time  = 0;
    ////    char OIDvalue[32];
    ////
    ////    int ret = sscanf(str.c_str(), "%24s %f %f %f", OIDvalue, &x_val, &y_val, &time);
    ////
    ////    return mongo_types::entry(x_val, y_val, time, OIDvalue);
    ////};
    //std::function<entry(const std::string&)> convert =
    //    [&epoch](const std::string& str){
    //    float x_val = 0.0f;
    //    float y_val = 0.0f;
    //    int   user  = 0;
    //    char OIDvalue[32];
    //    char timestr[32];

    //    int ret = sscanf(str.c_str(), "%f %f %d %21s %24s", &x_val, &y_val, &time, &user, timestr, OIDvalue);

    //    boost::posix_time::ptime entry_time = boost::posix_time::from_iso_string(timestr);
    //    int time = boost::posix_time::time_period(epoch, entry_time).length().total_seconds();

    //    return mongo_types::entry(x_val, y_val, time, OIDvalue);
    //};
    //rtree::rtree<
    //    entry,
    //    sample_entry,
    //    box,
    //    256, // node sample count
    //    256,  // leaf size
    //    16   // internal size
    //>::build_io_layers(inputfilename, outputfilename,
    //                   convert,  1024 * 1024 * 1024);

    test();
    return 0;

    if(argc != 3)
    {
        std::cerr << "argument: <entry count> <output filename>" << std::endl;
        return -1;
    }

    size_t element_count = boost::lexical_cast<size_t>(argv[1]);
    const char * filename = argv[2];

    generate(element_count, filename);

    return 0;
}


#ifndef PPTEST
#include <iostream>
#include <boost/mpl/is_sequence.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/insert.hpp>
#include <boost/mpl/insert_range.hpp>
#include <boost/mpl/eval_if.hpp>
#include <boost/mpl/size.hpp>
#include <boost/mpl/at.hpp>
#endif
#include <boost/preprocessor.hpp>
#include <typeinfo>

#ifndef L
#define L 20
#endif

template<
  typename Seq,
  typename Add
> struct next {
  typedef typename boost::mpl::eval_if<
      boost::mpl::is_sequence<Add>,
      boost::mpl::insert_range<
        Seq,
        typename boost::mpl::end<Seq>::type,
        Add
      >,
      boost::mpl::push_back<
        Seq,
        Add
      >
    >::type type;
};

template<
  BOOST_PP_ENUM_BINARY_PARAMS(
    L,
    typename P,
    = boost::mpl::vector0<> BOOST_PP_INTERCEPT
  )
>
struct combined {
  typedef boost::mpl::vector0<> type0;
  #define XNEXT(z,n,t) \
    typedef typename next< \
        BOOST_PP_CAT(type, n), \
        BOOST_PP_CAT(P, n) \
      >::type  BOOST_PP_CAT(type, BOOST_PP_INC(n)); \
      /**/
  BOOST_PP_REPEAT(L, XNEXT, ~)
  #undef XNEXT
  typedef BOOST_PP_CAT(type, L) type;
};

// ----
// create_vector from greatcontainer3
  template<typename Sequence, int Size>
  struct create_vector_impl;

  template<typename Sequence>
  struct create_vector {
    typedef
      typename create_vector_impl<Sequence, boost::mpl::size<Sequence>::value >::type
      type;
  };

  #define ELEMENT_AT(z, n, t) \
    BOOST_PP_COMMA_IF(n) \
    typename boost::mpl::at_c<Sequence, n>::type

  #define CREATE_VECTOR_IMPL(z, n, t) \
    template<typename Sequence> \
    struct create_vector_impl<Sequence, n> { \
      typedef BOOST_PP_CAT(boost::mpl::vector, n)< \
        BOOST_PP_REPEAT(n, ELEMENT_AT, ~) \
      > type; \
    };

  BOOST_PP_REPEAT(21, CREATE_VECTOR_IMPL, ~)

  #undef CREATE_VECTOR_IMPL
  #undef ELEMENT_AT
// ----

int main() {
  typedef combined<int, boost::mpl::vector3<char, int, char>, int, char, int, char, int, char, int, char, int, char, int, char, int, char, int, char>::type x1;
  std::cout << "_Z1x" << typeid(create_vector<x1>::type).name() << std::endl;
}


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include "Pad.hpp"
#include <backendsCommon/WorkloadData.hpp>
#include "TensorBufferArrayView.hpp"
#include "Encoders.hpp"

#include <boost/numeric/conversion/cast.hpp>
#include <cmath>
#include <cstddef>
#include <functional>
#include <limits>
#include <cassert>

namespace armnn
{

template <typename T>
void Pad(const TensorInfo& inputInfo,
         const TensorInfo& outputInfo,
         std::vector<std::pair<unsigned int, unsigned int>> m_padList,
         const T* inputData,
         T* outData,
         const float padValue)
{
    unsigned int numOutputElements = outputInfo.GetNumElements();

    TensorShape outputShape = outputInfo.GetShape();
    TensorShape inputShape = inputInfo.GetShape();

    unsigned int numInputDimensions = inputShape.GetNumDimensions();

    #ifndef NDEBUG

    unsigned int numOutputDimensions = outputShape.GetNumDimensions();
    assert(numInputDimensions == numOutputDimensions);

    #endif

    unsigned int inputBatches = 0;
    unsigned int inputChannels = 0;
    unsigned int inputHeight = 0;
    unsigned int inputWidth = 0;

    unsigned int outputChannels = 0;
    unsigned int outputHeight = 0;
    unsigned int outputWidth = 0;

    T convertedPadValue = static_cast<T>(padValue);

    for (unsigned int i = 0; i < numOutputElements; ++i)
    {
       outData[i] = convertedPadValue;
    }

    switch(numInputDimensions) {

        case 1:

            inputWidth = inputShape[0];

            for (unsigned int w = 0; w < inputWidth ; w++)
            {
                outData[w+std::get<0>(m_padList[0])] = inputData[w];
            }

            break;

        case 2  :

            inputHeight = inputShape[0];
            inputWidth = inputShape[1];
            outputHeight = outputShape[0];
            outputWidth = outputShape[1];

            for (unsigned int h = 0; h < inputHeight; h++)
            {
                for (unsigned int w = 0; w < inputWidth ; w++)
                {
                    outData[(h+std::get<0>(m_padList[0]))*outputWidth
                    + (w+std::get<0>(m_padList[1]))] = inputData[h * inputWidth + w];
                }
            }

            break;

        case 3  :

            inputChannels = inputShape[0];
            inputHeight = inputShape[1];
            inputWidth = inputShape[2];
            outputChannels = outputShape[0];
            outputHeight = outputShape[1];
            outputWidth = outputShape[2];

            for (unsigned int c = 0; c < inputChannels; c++)
            {
                for (unsigned int h = 0; h < inputHeight; h++)
                {
                    for (unsigned int w = 0; w < inputWidth ; w++)
                    {
                        outData[(c+std::get<0>(m_padList[0]))*outputHeight*outputWidth
                        + (h+std::get<0>(m_padList[1]))*outputWidth
                        + (w+std::get<0>(m_padList[2]))] = inputData[c * inputHeight * inputWidth
                                                                      + h * inputWidth
                                                                      + w];
                    }
                }
            }

            break;

        case 4  :

            inputBatches = inputShape[0];
            inputChannels = inputShape[1];
            inputHeight = inputShape[2];
            inputWidth = inputShape[3];
            outputChannels = outputShape[1];
            outputHeight = outputShape[2];
            outputWidth = outputShape[3];

            for (unsigned int b = 0; b < inputBatches; b++)
            {
                for (unsigned int c = 0; c < inputChannels; c++)
                {
                    for (unsigned int h = 0; h < inputHeight; h++)
                    {
                        for (unsigned int w = 0; w < inputWidth ; w++)
                        {
                            outData[(b+std::get<0>(m_padList[0])) * outputChannels * outputHeight * outputWidth
                                   + (c+std::get<0>(m_padList[1])) * outputHeight * outputWidth
                                   + (h+std::get<0>(m_padList[2])) * outputWidth
                                   + (w+std::get<0>(m_padList[3]))] = inputData[b * inputChannels * inputHeight
                                                                                * inputWidth
                                                                             + c * inputHeight * inputWidth
                                                                             + h * inputWidth
                                                                             + w];
                        }
                    }
                }
            }

            break;

        default :

            break;
    }
}

template void Pad<BFloat16>(const TensorInfo& inputInfo,
                            const TensorInfo& outputInfo,
                            std::vector<std::pair<unsigned int, unsigned int>> m_PadList,
                            const BFloat16* inputData,
                            BFloat16* outData,
                            const float padValue);

template void Pad<float>(const TensorInfo& inputInfo,
                         const TensorInfo& outputInfo,
                         std::vector<std::pair<unsigned int, unsigned int>> m_PadList,
                         const float* inputData,
                         float* outData,
                         const float padValue);
template void Pad<Half>(const TensorInfo& inputInfo,
                        const TensorInfo& outputInfo,
                        std::vector<std::pair<unsigned int, unsigned int>> m_PadList,
                        const Half* inputData,
                        Half* outData,
                        const float padValue);
template void Pad<uint8_t>(const TensorInfo& inputInfo,
                           const TensorInfo& outputInfo,
                           std::vector<std::pair<unsigned int, unsigned int>> m_PadList,
                           const uint8_t* inputData,
                           uint8_t* outData,
                           const float padValue);
template void Pad<int8_t>(const TensorInfo& inputInfo,
                          const TensorInfo& outputInfo,
                          std::vector<std::pair<unsigned int, unsigned int>> m_PadList,
                          const int8_t* inputData,
                          int8_t* outData,
                          const float padValue);
template void Pad<int16_t>(const TensorInfo& inputInfo,
                           const TensorInfo& outputInfo,
                           std::vector<std::pair<unsigned int, unsigned int>> m_PadList,
                           const int16_t* inputData,
                           int16_t* outData,
                           const float padValue);

} //namespace armnn


#ifndef PERIOR_TREE_POINT_TRAITS
#define PERIOR_TREE_POINT_TRAITS
#include <boost/type_traits.hpp>
#include <boost/array.hpp>

namespace perior
{
namespace traits
{

template<typename T>
struct is_point : boost::false_type{};
template<typename T, std::size_t N>
struct is_point<boost::array<T, N> > : boost::true_type{};

template<typename T>
struct dimension : boost::integral_constant<std::size_t, 0> {};
template<typename T, std::size_t N>
struct dimension<boost::array<T, N> >: boost::integral_constant<std::size_t, N>{};

template<typename T>
struct scalar_type_of{};
template<typename T, std::size_t N>
struct scalar_type_of<boost::array<T, N> >{typedef T type;};

template<typename T>
struct zero_vector_impl
{
    static BOOST_FORCEINLINE T invoke() BOOST_NOEXCEPT_OR_NOTHROW
    {
        T retval;
        for(std::size_t i=0; i<dimension<T>::value; ++i)
        {
            retval[i] = 0;
        }
        return retval;
    }
};
template<typename T, std::size_t N>
struct zero_vector_impl<boost::array<T, N> >
{
    static BOOST_FORCEINLINE boost::array<T, N> invoke() BOOST_NOEXCEPT_OR_NOTHROW
    {
        boost::array<T, N> retval; retval.fill(0);
        return retval;
    }
};

template<typename T>
BOOST_FORCEINLINE T zero_vector()
{
    return zero_vector_impl<T>::invoke();
}

} // traits
} // perior
#endif//PERIOR_TREE_POINT_TRAITS


#include <boost/test/unit_test.hpp>

#include "che/algo/aligner_dp.h" // header to test
#include "che/io/file_fasta.h"

using namespace biosim;

BOOST_AUTO_TEST_SUITE(suite_aligner_dp)

BOOST_AUTO_TEST_CASE(aligner_dp_align_pair) {
  che::assembly a(che::io::file_fasta::read("../test/data/P01236-shorter.fasta"));
  che::structure s(a.get_first_structure("A"));

  che::algo::aligner_dp aligner;
  std::list<che::scored_alignment> alignment_list(aligner.align_pair(che::alignment(s), che::alignment(s)));
  BOOST_CHECK(alignment_list.size() == 1);
  BOOST_CHECK(alignment_list.front().get_alignment().get_depth() == 2);
  BOOST_CHECK(alignment_list.front().get_alignment().get_length() == 5);
  BOOST_CHECK_CLOSE(alignment_list.front().get_score(), 24.2329, 1e-3);

  che::alignment two_m({s, s}, {0, 0}, {s.get_length(), s.get_length()});
  alignment_list = aligner.align_pair(che::alignment(s), two_m);
  BOOST_CHECK(alignment_list.size() == 1);
  BOOST_CHECK(alignment_list.front().get_alignment().get_depth() == 3);
  BOOST_CHECK(alignment_list.front().get_alignment().get_length() == 5);
  BOOST_CHECK_CLOSE(alignment_list.front().get_score(), 24.2329, 1e-3);

  alignment_list = aligner.align_pair(two_m, two_m);
  BOOST_CHECK(alignment_list.size() == 1);
  BOOST_CHECK(alignment_list.front().get_alignment().get_depth() == 4);
  BOOST_CHECK(alignment_list.front().get_alignment().get_length() == 5);
  BOOST_CHECK_CLOSE(alignment_list.front().get_score(), 24.2329, 1e-3);

  che::ps profile;
  profile.insert(profile.end(), 5, che::cc("ASX"));
  che::structure s2("st", "id", profile);
  alignment_list = aligner.align_pair(che::alignment(s), che::alignment(s2));
  BOOST_CHECK(alignment_list.size() == 5);
  BOOST_CHECK(alignment_list.front().get_alignment().get_depth() == 2);
  BOOST_CHECK(alignment_list.front().get_alignment().get_length() == 1);
  BOOST_CHECK_CLOSE(alignment_list.front().get_score(), 0.169935, 1e-3);

  alignment_list = aligner.align_pair(che::alignment(s2), che::alignment(s2));
  BOOST_CHECK(alignment_list.size() == 1);
  BOOST_CHECK(alignment_list.front().get_alignment().get_depth() == 2);
  BOOST_CHECK(alignment_list.front().get_alignment().get_length() == 5);
  BOOST_CHECK_CLOSE(alignment_list.front().get_score(), 17.4636, 1e-3);

  a = che::assembly(che::io::file_fasta::read("../test/data/P01942-shorter.fasta"));
  s = a.get_first_structure("A");
  a = che::assembly(che::io::file_fasta::read("../test/data/P02088-shorter.fasta"));
  s2 = a.get_first_structure("A");
  alignment_list = aligner.align_pair(che::alignment(s), che::alignment(s2));
  BOOST_CHECK(alignment_list.size() == 1);
  BOOST_CHECK(alignment_list.front().get_alignment().get_depth() == 2);
  BOOST_CHECK(alignment_list.front().get_alignment().get_length() == 26);
  BOOST_CHECK_CLOSE(alignment_list.front().get_score(), 50.0037, 1e-3);
}

BOOST_AUTO_TEST_CASE(aligner_dp_align_multiple) {
  che::assembly a(che::io::file_fasta::read("../test/data/P01236-shorter.fasta"));
  che::structure s(a.get_first_structure("A"));

  che::algo::aligner_dp aligner;
  std::list<che::scored_alignment> alignment_list(
      aligner.align_multiple({che::alignment(s), che::alignment(s), che::alignment(s)}));
  BOOST_CHECK(alignment_list.size() == 1);
  BOOST_CHECK(alignment_list.front().get_alignment().get_depth() == 3);
  BOOST_CHECK(alignment_list.front().get_alignment().get_length() == 5);
  BOOST_CHECK_CLOSE(alignment_list.front().get_score(), 24.2329, 1e-3);
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once

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

#define POINT3F                                                  \
  union EIGEN_ALIGN16 {                                          \
    float data[3];                                               \
    struct {                                                     \
      float x;                                                   \
      float y;                                                   \
      float z;                                                   \
    };                                                           \
  };                                                             \
  inline Eigen::Map<Eigen::Vector3f> point() {                   \
    return (Eigen::Vector3f::Map(data));                         \
  }                                                              \
  inline const Eigen::Map<const Eigen::Vector3f> point() const { \
    return (Eigen::Vector3f::Map(data));                         \
  }

#define POINT3D                                                  \
  union EIGEN_ALIGN16 {                                          \
    double data[3];                                              \
    struct {                                                     \
      double x;                                                  \
      double y;                                                  \
      double z;                                                  \
    };                                                           \
  };                                                             \
  inline Eigen::Map<Eigen::Vector3d> point() {                   \
    return (Eigen::Vector3d::Map(data));                         \
  }                                                              \
  inline const Eigen::Map<const Eigen::Vector3d> point() const { \
    return (Eigen::Vector3d::Map(data));                         \
  }

namespace point_type {
struct EIGEN_ALIGN16 Point3f {
  POINT3F;
  using FloatType = float;
  Point3f() = default;
  explicit Point3f(const float x_, const float y_, const float z_)
      : x(x_), y(y_), z(z_) {}
  explicit Point3f(const Eigen::Vector3f& point) { this->point() = point; }

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

inline std::ostream& operator<<(std::ostream& os, const Point3f& p) {
  os << "(" << p.x << ", " << p.y << ", " << p.z << ")";
  return os;
}

struct EIGEN_ALIGN16 Point3d {
  POINT3D;
  using FloatType = double;
  Point3d() = default;
  explicit Point3d(const double x_, const double y_, const double z_)
      : x(x_), y(y_), z(z_) {}
  explicit Point3d(const Eigen::Vector3d& point) { this->point() = point; }

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

inline std::ostream& operator<<(std::ostream& os, const Point3d& p) {
  os << "(" << p.x << ", " << p.y << ", " << p.z << ")";
  return os;
}
}  // namespace point_type