/*
----
This file is part of SECONDO.

Copyright (C) 2019,
Faculty of Mathematics and Computer Science,
Database Systems for New Applications.

SECONDO is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.

SECONDO 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 General Public License for more details.

You should have received a copy of the GNU General Public License
along with SECONDO; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
----


//[<] [\ensuremath{<}]
//[>] [\ensuremath{>}]

\setcounter{tocdepth}{3}
\tableofcontents



1 Implementation of the Geometric Primitives - Helper Classes

*/
#include "GeomPrimitive.h"

#include <limits>

#include "../utility/DbScanPoint.h"
#include "../utility/BitArray.h"
#include "../utility/DbScan.h"
#include "../utility/MathUtils.h"

using namespace pointcloud2;
using namespace std;

/*
2 Implementation Parts of template GeomPrimitive

2.1 projectClusterSetObjID

*/
template<unsigned dual>
void GeomPrimitive<dual>::projectClusterSetObjID(
        const vector<SamplePoint>& sample,
        shared_ptr<MMRCloud> cloud,
        const ParamsAnalyzeGeom& params,
        shared_ptr<ObjectIdManager> objManager,
        Timer& timer) {

    timer.getReportStream() << timer.getInset()
            << getCaption(true) << endl;

    /* use the given sample points to calculate points in dual space each
     * of which represents a possible instance of this primitive. */
    timer.startTask("- projectSampleToDual");
    shared_ptr<vector<DbScanPoint<dual>>> dualPoints =
            this->projectSampleToDual(sample, params, cloud->_cloudBbox);

    /* now cluster the points in dual space, refining large clusters
     * and then getting the weighted center of each cluster. This results
     * in a vector of dual points each of which represents a shape we
     * expect to find in the Pointcloud2. */
    timer.startTask("- clusterAndReduceToCenters");
    vector<PointBase<dual>> shapes = this->clusterAndReduceToCenters(
            dualPoints, params);

    /* iterate over the given shapes, using the mmRTree to identify all
     * candidate points in the Pointcloud2 that may belong to the
     * respective shape. Cluster these points and assign the points in big
     * enough clusters to a new object id. */
    timer.startTask("- matchShapesWithPoints");
    this->matchShapesWithPoints(shapes, cloud, params, objManager);
}


