#include "MarchingCubes.hpp"

#include <cassert>
#include <queue>

#include "GL/gl.h"

#include "GL/gl.h"

MarchingCubes::MarchingCubes(
        const ImplicitSurface& surface,
        const Cuboid& box,
        double resolution):
    surface(surface),
    box(box),
    resolution(resolution)
{}

MarchingCubes& MarchingCubes::add_hint(const Point& hint) {
    hints.push_back(hint);
    return *this;
}

Mesh MarchingCubes::operator()() {
    Mesh output;

    output.set_color(surface.get_color());

    perf_counter = 0;

    if(hints.empty())
        without_hints(output);
    else
        with_hints(output);

#ifdef MC_SHOW_PERF
    fprintf(stderr, "Explored cubes: %ld\n", perf_counter);
#endif // MC_SHOW_PERF

    output.translate(surface.getCenter());

    return output;
}

void MarchingCubes::without_hints(Mesh& output) {
    for(double x = box.low(0); x < box.high(0) + resolution;
            x += resolution) {
        for(double y = box.low(1); y < box.high(1) + resolution;
                y += resolution) {
            for(double z = box.low(2); z < box.high(2) + resolution;
                    z += resolution) {
                march_at(x, y, z, output);
                perf_counter++;
            }
        }
    }
}

void MarchingCubes::with_hints(Mesh& output) {
    std::set<PointLoc> explored_cubes;
    std::queue<PointLoc> process;

    for(const Point& hint: hints) {
        PointLoc coords = coords_of(hint);
        PointLoc start_point =
            seek_closest_intersection(coords, explored_cubes);
        explored_cubes.erase(start_point);
        process.push(start_point);
    }

    while(!process.empty()) {
        PointLoc pos = process.front();
        process.pop();
        if(explored_cubes.find(pos) != explored_cubes.end())
            continue;
        explored_cubes.insert(pos);

        Point pt = point_at_coords(pos);
        Intersections inters =
            mk_intersection_cube(pt.x, pt.y, pt.z, resolution);
        if(!inters.has_inters())
            continue;

        march_at(pt.x, pt.y, pt.z, output);

        for(int dx=-1; dx <= 1; ++dx) {
            for(int dy=-1; dy <= 1; ++dy) {
                for(int dz=-1; dz <= 1; ++dz) {
                    if(dx == 0 && dy == 0 && dz == 0)
                        continue;
                    PointLoc nPos = pos + PointLoc(dx, dy, dz);
                    if(coords_in_box(nPos))
                        process.push(nPos);
                }
            }
        }
    }
    perf_counter = explored_cubes.size();
}

PointLoc MarchingCubes::seek_closest_intersection(
        const PointLoc& loc, std::set<PointLoc>& visited) const {
    std::queue<PointLoc> process;
    process.push(loc);

    while(!process.empty()) {
        PointLoc pos = process.front();
        process.pop();
        if(visited.find(pos) != visited.end())
            continue;
        visited.insert(pos);

        Point pt = point_at_coords(pos);
        Intersections inters =
            mk_intersection_cube(pt.x, pt.y, pt.z, resolution);
        if(inters.has_inters())
            return pos;

        for(int dx=-1; dx <= 1; ++dx) {
            for(int dy=-1; dy <= 1; ++dy) {
                for(int dz=-1; dz <= 1; ++dz) {
                    if(dx == 0 && dy == 0 && dz == 0)
                        continue;
                    PointLoc nPos = pos + PointLoc(dx, dy, dz);
                    if(coords_in_box(nPos))
                        process.push(nPos);
                }
            }
        }
    }

    throw SurfaceNotFound();
}

Point MarchingCubes::align_on_bb(const Point& pt) const {
    auto align = [resolution = resolution](double v) {
        return resolution * floor(v / resolution);
    };
    Point dist = pt - box.low_pt();
    dist.x = align(dist.x);
    dist.y = align(dist.y);
    dist.z = align(dist.z);
    return box.low_pt() + dist;
}

