//#     Filename:       SpatialIndex.cpp
//#
//#     The SpatialIndex class is defined here.
//#
//#     Author:         Peter Z. Kunszt based on A. Szalay's code
//#     
//#     Date:           October 15, 1998
//#
//#
//#
//# Copyright (C) 2000  Peter Z. Kunszt, Alex S. Szalay, Aniruddha R. Thakar
//#                     The Johns Hopkins University
//#
//# This program is free software; you can redistribute it and/or
//# modify it under the terms of the GNU General Public License
//# as published by the Free Software Foundation; either version 2
//# of the License, or (at your option) any later version.
//#
//# This program is distributed in the hope that it will be useful,
//# but WITHOUT ANY WARRANTY; without even the implied warranty of
//# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//# GNU General Public License for more details.
//#
//# You should have received a copy of the GNU General Public License
//# along with this program; if not, write to the Free Software
//# Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
//#
//#
//#
//#     Modification History:
//#
#include <VarVecDef.h>
#include "SpatialIndex.h"

// ===========================================================================
//
// Macro definitions for readability
//
// ===========================================================================

#define N(x) nodes_.vector_[(x)]
#define V(x) vertices_.vector_[nodes_.vector_[index].v_[(x)]]
#define IV(x) nodes_.vector_[index].v_[(x)]
#define W(x) vertices_.vector_[nodes_.vector_[index].w_[(x)]]
#define IW(x) nodes_.vector_[index].w_[(x)]
#define ICHILD(x) nodes_.vector_[index].childID_[(x)]

#define IV_(x) nodes_.vector_[index_].v_[(x)]
#define IW_(x) nodes_.vector_[index_].w_[(x)]
#define ICHILD_(x) nodes_.vector_[index_].childID_[(x)]
#define IOFFSET 9
// ===========================================================================
//
// Member functions for class SpatialIndex
//
// ===========================================================================

/////////////CONSTRUCTOR//////////////////////////////////
//
SpatialIndex::SpatialIndex(size_t maxlevel, size_t buildlevel) :
  maxlevel_(maxlevel), 
  buildlevel_( (buildlevel == 0 || buildlevel > maxlevel) ? maxlevel 
	                                                  : buildlevel)
{
  size_t nodes,vertices;

  layers_.at(buildlevel_);   	// allocate enough space already
  vMax(&nodes,&vertices);
  nodes_.at(nodes); 		// allocate space for all nodes
  vertices_.at(vertices); 	// allocate space for all vertices

  N(0).index_ = 0; // initialize invalid node

  // initialize first layer
  layers_[0].level_ = 0;
  layers_[0].nVert_ = 6;
  layers_[0].nNode_ = 8;
  layers_[0].nEdge_ = 12;
  layers_[0].firstIndex_ = 1;
  layers_[0].firstVertex_ = 0;

  // set the first 6 vertices
  float64 v[6][3] = {
    {0.0L,  0.0L,  1.0L}, // 0
    {1.0L,  0.0L,  0.0L}, // 1
    {0.0L,  1.0L,  0.0L}, // 2
   {-1.0L,  0.0L,  0.0L}, // 3
    {0.0L, -1.0L,  0.0L}, // 4
    {0.0L,  0.0L, -1.0L}  // 5
  };

  for(int i = 0; i < 6; i++)
    vertices_[i].set( v[i][0], v[i][1], v[i][2]);

  // create the first 8 nodes - index 1 through 8
  index_ = 1;
  newNode(1,5,2,8,0);  // S0
  newNode(2,5,3,9,0);  // S1
  newNode(3,5,4,10,0); // S2
  newNode(4,5,1,11,0); // S3
  newNode(1,0,4,12,0); // N0
  newNode(4,0,3,13,0); // N1
  newNode(3,0,2,14,0); // N2
  newNode(2,0,1,15,0); // N3

  //  loop through buildlevel steps, and build the nodes for each layer
  size_t pl=0;
  size_t level = buildlevel_;
  while(level-- > 0) {
    SpatialEdge edge(*this, pl);
    edge.makeMidPoints();
    makeNewLayer(pl);
    ++pl;
  }
  sortIndex();
}

/////////////SHOWVERTICES/////////////////////////////////
// showVertices: print every vertex to the output stream
void
SpatialIndex::showVertices(ostream & out) const
{
  for(size_t i = 0; i < vertices_.length()-1; i++)
    out << vertices_.vector_[i] << endl;
}

