#include <stdint.h>
#include <alloca.h>
#include <Eigen/Core>
#include <omp.h>
#include "mex.h"

using namespace Eigen;
using namespace std;

// NOTE: mxSetProperty and possibly mxGetProperty make copies, even with
// classdef < handle! lame! I believe mxGetField does NOT make a copy, though I
// haven't tested it recently

// TODO if we aren't outputting gamma, don't need to write it to memory (just
// need t and t+1), so we can save the stack array for each HMM at the cost of
// a branch

void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{

    /* SETUP */
    //Only acceptable if we have at least the 5 basic args and some number of
    //name:value argument pairs
    if (nrhs < 5 || nrhs % 2 == 0) { mexErrMsgTxt("wrong number of arguments\n"); }

    //// pull out inputs
    IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
    // data
    if (!mxGetField(prhs[0],0,"data")) { mexErrMsgTxt("data missing 'data' field\n"); }
    int8_t *alldata = (int8_t *) mxGetData(mxGetField(prhs[0],0,"data"));
    int bigT = mxGetN(mxGetField(prhs[0],0,"data")); // total length of the data
    int num_subparts = mxGetM(mxGetField(prhs[0],0,"data")); // number of sub-parts

    if (!mxGetField(prhs[0],0,"resources")) { mexErrMsgTxt("data missing 'resources' field\n"); }
    int16_t *allresources = (int16_t *) mxGetData(mxGetField(prhs[0],0,"resources"));

    if (!mxGetField(prhs[0],0,"starts")) { mexErrMsgTxt("data missing 'starts' field\n"); }
    int32_t *starts = (int32_t *) mxGetData(mxGetField(prhs[0],0,"starts"));

    //TYPO WAS HERE
    int num_sequences = max(mxGetN(mxGetField(prhs[0],0,"starts")),
            mxGetM(mxGetField(prhs[0],0,"starts")));

    if (!mxGetField(prhs[0],0,"lengths")) { mexErrMsgTxt("data missing 'lengths' field\n"); }
    int32_t *lengths = (int32_t *) mxGetData(mxGetField(prhs[0],0,"lengths"));

    // parameters struct
    if (!mxGetField(prhs[1],0,"learns")) { mexErrMsgTxt("model missing 'learns' field\n"); }
    double *learns = mxGetPr(mxGetField(prhs[1],0,"learns"));
    int num_resources = max(mxGetM(mxGetField(prhs[1],0,"learns")),
            mxGetN(mxGetField(prhs[1],0,"learns")));

    if (!mxGetField(prhs[1],0,"forgets")) { mexErrMsgTxt("model missing 'forgets' field\n"); }
    double *forgets = mxGetPr(mxGetField(prhs[1],0,"forgets"));

    if (!mxGetField(prhs[1],0,"guesses")) { mexErrMsgTxt("model missing 'guesses' field\n"); }
    double *guess = mxGetPr(mxGetField(prhs[1],0,"guesses"));

    if (!mxGetField(prhs[1],0,"slips")) { mexErrMsgTxt("model missing 'slips' field\n"); }
    double *slip = mxGetPr(mxGetField(prhs[1],0,"slips"));

    if (!mxGetField(prhs[1],0,"prior")) { mexErrMsgTxt("model missing 'prior' field\n"); }
    double prior = mxGetScalar(mxGetField(prhs[1],0,"prior"));

    /*CHECK FOR OPTIONAL NAME:VALUE PARAMS*/
    bool normalizeLengths = false;
    if (nrhs >= 7) {
        int i = 5;
        char* optName;
        for(i; i + 1 < nrhs; i+= 2) {
            if(mxIsChar(prhs[i])) {
                optName = mxArrayToString(prhs[i]);
                if(optName != NULL) {
                    if(strcmp(optName, "normalize length") == 0) {
                        if(mxIsLogical(prhs[i + 1])) {
                            normalizeLengths = *mxGetLogicals(prhs[i + 1]);
                        }
                        else {
                            mexErrMsgIdAndTxt("E_Step:invalidInputValue",
                                    "Expected boolean value for normalize length");
                        }

                    }
                    //other options here, if you want
                    //If no matches, this is an unexpected param, so yell
                    else {
                        mexErrMsgIdAndTxt("E_Step:invalidInput",
                                "invalid named parameter");
                    }
                }

                mxFree(optName);
            } else {
                mexErrMsgIdAndTxt( "E_Step:invalidInputType",
                "Expected string for name:value pair.");
            }
        }
    }

    Array2d initial_distn;
    initial_distn << 1-prior, prior;

    MatrixXd As(2,2*num_resources);
    for (int n=0; n<num_resources; n++) {
        As.col(2*n) << 1-learns[n], learns[n];
        As.col(2*n+1) << forgets[n], 1-forgets[n];
    }

    Array2Xd Bn(2,2*num_subparts);
    for (int n=0; n<num_subparts; n++) {
        Bn.col(2*n) << 1-guess[n], slip[n]; // incorrect
        Bn.col(2*n+1) << guess[n], 1-slip[n]; // correct
    }

    //// outputs

    // rhs outputs
    Map<ArrayXXd,Aligned> all_trans_softcounts(mxGetPr(prhs[2]),2,2*num_resources);
    all_trans_softcounts.setZero();
    Map<Array2Xd,Aligned> all_emission_softcounts(mxGetPr(prhs[3]),2,2*num_subparts);
    all_emission_softcounts.setZero();
    Map<Array2d,Aligned> all_initial_softcounts(mxGetPr(prhs[4]));
    all_initial_softcounts.setZero();

    // lhs outputs
    Map<Array2Xd,Aligned> likelihoods_out(NULL,2,bigT);
    Map<Array2Xd,Aligned> gamma_out(NULL,2,bigT);
    Map<Array2Xd,Aligned> alpha_out(NULL,2,bigT);
    double s_total_loglike = 0;
    double *total_loglike = &s_total_loglike;
    switch (nlhs)
    {
        case 4:
            plhs[3] = mxCreateDoubleMatrix(2,bigT,mxREAL);
            new (&likelihoods_out) Map<Array2Xd,Aligned>(mxGetPr(plhs[3]),2,bigT);
        case 3:
            plhs[2] = mxCreateDoubleMatrix(2,bigT,mxREAL);
            new (&gamma_out) Map<Array2Xd,Aligned>(mxGetPr(plhs[2]),2,bigT);
        case 2:
            plhs[1] = mxCreateDoubleMatrix(2,bigT,mxREAL);
            new (&alpha_out) Map<Array2Xd,Aligned>(mxGetPr(plhs[1]),2,bigT);
        case 1:
            plhs[0] = mxCreateDoubleScalar(0.);
            total_loglike = mxGetPr(plhs[0]);
    }

    /* COMPUTATION */
    Eigen::initParallel();
    /* omp_set_dynamic(0); */
    /* omp_set_num_threads(6); */
    #pragma omp parallel
    {
        double s_trans_softcounts[2*2*num_resources] __attribute__((aligned(16)));
        double s_emission_softcounts[2*2*num_subparts] __attribute__((aligned(16)));
        Map<ArrayXXd,Aligned> trans_softcounts(s_trans_softcounts,2,2*num_resources);
        Map<ArrayXXd,Aligned> emission_softcounts(s_emission_softcounts,2,2*num_subparts);
        Array2d init_softcounts;
        double loglike;

        trans_softcounts.setZero();
        emission_softcounts.setZero();
        init_softcounts.setZero();
        loglike = 0;
        int num_threads = omp_get_num_threads();
        int blocklen = 1 + ((num_sequences - 1) / num_threads);
        int sequence_idx_start = blocklen * omp_get_thread_num();
        int sequence_idx_end = min(sequence_idx_start+blocklen,num_sequences);
        //mexPrintf("start:%d   end:%d\n", sequence_idx_start, sequence_idx_end);

        for (int sequence_index=sequence_idx_start; sequence_index < sequence_idx_end; sequence_index++) {

            // NOTE: -1 because Matlab indexing starts at 1
            int32_t sequence_start = starts[sequence_index] - 1;

            int32_t T = lengths[sequence_index];

            int8_t *data = alldata + num_subparts*sequence_start;
            int16_t *resources = allresources + sequence_start;

            //// likelihoods
            double s_likelihoods[2*T];
            Map<Array2Xd,Aligned> likelihoods(s_likelihoods,2,T);

            likelihoods.setOnes();
             for (int t=0; t<T; t++) {
                 for (int n=0; n<num_subparts; n++) {
                     if (data[n+num_subparts*t] != 0) {
                         likelihoods.col(t) *= Bn.col(2*n + (data[n+num_subparts*t] == 2));
                     }
                 }
             }

            //// forward messages
            double norm;
            double s_alpha[2*T] __attribute__((aligned(16)));
            double contribution;
            Map<MatrixXd,Aligned> alpha(s_alpha,2,T);
            alpha.col(0) = initial_distn * likelihoods.col(0);
            norm = alpha.col(0).sum();
            alpha.col(0) /= norm;
            contribution = log(norm);
            if(normalizeLengths) {
                contribution = contribution / T;
            }
            loglike += contribution;

            for (int t=0; t<T-1; t++) {
                alpha.col(t+1) = (As.block(0,2*(resources[t]-1),2,2) * alpha.col(t)).array()
                    * likelihoods.col(t+1);
                norm = alpha.col(t+1).sum();
                alpha.col(t+1) /= norm;
                contribution = log(norm);
                if(normalizeLengths) {
                    contribution = contribution / T;
                }
                loglike += contribution;
            }

            //// backward messages and statistic counting

            double s_gamma[2*T] __attribute__((aligned(16)));
            Map<Array2Xd,Aligned> gamma(s_gamma,2,T);
            gamma.col(T-1) = alpha.col(T-1);
            for (int n=0; n<num_subparts; n++) {
                if (data[n+num_subparts*(T-1)] != 0) {
                    emission_softcounts.col(2*n + (data[n+num_subparts*(T-1)] == 2)) += gamma.col(T-1);
                }
            }

            for (int t=T-2; t>=0; t--) {

                Matrix2d A = As.block(0,2*(resources[t]-1),2,2);
                Array22d pair = A.array();
                pair.rowwise() *= alpha.col(t).transpose().array();
                pair.colwise() *= gamma.col(t+1);
                pair.colwise() /= (A*alpha.col(t)).array();
                pair = (pair != pair).select(0.,pair); // NOTE: replace NaNs
                trans_softcounts.block(0,2*(resources[t]-1),2,2) += pair;

                gamma.col(t) = pair.colwise().sum().transpose();
                // NOTE: we have to touch the data again here
                for (int n=0; n<num_subparts; n++) {
                    if (data[n+num_subparts*t] != 0) {
                        emission_softcounts.col(2*n + (data[n+num_subparts*t] == 2)) += gamma.col(t);
                    }
                }
            }
            init_softcounts += gamma.col(0);

            switch (nlhs)
            {
                case 4:
                    likelihoods_out.block(0,sequence_start,2,T) = likelihoods;
                case 3:
                    gamma_out.block(0,sequence_start,2,T) = gamma;
                case 2:
                    alpha_out.block(0,sequence_start,2,T) = alpha;
            }
        }

        #pragma omp critical
        {
            all_trans_softcounts += trans_softcounts;
            all_emission_softcounts += emission_softcounts;
            all_initial_softcounts += init_softcounts;
            *total_loglike += loglike;
        }
    }

 }


// (C) Copyright 2007-2009 Andrew Sutton
//
// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0 (See accompanying file
// LICENSE_1_0.txt or http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GRAPH_DEGREE_CENTRALITY_HPP
#define BOOST_GRAPH_DEGREE_CENTRALITY_HPP

#include <boost/graph/graph_concepts.hpp>
#include <boost/concept/assert.hpp>

namespace boost
{

template < typename Graph > struct degree_centrality_measure
{
    typedef typename graph_traits< Graph >::degree_size_type degree_type;
    typedef typename graph_traits< Graph >::vertex_descriptor vertex_type;
};

template < typename Graph >
struct influence_measure : public degree_centrality_measure< Graph >
{
    typedef degree_centrality_measure< Graph > base_type;
    typedef typename base_type::degree_type degree_type;
    typedef typename base_type::vertex_type vertex_type;

    inline degree_type operator()(vertex_type v, const Graph& g)
    {
        BOOST_CONCEPT_ASSERT((IncidenceGraphConcept< Graph >));
        return out_degree(v, g);
    }
};

template < typename Graph >
inline influence_measure< Graph > measure_influence(const Graph&)
{
    return influence_measure< Graph >();
}

template < typename Graph >
struct prestige_measure : public degree_centrality_measure< Graph >
{
    typedef degree_centrality_measure< Graph > base_type;
    typedef typename base_type::degree_type degree_type;
    typedef typename base_type::vertex_type vertex_type;

