/*
  Copyright 2012 Lucanus Simonson

  Use, modification and distribution are subject to the Boost Software License,
  Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
  http://www.boost.org/LICENSE_1_0.txt).
*/
#ifndef BOOST_POLYGON_SKELETON_DETAIL_HPP
#define BOOST_POLYGON_SKELETON_DETAIL_HPP
#include <boost/polygon/point_concept.hpp>
#include <boost/polygon/segment_concept.hpp>
#include <boost/polygon/polygon_set_concept.hpp>
#include <boost/polygon/segment_utils.hpp>
#include <queue>
#include "skeleton_predicates.hpp"

namespace boost { namespace polygon {
#ifdef BOOST_POLYGON_DEBUG1
    inline void debug1(const char* msg) {std::cout << std::string(msg) << std::endl;}
#else
    inline void debug1(const char* msg) {}
#endif

    template <typename PointType, typename Segment2, typename Segment3>
    bool
    //computes in point the intersection of the lines of Segment2 and Segment3
    //if Segment2 and Segment3 are parallel and non-colinear it returns false
    //if segment2 and segment3 are parallel and colinear it returns the least low point in the direction
    //both segments are directed, or if opposite directions it returns the midpoint between their two low points
    generalized_intersection(PointType& point, const Segment2& segment2, const Segment3& segment3) {
      typedef typename point_traits<PointType>::coordinate_type UnitOut;
      UnitOut x11, x21, x12, x22, y12, y22, y11, y21, x_num, x_den, y_num, y_den, dx1, dy1, dx2, dy2;
      x11 = UnitOut(segment2.low().get(HORIZONTAL));
      x21 = UnitOut(segment3.low().get(HORIZONTAL));
      y11 = UnitOut(segment2.low().get(VERTICAL));
      y21 = UnitOut(segment3.low().get(VERTICAL));
      x12 = UnitOut(segment2.high().get(HORIZONTAL));
      x22 = UnitOut(segment3.high().get(HORIZONTAL));
      y12 = UnitOut(segment2.high().get(VERTICAL));
      y22 = UnitOut(segment3.high().get(VERTICAL));
      dx1 = x12 - x11;
      dy1 = y12 - y11;
      dx2 = x22 - x21;
      dy2 = y22 - y21;
      if(dx1 * dy2 - dx2 * dy1 == 0) {
        if((x22 - x11) * dx2 == (y22 - y11) * dy2)
          return false; //lines have same slope but not colinear
        UnitOut dll = (x11 - x21) * (x11 - x21) + (y11 - y21) * (y11 - y21);
        UnitOut dlh = (x11 - x22) * (x11 - x22) + (y11 - y22) * (y11 - y22);
        UnitOut dhl = (x12 - x21) * (x12 - x21) + (y12 - y21) * (y12 - y21);
        UnitOut dhh = (x12 - x22) * (x12 - x22) + (y12 - y22) * (y12 - y22);
        if(dhh < dhl && dhh < dlh && dhh < dll) {
          //segments are directed toward eachother
          point = PointType((x12+x22)/2,(y12+y22)/2); //average of high points
        } else {
          //away or in the same direction
          point = PointType((x11+x21)/2,(y11+y21)/2); //average of low points
        }
      }
      x_num = (x11 * dy1 * dx2 - x21 * dy2 * dx1 + y21 * dx1 * dx2 - y11 * dx1 * dx2);
      x_den = (dy1 * dx2 - dy2 * dx1);
      y_num = (y11 * dx1 * dy2 - y21 * dx2 * dy1 + x21 * dy1 * dy2 - x11 * dy1 * dy2);
      y_den = (dx1 * dy2 - dx2 * dy1);
      UnitOut x = x_num / x_den;
      UnitOut y = y_num / y_den;
      point = PointType(x, y);
      std::cout << x11 << "," << y11 << "->" << x12 << "," << y12 << " " << x21 << "," << y21 << "->" << x22 << "," << y22 << "\n";
      std::cout << "Intersection Point " << x << " " << y << std::endl;
      return true;
    }

    template <typename Segment1, typename Segment2, typename Segment3>
      bool
    //computes in segment1 a segment with low point at the intersection of the lines
    //of segment2 and segment3 oriented in the direction of the angle bisector of segment2
    // and segment3
    //degenerate case where segments are parallel and colinear
    bisector(Segment1& segment1, const Segment2& segment2, const Segment3& segment3) {
      typedef typename segment_traits<Segment1>::coordinate_type UnitOut;
      point_data<UnitOut> gint;
      if(generalized_intersection(gint, segment2, segment3)) {
        UnitOut x11, x21, x12, x22, y12, y22, y11, y21, dx1, dy1, dx2, dy2;
        segment1.low(gint);
        x11 = UnitOut(segment2.low().get(HORIZONTAL));
        x21 = UnitOut(segment3.low().get(HORIZONTAL));
        y11 = UnitOut(segment2.low().get(VERTICAL));
        y21 = UnitOut(segment3.low().get(VERTICAL));
        x12 = UnitOut(segment2.high().get(HORIZONTAL));
        x22 = UnitOut(segment3.high().get(HORIZONTAL));
        y12 = UnitOut(segment2.high().get(VERTICAL));
        y22 = UnitOut(segment3.high().get(VERTICAL));
        dx1 = x12 - x11;
        dy1 = y12 - y11;
        dx2 = x22 - x21;
        dy2 = y22 - y21;

        point_data<UnitOut> p1(x11, y11);
        point_data<UnitOut> p2(x21, y21);
        if(p1 == gint)
          p1 = point_data<UnitOut>(x12, y12);
        if(p2 == gint)
          p2 = point_data<UnitOut>(x22, y22);
        UnitOut dist1 = euclidean_distance(gint, p1);
        UnitOut dist2 = euclidean_distance(gint, p2);
        std::cout << dist1 << " " << dist2 << "\n";
        UnitOut x_out = gint.x() + ((p1.x() - gint.x())*dist2/dist1 + (p2.x()-gint.x()))/2;
        UnitOut y_out = gint.y() + ((p1.y() - gint.y())*dist2/dist1 + (p2.y()-gint.y()))/2;
        point_data<UnitOut> end_point(x_out, y_out);
        if(orientation(segment2, end_point) < 0) {
          x_out = gint.x() - ((p1.x() - gint.x())*dist2/dist1 + (p2.x()-gint.x()))/2;
          y_out = gint.y() - ((p1.y() - gint.y())*dist2/dist1 + (p2.y()-gint.y()))/2;
          end_point = point_data<UnitOut>(x_out, y_out);
        }
        std::cout << x_out << " " << y_out << "\n";
        if(orientation(segment2, end_point) != orientation(segment3, end_point)) {
          std::cout << "we want the other bisector in this case\n";
          y_out = gint.y() + ((p1.x() - gint.x())*dist2/dist1 + (p2.x()-gint.x()))/2;
          x_out = gint.x() - ((p1.y() - gint.y())*dist2/dist1 + (p2.y()-gint.y()))/2;
          end_point = point_data<UnitOut>(x_out, y_out);
          std::cout << x_out << " " << y_out << "\n";
          std::cout << segment2.low().x() << " " << segment2.low().y() << " " <<
            segment2.high().x() << " " << segment2.high().y() << " " << std::endl;
          if(orientation(segment2, end_point) < 0) {
            y_out = gint.y() - ((p1.x() - gint.x())*dist2/dist1 + (p2.x()-gint.x()))/2;
            x_out = gint.x() + ((p1.y() - gint.y())*dist2/dist1 + (p2.y()-gint.y()))/2;
            end_point = point_data<UnitOut>(x_out, y_out);
            std::cout << x_out << " " << y_out << "\n";
          }
        }
        end_point = point_data<UnitOut>(x_out, y_out);
        segment1.high(end_point);
        return true;
      }
      return false;
    }

    template <typename coordinate_type, typename output_coordinate_type>
    struct skeleton_detail {

      //DATA STRUCTURES
      typedef segment_data<output_coordinate_type> edge_type;
      typedef point_data<output_coordinate_type> vertex_type;
      typedef segment_data<output_coordinate_type> segment_type;

      //forward declare
      struct future_intersection;
      struct skeleton_node;

      //EVENT OBJECT can represent either split event or edge event with same type
      struct event {
        point_data<output_coordinate_type> point;
        output_coordinate_type distance; //use for lazy compare, exact compare can be done with original coordinates in *parent
        //event consists of intersection between angle bisectors of parent and parent->next or parent and split_on_next
        future_intersection* parent;
        future_intersection* parent2;
        //refer back to the edge between the target and its next
        future_intersection* split_on_next; //if null is an edge event

        bool operator<(const event& that) const {
          debug1("event::operator<");
          return distance < that.distance;
        }
        bool operator>(const event& that) const {
          debug1("event::operator>");
          return distance > that.distance;
        }

        output_coordinate_type x() const { return point.x(); }
        void x(const output_coordinate_type& val) { point.x(val); }

        output_coordinate_type y() const { return point.y(); }
        void y(const output_coordinate_type& val) { point.y(val); }

        output_coordinate_type r() const { return distance; }
        void r(const output_coordinate_type& val) { distance = val; }
      };
      typedef typename boost::polygon::detail::skeleton_predicates
      <typename boost::polygon::detail::skeleton_ctype_traits<coordinate_type> >::template event_formation_predicate<
        segment_data<coordinate_type>, event> create_event;

      //SET OF CIRCULAR LINKED LISTS OF FUTURE INTERSECTIONS
      // each future intersection is associated with a pair of edges of the polygon
      // originally coming from edges incident on each polygon vertex
      // polygon holes are circular sub lists of each list such that split events can be found
      struct future_intersection {
        edge_type edge; //the edge that is next between this future intersection and the one stored in *next
        future_intersection* prev;
        future_intersection* next;
        future_intersection* next_split_edge;
        skeleton_node* parent_node;
        output_coordinate_type active_distance;
        //split events that refer to the edge between this and next
        std::vector<typename std::set<event>::iterator> active_splits_on_next;
        std::string label;
        bool is_dead;
        future_intersection() : prev(0x0), next(0x0), next_split_edge(0x0),
                                parent_node(0x0), is_dead(false), active_distance(-1.0) {
          static char local_label = 'a';
          label = local_label;
          if(local_label == 'z')
            local_label = 'a';
          else
            ++local_label;
        }
        ~future_intersection() {
          debug1("~future_intersection()");
          prev = 0x0;
          next = 0x0;
          parent_node = 0x0;
        }
      };

      static void delete_future_intersection_ring(future_intersection* head) {
        debug1("delete_future_intersection_ring()");
        future_intersection* current = head;
        if(head != 0x0)
          if(head->prev != 0x0)
            head->prev->next = 0x0; //give us a stopping point
        while(current != 0x0) {
          if(current->prev != 0x0)
            current->prev->next = 0x0;
          future_intersection* dead_node = current;
          current = current->next;
          delete dead_node;
        }
      }

      //EVENT QUEUE of event objects by minimum weighted distance from edges
      struct event_priority {
        bool operator()(const event& e1, const event& e2) {
          return e1 > e2;
        }
      };
      typedef std::priority_queue<event, std::vector<event>, event_priority> event_queue_type;

