#ifndef DXF_ALGORITHMS_HPP
#define DXF_ALGORITHMS_HPP

#include "types.hpp"

/// SYSTEM
#include <boost/version.hpp>
#include <boost/geometry/algorithms/intersection.hpp>
#include <boost/geometry/algorithms/transform.hpp>
#include <boost/geometry/algorithms/correct.hpp>
#include <boost/geometry/algorithms/append.hpp>
#include <boost/geometry/algorithms/overlaps.hpp>
#include <boost/geometry/algorithms/within.hpp>
#include <boost/geometry/strategies/strategies.hpp>
#include <boost/geometry/algorithms/length.hpp>
#include <boost/geometry/arithmetic/dot_product.hpp>

#include <utility>

namespace cslibs_boost_geometry {
namespace algorithms {
template<typename PointT>
inline bool intersection
(const typename types::Line<PointT>::type       &line_a,
 const typename types::Line<PointT>::type       &line_b,
 typename types::PointSet<PointT>::type   &points)
{
    boost::geometry::intersection(line_a, line_b, points);
    return points.size() > 0;
}

template<typename PointT>
inline bool intersection
(const typename types::Line<PointT>::type       &line_a,
 const typename types::Polygon<PointT>::type    &polygon,
 typename types::PointSet<PointT>::type   &points)
{
    auto &ring = polygon.outer();
    auto it_first = ring.begin();
    auto it_second = it_first + 1;
    while(it_second != ring.end()) {
        typename types::Line<PointT>::type line_b(*it_first, *it_second);
        typename types::PointSet<PointT>::type tmp;
        if(intersection<PointT>(line_a, line_b, tmp)) {
            if(tmp.size() == 2) {
                points = std::move(tmp);
                return true;
            } else {
                points.push_back(tmp.front());
            }
        }
        ++it_first;
        ++it_second;
    }
    return points.size() > 0;
}

template<typename T,
         typename PointT>
inline T distance(const PointT &point,
                  const typename types::Line<PointT>::type &line)

{
    return boost::geometry::distance(point, line);
}

template<typename T,
         typename PointT>
inline T minEndPointDistance(const typename types::Line<PointT>::type &line_a,
                             const typename types::Line<PointT>::type &line_b)
{

    T min = std::numeric_limits<T>::max();
    const T d0 = boost::geometry::distance(line_a.first,  line_b.first);
    if(d0 < min)
        min = d0;
    const T d1 = boost::geometry::distance(line_a.second, line_b.first);
    if(d1 < min)
        min = d1;
    const T d2 = boost::geometry::distance(line_a.first,  line_b.second);
    if(d2 < min)
        min = d2;
    const T d3 = boost::geometry::distance(line_a.second, line_b.second);
    if(d3 < min)
        min = d3;
    return min;
}

template<typename T,
         typename PointT>
inline T length(const typename types::Line<PointT>::type &line)
{
    return std::hypot(line.first.x() - line.second.x(), line.first.y() - line.second.y());
}

template<typename PointT>
inline bool intersects
(const typename types::Line<PointT>::type &line_a,
 const typename types::Line<PointT>::type &line_b)

{
    return boost::geometry::intersects(line_a, line_b);
}

template<typename PointT>
inline bool intersects
(const typename types::Line<PointT>::type    &line_a,
 const typename types::Polygon<PointT>::type &polygon)
{
#ifdef _OPENMP
    auto &ring = polygon.outer();

    if(ring.size() < 2)
        return false;

    auto ring_ptr    = ring.data();
    typename types::Line<PointT>::type line_b(*(ring_ptr + ring.size() - 1), *ring_ptr);
    bool intersection = intersects<PointT>(line_a, line_b);

#pragma omp parallel for reduction(||:intersection)
    for(unsigned int i = 0 ; i < ring.size() - 1 ; ++i) {
        line_b.first  = *(ring_ptr + i);
        line_b.second = *(ring_ptr + i + 1);
        intersection |= intersects<PointT>(line_a, line_b);
    }
    return intersection;
#else
    auto &ring = polygon.outer();
    auto it_first = ring.begin();
    auto it_second = it_first + 1;

    while(it_second != ring.end()) {
        typename types::Line<PointT>::type line_b(*it_first, *it_second);
        if(intersects<PointT>(line_a, line_b))
            return true;
        ++it_first;
        ++it_second;
    }
    return false;
#endif
}

template<typename PointT, template <typename> class Set>
inline bool nearestIntersection
(const typename types::Line<PointT>::type    &line_a,
 const typename Set<PointT>::type            &lines_b,
 typename types::PointSet<PointT>::type      &points)
{
    if(lines_b.size() == 0)
        return false;

    const PointT &origin = line_a.first;
    double min = std::numeric_limits<double>::max();
    double dx(0.0), dy(0.0), dsq(0.0);
    typename types::PointSet<PointT>::type tmp_points;

    for(auto it =
        lines_b.begin() ;
        it != lines_b.end() ;
        ++it) {
        tmp_points.clear();
        if(intersection<PointT>(line_a, Set<PointT>::getSegment(it), tmp_points)) {
            if(tmp_points.size() == 2) {
                points = std::move(tmp_points);
                return true;
            }
            if(tmp_points.size() == 1) {
                const PointT &intersection = tmp_points.back();
                dx  = origin.x() - intersection.x();
                dy  = origin.y() - intersection.y();
                dsq = dx * dx + dy * dy;
                if(dsq < min) {
                    min = dsq;
                    std::swap(tmp_points, points);
                }
            }
        }
    }

    return points.size() > 0;
}

template<typename PointT>
inline bool nearestIntersection
(const typename types::Line<PointT>::type    &line_a,
 const typename types::LineSet<PointT>::type &lines_b,
 typename types::PointSet<PointT>::type      &points)
{
    return nearestIntersection<PointT, types::LineSet> (line_a, lines_b, points);
}

template<typename PointT>
inline bool nearestIntersection
(const typename types::Line<PointT>::type           &line_a,
 const typename types::IndexedLineSet<PointT>::type &lines_b,
 typename types::PointSet<PointT>::type       &points)
{
    return nearestIntersection<PointT, types::IndexedLineSet> (line_a, lines_b, points);
}

template<typename PointT, template <typename> class Set>
inline bool nearestIntersection
(const typename types::Line<PointT>::type    &line_a,
 const typename Set<PointT>::type            &lines_b,
 typename types::PointSet<PointT>::type      &points,
 typename types::Line<PointT>::type          &line_b)
{
    if(lines_b.size() == 0)
        return false;

    const PointT &origin = line_a.first;
    double min = std::numeric_limits<double>::max();
    double dx(0.0), dy(0.0), dsq(0.0);
    typename types::PointSet<PointT>::type tmp_points;

    for(auto it =
        lines_b.begin() ;
        it != lines_b.end() ;
        ++it) {
        tmp_points.clear();
        const typename types::Line<PointT>::type &line = Set<PointT>::getSegment(it);
        if(intersection<PointT>(line_a, line, tmp_points)) {
            if(tmp_points.size() == 2) {
                points = std::move(tmp_points);
                return true;
            }
            if(tmp_points.size() == 1) {
                const PointT &intersection = tmp_points.back();
                dx  = origin.x() - intersection.x();
                dy  = origin.y() - intersection.y();
                dsq = dx * dx + dy * dy;
                if(dsq < min) {
                    min = dsq;
                    line_b = line;
                    std::swap(tmp_points, points);
                }
            }
        }
    }

    return points.size() > 0;
}

template<typename PointT>
inline bool nearestIntersection
(const typename types::Line<PointT>::type    &line_a,
 const typename types::LineSet<PointT>::type &lines_b,
 typename types::PointSet<PointT>::type      &points,
 typename types::Line<PointT>::type          &line_b)
{
    return nearestIntersection<PointT, types::LineSet> (line_a, lines_b, points, line_b);
}

template<typename PointT>
inline bool nearestIntersection
(const typename types::Line<PointT>::type    &line_a,
 const typename types::IndexedLineSet<PointT>::type &lines_b,
 typename types::PointSet<PointT>::type      &points,
 typename types::Line<PointT>::type          &line_b)
{
    return nearestIntersection<PointT, types::IndexedLineSet> (line_a, lines_b, points, line_b);
}

template<typename PointT,
         typename T,
         template <typename> class Set>
inline T nearestIntersectionDistance
(const typename types::Line<PointT>::type    &line_a,
 const typename Set<PointT>::type            &lines_b,
 const T default_value)
{
    typename types::PointSet<PointT>::type  points;
    nearestIntersection<PointT, Set> (line_a, lines_b, points);

    if(points.size() == 0)
        return default_value;

    return boost::geometry::distance(line_a.first, points.front());
}

template<typename T,
         typename PointT>
inline T nearestIntersectionDistance
(const typename types::Line<PointT>::type    &line_a,
 const typename types::LineSet<PointT>::type &lines_b,
 const T default_value)
{
    return nearestIntersectionDistance<PointT, T, types::LineSet>(line_a, lines_b, default_value);
}

template<typename T,
         typename PointT>
inline T nearestIntersectionDistance
(const typename types::Line<PointT>::type           &line_a,
 const typename types::IndexedLineSet<PointT>::type &lines_b,
 const T default_value)
{
    return nearestIntersectionDistance<PointT, T, types::IndexedLineSet>(line_a, lines_b, default_value);
}

template<typename PointT,
         typename T,
         template <typename> class Set>
inline void nearestIntersectionDistance
(const typename types::Line<PointT>::type    &line_a,
 const typename Set<PointT>::type            &lines_b,
 T &distance,
 T &angle,
 const T default_distance,
 const T default_angle)
{
    typename types::PointSet<PointT>::type  points;
    typename types::Line<PointT>::type      line_b;
    nearestIntersection<PointT, Set> (line_a, lines_b, points, line_b);

    if(points.size() == 0) {
        distance = default_distance;
        angle = default_angle;
        return;
    }

    distance = boost::geometry::distance(line_a.first, points.front());

    PointT diff_a(line_a.first.x() - line_a.second.x(),
                  line_a.first.y() - line_a.second.y());
    PointT diff_b(line_b.first.x() - line_b.second.x(),
                  line_b.first.y() - line_b.second.y());

    angle = std::acos((boost::geometry::dot_product(diff_a, diff_b)) /
                      (boost::geometry::length(line_a)*
                       boost::geometry::length(line_b)));
}

template<typename T,
         typename PointT>
inline void nearestIntersectionDistance
(const typename types::Line<PointT>::type    &line_a,
 const typename types::LineSet<PointT>::type &lines_b,
 T &distance,
 T &angle,
 const T default_distance,
 const T default_angle)
{
    nearestIntersectionDistance<PointT, T, types::LineSet>(line_a, lines_b,
                                                       distance,
                                                       angle,
                                                       default_distance,
                                                       default_angle);
}

template<typename T,
         typename PointT>
inline void nearestIntersectionDistance
(const typename types::Line<PointT>::type           &line_a,
 const typename types::IndexedLineSet<PointT>::type &lines_b,
 T &distance,
 T &angle,
 const T default_distance,
 const T default_angle)
{
    nearestIntersectionDistance<PointT, T, types::IndexedLineSet>(line_a,
                                                              lines_b,
                                                              distance,
                                                              angle,
                                                              default_distance,
                                                              default_angle);
}

template<typename T,
         typename PointT>
inline void nearestIntersectionDistanceBatch(const typename types::LineSet<PointT>::type  &lines_a,
                                             const typename types::LineSet<PointT>::type  &lines_b,
                                             const T default_value,
                                             std::vector<T> &results)
{
    results.resize(lines_a.size());
    auto lines_a_ptr = lines_a.data();
    auto results_ptr = results.data();

#pragma omp parallel for
    for(unsigned int i = 0 ; i < lines_a.size() ; ++i) {
        auto &line = *(lines_a_ptr + i);
        auto &result = *(results_ptr + i);
        result = nearestIntersectionDistance<PointT, T, types::LineSet>(line, lines_b, default_value);
    }
}

template<typename PointT>
inline void nearestIntersectionBatch
(const typename types::LineSet<PointT>::type            &lines_a,
 const typename types::LineSet<PointT>::type            &lines_b,
 typename types::IntersectionResultSet<PointT>::type &results)
{
    unsigned int lines_a_size = lines_a.size();
    results.resize(lines_a_size);

    auto lines_a_ptr = lines_a.data();
    auto results_ptr = results.data();

#pragma omp parallel for
    for(unsigned int i = 0 ; i < lines_a_size ; ++i) {
        auto &result = *(results_ptr + i);
        auto &line = *(lines_a_ptr + i);
        result.valid = nearestIntersection<PointT>(line,
                                                   lines_b,
                                                   result.result);
    }
}

template<typename PointT>
inline bool translate
(const PointT                                    &src_point,
 const typename types::Translation<PointT>::type &translation,
 PointT                                    &dst_point)
{
    return boost::geometry::transform(src_point, dst_point, translation);
}

template<typename PointT>
inline bool translate
(const typename types::Line<PointT>::type        &src_line,
 const typename types::Translation<PointT>::type &translation,
 typename types::Line<PointT>::type        &dst_line)
{
    bool success = true;
    success &= translate<PointT>(src_line.first, translation, dst_line.first);
    success &= translate<PointT>(src_line.second, translation, dst_line.second);
    return success;
}

namespace impl{
/**
 * @brief Do translation for more than one geometry.
 * @param src           the source container
 * @param translation   the translation
 * @param dst           the destination container
 * @return              if all translations were susccessful
 */
template<typename PointT, typename ContainerT, typename TranslationT>
inline bool foreachTranslation(const ContainerT   &src_container,
                               const TranslationT &translation,
                               ContainerT &dst_container)
{
    dst_container.resize(src_container.size());
    auto src_ptr = src_container.data();
    auto dst_ptr = dst_container.data();
    bool success = true;

#pragma omp parallel for reduction(&&:success)
    for(unsigned int i = 0 ; i < src_container.size() ; ++i) {
        auto &src_geom = *(src_ptr + i);
        auto &dst_geom = *(dst_ptr + i);

        success &= translate<PointT>
                (src_geom, translation, dst_geom);
    }
    return success;
}
}

template<typename PointT>
inline bool translate
(const typename types::PointSet<PointT>::type    &src_points,
 const typename types::Translation<PointT>::type &translation,
 typename types::PointSet<PointT>::type    &dst_points)
{
    return impl::foreachTranslation<PointT,
            typename types::PointSet<PointT>::type,
            typename types::Translation<PointT>::type>
            (src_points, translation, dst_points);
}

template<typename PointT>
inline bool translate
(const typename types::LineSet<PointT>::type     &src_lines,
 const typename types::Translation<PointT>::type &translation,
 typename types::LineSet<PointT>::type     &dst_lines)
{
    return impl::foreachTranslation<PointT,
            typename types::LineSet<PointT>::type,
            typename types::Translation<PointT>::type>
            (src_lines, translation, dst_lines);
}

template<typename PointT>
inline bool rotate
(const PointT                                    &src_point,
 const typename types::Rotation<PointT>::type    &rotation,
 PointT                                          &dst_point)
{
    return boost::geometry::transform(src_point, dst_point, rotation);
}

template<typename PointT>
inline bool rotate(const typename types::Line<PointT>::type        &src_line,
                   const typename types::Rotation<PointT>::type    &rotation,
                   typename types::Line<PointT>::type              &dst_line)
{
    return boost::geometry::transform(src_line, dst_line, rotation);
}

template<typename T, typename PointT>
T dot(const typename types::Line<PointT>::type &line_a,
           const typename types::Line<PointT>::type &line_b)
{
    PointT diff_a(line_a.first.x() - line_a.second.x(),
                  line_a.first.y() - line_a.second.y());
    PointT diff_b(line_b.first.x() - line_b.second.x(),
                  line_b.first.y() - line_b.second.y());
    return boost::geometry::dot_product(diff_a, diff_b);
}

template<typename T>
inline bool equal
(const T value_1,
 const T value_2,
 const T epsilon)
{
    return std::abs(value_1 - value_2) < epsilon;
}

template<typename PointT>
inline bool withinExcl
(const PointT &p,
 const typename types::Polygon<PointT>::type &polygon)
{
    return boost::geometry::within(p, polygon);
}

template<typename PointT>
inline bool withinExcl
(const typename types::Line<PointT>::type &line,
 const typename types::Box<PointT>::type &box)
{
    return boost::geometry::within(line.first, box) &&
           boost::geometry::within(line.second, box);
}

template<typename PointT>
inline bool lessEqual
(const PointT &p1,
 const PointT &p2)
{
    return p1.x() <= p2.x() && p1.y() <= p2.y();
}

template<typename PointT>
inline bool greaterEqual
(const PointT &p1,
 const PointT &p2)
{
    return p1.x() >= p2.x() && p1.y() >= p2.y();
}

template<typename PointT, typename T>
inline bool equal
(const PointT &p1,
 const PointT &p2,
 const T eps)
{
    return equal<T>(p1.x(), p2.x(), eps)  && equal(p1.y(), p2.y(), eps);
}

template<typename PointT, typename T>
inline bool equal
(const typename types::Line<PointT>::type &line_a,
 const typename types::Line<PointT>::type &line_b,
 const T eps)
{
    return (equal<PointT, T>(line_a.first, line_b.first, eps)  && equal<PointT, T>(line_a.second, line_b.second, eps)) ||
           (equal<PointT, T>(line_a.first, line_b.second, eps) && equal<PointT, T>(line_a.second, line_b.first, eps));
}

template<typename T, typename PointT>
T angle(const typename types::Line<PointT>::type &line_a,
        const typename types::Line<PointT>::type &line_b)
{
    PointT diff_a(line_a.first.x() - line_a.second.x(),
                  line_a.first.y() - line_a.second.y());
    PointT diff_b(line_b.first.x() - line_b.second.x(),
                  line_b.first.y() - line_b.second.y());

    const double norm_a = std::hypot(diff_a.x(), diff_a.y());
    const double norm_b = std::hypot(diff_b.x(), diff_b.y());
    if(norm_a == 0.0)
        return 0.0;
    if(norm_b == 0.0)
        return 0.0;

    diff_a.x(diff_a.x() / norm_a);
    diff_a.y(diff_a.y() / norm_a);

    diff_b.x(diff_b.x() / norm_b);
    diff_b.y(diff_b.y() / norm_b);

    return std::acos(boost::geometry::dot_product(diff_a, diff_b));
}

template<typename T, typename PointT>
T angle(const typename types::Line<PointT>::type &line_a,
        const typename types::Line<PointT>::type &line_b,
        const T eps)
{
    PointT diff_a(line_a.first.x() - line_a.second.x(),
                  line_a.first.y() - line_a.second.y());
    PointT diff_b(line_b.first.x() - line_b.second.x(),
                  line_b.first.y() - line_b.second.y());

    double a = std::hypot(diff_a.x(), diff_a.y());
    double b = std::hypot(diff_b.x(), diff_b.y());
    double c = 0.0;

    if(equal<PointT, T>(line_a.first, line_b.first, eps)) {
        const double dx = line_a.second.x() - line_b.second.x();
        const double dy = line_a.second.y() - line_b.second.y();
        c = std::hypot(dx, dy);
    } else
    if(equal<PointT, T>(line_a.first, line_b.second, eps)) {
        const double dx = line_a.second.x() - line_b.first.x();
        const double dy = line_a.second.y() - line_b.first.y();
        c = std::hypot(dx, dy);
    } else
    if(equal<PointT, T>(line_a.second, line_b.first, eps)) {
        std::swap(a,b);
        const double dx = line_a.first.x() - line_b.second.x();
        const double dy = line_a.first.y() - line_b.second.y();
        c = std::hypot(dx, dy);
    } else
    if(equal<PointT, T>(line_a.second, line_b.second, eps)) {
        std::swap(a,b);
        const double dx = line_a.first.x() - line_b.first.x();
        const double dy = line_a.first.y() - line_b.first.y();
        c = std::hypot(dx, dy);
    } else {
        return angle<T, PointT>(line_a, line_b);
    }

    if(a == 0.0)
        return 0.0;
    if(b == 0.0)
        return 0.0;

    double angle = std::acos((a*a + b*b - c*c) / (2 * a * b));
    return angle == M_PI ? 0.0 : angle;
}

template<typename T>
inline bool withinIncl
(const T p_x,   const T p_y,
 const T min_x, const T min_y,
 const T max_x, const T max_y)
{
    return p_x >= min_x && p_y >= min_y &&
            p_x <= max_x && p_y <= max_y;
}

template<typename PointT>
inline bool withinIncl
(const PointT &p,
 const typename types::Box<PointT>::type &box)
{
    const PointT &min = box.min_corner();
    const PointT &max = box.max_corner();
    return greaterEqual(p, min) && lessEqual(p, max);
}

template<typename PointT>
inline bool withinIncl
(const typename types::Line<PointT>::type &line,
 const typename types::Box<PointT>::type  &box)
{
    return withinIncl(line.first, box) &&
            withinIncl(line.second, box);
}

template<typename PointT>
inline bool withinIncl
(const typename types::Box<PointT>::type &inner,
 const typename types::Box<PointT>::type &outer)
{
    return withinIncl(inner.min_corner(), outer) &&
            withinIncl(inner.max_corner(), outer);
}

template<typename PointT>
inline bool within
(const typename types::Polygon<PointT>::type &inner,
 const typename types::Polygon<PointT>::type &outer)
{
    bool within = true;
    auto inner_pts_ptr = inner.outer().data();
#pragma omp parallel for reduction(&&:within)
    for(unsigned int i = 0 ; i < inner.outer().size() ; ++i) {
        auto &pt  = *(inner_pts_ptr + i);
        within &= boost::geometry::within(pt, outer);
    }

    return within;
}

template<typename PointT>
inline bool covered_by(const typename types::Polygon<PointT>::type &covered,
                       const typename types::Polygon<PointT>::type &by)
{
#if BOOST_VERSION >= 105700
    return boost::geometry::covered_by(covered, by);
#else
    if(boost::geometry::intersects(covered, by))
        return true;
    if(within<PointT>(covered, by))
        return true;
    return false;
#endif
}

template<typename PointT>
inline bool touches
(const typename types::Line<PointT>::type     &line,
 const typename types::Polygon<PointT>::type  &polygon)
{
    if(withinExcl<PointT>(line.first, polygon))
        return true;
    if(withinExcl<PointT>(line.second, polygon))
        return true;
    if(intersects<PointT>(line, polygon))
        return true;

    return false;
}

template<typename PointT>
inline bool touches
(const typename types::Line<PointT>::type &line,
 const typename types::Box<PointT>::type  &box)
{
    if(boost::geometry::within(line.first, box))
        return true;
    if(boost::geometry::within(line.second, box))
        return true;

    const PointT& min = box.min_corner();
    const PointT& max = box.max_corner();
    PointT lup (min.x(), max.y());
    PointT rlo (max.x(), min.y());
    typename types::Line<PointT>::type t_edge(max, lup);
    if(intersects<PointT>(line, t_edge))
        return true;
    typename types::Line<PointT>::type l_edge(min, lup);
    if(intersects<PointT>(line, l_edge))
        return true;
    typename types::Line<PointT>::type r_edge(max, rlo);
    if(intersects<PointT>(line, r_edge))
        return true;
    typename types::Line<PointT>::type b_edge(min, rlo);
    if(intersects<PointT>(line, b_edge))
        return true;

    return false;
}

template<typename PointT>
typename types::Polygon<PointT>::type toPolygon
(const PointT &min,
 const PointT &max)
{
    typename types::Polygon<PointT>::type poly;
    boost::geometry::append(poly.outer(), PointT(min.x(), min.y()));
    boost::geometry::append(poly.outer(), PointT(max.x(), min.y()));
    boost::geometry::append(poly.outer(), PointT(max.x(), max.y()));
    boost::geometry::append(poly.outer(), PointT(min.x(), max.y()));
    return poly;
}

template<typename PointT>
typename types::Polygon<PointT>::type toPolygon
(const typename types::Box<PointT>::type &box)
{
    typename types::Polygon<PointT>::type poly;
    const PointT &min = box.min_corner();
    const PointT &max = box.max_corner();
    boost::geometry::append(poly.outer(), PointT(min.x(), min.y()));
    boost::geometry::append(poly.outer(), PointT(max.x(), min.y()));
    boost::geometry::append(poly.outer(), PointT(max.x(), max.y()));
    boost::geometry::append(poly.outer(), PointT(min.x(), max.y()));
    return poly;
}

template<typename PointT,
         typename Periodic>
inline void polarLineSet
(const PointT &center,
 const double center_line_orientation,
 const double opening_angle,
 const double angle_increment,
 const double length,
 typename types::LineSet<PointT>::type &lines)
{
    unsigned int num_rays = std::floor(opening_angle / angle_increment) + 1;
    lines.resize(num_rays);

    auto lines_ptr = lines.data();
    double start_angle = center_line_orientation - opening_angle * 0.5;
    double angle = 0.0;

#pragma omp parallel for private(angle)
    for(unsigned int i = 0 ; i < num_rays ; ++i) {
        auto &line = *(lines_ptr + i);
        angle = start_angle + i * angle_increment;
        types::Point2d &origin      = line.first;
        types::Point2d &destination = line.second;
        origin.x(center.x());
        origin.y(center.y());
        destination.x(center.x() + std::cos(angle) * length);
        destination.y(center.y() + std::sin(angle) * length);
    }
}

template<typename PointT,
         typename Periodic>
inline void polarLineSet
(const PointT &center,
 const double center_line_orientation,
 const double opening_angle,
 const double angle_increment,
 const double length,
 typename types::LineSet<PointT>::type &lines,
 std::vector<double> &angles)
{
    unsigned int num_rays = std::floor(opening_angle / angle_increment) + 1;
    lines.resize(num_rays);
    angles.resize(num_rays);

    auto angles_ptr = angles.data();
    auto lines_ptr  = lines.data();

    double start_angle = center_line_orientation - opening_angle * 0.5;

#pragma omp parallel for
    for(unsigned int i = 0 ; i < num_rays ; ++i) {
        auto &line  = *(lines_ptr + i);
        auto &angle = *(angles_ptr + i);

        angle = start_angle + i * angle_increment;

        types::Point2d &origin      = line.first;
        types::Point2d &destination = line.second;
        origin.x(center.x());
        origin.y(center.y());
        destination.x(center.x() + std::cos(angle) * length);
        destination.y(center.y() + std::sin(angle) * length);
    }
}

template<typename PointT,
         typename Periodic>
inline void polarLineSet
(const PointT         &center,
 const double          center_line_orientation,
 const double          opening_angle,
 const unsigned int    num_rays,
 const double          length,
 typename types::LineSet<PointT>::type &lines)
{
    lines.resize(num_rays);

    auto lines_ptr = lines.data();

    double angle_increment(opening_angle / (double) num_rays);
    double start_angle = center_line_orientation - opening_angle * 0.5;
    double angle = 0.0;
    double cos = 1.0;
    double sin = 0.0;

#pragma omp parallel for private(cos, sin, angle)
    for(unsigned int i = 0 ; i < num_rays ; ++i) {
        auto &line = *(lines_ptr + i);
        angle = start_angle + i * angle_increment;
        types::Point2d &origin      = line.first;
        types::Point2d &destination = line.second;
        origin.x(center.x());
        origin.y(center.y());
        Periodic::sin_cos(angle, sin, cos);
        destination.x(center.x() + cos * length);
        destination.y(center.y() + sin * length);
    }
}

template<typename PointT,
         typename Periodic>
inline void polarLineSet
(const PointT         &center,
 const double          center_line_orientation,
 const double          opening_angle,
 const unsigned int    num_rays,
 const double          length,
 typename types::LineSet<PointT>::type &lines,
 std::vector<double> &angles)
{
    lines.resize(num_rays);
    angles.resize(num_rays);
    double angle_increment(opening_angle / (double) num_rays);
    double start_angle = center_line_orientation - opening_angle * 0.5;
    double cos = 1.0;
    double sin = 0.0;

    auto lines_ptr = lines.data();
    auto angles_ptr = angles.data();
#pragma omp parallel for private(cos, sin)
    for(unsigned int i = 0 ; i < num_rays ; ++i) {
        auto &line  = *(lines_ptr + i);
        auto &angle = *(angles_ptr + i);
        angle = start_angle + i * angle_increment;
        types::Point2d &origin      = line.first;
        types::Point2d &destination = line.second;
        origin.x(center.x());
        origin.y(center.y());
        Periodic::sin_cos(angle, sin, cos);
        destination.x(center.x() + cos * length);
        destination.y(center.y() + sin * length);
    }
}

template<typename PointT>
inline void circularPolygonApproximation
(const PointT &center,
 const double  radius,
 const double  ang_res,
 typename types::Polygon<PointT>::type &polygon)
{
    unsigned int iterations = std::floor(2 * M_PI / ang_res + 0.5);
    double       angle = 0.0;

    for(unsigned int i = 0 ; i < iterations ; ++i, angle -= ang_res) {
        PointT p;
        p.x(center.x() + std::cos(angle) * radius);
        p.y(center.y() + std::sin(angle) * radius);
        boost::geometry::append(polygon.outer(), p);
    }
    boost::geometry::append(polygon.outer(), polygon.outer().front());
}
}
}
#endif // DXF_ALGORITHMS_HPP


