#include <algorithm>

#ifdef _WIN32
    #pragma warning(push, 0)
    #include <boost/polygon/polygon.hpp>
    #pragma warning(pop)
#else
    #include <boost/polygon/polygon.hpp>
#endif

#include <cradle/geometry/angle.hpp>
#include <cradle/geometry/clipper.hpp>
#include <cradle/geometry/intersection.hpp>
#include <cradle/geometry/polygonal.hpp>

namespace cradle {

// COMMON FUNCTIONS

static vector<2,double> get_corner(box<2,double> const& box, int index)
{
    vector<2,double> corner;
    while(index >= 4)
    {
        index -= 4;
    }
    switch (index)
    {
        case 0:
            return box.corner;
        case 1:
            return box.corner + make_vector(box.size[0], 0.);
        case 2:
            return box.corner + make_vector(box.size[0], box.size[1]);
        case 3:
            return box.corner + make_vector(0., box.size[1]);
    }
    return make_vector(0.,0.);
}

// POLYGONS

polygon2 make_polygon2(std::vector<vertex2> const& vertices)
{
    polygon2 poly;
    initialize(&poly.vertices, vertices);
    return poly;
}

double get_area(polygon2 const& poly)
{
    return get_area_and_centroid(poly).first;
}

vector2d get_centroid(polygon2 const& poly)
{
    return get_area_and_centroid(poly).second;
}

std::pair<double, vector2d> get_area_and_centroid(polygon2 const& poly)
{
    double a = 0.0;
    double cx = 0.0;
    double cy = 0.0;
    for (polygon2_edge_view ev(poly); !ev.done(); ev.advance())
    {
        double cross = ev.p0()[0] * ev.p1()[1] - ev.p0()[1] * ev.p1()[0];
        a += cross;
        cx += (ev.p0()[0] + ev.p1()[0]) * cross;
        cy += (ev.p0()[1] + ev.p1()[1]) * cross;
    }
    double scale = 3.0 * a;
    return std::make_pair(std::fabs(0.5 * a),
        make_vector(cx / scale, cy / scale));
}

polygon2 scale(polygon2 const& poly, double factor)
{
    return scale(poly, make_vector(factor, factor));
}

struct point_scaling_fn
{
    vector<2,double> factor;
    vertex2 operator()(vertex2 const& src) const
    {
        return make_vector(src[0] * factor[0], src[1] * factor[1]);
    }
};

polygon2 scale(polygon2 const& poly, vector<2,double> const& factor)
{
    point_scaling_fn fn;
    fn.factor = factor;
    return map_points(poly, fn);
}

bool is_inside(polygon2 const& poly, vector<2,double> const& p)
{
    bool c = false;
    for (polygon2_edge_view ev(poly); !ev.done(); ev.advance())
    {
        if (((ev.p0()[1] <= p[1] && p[1] < ev.p1()[1]) ||
            (ev.p1()[1] <= p[1] && p[1] < ev.p0()[1]))
         && (p[0] < (ev.p1()[0] - ev.p0()[0]) * (p[1] - ev.p0()[1]) /
            (ev.p1()[1] - ev.p0()[1]) + ev.p0()[0]))
        {
            c = !c;
        }
    }
    return c;
}

bool is_inside(polygon2 const& parent_poly, polygon2 const& child_poly)
{
    for (auto const& point : child_poly.vertices)
    {
        if (!is_inside(parent_poly, point))
        {
            return false;
        }
    }
    return true;
}

bool is_ccw(polygon2 const& poly)
{
    assert(poly.vertices.size() > 2);
    vector<2,double> edge0 = poly.vertices[1] - poly.vertices[0];
    vector<2,double> edge1 = poly.vertices[2] - poly.vertices[1];
    return (edge0[0] * edge1[1]) - (edge0[1] * edge1[0]) > 0;
}

polygon2 as_polygon(box<2,double> const& box)
{
    polygon2 poly;
    vertex2* vertices = allocate(&poly.vertices, 4);
    vertices[0] = box.corner;
    vertices[1] = box.corner + make_vector(box.size[0], 0.);
    vertices[2] = box.corner + box.size;
    vertices[3] = box.corner + make_vector(0., box.size[1]);
    return poly;
}

polygon2 as_polygon(circle<double> const& circle, unsigned n_segments)
{
    polygon2 poly;
    vertex2* vertices = allocate(&poly.vertices, n_segments);
    for (unsigned i = 0; i < n_segments; ++i)
    {
        angle<double,radians> a(2 * pi * i / n_segments);
        vertices[i] =
            make_vector(cos(a), sin(a)) * circle.radius + circle.center;
    }
    return poly;
}

polygon2 as_polygon(triangle<2,double> const& tri)
{
    polygon2 poly;
    vertex2* vertices = allocate(&poly.vertices, 3);
    for (unsigned i = 0; i != 3; ++i)
        vertices[i] = tri[i];
    return poly;
}

// POLYSETS

void create_polyset(polyset* set, polygon2 const& poly)
{
    *set = polyset();
    set->polygons.push_back(poly);
}

polyset make_polyset(polygon2 const& poly)
{
    polyset p;
    create_polyset(&p, poly);
    return p;
}

void add_polygon(polyset& set, polygon2 const& poly)
{
    polyset addition;
    create_polyset(&addition, poly);
    if (set.polygons.empty())
        swap(set, addition);
    else
        do_set_operation(&set, set_operation::UNION, addition, set);
}
void add_hole(polyset& set, polygon2 const& hole)
{
    assert(!set.polygons.empty());
    polyset subtraction;
    create_polyset(&subtraction, hole);
    do_set_operation(&set, set_operation::DIFFERENCE, set, subtraction);
}

polyset remove_polyset_holes(
    polyset const& original_shape)
{
    polyset shape = original_shape;

    while (!shape.holes.empty())
    {
        do_set_operation(
            &shape,
            set_operation::UNION,
            shape,
            make_polyset(shape.holes.front()));
    }

    return shape;
}

polyset scale(polyset const& set, double factor)
{
    return scale(set, make_vector(factor, factor));
}
polyset scale(polyset const& set, cradle::vector<2,double> const& factor)
{
    point_scaling_fn fn;
    fn.factor = factor;
    return map_points(set, fn);
}

std::pair<double, vector2d> static
get_area_and_centroid(polyset const& set)
{
    double area = 0;
    auto centroid = make_vector(0., 0.);
    for (auto const& i : set.polygons)
    {
        auto pair = get_area_and_centroid(i);
        area += pair.first;
        centroid += pair.second * pair.first;
    }
    for (auto const& i : set.holes)
    {
        auto pair = get_area_and_centroid(i);
        area -= get_area(i);
        centroid -= pair.second * pair.first;
    }
    centroid /= area;
    return std::make_pair(area, centroid);
}

double get_area(polyset const& set)
{
    return get_area_and_centroid(set).first;
}

vector2d get_centroid(polyset const& set)
{
    auto pair = get_area_and_centroid(set);
    if (pair.first == 0)
        throw exception("centroid requested for empty polyset");
    return pair.second;
}

bool is_inside(polyset const& set, vector<2,double> const& p)
{
    int inside_count = 0;
    for (auto const& i : set.polygons)
    {
        if (is_inside(i, p))
            ++inside_count;
    }
    for (auto const& i : set.holes)
    {
        if (is_inside(i, p))
            --inside_count;
    }
    return inside_count > 0;
}

typedef line_segment<2, double> line_segment2;

double distance_to_polyset(vector<2,double> const& p, polyset const& set)
{
    double d2p = 1.0e10;
    size_t polygon_count = set.polygons.size();
    for (size_t k = 0; k < polygon_count; ++k)
    {
        size_t vertex_count = set.polygons[k].vertices.n_elements;
        auto v0 = set.polygons[k].vertices.elements[vertex_count - 1];
        for (size_t i = 0; i < vertex_count; ++i)
        {
            // Get segment end point
            auto v1 = set.polygons[k].vertices.elements[i];
            line_segment2 segment(v0, v1);
            auto len = length(segment);

            // Ensure the line segment has finite length
            if (len >= 1.0e-8)
            {
                // Discretize line segment and determine distance
                double spacing = 0.25;
                int split_count = std::min(std::max(int(std::ceil(len / spacing)), 2), 30);
                for (int j = 0; j < split_count; ++j)
                {
                    double dist = distance(segment, p);

                    if (dist < std::fabs(d2p))
                    {
                        bool pip = point_in_polygon(p, set.polygons[k]);
                        d2p = pip ? -dist : dist;
                    }
                }
            }

            // Increment segment start point
            v0 = v1;
        }
    }

    // Handle Holes too
    size_t hole_count = set.holes.size();
    for (size_t k = 0; k < hole_count; ++k)
    {
        size_t vertex_count = set.holes[k].vertices.n_elements;
        auto v0 = set.holes[k].vertices.elements[vertex_count - 1];
        for (size_t i = 0; i < vertex_count; ++i)
        {
            // Get segment end point
            auto v1 = set.holes[k].vertices.elements[i];
            line_segment2 segment(v0, v1);
            auto len = length(segment);

            // Ensure the line segment has finite length
            if (len >= 1.0e-8)
            {
                // Discretize line segment and determine distance
                double spacing = 0.25;
                int split_count = std::min(std::max(int(std::ceil(len / spacing)), 2), 30);
                for (int j = 0; j < split_count; ++j)
                {
                    double dist = distance(segment, p);

                    if (dist < std::fabs(d2p))
                    {
                        bool pip = point_in_polygon(p, set.holes[k]);
                        d2p = pip ? dist : -dist;
                    }
                }
            }

            // Increment segment start point
            v0 = v1;
        }
    }

    return d2p;
}

namespace {

    typedef boost::polygon::point_data<int> boost_point;

    typedef boost::polygon::polygon_data<int> boost_polygon;

    typedef boost::polygon::polygon_with_holes_data<int>
        boost_polygon_with_holes;

    typedef boost::polygon::polygon_set_data<int> boost_polygon_set;

    void to_boost_polygon(boost_polygon& bp, polygon2 const& poly)
    {
        size_t n_points = poly.vertices.size();
        bp.coords_.resize(n_points);
        for (size_t i = 0; i != n_points; ++i)
        {
            vector<2,double> const& p = poly.vertices[i];
            bp.coords_[i] = boost_polygon::point_type(
                int(p[0] / clipper_integer_precision),
                int(p[1] / clipper_integer_precision));
        }
    }

