// Time-stamp: </b1/wrf/c/allclose/allnearpairs.cc, Wed, 24 Feb 2010, 20:56:17 EST, http://wrfranklin.org/>

// Process a set of points in E3 to find all points closer than a given radius to each point. 

// Documentation at http://www.ecse.rpi.edu/Homepages/wrf/pmwiki/Research/AllNearPairs

// W. Randolph Franklin, Professor
// ECSE Dept., 6026 JEC,
// Rensselaer Polytechnic Inst,
// 110 8th St,
// Troy NY, 12180 USA
// 
// mailATwrfranklin.org, frankwrATrpi.edu or wrfATecse.rpi.edu 
// +1 (518) 276-6077
// Fax: +1 (518) 276-6261
// 
// http://wrfranklin.org/

// Feb 2010.

// Disable boost.array subscript checking.
#define BOOST_DISABLE_ASSERTS

// Includes
#include <boost/multi_array.hpp> 
#include <algorithm>
#include <iomanip>
#include <iostream>
#include <fstream>
#include <math.h>
#include <omp.h>
#include <sys/resource.h>
#include <sys/stat.h>
#include <sys/time.h>
#include <sys/times.h>
#include <sys/types.h>
#include <time.h>
#include <unistd.h>

// Usings
using namespace std;
using boost::array;


// Debugging macros, constants, variables
// When printed, switch the output foreground or background to the specified color in bold,
// or to the default.
int printcolors(0);   // Use the following colors in PRINTC etc. or not?
const string redtty("\033[1;31m");   // tell tty to switch to bold red
const string bluetty("\033[1;34m");   // tell tty to switch to bold blue
const string greentty("\033[1;32m");   // tell tty to switch to bold green
const string yellowtty("\033[1;33m");   // tell tty to switch to bold yellow
const string deftty("\033[0m");      // tell tty to switch back to default color
const string blackbgtty("\033[40m");      // tell tty to switch background to black
const string defbgtty("\033[47m");      // tell tty to switch background to white

// Print an expression's name (perhaps in color) then its value, possible followed by a comma
// or endl.  Ex: cout << PRINTC(x) << PRINTN(y);

#define PRINT0(arg)  (printcolors?redtty:"") << (arg) << (printcolors?deftty:"") 
#define PRINT(arg)  (printcolors?redtty:"") << #arg << (printcolors?deftty:"") << "= " << (arg)
#define PRINTC(arg)  PRINT(arg) << ", "
#define PRINTN(arg)  PRINT(arg) << endl
#define EPRINTC(arg) cerr << PRINTC(arg)
#define EPRINTN(arg) cerr << PRINTN(arg)
#define OPRINTC(arg) cout << PRINTC(arg)
#define OPRINTN(arg) cout << PRINTN(arg)

// Macros
// Compact FOR loop.   Ex:   FOR(i,0,3,a[i]=0);
#define FOR(foriterator,formin,formaxplus1,forbody) {for(int foriterator=formin; foriterator<formaxplus1; foriterator++) {forbody;};}

// Expand and stringify a macro.
#define EXPANDANDSTRING(macro) EXPANDANDSTRING2(macro)
#define EXPANDANDSTRING2(macro)  #macro

// Typedefs
typedef array<double,3> PtT;

///////////// User sets the following and recompiles ///////////
// Input size
const int mp(20903);        // Max # points.
const double r(0.5);        // Radius of interest.
const double scale(0.07692);  // Scale factor to reduce to 1x1x1 cube.

// Other constants
const int mg(51);             // Max # grid cells in each direction.
const int mpc(100);           // Max allowable # points per cell.
const int mnpp(mp*50);       // Max # near point pairs.

/////////////// End of user settings /////////////////


// Variables
int np;                       // # points.
int ng;                       // Grid resolution.
array<PtT,mp> p;              // The points.
array<array<array<int,mg>,mg>,mg> npc;  // # points per cell.
array<array<array<array<int,mpc>,mg>,mg>,mg> g;  // The uniform grid, containing the point ids.
array<int,mpc+1> ppc;      // Histogram of # points per cell.
int nnpp;                     // # near point pairs.
array<array<int,2>,mnpp>  npp;// Near point pairs.
float clocks_per_sec = 0.0;   // will be set later.
double tottime;

const int nreps(1);     // How many times to repeat for better timing accuracy.
int irep;

// Debugging variables
int NPairsTested(0);
int NPairsPassed(0);