/////////////NODEVERTEX///////////////////////////////////
// nodeVertex: return index of vertices for a node
void 
SpatialIndex::nodeVertex(const size_t idx, 
			 size_t & v1, size_t & v2, size_t & v3) const {
 v1 = nodes_.vector_[idx].v_[0];
 v2 = nodes_.vector_[idx].v_[1];
 v3 = nodes_.vector_[idx].v_[2];
}

/////////////NODEVERTEX///////////////////////////////////
// nodeVertex: return the vectors of the vertices, based on the ID
// 
void 
SpatialIndex::nodeVertex(const uint64 id,
			 SpatialVector & v0,
			 SpatialVector & v1,
			 SpatialVector & v2) const {

  if(buildlevel_ == maxlevel_) {
    uint32 idx = (uint32)id;
    v0 = vertices_.vector_[nodes_.vector_[idx].v_[0]];
    v1 = vertices_.vector_[nodes_.vector_[idx].v_[1]];
    v2 = vertices_.vector_[nodes_.vector_[idx].v_[2]];
    return;
  }

  // buildlevel < maxlevel
  // get the id of the stored leaf that we are in
  // and get the vertices of the node we want
  uint64 sid = id >> ((maxlevel_ - buildlevel_)*2);
  uint32 idx = (uint32)(sid - storedleaves_ + IOFFSET);
  v0 = vertices_.vector_[nodes_.vector_[idx].v_[0]];
  v1 = vertices_.vector_[nodes_.vector_[idx].v_[1]];
  v2 = vertices_.vector_[nodes_.vector_[idx].v_[2]];

  // loop through additional levels,
  // pick the correct triangle accordingly, storing the
  // vertices in v1,v2,v3
  for(uint32 i = buildlevel_ + 1; i <= maxlevel_; i++) {
    uint64 j = ( id >> ((maxlevel_ - i)*2) ) & 3;
    SpatialVector w0 = v1 + v2; w0.normalize();
    SpatialVector w1 = v0 + v2; w1.normalize();
    SpatialVector w2 = v1 + v0; w2.normalize();

    switch(j) {
    case 0:
      v1 = w2;
      v2 = w1;
      break;
    case 1:
      v0 = v1;
      v1 = w0;
      v2 = w2;
      break;
    case 2:
      v0 = v2;
      v1 = w1;
      v2 = w0;
      break;
    case 3:
      v0 = w0;
      v1 = w1;
      v2 = w2;
      break;
    }
  }
}

/////////////MAKENEWLAYER/////////////////////////////////
// makeNewLayer: generate a new layer and the nodes in it
//
void 
SpatialIndex::makeNewLayer(size_t oldlayer)
{
  uint64 index, id;
  size_t newlayer = oldlayer + 1;

  layers_[newlayer].level_  = layers_[oldlayer].level_+1;
  layers_[newlayer].nVert_  = layers_[oldlayer].nVert_ + 
                              layers_[oldlayer].nEdge_;
  layers_[newlayer].nNode_  = 4 * layers_[oldlayer].nNode_;
  layers_[newlayer].nEdge_  = layers_[newlayer].nNode_ + 
                             layers_[newlayer].nVert_ - 2;
  layers_[newlayer].firstIndex_ = index_;
  layers_[newlayer].firstVertex_ = layers_[oldlayer].firstVertex_ + 
                                   layers_[oldlayer].nVert_;

  uint64 ioffset = layers_[oldlayer].firstIndex_ ; 
  for(index = ioffset;
      index < ioffset + layers_[oldlayer].nNode_; index++){
    id = N(index).id_ << 2;
    ICHILD(0) = newNode(IV(0),IW(2),IW(1),id++,index);
    ICHILD(1) = newNode(IV(1),IW(0),IW(2),id++,index);
    ICHILD(2) = newNode(IV(2),IW(1),IW(0),id++,index);
    ICHILD(3) = newNode(IW(0),IW(1),IW(2),id,index);
  }
}