    void to_boost_polygon(boost_polygon_set& boost_set,
        polyset const& cradle_set)
    {
        using namespace boost::polygon::operators;
        boost_set.clear();
        for (auto const& i : cradle_set.polygons)
        {
            boost_polygon poly;
            to_boost_polygon(poly, i);
            boost_set ^= poly;
        }
        for (auto const& i : cradle_set.holes)
        {
            boost_polygon poly;
            to_boost_polygon(poly, i);
            boost_set ^= poly;
        }
    }

    vector<2,double>
    from_boost_point(boost_point const& p)
    {
        return make_vector<double>(
            double(p.x()) * clipper_integer_precision,
            double(p.y()) * clipper_integer_precision);
    }

    void from_boost_polygon(polygon2& poly, boost_polygon const& bp)
    {
        size_t n_points = bp.size();
        if (n_points != 0)
        {
            // The first and last vertices are the same, so skip the last.
            --n_points;
            vertex2* vertices = allocate(&poly.vertices, n_points);
            for (size_t i = 0; i != n_points; ++i)
                vertices[i] = from_boost_point(bp.coords_[i]);
        }
        else
            clear(&poly.vertices);
    }

    void from_boost_polygon(polyset& cradle_set,
        boost_polygon_set const& boost_set)
    {
        cradle_set = polyset();
        std::vector<boost_polygon_with_holes> boost_polys;
        boost_set.get(boost_polys);
        for (auto const& bp : boost_polys)
        {
            polygon2 poly;
            from_boost_polygon(poly, bp.self_);
            cradle_set.polygons.push_back(poly);
            for (auto const& bh : bp.holes_)
            {
                polygon2 hole;
                from_boost_polygon(hole, bh);
                cradle_set.holes.push_back(hole);
            }
        }
    }

