#include "sorter.hpp"

#include <algorithm>
#include <iterator>
#include <cassert>
#include <iostream>
#include <bitset>
#include <climits>

namespace ae {

void sorter::sort(container& data) {
  // TODO Implement your sorting algorithm
  for (auto i = 1uz; i < data.placeholder_.size(); ++i) {
    std::ranges::copy(data.placeholder_[i], std::back_inserter(data.placeholder_[0]));
    data.placeholder_[i].clear();
  }
  sorter::msd_inplace_radix_sort(data.placeholder_[0], 0, [&](auto span) {sorter::robin_hood_sort(span);});
}

void sorter::msd_inplace_radix_sort(
  std::span<container::element_type> range, 
  size_t passes, 
  const std::function<void(std::span<container::element_type> bucket)>& bucket_sort
) {
  std::cerr << "passes: " << passes << "size: " << range.size() << std::endl;
  if (sorter::RADIX_ITERATIONS == passes) {
    std::cerr << "going to robin " << *std::begin(range) << std::endl;
    if (std::begin(range) + 1 < std::end(range)) {
      bucket_sort(range);
    }
    return;
  }

  auto lower = std::begin(range);
  auto upper = std::end(range);

  for (auto element = lower; element < upper;) {
    std::cerr << *element << " " << &*element << " " << &*lower << " " << &*upper << std::endl;
    // Mask out the <sorter::RADIX_ITERATIONS>-last bit and check if it is set
    // std::cerr << std::bitset<64>((1L << (sizeof(container::element_type) * CHAR_BIT - passes - 1))) << std::endl;
    // fprintf(stderr, "%lx\n", (1L << (sizeof(container::element_type) * CHAR_BIT - passes - 1)));
    if ((*element & (1L << (sizeof(container::element_type) * CHAR_BIT - passes - 1)))) {
      // The <passes> bit is set, so move to the beginning of the end section and decrement the upper iterator
      std::swap(*upper, *element);
      --upper;
    } else {
      // The <passes> bit is unset, so move to the end of the beginning section and increment the upper iterator
      std::swap(*lower, *element);
      ++lower;
      ++element;
    }
  }
  // assert(lower == upper);
  sorter::msd_inplace_radix_sort(std::span<container::element_type> (std::begin(range), lower), passes + 1, bucket_sort);
  sorter::msd_inplace_radix_sort(std::span<container::element_type> (upper, std::end(range)), passes + 1, bucket_sort);
}
void sorter::robin_hood_sort(std::span<container::element_type> bucket) {
  std::cerr << "robin hood lol" << std::endl;
  const auto size = bucket.size() + sorter::OVERHEAD_SIZE;
  const auto mask = ((1L) << (sizeof(container::element_type) * CHAR_BIT - sorter::RADIX_ITERATIONS)) - 1;
  container::element_type space[size];
  std::cerr << "robin hood before loop" << std::endl;
  for (auto element : bucket) {
    auto masked_element = (element & mask);
    std::cerr << "robin hood elements " << element << " " << masked_element << " " << size << " " << mask << std::endl;
    auto index = (masked_element * size) / mask;
    std::cerr << "robin hood index " << index << " " << size << std::endl;
    if (space[index] == -1) {
      space[index] = masked_element;
    } else {
      auto i = index;
      while (i < size && space[index] != -1) {++i;};
      space[i] = masked_element;
    }
  }
  std::cerr << "robin hood after loop" << std::endl;

  // One final pass to correct linear probing errors
  for (auto i = 1; i < size; ++i) {
    auto j = i;
    while (space[j-1] > space[j] && j > 0) {
      std::swap((space[j]),space[j-1]);
    }
  }

  // copy data back into original range
  auto i = 0;
  for (auto element = std::begin(bucket); element < std::end(bucket); ++element) {
    *element = space[i];
  }
}

}  // namespace ae