/*
 * quadtree implementation
 *
 * Copyright (C) Dorit Borrmann
 *
 * Released under the GPL version 3.
 *
 */

#include "shapes/quadtree.h"

int QuadTree::anz;
double QuadTree::qSize;
double QuadTree::dist = 10.0;
//Plane *QuadTree::basePlane;
/*
QuadTree::QuadTree(double **pts, int n, double _qSize, Plane *_basePlane){
  basePlane = _basePlane;

  anz = 0;
  qSize = _qSize;
  double xmin = pts[0][0], xmax = pts[0][0];
  double ymin = pts[0][1], ymax = pts[0][1];
  for (int i=1; i < n; i++) {
    xmin = min(xmin, pts[i][0]);
    xmax = max(xmax, pts[i][0]);
    ymin = min(ymin, pts[i][1]);
    ymax = max(ymax, pts[i][1]);
  }
  center[0] = 0.5f * (xmin+xmax);
  center[1] = 0.5f * (ymin+ymax);

  double m = max(0.5f * (xmax-xmin), 0.5f * (ymax-ymin));
  int count;

  //set up values
  child[0] = child[1] = child[2] = child[3] = 0;
  leaf = 0;

  x_size    = m;
  y_size    = m;

  // calculate new buckets
  double newcenter[4][2];
  double x_sizeNew, y_sizeNew;

  x_sizeNew = x_size / 2.0;
  y_sizeNew = y_size / 2.0;

  newcenter[0][0] = center[0] - x_size / 2.0;
  newcenter[0][1] = center[1] - y_size / 2.0;

  newcenter[1][0] = center[0] + x_size / 2.0;
  newcenter[1][1] = center[1] - y_size / 2.0;

  newcenter[2][0] = center[0] - x_size / 2.0;
  newcenter[2][1] = center[1] + y_size / 2.0;

  newcenter[3][0] = center[0] + x_size / 2.0;
  newcenter[3][1] = center[1] + y_size / 2.0;
  
  // if bucket is _not_ approximately (nearly) empty don't build tree further
  // -----------------------------------------------------------------------
  for (int i = 0; i < 4; i++) {
    vector<double*> splitPoints;
    count = countPointsAndQueue(pts, n, splitPoints, 
                                newcenter[i], x_sizeNew, y_sizeNew);
    if (count > MIN_PTS_IN_BUCKET) 
      child[i] = new QuadTree(splitPoints,
                             newcenter[i], x_sizeNew, y_sizeNew);
    splitPoints.clear();
  }
}*/

bool QuadTree::close(double* p, double *p2) {
  double stheta = sin(p[0]) * sin(p2[0]);
  double myd2 = acos( 
      stheta * cos(p[1]) * cos(p2[1])
      + stheta * sin(p[1]) * sin(p2[1])
      + cos(p[0]) * cos(p2[0]));
  if (myd2 < dist) {
    return true;
  }
  return false;
/*
  double dx = p2[0] - p[0];
  double dy = p2[1] - p[1];
  if ( sqrt(dx*dx + dy*dy) < dist ) {
    return true;
  }
  return false;*/
}
 
bool QuadTree::close(double* p, set<double*> &cluster) {
  for(set<double*>::iterator it = cluster.begin(); it != cluster.end(); it++) {
    if ( close(p, (*it)) ) {
      return true;
    }
  }
  return false;
}
/*
bool QuadTree::close(double* p, set<double*> &cluster) {
  double dx,dy;
  for(set<double*>::iterator it = cluster.begin(); it != cluster.end(); it++) {
    dx = (*it)[0] - p[0];
    dy = (*it)[1] - p[1];
    if ( sqrt(dx*dx + dy*dy) < dist ) {
      return true;
    }
  }
  return false;
}*/
int QuadTree::where(double *p1, vector<set<double*> >& clusters) {
  for (unsigned int i = 0; i < clusters.size(); i++) {
    if (clusters[i].count(p1) > 0) return i;
  }
  // this shouldnt happen
  //cout << "VERY BAD THINGS HAPPENED!" <<endl;
  return -1;
}