    void from_boost_polygon(std::vector<polygon2>& polys,
        boost_polygon_set const& set)
    {
        std::vector<boost_polygon> boost_polys;
        set.get(boost_polys);
        size_t n_polys = boost_polys.size();
        polys.resize(n_polys);
        for (size_t i = 0; i != n_polys; ++i)
        {
            boost_polygon const& bp = boost_polys[i];
            polygon2& poly = polys[i];
            from_boost_polygon(poly, bp);
        }
    }

} // anonymous namespace

std::vector<polygon2> as_polygon_list(polyset const& set)
{
    boost_polygon_set boost_set;
    to_boost_polygon(boost_set, set);
    std::vector<polygon2> polygons;
    from_boost_polygon(polygons, boost_set);
    return polygons;
}

bool almost_equal(polyset const& set1, polyset const& set2, double tolerance)
{
    polyset xor_;
    do_set_operation(&xor_, set_operation::XOR, set1, set2);
    return almost_equal(get_area(xor_), 0., tolerance);
}

bool almost_equal(polyset const& set1, polyset const& set2)
{
    polyset xor_;
    do_set_operation(&xor_, set_operation::XOR, set1, set2);
    return almost_equal(get_area(xor_), 0.);
}

void do_set_operation(
    polyset* result,
    set_operation op,
    polyset const& set1,
    polyset const& set2)
{
    ClipperLib::Clipper clipper;
    clipper.AddPolygons(to_clipper(set1), ClipperLib::ptSubject);
    clipper.AddPolygons(to_clipper(set2), ClipperLib::ptClip);
    ClipperLib::ClipType clipper_op;
    switch (op)
    {
     case set_operation::UNION:
        clipper_op = ClipperLib::ctUnion;
        break;
     case set_operation::INTERSECTION:
        clipper_op = ClipperLib::ctIntersection;
        break;
     case set_operation::DIFFERENCE:
        clipper_op = ClipperLib::ctDifference;
        break;
     case set_operation::XOR:
        clipper_op = ClipperLib::ctXor;
        break;
     default:
         throw exception("set operation undefined");
    }
    ClipperLib::Polygons solution;
    clipper.Execute(clipper_op, solution);
    from_clipper(result, solution);
}

api(fun)
polyset
polyset_combination(set_operation op, std::vector<polyset> const& polysets)
{
    if (polysets.empty())
    {
        return polyset();
    }
    else if (polysets.size() == 1)
    {
        return polysets[0];
    }
    polyset tmp[2];
    int output_index = 0;
    polyset const* input = &polysets.front();
    auto i = polysets.begin();
    ++i;
    for (; i != polysets.end(); ++i)
    {
        output_index = 1 - output_index;
        auto output = &tmp[output_index];
        do_set_operation(output, op, *input, *i);
        input = output;
    }
    return tmp[output_index];
}

// Triangulate a convex Boost polygon.
// Note that this does NOT clear the list of triangles.
// It simply pushes more triangles onto the back of it.
static void
triangulate_boost_polygon(
    std::vector<triangle<2,double> >& tris, boost_polygon const& poly)
{
    size_t n_points = poly.size();
    if (n_points > 0)
    {
        // The first and last vertices are the same, so skip the last.
        --n_points;
        for (size_t i = 2; i < n_points; ++i)
        {
            triangle<2,double> tri;
            tri[0] = from_boost_point(poly.coords_[0]);
            tri[1] = from_boost_point(poly.coords_[i - 1]);
            tri[2] = from_boost_point(poly.coords_[i]);
            tris.push_back(tri);
        }
    }
}

std::vector<triangle<2,double> >
triangulate_polyset(polyset const& set)
{
    boost_polygon_set boost_set;
    to_boost_polygon(boost_set, set);
    std::vector<boost_polygon> trapezoids;
    boost_set.get_trapezoids(trapezoids);
    size_t n_trapezoids = trapezoids.size();
    std::vector<triangle<2,double> > tris;
    tris.reserve(n_trapezoids * 2);
    for (size_t i = 0; i != n_trapezoids; ++i)
        triangulate_boost_polygon(tris, trapezoids[i]);
    return tris;
}

void
expand(polyset* dst, polyset const& src, double amount)
{
    using namespace boost::polygon::operators;
    auto clipper_in = to_clipper(src);
    ClipperLib::Polygons clipper_out;
    OffsetPaths(clipper_in, clipper_out, amount / clipper_integer_precision,
        ClipperLib::jtRound, ClipperLib::etClosed);
    from_clipper(dst, clipper_out);
}

polyset polyset_expansion(polyset const& src, double amount)
{
    polyset result;
    expand(&result, src, amount);
    return result;
}

void create_polyset_from_polygons(polyset* set,
    std::vector<polygon2> const& polygons)
{
    using namespace boost::polygon::operators;
    boost_polygon_set boost_set;
    for (std::vector<polygon2>::const_iterator
        i = polygons.begin(); i != polygons.end(); ++i)
    {
        boost_polygon bp;
        to_boost_polygon(bp, *i);
        boost_set += bp;
    }
    from_boost_polygon(*set, boost_set);
}

// STRUCTURES

bool is_inside(structure_geometry_slice const& s, double p)
{
    return ((p >= s.position - (s.thickness / 2.)) &&
        (p < s.position + (s.thickness / 2.)));
}

void static
reset_structure_to_slice_list(
    structure_geometry* structure,
    slice_description_list const& slices)
{
    structure->slices.clear();
    structure->master_slice_list.clear();
    structure->master_slice_list = slices;
}

structure_geometry static
remove_empty_slices(structure_geometry const& structure)
{
    structure_geometry result;
    result.master_slice_list = structure.master_slice_list;
    for (auto const& slice : structure.slices)
    {
        if (!is_empty(slice.second))
        {
            result.slices.insert(slice);
        }
    }
    return result;
}

polyset
get_slice(structure_geometry const& structure, double position)
{
    auto slice = get_structure_slice(structure, position);
    if (slice)
    {
        return get(slice).region;
    }

    return polyset();
}

structure_geometry_slice
find_slice_at_exact_position(
    structure_polyset_list const& slices,
    double position,
    double thickness)
{
    auto s = slices.find(position);
    if (s != slices.end())
    {
        return structure_geometry_slice(position, thickness, s->second);
    }
    return structure_geometry_slice(position, thickness, polyset());
}

optional<structure_geometry_slice>
get_structure_slice(structure_geometry const& structure, double position)
{
    auto const& masters = structure.master_slice_list;
    auto const& slices = structure.slices;
    auto p = position;

    if (masters.size() == 0 ||
        p < masters[0].position - 0.5 * masters[0].thickness ||
        p > masters.back().position + 0.5 * masters.back().thickness)
    {
        return none;
    }

    for (size_t i = 1; i < masters.size(); ++i)
    {
        if (p < masters[i].position)
        {
            if (p - masters[i - 1].position < masters[i].position - p)
            {
                return
                    some(
                        find_slice_at_exact_position(
                            slices,
                            masters[i - 1].position,
                            masters[i - 1].thickness));
            }
            else
            {
                return
                    some(
                        find_slice_at_exact_position(
                            slices,
                            masters[i].position,
                            masters[i].thickness));
            }
        }
    }
    // Valid case when p is past the last slice position, but within its thickness
    return
        some(
            find_slice_at_exact_position(
                slices,
                masters.rbegin()->position,
                masters.rbegin()->thickness));
}

std::vector<structure_geometry_slice>
get_structure_slices(structure_geometry const& structure, double p_low, double p_high)
{
    auto const& masters = structure.master_slice_list;
    auto const& slices = structure.slices;

    std::vector<structure_geometry_slice> output;

    if (masters.size() == 0 ||
        p_low > masters.back().position + 0.5 * masters.back().thickness ||
        p_high < masters[0].position - 0.5 * masters[0].thickness)
    {
        return output;
    }

    // Find the start and end slice indices
    // Note: the initial value set below is critical to this functioning properly
    size_t i_start = masters.size();

    for (size_t i = 1; i < masters.size(); ++i)
    {
        if (p_low < masters[i].position)
        {
            if (p_low - masters[i - 1].position < masters[i].position - p_low)
            {
                i_start = i - 1;
            }
            else
            {
                i_start = i;
            }
            output.push_back(
                find_slice_at_exact_position(
                    slices,
                    masters[i_start].position,
                    masters[i_start].thickness));
            break;
        }
    }

    for (size_t i = i_start + 1; i < masters.size(); ++i)
    {
        if (p_high < masters[i].position)
        {
            if (p_high - masters[i - 1].position < masters[i].position - p_high)
            {
                // Last slice is already in the list, just return now
                return output;
            }
            else
            {
                // Add the last slice and then return
                output.push_back(
                    find_slice_at_exact_position(
                        slices,
                        masters[i].position,
                        masters[i].thickness));
                return output;
            }
        }

        // Slice is within limits, add it
        output.push_back(
            find_slice_at_exact_position(
                slices,
                masters[i].position,
                masters[i].thickness));
    }

    // All slices have been added (but never reached p_high position), just return
    return output;
}

void static
fold_in(double& volume, vector3d& centroid,
    std::pair<double,vector2d> const& area_and_centroid,
    double z, double thickness)
{
    volume += area_and_centroid.first * thickness;
    centroid += unslice(area_and_centroid.second, 2, z) *
        area_and_centroid.first * thickness;
}

void static
fold_in_above(double& volume, vector3d& centroid,
    std::pair<double,vector2d> const& area_and_centroid,
    double z, double thickness)
{
    fold_in(volume, centroid, area_and_centroid, z + thickness / 2, thickness);
}

void static
fold_in_below(double& volume, vector3d& centroid,
    std::pair<double,vector2d> const& area_and_centroid,
    double z, double thickness)
{
    fold_in(volume, centroid, area_and_centroid, z - thickness / 2, thickness);
}

std::pair<double,vector3d> static
get_volume_and_centroid(structure_geometry const& structure)
{
    auto volume = 0.;
    auto centroid = make_vector(0., 0., 0.);

    auto const& masters = structure.master_slice_list;
    auto const& slices = structure.slices;

    auto begin_m = masters.begin();
    auto end_m = masters.end();
    auto i = begin_m;
    auto end_slice = slices.end();

    while (i != end_m)
    {
        auto slice = slices.find(i->position);
        if (slice != end_slice && get_area(slice->second) != 0.)
        {
            break;
        }
        ++i;
    }

    for (; i != end_m; ++i)
    {
        auto slice = slices.find(i->position);
        if (slice == end_slice) { continue; }

        auto i_info = get_area_and_centroid(slice->second);

        // Add lower half of slice thickness
        auto temp = i;
        if (i == begin_m)
        {
            fold_in_below(volume, centroid, i_info, i->position, i->thickness * 0.5);
        }
        else
        {
            --temp;
            fold_in_below(
                volume, centroid,
                i_info,
                i->position,
                0.5 * (i->position - temp->position));
        }

        // Add upper half of slice thickness
        temp = i;
        ++temp;
        if (temp == end_m)
        {
            fold_in_above(volume, centroid, i_info, i->position, i->thickness * 0.5);
        }
        else
        {
            fold_in_above(
                volume, centroid,
                i_info,
                i->position,
                0.5 * (temp->position - i->position));
        }
    }

    if (volume != 0.)
    {
        centroid /= volume;
    }

    return std::make_pair(volume, centroid);
}

double get_volume(structure_geometry const& structure)
{
    return get_volume_and_centroid(structure).first;
}

vector3d get_centroid(
    structure_geometry const& structure)
{
    auto pair = get_volume_and_centroid(structure);
    if (pair.first == 0)
        throw exception("centroid requested for empty structure");
    return pair.second;
}

bool is_inside(structure_geometry const& structure, vector<3,double> const& p)
{
    auto s = get_slice(structure, p[2]);
    return is_inside(s, slice(p, 2));
}

slice_description_list
get_slice_descriptions(structure_geometry const& s)
{
    return s.master_slice_list;
}

bool
almost_equal(
    structure_geometry const& volume1,
    structure_geometry const& volume2,
    double tolerance)
{
    if (volume1.master_slice_list.size() != volume2.master_slice_list.size())
        return false;

    auto i1 = volume1.master_slice_list.begin();
    auto end1 = volume1.master_slice_list.end();
    auto i2 = volume2.master_slice_list.begin();

    auto const& slices1 = volume1.slices;
    auto const& slices2 = volume2.slices;

    for (; i1 != end1; ++i1, ++i2)
    {
        if (!almost_equal(i1->position, i2->position, tolerance) ||
            !almost_equal(i1->thickness, i2->thickness, tolerance))
        {
            return false;
        }
        auto r1 = find_slice_at_exact_position(slices1, i1->position, i1->thickness);
        auto r2 = find_slice_at_exact_position(slices2, i2->position, i2->thickness);
        if (!almost_equal(r1.region, r2.region, tolerance))
        {
            return false;
        }
    }

    return true;
}

bool almost_equal(structure_geometry const& a, structure_geometry const& b)
{
    return almost_equal(a, b, default_equality_tolerance<double>());
}

void do_set_operation(
    structure_geometry* result,
    set_operation op,
    structure_geometry const& structure1,
    structure_geometry const& structure2)
{
    if (structure1.master_slice_list != structure2.master_slice_list)
    {
        throw exception("structure set_operation requires matching master lists");
    }

    auto const& master_slices = structure1.master_slice_list;

    result->master_slice_list = master_slices;
    result->slices.clear();
    for (auto const& i : master_slices)
    {
        auto s1 =
            find_slice_at_exact_position(structure1.slices, i.position, i.thickness);
        auto s2 =
            find_slice_at_exact_position(structure2.slices, i.position, i.thickness);

        if (!is_empty(s1.region) || !is_empty(s2.region))
        {
            polyset region;
            do_set_operation(&region, op, s1.region, s2.region);
            if (!is_empty(region))
            {
                result->slices[i.position] = region;
            }
        }
    }
}

structure_geometry
structure_combination(
    set_operation op,
    std::vector<structure_geometry> const& structures)
{
    if (structures.size() < 2)
    {
        throw exception("structure_combination requires at least two structures");
    }
    structure_geometry tmp[2];
    int output_index = 0;
    structure_geometry const* input = &structures.front();
    auto i = structures.begin();
    ++i;
    for (; i != structures.end(); ++i)
    {
        output_index = 1 - output_index;
        auto output = &tmp[output_index];
        do_set_operation(output, op, *input, *i);
        input = output;
    }
    return tmp[output_index];
}

std::vector<double>
slice_position_list(structure_geometry const& structure)
{
    return map(
        [](slice_description const& slice)
        {
            return slice.position;
        },
        structure.master_slice_list);
}

void expand_in_2d(
    structure_geometry* result,
    structure_geometry const& structure,
    double amount)
{
    auto slice_descriptions = get_slice_descriptions(structure);
    reset_structure_to_slice_list(result, slice_descriptions);
    for (auto const& slice : structure.slices)
    {
        polyset p;
        expand(&p, slice.second, amount);
        result->slices[slice.first] = p;
    }
}

box<2,double>
bounding_box(polygon2 const& poly)
{
    optional<box<2,double> > box;
    compute_bounding_box(box, poly);
    // If there is no box, then the polygon is empty, so just return any box.
    return box ? box.get() :
        make_box(make_vector(0., 0.), make_vector(0., 0.));
}
void compute_bounding_box(
    optional<box<2,double> >& box,
    polygon2 const& poly)
{
    vertex2_array::const_iterator
        i = poly.vertices.begin(),
        end = poly.vertices.end();
    if (i == end)
        return;

    vector<2,double> min, max;
    if (box)
    {
        min = get_low_corner(box.get());
        max = get_high_corner(box.get());
    }
    else
    {
        min = *i;
        max = *i;
        ++i;
    }

    for (; i != end; ++i)
    {
        for (unsigned j = 0; j != 2; ++j)
        {
            if ((*i)[j] < min[j])
                min[j] = (*i)[j];
            if ((*i)[j] > max[j])
                max[j] = (*i)[j];
        }
    }

    box = cradle::box<2,double>(min, max - min);
}

// for a polyset
box<2,double> bounding_box(polyset const& region)
{
    optional<box<2,double> > box;
    compute_bounding_box(box, region);
    // If there is no box, then the polyset is empty, so just return any box.
    return box ? box.get() :
        make_box(make_vector(0., 0.), make_vector(0., 0.));
}
void compute_bounding_box(optional<box<2,double> >& box, polyset const& region)
{
    for (auto const& i : region.polygons)
        compute_bounding_box(box, i);
}

// for a structure_geometry
box<3,double> bounding_box(structure_geometry const& structure)
{
    optional<box<3,double> > box;
    compute_bounding_box(box, structure);
    // If there is no box, then the structure is empty, so just return any box.
    return box ? box.get() :
        make_box(make_vector(0., 0., 0.), make_vector(0., 0., 0.));
}
void compute_bounding_box(optional<box<3,double> >& box,
    structure_geometry const& structure)
{
    optional<cradle::box<2,double> > xy_box;
    if (box)
    {
        xy_box = slice(box.get(), 2);
    }

    double zmin = 1.0e100;
    double zmax = -1.0e100;
    auto master_slices = get_slice_descriptions(structure);
    for (auto const& m_slice : master_slices)
    {
        auto slice =
            find_slice_at_exact_position(
                structure.slices,
                m_slice.position,
                m_slice.thickness);
        if (!is_empty(slice.region))
        {
            compute_bounding_box(xy_box, slice.region);

            if (slice.position - 0.5 * slice.thickness < zmin)
            {
                zmin = slice.position - 0.5 * slice.thickness;
            }
            if (slice.position + 0.5 * slice.thickness > zmax)
            {
                zmax = slice.position + 0.5 * slice.thickness;
            }
        }
    }

    if (!structure.slices.empty() && xy_box)
    {
        //double z_min = structure.slices.begin()->position -
        //    structure.slices.begin()->thickness / 2;
        //auto last = structure.slices.end();
        //--last;
        //double z_max = last->position + last->thickness / 2;

        vector<2,double> const& xy_corner = xy_box.get().corner;
        vector<2,double> const& xy_size = xy_box.get().size;

        box = cradle::box<3,double>(
            make_vector(xy_corner[0], xy_corner[1], zmin),
            make_vector(xy_size[0], xy_size[1], zmax - zmin));
    }
}

bool
overlapping(
    box3d const& box,
    structure_geometry const& sg,
    unsigned structure_axis,
    optional<box3d> const& sg_bounds)
{
    if (sg_bounds)
    {
        if (!overlapping(box, get(sg_bounds)))
        {
            return false;
        }
    }

    // Get slices that the box contains
    auto slices =
        get_structure_slices(
            sg,
            box.corner[structure_axis],
            get_high_corner(box)[structure_axis]);

    // Check for overlap on each slice
    for (auto const& s : slices)
    {
        if (s.region.polygons.size() == 0)
        {
            continue;
        }

        auto const box2 = slice(box, structure_axis);

        // Check center of voxel as this may be a fast "short-circuit" for many cases
        if (point_in_polyset(get_center(box2), s.region))
        {
            return true;
        }
        else
        {
            // Check all points in the polyset
            // Note this isn't 100% accurate because holes can actually make a polygon
            // vertex outside and we don't catch that case
            for (auto const& p : s.region.polygons)
            {
                for (auto const& v : p.vertices)
                {
                    if (contains(box2, v))
                    {
                        return true;
                    }
                }
            }
        }
    }

    return false;
}

bool almost_equal(polygon2 const& a, polygon2 const& b, double tolerance)
{
    polyset region_a, region_b, xor_region;
    create_polyset(&region_a, a);
    create_polyset(&region_b, b);
    do_set_operation(&xor_region, set_operation::XOR, region_a, region_b);
    return almost_equal(get_area(xor_region), 0., tolerance);
}
bool almost_equal(polygon2 const& a, polygon2 const& b)
{
    polyset region_a, region_b, xor_region;
    create_polyset(&region_a, a);
    create_polyset(&region_b, b);
    do_set_operation(&xor_region, set_operation::XOR, region_a, region_b);
    return almost_equal(get_area(xor_region), 0.);
}

}


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

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

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_CSCD_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_CSCD_HPP_INCLUDED

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

 /*!

    @ingroup group-trigonometric
    Function object implementing cscd capabilities

    cosecante in degree.

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = cscd(x);
    @endcode

    is similar to:

    @code
    T r = rec(sind(x));
    @endcode

    As most other trigonometric function cscd can be called with a second optional parameter
    which is a tag on speed and accuracy (see @ref cos for further details)

    @see csc, cscpi,

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

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

#endif


/**
 * Test cases for functions in utils::stds.
 *
 */

