// SPDX-License-Identifier: LGPL-2.1-or-later

/***************************************************************************
 *   Copyright (c) 2004 Werner Mayer <wmayer[at]users.sourceforge.net>     *
 *                                                                         *
 *   This file is part of the FreeCAD CAx development system.              *
 *                                                                         *
 *   This library is free software; you can redistribute it and/or         *
 *   modify it under the terms of the GNU Library General Public           *
 *   License as published by the Free Software Foundation; either          *
 *   version 2 of the License, or (at your option) any later version.      *
 *                                                                         *
 *   This library  is distributed in the hope that it will be useful,      *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU Library General Public License for more details.                  *
 *                                                                         *
 *   You should have received a copy of the GNU Library General Public     *
 *   License along with this library; see the file COPYING.LIB. If not,    *
 *   write to the Free Software Foundation, Inc., 59 Temple Place,         *
 *   Suite 330, Boston, MA  02111-1307, USA                                *
 *                                                                         *
 ***************************************************************************/


#include "PointsGrid.h"


using namespace Points;

PointsGrid::PointsGrid(const PointKernel& rclM)
    : _pclPoints(&rclM)
    , _ulCtElements(0)
    , _ulCtGridsX(0)
    , _ulCtGridsY(0)
    , _ulCtGridsZ(0)
    , _fGridLenX(0.0F)
    , _fGridLenY(0.0F)
    , _fGridLenZ(0.0F)
    , _fMinX(0.0F)
    , _fMinY(0.0F)
    , _fMinZ(0.0F)
{
    PointsGrid::RebuildGrid();
}

PointsGrid::PointsGrid()
    : _pclPoints(nullptr)
    , _ulCtElements(0)
    , _ulCtGridsX(POINTS_CT_GRID)
    , _ulCtGridsY(POINTS_CT_GRID)
    , _ulCtGridsZ(POINTS_CT_GRID)
    , _fGridLenX(0.0F)
    , _fGridLenY(0.0F)
    , _fGridLenZ(0.0F)
    , _fMinX(0.0F)
    , _fMinY(0.0F)
    , _fMinZ(0.0F)
{}

PointsGrid::PointsGrid(const PointKernel& rclM, unsigned long ulX, unsigned long ulY, unsigned long ulZ)
    : _pclPoints(&rclM)
    , _ulCtElements(0)
    , _ulCtGridsX(0)
    , _ulCtGridsY(0)
    , _ulCtGridsZ(0)
    , _fGridLenX(0.0F)
    , _fGridLenY(0.0F)
    , _fGridLenZ(0.0F)
    , _fMinX(0.0F)
    , _fMinY(0.0F)
    , _fMinZ(0.0F)
{
    PointsGrid::Rebuild(ulX, ulY, ulZ);
}

PointsGrid::PointsGrid(const PointKernel& rclM, int iCtGridPerAxis)
    : _pclPoints(&rclM)
    , _ulCtElements(0)
    , _ulCtGridsX(0)
    , _ulCtGridsY(0)
    , _ulCtGridsZ(0)
    , _fGridLenX(0.0F)
    , _fGridLenY(0.0F)
    , _fGridLenZ(0.0F)
    , _fMinX(0.0F)
    , _fMinY(0.0F)
    , _fMinZ(0.0F)
{
    PointsGrid::Rebuild(iCtGridPerAxis);
}

PointsGrid::PointsGrid(const PointKernel& rclM, double fGridLen)
    : _pclPoints(&rclM)
    , _ulCtElements(0)
    , _ulCtGridsX(0)
    , _ulCtGridsY(0)
    , _ulCtGridsZ(0)
    , _fGridLenX(0.0F)
    , _fGridLenY(0.0F)
    , _fGridLenZ(0.0F)
    , _fMinX(0.0F)
    , _fMinY(0.0F)
    , _fMinZ(0.0F)
{
    Base::BoundBox3d clBBPts;  // = _pclPoints->GetBoundBox();
    for (const auto& pnt : *_pclPoints) {
        clBBPts.Add(pnt);
    }
    PointsGrid::Rebuild(
        std::max<unsigned long>((unsigned long)(clBBPts.LengthX() / fGridLen), 1),
        std::max<unsigned long>((unsigned long)(clBBPts.LengthY() / fGridLen), 1),
        std::max<unsigned long>((unsigned long)(clBBPts.LengthZ() / fGridLen), 1)
    );
}

