/*
 ******************************************************************************
 * Title: BoundingBoxBinTree
 * Project: ColDetection Library
 ******************************************************************************
 * File: BoundingBoxBinTree.cpp
 * Author: Romain Rodriguez <Romain.Rodriguez@inrialpes.fr>
 * Created: 2003-02-18
 * Last update: 2003-05-20
 ******************************************************************************
 * Description: 
 * Bounding Box Binary Tree
 ******************************************************************************
 * Copyright (c) 2003, INRIA CYBERMOVE
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation; either version 2.1
 * of the License, or (at your option) any later version.
 *
 * This library 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
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 ******************************************************************************
 */

#include "vtkBoundingBoxBinTree.h"


vtkCxxRevisionMacro(vtkBoundingBoxBinTree, "$Revision: 0.1 $");

vtkBoundingBoxBinTree::vtkBoundingBoxBinTree(vtkMesh* mesh)
{
  _theMesh = mesh;
  _theTree = NULL;

  _nodes = _theMesh -> nbFacets * 2 - 1;
  _leafs = _theMesh -> nbFacets;
  
  _theAccessFacetBBT = new std::vector<BoundingBoxTree*>;
  BoundingBoxTree* tree;
  
  unsigned ii;
  for (ii = 0 ; ii < _leafs ; ii++) {
    tree = new BoundingBoxTree;
    _theAccessFacetBBT -> push_back(tree);
  }
  
  svPTree = new std::vector<BoundingBoxTree*>[_nodes];
  svPNode = new std::vector<BoundingBoxNode*>[_nodes];
  svPBBox = new std::vector<BoundingBox*>[_nodes];

  BoundingBoxTree* ptree;
  BoundingBoxNode* pnode;
  BoundingBox* pbbox;

  for (/*unsigned*/ ii = 0 ; ii < _nodes ; ii++) {
    ptree = new BoundingBoxTree;
    svPTree -> push_back(ptree);
    pnode = new BoundingBoxNode;
    svPNode -> push_back(pnode);
    pbbox = new BoundingBox;
    svPBBox -> push_back(pbbox);
  }
  
  buildTree();

vtkDebugMacro(<< "BoundingBoxBinTree::BoundingBoxBinTree printTree");
  
  mesh -> setAccessFacetBBT(_theAccessFacetBBT);
  mesh -> setBoundingBoxTree(_theTree);
}

vtkBoundingBoxBinTree::~vtkBoundingBoxBinTree()
{
  unsigned ii; 
  for (ii = 0 ; ii < _leafs ; ii++) {
    delete (* _theAccessFacetBBT)[ii];
    }

  delete _theAccessFacetBBT;
  
  for (ii = 0 ; ii < _nodes ; ii++) {
    delete (* svPTree)[ii];
    delete (* svPNode)[ii];
    delete (* svPBBox)[ii];
  }
  
  delete[] svPTree;
  delete[] svPNode;
  delete[] svPBBox;
}

void vtkBoundingBoxBinTree::buildLeaf()
{
  BoundingBoxTree* ptree;
  BoundingBoxNode* pnode;
  BoundingBox* pbbox;
  
  vtkVector3f* points = new vtkVector3f[3];
  
  const vtkVector3ui* facets = _theMesh -> facets();
  const vtkVector3f* vertices = _theMesh -> vertices();

  // Create tree node and bbox for each facet
  unsigned ii = 0;
  while (ii < _leafs) {
    pnode = (* svPNode)[ii];
    ptree = (* svPTree)[ii];
    pbbox = (* svPBBox)[ii];

    points[0] = vertices[facets[ii].a];
    points[1] = vertices[facets[ii].b];
    points[2] = vertices[facets[ii].c];
    
    constructBox(points, 3, * pbbox); 

    pnode -> type = LEAF;
    pnode -> id = ii;
    pnode -> box = pbbox;

    ptree -> lson = NULL;
    ptree -> rson = NULL;
    ptree -> node = pnode;
    ptree -> father = NULL;
    ptree -> changed = false;

    // Set the list of facet <-> BoundingBoxTree*
    (* _theAccessFacetBBT)[ii] = ptree;
    
    ii++;
  }

  delete[] points;
}