    inline degree_type operator()(vertex_type v, const Graph& g)
    {
        BOOST_CONCEPT_ASSERT((BidirectionalGraphConcept< Graph >));
        return in_degree(v, g);
    }
};

template < typename Graph >
inline prestige_measure< Graph > measure_prestige(const Graph&)
{
    return prestige_measure< Graph >();
}

template < typename Graph, typename Vertex, typename Measure >
inline typename Measure::degree_type degree_centrality(
    const Graph& g, Vertex v, Measure measure)
{
    BOOST_CONCEPT_ASSERT((DegreeMeasureConcept< Measure, Graph >));
    return measure(v, g);
}

template < typename Graph, typename Vertex >
inline typename graph_traits< Graph >::degree_size_type degree_centrality(
    const Graph& g, Vertex v)
{
    return degree_centrality(g, v, measure_influence(g));
}

// These are alias functions, intended to provide a more expressive interface.

template < typename Graph, typename Vertex >
inline typename graph_traits< Graph >::degree_size_type influence(
    const Graph& g, Vertex v)
{
    return degree_centrality(g, v, measure_influence(g));
}

template < typename Graph, typename Vertex >
inline typename graph_traits< Graph >::degree_size_type prestige(
    const Graph& g, Vertex v)
{
    return degree_centrality(g, v, measure_prestige(g));
}

template < typename Graph, typename CentralityMap, typename Measure >
inline void all_degree_centralities(
    const Graph& g, CentralityMap cent, Measure measure)
{
    BOOST_CONCEPT_ASSERT((VertexListGraphConcept< Graph >));
    typedef typename graph_traits< Graph >::vertex_descriptor Vertex;
    typedef typename graph_traits< Graph >::vertex_iterator VertexIterator;
    BOOST_CONCEPT_ASSERT((WritablePropertyMapConcept< CentralityMap, Vertex >));
    typedef typename property_traits< CentralityMap >::value_type Centrality;

    VertexIterator i, end;
    for (boost::tie(i, end) = vertices(g); i != end; ++i)
    {
        Centrality c = degree_centrality(g, *i, measure);
        put(cent, *i, c);
    }
}

template < typename Graph, typename CentralityMap >
inline void all_degree_centralities(const Graph& g, CentralityMap cent)
{
    all_degree_centralities(g, cent, measure_influence(g));
}

// More helper functions for computing influence and prestige.
// I hate the names of these functions, but influence and prestige
// don't pluralize too well.

template < typename Graph, typename CentralityMap >
inline void all_influence_values(const Graph& g, CentralityMap cent)
{
    all_degree_centralities(g, cent, measure_influence(g));
}

template < typename Graph, typename CentralityMap >
inline void all_prestige_values(const Graph& g, CentralityMap cent)
{
    all_degree_centralities(g, cent, measure_prestige(g));
}

} /* namespace boost */

#endif


/*******************************************************************************
 *
 * Construction and management of weak topological orderings (WTOs).
 *
 * The construction of weak topological orderings is based on F. Bourdoncle's
 * paper: "Efficient chaotic iteration strategies with widenings", Formal
 * Methods in Programming and Their Applications, 1993, pages 128-141.
 *
 * Author: Arnaud J. Venet (arnaud.j.venet@nasa.gov)
 * Contributors: Jorge A. Navas (jorge.navas@sri.com)
 *
 * Notices:
 *
 * Copyright (c) 2011 United States Government as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 * All Rights Reserved.
 *
 * Disclaimers:
 *
 * No Warranty: THE SUBJECT SOFTWARE IS PROVIDED "AS IS" WITHOUT ANY WARRANTY OF
 * ANY KIND, EITHER EXPRESSED, IMPLIED, OR STATUTORY, INCLUDING, BUT NOT LIMITED
 * TO, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL CONFORM TO SPECIFICATIONS,
 * ANY IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE,
 * OR FREEDOM FROM INFRINGEMENT, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL BE
 * ERROR FREE, OR ANY WARRANTY THAT DOCUMENTATION, IF PROVIDED, WILL CONFORM TO
 * THE SUBJECT SOFTWARE. THIS AGREEMENT DOES NOT, IN ANY MANNER, CONSTITUTE AN
 * ENDORSEMENT BY GOVERNMENT AGENCY OR ANY PRIOR RECIPIENT OF ANY RESULTS,
 * RESULTING DESIGNS, HARDWARE, SOFTWARE PRODUCTS OR ANY OTHER APPLICATIONS
 * RESULTING FROM USE OF THE SUBJECT SOFTWARE.  FURTHER, GOVERNMENT AGENCY
 * DISCLAIMS ALL WARRANTIES AND LIABILITIES REGARDING THIRD-PARTY SOFTWARE,
 * IF PRESENT IN THE ORIGINAL SOFTWARE, AND DISTRIBUTES IT "AS IS."
 *
 * Waiver and Indemnity:  RECIPIENT AGREES TO WAIVE ANY AND ALL CLAIMS AGAINST
 * THE UNITED STATES GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS, AS WELL
 * AS ANY PRIOR RECIPIENT.  IF RECIPIENT'S USE OF THE SUBJECT SOFTWARE RESULTS
 * IN ANY LIABILITIES, DEMANDS, DAMAGES, EXPENSES OR LOSSES ARISING FROM SUCH
 * USE, INCLUDING ANY DAMAGES FROM PRODUCTS BASED ON, OR RESULTING FROM,
 * RECIPIENT'S USE OF THE SUBJECT SOFTWARE, RECIPIENT SHALL INDEMNIFY AND HOLD
 * HARMLESS THE UNITED STATES GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS,
 * AS WELL AS ANY PRIOR RECIPIENT, TO THE EXTENT PERMITTED BY LAW.
 * RECIPIENT'S SOLE REMEDY FOR ANY SUCH MATTER SHALL BE THE IMMEDIATE,
 * UNILATERAL TERMINATION OF THIS AGREEMENT.
 *
 ******************************************************************************/

#pragma once

#include <vector>
#include <set>
#include <unordered_map>
#include <memory>
#include <boost/iterator/indirect_iterator.hpp>
#include <boost/container/slist.hpp>

#include <crab/common/types.hpp>
#include <crab/common/stats.hpp>
#include <crab/common/debug.hpp>
#include <crab/domains/interval.hpp>

// Define RECURSIVE_WTO to use older, recursive version.  It is
// retained for a while for performance comparison purposes.
// #define RECURSIVE_WTO

namespace ikos {

  template<typename G>
  class wto;

  template<typename G>
  class wto_vertex;

  template<typename G>
  class wto_cycle;

  template<typename G>
  class wto_component_visitor;

  template<typename G>
  class wto_nesting {

    friend class wto<G>;
    friend class wto_vertex<G>;
    friend class wto_cycle<G>;

  public:
    typedef wto_nesting<G> wto_nesting_t;

  private:
    typedef std::vector<typename boost::graph_traits<G>::vertex_descriptor> node_list_t;
    typedef std::shared_ptr<node_list_t> node_list_ptr;

    node_list_ptr _nodes;

  public:
    typedef typename node_list_t::iterator iterator;
    typedef typename node_list_t::const_iterator const_iterator;

  private:
    wto_nesting(node_list_ptr l): _nodes(std::make_shared<node_list_t>(*l)) { }

    int compare(wto_nesting_t& other) const {
      const_iterator this_it = this->begin(), other_it = other.begin();
      while (this_it != this->end()) {
	if (other_it == other.end()) {
	  return 1;
	} else if (*this_it == *other_it) {
	  ++this_it;
	  ++other_it;
	} else {
	  return 2; // Nestings are not comparable
	}
      }
      if (other_it == other.end()) {
	return 0;
      } else {
	return -1;
      }
    }

  public:
    wto_nesting(): _nodes(std::make_shared<node_list_t>()) { }

    void operator+=(typename boost::graph_traits<G>::vertex_descriptor n) {
      this->_nodes = std::make_shared<node_list_t>(*(this->_nodes));
      this->_nodes->push_back(n);
    }

    wto_nesting_t operator+(typename boost::graph_traits<G>::vertex_descriptor n) {
      wto_nesting_t res(this->_nodes);
      res._nodes->push_back(n);
      return res;
    }

    iterator begin() {
      return _nodes->begin();
    }

    iterator end() {
      return _nodes->end();
    }

    const_iterator begin() const {
      return _nodes->begin();
    }

    const_iterator end() const {
      return _nodes->end();
    }

    wto_nesting_t operator^(wto_nesting_t other) const {
      wto_nesting_t res;
      for (const_iterator this_it = this->begin(), other_it = other.begin();
           this_it != this->end() && other_it != other.end();
           ++this_it, ++other_it) {
        if (*this_it == *other_it) {
          res._nodes->push_back(*this_it);
        } else {
          break;
        }
      }
      return res;
    }

    bool operator<=(wto_nesting_t other) const {
      return this->compare(other) <= 0;
    }

    bool operator==(wto_nesting_t other) const {
      return this->compare(other) == 0;
    }

    bool operator>=(wto_nesting_t other) const {
      return this->operator<=(other, *this);
    }

    bool operator>(wto_nesting_t other) const {
      return this->compare(other) == 1;
    }

    void write(crab::crab_os& o) const {
      o << "[";
      for (const_iterator it = this->begin(); it != this->end(); ) {
	typename boost::graph_traits<G>::vertex_descriptor n = *it;
	o << n;
	++it;
	if (it != this->end()) {
	  o << ", ";
	}
      }
      o << "]";
    }
  }; // class nesting

  template<typename G>
  inline crab::crab_os& operator<<(crab::crab_os& o, const wto_nesting<G>& n) {
    n.write(o);
    return o;
  }

  template<typename G>
  class wto_component {

  public:
    typedef wto_nesting<G> wto_nesting_t;

    virtual void accept(wto_component_visitor<G> *) = 0;

    virtual ~wto_component() { }

    virtual void write(crab::crab_os& os) const = 0;

  }; // class wto_component

  template<typename G>
  inline crab::crab_os& operator<<(crab::crab_os& o, const wto_component<G>& c) {
    c.write(o);
    return o;
  }

  template<typename G>
  class wto_vertex: public wto_component<G> {

    friend class wto<G>;

  private:
    typename boost::graph_traits<G>::vertex_descriptor _node;

    wto_vertex(typename boost::graph_traits<G>::vertex_descriptor node): _node(node) { }

  public:
    typename boost::graph_traits<G>::vertex_descriptor node() {
      return this->_node;
    }

    void accept(wto_component_visitor<G> *v) {
      v->visit(*this);
    }

    void write(crab::crab_os& o) const {
      o <<this->_node;
    }

  }; // class wto_vertex

  template<typename G>
  class wto_cycle: public wto_component<G> {

    friend class wto<G>;

  public:
    typedef wto_component<G> wto_component_t;

  private:
    typedef std::shared_ptr<wto_component_t> wto_component_ptr;
    typedef boost::container::slist<wto_component_ptr> wto_component_list_t;
    typedef std::shared_ptr<wto_component_list_t> wto_component_list_ptr;

    typename boost::graph_traits<G>::vertex_descriptor _head;
    wto_component_list_ptr _wto_components;
    // number of times the wto cycle is analyzed by the fixpoint iterator
    unsigned _num_fixpo;

    wto_cycle(typename boost::graph_traits<G>::vertex_descriptor head,
              wto_component_list_ptr wto_components):
      _head(head), _wto_components(wto_components), _num_fixpo(0) { }

  public:

    typedef boost::indirect_iterator<typename wto_component_list_t::iterator> iterator;
    typedef boost::indirect_iterator<typename wto_component_list_t::const_iterator> const_iterator;

    typename boost::graph_traits<G>::vertex_descriptor head() {
      return this->_head;
    }

    void accept(wto_component_visitor<G> *v) {
      v->visit(*this);
    }

    iterator begin() {
      return boost::make_indirect_iterator(_wto_components->begin());
    }

    iterator end() {
      return boost::make_indirect_iterator(_wto_components->end());
    }

    const_iterator begin() const {
      return boost::make_indirect_iterator(_wto_components->begin());
    }

    const_iterator end() const {
      return boost::make_indirect_iterator(_wto_components->end());
    }

    void increment_fixpo_visits () {
      _num_fixpo++;
    }

    unsigned get_fixpo_visits () const {
      return _num_fixpo;
    }

    void write(crab::crab_os& o) const {
      o << "(" << this->_head;
      if (!this->_wto_components->empty()) {
	o << " ";
	for (const_iterator it = this->begin(); it != this->end(); ) {
	  const wto_component_t& c = *it;
	  o << c;
	  ++it;
	  if (it != this->end()) {
	    o << " ";
	  }
	}
      }
      o << ")";
      if (this->_num_fixpo > 0)
	o << "^{" << this->_num_fixpo << "}";
    }

  }; // class wto_cycle