void PointsGrid::Attach(const PointKernel& rclM)
{
    _pclPoints = &rclM;
    RebuildGrid();
}

void PointsGrid::Clear()
{
    _aulGrid.clear();
    _pclPoints = nullptr;
}

void PointsGrid::Rebuild(unsigned long ulX, unsigned long ulY, unsigned long ulZ)
{
    _ulCtGridsX = ulX;
    _ulCtGridsY = ulY;
    _ulCtGridsZ = ulZ;
    _ulCtElements = HasElements();
    RebuildGrid();
}

void PointsGrid::Rebuild(unsigned long ulPerGrid, unsigned long ulMaxGrid)
{
    _ulCtElements = HasElements();
    CalculateGridLength(ulPerGrid, ulMaxGrid);
    RebuildGrid();
}

void PointsGrid::Rebuild(int iCtGridPerAxis)
{
    _ulCtElements = HasElements();
    CalculateGridLength(iCtGridPerAxis);
    RebuildGrid();
}

void PointsGrid::InitGrid()
{
    assert(_pclPoints);

    // Calculate grid lengths if not initialized
    //
    if ((_ulCtGridsX == 0) || (_ulCtGridsY == 0) || (_ulCtGridsZ == 0)) {
        CalculateGridLength(POINTS_CT_GRID, POINTS_MAX_GRIDS);
    }

    // Determine the grid length and offset
    //
    {
        Base::BoundBox3d clBBPts;  // = _pclPoints->GetBoundBox();
        for (const auto& pnt : *_pclPoints) {
            clBBPts.Add(pnt);
        }

        double fLengthX = clBBPts.LengthX();
        double fLengthY = clBBPts.LengthY();
        double fLengthZ = clBBPts.LengthZ();

        {
            // Offset fGridLen/2
            //
            unsigned long num = _ulCtGridsX;
            if (num == 0) {
                num = 1;
            }
            _fGridLenX = (1.0F + fLengthX) / double(num);
            _fMinX = clBBPts.MinX - 0.5F;
        }

        {
            unsigned long num = _ulCtGridsY;
            if (num == 0) {
                num = 1;
            }
            _fGridLenY = (1.0F + fLengthY) / double(num);
            _fMinY = clBBPts.MinY - 0.5F;
        }

        {
            unsigned long num = _ulCtGridsZ;
            if (num == 0) {
                num = 1;
            }
            _fGridLenZ = (1.0F + fLengthZ) / double(num);
            _fMinZ = clBBPts.MinZ - 0.5F;
        }
    }

    // Create data structure
    _aulGrid.clear();
    _aulGrid.resize(_ulCtGridsX);
    for (unsigned long i = 0; i < _ulCtGridsX; i++) {
        _aulGrid[i].resize(_ulCtGridsY);
        for (unsigned long j = 0; j < _ulCtGridsY; j++) {
            _aulGrid[i][j].resize(_ulCtGridsZ);
        }
    }
}

unsigned long PointsGrid::InSide(
    const Base::BoundBox3d& rclBB,
    std::vector<unsigned long>& raulElements,
    bool bDelDoubles
) const
{
    unsigned long ulMinX {};
    unsigned long ulMinY {};
    unsigned long ulMinZ {};
    unsigned long ulMaxX {};
    unsigned long ulMaxY {};
    unsigned long ulMaxZ {};

    raulElements.clear();

    // Grid boxes for a more detailed selection
    Position(Base::Vector3d(rclBB.MinX, rclBB.MinY, rclBB.MinZ), ulMinX, ulMinY, ulMinZ);
    Position(Base::Vector3d(rclBB.MaxX, rclBB.MaxY, rclBB.MaxZ), ulMaxX, ulMaxY, ulMaxZ);

    for (auto i = ulMinX; i <= ulMaxX; i++) {
        for (auto j = ulMinY; j <= ulMaxY; j++) {
            for (auto k = ulMinZ; k <= ulMaxZ; k++) {
                raulElements
                    .insert(raulElements.end(), _aulGrid[i][j][k].begin(), _aulGrid[i][j][k].end());
            }
        }
    }

    if (bDelDoubles) {
        // remove duplicate mentions
        std::sort(raulElements.begin(), raulElements.end());
        raulElements.erase(std::unique(raulElements.begin(), raulElements.end()), raulElements.end());
    }

    return raulElements.size();
}

