#include "Benchmark.h"
#include <chrono>

uint32_t Benchmark::n = 1000;

void Benchmark::Run(uint32_t n)
{
  Benchmark::n = n;
  std::vector<SimpleVector> vectors = GenerateRandomVectors();
  auto PCLOctree = GeneratePCLOctree(vectors);
  auto COGSOctree = GenerateCOGSOctree(vectors);

  SimpleVector search_location;
  search_location.x = static_cast<float>(rand() % 1000) / 100.0f;
  search_location.y = static_cast<float>(rand() % 1000) / 100.0f;
  search_location.z = static_cast<float>(rand() % 1000) / 100.0f;

  // PCLOctree Radius Search
  auto start = std::chrono::high_resolution_clock::now();
  uint32_t pcl_points = PCLOctreeRadius(PCLOctree, search_location, 1);
  auto stop = std::chrono::high_resolution_clock::now();
  auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
  std::cout << "PCLOctree Radius Search took: " << duration.count() << " microseconds" << std::endl;
  std::cout << "PCLOctree Radius Search found: " << pcl_points << " points" << std::endl;

  // COGSOctree Radius Search
  start = std::chrono::high_resolution_clock::now();
  uint32_t cogs_points = COGSOctreeRadius(COGSOctree, search_location, 1);
  stop = std::chrono::high_resolution_clock::now();
  duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
  std::cout << "COGSOctree Radius Search took: " << duration.count() << " microseconds" << std::endl;
  std::cout << "COGSOctree Radius Search found: " << cogs_points << " points" << std::endl;

  // Naive Radius Search
  start = std::chrono::high_resolution_clock::now();
  uint32_t naive_points = NaiveRadius(vectors, search_location, 1);
  stop = std::chrono::high_resolution_clock::now();
  duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
  std::cout << "Naive Radius Search took: " << duration.count() << " microseconds" << std::endl;
  std::cout << "Naive Radius Search found: " << naive_points << " points" << std::endl;
}

std::vector<SimpleVector> Benchmark::GenerateRandomVectors()
{
  std::vector<SimpleVector> vectors;
  for (uint32_t i = 0; i < n; ++i)
  {
    SimpleVector vector;
    vector.x = static_cast<float>(rand() % 1000) / 100.0f;
    vector.y = static_cast<float>(rand() % 1000) / 100.0f;
    vector.z = static_cast<float>(rand() % 1000) / 100.0f;
    vectors.push_back(vector);
  }
  return vectors;
}

pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> Benchmark::GeneratePCLOctree(std::vector<SimpleVector> &points)
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
  cloud->width = n;
  cloud->height = 1;
  cloud->points.resize(cloud->width * cloud->height);
  for (uint32_t i = 0; i < n; ++i)
  {
    cloud->points[i].x = points[i].x;
    cloud->points[i].y = points[i].y;
    cloud->points[i].z = points[i].z;
  }
  pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(64);
  octree.setInputCloud(cloud);
  octree.addPointsFromInputCloud();
  return octree;
}

cogs::Octree Benchmark::GenerateCOGSOctree(std::vector<SimpleVector> &points)
{
  cogs::PointCloud pc;
  pc.Resize(n);
  glm::vec3 *v = pc.GetPositions();
  for (int i = 0; i < n; i++, v++)
  {
    v->x = points[i].x;
    v->y = points[i].y;
    v->z = points[i].z;
  }
  cogs::Octree octree(pc);
  return octree;
}

uint32_t Benchmark::PCLOctreeRadius(pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> &octree, SimpleVector search_location, float_t radius)
{
  pcl::PointXYZ location;
  location.x = search_location.x;
  location.y = search_location.y;
  location.z = search_location.z;
  std::vector<int> points_indices;
  std::vector<float> points_distances;
  octree.radiusSearch(location, radius, points_indices, points_distances);
  return points_indices.size();
}

uint32_t Benchmark::COGSOctreeRadius(cogs::Octree &octree, SimpleVector search_location, float_t radius)
{
  glm::vec3 location(search_location.x, search_location.y, search_location.z);
  std::vector <uint32_t> points_indices = octree.GetPointsInRadius(location, radius);
  return points_indices.size();
}

uint32_t Benchmark::NaiveRadius(std::vector<SimpleVector> &points, SimpleVector search_location, float_t radius)
{
  std::vector <uint32_t> points_indices;
  for (uint32_t i = 0; i < points.size(); i++)
  {
    float distance = std::sqrt(
        std::pow(points[i].x - search_location.x, 2) +
        std::pow(points[i].y - search_location.y, 2) +
        std::pow(points[i].z - search_location.z, 2));
    if (distance < radius)
    {
      points_indices.push_back(i);
    }
  }
  return points_indices.size();
}