      //SKELETON OUTPUT DATA STRUCTURE
      //OUTPUT REQUIREMENTS
      //provide floating point representation of skeleton nodes and also references to all the input polygon edges from which
      //the node is weighted equidistant, that way original coordinates can be used by algorithms consuming skeleton data
      //handle the case where there are more than three edges that are weighted cocirular by a zero length skeleton segment
      //that can be collapsed in post processing

      struct skeleton_node {
        skeleton_node(const point_data<output_coordinate_type>& pt) {
          debug1("skeleton_node()");
          static char local_label = 'A';
          label = local_label;
          if(local_label == 'Z')
            local_label = 'A';
          else
            ++local_label;
          vertex = pt;
          linked_nodes[0] = linked_nodes[1] = linked_nodes[2] = 0x0;
        }
        void add_edge(skeleton_node* link) {
          for(int i = 0; i < 3; ++i) {
            if(!linked_nodes[i]) {
              linked_nodes[i] = link;
              break;
            }
          }
        }
        std::string label;
        point_data<output_coordinate_type> vertex;
        skeleton_node* linked_nodes[3]; //three is the common case
        //for higher order nodes insert artificial zero length skeleton edges and duplicate node coordinates
        //to create a 3-ary tree
      };

      //static member functions

      static void find_split_event(future_intersection* first, future_intersection* second, event_queue_type& event_queue) {
        if(orientation(first->edge, second->edge) < 1) {
          //std::cout << "initialize reflex vertex\n";
          future_intersection* inner = second->next->next;
          while(inner != second) {
            event e2;
            if(compute_split_event(e2, second, inner)) {
              if(e2.distance <= e2.parent->active_distance || e2.parent->active_distance < 0) {
                //split event has no parent2
                std::cout << "inserting split event\n";
                event_queue.push(e2);
                e2.parent->active_distance = e2.distance;
              }
            }
            inner = inner->next;
          }
        }
      }

      //ORIGINAL POLYGON EDGES
      //simply copy the original polygon edges into the future intersection edge pair data structure
      static void initialize_skeleton_edges(std::vector<skeleton_node*>& skeleton,
                                            std::vector<future_intersection*>& all_faces,
                                            event_queue_type& event_queue,
                                            const std::vector<edge_type>& edges) {
        debug1("initialize_skeleton_edges");
        if(edges.empty()) return;
        rectangle_data<output_coordinate_type> domain;
        envelope_segments(domain, edges.begin(), edges.end());
        future_intersection* head = 0x0, *current = 0x0, *prev = 0x0;
        for(std::size_t i = 0; i < edges.size(); ++i) {
          skeleton.push_back(new skeleton_node(point_data<output_coordinate_type>
                                               (output_coordinate_type(edges[i].low().get(HORIZONTAL)),
                                                output_coordinate_type(edges[i].low().get(VERTICAL)))));
          current = new future_intersection();
          all_faces.push_back(current);
          if(i == 0) head = current;
          current->edge = edges[i];
          current->parent_node = (skeleton.back());
          current->prev = prev;
          current->is_dead = false;
          if(prev) {
            prev->next = current;
          }
          prev = current;
        }
        current->next = head;
        head->prev = current;

        current = head;
        do {
          debug1("loop");
          find_split_event(current->prev, current, event_queue);
          event e;
          if(compute_edge_event(e, current, current->next, domain)) {
            if((e.distance <= e.parent->active_distance || e.parent->active_distance < 0) &&
               (e.distance <= e.parent2->active_distance || e.parent2->active_distance < 0)) {
              event_queue.push(e);
              e.parent->active_distance = e.distance;
              e.parent2->active_distance = e.distance;
            }
          }
          current = current->next;
        } while(current != head);
      }

      template<typename polygon_data_type>
      static void initialize_skeleton_single_polygon(std::vector<skeleton_node*>& skeleton,
                                                     std::vector<future_intersection*>& all_faces,
                                                     event_queue_type& event_queue,
                                                     const polygon_data_type& polygon,
                                                     bool reverse_winding = false) {
        debug1("initialize_skeleton_single_polygon");
        //polygons may be open or closed
        if(polygon.size() == 0) return;
        std::vector<edge_type> edges;
        typename polygon_data_type::iterator_type itr = polygon.begin(), first, prev;
        first = itr;
        prev = itr;
        for(++itr; itr != polygon.end(); ++itr) {
          //ignore duplicate points
          if(*prev != *itr) {
            if(reverse_winding)
              edges.push_back(edge_type(*itr, *prev));
            else
              edges.push_back(edge_type(*prev, *itr));
            prev = itr;
          }
        }
        if(*prev != *first){
          if(reverse_winding)
            edges.push_back(edge_type(*first, *prev));
          else
            edges.push_back(edge_type(*prev, *first));
        }
        if(reverse_winding) {
          std::reverse(edges.begin(), edges.end());
        }
        initialize_skeleton_edges(skeleton, all_faces, event_queue, edges);
      }

      template<typename geometry_type>
      static void initialize_skeleton(std::vector<skeleton_node*>& skeleton, std::vector<future_intersection*>& all_faces,
                                      event_queue_type& event_queue,
                                      const geometry_type& polygon_set) {
        debug1("initialize_skeleton");
        std::vector<polygon_with_holes_data<coordinate_type> > polys;
        assign(polys, polygon_set); //this makes winding correct for polygons and holes
        for(std::size_t i = 0; i < polys.size(); ++i) {
          initialize_skeleton_single_polygon(skeleton, all_faces, event_queue, polys[i]);
          for(typename polygon_with_holes_data<coordinate_type>::iterator_holes_type itrh = polys[i].begin_holes();
              itrh != polys[i].end_holes(); ++itrh) {
            initialize_skeleton_single_polygon(skeleton, all_faces, event_queue, *itrh);
          }
        }
      }

      //COMPUTE EDGE EVENT COORDINATES
      //given four general line segments find the intersection between their angle bisectors or return false if it does not exist
      //robustness concern: if two line segments don't share a vertex the bisector line will be anchored on their
      //projected intersection, which is a rational quantity, use the original coordinates of the line segments in the formula
      //to compute the angle bisectors intersection rather than the anchor point of the bisector for computation
      //perform quick predicates to check if the two bisectors should ever intersect
      //include weight on each edge to bias the angle bisector
      //include bound on intersection weighted distance
      //required degenerate case, if they are colinear then the bisector is perpendicular at their mid point between them on the line
      //(shared coordinate if abutting) if they are at 180 degrees
      //optional degenerate case, if they are colinear but at zero degrees (spike) then the bisector is at the lesser of their
      //two start points and parallel to the segments

      template <typename Segment>
      static void print_segment(const Segment& s) {
        std::cout << s.low().x() << "," << s.low().y() << "->" << s.high().x() << "," << s.high().y() << "\n";
      }

      static bool compute_split_event_point(event& e, edge_type e1, edge_type e2, edge_type e3, edge_type e4, edge_type e5) {
        debug1("compute_split_event_point");
        segment_type segment1, segment2, segment3, segment4, segment5;
        print_segment(e1);
        print_segment(e2);
        print_segment(e4);
        point_data<output_coordinate_type> v;
        //compute that the intersection of the lines of e1 and e2 are on the correct side of the "opposite" segment e4
        if(!generalized_intersection(v, e1, e2)) {
          debug1("no intersection");
          return false;
        }
        if(orientation(e4, v) <= 0) { //check that reflex vertex ray originates on the correct side of "opposite" edge e4
          debug1("wrong side opposite");
          return false;
        }
        //compute the bisector of e1 and e2, this is the reflex vertex bisector
        if(bisector(segment2, e3, e4)) {
          debug1("has bisector2");
          if(bisector(segment3, e4, e5)) {
            debug1("has bisector3");
            segment_data<coordinate_type> s1, s2, s3;
            assign(s1, e1);
            assign(s2, e2);
            assign(s3, e4);
            if(!create_event()(s1, s2, s3, &e)) {
              return false;
            }

            point_data<output_coordinate_type> point = e.point;
            std::cout << point.x() << " " << point.y() << std::endl;
            //test that point is on the coorect side of e1 and e2 and e4
            if(orientation(e1, point) <= 0)
              return false;
            if(orientation(e2, point) <= 0)
              return false;
            if(orientation(e3, point) <= 0)
              return false;
            if(orientation(e4, point) <= 0)
              return false;
            if(orientation(e5, point) <= 0)
              return false;
            debug1("passed simple orientation tests");
              //test that the point is on the correct side of the besector of e3,e4 and e4,e5
              //it should be clockwise the bisector of e3,e4 and counterclockwise the bisector of e4,e5
              //if e3,e4/e4e5 are concave then the correct sides will be reversed
            print_segment(e3);
            print_segment(e4);
            //print_segment(segment2);
            if((distance_squared_to_line(point, e4) - distance_squared_to_line(point, e3)) > 1e-6)
              return false;
            // std::cout << orientation(e3, e4) << " " << orientation(segment2, point) << std::endl;
            // if(orientation(e3, e4) == orientation(segment2, point))
            //   return false;
            debug1("passed complex orientation test1");
            print_segment(e4);
            print_segment(e5);
            print_segment(segment3);
            std::cout << distance_squared_to_line(point, e4) << " " << distance_squared_to_line(point, e5) << "\n";
            std::cout << (distance_squared_to_line(point, e4) > distance_squared_to_line(point, e5)) << "\n";
            std::cout << (distance_squared_to_line(point, e4) - distance_squared_to_line(point, e5)) << std::endl;
            std::cout << (distance_squared_to_line(point, e4) - distance_squared_to_line(point, e5) > 1e-6) << std::endl;
            if((distance_squared_to_line(point, e4) - distance_squared_to_line(point, e5)) > 1e-6)
              return false;
            //if(orientation(e4, e5) != orientation(segment3, point))
            //  return false;
            debug1("passed complex orientation test2");
            //we seem to have a good split event point
            return true;
          }
        }
        return false;
      }

      template <typename SegmentType>
      static output_coordinate_type distance_squared_to_line(const point_data<output_coordinate_type>& pt, const SegmentType& segment) {
        //(ax+by+c)/sqrt(a^2+b^2)
        //(ax+by+c)(ax+by+c)/(a^2+b^2)
        output_coordinate_type x11, x12, y12, y11, dx1, dy1, slope;
        x11 = output_coordinate_type(segment.low().get(HORIZONTAL));
        y11 = output_coordinate_type(segment.low().get(VERTICAL));
        x12 = output_coordinate_type(segment.high().get(HORIZONTAL));
        y12 = output_coordinate_type(segment.high().get(VERTICAL));
        dx1 = x12 - x11;
        dy1 = y12 - y11;
        if(dx1 == 0.0)
          return (pt.x() - x11)*(pt.x() - x11);
        slope = dy1/dx1;
        //0 = -slope * x + y + slope * x11 - y11
        output_coordinate_type a = -slope;
        // b = 1;
        output_coordinate_type c = slope * x11 - y11;
        output_coordinate_type numf = a*pt.x() + pt.y() + c;
        return numf*numf/(a*a + 1);
      }

