// Copyright (c) 2015 INRIA Sophia-Antipolis (France).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org).
// 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 3 of the License, or (at your option) any later version.
//
// Licensees holding a valid commercial license may use this file in
// accordance with the commercial license agreement provided with the software.
//
// This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
// WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
//
// $URL$
// $Id$
// SPDX-License-Identifier: GPL-3.0+
//
//
// Author(s)     : Sven Oesau, Yannick Verdie, Clément Jamin, Pierre Alliez
//

#ifndef CGAL_SHAPE_DETECTION_3_OCTREE_H
#define CGAL_SHAPE_DETECTION_3_OCTREE_H

#include <CGAL/license/Point_set_shape_detection_3.h>


#include <limits>
#include <stack>

#include <CGAL/Random.h>
#include <CGAL/Bbox_3.h>
#include <CGAL/Shape_detection_3/Shape_base.h>


extern int scoreTime;

namespace CGAL {
  namespace Shape_detection_3 {
  
    template<class Traits> 
    class Efficient_RANSAC;
    
  namespace internal {

    const std::size_t size_t_max = (std::numeric_limits<std::size_t>::max)();
  
    template<class Sdt>
    class DirectPointAccessor {
    public:
      typedef Sdt Sd_traits;
      typedef typename Sd_traits::Input_range::iterator Input_iterator;

      DirectPointAccessor() {}
      DirectPointAccessor(const Input_iterator &begin,
                          const Input_iterator &beyond, 
                          std::size_t offset) : m_first(begin), m_offset(offset) {
        m_beyond = (beyond == begin) ? begin : beyond - 1;
      }

      Input_iterator at(std::size_t i) {
        return m_first + i;
      }

      std::size_t index(std::size_t i) {
        return i + m_offset;
      }

      std::size_t offset() {
        return m_offset;
      }

      std::size_t size() {
        return m_beyond - m_first + 1;
      }

      Input_iterator first() {
        return m_first;
      }

      Input_iterator beyond() {
        return m_beyond;
      }

      void setData(Input_iterator &begin, Input_iterator &beyond) {
        m_beyond = (beyond == begin) ? begin : beyond - 1;
      }

      void swap(std::size_t a, std::size_t b) {
        typename std::iterator_traits<Input_iterator>::value_type tmp;
        tmp = m_first[a];
        m_first[a] = m_first[b];
        m_first[b] = tmp;
      }

    protected:
      Input_iterator m_first;

    private:
      Input_iterator m_beyond;
      std::size_t m_offset;
    };

    template<class Sdt>
    class IndexedPointAccessor {
    public:
      typedef Sdt Sd_traits;
      typedef typename Sd_traits::Input_range::iterator Input_iterator;

      IndexedPointAccessor() {}
      IndexedPointAccessor(const Input_iterator &begin,
                           const Input_iterator &beyond, std::size_t)
                           : m_first(begin) {
        m_beyond = (beyond == begin) ? begin : beyond - 1;
        m_indices.resize(size());
        for (std::size_t i = 0;i<size();i++)
          m_indices[i] = i;
      }

      Input_iterator at(std::size_t i) {
        return m_first + m_indices[i];
      }

      std::size_t index(std::size_t i) {
        return m_indices[i];
      }

      std::size_t offset() {
        return 0;
      }

      Input_iterator first() {
        return m_first;
      }

      Input_iterator beyond() {
        return m_beyond;
      }

      void setData(Input_iterator &begin, Input_iterator &beyond) {
        m_beyond = (beyond == begin) ? begin : beyond - 1;
        m_indices.resize(size());
        for (std::size_t i = 0;i<size();i++)
          m_indices[i] = i;
      }

      std::size_t size() {
        return m_beyond - m_first + 1;
      }

      void swap(std::size_t a, std::size_t b) {
        std::size_t tmp = m_indices[a];
        m_indices[a] = m_indices[b];
        m_indices[b] = tmp;
      }

    protected:
      Input_iterator m_first;

    private:
      std::vector<std::size_t> m_indices;
      Input_iterator m_beyond;
    };

    template<class PointAccessor>
    class Octree : public PointAccessor {

      typedef typename PointAccessor::Sd_traits Sd_traits;
      typedef typename Sd_traits::Input_range::iterator Input_iterator;
      typedef Shape_base<Sd_traits> Shape;
      typedef typename Sd_traits::Point_3 Point_3;
      typedef typename Sd_traits::Vector_3 Vector_3;
      typedef typename Sd_traits::FT FT;
      typedef typename Sd_traits::Point_map Point_map;
      typedef typename Sd_traits::Normal_map Normal_map;