/*
2.2 project sample to dual

*/
template<unsigned dual>
shared_ptr<vector<DbScanPoint<dual>>>
GeomPrimitive<dual>::projectSampleToDual(
        const vector<SamplePoint>& sample,
        const ParamsAnalyzeGeom& params,
        const Rectangle<DIMENSIONS>& bboxOfEntireCloud) {

    size_t sampleSize = sample.size();
    /* "sample" is a random sample of points from the whole
     * Pointcloud2 in an undefined order. If the cloud contains e.g. 100
     * shapes to be identified, and if e.g. dual = 4 (so 4 points are needed
     * to calculate a dual point), the chance of randomly selecting 4 points
     * from the _same_ shape is 1 out of 1 million (100^3). And we'd need
     * many such dual points to get a cluster in the dual space while
     * 99.9999% of the points in the dual space would be noise. So, random
     * selection does not work.
     *
     * Instead, we will insert the points from sample into an MMRTree.
     * For each point p, the MMRTree will give us more points that are close
     * enough to p to be part of the same shape. Only such tuples will be
     * used to create a dual point. Despite the costs of the MMRTree this
     * will be far more efficient than random selection.
     *
     * Once we have a "neighborhood" of one core point p, we use it for some
     * other points q, r, s, ... that are close to the core of this
     * neighborhood. We use a BitArray to remember which points have
     * already been treated. Points at the edges of a neighborhood will be
     * treated later (in the context of a different neighborhood to which
     * they are more central).
     */

    // create the result vector. For each point in sample, we will
    // create (TUPLE_COUNT_PER_CENTER) tuples.
    shared_ptr<vector<DbScanPoint<dual>>> result = std::make_shared<
            vector<DbScanPoint<dual>>>();
    // since we introduced an additional "challenge", not every sample point
    // is turned into a dual point, so we can omit the next line:
    // result->reserve(1 + params._dualPointsPerSamplePoint * sampleSize);

    // since DbScan needs _dualPoints to be 1-based, add an empty entry
    // that will not be used
    DbScanPoint<dual> dualPoint;
    dualPoint.initialize();
    dualPoint.clearCoords();
    result->push_back(dualPoint);

    // construct an MMRTree from the sample
    MMCloudTreeT tree = MMCloudTreeT();
    for (unsigned i = 0; i < sampleSize; ++i)
        tree.insert(sample[i].getBoundingBox2(), i );

    if (REPORT_TO_CONSOLE) {
        using std::cout;
        using std::endl;
        cout << endl << "==================== " << getCaption(true) << endl;
        cout << "using MMTree to create dual points "
                "from neighborhoods:" << endl;
        cout << "sampleSize = " << formatInt(sampleSize) << endl;
        cout << "tree.bbox = (" << DbScanPoint<DIMENSIONS>::getStringForBBox(
                tree.getBBox().toRectangleAttr()) << endl;
        cout << "neighborhoodDiameter = "
                << params._neighborhoodDiameter << endl;
        if (REPORT_DETAILED)
            cout << "points used as center points marked by *" << endl;
     }
    // construct a BitArray that holds "true" for every point in the sample
    // which still needs to be used as the "center point" for dual point
    // construction. Noise points with too few neighbor points cannot be
    // used as center points, therefore the term "candidates"
    BitArray centerCandidates { sampleSize , false };
    centerCandidates.initializeAll(true);

    // iterate over the points that have not been treated yet (note that bits
    // get changed during the process, so the iterator will not "stop" at
    // every point)
    vector<PointIndex> neighbors; // contains indices into sample
    size_t smallNeighborhoodCount = 0;
    size_t largeNeighborhoodCount = 0;
    auto bitsIter = centerCandidates.getIterator();
    int i;
    while ((i = bitsIter->nextUncached()) >= 0) {
        // get the point for which a neighborhood will be retrieved
        const SamplePoint& coreOfNeighborhood = sample[i];
        Rectangle2<DIMENSIONS> neighBbox = coreOfNeighborhood.getBoundingBox2(
                params._neighborhoodDiameter / 2.0);

        // retrieve the indices of all points in this neighborhood
        // (including the coreOfNeighborhood point itself)
        neighbors.clear(); // reuse the vector so it does not need to
                           // resize and allocate thousands of times
        auto treeIter = tree.find(neighBbox);
        const PointIndex* neighborPtr;
        while ((neighborPtr = treeIter->next()) != 0)
            neighbors.push_back(*neighborPtr);
        delete treeIter;

        if (REPORT_DETAILED) {
            std::cout << "neighborhood of " << i <<
                    (centerCandidates.get(i) ? " " : "*") << ":";
            for (PointIndex n : neighbors) {
                std::cout << " " << n <<
                        (centerCandidates.get(n) ? " " : "*");
            }
            std::cout << std::endl;
        }

        // if the neighborhood is too small, we must keep this core for later
        // and try with a higher neighborhoodRadius
        if (neighbors.size() < params._neighborhoodSizeMin) {
            ++smallNeighborhoodCount;
            continue;
        }
        ++largeNeighborhoodCount;

        // we use a BitArray to create random selections of (getTupleSize())
        // neighborIndices. The BitArray prevents tuples from containing the
        // same point twice
        BitArray neighborIndexSelector = BitArray(neighbors.size(), false);
        neighborIndexSelector.initializeAll(true);

        // now every point close to the core of the neighborhood (including
        // the core point itself) can be used to construct a tuple, unless the
        // point has already been used
        for (PointIndex neighborIndex = 0; neighborIndex < neighbors.size();
                ++neighborIndex) {
            PointIndex j = neighbors[neighborIndex];
            // if the point has already been used because it was in close
            // to the center of a previously treated neighborhood, skip it
            if (!centerCandidates.get(j))
                continue;

            // calculate the minimum distance from this point to the border
            // of the neighBbox to determine whether the point is close
            // enough to the center
            const SamplePoint& centerCandidate = sample[j];
            if (!centerCandidate.isCloseToCenter(neighBbox))
                continue; // this point is not close enough to the center

            // create tuples from this point and others from the neighborhood
            // and project them to the dual space
            neighborIndexSelector.set(neighborIndex, false);
            vector<DbScanPoint<dual>> projected_sample =
                    this->projectNeighborhoodToDual(neighbors, j,
                            neighborIndexSelector, sample, params,
                            bboxOfEntireCloud);

            result->insert(result->end(), projected_sample.begin(),
                    projected_sample.end());

            neighborIndexSelector.set(neighborIndex, true);

            // mark this center point as treated, so bitsIter will skip it
            // if it comes along this point later
            centerCandidates.set(j, false);
        }

        if (REPORT_DETAILED) {
            std::cout << " ->  changed to " << i <<
                    (centerCandidates.get(i) ? " " : "*") << ":";
            for (size_t n : neighbors) {
                std::cout << " " << n << (centerCandidates.get(n)
                        ? " " : "*");
            }
            std::cout << std::endl;
        }
    }

    if (REPORT_TO_CONSOLE) {
        std::cout << formatInt(result->size() - 1) << " dual points were "
                "constructed from " << formatInt(largeNeighborhoodCount)
                << " neighborhoods" << std::endl;
        std::cout << formatInt(smallNeighborhoodCount)
                << " neighborhoods were considered too small (< "
                << params._neighborhoodSizeMin << " points) to be used "
                "for dual point construction." << std::endl;
        std::cout << formatInt(centerCandidates.getTrueCount())
                << " points were never used as center points (noise?)"
                << std::endl;
    }
    // TODO: wenn mit diesem neighborhoodRadius viele Punkte noch nicht als
    // center points genutzt wurden (z.B. bei bits.getTrueCount() > 0.1 *
    // sampleSize), dann war vielleicht neighborhoodRadius insgesamt zu klein.
    // Dann neighborhoodRadius verdoppeln und für diese Punkte erneut über
    // bits iterieren? Oder: grundsätzlich einen zweiten Durchgang machen
    // und neighborhoodRadius dabei an einem typicalObjectExtent orientieren!

    return result;
}