      static bool compute_split_event(event& e, future_intersection* first, future_intersection* opposite) {
        debug1("compute_split_event");
        point_data<output_coordinate_type> pt;
        if(compute_split_event_point(e, first->prev->edge, first->edge, opposite->prev->edge, opposite->edge, opposite->next->edge)) {
          std::cout << "found split event point\n";
          e.parent = first;
          e.parent2 = first->prev;
          e.split_on_next = opposite;
          return true;
        }
        return false;
      }

      static bool compute_edge_event_point(event& e,
                                           edge_type e1, edge_type e2, edge_type e3, edge_type e4) {
        segment_data<coordinate_type> s1, s2, s3;
        assign(s1, e1);
        assign(s2, e2);
        assign(s3, e4);
        return create_event()(s1, s2, s3, &e);
      }

      static bool compute_edge_event(event& e, future_intersection* first, future_intersection* second,
                                     const rectangle_data<output_coordinate_type>& domain) {
        point_data<output_coordinate_type> pt;
        if(compute_edge_event_point(e, first->prev->edge, first->edge, second->prev->edge, second->edge)) {
          debug1("domain");
          point_data<output_coordinate_type> pt = e.point;
          std::cout << pt.x() << " " << pt.y() << std::endl;
          //edge event points outside (or on the boundary of) the polygon bounding box are useless and not worth processing
          //they would never be reached by the forward progress of the algorithm through the event queue
          //and would just slow it down and waste space
          if(contains(domain, pt, false)) {
            //compute distance from point to edge
            segment_type s1;
            assign(s1, first->edge);
            e.parent = first;
            e.parent2 = second;
            e.split_on_next = 0x0;
            return true;
          }
        }
        return false;
      }

      //COMPUTE SPLIT EVENT COORDINATES
      //given three general line segments find the intersection between any two angle bisectors of the three segments
      //if two of the three segments are parallel choose the other angle bisector
      //if all three are parallel (two should be colinear) find the point half way between the bisector of the colinear points
      //the line of the third segment

      //TODO

      static void find_edge_event(future_intersection* i1, future_intersection* i2,
                                  event_queue_type& event_queue, const rectangle_data<output_coordinate_type>& domain)
      {
        event new_event;
        if(compute_edge_event(new_event, i1, i2, domain)) {
          std::cout << "new event " << new_event.point.x() << " " << new_event.point.y() << std::endl;
          if(orientation(i1->prev->edge, new_event.point) > 0 &&
             orientation(i1->edge, new_event.point) > 0 &&
             orientation(i2->edge, new_event.point) > 0) {
            if((new_event.distance <= new_event.parent->active_distance || new_event.parent->active_distance < 0) &&
               (new_event.distance <= new_event.parent->next->active_distance || new_event.parent->next->active_distance < 0)) {
              event_queue.push(new_event);
              new_event.parent->active_distance = new_event.distance;
              new_event.parent->next->active_distance = new_event.distance;
            } else {
              std::cout << "distance test rejection " << new_event.parent->active_distance << "\n";
            }
          } else {
            std::cout << "orientation test rejection\n";
          }
        }
      }

      static void print_ring(future_intersection* fi) {
        future_intersection* current = fi;
        do {
          std::cout << current->label << "->";
          current = current->next;
        } while(current != fi);
        std::cout << fi->label << std::endl;
        do {
          std::cout << current->label << "->";
          current = current->prev;
        } while(current != fi);
        std::cout << fi->label << std::endl;
      }

      static void process_event(event current_event, std::vector<skeleton_node*>& skeleton, std::vector<future_intersection*>& all_faces,
                                event_queue_type& event_queue, const rectangle_data<output_coordinate_type>& domain) {
        if(current_event.split_on_next) {
          //split_event

          std::cout << "split event " << current_event.point.x() << " " << current_event.point.y() << std::endl;
          print_ring(current_event.parent);
          //if the split_on_next edge was previously split it will have a circular linked list of potential
          //future intersections that we need to search for the right one
          bool found = false;
          if(current_event.split_on_next->next_split_edge) {
            future_intersection* current = current_event.split_on_next;
            do{
              std::cout << current << std::endl;
              if(!current->is_dead) {
                debug1("is alive");
                event e2;
                if(compute_split_event(e2, current_event.parent, current)) {
                  debug1("found correct split edge");
                  current_event.split_on_next = current;
                  found = true;
                  break;
                }
              }
              current = current->next_split_edge;
            } while (current != current_event.split_on_next);
          } else {
            found = true;
          }
          if(!found)
            return;
          //this is the dual split event of a pseudo split
          if(current_event.parent->prev != current_event.parent2)
            return;
          //check for pseudo split
          std::cout << distance_squared_to_line(current_event.point, current_event.split_on_next->edge) << std::endl;
          std::cout << distance_squared_to_line(current_event.point, current_event.split_on_next->next->edge) << std::endl;

          if(abs(distance_squared_to_line(current_event.point, current_event.split_on_next->edge) -
                 distance_squared_to_line(current_event.point, current_event.split_on_next->next->edge)) < 1e-6) {
            std::cout << "pseudo split 1\n";

            skeleton.push_back(new skeleton_node(current_event.point));
            current_event.parent->parent_node->add_edge(skeleton.back());
            std::cout << current_event.parent->parent_node->label << "->" << skeleton.back()->label << std::endl;
            skeleton.back()->add_edge(current_event.parent->parent_node);
            current_event.parent->parent_node = skeleton.back();

            skeleton.push_back(new skeleton_node(current_event.point));
            current_event.split_on_next->next->parent_node->add_edge(skeleton.back());
            std::cout << current_event.split_on_next->next->parent_node->label << "->" << skeleton.back()->label << std::endl;
            skeleton.back()->add_edge(current_event.split_on_next->next->parent_node);
            current_event.split_on_next->next->parent_node = skeleton.back();

            skeleton.back()->add_edge(skeleton[skeleton.size()-2]);
            skeleton[skeleton.size()-2]->add_edge(skeleton.back());

            current_event.parent->prev = current_event.split_on_next;
            current_event.parent2->next = current_event.split_on_next->next;
            current_event.split_on_next->next->prev = current_event.parent2;
            current_event.split_on_next->next = current_event.parent;

            current_event.parent->active_distance = -1;
            current_event.parent2->active_distance = -1;
            current_event.parent->prev->active_distance = -1;
            current_event.parent2->next->active_distance = -1;

            find_edge_event(current_event.parent->prev, current_event.parent, event_queue, domain);
            find_edge_event(current_event.parent, current_event.parent->next, event_queue, domain);
            find_edge_event(current_event.parent2, current_event.parent2->next, event_queue, domain);
            find_edge_event(current_event.parent2->next, current_event.parent2->next->next, event_queue, domain);
            find_split_event(current_event.parent->prev, current_event.parent, event_queue);
            find_split_event(current_event.parent2, current_event.parent2->next, event_queue);
            print_ring(current_event.parent);
            print_ring(current_event.parent2);
            return;
          }
          if(abs(distance_squared_to_line(current_event.point, current_event.split_on_next->edge) -
                 distance_squared_to_line(current_event.point, current_event.split_on_next->prev->edge)) < 1e-6) {
            //event point is on the boundary of the "zone" of the opposite edge
            std::cout << "pseudo split 2\n";
            skeleton.push_back(new skeleton_node(current_event.point));
            current_event.parent->parent_node->add_edge(skeleton.back());
            std::cout << current_event.parent->parent_node->label << "->" << skeleton.back()->label << std::endl;
            skeleton.back()->add_edge(current_event.parent->parent_node);
            current_event.parent->parent_node = skeleton.back();

            skeleton.push_back(new skeleton_node(current_event.point));
            current_event.split_on_next->parent_node->add_edge(skeleton.back());
            std::cout << current_event.split_on_next->parent_node->label << "->" << skeleton.back()->label << std::endl;
            skeleton.back()->add_edge(current_event.split_on_next->parent_node);
            current_event.split_on_next->parent_node = skeleton.back();

            current_event.parent->prev = current_event.split_on_next->prev;
            current_event.parent2->next = current_event.split_on_next;
            current_event.split_on_next->prev->next = current_event.parent;
            current_event.split_on_next->prev = current_event.parent2;
            find_edge_event(current_event.parent->prev, current_event.parent, event_queue, domain);
            find_edge_event(current_event.parent, current_event.parent->next, event_queue, domain);
            find_edge_event(current_event.parent2, current_event.parent2->next, event_queue, domain);
            find_edge_event(current_event.parent2->next, current_event.parent2->next->next, event_queue, domain);
            print_ring(current_event.parent);
            print_ring(current_event.parent2);
            return;
          }
          //create one new skeleton node with the event parent future intersection's parent-node connected to it
          skeleton.push_back(new skeleton_node(current_event.point));
          current_event.parent->parent_node->add_edge(skeleton.back());
          std::cout << current_event.parent->parent_node->label << "->" << skeleton.back()->label << std::endl;
          skeleton.back()->add_edge(current_event.parent->parent_node);
          current_event.parent->parent_node = skeleton.back();

          //event parent node is not dead
          //create one new future intersection associated with the with a copy of the "opposite" edge as its edge
          //that split edge will appear in both rings
          //the future intersection of the opposite edge gets linked to the parent of the split event as its new prev
          //the newly created future intersection gets linked to the old prev of the parent of the split even as its new next
          future_intersection* new_active = new future_intersection();
          all_faces.push_back(new_active);
          new_active->edge = current_event.split_on_next->edge;
          new_active->prev = current_event.parent->prev;
          new_active->next = current_event.split_on_next->next;

          //link new future intersection into the circular list of future intersections
          //associated with splits of this edge
          new_active->next_split_edge = current_event.split_on_next->next_split_edge;
          if(new_active->next_split_edge == 0x0)
            new_active->next_split_edge = current_event.split_on_next;
          std::cout << "next split edge " << new_active->next_split_edge << std::endl;
          current_event.split_on_next->next_split_edge = new_active;
          std::cout << "next split edge " << new_active << std::endl;

          new_active->parent_node = skeleton.back();
          new_active->active_distance = -1;
          new_active->next->prev = new_active;
          current_event.parent->prev->next = new_active;
          current_event.parent->prev = current_event.split_on_next;
          current_event.parent->prev = current_event.split_on_next;
          current_event.parent->active_distance = -1;
          current_event.split_on_next->next = current_event.parent;

          find_edge_event(new_active, new_active->next, event_queue, domain);
          find_edge_event(new_active->prev, new_active, event_queue, domain);

          find_edge_event(current_event.split_on_next, current_event.split_on_next->next, event_queue, domain);
          find_edge_event(current_event.split_on_next->next, current_event.split_on_next->next->next, event_queue, domain);
          print_ring(current_event.split_on_next);
          print_ring(new_active);

          //find split event here to handle ties between reflex vertices that generate a new reflect vertex
          //find_split_event(current_event.parent->prev, current_event.parent->next, event_queue);
        } else {
          //edge event
          if(current_event.parent->next != current_event.parent2)
            return;
          std::cout << "event " << current_event.point.x() << " " << current_event.point.y() << std::endl;
          std::cout << current_event.parent->parent_node->label << " " << current_event.parent2->parent_node->label << std::endl;
          skeleton.push_back(new skeleton_node(current_event.point));
          //this should implrement couter clockwise order
          current_event.parent->parent_node->add_edge(skeleton.back());
          std::cout << current_event.parent->parent_node->label << "->" << skeleton.back()->label << std::endl;
          current_event.parent->next->parent_node->add_edge(skeleton.back());
          std::cout << current_event.parent->next->parent_node->label << "->" << skeleton.back()->label << std::endl;
          skeleton.back()->add_edge(current_event.parent->next->parent_node);
          std::cout << skeleton.back()->label  << "->" << current_event.parent->next->parent_node->label << std::endl;
          skeleton.back()->add_edge(current_event.parent->parent_node);
          std::cout << skeleton.back()->label << "->" << current_event.parent->parent_node->label << std::endl;
          //parent needs to be removed from the ring because it's face has collapsed at this event
          current_event.parent->prev->next = current_event.parent->next;
          current_event.parent->next->prev = current_event.parent->prev;
          current_event.parent->is_dead = true;
          current_event.parent->next->parent_node = skeleton.back();
          if(current_event.parent->prev->next == current_event.parent->prev->prev) {
            debug1("closed ring");
            //we are done with this ring
            current_event.parent->next->parent_node->add_edge(current_event.parent->prev->parent_node);
            std::cout << current_event.parent->next->parent_node->label << "->" << current_event.parent->prev->parent_node->label << std::endl;
            current_event.parent->prev->is_dead = true;
            current_event.parent->prev->parent_node->add_edge(current_event.parent->next->parent_node);
            std::cout << current_event.parent->prev->parent_node->label << "->" << current_event.parent->next->parent_node->label << std::endl;
            current_event.parent->next->is_dead = true;
            return;
          }
          current_event.parent->active_distance = -1;
          current_event.parent2->active_distance = -1;
          find_edge_event(current_event.parent->prev, current_event.parent->next, event_queue, domain);
          find_edge_event(current_event.parent->next, current_event.parent->next->next, event_queue, domain);

          //we may need to search i-2 and i+2 for edge events

        }
      }