  template<typename G>
  class wto_component_visitor {

  public:
    typedef wto_vertex<G> wto_vertex_t;
    typedef wto_cycle<G> wto_cycle_t;

    virtual void visit(wto_vertex_t&) = 0;
    virtual void visit(wto_cycle_t&) = 0;
    virtual ~wto_component_visitor() {}

  }; // class wto_component_visitor

  template<typename G>
  class wto {

  public:
    typedef wto_nesting<G> wto_nesting_t;
    typedef wto_component<G> wto_component_t;
    typedef wto_vertex<G> wto_vertex_t;
    typedef wto_cycle<G> wto_cycle_t;
    typedef wto<G> wto_t;

  private:
    typedef std::shared_ptr<wto_component_t> wto_component_ptr;
    typedef std::shared_ptr<wto_vertex_t> wto_vertex_ptr;
    typedef std::shared_ptr<wto_cycle_t> wto_cycle_ptr;
    typedef boost::container::slist<wto_component_ptr> wto_component_list_t;
    typedef std::shared_ptr<wto_component_list_t> wto_component_list_ptr;
    typedef bound<z_number> dfn_t;
    typedef std::unordered_map<typename boost::graph_traits<G>::vertex_descriptor, dfn_t> dfn_table_t;
    typedef std::shared_ptr<dfn_table_t> dfn_table_ptr;
    typedef std::vector<typename boost::graph_traits<G>::vertex_descriptor> stack_t;
    typedef std::shared_ptr<stack_t> stack_ptr;
    typedef std::unordered_map<typename boost::graph_traits<G>::vertex_descriptor, wto_nesting_t> nesting_table_t;
    typedef std::shared_ptr<nesting_table_t> nesting_table_ptr;

    wto_component_list_ptr _wto_components;
    dfn_table_ptr _dfn_table;
    dfn_t _num;
    stack_ptr _stack;
    nesting_table_ptr _nesting_table;

    class nesting_builder: public wto_component_visitor<G> {

    public:
      typedef wto_vertex<G> wto_vertex_t;
      typedef wto_cycle<G> wto_cycle_t;

    private:
      wto_nesting_t _nesting;
      nesting_table_ptr _nesting_table;

    public:
      nesting_builder(nesting_table_ptr nesting_table):
	_nesting_table(nesting_table) { }

      void visit(wto_cycle_t& cycle) {
        typename boost::graph_traits<G>::vertex_descriptor head = cycle.head();
        wto_nesting_t previous_nesting = this->_nesting;
        this->_nesting_table->insert(std::make_pair(head, this->_nesting));
        this->_nesting += head;
        for (typename wto_cycle_t::iterator it = cycle.begin(); it != cycle.end(); ++it) {
          it->accept(this);
        }
        this->_nesting = previous_nesting;
      }

      void visit(wto_vertex_t& vertex) {
        this->_nesting_table->insert(std::make_pair(vertex.node(), this->_nesting));
      }

    }; // class nesting_builder

    dfn_t get_dfn(typename boost::graph_traits<G>::vertex_descriptor n) {
      typename dfn_table_t::iterator it = this->_dfn_table->find(n);
      if (it == this->_dfn_table->end()) {
        return 0;
      } else {
        return it->second;
      }
    }

    void set_dfn(typename boost::graph_traits<G>::vertex_descriptor n, dfn_t dfn) {
      std::pair<typename dfn_table_t::iterator, bool> res =
	this->_dfn_table->insert(std::make_pair(n, dfn));
      if (!res.second) {
        (res.first)->second = dfn;
      }
    }

    typename boost::graph_traits<G>::vertex_descriptor pop() {
      if (this->_stack->empty()) {
        CRAB_ERROR("WTO computation: empty stack");
      } else {
        typename boost::graph_traits<G>::vertex_descriptor top = this->_stack->back();
        this->_stack->pop_back();
        return top;
      }
    }

    void push(typename boost::graph_traits<G>::vertex_descriptor n) {
      this->_stack->push_back(n);
    }

    wto_cycle_ptr component(G g, typename boost::graph_traits<G>::vertex_descriptor vertex) {
      auto partition = std::make_shared<wto_component_list_t>();
      std::pair<typename boost::graph_traits<G>::out_edge_iterator,
		typename boost::graph_traits<G>::out_edge_iterator>
	succ_edges = out_edges(vertex, g);
      for (typename boost::graph_traits<G>::out_edge_iterator it = succ_edges.first,
	     et = succ_edges.second; it!=et; ++it) {
	typename boost::graph_traits<G>::vertex_descriptor succ = target(*it, g);
        if (this->get_dfn(succ) == 0) {
          this->visit(g, succ, partition);
        }
      }
      return wto_cycle_ptr(new wto_cycle_t(vertex, partition));
    }

    #ifndef RECURSIVE_WTO
    struct visit_stack_elem {
      typedef typename boost::graph_traits<G>::out_edge_iterator succ_iterator;
      typename boost::graph_traits<G>::vertex_descriptor _node;
      succ_iterator _it; // begin iterator for node's successors
      succ_iterator _et; // end iterator for node's successors
      dfn_t _min;        // smallest dfn number of any (direct or
			 // indirect) node's successor through node's
			 // DFS subtree, included node.

      visit_stack_elem(typename boost::graph_traits<G>::vertex_descriptor node,
		       std::pair<succ_iterator, succ_iterator> succs, dfn_t min)
	: _node(node)
	, _it(succs.first)
	, _et(succs.second)
	, _min(min) {}
    };

    void visit(G g, typename boost::graph_traits<G>::vertex_descriptor vertex,
	       wto_component_list_ptr partition) {

      std::vector<visit_stack_elem> visit_stack;
      std::set<typename boost::graph_traits<G>::vertex_descriptor> loop_nodes;

      /* discover vertex */
      push(vertex);
      _num += 1;
      set_dfn(vertex, _num);

      visit_stack.push_back(visit_stack_elem(vertex, out_edges(vertex, g), _num));
      CRAB_LOG("wto-nonrec",
	       crab::outs() << "WTO: Node " << vertex << ": dfs num=" << _num << "\n";);
      while (!visit_stack.empty()) {
	/*
	 * Perform dfs.
	 *
	 * When this loop terminates, visit_stack.back()_node's children
	 * have been processed.  For each loop iteration we push in
	 * visit_stack one more descendant.
	 */
	while (visit_stack.back()._it != visit_stack.back()._et) {
	  typename boost::graph_traits<G>::edge_descriptor e = *visit_stack.back()._it++;
	  typename boost::graph_traits<G>::vertex_descriptor child = target(e, g);
	  dfn_t child_dfn = get_dfn(child);
	  if (child_dfn == 0) {
	    /* discover new vertex */
	    push(child);
	    _num += 1;
	    set_dfn(child, _num);
	    visit_stack.push_back(visit_stack_elem(child, out_edges(child, g), _num));
	    CRAB_LOG("wto-nonrec",
		     crab::outs() << "WTO: Node " << child << ": dfs num=" << _num << "\n";);
	  } else {
	    if (child_dfn <= visit_stack.back()._min) {
	      visit_stack.back()._min = child_dfn;
	      CRAB_LOG("wto-nonrec",
		       crab::outs() << "WTO: loop found " << child << "\n";);
	      loop_nodes.insert(child);
	    }
	  }
	}

	// propagate min from child to parent
	typename boost::graph_traits<G>::vertex_descriptor visiting_node =
	  visit_stack.back()._node;
	dfn_t min_visiting_node = visit_stack.back()._min;
	bool is_loop = loop_nodes.count(visiting_node)> 0;
	visit_stack.pop_back();
	if (!visit_stack.empty() && visit_stack.back()._min > min_visiting_node) {
	  visit_stack.back()._min = min_visiting_node;
	}

	auto dfn_visiting_node = get_dfn(visiting_node);
	CRAB_LOG("wto-nonrec",
	    crab::outs() << "WTO: popped node " << visiting_node
	                 << " dfs num= " <<  dfn_visiting_node
	                 << ": min=" << min_visiting_node << "\n";);

	if (min_visiting_node == get_dfn(visiting_node)) {
	  CRAB_LOG("wto-nonrec",
		   crab::outs() << "WTO: BEGIN building partition for node "
	                        << visiting_node << "\n";);
	  set_dfn(visiting_node, dfn_t::plus_infinity());
	  typename boost::graph_traits<G>::vertex_descriptor element = pop();
	  if (is_loop) {
	    while (!(element == visiting_node)) {
	      set_dfn(element, 0);
	      CRAB_LOG("wto-nonrec",
		       crab::outs () << "\tWTO: node " << element << ": dfn num=0\n";);
	      element = pop();
	    }
	    CRAB_LOG("wto-nonrec",
		     crab::outs() << "\tWTO: adding component starting from "
		                 << visiting_node << "\n";);
	    partition->push_front(std::static_pointer_cast<wto_component_t,wto_cycle_t>
				  (component(g, visiting_node)));
	  } else {
	    CRAB_LOG("wto-nonrec",
		     crab::outs() << "\tWTO: adding vertex " << visiting_node << "\n";);
	    partition->push_front(std::static_pointer_cast< wto_component_t, wto_vertex_t>
				  (wto_vertex_ptr(new wto_vertex_t(visiting_node))));
	  }
	  CRAB_LOG("wto-nonrec", crab::outs() << "WTO: END building partition\n";);
	}
      } // end while (!visit_stack.empty())
    }

    #else
    dfn_t visit(G g,
		typename boost::graph_traits<G>::vertex_descriptor vertex,
		wto_component_list_ptr partition) {
      dfn_t head = 0, min = 0;
      bool loop;
      typename boost::graph_traits<G>::vertex_descriptor element;

      this->push(vertex);
      this->_num += 1;
      head = this->_num;
      this->set_dfn(vertex, head);
      loop = false;

      std::pair<typename boost::graph_traits<G>::out_edge_iterator,
		typename boost::graph_traits<G>::out_edge_iterator>
	succ_edges = out_edges(vertex, g);
      for (typename boost::graph_traits<G>::out_edge_iterator it = succ_edges.first,
	     et = succ_edges.second; it!=et; ++it) {
	typename boost::graph_traits<G>::vertex_descriptor succ = target(*it, g);
        dfn_t succ_dfn = this->get_dfn(succ);
        if (succ_dfn == 0) {
          min = this->visit(g, succ, partition);
        } else {
          min = succ_dfn;
        }
        if (min <= head) {
          head = min;
	  loop = true;
        }
      }
      if (head == this->get_dfn(vertex)) {
        this->set_dfn(vertex, dfn_t::plus_infinity());
        element = this->pop();
        if (loop) {
          while (!(element == vertex)) {
            this->set_dfn(element, 0);
            element = this->pop();
          }
          partition->push_front(std::static_pointer_cast<wto_component_t,
                                wto_cycle_t>(this->component(g, vertex)));
        } else {
          partition->push_front(std::static_pointer_cast<wto_component_t,
                                wto_vertex_t>(wto_vertex_ptr(new wto_vertex_t(vertex))));
	}
      }
      return head;
    }
    #endif

    void build_nesting() {
      nesting_builder builder(this->_nesting_table);
      for (iterator it = this->begin(); it != this->end(); ++it) {
        it->accept(&builder);
      }
    }

  public:

    typedef boost::indirect_iterator<typename wto_component_list_t::iterator> iterator;
    typedef boost::indirect_iterator<typename wto_component_list_t::const_iterator> const_iterator;

    wto(G g):
      _wto_components(std::make_shared<wto_component_list_t>()),
      _dfn_table(std::make_shared<dfn_table_t>()),
      _num(0), _stack(std::make_shared<stack_t>()),
      _nesting_table(std::make_shared<nesting_table_t>()) {
      crab::ScopedCrabStats __st__("Fixpo.WTO");

      this->visit(g, entry(g), this->_wto_components);
      this->_dfn_table.reset();
      this->_stack.reset();
      this->build_nesting();
    }

    // deep copy
    wto(const wto_t &other):
      _wto_components(std::make_shared<wto_component_list_t>(*other._wto_components)),
      _dfn_table(other._dfn_table ?
		 std::make_shared<dfn_table_t>(*other._dfn_table):
		 nullptr),
      _num(other._num),
      _stack(other._stack ?
	     std::make_shared<stack_t>(*other._stack):
	     nullptr),
      _nesting_table(std::make_shared<nesting_table_t>(*other._nesting_table)) { }

