//
// ********************************************************************
// * License and Disclaimer                                           *
// *                                                                  *
// * The  Geant4 software  is  copyright of the Copyright Holders  of *
// * the Geant4 Collaboration.  It is provided  under  the terms  and *
// * conditions of the Geant4 Software License,  included in the file *
// * LICENSE and available at  http://cern.ch/geant4/license .  These *
// * include a list of copyright holders.                             *
// *                                                                  *
// * Neither the authors of this software system, nor their employing *
// * institutes,nor the agencies providing financial support for this *
// * work  make  any representation or  warranty, express or implied, *
// * regarding  this  software system or assume any liability for its *
// * use.  Please see the license in the file  LICENSE  and URL above *
// * for the full disclaimer and the limitation of liability.         *
// *                                                                  *
// * This  code  implementation is the result of  the  scientific and *
// * technical work of the GEANT4 collaboration.                      *
// * By using,  copying,  modifying or  distributing the software (or *
// * any work based  on the software)  you  agree  to acknowledge its *
// * use  in  resulting  scientific  publications,  and indicate your *
// * acceptance of all terms of the Geant4 Software license.          *
// ********************************************************************
//
//
// Author: Mathieu Karamitros (kara (AT) cenbg . in2p3 . fr) 
//
// History:
// -----------
// 10 Oct 2011 M.Karamitros created
//
// -------------------------------------------------------------------
/*
 * Based on ``kdtree'', a library for working with kd-trees.
 * Copyright (C) 2007-2009 John Tsiombikas <nuclear@siggraph.org>
 * The original open-source version of this code
 * may be found at http://code.google.com/p/kdtree/
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 * 1. Redistributions of source code must retain the above copyright
 * notice, this
 * list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 * notice,
 *  this list of conditions and the following disclaimer in the
 * documentation
 *  and/or other materials provided with the distribution.
 * 3. The name of the author may not be used to endorse or promote products
 *  derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
/* single nearest neighbor search written by Tamas Nepusz
 * <tamas@cs.rhul.ac.uk>
 */

template<typename PointT>
  G4KDNode_Base* G4KDTree::InsertMap(PointT* point)
  {
    auto  node = new G4KDNode<PointT>(this, point, 0);
    this->__InsertMap(node);
    return node;
  }

template<typename PointT>
  G4KDNode_Base* G4KDTree::Insert(PointT* pos)
  {
    G4KDNode_Base* node = nullptr;
    if ((fRoot == nullptr) || !(fRoot->IsValid()))
    {
      fRoot = new G4KDNode<PointT>(this, pos, nullptr);
      node = fRoot;
      fNbNodes = 0;
      fNbNodes++;
      fNbActiveNodes++;
    }
    else
    {
      if ((node = fRoot->Insert<PointT>(pos)))
      {
        fNbNodes++;
        fNbActiveNodes++;
      }
    }

    if (fRect == nullptr)
    {
      fRect = new HyperRect(fDim);
      fRect->SetMinMax(*pos, *pos);
    }
    else
    {
      fRect->Extend(*pos);
    }

    return node;
  }

template<typename PointT>
  G4KDNode_Base* G4KDTree::Insert(const PointT& pos)
  {
    G4KDNode_Base* node = nullptr;
    if (!fRoot)
    {
      fRoot = new G4KDNodeCopy<PointT>(this, pos, 0);
      node = fRoot;
      fNbNodes = 0;
      fNbNodes++;
      fNbActiveNodes++;
    }
    else
    {
      if ((node = fRoot->Insert<PointT>(pos)))
      {
        fNbNodes++;
        fNbActiveNodes++;
      }
    }

    if (fRect == nullptr)
    {
      fRect = new HyperRect(fDim);
      fRect->SetMinMax(pos, pos);
    }
    else
    {
      fRect->Extend(pos);
    }

    return node;
  }