      //main loop
      static void execute_skeleton(std::vector<skeleton_node*>& skeleton, std::vector<future_intersection*>& all_faces,
                                   event_queue_type& event_queue, const rectangle_data<output_coordinate_type>& domain,
                                   output_coordinate_type bound, bool handle_reflex_vertices = true) {
        while(!event_queue.empty()) {
          event current_event = event_queue.top();
          event_queue.pop();
          if(bound >=0 && current_event.distance > bound)
            break;
          if(!current_event.parent->is_dead &&
             !current_event.parent2->is_dead)
            process_event(current_event, skeleton, all_faces, event_queue, domain);
        }
      }

      //EAGERNESS
      //it would be desirable when a split event occurs to recurse the algorithm into each sub-polygon to eagerly process it
      //holes would need to be split between each sub-polygon and the event queue would need to be split as well
      //the cost of splitting may more than offset the advantages
      //if each future intersection of the sub polygon can be inserted into a new sub event queue they will be invalidated
      //in the main event queue
      //hole discovery could be costly

      //NOTE ABOUT RESIZING
      //for skeleton based resizing all that is needed is to discard edges of the original polygon that collapse
      //with bounded edge events and handle reflex vertices of the orignal polygon that participate in bounded split events
      //by associating their edges with the "opposite" split edge for computing bounded angle bisector output polygon vertex
      //Simply terminate skeleton algorithm early and process the set of lists of active