unsigned long PointsGrid::InSide(
    const Base::BoundBox3d& rclBB,
    std::vector<unsigned long>& raulElements,
    const Base::Vector3d& rclOrg,
    double fMaxDist,
    bool bDelDoubles
) const
{
    unsigned long ulMinX {}, ulMinY {}, ulMinZ {};
    unsigned long ulMaxX {}, ulMaxY {}, ulMaxZ {};
    double fGridDiag = GetBoundBox(0, 0, 0).CalcDiagonalLength();
    double fMinDistP2 = (fGridDiag * fGridDiag) + (fMaxDist * fMaxDist);

    raulElements.clear();

    // Grid boxes for a more detailed selection
    Position(Base::Vector3d(rclBB.MinX, rclBB.MinY, rclBB.MinZ), ulMinX, ulMinY, ulMinZ);
    Position(Base::Vector3d(rclBB.MaxX, rclBB.MaxY, rclBB.MaxZ), ulMaxX, ulMaxY, ulMaxZ);

    for (auto i = ulMinX; i <= ulMaxX; i++) {
        for (auto j = ulMinY; j <= ulMaxY; j++) {
            for (auto k = ulMinZ; k <= ulMaxZ; k++) {
                if (Base::DistanceP2(GetBoundBox(i, j, k).GetCenter(), rclOrg) < fMinDistP2) {
                    raulElements.insert(
                        raulElements.end(),
                        _aulGrid[i][j][k].begin(),
                        _aulGrid[i][j][k].end()
                    );
                }
            }
        }
    }

    if (bDelDoubles) {
        // remove duplicate mentions
        std::sort(raulElements.begin(), raulElements.end());
        raulElements.erase(std::unique(raulElements.begin(), raulElements.end()), raulElements.end());
    }

    return raulElements.size();
}

unsigned long PointsGrid::InSide(const Base::BoundBox3d& rclBB, std::set<unsigned long>& raulElements) const
{
    unsigned long ulMinX {}, ulMinY {}, ulMinZ {};
    unsigned long ulMaxX {}, ulMaxY {}, ulMaxZ {};

    raulElements.clear();

    // Grid boxes for a more detailed selection
    Position(Base::Vector3d(rclBB.MinX, rclBB.MinY, rclBB.MinZ), ulMinX, ulMinY, ulMinZ);
    Position(Base::Vector3d(rclBB.MaxX, rclBB.MaxY, rclBB.MaxZ), ulMaxX, ulMaxY, ulMaxZ);

    for (auto i = ulMinX; i <= ulMaxX; i++) {
        for (auto j = ulMinY; j <= ulMaxY; j++) {
            for (auto k = ulMinZ; k <= ulMaxZ; k++) {
                raulElements.insert(_aulGrid[i][j][k].begin(), _aulGrid[i][j][k].end());
            }
        }
    }

    return raulElements.size();
}

void PointsGrid::Position(
    const Base::Vector3d& rclPoint,
    unsigned long& rulX,
    unsigned long& rulY,
    unsigned long& rulZ
) const
{
    if (rclPoint.x <= _fMinX) {
        rulX = 0;
    }
    else {
        rulX = std::min<unsigned long>(
            (unsigned long)((rclPoint.x - _fMinX) / _fGridLenX),
            _ulCtGridsX - 1
        );
    }

    if (rclPoint.y <= _fMinY) {
        rulY = 0;
    }
    else {
        rulY = std::min<unsigned long>(
            (unsigned long)((rclPoint.y - _fMinY) / _fGridLenY),
            _ulCtGridsY - 1
        );
    }

    if (rclPoint.z <= _fMinZ) {
        rulZ = 0;
    }
    else {
        rulZ = std::min<unsigned long>(
            (unsigned long)((rclPoint.z - _fMinZ) / _fGridLenZ),
            _ulCtGridsZ - 1
        );
    }
}

