#include <ctime>
#include <cassert>
#include <cstdlib>
#include <iostream>
#include "mykdtree.hpp"

#define NTRY 3
#define ND 2

using namespace std;
using namespace CosmoTool;

typedef KDTree<ND,char> MyTree;
typedef KDCell<ND,char> MyCell;

MyCell *findNearest(MyTree::coords& xc, MyCell *cells, uint32_t Ncells)
{
    MyCell *near2 = 0;
    double R2 = INFINITY;
    for (int i = 0; i < Ncells; i++)
      {
	double d2 = 0;
	for (int j = 0; j < ND; j++)
	  {
	    double delta = xc[j]-cells[i].coord[j];
	    d2 += delta*delta;
	  }
	if (d2 < R2)
	  {
	    near2 = &cells[i];
	    R2 = d2;
	  }
      }
    return near2;
}

int main()
{
  uint32_t Ncells = 100000;
  MyCell *cells = new MyCell[Ncells];
  
  for (int i = 0; i < Ncells; i++)
    {
      cells[i].active = true;
      for (int l = 0; l < ND; l++)
	cells[i].coord[l] = drand48();
    }
  
  MyTree tree(cells, Ncells);
  
  MyTree::coords *xc = new MyTree::coords[NTRY];

  cout << "Generating seeds..." << endl;
  for (int k = 0; k < NTRY; k++)
    {
      for (int l = 0; l < ND; l++)
	xc[k][l] = drand48();
    }

  // Check consistency
  cout << "Check consistency..." << endl;
  MyCell **ngb = new MyCell *[12];
  
  for (int k = 0; k < NTRY; k++) {
    cout << "Seed = " << xc[k][0] << " " << xc[k][1] << " " << xc[k][2] << endl;
    tree.getNearestNeighbours(xc[k], 12, ngb);

    for (uint32_t i = 0; i < 12; i++)
      {
	double d2 = 0;
	for (int l = 0; l < 3; l++)
	  d2 += ({double delta = xc[k][l] - ngb[i]->coord[l]; delta*delta;});
	cout << ngb[i]->coord[0] << " " << ngb[i]->coord[1] << " " << ngb[i]->coord[2] << " " << sqrt(d2) << endl;
      }
  }
  
  return 0;
}