      //BUILT IN TEST
      template <typename stream_type>
      static bool test_initialize_skeleton(stream_type& stdcout) {
        debug1("test_initialize_skeleton");
        std::string stdendl = "\n";
        rectangle_data<coordinate_type> rect(100, 200, 300, 500);
        rectangle_data<output_coordinate_type> domain(100, 200, 300, 500);
        std::vector<skeleton_node*> skeleton;
        std::vector<future_intersection*> all_faces;
        event_queue_type event_queue;
        initialize_skeleton(skeleton, all_faces, event_queue, rect);
        if(skeleton.size() == 4 && all_faces.size() == 4) {
          stdcout << event_queue.size() << stdendl;
          for(typename std::vector<skeleton_node*>::iterator itr = skeleton.begin();
              itr != skeleton.end(); ++itr) {
            std::cout << (*itr) << " " << (*itr)->vertex.x() << " " << (*itr)->vertex.y() << " " <<
              ((*itr)->linked_nodes[0]) << " " <<
              ((*itr)->linked_nodes[1]) << " " <<
              ((*itr)->linked_nodes[2]) << "\n";
          }
          execute_skeleton(skeleton, all_faces, event_queue, domain, 1000);
          for(typename std::vector<future_intersection*>::iterator itr = all_faces.begin();
              itr != all_faces.end(); ++itr)
            delete *itr;
          all_faces.clear();
          for(typename std::vector<skeleton_node*>::iterator itr = skeleton.begin();
              itr != skeleton.end(); ++itr) {
            std::cout << (*itr)->label << " " << (*itr)->vertex.x() << " " << (*itr)->vertex.y() << " " <<
              ( ((*itr)->linked_nodes[0]) ? (*itr)->linked_nodes[0]->label : std::string("0"))  << " " <<
              ( ((*itr)->linked_nodes[1]) ? (*itr)->linked_nodes[1]->label : std::string("0"))  << " " <<
              ( ((*itr)->linked_nodes[2]) ? (*itr)->linked_nodes[2]->label : std::string("0"))  << "\n";
          }
          for(typename std::vector<skeleton_node*>::iterator itr = skeleton.begin();
              itr != skeleton.end(); ++itr)
            delete *itr;
          skeleton.clear();
          {
          point_data<coordinate_type> pts[] = {point_data<coordinate_type>(120, 200),
                                               point_data<coordinate_type>(300, 200),
                                               point_data<coordinate_type>(300, 500),
                                               point_data<coordinate_type>(100, 500),
                                               point_data<coordinate_type>(100, 220),
          };
          polygon_data<coordinate_type> poly(pts, pts+5);
          stdcout << "five point case\n";
          initialize_skeleton(skeleton, all_faces, event_queue, poly);
          execute_skeleton(skeleton, all_faces, event_queue, domain, 1000);
          for(typename std::vector<skeleton_node*>::iterator itr = skeleton.begin();
              itr != skeleton.end(); ++itr) {
            std::cout << (*itr)->label << " " << (*itr)->vertex.x() << " " << (*itr)->vertex.y() << " " <<
              ( ((*itr)->linked_nodes[0]) ? (*itr)->linked_nodes[0]->label : std::string("0"))  << " " <<
              ( ((*itr)->linked_nodes[1]) ? (*itr)->linked_nodes[1]->label : std::string("0"))  << " " <<
              ( ((*itr)->linked_nodes[2]) ? (*itr)->linked_nodes[2]->label : std::string("0"))  << "\n";
          }
          }
          for(typename std::vector<future_intersection*>::iterator itr = all_faces.begin();
              itr != all_faces.end(); ++itr)
            delete *itr;
          all_faces.clear();
          for(typename std::vector<skeleton_node*>::iterator itr = skeleton.begin();
              itr != skeleton.end(); ++itr)
            delete *itr;
          skeleton.clear();

          {
          point_data<coordinate_type> pts[] = {point_data<coordinate_type>(120, 200),
                                               point_data<coordinate_type>(280, 200),
                                               point_data<coordinate_type>(300, 220),
                                               point_data<coordinate_type>(300, 480),
                                               point_data<coordinate_type>(280, 500),
                                               point_data<coordinate_type>(120, 500),
                                               point_data<coordinate_type>(100, 480),
                                               point_data<coordinate_type>(100, 220),
          };
          polygon_data<coordinate_type> poly(pts, pts+8);
          stdcout << "eight point case\n";
          initialize_skeleton(skeleton, all_faces, event_queue, poly);
          execute_skeleton(skeleton, all_faces, event_queue, domain, 1000);
          for(typename std::vector<skeleton_node*>::iterator itr = skeleton.begin();
              itr != skeleton.end(); ++itr) {
            std::cout << (*itr)->label << " " << (*itr)->vertex.x() << " " << (*itr)->vertex.y() << " " <<
              ( ((*itr)->linked_nodes[0]) ? (*itr)->linked_nodes[0]->label : std::string("0"))  << " " <<
              ( ((*itr)->linked_nodes[1]) ? (*itr)->linked_nodes[1]->label : std::string("0"))  << " " <<
              ( ((*itr)->linked_nodes[2]) ? (*itr)->linked_nodes[2]->label : std::string("0"))  << "\n";
          }
          }

          for(typename std::vector<future_intersection*>::iterator itr = all_faces.begin();
              itr != all_faces.end(); ++itr)
            delete *itr;
          all_faces.clear();
          for(typename std::vector<skeleton_node*>::iterator itr = skeleton.begin();
              itr != skeleton.end(); ++itr)
            delete *itr;
          skeleton.clear();
          {
          point_data<coordinate_type> pts[] = {point_data<coordinate_type>(120, 200),
                                               point_data<coordinate_type>(280, 200),
                                               point_data<coordinate_type>(300, 220),
                                               point_data<coordinate_type>(300, 480-100),
                                               point_data<coordinate_type>(280, 500-100),
                                               point_data<coordinate_type>(120, 500-100),
                                               point_data<coordinate_type>(100, 480-100),
                                               point_data<coordinate_type>(100, 220),
          };
          polygon_data<coordinate_type> poly(pts, pts+8);
          stdcout << "co-circular eight point case\n";
          initialize_skeleton(skeleton, all_faces, event_queue, poly);
          execute_skeleton(skeleton, all_faces, event_queue, domain, 1000);
          for(typename std::vector<skeleton_node*>::iterator itr = skeleton.begin();
              itr != skeleton.end(); ++itr) {
            std::cout << (*itr)->label << " " << (*itr)->vertex.x() << " " << (*itr)->vertex.y() << " " <<
              ( ((*itr)->linked_nodes[0]) ? (*itr)->linked_nodes[0]->label : std::string("0"))  << " " <<
              ( ((*itr)->linked_nodes[1]) ? (*itr)->linked_nodes[1]->label : std::string("0"))  << " " <<
              ( ((*itr)->linked_nodes[2]) ? (*itr)->linked_nodes[2]->label : std::string("0"))  << "\n";
          }
          }

          for(typename std::vector<future_intersection*>::iterator itr = all_faces.begin();
              itr != all_faces.end(); ++itr)
            delete *itr;
          all_faces.clear();
          for(typename std::vector<skeleton_node*>::iterator itr = skeleton.begin();
              itr != skeleton.end(); ++itr)
            delete *itr;
          skeleton.clear();

          {
          point_data<coordinate_type> pts[] = {point_data<coordinate_type>(100, 200),
                                               point_data<coordinate_type>(150, 220),
                                               point_data<coordinate_type>(200, 200),
                                               point_data<coordinate_type>(180, 250),
                                               point_data<coordinate_type>(200, 300),
                                               point_data<coordinate_type>(150, 280),
                                               point_data<coordinate_type>(100, 300),
                                               point_data<coordinate_type>(120, 250),
          };
          polygon_data<coordinate_type> poly(pts, pts+8);
          stdcout << "ninja star\n";
          initialize_skeleton(skeleton, all_faces, event_queue, poly);
          execute_skeleton(skeleton, all_faces, event_queue, domain, 1000);
          for(typename std::vector<skeleton_node*>::iterator itr = skeleton.begin();
              itr != skeleton.end(); ++itr) {
            std::cout << (*itr)->label << " " << (*itr)->vertex.x() << " " << (*itr)->vertex.y() << " " <<
              ( ((*itr)->linked_nodes[0]) ? (*itr)->linked_nodes[0]->label : std::string("0"))  << " " <<
              ( ((*itr)->linked_nodes[1]) ? (*itr)->linked_nodes[1]->label : std::string("0"))  << " " <<
              ( ((*itr)->linked_nodes[2]) ? (*itr)->linked_nodes[2]->label : std::string("0"))  << "\n";
          }
          }

          for(typename std::vector<future_intersection*>::iterator itr = all_faces.begin();
              itr != all_faces.end(); ++itr)
            delete *itr;
          all_faces.clear();
          for(typename std::vector<skeleton_node*>::iterator itr = skeleton.begin();
              itr != skeleton.end(); ++itr)
            delete *itr;
          skeleton.clear();

          {
          point_data<coordinate_type> pts[] = {point_data<coordinate_type>(100, 200),
                                               point_data<coordinate_type>(140, 240),
                                               point_data<coordinate_type>(200, 200),
                                               point_data<coordinate_type>(160, 240),
                                               point_data<coordinate_type>(200, 300),
                                               point_data<coordinate_type>(160, 260),
                                               point_data<coordinate_type>(100, 300),
                                               point_data<coordinate_type>(140, 260),
          };
          polygon_data<coordinate_type> poly(pts, pts+8);
          stdcout << "evil ninja star\n";
          initialize_skeleton(skeleton, all_faces, event_queue, poly);
          std::cout << event_queue.size() << " GREP\n";
          execute_skeleton(skeleton, all_faces, event_queue, domain, 1000);
          for(typename std::vector<skeleton_node*>::iterator itr = skeleton.begin();
              itr != skeleton.end(); ++itr) {
            std::cout << (*itr)->label << " " << (*itr)->vertex.x() << " " << (*itr)->vertex.y() << " " <<
              ( ((*itr)->linked_nodes[0]) ? (*itr)->linked_nodes[0]->label : std::string("0"))  << " " <<
              ( ((*itr)->linked_nodes[1]) ? (*itr)->linked_nodes[1]->label : std::string("0"))  << " " <<
              ( ((*itr)->linked_nodes[2]) ? (*itr)->linked_nodes[2]->label : std::string("0"))  << "\n";
          }
          }

          for(typename std::vector<future_intersection*>::iterator itr = all_faces.begin();
              itr != all_faces.end(); ++itr)
            delete *itr;
          all_faces.clear();
          for(typename std::vector<skeleton_node*>::iterator itr = skeleton.begin();
              itr != skeleton.end(); ++itr)
            delete *itr;
          skeleton.clear();

          {
          point_data<coordinate_type> pts[] = {point_data<coordinate_type>(100, 200),
                                               point_data<coordinate_type>(300, 200),
                                               point_data<coordinate_type>(300, 500),
                                               point_data<coordinate_type>(100, 500),
                                               point_data<coordinate_type>(290, 350),
          };
          polygon_data<coordinate_type> poly(pts, pts+5);
          stdcout << "concave polygon\n";
          initialize_skeleton(skeleton, all_faces, event_queue, poly);
          execute_skeleton(skeleton, all_faces, event_queue, domain, 1000);
          for(typename std::vector<skeleton_node*>::iterator itr = skeleton.begin();
              itr != skeleton.end(); ++itr) {
            std::cout << (*itr)->label << " " << (*itr)->vertex.x() << " " << (*itr)->vertex.y() << " " <<
              ( ((*itr)->linked_nodes[0]) ? (*itr)->linked_nodes[0]->label : std::string("0"))  << " " <<
              ( ((*itr)->linked_nodes[1]) ? (*itr)->linked_nodes[1]->label : std::string("0"))  << " " <<
              ( ((*itr)->linked_nodes[2]) ? (*itr)->linked_nodes[2]->label : std::string("0"))  << "\n";
          }
          }

          for(typename std::vector<future_intersection*>::iterator itr = all_faces.begin();
              itr != all_faces.end(); ++itr)
            delete *itr;
          all_faces.clear();
          for(typename std::vector<skeleton_node*>::iterator itr = skeleton.begin();
              itr != skeleton.end(); ++itr)
            delete *itr;
          skeleton.clear();

          {
          point_data<coordinate_type> pts[] = {point_data<coordinate_type>(100, 200),
                                               point_data<coordinate_type>(300, 200),
                                               point_data<coordinate_type>(300, 500),
                                               point_data<coordinate_type>(100, 500),
                                               point_data<coordinate_type>(100, 490),
                                               point_data<coordinate_type>(120, 490),
                                               point_data<coordinate_type>(100, 489),
                                               point_data<coordinate_type>(100, 450),
                                               point_data<coordinate_type>(122, 450),
                                               point_data<coordinate_type>(100, 449),
                                               point_data<coordinate_type>(100, 420),
                                               point_data<coordinate_type>(120, 420),
                                               point_data<coordinate_type>(100, 419),
          };
          polygon_data<coordinate_type> poly(pts, pts+10);
          stdcout << "multiple split\n";
          initialize_skeleton(skeleton, all_faces, event_queue, poly);
          execute_skeleton(skeleton, all_faces, event_queue, domain, 1000);
          for(typename std::vector<skeleton_node*>::iterator itr = skeleton.begin();
              itr != skeleton.end(); ++itr) {
            std::cout << (*itr)->label << " " << (*itr)->vertex.x() << " " << (*itr)->vertex.y() << " " <<
              ( ((*itr)->linked_nodes[0]) ? (*itr)->linked_nodes[0]->label : std::string("null"))  << " " <<
              ( ((*itr)->linked_nodes[1]) ? (*itr)->linked_nodes[1]->label : std::string("null"))  << " " <<
              ( ((*itr)->linked_nodes[2]) ? (*itr)->linked_nodes[2]->label : std::string("null"))  << "\n";
          }
          }

          for(typename std::vector<future_intersection*>::iterator itr = all_faces.begin();
              itr != all_faces.end(); ++itr)
            delete *itr;
          all_faces.clear();
          for(typename std::vector<skeleton_node*>::iterator itr = skeleton.begin();
              itr != skeleton.end(); ++itr)
            delete *itr;
          skeleton.clear();

          {
          point_data<coordinate_type> pts[] = {point_data<coordinate_type>(100, 201),
                                               point_data<coordinate_type>(120, 220),
                                               point_data<coordinate_type>(101, 200),
                                               point_data<coordinate_type>(299, 200),
                                               point_data<coordinate_type>(280, 220),
                                               point_data<coordinate_type>(300, 201),
                                               point_data<coordinate_type>(300, 500),
                                               point_data<coordinate_type>(100, 500),
          };
          polygon_data<coordinate_type> poly(pts, pts+8);
          stdcout << "2nd order reflex vertex\n";
          initialize_skeleton(skeleton, all_faces, event_queue, poly);
          execute_skeleton(skeleton, all_faces, event_queue, domain, 1000);
          for(typename std::vector<skeleton_node*>::iterator itr = skeleton.begin();
              itr != skeleton.end(); ++itr) {
            std::cout << (*itr)->label << " " << (*itr)->vertex.x() << " " << (*itr)->vertex.y() << " " <<
              ( ((*itr)->linked_nodes[0]) ? (*itr)->linked_nodes[0]->label : std::string("0"))  << " " <<
              ( ((*itr)->linked_nodes[1]) ? (*itr)->linked_nodes[1]->label : std::string("0"))  << " " <<
              ( ((*itr)->linked_nodes[2]) ? (*itr)->linked_nodes[2]->label : std::string("0"))  << "\n";
          }
          }
          for(typename std::vector<future_intersection*>::iterator itr = all_faces.begin();
              itr != all_faces.end(); ++itr)
            delete *itr;
          all_faces.clear();
          for(typename std::vector<skeleton_node*>::iterator itr = skeleton.begin();
              itr != skeleton.end(); ++itr)
            delete *itr;
          skeleton.clear();

          return true;
        }
        return false;
      }

    };
  }
}

#endif


#include <boost/graph/biconnected_components.hpp>


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

#include "camera_calibration/tools/tools.h"

#include <fstream>

#include <boost/filesystem.hpp>
#include <libvis/camera.h>
#include <libvis/geometry.h>
#include <libvis/image.h>
#include <QDir>

#include "camera_calibration/feature_detection/feature_detector_tagged_pattern.h"