/*
 * main.cpp
 *
 *  Created on: 20 Aug 2018
 *      Author: scsjd
 */
#include <iostream>
#include <fstream>
#include <jsoncpp/json/json.h>
#include <NTL/ZZ.h>
#include <NTL/ZZ_p.h>
#include <HE1NDecrypter.h>

void usage() {
    std::cerr << "[ERROR] Bad number of parameters" << std::endl;
    std::cout << "    Usage: innervalidate <cipher secrets file> <cipher parameters file> <result file> <output file>" << std::endl;
}

int main(int argc, char **argv) {
	const char* secretsPath;
	const char* parametersPath;
	const char* resultPath;
	const char* outputPath;
	if (argc < 5){
		usage();
		return -1;
	}
	else{
		secretsPath = argv[1];
		parametersPath = argv[2];
		resultPath = argv[3];
		outputPath = argv[4];
	}

	std::cout << "Starting innervalidate with the following configuration:" << std::endl;
	std::cout << "\tcipher secrets file: " << secretsPath << std::endl;
	std::cout << "\tcipher parameters file: " << parametersPath << std::endl;
	std::cout << "\tplaintext result file: " << resultPath << std::endl;
	std::cout << "\tciphertext result file: " << outputPath << std::endl;

	//read in value from result file
	NTL::ZZ result;
	std::ifstream ifs_res(resultPath);
	if (ifs_res.is_open()){
		ifs_res >> result;
		ifs_res.close();
	}

	std::cout << "Result from computing on plaintexts: " << result << std::endl;

	//read in modulus from parameters file
	std::ifstream ifs_params(parametersPath);
	if (!ifs_params.is_open()) throw std::ios_base::failure("Could not open cipher parameters file.");
	std::string parameters;
	getline(ifs_params,parameters);
	ifs_params.close();
	Json::Value root;
	Json::Reader reader;
	NTL::ZZ modulus;
	bool parsedOK = reader.parse(parameters, root);
	if (parsedOK){
		modulus = NTL::conv<NTL::ZZ>(root["modulus"].asCString());
	}
	std::cout << "Modulus: " << modulus << std::endl;

	//read in value from output file
	NTL::ZZ_p output;
	std::ifstream ifs_out(outputPath);
	if (ifs_out.is_open()){
		NTL::ZZ_p::init(modulus);
		ifs_out >> output;
		ifs_out.close();
	}
	std::cout << "Result from computing on ciphertexts: " << output << std::endl;

	//read in secrets and parameters
	std::ifstream ifs_secrets(secretsPath);
	if (!ifs_secrets.is_open()) throw std::ios_base::failure("Could not open cipher secrets file.");
	std::string secrets;
	getline(ifs_secrets,secrets);
	ifs_secrets.close();

	//decrypt
	HE1NDecrypter d;
	d.readSecretsFromJSON(secrets);
	NTL::ZZ decrypted = d.decrypt(output);
	std::cout << "Decrypted result from computing on ciphertexts: " << decrypted << std::endl;
	//compare with value from result file
	if(decrypted==result){
		std::cout << "Result computed correctly" << std::endl;
	}
	else{
		std::cout << "Something went wrong" << std::endl;
		std::cout << "\tdecrypted=" << decrypted << std::endl;
		std::cout << "\tresult=" << result << std::endl;
	}
}


