// Copyright (c) 2020 Graphcore Ltd. All rights reserved.

#include <popops/Cast.hpp>
#include <popops/Encoding.hpp>
#include <popops/TopK.hpp>

#include <poputil/DebugInfo.hpp>
#include <poputil/TileMapping.hpp>
#include <poputil/Util.hpp>
#include <poputil/VertexTemplates.hpp>
#include <poputil/exceptions.hpp>

#include <poplibs_support/Algorithm.hpp>
#include <poplibs_support/VectorUtils.hpp>
#include <poplibs_support/gcd.hpp>
#include <poplibs_support/logging.hpp>
#include <poplibs_support/print.hpp>

#include <type_traits>
#include <unordered_map>

#include <boost/functional/hash.hpp>

using namespace poplar;
using namespace poplar::program;
using namespace poplibs_support;
using namespace poputil;

namespace poputil {

template <> poplar::ProfileValue toProfileValue(const popops::SortOrder &o) {
  switch (o) {
  case popops::SortOrder::NONE:
    return poplar::ProfileValue("NONE");
  case popops::SortOrder::ASCENDING:
    return poplar::ProfileValue("ASCENDING");
  case popops::SortOrder::DESCENDING:
    return poplar::ProfileValue("DESCENDING");
  default:
    return poplar::ProfileValue("<UNKNOWN>");
  }
}
} // namespace poputil

namespace popops {
namespace bitonic {

/** This returns the number of comparisons we will do at the given distance
 *  into a flat array with n elements.
 *
 *  e.g. n = 15 and distance = 4
 *
 *  +---+---+---+---+---+---+---+---+---+---+---+---+---+---+---+
 *  |   |   |   |   |   |   |   |   |   |   |   |   |   |   |   |
 *  +---+---+---+---+---+---+---+---+---+---+---+---+---+---+---+
 *    +--------------->               +--------------->
 *        +--------------->               +--------------->
 *            +--------------->               +--------------->
 *                +--------------->
 *
 *  gives the number of comparisons as 7.
 */
template <typename T>
static inline T numComparisonsAtDistance(T n, T distance) {
  const auto q = floordiv(n, distance * 2);
  const auto r = n % (distance * 2);
  const auto comparisons = q * distance + r - std::min(r, distance);
  return comparisons;
}

namespace {

struct TensorKey {
  unsigned n;
  unsigned comparisons;
  unsigned distance;
  TensorKey(unsigned n, unsigned distance, unsigned nActive)
      : n(n), comparisons(numComparisonsAtDistance(nActive, distance)),
        distance(distance) {}
  inline bool operator<(const TensorKey &other) const {
    return std::tie(n, comparisons, distance) <
           std::tie(other.n, other.comparisons, other.distance);
  }
  inline bool operator==(const TensorKey &other) const {
    return std::tie(n, comparisons, distance) ==
           std::tie(other.n, other.comparisons, other.distance);
  }
};

struct TensorKeyHash {
  std::size_t operator()(const TensorKey &k) const {
    std::size_t seed = 0;
    boost::hash_combine(seed, k.n);
    boost::hash_combine(seed, k.comparisons);
    boost::hash_combine(seed, k.distance);
    return seed;
  }
};

using TensorCache = std::unordered_map<TensorKey, Tensor, TensorKeyHash>;

} // end anonymous namespace

/** Allocates a tensor to be used when doing a compare and swap step
 *  with the given parameters. In particular this manages the correct
 *  mapping of elements to the tiles on which they will be processed
 *  and correct balancing of elements not processed across remaining
 *  tiles.
 *
 *  This is closely tied to the compareAndSwapAtDistance function.
 */
static Tensor allocate(Graph &graph, const Type &type, std::size_t n,
                       std::size_t nActive, std::size_t distance,
                       const DebugNameAndId &dnai) {
  const auto t = graph.addVariable(type, {n}, {dnai});
  const auto numTiles = graph.getTarget().getNumTiles();

  const auto comparisons = numComparisonsAtDistance(nActive, distance);
  // Allocation in each stage is done by first allocating to tiles elements
  // that are 'active' i.e. take part in a comparison at the given distance.
  const auto maxComparisonsPerTile = ceildiv(comparisons, numTiles);
  const auto activeTiles = ceildiv0(comparisons, maxComparisonsPerTile);

  // We know from the total number of elements n what the layout will be for
  // a tensor with distance equal to 1, which we use to determine how to
  // spread inactive elements over tiles.
  const auto maxActivePairs = floordiv(n, 2u);
  const auto maxAllocatedPairsPerTile = ceildiv(maxActivePairs, numTiles);
  const auto maxUsedTiles = ceildiv0(maxActivePairs, maxAllocatedPairsPerTile);

  // First allocate first comparisons * 2 elements to tiles according to
  // which will process those comparisons.
  for (unsigned tile = 0; tile < activeTiles; ++tile) {
    const auto begin = tile * maxComparisonsPerTile;
    const auto end = std::min((tile + 1) * maxComparisonsPerTile, comparisons);
    graph.setTileMapping(t.slice(begin * 2, end * 2), tile);
  }
  // Then allocate inactive elements to tiles, balancing total number of
  // elements allocated as much as possible.
  auto offset = comparisons * 2;
  auto inactiveRemaining = n - comparisons * 2;
  for (unsigned tile = 0; tile < maxUsedTiles; ++tile) {
    std::size_t activePairsThisTile = 0;
    if (tile < activeTiles) {
      const auto begin = tile * maxComparisonsPerTile;
      const auto end =
          std::min((tile + 1) * maxComparisonsPerTile, comparisons);
      activePairsThisTile = (end - begin);
    }
    const auto inactiveThisTile =
        std::min((maxAllocatedPairsPerTile - activePairsThisTile) * 2,
                 inactiveRemaining);
    graph.setTileMapping(t.slice(offset, offset + inactiveThisTile), tile);
    inactiveRemaining -= inactiveThisTile;
    offset += inactiveThisTile;
  }

  // If there is an odd element, we'll arbitrarily map it to the last tile.
  if (offset < n) {
    // We assume there is only ever 1 'odd' element
    assert(n - offset == 1);
    graph.setTileMapping(t.slice(offset, offset + 1),
                         std::max(maxUsedTiles, 1ul) - 1);
    offset += 1;
  }

  assert(offset == n);

  return t;
}

/** This gives the inverse of a permutation of elements of a tensor.
 */
static std::vector<Interval>
getInversePermutation(const std::vector<Interval> &is) {
  std::vector<Interval> matchingSlices;
  matchingSlices.reserve(is.size());
  std::size_t offset = 0;
  for (const auto &i : is) {
    matchingSlices.emplace_back(offset, offset + i.size());
    offset += i.size();
  }

  std::vector<std::size_t> sortedIndices(is.size());
  std::iota(sortedIndices.begin(), sortedIndices.end(), 0);
  std::sort(sortedIndices.begin(), sortedIndices.end(),
            [&](const auto a, const auto b) {
              return is[a].begin() < is[b].begin();
            });

  std::vector<Interval> inverse;
  inverse.reserve(is.size());
  for (const auto &i : sortedIndices) {
    inverse.push_back(matchingSlices[i]);
  }
  return inverse;
}

/** Given an offset into a number of comparisons to do and the distance
 *  at which to compare, this gives the offset in elements for the lhs
 *  element taking part on that comparison in a flat array.
 *
 *  The calculation is essentially taking the number of multiples of
 *  the distance d * 2 to get number of elements plus any remainder.
 *
 *  e.g. for distance = 4 we have a flat array and the comparisons that
 *  will be done on that array
 *
 *  +---+---+---+---+---+---+---+---+---+---+---+---+---+---+
 *  |   |   |   |   |   |   |   |   |   |   |   |   |   |   |
 *  +---+---+---+---+---+---+---+---+---+---+---+---+---+---+
 *    +--------------->               +--------------->
 *    0   +--------------->           4   +--------------->
 *        1   +--------------->           5
 *            2   +--------------->
 *                3
 *
 *  For comparison offset 3 we have no multiples of distance (4), plus
 *  the remainder 3, so the offset for the lhs element in that comparison
 *  is 3 as can be seen from the diagram. For comparison offset 5 we have
 *  1 multiple of 4, multiplied by 2 and add the remainder 1 equals 9 which
 *  again is evident from the diagram.
 */
template <typename T>
static inline T comparisonToElemOffset(T offset, T distance) {
  return roundDown(offset, distance) * 2 + offset % distance;
}

/** Given a tensor which was allocated with the given parameters, reorder
 *  it to give the canonical order. The canonical order is with all elements
 *  in the order they were given to the top-k operation.
 *
 *  We use the canonical order as a common ordering in which to transform
 *  tensors between different steps of the top-k operation.
 */
static Tensor toCanonicalOrder(const Graph &graph, const Tensor &t,
                               std::size_t distance, std::size_t nActive) {
  assert(t.rank() == 1);

  const auto n = t.dim(0);

  // Already in canonical order
  if (distance == 1 && nActive == n) {
    return t;
  }

  const auto numTiles = graph.getTarget().getNumTiles();

  const auto comparisons = numComparisonsAtDistance(nActive, distance);
  const auto maxComparisonsPerTile = ceildiv(comparisons, numTiles);
  const auto activeTiles = ceildiv0(comparisons, maxComparisonsPerTile);

  // This function just combines slices that are actually continuous.
  std::vector<Interval> slices;
  // Just reserve something, the exact number is hard to calculate up front.
  slices.reserve(activeTiles);
  const auto appendSlice = [&](const auto begin, const auto end) {
    if (begin == end) {
      return;
    }
    if (!slices.empty() && slices.back().end() == begin) {
      slices.back() = Interval(slices.back().begin(), end);
    } else {
      slices.emplace_back(begin, end);
    }
  };

  // We actually build the permutation of the canonically ordered tensor's
  // elements you would take to get the input tensor and then get the inverse
  // permutation and apply that to the tensor to retrieve the canonical form.
  for (unsigned tile = 0; tile < activeTiles; ++tile) {
    auto begin = tile * maxComparisonsPerTile;
    const auto end = std::min((tile + 1) * maxComparisonsPerTile, comparisons);

    if (const auto pre = std::min(distance - begin % distance, end - begin)) {
      const auto elemOffset = comparisonToElemOffset(begin, distance);
      appendSlice(elemOffset, elemOffset + pre);
      appendSlice(elemOffset + distance, elemOffset + distance + pre);
      begin += pre;
    }
    // Add multiples of 2 * distance chunks of elements.
    if (const auto multipleOfDistanceComparisons =
            roundDown(end - begin, distance)) {
      appendSlice(begin * 2, (begin + multipleOfDistanceComparisons) * 2);
      begin += multipleOfDistanceComparisons;
    }
    if (const auto post = end - begin) {
      const auto elemOffset = comparisonToElemOffset(begin, distance);
      appendSlice(elemOffset, elemOffset + post);
      appendSlice(elemOffset + distance, elemOffset + distance + post);
      begin += post;
    }
    assert(begin == end);
  }

  // This method is tied closely with the bitonic allocate method, any inactive
  // elements when comparing at a particular distance are always at the end of
  // the tensor so we can just tack them on.
  const auto remainingOffset = comparisonToElemOffset(comparisons, distance);
  appendSlice(remainingOffset, remainingOffset + n - comparisons * 2);

  return concat(t.slices(getInversePermutation(slices)));
}

/** Allocate a new tensor and rearrange the given input into it in preparation
 *  for a compare and swap step with the given parameters.
 *
 *  Assumes src is in canonical order.
 */
static Tensor rearrangeForStep(Graph &graph, Sequence &prog, const Tensor &src,
                               unsigned dstDistance, unsigned dstActive,
                               TensorCache &tensorCache,
                               const DebugNameAndId &dnai) {
  const auto n = src.dim(0);
  const TensorKey dstKey(n, dstDistance, dstActive);
  auto cacheIt = tensorCache.find(dstKey);
  if (cacheIt == tensorCache.end()) {
    const auto t =
        allocate(graph, src.elementType(), n, dstActive, dstDistance, dnai);
    cacheIt = tensorCache.emplace(dstKey, t).first;
  }
  const auto &dst = cacheIt->second;
  const auto &dstReordered =
      toCanonicalOrder(graph, dst, dstDistance, dstActive);
  prog.add(Copy(src, dstReordered));
  return dst;
}

/** WorklistBuilder is a one-time use class that is used per-tile to
 *  build worklists based on the available work on a tile.
 */
struct WorklistBuilder {
  const unsigned numWorkers;
  const unsigned maxComparisonsPerWorker;
  const bool initialOrder;
  const unsigned distanceToChangeOrder;
  unsigned globalComparisonOffset;