vector<set<double*> >& QuadTree::clusterCells(vector<set<double*> > &clusters1, set<double*> border1, 
                           vector<set<double*> > &clusters2, set<double*> border2, vector< set<double*> > &clusters) {
  if (border1.size() <= 0 || border2.size() <= 0) {
    clusters.insert( clusters.end(), clusters1.begin(), clusters1.end());
    clusters.insert( clusters.end(), clusters2.begin(), clusters2.end());
    return clusters;
  }
  // some clusters might not have a border connection, remember those?
  set<int> non_used_clusters;
  for (unsigned int i = 0; i < clusters1.size(); i++) {
    non_used_clusters.insert( -1 -i);
  }
  for (unsigned int i = 0; i < clusters2.size(); i++) {
    non_used_clusters.insert( i);
  }

  // contains the edges of the correspondence graph
  // indices of cluster 1 are stored negated
  vector<int*> cgraph;
//cout << "bordersizes: " << border1.size() << " " <<  border2.size() << endl;
  for (set<double*>::iterator bit1 = border1.begin(); bit1 != border1.end(); bit1++) {
    for (set<double*>::iterator bit2 = border2.begin(); bit2 != border2.end(); bit2++) {
      if (close( *bit1, *bit2) ) {
        int *edge = new int[3];
        int w1,w2; 
        w1 = where(*bit1, clusters1);
        if (w1 == -1) {
          continue;
        }
        w2 = where(*bit2, clusters2);
        if (w2 == -1) {
          continue;
        }

        edge[0] = -1 - w1;
        edge[1] = w2; 
        // label the edge
        edge[2] = -1;
        cgraph.push_back(edge);
//        cout << "point to point " << edge[0] << " " << edge[1] << endl; 
      }
    }
  }
  //cout << "borders connected" << endl;
  vector< set<int> > index_clusters;
  int nr_cluster = 0;
  for (unsigned int i = 0; i < cgraph.size(); i++) {
    if (cgraph[i][2] != -1) {continue;}
    // unlabeled edge
    cgraph[i][2] = nr_cluster;
    set<int> index_cluster;
    index_cluster.insert(cgraph[i][0]);
    index_cluster.insert(cgraph[i][1]);
        non_used_clusters.erase(cgraph[i][0]);
        non_used_clusters.erase(cgraph[i][1]);
    for (unsigned int j = i + 1; j < cgraph.size(); j++) {
//    cout << j << " j " << cgraph.size() << endl;
      if (cgraph[j][2] != -1) {continue;}
      // if either of the nodes is already in my cluster, label edge and add
      // nodes
      if (index_cluster.count(cgraph[j][0]) > 0 
          || index_cluster.count(cgraph[j][1]) > 0 ) {
        index_cluster.insert(cgraph[j][0]);
        index_cluster.insert(cgraph[j][1]);

        non_used_clusters.erase(cgraph[j][0]);
        non_used_clusters.erase(cgraph[j][1]);
        cgraph[j][2] = nr_cluster;
      }
    }
    //non_used_clusters.erase(index_cluster.begin(), index_cluster.end());
    index_clusters.push_back(index_cluster);
    nr_cluster++;
  }
//cout << "cluster connections resolved" << endl;
  for (unsigned int i = 0; i < index_clusters.size(); i++) {
    set<double*> cluster;
    for (set<int>::iterator it = index_clusters[i].begin(); 
         it != index_clusters[i].end(); it++) {
      if (*it < 0 ) {
        int index = -1 - (*it);
        cluster.insert( clusters1[index].begin(), clusters1[index].end() );
      } else {
        cluster.insert( clusters2[*it].begin(), clusters2[*it].end() );
      }
    }
    clusters.push_back(cluster);
  }
//cout << "clusters joined. add remaining " << non_used_clusters.size() << endl;
  for (set<int>::iterator it = non_used_clusters.begin(); it != non_used_clusters.end(); it++) {
    set<double*> cluster;
    if (*it < 0 ) {
      int index = -1 - (*it);
      cluster.insert( clusters1[index].begin(), clusters1[index].end() );
    } else {
      cluster.insert( clusters2[*it].begin(), clusters2[*it].end() );
    }
    clusters.push_back(cluster);
  }
//cout << "done." << endl;
  // cleanup
  for (unsigned int i = 0; i < cgraph.size(); i++) {
    delete[] cgraph[i];
  }
  cgraph.clear();
//cout << "   cleanup done returning" << endl;
  return clusters;
//  return clusters1;
}

