#ifndef CLUSTERSECTION_H
#define CLUSTERSECTION_H

#include <QObject>
#include "ct_itemdrawable/ct_pointcluster.h"
#include "ct_itemdrawable/ct_referencepoint.h"

class ClusterSection : public QObject
{
    Q_OBJECT
public:

    ClusterSection()
    {
        _zmin = std::numeric_limits<double>::max();
        _zmax = -std::numeric_limits<double>::max();
    }

    void addCluster(const CT_PointCluster* cluster)
    {
        _clusters.append(cluster);

        if (cluster->minZ() < _zmin) {_zmin = cluster->minZ();}
        if (cluster->maxZ() > _zmax) {_zmax = cluster->maxZ();}
    }

    void initRefPoints(double thickness)
    {
        // Init vectors
        double halfThickness = thickness / 2.0;
        for (double z = _zmin ; z <= _zmax ; z += thickness)
        {
            _refZvalues.push_back(z + halfThickness);
            _refPoints.push_back(Eigen::Vector4d(0,0,0,0));
        }

        // Compute barycenters
        for (int i = 0 ; i < _clusters.size() ; i++)
        {
            const CT_PointCluster* cluster = _clusters.at(i);

            CT_PointIterator itPt(cluster->pointCloudIndex());
            while (itPt.hasNext())
            {
                const CT_Point &point = itPt.next().currentPoint();

                bool found = false;
                for (int j = 0 ; !found && j < _refZvalues.size() ; j++)
                {
                    if (point(2) >= _refZvalues[j] - halfThickness &&
                            point(2) <  _refZvalues[j] + halfThickness)
                    {
                        _refPoints[j](0) += point(0);
                        _refPoints[j](1) += point(1);
                        _refPoints[j](2) += point(2);
                        _refPoints[j](3)++;
                        found = true;
                    }
                }
            }
        }

        // Finalize barycenters
        for (int j = 0 ; j < _refZvalues.size() ; j++)
        {
            if (_refPoints[j](3) > 0)
            {
                _refPoints[j](0) /= _refPoints[j](3);
                _refPoints[j](1) /= _refPoints[j](3);
                _refPoints[j](2) /= _refPoints[j](3);
                _refPoints[j](3) = 0;

            }
        }

        // Compute buffers
        for (int i = 0 ; i < _clusters.size() ; i++)
        {
            const CT_PointCluster* cluster = _clusters.at(i);
            CT_PointIterator itPt(cluster->pointCloudIndex());
            while (itPt.hasNext())
            {
                const CT_Point &point = itPt.next().currentPoint();

                bool found = false;
                for (int j = 0 ; !found && j < _refZvalues.size() ; j++)
                {
                    if (point(2) >= _refZvalues[j] - halfThickness &&
                            point(2) <  _refZvalues[j] + halfThickness)
                    {
                        double distance = std::sqrt(pow(_refPoints[j](0) - point(0), 2) + pow(_refPoints[j](1) - point(1), 2));
                                                        if (distance > _refPoints[j](3)) {_refPoints[j](3) = distance;}

                        found = true;
                    }
                }
            }
        }
    }

    void computeOutData(double thickness)
    {
        for (int j = 0 ; j < _refZvalues.size() ; j++)
        {
            CT_PointCluster* cluster = new CT_PointCluster();
            CT_ReferencePoint* refPoint = new CT_ReferencePoint(_refPoints[j](0), _refPoints[j](1), _refPoints[j](2), _refPoints[j](3));

            _outClusters.push_back(cluster);
            _outRefPoints.push_back(refPoint);
        }

        double halfThickness = thickness / 2.0;

        for (int cl = 0 ; cl < _clusters.size() ; cl++)
        {
            const CT_PointCluster* cluster = _clusters.at(cl);

            CT_PointIterator itPt(cluster->pointCloudIndex());
            while (itPt.hasNext())
            {
                const CT_Point &point = itPt.next().currentPoint();

                bool found = false;
                for (int j = 0 ; !found && j < _refZvalues.size() ; j++)
                {
                    if (point(2) >= _refZvalues[j] - halfThickness &&
                            point(2) <  _refZvalues[j] + halfThickness)
                    {
                        _outClusters[j]->addPoint(itPt.currentGlobalIndex());
                        found = true;
                    }
                }
            }
        }
    }


    static bool sortByLength (ClusterSection* a, ClusterSection* b) {

        double la = a->_zmax - a->_zmin;
        double lb = b->_zmax - b->_zmin;
        return (la < lb);
    }

    static ClusterSection* mergeSections(const ClusterSection *baseSection, const ClusterSection *testedSection, double thickness)
    {
        ClusterSection* mergedSection = new ClusterSection();

        for (const CT_PointCluster *cluster : baseSection->_clusters)
        {
            if (cluster != nullptr) {mergedSection->addCluster(cluster);}
        }

        for (const CT_PointCluster *cluster : testedSection->_clusters)
        {
            if (cluster != nullptr) {mergedSection->addCluster(cluster);}
        }

        mergedSection->initRefPoints(thickness);

        return mergedSection;
    }



    QList<const CT_PointCluster*>   _clusters;
    double                          _zmin;
    double                          _zmax;
    QVector<double>                 _refZvalues;
    QVector<Eigen::Vector4d>        _refPoints;

    QVector<CT_PointCluster*>       _outClusters;
    QVector<CT_ReferencePoint*>     _outRefPoints;


};

#endif // CLUSTERSECTION_H