namespace vis {

int RenderSyntheticDataset(const char* binary_path, const string& path) {
  srand(time(nullptr));

  // Pinhole camera settings
  int width = 640;
  int height = 480;
  float fx = height;
  float fy = height;
  float cx = 0.5 * width;
  float cy = 0.5 * height;

  vector<float> camera_parameters = {fx, fy, cx, cy};
  PinholeCamera4f camera(width, height, camera_parameters.data());

  // TODO: Make the pattern to use configurable
  // This is a symlink to the test_data folder installed via CMakeLists.txt
  string test_data_path = (boost::filesystem::path(binary_path).parent_path() / "test_data").string();
  string pattern_name = "pattern_resolution_17x24_segments_16_apriltag_0";

  // Load the pattern as an image
  string pattern_image_path = (boost::filesystem::path(test_data_path) / (pattern_name + ".png")).string();
  Image<u8> pattern_image(pattern_image_path);
  CHECK(!pattern_image.empty()) << "Cannot load the pattern image from: " << pattern_image_path;

  // Set up the feature detector to obtain the pattern geometry from it
  int feature_window_extent = 10;
  string pattern_yaml_path = (boost::filesystem::path(test_data_path) / (pattern_name + ".yaml")).string();
  vector<string> patterns = {pattern_yaml_path};
  FeatureDetectorTaggedPattern detector(patterns, feature_window_extent, FeatureRefinement::GradientsXY, /*use_cuda*/ true);
  if (!detector.valid()) {
    LOG(ERROR) << "Failed to load: " << pattern_yaml_path;
    return EXIT_FAILURE;
  }

  const PatternData& pattern = detector.GetPatternData(0);

  auto pattern_to_pattern_image_coord = [&](const Vec2f& pattern_coord) {
    Vec2f pattern_image_coord_mm =
        Vec2f(pattern.pattern_start_x_mm,
              pattern.pattern_start_y_mm) +
            (pattern_coord + Vec2f::Constant(1)).cwiseQuotient(Vec2f(pattern.squares_x, pattern.squares_y)).cwiseProduct(
            Vec2f(pattern.pattern_end_x_mm - pattern.pattern_start_x_mm,
                  pattern.pattern_end_y_mm - pattern.pattern_start_y_mm));
    return Vec2f(pattern_image.size().cast<float>().cwiseQuotient(Vec2f(pattern.page_width_mm, pattern.page_height_mm)).cwiseProduct(pattern_image_coord_mm));
  };

  vector<Vec3f> apriltag_min_xy;
  vector<Vec3f> apriltag_max_xy;
  for (int i = 0; i < pattern.tags.size(); ++ i) {
    const auto& tag = pattern.tags[i];
    Vec2f min_xy_2d = pattern_to_pattern_image_coord(Vec2f(
        tag.x - 1,
        tag.y - 1));
    apriltag_min_xy.push_back(Vec3f(min_xy_2d.x(), min_xy_2d.y(), 0));
    Vec2f max_xy_2d = pattern_to_pattern_image_coord(Vec2f(
        tag.x - 1 + tag.width,
        tag.y - 1 + tag.height));
    apriltag_max_xy.push_back(Vec3f(max_xy_2d.x(), max_xy_2d.y(), 0));
  }

  // Get the pattern geometry in the pattern plane and lift it to 3D in pattern pixel coordinates.
  // NOTE: If we used any kind of lens distortion, we would also need to
  //       subsample the pattern here as in feature_detection_test.cc.
  forward_list<vector<Vec2f>> geometry;
  pattern.ComputePatternGeometry(&geometry);

  forward_list<vector<Vec3f>> geometry_3d;
  for (vector<Vec2f>& polygon : geometry) {
    vector<Vec3f> polygon_3d(polygon.size());
    for (usize i = 0; i < polygon.size(); ++ i) {
      Vec2f pattern_image_coord = pattern_to_pattern_image_coord(polygon[i]);
      polygon_3d[i] = Vec3f(pattern_image_coord.x(), pattern_image_coord.y(), 0);
    }
    geometry_3d.push_front(polygon_3d);
  }

  Eigen::Hyperplane<float, 3> pattern_plane(Vec3f(0, 0, -1), Vec3f(0, 0, 0));

  Vec2f pattern_corners[4] = {
    Vec2f(0, 0),
    Vec2f(pattern_image.width(), 0),
    Vec2f(0, pattern_image.height()),
    Vec2f(pattern_image.width(), pattern_image.height()),
  };
  Vec2f pattern_right_vector = (pattern_corners[1] - pattern_corners[0]).normalized();
  Vec2f pattern_down_vector = (pattern_corners[2] - pattern_corners[0]).normalized();

  // Create output directory
  QDir dataset_dir(QString::fromStdString(path));
  dataset_dir.mkpath(".");

  // Write dataset.yaml file
  std::string dataset_yaml_path = dataset_dir.absoluteFilePath("dataset.yaml").toStdString();
  std::ofstream dataset_yaml_file(dataset_yaml_path.c_str(), std::ios::out);
  if (dataset_yaml_file) {
    dataset_yaml_file << "- camera: \"Synthetic pinhole camera (fx: " << fx << ", fy: " << fy << ", cx: " << cx << ", cy: " << cy << ", 'pixel corner' coordinate origin convention)\"" << endl;
    dataset_yaml_file << "  path: \"images0\"" << endl;
  } else {
    LOG(ERROR) << "Failed to write dataset YAML file at: " << dataset_yaml_path;
    return EXIT_FAILURE;
  }

  // Create images directory
  QDir images_dir = dataset_dir.filePath("images0");
  images_dir.mkpath(".");

  // Render the images
  Image<float> pattern_rendering(width, height);
  Image<u8> rendered_image(width, height);
  int num_images = 500;
  for (int image_index = 0; image_index < num_images; ++ image_index) {
    LOG(INFO) << "Rendering image " << image_index << " ...";

    // Determine a random image pose in front of the pattern, ensuring that all
    // corners of the AprilTag are visible
    SE3d camera_tr_global;
    Mat3f camera_r_global;
    Vec3f camera_t_global;

    bool any_apriltag_visible = false;
    while (!any_apriltag_visible) {
      camera_tr_global = SE3d();
      camera_tr_global.translation().x() -= (-1 + 2 * ((rand() % 10000) / 10000.f)) * pattern_image.width();
      camera_tr_global.translation().y() -= (-1 + 2 * ((rand() % 10000) / 10000.f)) * pattern_image.height();
      camera_tr_global.translation().z() += 500 + 800 * ((rand() % 10000) / 10000.f);
      camera_tr_global = SE3d::exp(0.5 * SE3d::Tangent::Random()) * camera_tr_global;

      camera_r_global = camera_tr_global.rotationMatrix().cast<float>();
      camera_t_global = camera_tr_global.translation().cast<float>();

      for (int i = 0; i < apriltag_min_xy.size(); ++ i) {
        const Vec3f& top_left = apriltag_min_xy[i];
        const Vec3f& bottom_right = apriltag_max_xy[i];
        Vec3f top_right = Vec3f(bottom_right.x(), top_left.y(), bottom_right.z());
        Vec3f bottom_left = Vec3f(top_left.x(), bottom_right.y(), bottom_right.z());

        Vec2f dummy;
        if (!camera.ProjectToPixelCornerConvIfVisible(camera_r_global * top_left + camera_t_global, 0, &dummy)) {
          continue;
        }
        if (!camera.ProjectToPixelCornerConvIfVisible(camera_r_global * top_right + camera_t_global, 0, &dummy)) {
          continue;
        }
        if (!camera.ProjectToPixelCornerConvIfVisible(camera_r_global * bottom_left + camera_t_global, 0, &dummy)) {
          continue;
        }
        if (!camera.ProjectToPixelCornerConvIfVisible(camera_r_global * bottom_right + camera_t_global, 0, &dummy)) {
          continue;
        }

        any_apriltag_visible = true;
        break;
      }
    }

    SE3d global_tr_camera = camera_tr_global.inverse();
    Mat3f global_r_camera = global_tr_camera.rotationMatrix().cast<float>();
    Vec3f global_t_camera = global_tr_camera.translation().cast<float>();

    // Render the polygons:
    // - Loop over all polygons and project their points into the image
    // - For a polygon, loop over all pixels in its bounding box
    // - Clip the polygon with the pixel's area as (convex) clipping path
    // - Darken the pixel by the amount of its area that is covered by the
    //   polygon (this works since the polygons are non-overlapping).
    pattern_rendering.SetTo(1.f);
    for (vector<Vec3f>& polygon : geometry_3d) {
      // Project the polygon points into the image
      vector<Vec2d> projected(polygon.size());
      for (usize i = 0; i < polygon.size(); ++ i) {
        projected[i] = camera.ProjectToPixelCornerConv(camera_r_global * polygon[i] + camera_t_global).cast<double>();
      }

      // Compute the bounding box of the polygon
      Eigen::AlignedBox2d bbox;
      for (const Vec2d& point : projected) {
        bbox.extend(point);
      }

      // Loop over all pixels that intersect the bounding box
      int min_x = max<int>(0, bbox.min().x());
      int max_x = min<int>(pattern_rendering.width() - 1, bbox.max().x());
      int min_y = max<int>(0, bbox.min().y());
      int max_y = min<int>(pattern_rendering.height() - 1, bbox.max().y());
      for (int y = min_y; y <= max_y; ++ y) {
        for (int x = min_x; x <= max_x; ++ x) {
          // Intersect the pixel area and the projected polygon
          vector<Vec2d> pixel_area(4);
          pixel_area[0] = Vec2d(x, y);
          pixel_area[1] = Vec2d(x + 1, y);
          pixel_area[2] = Vec2d(x + 1, y + 1);
          pixel_area[3] = Vec2d(x, y + 1);
          vector<Vec2d> intersection;
          ConvexClipPolygon(
              projected,
              pixel_area,
              &intersection);
          pattern_rendering(x, y) -= PolygonArea(intersection);
        }
      }
    }

    // Loop over all pixels in the image and either use the pattern rendering
    // or the loaded pattern image depending on if the pixel is within the
    // pattern or not.
    for (int y = 0; y < height; ++ y) {
      for (int x = 0; x < width; ++ x) {
        // Determine the intersection of the center viewing ray of this pixel with the pattern plane
        Vec2f rendering_coord(x + 0.5f, y + 0.5f);  // in "pixel corner" convention
        Vec3f local_direction = camera.UnprojectFromPixelCornerConv(rendering_coord);  // not necessarily normalized

        Vec3f global_direction = global_r_camera * local_direction;
        const Vec3f& global_camera_position = global_t_camera;
        Eigen::ParametrizedLine<float, 3> viewing_line(global_camera_position, global_direction.normalized());

        Vec3f pattern_intersection = viewing_line.intersectionPoint(pattern_plane);

        // Compute the local 2D pattern coordinates of the intersection point
        Vec2f local_pattern_intersection_2d(pattern_intersection.x() - pattern_corners[0].x(), pattern_intersection.y() - - pattern_corners[0].y());
        Vec2f pattern_image_coord(
            pattern_right_vector.dot(local_pattern_intersection_2d),
            pattern_down_vector.dot(local_pattern_intersection_2d));

        // Transform the coordinate from the pattern image into the pattern feature coordinate system.
        Vec2f pattern_image_coord_mm = Vec2f(pattern.page_width_mm, pattern.page_height_mm).cwiseQuotient(pattern_image.size().cast<float>()).cwiseProduct(pattern_image_coord);
        Vec2f pattern_coord =
            (pattern_image_coord_mm - Vec2f(pattern.pattern_start_x_mm, pattern.pattern_start_y_mm))
                .cwiseQuotient(Vec2f(pattern.pattern_end_x_mm - pattern.pattern_start_x_mm, pattern.pattern_end_y_mm - pattern.pattern_start_y_mm))
                .cwiseProduct(Vec2f(pattern.squares_x, pattern.squares_y)) -
            Vec2f::Constant(1);

        // If the pattern coordinate is within the area of the repeating
        // pattern, use the definition of the pattern to render it ourselves
        // (instead of looking up the pattern image). This ensures that no
        // potential biases are introduced by the rendering process of that
        // image or by its pixel sampling pattern.
        const PatternData& pattern = detector.GetPatternData(0);
        if (pattern.IsValidPatternCoord(pattern_coord.x(), pattern_coord.y())) {
          rendered_image(x, y) = std::max<float>(0.f, 255.99f * pattern_rendering(x, y));
        } else {
          Vec2f pattern_image_coord_pixel_center = pattern_image_coord - Vec2f::Constant(0.5f);
          if (pattern_image.ContainsPixelCenterConv(pattern_image_coord_pixel_center)) {
            rendered_image(x, y) = pattern_image.InterpolateBilinear<float>(pattern_image_coord_pixel_center);
          } else {
            rendered_image(x, y) = 0;
          }
        }
      }
    }

    // Save the rendered image.
    rendered_image.Write(images_dir.absoluteFilePath(QString::number(image_index).rightJustified(6, '0') + ".png").toStdString());
  }

  return EXIT_SUCCESS;
}

}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2011 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2011 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// 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_STRATEGIES_AGNOSTIC_POINT_IN_BOX_BY_SIDE_HPP
#define BOOST_GEOMETRY_STRATEGIES_AGNOSTIC_POINT_IN_BOX_BY_SIDE_HPP

#include <boost/array.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/algorithms/assign.hpp>
#include <boost/geometry/strategies/covered_by.hpp>
#include <boost/geometry/strategies/within.hpp>