vector<set<double*> >& QuadTree::getClusters() {
  // if leaf node
  if (leaf == 1) {
    if (points.size() <= 0) return clusters;
//    cout << "leaf clustering ";
//    cout << center[0] << " " << center[1] << " " << points.size() << " " <<endl; 
    clusters.push_back(set<double*> ());
    clusters[0].insert(points[0]);

    for (unsigned int j = 1; j < points.size(); j++) {
      bool found_cluster = false;
      int oldindex = -1;
      for (unsigned int i = 0; i < clusters.size(); i++) {
        if ( close(points[j], clusters[i]) ) {
          if (!found_cluster) {
            clusters[i].insert(points[j]);
            found_cluster = true;
            oldindex = i;
            //break;
          } else {
//            cout << "    join cluster " << oldindex << " and " << i << endl;
            clusters[oldindex].insert(clusters[i].begin(), clusters[i].end());
            clusters.erase(clusters.begin() + i);
            i--;
            //clusters[i]
          }
        }
      }
      if (!found_cluster) { // havent found a cose enough cluster -> new cluster
        set<double*> cluster;
        cluster.insert(points[j]);
        clusters.push_back(cluster);
      }
    }
//    cout << "    this many cluster found in leaf  " << clusters.size() << endl;
    return clusters;
  }

 //cout << "my leafs: "<< child[0] << " " << child[1] << " " << child[2] << " " << child[3] << endl;
  vector< set<double *> > southclusters; 
            clusterCells(child[0]->getClusters(), child[0]->east,
                         child[1]->getClusters(), child[1]->west, southclusters);
//  cout << "northern cluste: " << endl; 
  vector< set<double *> > northclusters;
            clusterCells(child[2]->getClusters(), child[2]->east,
                         child[3]->getClusters(), child[3]->west, northclusters);
//  cout << "south: " << southclusters.size() << endl;
//  cout << "north: " << northclusters.size() << endl;

  // join borders
  set<double*> nborders01;
  nborders01.insert(child[0]->north.begin(), child[0]->north.end());
  nborders01.insert(child[1]->north.begin(), child[1]->north.end());
  
  set<double*> sborders23;
  sborders23.insert(child[2]->south.begin(), child[2]->south.end());
  sborders23.insert(child[3]->south.begin(), child[3]->south.end());

  clusters = clusterCells(northclusters, sborders23, southclusters, nborders01, clusters);
  if (clusters.size() > 0) {
  //cout << "node clustered " << clusters.size() << endl;
  //cout << center[0] << " " << center[1] << " " << points.size() << endl; 
  }

  return clusters;
}

QuadTree::QuadTree(double **pts, int n, double _qSize, double min_d){
  dist = min_d;
  anz = 0;
  qSize = _qSize;
  double xmin = pts[0][0], xmax = pts[0][0];
  double ymin = pts[0][1], ymax = pts[0][1];
  for (int i=1; i < n; i++) {
    xmin = min(xmin, pts[i][0]);
    xmax = max(xmax, pts[i][0]);
    ymin = min(ymin, pts[i][1]);
    ymax = max(ymax, pts[i][1]);
  }
  double eps = 0.00001;
  xmin -= eps;
  ymin -= eps;
  xmax += eps;
  ymax += eps;
  center[0] = 0.5 * (xmin+xmax);
  center[1] = 0.5 * (ymin+ymax);

  double m = max(0.5 * (xmax-xmin), 0.5 * (ymax-ymin));
  int count;

  //set up values
  child[0] = child[1] = child[2] = child[3] = 0;
  leaf = 0;

  x_size    = m;
  y_size    = m;

  // calculate new buckets
  double newcenter[4][2];
  double x_sizeNew, y_sizeNew;

  x_sizeNew = x_size / 2.0;
  y_sizeNew = y_size / 2.0;

  newcenter[0][0] = center[0] - x_size / 2.0;
  newcenter[0][1] = center[1] - y_size / 2.0;

  newcenter[1][0] = center[0] + x_size / 2.0;
  newcenter[1][1] = center[1] - y_size / 2.0;

  newcenter[2][0] = center[0] - x_size / 2.0;
  newcenter[2][1] = center[1] + y_size / 2.0;

  newcenter[3][0] = center[0] + x_size / 2.0;
  newcenter[3][1] = center[1] + y_size / 2.0;
  
  // if bucket is _not_ approximately (nearly) empty don't build tree further
  // -----------------------------------------------------------------------
  for (int i = 0; i < 4; i++) {
    vector<double*> splitPoints;
    set<double *> no,e,s,w;
    count = countPointsAndQueue(pts, n, splitPoints, 
                                newcenter[i], x_sizeNew, y_sizeNew, 
                                no, e, s, w);
    if (count > MIN_PTS_IN_BUCKET) 
      child[i] = new QuadTree(splitPoints,
                             newcenter[i], x_sizeNew, y_sizeNew, no,e,s,w);
    splitPoints.clear();
  }
}