    wto(const wto_t &&other):
      _wto_components(std::move(other._wto_components)),
      _dfn_table(std::move(other._dfn_table)),
      _num(other._num),
      _stack(std::move(other._stack)),
      _nesting_table(std::move(other._nesting_table)) { }

    wto_t& operator=(const wto_t &other) {
      if (this != &other) {
	this->_wto_components = other._wto_components;
	this->_dfn_table = other._dfn_table;
	this->_num = other._num;
	this->_stack = other._stack;
	this->_nesting_table = other._nesting_table;
      }
      return *this;
    }

    iterator begin() {
      return boost::make_indirect_iterator(_wto_components->begin());
    }

    iterator end() {
      return boost::make_indirect_iterator(_wto_components->end());
    }

    const_iterator begin() const {
      return boost::make_indirect_iterator(_wto_components->begin());
    }

    const_iterator end() const {
      return boost::make_indirect_iterator(_wto_components->end());
    }

    wto_nesting_t nesting(typename boost::graph_traits<G>::vertex_descriptor n) {
      typename nesting_table_t::iterator it = this->_nesting_table->find(n);
      if (it == this->_nesting_table->end()) {
        CRAB_ERROR("WTO nesting: node ", n," not found");
      } else {
        return it->second;
      }
    }

    void accept(wto_component_visitor<G> *v) {
      for (iterator it = this->begin(); it != this->end(); ++it) {
        it->accept(v);
      }
    }

    void write(crab::crab_os& o) const {
      for (const_iterator it = this->begin(); it != this->end(); ) {
        const wto_component_t& c = *it;
        o << c;
        ++it;
        if (it != this->end()) {
          o << " ";
        }
      }
    }

    friend crab::crab_os& operator<<(crab::crab_os &o, const wto_t &wto) {
      wto.write(o);
      return o;
    }

  }; // class wto

} // namespace ikos


/*  $Id: voronoidiagram.cpp 752 2011-06-07 12:51:45Z anders.e.e.wallin $
 *
 *  Copyright 2010-2011 Anders Wallin (anders.e.e.wallin "at" gmail.com)
 *
 *  This file is part of OpenCAMlib.
 *
 *  OpenCAMlib 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.
 *
 *  OpenCAMlib 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 OpenCAMlib.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <boost/foreach.hpp>

#include "voronoidiagram.hpp"
#include "numeric.h"

namespace ocl
{

int VertexProps::count = 0;

VoronoiDiagram::VoronoiDiagram(double far, unsigned int n_bins) {
    fgrid = new FaceGrid(far, n_bins);
    far_radius=far;
    gen_count=3;
    init();
}

VoronoiDiagram::~VoronoiDiagram() {
    delete fgrid;
}

// add one vertex at origo and three vertices at 'infinity' and their associated edges
void VoronoiDiagram::init() {
    //std::cout << "VD init() \n";
    double far_multiplier = 6;
    // add vertices
    HEVertex v0;
    VertexProps v0prop(Point(0,0), UNDECIDED);
    VertexProps v1prop(Point(0, far_multiplier*far_radius), OUT);
    VertexProps v2prop(Point( cos(-5*PI/6)*far_multiplier*far_radius, sin(-5*PI/6)*far_multiplier*far_radius), OUT);
    VertexProps v3prop(Point( cos(-PI/6)*far_multiplier*far_radius, sin(-PI/6)*far_multiplier*far_radius), OUT);
    v0  = hedi::add_vertex( v0prop, g );
    v01 = hedi::add_vertex( v1prop, g );
    v02 = hedi::add_vertex( v2prop, g );
    v03 = hedi::add_vertex( v3prop, g );

    // the locations of the initial generators:
    double gen_mutliplier = 3;
    gen2 = Point(cos(PI/6)*gen_mutliplier*far_radius, sin(PI/6)*gen_mutliplier*far_radius);
    gen3 = Point(cos(5*PI/6)*gen_mutliplier*far_radius, sin(5*PI/6)*gen_mutliplier*far_radius);
    gen1 = Point( 0,-gen_mutliplier*far_radius);
    g[v0].set_J( gen1, gen2, gen3 ); // this sets J2,J3,J4 and pk, so that detH(pl) can be called later

    // add face 1: v0-v1-v2
    HEEdge e1 =  hedi::add_edge( v0 , v01 , g);
    HEEdge e2 =  hedi::add_edge( v01, v02 , g);
    HEEdge e3 =  hedi::add_edge( v02, v0  , g);
    HEFace f1 =  hedi::add_face( FaceProps(e2, gen3, NONINCIDENT), g );
    fgrid->add_face( g[f1] );
    g[e1].face = f1;
    g[e2].face = f1;
    g[e3].face = f1;
    g[e1].next = e2;
    g[e2].next = e3;
    g[e3].next = e1;

    // add face 2: v0-v2-v3
    HEEdge e4 = hedi::add_edge( v0, v02  , g );
    HEEdge e5 = hedi::add_edge( v02, v03 , g );
    HEEdge e6 = hedi::add_edge( v03, v0  , g );
    HEFace f2 =  hedi::add_face( FaceProps(e5, gen1, NONINCIDENT), g );
    fgrid->add_face( g[f2] );
    g[e4].face = f2;
    g[e5].face = f2;
    g[e6].face = f2;
    g[e4].next = e5;
    g[e5].next = e6;
    g[e6].next = e4;

    // add face 3: v0-v3-v1
    HEEdge e7 = hedi::add_edge( v0 , v03 , g);
    HEEdge e8 = hedi::add_edge( v03, v01 , g);
    HEEdge e9 = hedi::add_edge( v01, v0  , g);
    HEFace f3 =  hedi::add_face( FaceProps(e8, gen2, NONINCIDENT), g );
    fgrid->add_face( g[f3] );
    g[e7].face = f3;
    g[e8].face = f3;
    g[e9].face = f3;
    g[e7].next = e8;
    g[e8].next = e9;
    g[e9].next = e7;

    // twin edges
    g[e1].twin = e9;
    g[e9].twin = e1;
    g[e2].twin = HEEdge(); // the outermost edges have invalid twins
    g[e5].twin = HEEdge();
    g[e8].twin = HEEdge();
    g[e3].twin = e4;
    g[e4].twin = e3;
    g[e6].twin = e7;
    g[e7].twin = e6;

    assert( vdChecker.isValid(this) );
    //std::cout << " VD init() done.\n";
}

// comments relate to Sugihara-Iri paper
// this is roughly "algorithm A" from the paper, page 15/50
void VoronoiDiagram::addVertexSite(const Point& p) {
    // only add vertices within the far_radius circle
    assert( p.xyNorm() < far_radius );

    // 1) find the closest face and associated generator
    gen_count++;
    HEFace closest_face = fgrid->grid_find_closest_face( p );

    // 2) among the vertices on the closest_face
    //    find the seed, which has the lowest detH
    HEVertex v_seed = findSeedVertex(closest_face, p);
    g[v_seed].type = IN;
    VertexVector v0;
    v0.push_back(v_seed);

    // 3) augment the vertex set to be deleted
    //    vertex set must remain a tree
    //    must not delete cycles
    augment_vertex_set_M(v0, p);

    // 4) add new vertices on all edges that connect v0 IN edges to OUT edges
    add_new_voronoi_vertices(v0, p);

    // 5) generate new edges that form a loop around the region to be deleted
    HEFace newface = split_faces(p);

    // 6) fix the next-pointers in newface, then remove set v0
    remove_vertex_set(v0, newface);

    // 7) reset IN/OUT/UNDECIDED for verts, and INCIDENT/NONINCIDENT for faces
    reset_labels();

    assert( vdChecker.isValid(this) );
}

HEFace VoronoiDiagram::split_faces(const Point& p) {
    HEFace newface =  hedi::add_face( FaceProps( HEEdge(), p, NONINCIDENT ), g );
    fgrid->add_face( g[newface] );
    BOOST_FOREACH( HEFace f, incident_faces ) {
        split_face(newface, f); // each INCIDENT face is split into two parts: newface and f
    }
    return newface;
}

void VoronoiDiagram::reset_labels() {
    BOOST_FOREACH( HEVertex v, in_vertices ) {
        g[v].reset();
    }
    in_vertices.clear();
    g[v01].type = OUT; // the outer vertices are special.
    g[v02].type = OUT;
    g[v03].type = OUT;
    BOOST_FOREACH(HEFace f, incident_faces ) {
        g[f].type = NONINCIDENT;
    }
    incident_faces.clear();
}

// remove vertices in the set v0
void VoronoiDiagram::remove_vertex_set(VertexVector& v0 , HEFace newface) {
    HEEdge current_edge = g[newface].edge;
    HEEdge start_edge = current_edge;
    // this repairs the next-pointers for newface that are broken.
    bool done = false;
    while (!done) {
        HEVertex current_target = hedi::target( current_edge , g); // an edge on the new face
        HEVertex current_source = hedi::source( current_edge , g);
        BOOST_FOREACH( HEEdge edge, hedi::out_edges( current_target, g ) ) { // loop through potential "next" candidates
            HEVertex out_target = hedi::target( edge , g);
            if ( g[out_target].type == NEW ) { // the next vertex along the face should be "NEW"
                if ( out_target != current_source ) { // but not where we came from
                    g[current_edge].next = edge; // this is the edge we want to take

                    // current and next should belong on the same face

                    if (g[current_edge].face !=  g[ g[current_edge].next ].face) {
                        std::cout << " VD remove_vertex_set() ERROR.\n";
                        std::cout << "current.face = " << g[current_edge].face << " IS NOT next_face = " << g[ g[current_edge].next ].face << std::endl;
                        HEVertex c_trg = hedi::target( current_edge , g);
                        HEVertex c_src = hedi::source( current_edge , g);
                        HEVertex n_trg = hedi::target( g[current_edge].next , g);
                        HEVertex n_src = hedi::source( g[current_edge].next , g);

                        std::cout << "current_edge = " << g[c_src].index << " - " << g[c_trg].index << "\n";
                        std::cout << "next_edge = " << g[n_src].index << " - " << g[n_trg].index << "\n";

                        printFaceVertexTypes( g[current_edge].face );
                        printFaceVertexTypes( g[ g[current_edge].next ].face );

                        std::cout << " printing all incident faces for debug: \n";
                        BOOST_FOREACH( HEFace f, incident_faces ) {
                            printFaceVertexTypes( f );
                        }
                    }
                    assert( g[current_edge].face ==  g[ g[current_edge].next ].face );
                }
            }
        }

        current_edge = g[current_edge].next; // jump to the next edge
        if ( g[current_edge].next == start_edge )
            done = true;
    }
    // it should now be safe to delete v0
    BOOST_FOREACH( HEVertex v, v0 ) {
        assert( g[v].type == IN );
        hedi::delete_vertex(v,g); // this also removes edges connecting to v
    }
}

// split the face f into one part which is newface, and the other part is the old f
void VoronoiDiagram::split_face(HEFace newface, HEFace f) {
    HEVertex new_source; // this is found as OUT-NEW-IN
    HEVertex new_target; // this is found as IN-NEW-OUT
    // the new vertex on face f connects new_source -> new_target
    HEEdge current_edge = g[f].edge;
    assert( f == g[current_edge].face );
    HEEdge start_edge = current_edge;
    VoronoiVertexType currentType = OUT;
    VoronoiVertexType nextType  = NEW;
    HEEdge new_previous;
    HEEdge new_next;
    HEEdge twin_next;
    HEEdge twin_previous;
    bool found = false;
    while (!found) {
        HEVertex current_vertex = hedi::target( current_edge , g);
        HEEdge next_edge = g[current_edge].next;
        HEVertex next_vertex = hedi::target( next_edge , g);
        if ( g[current_vertex].type == currentType ) {
            if ( g[next_vertex].type == nextType ) {
                new_source = next_vertex;
                new_previous = next_edge;
                twin_next = g[next_edge].next;
                found = true;
            }
        }
        current_edge = g[current_edge].next;
    }
    found = false;
    currentType = IN;
    nextType = NEW;
    current_edge = g[f].edge;
    while (!found) {
        HEVertex current_vertex = hedi::target( current_edge , g);
        HEEdge next_edge = g[current_edge].next;
        HEVertex next_vertex = hedi::target( next_edge , g);
        if ( g[current_vertex].type == currentType ) {
            if ( g[next_vertex].type == nextType ) {
                new_target = next_vertex;
                new_next = g[next_edge].next;
                twin_previous = next_edge;
                found = true;
            }
        }
        current_edge = g[current_edge].next;
    }
    // now connect new_previous -> new_source -> new_target -> new_next
    HEEdge e_new = hedi::add_edge( new_source, new_target , g); // face,next,twin
    g[new_previous].next = e_new;
    g[e_new].next = new_next;
    g[e_new].face = f;
    g[f].edge = e_new;

    // the twin edge that bounds the new face
    HEEdge e_twin = hedi::add_edge( new_target, new_source , g);
    g[twin_previous].next = e_twin;
    g[e_twin].next = twin_next;
    g[e_twin].face = newface;
    g[newface].edge = e_twin;

    g[e_twin].twin = e_new;
    g[e_new].twin = e_twin;

    //assert( isDegreeThree() );
}

EdgeVector VoronoiDiagram::find_edges(VertexVector& inVertices, VoronoiVertexType vtype) {
    assert( !inVertices.empty() );
    EdgeVector output; // new vertices generated on these edges
    BOOST_FOREACH( HEVertex v, inVertices ) {
        assert( g[v].type == IN ); // all verts in v0 are IN
        BOOST_FOREACH( HEEdge edge, hedi::out_edges( v , g) ) {
            HEVertex adj_vertex = hedi::target( edge , g);
            if ( g[adj_vertex].type == vtype )
                output.push_back(edge); // this is an IN-vtype edge
        }
    }
    return output;
}

// the set v0 are IN vertices that should be removed
// generate new voronoi-vertices on all edges connecting v0 to OUT-vertices
void VoronoiDiagram::add_new_voronoi_vertices(VertexVector& v0, const Point& p) {
    assert( !v0.empty() );
    EdgeVector q_edges = find_edges(v0, OUT); // new vertices generated on these IN-OUT edges
    assert( !q_edges.empty() );

    for( unsigned int m=0; m<q_edges.size(); ++m )  {  // create new vertices on all edges q_edges[]
        HEVertex q = hedi::add_vertex(g);
        g[q].type = NEW;
        in_vertices.push_back(q);
        HEFace face = g[q_edges[m]].face;     assert(  g[face].type == INCIDENT);
        HEEdge twin = g[q_edges[m]].twin;
        HEFace twin_face = g[twin].face;      assert( g[twin_face].type == INCIDENT);
        g[q].set_J( g[face].generator  , g[twin_face].generator  , p);
        g[q].set_position();

        // check new vertex position, should lie between endpoints of q_edges[m]
        HEVertex trg = hedi::target(q_edges[m], g);
        HEVertex src = hedi::source(q_edges[m], g);
        Point trgP = g[trg].position;
        Point srcP = g[src].position;
        Point newP = g[q].position;
        if (( trgP - srcP ).xyNorm() <= 0 ) {
            /*
            std::cout << "add_new_voronoi_vertices() WARNING ( trgP - srcP ).xyNorm()= " << ( trgP - srcP ).xyNorm() << "\n";
            std::cout << " src = " << srcP << "\n";
            std::cout << " trg= " << trgP << "\n";
            */
            //std::cout << "add_new_voronoi_vertices() WARNING zero-length edge! \n";
            g[q].position = srcP;
        } else {
            assert( ( trgP - srcP ).xyNorm() > 0.0 ); // edge has finite length
            assert( ( trgP - srcP ).dot( trgP - srcP ) > 0.0 ); // length squared
            double t = ((newP - srcP).dot( trgP - srcP )) / ( trgP - srcP ).dot( trgP - srcP ) ;
            bool warn = false;
            //double t_orig=t;
            if (t < 0.0) {
                warn = true;
                t=0.0;
            } else if (t> 1.0) {
                warn = true;
                t=1.0;
            }
            if ( warn ) {
                //std::cout << "add_new_voronoi_vertices() WARNING positioning vertex outside edge! t_orig= " << t_orig << "\n";
                // CORRECT the position....
                g[q].position = srcP + t*( trgP-srcP);
                t = ( g[q].position - srcP).dot( trgP - srcP ) / ( trgP - srcP ).dot( trgP - srcP ) ;
                //std::cout << "add_new_voronoi_vertices() CORRECTED t= " << t << "\n";
            }
            assert( t >= 0.0 );
            assert( t <= 1.0 );

            double dtl = g[q].position.xyDistanceToLine(srcP, trgP);
            if (dtl > 1e-3* ( trgP - srcP ).xyNorm() ) {
                //std::cout << "add_new_voronoi_vertices() WARNING new point far from edge!\n";
                //std::cout << "add_new_voronoi_vertices() WARNING edge length= " << ( trgP - srcP ).xyNorm()  << "\n";
                //std::cout << "add_new_voronoi_vertices() WARNING distance to edge= " << dtl  << "\n";
                t = ( g[q].position - srcP).dot( trgP - srcP ) / ( trgP - srcP ).dot( trgP - srcP ) ;
                g[q].position = srcP + t*( trgP-srcP);
                //newP = hed[q].position;
                dtl = g[q].position.xyDistanceToLine(srcP, trgP);
                //std::cout << "add_new_voronoi_vertices() WARNING corrected distance to edge= " << dtl  << "\n";
            }
            assert( dtl < 1e-3* ( trgP - srcP ).xyNorm() );
        }

        hedi::insert_vertex_in_edge( q, q_edges[m] , g);
        // sanity check on new vertex
        assert( g[q].position.xyNorm() < 6.1*far_radius); // see init() for placement of the three initial vertices
    }
}