void PointsGrid::CalculateGridLength(unsigned long ulCtGrid, unsigned long ulMaxGrids)
{
    // Calculate grid lengths or number of grids per dimension
    // There should be about 10 (?!?!) facets per grid
    // or max grids should not exceed 10000
    Base::BoundBox3d clBBPtsEnlarged;  // = _pclPoints->GetBoundBox();
    for (const auto& pnt : *_pclPoints) {
        clBBPtsEnlarged.Add(pnt);
    }
    double fVolElem {};

    if (_ulCtElements > (ulMaxGrids * ulCtGrid)) {
        fVolElem = (clBBPtsEnlarged.LengthX() * clBBPtsEnlarged.LengthY() * clBBPtsEnlarged.LengthZ())
            / float(ulMaxGrids * ulCtGrid);
    }
    else {
        fVolElem = (clBBPtsEnlarged.LengthX() * clBBPtsEnlarged.LengthY() * clBBPtsEnlarged.LengthZ())
            / float(_ulCtElements);
    }

    double fVol = fVolElem * float(ulCtGrid);
    double fGridLen = float(pow((float)fVol, 1.0F / 3.0F));

    if (fGridLen > 0) {
        _ulCtGridsX = std::max<unsigned long>((unsigned long)(clBBPtsEnlarged.LengthX() / fGridLen), 1);
        _ulCtGridsY = std::max<unsigned long>((unsigned long)(clBBPtsEnlarged.LengthY() / fGridLen), 1);
        _ulCtGridsZ = std::max<unsigned long>((unsigned long)(clBBPtsEnlarged.LengthZ() / fGridLen), 1);
    }
    else {
        // Degenerated grid
        _ulCtGridsX = 1;
        _ulCtGridsY = 1;
        _ulCtGridsZ = 1;
    }
}