#include <gtest/gtest.h>

#include <lens/OrthogonalLens.h>
#include <fanTriangleMesh.h>
#include <objects/TriangleMeshObject.h>

#include <texture/film/SDLFilm.h>
#include <camera/PointScannerCamera.h>
#include <texture/film/FreeImageFilm.h>
#include <fanBufferObject.h>
#include <objects/TriangleMeshObject.h>
#include <fanScanner.h>

#include <boost/shared_ptr.hpp>

using namespace fan;

/*                 ^ z
 *                 |    ^ y
 *                 |   /
 *                 |  /
 *                 | /
 *                 |/
 *                 .------------> x
 *           ^ y  /
 *           |   /
 *    ---------------
 *   |       | / z   |
 *   |       |/      |
 *   |       .-------|--> x
 *   |               |
 *   |               |
 *    ---------------
 *   Screen coordinate
 */

TEST(OrthogonalLens,LookThroughPositiveYAxis) {
    OrthogonalLens lens( fanVector3<float>(0, -100, 0),
                         fanVector3<float>(0, 0, 0),
                         fanVector3<float>(0, 0, 1),
                         fanVector3<float>(100, 100, 100) );

    fanVector<int, 2> size;
    size[0] = 100; size[1] = 100;
    fanVector<float, 2> result;
    fanVector<float, 4> homoPos;
    fanMatrix<float, 4, 4> pos { 1,0,0,0,
                                 0,1,0,0,
                                 0,0,1,0,
                                 0,0,0,1 };
    project( transform( pos, fanVector3<float>(0, 0, 1) ),
                    lens, size,
                    result, homoPos );
    EXPECT_EQ( 50, result[0] );
    EXPECT_EQ( 51, result[1] );

    project( transform( pos, fanVector3<float>( 0, 0, 2 ) ),
                    lens, size,
                    result, homoPos );

    EXPECT_EQ( 50, result[0] );
    EXPECT_EQ( 52, result[1] );

    project( transform( pos, fanVector3<float>( 1, 0, 2 ) ),
                    lens, size,
                    result, homoPos );

    EXPECT_EQ( 51, result[0] );
    EXPECT_EQ( 52, result[1] );

    project( transform( pos, fanVector3<float>( 2, 0, 2 ) ),
                    lens, size,
                    result, homoPos );

    EXPECT_EQ( 52, result[0] );
    EXPECT_EQ( 52, result[1] );

}