void vtkBoundingBoxBinTree::buildTree()
{
  BoundingBoxTree* ptree;
  BoundingBoxNode* pnode;
  BoundingBox* pbbox;
  
  buildLeaf();

  // Flags to indicate if a bbox is already used
  bool* svFlags = new bool[_nodes]; 
  
  // initialize flags
  unsigned i;
  for (i = 0 ; i < _nodes ; i++) 
    svFlags[i] = true;
  vtkDebugMacro(<< "Building Bbox hierarchy in progress ...");
  
  // index of the 2 bbox to be enclosed
  unsigned iFirst = 0, iSecond = 0;   

  unsigned ii = _leafs;
  while (ii < _nodes) {
    
    // there is at least 2 BoundingBox free in the current level
    // select a couple : box1 box2
    // bbox1 is the firts unenclosed BoundingBox
    // bbox2 is the BoundingBox with minimum lost of this level 
    // (index < ii)
    
    // get the first unenclosed bbox, look from 0 to be sure not
    // forget one
    for (iFirst = 0 ; iFirst < _nodes ; iFirst++) 
      if (svFlags[iFirst]) break;
    
    svFlags[iFirst] = false;
    
    // then search for the bbox element with minimum lost 
    double minLost = MAX_REAL;
    double thisLost;
    
    for (unsigned i = iFirst + 1 ; i < ii ; i++) {
      if (svFlags[i]) {
	thisLost = lostVolume(* (* svPBBox)[iFirst], 
			      * (* svPBBox)[i]); 
	if (thisLost < minLost) {
	  iSecond = i;
	  minLost = thisLost;
	}
      }
    }
    
    // remove second from further use 
    svFlags[iSecond] = false;

    ptree = (* svPTree)[ii];
    pnode = (* svPNode)[ii];
    pbbox = (* svPBBox)[ii];
    
    // set the bbox enclosing first and second 
    combineBox(* (* svPBBox)[iFirst], 
	       * (* svPBBox)[iSecond], 
	       * pbbox);
    
    pnode -> type = TREE;
    pnode -> id = ii; 
    pnode -> box = pbbox;

    ptree -> lson = (* svPTree)[iFirst];
    ptree -> rson = (* svPTree)[iSecond];
    ptree -> node = pnode;
    ptree -> changed = false;
	ptree -> father = NULL;    
    ptree -> lson -> father = ptree;
    ptree -> rson -> father = ptree;

    ii++; // index of the next free bbox
  }
  
  // free svFlags 
  delete[] svFlags;
  
  // set the root of the bin tree
  _theTree = (* svPTree)[_nodes - 1];

  vtkDebugMacro(<< "Building bbox hierarchy done.");
}

void vtkBoundingBoxBinTree::combineBox(const BoundingBox& box1,
				    const BoundingBox& box2,
				    BoundingBox& boxRes)
{
  double up, low;

  // x
  up = MAX(box1.center.x + box1.diagonal.x, box2.center.x + box2.diagonal.x);
  low = MIN(box1.center.x - box1.diagonal.x, box2.center.x - box2.diagonal.x);
	
  boxRes.center.x = HALF * (up + low);
  boxRes.diagonal.x = MAX(MIN_REAL, HALF * (up - low));
  
  // y
  up = MAX(box1.center.y + box1.diagonal.y, box2.center.y + box2.diagonal.y);
  low = MIN(box1.center.y - box1.diagonal.y, box2.center.y - box2.diagonal.y);
	
  boxRes.center.y = HALF * (up + low);
  boxRes.diagonal.y = MAX(MIN_REAL, HALF * (up - low));
  
  // z
  up = MAX(box1.center.z + box1.diagonal.z, box2.center.z + box2.diagonal.z);
  low = MIN(box1.center.z - box1.diagonal.z, box2.center.z - box2.diagonal.z);
	
  boxRes.center.z = HALF * (up + low);
  boxRes.diagonal.z = MAX(BBBT_MIN_DIAG, HALF * (up - low));	

  vtkVector3f vup, vlow;
  
  // get the upper and lower vector
  vup = vlow = boxRes.center;
  vup -= boxRes.diagonal;
  vlow += boxRes.diagonal;

  // set the vertices in OpenGL format. may be usefull
  vtkVector3f::setXYZ(boxRes.vertices[0], vlow.x, vlow.y, vlow.z);
  vtkVector3f::setXYZ(boxRes.vertices[1], vup.x,  vlow.y, vlow.z);
  vtkVector3f::setXYZ(boxRes.vertices[7], vlow.x, vlow.y, vup.z );
  vtkVector3f::setXYZ(boxRes.vertices[6], vup.x,  vlow.y, vup.z );
  vtkVector3f::setXYZ(boxRes.vertices[3], vlow.x, vup.y,  vlow.z);
  vtkVector3f::setXYZ(boxRes.vertices[2], vup.x,  vup.y,  vlow.z);
  vtkVector3f::setXYZ(boxRes.vertices[4], vlow.x, vup.y,  vup.z );
  vtkVector3f::setXYZ(boxRes.vertices[5], vup.x,  vup.y,  vup.z );
  
  // finally pumpup the volume
  boxRes.volume = 
    8.0 * boxRes.diagonal.x * boxRes.diagonal.y * boxRes.diagonal.z ;	
}