void PointsGrid::CalculateGridLength(int iCtGridPerAxis)
{
    if (iCtGridPerAxis <= 0) {
        CalculateGridLength(POINTS_CT_GRID, POINTS_MAX_GRIDS);
        return;
    }

    // Calculate grid lengths or number of grids per dimension
    // There should be about 10 (?!?!) facets per grid
    // or max grids should not exceed 10000
    Base::BoundBox3d clBBPts;  // = _pclPoints->GetBoundBox();
    for (const auto& pnt : *_pclPoints) {
        clBBPts.Add(pnt);
    }

    double fLenghtX = clBBPts.LengthX();
    double fLenghtY = clBBPts.LengthY();
    double fLenghtZ = clBBPts.LengthZ();

    double fLenghtD = clBBPts.CalcDiagonalLength();

    double fLengthTol = 0.05F * fLenghtD;

    bool bLenghtXisZero = (fLenghtX <= fLengthTol);
    bool bLenghtYisZero = (fLenghtY <= fLengthTol);
    bool bLenghtZisZero = (fLenghtZ <= fLengthTol);

    int iFlag = 0;

    int iMaxGrids = 1;

    if (bLenghtXisZero) {
        iFlag += 1;
    }
    else {
        iMaxGrids *= iCtGridPerAxis;
    }

    if (bLenghtYisZero) {
        iFlag += 2;
    }
    else {
        iMaxGrids *= iCtGridPerAxis;
    }

    if (bLenghtZisZero) {
        iFlag += 4;
    }
    else {
        iMaxGrids *= iCtGridPerAxis;
    }

    unsigned long ulGridsFacets = 10;

    double fFactorVolumen = 40.0;
    double fFactorArea = 10.0;

    switch (iFlag) {
        case 0: {
            double fVolumen = fLenghtX * fLenghtY * fLenghtZ;

            double fVolumenGrid = (fVolumen * ulGridsFacets) / (fFactorVolumen * _ulCtElements);

            if ((fVolumenGrid * iMaxGrids) < fVolumen) {
                fVolumenGrid = fVolumen / (float)iMaxGrids;
            }

            double fLengthGrid = float(pow((float)fVolumenGrid, 1.0F / 3.0F));

            _ulCtGridsX = std::max<unsigned long>((unsigned long)(fLenghtX / fLengthGrid), 1);
            _ulCtGridsY = std::max<unsigned long>((unsigned long)(fLenghtY / fLengthGrid), 1);
            _ulCtGridsZ = std::max<unsigned long>((unsigned long)(fLenghtZ / fLengthGrid), 1);

        } break;
        case 1: {
            _ulCtGridsX = 1;  // bLenghtXisZero

            double fArea = fLenghtY * fLenghtZ;

            double fAreaGrid = (fArea * ulGridsFacets) / (fFactorArea * _ulCtElements);

            if ((fAreaGrid * iMaxGrids) < fArea) {
                fAreaGrid = fArea / (double)iMaxGrids;
            }

            double fLengthGrid = double(sqrt(fAreaGrid));

            _ulCtGridsY = std::max<unsigned long>((unsigned long)(fLenghtY / fLengthGrid), 1);
            _ulCtGridsZ = std::max<unsigned long>((unsigned long)(fLenghtZ / fLengthGrid), 1);
        } break;
        case 2: {
            _ulCtGridsY = 1;  // bLenghtYisZero

            double fArea = fLenghtX * fLenghtZ;

            double fAreaGrid = (fArea * ulGridsFacets) / (fFactorArea * _ulCtElements);

            if ((fAreaGrid * iMaxGrids) < fArea) {
                fAreaGrid = fArea / (double)iMaxGrids;
            }

            double fLengthGrid = double(sqrt(fAreaGrid));

            _ulCtGridsX = std::max<unsigned long>((unsigned long)(fLenghtX / fLengthGrid), 1);
            _ulCtGridsZ = std::max<unsigned long>((unsigned long)(fLenghtZ / fLengthGrid), 1);
        } break;
        case 3: {
            _ulCtGridsX = 1;          // bLenghtXisZero
            _ulCtGridsY = 1;          // bLenghtYisZero
            _ulCtGridsZ = iMaxGrids;  // bLenghtYisZero
        } break;
        case 4: {
            _ulCtGridsZ = 1;  // bLenghtZisZero

            double fArea = fLenghtX * fLenghtY;

            double fAreaGrid = (fArea * ulGridsFacets) / (fFactorArea * _ulCtElements);

            if ((fAreaGrid * iMaxGrids) < fArea) {
                fAreaGrid = fArea / (float)iMaxGrids;
            }

            double fLengthGrid = double(sqrt(fAreaGrid));

            _ulCtGridsX = std::max<unsigned long>((unsigned long)(fLenghtX / fLengthGrid), 1);
            _ulCtGridsY = std::max<unsigned long>((unsigned long)(fLenghtY / fLengthGrid), 1);
        } break;
        case 5: {
            _ulCtGridsX = 1;          // bLenghtXisZero
            _ulCtGridsZ = 1;          // bLenghtZisZero
            _ulCtGridsY = iMaxGrids;  // bLenghtYisZero
        } break;
        case 6: {
            _ulCtGridsY = 1;          // bLenghtYisZero
            _ulCtGridsZ = 1;          // bLenghtZisZero
            _ulCtGridsX = iMaxGrids;  // bLenghtYisZero
        } break;
        case 7: {
            _ulCtGridsX = iMaxGrids;  // bLenghtXisZero
            _ulCtGridsY = iMaxGrids;  // bLenghtYisZero
            _ulCtGridsZ = iMaxGrids;  // bLenghtZisZero
        } break;
    }
}