/*                 ^ z                                ^
 *                 |    ^ y                   y ^    / x
 *                 |   /                        | /|/
 *                 |  /                         |/ |
 *                 | /                          / /|
 *                 |/                          /|/ |
 *                 .------------>-------------|-.  /
 *                /             x             |   /
 *                                            |  /
 *                                            | /
 *                                            |/
 *
 */

TEST(OrthogonalLens,LookThroughNegativeXAxis) {
    OrthogonalLens lens( fanVector3<float>(100, 0, 0),
                         fanVector3<float>(0, 0, 0),
                         fanVector3<float>(0, 0, 1),
                         fanVector3<float>(100, 100, 10000) );

    fanVector<int, 2> size;
    fanVector<float, 4> homoPos;
    fanMatrix<float, 4, 4> pos { 1,0,0,0,
                                 0,1,0,0,
                                 0,0,1,0,
                                 0,0,0,1 };
    size[0] = 100; size[1] = 100;
    fanVector<float, 2> result;
    project( transform( pos, fanVector3<float>(0, 0, 1) ),
                    lens, size,
                    result, homoPos );

    EXPECT_EQ( 50, result[0] );
    EXPECT_EQ( 51, result[1] );

    project( transform( pos, fanVector3<float>( 0, 0, 2 ) ),
                    lens, size,
                    result, homoPos );

    EXPECT_EQ( 50, result[0] );
    EXPECT_EQ( 52, result[1] );

    project( transform( pos, fanVector3<float>( 0, 1, 2 ) ),
                    lens, size,
                    result, homoPos );

    EXPECT_EQ( 51, result[0] );
    EXPECT_EQ( 52, result[1] );

    project( transform( pos, fanVector3<float>( 0, 2, 2 ) ),
                    lens, size,
                    result, homoPos );

    EXPECT_EQ( 52, result[0] );
    EXPECT_EQ( 52, result[1] );

}

TEST(OrthogonalLens,RenderAxis) {
    OrthogonalLens lens( fanVector3<float>( 1000, -1000, 1000),
                         fanVector3<float>(0, 0, 0),
                         fanVector3<float>(0, 0, 1),
                         fanVector3<float>(800, 600, 10000) );

    fan::fanVector<int, 2> size;
    size[0] = 800; size[1] = 600;
    FreeImageFilm film( size, "RenderAxis.png" );
    fan::fanScene scene;

    boost::shared_ptr<fan::fanBufferObject<fan::fanVector3<float> > >
        vertices( new fan::fanBufferObject<fan::fanVector3<float> >( 3000 ) );
    boost::shared_ptr<fan::fanBufferObject<fan::fanTriangle> >
        faces( new fan::fanBufferObject<fan::fanTriangle>( 0 ) );
    boost::shared_ptr<fan::fanTriangleMesh>
        mesh( new fan::fanTriangleMesh() );
    mesh->mVertices = vertices;
    mesh->mFaces = faces;

    boost::shared_ptr<TriangleMeshObject>
        object(new TriangleMeshObject(fanMatrix<float, 4, 4>{1,0,0,0,
                                                             0,1,0,0,
                                                             0,0,1,0,
                                                             0,0,0,1,} ) );
    object->mMeshes.push_back( mesh );
    int verticesNum = 0;
    for ( size_t i = 0; i < 1000; ++i ) {
        vertices->mBuffer[verticesNum] = fan::fanVector3<float>( i, 0, 0 );
        ++verticesNum;
    }
    for ( size_t i = 0; i < 1000; i+=4 ) {
        vertices->mBuffer[verticesNum] = fan::fanVector3<float>( 0, i, 0 );
        ++verticesNum;
    }
    for ( size_t i = 0; i < 1000; i+=8 ) {
        vertices->mBuffer[verticesNum] = fan::fanVector3<float>( 0, 0, i );
        ++verticesNum;
    }

    scene.mTriangleMeshObjects.push_back( object );
    PointScannerCamera camera;
    camera.takePicture( scene, film, lens );
    film.develope();

}


#include "network.h"
#include <stdlib.h>
#include <math.h>
#include <algorithm>
#include <assert.h>
#include <armadillo>
#include <iostream>

void relu_kernel(int size, double* data);
void relu_prime_kernel(int size, double* data);
void lin_kernel(int size, double* data);
void lin_prime_kernel(int size, double* data);
void sigmoid_kernel(int size, double* data);
void sigmoid_prime_kernel(int size, double* data);
void tanh_kernel(int size, double* data);
void tanh_prime_kernel(int size, double* data);

static const ActivationFunction activation_functions[] = {
    {false, 0.0, true, 0.0, -1.0, &relu_kernel, &relu_prime_kernel},
    {false, 0.0, false, 0.0, 0.0, &lin_kernel, &lin_prime_kernel},
    {true, 1.0, true, 0.0, -1e46, &sigmoid_kernel, &sigmoid_prime_kernel},
    {true, 1.0, true, -1.0, -1e46, &tanh_kernel, &tanh_prime_kernel}
};

ActivationFunction getActivationFunction(ActivationFunction::Enum func) {
    return activation_functions[func];
}

void layer_init(Layer* layer, int sI, int sO, ActivationFunction::Enum func, double* parameters) {
    layer->nInputs = sI;
    layer->nOutputs = sO;
    layer->nN = sO;
    layer->activation = activation_functions[func].activation;
    layer->activation_prime = activation_functions[func].activation_prime;
    layer->activation_enum = func;
    layer->outputs = (double*)calloc(sO, sizeof *layer->outputs);
    layer->weights = parameters;
    layer->biases = parameters + sI*sO;
}

void layer_free(Layer* layer) {
    free(layer->outputs);
}

void layer_compute_z(Layer* layer, const double* inputs) {
    arma::mat weights(layer->weights, layer->nOutputs, layer->nInputs, false, true);
    arma::colvec bias(layer->biases, layer->nOutputs, false, true);
    arma::colvec output(layer->outputs, layer->nOutputs, false, true);
    arma::colvec input((double*)inputs, layer->nInputs, false, true);
    output = weights*input + bias;
}

void layer_compute_activation(Layer* layer, const double* inputs) {
    layer_compute_z(layer, inputs);
    layer->activation(layer->nOutputs, layer->outputs);
}

Network* network_create(int input_size, int num_layers, const int* layer_sizes,
    int num_activations, const ActivationFunction::Enum* func) {
    assert(input_size > 0);
    assert(num_layers > 0);
    assert(layer_sizes != NULL);
    assert(func != NULL);
    Network* n = (Network*)calloc(1, sizeof *n);
    int layers = num_layers;
    n->nLayers = layers;
    n->nHiddenLayers = layers - 1;
    n->layers = (Layer*)calloc(layers, sizeof *n->layers);
    n->parameter_size = input_size*layer_sizes[0];
    for(int i = 1; i < layers; ++i) {
        n->parameter_size += layer_sizes[i-1]*layer_sizes[i] + layer_sizes[i];
    }
    n->parameters = (double*)calloc(n->parameter_size, sizeof *n->parameters);
    layer_init(n->layers, input_size, layer_sizes[0], func[0], n->parameters);
    int parameter_index = input_size*layer_sizes[0];
    for(int i = 1; i < layers; ++i) {
        layer_init(&n->layers[i], layer_sizes[i-1], layer_sizes[i], func[i],
            &n->parameters[parameter_index]);
        parameter_index += layer_sizes[i-1]*layer_sizes[i] + layer_sizes[i];
    }
    n->nInputs = input_size;
    n->nOutputs = layer_sizes[num_layers-1];
    n->outputs = n->layers[layers-1].outputs;
    return n;
}

void network_load_parameters(Network* n, int parameter_size, const double* parameters) {
    assert(parameter_size == n->parameter_size);
    //Copy the parameters to network (copying not always needed?)
    //memcpy(n->parameters, parameters, parameter_size*sizeof(*parameters));
    for(int i = 0; i < parameter_size; ++i)
        n->parameters[i] = parameters[i];
}
void network_compute(Network* n, int size, const double* input) {
    assert(size == n->nInputs);
    layer_compute_activation(&n->layers[0], input);
    for(int i = 1; i < n->nLayers; ++i) {
        layer_compute_activation(&n->layers[i], n->layers[i-1].outputs);
    }
}

void network_destroy(Network* n) {
    for(int i = 0; i < n->nLayers; ++i) {
        layer_free(&n->layers[i]);
    }
    free(n->layers);
    free(n->parameters);
    free(n);
}

const double* network_output(Network* n, int* output_size) {
    if(output_size != NULL)
        *output_size = n->nOutputs;
    return n->outputs;
}

/*Armadillo interface*/
void network_load_parameters(Network* n, const arma::vec& parameters) {
    network_load_parameters(n, parameters.n_rows, parameters.memptr());
}
void network_compute(Network* n, const arma::vec& input) {
    network_compute(n, input.n_rows, input.memptr());
}
void network_output(Network* n, arma::vec& output) {
    output = arma::colvec(n->outputs, n->nOutputs, true);
}

/*Backpropagation specifics*/

void layer_update_parameters(Layer* l, const double* input, double* delta, double rate) {
    //Outer product
    for(int j = 0; j < l->nInputs; ++j) {
        double input_ = input[j];
        for(int k = 0; k < l->nOutputs; ++k) {
            l->weights[j*l->nOutputs + k] += rate*input_*delta[k];
        }
    }
    for(int k = 0; k < l->nOutputs; ++k)
        l->biases[k] += rate*delta[k];
}

/*Function overwrites layer_j_activation_prime with new delta*/
void layer_backpropagate_delta( Layer* kl, Layer* jl, double* layer_j_activation_prime, double* delta)
{
    double* new_delta = layer_j_activation_prime;
    for(int j = 0; j < jl->nOutputs; ++j) {
        for(int k = 0; k < kl->nOutputs; ++k) {
            new_delta[j] *= delta[k]*kl->weights[j*kl->nOutputs + k];
        }
    }
}