void vtkBoundingBoxBinTree::constructBox(vtkVector3f vertices[],
				      unsigned nbVert,
				      BoundingBox& bBoxRes)
{
  double xx = MAX_REAL, yy = MAX_REAL, zz = MAX_REAL;
  double XX = - MAX_REAL, YY = - MAX_REAL, ZZ = - MAX_REAL;

  // look all vertices to set the in MAX in each dimension 
  unsigned ii;
  for (ii = 0; ii < nbVert; ii++) {
    xx = MIN(vertices[ii].x, xx);
    yy = MIN(vertices[ii].y, yy);
    zz = MIN(vertices[ii].z, zz);
    XX = MAX(vertices[ii].x, XX);
    YY = MAX(vertices[ii].y, YY);
    ZZ = MAX(vertices[ii].z, ZZ);
  }
  
  // then set Center and Diagonal from min MAX

  bBoxRes.center.x = HALF * (XX + xx);
  bBoxRes.center.y = HALF * (YY + yy);
  bBoxRes.center.z = HALF * (ZZ + zz);

  bBoxRes.diagonal.x = MAX(BBBT_MIN_DIAG, HALF * (XX - xx));
  bBoxRes.diagonal.y = MAX(BBBT_MIN_DIAG, HALF * (YY - yy));
  bBoxRes.diagonal.z = MAX(BBBT_MIN_DIAG, HALF * (ZZ - zz));

  vtkVector3f up, low;
  
  // get the upper and lower vector
  up = low = bBoxRes.center;
  up -= bBoxRes.diagonal;
  low += bBoxRes.diagonal;

  // set the vertices in OpenGL format. may be usefull
  vtkVector3f::setXYZ(bBoxRes.vertices[0], low.x, low.y, low.z);
  vtkVector3f::setXYZ(bBoxRes.vertices[1], up.x,  low.y, low.z);
  vtkVector3f::setXYZ(bBoxRes.vertices[7], low.x, low.y, up.z );
  vtkVector3f::setXYZ(bBoxRes.vertices[6], up.x,  low.y, up.z );
  vtkVector3f::setXYZ(bBoxRes.vertices[3], low.x, up.y,  low.z);
  vtkVector3f::setXYZ(bBoxRes.vertices[2], up.x,  up.y,  low.z);
  vtkVector3f::setXYZ(bBoxRes.vertices[4], low.x, up.y,  up.z );
  vtkVector3f::setXYZ(bBoxRes.vertices[5], up.x,  up.y,  up.z );
  
  // finally pumpup the volume
  bBoxRes.volume = 
    8.0 * bBoxRes.diagonal.x * bBoxRes.diagonal.y * bBoxRes.diagonal.z;
}

double vtkBoundingBoxBinTree::lostVolume(const BoundingBox& box1, 
				      const BoundingBox& box2)
{
  BoundingBox box; 
  combineBox(box1, box2, box);
 
  return (box.volume - (box1.volume + box2.volume));
}