void PointsGrid::SearchNearestFromPoint(const Base::Vector3d& rclPt, std::set<unsigned long>& raclInd) const
{
    raclInd.clear();
    Base::BoundBox3d clBB = GetBoundBox();

    if (clBB.IsInBox(rclPt)) {  // Point lies within
        unsigned long ulX {};
        unsigned long ulY {};
        unsigned long ulZ {};
        Position(rclPt, ulX, ulY, ulZ);
        unsigned long ulLevel = 0;
        while (raclInd.empty()) {
            GetHull(ulX, ulY, ulZ, ulLevel++, raclInd);
        }
        GetHull(ulX, ulY, ulZ, ulLevel, raclInd);
    }
    else {  // Point outside
        Base::BoundBox3d::SIDE tSide = clBB.GetSideFromRay(rclPt, clBB.GetCenter() - rclPt);
        switch (tSide) {
            case Base::BoundBox3d::RIGHT: {
                int nX = 0;
                while (raclInd.empty()) {
                    for (unsigned long i = 0; i < _ulCtGridsY; i++) {
                        for (unsigned long j = 0; j < _ulCtGridsZ; j++) {
                            raclInd.insert(_aulGrid[nX][i][j].begin(), _aulGrid[nX][i][j].end());
                        }
                    }
                    nX++;
                }
                break;
            }
            case Base::BoundBox3d::LEFT: {
                int nX = _ulCtGridsX - 1;
                while (raclInd.empty()) {
                    for (unsigned long i = 0; i < _ulCtGridsY; i++) {
                        for (unsigned long j = 0; j < _ulCtGridsZ; j++) {
                            raclInd.insert(_aulGrid[nX][i][j].begin(), _aulGrid[nX][i][j].end());
                        }
                    }
                    nX++;
                }
                break;
            }
            case Base::BoundBox3d::TOP: {
                int nY = 0;
                while (raclInd.empty()) {
                    for (unsigned long i = 0; i < _ulCtGridsX; i++) {
                        for (unsigned long j = 0; j < _ulCtGridsZ; j++) {
                            raclInd.insert(_aulGrid[i][nY][j].begin(), _aulGrid[i][nY][j].end());
                        }
                    }
                    nY++;
                }
                break;
            }
            case Base::BoundBox3d::BOTTOM: {
                int nY = _ulCtGridsY - 1;
                while (raclInd.empty()) {
                    for (unsigned long i = 0; i < _ulCtGridsX; i++) {
                        for (unsigned long j = 0; j < _ulCtGridsZ; j++) {
                            raclInd.insert(_aulGrid[i][nY][j].begin(), _aulGrid[i][nY][j].end());
                        }
                    }
                    nY--;
                }
                break;
            }
            case Base::BoundBox3d::BACK: {
                int nZ = 0;
                while (raclInd.empty()) {
                    for (unsigned long i = 0; i < _ulCtGridsX; i++) {
                        for (unsigned long j = 0; j < _ulCtGridsY; j++) {
                            raclInd.insert(_aulGrid[i][j][nZ].begin(), _aulGrid[i][j][nZ].end());
                        }
                    }
                    nZ++;
                }
                break;
            }
            case Base::BoundBox3d::FRONT: {
                int nZ = _ulCtGridsZ - 1;
                while (raclInd.empty()) {
                    for (unsigned long i = 0; i < _ulCtGridsX; i++) {
                        for (unsigned long j = 0; j < _ulCtGridsY; j++) {
                            raclInd.insert(_aulGrid[i][j][nZ].begin(), _aulGrid[i][j][nZ].end());
                        }
                    }
                    nZ--;
                }
                break;
            }

            default:
                break;
        }
    }
}

void PointsGrid::GetHull(
    unsigned long ulX,
    unsigned long ulY,
    unsigned long ulZ,
    unsigned long ulDistance,
    std::set<unsigned long>& raclInd
) const
{
    int nX1 = std::max<int>(0, int(ulX) - int(ulDistance));
    int nY1 = std::max<int>(0, int(ulY) - int(ulDistance));
    int nZ1 = std::max<int>(0, int(ulZ) - int(ulDistance));
    int nX2 = std::min<int>(int(_ulCtGridsX) - 1, int(ulX) + int(ulDistance));
    int nY2 = std::min<int>(int(_ulCtGridsY) - 1, int(ulY) + int(ulDistance));
    int nZ2 = std::min<int>(int(_ulCtGridsZ) - 1, int(ulZ) + int(ulDistance));

    // top plane
    for (int i = nX1; i <= nX2; i++) {
        for (int j = nY1; j <= nY2; j++) {
            GetElements(i, j, nZ1, raclInd);
        }
    }
    // bottom plane
    for (int i = nX1; i <= nX2; i++) {
        for (int j = nY1; j <= nY2; j++) {
            GetElements(i, j, nZ2, raclInd);
        }
    }
    // left plane
    for (int i = nY1; i <= nY2; i++) {
        for (int j = (nZ1 + 1); j <= (nZ2 - 1); j++) {
            GetElements(nX1, i, j, raclInd);
        }
    }
    // right plane
    for (int i = nY1; i <= nY2; i++) {
        for (int j = (nZ1 + 1); j <= (nZ2 - 1); j++) {
            GetElements(nX2, i, j, raclInd);
        }
    }
    // front plane
    for (int i = (nX1 + 1); i <= (nX2 - 1); i++) {
        for (int j = (nZ1 + 1); j <= (nZ2 - 1); j++) {
            GetElements(i, nY1, j, raclInd);
        }
    }
    // back plane
    for (int i = (nX1 + 1); i <= (nX2 - 1); i++) {
        for (int j = (nZ1 + 1); j <= (nZ2 - 1); j++) {
            GetElements(i, nY2, j, raclInd);
        }
    }
}