  using Worklists = std::vector<std::vector<std::size_t>>;
  Worklists lists;

  // The number of entries in the worklist for the current worker.
  unsigned numWorkerEntries = 0;
  // The number of remaining comparisons this worker can do based
  // on the maximum per worker.
  unsigned remainingWorkerComparisons = 0;
  // The offset into the data on this tile so far.
  unsigned tileOffset = 0;

  WorklistBuilder(unsigned numWorkers, unsigned maxComparisonsPerWorker,
                  bool initialOrder, unsigned distanceToChangeOrder,
                  unsigned globalComparisonOffset)
      : numWorkers(numWorkers),
        maxComparisonsPerWorker(maxComparisonsPerWorker),
        initialOrder(initialOrder),
        distanceToChangeOrder(distanceToChangeOrder),
        globalComparisonOffset(globalComparisonOffset) {
    lists.reserve(numWorkers);
  }

  // Work is limited by and assumed to be a multiple of the given distance
  // hence the parameters are the distance and a multiple, rather than a more
  // flexible total number of comparisons.
  void addWork(unsigned distance, unsigned repeats = 1) {
    const auto numComparisons = distance * repeats;
    auto remainingComparisons = numComparisons;

    unsigned comparisonOffset = 0;
    // NOTE: The encoding of the worklists is described alongside the c++
    // implementation of the vertex.
    while (remainingComparisons != 0) {
      // Start a new worker
      if (remainingWorkerComparisons == 0) {
        // Fill out the number of entries for the last worker if there
        // was one.
        if (numWorkerEntries != 0) {
          assert(!lists.empty());
          lists.back().front() = numWorkerEntries - 1;
          numWorkerEntries = 0;
        }

        lists.emplace_back();
        // Reserve a spot for number of entries in the worklist
        lists.back().emplace_back();
        const auto currWorkOffset =
            comparisonToElemOffset(comparisonOffset, distance);
        lists.back().emplace_back(tileOffset + currWorkOffset);

        // Knowing the starting order of comparison over the whole
        // input, we can work out for the slice of comparisons that
        // this worker does what order it should start comparing
        // in by using the starting offset for this worker into the
        // whole input to determine how many changes of order have come before.
        const bool workerInitialOrder =
            initialOrder ^
            ((globalComparisonOffset / distanceToChangeOrder) & 1u);
        // The worker keeps a count of comparisons left before it must
        // flip the order in which it compares and swaps. Because the
        // worker may start at any offset into the comparisons to be
        // done over the whole input, we provide a count to initialise
        // the counter with based on the offset into the whole input that
        // this worker starts its work. The worker will calculate
        // the remaining compare and swaps to do before flipping the order
        // by taking distanceToChangeOrder - initialCount.
        const auto initialCount =
            globalComparisonOffset % distanceToChangeOrder;
        assert(initialCount < (1u << 31));
        // We encode the initial direction and initial offset into
        // 32-bits in the worklist where the upper 31 bits are allocated
        // to the initial count and the lowest bit is allocated to the
        // initial order.
        const unsigned short initialLower =
            ((initialCount & ((1u << 15u) - 1)) << 1u) | workerInitialOrder;
        const unsigned short initialUpper =
            (initialCount >> 15u) & ((1u << 16) - 1);
        lists.back().emplace_back(initialLower);
        lists.back().emplace_back(initialUpper);

        remainingWorkerComparisons = maxComparisonsPerWorker;

        // Similar to initialCount, due to the fact that a worker can
        // start at any offset into the work to be done on this tile/
        // over the whole input, we provide an initial count for the
        // innermost loop that would ordinarily otherwise be set to
        // 'distance' for the first entry in the worklist.
        const auto firstInnerCount = std::min(
            distance - comparisonOffset % distance,
            std::min(remainingComparisons, remainingWorkerComparisons));
        lists.back().emplace_back(firstInnerCount);
      }

      const auto comparisonsThisEntry =
          std::min(remainingComparisons, remainingWorkerComparisons);

      lists.back().emplace_back(distance);
      lists.back().emplace_back(comparisonsThisEntry);

      comparisonOffset += comparisonsThisEntry;
      globalComparisonOffset += comparisonsThisEntry;
      remainingWorkerComparisons -= comparisonsThisEntry;
      remainingComparisons -= comparisonsThisEntry;
      ++numWorkerEntries;
    }
    tileOffset += numComparisons * 2;
  }

