// This file is part of the AliceVision project.
// Copyright (c) 2017 AliceVision contributors.
// This Source Code Form is subject to the terms of the Mozilla Public License,
// v. 2.0. If a copy of the MPL was not distributed with this file,
// You can obtain one at https://mozilla.org/MPL/2.0/.

#include "VoxelsGrid.hpp"
#include <aliceVision/system/Logger.hpp>
#include <aliceVision/mvsData/Pixel.hpp>
#include <aliceVision/mvsUtils/common.hpp>
#include <aliceVision/mvsUtils/fileIO.hpp>
#include <aliceVision/fuseCut/delaunayGraphCutTypes.hpp>
#include <aliceVision/alicevision_omp.hpp>

#include <boost/filesystem.hpp>

namespace aliceVision {
namespace fuseCut {

namespace bfs = boost::filesystem;

VoxelsGrid::VoxelsGrid() {}

VoxelsGrid::VoxelsGrid(const Voxel& dimensions, Point3d* _space, mvsUtils::MultiViewParams* _mp, const std::string& _spaceRootDir, bool _doVisualize)
{
    doVisualize = _doVisualize;
    mp = _mp;
    voxelDim = dimensions;
    for (int k = 0; k < 8; k++)
    {
        space[k] = _space[k];  // TODO faca
    }
    voxels = mvsUtils::computeVoxels(space, dimensions);
    spaceRootDir = _spaceRootDir;
    bfs::create_directory(spaceRootDir);

    spaceCamsTracksDir = _spaceRootDir + "camsTracks/";
    bfs::create_directory(spaceCamsTracksDir);
}

VoxelsGrid* VoxelsGrid::clone(const std::string& _spaceRootDir)
{
    VoxelsGrid* out = new VoxelsGrid();

    out->doVisualize = doVisualize;
    out->mp = mp;
    out->voxelDim = voxelDim;
    out->voxels = new StaticVector<Point3d>();
    out->voxels->resize(voxels->size());
    for (int i = 0; i < voxels->size(); i++)
    {
        out->voxels->push_back((*voxels)[i]);
    }
    for (int k = 0; k < 8; k++)
    {
        out->space[k] = space[k];
    }
    out->spaceRootDir = _spaceRootDir;
    bfs::create_directory(out->spaceRootDir);

    out->spaceCamsTracksDir = _spaceRootDir + "camsTracks/";
    bfs::create_directory(out->spaceCamsTracksDir);

    return out;
}

VoxelsGrid::~VoxelsGrid() { delete voxels; }

StaticVector<int>* VoxelsGrid::getNVoxelsTracks()
{
    ALICEVISION_LOG_DEBUG("reading number of tracks for each voxel file.");

    StaticVector<int>* nVoxelsTracks = new StaticVector<int>();
    nVoxelsTracks->reserve(voxels->size() / 8);
    // long t1 = mvsUtils::initEstimate();
    for (int i = 0; i < voxels->size() / 8; i++)
    {
        std::string folderName = getVoxelFolderName(i);
        std::string fileNameTracksPts;
        fileNameTracksPts = folderName + "tracksGridPts.bin";
        int n = getArrayLengthFromFile(fileNameTracksPts);
        // printf("%i %i\n",i,n);
        nVoxelsTracks->push_back(n);
        // mvsUtils::printfEstimate(i, voxels->size() / 8, t1);
    }
    // mvsUtils::finishEstimate();

    return nVoxelsTracks;
}

unsigned long VoxelsGrid::getNTracks() const
{
    ALICEVISION_LOG_DEBUG("computing number of all tracks.");

    unsigned long ntracks = 0;
    // long t1 = mvsUtils::initEstimate();
    for (int i = 0; i < voxels->size() / 8; i++)
    {
        const std::string folderName = getVoxelFolderName(i);
        std::string fileNameTracksPts;
        fileNameTracksPts = folderName + "tracksGridPts.bin";
        int n = getArrayLengthFromFile(fileNameTracksPts);
        // printf("%i %i\n",i,n);
        ntracks += (unsigned long)n;
        // mvsUtils::printfEstimate(i, voxels->size() / 8, t1);
    }
    // mvsUtils::finishEstimate();

    return ntracks;
}

int VoxelsGrid::getIdForVoxel(const Voxel& v) const { return v.x * voxelDim.y * voxelDim.z + v.y * voxelDim.z + v.z; }

Voxel VoxelsGrid::getVoxelForId(int id) const
{
    int xp = id / (voxelDim.y * voxelDim.z);
    return Voxel(xp, (id - xp * voxelDim.y * voxelDim.z) / voxelDim.z, (id - xp * voxelDim.y * voxelDim.z) % voxelDim.z);
}

bool VoxelsGrid::isValidVoxel(const Voxel& v)
{
    return ((v.x >= 0) && (v.x < voxelDim.x) && (v.y >= 0) && (v.y < voxelDim.y) && (v.z >= 0) && (v.z < voxelDim.z));
}

std::string VoxelsGrid::getVoxelFolderName(int id) const
{
    const Voxel v = getVoxelForId(id);
    // std::string fnx = spaceRootDir + "X"+num2str(v.x)+"/";
    // std::string fnxyz = fnx + "Y"+num2str(v.y)+"Z"+num2str(v.z)+"/";
    // bfs::create_directory(fnx);
    // bfs::create_directory(fnxyz);

    std::string fnxyz = spaceRootDir + "X" + mvsUtils::num2str(v.x) + "Y" + mvsUtils::num2str(v.y) + "Z" + mvsUtils::num2str(v.z) + "/";
    // bfs::create_directory(fnxyz);

    // if (bfs::is_directory(fnx)==false) {
    //	printf("Warning folder %s does not exist!\n",fnx.c_str());
    //}

    // if (bfs::is_directory(fnxyz)==false) {
    //	printf("Warning folder %s does not exist!\n",fnxyz.c_str());
    //}

    return fnxyz;
}

StaticVector<OctreeTracks::trackStruct*>* VoxelsGrid::loadTracksFromVoxelFiles(StaticVector<int>** cams, int id)
{
    const std::string folderName = getVoxelFolderName(id);

    const std::string fileNameTracksCams = folderName + "tracksGridCams.bin";
    const std::string fileNameTracksPts = folderName + "tracksGridPts.bin";
    const std::string fileNameTracksPtsCams = folderName + "tracksGridPtsCams.bin";
    const std::string fileNameTracksStat = folderName + "tracksGridStat.bin";

    if (!bfs::exists(fileNameTracksPts))
        return nullptr;

    StaticVector<Point3d>* tracksStat = loadArrayFromFile<Point3d>(fileNameTracksStat);  // minPixSize, minSim, npts
    StaticVector<Point3d>* tracksPoints = loadArrayFromFile<Point3d>(fileNameTracksPts);
    StaticVector<StaticVector<Pixel>*>* tracksPointsCams = loadArrayOfArraysFromFile<Pixel>(fileNameTracksPtsCams);
    *cams = loadArrayFromFile<int>(fileNameTracksCams);

    StaticVector<OctreeTracks::trackStruct*>* tracks = new StaticVector<OctreeTracks::trackStruct*>();
    tracks->reserve(tracksPoints->size());

    for (int i = 0; i < tracksPoints->size(); i++)
    {
        StaticVector<Pixel>* tcams = (*tracksPointsCams)[i];
        OctreeTracks::trackStruct* t = new OctreeTracks::trackStruct(0.0f, 0.0f, Point3d(), 0);
        t->point = (*tracksPoints)[i];
        t->minPixSize = (*tracksStat)[i].x;
        t->minSim = (*tracksStat)[i].y;
        t->npts = (int)(*tracksStat)[i].z;
        t->cams = *tcams;
        tracks->push_back(t);
    }

    delete tracksStat;
    delete tracksPoints;
    deleteArrayOfArrays<Pixel>(&tracksPointsCams);

    return tracks;
}

bool VoxelsGrid::saveTracksToVoxelFiles(StaticVector<int>* cams, StaticVector<OctreeTracks::trackStruct*>* tracks, int id)
{
    if (sizeOfStaticVector<OctreeTracks::trackStruct*>(tracks) <= 10)
    {
        return false;
    }

    std::string folderName = getVoxelFolderName(id);

    bfs::create_directory(folderName);
    if (!bfs::is_directory(folderName))
    {
        ALICEVISION_LOG_WARNING("Folder '" << folderName << "' does not exist.");
    }

    StaticVector<Point3d>* tracksPoints = new StaticVector<Point3d>();
    tracksPoints->reserve(tracks->size());
    StaticVector<StaticVector<Pixel>*>* tracksPointsCams = new StaticVector<StaticVector<Pixel>*>();
    tracksPointsCams->reserve(tracks->size());
    StaticVector<Point3d>* tracksStat = new StaticVector<Point3d>();
    tracksStat->reserve(tracks->size());

    for (int j = 0; j < tracks->size(); j++)
    {
        OctreeTracks::trackStruct* info = (*tracks)[j];
        tracksPoints->push_back(info->point);
        tracksStat->push_back(Point3d(info->minPixSize, info->minSim, info->npts));

        StaticVector<Pixel>* tcams = new StaticVector<Pixel>();
        tcams->reserve(info->cams.size());
        for (int k = 0; k < info->cams.size(); k++)
        {
            tcams->push_back(info->cams[k]);
        }
        tracksPointsCams->push_back(tcams);
    }

    // printf("SAVING %i-th VOXEL TRACKS\n",i);
    std::string fileNameTracksCams, fileNameTracksPts, fileNameTracksPtsCams, fileNameTracksStat;

    fileNameTracksCams = folderName + "tracksGridCams.bin";
    fileNameTracksPts = folderName + "tracksGridPts.bin";
    fileNameTracksPtsCams = folderName + "tracksGridPtsCams.bin";
    fileNameTracksStat = folderName + "tracksGridStat.bin";

    saveArrayToFile<int>(fileNameTracksCams, cams);
    saveArrayToFile<Point3d>(fileNameTracksPts, tracksPoints);
    saveArrayToFile<Point3d>(fileNameTracksStat, tracksStat);
    saveArrayOfArraysToFile<Pixel>(fileNameTracksPtsCams, tracksPointsCams);

    delete tracksPoints;
    delete tracksStat;
    deleteArrayOfArrays<Pixel>(&tracksPointsCams);

    return true;
}

void VoxelsGrid::generateTracksForEachVoxel(StaticVector<Point3d>* ReconstructionPlan,
                                            int numSubVoxs,
                                            int maxPts,
                                            int level,
                                            int& maxlevel,
                                            const std::string& depthMapsPtsSimsTmpDir)
{
    ALICEVISION_LOG_DEBUG("generateTracksForEachVoxel recursive "
                          << "\t- numSubVoxs: " << numSubVoxs << std::endl
                          << "\t- maxPts: " << maxPts << std::endl
                          << "\t- level: " << level);

    ALICEVISION_LOG_DEBUG("Voxel Dim: " << voxelDim.x << ", " << voxelDim.y << ", " << voxelDim.z);

    maxlevel = std::max(maxlevel, level);

    long tall = clock();
    int nvoxs = voxels->size() / 8;

    StaticVector<int>* toRecurse = new StaticVector<int>();
    toRecurse->reserve(nvoxs);

#pragma omp parallel for
    for (int i = 0; i < nvoxs; i++)
    {
        // printf("GENERATING TRIANGLUATION FOR %i-th VOXEL OF %i\n",i,nvoxs);

        std::string folderName = getVoxelFolderName(i);

        long t1 = clock();
        OctreeTracks* ott = new OctreeTracks(&(*voxels)[i * 8], mp, Voxel(numSubVoxs, numSubVoxs, numSubVoxs));
        StaticVector<OctreeTracks::trackStruct*>* tracks = ott->fillOctree(maxPts, depthMapsPtsSimsTmpDir);
        if (mp->verbose)
            mvsUtils::printfElapsedTime(t1, "fillOctree");
        if (tracks == nullptr)
        {
            if (mp->verbose)
                ALICEVISION_LOG_DEBUG("deleting OTT: " << i);
            delete ott;
            if (mp->verbose)
                ALICEVISION_LOG_DEBUG("deleted " << i);
#pragma omp critical
            {
                toRecurse->push_back(i);
            }
        }
        else
        {
            // printf("SAVING %i-th VOXEL TRACKS FILES\n",i);
            if (tracks->size() > 10)
            {
                if (ReconstructionPlan != nullptr)
                {
#pragma omp critical
                    {
                        for (int k = 0; k < 8; k++)
                        {
                            if (ReconstructionPlan->size() < ReconstructionPlan->capacity())
                            {
                                ReconstructionPlan->push_back((*voxels)[i * 8 + k]);
                            }
                            else
                            {
                                ALICEVISION_LOG_WARNING("reconstruction plan array is full.");
                            }
                        }
                    }
                }
                StaticVector<int>* cams = ott->getTracksCams(tracks);
                saveTracksToVoxelFiles(cams, tracks, i);
                delete cams;
            }
            delete tracks;
            if (mp->verbose)
                ALICEVISION_LOG_DEBUG("deleting OTT: " << i);
            delete ott;
            if (mp->verbose)
                ALICEVISION_LOG_DEBUG("deleted " << i);
        }
    }

    if (mp->verbose)
        ALICEVISION_LOG_DEBUG("toRecurse " << toRecurse->size());

    for (int j = 0; j < toRecurse->size(); j++)
    {
        int i = (*toRecurse)[j];
        // recursion
        std::string folderName = getVoxelFolderName(i);
        // std::string subfn = folderName + "sub/";
        std::string subfn = folderName;
        bfs::create_directory(subfn);

        // create file that indicates that the voxel has subvoxels
        std::string subfnFileMark = folderName + "sub.txt";
        FILE* f = fopen(subfnFileMark.c_str(), "w");
        fclose(f);

        VoxelsGrid* vgnew = new VoxelsGrid(Voxel(2, 2, 2), &(*voxels)[i * 8], mp, subfn, doVisualize);
        vgnew->generateTracksForEachVoxel(ReconstructionPlan, numSubVoxs / 2, maxPts, level + 1, maxlevel, depthMapsPtsSimsTmpDir);
        delete vgnew;
    }

    delete toRecurse;

    mvsUtils::printfElapsedTime(tall);

    if (doVisualize)
        vizualize();
}

void VoxelsGrid::generateSpace(VoxelsGrid* vgnew, const Voxel& LU, const Voxel& RD, const std::string& depthMapsPtsSimsTmpDir)
{
    if (mp->verbose)
        ALICEVISION_LOG_DEBUG("generateSpace recursive: " << std::endl
                                                          << "\t- LU: " << LU.x << " " << LU.y << " " << LU.z << std::endl
                                                          << "\t- RD: " << RD.x << " " << RD.y << " " << RD.z);

    int nvoxs = voxels->size() / 8;
    for (int voxid = 0; voxid < nvoxs; voxid++)
    {
        std::string folderName = getVoxelFolderName(voxid);
        // std::string subfn = folderName + "sub/";
        std::string subfn = folderName;
        std::string subfnFileMark = folderName + "sub.txt";

        Voxel v = getVoxelForId(voxid);
        Voxel ns = RD - LU;
        ns.x /= voxelDim.x;
        ns.y /= voxelDim.y;
        ns.z /= voxelDim.z;
        Voxel subLU = LU + v * ns;
        Voxel subRD = LU + (v + 1) * ns;

        // if (bfs::is_directory(subfn)==true)
        if (bfs::exists(subfnFileMark))
        {
            VoxelsGrid* vgrec = new VoxelsGrid(Voxel(2, 2, 2), &(*voxels)[voxid * 8], mp, subfn, doVisualize);
            vgrec->generateSpace(vgnew, subLU, subRD, depthMapsPtsSimsTmpDir);
            delete vgrec;
        }
        else
        {
            Voxel part = subRD - subLU;
            OctreeTracks* ott = new OctreeTracks(&(*voxels)[voxid * 8], mp, part);
            VoxelsGrid* vgg = new VoxelsGrid(part, &(*voxels)[voxid * 8], mp, folderName, doVisualize);
            StaticVector<int>* cams = nullptr;
            StaticVector<OctreeTracks::trackStruct*>* tracks = loadTracksFromVoxelFiles(&cams, voxid);
            Voxel vrel;

            if ((tracks != nullptr) && (tracks->size() > 0))
            {
                // estimate nubers // TODO FACA: remove costly estimation of numbers
                StaticVector<int>* nnewVoxsTracks = new StaticVector<int>();
                nnewVoxsTracks->reserve(part.x * part.y * part.z);
                nnewVoxsTracks->resize_with(part.x * part.y * part.z, 0);
                for (int i = 0; i < tracks->size(); i++)
                {
                    if (ott->getVoxelOfOctreeFor3DPoint(vrel, (*tracks)[i]->point))
                    {
                        (*nnewVoxsTracks)[vgg->getIdForVoxel(vrel)] += 1;
                    }
                }

                // allocate
                StaticVector<StaticVector<OctreeTracks::trackStruct*>*>* newVoxsTracks =
                  new StaticVector<StaticVector<OctreeTracks::trackStruct*>*>();
                newVoxsTracks->reserve(part.x * part.y * part.z);
                for (int i = 0; i < part.x * part.y * part.z; i++)
                {
                    auto* newVoxTracks = new StaticVector<OctreeTracks::trackStruct*>();
                    newVoxTracks->reserve((*nnewVoxsTracks)[i]);
                    newVoxsTracks->push_back(newVoxTracks);
                }

                // fill
                for (int i = 0; i < tracks->size(); i++)
                {
                    if (ott->getVoxelOfOctreeFor3DPoint(vrel, (*tracks)[i]->point))
                    {
                        (*newVoxsTracks)[vgg->getIdForVoxel(vrel)]->push_back((*tracks)[i]);
                    }
                }

                for (int i = 0; i < part.x * part.y * part.z; i++)
                {
                    Voxel vact = vgg->getVoxelForId(i);
                    Voxel vglob = subLU + vact;
                    int newid = vgnew->getIdForVoxel(vglob);

                    vgnew->saveTracksToVoxelFiles(cams, (*newVoxsTracks)[i], newid);

                    /*
                    printf("idlocal %i of %i, voxel local %i %i %i, voxel global %i %i %i, id global %i, ntracks %i \n",
                            i,part.x*part.y*part.z, vact.x, vact.y, vact.z, vglob.x, vglob.y, vglob.z, newid,
                            (*newVoxsTracks)[i]->size()
                    );
                    */
                }

                // deallocate
                delete newVoxsTracks;
                delete nnewVoxsTracks;
            }

            if (cams != nullptr)
            {
                delete cams;
            }

            if (tracks != nullptr)
            {
                for (int i = 0; i < tracks->size(); i++)
                {
                    delete (*tracks)[i];
                }
                delete tracks;
            }

            delete vgg;
            delete ott;
        }
    }
}

void VoxelsGrid::cloneSpaceVoxel(int voxelId, int numSubVoxs, VoxelsGrid* newSpace)
{
    std::string folderName = getVoxelFolderName(voxelId);
    std::string fileNameTracksPts = folderName + "tracksGridPts.bin";

    if (bfs::exists(fileNameTracksPts))
    {
        OctreeTracks* ott = new OctreeTracks(&(*voxels)[voxelId * 8], mp, Voxel(numSubVoxs, numSubVoxs, numSubVoxs));
        StaticVector<int>* tcams;
        StaticVector<OctreeTracks::trackStruct*>* tracksOld = loadTracksFromVoxelFiles(&tcams, voxelId);
        StaticVector<OctreeTracks::trackStruct*>* tracksNew = ott->fillOctreeFromTracks(tracksOld);
        for (int i = 0; i < tracksOld->size(); i++)
        {
            delete (*tracksOld)[i];
        }
        delete tracksOld;
        newSpace->saveTracksToVoxelFiles(tcams, tracksNew, voxelId);

        delete tcams;
        delete tracksNew;  // DO NOT NEEDED TO DELETE PARTICULAR POINTERS BECAUSE THEY POINT TO ott STRUCTURES
        delete ott;
    }
}

VoxelsGrid* VoxelsGrid::cloneSpace(int numSubVoxs, const std::string& newSpaceRootDir)
{
    if (mp->verbose)
        ALICEVISION_LOG_DEBUG("cloning space.");

    VoxelsGrid* out = clone(newSpaceRootDir);

    long t1 = mvsUtils::initEstimate();
    for (int i = 0; i < voxels->size() / 8; i++)
    {
        cloneSpaceVoxel(i, numSubVoxs, out);
        mvsUtils::printfEstimate(i, voxels->size() / 8, t1);
    }
    mvsUtils::finishEstimate();

    if (doVisualize)
        out->vizualize();

    return out;
}

void VoxelsGrid::copySpaceVoxel(int voxelId, VoxelsGrid* newSpace)
{
    std::string folderName = getVoxelFolderName(voxelId);
    std::string fileNameTracksPts = folderName + "tracksGridPts.bin";

    if (bfs::exists(fileNameTracksPts))
    {
        StaticVector<int>* tcams;
        StaticVector<OctreeTracks::trackStruct*>* tracksOld = loadTracksFromVoxelFiles(&tcams, voxelId);
        newSpace->saveTracksToVoxelFiles(tcams, tracksOld, voxelId);

        for (int i = 0; i < tracksOld->size(); i++)
        {
            delete (*tracksOld)[i];
        }
        delete tracksOld;
        delete tcams;
    }
}

VoxelsGrid* VoxelsGrid::copySpace(const std::string& newSpaceRootDir)
{
    if (mp->verbose)
        ALICEVISION_LOG_DEBUG("Copy space.");

    VoxelsGrid* out = clone(newSpaceRootDir);

    long t1 = mvsUtils::initEstimate();
    for (int i = 0; i < voxels->size() / 8; i++)
    {
        copySpaceVoxel(i, out);
        mvsUtils::printfEstimate(i, voxels->size() / 8, t1);
    }
    mvsUtils::finishEstimate();

    if (doVisualize)
        out->vizualize();

    return out;
}

void VoxelsGrid::generateCamsPtsFromVoxelsTracks()
{
    if (mp->verbose)
        ALICEVISION_LOG_DEBUG("Distributing pts from voxels tracks to camera files.");

    long t1 = mvsUtils::initEstimate();
    int nvoxs = voxels->size() / 8;
    for (int i = 0; i < nvoxs; i++)
    {
        std::string folderName = getVoxelFolderName(i);
        std::string fileNameTracksCams, fileNameTracksPts, fileNameTracksPtsCams;
        fileNameTracksCams = folderName + "tracksGridCams.bin";
        fileNameTracksPts = folderName + "tracksGridPts.bin";
        fileNameTracksPtsCams = folderName + "tracksGridPtsCams.bin";

        // printf("SAVING %i-th VOXEL POINTS TO CAMS FILES\n",i);

        if (bfs::exists(fileNameTracksPts))
        {
            StaticVector<Point3d>* tracksPoints = loadArrayFromFile<Point3d>(fileNameTracksPts);
            StaticVector<StaticVector<Pixel>*>* tracksPointsCams = loadArrayOfArraysFromFile<Pixel>(fileNameTracksPtsCams);
            StaticVector<int>* cams = loadArrayFromFile<int>(fileNameTracksCams);

            // printf("distributing %i tracks to %i camspts files  \n", tracksPoints->size(), cams->size());
            StaticVector<StaticVector<Pixel>*>* camsTracksPoints = convertObjectsCamsToCamsObjects(*mp, tracksPointsCams);

#pragma omp parallel for
            for (int c = 0; c < cams->size(); c++)
            {
                int rc = (*cams)[c];

                // open camPtsFile for append
                std::string camPtsFileName = spaceCamsTracksDir + "camPtsGrid_" + std::to_string(mp->getViewId(rc)) + ".bin";
                FILE* fin = fopen(camPtsFileName.c_str(), "ab");
                StaticVector<Pixel>* camPtsIds = (*camsTracksPoints)[rc];
                for (int j = 0; j < sizeOfStaticVector<Pixel>(camPtsIds); j++)
                {
                    int ptid = (*camPtsIds)[j].x;
                    int nrc = (*camPtsIds)[j].y;
                    GC_camVertexInfo p;
                    p.point = (*tracksPoints)[ptid];
                    p.sim = -0.91;  // TODO FACA: why??
                    p.nrc = nrc;
                    p.ncams = sizeOfStaticVector<Pixel>((*tracksPointsCams)[ptid]);
                    p.fwriteinfo(fin);
                }
                fclose(fin);
            }  // for cams

            delete cams;
            delete tracksPoints;
            deleteArrayOfArrays<Pixel>(&camsTracksPoints);
            deleteArrayOfArrays<Pixel>(&tracksPointsCams);
        }

        mvsUtils::printfEstimate(i, nvoxs, t1);
    }  // for i
    mvsUtils::finishEstimate();
}

void VoxelsGrid::vizualize()
{
    std::string spaceWrlFileName = spaceRootDir + "tracks.wrl";
    FILE* f = fopen(spaceWrlFileName.c_str(), "w");
    fprintf(f, "#VRML V2.0 utf8\n");
    fprintf(f, "Background {\n skyColor 1 1 1 \n } \n");
    int nvoxs = voxels->size() / 8;
    for (int i = 0; i < nvoxs; i++)
    {
        std::string subFoldeName = getVoxelFolderName(i);
        std::string fname = subFoldeName + "tracks.wrl";
        if (bfs::is_directory(subFoldeName))
        {
            fprintf(f, "Inline{ url [\"%s\"] \n }\n", fname.c_str());
        }
    }
    fclose(f);
}

void VoxelsGrid::getHexah(Point3d* hexahOut, const Voxel& LUi, const Voxel& RDi)
{
    Voxel LU, RD;
    LU.x = std::min(LUi.x, RDi.x);
    LU.y = std::min(LUi.y, RDi.y);
    LU.z = std::min(LUi.z, RDi.z);
    RD.x = std::max(LUi.x, RDi.x);
    RD.y = std::max(LUi.y, RDi.y);
    RD.z = std::max(LUi.z, RDi.z);

    int luid = getIdForVoxel(LU);

    // Point3d *vox = &(*voxels)[0];
    // Point3d O   = vox[0];
    // Point3d vvx = vox[1]-vox[0];
    // Point3d vvy = vox[3]-vox[0];
    // Point3d vvz = vox[4]-vox[0];

    Voxel X, Y, Z;
    int Xid, Yid, Zid;
    X.x = RD.x;
    X.y = LU.y;
    X.z = LU.z;
    Xid = getIdForVoxel(X);
    Y.x = LU.x;
    Y.y = RD.y;
    Y.z = LU.z;
    Yid = getIdForVoxel(Y);
    Z.x = LU.x;
    Z.y = LU.y;
    Z.z = RD.z;
    Zid = getIdForVoxel(Z);

    Point3d O = (*voxels)[luid * 8];
    Point3d vvx = (*voxels)[Xid * 8 + 1] - O;
    Point3d vvy = (*voxels)[Yid * 8 + 3] - O;
    Point3d vvz = (*voxels)[Zid * 8 + 4] - O;

    hexahOut[0] = O;
    hexahOut[1] = O + vvx;
    hexahOut[2] = O + vvx + vvy;
    hexahOut[3] = O + vvy;
    hexahOut[4] = O + vvz;
    hexahOut[5] = O + vvz + vvx;
    hexahOut[6] = O + vvz + vvx + vvy;
    hexahOut[7] = O + vvz + vvy;
}

}  // namespace fuseCut
}  // namespace aliceVision
