/*
 * photonkdtree.C : implementation of specific kdtree for photonmaps
 */

#include "photonkdtree.H"

class Cnormalquery
{
public:
  CIrrPhoton *photon;
  float* point;
  VECTOR normal;
  float threshold;
  float maxdist;
};

static Cnormalquery qdat_s;

// Distance calculation COPY FROM kdtree.C !
inline static float SqrDistance3D(float *a, float *b)
{
  float result,tmp;

  tmp = *(a++) - *(b++);
  result = tmp*tmp;
  //  result = *ptmp++

  tmp = *(a++) - *(b++);
  result += tmp*tmp;

  tmp = *a - *b;
  result += tmp*tmp;

  return result;
}


void CPhotonkdtree::NormalBQuery_rec(const int index)
{
  const Cbkdnode &node=m_broot[index];
  int discr = node.Discriminator();

  float dist;
  int nearIndex, farIndex;
    
  // Recursive call to the child nodes
   
  // Test discr (reuse dist)

  if(index < m_firstLeaf)
  {
    dist = ((float *)node.m_data)[discr] - (qdat_s.point)[discr];
    
    if( dist >= 0.0 )  // qdat_s.point[discr] <= ((float *)node->m_data)[discr]
    {
      nearIndex = (index << 1) + 1; //node->loson;
      farIndex = nearIndex+1; //node->hison;
    }
    else
    {
      farIndex = (index << 1) + 1;//node->loson;
      nearIndex = farIndex+1;//node->hison;
    }

    // Always call near node recursively
    
    if(nearIndex < m_numBalanced)
    {
      NormalBQuery_rec(nearIndex);
    }
    
    dist *= dist; // Square distance to the separator plane
    if((farIndex < m_numBalanced) && (dist < qdat_s.maxdist))
    {
      // Discriminator line closer than maxdist : nearer points can lie
      // on the far side. Or there are still not enough nodes found
      NormalBQuery_rec(farIndex);
    }
  }

  // if(!(node->m_flags & qdat_s.excludeFlags))
  {
    dist = SqrDistance3D((float *)node.m_data, qdat_s.point);

    if(dist < qdat_s.maxdist)
    {
      // Normal constraint

      if((((CIrrPhoton *)node.m_data)->Normal() & qdat_s.normal) 
	 > qdat_s.threshold)
      {
	// Replace point if distance < maxdist AND normal is similar
	qdat_s.maxdist = dist;
	qdat_s.photon = (CIrrPhoton *)node.m_data;
      }
    }
  }
}


CIrrPhoton* CPhotonkdtree::NormalPhotonQuery(VECTOR *pos, VECTOR *normal, 
					     float threshold, float maxR2)
{
  // Fill qdat_s
  
  qdat_s.photon = NULL;
  qdat_s.normal = *normal;
  qdat_s.point = (float *)pos;
  qdat_s.threshold = threshold;
  qdat_s.maxdist = maxR2;

  if(m_broot && (m_numNodes > 0) && (m_numUnbalanced == 0))
  {
    // Find the best photon
    NormalBQuery_rec(0);
  }
  return qdat_s.photon;
}