/////////////NEWNODE//////////////////////////////////////
// newNode: make a new node
//
uint64
SpatialIndex::newNode(size_t v1, size_t v2,size_t v3,uint64 id,uint64 parent)
{
  IV_(0) = v1;		// vertex indices
  IV_(1) = v2;
  IV_(2) = v3;
  IW_(0) = 0;		// middle point indices
  IW_(1) = 0;
  IW_(2) = 0;
  ICHILD_(0) = 0;	// child indices
  ICHILD_(1) = 0;	// index 0 is invalid node.
  ICHILD_(2) = 0;
  ICHILD_(3) = 0;

  N(index_).id_ = id;		// set the id
  N(index_).index_ = index_;	// set the index
  N(index_).parent_ = parent;	// set the parent
  return index_++;
}


float64
SpatialIndex::area(uint64 ID) const
{

  SpatialVector n0;
  SpatialVector n1;
  SpatialVector n2;

  nodeVertex(ID, n0, n1, n2);
  return area(n0,n1,n2);
}


/////////////AREA////////////////////////////////////////
// area: routine to precompute the area of a node using
//
//   AREA = 4*arctan sqrt(tan(s/2)tan((s-a)/2)tan((s-b)/2)tan((s-c)/2))
//
//   with s = (a+b+c)/2
//
// (with many thanks to Eduard Masana, emasana@pchpc10.am.ub.es )
//
float64
SpatialIndex::area(const SpatialVector & v0, 
		   const SpatialVector & v1,
		   const SpatialVector & v2) const {

  float64 a = acos( v0 * v1);
  float64 b = acos( v1 * v2);
  float64 c = acos( v2 * v0);

  float64 s = (a + b + c)/2.0;

  float64 area = 4.0*atan(sqrt(tan(s/2.0)*
			       tan((s-a)/2.0)*
			       tan((s-b)/2.0)*
			       tan((s-c)/2.0)));        
  return area;
}

/////////////VMAX/////////////////////////////////////////
// vMax: compute the maximum number of vertices for the
//       polyhedron after buildlevel of subdivisions and
//       the total number of nodes that we store
//       also, calculate the number of leaf nodes that we eventually have.
//
void
SpatialIndex::vMax(size_t *nodes, size_t *vertices) {
  uint64 nv = 6;    // initial values
  uint64 ne = 12;
  uint64 nf = 8;
  int32 i  = buildlevel_;
  *nodes = (size_t)nf;

  while(i-->0){
    nv += ne;
    nf *= 4;
    ne  = nf + nv -2;
    *nodes += (size_t)nf;
  }
  *vertices = (size_t)nv;
  storedleaves_ = nf;

  // calculate number of leaves
  i = maxlevel_ - buildlevel_;
  while(i-- > 0)
    nf *= 4;
  leaves_ = nf;
}

/////////////SORTINDEX////////////////////////////////////
// sortIndex: sort the index so that the first node is the invalid node
//            (index 0), the next 8 nodes are the root nodes
//            and then we put all the leaf nodes in the following block
//            in ascending id-order.
//            All the rest of the nodes is at the end.
void
SpatialIndex::sortIndex() {
  ValVec<QuadNode> oldnodes(nodes_); // create a copy of the node list
  size_t index;
  size_t nonleaf;
  size_t leaf;

#define ON(x) oldnodes.vector_[(x)]

  // now refill the nodes_ list according to our sorting.
  for( index=IOFFSET, leaf=IOFFSET, nonleaf=nodes_.length()-1; 
       index < nodes_.length(); index++) {

    if( ON(index).childID_[0] == 0 ) { // childnode
      // set leaf into list
      N(leaf) = ON(index);
      // set parent's pointer to this leaf
      for (size_t i = 0; i < 4; i++) {
	if(N(N(leaf).parent_).childID_[i] == index) {
	  N(N(leaf).parent_).childID_[i] = leaf;
	  break;
	}
      }
      leaf++;
    } else {
      // set nonleaf into list from the end
      // set parent of the children already to this
      // index, they come later in the list.
      N(nonleaf) = ON(index);
      ON(N(nonleaf).childID_[0]).parent_ = nonleaf;
      ON(N(nonleaf).childID_[1]).parent_ = nonleaf;
      ON(N(nonleaf).childID_[2]).parent_ = nonleaf;
      ON(N(nonleaf).childID_[3]).parent_ = nonleaf;
      // set parent's pointer to this leaf
      for (size_t i = 0; i < 4; i++) {
	if(N(N(nonleaf).parent_).childID_[i] == index) {
	  N(N(nonleaf).parent_).childID_[i] = nonleaf;
	  break;
	}
      }
      nonleaf--;
    }
  }
}
//////////////////IDBYNAME/////////////////////////////////////////////////
// Translate ascii leaf name to a uint32
//
// The following encoding is used:
//
// The string leaf name has the always the same structure, it begins with
// an N or S, indicating north or south cap and then numbers 0-3 follow 
// indicating which child to descend into. So for a depth-5-index we have
// strings like
//                 N012023  S000222  N102302  etc
//
// Each of the numbers correspond to 2 bits of code (00 01 10 11) in the
// uint32. The first two bits are 10 for S and 11 for N. For example
//
//                 N 0 1 2 0 2 3
//                 11000110001011  =  12683 (dec)
//
// The leading bits are always 0.
//
// --- WARNING: This works only up to 15 levels. 
//              (we probably never need more than 7)
//