#include "utils/random.h"

#ifndef LINK_STATIC
#define BOOST_TEST_DYN_LINK
#endif
#define BOOST_TEST_MODULE LinspaceTest
#include <boost/test/unit_test.hpp>

#include <iostream>
#include <random>
#include <map>
#include <string>
#include <iomanip>

using namespace utils::random;
using namespace std;

BOOST_AUTO_TEST_CASE(linspace_with_count_generates_int_vector)
{
    size_t N = 0;
    double min = 0, max = 9;

    auto vec = utils::stds::linspace(min, max, N);
    BOOST_REQUIRE_EQUAL(vec.size(), N);

    N = 1;
    vec = utils::stds::linspace(min, max, N);
    BOOST_REQUIRE_EQUAL(vec.size(), N);
    BOOST_CHECK_EQUAL(vec[0], min);

    N = 2;
    vec = utils::stds::linspace(min, max, N);
    BOOST_REQUIRE_EQUAL(vec.size(), N);
    BOOST_CHECK_EQUAL(vec[0], min);
    BOOST_CHECK_EQUAL(vec[1], max);

    N = 10;
    vec = utils::stds::linspace(min, max, N);
    BOOST_REQUIRE_EQUAL(vec.size(), N);
    BOOST_CHECK_EQUAL(vec[0], min);
    BOOST_CHECK_EQUAL(vec[4], 4);
    BOOST_CHECK_EQUAL(vec[5], 5);
    BOOST_CHECK_EQUAL(vec[N-1], max);

}

BOOST_AUTO_TEST_CASE(linspace_with_delta_generates_int_vector)
{
    double min, max, delta;

    min = 0, max = 0, delta = 0;
    auto vec = utils::stds::linspace(min, max, delta);
    BOOST_REQUIRE_EQUAL(vec.size(), 0);

    min = 1, max = 1, delta = 1;
    vec = utils::stds::linspace(min, max, delta);
    BOOST_REQUIRE_EQUAL(vec.size(), 1);
    BOOST_CHECK_EQUAL(vec[0], min);

    min = 0, max = 1, delta = 1;
    vec = utils::stds::linspace(min, max, delta);
    BOOST_REQUIRE_EQUAL(vec.size(), 2);
    BOOST_CHECK_EQUAL(vec[0], min);
    BOOST_CHECK_EQUAL(vec[1], max);

    min = 0, max = 9, delta = 1;
    vec = utils::stds::linspace(min, max, delta);
    BOOST_REQUIRE_EQUAL(vec.size(), 10);
    BOOST_CHECK_EQUAL(vec[0], min);
    BOOST_CHECK_EQUAL(vec[4], 4);
    BOOST_CHECK_EQUAL(vec[5], 5);
    BOOST_CHECK_EQUAL(vec[9], max);

}


/*
* Bundle adjustment using ceres
*/

//#define V3DLIB_ENABLE_SUITESPARSE
#include "BAHandler.h"

// #include "ba/v3d_linear.h"
// #include "ba/v3d_vrmlio.h"
// #include "ba/v3d_metricbundle.h"
// #include "ba/v3d_stereobundle.h"

#include "Camera.h"
// #include "ceres/EigenQuaternionParameterization.h"
#include "datastructs/Data.h"

#include <opencv2/features2d/features2d.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <ceres/rotation.h>

#include <Eigen/Eigen>

// using namespace V3D;
using namespace std;
using namespace cv;

// namespace
// {
// 	inline double
// 	showErrorStatistics(double const f0,
// 						StdDistortionFunction const& distortion,
// 						vector<CameraMatrix> const& cams,
// 						vector<Vector3d> const& Xs,
// 						vector<Vector2d> const& measurements,
// 						vector<int> const& correspondingView,
// 						vector<int> const& correspondingPoint)
// 	{
// 		int const K = measurements.size();

// 		double meanReprojectionError = 0.0;
// 		for (int k = 0; k < K; ++k)
// 		{
// 			int const i = correspondingView[k];
// 			int const j = correspondingPoint[k];
// 			Vector2d p = cams[i].projectPoint(distortion, Xs[j]);

// 			double reprojectionError = norm_L2(f0 * (p - measurements[k]));
// 			meanReprojectionError += reprojectionError;
// 		}
// 		//cout << "mean reprojection error (in pixels): " << meanReprojectionError/K << endl;

// 		return meanReprojectionError/K;
// 	}
// }

// void BAHandler::adjustBundle(){

// 	cout<<"Bundle Adjust Method: no copy"<<endl;
// 	Data 		&data 	= Data::GetInstance();
// 	Camera 		&camera	= Camera::GetInstance();
// 	//important
// 	data.deleteTrashes();

// 	Mat &cam_matrix 				= camera.camMat;
// 	Mat &distortion_coefficients	= camera.distortionMat;

// 	vector<int>			imgIdxs;
// 	vector<int>			pt3DIdxs;
// 	double ppx,ppy;
// 	double 	meanErrorBefore, meanErrorAfter;

// 	int N = data.countFrames(), M = data.countLandMarks(), K = data.countMeasurements();
// 	cout << "N (cams) = " << N << " M (points) = " << M << " K (measurements) = " << K << endl;

// 	StdDistortionFunction distortion;
// 	distortion.k1 = distortion_coefficients.at<double>(0,0);
// 	distortion.k2 = distortion_coefficients.at<double>(0,1);
// 	distortion.p1 = distortion_coefficients.at<double>(0,2);
// 	distortion.p2 = distortion_coefficients.at<double>(0,3);

// 	//convert camera intrinsics to BA datastructs
// 	Matrix3x3d KMat;
// 	makeIdentityMatrix(KMat);
// 	KMat[0][0] = cam_matrix.at<double>(0,0); //fx
// 	KMat[1][1] = cam_matrix.at<double>(1,1); //fy
// 	KMat[0][1] = cam_matrix.at<double>(0,1); //skew
// 	KMat[0][2] = cam_matrix.at<double>(0,2); //ppx
// 	KMat[1][2] = cam_matrix.at<double>(1,2); //ppy

// 	ppx = KMat[0][2];
// 	ppy = KMat[1][2];

// 	double const f0 = KMat[0][0];
// 	double const f0_inv = 1.0/f0;

// 	//cout << "Cam = "<<endl;
// 	//displayMatrix(KMat);

// 	Matrix3x3d Knorm = KMat;
// 	// Normalize the intrinsic to have unit focal length.
// 	scaleMatrixIP(f0_inv, Knorm);
// 	Knorm[2][2] = 1.0;

// 	//convert 3D point cloud to BA datastructs
// 	const vector<LandMark::Ptr> &lms = data.getLandMarks();
// 	map<LandMark::Ptr, int, Data::LandMarkPtrCompare> lm2idx;
// 	vector<Vector3d > Xs;
// 	int idx = 0;
// 	Xs.reserve(lms.size());
// 	for (vector<LandMark::Ptr>::const_iterator it = lms.begin(); it!=lms.end(); ++it, ++idx)
// 	{
// 		assert(!(*it)->deleted);
// 		Xs.push_back(Vector3d((*it)->pt[0], (*it)->pt[1], (*it)->pt[2]));
// 		lm2idx[*it] = idx;
// 	}

// 	//convert cameras to BA datastructs
// 	const vector<Frame::Ptr> &frames = data.getFrames();
// 	map<Frame::Ptr, int, Data::FramePtrCompare> frame2idx;
// 	vector<CameraMatrix> cams;
// 	cams.reserve(frames.size());
// 	idx = 0;
// 	for (vector<Frame::Ptr>::const_iterator it = frames.begin(); it!=frames.end(); ++it, ++idx)
// 	{
// 		Matrix3x3d R;
// 		Vector3d T;

// 		Matx34d P = (*it)->getCVTransform();

// 		R[0][0] = P(0,0); R[0][1] = P(0,1); R[0][2] = P(0,2); T[0] = P(0,3);
// 		R[1][0] = P(1,0); R[1][1] = P(1,1); R[1][2] = P(1,2); T[1] = P(1,3);
// 		R[2][0] = P(2,0); R[2][1] = P(2,1); R[2][2] = P(2,2); T[2] = P(2,3);

// 		CameraMatrix cam;
// 		cam.setIntrinsic(Knorm);
// 		cam.setRotation(R);
// 		cam.setTranslation(T);
// 		cams.push_back(cam);
// 		frame2idx[*it] = idx;
// 	}

// 	const vector<Measurement::Ptr> &ms = data.getMeasurements();
// 	vector<Vector2d > measurements;
// 	vector<int> correspondingView; 		//corresponding camera mat idx
// 	vector<int> correspondingPoint;		//corresponding 3d point idx
// 	measurements.reserve(K);
// 	correspondingView.reserve(K);
// 	correspondingPoint.reserve(K);

// 	//convert 2D measurements to BA datastructs
// 	for (vector<Measurement::Ptr>::const_iterator it = ms.begin(); it!=ms.end(); ++it){
// 		assert(!(*it)->deleted);
// 		LandMark::Ptr &lm = (*it)->landmark;
// 		Frame::Ptr &frame = (*it)->frame;
// 		Point2f &xy		  = frame->kpts[(*it)->featureIdx].pt;
// 		measurements.push_back(Vector2d(xy.x*f0_inv, xy.y*f0_inv));
// 		correspondingView.push_back(frame2idx[frame]);
// 		correspondingPoint.push_back(lm2idx[lm]);
// 	}