void network_backpropagate(Network* n, const double* inputs, const double* result, double rate) {

    double** temps = (double**)calloc(n->nLayers, sizeof (double*));
    for(int i = 0; i < n->nLayers; ++i)
        temps[i] = (double*)calloc(n->layers[i].nOutputs, sizeof(double));
    //Feedforward and take the z_s (values before activation) into memory
    layer_compute_z(&n->layers[0], inputs);
    memcpy(temps[0], n->layers[0].outputs, n->layers[0].nOutputs*sizeof(double));
    n->layers[0].activation(n->layers[0].nOutputs, n->layers[0].outputs);
    for(int i = 1; i < n->nLayers; ++i) {
        Layer* l = &n->layers[i];
        layer_compute_z(l, n->layers[i-1].outputs);
        memcpy(temps[i], l->outputs, l->nOutputs*sizeof(double));
        l->activation(l->nOutputs, l->outputs);
    }

    //Backpropagate, at each step one z is replaced with backpropagated delta
    Layer* last = &n->layers[n->nLayers-1];
    last->activation_prime(last->nOutputs, temps[n->nLayers-1]);
    for(int i = 0; i < last->nOutputs; ++i)
        temps[n->nLayers-1][i] *= (result[i] - n->outputs[i]);
    for(int i = n->nLayers-1; i > 0; --i) {
        Layer* k = &n->layers[i];
        Layer* j = &n->layers[i-1];
        layer_update_parameters(k, j->outputs, temps[i], rate);
        j->activation_prime(j->nOutputs, temps[i-1]);
        layer_backpropagate_delta(k, j, temps[i-1], temps[i]);
    }
    layer_update_parameters(&n->layers[0], inputs, temps[0], rate);

    for(int i = 0; i < n->nLayers; ++i)
        free(temps[i]);
    free(temps);
}

void relu_kernel(int size, double* data) {
    for(int i = 0; i < size; ++i) {
        data[i] = std::max(0.0, data[i]);
    }
}

void relu_prime_kernel(int size, double* data) {
    for(int i = 0; i < size; ++i) {
        data[i] = data[i] < 0.0 ? 0.0 : 1.0;
    }
}

void lin_kernel(int size, double* data) {
    return;
}

void lin_prime_kernel(int size, double* data) {
    for(int i = 0; i < size; ++i)
        data[i] = 1.0;
}

void sigmoid_kernel(int size, double* data) {
    for(int i = 0; i< size; ++i) {
        data[i] = 1.0/(1.0 + exp(-data[i]));
    }
}

void sigmoid_prime_kernel(int size, double* data) {
    for(int i = 0; i < size; ++i) {
        double f = 1.0/(1.0 + exp(-data[i]));
        data[i] = f*(1-f);
    }
}

void tanh_kernel(int size, double* data) {
    for(int i = 0; i < size; ++i) {
        data[i] = tanh(data[i]);
    }
}

void tanh_prime_kernel(int size, double* data) {
    for(int i = 0; i < size; ++i) {
        data[i] = 1.0 - pow(tanh(data[i]), 2);
    }
}


#include <string>
#include <iostream>
#include <stdio.h>
#include <time.h>
#include <string.h>
#include <boost/algorithm/string/trim.hpp>
//#include <regex>

auto constexpr iso8601_min_year = 1583;
auto constexpr max_month = 12;
auto constexpr max_day = 31;

bool check_days_in_manth(int day, int month, int year)
{
    static constexpr auto year_fact = 400;
    static constexpr auto year_dis_fact = 100;
    static constexpr auto year_dis_fact2 = 4;
    static const int months[] = {
        31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31
    };
    if (months[month] >= day) {
        return true;
    } else {
        if (month == 1) {
            if (year % year_fact == 0 || (year % year_dis_fact != 0 && year % year_dis_fact2 == 0)) {
                return day <= months[month]+1;
            }
        }
    }
    return false;
}

bool check_values(int year, int month, int day)
{

    if (year < iso8601_min_year) {
        return false;
    }

    if (month < 1 || month > max_month) {
        return false;
    } else {
        --month;
    }
    if (day < 1 || day > max_day) {
        return false;
    }
    return check_days_in_manth(day, month, year);
}

bool none_iso8601_match(const char* str) {
    int year = -1, month = -1, day = -1;
    // in this case we don't know the actual order..
    static const char* patterns[] = {
        "%d/%d/%d", "%d:%d:%d"
    };
    for (auto i = std::begin(patterns); i != std::end(patterns); i++) {
        auto r = sscanf(str, *i, &year, &month, &day);
        if (r == 3) {
            if (month > max_month && day <= max_month) {
                std::swap(month, day);
            }
            if (check_values(year, month, day)) {
                return true;
            }
        }
    }
    return false;
}

bool iso8601_submatch(const char* str)
{
    int year = -1, month = -1, day = -1;
    auto r = sscanf(str, "%4d%2d%2d", &year, &month, &day);
    switch (r) {
        case 1:
            return false;
        case 2:
            return check_values(year, month, 0);
        case 3:
            return check_values(year, month, day);
        default:
            return false;
    }
}

bool iso8601_match(const char* str)
{
    int year = -1, month = -1, day = -1;
    auto r = sscanf(str, "%4d-%2d-%2d", &year, &month, &day);  // ISO format yyyy-mm-dd
    switch (r) {
        case 1: // only year, this may mean we have no delimiter
            return iso8601_submatch(str);
        case 2:
            return check_values(year, month, 0);
        case 3:
            return check_values(year, month, day);
        default:
            return iso8601_submatch(str);
    }
}
#if 0
bool regex_iso8601(const std::string& str) {
    static const std::regex matcher("^(\\d{4}(?:(?:(?:\\-)?(?:00[1-9]|0[1-9][0-9]|[1-2][0-9][0-9]|3[0-5][0-9]|36[0-6]))?|(?:(?:\\-)?(?:1[0-2]|0[1-9]))?|(?:(?:\\-)?(?:1[0-2]|0[1-9])(?:\\-)?(?:0[1-9]|[12][0-9]|3[01]))?|(?:(?:\\-)?W(?:0[1-9]|[1-4][0-9]5[0-3]))?|(?:(?:\\-)?W(?:0[1-9]|[1-4][0-9]5[0-3])(?:\\-)?[1-7])?)?)$");
    if (std::regex_search(str, matcher)) {
        return true;
    }
    return false;
}
#endif
bool is_date(std::string input) {
    boost::algorithm::trim(input);
    if (input.empty()) {
        return false;
    }
    return iso8601_match(input.c_str()) || none_iso8601_match(input.c_str());
}


#include <vector>
#include <map>
#include <iostream>
#include <chrono>
#include <cassert>

#include <boost/optional.hpp>
#include <boost/variant.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/hawick_circuits.hpp>

#include <Eigen/Core>  // ensure that this is Tensorflow's version of Eigen, not libIGL's -- see CMakeLists for info

#include <igl/opengl/glfw/Viewer.h>

#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
#include <CGAL/Triangle_2.h>
#include <CGAL/intersections.h>
#include <CGAL/Barycentric_coordinates_2/Triangle_coordinates_2.h>

#include <gurobi_c++.h>

#include "mesh_intersections.h"

//#define CHECK_FOR_CYCLES  // if set, use boost::hawick_cycles to check the face-graph is acyclic
//#define CHECK_LINEAR_SOLVE  // if set, check the result of re-solving the linear system defined by the active constraints using Eigen
//#define RETURN_LINEAR_SOLVE  // if set, return that Eigen solution instead of the Gurobi solution
//#define VISUALISE_ACTIVE_CONSTRAINTS  // if set, display active constraint set after solving with gurobi
//#define MEASURE_TIMES  // if set, measure and report cumulative times for building the graph / LP and solving

typedef CGAL::Epeck CGALKernel;
typedef CGAL::Point_2<CGALKernel> Point2D;
typedef CGAL::Line_2<CGALKernel> Line2D;
typedef CGAL::Segment_2<CGALKernel> Segment2D;
typedef CGAL::Triangle_2<CGALKernel> Triangle2D;

struct IntersectionVertex
{
	float first_z, second_z;
	std::array<float, 3> first_barycentric, second_barycentric;  // corresponding to near/far triangle respectively

	void flip() {
		std::swap(first_z, second_z);
		std::swap(first_barycentric, second_barycentric);
	}
};

float interpolate_barycentric(std::array<float, 3> const &barycentric, float const v0, float const v1, float const v2)
{
	return v0 * barycentric[0] + v1 * barycentric[1] + v2 * barycentric[2];
}

float get_interpolated_z(std::array<float, 3> const &barycentric, Eigen::Vector3i const &face, Eigen::MatrixXf const &projected_vertices)
{
	auto const z_v0 = projected_vertices(face[0], 2);
	auto const z_v1 = projected_vertices(face[1], 2);
	auto const z_v2 = projected_vertices(face[2], 2);

	return interpolate_barycentric(barycentric, z_v0, z_v1, z_v2);
}

Eigen::Vector3f get_position_from_barycentric(std::array<float, 3> const &barycentric, Eigen::Vector3i const &face, Eigen::MatrixXf const &vertices)
{
	Eigen::Vector3f const v0 = vertices.row(face[0]);
	Eigen::Vector3f const v1 = vertices.row(face[1]);
	Eigen::Vector3f const v2 = vertices.row(face[2]);
	return v0 * barycentric[0] + v1 * barycentric[1] + v2 * barycentric[2];
}

typedef boost::adjacency_list<
    boost::vecS,
    boost::vecS,
    boost::directedS,
    boost::no_property,
    std::vector<IntersectionVertex>
> FaceGraph;

typedef std::array<float, 3> Barycentric;

struct TriangleVertex
{
	Eigen::Vector3f position;
	Point2D const &position_cgal;
	unsigned char index_in_face;

	Barycentric get_barycentric() const {
		Barycentric result {0.f, 0.f, 0.f};
		result[index_in_face] = 1.f;
		return result;
	}

	Eigen::Vector2d position_2d() const {
		return Eigen::Vector2d(position[0], position[1]);
	}
};

typedef std::array<TriangleVertex, 3> TriangleVertices;

struct BBox
{
	float min_x, min_y, max_x, max_y;

	explicit BBox(TriangleVertices const &vertices) :
		min_x(std::min(std::min(vertices[0].position[0], vertices[1].position[0]), vertices[2].position[0])),
		min_y(std::min(std::min(vertices[0].position[1], vertices[1].position[1]), vertices[2].position[1])),
		max_x(std::max(std::max(vertices[0].position[0], vertices[1].position[0]), vertices[2].position[0])),
		max_y(std::max(std::max(vertices[0].position[1], vertices[1].position[1]), vertices[2].position[1]))
	{
	}

	bool almost_intersects(BBox const &other, float const epsilon) const {
		if (std::max(min_x, other.min_x) > std::min(max_x, other.max_x) + epsilon)
			return false;
		else if (std::max(min_y, other.min_y) > std::min(max_y, other.max_y) + epsilon)
			return false;
		else
			return true;
	}
};