uint64
SpatialIndex::idByName(const char *name) {

  uint64 out=0, i;
  uint32 size = 0;

  if(name == 0)              // null pointer-name
    throw SpatialFailure("SpatialIndex:idByName:no name given");
  if(name[0] != 'N' && name[0] != 'S')  // invalid name
    throw SpatialFailure("SpatialIndex:idByName:invalid name",name);

  size = strlen(name);       // determine string length
  // at least size-2 required, don't exceed max
  if(size < 2)
    throw SpatialFailure("SpatialIndex:idByName:invalid name - too short ",name);
  if(size > HTMNAMEMAX)
    throw SpatialFailure("SpatialIndex:idByName:invalid name - too long ",name);

  for(i = size-1; i > 0; i--) {// set bits starting from the end
    if(name[i] > '3' || name[i] < '0') // invalid name
      throw SpatialFailure("SpatialIndex:idByName:invalid name digit ",name);
    out += (uint64(name[i]-'0') << 2*(size - i -1));
  }

  i = 2;                     // set first pair of bits, first bit always set
  if(name[0]=='N') i++;      // for north set second bit too
  out += (i << (2*size - 2) );

  /************************
  // This code may be used later for hashing !
  if(size==2)out -= 8;
  else {
    size -= 2;
    uint32 offset = 0, level4 = 8;
    for(i = size; i > 0; i--) { // calculate 4 ^ (level-1), level = size-2
      offset += level4;
      level4 *= 4;
    }
    out -= level4 - offset;
  }
  **************************/
  return out;
}


//////////////////NAMEBYID/////////////////////////////////////////////////
// Translate uint32 to an ascii leaf name
//
// The encoding described above may be decoded again using the following
// procedure:
//
//  * Traverse the uint32 from left to right.
//  * Find the first 'true' bit.
//  * The first pair gives N (11) or S (10).
//  * The subsequent bit-pairs give the numbers 0-3.
//

char *
SpatialIndex::nameById(uint64 id, char * name){

  uint32 size=0, i;
#ifdef _WIN32
  uint64 IDHIGHBIT = 1;
  uint64 IDHIGHBIT2= 1;
  IDHIGHBIT = IDHIGHBIT << 63;
  IDHIGHBIT2 = IDHIGHBIT2 << 62;
#endif

  /*************
  // This code might be useful for hashing later !!

  // calculate the level (i.e. 8*4^level) and add it to the id:
  uint32 level=0, level4=8, offset=8;
  while(id >= offset) {
    if(++level > 13) { ok = false; offset = 0; break; }// level too deep
    level4 *= 4;
    offset += level4;
  }
  id += 2 * level4 - offset;
  **************/

  // determine index of first set bit
  for(i = 0; i < IDSIZE; i+=2) {
	if ( (id << i) & IDHIGHBIT ) break;
    if ( (id << i) & IDHIGHBIT2 )  // invalid id
		throw SpatialFailure("SpatialIndex:nameById: invalid ID");
  }
  if(id == 0)
    throw SpatialFailure("SpatialIndex:nameById: invalid ID");

  size=(IDSIZE-i) >> 1;
  // allocate characters
  if(!name)
    name = new char[size+1];

  // fill characters starting with the last one
  for(i = 0; i < size-1; i++)
    name[size-i-1] = '0' + char( (id >> i*2) & 3);

  // put in first character
  if( (id >> (size*2-2)) & 1 ) {
    name[0] = 'N';
  } else {
    name[0] = 'S';
  }
  name[size] = 0; // end string

  return name;
}
//////////////////IDBYPOINT////////////////////////////////////////////////
// Find a leaf node where a vector points to
//