// 	meanErrorBefore = showErrorStatistics(f0, distortion, cams, Xs, measurements, correspondingView, correspondingPoint);

// 	double const inlierThreshold = 2.0 / fabs(f0);

// 	Matrix3x3d K0 = cams[0].getIntrinsic();
// 	//cout << "K0 = "; displayMatrix(K0);

// 	bool good_adjustment = false;
// 	{
// 		ScopedBundleExtrinsicNormalizer extNorm(cams, Xs);
// 		ScopedBundleIntrinsicNormalizer intNorm(cams,measurements,correspondingView);
// 		CommonInternalsMetricBundleOptimizer opt(V3D::FULL_BUNDLE_FOCAL_LENGTH_PP, inlierThreshold, K0, distortion, cams, Xs,
// 												 measurements, correspondingView, correspondingPoint);

// 		opt.tau = 1e-3;
// 		opt.maxIterations = 50;
// 		opt.minimize();

// 		cout << "optimizer status = " << opt.status << endl;

// 		good_adjustment = (opt.status != 2);
// 	}

// 	//cout << "refined K = "; displayMatrix(K0);

// 	for (int i = 0; i < N; ++i) cams[i].setIntrinsic(K0);

// 	Matrix3x3d Knew = K0;
// 	scaleMatrixIP(f0, Knew);
// 	Knew[2][2] = 1.0;

// 	//cout << "Cam new = "<<endl;
// 	//displayMatrix(Knew);

// 	meanErrorAfter = showErrorStatistics(f0, distortion, cams, Xs, measurements, correspondingView, correspondingPoint);

// 	if(good_adjustment) {

// 		//extract 3D points
// 		for (unsigned int j = 0; j < Xs.size(); ++j)
// 		{
// 			lms[j]->pt[0] = Xs[j][0];
// 			lms[j]->pt[1] = Xs[j][1];
// 			lms[j]->pt[2] = Xs[j][2];
// 		}

// 		//extract adjusted cameras
// 		for (unsigned int i = 0; i < N; ++i)
// 		{
// 			Matrix3x3d R = cams[i].getRotation();
// 			Vector3d T = cams[i].getTranslation();

// 			Eigen::Matrix3d R_eigen;
// 			Eigen::Vector3d t_eigen;
// 			for(unsigned int r = 0; r<3; r++){
// 				for(unsigned int c = 0; c<3; c++){
// 					R_eigen(r,c) = R[r][c];
// 				}
// 				t_eigen(r) = T[r];
// 			}
// 			frames[i]->rotation = Eigen::Quaterniond(R_eigen);
// 			frames[i]->position = -R_eigen.transpose()*t_eigen;
// 		}

// 		cam_matrix.at<double>(0,0) = Knew[0][0];
// 		cam_matrix.at<double>(0,1) = Knew[0][1];
// 		cam_matrix.at<double>(0,2) = Knew[0][2];
// 		cam_matrix.at<double>(1,1) = Knew[1][1];
// 		cam_matrix.at<double>(1,2) = Knew[1][2];
// 	}

// 	cout<<"focal :"<<f0<<" -> "<<Knew[0][0]<<endl;
// 	cout<<"center:("<<ppx<<","<<ppy<<") -> ("<<Knew[0][2]<<","<<Knew[1][2]<<")"<<endl;
// 	cout<<"error :"<<meanErrorBefore<<" -> "<<meanErrorAfter<<endl;
// }

struct ReprojectionError {
	// (u, v): the position of the observation with respect to the image top left corner
	  // u is rightward positive and v is downward positive
	ReprojectionError(double observed_x, double observed_y, double f, double ppx, double ppy)
	:observed_x(observed_x)
	,observed_y(observed_y)
	,f(f)
	,ppx(ppx)
	,ppy(ppy)
	{

	}

	  template <typename T>
	  bool operator()(const T* const camera_rotation,
			  	  	  const T* const camera_translation,
	                  const T* const point,
	                  T* residuals) const {

		  //XXX: quaternion must be normalized before!
		  // Map the T* array to an Eigen Quaternion (no copy)
		  Eigen::Quaternion<T> q = Eigen::Map<const Eigen::Quaternion<T>>(camera_rotation);

		  // Map T* to Eigen Vector3 (no copy)
		  Eigen::Matrix<T,3,1> t = Eigen::Map<const Eigen::Matrix<T,3,1>>(camera_translation);

		  //copy point data
		  Eigen::Matrix<T,3,1> p;
		  p << T(point[0]), T(point[1]), T(point[2]);

		  //transform point to camera view space
		  p = q*(p-t);	//q is rotation of camera back to world axis, t is position of camera in world (opengv convention)
		  //p = q*p+t		//q is rotation of camera back to world axis, t is translation of camera before rotation (opencv convention)

	    // Compute the center of distortion. The sign change comes from
	    // the camera model that Noah Snavely's Bundler assumes, whereby
	    // the camera coordinate system has a negative z axis.
	    const T xp = p[0] / p[2];
	    const T yp = p[1] / p[2];

	    // Compute final projected point position.
	    const T predicted_x = f * xp+ppx;
	    const T predicted_y = f * yp+ppy;
	    //const T predicted_x = -f * xp+ppx;
	    //const T predicted_y = f * yp+ppy;

	    // The error is the difference between the predicted and observed position.
	    residuals[0] = predicted_x - observed_x;
	    residuals[1] = predicted_y - observed_y;

	    return true;
	  }

	  // Factory to hide the construction of the CostFunction object from
	  // the client code.
	  static ceres::CostFunction* Create(const double observed_x,
	                                     const double observed_y,
										 const double f,
										 const double ppx,
										 const double ppy) {
	    return (new ceres::AutoDiffCostFunction<
	    		ReprojectionError, 2, 4, 3, 3>(
	                new ReprojectionError(observed_x,
	                                      observed_y,
										  f,
										  ppx,
										  ppy)));
	  }

	  double observed_x;
	  double observed_y;
	  //since we fix camera paramters, use them as residule rather than parameter
	  //this avoids unnecessary jacobian calculation which speeds up a lot
	  double f;
	  double ppx;
	  double ppy;
};

// struct OctaveAwareReprojectionError {
// 	// (u, v): the position of the observation with respect to the image top left corner
// 	  // u is rightward positive and v is downward positive
// 	OctaveAwareReprojectionError(double observed_x, double observed_y, double f, double ppx, double ppy, double octave)
// 	:observed_x(observed_x)
// 	,observed_y(observed_y)
// 	,f(f)
// 	,ppx(ppx)
// 	,ppy(ppy)
// 	,octave(octave)
// 	{

// 	}

// 	  template <typename T>
// 	  bool operator()(const T* const camera_rotation,
// 			  	  	  const T* const camera_translation,
// 	                  const T* const point,
// 	                  T* residuals) const {

// 		  //XXX: quaternion must be normalized before!
// 		  // Map the T* array to an Eigen Quaternion (no copy)
// 		  Eigen::Quaternion<T> q = Eigen::Map<const Eigen::Quaternion<T>>(camera_rotation);

// 		  // Map T* to Eigen Vector3 (no copy)
// 		  Eigen::Matrix<T,3,1> t = Eigen::Map<const Eigen::Matrix<T,3,1>>(camera_translation);

// 		  //copy point data
// 		  Eigen::Matrix<T,3,1> p;
// 		  p << T(point[0]), T(point[1]), T(point[2]);

// 		  //transform point to camera view space
// 		  p = q*(p-t);	//q is rotation of camera back to world axis, t is position of camera in world (opengv convention)
// 		  //p = q*p+t		//q is rotation of camera back to world axis, t is translation of camera before rotation (opencv convention)

// 	    // Compute the center of distortion. The sign change comes from
// 	    // the camera model that Noah Snavely's Bundler assumes, whereby
// 	    // the camera coordinate system has a negative z axis.
// 	    const T xp = p[0] / p[2];
// 	    const T yp = p[1] / p[2];

// 	    // Compute final projected point position.
// 	    const T predicted_x = f * xp+ppx;
// 	    const T predicted_y = f * yp+ppy;
// 	    //const T predicted_x = -f * xp+ppx;
// 	    //const T predicted_y = f * yp+ppy;

// 	    // The error is the difference between the predicted and observed position.
// 	    residuals[0] = (predicted_x - observed_x)/octave;
// 	    residuals[1] = (predicted_y - observed_y)/octave;

// 	    return true;
// 	  }

// 	  // Factory to hide the construction of the CostFunction object from
// 	  // the client code.
// 	  static ceres::CostFunction* Create(const double observed_x,
// 	                                     const double observed_y,
// 										 const double f,
// 										 const double ppx,
// 										 const double ppy,
// 										 const double octave) {
// 	    return (new ceres::AutoDiffCostFunction<
// 	    		OctaveAwareReprojectionError, 2, 4, 3, 3>(
// 	                new OctaveAwareReprojectionError(observed_x,
// 	                                      observed_y,
// 										  f,
// 										  ppx,
// 										  ppy,
// 										  octave)));
// 	  }

// 	  double observed_x;
// 	  double observed_y;
// 	  //since we fix camera paramters, use them as residule rather than parameter
// 	  //this avoids unnecessary jacobian calculation which speeds up a lot
// 	  double f;
// 	  double ppx;
// 	  double ppy;
// 	  double octave;
// };