std::vector<IntersectionVertex> get_intersection_vertices(Eigen::Vector3i const &first_face, TriangleVertices const &first_vertices, Eigen::Vector3i const &second_face, TriangleVertices const &second_vertices)
{
	// Note that if the triangles touch at an edge or vertex, this function *may* return a one-/two-element
	// intersection, or *may* return an empty intersection; non-trivial intersections are always returned

	// Find the set of vertices that are shared by the two triangles; record the index-in-face for each triangle for each such vertex
	std::vector<std::pair<int, int>> shared_vertex_index_indices;
	shared_vertex_index_indices.reserve(3);
	for (int first_vertex_index_index = 0; first_vertex_index_index < 3; ++first_vertex_index_index) {
		for (int second_vertex_index_index = 0; second_vertex_index_index < 3; ++second_vertex_index_index) {
			if (first_face[first_vertex_index_index] == second_face[second_vertex_index_index]) {
				shared_vertex_index_indices.emplace_back(first_vertex_index_index, second_vertex_index_index);
				break;
			}
		}
	}

	// Find the complement of the above set wrt each triangle, i.e. the sets of non-shared vertex-indices-in-face for each triangle
	std::vector<int> first_nonshared_vertex_index_indices, second_nonshared_vertex_index_indices;
	first_nonshared_vertex_index_indices.reserve(3 - shared_vertex_index_indices.size());
	second_nonshared_vertex_index_indices.reserve(3 - shared_vertex_index_indices.size());
	for (int vertex_index_index = 0; vertex_index_index < 3; ++vertex_index_index) {
		bool shared_in_first = false, shared_in_second = false;
		for (auto [shared_index_index_in_first, shared_index_index_in_second] : shared_vertex_index_indices) {
			if (shared_index_index_in_first == vertex_index_index)
				shared_in_first = true;
			if (shared_index_index_in_second == vertex_index_index)
				shared_in_second = true;
		}
		if (!shared_in_first)
			first_nonshared_vertex_index_indices.push_back(vertex_index_index);
		if (!shared_in_second)
			second_nonshared_vertex_index_indices.push_back(vertex_index_index);
	}
	assert(shared_vertex_index_indices.size() + first_nonshared_vertex_index_indices.size() == 3);
	assert(shared_vertex_index_indices.size() + second_nonshared_vertex_index_indices.size() == 3);

	auto const triangle_to_cgal = [] (TriangleVertices const &triangle_vertices) {
		return Triangle2D{triangle_vertices[0].position_cgal, triangle_vertices[1].position_cgal, triangle_vertices[2].position_cgal};
	};

	auto const get_barycentric = [] (Eigen::Vector2d const &point_d, TriangleVertices const &triangle_vertices) {
		auto const cross2 = [] (Eigen::Vector2d const &first, Eigen::Vector2d const&second) {
			return first[0] * second[1] - first[1] * second[0];
		};
		Eigen::Vector2d const v0 = triangle_vertices[1].position_2d() - triangle_vertices[0].position_2d();
		Eigen::Vector2d const v1 = triangle_vertices[2].position_2d() - triangle_vertices[0].position_2d();
		Eigen::Vector2d const v2 = point_d - triangle_vertices[0].position_2d();
		auto const denominator = cross2(v0, v1);
		auto const v = cross2(v2, v1) / denominator;
		auto const w = cross2(v0, v2) / denominator;
		auto const u = 1.f - v - w;
		float const bounds_epsilon = 1.e-3;
		assert(-bounds_epsilon <= u && u <= 1.f + bounds_epsilon);
		assert(-bounds_epsilon <= v && v <= 1.f + bounds_epsilon);
		assert(-bounds_epsilon <= w && w <= 1.f + bounds_epsilon);
		return Barycentric{static_cast<float>(u), static_cast<float>(v), static_cast<float>(w)};
	};

	auto const get_barycentric_and_z = [&] (Eigen::Vector2d const &point_d, TriangleVertices const &triangle_vertices) {
		auto const barycentric = get_barycentric(point_d, triangle_vertices);
		auto const z = interpolate_barycentric(barycentric, triangle_vertices[0].position[2], triangle_vertices[1].position[2], triangle_vertices[2].position[2]);
		return std::make_pair(barycentric, z);
	};

	auto const with_barycentrics_and_zs = [&] (Point2D const &point) {
		Eigen::Vector2d const point_d(CGAL::to_double(point[0]), CGAL::to_double(point[1]));
		auto const [first_barycentric, first_z] = get_barycentric_and_z(point_d, first_vertices);
		auto const [second_barycentric, second_z] = get_barycentric_and_z(point_d, second_vertices);
		return IntersectionVertex{first_z, second_z, first_barycentric, second_barycentric};
	};

	if (shared_vertex_index_indices.size() == 0 || shared_vertex_index_indices.size() == 1) {

		auto const first_triangle = triangle_to_cgal(first_vertices);
		auto const second_triangle = triangle_to_cgal(second_vertices);

		auto const intersection = CGAL::intersection(first_triangle, second_triangle);
		if (intersection) {
			if (auto point = boost::get<Point2D>(&*intersection))
				return {};
			else if (auto segment = boost::get<Segment2D>(&*intersection))
				return {};
			else if (auto triangle = boost::get<Triangle2D>(&*intersection))
				return {
					with_barycentrics_and_zs(triangle->vertex(0)),
					with_barycentrics_and_zs(triangle->vertex(1)),
					with_barycentrics_and_zs(triangle->vertex(2))
				};
			else if (auto poly = boost::get<std::vector<Point2D>>(&*intersection)) {
				std::vector<IntersectionVertex> result;
				result.reserve(poly->size());
				for (auto const &point : *poly)
					result.push_back(with_barycentrics_and_zs(point));
				return result;
			} else
				assert(false);
		} else {
			return {};
		}

	} else if (shared_vertex_index_indices.size() == 2) {

		Segment2D const shared_edge{
			first_vertices[shared_vertex_index_indices[0].first].position_cgal,
			first_vertices[shared_vertex_index_indices[1].first].position_cgal
		};
		Point2D const first_unshared_vertex = first_vertices[first_nonshared_vertex_index_indices[0]].position_cgal;
		Point2D const second_unshared_vertex = second_vertices[second_nonshared_vertex_index_indices[0]].position_cgal;

		Line2D const shared_line = shared_edge.supporting_line();
		if (shared_line.oriented_side(first_unshared_vertex) == shared_line.oriented_side(second_unshared_vertex)) {

			// There is an area intersection; could have two edges crossing, or could have one non-shared vertex inside the other triangle

			std::array<IntersectionVertex, 2> const shared_edge_intersection_vertices {
				IntersectionVertex{
					first_vertices[shared_vertex_index_indices[0].first].position[2],
					second_vertices[shared_vertex_index_indices[0].second].position[2],
					first_vertices[shared_vertex_index_indices[0].first].get_barycentric(),
					second_vertices[shared_vertex_index_indices[0].second].get_barycentric()
				},
				IntersectionVertex{
					first_vertices[shared_vertex_index_indices[1].first].position[2],
					second_vertices[shared_vertex_index_indices[1].second].position[2],
					first_vertices[shared_vertex_index_indices[1].first].get_barycentric(),
					second_vertices[shared_vertex_index_indices[1].second].get_barycentric()
				}
			};

			auto const first_triangle = triangle_to_cgal(first_vertices);
			auto const second_triangle = triangle_to_cgal(second_vertices);

			if (!first_triangle.has_on_unbounded_side(second_unshared_vertex)) {  // not-unbounded accounts for possibility of lying exactly on the boundary
				auto const [first_barycentric, first_z] = get_barycentric_and_z(
					second_vertices[second_nonshared_vertex_index_indices[0]].position_2d(),
					first_vertices
				);
				return {
					shared_edge_intersection_vertices[0],
					shared_edge_intersection_vertices[1],
					IntersectionVertex{
						first_z,
						second_vertices[second_nonshared_vertex_index_indices[0]].position[2],
						first_barycentric,
						second_vertices[second_nonshared_vertex_index_indices[0]].get_barycentric()
					}
				};
			}
			if (!second_triangle.has_on_unbounded_side(first_unshared_vertex)) {
				auto const [second_barycentric, second_z] = get_barycentric_and_z(
					first_vertices[first_nonshared_vertex_index_indices[0]].position_2d(),
					second_vertices
				);
				return {
					shared_edge_intersection_vertices[0],
					shared_edge_intersection_vertices[1],
					IntersectionVertex{
						first_vertices[first_nonshared_vertex_index_indices[0]].position[2],
						second_z,
						first_vertices[first_nonshared_vertex_index_indices[0]].get_barycentric(),
						second_barycentric
					}
				};
			}

			std::array<Segment2D, 2> const first_nonshared_edges {
				Segment2D{shared_edge[0], first_unshared_vertex},
				Segment2D{shared_edge[1], first_unshared_vertex}
			};
			std::array<Segment2D, 2> const second_nonshared_edges {
				Segment2D{shared_edge[0], second_unshared_vertex},
				Segment2D{shared_edge[1], second_unshared_vertex}
			};

			auto const check_edge_intersection = [&] (int const first_nonshared_edge_index, int const second_nonshared_edge_index) -> std::optional<std::vector<IntersectionVertex>> {
				if (auto const intersection = CGAL::intersection(
					first_nonshared_edges[first_nonshared_edge_index],
					second_nonshared_edges[second_nonshared_edge_index]
				)) {
					if (auto const point = boost::get<Point2D>(&*intersection))
						return {{shared_edge_intersection_vertices[0], shared_edge_intersection_vertices[1], with_barycentrics_and_zs(*point)}};
					else
						assert(false);  // this implies the two segments are collinear, in which case the not-on-unbounded-side check above would have passed
				} else {
					return std::nullopt;
				}
			};

			if (auto const result = check_edge_intersection(0, 1))
				return *result;
			if (auto const result = check_edge_intersection(1, 0))
				return *result;

			assert(false);  // ...as the above cases should be exhaustive

		} else {
			// If the two non-shared vertices lie on different sides of the shared edge, then the triangles touch at the edge, but do not intersect
			return {};
		}

	} else {
		assert(false);
	}
}

FaceGraph get_face_graph_hybrid(Eigen::MatrixXf const &projected_vertices, Eigen::MatrixXi const &faces, Eigen::ArrayXf const &offset_magnitudes)
{
	// This returns a directed graph, whose nodes correspond to faces. Edges represent 'possible bumpings', i.e.
	// existence of an edge F --> G implies that F and G overlap in projection, and F is 'further back' than G, hence
	// may bump into it when moved 'forward'
	// It is 'hybrid' because it uses fast float32 pre-checks to avoid expensive infinite-precision CGAL intersection tests

	float const box_intersection_epsilon = 1.e-2f;

	FaceGraph graph(faces.rows());

	std::vector<Point2D> projected_vertices_cgal;
	projected_vertices_cgal.reserve(projected_vertices.size());
	for (int vertex_index = 0; vertex_index < projected_vertices.rows(); ++vertex_index)
		projected_vertices_cgal.push_back(Point2D{projected_vertices(vertex_index, 0), projected_vertices(vertex_index, 1)});

	for (int first_face_index = 0; first_face_index < faces.rows(); ++first_face_index) {

		Eigen::Vector3i const first_face = faces.row(first_face_index);
		TriangleVertices const first_face_vertices {
			TriangleVertex{projected_vertices.row(first_face[0]), projected_vertices_cgal[first_face[0]], 0},
			TriangleVertex{projected_vertices.row(first_face[1]), projected_vertices_cgal[first_face[1]], 1},
			TriangleVertex{projected_vertices.row(first_face[2]), projected_vertices_cgal[first_face[2]], 2}
		};
		BBox const first_bbox(first_face_vertices);

		for (int second_face_index = 0; second_face_index < first_face_index; ++second_face_index) {

			Eigen::Vector3i const second_face = faces.row(second_face_index);
			TriangleVertices const second_face_vertices {
				TriangleVertex{projected_vertices.row(second_face[0]), projected_vertices_cgal[second_face[0]], 0},
				TriangleVertex{projected_vertices.row(second_face[1]), projected_vertices_cgal[second_face[1]], 1},
				TriangleVertex{projected_vertices.row(second_face[2]), projected_vertices_cgal[second_face[2]], 2}
			};
			BBox const second_bbox(second_face_vertices);

			if (!first_bbox.almost_intersects(second_bbox, box_intersection_epsilon))
				continue;

			auto const intersection_vertices = get_intersection_vertices(first_face, first_face_vertices, second_face, second_face_vertices);
			if (intersection_vertices.size() < 3)
				continue;

			// This is necessary as some overlapping triangles may share one or two vertices, which then have zero z-difference
			float largest_magnitude_second_z_minus_first_z = 0.f;
			for (auto const &intersection_vertex : intersection_vertices) {
				auto const z_difference = intersection_vertex.second_z - intersection_vertex.first_z;
				if (std::abs(z_difference) > std::abs(largest_magnitude_second_z_minus_first_z))
					largest_magnitude_second_z_minus_first_z = z_difference;
			}
			assert(largest_magnitude_second_z_minus_first_z != 0.f);
			bool const first_nearer_than_second = largest_magnitude_second_z_minus_first_z > 0.f;
			float const epsilon = 1.e-4;
			for (auto const &intersection_vertex : intersection_vertices) {
				auto const z_difference = intersection_vertex.second_z - intersection_vertex.first_z;
				assert(std::abs(z_difference) < epsilon || (z_difference > 0) == first_nearer_than_second);
			}

			if (first_nearer_than_second)
				boost::add_edge(first_face_index, second_face_index, intersection_vertices, graph);
			else {
				auto flipped_intersection_vertices = intersection_vertices;
				for (auto &intersection_vertex : flipped_intersection_vertices)
					intersection_vertex.flip();
				boost::add_edge(second_face_index, first_face_index, flipped_intersection_vertices, graph);
			}
		}
	}

	return graph;
}