  // Invalidates the builder and returns the worklists that were built.
  Worklists finish() {
    if (numWorkerEntries != 0) {
      assert(!lists.empty());
      lists.back().front() = numWorkerEntries - 1;
      numWorkerEntries = 0;
    }
    return std::move(lists);
  }
};

/** The basic building block for the algorithm. This step
 *  performs a compare and swap at the given distance if the input
 *  tensors were flat array in canonical order.
 *
 *  e.g. nActive = 15, distance = 4, distanceToChangeOrder = 4
 *
 *  +---+---+---+---+---+---+---+---+---+---+---+---+---+---+---+
 *  |   |   |   |   |   |   |   |   |   |   |   |   |   |   |   |
 *  +---+---+---+---+---+---+---+---+---+---+---+---+---+---+---+
 *    +--------------->               <---------------+
 *        +--------------->               <---------------+
 *            +--------------->               <---------------+
 *                +--------------->
 *
 *  e.g. nActive = 15, distance = 2, distanceToChangeOrder = 4
 *
 *  +---+---+---+---+---+---+---+---+---+---+---+---+---+---+---+
 *  |   |   |   |   |   |   |   |   |   |   |   |   |   |   |   |
 *  +---+---+---+---+---+---+---+---+---+---+---+---+---+---+---+
 *    +------->       +------->       <-------+       <-------+
 *        +------->       +------->       <-------+
 *
 *  In these examples each arrow indicates the 2 elements are compared,
 *  and if the element the arrow points to is less than the element it
 *  starts from, the elements are swapped.
 *
 *  We add some limitations to this step in order to appease the
 *  hardware. The main one is that the distance is consistent for the
 *  entire operation, as is the distance at which we flip the order of
 *  comparison.
 *
 *  The actual implementation maps comparisons to tiles by taking the
 *  flat array, and evenly splitting the work amongst tiles. Where the
 *  lhs and rhs of the comparison reside on different tiles, we gather
 *  the comparisons to be done in chunks of powers of 2. This limits
 *  the amount of exchange code required to rearrange for each step,
 *  and limits the overhead we add would add by having the vertex deal
 *  with different distances at which to compare to a number of different
 *  distances that is roughly logarithmic with respect to the number
 *  of elements per tile (in each compare and swap step).
 */
static std::string getVertexClass(const Tensor &keys,
                                  const std::optional<Tensor> &values) {
  if (values) {
    return templateVertex("popops::CompareAndSwapAtDistanceKeyVal",
                          keys.elementType(), values->elementType());
  } else {
    return templateVertex("popops::CompareAndSwapAtDistance",
                          keys.elementType());
  }
}
static void
compareAndSwapAtDistance(Graph &graph, Sequence &prog, const Tensor &keys,
                         const std::optional<Tensor> &values, unsigned distance,
                         unsigned distanceToChangeOrder, bool initialOrder,
                         unsigned nActive, const DebugNameAndId &dnai) {
  assert(!values || keys.shape() == values->shape());

  const auto vertexClass = getVertexClass(keys, values);

  const auto &target = graph.getTarget();
  const auto numTiles = target.getNumTiles();
  const auto numWorkers = target.getNumWorkerContexts();

  const auto comparisons = numComparisonsAtDistance(nActive, distance);
  const auto maxComparisonsPerTile = ceildiv(comparisons, numTiles);
  const auto activeTiles = ceildiv0(comparisons, maxComparisonsPerTile);

  const auto cs = graph.addComputeSet({dnai, "CompareAndSwap"});
  for (unsigned tile = 0; tile < activeTiles; ++tile) {
    auto begin = tile * maxComparisonsPerTile;
    const auto end = std::min((tile + 1) * maxComparisonsPerTile, comparisons);
    const auto comparisonsThisTile = end - begin;

    const auto v = graph.addVertex(cs, vertexClass);
    graph.setTileMapping(v, tile);

    graph.connect(v["keys"], keys.slice(begin * 2, end * 2).flatten());
    if (values) {
      graph.connect(v["values"], values->slice(begin * 2, end * 2).flatten());
    }

    graph.setInitialValue(v["distanceToChangeOrder"], distanceToChangeOrder);

    // We just flatten all the comparisons to be done and evenly spread
    // them amongst workers for now without consideration of how the split
    // might introduce uneven overheads between workers.
    const auto comparisonsPerWorker = ceildiv(comparisonsThisTile, numWorkers);
    WorklistBuilder builder(numWorkers, comparisonsPerWorker, initialOrder,
                            distanceToChangeOrder, begin);

    if (const auto pre = std::min(distance - begin % distance, end - begin)) {
      builder.addWork(pre);
      begin += pre;
    }
    if (const auto multipleOfDistanceComparisons =
            floordiv(end - begin, distance)) {
      builder.addWork(distance, multipleOfDistanceComparisons);
      begin += multipleOfDistanceComparisons * distance;
    }
    if (const auto post = end - begin) {
      builder.addWork(post);
      begin += post;
    }
    assert(begin == end);

    const auto &worklists = builder.finish();

    const auto worklistsField = v["worklists"];
    graph.setFieldSize(worklistsField, worklists.size());
    for (unsigned i = 0; i < worklists.size(); ++i) {
      const auto t =
          graph.addConstant(UNSIGNED_SHORT, {worklists[i].size()},
                            worklists[i].data(), {dnai, "worklists"});
      graph.setTileMapping(t, tile);
      graph.connect(worklistsField[i], t);
    }
  }
  prog.add(Execute(cs));
}

/** Create an input for a top-k laid out in the most efficient
 *  way.
 */
Tensor createTopKInputImpl(Graph &graph, const Type &type,
                           const std::vector<std::size_t> &shape,
                           const DebugNameAndId &dnai) {
  if (shape.empty()) {
    throw poplibs_error("shape must have at least 1 dimension");
  }
  const auto numElems = product(shape);
  const auto n = shape.back();
  const auto b = numElems / n;
  auto t = allocate(graph, type, b * n, b * n, 1, dnai);
  return t.reshape({n, b}).transpose().reshape(shape);
}

/** Core implementation for all variants.
 *
 *  All the variants are implemented by application of our basic
 *  compareAndSwapAtDistance operation some number of times with the
 *  right parameters. A key limitation of compareAndSwapAtDistance is that
 *  it requires us to compare and swap at a consistent distance across the
 *  entire array, flipping the order imposed at another consistent interval.
 *
 *  ========= Sort =========
 *  I will not describe a bitonic sorting network for powers of 2 as this is
 *  well documented. For n a power of 2 the above limitation on
 *  compareAndSwapAtDistance is not a problem, as the size of partitions in
 *  each step (the distance at which we compare and swap) is always a
 *  consistent factor of 2 greater than the last step across the whole n
 *  elements to be sorted.
 *
 *  For n not a power of 2 we compose the sorting network of power-of-2
 *  sized sorting networks. e.g. if n=100, we compose the full sort with
 *  smaller sorting networks of size 64, 32, 4 (64 + 32 + 4 = 100). The
 *  result of these power of 2 sized networks are merged and re-sorted
 *  hierarchically.
 *
 *  With regards the limitation on compare and swap described above,
 *  the key insight is that when all the networks are of size a power of 2,
 *  sorting the sub-sequences into order always uses ascending powers of 2
 *  distances at which to compare and swap. Once the sub-sequences are in
 *  order merging them to form a larger non-power of 2 sized sequence is
 *  always a compare and swap at the next largest power of 2. We can therefore
 *  apply a series of compare and swap steps with increasing powers of 2
 *  distances to form our final fully sorted sequence and in each step the
 *  distance to compare and swap is always consistent across the whole
 *  array.
 *
 *  When merging the result of our power of 2 sized sorting networks,
 *  we must take care. In these cases we can do one of the following:
 *
 *   * Given an ascending/descending bitonic sequence s of size n and
 *     known inflection point p, we can compare and swap at distance
 *     p with descending order, and the result is guaranteed to be such
 *     that all in the set of elements left of p are greater than all
 *     in the set of elements right of p.
 *   * Conversely, given a descending/ascending bitonic sequence s of
 *     size n and known inflection point p, we can compare and swap at
 *     distance p with ascending order, and the result is guaranteed
 *     to be such that all in the set of elements left of p are less
 *     than all in the set of elements right of p.
 *
 *  In order to use the above this means each time we merge sorted power
 *  of 2 sequences, they must be sorted in a particular order. Different
 *  powers of 2 will take different numbers of compare and swap steps to
 *  end up in the correct order for merging. Since the smaller powers of
 *  2 must always take part in the same or fewer compare and swap operations
 *  than the larger powers of 2, we can carefully omit them from the
 *  right compare and swap operations on larger powers of 2 to ensure they
 *  still end up in the right order for merging and we get the correct
 *  result.
 *
 *  ========= Top-K =========
 *  In order to handle top-k, the general idea is to build sorted sequences
 *  of size k with alternating ascending/descending order, allowing us to
 *  merge 2 sequences of k with opposing ordering giving a result where one of
 *  the sequences has elements which are all greater than the elements in
 *  the other sequence. Each time we do this, we discard the smaller
 *  sequence and re-sort the sequences of k elements into ascending/
 *  descending order and repeat until we are left with k elements.
 *
 *  We make a small modification to this which is we actually first build
 *  sorted sequences of k' elements where k' is the next power of 2 greater
 *  then or equal to k. This again lets us keep our distance of compare and
 *  swap consistent in each step and doesn't further complicate the compare
 *  and swap operation. To get the final result if k is not a power of 2
 *  we keep sorting and discarding descreasing sized powers of 2 until we
 *  are left with just k elements.
 */
std::pair<Tensor, Tensor> topKImpl(Graph &graph, Sequence &prog,
                                   const Tensor &t_,
                                   const std::optional<Tensor> &other_,
                                   const unsigned k, const bool largest,
                                   const bool sorted, const bool ascendingOrder,
                                   const DebugNameAndId &dnai) {

  const auto inputType = t_.elementType();
  if (inputType != HALF && inputType != FLOAT) {
    throw poplibs_error(
        "Unsupported data type for top-k " + inputType.toString() +
        ". half and float are the only supported types at present.");
  }

  if (other_) {
    const auto otherType = other_->elementType();
    if (otherType != UNSIGNED_INT) {
      throw poplibs_error("Unsupported data type for other tensor in top-k " +
                          otherType.toString() +
                          ". Only unsigned int is supported at present");
    }
    if (other_->shape() != t_.shape()) {
      throw poplibs_error("t.shape() (" + toString(t_.shape()) +
                          " != other.shape() (" + toString(other_->shape()) +
                          "). Other tensor in topKKeyValue must have matching "
                          "shape to tensor in which to find top-k.");
    }
  }

  if (t_.rank() == 0) {
    throw poplibs_error("t must have at least one dimension");
  }

  const std::vector<std::size_t> outputShape = [&] {
    auto s = t_.shape();
    s.back() = k;
    return s;
  }();

  // We use {n, batch} as our canonical internal representation for
  // the shape.
  auto t =
      t_.rank() >= 2 ? t_.flatten(0, t_.rank() - 1) : t_.flatten().expand({0});
  t = t.transpose();
  std::optional<Tensor> other;
  if (other_) {
    other = other_->rank() >= 2 ? other_->flatten(0, other_->rank() - 1)
                                : other_->flatten().expand({0});
    other = other->transpose();
  }

  unsigned n = t.dim(0);
  const unsigned b = t.dim(1);

  logging::popops::debug("bitonicTopK(batchSize={}, n={}, k={}, sorted={}, "
                         "haveOther={}, debugPath='{}')",
                         b, n, k, sorted, (other ? "true" : "false"),
                         dnai.getPathName());

  if (k > n) {
    throw poplibs_error(
        "k (" + std::to_string(k) +
        ") must be less than or equal to the number of input elements (" +
        std::to_string(n) + ")");
  }

  const auto logK = ceilLog2(k);
  const auto logN = ceilLog2(n);
  // We define k' as the next power of 2 greater or equal than k.
  const auto kDash = (1u << logK);
  const auto stepsToSortK = nthTriangular(logK);
  const auto totalSteps = nthTriangular(logN);

  logging::popops::debug(
      "bitonicTopKImpl: Calculated no. of steps: total={}, k={}", totalSteps,
      stepsToSortK);

  // Edge cases where the output is zero-sized.
  if (b * k == 0) {
    Tensor tResult =
        graph.addVariable(t.elementType(), outputShape, {dnai, "keys"});
    Tensor otherResult;
    if (other) {
      otherResult = graph.addVariable(other->elementType(), outputShape,
                                      {dnai, "values"});
    }
    return std::make_pair(std::move(tResult), std::move(otherResult));
  }

  t = t.flatten();
  if (other) {
    other = other->flatten();
  }

  // Handle some trivial cases where this is a no-op.
  //
  //  * We already have the top/bottom k elements and the result doesn't
  //    need to sorted.
  //  * n is 1, the output is sorted by default.
  if ((k == n && !sorted) || n == 1) {
    // We assume the result does not alias the input and is writeable so
    // ensure that is the case.
    t = poputil::duplicate(graph, t, prog, {dnai, "keys"});
    t = t.reshape({k, b}).transpose().reshape(outputShape);
    if (other) {
      other = poputil::duplicate(graph, *other, prog, {dnai, "values"});
      other = other->reshape({k, b}).transpose().reshape(outputShape);
    }
    return std::make_pair(std::move(t), other.value_or(Tensor{}));
  }

  // We use the float path to implement half data type for now.
  const auto dataType = FLOAT;
  if (inputType != dataType) {
    t = cast(graph, t, dataType, prog, dnai);
  }

  // Because we always discard the upper of a pair of sequences of k'
  // elements, the merge order when merging sequences is based off
  // leaving the top/bottom k' elements in each pair of k' sequences
  // in the lower k' elements.
  const bool mergeKSequencesOrder = !largest;
  const bool oddKSequences = bool((n / kDash) & 1u);

  TensorCache tCache, otherCache;
  for (unsigned mergeStep = 0; mergeStep < logN; ++mergeStep) {
    const auto logMergeDistance = std::min(mergeStep, logK);
    const auto mergeDistance = 1u << logMergeDistance;
    const auto stepName = "Merge" + std::to_string(mergeStep);

    const bool mergeOrder = [&] {
      bool order;
      if (mergeStep >= logK) {
        // Use the merge order for merging sequences of k' elements if that
        // is what we are doing.
        order = mergeKSequencesOrder;
      } else {
        // If this is the last step, we need to sort in the desired order.
        if (mergeStep == logN - 1) {
          order = ascendingOrder;
          // If this is the last step of sorting k' sequences, make sure they
          // are sorted in the order needed for merging sequences of k'
          // elements.
        } else if (mergeStep + 1 == logK) {
          order = !mergeKSequencesOrder;
        } else {
          // Otherwise, we merge based on creating sorted sequences in the
          // order needed either to create the final sorted sequence or to
          // merge k' sequences correctly.
          order = logK != logN ? (mergeKSequencesOrder ^ oddKSequences)
                               : !ascendingOrder;
        }
      }
      return order;
    }();

    // As mentioned at the signature of this function, we must sometimes
    // omit the parts of n composed of smaller powers of 2 in order to
    // result in the correctly ordered sequence once merged with another
    // power of 2. This check was sort of experimentally worked out. In
    // the interests of time, I'm leaving it as it works but this should
    // be justified and there may be a simpler calculation.
    const auto nMod2d = n % (mergeDistance * 2);
    const auto nMod4d = n % (mergeDistance * 4);
    auto nThisStep = n;
    if (mergeStep + 1 < logK && (nMod2d == 0 || nMod2d == nMod4d)) {
      nThisStep -= nMod4d;
    }

    t = rearrangeForStep(graph, prog, t, mergeDistance * b, nThisStep * b,
                         tCache, {dnai, "keys" + stepName});
    if (other) {
      other = rearrangeForStep(graph, prog, *other, mergeDistance * b,
                               nThisStep * b, otherCache,
                               {dnai, "values" + stepName});
    }

    // While we are building sorted sequences of k' elements, the distance
    // at which we change direction is the same as the merge distance,
    // otherwise the direction should always be descending as we always
    // discard the higher k' elements.
    const auto changeDirDistance =
        mergeStep < logK ? mergeDistance : 1u << (logN - 1);
    compareAndSwapAtDistance(graph, prog, t, other, mergeDistance * b,
                             changeDirDistance * b, mergeOrder, nThisStep * b,
                             {dnai, stepName});

    t = toCanonicalOrder(graph, t, mergeDistance * b, nThisStep * b);
    if (other) {
      other = toCanonicalOrder(graph, *other, mergeDistance * b, nThisStep * b);
    }

    // If we're done building sequences of k' elements, and we still have
    // more than k' elements, discard the upper k' elements of each
    // pair of k' sized sequences as by this point we have imposed an
    // ordering between them.
    if (logMergeDistance >= logK && n > kDash) {
      const auto kDashMultiples = floordiv(n, 2 * kDash);
      const auto remainder = n - kDashMultiples * 2 * kDash;
      const auto t2d = t.reshape({n, b});
      const auto evenPart =
          t2d.slice(0, kDashMultiples * 2 * kDash)
              .reshapePartial(0, 1, {kDashMultiples, 2 * kDash});
      const auto oddPart = t2d.slice(kDashMultiples * 2 * kDash, n);
      t = concat(evenPart.slice(0, kDash, 1).flatten(0, 2),
                 oddPart.slice(0, std::min(remainder, kDash)))
              .flatten();
      if (other) {
        const auto t2d = other->reshape({n, b});
        const auto evenPart =
            t2d.slice(0, kDashMultiples * 2 * kDash)
                .reshapePartial(0, 1, {kDashMultiples, 2 * kDash});
        const auto oddPart = t2d.slice(kDashMultiples * 2 * kDash, n);
        other = concat(evenPart.slice(0, kDash, 1).flatten(0, 2),
                       oddPart.slice(0, std::min(remainder, kDash)))
                    .flatten();
      }
      n = nThisStep = kDashMultiples * kDash + std::min(remainder, kDash);
    }

    for (unsigned sortStep = 0; sortStep < logMergeDistance; ++sortStep) {
      const auto stepName =
          "Sort" + std::to_string(logMergeDistance - sortStep - 1);

      const auto sortDistance = 1u << (logMergeDistance - sortStep - 1);
      // Once we have built sorted sequences of k' elements, we keep
      // merging and re-sorting so the distance at which we change
      // directions once we start discarding is always k'/2.
      const auto changeDirDistance =
          std::min(mergeDistance, (1u << (logK - 1)));

      const bool sortOrder = [&] {
        bool order;
        if (mergeStep < logK) {
          // If we are still building sorted sequences of k' elements, the
          // sort order is the same as the merge order
          order = mergeOrder;
        } else {
          if (mergeStep == logN - 1) {
            // If this is the last step then sort into the final desired
            // order.
            order = ascendingOrder;
          } else {
            // Otherwise sort into the order needed to correctly merge
            // sequences of k' elements.
            order = !mergeKSequencesOrder;
          }
        }
        return order;
      }();

      t = rearrangeForStep(graph, prog, t, sortDistance * b, nThisStep * b,
                           tCache, {dnai, "keys" + stepName});
      if (other) {
        other = rearrangeForStep(graph, prog, *other, sortDistance * b,
                                 nThisStep * b, otherCache,
                                 {dnai, "values" + stepName});
      }

      compareAndSwapAtDistance(graph, prog, t, other, sortDistance * b,
                               changeDirDistance * b, sortOrder, nThisStep * b,
                               {dnai, stepName});
      t = toCanonicalOrder(graph, t, sortDistance * b, nThisStep * b);
      if (other) {
        other =
            toCanonicalOrder(graph, *other, sortDistance * b, nThisStep * b);
      }

      // If we have finished building sequences of k' elements and
      // there are more than k elements remaining, we can discard descending
      // powers of 2 during the re-sort until we are left with exactly k
      // elements.
      const auto lastKDashElements = ((n - 1) % kDash) + 1;
      // We either discard elements from the start or end of the last k'
      // elements depending on whether we want the top or bottom k elements
      // and depending on the sort order and offset into the array when it
      // was sorted.
      const bool fromStart =
          sortOrder ^ !largest ^ bool(((n - lastKDashElements) / kDash) & 1u);
      const auto nToDiscard =
          fromStart ? sortDistance
                    : ((lastKDashElements - 1) % sortDistance) + 1;
      if (logMergeDistance + 1 >= logK && lastKDashElements >= nToDiscard &&
          lastKDashElements - nToDiscard >= k) {
        std::vector<Interval> slices;
        if (fromStart) {
          slices.emplace_back(0, n - lastKDashElements);
          slices.emplace_back(n - lastKDashElements + nToDiscard, n);
        } else {
          slices.emplace_back(0, n - nToDiscard);
        }
        const auto t2d = t.reshape({n, b});
        t = concat(t2d.slices(slices)).flatten();
        if (other) {
          const auto t2d = other->reshape({n, b});
          other = concat(t2d.slices(slices)).flatten();
        }
        n -= nToDiscard;
        nThisStep = n;
      }
    }
  }

  if (inputType != dataType) {
    t = cast(graph, t, inputType, prog, dnai);
  }

  t = t.reshape({k, b}).transpose().reshape(outputShape);
  if (other) {
    other = other->reshape({k, b}).transpose().reshape(outputShape);
  }
  return std::make_pair(std::move(t), other.value_or(Tensor{}));
}

} // end namespace bitonic
} // end namespace popops