namespace boost { namespace geometry { namespace strategy
{

namespace within
{

struct decide_within
{
    static inline bool apply(int side, bool& result)
    {
        if (side != 1)
        {
            result = false;
            return false;
        }
        return true; // continue
    }
};

struct decide_covered_by
{
    static inline bool apply(int side, bool& result)
    {
        if (side != 1)
        {
            result = side >= 0;
            return false;
        }
        return true; // continue
    }
};

template <typename Point, typename Box, typename Decide = decide_within>
struct point_in_box_by_side
{
    typedef typename strategy::side::services::default_strategy
    <
        typename cs_tag<Box>::type
    >::type side_strategy_type;

    static inline bool apply(Point const& point, Box const& box)
    {
        // Create (counterclockwise) array of points, the fifth one closes it
        // Every point should be on the LEFT side (=1), or ON the border (=0),
        // So >= 1 or >= 0
        boost::array<typename point_type<Box>::type, 5> bp;
        geometry::detail::assign_box_corners_oriented<true>(box, bp);
        bp[4] = bp[0];

        bool result = true;
        side_strategy_type strategy;
        boost::ignore_unused_variable_warning(strategy);

        for (int i = 1; i < 5; i++)
        {
            int const side = strategy.apply(point, bp[i - 1], bp[i]);
            if (! Decide::apply(side, result))
            {
                return result;
            }
        }

        return result;
    }
};

} // namespace within

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

namespace within { namespace services
{

template <typename Point, typename Box>
struct default_strategy
    <
        point_tag, box_tag,
        point_tag, areal_tag,
        spherical_tag, spherical_tag,
        Point, Box
    >
{
    typedef within::point_in_box_by_side
                <
                    Point, Box, within::decide_within
                > type;
};

}} // namespace within::services

namespace covered_by { namespace services
{

template <typename Point, typename Box>
struct default_strategy
    <
        point_tag, box_tag,
        point_tag, areal_tag,
        spherical_tag, spherical_tag,
        Point, Box
    >
{
    typedef within::point_in_box_by_side
                <
                    Point, Box, within::decide_covered_by
                > type;
};

}} // namespace covered_by::services

#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}}} // namespace boost::geometry::strategy

#endif // BOOST_GEOMETRY_STRATEGIES_AGNOSTIC_POINT_IN_BOX_BY_SIDE_HPP


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

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

#include "graph_augment.hh"

#include <boost/graph/max_cardinality_matching.hpp>

#include <boost/bind.hpp>

using namespace graph_tool;
using namespace boost;

struct get_max_cardinality_matching
{
    template <class Graph, class VertexIndex, class MatchMap>
    void operator()(Graph& g, VertexIndex vertex_index, MatchMap match,
                    bool &check) const
    {
        typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;
        unchecked_vector_property_map<vertex_t,VertexIndex>
            mate(vertex_index, num_vertices(g));

        typename graph_traits<Graph>::edge_iterator e, e_end;
        for (tie(e, e_end) = edges(g); e != e_end; ++e)
            match[*e] = false;

        check =
            checked_edmonds_maximum_cardinality_matching(g, mate, vertex_index);

        for (tie(e, e_end) = edges(g); e != e_end; ++e)
        {
            if (mate[source(*e,g)] != graph_traits<Graph>::null_vertex() &&
                mate[source(*e,g)] == target(*e,g))
            {
                // search for an already matched parallel edge
                bool matched = false;
                typename graph_traits<Graph>::out_edge_iterator oe, oe_end;
                for (tie(oe, oe_end) = out_edges(source(*e, g),g); oe != oe_end;
                     ++oe)
                {
                    if (match[*oe])
                    {
                        matched = true;
                        break;
                    }
                }
                if (!matched)
                    match[*e] = true;
            }
        }
    }
};

bool max_cardinality_matching(GraphInterface& gi, boost::any match)
{
    bool check;
    run_action<graph_tool::detail::never_directed>()
        (gi, std::bind(get_max_cardinality_matching(),
                        placeholders::_1, gi.GetVertexIndex(),
                       placeholders::_2, std::ref(check)),
         writable_edge_scalar_properties()) (match);
    return check;
}


#include <algorithm>
#include <chrono>
#include <cmath>
#include <cstdint>
#include <fstream>
#include <iostream>
#include <random>
#include <string>
#include <vector>

#include <boost/program_options.hpp>

#include <libbenchmark/testsuite.hpp>

#include <libsimd/algorithms/scale/naive.hpp>

#ifdef ENABLE_SIMD_AVX2

#include <libsimd/algorithms/scale/avx2/fma.hpp>

#endif

const std::size_t repeats = 100;

benchmark::testrun perform_test(std::uniform_int_distribution<> & distribution, std::mt19937 & generator, std::size_t values)
{
	std::vector<std::uint8_t> data;
	data.reserve(values);

 	for (std::size_t n = 0; n < values; ++n)
	{
        data.push_back(distribution(generator));
    }

    std::vector<std::uint8_t> result;
    result.resize(data.size());

	benchmark::testrun tr(values);

	// naive without SIMD
	{
		std::vector<std::chrono::microseconds> durations;
		durations.reserve(repeats);

		for (std::size_t i = 0; i < repeats; ++i)
		{
			auto start = std::chrono::system_clock::now();

			simd::algorithms::scale_naive(&data[0], data.size(), &result[0], 3.1415, 42);

			auto stop = std::chrono::system_clock::now();

			durations.push_back(std::chrono::duration_cast<std::chrono::microseconds>(stop - start));
		}

		auto sum = std::accumulate(durations.begin(),
								   durations.end(),
								   static_cast<std::size_t>(0),
								   [](std::size_t sum, auto duration)
								   {
									   return sum + duration.count();
								   });

		tr.add_result("naive", sum / static_cast<double>(repeats));
	}

#ifdef ENABLE_SIMD_AVX2
	// AVX2 with 'fma' instruction
	{
		std::vector<std::chrono::microseconds> durations;
		durations.reserve(repeats);

		for (std::size_t i = 0; i < repeats; ++i)
		{
			auto start = std::chrono::system_clock::now();

			simd::algorithms::avx2::scale_fma(&data[0], data.size(), &result[0], 3.1415, 42);

			auto stop = std::chrono::system_clock::now();

			durations.push_back(std::chrono::duration_cast<std::chrono::microseconds>(stop - start));
		}

		auto sum = std::accumulate(durations.begin(),
								   durations.end(),
								   static_cast<std::size_t>(0),
								   [](std::size_t sum, auto duration)
								   {
									   return sum + duration.count();
								   });

		tr.add_result("AVX2 (fma)", sum / static_cast<double>(repeats));
	}
#endif

	return tr;
}

int main(int argc, char * argv[])
{
	namespace po = boost::program_options;

	po::options_description general_options("general options");
	general_options.add_options()
		("help,h", "show this help text")
		("csv,c", "generate separate CSV files of performance results")
		("csv-all", "generate a single CSV file of all performance results")
	;

	po::options_description cmdline_options("usage: bench_sum [options]");
	cmdline_options.add(general_options);

	po::variables_map variables;
	po::store(po::parse_command_line(argc, argv, cmdline_options), variables);
	po::notify(variables);

	if (variables.count("help"))
	{
		std::cout << cmdline_options;
		return 1;
	}

	// create random numbers (inspired by https://en.cppreference.com/w/cpp/numeric/random/uniform_int_distribution)

    std::random_device rd;  // will be used to obtain a seed for the random number engine
    std::mt19937 gen(rd()); // standard mersenne_twister_engine seeded with rd()

    std::uniform_int_distribution<> dist(0, 255);

	benchmark::testsuite ts("scale");

	for (std::size_t i = 4; i <= 14; ++i)
	{
		ts.add(perform_test(dist, gen, 1024 * std::pow(2, i)));
	}

	std::cout << ts.to_string() << std::endl;

	if (variables.count("csv"))
	{
		for (auto const & r : ts.to_csv())
		{
			std::string filename(ts.title());
			filename.append("_");
			filename.append(std::to_string(r.quantity));
			filename.append(".csv");
			std::replace(filename.begin(), filename.end(), ' ', '_');

			std::ofstream file;
			file.open(filename);
			file << r.data;
			file.close();
		}
	}

	if (variables.count("csv-all"))
	{
		auto r = ts.to_csv(false);

		if (!r.empty())
		{
			std::string filename(ts.title());
			filename.append("_all.csv");
			std::replace(filename.begin(), filename.end(), ' ', '_');

			std::ofstream file;
			file.open(filename);
			file << r.front().data;
			file.close();
		}
	}

	return 0;
}


///////////////////////////////////////////////////////////////////////////////
// distribution::survival::response::right_truncated::event.hpp        		 //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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 BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_RESPONSE_RIGHT_TRUNCATED_EVENT_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_RESPONSE_RIGHT_TRUNCATED_EVENT_HPP_ER_2009
#include <limits>
#include <ostream>
#include <boost/format.hpp>
#include <boost/operators.hpp>
//#include <boost/archive/text_oarchive.hpp>
//#include <boost/archive/text_iarchive.hpp>
#include <boost/statistics/detail/math/constant/shortcut.hpp>
#include <boost/statistics/detail/math/function/versatile_equal.hpp>

namespace boost {
namespace statistics{
namespace detail{
namespace distribution{
namespace survival {
namespace response {

    // Abstraction for failure indicator and event time.
    //
    // B == T can be useful to represent a proportion of failures (see
    // mean_event)
    template<typename T,typename B = bool>
    class event : equality_comparable<event<T,B> >{
        typedef statistics::detail::math::constant::shortcut<T> const_;
        public:
        typedef T value_type;
        typedef B failure_type;
        // Construction
        event():failure_(false),time_(const_::inf){}
        explicit event(B isf,value_type rt):failure_(isf),time_(rt){}
        event(const event& that):failure_(that.failure_),time_(that.time_){}
        event& operator=(const event& that){
        	if(&that!=this){
            	failure_ = (that.failure_);
            	time_ = (that.time_);
        	}
        	return *this;
    	}

        // Access
        B failure()const{ return failure_; }
        value_type time()const{ return time_; }

        // Operators
        bool operator==(const event& e)
    	{
        	bool eq_1 = math::function::versatile_equal(
            	e.time(),
            	(this->time())
        	);
        	bool eq_2 = math::function::versatile_equal(
            	(this->failure_) , (e.failure())
        	);
        	return  (eq_1 && eq_2);
    	}

        protected:
        B failure_;         //!censored
        value_type time_;   //since entry time
    };

    template<typename T,typename B>
    std::ostream& operator<<(std::ostream& out,const event<T,B>& e){
        static const char* str = "(%1%,%2%)";
        typedef boost::format f_;
        f_ f(str);
        f % e.failure() % e.time();
        out << f.str();
        return out;
    }

}// response
}// survival
}// distribution
}// detail
}// statistics
}// boost

#endif


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  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_SIMD_FUNCTION_FAST_RSQRT_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_FAST_RSQRT_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!
  @ingroup group-arithmetic
    Function object implementing fast_rsqrt capabilities

    Computes fast_rsqrt value of its parameter.