/*
2.3 project neighborhood to dual

*/
template<unsigned dual>
vector<DbScanPoint<dual>> GeomPrimitive<dual>::projectNeighborhoodToDual(
        const vector<PointIndex>& neighbors,
        const PointIndex centerPoint,
        const BitArray& neighborIndexSelector,
        const vector<SamplePoint>& sample,
        const ParamsAnalyzeGeom& params,
        const Rectangle<DIMENSIONS>& bboxOfEntireCloud) {

    unsigned tupleSize = getTupleSize();
    vector<SamplePoint> tuple; // contains indices into sample
    tuple.reserve(tupleSize);
    vector<DbScanPoint<dual>> result;

    // construct tuples from center2 and randomly selected
    // points in the same neighborhood
    for (size_t t = 0; t < params._dualPointsPerSamplePoint; ++t) {
        // use point as center point
        tuple.push_back(sample[centerPoint]);

        // add further points from the neighborhood to the tuple;
        // these points may be at the edge of neighBbox
        // the iterator returns (tupleSize - 1) randomly selected, unique
        // values; the value (centerPoint) was already excluded
        auto iter = neighborIndexSelector.getIterator(tupleSize - 1);
        int neighborIndex;
        while ((neighborIndex = iter->next()) >= 0)
            tuple.push_back(sample[neighbors[neighborIndex]]);

        // create the point in dual space
        unique_ptr<vector<DbScanPoint<dual>>> projected_tuple =
                projectTupleToDual(tuple, params, bboxOfEntireCloud);

        if( projected_tuple ) {
            result.insert(result.end(), projected_tuple->begin(),
                    projected_tuple->end());
        }
        tuple.clear();
    }

    // the resulting points in sample each represent one
    // geometric shape in regular space
    return result;
}