#ifndef SPS_SPS_HPP
#define SPS_SPS_HPP

#include <Eigen/Core>
#include <memory>
#include <sps/plane.hpp>
#include <sps/strategy.hpp>
#include <vector>

namespace sequential_line_search
{
    class PreferenceRegressor;
    class PreferenceDataManager;
} // namespace sequential_line_search

namespace sps
{
    class Optimizer
    {
    public:
        Optimizer(
            const unsigned                     num_dims,
            const bool                         use_map_hyperparams = true,
            std::shared_ptr<PlaneInitStrategy> init_strategy = std::make_shared<FixedCenterFreeCrossVecsInitStrategy>(),
            std::shared_ptr<PlaneConstructionStrategy> construction_strategy =
                std::make_shared<FirstEiThenPlaneIntegralStrategy>(),
            const PlaneCenterSelectionStrategy center_strategy = PlaneCenterSelectionStrategy::LastSelection);

        void SubmitData(const Eigen::VectorXd& x_preferred, const std::vector<Eigen::VectorXd>& x_others);
        std::shared_ptr<const AbstractPlane> RetrieveSearchPlane() const;

        double GetCurrentPredictiveMean(const Eigen::VectorXd& params) const;
        double GetCurrentPredictiveVar(const Eigen::VectorXd& params) const;
        double GetCurrentExpectedImprovement(const Eigen::VectorXd& params) const;

    private:
        const bool                                       m_use_map_hyperparams;
        const std::shared_ptr<PlaneInitStrategy>         m_init_strategy;
        const std::shared_ptr<PlaneConstructionStrategy> m_construction_strategy;
        const PlaneCenterSelectionStrategy               m_center_strategy;

        const unsigned m_num_dims;

        std::shared_ptr<AbstractPlane> m_plane;

        std::shared_ptr<sequential_line_search::PreferenceRegressor>   m_regressor;
        std::shared_ptr<sequential_line_search::PreferenceDataManager> m_data;
    };
} // namespace sps

#endif // SPS_SPS_HPP