bool MarchingCubes::coords_in_box(const PointLoc& pt) const {
    Point low = point_at_coords(pt),
          high = point_at_coords(pt + PointLoc(1, 1, 1));
    return
        low.x >= box.low(0) && low.y >= box.low(1) && low.z >= box.low(2)
        && high.x <= box.high(0) && high.y <= box.high(1)
        && high.z <= box.high(2);
}

PointLoc MarchingCubes::coords_of(const Point& pt) const {
    Point dist = pt - box.low_pt();
    return PointLoc(
            dist.x / resolution,
            dist.y / resolution,
            dist.z / resolution);
}

Point MarchingCubes::point_at_coords(const PointLoc& coords) const {
    return box.low_pt()
        + Point(coords.x * resolution,
                coords.y * resolution,
                coords.z * resolution);
}

Point MarchingCubes::CubeEdge::at(double pos,
        double bx, double by, double bz, double resolution) const
{
    auto bary = [pos](double x, double y) {
        return pos * x + (1.-pos) * y;
    };

    Point p1(
            bx + x[0] * resolution,
            by + y[0] * resolution,
            bz + z[0] * resolution);
    Point p2(
            bx + x[1] * resolution,
            by + y[1] * resolution,
            bz + z[1] * resolution);

    return Point(
            bary(p1.x, p2.x),
            bary(p1.y, p2.y),
            bary(p1.z, p2.z));
}

bool MarchingCubes::march_at(double x, double y, double z, Mesh& output) {
    /* =====
     * NOTE: this code currently computes 8 times the surface value
     * at each corner point of the inspected space. This is okay
     * for now, because such computations are ultra light and
     * actually better than storing values.
     * If a time comes when this computation is heavier, because we
     * are looking at more complex implicit surfaces,
     *         IT WILL BE NECESSARY TO ENHANCE THIS CODE!
     * ==== */

    Intersections intersections = mk_intersection_cube(x, y, z, resolution);

    const std::vector<CubeTri>& cur_triangles =
        edges_of_intersection[intersections.value()];

    for(const CubeTri& cube_tri: cur_triangles) {
        Point verts[3] = {
            intersect_location(cube_tri.edge[0],
                    x, y, z),
            intersect_location(cube_tri.edge[1],
                    x, y, z),
            intersect_location(cube_tri.edge[2],
                    x, y, z),
        };

        size_t vert_ids[3];
        for(int i=0; i < 3; ++i) {
            Point normal = surface.normal_at(verts[i]);
            vert_ids[i] = output.add_vertice(verts[i], normal);
        }

        for(int i=0; i < 3; ++i) {
            output.add_face(
                    vert_ids[i],
                    vert_ids[(i+1) % 3],
                    vert_ids[(i+2) % 3]);
        }

    }

    return !cur_triangles.empty();
}

MarchingCubes::Intersections MarchingCubes::mk_intersection_cube(
        double bx, double by, double bz, double side_len) const
{
    Intersections intersections;
    for(int dx=0; dx <= 1; dx++) {
        for(int dy=0; dy <= 1; dy++) {
            for(int dz=0; dz <= 1; dz++) {
                double cx = bx + side_len * dx;
                double cy = by + side_len * dy;
                double cz = bz + side_len * dz;
                intersections.set_corner(dx, dy, dz,
                        surface(cx, cy, cz) > 0);
            }
        }
    }
    return intersections;
}

Point MarchingCubes::intersect_location(const CubeEdge& edge,
        double bx, double by, double bz) const
{
    std::function<Point(double, double)> compute =
        [&](double low_prop, double high_prop)
        {
            double med_prop = (low_prop + high_prop) / 2;
            Point med = edge.at(med_prop, bx, by, bz, resolution),
                  low = edge.at(low_prop, bx, by, bz, resolution),
                  high = edge.at(high_prop, bx, by, bz, resolution);

            if(high_prop - low_prop < 1e-8)
                return med;

            double sLow = surface(low),
                   sMed = surface(med),
                   sHigh = surface(high);

            assert(sLow * sHigh <= 0);
            // ^ Can still binary search

            if(sLow * sMed <= 0)
                return compute(low_prop, med_prop);
            return compute(med_prop, high_prop);
        };

    return compute(0, 1);
}