#include "sorter.hpp"

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

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 << std::endl;
  if (sorter::RADIX_ITERATIONS == passes) {
    bucket_sort(range);
    return;
  }

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

  for (auto element = lower; element < std::end(range);) {
    std::cerr << *element << " " << &*element << " " << &*lower << " " << &*upper << std::endl;
    // Mask out the <sorter::RADIX_ITERATIONS>-last bit and check if it is set
    if ((*element & (1uz << sizeof(container::element_type) * 8 - passes))) {
      // 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;
    }
    if (element < lower) {
      ++element;
    } else if (element > upper) {
      --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) {
  const auto size = bucket.size() + sorter::OVERHEAD_SIZE;
  const size_t mask = ~(-1 >> sorter::RADIX_ITERATIONS);
  container::element_type space[size];

  for (auto element : bucket) {
    auto masked_element = (element & mask);
    auto index = masked_element * size/ mask;
    if (space[index] == -1) {
      space[index] = masked_element;
    } else {
      auto i = index;
      while (i < size && space[index] != -1) {++i;};
      space[i] = masked_element;
    }
  }

  // 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