//__________________________________________________________________
template<typename Position>
  G4int G4KDTree::__NearestInRange(G4KDNode_Base* node,
                                   const Position& pos,
                                   const double& range_sq,
                                   const double& range,
                                   G4KDTreeResult& list,
                                   G4int ordered,
                                   G4KDNode_Base *source_node)
  {
    if (!node) return 0;

    G4double dist_sq(DBL_MAX), dx(DBL_MAX);
    G4int ret(-1), added_res(0);

    if (node->IsValid() && node != source_node)
    {
      G4bool do_break = false;
      dist_sq = 0;
      for (std::size_t i = 0; i < fDim; ++i)
      {
        dist_sq += sqr((*node)[i] - pos[(G4int)i]);
        if (dist_sq > range_sq)
        {
          do_break = true;
          break;
        }
      }
      if (!do_break && dist_sq <= range_sq)
      {
        list.Insert(dist_sq, node);
        added_res = 1;
      }
    }

    dx = pos[node->GetAxis()] - (*node)[node->GetAxis()];

    ret = __NearestInRange(dx <= 0.0 ? node->GetLeft() : node->GetRight(), pos,
                           range_sq, range, list, ordered, source_node);
    if (ret >= 0 && std::fabs(dx) <= range)
    {
      added_res += ret;
      ret = __NearestInRange(dx <= 0.0 ? node->GetRight() : node->GetLeft(),
                             pos, range_sq, range, list, ordered, source_node);
    }

    if (ret == -1)
    {
      return -1;
    }
    added_res += ret;

    return added_res;
  }

//__________________________________________________________________
template<typename Position>
  void G4KDTree::__NearestToPosition(G4KDNode_Base *node,
                                     const Position &pos,
                                     G4KDNode_Base *&result,
                                     G4double *result_dist_sq,
                                     HyperRect* rect)
  {
    G4int dir = node->GetAxis();
    G4double dummy(0.), dist_sq(-1.);
    G4KDNode_Base* nearer_subtree(nullptr), *farther_subtree(nullptr);
    G4double *nearer_hyperrect_coord(nullptr), *farther_hyperrect_coord(nullptr);

    /* Decide whether to go left or right in the tree */
    dummy = pos[dir] - (*node)[dir];
    if (dummy <= 0)
    {
      nearer_subtree = node->GetLeft();
      farther_subtree = node->GetRight();

      nearer_hyperrect_coord = rect->GetMax() + dir;
      farther_hyperrect_coord = rect->GetMin() + dir;
    }
    else
    {
      nearer_subtree = node->GetRight();
      farther_subtree = node->GetLeft();
      nearer_hyperrect_coord = rect->GetMin() + dir;
      farther_hyperrect_coord = rect->GetMax() + dir;
    }

    if (nearer_subtree)
    {
      /* Slice the hyperrect to get the hyperrect of the nearer subtree */
      dummy = *nearer_hyperrect_coord;
      *nearer_hyperrect_coord = (*node)[dir];
      /* Recurse down into nearer subtree */
      __NearestToPosition(nearer_subtree, pos, result, result_dist_sq, rect);
      /* Undo the slice */
      *nearer_hyperrect_coord = dummy;
    }

    /* Check the distance of the point at the current node, compare it
     * with our best so far */
    if (node->IsValid()) // TODO
    {
      dist_sq = 0;
      G4bool do_break = false;
      for (std::size_t i = 0; i < fDim; ++i)
      {
        dist_sq += sqr((*node)[i] - pos[(G4int)i]);
        if (dist_sq > *result_dist_sq)
        {
          do_break = true;
          break;
        }
      }
      if (!do_break && dist_sq < *result_dist_sq)
      {
        result = node;
        *result_dist_sq = dist_sq;
      }
    }

    if (farther_subtree)
    {
      /* Get the hyperrect of the farther subtree */
      dummy = *farther_hyperrect_coord;
      *farther_hyperrect_coord = (*node)[dir];
      /* Check if we have to recurse down by calculating the closest
       * point of the hyperrect and see if it's closer than our
       * minimum distance in result_dist_sq. */
      if (rect->CompareDistSqr(pos, result_dist_sq))
      {
        /* Recurse down into farther subtree */
        __NearestToPosition(farther_subtree, pos, result, result_dist_sq, rect);
      }
      /* Undo the slice on the hyperrect */
      *farther_hyperrect_coord = dummy;
    }
  }

