#include "../../render/render.h"
#include <unordered_set>
#include "../../processPointClouds.h"
// using templates for processPointClouds so also include .cpp to help linker
#include "../../processPointClouds.cpp"
#include <random>
#include <algorithm>
#include <experimental/algorithm>

std::unordered_set<int> Ransac(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, int maxIterations, float distanceTol)
{
    std::unordered_set<int> inliersResult;
    srand(time(NULL));

    while(maxIterations--) {
        std::unordered_set<int> inliers; // store the inliers here in each iteration
        int sample_size {2};
        std::vector<pcl::PointXYZ> sample (sample_size);
        std::sample(cloud->points.begin(), cloud->points.end(), std::back_inserter(sample),
                sample_size, std::mt19937{std::random_device{}()});

        // fit a line ax + by+ c = 0 to the points in sample using (y0−y1)x+(x1−x0)y+(x0∗y1−x1∗y0)=0
        double x0 {sample.at(0).x}, y0 {sample.at(0).y}, x1 {sample.at(1).x}, y1 {sample.at(1).y};
        double a {y0 - y1};
        double b {x1 - x0};
        double c {x0 * y1 - x1 * y0};

        // find the distance to this line for all points in the cloud. If the distance is less than
        // distanceTol at the index of the point to inliers
        for(auto& point: cloud->points) {
            double distance = fabs(a * point.x - b * point.y - c) / sqrt(std::pow(a, 2) + std::pow(b, 2));
            if (distance < distanceTol) {
                // add the index of the point to inliers
                auto index = std::find(cloud->points.begin(), cloud->points.end(), point);
                inliers.emplace(index);
            }
        }
        if (inliers.size() > inliersResult.size()) { inliersResult = inliers; }
    }

    return inliersResult;
}

error: no member named 'sample' in namespace 'std'
            std::sample(cloud->points.begin(), cloud->points.end(), std::back_inserter(sample),

/Library/Developer/CommandLineTools/usr/include/c++/v1/algorithm:999:22: error: invalid operands to binary expression ('pcl::PointXYZ' and 'const pcl::PointXYZ')
        if (*__first == __value_)



/Library/Developer/CommandLineTools/usr/include/c++/v1/memory:1825:31: error: no viable conversion from 'std::__1::__wrap_iter<pcl::PointXYZ *>' to 'int'
            ::new((void*)__p) _Up(_VSTD::forward<_Args>(__args)...);