unsigned long PointsGrid::GetElements(
    unsigned long ulX,
    unsigned long ulY,
    unsigned long ulZ,
    std::set<unsigned long>& raclInd
) const
{
    const std::set<unsigned long>& rclSet = _aulGrid[ulX][ulY][ulZ];
    if (!rclSet.empty()) {
        raclInd.insert(rclSet.begin(), rclSet.end());
        return rclSet.size();
    }

    return 0;
}

void PointsGrid::AddPoint(const Base::Vector3d& rclPt, unsigned long ulPtIndex, float /*fEpsilon*/)
{
    unsigned long ulX {}, ulY {}, ulZ {};
    Pos(Base::Vector3d(rclPt.x, rclPt.y, rclPt.z), ulX, ulY, ulZ);
    if ((ulX < _ulCtGridsX) && (ulY < _ulCtGridsY) && (ulZ < _ulCtGridsZ)) {
        _aulGrid[ulX][ulY][ulZ].insert(ulPtIndex);
    }
}

void PointsGrid::Validate(const PointKernel& rclPoints)
{
    if (_pclPoints != &rclPoints) {
        Attach(rclPoints);
    }
    else if (rclPoints.size() != _ulCtElements) {
        RebuildGrid();
    }
}

void PointsGrid::Validate()
{
    if (!_pclPoints) {
        return;
    }

    if (_pclPoints->size() != _ulCtElements) {
        RebuildGrid();
    }
}

bool PointsGrid::Verify() const
{
    if (!_pclPoints) {
        return false;  // no point cloud attached
    }
    if (_pclPoints->size() != _ulCtElements) {
        return false;  // not up-to-date
    }

    PointsGridIterator it(*this);
    for (it.Init(); it.More(); it.Next()) {
        std::vector<unsigned long> aulElements;
        it.GetElements(aulElements);
        for (unsigned long element : aulElements) {
            const Base::Vector3d& cP = _pclPoints->getPoint(element);
            if (!it.GetBoundBox().IsInBox(cP)) {
                return false;  // point doesn't lie inside the grid element
            }
        }
    }

    return true;
}

void PointsGrid::RebuildGrid()
{
    _ulCtElements = _pclPoints->size();

    InitGrid();

    // Fill data structure

    unsigned long i = 0;
    for (const auto& pnt : *_pclPoints) {
        AddPoint(pnt, i++);
    }
}

void PointsGrid::Pos(
    const Base::Vector3d& rclPoint,
    unsigned long& rulX,
    unsigned long& rulY,
    unsigned long& rulZ
) const
{
    rulX = (unsigned long)((rclPoint.x - _fMinX) / _fGridLenX);
    rulY = (unsigned long)((rclPoint.y - _fMinY) / _fGridLenY);
    rulZ = (unsigned long)((rclPoint.z - _fMinZ) / _fGridLenZ);
}

unsigned long PointsGrid::FindElements(
    const Base::Vector3d& rclPoint,
    std::set<unsigned long>& aulElements
) const
{
    unsigned long ulX {}, ulY {}, ulZ {};
    Pos(rclPoint, ulX, ulY, ulZ);

    // check if the given point is inside the grid structure
    if ((ulX < _ulCtGridsX) && (ulY < _ulCtGridsY) && (ulZ < _ulCtGridsZ)) {
        return GetElements(ulX, ulY, ulZ, aulElements);
    }

    return 0;
}

// ----------------------------------------------------------------

PointsGridIterator::PointsGridIterator(const PointsGrid& rclG)
    : _rclGrid(rclG)
    , _clPt(0.0F, 0.0F, 0.0F)
    , _clDir(0.0F, 0.0F, 0.0F)
{}

