// Copyright (c) 2013-2020, SIB - Swiss Institute of Bioinformatics and
//                          Biozentrum - University of Basel
// 
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
// 
//   http://www.apache.org/licenses/LICENSE-2.0
// 
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.


#ifndef PROMOD3_CORE_TETRAHEDRAL_POLYTOPE_HH
#define PROMOD3_CORE_TETRAHEDRAL_POLYTOPE_HH

#include <promod3/core/runtime_profiling.hh>
#include <promod3/config.hh>
#include <ost/config.hh>
#include <vector>

#if PM3_ENABLE_SSE && OST_DOUBLE_PRECISION == 0
#include <xmmintrin.h>
#endif

namespace promod3{ namespace core{

// Tetrahedral polytope for collision detection.
// Initialize it with a position and a collision distance (radius)
// The Overlap function guarantees to return true if two polytopes are closer
// than the sum of their radii but it might already return true if they're
// a bit further away...

struct TetrahedralPolytope{

  TetrahedralPolytope() { }

  TetrahedralPolytope(Real x, Real y, Real z, Real radius);

  TetrahedralPolytope(Real* min_bounds, Real* max_bounds){
    memcpy(&bounds[0], min_bounds, 4 * sizeof(Real));
    memcpy(&bounds[4], max_bounds, 4 * sizeof(Real));
  }

  #if PM3_ENABLE_SSE && OST_DOUBLE_PRECISION == 0

  TetrahedralPolytope(const __m128& min_bounds, const __m128& max_bounds) {
    _mm_store_ps(&bounds[0], min_bounds);
    _mm_store_ps(&bounds[4], max_bounds);
  }

  inline bool Overlap(const TetrahedralPolytope& other) const{
    __m128 v1 = _mm_load_ps(bounds);            // min values
    __m128 v2 = _mm_load_ps(&bounds[4]);        // max_values
    __m128 v3 = _mm_load_ps(other.bounds);      // other_min_values
    __m128 v4 = _mm_load_ps(&other.bounds[4]);  // other_max_values
    v1 = _mm_cmpgt_ps(v1,v4);
    v2 = _mm_cmplt_ps(v2,v3);
    v1 = _mm_or_ps(v1,v2);
    return _mm_movemask_ps(v1) == 0;
  }
  #else

  inline bool Overlap(const TetrahedralPolytope& other) const{
    return !(bounds[0] > other.bounds[4] || bounds[1] > other.bounds[5] || 
             bounds[2] > other.bounds[6] || bounds[3] > other.bounds[7] ||
             bounds[4] < other.bounds[0] || bounds[5] < other.bounds[1] || 
             bounds[6] < other.bounds[2] || bounds[7] < other.bounds[3]);
  }
  #endif
 
  Real bounds[8];
  
  // bounds[0] : e1min
  // bounds[1] : e2min
  // bounds[2] : e3min
  // bounds[3] : e4min
  // bounds[4] : e1max
  // bounds[5] : e2max
  // bounds[6] : e3max
  // bounds[7] : e4max  
  // where ex are the tetrahedral polytope basevectors
}__attribute__ ((aligned (16)));


// A collision tree that allows for collision detection between two sets 
// of points. The OverlappingPolytopes function fills a vector of pairs with 
// first element being the colliding particle in the tree itself and second
// element the colliding particle in the other tree
// The TetrahedralPolytopeTree also provides an Overlap functionality 
// with another tree as input. It is not guaranteed, that there are any 
// overlapping particles if this function returns true. But if returns false
// you can be sure, that there are no overlapping particles...

class TetrahedralPolytopeTree{
public:

  TetrahedralPolytopeTree() { }

  TetrahedralPolytopeTree(const std::vector<Real>& x,
                          const std::vector<Real>& y,
                          const std::vector<Real>& z,
                          const std::vector<Real>& radii);

  void ResetTree(const std::vector<Real>& x,
                 const std::vector<Real>& y,
                 const std::vector<Real>& z,
                 const std::vector<Real>& radii);

  bool Overlap(const TetrahedralPolytope& other) const{
    if(num_leaf_nodes_ == 0) {
      return false;
    } else {
      return nodes_[root_node_].Overlap(other);
    }
  }

  bool Overlap(const TetrahedralPolytopeTree& other) const{
    if(num_leaf_nodes_ == 0 || other.num_leaf_nodes_ == 0) {
      return false;
    } else {
      return nodes_[root_node_].Overlap(other.nodes_[other.root_node_]);
    }
  }

  void OverlappingPolytopes(const TetrahedralPolytopeTree& other,
                            std::vector<std::pair<int,int> >& overlap) const{

    ScopedTimerPtr prof = StaticRuntimeProfiler::StartScoped(
                            "TetrahedralPolytopeTree::OverlappingPolytopes", 2);

    if(num_leaf_nodes_ == 0 || other.num_leaf_nodes_ == 0) {
      return; // no overlaps...
    }

    TraverseTree(other, root_node_, other.root_node_, overlap);
  }

private:

  inline bool IsLeaf(int idx) const { return idx < num_leaf_nodes_; }

  int Generate(const std::vector<Real>& x,
               const std::vector<Real>& y,
               const std::vector<Real>& z,
               const std::vector<int>& indices);

  void AssignPolytope(const std::vector<Real>& x,
                      const std::vector<Real>& y,
                      const std::vector<Real>& z,
                      const std::vector<int>& indices,
                      int node_idx);

  inline void TraverseTree(const TetrahedralPolytopeTree& other_tree,
                           int this_idx, int other_idx,
                           std::vector<std::pair<int,int> >& result) const{

    if(!nodes_[this_idx].Overlap(other_tree.nodes_[other_idx])) return;

    if(IsLeaf(this_idx)){
      if(other_tree.IsLeaf(other_idx)){
        result.push_back(std::make_pair(this_idx, other_idx));
        return;
      }
      TraverseTree(other_tree, this_idx, 
                   other_tree.connectivity_[other_idx].first,
                   result);

      TraverseTree(other_tree, this_idx, 
                   other_tree.connectivity_[other_idx].second,
                   result);
      return;
    }

    TraverseTree(other_tree, connectivity_[this_idx].first, other_idx,
                 result);

    TraverseTree(other_tree, connectivity_[this_idx].second, other_idx,
                 result);   
  }

  int root_node_;
  int num_leaf_nodes_;
  std::vector<TetrahedralPolytope> nodes_;
  std::vector<std::pair<int, int> > connectivity_; 

  // helper variable to keep track of recursive Generate calls
  int idx_helper_;
};

}} // ns

#endif