  **/
  const boost::dispatch::functor<tag::fast_rsqrt_> fast_rsqrt = {};
} }
#endif

#include <boost/simd/function/scalar/fast_rsqrt.hpp>

#endif


#include <cstddef>
#include <iostream>

#include <boost/random.hpp>
#include <boost/numeric/ublas/io.hpp>

#include "polar_decomposition.hpp"

int main()
{
    boost::mt19937 seed(42);
    boost::uniform_real<> dist(-10.0, 10.0);
    boost::variate_generator<boost::mt19937&, boost::uniform_real<> > random(seed, dist);

    boost::numeric::ublas::matrix<double> A(3,3), U, H;

    for (std::size_t i = 0; i < 100000; ++i)
    {
        std::generate(A.data().begin(), A.data().end(), random);
        polar::polar_decomposition(A, U, H);
    }

    std::cout << A << std::endl;
    std::cout << prod(U, trans(U)) << std::endl;
}


#include <cassert>
#include <climits>
#include <cmath>
#include <random>
#include <unordered_set>
#include <vector>

#include <boost/math/special_functions/prime.hpp>
#include <boost/multiprecision/miller_rabin.hpp>

#include "orbit.hpp"
#include "perm.hpp"
#include "perm_set.hpp"
#include "pr_randomizer.hpp"
#include "util.hpp"

namespace mpsym
{

namespace internal
{

PrRandomizer::PrRandomizer(PermSet const &generators,
                           unsigned n_generators,
                           unsigned iterations)
: _gens_orig(generators)
{
  generators.assert_not_empty();

  _gens.insert(Perm(generators.degree()));

  if (generators.size() >= n_generators) {
    _gens.insert(generators.begin(), generators.end());
    n_generators = generators.size();
  } else {
    while (_gens.size() < n_generators) {
      unsigned missing = n_generators - _gens.size();
      if (missing > generators.size()) {
        _gens.insert(generators.begin(), generators.end());
      } else {
        _gens.insert(generators.begin(), generators.begin() + missing);
        break;
      }
    }
  }

  for (unsigned i = 0u; i < iterations; ++i)
    next();
}

Perm PrRandomizer::next()
{
  static auto re(util::random_engine());

  std::uniform_int_distribution<> randbool(0, 1);
  std::uniform_int_distribution<> rands(1, _gens.size() - 1);
  std::uniform_int_distribution<> randt(1, _gens.size() - 1);

  int s, t;

  s  = rands(re);
  do { t = randt(re); } while (t == s);

  if (randbool(re)) {
    _gens[s] *= (randbool(re) ? _gens[t] : ~_gens[t]);
    _gens[0] *= _gens[s];
  } else {
    _gens[s] = (randbool(re) ? _gens[t] : ~_gens[t]) * _gens[s];
    _gens[0] = _gens[s] * _gens[0];
  }

  return _gens[0];
}

bool PrRandomizer::test_symmetric(double epsilon)
{
  if (!test_altsym(epsilon))
    return false;

  return !generators_even();
}

bool PrRandomizer::test_altsym(double epsilon)
{
  assert(epsilon > 0.0);

  assert(_gens_orig.degree() >= 8u);

  // build prime number lookup table
  static std::unordered_set<unsigned> prime_lookup;

  if (prime_lookup.empty()) {
    //for (auto i = 0u; i <= boost::math::max_prime; ++i)
    for (auto i = 0u; i <= 1000u; ++i)
      prime_lookup.insert(boost::math::prime(i));
  }

  // check whether group is even transitive
  auto orbit(Orbit::generate(1, _gens_orig.with_inverses()));

  if (orbit.size() != _gens_orig.degree())
    return false;

  // determine number of random elements to be tested
  double d = std::log(2.0) / std::log(static_cast<double>(_gens_orig.degree()));
  if (_gens_orig.degree() <= 16u)
    d *= 0.23;
  else
    d *= 0.39;

  double iterations_lower_bound = -std::log(epsilon) / d;
  assert(iterations_lower_bound < static_cast<double>(UINT_MAX));

  unsigned iterations =
    static_cast<unsigned>(std::ceil(iterations_lower_bound));

  // test whether random element contains p-cycle
  unsigned p_lower_bound = _gens_orig.degree() / 2u;
  unsigned p_upper_bound = _gens_orig.degree() - 2u;

  for (unsigned i = 0u; i < iterations; ++i) {
    for (auto const &cycle : next().cycles()) {
      auto cycle_len = cycle.size();

      bool is_prime = cycle_len <= boost::math::max_prime ?
        prime_lookup.find(cycle_len) != prime_lookup.end() :
        boost::multiprecision::miller_rabin_test(cycle_len, 25);

      if (!is_prime)
        continue;

      if (cycle_len > p_lower_bound && cycle_len < p_upper_bound)
        return true;
    }
  }

  return false;
}

bool PrRandomizer::generators_even()
{
  for (auto const &gen : _gens_orig) {
    if (!gen.even()) {
      return false;
    }
  }

  return true;
}

} // namespace internal

} // namespace mpsym


// Copyright Oleg Maximenko 2014.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// See http://github.com/svgpp/svgpp for library home page.

#pragma once

#include <svgpp/traits/length_units.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/mpl/has_key.hpp>
#include <cmath>

namespace svgpp { namespace factory { namespace length
{

template<class LengthType = double, class NumberType = double, class ReferenceAbsoluteUnits = tag::length_units::mm>
class unitless_absolute
{
public:
  typedef LengthType length_type;
  typedef NumberType number_type;

  unitless_absolute()
  {
    // Set 96 PPI as reasonable default
    set_absolute_units_coefficient(96, tag::length_units::in());
  }

  unitless_absolute(number_type reference_to_output_coeff)
    : reference_to_output_coeff_(reference_to_output_coeff)
  {}

  template<class AbsoluteUnits>
  void set_absolute_units_coefficient(number_type coeff, AbsoluteUnits)
  {
    BOOST_MPL_ASSERT((boost::mpl::has_key<traits::absolute_length_units, AbsoluteUnits>));
    typedef typename traits::absolute_length_conversion_coefficient<ReferenceAbsoluteUnits, AbsoluteUnits>::ratio ratio;
    reference_to_output_coeff_ = coeff * ratio::num / ratio::den;
  }

  template<class AbsoluteUnits>
  number_type get_absolute_units_coefficient(AbsoluteUnits) const
  {
    BOOST_MPL_ASSERT((boost::mpl::has_key<traits::absolute_length_units, AbsoluteUnits>));
    typedef typename traits::absolute_length_conversion_coefficient<AbsoluteUnits, ReferenceAbsoluteUnits>::ratio ratio;
    return reference_to_output_coeff_ * ratio::num / ratio::den;
  }

  template<class Units>
  typename boost::enable_if<boost::mpl::has_key<traits::absolute_length_units, Units>, length_type>::type
  create_length(number_type number, Units) const
  {
    typedef typename traits::absolute_length_conversion_coefficient<Units, ReferenceAbsoluteUnits>::ratio ratio;
    return static_cast<length_type>(number * reference_to_output_coeff_ * ratio::num / ratio::den );
  }

private:
  number_type reference_to_output_coeff_;
};

template<class LengthType = double, class NumberType = double>
class unitless_viewport
{
public:
  unitless_viewport(LengthType viewportWidth = 500, LengthType viewportHeight = 500)
  {
    set_viewport_size(viewportWidth, viewportHeight);
  }

  void set_viewport_size(LengthType width, LengthType height)
  {
    // TODO: what if one of the types is integer?
    widthCoefficient_ = width * 0.01;
    heightCoefficient_ = height * 0.01;
    otherCoefficient_ = std::sqrt((width * width + height * height) / 2) * 0.01;
  }

  LengthType create_length(NumberType number, tag::length_units::percent, tag::length_dimension::width) const
  {
    return static_cast<LengthType>(widthCoefficient_ * number);
  }

  LengthType create_length(NumberType number, tag::length_units::percent, tag::length_dimension::height) const
  {
    return static_cast<LengthType>(heightCoefficient_ * number);
  }

  LengthType create_length(NumberType number, tag::length_units::percent, tag::length_dimension::not_width_nor_height) const
  {
    return static_cast<LengthType>(otherCoefficient_ * number);
  }

private:
  NumberType widthCoefficient_, heightCoefficient_, otherCoefficient_;
};

template<class LengthType = double, class NumberType = double>
class unitless_user_units
{
public:
  unitless_user_units(NumberType user_units_coefficient = 1)
    : user_units_coefficient_(user_units_coefficient)
  {}

  LengthType create_length(NumberType number, tag::length_units::px) const
  {
    return number * user_units_coefficient_;
  }

  LengthType create_length(NumberType number, tag::length_units::none) const
  {
    return number * user_units_coefficient_;
  }

  void set_user_units_coefficient(NumberType coeff) { user_units_coefficient_ = coeff; }
  NumberType get_user_units_coefficient() const { return user_units_coefficient_; }

private:
  NumberType user_units_coefficient_;
};

template<class LengthType = double, class NumberType = double, class ReferenceAbsoluteUnits = tag::length_units::mm>
class unitless:
  public unitless_absolute<LengthType, NumberType, ReferenceAbsoluteUnits>,
  public unitless_viewport<LengthType, NumberType>,
  public unitless_user_units<LengthType, NumberType>
{
public:
  using unitless_absolute<LengthType, NumberType, ReferenceAbsoluteUnits>::create_length;
  using unitless_viewport<LengthType, NumberType>::create_length;
  using unitless_user_units<LengthType, NumberType>::create_length;

  unitless()
    : em_coefficient_(10)
    , ex_coefficient_(10)
  {}

  LengthType create_length(NumberType number, tag::length_units::em) const
  {
    return number * em_coefficient_;
  }

  LengthType create_length(NumberType number, tag::length_units::ex) const
  {
    return number * ex_coefficient_;
  }

  void set_em_coefficient(NumberType coeff)
  {
    em_coefficient_ = coeff;
  }

  void set_ex_coefficient(NumberType coeff)
  {
    ex_coefficient_ = coeff;
  }

  void set_em_coefficient(NumberType coeff, tag::length_units::px)
  {
    em_coefficient_ = this->get_user_units_coefficient() * coeff;
  }

  void set_ex_coefficient(NumberType coeff, tag::length_units::px)
  {
    ex_coefficient_ = this->get_user_units_coefficient() * coeff;
  }

  template<class AbsoluteUnits>
  typename boost::enable_if<boost::mpl::has_key<traits::absolute_length_units, AbsoluteUnits> >::type
  set_em_coefficient(NumberType coeff, AbsoluteUnits unitsTag)
  {
    em_coefficient_ = coeff * this->get_absolute_units_coefficient(unitsTag);
  }

  template<class AbsoluteUnits>
  typename boost::enable_if<boost::mpl::has_key<traits::absolute_length_units, AbsoluteUnits> >::type
  set_ex_coefficient(NumberType coeff, AbsoluteUnits unitsTag)
  {
    ex_coefficient_ = coeff * this->get_absolute_units_coefficient(unitsTag);
  }

  // Optional length_to_user_coordinate interface that may be used when converting
  // basic shapes coordinates to path in some adapters
  typedef LengthType coordinate_type;
  static coordinate_type length_to_user_coordinate(LengthType length, tag::length_dimension::any const &)
  {
    return length;
  }

private:
  NumberType em_coefficient_, ex_coefficient_;
};

typedef unitless<> default_factory;

}}}