//==================================================================================================
/*!
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/pack.hpp>
#include <boost/simd/memory/aligned_object.hpp>
#include <boost/simd/memory/allocator.hpp>
#include <iostream>
#include <vector>

struct foo
{
  BOOST_SIMD_ALIGNED_OBJECT(foo);

  foo() = default;
  foo(boost::simd::pack<float> const& a) : p(a) {}

  boost::simd::pack<char,16> q;
  boost::simd::pack<float> p;
};

int main()
{
  std::vector<foo, boost::simd::allocator<foo>> p(5, foo{ boost::simd::pack<float>{3.f} });

  for(auto const& e : p)
    std::cout << (void*)(&e) << " " << e.p << "\n";
}


#include "program.h"
#include "quad_bcs.h"
#include "global_variables.h"
#include "preprocessor.h"
#include <ctime>
#include "quad_bcs_plus.h"
#include "domain_geometry.h"
#include "Mesh.h"
#include "unstructured_mesh.h"
#include "Boundary_Conditions.h"
#include "external_forces.h"
#include "Solution.h"
#include "Solver.h"
#include <fstream>
#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include "TECIO.h"
#include <sstream>
#include "tecplot_output.h"
#include <boost/filesystem.hpp>
#include "post_processing.h"
#include "unstructured_bcs.h"
#include "Solver_gpu.h"
#include "lagrangian_object.h"

namespace fs = boost::filesystem;

program::program()
{
    //ctor
}

program::~program()
{
    //dtor
}

void program::run(char* xml_input){

    quad_bcs_plus bcs;
    unstructured_bcs u_bcs;
    global_variables globals;
    domain_geometry domain;
    initial_conditions initial_conds;
	gpu_solver gpu;
     Solver solve;
    int mg =0; // first multigrid cycle
    int fmg =0; // first full multigrid cycle
    std::clock_t start = clock();
    double duration;
    duration = 0;
	std::vector< lagrangian_object> lagrangian_object_vec;

    preprocessor pre_processor;

    //postprocessor post_processor;
	std::cout << "Initialising Globals" << endl;

    pre_processor.initialise_program_variables(xml_input, globals, domain,initial_conds,bcs,u_bcs, lagrangian_object_vec);
	tecplot_output tp;

    copyfile(xml_input,globals.output_file);

    remove_existing_files(globals);

	std::cout << "Lagrangian Object Initialisation" << endl;
	lagrangian_object_vec[0].initialise(globals.PI);
	if (lagrangian_object_vec[0].type == 2) {
		tp.spring_network_output(globals.output_file, false, lagrangian_object_vec[0]);
	}else {
		tp.tecplot_output_lagrangian_object(lagrangian_object_vec[0], globals, domain, 0.0);
	}

    Solution soln;
    Solution residual;
	std::cout << "Importing Mesh" << endl;
     if(globals.mesh_type > 2){
        unstructured_mesh uns_mesh(domain,globals);
		std::cout << "Set Boundary Conditions" << endl;
        Boundary_Conditions bc(uns_mesh.get_num_bc());
        // assign external force terms
		std::cout << "Set External Forces" << endl;
        external_forces source_term(uns_mesh.get_total_cells());
        source_term.set_uniform_force(initial_conds.pressure_gradient); //pressure force applied through external force

		std::cout << "Set Initial Solution" << endl;
        //create solution
        soln.assign_memory(uns_mesh.get_total_cells());
        residual.assign_memory(uns_mesh.get_total_cells());
		std::cout << "Set Post Processor" << endl;
        post_processing post_processor(uns_mesh.get_total_cells());

		std::cout << "Initialising Solution" << endl;
        if(globals.restart_analysis == "true"){
            soln.import(globals);
			//lagrangian_object_vec[0].solution_read(globals);

        }else{
                soln.assign_pressure_gradient(initial_conds.rho_gradient,initial_conds.origin_loc,
                                    initial_conds.rho_origin_mag,uns_mesh,globals);
                soln.assign_velocity_gradient(initial_conds.vel_gradient,initial_conds.origin_loc,
                                        initial_conds.vel_origin_mag,uns_mesh,globals);
                soln.set_average_rho(initial_conds.average_rho);

        }

        bc.assign_boundary_conditions(uns_mesh,u_bcs);
		std::cout << "Running Solver" << endl;
		if (globals.gpu_solver == 1) {
			gpu.General_Purpose_Solver_mk_i(uns_mesh, soln, bc, source_term, globals, domain, initial_conds, u_bcs,
				mg, residual, fmg, post_processor, lagrangian_object_vec);

		}else {
			solve.General_Purpose_Solver_mk_i(uns_mesh,soln,bc,source_term,globals,domain,initial_conds,u_bcs,
										  mg,residual,fmg,post_processor);

		}

		//output object solution for restart
		if (lagrangian_object_vec[0].type == 2) {
			tp.spring_network_output(globals.output_file, false, lagrangian_object_vec[0]);
		}
		else {
			tp.tecplot_output_lagrangian_object(lagrangian_object_vec[0], globals, domain, 0.0);
		}

        soln.output(globals.output_file, globals, domain);
    //soln.output_centrelines(globals.output_file,globals,mesh);
    //post_processor.output_vtk_mesh(globals.output_file,globals,domain);

        std::clock_t end = clock();

        duration = double(end-start)/ CLOCKS_PER_SEC;

        output_globals(globals,duration,post_processor);

        std::cout << duration << std::endl;
        std::cout << "CPU Cycles" << double(end-start) << std::endl;

     }

}

void program::remove_existing_files(global_variables &globals){

    std::string output_location , temp;

    output_location = globals.output_file;

    temp = output_location + "/uy";
    remove_directory_files(temp);

    temp = output_location + "/vx";
     remove_directory_files(temp);

    temp = output_location + "/plt";
    remove_directory_files(temp);

    temp = output_location + "/plt/grid";
    remove_directory_files(temp);

	temp = output_location + "/plt/object";
	remove_directory_files(temp);

    }

void program::remove_directory_files(std::string output_location){

    fs::path p(output_location);

    std::string plt,dat,szplt;
    plt = ".plt";
    dat = ".dat";
    szplt = ".szplt";

    if(fs::exists(p) && fs::is_directory(p)){
        fs::directory_iterator end;

            for (fs::directory_iterator it(p); it !=end; ++it){
                try{
                    if( fs::is_regular_file(it->status()) && (it->path().extension().compare(plt) == 0 ||
                    it->path().extension().compare(dat) == 0 ||
                    it->path().extension().compare(szplt) == 0))
                    {
                        fs:remove(it->path());
                    }
                }catch(const std::exception &ex){
                    ex;
                }
            }
        }

    }

void program::output_globals (global_variables globals,double duration,post_processing &pp){

    std::string output_location;
    std::string filename;
    std::ofstream globals_txt ;
    std::string globals_file;
    output_location = globals.output_file;
    filename = globals.simulation_name;
    globals_file = output_location + "/globals.txt";
    double cycles;

    cycles = duration * CLOCKS_PER_SEC;
/// Generic Load Case Input

    globals_txt.open(globals_file.c_str(), ios::out);

    globals_txt << "Runtime:" << duration << "s" << endl;
    globals_txt << "CPU Cycles:" << cycles << endl;
    globals_txt << "File:" << filename.c_str()  << endl;

    globals_txt << "Tau:"  << globals.tau << endl;
    globals_txt << "Mach:" << globals.max_velocity *sqrt(3)<< endl;
    globals_txt << "Reynolds: " << globals.reynolds_number << endl;
    globals_txt << "Drag Coefficient: " << pp.drag_coef << endl;
    globals_txt << "Lift Coefficient: " << pp.lift_coef <<  endl;

    std::string reynolds_text;
    reynolds_text = globals.reynolds_number;

    globals_txt << "Separation Angles:" << endl;
    for(int i =0; i < pp.separation_angle.size(); i++){

        globals_txt << "grad magnitude: " << pp.separation_grad[i] << " separation angle: " <<
                pp.separation_angle[i] << " ," << pp.separation_i[i] << " ," << pp.separation_j[i]
                << "," << pp.separation_x[i] << "," << pp.separation_y[i] << endl;

    }

    globals_txt.close();

}

    // copy in binary mode
void program::copyfile( char* SRC,  std::string  DEST)
{
    DEST.append("/input.xml");

    std::ifstream src(SRC, std::ios::binary);
    std::ofstream dest(DEST, std::ios::binary);
    dest << src.rdbuf();

}


#include "Delaunay3D.hpp"
#include "Predicates3D.hpp"
#include <limits>
#include <algorithm>
#include <fstream>
#include "HilbertOrder3D.hpp"
#include <boost/foreach.hpp>
#include <iostream>
//#define runcheks 1

namespace
{
	bool InsideBigTetra(Vector3D const& tocheck, vector<Vector3D> const& cor, size_t Norg)
	{
		int sum = 0;
#ifdef __INTEL_COMPILER
#pragma omp simd reduction(+:sum)
#endif
		for (size_t i = 0; i < 4; ++i)
		{
			Vector3D normal = CrossProduct(cor[Norg + (1 + i) % 4] - cor[Norg + i], cor[Norg + (2 + i) % 4] - cor[Norg + i]);
			double s1 = ScalarProd(normal, cor[Norg + (3 + i) % 4] - cor[Norg + i]);
			double s2 = ScalarProd(normal, tocheck - cor[Norg + i]);
			sum += (s1*s2 < 0) ? 1 : 0;
		}
		return (sum==0);
	}

	bool PlaneLineIntersection(std::array<Vector3D, 3> &plane, Vector3D const& A, Vector3D const& B, Vector3D &res)
	{
		plane[1] -= plane[0];
		plane[2] -= plane[0];
		Vector3D N = CrossProduct(plane[1], plane[2]);
		double Nsize = ScalarProd(N, N);
		double Rmin = std::min(ScalarProd(plane[1], plane[1]),ScalarProd(plane[2], plane[2]));
		plane[1] -= plane[2];
		Rmin = std::min(Rmin,ScalarProd(plane[1], plane[1]));
		if (Nsize > (Rmin*Rmin * 1e4))
			return false;
		plane[1] += plane[2];
		plane[1] += plane[0];
		plane[2] += plane[0];
		Vector3D mu = B;
		mu -= A;
		double sp = ScalarProd(mu, N);
		if (sp*sp < ScalarProd(mu,mu)*Nsize*1e-8)
			return false;
		double m = ScalarProd(N, plane[0] - A) / sp;
		res = A + m*mu;
		return true;
	}

	void GetOppositePoint(Tetrahedron const& tetra, std::size_t neighbor,size_t &res)
	{
#ifdef __INTEL_COMPILER
#pragma ivdep
#endif
		for (size_t i=0; i < 4; i++)
			if (tetra.neighbors[i] == neighbor)
			{
				res = i;
				return;
			}
	}

	void GetPointLocationInTetra(Tetrahedron const& tetra, std::size_t point,size_t &res)
	{
#ifdef __INTEL_COMPILER
#pragma ivdep
#endif
		for (size_t i = 0; i < 4; i++)
			if (tetra.points[i] == point)
			{
				res = i;
				return;
			}
	}

	std::pair<std::size_t,double> InTriangle(std::array<Vector3D, 3> &triangle, Vector3D &p)
	{
		// returns the smallest area of the cross product in units of the triangle area, negative if outside
		Vector3D temp,d;
		triangle[1] -= triangle[0];
		triangle[2] -= triangle[0];
		CrossProduct(triangle[1], triangle[2], d);
		p -= triangle[0];
		CrossProduct(triangle[1], p, temp);
		double ad = ScalarProd(d, temp);
		p -= triangle[1];
		triangle[2] -= triangle[1];
		CrossProduct(triangle[2], p, temp);
		double bd = ScalarProd(d, temp);
		p -= triangle[2];
		triangle[2] += triangle[1];
		CrossProduct(p, triangle[2], temp);
		double cd = ScalarProd(d, temp);

		std::pair<std::size_t, double> res(0, 0);
		if (ad < 0)
			++res.first;
		if (bd < 0)
			++res.first;
		if (cd < 0)
			++res.first;
		res.second = std::min(std::min(std::abs(ad), std::abs(bd)), std::abs(cd)) / (ScalarProd(d, d));
		return res;
	}

	bool Are44(Tetrahedron const& T0, Tetrahedron const& T1, std::size_t loc_in_0, vector<Tetrahedron> const& tetras,
		std::size_t &N3,std::size_t &N4)
	{
		N3 = T0.neighbors[loc_in_0];
		size_t loc1=0;
		GetPointLocationInTetra(T1, T0.points[loc_in_0], loc1);
		N4 = T1.neighbors[loc1];
		for (std::size_t i = 0; i < 4; ++i)
			if (tetras[N3].neighbors[i] == N4)
				return true;
		return false;
	}

}

Delaunay3D& Delaunay3D::operator=(Delaunay3D const& other)
{
	if (this == &other)
		return *this;
	tetras_ = other.tetras_;
	points_ = other.points_;
	empty_tetras_ = other.empty_tetras_;
	Norg_ = other.Norg_;
	outside_neighbor_ = other.outside_neighbor_;
	return *this;
}

Delaunay3D::Delaunay3D(Delaunay3D const& other) :  tetras_(other.tetras_),points_(other.points_),empty_tetras_(other.empty_tetras_),Norg_(other.Norg_),
	outside_neighbor_(other.outside_neighbor_),b3_temp_(std::array<Vector3D, 3> ()),b3_temp2_(std::array<Vector3D, 3> ()),
	b4_temp_(std::array<Vector3D, 4>()), b5_temp_(std::array<Vector3D, 5>()),b4s_temp_(std::array<std::size_t, 4> ()),
	b4s_temp2_(std::array<std::size_t, 4> ()),b8s_temp_(std::array<std::size_t, 8> ()),to_check_(vector<std::size_t>()),
	last_checked_(0), tet_temp0_(Tetrahedron()), tet_temp1_(Tetrahedron()), newtet_(Tetrahedron()) {}

void Delaunay3D::flip23(std::size_t tetra0, std::size_t tetra1, std::size_t location0,bool flat_check)
{
	bool used_empty = false;
	std::size_t Nloc = tetras_.size();
	if (!empty_tetras_.empty())
	{
		used_empty = true;
		Nloc = *empty_tetras_.begin();
		empty_tetras_.erase(empty_tetras_.begin());
	}

	// location is the location of the point in tetra that is not in the joint triangle
	tet_temp0_ = tetras_[tetra0];
	tet_temp1_ = tetras_[tetra1];
	size_t location1=0;
	GetOppositePoint(tet_temp1_, tetra0, location1);

	newtet_.points[1] = tet_temp0_.points[(location0 + 1) % 4];
	newtet_.points[2] = tet_temp0_.points[(location0 + 2) % 4];

	if (location0 % 2 == 1)
	{
		newtet_.points[0] = tet_temp1_.points[location1];
		newtet_.points[3] = tet_temp0_.points[location0];
		size_t loctemp=0;
		GetPointLocationInTetra(tet_temp1_, tet_temp0_.points[(location0 + 3) % 4], loctemp);
		newtet_.neighbors[3] = tet_temp1_.neighbors[loctemp];
		newtet_.neighbors[0] = tet_temp0_.neighbors[(location0 + 3) % 4];
		if (newtet_.neighbors[0] != outside_neighbor_)
		{
			GetOppositePoint(tetras_[newtet_.neighbors[0]], tetra0, loctemp);
			tetras_[newtet_.neighbors[0]].neighbors[loctemp] = Nloc;
		}
		if (newtet_.neighbors[3] != outside_neighbor_)
		{
			GetOppositePoint(tetras_[newtet_.neighbors[3]], tetra1, loctemp);
			tetras_[newtet_.neighbors[3]].neighbors[loctemp] = Nloc;
		}
	}
	else
	{
		newtet_.points[3] = tet_temp1_.points[location1];
		newtet_.points[0] = tet_temp0_.points[location0];
		size_t loctemp=0;
		GetPointLocationInTetra(tet_temp1_, tet_temp0_.points[(location0 + 3) % 4], loctemp);
		newtet_.neighbors[0] = tet_temp1_.neighbors[loctemp];
		newtet_.neighbors[3] = tet_temp0_.neighbors[(location0 + 3) % 4];
		if (newtet_.neighbors[0] != outside_neighbor_)
		{
			GetOppositePoint(tetras_[newtet_.neighbors[0]], tetra1, loctemp);
			tetras_[newtet_.neighbors[0]].neighbors[loctemp] = Nloc;
		}
		if (newtet_.neighbors[3] != outside_neighbor_)
		{
			GetOppositePoint(tetras_[newtet_.neighbors[3]], tetra0, loctemp);
			tetras_[newtet_.neighbors[3]].neighbors[loctemp] = Nloc;
		}
	}
	newtet_.neighbors[1] = tetra0;
	newtet_.neighbors[2] = tetra1;

	if (used_empty)
		tetras_[Nloc] = newtet_;
	else
		tetras_.push_back(newtet_);

	if (location0 % 2 == 0)
	{
		tetras_[tetra0].points[0] = tet_temp0_.points[location0];
		tetras_[tetra0].points[3] = tet_temp1_.points[location1];
		size_t loctemp=0;
		GetPointLocationInTetra(tet_temp1_, tet_temp0_.points[(location0 + 1) % 4], loctemp);
		tetras_[tetra0].neighbors[0] = tet_temp1_.neighbors[loctemp];
		tetras_[tetra0].neighbors[3] = tet_temp0_.neighbors[(location0 + 1) % 4];
		if (tetras_[tetra0].neighbors[0] != outside_neighbor_)
		{
			GetOppositePoint(tetras_[tetras_[tetra0].neighbors[0]], tetra1, loctemp);
			tetras_[tetras_[tetra0].neighbors[0]].neighbors[loctemp] = tetra0;
		}
	}
	else
	{
		tetras_[tetra0].points[3] = tet_temp0_.points[location0];
		tetras_[tetra0].points[0] = tet_temp1_.points[location1];
		tetras_[tetra0].neighbors[0] = tet_temp0_.neighbors[(location0 + 1) % 4];
		size_t loctemp=0;
		GetPointLocationInTetra(tet_temp1_, tet_temp0_.points[(location0 + 1) % 4], loctemp);
		tetras_[tetra0].neighbors[3] = tet_temp1_.neighbors[loctemp];
		if (tetras_[tetra0].neighbors[3] != outside_neighbor_)
		{
			GetOppositePoint(tetras_[tetras_[tetra0].neighbors[3]], tetra1, loctemp);
			tetras_[tetras_[tetra0].neighbors[3]].neighbors[loctemp] = tetra0;
		}
	}
	tetras_[tetra0].points[1] = tet_temp0_.points[(location0 + 2) % 4];
	tetras_[tetra0].points[2] = tet_temp0_.points[(location0 + 3) % 4];
	tetras_[tetra0].neighbors[1] = tetra1;
	tetras_[tetra0].neighbors[2] = Nloc;

	if (location0 % 2 == 0)
	{
		tetras_[tetra1].points[0] = tet_temp0_.points[location0];
		tetras_[tetra1].points[3] = tet_temp1_.points[location1];
		size_t loctemp=0;
		GetPointLocationInTetra(tet_temp1_, tet_temp0_.points[(location0 + 2) % 4], loctemp);
		tetras_[tetra1].neighbors[0] = tet_temp1_.neighbors[loctemp];
		tetras_[tetra1].neighbors[3] = tet_temp0_.neighbors[(location0 + 2) % 4];
		if (tetras_[tetra1].neighbors[3] != outside_neighbor_)
		{
			GetOppositePoint(tetras_[tetras_[tetra1].neighbors[3]], tetra0, loctemp);
			tetras_[tetras_[tetra1].neighbors[3]].neighbors[loctemp] = tetra1;
		}
	}
	else
	{
		tetras_[tetra1].points[0] = tet_temp1_.points[location1];
		tetras_[tetra1].points[3] = tet_temp0_.points[location0];
		size_t loctemp=0;
		GetPointLocationInTetra(tet_temp1_, tet_temp0_.points[(location0 + 2) % 4], loctemp);
		tetras_[tetra1].neighbors[0] = tet_temp0_.neighbors[(location0 + 2) % 4];
		tetras_[tetra1].neighbors[3] = tet_temp1_.neighbors[loctemp];
		if (tetras_[tetra1].neighbors[0] != outside_neighbor_)
		{
			GetOppositePoint(tetras_[tetras_[tetra1].neighbors[0]], tetra0, loctemp);
			tetras_[tetras_[tetra1].neighbors[0]].neighbors[loctemp] = tetra1;
		}
	}
	tetras_[tetra1].points[1] = tet_temp0_.points[(location0 + 3) % 4];
	tetras_[tetra1].points[2] = tet_temp0_.points[(location0 + 1) % 4];
	tetras_[tetra1].neighbors[1] = Nloc;
	tetras_[tetra1].neighbors[2] = tetra0;

	if(flat_check)
	{
		for (size_t i = 0; i < 4; ++i)
			b4_temp_[i] = points_[tetras_[Nloc].points[i]];
		double orient = orient3d(b4_temp_);
		if (std::abs(orient) == 0)
		{
			std::size_t my_loc=0;
			GetPointLocationInTetra(tetras_[Nloc], tet_temp0_.points[location0],my_loc);
			std::size_t neigh = tetras_[Nloc].neighbors[my_loc];
			std::size_t opp=0;
			GetOppositePoint(tetras_[neigh], Nloc,opp);
			for (size_t i = 0; i < 3; ++i)
				b4_temp_[i] = points_[tetras_[Nloc].points[(my_loc + (i) + 1) % 4]];
			b4_temp_[3] = points_[tetras_[neigh].points[opp]];
			double o = orient3d(b4_temp_);
			o *= 1.0 - static_cast<double>(2 * (location0 % 2));
			if (o > 0)
			{
				std::size_t temp = tetras_[Nloc].points[0];
				tetras_[Nloc].points[0] = tetras_[Nloc].points[1];
				tetras_[Nloc].points[1] = temp;
				temp = tetras_[Nloc].neighbors[0];
				tetras_[Nloc].neighbors[0] = tetras_[Nloc].neighbors[1];
				tetras_[Nloc].neighbors[1] = temp;
			}
		}

		for (size_t i = 0; i < 4; ++i)
			b4_temp_[i] = points_[tetras_[tetra0].points[i]];
		orient = orient3d(b4_temp_);
		if (std::abs(orient) == 0)
		{
			std::size_t my_loc=0;
			GetPointLocationInTetra(tetras_[tetra0], tet_temp0_.points[location0],my_loc);
			std::size_t neigh = tetras_[tetra0].neighbors[my_loc];
			std::size_t opp=0;
			GetOppositePoint(tetras_[neigh], tetra0,opp);
			for (size_t i = 0; i < 3; ++i)
				b4_temp_[i] = points_[tetras_[tetra0].points[(my_loc + i + 1) % 4]];
			b4_temp_[3] = points_[tetras_[neigh].points[opp]];
			double o = orient3d(b4_temp_);
			o *= 1.0 - static_cast<double>(2 * (location0 % 2));
			if (o > 0)
			{
				std::size_t temp = tetras_[tetra0].points[0];
				tetras_[tetra0].points[0] = tetras_[tetra0].points[1];
				tetras_[tetra0].points[1] = temp;
				temp = tetras_[tetra0].neighbors[0];
				tetras_[tetra0].neighbors[0] = tetras_[tetra0].neighbors[1];
				tetras_[tetra0].neighbors[1] = temp;
			}
		}
		for (size_t i = 0; i < 4; ++i)
			b4_temp_[i] = points_[tetras_[tetra1].points[i]];
		orient = orient3d(b4_temp_);
		if (std::abs(orient) == 0)
		{
			std::size_t my_loc=0;
			GetPointLocationInTetra(tetras_[tetra1], tet_temp0_.points[location0],my_loc);
			std::size_t neigh = tetras_[tetra1].neighbors[my_loc];
			std::size_t opp=0;
			GetOppositePoint(tetras_[neigh], tetra1,opp);
			for (size_t i = 0; i < 3; ++i)
				b4_temp_[i] = points_[tetras_[tetra1].points[(my_loc + i + 1) % 4]];
			b4_temp_[3] = points_[tetras_[neigh].points[opp]];
			double o = orient3d(b4_temp_);
			o *= 1.0 - static_cast<double>(2 * (location0 % 2));
			if (o > 0)
			{
				std::size_t temp = tetras_[tetra1].points[0];
				tetras_[tetra1].points[0] = tetras_[tetra1].points[1];
				tetras_[tetra1].points[1] = temp;
				temp = tetras_[tetra1].neighbors[0];
				tetras_[tetra1].neighbors[0] = tetras_[tetra1].neighbors[1];
				tetras_[tetra1].neighbors[1] = temp;
			}
		}
	}

	to_check_.push_back(tetra0);
	to_check_.push_back(tetra1);
	to_check_.push_back(Nloc);
}

void Delaunay3D::flip32(std::size_t tetra0, std::size_t tetra1, std::size_t location0,std::size_t shared_loction,
	bool flat_check)
{
	// shared_loction is the point that is to be shared in the 2 new tetras. It is the point opposite to the shared edge by the three tetras in the triangle joint with the two tetras to check
	std::size_t other_point = 20, other_point2 = 20;
	for (std::size_t i = 0; i < 4; ++i)
	{
		if (i == location0 || i == shared_loction)
			continue;
		if (other_point == 20)
			other_point = i;
		else
			other_point2 = i;
	}

	std::size_t location1=0;
	GetOppositePoint(tetras_[tetra1], tetra0,location1);
	std::size_t third_tetra = tetras_[tetra0].neighbors[shared_loction];
	tet_temp0_ = tetras_[tetra0];

	if (location0 % 2 == 0)
	{
		newtet_.points[0] = tetras_[tetra0].points[location0];
		newtet_.points[1] = tetras_[tetra0].points[shared_loction];
		size_t loctemp=0;
		GetPointLocationInTetra(tetras_[tetra1], tetras_[tetra0].points[other_point2], loctemp);
		newtet_.neighbors[0] = tetras_[tetra1].neighbors[loctemp];
		GetPointLocationInTetra(tetras_[third_tetra], tetras_[tetra0].points[other_point2], loctemp);
		newtet_.neighbors[1] = tetras_[third_tetra].neighbors[loctemp];
		if (newtet_.neighbors[0] != outside_neighbor_)
		{
			GetOppositePoint(tetras_[newtet_.neighbors[0]], tetra1, loctemp);
			tetras_[newtet_.neighbors[0]].neighbors[loctemp] = tetra0;
		}
		if (newtet_.neighbors[1] != outside_neighbor_)
		{
			GetOppositePoint(tetras_[newtet_.neighbors[1]], third_tetra, loctemp);
			tetras_[newtet_.neighbors[1]].neighbors[loctemp] = tetra0;
		}
	}
	else
	{
		newtet_.points[0] = tetras_[tetra0].points[shared_loction];
		newtet_.points[1] = tetras_[tetra0].points[location0];
		size_t loctemp=0;
		GetPointLocationInTetra(tetras_[tetra1], tetras_[tetra0].points[other_point2], loctemp);
		newtet_.neighbors[1] = tetras_[tetra1].neighbors[loctemp];
		GetPointLocationInTetra(tetras_[third_tetra], tetras_[tetra0].points[other_point2], loctemp);
		newtet_.neighbors[0] = tetras_[third_tetra].neighbors[loctemp];
		if (newtet_.neighbors[1] != outside_neighbor_)
		{
			GetOppositePoint(tetras_[newtet_.neighbors[1]], tetra1, loctemp);
			tetras_[newtet_.neighbors[1]].neighbors[loctemp] = tetra0;
		}
		if (newtet_.neighbors[0] != outside_neighbor_)
		{
			GetOppositePoint(tetras_[newtet_.neighbors[0]], third_tetra, loctemp);
			tetras_[newtet_.neighbors[0]].neighbors[loctemp] = tetra0;
		}
	}
	newtet_.points[2] = tetras_[tetra0].points[other_point];
	newtet_.points[3] = tetras_[tetra1].points[location1];

	newtet_.neighbors[2] = tetra1;
	newtet_.neighbors[3] = tetras_[tetra0].neighbors[other_point2];
	if (flat_check)
	{
		for (size_t i = 0; i < 4; ++i)
			b4_temp_[i] = points_[newtet_.points[i]];
		if (orient3d(b4_temp_) > 0)
		{
			std::size_t temp = newtet_.points[0];
			newtet_.points[0] = newtet_.points[1];
			newtet_.points[1] = temp;
			temp = newtet_.neighbors[0];
			newtet_.neighbors[0] = newtet_.neighbors[1];
			newtet_.neighbors[1] = temp;
		}
	}
	tetras_[tetra0] = newtet_;

	if (location0 % 2 == 1)
	{
		newtet_.points[0] = tet_temp0_.points[location0];
		newtet_.points[1] = tet_temp0_.points[shared_loction];
		size_t loctemp=0;
		GetPointLocationInTetra(tetras_[tetra1], tet_temp0_.points[other_point], loctemp);
		newtet_.neighbors[0] = tetras_[tetra1].neighbors[loctemp];
		GetPointLocationInTetra(tetras_[third_tetra], tet_temp0_.points[other_point], loctemp);
		newtet_.neighbors[1] = tetras_[third_tetra].neighbors[loctemp];
		if (newtet_.neighbors[1] != outside_neighbor_)
		{
			GetOppositePoint(tetras_[newtet_.neighbors[1]], third_tetra, loctemp);
			tetras_[newtet_.neighbors[1]].neighbors[loctemp] = tetra1;
		}
	}
	else
	{
		newtet_.points[0] = tet_temp0_.points[shared_loction];
		newtet_.points[1] = tet_temp0_.points[location0];
		size_t loctemp=0;
		GetPointLocationInTetra(tetras_[tetra1], tet_temp0_.points[other_point], loctemp);
		newtet_.neighbors[1] = tetras_[tetra1].neighbors[loctemp];
		GetPointLocationInTetra(tetras_[third_tetra], tet_temp0_.points[other_point], loctemp);
		newtet_.neighbors[0] = tetras_[third_tetra].neighbors[loctemp];
		if (newtet_.neighbors[0] != outside_neighbor_)
		{
			GetOppositePoint(tetras_[newtet_.neighbors[0]], third_tetra, loctemp);
			tetras_[newtet_.neighbors[0]].neighbors[loctemp] = tetra1;
		}
	}
	newtet_.points[2] = tet_temp0_.points[other_point2];
	newtet_.points[3] = tetras_[tetra1].points[location1];

	newtet_.neighbors[2] = tetra0;
	newtet_.neighbors[3] = tet_temp0_.neighbors[other_point];
	if (newtet_.neighbors[3] != outside_neighbor_)
	{
		size_t loctemp=0;
		GetOppositePoint(tetras_[newtet_.neighbors[3]], tetra0, loctemp);
		tetras_[newtet_.neighbors[3]].neighbors[loctemp] = tetra1;
	}
	if (flat_check)
	{
		for (int i = 0; i < 4; ++i)
			b4_temp_[static_cast<size_t>(i)] = points_[newtet_.points[static_cast<size_t>(i)]];
		if (orient3d(b4_temp_) > 0)
		{
			std::size_t temp = newtet_.points[0];
			newtet_.points[0] = newtet_.points[1];
			newtet_.points[1] = temp;
			temp = newtet_.neighbors[0];
			newtet_.neighbors[0] = newtet_.neighbors[1];
			newtet_.neighbors[1] = temp;
		}
	}
	tetras_[tetra1] = newtet_;

	to_check_.push_back(tetra0);
	to_check_.push_back(tetra1);

	empty_tetras_.insert(third_tetra);
	if (third_tetra == last_checked_)
		last_checked_ = tetra0;
}

void Delaunay3D::flip44(std::size_t tetra0, std::size_t tetra1, std::size_t location0, std::size_t neigh0,std::size_t neigh1)
{
	std::size_t shared_location = 20;
	for (std::size_t i = 0; i < 4; ++i)
	{
		if (tetras_[neigh0].neighbors[i] == tetra0)
		{
			shared_location = i;
			break;
		}
	}
	assert(shared_location != 20);

	flip23(tetra0, tetra1, location0,true);
	location0 = 20;
	for (std::size_t i = 0; i < 4; ++i)
	{
		if (tetras_[neigh0].neighbors[i] == neigh1)
		{
			location0 = i;
			break;
		}
	}
	assert(location0 != 20);

	flip32(neigh0, neigh1, location0, shared_location,true);
}

Delaunay3D::Delaunay3D() :tetras_(vector<Tetrahedron> ()),points_(vector<Vector3D> ()),empty_tetras_(boost::container::flat_set<size_t> ()),Norg_(0),outside_neighbor_(0),
	b3_temp_(std::array<Vector3D, 3> ()),b3_temp2_(std::array<Vector3D, 3> ()),b4_temp_(std::array<Vector3D, 4> ()),b5_temp_(std::array<Vector3D, 5> ()),
	b4s_temp_(std::array<std::size_t, 4>()),b4s_temp2_(std::array<std::size_t, 4>()),b8s_temp_(std::array<std::size_t, 8>()),
	to_check_(vector<std::size_t>()),last_checked_(0), tet_temp0_(Tetrahedron()),tet_temp1_(Tetrahedron()), newtet_(Tetrahedron())
{
	empty_tetras_.reserve(15);
	to_check_.reserve(100);
}

Delaunay3D::~Delaunay3D()
{}

void Delaunay3D::BuildExtra(vector<Vector3D> const& points)
{
	size_t Nstart = points_.size();
	points_.insert(points_.end(), points.begin(), points.end());
	std::vector<size_t> order = HilbertOrder3D(points);
	assert(to_check_.empty());
	for (std::size_t i = 0; i < points.size(); ++i)
	{
		if(InsideBigTetra(points_[order[i] + Nstart],points_,Norg_))
			InsertPoint(order[i] + Nstart);
	}
}

void Delaunay3D::Build(vector<Vector3D> const & points, Vector3D const& maxv, Vector3D const& minv,
	std::vector<size_t> &order)
{
	empty_tetras_.clear();
	std::size_t Norg = points.size();
	Norg_ = Norg;
	points_.reserve(Norg+ static_cast<std::size_t>(std::pow(Norg,0.6666)*14));
	points_.assign(points.begin(), points.end());
	// Create large tetra points
	double factor = 500;
	double dx = std::max(std::max(maxv.x - minv.x,maxv.y-minv.y),maxv.z-minv.z);
	points_.push_back(Vector3D(minv.x - 1.01*factor * dx, minv.y - factor * dx, minv.z - factor * dx));
	points_.push_back(Vector3D(0.5*(minv.x + maxv.x), maxv.y + 1.02*factor *dx, minv.z - factor * dx));
	points_.push_back(Vector3D(maxv.x + 0.99*factor * dx, minv.y - factor * dx, minv.z - factor * dx));
	points_.push_back(Vector3D(0.5*(minv.x + maxv.x), 0.5*(minv.y + maxv.y), maxv.z + factor * dx));
	// Create large tetra
	outside_neighbor_=std::numeric_limits<std::size_t>::max();
	Tetrahedron tetra;
	tetra.points[0] = Norg;
	tetra.points[1] = Norg+2;
	tetra.points[2] = Norg+1;
	tetra.points[3] = Norg+3;
	tetra.neighbors[0] = outside_neighbor_;
	tetra.neighbors[1] = outside_neighbor_;
	tetra.neighbors[2] = outside_neighbor_;
	tetra.neighbors[3] = outside_neighbor_;
	tetras_.reserve(points_.capacity() * 7);
	tetras_.push_back(tetra);
	last_checked_ = 0;

	assert(to_check_.empty());
	if (order.empty())
		order = HilbertOrder3D(points);
	for (std::size_t i = 0; i < Norg; ++i)
		InsertPoint(order[i]);
}

void Delaunay3D::output(string const & filename) const
{
	std::ofstream fh(filename.c_str(), std::ios::out | std::ostream::binary);

	std::size_t temp = tetras_.size() - empty_tetras_.size();
	fh.write(reinterpret_cast<const char*>(&temp), sizeof(std::size_t));
	temp = points_.size();
	fh.write(reinterpret_cast<const char*>(&Norg_), sizeof(std::size_t));
	fh.write(reinterpret_cast<const char*>(&temp), sizeof(std::size_t));

	for (std::size_t i = 0; i<points_.size(); ++i)
	{
		fh.write(reinterpret_cast<const char*>(&points_[i].x), sizeof(double));
		fh.write(reinterpret_cast<const char*>(&points_[i].y), sizeof(double));
		fh.write(reinterpret_cast<const char*>(&points_[i].z), sizeof(double));
	}

	for (std::size_t i = 0; i<tetras_.size(); ++i)
	{
		if (empty_tetras_.find(i) == empty_tetras_.end())
		{
			for (std::size_t j = 0; j < 4; ++j)
			{
				fh.write(reinterpret_cast<const char*>(&tetras_[i].points[j]), sizeof(std::size_t));
			}
		}
	}

	fh.close();
}

std::size_t Delaunay3D::FindThirdNeighbor(std::size_t tetra0,std::size_t tetra1)
{
	for (size_t i = 0; i < 4; ++i)
	{
		for (size_t j = 0; j < 4; ++j)
		{
			if (tetras_[tetra0].neighbors[i] == tetras_[tetra1].neighbors[j])
			{
				b8s_temp_[0] = tetras_[tetra1].neighbors[j];
				return 1;
			}
		}
	}
	return 0;
}

void Delaunay3D::ExactFlip(std::size_t tetra0, std::size_t tetra1, std::size_t p)
{
	std::size_t out_counter = 0;
	std::size_t other_point = 0;
	std::size_t in_counter = 0;
	std::size_t flat_counter = 0;
	std::array<std::size_t, 3> out_check;
	Tetrahedron const& T0 = tetras_[tetra0];
	Tetrahedron const& T1 = tetras_[tetra1];
	std::size_t p_loc=0;
	GetPointLocationInTetra(T0, p, p_loc);
	for (int i = 0; i < 3; ++i)
		b3_temp_[i] = points_[T0.points[(p_loc + 1 + static_cast<size_t>(i)) % 4]];

	for (size_t i = 0; i < 4; i++)
	{
		if (T1.neighbors[i] == tetra0)
		{
			other_point = T1.points[i];
			break;
		}
	}

	b4_temp_[0] = b3_temp_[1];
	b4_temp_[1] = b3_temp_[2];
	b4_temp_[2] = points_[p];
	b4_temp_[3] = b3_temp_[0];
	double test0 = orient3d(b4_temp_);
	if (std::abs(test0) == 0)
		in_counter++;
	b4_temp_[3] = points_[other_point];
	double test1 = orient3d(b4_temp_);
	if (std::abs(test1) == 0)
		flat_counter++;
	if (test0*test1 > 0)
		out_check[0] = 0;
	else
	{
		out_check[0] = 1;
		++out_counter;
	}

	b4_temp_[0] = b3_temp_[2];
	b4_temp_[1] = b3_temp_[0];
	b4_temp_[2] = points_[p];
	b4_temp_[3] = b3_temp_[1];
	test0 = orient3d(b4_temp_);
	if (std::abs(test0) == 0)
		in_counter++;
	b4_temp_[3] = points_[other_point];
	test1 = orient3d(b4_temp_);
	if (std::abs(test1) == 0)
		flat_counter++;
	if (test0*test1 > 0)
		out_check[1] = 0;
	else
	{
		out_check[1] = 1;
		++out_counter;
	}

	b4_temp_[0] = b3_temp_[0];
	b4_temp_[1] = b3_temp_[1];
	b4_temp_[2] = points_[p];
	b4_temp_[3] = b3_temp_[2];
	test0 = orient3d(b4_temp_);
	if (std::abs(test0) == 0)
		in_counter++;
	b4_temp_[3] = points_[other_point];
	test1 = orient3d(b4_temp_);
	if (std::abs(test1) == 0)
		flat_counter++;
	if (test0*test1 > 0)
		out_check[2] = 0;
	else
	{
		out_check[2] = 1;
		++out_counter;
	}

	if (out_counter == 0)
		flip23(tetra0, tetra1, p_loc,true);
	else
	{
		if (out_counter == 1)
		{
			std::size_t N_shared = FindThirdNeighbor(tetra0, tetra1);
			if (N_shared == 1)
			{
				std::size_t shared_loc=0;
				GetOppositePoint(tetras_[tetra0], b8s_temp_[0],shared_loc);
				flip32(tetra0, tetra1, p_loc, shared_loc,true);
				return;
			}
			else
			{
				if (flat_counter > 0)
				{
					for (std::size_t i = 0; i < 3; ++i)
					{
						std::size_t N3=0, N4=0;
						if (out_check[i] == 1 && Are44(T0, T1, (p_loc + i + 1) % 4, tetras_, N3, N4))
						{
							flip44(tetra0, tetra1, p_loc, N3, N4);
							return;
						}
					}
				}
			}
		}
		else
		{
			if (in_counter == 3) // tetra0 is flat
			{
				//is a 32 flip possible?
				std::size_t N_shared = FindThirdNeighbor(tetra0, tetra1);
				if (N_shared == 1)
				{
					std::size_t shared_loc=0;
					GetOppositePoint(tetras_[tetra0], b8s_temp_[0],shared_loc);
					flip32(tetra0, tetra1, p_loc, shared_loc,true);
					return;
				}
				else
					flip23(tetra0, tetra1, p_loc,true);
			}
		}
	}
}

void Delaunay3D::FindFlip(std::size_t tetra0,std::size_t tetra1,std::size_t p,size_t p_loc,size_t other_point_loc)
{
	size_t *tetcheck = tetras_[tetra0].points;
#ifdef __INTEL_COMPILER
#pragma ivdep
#endif
	for (std::size_t i = 0; i < 3;++i)
		//b3_temp_[i] = points_[tetras_[tetra0].points[(p_loc + i + 1) % 4]];
		b3_temp_[i] = points_[tetcheck[(p_loc + i + 1) % 4]];
	Vector3D intersection;
	bool good_intersection = PlaneLineIntersection(b3_temp_, points_[p],
		points_[tetras_[tetra1].points[other_point_loc]],intersection);
	std::pair<std::size_t, double> outside_intersection;
	if (!good_intersection)
		outside_intersection.second = 0;
	else
		outside_intersection= InTriangle(b3_temp_, intersection);
	if (outside_intersection.second<1e-6)
	{
		ExactFlip(tetra0, tetra1, p);
		return;
	}
	if (outside_intersection.first == 0)
	{
		flip23(tetra0, tetra1, p_loc,false);
		return;
	}
	if (outside_intersection.first == 1)
	{
		// Do we have a shared neighbor?
		std::size_t Nshared = FindThirdNeighbor(tetra0, tetra1);
		if (Nshared == 1)
		{
			std::size_t shared_loc=0;
			GetOppositePoint(tetras_[tetra0], b8s_temp_[0],shared_loc);
			flip32(tetra0, tetra1, p_loc, shared_loc,false);
		}
	}
}

void Delaunay3D::InsertPoint(std::size_t index)
{
	std::size_t to_split = Walk(index, last_checked_);
	last_checked_ = to_split;
	flip14(index, to_split);
	while (!to_check_.empty())
	{
		std::size_t cur_check = to_check_.back();
		to_check_.pop_back();
		size_t p_loc=0;
		GetPointLocationInTetra(tetras_[cur_check], index,p_loc);
		std::size_t to_flip = tetras_[cur_check].neighbors[p_loc];
		if (to_flip == outside_neighbor_ || (empty_tetras_.find(cur_check) != empty_tetras_.end()))
			continue;
		size_t other_point_loc=0;
		GetOppositePoint(tetras_[to_flip], cur_check,other_point_loc);
		b5_temp_[4] = points_[tetras_[to_flip].points[other_point_loc]];
		b5_temp_[0] = points_[tetras_[cur_check].points[0]];
		b5_temp_[1] = points_[tetras_[cur_check].points[1]];
		b5_temp_[2] = points_[tetras_[cur_check].points[2]];
		b5_temp_[3] = points_[tetras_[cur_check].points[3]];
		if (insphere(b5_temp_) < -0)
		{
			FindFlip(cur_check, to_flip,index,p_loc,other_point_loc);
		}
	}
}

std::size_t Delaunay3D::Walk(std::size_t point, std::size_t first_guess)
{
	bool good = false;
	std::size_t cur_facet = first_guess;
	std::size_t counter=0;
	b4_temp_[3] = points_[point];
	while (!good)
	{
		++counter;
		good = true;
		for (size_t i = 0; i < 4; ++i)
		{
			for (size_t j = 0; j < 3; j++)
				b4_temp_[j] = points_[tetras_[cur_facet].points[(i + j + 1) % 4]];
			int sign = 2 * static_cast<int>(i % 2) - 1;
			if ((orient3d(b4_temp_)*sign)>0)
			{
				good = false;
				size_t old_facet = cur_facet;
				cur_facet = tetras_[cur_facet].neighbors[i];
				if(cur_facet == outside_neighbor_)
				{
					std::cout<<"Walk wanted to goto outside neighbor"<<std::endl;
					std::cout << "Total of "<< Norg_ << " points" << std::endl;
					std::cout<<"point "<<point<<" "<<points_[point].x<<" "<<points_[point].y<<" "<<points_[point].z<<" "<<std::endl;
					std::cout<<"Big tetrahedron "<<points_[Norg_].x<<" "<<points_[Norg_].y<<" "<<points_[Norg_].z<<" "<<std::endl;
					std::cout<<"Big tetrahedron "<<points_[Norg_+1].x<<" "<<points_[Norg_+1].y<<" "<<points_[Norg_+1].z<<" "<<std::endl;
					std::cout<<"Big tetrahedron "<<points_[Norg_+2].x<<" "<<points_[Norg_+2].y<<" "<<points_[Norg_+2].z<<" "<<std::endl;
					std::cout<<"Big tetrahedron "<<points_[Norg_+3].x<<" "<<points_[Norg_+3].y<<" "<<points_[Norg_+3].z<<" "<<std::endl;
					std::cout << "Came from face " << old_facet << std::endl;
					for (size_t j = 0; j < 3; ++j)
						std::cout <<"point "<< tetras_[old_facet].points[(i + static_cast<size_t>(j) + 1) % 4]<<" "<< b4_temp_[j].x << " " << b4_temp_[j].y << " " << b4_temp_[j].z << std::endl;
					UniversalError eo("Bad Walk");
					throw eo;
				}
				break;
			}
		}
		assert(counter < 100000);
	}
	return cur_facet;
}

void Delaunay3D::flip14(std::size_t point, std::size_t tetra)
{
	Tetrahedron toadd;
	std::array<std::size_t, 3> Nloc;
	bool cleared_empty = false;
	if (empty_tetras_.size()>3)
	{
		cleared_empty = true;
		for (size_t i = 0; i < 3; i++)
		{
			Nloc[i] = *empty_tetras_.begin();
			empty_tetras_.erase(empty_tetras_.begin());
		}
	}
	else
	{
		size_t Ntet = tetras_.size();
		for (size_t i = 0; i < 3; i++)
			Nloc[i] = Ntet + i;
	}

	toadd.neighbors[0] = Nloc[1];
	toadd.neighbors[1] = Nloc[2];
	toadd.neighbors[2] = tetras_[tetra].neighbors[0];
	toadd.neighbors[3] = tetra;
	toadd.points[0] = tetras_[tetra].points[1];
	toadd.points[1] = tetras_[tetra].points[2];
	toadd.points[2] = point;
	toadd.points[3] = tetras_[tetra].points[3];
	if (toadd.neighbors[2] != outside_neighbor_)
	{
		size_t temploc = toadd.neighbors[2];
		size_t loctemp=0;
		GetOppositePoint(tetras_[temploc], tetra, loctemp);
		tetras_[toadd.neighbors[2]].neighbors[loctemp] = Nloc[0];
	}
	if (!cleared_empty)
		tetras_.push_back(toadd);
	else
		tetras_[Nloc[0]] = toadd;

	toadd.neighbors[0] = Nloc[0];
	toadd.neighbors[1] = Nloc[2];
	toadd.neighbors[2] = tetra;
	toadd.neighbors[3] = tetras_[tetra].neighbors[1];
	toadd.points[0] = tetras_[tetra].points[0];
	toadd.points[1] = tetras_[tetra].points[2];
	toadd.points[2] = tetras_[tetra].points[3];
	toadd.points[3] = point;
	if (toadd.neighbors[3] != outside_neighbor_)
	{
		size_t loctemp=0;
		GetOppositePoint(tetras_[toadd.neighbors[3]], tetra, loctemp);
		tetras_[toadd.neighbors[3]].neighbors[loctemp] = Nloc[1];
	}
	if (!cleared_empty)
		tetras_.push_back(toadd);
	else
		tetras_[Nloc[1]] = toadd;

	toadd.neighbors[0] = Nloc[0];
	toadd.neighbors[1] = tetra;
	toadd.neighbors[2] = Nloc[1];
	toadd.neighbors[3] = tetras_[tetra].neighbors[2];
	toadd.points[0] = tetras_[tetra].points[0];
	toadd.points[1] = tetras_[tetra].points[3];
	toadd.points[2] = tetras_[tetra].points[1];
	toadd.points[3] = point;
	if (toadd.neighbors[3] != outside_neighbor_)
	{
		size_t loctemp=0;
		GetOppositePoint(tetras_[toadd.neighbors[3]], tetra, loctemp);
		tetras_[toadd.neighbors[3]].neighbors[loctemp] = Nloc[2];
	}
	if (!cleared_empty)
		tetras_.push_back(toadd);
	else
		tetras_[Nloc[2]] = toadd;

	tetras_[tetra].neighbors[0] = Nloc[0];
	tetras_[tetra].neighbors[1] = Nloc[1];
	tetras_[tetra].neighbors[2] = Nloc[2];
	tetras_[tetra].points[3] = point;

	to_check_.push_back(tetra);
	to_check_.push_back(Nloc[0]);
	to_check_.push_back(Nloc[1]);
	to_check_.push_back(Nloc[2]);
#ifdef runcheks
	for (std::size_t i = 0; i < 4; ++i)
		b4_temp_[i] = points_[tetras_[tetras_.size() - 1].points[i]];
	assert(orient3d(b4_temp_) <= 0);
	for (std::size_t i = 0; i < 4; ++i)
		b4_temp_[i] = points_[tetras_[tetras_.size()-2].points[i]];
	assert(orient3d(b4_temp_) <= 0);
	for (std::size_t i = 0; i < 4; ++i)
		b4_temp_[i] = points_[tetras_[tetras_.size() - 3].points[i]];
	assert(orient3d(b4_temp_) <= 0);
	for (std::size_t i = 0; i < 4; ++i)
		b4_temp_[i] = points_[tetras_[tetra].points[i]];
	assert(orient3d(b4_temp_) <= 0);
#endif
}

bool Delaunay3D::CheckCorrect(void)
{
	std::size_t Ntetra = tetras_.size();

	for (std::size_t i = 0; i < Ntetra; ++i)
	{
		if (empty_tetras_.find(i) != empty_tetras_.end())
			continue;
		Tetrahedron const& T = tetras_[i];
		b5_temp_[0] = points_[T.points[0]];
		b5_temp_[1] = points_[T.points[1]];
		b5_temp_[2] = points_[T.points[2]];
		b5_temp_[3] = points_[T.points[3]];
		for (std::size_t j = 0; j < 4; ++j)
		{
			size_t loctemp=4;
			// Check same neighbors
			if (T.neighbors[j] != outside_neighbor_)
			{
				GetOppositePoint(tetras_[T.neighbors[j]], i, loctemp);
				assert(loctemp < 4);
			}
			// Check insphere
			if (T.neighbors[j] != outside_neighbor_)
			{
				std::size_t other=4;
				GetOppositePoint(tetras_[T.neighbors[j]], i,other);
				assert(other < 4);
				b5_temp_[4] = points_[tetras_[T.neighbors[j]].points[other]];
				assert(!(insphere(b5_temp_) < 0));
			}
		}
	}
	return true;
}

void Delaunay3D::Clean(void)
{
	tetras_.clear();
	points_.clear();
	empty_tetras_.clear();
}