bool PointsGridIterator::InitOnRay(
    const Base::Vector3d& rclPt,
    const Base::Vector3d& rclDir,
    float fMaxSearchArea,
    std::vector<unsigned long>& raulElements
)
{
    bool ret = InitOnRay(rclPt, rclDir, raulElements);
    _fMaxSearchArea = fMaxSearchArea;
    return ret;
}

bool PointsGridIterator::InitOnRay(
    const Base::Vector3d& rclPt,
    const Base::Vector3d& rclDir,
    std::vector<unsigned long>& raulElements
)
{
    // needed in NextOnRay() to avoid an infinite loop
    _cSearchPositions.clear();

    _fMaxSearchArea = std::numeric_limits<float>::max();

    raulElements.clear();

    _clPt = rclPt;
    _clDir = rclDir;
    _bValidRay = false;

    // point lies within global BB
    if (_rclGrid.GetBoundBox().IsInBox(rclPt)) {  // determine the voxel by the starting point
        _rclGrid.Position(rclPt, _ulX, _ulY, _ulZ);
        raulElements.insert(
            raulElements.end(),
            _rclGrid._aulGrid[_ulX][_ulY][_ulZ].begin(),
            _rclGrid._aulGrid[_ulX][_ulY][_ulZ].end()
        );
        _bValidRay = true;
    }
    else {  // StartPoint outside
        Base::Vector3d cP0, cP1;
        if (_rclGrid.GetBoundBox().IntersectWithLine(
                rclPt,
                rclDir,
                cP0,
                cP1
            )) {  // determine the next point
            if ((cP0 - rclPt).Length() < (cP1 - rclPt).Length()) {
                _rclGrid.Position(cP0, _ulX, _ulY, _ulZ);
            }
            else {
                _rclGrid.Position(cP1, _ulX, _ulY, _ulZ);
            }

            raulElements.insert(
                raulElements.end(),
                _rclGrid._aulGrid[_ulX][_ulY][_ulZ].begin(),
                _rclGrid._aulGrid[_ulX][_ulY][_ulZ].end()
            );
            _bValidRay = true;
        }
    }

    return _bValidRay;
}

bool PointsGridIterator::NextOnRay(std::vector<unsigned long>& raulElements)
{
    if (!_bValidRay) {
        return false;  // not initialized or beam exited
    }

    raulElements.clear();

    Base::Vector3d clIntersectPoint;

    // Look for the next adjacent BB on the search beam
    Base::BoundBox3d::SIDE tSide
        = _rclGrid.GetBoundBox(_ulX, _ulY, _ulZ).GetSideFromRay(_clPt, _clDir, clIntersectPoint);

    // Search area
    //
    if ((_clPt - clIntersectPoint).Length() > _fMaxSearchArea) {
        _bValidRay = false;
    }
    else {
        switch (tSide) {
            case Base::BoundBox3d::LEFT:
                _ulX--;
                break;
            case Base::BoundBox3d::RIGHT:
                _ulX++;
                break;
            case Base::BoundBox3d::BOTTOM:
                _ulY--;
                break;
            case Base::BoundBox3d::TOP:
                _ulY++;
                break;
            case Base::BoundBox3d::FRONT:
                _ulZ--;
                break;
            case Base::BoundBox3d::BACK:
                _ulZ++;
                break;

            default:
            case Base::BoundBox3d::INVALID:
                _bValidRay = false;
                break;
        }

        GridElement pos(_ulX, _ulY, _ulZ);
        if (_cSearchPositions.find(pos) != _cSearchPositions.end()) {
            _bValidRay = false;  // grid element already visited => result from GetSideFromRay invalid
        }
    }

    if (_bValidRay && _rclGrid.CheckPos(_ulX, _ulY, _ulZ)) {
        GridElement pos(_ulX, _ulY, _ulZ);
        _cSearchPositions.insert(pos);
        raulElements.insert(
            raulElements.end(),
            _rclGrid._aulGrid[_ulX][_ulY][_ulZ].begin(),
            _rclGrid._aulGrid[_ulX][_ulY][_ulZ].end()
        );
    }
    else {
        _bValidRay = false;  // ray exited
    }

    return _bValidRay;
}