int VoronoiDiagram::outVertexCount(HEFace f) {
    int outCount = 0;
    VertexVector face_verts = hedi::face_vertices(f, g);
    BOOST_FOREACH( HEVertex v, face_verts ) {
        if (g[v].type == OUT )
            ++outCount;
    }
    return outCount;
}

// check that the vertices TYPE are connected
bool VoronoiDiagram::faceVerticesConnected( HEFace f, VoronoiVertexType Vtype ) {
    VertexVector face_verts = hedi::face_vertices(f,g);
    VertexVector type_verts;
    BOOST_FOREACH( HEVertex v, face_verts ) {
        if ( g[v].type == Vtype )
            type_verts.push_back(v);
    }
    assert( !type_verts.empty() );
    if (type_verts.size()==1) // set of 1 is allways connected
        return true;

    // check that type_verts are connected
    HEEdge currentEdge = g[f].edge;
    HEVertex endVertex = hedi::source(currentEdge, g); // stop when target here
    EdgeVector startEdges;
    bool done = false;
    while (!done) {
        HEVertex src = hedi::source( currentEdge, g );
        HEVertex trg = hedi::target( currentEdge, g );
        if ( g[src].type != Vtype ) { // seach ?? - Vtype
            if ( g[trg].type == Vtype ) {
                // we have found ?? - Vtype
                startEdges.push_back( currentEdge );
            }
        }
        currentEdge = g[currentEdge].next;
        if ( trg == endVertex ) {
            done = true;
        }
    }
    assert( !startEdges.empty() );
    if ( startEdges.size() != 1 ) // when the Vtype vertices are connected, there is exactly one startEdge
        return false;
    else
        return true;
}

void VoronoiDiagram::printFaceVertexTypes(HEFace f) {
    std::cout << " Face " << f << ": ";
    VertexVector face_verts = hedi::face_vertices(f,g);
    unsigned count=1;
    BOOST_FOREACH( HEVertex v, face_verts ) {
        std::cout << g[v].index  << "(" << g[v].type  << ")";
        if (count != face_verts.size() )
            std::cout << "-";
        count++;
    }
    std::cout << "\n";
}

void VoronoiDiagram::printVertices(VertexVector& q) {
    BOOST_FOREACH( HEVertex v, q) {
        std::cout << g[v].index << " ";
    }
    std::cout << std::endl;
}

void VoronoiDiagram::pushAdjacentVertices(  HEVertex v , std::queue<HEVertex>& Q) {
    VertexVector adj_verts = hedi::adjacent_vertices(v,g);
    BOOST_FOREACH( HEVertex w, adj_verts ) {
        if ( g[w].type == UNDECIDED ) {
            if ( !g[w].in_queue ) {
                Q.push(w); // push adjacent undecided verts for testing.
                g[w].in_queue=true;
            }
        }
    }
}

int VoronoiDiagram::adjacentInCount(HEVertex v) {
    VertexVector adj_v = hedi::adjacent_vertices(v,g);
    int in_count=0;
    BOOST_FOREACH( HEVertex w, adj_v) {
        if ( g[w].type == IN )
            in_count++;
    }
    return in_count;
}
FaceVector VoronoiDiagram::adjacentIncidentFaces(HEVertex v) {
    FaceVector adj_faces = hedi::adjacent_faces(v,g);
    assert( adj_faces.size() == 3 );
    FaceVector inc_faces;
    BOOST_FOREACH( HEFace f, adj_faces ) {
        if ( g[f].type == INCIDENT )
            inc_faces.push_back( f );
    }
    assert( !inc_faces.empty() );
    return inc_faces;
}

bool VoronoiDiagram::incidentFacesHaveAdjacentInVertex(HEVertex v) {
    bool all_found = true;
    BOOST_FOREACH( HEFace f, adjacentIncidentFaces(v) ) { // check each face f
        // v should be adjacent to an IN vertex on the face
        // VertexVector face_verts = hed.face_vertices(f);
        bool face_found=false;
        BOOST_FOREACH( HEVertex w, hedi::face_vertices(f,g) ) {
            if ( w != v && g[w].type == IN && hedi::has_edge(w,v,g) )
                face_found = true;
        }
        if (!face_found)
            all_found=false;
    }
    return all_found;
}

// from the "one million" paper, growing the tree by breadth-first search
void VoronoiDiagram::augment_vertex_set_M(VertexVector& v0, const Point& p) {
    assert(v0.size()==1);
    std::queue<HEVertex> Q;
    // a priority_queue could/should be used here instead.
    // this woiuld allways examine and decide on the vertex with largest detH,
    // since that vertex has the most reliable detH sign.
    in_vertices.push_back( v0[0] );
    markAdjecentFacesIncident( v0[0] );
    assert( Q.empty() );
    pushAdjacentVertices( v0[0] , Q);
    while( !Q.empty() ) {
        HEVertex v = Q.front();
        assert( g[v].type == UNDECIDED );
        // add to v0 if detH<0 and passes tests. otherwise OUT
        double h = g[v].detH( p );
        if ( h < 0.0 ) { // try to mark IN
            // (C4) v should not be adjacent to two or more IN vertices
            // (C5) for an incident face containing v: v is adjacent to an IN vertex on this face
            if ( (adjacentInCount(v) >= 2) || (!incidentFacesHaveAdjacentInVertex(v)) ) {
                assert( g[v].type == UNDECIDED );
                g[v].type = OUT;
                //std::cout << " v " << hed[v].index << " decision is " << hed[v].type << " IN_COUNT>=2 \n";
                in_vertices.push_back( v );
            } else {
                g[v].type = IN;
                //std::cout << " v " << hed[v].index << " decision is " << hed[v].type << " (h<0)\n";
                in_vertices.push_back( v );
                v0.push_back( v );
                markAdjecentFacesIncident( v );
                pushAdjacentVertices( v , Q);
            }
        } else { // mark OUT
            //std::cout << " v " << hed[v].index << " decision is " << hed[v].type << " (h>0)\n";
            assert( h >= 0.0 );
            assert( g[v].type == UNDECIDED );
            g[v].type = OUT;
            in_vertices.push_back( v );
        }
        Q.pop(); // delete from queue
    }

    BOOST_FOREACH( HEFace f, incident_faces ) {
        if ( !faceVerticesConnected( f, IN ) ) {
            std::cout << " augment_vertex_set_M() ERROR, IN-vertices not connected.\n";
            std::cout << " printing all incident faces for debug: \n";
            BOOST_FOREACH( HEFace f, incident_faces ) {
                printFaceVertexTypes( f );
            }
        }
        assert( faceVerticesConnected( f, IN ) );
    }
    //std::cout << " augment_M done:\n";
    //printVertices(v0);
}

// used by augment_M
void VoronoiDiagram::markAdjecentFacesIncident( HEVertex v) {
    assert( g[v].type == IN );
    FaceVector new_adjacent_faces = hedi::adjacent_faces( v, g );
    assert( new_adjacent_faces.size()==3 );
    BOOST_FOREACH( HEFace adj_face, new_adjacent_faces ) {
        if ( g[adj_face].type  != INCIDENT ) {
            g[adj_face].type = INCIDENT;
            incident_faces.push_back(adj_face);
        }
    }
}