void visualise_ordering(std::vector<int> const &face_ordering, Eigen::MatrixXf const &projected_vertices, Eigen::MatrixXi const &faces)
{
	Eigen::MatrixXd vertices_d = projected_vertices.cast<double>();
	for (int vertex_index = 0; vertex_index < vertices_d.rows(); ++vertex_index) {
		if (vertices_d(vertex_index, 2) < -0.01)
			vertices_d(vertex_index, 2) -= 0.1;
		else if (vertices_d(vertex_index, 2) > 0.01)
			vertices_d(vertex_index, 2) += 0.1;
	}

	igl::opengl::glfw::Viewer viewer;
	viewer.data().set_mesh(vertices_d, faces);
	viewer.data().set_face_based(true);

	Eigen::MatrixXd face_colours(faces.rows(), 3);
	face_colours.setConstant(0.5);
	viewer.data().set_colors(face_colours);

	int current_index_in_ordering = 0;
	viewer.callback_key_down = [&] (igl::opengl::glfw::Viewer &viewer, unsigned int key, int mod) {
		if (key == GLFW_KEY_RIGHT && current_index_in_ordering < face_ordering.size() - 1) {
			face_colours.row(face_ordering[current_index_in_ordering]).setConstant(1.);
			++current_index_in_ordering;
			viewer.data().set_colors(face_colours);
			return true;
		} else if (key == GLFW_KEY_LEFT && current_index_in_ordering > 0) {
			--current_index_in_ordering;
			face_colours.row(face_ordering[current_index_in_ordering]).setConstant(0.5);
			viewer.data().set_colors(face_colours);
			return true;
		}
		return false;
	};

	viewer.launch();
}

void visualise_active_constraints(Eigen::MatrixXf const &vertices, Eigen::MatrixXi const &faces, std::vector<int> const &active_vertex_indices, std::vector<std::pair<int, int>> const &active_face_indices)
{
	igl::opengl::glfw::Viewer viewer;
	viewer.data().set_mesh(vertices.cast<double>(), faces);
	viewer.data().set_face_based(true);

	Eigen::MatrixXd face_colours(faces.rows(), 3);
	face_colours.setConstant(0.5);
	for (auto const &[near_face_index, far_face_index] : active_face_indices) {
		face_colours(near_face_index, 0) = 1.;
		face_colours(far_face_index, 2) = 1.;
	}
	viewer.data().set_colors(face_colours);

	Eigen::MatrixXd point_colours(vertices.rows(), 3);
	point_colours.setZero();
	point_colours.col(0).setConstant(1.);
	for (auto const &active_vertex_index : active_vertex_indices)
		point_colours.row(active_vertex_index) = Eigen::Vector3d(0., 1., 0.);
	viewer.data().set_points(vertices.cast<double>(), point_colours);
	viewer.data().point_size = 10;

	viewer.launch();
}

float const buffer_distance = .05f;  // this is how close triangles are allowed to come to another before they collide
float const initial_z_difference_tolerance = 1.e-4f;  // we allow faces to have passed each other by this far without asserting

std::pair<std::pair<int, int>, std::pair<int, int>> get_segment_vertex_indices(
	Eigen::Vector3i const &near_face,
	Eigen::Vector3i const &far_face,
	IntersectionVertex const &intersection_vertex
) {
	// This returns the indices of the vertices at the ends of the two lines that intersect at the given intersection-vertex

	// If we're at a corner of the near/far face, then return the two edges meeting there

	float const corner_epsilon = 1.e-5;
	float const edge_epsilon = 1.e-4;

	auto const check_corners = [&] (
		std::array<float, 3> const &barycentric,
		Eigen::Vector3i const &face
	) -> std::optional<std::pair<std::pair<int, int>, std::pair<int, int>>> {
		if (barycentric[0] >= 1. - corner_epsilon)
			return { {{face[0], face[1]}, {face[0], face[2]}} };
		else if (barycentric[1] >= 1. - corner_epsilon)
			return { {{face[1], face[0]}, {face[1], face[2]}} };
		else if (barycentric[2] >= 1. - corner_epsilon)
			return { {{face[2], face[0]}, {face[2], face[1]}} };
		else
			return {};
	};

	if (auto corner_result = check_corners(intersection_vertex.first_barycentric, near_face))
		return *corner_result;
	if (auto corner_result = check_corners(intersection_vertex.second_barycentric, far_face))
		return *corner_result;

	// We're not at a corner of either triangle, but we are (necessarily) on an edge of each, and there are zero
	// barycentrics (one per triangle) opposite these edges

	auto const get_edge_opposite_zero_barycentric = [&] (std::array<float, 3> const &barycentric, Eigen::Vector3i const &face) -> std::pair<int, int> {
		if (barycentric[0] <= edge_epsilon) {
			assert(barycentric[1] > edge_epsilon && barycentric[2] > edge_epsilon);
			return {face[1], face[2]};
		}
		else if (barycentric[1] <= edge_epsilon) {
			assert(barycentric[0] > edge_epsilon && barycentric[2] > edge_epsilon);
			return {face[0], face[2]};
		}
		else if (barycentric[2] <= edge_epsilon) {
			assert(barycentric[0] > edge_epsilon && barycentric[1] > edge_epsilon);
			return {face[0], face[1]};
		} else {
			std::ostringstream message_ss;
			message_ss << "get_segment_vertex_indices::get_edge_opposite_zero_barycentric did not find any barycentric less than " << edge_epsilon
				<< "; values: " << barycentric[0] << ", " << barycentric[1] << ", " << barycentric[2];
			throw std::runtime_error(message_ss.str());
		}
	};

	return {
		get_edge_opposite_zero_barycentric(intersection_vertex.first_barycentric, near_face),
		get_edge_opposite_zero_barycentric(intersection_vertex.second_barycentric, far_face)
	};
}

OffsetMagnitudesAndActiveConstraints solve_for_offsets_gurobi(
	Eigen::MatrixXf const &projected_vertices,
	Eigen::MatrixXi const &faces,
	FaceGraph const &face_graph,
	Eigen::ArrayXf const &initial_offset_magnitudes
) {
	static GRBEnv gurobi_env;

#ifdef MEASURE_TIMES
	auto start = std::chrono::high_resolution_clock::now();
#endif

	GRBModel model(gurobi_env);
#ifdef NDEBUG
	model.set(GRB_IntParam_LogToConsole, 0);
#endif
	model.set(GRB_IntParam_Threads, 1);
	model.set(GRB_DoubleParam_TimeLimit, 30.);
	assert(model.get(GRB_IntAttr_ModelSense) ==  GRB_MINIMIZE);

	std::vector<GRBVar> offset_variables;
	std::vector<GRBConstr> offset_greater_than_initial_constraints;
	offset_variables.reserve(projected_vertices.rows());
	for (int vertex_index = 0; vertex_index < projected_vertices.rows(); ++vertex_index) {
		// This defines the objective as the (unweighted) sum of all offset-magnitudes, and the lower-bound on each as the original value
		offset_variables.push_back(model.addVar(0., std::numeric_limits<double>::infinity(), 1., GRB_CONTINUOUS));
		offset_greater_than_initial_constraints.push_back(model.addConstr(offset_variables.back() >= initial_offset_magnitudes[vertex_index]));
	}

#ifdef CHECK_LINEAR_SOLVE
	// This linear system has one row per constraint, and one column per variable (i.e. the offset-magnitudes)
	// We first collect all constraints (which do not have an equality solution), then later extract the active ones
	// Here, we construct the LHS and RHS explicitly, but for tensorflow, we return a set of face/vertex indices
	// describing how to construct them
	std::vector<Eigen::VectorXf> all_push_constraint_linear_matrix_rows;
	std::vector<float> all_push_constraint_linear_vector_elements;
#endif

	std::vector<std::tuple<GRBConstr, int, int, IntersectionVertex const *>> face_pushing_constraints_and_relevant_face_indices_and_intersection_vertex_ptrs;
	for (auto [edge_it, end_edge_it] = boost::edges(face_graph); edge_it != end_edge_it; ++edge_it) {
		int const near_face_index = boost::source(*edge_it, face_graph);
		int const far_face_index = boost::target(*edge_it, face_graph);
		Eigen::Vector3i const near_face = faces.row(near_face_index);
		Eigen::Vector3i const far_face = faces.row(far_face_index);
		for (auto const &intersection_vertex : face_graph[*edge_it]) {

			auto const near_initial_z = get_interpolated_z(
				intersection_vertex.first_barycentric,
				near_face,
				projected_vertices
			);
			auto const far_initial_z = get_interpolated_z(
				intersection_vertex.second_barycentric,
				far_face,
				projected_vertices
			);

			auto const initial_z_difference = far_initial_z - near_initial_z;
			assert(initial_z_difference >= -initial_z_difference_tolerance);
			auto const required_z_difference = std::min(initial_z_difference, buffer_distance);

			Eigen::VectorXf linear_matrix_row;
			linear_matrix_row.setZero(projected_vertices.rows());
			for (int index_in_face = 0; index_in_face < 3; ++index_in_face) {
				linear_matrix_row[near_face[index_in_face]] += intersection_vertex.first_barycentric[index_in_face];
				linear_matrix_row[far_face[index_in_face]] -= intersection_vertex.second_barycentric[index_in_face];
			}
			float const linear_vector_element = far_initial_z - near_initial_z - required_z_difference;

			if (linear_matrix_row.cwiseAbs().maxCoeff() < 1.e-9 && linear_vector_element == 0.) {
				// Such a trivial constraint arises when the intersection-vertex is exactly at a vertex shared between the near and far faces
				continue;
			} else if (linear_matrix_row.cwiseAbs().maxCoeff() < 1.e-5) {
				std::cout << "input constraint max-abs-coeff = " << linear_matrix_row.cwiseAbs().maxCoeff() << "; rhs = " << linear_vector_element << std::endl;
			}

#ifdef CHECK_LINEAR_SOLVE
			all_push_constraint_linear_matrix_rows.push_back(linear_matrix_row);
			all_push_constraint_linear_vector_elements.push_back(linear_vector_element);
#endif

			GRBLinExpr const near_offset_magnitude =
				intersection_vertex.first_barycentric[0] * offset_variables[near_face[0]] +
				intersection_vertex.first_barycentric[1] * offset_variables[near_face[1]] +
				intersection_vertex.first_barycentric[2] * offset_variables[near_face[2]];
			GRBLinExpr const far_offset_magnitude =
				intersection_vertex.second_barycentric[0] * offset_variables[far_face[0]] +
				intersection_vertex.second_barycentric[1] * offset_variables[far_face[1]] +
				intersection_vertex.second_barycentric[2] * offset_variables[far_face[2]];

			GRBConstr const constraint = model.addConstr(near_initial_z + near_offset_magnitude <= far_initial_z + far_offset_magnitude - required_z_difference);
			face_pushing_constraints_and_relevant_face_indices_and_intersection_vertex_ptrs.push_back({constraint, near_face_index, far_face_index, &intersection_vertex});
		}
	}

#ifdef MEASURE_TIMES
	static long long total_build_ms = 0;
	auto const total_build_ms_incremented = __sync_add_and_fetch(&total_build_ms, std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start).count());
	std::cout << "gurobi build -> total " << total_build_ms_incremented << "ms\n";
	start = std::chrono::high_resolution_clock::now();