      template<class Sd_traits>
        friend class ::CGAL::Shape_detection_3::Efficient_RANSAC;

      struct Cell {
        std::size_t first, last;
        Cell *child[8];
        Point_3 center;
        std::size_t level;

        Cell(std::size_t first, std::size_t last, Point_3 center, std::size_t level)
          : first(first), last(last), center(center), level(level) {
            memset(child, 0, sizeof(Cell *) * 8);
        }

        bool isLeaf() const {
          for (std::size_t i = 0;i<8;i++) {
            if (child[i])
              return false;
          }
          return true;
        }

        std::size_t size() const {
          if (first == size_t_max || last == size_t_max)
            return 0;
          else return (last - first + 1);
        }
      };

      // --------------------------------------------------------------------------
      // Utilities
      // --------------------------------------------------------------------------
      FT get_x(const Vector_3& v){ return m_traits.compute_x_3_object()(v); }
      FT get_y(const Vector_3& v){ return m_traits.compute_y_3_object()(v); }
      FT get_z(const Vector_3& v){ return m_traits.compute_z_3_object()(v); }
      FT get_x(const Point_3& p){ return m_traits.compute_x_3_object()(p); }
      FT get_y(const Point_3& p){ return m_traits.compute_y_3_object()(p); }
      FT get_z(const Point_3& p){ return m_traits.compute_z_3_object()(p); }
      FT get_coord(const Point_3& p, unsigned int d)
      {
        CGAL_assertion(d >= 0 && d < 3);
        switch (d)
        {
          case 0: return get_x(p);
          case 1: return get_y(p);
          case 2: return get_z(p);
          default: return FT(0);
        }
      }

      Point_3 constr_pt(FT x, FT y, FT z) const
      { return m_traits.construct_point_3_object()(x, y, z); }
      Vector_3 constr_vec(const Point_3& p, const Point_3& q) const
      { return m_traits.construct_vector_3_object()(p, q); }

      Point_3 transl(const Point_3& p, const Vector_3 &v)
      { return m_traits.construct_translated_point_3_object()(p, v); }
        
    public:
      Octree(Sd_traits const& traits) 
        : m_traits(traits), m_bucket_size(20), m_set_max_level(10), m_root(NULL) {}
      Octree(Sd_traits const& traits,
             const Input_iterator &first,
             const Input_iterator &beyond,
             Point_map& point_pmap,
             Normal_map& normal_pmap,
             std::size_t offset = 0,
             std::size_t bucketSize = 20, 
             std::size_t maxLevel = 10)
             : PointAccessor(first, beyond, offset),
               m_traits(traits),
               m_root(NULL),
               m_bucket_size(bucketSize),
               m_set_max_level(maxLevel),
               m_point_pmap (point_pmap),
               m_normal_pmap (normal_pmap) {}

      ~Octree() {
        if (!m_root)
          return;

        std::stack<Cell *> stack;
        stack.push(m_root);
        while (!stack.empty()) {
          Cell *cell = stack.top();
          stack.pop();

          for (std::size_t i = 0;i<8;i++)
            if (cell->child[i])
              stack.push(cell->child[i]);

          delete cell;
        }
      }