void BAHandler::adjustBundle_ceres_nocopy(){
	cout<<"Bundle Adjust Method: no copy"<<endl;

	clock_t				time;
	double				t_preprocess;

	time				= clock();
	Data 		&data 	= Data::GetInstance();
	Camera 		&camera	= Camera::GetInstance();
	//important
	data.deleteTrashes();

	double intrinsics[3];
	intrinsics[0] = camera.getCamFocal();
	intrinsics[1] = camera.getCamPrinciple().x;
	intrinsics[2] = camera.getCamPrinciple().y;

	ceres::Problem problem;
	ceres::LocalParameterization *eigenQuaternionParameterization = new ceres::EigenQuaternionParameterization;

	const vector<Measurement::Ptr> &ms = data.getMeasurements();
	for(vector<Measurement::Ptr>::const_iterator it = ms.begin(); it!=ms.end(); ++it){

		assert(!((*it)->deleted));

		Frame::Ptr 		&f 	= (*it)->frame;
		LandMark::Ptr 	&p 	= (*it)->landmark;
		int			&feIdx	= (*it)->featureIdx;

		Point2f &pt2D 	= f->kpts[feIdx].pt;
		double *q		= f->rotation.coeffs().data();
		double *t		= f->position.data();
		double *point	= p->pt.data();

		ceres::CostFunction* cost_function = ReprojectionError::Create( pt2D.x, pt2D.y, intrinsics[0], intrinsics[1], intrinsics[2]);
		problem.AddResidualBlock(cost_function, NULL, q, t, point);
		problem.SetParameterization(q,eigenQuaternionParameterization);
		if(f->fixed){
			//fix first camera transformation
			problem.SetParameterBlockConstant(q);
			problem.SetParameterBlockConstant(t);
		}
	}

	t_preprocess = double(clock()-time) / CLOCKS_PER_SEC;

	// Set a few options
	ceres::Solver::Options options;
	options.use_nonmonotonic_steps = true;
	options.preconditioner_type = ceres::SCHUR_JACOBI;
	options.linear_solver_type = ceres::SPARSE_SCHUR; //ceres::ITERATIVE_SCHUR; //
	options.max_num_iterations = 100;
	//options.max_solver_time_in_seconds = 0.015;

	ceres::Solver::Summary summary;
	ceres::Solve(options, &problem, &summary);

	//std::cout << "Final report:\n" << summary.FullReport();
	std::cout << "Time(s) preprocess :"<<t_preprocess<<endl;
	std::cout << "Time(s) BA :"<<summary.total_time_in_seconds<<endl;

}

void BAHandler::adjustBundle_ceres_local_nocopy(){

	cout<<"Bundle Adjust Method: local no copy"<<endl;

	clock_t				time;
	double				t_preprocess;

	time				= clock();
	Data 		&data 	= Data::GetInstance();
	Camera 		&camera	= Camera::GetInstance();
	//important
	data.deleteTrashes();

	double intrinsics[3];
	intrinsics[0] = camera.getCamFocal();
	intrinsics[1] = camera.getCamPrinciple().x;
	intrinsics[2] = camera.getCamPrinciple().y;

	ceres::Problem problem;
	ceres::LocalParameterization *eigenQuaternionParameterization = new ceres::EigenQuaternionParameterization;

	if(data.countFrames()<1) return;
	//fix all frames other than the last frame
	data.fixAllFrames();
	Frame::Ptr lastAddedFrame = data.getFrames().back();
	lastAddedFrame -> fixed = false;

	//for all measures in last added frame
	const vector<Measurement::Ptr> ms = data.getMeasurements(lastAddedFrame);
	for(vector<Measurement::Ptr>::const_iterator it = ms.begin(); it!=ms.end(); ++it){
		LandMark::Ptr &lmk = (*it)->landmark;
		//for all measurements tied to this landmark
		vector<Measurement::Ptr> lmkms = data.getMeasurements(lmk);
		for(vector<Measurement::Ptr>::const_iterator jt = lmkms.begin(); jt!=lmkms.end(); ++jt){
			assert(!((*jt)->deleted));

			Frame::Ptr 		&f 	= (*jt)->frame;
			LandMark::Ptr 	&p 	= (*jt)->landmark;
			int			&feIdx	= (*jt)->featureIdx;

			Point2f &pt2D 	= f->kpts[feIdx].pt;
			double *q		= f->rotation.coeffs().data();
			double *t		= f->position.data();
			double *point	= p->pt.data();

			ceres::CostFunction* cost_function = ReprojectionError::Create( pt2D.x, pt2D.y, intrinsics[0], intrinsics[1], intrinsics[2]);
			problem.AddResidualBlock(cost_function, NULL, q, t, point);
			problem.SetParameterization(q,eigenQuaternionParameterization);
			if(f->fixed){
				problem.SetParameterBlockConstant(q);
				problem.SetParameterBlockConstant(t);
			}
		}
	}

	t_preprocess = double(clock()-time) / CLOCKS_PER_SEC;

	// Set a few options
	ceres::Solver::Options options;
	options.use_nonmonotonic_steps = true;
	options.preconditioner_type = ceres::SCHUR_JACOBI;
	options.linear_solver_type = ceres::SPARSE_SCHUR; //ceres::ITERATIVE_SCHUR; //
	options.max_num_iterations = 100;
	//options.max_solver_time_in_seconds = 0.015;

	ceres::Solver::Summary summary;
	ceres::Solve(options, &problem, &summary);

	//std::cout << "Final report:\n" << summary.FullReport();
	std::cout << "Time(s) preprocess :"<<t_preprocess<<endl;
	std::cout << "Time(s) BA :"<<summary.total_time_in_seconds<<endl;
}

void BAHandler::adjustBundle_ceres_local_nocopy(vector<Measurement::Ptr> &ms){

	cout<<"Bundle Adjust Method: local no copy"<<endl;

	clock_t				time;
	double				t_preprocess;

	time				= clock();
	Data 		&data 	= Data::GetInstance();
	Camera 		&camera	= Camera::GetInstance();
	//important
	data.deleteTrashes();

	double intrinsics[3];
	intrinsics[0] = camera.getCamFocal();
	intrinsics[1] = camera.getCamPrinciple().x;
	intrinsics[2] = camera.getCamPrinciple().y;

	ceres::Problem problem;
	ceres::LocalParameterization *eigenQuaternionParameterization = new ceres::EigenQuaternionParameterization;

	if(data.countFrames()<1) return;
	if(ms.empty()) return;
	//fix all frames, the provided measurements should all generate from a frame that is not in data
	data.fixAllFrames();
	//get measures already added to data if exists
	Frame::Ptr &mFrame = ms[0]->frame;
	//unfix this measure frame
	mFrame->fixed = false;

	//for all given measures
	for(vector<Measurement::Ptr>::const_iterator it = ms.begin(); it!=ms.end(); ++it){
		assert((*it)->frame.get() == mFrame.get());
		LandMark::Ptr &lmk = (*it)->landmark;
		//for all measurements tied to this landmark
		vector<Measurement::Ptr> lmkms = data.getMeasurements(lmk);
		for(vector<Measurement::Ptr>::iterator jt = lmkms.begin(); jt!=lmkms.end(); ++jt){
			assert(!((*jt)->deleted));

			Frame::Ptr 		&f 	= (*jt)->frame;
			LandMark::Ptr 	&p 	= (*jt)->landmark;
			int			&feIdx	= (*jt)->featureIdx;

			Point2f &pt2D 	= f->kpts[feIdx].pt;
			double *q		= f->rotation.coeffs().data();
			double *t		= f->position.data();
			double *point	= p->pt.data();

			ceres::CostFunction* cost_function = ReprojectionError::Create( pt2D.x, pt2D.y, intrinsics[0], intrinsics[1], intrinsics[2]);
			problem.AddResidualBlock(cost_function, NULL, q, t, point);
			problem.SetParameterization(q,eigenQuaternionParameterization);
			if(f->fixed){
				problem.SetParameterBlockConstant(q);
				problem.SetParameterBlockConstant(t);
			}
		}
	}

	//if mFrame was already added to data, we add these measures as well for bundle adjustment
	vector<Measurement::Ptr> dataMs = data.getMeasurements(mFrame);
	for(vector<Measurement::Ptr>::const_iterator it = dataMs.begin(); it!=dataMs.end(); ++it){
		assert((*it)->frame.get() == mFrame.get());
		LandMark::Ptr &lmk = (*it)->landmark;
		//for all measurements tied to this landmark
		vector<Measurement::Ptr> lmkms = data.getMeasurements(lmk);
		for(vector<Measurement::Ptr>::iterator jt = lmkms.begin(); jt!=lmkms.end(); ++jt){
			assert(!((*jt)->deleted));

			Frame::Ptr 		&f 	= (*jt)->frame;
			LandMark::Ptr 	&p 	= (*jt)->landmark;
			int			&feIdx	= (*jt)->featureIdx;

			Point2f &pt2D 	= f->kpts[feIdx].pt;
			double *q		= f->rotation.coeffs().data();
			double *t		= f->position.data();
			double *point	= p->pt.data();

			ceres::CostFunction* cost_function = ReprojectionError::Create( pt2D.x, pt2D.y, intrinsics[0], intrinsics[1], intrinsics[2]);
			problem.AddResidualBlock(cost_function, NULL, q, t, point);
			problem.SetParameterization(q,eigenQuaternionParameterization);
			if(f->fixed){
				problem.SetParameterBlockConstant(q);
				problem.SetParameterBlockConstant(t);
			}
		}
	}

	t_preprocess = double(clock()-time) / CLOCKS_PER_SEC;

	// Set a few options
	ceres::Solver::Options options;
	options.use_nonmonotonic_steps = true;
	options.preconditioner_type = ceres::SCHUR_JACOBI;
	options.linear_solver_type = ceres::SPARSE_SCHUR; //ceres::ITERATIVE_SCHUR; //
	options.max_num_iterations = 100;
	//options.max_solver_time_in_seconds = 0.015;

	ceres::Solver::Summary summary;
	ceres::Solve(options, &problem, &summary);

	//std::cout << "Final report:\n" << summary.FullReport();
	std::cout << "Time(s) preprocess :"<<t_preprocess<<endl;
	std::cout << "Time(s) BA :"<<summary.total_time_in_seconds<<endl;
}

// void BAHandler::adjustBundle_ceres_local_fixPoints_nocopy(vector<Measurement::Ptr> &ms){

// 	cout<<"Bundle Adjust Method: local fix points no copy"<<endl;

// 	clock_t				time;
// 	double				t_preprocess;

// 	time				= clock();
// 	Data 		&data 	= Data::GetInstance();
// 	Camera 		&camera	= Camera::GetInstance();
// 	//important
// 	data.deleteTrashes();

// 	double intrinsics[3];
// 	intrinsics[0] = camera.getCamFocal();
// 	intrinsics[1] = camera.getCamPrinciple().x;
// 	intrinsics[2] = camera.getCamPrinciple().y;

// 	ceres::Problem problem;
// 	ceres::LocalParameterization *eigenQuaternionParameterization = new ceres::EigenQuaternionParameterization;

// 	if(data.countFrames()<1) return;
// 	if(ms.empty()) return;

// 	//for all given measures
// 	for(vector<Measurement::Ptr>::const_iterator it = ms.begin(); it!=ms.end(); ++it){

// 		Frame::Ptr 		&f 	= (*it)->frame;
// 		LandMark::Ptr 	&p 	= (*it)->landmark;
// 		int			&feIdx	= (*it)->featureIdx;
// 		double 		oct		= (double) (f->kpts[feIdx].octave + 1);

