#pragma once

#include "thread_local.hpp"

#include <twist/stdlike/atomic.hpp>

#include <twist/support/compiler.hpp>

#include <functional>

namespace solutions {

// Michael-Scott lock-free queue

template <typename T>
class LockFreeQueue {
 private:
  struct Node {
    twist::atomic<Node*> next;
    T item;

    Node(T my_item) {
      item = my_item;
      next.store(nullptr);
    }

    ~Node() {
      if (next.load()) {
        delete next.load();
      }
    }
  };

  ThreadLocal<std::vector<Node*> > hazard_pointers_;
  ThreadLocal<std::vector<Node*> > delete_obj_;
  twist::atomic<Node*> head_;
  twist::atomic<Node*> tail_;
  size_t hp_cnt_{0};

  void Delete_useless_ptrs() {
    std::vector<Node*> new_delete_obj;
    for (Node* ptr : (*delete_obj_)) {
      for (auto &hp : hazard_pointers_) {
        if (hp[0] == ptr || hp[1] == ptr) {
          new_delete_obj.push_back(ptr);
        }
        else {
          delete ptr;
        }
      }
    }
    (*delete_obj_) = new_delete_obj;
  }

 public:
  LockFreeQueue() {
    Node* dummy = new Node(T());
    head_.store(dummy);
    tail_.store(dummy);

    for (auto& hp : hazard_pointers_) {
      hp.resize(2, nullptr);
    }
  }

  ~LockFreeQueue() {
    for (auto& hp : hazard_pointers_) {
      delete hp[0];
      delete hp[1];
    }

    for (auto& delete_hp : delete_obj_) {
      for (auto& ptr : delete_hp) {
        delete ptr;
      }
    }

    Node* my_head = head_.load();
    Node* my_tail = tail_.load();
    head_.store(nullptr);
    tail_.store(nullptr);

    if (my_head) {
      delete my_head;
    }
  }

  void Enqueue(T item) {
    if (!(*hazard_pointers_).size()) {
      (*hazard_pointers_).resize(2, nullptr);
    }

    Node* new_node = new Node(item);
    Node* my_tail;

    for (;;) {
      my_tail = tail_.load();
      (*hazard_pointers_)[0] = my_tail;
      if (my_tail != tail_.load()) {
        continue;
      }

      Node* next = my_tail->next;
      if (next != nullptr) {
        tail_.compare_exchange_weak(my_tail, next);
        continue;
      }

      Node* nothing = nullptr;
      if (my_tail->next.compare_exchange_strong(nothing, new_node)) {
        break;
      }
    }

    tail_.compare_exchange_strong(my_tail, new_node);
    (*hazard_pointers_)[0] = nullptr;
  }

  bool Dequeue(T& item) {
    Node* my_head;

    for (;;) {
      my_head = head_.load();
      (*hazard_pointers_)[0] = my_head;
      if (my_head != head_.load()) {
        continue;
      }

      Node* next = my_head->next.load();
      (*hazard_pointers_)[1] = next;
      if (head_ != head_.load()) {
        continue;
      }

      if (!next) {
        (*hazard_pointers_)[0] = nullptr;
        return false;
      }

      Node* my_tail = tail_.load();
      if (my_head == my_tail) {
        tail_.compare_exchange_strong(my_tail, next);
        continue;
      }

      item = next->item;
      if (head_.compare_exchange_strong(my_head, next)) {
        break;
      }
    }

    (*hazard_pointers_)[0] = nullptr;
    (*hazard_pointers_)[1] = nullptr;

    (*delete_obj_).push_back(my_head);

    size_t hp_cnt = 0;
    for (auto& hp : hazard_pointers_) {
      UNUSED(hp);
      hp_cnt++;
    }
    hp_cnt *= 2;
    if ((*delete_obj_).size() == 2 * hp_cnt_) {
      Delete_useless_ptrs();
    }

    return true;
  }
};

}  // namespace solutions