/*
2.4 cluster and reduce to centers

*/
template<unsigned dual>
vector<PointBase<dual>> GeomPrimitive<dual>::clusterAndReduceToCenters(
        shared_ptr<vector<DbScanPoint<dual>>> dualPoints,
        const ParamsAnalyzeGeom& params) {

    DbScan<dual> dbscan { params._dualScanEps, params._dualScanMinPts,
        dualPoints, false };
    vector<PointBase<dual>> shapes = dbscan.runRecursive(
            params._dualScanRequiredClusterSize,
            params._dualScanMaxBboxExtent);

    return shapes;
}

/*
2.5 match shapes with points

*/
template<unsigned dual>
void GeomPrimitive<dual>::matchShapesWithPoints(
        const vector<PointBase<dual>>& shapes,
        shared_ptr<MMRCloud> cloud,
        const ParamsAnalyzeGeom& params,
        shared_ptr<ObjectIdManager> objManager) const{

    if (REPORT_TO_CONSOLE) {
        cout << "-------------------- Matching " << getCaption(false)
                << " shapes with actual points in cloud:" << endl << endl;
    }

    // create a filter which can be used to perform DbScan on a subset
    // of "points" (without copying the points to an additional array)
    std::shared_ptr<BitArray> scanFilter = std::make_shared<BitArray>(
            cloud->_points->size() + 1, true);

    // iterate over the shapes which were found in the dual space by DbScan
    unsigned shapeIndex = 0;
    for (const PointBase<dual> shape : shapes) {
        ++shapeIndex;
        if (REPORT_TO_CONSOLE) {
            cout << "matching " << getCaption(false) << " "
                    << formatInt(shapeIndex) << " / "
                    << formatInt(shapes.size()) << " with points in cloud"
                    << endl;
        }

        // get the predicates that enable us to efficiently determine whether
        // a given bbox or point possibly intersects with, or is part of
        // the current shape
        BboxPredicate bboxPredicate = this->getBboxPredicateForShape(shape,
                params);
        GeomPredicate pointPredicate = this->getPredicateForShape(shape,
                params);

        // points near this shape cannot simply be assigned to the shape
        // but need clustering first. Therefore, we initialize the scanFilter
        // and will set a "true" bit for each candidate point
        scanFilter->clear();

        // find sequences which are in a bbox that (possibly) intersects
        // with the current shape. The iterator returns the start index of
        // a sequence of points (as storing all individual points in the
        // mmRTree would use up too much memory). These sequences were
        // created in Pointcloud2::getAlloints(); each sequence represents
        // the points stored in the persistent RTree
        auto iter = cloud->_tree->find(&bboxPredicate);
        size_t matchLeafCount = 0; // the number of matching mmRTree leafs
        size_t testPointCount = 0;
        const PointIndex* startIndexPtr;
        while ((startIndexPtr = iter->next()) != 0) {
            // the mmRTree iterator found a leaf with a bbox that fulfills
            // the bboxPredicate
            ++matchLeafCount;

            // iterate over the points in this sequence
            PointIndex index = *startIndexPtr;
            do {
                // get the next point and test it
                DbScanPoint<DIMENSIONS>& point = cloud->_points->at(index);
                ++testPointCount;
                if (pointPredicate(point)) {
                    // found a candidate! Activate its bit in the scanFilter
                    scanFilter->set(index, true);
                    point._clusterId = SCAN_UNCLASSIFIED;
                }
                if (point._isLastInSeq)
                    break; // end of point sequence
                ++index;
            } while (true);
        }
        delete iter;

        if (REPORT_TO_CONSOLE) {
            size_t foundCount = scanFilter->getTrueCount();
            cout << formatInt(foundCount) << " points in "
                    << formatInt(matchLeafCount) << " mmRTree leafs found "
                    << "(= " << (foundCount / (double)testPointCount * 100.0)
                    << "% of tested points) for " << getCaption(false)
                    << " " << shape.toString() << endl;
        }

        // if no candidates were found, continue with next shape
        if (scanFilter->getTrueCount() == 0)
            continue;

        // now we need to cluster the candidates. For reasons of efficiency,
        // we do not copy them into a points vector of their own but rather
        // pass our scanFilter to DbScan which will then operate only on
        // the points that are "true" in the filter.
        DbScan<DIMENSIONS> dbscan { params._matchScanEps,
            params._matchScanMinPts, cloud->_points, scanFilter };
        dbscan.run();

        // DbScan has now entered _clusterIds into the candidate points.
        // From the clusters found by DbScan we now select those which we
        // actually want to keep. The same shape can lead to many clusters
        // (e.g. flat roofs on the same plane) and therefore to many objIds.
        size_t clusterCount = dbscan.getClusterCount();
        vector<int> newObjIDs(1 + clusterCount, SCAN_UNCLASSIFIED);
        size_t useObjCount = 0;
        for (size_t clusterId = 1; clusterId <= clusterCount; ++clusterId) {
            unsigned clusterSize = dbscan.getClusterSize(clusterId);
            unsigned clusterCohesion = dbscan.getClusterCohesion(clusterId);
            if (clusterCohesion >= params._matchScanRequiredCohesion) {
                // previously:
                // if (clusterSize >= params._matchScanRequiredClusterSize) {

                const Rectangle2<DIMENSIONS>& clusterBbox =
                        dbscan.getClusterBbox(clusterId);
                newObjIDs[clusterId] = objManager->addObject(
                        clusterSize, clusterCohesion, clusterBbox.Perimeter());
                ++useObjCount;
            } // otherwise, keep SCAN_UNCLASSIFIED as cluster is too small
        }

        // now enter the new clusterIds into the points, using scanFilter to
        // iterate over them. All this is still done within the main memory
        // and only at the very end made persistent in the Pointcloud2.
        // Note that a point that has previously been assigned to an object
        // may be reassigned now if the current object encompasses more points
        // than the previously assigned object. This is decided by the
        // objectManager.
        auto idIter = scanFilter->getIterator();
        int pointIndex;
        size_t assignCount = 0;
        while ((pointIndex = idIter->next()) >= 0) {
            DbScanPoint<DIMENSIONS>& point = cloud->_points->at(pointIndex);
            int newObjID = newObjIDs[point._clusterId];
            if (newObjID != SCAN_UNCLASSIFIED && objManager->
                    setNewObjIdIfObjectBigger(pointIndex, newObjID)) {
                // once assigned to an object, we could "deactivate" a point
                // to prevent it from being assigned to a different geometric
                // primitive, using cloud->_bitMask->set(pointIndex, false);
                // however, experiments have shown that the quality of the
                // analysis is much better if we allow the point to be
                // reassigned to an object that may fit much better than the
                // one found here.
                ++assignCount;
            }
        }

        if (REPORT_TO_CONSOLE) {
            cout << "+ " << formatInt(assignCount) << " points "
                    "assigned to " << useObjCount << " "
                    << (useObjCount == 1 ? "object" : "objects") << " in "
                    << getCaption(false) << " " << shape.toString
                    () << endl << endl;
        }

    } // next shape
}

