#include "MarchingCubes.hpp"

#include <cassert>

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

void MarchingCubes::add_hint(const Cuboid& hint) {
    hints.push_back(hint);
}

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

    without_hints(output);

    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);
            }
        }
    }
}

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)
            vert_ids[i] = output.add_vertice(verts[i]);

        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);
}