// evaluate H on all face vertices and return
// vertex with the lowest H
HEVertex VoronoiDiagram::findSeedVertex(HEFace f, const Point& p) {
    VertexVector face_verts = hedi::face_vertices(f,g);
    assert( face_verts.size() >= 3 );
    double minimumH; // safe, because we expect the min H to be negative...
    HEVertex minimalVertex;
    double h;
    bool first = true;
    BOOST_FOREACH( HEVertex q, face_verts) {
        if ( g[q].type != OUT ) {
            h = g[q].detH( p );
            //std::cout << "  detH = " << h << " ! \n";
            if ( first || (h<minimumH) ) {
                minimumH = h;
                minimalVertex = q;
                first = false;
            }
        }
    }

    if (!(minimumH < 0) ) {
        std::cout << " VD find_seed_vertex() WARNING\n";
        std::cout << " WARNING: searching for seed when inserting " << p  << "  \n";
        std::cout << " WARNING: closest face is  " << f << " with generator " << g[f].generator  << " \n";
        std::cout << " WARNING: minimal vd-vertex " << g[minimalVertex].index << " has deth= " << g[minimalVertex].detH( p ) << "\n";
    }
    //assert( minimumH < 0 );
    return minimalVertex;
}

std::string VoronoiDiagram::str() const {
    std::ostringstream o;
    o << "VoronoiDiagram (nVerts="<< hedi::num_vertices(g) << " , nEdges="<< hedi::num_edges(g) <<"\n";
    return o.str();
}

} // end namespace
// end file voronoidiagram.cpp


#include "Vertex.h"

#include <algorithm>

#include <boost/spirit/include/qi.hpp>

#include "Matrix.h"
#include "Vector.h"

Vertex::Vertex()
{
	vertex_[0] = std::numeric_limits<double>::quiet_NaN();
	vertex_[1] = std::numeric_limits<double>::quiet_NaN();
	vertex_[2] = std::numeric_limits<double>::quiet_NaN();
}

Vertex::Vertex(double x, double y, double z)
{
	vertex_[0] = x;
	vertex_[1] = y;
	vertex_[2] = z;
}

Vertex::Vertex(std::string_view str)
{
	parsestr(str);
}

const Vertex Vertex::unitX = { 1, 0, 0 };
const Vertex Vertex::unitY = { 0, 1, 0 };
const Vertex Vertex::unitZ = { 0, 0, 1 };

void Vertex::x(double x)
{
	vertex_[0] = x;
}

void Vertex::y(double y)
{
	vertex_[1] = y;
}

void Vertex::z(double z)
{
	vertex_[2] = z;
}

double Vertex::x() const
{
	return vertex_[0];
}

double Vertex::y() const
{
	return vertex_[1];
}

double Vertex::z() const
{
	return vertex_[2];
}

Vertex Vertex::rotate(const Matrix3d& rotmat) const
{
	auto th = this->toMat();
	auto res = rotmat * th;
	return Vertex(res.get(0, 0), res.get(1, 0), res.get(2, 0));
}

Vertex Vertex::rotate(const Vertex& point, const Matrix3d& rotmat) const
{
	Vector vec = Vector::diff(point, *this);
	vec.rotate(rotmat);
	return Vertex(vec.end());
}

void Vertex::rotateInPlace(const Vertex& point, const Matrix3d& rotmat)
{
	Vector vec = Vector::diff(point, *this);
	vec.rotate(rotmat);
	*this = vec.end();
}

void Vertex::rotateInPlace(const Matrix3d& rotmat)
{
	auto vecmat = toMat();
	auto res = rotmat * vecmat;
	x(res.get(0, 0));
	y(res.get(1, 0));
	z(res.get(2, 0));
}

double Vertex::length() const
{
	return sqrt(
		  x() * x()
		+ y() * y()
		+ z() * z());
}

double Vertex::length(const Vertex& v)
{
	return v.length();
}

double Vertex::dotProduct(const Vertex& lhs, const Vertex& rhs)
{
	return lhs.x() * rhs.x() + lhs.y() * rhs.y() + lhs.z() * rhs.z();
}

double Vertex::dotProduct2D(const Vertex& lhs, const Vertex& rhs)
{
	return lhs.x() * rhs.x() + lhs.y() * rhs.y();
}

Vertex Vertex::absolute(const Vertex& v)
{
	return { fabs(v.x()), fabs(v.y()), fabs(v.z()) };
}

Vertex Vertex::normalize(const Vertex& v)
{
	double len = v.length();
	if (doubleeq(len, 0.0)) return v;
	return {
		v.x() / len,
		v.y() / len,
		v.z() / len
	};
}

Vertex Vertex::normalize() const
{
	double len = length();
	if (doubleeq(len, 0.0)) return Vertex(*this);
	return Vertex(
		vertex_[0] / len,
		vertex_[1] / len,
		vertex_[2] / len);
}

Vertex Vertex::crossProduct(const Vertex& lhs, const Vertex& rhs)
{
	return Vertex(
		lhs.vertex_[1] * rhs.vertex_[2] - lhs.vertex_[2] * rhs.vertex_[1],
		lhs.vertex_[2] * rhs.vertex_[0] - lhs.vertex_[0] * rhs.vertex_[2],
		lhs.vertex_[0] * rhs.vertex_[1] - lhs.vertex_[1] * rhs.vertex_[0]);
}

size_t Vertex::countDifferentAxes(const Vertex& lhs, const Vertex& rhs)
{
	size_t ret = 0;

	if (!doubleeq(lhs.x(), rhs.x()))
		++ret;

	if (!doubleeq(lhs.y(), rhs.y()))
		++ret;

	if (!doubleeq(lhs.z(), rhs.z()))
		++ret;

	return ret;
}

void Vertex::parsestr(std::string_view str)
{
	namespace qi = boost::spirit::qi;
	namespace ascii = boost::spirit::ascii;

	//size_t fspos = str.find_first_of(' '), espos = str.find_last_of(' ');
	//if (fspos == std::string::npos || espos == std::string::npos || fspos == espos)
	//{
	//	vertex_[0] = std::numeric_limits<double>::quiet_NaN();
	//	vertex_[1] = std::numeric_limits<double>::quiet_NaN();
	//	vertex_[2] = std::numeric_limits<double>::quiet_NaN();
	//	return;
	//}
	//size_t pos = 0;
	//auto xstr = nextword(pos, str);
	//auto ystr = nextword(pos, str);
	//auto zstr = nextword(pos, str);

	if (!qi::phrase_parse(str.cbegin(), str.cend(), qi::double_ >> qi::double_ >> qi::double_, ascii::space | qi::lit('(') | qi::lit(')'),  vertex_[0], vertex_[1], vertex_[2]))
	{
		vertex_[0] = std::numeric_limits<double>::quiet_NaN();
		vertex_[1] = std::numeric_limits<double>::quiet_NaN();
		vertex_[2] = std::numeric_limits<double>::quiet_NaN();
	}
}

Vertex& Vertex::operator=(const Vertex& rhs)
{
	this->vertex_[0] = rhs.vertex_[0];
	this->vertex_[1] = rhs.vertex_[1];
	this->vertex_[2] = rhs.vertex_[2];
	return *this;
}

Vertex& Vertex::operator+=(const Vertex& rhs)
{
	this->vertex_[0] += rhs.vertex_[0];
	this->vertex_[1] += rhs.vertex_[1];
	this->vertex_[2] += rhs.vertex_[2];
	return *this;
}

Vertex& Vertex::operator-=(const Vertex& rhs)
{
	this->vertex_[0] -= rhs.vertex_[0];
	this->vertex_[1] -= rhs.vertex_[1];
	this->vertex_[2] -= rhs.vertex_[2];
	return *this;
}

Vertex& Vertex::operator*=(double mod)
{
	vertex_[0] *= mod;
	vertex_[1] *= mod;
	vertex_[2] *= mod;
	return *this;
}

Vertex & Vertex::operator/=(double mod)
{
	vertex_[0] /= mod;
	vertex_[1] /= mod;
	vertex_[2] /= mod;
	return *this;
}

bool Vertex::operator==(const Vertex& rhs) const
{
	return doubleeq(x(), rhs.x()) && doubleeq(y(), rhs.y()) && doubleeq(z(), rhs.z());
}

bool Vertex::operator!=(const Vertex& rhs) const
{
	return !(*this == rhs);
}

bool Vertex::operator<(const Vertex& rhs) const
{
	if (!doubleeq(x(), rhs.x()) && x() < rhs.x())
		return true;

	if (!doubleeq(y(), rhs.y()) && y() < rhs.y())
		return true;

	if (!doubleeq(z(), rhs.z()) && z() < rhs.z())
		return true;

	return false;
}

bool Vertex::parallel(const Vertex& lhs, const Vertex& rhs)
{
	//crossproduct of parallel vectors is (0, 0, 0)
	Vertex pvert = crossProduct(normalize(lhs), normalize(rhs));
	return doubleeq(pvert.x(), 0.0)
		&& doubleeq(pvert.y(), 0.0)
		&& doubleeq(pvert.z(), 0.0);
}

bool Vertex::equals(const Vertex& lhs, const Vertex& rhs)
{
	return doubleeq(lhs.x(), rhs.x())
		&& doubleeq(lhs.y(), rhs.y())
		&& doubleeq(lhs.z(), rhs.z());
}

double Vertex::dotProduct(const Vertex& rhs) const
{
	return
		x() * rhs.x() +
		y() * rhs.y() +
		z() * rhs.z();
}

Vertex Vertex::closestAxis() const
{
	Vertex abs = absolute(*this);

	if (abs.x() >= abs.y() && abs.x() >= abs.z()) return unitX;
	if (abs.y() >= abs.z()) return unitY;
	return unitZ;
}

Vertex Vertex::crossProduct(const Vertex& rhs) const
{
	return Vertex(
		vertex_[1] * rhs.vertex_[2] - vertex_[2] * rhs.vertex_[1],
		vertex_[2] * rhs.vertex_[0] - vertex_[0] * rhs.vertex_[2],
		vertex_[0] * rhs.vertex_[1] - vertex_[1] * rhs.vertex_[0]);
}

double Vertex::operator[](size_t pos) const
{
	return vertex_[pos];
}

double& Vertex::operator[](size_t pos)
{
	return vertex_[pos];
}

Matrix<double, 3, 1> Vertex::toMat() const
{
	Matrix<double, 3, 1> ret;
	ret.set(0, 0, x());
	ret.set(1, 0, y());
	ret.set(2, 0, z());
	return ret;
}

std::string Vertex::toStr() const
{
	//static std::ostringstream os;
	//os.str("");
	//os << x() << " " << y() << " " << z();
	//return os.str();

	return std::to_string(x()) + ' ' + std::to_string(y()) + ' ' + std::to_string(z());
}

Vertex Vertex::allmin(const Vertex& v1, const Vertex& v2)
{
	Vertex v;
	v.x(std::min(v1.x(), v2.x()));
	v.y(std::min(v1.y(), v2.y()));
	v.z(std::min(v1.z(), v2.z()));
	return v;
}

Vertex Vertex::allmax(const Vertex& v1, const Vertex& v2)
{
	Vertex v;
	v.x(std::max(v1.x(), v2.x()));
	v.y(std::max(v1.y(), v2.y()));
	v.z(std::max(v1.z(), v2.z()));
	return v;
}

