gplib/include/gp/containers/flat_tree.hpp

#pragma once

#include <gp/algorithms/sort.hpp>
#include <gp/containers/array.hpp>
#include <gp/math/integral.hpp>
#include <gp/utils/iterator.hpp>
#include <gp/functional/optional.hpp>
// UNIMPLEMENTED: see filename

namespace gp {

	namespace ordinators {
		struct less {
			template<typename T>
			bool operator() (const T& lhs, const T& rhs){
				return lhs < rhs;
			}
		};
	}

	/**
	 * @brief 
	 * 
	 * @tparam T The type to store, must be either final or primitive
	 * @tparam capacity The number of elements that can be stored in the structure
	 */
	template<typename T, size_t capacity, typename ordinator = gp::ordinators::less>
	class flat_tree {
		using node_t = gp::optional<T>;
		gp::array<node_t, capacity> data_;

		static void ordain(auto destination_space, auto value_workspace, index_to_1 idx) {
			if(!value_workspace.size()) return;
			size_t pivot;
			
			if(value_workspace.size()%2) {
				pivot = value_workspace.size()/2 + 1;
			} else {
				pivot = value_workspace.size()/2;
			}

			destination_space[idx] = gp::move(value_workspace[pivot]);
			auto left = idx;
			left.left();
			ordain(destination_space, value_workspace.slice_start(pivot), left);
			auto right = idx;
			right.right();
			ordain(destination_space, value_workspace.trim_start(pivot+1), left);
		};

	public:
		using iterator = gp::flat_tree_iterator<flat_tree, T, 1>;
		using riterator = gp::flat_tree_iterator<flat_tree, T, -1>;

		friend iterator;
		friend riterator;
		
		flat_tree(auto data) {
			gp_config::assertion(data.size() <= data_.size(), "Couldn't construct flat tree from C array");
			for(auto v : data) {
				insert(v);
			}
		}

		bool insert(T& value) {
			index_to_1 idx(0);
			while(idx < data_.size() && data()[idx].has_value()) {
				if(ordinator()(data()[idx].value(), value)) {
					idx.left();
				} else {
					idx.right();
				}
			}

			if (idx >= data_.size()) [[unlikely]] {
				if(rebalance_with_value(value)) return true;
				if constexpr (gp_config::has_exceptions) {

				}
				return false;
			}

			data()[idx] = value;
			return true;
		}

		/**
		 * @brief Rebalance the tree
		 * 
	 	 * @param value The value to try to push along with the rebalance
		 * 
		 * @return true if the operation suceeded
		 * @return false if the operation failed
		 */
		bool rebalance_with_value(T& value) {
			// TODO: Add constant memory shenanigans 
			/*
			auto workspace = data_.as_buffer();

			bool ok = false;
			for(auto& elem : workspace) {
				if(!elem.has_value()) {
					ok = true;
					break;
				}
			}

			if(!ok) return false;

			gp::sort(workspace.begin(), workspace.end(), [](const node_t& lhs, const node_t& rhs){ return lhs.has_value() > rhs.has_value(); });

			size_t cnt = 0;
			for(auto& elem : workspace) {
				if(elem.has_value()) break;
				++cnt;
			}

			--cnt;
			workspace[cnt] = value;

			auto value_workspace = workspace.trim_start(cnt);

			gp::sort(value_workspace.begin(), value_workspace.end(), [](const node_t& lhs, const node_t& rhs){ return ordinator(lhs.value(), rhs.value()); });

			auto destination_space = workspace.slice_start(value_workspace.size());

			for(auto )
			*/

			auto oth = gp::move(data_);
			auto workspace = oth.as_buffer();

			bool ok = false;
			for(auto& elem : workspace) {
				if(!elem.has_value()) {
					ok = true;
					break;
				}
			}

			if(!ok) return false;

			gp::sort(workspace.begin(), workspace.end(), [](const node_t& lhs, const node_t& rhs){ return lhs.has_value() > rhs.has_value(); });

			size_t cnt = 0;
			for(auto& elem : workspace) {
				if(elem.has_value()) break;
				++cnt;
			}

			--cnt;
			workspace[cnt] = value;

			auto value_workspace = workspace.trim_start(cnt);

			gp::sort(value_workspace.begin(), value_workspace.end(), [](const node_t& lhs, const node_t& rhs){ return ordinator()(lhs.value(), rhs.value()); });

			auto destination_space = workspace.slice_start(value_workspace.size());

			ordain(destination_space, value_workspace, 0);
 
			return true;
		}

		iterator begin() {
			return ++iterator{.tree = *this, .index = (size_t)-1};
		}

		iterator end() {
			return iterator{.tree = *this, .index = (size_t)-1};
		}

		auto data() {
			return data_.as_buffer();
		}
	};
}