#endif

	model.optimize();
	auto const optimisation_status = model.get(GRB_IntAttr_Status);
	if (optimisation_status != GRB_OPTIMAL) {
		std::cout << "WARNING: optimisation not solved; status = " << optimisation_status << "; assuming all lower-bound constraints active" << std::endl;
		std::vector<int> all_vertex_indices(projected_vertices.rows());
		std::iota(all_vertex_indices.begin(), all_vertex_indices.end(), 0);
		return {initial_offset_magnitudes, all_vertex_indices, {}, {}};
	}

#ifdef MEASURE_TIMES
	static long long total_solve_ms = 0;
	auto const total_solve_ms_incremented = __sync_add_and_fetch(&total_solve_ms, std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start).count());
	std::cout << "gurobi solve -> total " << total_solve_ms_incremented << "ms\n";
#endif

#ifdef CHECK_LINEAR_SOLVE
	Eigen::MatrixXf active_constraint_linear_matrix(0, projected_vertices.rows());
	Eigen::VectorXf active_constraint_linear_vector(0, 1);
	auto append_constraint_to_linear_system = [&] (Eigen::VectorXf const &lhs, float const rhs) {
		active_constraint_linear_matrix.conservativeResize(active_constraint_linear_matrix.rows() + 1, active_constraint_linear_matrix.cols());
		active_constraint_linear_vector.conservativeResize(active_constraint_linear_vector.rows() + 1);
		active_constraint_linear_matrix.row(active_constraint_linear_matrix.rows() - 1) = lhs;
		active_constraint_linear_vector[active_constraint_linear_vector.rows() - 1] = rhs;
	};
#endif

	double const slack_epsilon = 1.e-5;  // note that Gurobi's default feasibility tolerance is 1.e-6

	std::unique_ptr<double const> const gurobi_offset_magnitudes_ptr(model.get(GRB_DoubleAttr_X, &offset_variables[0], offset_variables.size()));
	Eigen::VectorXf const gurobi_solved_offset_magnitudes = Eigen::Map<Eigen::VectorXd const>(gurobi_offset_magnitudes_ptr.get(), offset_variables.size()).cast<float>();

	std::vector<int> active_bound_constraint_vertex_indices;
	for (int vertex_index = 0; vertex_index < projected_vertices.rows(); ++vertex_index) {
		double const slack = offset_greater_than_initial_constraints[vertex_index].get(GRB_DoubleAttr_Slack);
		assert(slack <= slack_epsilon);  // these slack variables are negative as the constraints are greater-than
		if (slack >= -slack_epsilon) {
			active_bound_constraint_vertex_indices.push_back(vertex_index);
#ifdef CHECK_LINEAR_SOLVE
			Eigen::VectorXf linear_matrix_row;
			linear_matrix_row.setZero(projected_vertices.rows());
			linear_matrix_row[vertex_index] = 1.f;
			append_constraint_to_linear_system(linear_matrix_row, initial_offset_magnitudes[vertex_index]);
#endif
		}
	}

	std::vector<std::pair<int, int>> active_push_constraint_face_indices;  // pairs of near-face, far-face
	std::vector<std::pair<std::pair<int, int>, std::pair<int, int>>> active_push_constraint_vertex_indices;  // identifies the IntersectionVertex as the intersection point of two edges; pairs of first-edge, second-edge -- each a pair of start-vertex, end-vertex
	for (int constraint_index = 0; constraint_index < face_pushing_constraints_and_relevant_face_indices_and_intersection_vertex_ptrs.size(); ++constraint_index) {
		auto const &[constraint, near_face_index, far_face_index, intersection_vertex_ptr] = face_pushing_constraints_and_relevant_face_indices_and_intersection_vertex_ptrs[constraint_index];
		double const slack = constraint.get(GRB_DoubleAttr_Slack);
		assert(slack >= -slack_epsilon);  // these slack variables are positive as the constraints are less-than
		if (slack <= slack_epsilon) {
			active_push_constraint_face_indices.push_back({near_face_index, far_face_index});
			active_push_constraint_vertex_indices.push_back(get_segment_vertex_indices(faces.row(near_face_index), faces.row(far_face_index), *intersection_vertex_ptr));
#ifdef CHECK_LINEAR_SOLVE
			append_constraint_to_linear_system(
				all_push_constraint_linear_matrix_rows[constraint_index],
				all_push_constraint_linear_vector_elements[constraint_index]
			);
#endif
		}
	}
#ifndef NDEBUG
	std::cout <<
		active_bound_constraint_vertex_indices.size() << "/" << offset_greater_than_initial_constraints.size() << " active lower-bound constraints; " <<
		active_push_constraint_face_indices.size() << "/" << face_pushing_constraints_and_relevant_face_indices_and_intersection_vertex_ptrs.size() << " active face-push constraints; " <<
		projected_vertices.rows() << " variables" <<
		std::endl;
#endif
	assert(active_push_constraint_face_indices.size() == active_push_constraint_vertex_indices.size());
	int const active_constraint_count = active_bound_constraint_vertex_indices.size() + active_push_constraint_face_indices.size();
	assert(active_constraint_count >= projected_vertices.rows());

#ifdef VISUALISE_ACTIVE_CONSTRAINTS
	visualise_active_constraints(projected_vertices, faces, active_bound_constraint_vertex_indices, active_push_constraint_face_indices);
#endif

#ifdef CHECK_LINEAR_SOLVE

	auto const final_row_max = active_constraint_linear_matrix.row(active_constraint_linear_matrix.rows() - 1).cwiseAbs().maxCoeff();
	if (final_row_max < 1.e-2)
		std::cout << "final row max abs = " << final_row_max << std::endl;

	assert(active_constraint_linear_matrix.rows() == active_constraint_linear_vector.rows() && active_constraint_linear_matrix.rows() == active_constraint_count);
	float const lambda = 1.e-12f;
	Eigen::VectorXf const eigen_solved_offset_magnitudes = (
		active_constraint_linear_matrix.transpose() * active_constraint_linear_matrix + Eigen::MatrixXf::Identity(active_constraint_linear_matrix.cols(), active_constraint_linear_matrix.cols()) * lambda
	).llt().solve(active_constraint_linear_matrix.transpose() * active_constraint_linear_vector);
	for (int vertex_index = 0; vertex_index < projected_vertices.rows(); ++vertex_index) {
		float const gurobi_value = offset_variables[vertex_index].get(GRB_DoubleAttr_X);
		float const eigen_value = eigen_solved_offset_magnitudes[vertex_index];
		if (std::abs(gurobi_value - eigen_value) > 1.e-2) {
			std::cout << "vertex #" << vertex_index << ": gurobi = " << gurobi_value << ", eigen = " << eigen_value;
			if (gurobi_value != 0.)
				std::cout << "; relative error = " << std::abs(eigen_value - gurobi_value) / std::abs(gurobi_value) * 100 << "%";
			std::cout << std::endl;
		}
	}

	for (int active_constraint_index = 0; active_constraint_index < active_constraint_count; ++active_constraint_index) {
		auto const lhs_eigen = active_constraint_linear_matrix.row(active_constraint_index).dot(eigen_solved_offset_magnitudes);
		auto const lhs_gurobi = active_constraint_linear_matrix.row(active_constraint_index).dot(gurobi_solved_offset_magnitudes);
		auto const rhs = active_constraint_linear_vector[active_constraint_index];
		if (std::abs(lhs_eigen - rhs) > 1.e-4) {
			std::cout << "active constraint #" << active_constraint_index << ": lhs = " << lhs_eigen << ", rhs = " << rhs;
			if (rhs != 0.)
				std::cout << "; relative error = " << std::abs(lhs_eigen - rhs) / rhs * 100 << "%";
			std::cout << " (gurobi lhs = " << lhs_gurobi << ")" << std::endl;
		}
	}

#endif

#ifdef RETURN_LINEAR_SOLVE

	return {
		eigen_solved_offset_magnitudes.array(),
		active_bound_constraint_vertex_indices,
		active_push_constraint_face_indices,
		active_push_constraint_vertex_indices
	};

#else

	return {
		gurobi_solved_offset_magnitudes.array(),
		active_bound_constraint_vertex_indices,
		active_push_constraint_face_indices,
		active_push_constraint_vertex_indices
	};

#endif
}

struct CycleVisitor {
	template<class Cycle>
	void cycle(Cycle const &, FaceGraph const &) {
		assert(false);
	}
};

OffsetMagnitudesAndActiveConstraints get_pushed_offset_magnitudes_and_active_constraints(Eigen::MatrixXf const &projected_vertices, Eigen::MatrixXi const &faces, Eigen::ArrayXf const &initial_offset_magnitudes)
{
	assert(projected_vertices.rows() == initial_offset_magnitudes.size());
	for (int vertex_index = 0; vertex_index < projected_vertices.rows(); ++vertex_index)
		assert(initial_offset_magnitudes[vertex_index] >= 0.f);

#ifdef MEASURE_TIMES
	auto start = std::chrono::high_resolution_clock::now();
#endif

	auto const face_graph = get_face_graph_hybrid(projected_vertices, faces, initial_offset_magnitudes);

#ifdef MEASURE_TIMES
	static long long total_ms = 0;
	auto const total_ms_incremented = __sync_add_and_fetch(&total_ms, std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start).count());
	std::cout << "intersections & graph construction -> total " << total_ms_incremented << "ms\n";
#endif

#ifdef CHECK_FOR_CYCLES
	boost::hawick_circuits(face_graph, CycleVisitor());
#endif

	return solve_for_offsets_gurobi(projected_vertices, faces, face_graph,  initial_offset_magnitudes);
}