// 		Point2f &pt2D 	= f->kpts[feIdx].pt;
// 		double *q		= f->rotation.coeffs().data();
// 		double *t		= f->position.data();
// 		double *point	= p->pt.data();

// 		ceres::CostFunction* cost_function = OctaveAwareReprojectionError::Create( pt2D.x, pt2D.y, intrinsics[0], intrinsics[1], intrinsics[2], oct);
// 		problem.AddResidualBlock(cost_function, NULL, q, t, point);
// 		problem.SetParameterization(q,eigenQuaternionParameterization);
// 		problem.SetParameterBlockConstant(point);

// 	}

// /*

// 	//fix all frames, the provided measurements should all generate from a frame that is not in data
// 	data.fixAllFrames();
// 	//get measures already added to data if exists
// 	Frame::Ptr &mFrame = ms[0]->frame;
// 	//unfix this measure frame
// 	mFrame->fixed = false;

// 	//for all given measures
// 	for(vector<Measurement::Ptr>::const_iterator it = ms.begin(); it!=ms.end(); ++it){
// 		assert((*it)->frame.get() == mFrame.get());
// 		LandMark::Ptr &lmk = (*it)->landmark;
// 		//for all measurements tied to this landmark
// 		vector<Measurement::Ptr> lmkms = data.getMeasurements(lmk);
// 		for(vector<Measurement::Ptr>::iterator jt = lmkms.begin(); jt!=lmkms.end(); ++jt){
// 			assert(!((*jt)->deleted));

// 			Frame::Ptr 		&f 	= (*jt)->frame;
// 			LandMark::Ptr 	&p 	= (*jt)->landmark;
// 			int			&feIdx	= (*jt)->featureIdx;

// 			Point2f &pt2D 	= f->kpts[feIdx].pt;
// 			double *q		= f->rotation.coeffs().data();
// 			double *t		= f->position.data();
// 			double *point	= p->pt.data();

// 			ceres::CostFunction* cost_function = ReprojectionError::Create( pt2D.x, pt2D.y, intrinsics[0], intrinsics[1], intrinsics[2]);
// 			problem.AddResidualBlock(cost_function, NULL, q, t, point);
// 			problem.SetParameterization(q,eigenQuaternionParameterization);
// 			problem.SetParameterBlockConstant(point);
// 			if(f->fixed){
// 				problem.SetParameterBlockConstant(q);
// 				problem.SetParameterBlockConstant(t);
// 			}
// 		}
// 	}

// 	//if mFrame was already added to data, we add these measures as well for bundle adjustment
// 	vector<Measurement::Ptr> dataMs = data.getMeasurements(mFrame);
// 	for(vector<Measurement::Ptr>::const_iterator it = dataMs.begin(); it!=dataMs.end(); ++it){
// 		assert((*it)->frame.get() == mFrame.get());
// 		LandMark::Ptr &lmk = (*it)->landmark;
// 		//for all measurements tied to this landmark
// 		vector<Measurement::Ptr> lmkms = data.getMeasurements(lmk);
// 		for(vector<Measurement::Ptr>::iterator jt = lmkms.begin(); jt!=lmkms.end(); ++jt){
// 			assert(!((*jt)->deleted));

// 			Frame::Ptr 		&f 	= (*jt)->frame;
// 			LandMark::Ptr 	&p 	= (*jt)->landmark;
// 			int			&feIdx	= (*jt)->featureIdx;

// 			Point2f &pt2D 	= f->kpts[feIdx].pt;
// 			double *q		= f->rotation.coeffs().data();
// 			double *t		= f->position.data();
// 			double *point	= p->pt.data();

// 			ceres::CostFunction* cost_function = ReprojectionError::Create( pt2D.x, pt2D.y, intrinsics[0], intrinsics[1], intrinsics[2]);
// 			problem.AddResidualBlock(cost_function, NULL, q, t, point);
// 			problem.SetParameterization(q,eigenQuaternionParameterization);
// 			problem.SetParameterBlockConstant(point);
// 			if(f->fixed){
// 				problem.SetParameterBlockConstant(q);
// 				problem.SetParameterBlockConstant(t);
// 			}
// 		}
// 	}
// */

// 	t_preprocess = double(clock()-time) / CLOCKS_PER_SEC;

// 	// Set a few options
// 	ceres::Solver::Options options;
// 	options.use_nonmonotonic_steps = true;
// 	options.preconditioner_type = ceres::SCHUR_JACOBI;
// 	options.linear_solver_type = ceres::SPARSE_SCHUR; //ceres::ITERATIVE_SCHUR; //
// 	options.max_num_iterations = 100;
// 	//options.max_solver_time_in_seconds = 0.015;

// 	ceres::Solver::Summary summary;
// 	ceres::Solve(options, &problem, &summary);

// 	std::cout << "Final report:\n" << summary.FullReport();
// 	std::cout << "Time(s) preprocess :"<<t_preprocess<<endl;
// 	std::cout << "Time(s) BA :"<<summary.total_time_in_seconds<<endl;
// }


#include <boost/config.hpp>
#include <boost/program_options.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <iostream>
#include <random>
#include <sys/time.h>
#include <exception>

namespace po = boost::program_options;

static int SIZEX = 800;
static int SIZEY = 600;
static int ITER = 20;
static int RADIUS = 15;
static double TEMPERATURE = 2.0;
static double alpha = 0.05;
static int mouse_x, mouse_y;
static bool pressed = false;
static bool run_evolve = false;
static bool DEBUG = false;

inline double double_rand(const double & min, const double & max)
{
    static thread_local std::mt19937 generator;
    std::uniform_real_distribution<double> distribution(min,max);
    return distribution(generator);
}

void mouse_callback(int event, int x, int y, int flags, void *data)
{
    switch(event)
    {
        case CV_EVENT_MOUSEMOVE:
            mouse_x = x;
            mouse_y = y;

            // printf("(%d,%d)\n", x, y);
            break;
        case CV_EVENT_LBUTTONDOWN:
            pressed = true;
            break;
        case CV_EVENT_LBUTTONUP:
            pressed = false;
            break;
    }
}

void evolve(double **data, double **buffer)
{
    // ~ 62 bits of accuracy (as in of conservation only)
    int i,j;
    double dr = 1. / SIZEX;
    double dt = dr * dr;
    double *u = *data;
    double *next_state = *buffer;

    struct timeval start, end;
    gettimeofday(&start, NULL);

    for(int k = 0; k < ITER; k++)
    {
    #pragma omp parallel for private(i, j), shared(u, next_state, dt, dr, alpha)
        for(i=0; i < SIZEY; i++)
        {
            if(i == 0)
            {
                next_state[0]= u[0] + alpha * (
                    u[1] +
                    u[SIZEX] -
                    2*u[0]); // j == 0

                next_state[SIZEX - 1]= u[SIZEX - 1] + alpha * (
                    u[2 * SIZEX -1] +
                    u[SIZEX - 2] -
                    2*u[SIZEX - 1]); // j == SIZEX - 1

                for(j = 1; j < SIZEX-1; j++)
                    next_state[j]= u[j] + alpha * (
                        u[SIZEX + j] +
                        u[j + 1] +
                        u[j - 1] -
                        3*u[j]);
            }
            else if(i == SIZEY - 1)
            {
                next_state[i*SIZEX]= u[i*SIZEX] + alpha * (
                    u[(i-1)*SIZEX] +
                    u[i*SIZEX + 1] -
                    2*u[i*SIZEX]); // j == 0

                next_state[i*SIZEX + SIZEX - 1]= u[i*SIZEX + SIZEX - 1] + alpha * (
                    u[(i-1)*SIZEX + SIZEX - 1] +
                    u[i*SIZEX + SIZEX - 2] -
                    2*u[i*SIZEX + SIZEX - 1]); // j == SIZEX - 1

                for(j = 1; j < SIZEX-1; j++)
                    next_state[i*SIZEX + j]= u[i*SIZEX + j] + alpha * (
                        u[(i-1)*SIZEX + j] +
                        u[i*SIZEX + j + 1] +
                        u[i*SIZEX + j - 1] -
                        3*u[i*SIZEX + j]);
            }
            else
            {
                next_state[i * SIZEX]= u[i * SIZEX] + alpha * (
                    u[(i-1)*SIZEX] +
                    u[(i+1)*SIZEX] +
                    u[i*SIZEX + 1] -
                    3*u[i*SIZEX]); // j == 0

                next_state[i * SIZEX + SIZEX - 1]= u[i * SIZEX + SIZEX - 1] + alpha * (
                    u[(i-1)*SIZEX + SIZEX - 1] +
                    u[(i+1)*SIZEX + SIZEX - 1] +
                    u[i*SIZEX + SIZEX -2] -
                    3*u[i*SIZEX + SIZEX -1]); // j == SIZEX - 1

                for(j=1; j < SIZEX-1; j++)
                    next_state[i*SIZEX + j]= u[i * SIZEX + j] + alpha * (
                        u[(i-1)*SIZEX + j] +
                        u[(i+1)*SIZEX + j] +
                        u[i*SIZEX + j + 1] +
                        u[i*SIZEX + j - 1] -
                        4*u[i*SIZEX + j]);
            }

        }
        if(k != ITER)
        {
            auto tmp = next_state;
            next_state = u;
            u = tmp;
        }
    }
    *buffer = *data;
    *data = next_state;

    if(DEBUG)
    {
        double sum = 0.0;
        #pragma omp parallel for private(i) reduction(+:sum), shared(next_state)
        for(i=0; i < SIZEY*SIZEX; i++) sum += next_state[i];

        gettimeofday(&end, NULL);

        double delta = ((end.tv_sec  - start.tv_sec) * 1000000u +
            end.tv_usec - start.tv_usec) / 1.e3;
        printf("time (ms): %.5f \t total: %.3f \t average: %.4f\n", delta, sum, sum / SIZEX / SIZEY);
        // std::cout << "time (ms): " << delta << "\t\ttotal: " << sum << std::endl;
    }
}