      // Sorting data in a way such that points of one cell
      // are always in one range and ordered child-wise:
      // +---+---+
      // | 1.| 0.|
      // +---+---+
      // | 3.| 2.|
      // +---+---+
      // z max before z min, then y max before y min, then x max before x min
      void createTree(double cluster_epsilon_for_max_level_recomputation = -1.) {
        buildBoundingCube();
        std::size_t count = 0;
        m_max_level = 0;

        if (cluster_epsilon_for_max_level_recomputation > 0.)
        {
          FT bbox_diagonal = (FT) CGAL::sqrt(
            (m_bBox.xmax() - m_bBox.xmin()) * (m_bBox.xmax() - m_bBox.xmin())
            + (m_bBox.ymax() - m_bBox.ymin()) * (m_bBox.ymax() - m_bBox.ymin()) 
            + (m_bBox.zmax() - m_bBox.zmin()) * (m_bBox.zmax() - m_bBox.zmin()));

          m_set_max_level = std::size_t (std::log (bbox_diagonal
                                                    / cluster_epsilon_for_max_level_recomputation)
                                         / std::log (2.0));
        }

        std::stack<Cell *> stack;
        m_root = new Cell(0, this->size() - 1, m_center, 0);
        stack.push(m_root);
        while (!stack.empty()) {
          Cell *cell= stack.top();
          stack.pop();

          m_max_level = std::max<std::size_t>(m_max_level, cell->level);
          if (cell->level == m_set_max_level)
            continue;
          
          std::size_t zLowYHighXSplit, zLowYLowXSplit, zLowYSplit;
          std::size_t zHighYSplit, zHighYHighXSplit, zHighYLowXSplit;

          std::size_t zSplit = split(cell->first, cell->last, 2, get_z(cell->center));

          if (zSplit != size_t_max) {

            zLowYSplit = split(cell->first, zSplit, 1, get_y(cell->center));
            if (zLowYSplit != size_t_max) {
              zLowYLowXSplit = split(cell->first,
                                     zLowYSplit, 0, get_x(cell->center));
              zLowYHighXSplit = split(zLowYSplit + 1,
                                      zSplit, 0, get_x(cell->center)); 
            }
            else {
              zLowYLowXSplit = size_t_max;
              zLowYHighXSplit = split(cell->first, zSplit, 0, get_x(cell->center));
            }

            zHighYSplit = split(zSplit + 1, cell->last, 1, get_y(cell->center));  

            if (zHighYSplit != size_t_max) {
              zHighYHighXSplit = split(zHighYSplit + 1,
                                       cell->last, 0, get_x(cell->center));
              zHighYLowXSplit = split(zSplit + 1,
                                      zHighYSplit, 0, get_x(cell->center));
            }
            else {
              zHighYLowXSplit = size_t_max;
              zHighYHighXSplit = split(zSplit + 1,
                                       cell->last, 0, get_x(cell->center));
            }
          }
          else {
            zLowYSplit = size_t_max;
            zLowYLowXSplit = size_t_max;
            zLowYHighXSplit = size_t_max;

            zHighYSplit = split(cell->first,
                                cell->last,
                                1,
                                get_y(cell->center)); 

            if (zHighYSplit != size_t_max) {
              zHighYHighXSplit = split(zHighYSplit + 1, 
                                       cell->last, 
                                       0, 
                                       get_x(cell->center));

              zHighYLowXSplit = split(cell->first, 
                                      zHighYSplit,
                                      0, 
                                      get_x(cell->center)); 
            }
            else {
              zHighYLowXSplit = size_t_max;
              zHighYHighXSplit = split(cell->first,
                                       cell->last, 
                                       0,
                                       get_x(cell->center));
            }
          }


          FT width = m_width / (1<<(cell->level + 1));

          if (zSplit != size_t_max) {
            if (zLowYSplit != size_t_max) {
              if (zLowYLowXSplit != size_t_max) {

                if (cell->first <= zLowYLowXSplit) {
                  //---
                  cell->child[7] = new Cell(cell->first,
                                            zLowYLowXSplit, 
                                            transl(cell->center, constr_vec(
                                              ORIGIN, constr_pt(-width,-width,-width))),
                                            cell->level + 1);

                  if (cell->child[7]->size() > m_bucket_size)
                    stack.push(cell->child[7]);
                }
              }
              else zLowYLowXSplit = cell->first - 1;

              if (zLowYLowXSplit < zLowYSplit || zLowYLowXSplit == size_t_max) {
                //+--
                cell->child[6] = new Cell(zLowYLowXSplit + 1,
                                          zLowYSplit, 
                                          transl(cell->center, constr_vec(
                                            ORIGIN, constr_pt(width,-width,-width))),
                                          cell->level + 1);

                if (cell->child[6]->size() > m_bucket_size)
                  stack.push(cell->child[6]);
              }
            }
            else zLowYSplit = cell->first - 1;

            if (zLowYHighXSplit != size_t_max) {

              if (zLowYSplit < zLowYHighXSplit || zLowYSplit == size_t_max) {
                //-+-
                cell->child[5] = new Cell(zLowYSplit + 1, 
                                          zLowYHighXSplit, 
                                          transl(cell->center, constr_vec(
                                            ORIGIN, constr_pt(-width, width,-width))),
                                          cell->level + 1);

                if (cell->child[5]->size() > m_bucket_size)
                  stack.push(cell->child[5]);
              }
            }
            else zLowYHighXSplit = zLowYSplit;

            if (zLowYHighXSplit < zSplit || zLowYHighXSplit == size_t_max) {
              //++-
              cell->child[4] = new Cell(zLowYHighXSplit + 1,
                                        zSplit, 
                                        transl(cell->center, constr_vec(
                                          ORIGIN, constr_pt(width, width,-width))),
                                        cell->level + 1);

              if (cell->child[4]->size() > m_bucket_size)
                stack.push(cell->child[4]);
            }
          }
          else zSplit = cell->first - 1;

          if (zHighYSplit != size_t_max) {
            if (zHighYLowXSplit != size_t_max) {

              if (zSplit < zHighYLowXSplit || zSplit == size_t_max) {
                //--+
                cell->child[3] = new Cell(zSplit + 1,
                                          zHighYLowXSplit,
                                          transl(cell->center, constr_vec(
                                            ORIGIN, constr_pt(-width,-width, width))),
                                          cell->level + 1);

                if (cell->child[3]->size() > m_bucket_size)
                  stack.push(cell->child[3]);
              }
            }
            else zHighYLowXSplit = zSplit;

            if (zHighYLowXSplit < zHighYSplit || zHighYLowXSplit == size_t_max) {
              //+-+
              cell->child[2] = new Cell(zHighYLowXSplit + 1,
                                        zHighYSplit, 
                                        transl(cell->center, constr_vec(
                                          ORIGIN, constr_pt(width,-width, width))),
                                        cell->level + 1);

              if (cell->child[2]->size() > m_bucket_size)
                stack.push(cell->child[2]);
            }

          }
          else zHighYSplit = zSplit;

          if (zHighYHighXSplit != size_t_max) {
            if (zHighYSplit < zHighYHighXSplit || zHighYSplit == size_t_max) {
              //-++
              cell->child[1] = new Cell(zHighYSplit + 1,
                                        zHighYHighXSplit, 
                                        transl(cell->center, constr_vec(
                                          ORIGIN, constr_pt(-width, width, width))), 
                                        cell->level + 1);

              if (cell->child[1]->size() > m_bucket_size)
                stack.push(cell->child[1]);
            }
          }
          else zHighYHighXSplit = zHighYSplit;

          if (zHighYHighXSplit <= cell->last || zHighYHighXSplit == size_t_max) {
            if (zHighYHighXSplit < cell->last || zHighYHighXSplit == size_t_max) {
              //+++
              cell->child[0] = new Cell(zHighYHighXSplit + 1, 
                                        cell->last,
                                        transl(cell->center, constr_vec(
                                          ORIGIN, constr_pt(width, width, width))),
                                        cell->level + 1);

              if (cell->child[0]->size() > m_bucket_size)
                stack.push(cell->child[0]);
            }
          }

          std::size_t sum = 0;
          for (std::size_t i = 0;i<8;i++)
            sum += (cell->child[i]) ? cell->child[i]->size() : 0;

          count++;
        }
      }

