/**
 * ------------------------------------
 * @author: Weipeng Kong
 * @date: 2021/12/6
 * @email: yjxkwp@foxmail.com
 * @site: https://donot.fit
 * @description: 
 * ------------------------------------
**/

#include "search_assign.h"
#include <pMesh/mesh/TriangleMesh.h>
#include <iostream>

bool *visited;

void
dfs_for_assigning(Octree::UDFData &udf, int current_x, int current_y, int current_z, int ind, const double threshold,
                  std::function<void(const double *)> recorder) {
    if (visited[ind] || ind == -1) return;
    visited[ind] = true;

    if (abs(udf.value[ind]) < threshold) {
        return;
    }

    udf.value[ind] = -abs(udf.value[ind]);

    // sub routine
    for (int i = -1; i <= 1; ++i) {
        int x = current_x + i;
        if (x < 0 || x >= udf._x) continue;

        for (int j = -1; j <= 1; ++j) {
            int y = current_y + j;
            if (y < 0 || y >= udf._y) continue;

            for (int k = -1; k <= 1; ++k) {
                int z = current_z + k;
                if (z < 0 || z >= udf._z) continue;

                int next_ind = udf.get_array_index(x, y, z);
                dfs_for_assigning(udf, x, y, z, next_ind, threshold, recorder);
            }
        }
    }
}

#include <queue>
#include <boost/tuple/tuple.hpp>

struct BFSParam{
    int x;
    int y;
    int z;
    int id;
};

void
bfs_for_assigning(Octree::UDFData &udf, int start_x, int start_y, int start_z, const double threshold,
                  std::function<void(const double *)> recorder) {
    int start_id = udf.get_array_index(start_x, start_y, start_z);
    std::queue<BFSParam> Q;
    Q.push({.x=start_x, .y=start_y, .z=start_z, .id=start_id});

    while(not Q.empty()){
        auto [current_x, current_y, current_z, ind] = Q.front();
        Q.pop();

        if(visited[ind]) continue;
        visited[ind] = true;

        if (abs(udf.value[ind]) < threshold) {
            continue;
        }
        udf.value[ind] = -abs(udf.value[ind]);

        // sub routine
        for (int i = -1; i <= 1; ++i) {
            int x = current_x + i;
            if (x < 0 || x >= udf._x) continue;

            for (int j = -1; j <= 1; ++j) {
                int y = current_y + j;
                if (y < 0 || y >= udf._y) continue;

                for (int k = -1; k <= 1; ++k) {
                    int z = current_z + k;
                    if (z < 0 || z >= udf._z) continue;

                    int next_ind = udf.get_array_index(x, y, z);
                    Q.push({.x=x, .y=y, .z=z, .id=next_ind});
                }
            }
        }

    }


}

#include <igl/point_mesh_squared_distance.h>
#include <igl/signed_distance.h>
#include <igl/AABB.h>
#include <igl/in_element.h>

void search_assign(Octree::UDFData &udf, int *start, double threshold) {
    visited = new bool[udf._x * udf._y * udf._z];

    pMesh::Triangle3dMesh point_cloud;
    auto recoder = [&](const double *pos) {
//        point_cloud.vertices.emplace_back(pMesh::Triangle3dMesh::VertexField())
//        std::cout << pos[0] << " " << pos[1] << " " << pos[2] << std::endl;
    };

    // TODO: 选取开始点
    bfs_for_assigning(udf, start[0], start[1], start[2], threshold, recoder);

    delete visited;
    visited = nullptr;
};