QuadTree::QuadTree(vector<double*> &splitPoints, double _center[2], 
                 double _x_size, double _y_size, set<double*> &n,
                 set<double*> &e, set<double*> &s, set<double*> &w) {
  int count;
  /*
  this->north = n;
  this->east = e;
  this->south = s;
  this->west = w;
  */
  this->north.insert(n.begin(), n.end());
  this->south.insert(s.begin(), s.end());
  this->east.insert(e.begin(), e.end());
  this->west.insert(w.begin(), w.end());

  //set up values
  child[0] = child[1] = child[2] = child[3] = 0;
  leaf = 0;

  center[0] = _center[0];
  center[1] = _center[1];
  x_size    = _x_size;
  y_size    = _y_size;

  // if bucket is too small stop building tree
  // -----------------------------------------
  if ((y_size <= qSize)) {
    // copy points
    for (unsigned int i = 0; i < splitPoints.size(); i++) {
      points.push_back(splitPoints[i]);
    }
    leaf = 1;
    anz++;
    return; 
  }  

  // calculate new buckets
  double newcenter[4][3];
  double x_sizeNew, y_sizeNew;

  x_sizeNew = x_size / 2.0;
  y_sizeNew = y_size / 2.0;

  newcenter[0][0] = center[0] - x_size / 2.0;
  newcenter[0][1] = center[1] - y_size / 2.0;

  newcenter[1][0] = center[0] + x_size / 2.0;
  newcenter[1][1] = center[1] - y_size / 2.0;

  newcenter[2][0] = center[0] - x_size / 2.0;
  newcenter[2][1] = center[1] + y_size / 2.0;

  newcenter[3][0] = center[0] + x_size / 2.0;
  newcenter[3][1] = center[1] + y_size / 2.0;
  
  // if bucket is _not_ approximately (nearly) empty don't build tree further
  // ------------------------------------------------------------------------
  for (int i = 0; i < 4; i++) {
    vector<double*> new_splitPoints;
    set<double *> n,e,s,w;
    count = countPointsAndQueue(splitPoints, new_splitPoints, 
                                newcenter[i], x_sizeNew, y_sizeNew,
                                n, e, s, w);
    if (count > MIN_PTS_IN_BUCKET) {
      child[i] = new QuadTree(new_splitPoints,
                             newcenter[i], x_sizeNew, y_sizeNew, n,e,s,w);
    }
    new_splitPoints.clear();
  }
}

