/*
    GNU Octave level-set package.
    Copyright (C) 2013-2014  Daniel Kraft <d@domob.eu>

    This program is free software: 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.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

/* Template implementations of Heap.hpp.  */

#define AGGRESSIVE_CHECKS 0

namespace fastMarching
{

/* ************************************************************************** */
/* Heap class itself.  */

/**
 * Destruct freeing all memory.
 */
template<typename T, typename C>
  Heap<T, C>::~Heap ()
{
  for (auto e : data)
    delete e;
}

/**
 * Pop off and remove the top.
 * @throws std::out_of_range if the heap is empty.
 */
template<typename T, typename C>
  void
  Heap<T, C>::pop ()
{
  if (empty ())
    throw std::out_of_range ("pop() for empty heap");

  if (data.front () == data.back ())
    {
      assert (data.size () == 1);
      delete data.back ();
      data.pop_back ();
      return;
    }

  assert (data.size () > 1);
  EntryData::swap (*data.front (), *data.back ());

  delete data.back ();
  data.pop_back ();

  bubbleDown (top ());

#if AGGRESSIVE_CHECKS
  top ()->verifySubtree ();
#endif
}

/**
 * Push a new element and return its entry.
 * @param e The new element.
 * @return Entry reference to it after finding the correct position.
 */
template<typename T, typename C>
  typename Heap<T, C>::Entry
  Heap<T, C>::push (const T& e)
{
  Entry newEntry = new EntryData (*this, e, data.size ());
  data.push_back (newEntry);
  bubbleUp (newEntry);

#if AGGRESSIVE_CHECKS
  top ()->verifySubtree ();
#endif

  return newEntry;
}

/**
 * Bubble an element pointed to by the entry up to the correct position.
 * @param e Entry to bubble up.
 * @throws std::invalid_argument if the element belongs not to this heap.
 */
template<typename T, typename C>
  void
  Heap<T, C>::bubbleUp (Entry e)
{
  if (&e->parent != this)
    throw std::invalid_argument ("element belongs to a different heap");

  const Entry p = e->up ();
  if (!p)
    return;

  if (!compare (**p, **e))
    return;

  EntryData::swap (*p, *e);
#if AGGRESSIVE_CHECKS
  e->verifySubtree ();
#endif

  bubbleUp (e);
}

/**
 * Bubble an element pointed to by the entry down to the correct position.
 * @param e Entry to bubble down.
 * @throws std::invalid_argument if the element belongs not to this heap.
 */
template<typename T, typename C>
  void
  Heap<T, C>::bubbleDown (Entry e)
{
  if (&e->parent != this)
    throw std::invalid_argument ("element belongs to a different heap");

  const Entry c1 = e->down (0);
  const Entry c2 = e->down (1);

  if (!c1)
    {
      assert (!c2);
      return;
    }

  Entry largerC;
  if (!c2 || compare (**c2, **c1))
    largerC = c1;
  else
    largerC = c2;
  assert (largerC);

  if (!compare (**e, **largerC))
    return;

  EntryData::swap (*largerC, *e);
  /* Here, we can't yet check the tree since it is not finished and the
     subtrees not yet satisfy the heap condition.  */

  bubbleDown (e);
}

/* ************************************************************************** */
/* Heap entry class.  */

/**
 * Verify that the subtree belonging to this entry is in fact really
 * a heap satisfying the invariant.  An assertion failure is thrown if not.
 */
template<typename T, typename C>
  void
  Heap<T, C>::EntryData::verifySubtree ()
{
  for (parentT::indexT i = 0; i <= 1; ++i)
    {
      const auto child = down (i);
      if (child)
        {
          assert (!parent.compare (**this, **child));
          child->verifySubtree ();
        }
    }
}

/**
 * Swap two entries with each other in the vector.  This is a utility
 * routine, which updates the vector holding the entries as well
 * as updating the indices.  Of course, they must belong to the same
 * parent heap.
 * @param a First entry.
 * @param b Second entry.
 */
template<typename T, typename C>
  void
  Heap<T, C>::EntryData::swap (EntryData& a, EntryData& b)
{
  assert (&a.parent == &b.parent);

  const auto indexA = a.index;
  const auto indexB = b.index;

  a.parent.data[indexA] = &b;
  a.parent.data[indexB] = &a;

  a.index = indexB;
  b.index = indexA;
}

/**
 * Get pointer to entry of parent in the tree.
 * @return Parent in the tree as Entry or NULL if we're on top.
 */
template<typename T, typename C>
  typename Heap<T, C>::Entry
  Heap<T, C>::EntryData::up () const
{
  if (index == 0)
    return nullptr;

  return parent.data[(index - 1) / 2];
}

/**
 * Get pointer to one of the childs in the tree.
 * @param c Which child to get (0 or 1).
 * @return Selected child as Entry or NULL if there's no such child.
 * @throws std::invalid_argument if c is not 0 or 1.
 */
template<typename T, typename C>
  typename Heap<T, C>::Entry
  Heap<T, C>::EntryData::down (parentT::indexT c) const
{
  if (c != 0 && c != 1)
    throw std::invalid_argument ("down() only accepts 0 or 1");
  const auto ind = 2 * index + c + 1;

  if (ind >= parent.data.size ())
    return nullptr;

  return parent.data[ind];
}

/* ************************************************************************** */

} // Namespace fastMarching.