bool Vertex::isVertex(const Vertex& v)
{
	return (v.x() == v.x() && v.y() == v.y() && v.z() == v.z());
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2012 John Maddock. 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_MP_COMPARE_HPP
#define BOOST_MP_COMPARE_HPP

#include <boost/multiprecision/traits/is_backend.hpp>

//
// Comparison operators for number.
//

namespace geofeatures_boost {} namespace boost = geofeatures_boost; namespace geofeatures_boost{ namespace multiprecision{

namespace default_ops{

//
// The dispatching mechanism used here to deal with differently typed arguments
// could be better replaced with enable_if overloads, but that breaks MSVC-12
// under strange and hard to reproduce circumstances.
//
template <class B>
inline bool eval_eq(const B& a, const B& b)
{
   return a.compare(b) == 0;
}
template <class T, class U>
inline bool eval_eq_imp(const T& a, const U& b, const mpl::true_&)
{
   typename geofeatures_boost::multiprecision::detail::number_from_backend<T, U>::type t(b);
   return eval_eq(a, t.backend());
}
template <class T, class U>
inline bool eval_eq_imp(const T& a, const U& b, const mpl::false_&)
{
   typename geofeatures_boost::multiprecision::detail::number_from_backend<U, T>::type t(a);
   return eval_eq(t.backend(), b);
}
template <class T, class U>
inline bool eval_eq(const T& a, const U& b)
{
   typedef mpl::bool_<geofeatures_boost::multiprecision::detail::is_first_backend<T, U>::value> tag_type;
   return eval_eq_imp(a, b, tag_type());
}

template <class B>
inline bool eval_lt(const B& a, const B& b)
{
   return a.compare(b) < 0;
}
template <class T, class U>
inline bool eval_lt_imp(const T& a, const U& b, const mpl::true_&)
{
   typename geofeatures_boost::multiprecision::detail::number_from_backend<T, U>::type t(b);
   return eval_lt(a, t.backend());
}
template <class T, class U>
inline bool eval_lt_imp(const T& a, const U& b, const mpl::false_&)
{
   typename geofeatures_boost::multiprecision::detail::number_from_backend<U, T>::type t(a);
   return eval_lt(t.backend(), b);
}
template <class T, class U>
inline bool eval_lt(const T& a, const U& b)
{
   typedef mpl::bool_<geofeatures_boost::multiprecision::detail::is_first_backend<T, U>::value> tag_type;
   return eval_lt_imp(a, b, tag_type());
}

template <class B>
inline bool eval_gt(const B& a, const B& b)
{
   return a.compare(b) > 0;
}
template <class T, class U>
inline bool eval_gt_imp(const T& a, const U& b, const mpl::true_&)
{
   typename geofeatures_boost::multiprecision::detail::number_from_backend<T, U>::type t(b);
   return eval_gt(a, t.backend());
}
template <class T, class U>
inline bool eval_gt_imp(const T& a, const U& b, const mpl::false_&)
{
   typename geofeatures_boost::multiprecision::detail::number_from_backend<U, T>::type t(a);
   return eval_gt(t.backend(), b);
}
template <class T, class U>
inline bool eval_gt(const T& a, const U& b)
{
   typedef mpl::bool_<geofeatures_boost::multiprecision::detail::is_first_backend<T, U>::value> tag_type;
   return eval_gt_imp(a, b, tag_type());
}

} // namespace default_ops

namespace detail{

template <class Num, class Val>
struct is_valid_mixed_compare : public mpl::false_ {};

template <class B, expression_template_option ET, class Val>
struct is_valid_mixed_compare<number<B, ET>, Val> : public is_convertible<Val, number<B, ET> > {};

template <class B, expression_template_option ET>
struct is_valid_mixed_compare<number<B, ET>, number<B, ET> > : public mpl::false_ {};

template <class B, expression_template_option ET, class tag, class Arg1, class Arg2, class Arg3, class Arg4>
struct is_valid_mixed_compare<number<B, ET>, expression<tag, Arg1, Arg2, Arg3, Arg4> >
   : public mpl::bool_<is_convertible<expression<tag, Arg1, Arg2, Arg3, Arg4>, number<B, ET> >::value> {};

template <class tag, class Arg1, class Arg2, class Arg3, class Arg4, class B, expression_template_option ET>
struct is_valid_mixed_compare<expression<tag, Arg1, Arg2, Arg3, Arg4>, number<B, ET> >
   : public mpl::bool_<is_convertible<expression<tag, Arg1, Arg2, Arg3, Arg4>, number<B, ET> >::value> {};

template <class Backend, expression_template_option ExpressionTemplates>
inline BOOST_CONSTEXPR typename geofeatures_boost::enable_if_c<number_category<Backend>::value != number_kind_floating_point, bool>::type is_unordered_value(const number<Backend, ExpressionTemplates>&)
{
   return false;
}
template <class Backend, expression_template_option ExpressionTemplates>
inline BOOST_CONSTEXPR typename geofeatures_boost::enable_if_c<number_category<Backend>::value == number_kind_floating_point, bool>::type is_unordered_value(const number<Backend, ExpressionTemplates>& a)
{
   using default_ops::eval_fpclassify;
   return eval_fpclassify(a.backend()) == FP_NAN;
}

template <class Arithmetic>
inline BOOST_CONSTEXPR typename geofeatures_boost::enable_if_c<number_category<Arithmetic>::value != number_kind_floating_point, bool>::type is_unordered_value(const Arithmetic&)
{
   return false;
}
template <class Arithmetic>
inline BOOST_CONSTEXPR typename geofeatures_boost::enable_if_c<number_category<Arithmetic>::value == number_kind_floating_point, bool>::type is_unordered_value(const Arithmetic& a)
{
   return (geofeatures_boost::math::isnan)(a);
}

template <class T, class U>
inline BOOST_CONSTEXPR bool is_unordered_comparison(const T& a, const U& b)
{
   return is_unordered_value(a) || is_unordered_value(b);
}

}

template <class Backend, expression_template_option ExpressionTemplates, class Backend2, expression_template_option ExpressionTemplates2>
inline bool operator == (const number<Backend, ExpressionTemplates>& a, const number<Backend2, ExpressionTemplates2>& b)
{
   using default_ops::eval_eq;
   if(detail::is_unordered_comparison(a, b)) return false;
   return eval_eq(a.backend(), b.backend());
}
template <class Backend, expression_template_option ExpressionTemplates, class Arithmetic>
inline typename enable_if_c<detail::is_valid_mixed_compare<number<Backend, ExpressionTemplates>, Arithmetic>::value, bool>::type
   operator == (const number<Backend, ExpressionTemplates>& a, const Arithmetic& b)
{
   using default_ops::eval_eq;
   if(detail::is_unordered_comparison(a, b)) return false;
   return eval_eq(a.backend(), number<Backend, ExpressionTemplates>::canonical_value(b));
}
template <class Arithmetic, class Backend, expression_template_option ExpressionTemplates>
inline typename enable_if_c<detail::is_valid_mixed_compare<number<Backend, ExpressionTemplates>, Arithmetic>::value, bool>::type
   operator == (const Arithmetic& a, const number<Backend, ExpressionTemplates>& b)
{
   using default_ops::eval_eq;
   if(detail::is_unordered_comparison(a, b)) return false;
   return eval_eq(b.backend(), number<Backend, ExpressionTemplates>::canonical_value(a));
}
template <class Arithmetic, class Tag, class A1, class A2, class A3, class A4>
inline typename enable_if_c<detail::is_valid_mixed_compare<typename detail::expression<Tag, A1, A2, A3, A4>::result_type, Arithmetic>::value, bool>::type
   operator == (const Arithmetic& a, const detail::expression<Tag, A1, A2, A3, A4>& b)
{
   typedef typename detail::expression<Tag, A1, A2, A3, A4>::result_type result_type;
   using default_ops::eval_eq;
   result_type t(b);
   if(detail::is_unordered_comparison(a, t)) return false;
   return eval_eq(t.backend(), result_type::canonical_value(a));
}
template <class Tag, class A1, class A2, class A3, class A4, class Arithmetic>
inline typename enable_if_c<detail::is_valid_mixed_compare<typename detail::expression<Tag, A1, A2, A3, A4>::result_type, Arithmetic>::value, bool>::type
   operator == (const detail::expression<Tag, A1, A2, A3, A4>& a, const Arithmetic& b)
{
   typedef typename detail::expression<Tag, A1, A2, A3, A4>::result_type result_type;
   using default_ops::eval_eq;
   result_type t(a);
   if(detail::is_unordered_comparison(t, b)) return false;
   return eval_eq(t.backend(), result_type::canonical_value(b));
}
template <class Tag, class A1, class A2, class A3, class A4, class Tagb, class A1b, class A2b, class A3b, class A4b>
inline typename enable_if<is_same<typename detail::expression<Tag, A1, A2, A3, A4>::result_type, typename detail::expression<Tagb, A1b, A2b, A3b, A4b>::result_type>, bool>::type
   operator == (const detail::expression<Tag, A1, A2, A3, A4>& a, const detail::expression<Tagb, A1b, A2b, A3b, A4b>& b)
{
   using default_ops::eval_eq;
   typename detail::expression<Tag, A1, A2, A3, A4>::result_type t(a);
   typename detail::expression<Tagb, A1b, A2b, A3b, A4b>::result_type t2(b);
   if(detail::is_unordered_comparison(t, t2)) return false;
   return eval_eq(t.backend(), t2.backend());
}

template <class Backend, expression_template_option ExpressionTemplates, class Backend2, expression_template_option ExpressionTemplates2>
inline bool operator != (const number<Backend, ExpressionTemplates>& a, const number<Backend2, ExpressionTemplates2>& b)
{
   using default_ops::eval_eq;
   if(detail::is_unordered_comparison(a, b)) return true;
   return !eval_eq(a.backend(), b.backend());
}
template <class Backend, expression_template_option ExpressionTemplates, class Arithmetic>
inline typename enable_if_c<detail::is_valid_mixed_compare<number<Backend, ExpressionTemplates>, Arithmetic>::value, bool>::type
   operator != (const number<Backend, ExpressionTemplates>& a, const Arithmetic& b)
{
   using default_ops::eval_eq;
   if(detail::is_unordered_comparison(a, b)) return true;
   return !eval_eq(a.backend(), number<Backend, et_on>::canonical_value(b));
}
template <class Arithmetic, class Backend, expression_template_option ExpressionTemplates>
inline typename enable_if_c<detail::is_valid_mixed_compare<number<Backend, ExpressionTemplates>, Arithmetic>::value, bool>::type
   operator != (const Arithmetic& a, const number<Backend, ExpressionTemplates>& b)
{
   using default_ops::eval_eq;
   if(detail::is_unordered_comparison(a, b)) return true;
   return !eval_eq(b.backend(), number<Backend, et_on>::canonical_value(a));
}
template <class Arithmetic, class Tag, class A1, class A2, class A3, class A4>
inline typename enable_if_c<detail::is_valid_mixed_compare<typename detail::expression<Tag, A1, A2, A3, A4>::result_type, Arithmetic>::value, bool>::type
   operator != (const Arithmetic& a, const detail::expression<Tag, A1, A2, A3, A4>& b)
{
   typedef typename detail::expression<Tag, A1, A2, A3, A4>::result_type result_type;
   using default_ops::eval_eq;
   result_type t(b);
   if(detail::is_unordered_comparison(a, t)) return true;
   return !eval_eq(t.backend(), result_type::canonical_value(a));
}
template <class Tag, class A1, class A2, class A3, class A4, class Arithmetic>
inline typename enable_if_c<detail::is_valid_mixed_compare<typename detail::expression<Tag, A1, A2, A3, A4>::result_type, Arithmetic>::value, bool>::type
   operator != (const detail::expression<Tag, A1, A2, A3, A4>& a, const Arithmetic& b)
{
   typedef typename detail::expression<Tag, A1, A2, A3, A4>::result_type result_type;
   using default_ops::eval_eq;
   result_type t(a);
   if(detail::is_unordered_comparison(t, b)) return true;
   return !eval_eq(t.backend(), result_type::canonical_value(b));
}
template <class Tag, class A1, class A2, class A3, class A4, class Tagb, class A1b, class A2b, class A3b, class A4b>
inline typename enable_if<is_same<typename detail::expression<Tag, A1, A2, A3, A4>::result_type, typename detail::expression<Tagb, A1b, A2b, A3b, A4b>::result_type>, bool>::type
   operator != (const detail::expression<Tag, A1, A2, A3, A4>& a, const detail::expression<Tagb, A1b, A2b, A3b, A4b>& b)
{
   using default_ops::eval_eq;
   typename detail::expression<Tag, A1, A2, A3, A4>::result_type t(a);
   typename detail::expression<Tagb, A1b, A2b, A3b, A4b>::result_type t2(b);
   if(detail::is_unordered_comparison(t, t2)) return true;
   return !eval_eq(t.backend(), t2.backend());
}

template <class Backend, expression_template_option ExpressionTemplates, class Backend2, expression_template_option ExpressionTemplates2>
inline bool operator < (const number<Backend, ExpressionTemplates>& a, const number<Backend2, ExpressionTemplates2>& b)
{
   using default_ops::eval_lt;
   if(detail::is_unordered_comparison(a, b)) return false;
   return eval_lt(a.backend(), b.backend());
}
template <class Backend, expression_template_option ExpressionTemplates, class Arithmetic>
inline typename enable_if_c<detail::is_valid_mixed_compare<number<Backend, ExpressionTemplates>, Arithmetic>::value, bool>::type
   operator < (const number<Backend, ExpressionTemplates>& a, const Arithmetic& b)
{
   using default_ops::eval_lt;
   if(detail::is_unordered_comparison(a, b)) return false;
   return eval_lt(a.backend(), number<Backend, ExpressionTemplates>::canonical_value(b));
}
template <class Arithmetic, class Backend, expression_template_option ExpressionTemplates>
inline typename enable_if_c<detail::is_valid_mixed_compare<number<Backend, ExpressionTemplates>, Arithmetic>::value, bool>::type
   operator < (const Arithmetic& a, const number<Backend, ExpressionTemplates>& b)
{
   using default_ops::eval_gt;
   if(detail::is_unordered_comparison(a, b)) return false;
   return eval_gt(b.backend(), number<Backend, ExpressionTemplates>::canonical_value(a));
}
template <class Arithmetic, class Tag, class A1, class A2, class A3, class A4>
inline typename enable_if_c<detail::is_valid_mixed_compare<typename detail::expression<Tag, A1, A2, A3, A4>::result_type, Arithmetic>::value, bool>::type
   operator < (const Arithmetic& a, const detail::expression<Tag, A1, A2, A3, A4>& b)
{
   typedef typename detail::expression<Tag, A1, A2, A3, A4>::result_type result_type;
   using default_ops::eval_gt;
   result_type t(b);
   if(detail::is_unordered_comparison(a, t)) return false;
   return eval_gt(t.backend(), result_type::canonical_value(a));
}
template <class Tag, class A1, class A2, class A3, class A4, class Arithmetic>
inline typename enable_if_c<detail::is_valid_mixed_compare<typename detail::expression<Tag, A1, A2, A3, A4>::result_type, Arithmetic>::value, bool>::type
   operator < (const detail::expression<Tag, A1, A2, A3, A4>& a, const Arithmetic& b)
{
   typedef typename detail::expression<Tag, A1, A2, A3, A4>::result_type result_type;
   using default_ops::eval_lt;
   result_type t(a);
   if(detail::is_unordered_comparison(t, b)) return false;
   return eval_lt(t.backend(), result_type::canonical_value(b));
}
template <class Tag, class A1, class A2, class A3, class A4, class Tagb, class A1b, class A2b, class A3b, class A4b>
inline typename enable_if<is_same<typename detail::expression<Tag, A1, A2, A3, A4>::result_type, typename detail::expression<Tagb, A1b, A2b, A3b, A4b>::result_type>, bool>::type
   operator < (const detail::expression<Tag, A1, A2, A3, A4>& a, const detail::expression<Tagb, A1b, A2b, A3b, A4b>& b)
{
   using default_ops::eval_lt;
   typename detail::expression<Tag, A1, A2, A3, A4>::result_type t(a);
   typename detail::expression<Tagb, A1b, A2b, A3b, A4b>::result_type t2(b);
   if(detail::is_unordered_comparison(t, t2)) return false;
   return eval_lt(t.backend(), t2.backend());
}

template <class Backend, expression_template_option ExpressionTemplates, class Backend2, expression_template_option ExpressionTemplates2>
inline bool operator > (const number<Backend, ExpressionTemplates>& a, const number<Backend2, ExpressionTemplates2>& b)
{
   using default_ops::eval_gt;
   if(detail::is_unordered_comparison(a, b)) return false;
   return eval_gt(a.backend(), b.backend());
}
template <class Backend, expression_template_option ExpressionTemplates, class Arithmetic>
inline typename enable_if_c<detail::is_valid_mixed_compare<number<Backend, ExpressionTemplates>, Arithmetic>::value, bool>::type
   operator > (const number<Backend, ExpressionTemplates>& a, const Arithmetic& b)
{
   using default_ops::eval_gt;
   if(detail::is_unordered_comparison(a, b)) return false;
   return eval_gt(a.backend(), number<Backend, ExpressionTemplates>::canonical_value(b));
}
template <class Arithmetic, class Backend, expression_template_option ExpressionTemplates>
inline typename enable_if_c<detail::is_valid_mixed_compare<number<Backend, ExpressionTemplates>, Arithmetic>::value, bool>::type
   operator > (const Arithmetic& a, const number<Backend, ExpressionTemplates>& b)
{
   using default_ops::eval_lt;
   if(detail::is_unordered_comparison(a, b)) return false;
   return eval_lt(b.backend(), number<Backend, ExpressionTemplates>::canonical_value(a));
}
template <class Arithmetic, class Tag, class A1, class A2, class A3, class A4>
inline typename enable_if_c<detail::is_valid_mixed_compare<typename detail::expression<Tag, A1, A2, A3, A4>::result_type, Arithmetic>::value, bool>::type
   operator > (const Arithmetic& a, const detail::expression<Tag, A1, A2, A3, A4>& b)
{
   typedef typename detail::expression<Tag, A1, A2, A3, A4>::result_type result_type;
   using default_ops::eval_lt;
   result_type t(b);
   if(detail::is_unordered_comparison(a, t)) return false;
   return a > t;
}
template <class Tag, class A1, class A2, class A3, class A4, class Arithmetic>
inline typename enable_if_c<detail::is_valid_mixed_compare<typename detail::expression<Tag, A1, A2, A3, A4>::result_type, Arithmetic>::value, bool>::type
   operator > (const detail::expression<Tag, A1, A2, A3, A4>& a, const Arithmetic& b)
{
   typedef typename detail::expression<Tag, A1, A2, A3, A4>::result_type result_type;
   using default_ops::eval_gt;
   result_type t(a);
   if(detail::is_unordered_comparison(t, b)) return false;
   return t > b;
}
template <class Tag, class A1, class A2, class A3, class A4, class Tagb, class A1b, class A2b, class A3b, class A4b>
inline typename enable_if<is_same<typename detail::expression<Tag, A1, A2, A3, A4>::result_type, typename detail::expression<Tagb, A1b, A2b, A3b, A4b>::result_type>, bool>::type
   operator > (const detail::expression<Tag, A1, A2, A3, A4>& a, const detail::expression<Tagb, A1b, A2b, A3b, A4b>& b)
{
   using default_ops::eval_gt;
   typename detail::expression<Tag, A1, A2, A3, A4>::result_type t(a);
   typename detail::expression<Tagb, A1b, A2b, A3b, A4b>::result_type t2(b);
   if(detail::is_unordered_comparison(t, t2)) return false;
   return t > t2;
}

template <class Backend, expression_template_option ExpressionTemplates, class Backend2, expression_template_option ExpressionTemplates2>
inline bool operator <= (const number<Backend, ExpressionTemplates>& a, const number<Backend2, ExpressionTemplates2>& b)
{
   using default_ops::eval_gt;
   if(detail::is_unordered_comparison(a, b)) return false;
   return !eval_gt(a.backend(), b.backend());
}
template <class Backend, expression_template_option ExpressionTemplates, class Arithmetic>
inline typename enable_if_c<detail::is_valid_mixed_compare<number<Backend, ExpressionTemplates>, Arithmetic>::value, bool>::type
   operator <= (const number<Backend, ExpressionTemplates>& a, const Arithmetic& b)
{
   using default_ops::eval_gt;
   if(detail::is_unordered_comparison(a, b)) return false;
   return !eval_gt(a.backend(), number<Backend, ExpressionTemplates>::canonical_value(b));
}
template <class Arithmetic, class Backend, expression_template_option ExpressionTemplates>
inline typename enable_if_c<detail::is_valid_mixed_compare<number<Backend, ExpressionTemplates>, Arithmetic>::value, bool>::type
   operator <= (const Arithmetic& a, const number<Backend, ExpressionTemplates>& b)
{
   using default_ops::eval_lt;
   if(detail::is_unordered_comparison(a, b)) return false;
   return !eval_lt(b.backend(), number<Backend, ExpressionTemplates>::canonical_value(a));
}
template <class Arithmetic, class Tag, class A1, class A2, class A3, class A4>
inline typename enable_if_c<detail::is_valid_mixed_compare<typename detail::expression<Tag, A1, A2, A3, A4>::result_type, Arithmetic>::value, bool>::type
   operator <= (const Arithmetic& a, const detail::expression<Tag, A1, A2, A3, A4>& b)
{
   typedef typename detail::expression<Tag, A1, A2, A3, A4>::result_type result_type;
   using default_ops::eval_lt;
   if(detail::is_unordered_value(a) || detail::is_unordered_value(b))
      return false;
   result_type t(b);
   if(detail::is_unordered_comparison(a, t)) return false;
   return !eval_lt(t.backend(), result_type::canonical_value(a));
}
template <class Tag, class A1, class A2, class A3, class A4, class Arithmetic>
inline typename enable_if_c<detail::is_valid_mixed_compare<typename detail::expression<Tag, A1, A2, A3, A4>::result_type, Arithmetic>::value, bool>::type
   operator <= (const detail::expression<Tag, A1, A2, A3, A4>& a, const Arithmetic& b)
{
   typedef typename detail::expression<Tag, A1, A2, A3, A4>::result_type result_type;
   using default_ops::eval_gt;
   result_type t(a);
   if(detail::is_unordered_comparison(t, b)) return false;
   return !eval_gt(t.backend(), result_type::canonical_value(b));
}
template <class Tag, class A1, class A2, class A3, class A4, class Tagb, class A1b, class A2b, class A3b, class A4b>
inline typename enable_if<is_same<typename detail::expression<Tag, A1, A2, A3, A4>::result_type, typename detail::expression<Tagb, A1b, A2b, A3b, A4b>::result_type>, bool>::type
   operator <= (const detail::expression<Tag, A1, A2, A3, A4>& a, const detail::expression<Tagb, A1b, A2b, A3b, A4b>& b)
{
   using default_ops::eval_gt;
   typename detail::expression<Tag, A1, A2, A3, A4>::result_type t(a);
   typename detail::expression<Tagb, A1b, A2b, A3b, A4b>::result_type t2(b);
   if(detail::is_unordered_comparison(t, t2)) return false;
   return !eval_gt(t.backend(), t2.backend());
}

template <class Backend, expression_template_option ExpressionTemplates, class Backend2, expression_template_option ExpressionTemplates2>
inline bool operator >= (const number<Backend, ExpressionTemplates>& a, const number<Backend2, ExpressionTemplates2>& b)
{
   using default_ops::eval_lt;
   if(detail::is_unordered_comparison(a, b)) return false;
   return !eval_lt(a.backend(), b.backend());
}
template <class Backend, expression_template_option ExpressionTemplates, class Arithmetic>
inline typename enable_if_c<detail::is_valid_mixed_compare<number<Backend, ExpressionTemplates>, Arithmetic>::value, bool>::type
   operator >= (const number<Backend, ExpressionTemplates>& a, const Arithmetic& b)
{
   using default_ops::eval_lt;
   if(detail::is_unordered_comparison(a, b)) return false;
   return !eval_lt(a.backend(), number<Backend, ExpressionTemplates>::canonical_value(b));
}
template <class Arithmetic, class Backend, expression_template_option ExpressionTemplates>
inline typename enable_if_c<detail::is_valid_mixed_compare<number<Backend, ExpressionTemplates>, Arithmetic>::value, bool>::type
   operator >= (const Arithmetic& a, const number<Backend, ExpressionTemplates>& b)
{
   using default_ops::eval_gt;
   if(detail::is_unordered_comparison(a, b)) return false;
   return !eval_gt(b.backend(), number<Backend, ExpressionTemplates>::canonical_value(a));
}
template <class Arithmetic, class Tag, class A1, class A2, class A3, class A4>
inline typename enable_if_c<detail::is_valid_mixed_compare<typename detail::expression<Tag, A1, A2, A3, A4>::result_type, Arithmetic>::value, bool>::type
   operator >= (const Arithmetic& a, const detail::expression<Tag, A1, A2, A3, A4>& b)
{
   typedef typename detail::expression<Tag, A1, A2, A3, A4>::result_type result_type;
   using default_ops::eval_gt;
   result_type t(b);
   if(detail::is_unordered_comparison(a, t)) return false;
   return !eval_gt(t.backend(), result_type::canonical_value(a));
}
template <class Tag, class A1, class A2, class A3, class A4, class Arithmetic>
inline typename enable_if_c<detail::is_valid_mixed_compare<typename detail::expression<Tag, A1, A2, A3, A4>::result_type, Arithmetic>::value, bool>::type
   operator >= (const detail::expression<Tag, A1, A2, A3, A4>& a, const Arithmetic& b)
{
   typedef typename detail::expression<Tag, A1, A2, A3, A4>::result_type result_type;
   using default_ops::eval_lt;
   result_type t(a);
   if(detail::is_unordered_comparison(t, b)) return false;
   return !eval_lt(t.backend(), result_type::canonical_value(b));
}
template <class Tag, class A1, class A2, class A3, class A4, class Tagb, class A1b, class A2b, class A3b, class A4b>
inline typename enable_if<is_same<typename detail::expression<Tag, A1, A2, A3, A4>::result_type, typename detail::expression<Tagb, A1b, A2b, A3b, A4b>::result_type>, bool>::type
   operator >= (const detail::expression<Tag, A1, A2, A3, A4>& a, const detail::expression<Tagb, A1b, A2b, A3b, A4b>& b)
{
   using default_ops::eval_lt;
   typename detail::expression<Tag, A1, A2, A3, A4>::result_type t(a);
   typename detail::expression<Tagb, A1b, A2b, A3b, A4b>::result_type t2(b);
   if(detail::is_unordered_comparison(t, t2)) return false;
   return !eval_lt(t.backend(), t2.backend());
}

}} // namespaces

#endif // BOOST_MP_COMPARE_HPP