template<typename Position>
  G4KDTreeResultHandle G4KDTree::Nearest(const Position& pos)
  {
    //    G4cout << "Nearest(pos)" << G4endl ;

    if (!fRect) return nullptr;

    G4KDNode_Base *result(nullptr);
    G4double dist_sq = DBL_MAX;

    /* Duplicate the bounding hyperrectangle, we will work on the copy */
    auto  newrect = new HyperRect(*fRect);

    /* Our first estimate is the root node */
    /* Search for the nearest neighbour recursively */
    __NearestToPosition(fRoot, pos, result, &dist_sq, newrect);

    /* Free the copy of the hyperrect */
    delete newrect;

    /* Store the result */
    if (result)
    {
      G4KDTreeResultHandle rset = new G4KDTreeResult(this);
      rset->Insert(dist_sq, result);
      rset->Rewind();
      return rset;
    }
    
    return nullptr;
  }

//__________________________________________________________________
template<typename Position>
  void G4KDTree::__NearestToNode(G4KDNode_Base* source_node,
                                 G4KDNode_Base* node,
                                 const Position& pos,
                                 std::vector<G4KDNode_Base*>& result,
                                 G4double *result_dist_sq,
                                 HyperRect* rect,
                                 G4int& nbresult)
  {
    G4int dir = node->GetAxis();
    G4double dummy, dist_sq;
    G4KDNode_Base *nearer_subtree(nullptr), *farther_subtree(nullptr);
    G4double *nearer_hyperrect_coord(nullptr), *farther_hyperrect_coord(nullptr);

    /* Decide whether to go left or right in the tree */
    dummy = pos[dir] - (*node)[dir];
    if (dummy <= 0)
    {
      nearer_subtree = node->GetLeft();
      farther_subtree = node->GetRight();
      nearer_hyperrect_coord = rect->GetMax() + dir;
      farther_hyperrect_coord = rect->GetMin() + dir;
    }
    else
    {
      nearer_subtree = node->GetRight();
      farther_subtree = node->GetLeft();
      nearer_hyperrect_coord = rect->GetMin() + dir;
      farther_hyperrect_coord = rect->GetMax() + dir;
    }

    if (nearer_subtree)
    {
      /* Slice the hyperrect to get the hyperrect of the nearer subtree */
      dummy = *nearer_hyperrect_coord;
      *nearer_hyperrect_coord = (*node)[dir];
      /* Recurse down into nearer subtree */
      __NearestToNode(source_node, nearer_subtree, pos, result, result_dist_sq,
                      rect, nbresult);
      /* Undo the slice */
      *nearer_hyperrect_coord = dummy;
    }

    /* Check the distance of the point at the current node, compare it
     * with our best so far */
    if (node->IsValid() && node != source_node)
    {
      dist_sq = 0;
      G4bool do_break = false;
      for (std::size_t i = 0; i < fDim; ++i)
      {
        dist_sq += sqr((*node)[i] - pos[i]);
        if (dist_sq > *result_dist_sq)
        {
          do_break = true;
          break;
        }
      }
      if (!do_break)
      {
        if (dist_sq < *result_dist_sq)
        {
          result.clear();
          nbresult = 1;
          result.push_back(node);
          *result_dist_sq = dist_sq;
        }
        else if (dist_sq == *result_dist_sq)
        {
          result.push_back(node);
          nbresult++;
        }
      }
    }

    if (farther_subtree)
    {
      /* Get the hyperrect of the farther subtree */
      dummy = *farther_hyperrect_coord;
      *farther_hyperrect_coord = (*node)[dir];
      /* Check if we have to recurse down by calculating the closest
       * point of the hyperrect and see if it's closer than our
       * minimum distance in result_dist_sq. */
      //        if (hyperrect_dist_sq(rect, pos) < *result_dist_sq)
      if (rect->CompareDistSqr(pos, result_dist_sq))
      {
        /* Recurse down into farther subtree */
        __NearestToNode(source_node, farther_subtree, pos, result,
                        result_dist_sq, rect, nbresult);
      }
      /* Undo the slice on the hyperrect */
      *farther_hyperrect_coord = dummy;
    }
  }

template<typename Position>
  G4KDTreeResultHandle G4KDTree::NearestInRange(const Position& pos,
                                                const G4double& range)
  {
    G4int ret(-1);

    const G4double range_sq = sqr(range);

    G4KDTreeResultHandle rset = new G4KDTreeResult(this);
    if ((ret = __NearestInRange(fRoot, pos, range_sq, range, *(rset()), 0)) == -1)
    {
      rset = nullptr;
      return rset;
    }
    rset->Sort();
    rset->Rewind();
    return rset;
  }