#ifndef INDEX_PRIORITY_QUEUE_HPP_
#define INDEX_PRIORITY_QUEUE_HPP_

#include <vector>
#include <algorithm>
#include <iostream>

template <typename T>
class IndexPriorityQueue {
 private:
  // vector to hold priorities.  
  // priorities.at(i) is the priority associated to index i
  // the heap will only contain indices, we look up priorities
  // in this vector as needed
  std::vector<T> priorities {};
  // priorityQueue stores indices: priorityQueue.at(i) is an index
  // priorityQueue functions as the heap and is heap ordered: 
  // priorities.at(priorityQueue.at(i)) <= priorities.at(priorityQueue.at(2 * i)) 
  // priorities.at(priorityQueue.at(i)) <= priorities.at(priorityQueue.at(2 * i) + 1) 
  std::vector<int> priorityQueue {};
  // indexToPosition.at(i) is the position in priorityQueue of index i
  // priorityQueue.at(indexToPosition.at(i)) = i
  // indexToPosition.at(priorityQueue.at(j)) = j
  std::vector<int> indexToPosition {};
  int size_ = 0;

 public:
  explicit IndexPriorityQueue(int); //DONE
  void push(const T&, int); //DONE
  void pop(); //DONE
  void erase(int); //DONE
  bool contains(int) const; //DONE
  void changeKey(const T&, int); //IN PROGRESS
  std::pair<T, int> top() const; //DONE
  bool empty() const; //DONE
  int size() const; //DONE

 private:
  // TODO: you may want to add your own member functions. swim? sink?
  //MY SINK FUNCTION
  void sink(int);

  //MY SWIM FUNCTION
  void swim(int);
  void swap(int, int);
};

// Useful helper functions
int leftChild(int i) {
  return 2*i;
}

int rightChild(int i) {
  return 2*i + 1;
}

int parent(int i) {
  return i/2;
}

// IndexPriorityQueue member functions
template <typename T>
IndexPriorityQueue<T>::IndexPriorityQueue(int N) {
  priorityQueue.resize(N+1);
  for (int i = 0; i < N; ++i) {
    priorityQueue.at(i+1) = i;
  }
  indexToPosition.resize(N);
  for (int i = 0; i < N; ++i) {
    indexToPosition.at(i) = i+1;
  }
  priorities.resize(N);
}

template <typename T>
bool IndexPriorityQueue<T>::empty() const {
  return size_ == 0;
}

template <typename T>
int IndexPriorityQueue<T>::size() const {
  return size_;
}

template <typename T>
void IndexPriorityQueue<T>::push(const T& priority, int index) {


  if (!contains(index)) { //Check if index already exists
    priorities.at(index) = priority; //Define the priority
    swap(indexToPosition.at(index), size_+1);
    ++size_; //Finally, increment the queue size
    swim(index);//Swimming time!
  }
}

template <typename T>
void IndexPriorityQueue<T>::pop() {
  erase(priorityQueue.at(1));
}

template <typename T>
void IndexPriorityQueue<T>::erase(int index) {
  if (contains(index)){
    int sinker = priorityQueue.at(size_);//Index of sinker
    swap(indexToPosition.at(index), size_);
    --size_;
    if (parent(indexToPosition.at(sinker)) > 0 && priorities.at(priorityQueue.at(parent(indexToPosition.at(sinker)))) > priorities.at(sinker)) {
      swim(sinker);
    } else {
      sink(sinker);
    }
  }

}

template <typename T>
std::pair<T, int> IndexPriorityQueue<T>::top() const {
  return {priorities.at(priorityQueue.at(1)), priorityQueue.at(1)};
}

// if vertex i is not present, insert it with key
// otherwise change the associated key value of i to key
template <typename T>
void IndexPriorityQueue<T>::changeKey(const T& key, int index) {
  if (contains(index)) {
    erase(index);
  }
  push(key, index);
}

template <typename T>
bool IndexPriorityQueue<T>::contains(int index) const {
  if (index >= 0 && index < (int) indexToPosition.size()) {
    return (indexToPosition.at(index) <= size_);
  }
  return false;
}






template <typename T>
void IndexPriorityQueue<T>::swim(int index) {
  int swimmer = indexToPosition.at(index);//save swimmer's initial position

  while (swimmer > 1 && (priorities.at(priorityQueue.at(swimmer)) < priorities.at(priorityQueue.at(parent(swimmer))))) {
    swap(swimmer, parent(swimmer)); //Swim the index up

    swimmer = parent(swimmer); //Update parent for next iteration
  }
  /*std::cout << "SWIM{";
    for (int i : priorityQueue) {
      if (indexToPosition.at(i) <= size_) {
        std::cout << '{';
        std::cout << priorities.at(i);
        std::cout << ',';
        std::cout << i;
        std::cout << '}';
        std::cout << ',';
      }
    }
    std::cout << "}";
    std::cout << size_;
    std::cout << "\n";*/
    
}


template <typename T>
void IndexPriorityQueue<T>::sink(int index) {
  int sinker = indexToPosition.at(index); // position of node trying to sink
  while ((leftChild(sinker) <= size_ && priorities.at(priorityQueue.at(sinker)) > priorities.at(priorityQueue.at(leftChild(sinker)))) || (rightChild(sinker) <= size_ && priorities.at(priorityQueue.at(sinker)) > priorities.at(priorityQueue.at(rightChild(sinker))))) {
    if (priorities.at(priorityQueue.at(leftChild(sinker))) < priorities.at(priorityQueue.at(rightChild(sinker))) || rightChild(sinker) > size_){
      swap(sinker, leftChild(sinker));
      sinker = leftChild(sinker);
    } else {
      swap(sinker, rightChild(sinker));
      sinker = rightChild(sinker);
    }
  }
  /*std::cout << "SINK{";
    for (int i : priorityQueue) {
      if (indexToPosition.at(i) <= size_) {
        std::cout << '{';
        std::cout << priorities.at(i);
        std::cout << ',';
        std::cout << i;
        std::cout << '}';
        std::cout << ',';
      }
    }
    std::cout << "}";
    std::cout << size_;
    std::cout << "\n";*/
}

template <typename T>
void IndexPriorityQueue<T>::swap(const int pos1, const int pos2) {//Generic programming for a swap function that takes two positions of the piroirty queue as parameters
  int buff = priorityQueue.at(pos1); //save pos1 index to this temporary buffer
  priorityQueue.at(pos1) = priorityQueue.at(pos2); //store pos2 index at pos1
  priorityQueue.at(pos2) = buff; // store pos1's saved index (buf) to pos2
  //Now the ITP vector must be updated to maintain the invariant condition...
  indexToPosition.at(priorityQueue.at(pos2)) = pos2; 
  indexToPosition.at(priorityQueue.at(pos1)) = pos1;
}




#endif      // INDEX_PRIORITY_QUEUE_HPP_