void parse_arguments(int argc, char** argv)
{
    try
    {
        po::options_description desc("Allowed arguments");
        desc.add_options()
            ("help,h", "print this message.")
            ("height,y", po::value<int>()->default_value(600), "set height of the window.")
            ("width,x", po::value<int>()->default_value(800), "set width of the window")
            ("alpha,a", po::value<double>()->default_value(0.05), "alpha parameter of heat equation.")
            ("iter,i", po::value<int>()->default_value(20), "number of iterations per render.")
            ("size,s", po::value<int>()->default_value(15), "radius of the brush when drawing.")
            ("temp,t", po::value<double>()->default_value(1.3), "temperature of the brush.")
            ("debug,d", "debug flag to print extra states' info");

        po::variables_map vmap;
        po::store(po::parse_command_line(argc, argv, desc), vmap);
        po::notify(vmap);

        if(vmap.count("help"))
        {
            std::cout << desc;
            exit(0);
        }
        SIZEY = vmap["height"].as<int>();
        SIZEX = vmap["width"].as<int>();
        alpha = vmap["alpha"].as<double>();
        ITER = vmap["iter"].as<int>();
        RADIUS = vmap["size"].as<int>();
        TEMPERATURE = vmap["temp"].as<double>();
        if(vmap.count("debug")) DEBUG = true;
    }
    catch(std::exception& e)
    {
        std::cerr << "error: " << e.what() << std::endl;
        exit(1);
    }
    catch(...)
    {
        std::cerr << "error: Unknown error" << std::endl;
        exit(2);
    }
}

int main( int argc, char** argv )
{
    parse_arguments(argc, argv);

    double *data = new double[SIZEX*SIZEY];
    double *buffer = new double[SIZEX*SIZEY];
    cv::Mat image(SIZEY, SIZEX, CV_64F, data);
    cv::Mat scaled, color;

    std::string name = "Heat equation visualization";
    cv::namedWindow(name, cv::WINDOW_AUTOSIZE);
    cv::setMouseCallback(name, mouse_callback, nullptr);
    int val,x,y;

    auto vcap = new cv::VideoWriter("heat2d_video.avi",
            CV_FOURCC('M','J','P','G'),
            120,
            cv::Size(SIZEX, SIZEY),
            true);

    while( (val = cv::waitKey(1000/60)) != 27) // escape
    {
        if(val == 32) //space
        {
            run_evolve = !run_evolve;
        }
        if((x != mouse_x || y != mouse_y) && pressed)
        {
            x = mouse_x;
            y = mouse_y;
            cv::circle(image, cv::Point2d(x,y), RADIUS, cv::Scalar(TEMPERATURE), -1);
        }
        image.convertTo(scaled, CV_8UC1,255, 0);
        cv::applyColorMap(scaled, color, cv::COLORMAP_JET);
        if(run_evolve)
        {
            evolve(&data, &buffer);
        }
        image = cv::Mat(SIZEY, SIZEX, CV_64F, data);
        vcap->write(color);
        cv::imshow(name, color);
    }

    vcap->release();

    delete[] data;
    delete[] buffer;
    return 0;
}


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

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SAFE_MAX_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SAFE_MAX_HPP_INCLUDED

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

 /*!

    @ingroup group-ieee
    Function object implementing safe_max capabilities

    Returns a safe_max relative to the input,  i.e. a
    value which will not overflow when multiplied by the input.

    @par Semantic:

    For every parameter of type @c T

    @code
    auto r = safe_max(x);
    @endcode

    is similar to:

    @code
    auto r = x ? Sqrtvalmax<T>()/abs(x) : Inf<T>();
    @endcode

    @see Sqrtvalmax, safe_min

  **/
  Value safe_max(Value const & v0);
} }
#endif

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

#endif


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

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

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_TAND_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_TAND_HPP_INCLUDED

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

 /*!

    @ingroup group-trigonometric
    Function object implementing tand capabilities

    tangent of the input in degrees.

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = tand(x);
    @endcode

    is similar to:

    @code
    T r =  sind(x)/cosd(x);
    @endcode

    As most other trigonometric function tand can be called with a second optional parameter
    which is a tag on speed and accuracy (see @ref cos for further details)

    @see tan, tanpi

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

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

#endif


#include "Polygon.h"

#include <algorithm>
#include <numeric>

#include <boost/range/adaptors.hpp>
#include <boost/range/algorithm.hpp>

#include "Vector.h"
#include "Vertex.h"

Polygon::classification Polygon::classify(const Plane& plane) const
{
	size_t count = points.size();

	size_t front = 0, back = 0, onplane = 0;

	for (const auto& p : points)
	{
		auto test = plane.evaluate(p);

		if (test <= 0) back++;
		if (test >= 0) front++;
		if (test == 0) onplane++;
	}

	if (onplane == count) return Polygon::classification::onPlane;
	if (front == count) return Polygon::classification::front;
	if (back == count) return Polygon::classification::back;
	return Polygon::classification::spanning;
}

Vertex Polygon::origin() const
{
	return std::accumulate(points.cbegin(), points.cend(), Vertex{ 0, 0, 0 }) / double(points.size());
}

Plane Polygon::plane() const
{
	if(points.size() < 3)
		return Plane();

	return { points[0], points[1], points[2] };
}

void Polygon::rotate(const Vertex & point, const Matrix3d & rotmat)
{
	for (auto &p : points)
		p = p.rotate(point, rotmat);
}

void Polygon::move(const Vertex & v)
{
	for (auto &p : points)
		p += v;
}

void Polygon::moveTo(const Vertex & p)
{
	Vertex dist = p - origin();

	for (auto &p : points)
		p += dist;
}

void Polygon::scale(const Vertex &scale)
{
	this->scale(origin(), scale);
}

void Polygon::scale(const Vertex& origin, const Vertex& scale)
{
	for (auto &point : points)
	{
		Vertex vec = Vector::diff(origin, point).vec();
		vec.x(vec.x() * scale.x());
		vec.y(vec.y() * scale.y());
		vec.z(vec.z() * scale.z());

		point = origin + vec;
	}
}

void Polygon::sliceThis(const Plane &plane)
{
	auto[back, front] = slice(plane);
	if (!back.points.empty() && !front.points.empty())
		points = back.points;
}

std::pair<Polygon, Polygon> Polygon::slice(const Plane& plane) const
{
	auto classification = classify(plane);

	std::pair<Polygon, Polygon> ret;

	if (classification != classification::spanning)
	{
		if (classification == classification::back) ret.first = *this;
		else if (classification == classification::front) ret.second = *this;
		return ret;
	}

	size_t prev = 0;

	for (size_t i = 0; i <= points.size(); i++)
	{
		size_t index = i % points.size();
		Vertex end = points[index];
		auto c = plane.evaluate(end);

		if (i > 0 && c != 0 && prev != 0 && c != prev)
		{
			Vertex start = points[i - 1];
			Vector line = Vector::diff(start, end);
			Vertex intersect = Plane::intersectPoint(plane, line);
			if (!Vertex::isVertex(intersect))
				throw new std::exception("Expected intersection");
			ret.first.points.push_back(intersect);
			ret.second.points.push_back(intersect);
		}

		if (i < points.size())
		{
			if (c <= 0) ret.first.points.push_back(end);
			if (c >= 0) ret.second.points.push_back(end);
		}

		prev = c;
	}

	return ret;
}

void Polygon::flip()
{
	std::reverse(points.begin(), points.end());
}

void Polygon::roundPoints(size_t precision)
{
	double exp = std::pow(10, precision);
	std::transform(points.begin(), points.end(), points.begin(), [exp](Vertex v) {
		v.x(std::round(v.x() * exp) / exp);
		v.y(std::round(v.y() * exp) / exp);
		v.z(std::round(v.z() * exp) / exp);
		return v;
	});
}

Vertex Polygon::intersectPoint(const Vector& line, int flags) const
{
	Plane plane = this->plane();

	auto point = Plane::intersectPoint(plane, line);

	Vertex defaultRet = (flags & lineBoundsFlag::RETURN_END_ON_FAIL) > 0 ? line.end() : Vertex();

	flags = flags & lineBoundsFlag::ALLOW_BOTH;

	if (!Vertex::isVertex(point))
		return defaultRet;

	bool test = testCollision(point);

	if (!test)
		return defaultRet;

	if (flags != lineBoundsFlag::ALLOW_BOTH)
	{
		double position = line.calculatePosition(point);

		if ((lineBoundsFlag::ALLOW_BACK & flags) == 0 && (position < 0 || doubleeq(position, 0)))
			return defaultRet;

		if ((lineBoundsFlag::ALLOW_FRONT & flags) == 0 && (position > 1 || doubleeq(position, 1)))
			return defaultRet;
	}

	return point;
}

bool Polygon::testCollision(const Vertex& point) const
{
	double sum = 0;

	for (size_t n = 0; n < points.size(); n++)
	{
		Vertex p1 = points[n] - point;
		Vertex p2 = points[(n + 1) % points.size()] - point;

		double nom = p1.length() * p2.length();

		if (doubleeq(nom, 0))
			return false;

		sum += acos(p1.dotProduct(p2) / nom);
	}

	return doubleeq(sum, M_PI * 2);
}

bool Polygon::testCollision(const Vector& line, int flags) const
{
	// Ensure method returns NaN vector if it fails so we can test it.
	Vertex intersect = intersectPoint(line, flags);

	if (!Vertex::isVertex(intersect))
		return false;

	return true;
}

bool Polygon::testCollision(const Polygon& polygon) const
{
	if (&polygon == this)
		return true;

	if (points.size() < 3 || polygon.points.size() < 3)
		return false;

	Vertex thisNorm = this->plane().normal();
	Vertex polyNorm = polygon.plane().normal();

	if (thisNorm == polyNorm || thisNorm == -polyNorm)
		return false;

	for (size_t n = 0; n < points.size(); n++)
	{
		Vector line = Vector::diff(points[n], points[(n + 1) % points.size()]);

		if (polygon.testCollision(line, ALLOW_NONE))
			return true;
	}

	for (size_t n = 0; n < polygon.points.size(); n++)
	{
		Vector line = Vector::diff(polygon.points[n], polygon.points[(n + 1) % polygon.points.size()]);

		if (this->testCollision(line, ALLOW_NONE))
			return true;
	}

	return false;
}

Polygon::Polygon(const Plane & p)
{
	auto dir = p.closestAxisToNormal();
	auto tempV = dir == Vertex::unitZ ? -Vertex::unitY : -Vertex::unitZ;

	auto normal = p.normal();

	auto up = tempV.crossProduct(normal).normalize();
	auto right = normal.crossProduct(up).normalize();

	points = {
		p.p1() + right + up,
		p.p1() - right + up,
		p.p1() - right - up,
		p.p1() + right + up
	};

	auto orig = origin();

	std::transform(points.begin(), points.end(), points.begin(), [&orig](Vertex v) {
		return (v - orig).normalize() * 10'000'000.0 + orig;
	});

}

Polygon::Polygon(const std::initializer_list<Vertex>& points)
	: points(points)
{
}