      bool drawSamplesFromCellContainingPoint(const Point_3 &p,
                                              std::size_t level,
                                              std::set<std::size_t>& indices,
                                              const std::vector<int>& shapeIndex,
                                              std::size_t requiredSamples) {

        bool upperZ, upperY, upperX;
        Cell *cur = m_root;

        while (cur && cur->level < level) {
          upperX = get_x(cur->center) <= get_x(p);
          upperY = get_y(cur->center) <= get_y(p);
          upperZ = get_z(cur->center) <= get_z(p);

          if (upperZ) {
            if (upperY)
              cur = (upperX) ? cur->child[0] : cur->child[1];
            else cur = (upperX) ? cur->child[2] : cur->child[3];
          }
          else {
            if (upperY)
              cur = (upperX) ? cur->child[4] : cur->child[5];
            else cur = (upperX) ? cur->child[6] : cur->child[7];
          }
        }

        if (cur) {
          std::size_t enough = 0;
          for (std::size_t i = cur->first;i<=cur->last;i++) {
            std::size_t j = this->index(i);
            if (shapeIndex[j] == -1) {
              enough++;
              if (enough >= requiredSamples)
                break;
            }
          }
          if (enough >= requiredSamples) {
            do {
              std::size_t p = CGAL::get_default_random().
                uniform_int<std::size_t>(0, cur->size() - 1);
              std::size_t j = this->index(cur->first + p);
              if (shapeIndex[j] == -1)
                indices.insert(j);
            } while (indices.size() < requiredSamples);

            return true;
          }
          else return false;
        }
        else return false;
      }

      std::size_t maxLevel() {
        return m_max_level;
      }

      std::size_t fullScore(Shape *candidate,
                       std::vector<int> &shapeIndex,
                       FT epsilon, 
                       FT normal_threshold) {

        std::vector<std::size_t> indices(m_root->size());
        for (std::size_t i = 0;i<m_root->size();i++) {
          indices[i] = index(m_root->first + i);
        }
        
        candidate->cost_function(this->begin() + m_root->first, 
                                 this->begin() + m_root->last,
                                 shapeIndex,
                                 epsilon, 
                                 normal_threshold, 
                                 indices);

        return candidate->m_indices.size();
      }

