#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;

    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)
            {
                Intersections intersections;
                /* =====
                 * 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!
                 * ==== */
                for(int dx=0; dx <= 1; dx++) {
                    for(int dy=0; dy <= 1; dy++) {
                        for(int dz=0; dz <= 1; dz++) {
                            double cx = x + resolution * dx;
                            double cy = y + resolution * dy;
                            double cz = z + resolution * dz;
                            intersections.set_corner(dx, dy, dz,
                                    surface(cx, cy, cz) > 0);
                        }
                    }
                }

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

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