/*
2.6 is point far outside

*/
template<unsigned dual>
bool GeomPrimitive<dual>::isPointFarOutside(const double x, const double y,
        const double z, const Rectangle<DIMENSIONS>& bbox) {
    // points are considered "far outside" if their distance to the bbox is
    // larger than (MAX_FACTOR * maximum bbox range) in at least one dimension
    const double MAX_FACTOR = 2.0;

    double dist = 0.0;
    dist = MAX(dist, bbox.MinD(0) - x);
    dist = MAX(dist, x - bbox.MaxD(0));
    dist = MAX(dist, bbox.MinD(1) - y);
    dist = MAX(dist, y - bbox.MaxD(1));
    dist = MAX(dist, bbox.MinD(2) - z);
    dist = MAX(dist, z - bbox.MaxD(2));
    if (dist == 0.0) // the point is inside the bbox
        return false;

    double diam = 0.0;
    for (unsigned i = 0; i < DIMENSIONS; ++i)
        diam = MAX(diam, bbox.MaxD(i) - bbox.MinD(i));

    return (dist / diam > MAX_FACTOR);
}
namespace pointcloud2 {
template class GeomPrimitive<3>;
template class GeomPrimitive<4>;
template class GeomPrimitive<5>;
template class GeomPrimitive<6>;
}