      std::size_t score(Shape *candidate, 
                   std::vector<int> &shapeIndex,
                   FT epsilon,
                   FT normal_threshold) {

        std::stack<Cell *> stack;
        stack.push(m_root);

        while(!stack.empty()) {
          Cell *cell = stack.top();
          stack.pop();

          FT width = m_width / (1<<(cell->level));

          FT diag = CGAL::sqrt(FT(3) * width * width) + epsilon;

          FT dist = candidate->squared_distance(cell->center);

          if (dist > (diag * diag))
            continue;

          // differ between full or partial overlap?
          // if full overlap further traversal of this branch is not necessary
          if (cell->isLeaf()) {
            std::vector<std::size_t> indices;
            indices.reserve(cell->size());
            for (std::size_t i = 0;i<cell->size();i++) {
              if (shapeIndex[this->index(cell->first + i)] == -1) {
                indices.push_back(this->index(cell->first + i));
              }
            }

            candidate->cost_function(epsilon, 
                                     normal_threshold, 
                                     indices);
          }
          else {
            for (std::size_t i = 0;i<8;i++)
              if (cell->child[i])
                stack.push(cell->child[i]);
          }

        }

        return candidate->m_indices.size();
      }

      void setBucketSize(std::size_t bucketSize) {
        m_bucket_size = bucketSize;
      }

      const Bbox_3 &boundingBox() {
        return m_bBox;
      }
        
      const Bbox_3 &buildBoundingCube() {
        FT min[] = {std::numeric_limits<FT>::infinity(),
                    std::numeric_limits<FT>::infinity(),
                    std::numeric_limits<FT>::infinity()};
        FT max[] = {-std::numeric_limits<FT>::infinity(),
                    -std::numeric_limits<FT>::infinity(),
                    -std::numeric_limits<FT>::infinity()};

        for (std::size_t i = 0;i<this->size();i++) {
          Point_3 p = get(m_point_pmap, *this->at(i));
          min[0] = (std::min<FT>)(min[0], get_x(p));
          min[1] = (std::min<FT>)(min[1], get_y(p));
          min[2] = (std::min<FT>)(min[2], get_z(p));
          max[0] = (std::max<FT>)(max[0], get_x(p));
          max[1] = (std::max<FT>)(max[1], get_y(p));
          max[2] = (std::max<FT>)(max[2], get_z(p));
        }

        m_bBox = Bbox_3(min[0], min[1], min[2], max[0], max[1], max[2]);

        m_width = (std::max)(max[0] - min[0], 
          (std::max)(max[1] - min[1], max[2] - min[2])) * (FT) 0.5;
        m_center = constr_pt((min[0] + max[0]) * (FT) 0.5,
                             (min[1] + max[1]) * (FT) 0.5,
                             (min[2] + max[2]) * (FT) 0.5);

        return m_bBox;
      }

      // returns index of last point below threshold
      std::size_t split(std::size_t first, std::size_t last, std::size_t dimension, FT threshold) {
        if (last == size_t_max || first == size_t_max) 
          return size_t_max;

        if (first > last)
          return first - 1;

        std::size_t origFirst = first;

        while(first < last) {
          // find first above threshold
          while (get_coord(
                   get(m_point_pmap, *this->at(first)),
                   static_cast<unsigned int>(dimension)) < threshold
                 && first < last) {
            first++;
          }

          // check if last has been reached
          if (first == last) {
            return (get_coord(
                      get(m_point_pmap, *this->at(first)),
                      static_cast<unsigned int>(dimension)) < threshold) ?
                   first : (first == origFirst) ? size_t_max : first - 1;
          }

          // find last below threshold
          while (get_coord(
                   get(m_point_pmap, *this->at(last)),
                   static_cast<unsigned int>(dimension)) >= threshold
                 && last > first) {
            last--;
          }

          // check if first has been reached
          if (last == first) {
            return (get_coord(
                      get(m_point_pmap, *this->at(first)),
                      static_cast<unsigned int>(dimension)) < threshold) ?
                    first : (first == origFirst) ? size_t_max : first - 1;
          }

          this->swap(first, last);
          first++;
          last--;
        }

        return (get_coord(
                  get(m_point_pmap, *this->at(first)),
                  static_cast<unsigned int>(dimension)) < threshold) ?
               first : (first == origFirst) ? size_t_max : first - 1;
      }

      Sd_traits m_traits;
      Bbox_3 m_bBox;
      Cell *m_root;
      Point_3 m_center;
      FT m_width;
      std::size_t m_bucket_size;
      std::size_t m_set_max_level;
      std::size_t m_max_level;
      Point_map m_point_pmap;
      Normal_map m_normal_pmap;
    };
  }
  }
}

#endif