#pragma once

#include <forstio/id.hpp>
#include <forstio/error.hpp>

#include <deque>

namespace saw {
/**
 * Fast random access id based container.
 * 
 * Access - O(1)
 * Insert - O(1)
 * Erase - O(n) ? Dunno
 */
template<typename T, typename Encoding, typename Storage>
class id_map final {
private:
		/**
		 * Container which stores the primary data
		 */
		std::vector<data<T,Encoding>> data_;
		/**
		 * Container which tracks free'd/fragmented elements within the
		 * main container
		 */
		std::deque<id<T>> free_ids_;

private:
		/**
		 * Tries to reduce top ids
		 */
		void reduce_free_ids() noexcept {
				for(;;){
						/**
						 * If we have no free ids left, we also have nothing to reduce
						 */
						if(free_ids_.empty()){
								break;
						}

						/**
						 * The front contains the highest free id.
						 */
						if((free_ids_.front().get_value() + 1) < data_.size()){
								break;
						}
						
						/**
						 * Can this throw?
						 */
						free_ids_.pop_front();
				}
		}
public:
		/**
		 * Default constructor
		 */
		id_map() = default;

		SAW_FORBID_COPY(id_map);
		SAW_DEFAULT_MOVE(id_map);

		/**
		 * Inserts an element into the container and returns either an id on success
		 * or an error on failure.
		 */
		error_or<id<T>> insert(data<T,Encoding> val) noexcept {
				/// @todo Fix size_t and id base type
				if(free_ids_.empty()){
						try {
								size_t i = data_.size();
								data_.emplace_back(std::move(val));
								return saw::id<T>{i};
						} catch(std::exception& e) {
								return make_error<err::out_of_memory>();
						}
				} else {
						auto f_id = std::move(free_ids_.back());
						free_ids_.pop_back();
						data_.at(f_id.get_value()) = std::move(val);
						return f_id;
				}

				exit(-1);
				// Dummy return since this is not reachable
				return make_error<err::critical>();
		}

		/**
		 * Insert as data with associated id. This can fail when it doesn't adhere to the standard approach.  
		 */
		error_or<void> insert_as(data<T,Encoding> val, id<T> id) noexcept {
			if(free_ids_.empty()){
				if( id.get_value() != data_.size() ){
					return make_error<err::invalid_state>("Can't insert_as with provided ID. Doesn't match.");
				}
				try {
					data_.emplace_back(std::move(val));
				}catch(std::exception& e){
					return make_error<err::out_of_memory>();
				}
				return void_t{};
			}

			if(free_ids_.back() != id){
				return make_error<err::invalid_state>("Can't insert_as with provided ID. Doesn't match next id.");
			}
			data_.at(id.get_value()) = std::move(val);
			return void_t{};
		}

		/**
		 * Erase a value at this id. If this id isn't in the map, then it returns an error.
		 */
		error_or<void> erase(const id<T>& val) noexcept {
				/**
				 * If id is bigger than the available vector then return an error.
				 */
				if(val.get_value() >= data_.size()){
						return make_error<err::not_found>("ID is too large");
				}

				/**
				 * Check if it's the highest ID. Then we can just try to reduce the highest
				 * IDs.
				 */
				if((val.get_value() + 1) == data_.size()){
						data_.pop_back();
						this->reduce_free_ids();
						if(data_.size()*2 <= data_.capacity()){
								try {
										data_.shrink_to_fit();
								}catch(std::exception& e){
										return make_error<err::out_of_memory>();
								}
						}
						return void_t{};
				}

				/**
				 * Check if ID already exists with the free IDs.
				 * This would mean that a double free has occured.
				 */
				auto find_id = std::find(free_ids_.begin(), free_ids_.end(), val);
				if(find_id != free_ids_.end()){
						return make_error<err::not_found>("ID value has already been freed");
				}

				/**
				 * Insert id into deque and sort it.
				 */
				try {
						free_ids_.push_back(val);
				} catch(std::exception& e){
						return make_error<err::out_of_memory>();
				}
				std::stable_sort(free_ids_.begin(), free_ids_.end(),
						[](const id<T>& left, const id<T>& right) -> bool {
								return left.get_value() > right.get_value();
						}
				);
				return void_t{};
		}

		/**
		 * Tries to return the next free id
		 */
		id<T> next_free_id() const {
			if(free_ids_.empty()){
				return {data_.size()};
			}
			return free_ids_.back();
		}

		/**
		 * Tries to find a value based on an id.
		 * Returns an error on failure and returns
		 * a value pointer on success.
		 */
		error_or<data<T,Encoding>*> find(const id<T>& val){
			if(val.get_value() >= data_.size()){
				return make_error<err::not_found>("ID is too large");
			}
			
			/**
			 * This can be removed technically if we are not in a debug state?
			 */
			auto find_id = std::find(free_ids_.begin(), free_ids_.end(), val);
			if(find_id != free_ids_.end()){
				return make_error<err::not_found>("ID value has already been freed");
			}
			
			return &data_.at(val.get_value());
		}
};
}