uint64
SpatialIndex::idByPoint(SpatialVector & v) const {
    uint64 index;

    // start with the 8 root triangles, find the one which v points to
    for(index=1; index <=8; index++) {
	if( (V(0) ^ V(1)) * v < -gEpsilon) continue;
	if( (V(1) ^ V(2)) * v < -gEpsilon) continue;
	if( (V(2) ^ V(0)) * v < -gEpsilon) continue;
	break;
    }
    // loop through matching child until leaves are reached
    while(ICHILD(0)!=0) {
	uint64 oldindex = index;
	for(size_t i = 0; i < 4; i++) {
	    index = nodes_.vector_[oldindex].childID_[i];
	    if( (V(0) ^ V(1)) * v < -gEpsilon) continue;
	    if( (V(1) ^ V(2)) * v < -gEpsilon) continue;
	    if( (V(2) ^ V(0)) * v < -gEpsilon) continue;
	    break;
	}
    }
    // return if we have reached maxlevel
    if(maxlevel_ == buildlevel_)return N(index).id_;

    // from now on, continue to build name dynamically.
    // until maxlevel_ levels depth, continue to append the
    // correct index, build the index on the fly.
    char name[HTMNAMEMAX];
    nameById(N(index).id_,name);
    size_t len = strlen(name);
    SpatialVector v0 = V(0);
    SpatialVector v1 = V(1);
    SpatialVector v2 = V(2);

    size_t level = maxlevel_ - buildlevel_;
    while(level--) {
      SpatialVector w0 = v1 + v2; w0.normalize();
      SpatialVector w1 = v0 + v2; w1.normalize();
      SpatialVector w2 = v1 + v0; w2.normalize();

      if(isInside(v, v0, w2, w1)) {
	name[len++] = '0';
	v1 = w2; v2 = w1;
	continue;
      } else if(isInside(v, v1, w0, w2)) {
	name[len++] = '1';
	v0 = v1; v1 = w0; v2 = w2;
	continue;
      } else if(isInside(v, v2, w1, w0)) {
	name[len++] = '2';
	v0 = v2; v1 = w1; v2 = w0;
	continue;
      } else if(isInside(v, w0, w1, w2)) {
	name[len++] = '3';
	v0 = w0; v1 = w1; v2 = w2;
	continue;
      }
    }
    name[len] = '\0';
    return idByName(name);
}

//////////////////ISINSIDE/////////////////////////////////////////////////
// Test whether a vector is inside a triangle. Input triangle has
// to be sorted in a counter-clockwise direction.
//
bool
SpatialIndex::isInside(const SpatialVector & v, const SpatialVector & v0,
	       const SpatialVector & v1, const SpatialVector & v2) const
{
  if( (v0 ^ v1) * v < -gEpsilon) return false;
  if( (v1 ^ v2) * v < -gEpsilon) return false;
  if( (v2 ^ v0) * v < -gEpsilon) return false;
  return true;
}

//////////////////CASStriangleCenter/////////////////////////////////////////////////
//find the center of a given spatial triangle  - added 11/6/02
//
void
SpatialIndex::CASStriangleCenter( uint64 nodename, float64 *ra, float64 *dec ) const
{
  SpatialVector v1,v2,v3;

  //get vertices of triangle
  nodeVertex(nodename, v1, v2, v3);

  //average over ra/dec of vertices to find center ra/dec
  *ra = ( v1.ra() + v2.ra() + v3.ra() ) / 3;
  *dec = ( v1.dec() + v2.dec() + v3.dec() ) / 3;

  if ( (fabs(*ra - v1.ra())) > 10 )
  {
    if (v1.ra()>180)  v1.set(v1.ra() - 360,v1.dec());
    if (v2.ra()>180)  v2.set(v2.ra() - 360,v2.dec());
    if (v3.ra()>180)  v3.set(v3.ra() - 360,v3.dec());

  *ra = ( v1.ra() + v2.ra() + v3.ra() ) / 3;
  // 0<ra<360
  if(*ra<0)  *ra = *ra + 360;
  }

  return;
}