double Process_CPU_Time () { // Return CPU (user+system) time since start of process.
  struct tms *time_buffer = new tms;
  (void) times (time_buffer);
  return ((time_buffer->tms_utime + time_buffer->tms_stime) / clocks_per_sec); }

double Read_Delta_Time (double &new_time) { // Returns time in seconds since last call.
  //  Automatically initializes itself on 1st call and
  //  returns 0.  Also, return time from process start in its arg.
  static double old_time = 0.0;
  double  delta;
  new_time = Process_CPU_Time ();
  delta = new_time - old_time;
  old_time = new_time;
  return delta;  }

void Print_Time (string msg) { // Print time since last call, with a message. 
  double  incrtime = Read_Delta_Time (tottime);
  cout << (printcolors?bluetty:"") << "CPU Time " << msg << " =" << tottime << ", increment="
       << incrtime  << (printcolors?deftty:"") << endl;  }


// Routines

ostream &operator<<(ostream &o, const PtT p) {
  o << setw(7) << p[0] << ' ' << setw(7) << p[1] << ' ' << setw(7) << p[2];
  return o; }


inline int CtoG(const double x) {   // Convert a coord in legal range to a grid cell #.
  return x*(scale*ng);   }


void InsertPt(const int i) {  // Insert a point into the grid.
  const PtT &thisp(p[i]);
  const int ix=CtoG(thisp[0]);
  const int iy=CtoG(thisp[1]);
  const int iz=CtoG(thisp[2]);
  g[ix][iy][iz][npc[ix][iy][iz]++] = i;
  if (npc[ix][iy][iz]==mpc) { cerr << "ERR: Too many pts per cell " << PRINTC(i) << PRINTC(thisp) << PRINTC(ix) << PRINTC(iy) << PRINTN(iz);
    npc[ix][iy][iz]--;   }}


void InsertPts() {  FOR(i,0,np,InsertPt(i)); }


inline double sqr(const double x) { return x*x; }


inline double Dist2(const int i, const int j) { // Find Dist^2 between 2 points.
  const PtT &pi(p[i]);
  const PtT &pj(p[j]);
  return sqr(pi[0]-pj[0])+sqr(pi[1]-pj[1])+sqr(pi[2]-pj[2]); }


void FindNearPts() {
  for(int ix=0; ix<ng; ix++) {  // Iterate thru the cells.
    const int ixl=max(0,ix-1);
    const int ixh=min(ng-1,ix+1);
    for(int iy=0; iy<ng; iy++) {
      const int iyl=max(0,iy-1);
      const int iyh=min(ng-1,iy+1);
      for(int iz=0; iz<ng; iz++) { // Process cell (ix,iy,iz).
	const int ip(npc[ix][iy][iz]);   // # points in this cell.
	if (ip==0) continue;
	const int izl=max(0,iz-1);
	const int izh=min(ng-1,iz+1);
	for (int jx=ixl; jx<=ixh; jx++) // Iterate thru the nearby cells, i.e., +-1.
	  for (int jy=iyl; jy<=iyh; jy++)
	    for (int jz=izl; jz<=izh; jz++) { // Process cell (jx,jy,jz).
	      const int jp(npc[jx][jy][jz]);   // # points in this cell.
	      if (jp==0) continue;
	      for(int i=0; i<ip; i++) {
		const int p1(g[ix][iy][iz][i]);
		for(int j=0; j<jp; j++) {	// Compare all the points in the first cell with all the points in the 2nd cell.
		  const int p2(g[jx][jy][jz][j]);
		  if (p1>=p2) continue;
		  NPairsTested++;
		  if (Dist2(p1,p2)<r*r) {  // Found a near pair.
		    // cout << PRINTC(p1) << PRINTC(p2) << PRINTC(Dist2(p1,p2)) << PRINTN(r*r);
		    NPairsPassed++;
		    if (nnpp>=mnpp) { 
		      cout << "ERR: Increase mnpp." << endl;
		      nnpp=0;}
		    npp[nnpp][0] = p1;
		    npp[nnpp++][1] = p2;   
		  }
		}
	      }
	    }
      }
    }
  }
}



void ReadPts() {
  ifstream pin("points");
  for (int i=0; i<np; i++) {
    for (int j=0; j<3; j++) {
      pin>>p[i][j];
      if (p[i][j]<0. || p[i][j]>=1./scale ) { 
	cout << "ERR: illegal " << PRINTC(i) << PRINTC(j) << PRINTN(p[i][j]); }
    }}}