int QuadTree::countPointsAndQueue(const vector<double*> &i_points,
                                 vector<double*> &points,
                                 double center[2], 
                                 double x_size, double y_size,
                                 set<double*> &n,
                                 set<double*> &e,
                                 set<double*> &s,
                                 set<double*> &w)
{
  int count = 0;
  for (unsigned int i = 0; i < i_points.size(); i++) {
    if (fabs(i_points[i][0] - center[0]) <= x_size) {
      if (fabs(i_points[i][1] - center[1]) <= y_size) {
        count++;
        points.push_back(i_points[i]);
        double top = asin(sin(i_points[i][0]) * sin( fabs( (center[1] + y_size) - i_points[i][1]) ) );
        double bottom = asin(sin(i_points[i][0]) * sin( fabs( i_points[i][1] - (center[1] - y_size) ) ) );

        if ( fabs( (center[0] + x_size) - i_points[i][0]) < dist ) {
          e.insert(i_points[i]);
        }
        if ( fabs( i_points[i][0] - (center[0] - x_size) ) < dist ) {
          w.insert(i_points[i]);
        }
        if ( top < dist ) {
          n.insert(i_points[i]);
        }
        if ( bottom < dist ) {
          s.insert(i_points[i]);
        }
      }
    } 
  } 
  return count;
}

int QuadTree::countPointsAndQueue(double **pts, int n, 
                                 vector<double*> &points,
                                 double center[2], 
                                 double x_size,double y_size,
                                 set<double*> &no,
                                 set<double*> &e,
                                 set<double*> &s,
                                 set<double*> &w)
{

  int count = 0;
  for (int i=0; i < n; i++) {
    if (fabs(pts[i][0] - center[0]) <= x_size) {
      if (fabs(pts[i][1] - center[1]) <= y_size) {
        count++;
        double *p = pts[i];
        points.push_back(p);

        double top = asin(sin(p[0]) * sin( fabs( (center[1] + y_size) - p[1]) ) );
        double bottom = asin(sin(p[0]) * sin( fabs( p[1] - (center[1] - y_size) ) ) );
        
        if ( fabs( (center[0] + x_size) - p[0]) < dist ) {
          e.insert(p);
        }
        if ( fabs( p[0] - (center[0] - x_size) ) < dist ) {
          w.insert(p);
        }
        if ( top < dist ) {
          no.insert(p);
        }
        if ( bottom < dist ) {
          s.insert(p);
        }
      }
    } 
  } 
  return count;
}

int QuadTree::countPoints(double **pts, int n, double center[2], 
                         double x_size, double y_size)
{
  int count = 0;
  for (int i=0; i < n; i++) {
    if (fabs(pts[i][0] - center[0]) <= x_size) {
      if (fabs(pts[i][1] - center[1]) <= y_size) {
        count++;
      }
    } 
  } 
  return count;
}
/*
void QuadTree::getQuadTreePoints(vector<double> &quad_x,  vector<double> &quad_y,  vector<double> &quad_z)
{
  double x,y,z;
  
  if (leaf == 1) {
    basePlane->projTo3D(center[0] - x_size,
				    center[1] - y_size,
				    x, y, z);
    quad_x.push_back(x); 
    quad_y.push_back(y); 
    quad_z.push_back(z);
    basePlane->projTo3D(center[0] + x_size,
				    center[1] - y_size,
				    x, y, z);
    quad_x.push_back(x); 
    quad_y.push_back(y); 
    quad_z.push_back(z);
    basePlane->projTo3D(center[0] + x_size,
				    center[1] + y_size,
				    x, y, z);
    quad_x.push_back(x); 
    quad_y.push_back(y); 
    quad_z.push_back(z);
    basePlane->projTo3D(center[0] - x_size,
				    center[1] + y_size,
				    x, y, z);
    quad_x.push_back(x); 
    quad_y.push_back(y); 
    quad_z.push_back(z);
  } else {
    for( int i = 0; i < 4; i++){ 
	 if (child[i] != 0) {
	   child[i]->getQuadTreePoints(quad_x,  quad_y,  quad_z);
	 }
    }
  }
}
*/
/*
void QuadTree::getQuadTreeColor(double &colorR,  double &colorG, double &colorB)
{
    switch (basePlane->getColor()) {
	 case  1: colorR = 0.6; colorG = 0.0; colorB = 0.0; break;
      case  2: colorR = 0.0; colorG = 0.6; colorB = 0.0; break;
      case  3: colorR = 0.0; colorG = 0.0; colorB = 0.6; break;
      case  4: colorR = 0.6; colorG = 0.6; colorB = 0.0; break;
      case  5: colorR = 0.6; colorG = 0.0; colorB = 0.6; break;
      case  6: colorR = 0.0; colorG = 0.6; colorB = 0.6; break;
      case  7: colorR = 0.6; colorG = 0.3; colorB = 0.3; break;
      case  8: colorR = 0.3; colorG = 0.3; colorB = 0.3; break;
      case  9: colorR = 0.3; colorG = 0.3; colorB = 0.6; break;
      case 10: colorR = 0.6; colorG = 0.6; colorB = 0.6; break;
	 case 11: colorR = 0.1; colorG = 0.1; colorB = 0.6; break;
	 case 12: colorR = 0.1; colorG = 0.1; colorB = 0.1; break;
	 case 13: colorR = 0.1; colorG = 0.3; colorB = 0.1; break;
      case 14: colorR = 0.1; colorG = 0.1; colorB = 0.3; break;
      case 15: colorR = 0.3; colorG = 0.6; colorB = 0.3; break;
      case 16: colorR = 0.3; colorG = 0.3; colorB = 0.1; break; 
      default: colorR = 0.3; colorG = 0.1; colorB = 0.1;
    }
}
*/
/*
int QuadTree::outputQuad(char *dir)
{
  cout << "writing ";
  ofstream quadout;
  char fname[64];
  strcpy(fname,dir);strcat(fname, "/quad.dat");

  quadout.open(fname,ios::binary|ios::out|ios::app);
  if (!quadout) {
    cerr << "*** error: can't create file " << fname << " ***\n";
    exit(2);
  }
  int ret = _OutputQuad(quadout);
  quadout.close();
  cout << ret << " quads from quadtree." << endl;
  return ret;  
}

int QuadTree::_OutputQuad(ofstream &bBoxout)
{
  int ret = 0;
  if (leaf == 1) {

    double x,y,z;
    //    cout << endl;
    basePlane->projTo3D(center[0] - x_size,
				  center[1] - y_size,
				  x,y,z);
    //    cout << x << " " << y << " " << z << endl;
    bBoxout.write((char*)&x, sizeof(double));
    bBoxout.write((char*)&y, sizeof(double));
    bBoxout.write((char*)&z, sizeof(double));
    basePlane->projTo3D(center[0] - x_size,
				  center[1] + y_size,
				  x,y,z);
    //    cout << x << " " << y << " " << z << endl;
    bBoxout.write((char*)&x, sizeof(double));
    bBoxout.write((char*)&y, sizeof(double));
    bBoxout.write((char*)&z, sizeof(double));
    basePlane->projTo3D(center[0] + x_size,
				  center[1] + y_size,
				  x,y,z);
    //    cout << x << " " << y << " " << z << endl;
    bBoxout.write((char*)&x, sizeof(double));
    bBoxout.write((char*)&y, sizeof(double));
    bBoxout.write((char*)&z, sizeof(double));
    basePlane->projTo3D(center[0] + x_size,
				  center[1] - y_size,
				  x,y,z);
    //    cout << x << " " << y << " " << z << endl;
    bBoxout.write((char*)&x, sizeof(double));
    bBoxout.write((char*)&y, sizeof(double));
    bBoxout.write((char*)&z, sizeof(double));
    
    
//    float col = 0.0;
//    for (int i = 0; i < points.size(); i++) {
//      col = max(col, points[i][2]);
//    }
//
//    col = col / 3.0;
// 
    
    float col = 0; //!!!
    
    double gcolorR, gcolorG, gcolorB;
    getQuadTreeColor(gcolorR, gcolorG, gcolorB);

    float col_r = col + gcolorR;
    float col_g = col + gcolorG;
    float col_b = col + gcolorB;
    bBoxout.write((char*)&col_r, sizeof(float));
    bBoxout.write((char*)&col_g, sizeof(float));
    bBoxout.write((char*)&col_b, sizeof(float));

    return 1;
  }

  for( int i = 0; i < 4; i++){ 
    if (child[i] != 0) {
      ret += child[i]->_OutputQuad(bBoxout);
    }
  }
  return ret;
}
*/

QuadTree::~QuadTree(){
  for(int i = 0; i < 4; i++) {
    if (child[i] !=0) {
      delete child[i];
      child[i] = 0;
    }
  }
}