void WritePair(ostream &pout, const int i, const int j) {
  pout << fixed << setw(5) << i << setw(6) << j << "    " << p[i] 
       << "    " << p[j] << "    "  << Dist2(i,j) << endl; }


void WritePairs() {
  ofstream pout("pairs");
  pout.precision(4);
  pout.fixed;
  FOR(i,0,nnpp,WritePair(pout, npp[i][0], npp[i][1])); 
}

int main() {
  clocks_per_sec = sysconf (_SC_CLK_TCK);

#ifdef DEBUG
  cout << PRINT0("=========allnearpairs============") << endl
       << "Compiled from " << __FILE__ << " on " << __DATE__ << ", " 
       << __TIME__ << endl
    // assumes the compile had -DCXX=.....
       << "CXX=" << EXPANDANDSTRING(CXX) << ", CXXFLAGS=" 
       << EXPANDANDSTRING(CXXFLAGS) << endl
       << "Run date and environment: " << endl;
  system ("/bin/date");
  system ("/bin/uname -a");
  system("grep MHz /proc/cpuinfo");
  cout << endl;
#endif
  np=mp;
  ng= 1/r/scale;
  OPRINTC(np) << PRINTC(mp) << PRINTC(scale) << PRINTC(r) << PRINTC(ng) << PRINTC(mg) << PRINTC(mpc) << PRINTC(mnpp) << endl;
  if (ng>mg) { cerr << "ERR: mg is too small for r.  Must be at least 1/r/scale" << endl;
    exit(1); }
  ReadPts();
#ifdef DEBUG
  for (int i=0;i<np;i++) {
    cout << "Pt #" << i << ": ";
    FOR(j,0,3,cout << p[i][j] << ' ');
    cout << endl;  }
#endif
  FOR(i,0,mpc+1,ppc[i]=0);
  Print_Time("Before InsertPts");
  FOR(i,0,ng,FOR(j,0,ng,FOR(k,0,ng,npc[i][j][k]=0)));
  InsertPts();
  Print_Time("After InsertPts");
#ifndef DEBUG
  FOR(i,0,ng,FOR(j,0,ng,FOR(k,0,ng,ppc[npc[i][j][k]]++)));
  cout << "Histogram of # cells with k points, 0<=k<=" << mpc <<": "<<endl;
  FOR(i,0,mpc+1,cout << setw(6) << ppc[i]<< (i%10==9?'\n':' '));
  cout << endl;
  int npCheck = 0;
  FOR(i,1,mpc+1,npCheck += i*ppc[i]);
  OPRINTN(npCheck);
#endif
  int NCells(ng*ng*ng);
  double MeanPtsPerCell((np+0.0)/NCells);
  cout << PRINTC(NCells) << PRINTN(MeanPtsPerCell);
  Print_Time("Before FindNearPts");
  cout << PRINTN(nreps);
  for (irep=0;irep<nreps;irep++) {
    nnpp=0;
    NPairsPassed=0;
    NPairsTested=0;
    FindNearPts();
  }
  Print_Time("After FindNearPts");
  OPRINTN(nnpp);
#ifdef DEBUG
  cout << "Some near pairs:" << endl;
  for (int ii=0;ii<100;ii++) {
    const int i(nnpp/100*ii);
    const int j(npp[i][0]);
    const int k(npp[i][1]);
    cout << i << ": (" << j  << ", " << k << ") = ((";
    FOR(l,0,3,cout << p[j][l] << " ");
    cout << "), (";
    FOR(l,0,3,cout << p[k][l] << " ");
    cout << ")).  d2= " << Dist2(j,k) << endl;
  }
  cout << "Pair 1000: " << npp[1000][0] << ' ' << npp[1000][1] << endl;
#endif
  sort(npp.begin(),npp.begin()+nnpp);
  Print_Time("After sort");
  WritePairs();
  Print_Time("After WritePairs");
  const double FracNearPairs((NPairsPassed+0.0)/NPairsTested);
  OPRINTC(NPairsTested) << PRINTC(NPairsPassed) << PRINTN(FracNearPairs) ;
  const double PairsTestedPerPt((NPairsTested+0.0)/np);
  const double NearPairsPerPt((NPairsPassed+0.0)/np);
  OPRINTC(PairsTestedPerPt) << PRINTN(NearPairsPerPt);
}