/*!
 * Different storage types used internally to store entities components
 *
 * By default a component will be stored in a `DenseVec` but one can
 * specify different storage types by using the storage attribute when
 * deriving `Component`
 *
 * ```
 * # #[macro_use] extern crate rinecs_derive;
 * #[derive(Component, Debug)]
 * #[storage(VecStorage)]
 * struct Position{ x: f32, y : f32 }
 * ```
 *
 * The default storage type, `DenseVec`, stores components contiguously using
 * an index to map entity id to position in a vector. It needs 2 memory reads
 * to access an element but minimizes memory use in case the component is not
 * used by most entities.
 *
 * For components where almost every entity has them, a `VecStorage` can be
 * faster since it doesn't have an index. A `VecStorage` is just a Vec of the
 * component where the position in the vector is the same as the entity id.
 *
 * Hierarchical components always use a `Forest` storage which is a tree
 * like structure implemented on a `Vec` to maximize memory contiguity.
 *
 * One to N components, or slice components are always stored in `DenseOneToNVec`,
 * a storage that stores slices contiguosly in memory using a Vec plus an
 * index to know the range that belongs to each Entity. Right now adding new
 * elements to already created structures of this type can be really slow so
 * in those cases you might want to just use a custom component that contains a
 * collection instead.
 */
use std::fmt::Debug;
use std::marker::PhantomData;
use crate::{Entities, Read, Write, idtree, operators::{read::StorageRead, write::StorageWrite}};
use hashbrown::HashMap;
use crate::component;
use std::any::Any;
use crate::sync::{
    ReadGuardRef, WriteGuardRef, StorageWriteGuard, IndexGuard, OrderedIndexGuard, Lock,
    ReadGuard, WriteGuard
};
#[cfg(not(feature = "parallel_systems"))]
use std::cell::UnsafeCell;
use crate::component::{
    Component, ComponentSend, ComponentThreadLocal,
    OneToNComponentSend, OneToNComponentThreadLocal,
    HasChanged, Changes, NToOneComponent,
};
use crate::bitmask::{self, Bitmask, MaskType, NextMask};
#[cfg(not(feature="debug_locks"))]
use std::cell::RefCell;
#[cfg(feature="debug_locks")]
use crate::mutcell_dbg::MutCell as RefCell;
#[cfg(feature = "debug_parameters")]
use erased_serde::{Serializer};
use crate::DebugParameter;
use crate::entity::Entity;
use std::slice;
use crate::utils::*;
use std::ops::{Deref, DerefMut};
use hashbrown::HashSet;
#[cfg(feature="parallel_systems")]
use crossbeam_skiplist::SkipMap;
use std::borrow::{Borrow, BorrowMut};
#[cfg(feature = "debug_parameters")]
use serde::Serialize;
#[cfg(components_64)]
use num_traits::{Zero};
use std::any::TypeId;
use hashbrown::hash_map::Entry;
use unchecked_unwrap::*;
use crate::unwrap_or_return;
use crate::DataAccesses;

// #[doc(hidden)]
// pub mod assoc_vec;
// #[doc(hidden)]
// pub mod hashmap;
#[doc(hidden)]
pub mod oneton_forest;

pub mod dense_vec;
pub mod forest;
pub mod oneton_densevec;
pub mod vec;
pub mod changed;
pub mod autochanged;
pub mod tags;
pub mod unique_densevec;

#[doc(inline)]
pub use densevec::DenseVec;
#[doc(inline)]
pub use unique_densevec::UniqueDenseVec;
#[doc(inline)]
pub use self::vec::VecStorage;
#[doc(inline)]
pub use self::forest::Forest;
#[doc(inline)]
pub use self::oneton_densevec::{DenseOneToNVec, ChangeDenseOneToNVec};
// #[doc(hidden)]
// pub use self::assoc_vec::AssocVec;
#[doc(hidden)]
pub use self::oneton_forest::OneToNForest;
#[doc(hidden)]
pub use self::changed::Changed;
#[doc(hidden)]
pub use self::autochanged::AutoChanged;
#[doc(hidden)]
pub use tags::Tags;

pub type ChangedDenseVec<T> = Changed<DenseVec<T>,T>;
pub type ChangedVec<T> = Changed<VecStorage<T>,T>;
pub type ChangedForest<T> = Changed<Forest<T>,T>;
pub type ChangedOneToN<T> = Changed<ChangeDenseOneToNVec<T>, T>;
pub type AutoChangedDenseVec<T> = AutoChanged<DenseVec<T>,T>;
pub type AutoChangedVec<T> = AutoChanged<VecStorage<T>,T>;
pub type AutoChangedForest<T> = AutoChanged<Forest<T>,T>;
pub type AutoChangedOneToN<T> = AutoChanged<ChangeDenseOneToNVec<T>, T>;

const FASTINDEX_ARRAY_LEN: usize = crate::utils::STORAGE_MACROS_NUM * 2 * std::mem::size_of::<usize>();
pub struct HiddenFastIndex{
    value: [u8; FASTINDEX_ARRAY_LEN],

    #[cfg(feature="debug_fast_indices")]
    type_id: TypeId,
    #[cfg(feature="debug_fast_indices")]
    type_name: &'static str,
}

impl HiddenFastIndex{
    pub fn new<T: 'static>(t: T) -> HiddenFastIndex {
        let mut slice: [u8; FASTINDEX_ARRAY_LEN] = unsafe{ std::mem::MaybeUninit::uninit().assume_init() };
        if std::mem::size_of::<T>() > FASTINDEX_ARRAY_LEN {
            panic!("Too many operators in this query")
        }
        unsafe{
            std::ptr::write(
                slice.as_mut_ptr() as *mut T,
                t
            );
        }

        HiddenFastIndex{
            value: slice,

            #[cfg(feature="debug_fast_indices")]
            type_id: std::any::TypeId::of::<T>(),
            #[cfg(feature="debug_fast_indices")]
            type_name: std::any::type_name::<T>(),
        }
    }

    pub unsafe fn unchecked_downcast_ref<T: 'static>(&self) -> &T {
        #[cfg(feature="debug_fast_indices")]
        if std::any::TypeId::of::<T>() != self.type_id {
            panic!("Trying to downcast ids of different types: stored as {} trying to downcast as {}", self.type_name, std::any::type_name::<T>());
        }

        &*(self.value.as_ptr() as *const T)
    }
}

#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct OrderedId<F = usize> {
    pub guid: usize,
    pub fast: F,
}

pub trait Storage<'a, T>{
    type Get;
    type GetMut;
    type DerefTarget: ?Sized;
    type IdedIter: Iterator<Item = (usize, Self::Get)>;
    type Iter: Iterator<Item = Self::Get>;
    type IterMut: Iterator<Item = Self::GetMut>;
    fn new() -> Self;
    fn with_capacity(capacity: usize) -> Self;
    fn insert(&mut self, guid: usize, t: T);
    fn remove(&mut self, guid: usize);
    unsafe fn get(&self, guid: usize) -> Self::Get;
    unsafe fn get_mut(&mut self, guid: usize) -> Self::GetMut;
    unsafe fn fast_get(&self, idx: usize) -> Self::Get;
    unsafe fn fast_get_mut(&mut self, idx: usize) -> Self::GetMut;
    fn fast_index(&self, guid: usize) -> usize;
    fn guid_from_fast_index(&self, guid: usize) -> usize;
    fn contains(&self, guid: usize) -> bool;
    fn update(&mut self){}
    fn tracks_changes() -> bool { false }
    fn storage_changed(&self) -> bool { false }
    fn ided_iter(&self) -> Self::IdedIter;
    fn iter(&self) -> Self::Iter;
    fn iter_mut(&mut self) -> Self::IterMut;
}

pub trait StorageAssociatedComponent{
    type Component;
}

#[cfg(feature = "parallel_iter")]
pub trait ParStorage<'a, T> {
    type ParIter;
    type ParIterMut;
    fn par_iter(&self) -> Self::ParIter;
    fn par_iter_mut(&mut self) -> Self::ParIterMut;
}

pub trait ContiguousStorage<T>{
    fn contiguous_memory(&self) -> &[T];
}

pub trait IntoIter{
    type Iter: Iterator;
    fn into_iter(self) -> Self::Iter;
}

pub trait IntoIterMut{
    type IterMut: Iterator;
    fn into_iter_mut(self) -> Self::IterMut;
}

pub trait IntoIdedIterMut{
    type IdedIterMut: Iterator;
    fn into_ided_iter_mut(self) -> Self::IdedIterMut;
}

pub unsafe trait ReadOnlyStorage{}

pub trait FastIndexExt {
    type FastIndex: 'static + Copy;
    type StaticTypeId: 'static;
    fn fast_index(&self, guid: usize) -> Self::FastIndex;
}

pub trait StorageRef<'r>: FastIndexExt {
    type Component;
    type Data;

    unsafe fn get_fast_unchecked(&'r mut self, idx: Self::FastIndex) -> Self::Component;
    unsafe fn get_unchecked(&'r mut self, guid: usize) -> Self::Component;
    fn get(&'r mut self, entity: &Entity) -> Option<Self::Component>{
        if self.contains(entity.guid()){
            unsafe{ Some(self.get_unchecked(entity.guid())) }
        }else{
            None
        }
    }
    fn contains(&self, guid: usize) -> bool;
}

pub trait IntoSendStorage<'r> {
    type SendStorage: StorageRef<'r>;
    fn into_send_storage(&'r mut self) -> Self::SendStorage;
}

pub trait StorageEntitiesExt<'a, S>{
    type IntoStorage;
    type Component;
    fn into_storage(self) -> Option<Self::IntoStorage>;
    fn entities(&self) -> &Entities<'a>;
    fn get(self, entity: &Entity) -> Option<Self::Component>;
}

pub trait IntoStorages{
    type Storages;
    fn into_storages(self) -> Self::Storages;
}

pub trait IntoOrderedIter{
    type OrderedIter: Iterator;
    fn into_ordered_iter(self) -> Self::OrderedIter;
}

pub trait IntoOrderedIterMut{
    type OrderedIterMut: Iterator;
    fn into_ordered_iter_mut(self) -> Self::OrderedIterMut;
}

pub trait IntoOrderedIdedIterMut{
    type OrderedIdedIterMut: Iterator;
    fn into_ordered_ided_iter_mut(self) -> Self::OrderedIdedIterMut;
}

pub trait IntoHierarchicalIter<'a, T>{
    fn into_hierarchical_iter(self) -> forest::ForestHierarchicalIter<'a, T>;
}

pub trait IntoHierarchicalIterMut<'a, T>{
    fn into_hierarchical_iter_mut(self) -> forest::ForestHierarchicalIterMut<'a, T>;
}

impl<'a,T:'a + Debug,I> IntoHierarchicalIter<'a,T> for I
    where I: IntoOrderedIter<OrderedIter = forest::ForestHierarchicalIter<'a,T>>
{
    fn into_hierarchical_iter(self) -> forest::ForestHierarchicalIter<'a, T>{
        self.into_ordered_iter()
    }
}

impl<'a,T:'a + Debug,I> IntoHierarchicalIterMut<'a,T> for I
    where I: IntoOrderedIterMut<OrderedIterMut = forest::ForestHierarchicalIterMut<'a,T>>
{
    fn into_hierarchical_iter_mut(self) -> forest::ForestHierarchicalIterMut<'a, T>{
        self.into_ordered_iter_mut()
    }
}

pub trait HierarchicalStorage<'a,T>: Storage<'a,T>{
    unsafe fn insert_child(&mut self, parent_guid: usize, guid: usize, value: T);
    unsafe fn get_node(&self, guid: usize) -> idtree::NodeRef<T>;
    unsafe fn get_node_mut(&mut self, guid: usize) -> idtree::NodeRefMut<T>;
    unsafe fn fast_get_node(&self, idx: idtree::NodeId) -> idtree::NodeRef<T>;
    unsafe fn fast_get_node_mut(&mut self, idx: idtree::NodeId) -> idtree::NodeRefMut<T>;
    unsafe fn guid_for(&self, node: idtree::NodeId) -> usize;
    unsafe fn ordered_fast_index(&self, guid: usize) -> idtree::NodeId;
    fn ordered_ids(&self) -> ReadGuardRef<Vec<OrderedId<idtree::NodeId>>>;
}

#[derive(Debug)]
pub struct SliceView<'a, T>{
    might_changed: bool,
    slice: &'a [T]
}

impl<'a,T> Clone for SliceView<'a,T>{
    fn clone(&self) -> SliceView<'a,T>{
        SliceView{
            might_changed: self.might_changed,
            slice: self.slice,
        }
    }
}

impl<'a,T> Copy for SliceView<'a,T>{}

impl<'a, T: PartialEq> PartialEq for SliceView<'a,T>{
    fn eq(&self, other: &Self) -> bool {
        self.slice == other.slice
    }
}

impl<'a, T: Eq> Eq for SliceView<'a,T>{}

impl<'a, T> Deref for SliceView<'a, T>{
    type Target = [T];
    fn deref(&self) -> &[T]{
        self.slice
    }
}

impl<'a, T> Borrow<[T]> for SliceView<'a, T> {
    fn borrow(&self) -> &[T] {
        self.slice
    }
}

impl<'a,T> IntoIterator for SliceView<'a,T>{
    type Item = &'a T;
    type IntoIter = slice::Iter<'a, T>;
    fn into_iter(self) -> slice::Iter<'a, T> {
        self.slice.into_iter()
    }
}

impl<'a, T> HasChanged for SliceView<'a,T>
where for<'b> &'b T: HasChanged
{
    fn changed(&self) -> bool{
        self.might_changed && self.slice.iter().any(|v| v.changed())
    }
}

#[derive(Debug)]
pub struct SliceViewMut<'a, T>{
    might_changed: Option<&'a mut bool>,
    slice: &'a mut [T],
}

impl<'a, T> Deref for SliceViewMut<'a, T>{
    type Target = [T];
    fn deref(&self) -> &[T]{
        self.slice
    }
}

impl<'a, T> DerefMut for SliceViewMut<'a, T>{
    fn deref_mut(&mut self) -> &mut [T]{
        if let Some(changed) = self.might_changed.as_mut() {
            **changed = true;
        }
        self.slice
    }
}

impl<'a, T> Borrow<[T]> for SliceViewMut<'a, T> {
    fn borrow(&self) -> &[T] {
        self.slice
    }
}

impl<'a, T> BorrowMut<[T]> for SliceViewMut<'a, T> {
    fn borrow_mut(&mut self) -> &mut [T] {
        self.slice
    }
}

impl<'a,T> IntoIterator for SliceViewMut<'a,T>{
    type Item = &'a mut T;
    type IntoIter = slice::IterMut<'a, T>;
    fn into_iter(mut self) -> slice::IterMut<'a, T> {
        if let Some(changed) = self.might_changed.as_mut() {
            **changed = true;
        }
        self.slice.into_iter()
    }
}

impl<'a, T> HasChanged for SliceViewMut<'a,T>
where for<'b> &'b T: HasChanged
{
    fn changed(&self) -> bool{
        self.might_changed.as_ref().map(|c| **c).unwrap_or(false)
            && self.slice.iter().any(|v| v.changed())
    }
}

impl<'a, T> Changes for SliceViewMut<'a, T>
where
    for<'b> &'b T: HasChanged,
    for<'b> &'b mut T: Changes,
{
    fn has_changed(&self) -> bool{
        self.changed()
    }

    fn reset_changed(&mut self){
        if let Some(changed) = self.might_changed.as_mut(){
            **changed = false;
        }

        for mut v in self.slice.iter_mut() {
            v.reset_changed();
        }
    }
}

pub trait OneToNStorage<'a,T>: Storage<'a,T>{
    fn insert_slice<I: IntoIterator<Item = T>>(&mut self, guid: usize, t: I);
    unsafe fn get_slice(&self, guid: usize) -> SliceView<T>;
    unsafe fn get_slice_mut(&mut self, guid: usize) -> SliceViewMut<T>;
}


pub trait HierarchicalOneToNStorage<'a,T>: Storage<'a,T>{
    unsafe fn insert_root(&mut self, guid: usize, t: T) -> idtree::NodeRefMut<T>;
    unsafe fn insert_child(&mut self, parent: idtree::NodeId, t: T) -> idtree::NodeRefMut<T>;
}

pub trait AutoChangedStorageExt<'a>{
    fn update_changed(&mut self);
}

pub trait ChangedStorageExt<'a> {
    fn changed_entities(&self) -> hashbrown::hash_set::Iter<usize>;
}

pub trait IntoChangesIter<'a,T>{
    type ChangesItem;
    type RawSubStorage: Storage<'a, T> + 'a;
    type SubStorage: DerefMut<Target = Self::RawSubStorage>;
    fn extend_changes<F>(self, has_chagend: F)
    where F: FnMut(Self::ChangesItem) -> bool;
    unsafe fn into_storage_and_changes(self) -> (Self::SubStorage, &'a mut HashSet<usize>);
}

pub trait IntoOrderedChangesIter<'a,T>{
    type OrderedChangesItem;
    type RawSubStorage: HierarchicalStorage<'a,T> + 'a;
    type SubStorage: DerefMut<Target = Self::RawSubStorage>;
    fn ordered_extend_changes<F>(self, has_chagend: F)
    where F: FnMut(Self::OrderedChangesItem) -> bool;
    unsafe fn into_storage_and_changes(self) -> (Self::SubStorage, &'a mut HashSet<usize>);
}

type RemoveComponentFn = dyn Fn(&Storages, usize);
#[cfg(feature = "debug_parameters")]
type DebugComponentFn = dyn Fn(&Storages, usize, &mut dyn Serializer) -> Result<(), erased_serde::Error>;
#[cfg(not(feature = "debug_parameters"))]
type DebugComponentFn = dyn Fn(&Storages, usize) -> String;

#[cfg(not(feature = "parallel_systems"))]
type SkipMap<K,V> = UnsafeCell<HashMap<K,V>>;

#[derive(Clone)]
pub struct SubStorages<'a> {
    storages: &'a Storages,
    resource_mask_r: Option<MaskType>,
    resource_mask_w: Option<MaskType>,
    system_info: &'a str,
}

impl<'a> SubStorages<'a> {
    pub fn split<U: DataAccesses>(&mut self) -> (SubStorages, SubStorages) {
        let split_mask_r = U::reads().into_iter()
            .fold(MaskType::zero(), |mask, ty| mask | self.storages.mask_for_type_id(ty));
        let split_mask_w = U::writes().into_iter()
            .fold(MaskType::zero(), |mask, ty| mask | self.storages.mask_for_type_id(ty));
        if let Some(resource_mask_r) = self.resource_mask_r.as_ref() {
            if &split_mask_r & resource_mask_r != split_mask_r {
                panic!("Trying to split entities that don't contain the required types for read from: {}", self.system_info);
            }
        }
        if let Some(resource_mask_w) = self.resource_mask_w.as_ref() {
            if &split_mask_w & resource_mask_w != split_mask_w {
                panic!("Trying to split entities that don't contain the required types for write from: {}", self.system_info);
            }
        }
        let rest_mask_r = self.resource_mask_r.clone().unwrap_or(MaskType::max_value()) ^ &split_mask_r;
        let rest_mask_w = self.resource_mask_w.clone().unwrap_or(MaskType::max_value()) ^ &split_mask_w;
        let split = SubStorages {
            storages: self.storages,
            resource_mask_r: Some(split_mask_r),
            resource_mask_w: Some(split_mask_w),
            system_info: self.system_info,
        };
        let rest = SubStorages {
            storages: self.storages,
            resource_mask_r: Some(rest_mask_r),
            resource_mask_w: Some(rest_mask_w),
            system_info: self.system_info,
        };
        (split, rest)
    }

    pub fn storage<C: Component>(&self) -> Option<ReadGuardRef<'a, <C as Component>::Storage>> {
        self.storages.masked_storage::<C>(self.resource_mask_r.as_ref(), self.system_info)
    }

    pub fn storage_mut<C: Component>(&self) -> Option<StorageWriteGuard<'a, C>>
    {
        self.storages.masked_storage_mut::<C>(self.resource_mask_w.as_ref(), self.system_info)
    }

    pub fn storage_thread_local<C: Component>(&self) -> Option<ReadGuardRef<'a, <C as Component>::Storage>> {
        self.storages.masked_storage_thread_local::<C>(self.resource_mask_r.as_ref(), self.system_info)
    }

    pub fn storage_thread_local_mut<C: Component>(&self) -> Option<StorageWriteGuard<'a, C>>
    {
        self.storages.masked_storage_thread_local_mut::<C>(self.resource_mask_w.as_ref(), self.system_info)
    }

    pub fn update_changed<C: Component>(&self)
    where <C as Component>::Storage: AutoChangedStorageExt<'a>
    {
        self.storages.update_changed::<C>(self.resource_mask_w.as_ref(), self.system_info)
    }

    pub fn update_changed_thread_local<C: Component>(&self)
    where <C as Component>::Storage: AutoChangedStorageExt<'a>
    {
        self.storages.update_changed_thread_local::<C>(self.resource_mask_w.as_ref(), self.system_info)
    }

    pub fn component_mask<C: 'static>(&self) -> MaskType{
        self.storages.storages.get(&TypeId::of::<C>())
            .map(|storage| storage.component_mask.clone())
            .unwrap_or_else(|| MaskType::zero())
    }

    pub fn entities_ref(&self) -> &'a [(Entity, MaskType)]{
        &self.storages.entities
    }

    pub fn check_storage_changed(&self, id: component::Id) -> bool{
        self.storages.check_storage_changed(id)
    }

    pub fn entities_for_mask<S: FastIndexExt>(&self, query_mask: Bitmask, storage: &S) -> IndexGuard<'a>{
        self.storages.entities_for_mask(query_mask, storage)
    }

    pub fn any_has_mask(&self, cmask: Bitmask) -> bool {
        self.storages.any_has_mask(cmask)
    }

    pub fn ordered_entities_for_thread_local<C: Component, S: FastIndexExt>(&self, query_mask: Bitmask, unordered_storage: &S) -> OrderedIndexGuard<'a>
        where for<'b> <C as Component>::Storage: HierarchicalStorage<'b, C>
    {
       self.storages.ordered_entities_for_thread_local::<C,S>(query_mask, unordered_storage)
    }

    pub fn ordered_entities_for<C: Component, S: FastIndexExt>(&self, query_mask: Bitmask, unordered_storage: &S) -> OrderedIndexGuard<'a>
        where for<'b> <C as Component>::Storage: HierarchicalStorage<'b, C>
    {
        self.storages.ordered_entities_for::<C, S>(query_mask, unordered_storage)
    }

    pub unsafe fn creation_storage<C: ComponentThreadLocal>(&mut self) -> CreationSto<C>{
        self.storages_mut().creation_storage::<C>()
    }

    pub unsafe fn storages(&self) -> &Storages {
        #[cfg(all(feature="parallel_systems", feature="lockfree"))]
        assert!(self.resource_mask_r.is_none());
        #[cfg(all(feature="parallel_systems", feature="lockfree"))]
        assert!(self.resource_mask_w.is_none());

        &self.storages
    }

    pub unsafe fn storages_mut(&self) -> &mut Storages {
        #[cfg(all(feature="parallel_systems", feature="lockfree"))]
        assert!(self.resource_mask_r.is_none());
        #[cfg(all(feature="parallel_systems", feature="lockfree"))]
        assert!(self.resource_mask_w.is_none());

        &mut *(self.storages as *const Storages as *mut Storages)
    }

    #[cfg(feature = "debug_parameters")]
    pub fn debug_component<S: Serializer>(&self, entity: &Entity, id: &component::Id, mut serializer: S) -> Result<(), erased_serde::Error>{
        self.storages.registry.debug_components_mask_index[id](&self.storages, entity.guid(), &mut serializer)
    }

    pub fn components_masks_iter(&self) -> impl Iterator<Item = (&TypeId, &MaskType)> {
        self.storages.storages.iter().map(|(id, s)| (id, &s.component_mask))
    }

    pub fn check(&self, entity: &Entity, entity_mask: &MaskType, mask: &Bitmask) -> bool {
        self.storages.check(entity, entity_mask, mask)
    }
}

pub struct MaskedStorage {
    dependency_mask: MaskType,
    component_mask: MaskType,
    storage: Box<dyn Any>,
}

impl MaskedStorage {
    pub fn component_mask(&self) -> &MaskType {
        &self.component_mask
    }
}

type QueryId = TypeId;

#[doc(hidden)]
#[derive(Default)]
pub struct StorageRegistry {
    pre_register: HashMap<component::Id, MaskType>,
    components_mask_index: HashMap<component::Id, MaskType>,
    reverse_components_mask_index: HashMap<MaskType, component::Id>,
    remove_components_mask_index: HashMap<MaskType, Box<RemoveComponentFn>>,
    debug_components_mask_index: HashMap<component::Id, Box<DebugComponentFn>>,
    update_storages: Vec<Box<dyn Fn(&mut HashMap<component::Id, MaskedStorage>)>>,
    storages_changed: HashMap<
        component::Id,
        Box<dyn Fn(&HashMap<component::Id, MaskedStorage>) -> bool>>,
    component_names: HashMap<component::Id, &'static str>,
    component_references: HashMap<component::Id, Box<dyn Fn(&Storages, &Entity) -> Entity>>,
    next_component_mask: NextMask,
}

impl StorageRegistry {
    // Needed to register conditions before the components are registered
    pub fn component_mask<C: 'static>(&mut self) -> MaskType {
        let next_component_mask = &mut self.next_component_mask;
        let pre_register = &mut self.pre_register;
        self.components_mask_index.get(&TypeId::of::<C>())
            .cloned()
            .unwrap_or_else(|| pre_register
                .entry(TypeId::of::<C>())
                .or_insert_with(|| next_component_mask.next())
                .clone())
    }

    // Used internally when registering a compoenent to get it's mask if it was pre-registered
    // or create it if not
    fn next_mask<C: 'static>(&mut self) -> MaskType {
        self.pre_register
            .remove(&TypeId::of::<C>())
            .unwrap_or_else(|| self.next_component_mask.next())
    }

    fn register_storage<'r, C: ComponentSend>(&mut self, component_mask: MaskType)
    where
        for<'s> <C as Component>::Storage: Storage<'s, C>,
        <<C as Component>::Storage as Storage<'r, C>>::Get: DebugParameter,
    {
        self.components_mask_index.insert(C::id(), component_mask.clone());
        self.reverse_components_mask_index.insert(component_mask.clone(), C::id());
        self.remove_components_mask_index.insert(component_mask.clone(), Box::new(move |world, guid|{
            world.storage_mut::<C>()
                .unwrap_or_else(|| panic!("Trying to delete component {} without registering first", C::type_name()))
                .remove(guid);
        }));

        if <C as Component>::Storage::tracks_changes() {
            self.update_storages.push(Box::new(|storages|{
                let storage = &storages[&C::id()];
                let storage: &Lock<<C as Component>::Storage> = storage.storage.downcast_ref().unwrap();
                storage.write().update();
            }));

            self.storages_changed.insert(C::id(), Box::new(|storages|{
                let storage = &storages[&C::id()];
                let storage: &Lock<<C as Component>::Storage> = storage.storage.downcast_ref().unwrap();
                storage.read().storage_changed()
            }));
        }

        self.component_names.insert(C::id(), C::type_name());

        #[cfg(feature = "debug_parameters")]
        self.debug_components_mask_index.insert(C::id(), Box::new(move |world, guid, serializer|{
            let storage = unsafe{ world.storage::<C>().unchecked_unwrap() };
            let c = unsafe { storage.get(guid) };
            c.debug(serializer).map(|_| ())
        }));


        #[cfg(not(feature = "debug_parameters"))]
        self.debug_components_mask_index.insert(C::id(), Box::new(move |world, guid|{
            let storage = unsafe{ world.storage::<C>().unchecked_unwrap() };
            let c = unsafe { storage.get(guid) };
            format!("{:#?}", c)
        }));
    }

    fn register_storage_thread_local<'r, C: Component>(&mut self, component_mask: MaskType)
    where
        for<'s> <C as Component>::Storage: Storage<'s, C>,
        <<C as Component>::Storage as Storage<'r, C>>::Get: DebugParameter,
    {
        self.components_mask_index.insert(C::id(), component_mask.clone());
        self.reverse_components_mask_index.insert(component_mask.clone(), C::id());
        self.remove_components_mask_index.insert(component_mask.clone(), Box::new(move |world, guid|{
            unsafe{
                world.storage_thread_local_mut::<C>()
                    .unchecked_unwrap()
                    .remove(guid);
            }
        }));

        if <C as Component>::Storage::tracks_changes() {
            self.update_storages.push(Box::new(|storages|{
                let storage = &storages[&C::id()];
                let storage: &RefCell<<C as Component>::Storage> = storage.storage.downcast_ref().unwrap();
                storage.borrow_mut().update();
            }));

            self.storages_changed.insert(C::id(), Box::new(|storages|{
                let storage = &storages[&C::id()];
                let storage: &RefCell<<C as Component>::Storage> = storage.storage.downcast_ref().unwrap();
                storage.borrow().storage_changed()
            }));
        }

        self.component_names.insert(C::id(), C::type_name());

        #[cfg(feature = "debug_parameters")]
        self.debug_components_mask_index.insert(C::id(), Box::new(move |world, guid, serializer|{
            let storage = unsafe{ world.storage_thread_local::<C>().unchecked_unwrap() };
            let c = unsafe { storage.get(guid) };
            c.debug(serializer).map(|_| ())
        }));

        #[cfg(not(feature = "debug_parameters"))]
        self.debug_components_mask_index.insert(C::id(), Box::new(move |world, guid|{
            let storage = unsafe{ world.storage_thread_local::<C>().unchecked_unwrap() };
            let c = unsafe { storage.get(guid) };
            format!("{:#?}", c)
        }));
    }

    fn register_tag_storage<C: 'static>(&mut self, component_mask: MaskType) {
        self.components_mask_index.insert(TypeId::of::<C>(), component_mask.clone());
        self.reverse_components_mask_index.insert(component_mask.clone(), TypeId::of::<C>());
        self.component_names.insert(TypeId::of::<C>(), std::any::type_name::<C>());

        #[cfg(feature = "debug_parameters")]
        {
            self.debug_components_mask_index.insert(TypeId::of::<C>(), Box::new(move |_, _, serializer|{
                std::any::type_name::<C>().serialize(serializer).map(|_| ())
            }));
        }

        #[cfg(not(feature = "debug_parameters"))]
        {
            self.debug_components_mask_index.insert(TypeId::of::<C>(), Box::new(move |_, _|{
                format!("{:#?}", std::any::type_name::<C>())
            }));
        }
    }
}


#[doc(hidden)]
#[derive(Default)]
pub struct Storages{
    storages: HashMap<component::Id, MaskedStorage>,
    entities_index_per_mask: SkipMap<(Bitmask, QueryId), Vec<HiddenFastIndex>>,
    ordered_entities_index_per_mask: SkipMap<(component::Id, Bitmask), Vec<HiddenFastIndex>>,
    entities: Vec<(Entity, MaskType)>, // Doesn't need lock cause never accesed mut from Entities?
    resources_index: HashMap<component::Id, MaskType>,

    registry: StorageRegistry,
}

impl Storages{
    pub fn set_resources_index(&mut self, resources_index: HashMap<component::Id, MaskType>){
        self.resources_index = resources_index.clone();
        for (typeid, storage) in self.storages.iter_mut(){
            storage.dependency_mask = resources_index.get(&typeid).cloned().unwrap_or(MaskType::zero());
        }
    }

    pub fn registry(&mut self) -> &mut StorageRegistry {
        &mut self.registry
    }

    pub fn substorages<'a>(&'a self,
        _resource_mask_r: Option<&'a MaskType>,
        _resource_mask_w: Option<&'a MaskType>,
        system_info: &'a str) -> SubStorages
    {
        SubStorages {
            storages: &self,
            #[cfg(any(feature="debug_locks", feature="lockfree"))]
            resource_mask_r: _resource_mask_r.cloned(),
            #[cfg(any(feature="debug_locks", feature="lockfree"))]
            resource_mask_w: _resource_mask_w.cloned(),
            #[cfg(not(any(feature="debug_locks", feature="lockfree")))]
            resource_mask_r: None,
            #[cfg(not(any(feature="debug_locks", feature="lockfree")))]
            resource_mask_w: None,
            system_info,
        }
    }

    pub fn substorages_all(&self) -> SubStorages {
        SubStorages {
            storages: &self,
            resource_mask_r: None,
            resource_mask_w: None,
            system_info: ""
        }
    }

    #[cfg(not(feature = "parallel_systems"))]
    pub fn storage<C: Component>(&self) -> Option<ReadGuardRef<<C as Component>::Storage>> {
        self.masked_storage_thread_local::<C>(None, "")
    }

    #[cfg(not(feature = "parallel_systems"))]
    pub fn storage_mut<C: Component>(&self) -> Option<StorageWriteGuard<C>>
    {
        self.masked_storage_thread_local_mut::<C>(None, "")
    }

    #[cfg(not(feature = "parallel_systems"))]
    pub fn storage_thread_local<C: Component>(&self) -> Option<ReadGuardRef<<C as Component>::Storage>> {
        self.masked_storage_thread_local::<C>(None, "")
    }

    #[cfg(not(feature = "parallel_systems"))]
    pub fn storage_thread_local_mut<C: Component>(&self) -> Option<StorageWriteGuard<C>>
    {
        self.masked_storage_thread_local_mut::<C>(None, "")
    }

    #[cfg(feature = "parallel_systems")]
    pub fn storage<C: Component>(&self) -> Option<ReadGuardRef<<C as Component>::Storage>> {
        self.masked_storage::<C>(None, "")
    }

    #[cfg(feature = "parallel_systems")]
    pub fn storage_mut<C: Component>(&self) -> Option<StorageWriteGuard<C>>
    {
        self.masked_storage_mut::<C>(None, "")
    }

    #[cfg(feature = "parallel_systems")]
    pub fn storage_thread_local<C: Component>(&self) -> Option<ReadGuardRef<<C as Component>::Storage>> {
        self.masked_storage_thread_local::<C>(None, "")
    }

    #[cfg(feature = "parallel_systems")]
    pub fn storage_thread_local_mut<C: Component>(&self) -> Option<StorageWriteGuard<C>>
    {
        self.masked_storage_thread_local_mut::<C>(None, "")
    }

    #[cfg(feature = "parallel_systems")]
    pub fn masked_storage<C: Component>(&self, mask: Option<&MaskType>, system_info: &str) -> Option<ReadGuardRef<<C as Component>::Storage>> {
        self.storages.get(&C::id()).and_then(|s| {
            if let Some(mask) = mask {
                if mask & &s.dependency_mask == MaskType::zero() {
                    panic!("Trying to retrieve storage {} that this system doesn't have read access to. {}",
                        std::any::type_name::<C>(),
                        system_info
                    );
                }
            }
            let s: &Lock<<C as Component>::Storage> = s.storage.downcast_ref().unwrap();
            Some(ReadGuardRef::new(s.read()))
        })
    }

    #[cfg(feature = "parallel_systems")]
    pub fn masked_storage_mut<C: Component>(&self, mask: Option<&MaskType>, system_info: &str) -> Option<StorageWriteGuard<C>>
    {
        self.storages.get(&C::id()).and_then(|s| {
            if let Some(mask) = mask {
                if mask & &s.dependency_mask == MaskType::zero() {
                    panic!("Trying to retrieve storage {} that this system doesn't have write access to. {}",
                        std::any::type_name::<C>(),
                        system_info
                    );
                }
            }
            let storage: &Lock<<C as Component>::Storage> = s.storage.downcast_ref().unwrap();

            Some(WriteGuardRef::with_cache_guard(
                storage.write(),
                &self.entities_index_per_mask,
                &s.component_mask,
            ))
        })
    }

    #[cfg(feature = "parallel_systems")]
    pub fn masked_storage_thread_local<C: Component>(&self, mask: Option<&MaskType>, system_info: &str) -> Option<ReadGuardRef<<C as Component>::Storage>> {
        let local = self.storages.get(&C::id()).and_then(|s| {
            #[cfg(feature="lockfree")]
            if let Some(mask) = mask {
                if mask & &s.dependency_mask == MaskType::zero() {
                    panic!("Trying to retrieve storage {} that this system doesn't have read access to. {}",
                        std::any::type_name::<C>(),
                        system_info
                    );
                }
            }
            let s: &RefCell<<C as Component>::Storage> = s.storage.downcast_ref()?;
            Some(ReadGuard::ThreadLocal(s.borrow()))
        });
        if local.is_some(){
            local.map(|local| ReadGuardRef::new(local))
        }else{
            self.masked_storage::<C>(mask, system_info)
        }
    }

    #[cfg(feature = "parallel_systems")]
    pub fn masked_storage_thread_local_mut<C: Component>(&self, mask: Option<&MaskType>, system_info: &str) -> Option<StorageWriteGuard<C>>
    {
        let local = self.storages.get(&C::id()).and_then(|s| {
            if let Some(mask) = mask {
                if mask & &s.dependency_mask == MaskType::zero() {
                    panic!("Trying to retrieve storage {} that this system doesn't have write access to. {}",
                        std::any::type_name::<C>(),
                        system_info
                    );
                }
            }
            let storage: &RefCell<<C as Component>::Storage> = s.storage.downcast_ref()?;
            let local = WriteGuard::ThreadLocal(storage.borrow_mut());

            Some(WriteGuardRef::with_cache_guard(
                local,
                &self.entities_index_per_mask,
                &s.component_mask,
            ))
        });

        local.or_else(|| self.masked_storage_mut::<C>(mask, system_info))
    }

    #[cfg(not(feature = "parallel_systems"))]
    pub fn masked_storage<C: Component>(&self, mask: Option<&MaskType>, system_info: &str) -> Option<ReadGuardRef<<C as Component>::Storage>> {
        self.masked_storage_thread_local::<C>(mask, system_info)
    }

    #[cfg(not(feature = "parallel_systems"))]
    pub fn masked_storage_mut<C: Component>(&self, mask: Option<&MaskType>, system_info: &str) -> Option<StorageWriteGuard<C>> {
        self.masked_storage_thread_local_mut::<C>(mask, system_info)
    }

    #[cfg(not(feature = "parallel_systems"))]
    pub fn masked_storage_thread_local<C: Component>(&self, mask: Option<&MaskType>, system_info: &str) -> Option<ReadGuardRef<<C as Component>::Storage>> {
        self.storages.get(&C::id()).and_then(|s| {
            if let Some(mask) = mask {
                if mask & &s.dependency_mask == MaskType::zero() {
                    panic!("Trying to retrieve storage {} that this system doesn't have write access to. {}",
                        std::any::type_name::<C>(),
                        system_info
                    );
                }
            }
            let s: &RefCell<<C as Component>::Storage> = s.storage.downcast_ref().unwrap();
            let s = ReadGuard::ThreadLocal(s.borrow());
            Some(ReadGuardRef::new(s))
        })
    }

    #[cfg(not(feature = "parallel_systems"))]
    pub fn masked_storage_thread_local_mut<C: Component>(&self, mask: Option<&MaskType>, system_info: &str) -> Option<StorageWriteGuard<C>> {
        self.storages.get(&C::id()).and_then(|s| {
            if let Some(mask) = mask {
                if mask & &s.dependency_mask == MaskType::zero() {
                    panic!("Trying to retrieve storage {} that this system doesn't have write access to. {}",
                        std::any::type_name::<C>(),
                        system_info
                    );
                }
            }

            let storage: &RefCell<<C as Component>::Storage> = s.storage.downcast_ref().unwrap();
            let storage = WriteGuard::ThreadLocal(storage.borrow_mut());

            Some(WriteGuardRef::with_cache_guard(
                storage,
                unsafe{ &mut *self.entities_index_per_mask.get() },
                &s.component_mask,
            ))
        })
    }

    #[cfg(feature = "parallel_systems")]
    pub fn update_changed<'a, C: Component>(&self, mask: Option<&MaskType>, system_info: &str)
    where <C as Component>::Storage: AutoChangedStorageExt<'a>
    {
        if let Some(s) = self.storages.get(&C::id()) {
            if let Some(mask) = mask {
                if mask & &s.dependency_mask == MaskType::zero() {
                    panic!("Trying to retrieve storage {} that this system doesn't have write access to. {}",
                        std::any::type_name::<C>(),
                        system_info
                    );
                }
            }
            let storage: &Lock<<C as Component>::Storage> = s.storage.downcast_ref().unwrap();
            storage.write().update_changed();
        }
    }

    #[cfg(not(feature = "parallel_systems"))]
    pub fn update_changed<'a, C: Component>(&self, mask: Option<&MaskType>, system_info: &str)
    where <C as Component>::Storage: AutoChangedStorageExt<'a>
    {
        self.update_changed_thread_local::<C>(mask, system_info)
    }

    #[cfg(feature = "parallel_systems")]
    pub fn update_changed_thread_local<'a, C: Component>(&self, mask: Option<&MaskType>, system_info: &str)
    where <C as Component>::Storage: AutoChangedStorageExt<'a>
    {
        if let Some(s) = self.storages.get(&C::id()) {
            if let Some(mask) = mask {
                if mask & &s.dependency_mask == MaskType::zero() {
                    panic!("Trying to retrieve storage {} that this system doesn't have write access to. {}",
                        std::any::type_name::<C>(),
                        system_info
                    );
                }
            }
            let storage: Option<&RefCell<<C as Component>::Storage>> = s.storage.downcast_ref();
            if let Some(storage) = storage {
                storage.borrow_mut().update_changed();
            }else{
                let storage: &Lock<<C as Component>::Storage> = s.storage.downcast_ref().unwrap();
                storage.write().update_changed();
            }
        }
    }

    #[cfg(not(feature = "parallel_systems"))]
    pub fn update_changed_thread_local<'a, C: Component>(&self, mask: Option<&MaskType>, system_info: &str)
    where <C as Component>::Storage: AutoChangedStorageExt<'a>
    {
        if let Some(s) = self.storages.get(&C::id()) {
            if let Some(mask) = mask {
                if mask & &s.dependency_mask == MaskType::zero() {
                    panic!("Trying to retrieve storage {} that this system doesn't have write access to. {}",
                        std::any::type_name::<C>(),
                        system_info
                    );
                }
            }
            let storage: &RefCell<<C as Component>::Storage> = s.storage.downcast_ref().unwrap();
            storage.borrow_mut().update_changed();
        }
    }

    #[cfg(feature = "parallel_systems")]
    pub fn register<'r, C: ComponentSend>(&mut self, capacity: Option<usize>)
    where
        for<'s> <C as Component>::Storage: Storage<'s, C>,
        <<C as Component>::Storage as Storage<'r, C>>::Get: DebugParameter,
    {
        if self.storages.get(&C::id()).is_some(){
            panic!("{} already registered or not unique component id", C::type_name());
        }
        let storage = if let Some(capacity) = capacity {
            Box::new(Lock::new(<C as Component>::Storage::with_capacity(capacity))) as Box<dyn Any>
        }else{
            Box::new(Lock::new(<C as Component>::Storage::new())) as Box<dyn Any>
        };
        let next_mask = self.registry.next_mask::<C>();
        self.storages.insert(C::id(), MaskedStorage{
            dependency_mask: self.resources_index.get(&C::id()).cloned().unwrap_or(MaskType::zero()),
            component_mask: next_mask.clone(),
            storage
        });
        self.registry.register_storage::<C>(next_mask);
    }

    pub fn register_thread_local<'r, C: ComponentThreadLocal>(&mut self, capacity: Option<usize>)
    where
        for<'s> <C as Component>::Storage: Storage<'s, C>,
        <<C as Component>::Storage as Storage<'r, C>>::Get: DebugParameter,
    {
        let storage = if let Some(capacity) = capacity {
            Box::new(RefCell::new(<C as Component>::Storage::with_capacity(capacity))) as Box<dyn Any>
        }else{
            Box::new(RefCell::new(<C as Component>::Storage::new())) as Box<dyn Any>
        };
        if self.storages.get(&C::id()).is_some(){
            panic!("{} already registered or not unique component id", C::type_name());
        }
        let next_mask = self.registry.next_mask::<C>();
        self.storages.insert(C::id(), MaskedStorage{
            dependency_mask: self.resources_index.get(&C::id()).cloned().unwrap_or(MaskType::zero()),
            component_mask: next_mask.clone(),
            storage
        });
        self.registry.register_storage_thread_local::<C>(next_mask);
    }

    #[cfg(not(feature = "parallel_systems"))]
    pub fn register<'r, C: Component>(&mut self, capacity: Option<usize>)
    where
        for<'s> <C as Component>::Storage: Storage<'s, C>,
        <<C as Component>::Storage as Storage<'r, C>>::Get: DebugParameter,
    {
        self.register_thread_local::<C>(capacity)
    }

    pub fn register_reference_thread_local<'a, C: NToOneComponent<'a>>(&mut self)
    where
        for<'s> <C as Component>::Storage: Storage<'s, C>,
        <<C as Component>::Storage as Storage<'a, C>>::Get: DebugParameter,
    {
        if self.storages.get(&C::id()).is_some(){
            panic!("{} already registered or not unique component id", C::type_name());
        }
        let storage = Box::new(RefCell::new(<C as Component>::Storage::new())) as Box<dyn Any>;
        let next_mask = self.registry.next_mask::<C>();
        self.storages.insert(C::id(), MaskedStorage{
            dependency_mask: self.resources_index.get(&C::id()).cloned().unwrap_or(MaskType::zero()),
            component_mask: next_mask.clone(),
            storage
        });
        self.registry.register_storage_thread_local::<C>(next_mask);
        self.registry.component_references.insert(C::id(), C::references_other().unwrap());
    }

    #[cfg(feature = "parallel_systems")]
    pub fn register_reference<'a, C: NToOneComponent<'a> + Send>(&mut self)
    where
        for<'s> <C as Component>::Storage: Storage<'s, C>,
        <<C as Component>::Storage as Storage<'a, C>>::Get: DebugParameter,
    {
        if self.storages.get(&C::id()).is_some(){
            panic!("{} already registered or not unique component id", C::type_name());
        }
        let storage = Box::new(Lock::new(<C as Component>::Storage::new())) as Box<dyn Any>;
        let next_mask = self.registry.next_mask::<C>();
        self.storages.insert(C::id(), MaskedStorage{
            dependency_mask: self.resources_index.get(&C::id()).cloned().unwrap_or(MaskType::zero()),
            component_mask: next_mask.clone(),
            storage
        });
        self.registry.register_storage::<C>(next_mask);
        self.registry.component_references.insert(C::id(), C::references_other().unwrap());
    }

    #[cfg(not(feature = "parallel_systems"))]
    pub fn register_reference<'a, C: NToOneComponent<'a>>(&mut self)
    where
        for<'s> <C as Component>::Storage: Storage<'s, C>,
        <<C as Component>::Storage as Storage<'a, C>>::Get: DebugParameter,
    {
        self.register_reference_thread_local::<C>()
    }

    pub fn register_tag<C: 'static>(&mut self) {
        let next_mask = self.registry.next_mask::<C>();
        self.storages.insert(TypeId::of::<C>(), MaskedStorage {
            dependency_mask: MaskType::zero(),
            component_mask: next_mask.clone(),
            storage: Box::new(())
        });
        self.registry.register_tag_storage::<C>(next_mask);
    }

    // pub fn components_mask_index(&self) -> &HashMap<component::Id, MaskType> {
    //     &self.components_mask_index
    // }

    #[cfg(not(feature = "debug_parameters"))]
    pub fn debug_components_mask_index(&self, entity: &Entity, id: &component::Id) -> String {
        self.registry.debug_components_mask_index[id](self, entity.guid())
    }

    // fn clear_entities_per_mask_index(&mut self){
    //     unsafe{ (*self.entities_index_per_mask.get()).clear() };
    // }

    // fn clear_ordered_entities_per_mask_index(&mut self){
    //     unsafe{ (*self.ordered_entities_index_per_mask.get()).clear() };
    // }

    #[cfg(feature = "parallel_systems")]
    fn entities_index_per_mask(&self) -> &SkipMap<(Bitmask, TypeId), Vec<HiddenFastIndex>>{
        &self.entities_index_per_mask
    }

    #[cfg(not(feature = "parallel_systems"))]
    fn entities_index_per_mask(&self) -> &mut HashMap<(Bitmask, TypeId), Vec<HiddenFastIndex>>{
        unsafe{ &mut *self.entities_index_per_mask.get() }
    }

    #[cfg(feature = "parallel_systems")]
    fn ordered_entities_index_per_mask(&self) -> &SkipMap<(component::Id, Bitmask), Vec<HiddenFastIndex>>{
        &self.ordered_entities_index_per_mask
    }

    #[cfg(not(feature = "parallel_systems"))]
    fn ordered_entities_index_per_mask(&self) -> &mut HashMap<(component::Id, Bitmask), Vec<HiddenFastIndex>>{
        unsafe{ &mut *self.ordered_entities_index_per_mask.get() }
    }

    #[cfg(feature = "parallel_systems")]
    pub fn refresh_indices_component_changed(
        &self,
        entity_mask: &MaskType,
        prev_entity_mask: &MaskType)
    {
        if let Some(mut entry) = self.entities_index_per_mask().front(){
            loop {
                let (mask, _storage_id) = entry.key();
                let components_mask = &mask.components();
                if entity_mask & components_mask != prev_entity_mask & components_mask {
                    entry.remove();
                }
                if !entry.move_next(){
                    break;
                }
            }
        }
        if let Some(mut entry) = self.ordered_entities_index_per_mask().front(){
            loop {
                let (_, mask) = entry.key();
                let components_mask = &mask.components();
                if entity_mask & components_mask != prev_entity_mask & components_mask {
                    entry.remove();
                }
                if !entry.move_next(){
                    break;
                }
            }
        }
    }

    #[cfg(feature = "parallel_systems")]
    pub fn refresh_indices_entity_changed(
        &self,
        entity_mask: &MaskType)
    {
        if let Some(mut entry) = self.entities_index_per_mask().front(){
            loop {
                let (mask, _storage_id) = entry.key();
                if entity_mask & mask.components() != MaskType::zero() {
                    entry.remove();
                }
                if !entry.move_next(){
                    break;
                }
            }
        }
        if let Some(mut entry) = self.ordered_entities_index_per_mask().front(){
            loop {
                let (_, mask) = entry.key();
                if entity_mask & mask.components() != MaskType::zero() {
                    entry.remove();
                }
                if !entry.move_next(){
                    break;
                }
            }
        }
    }

    #[cfg(not(feature = "parallel_systems"))]
    pub fn refresh_indices_component_changed(
        &self,
        entity_mask: &MaskType,
        prev_entity_mask: &MaskType)
    {
        self.entities_index_per_mask().retain(|(mask, _), _|{
            let components_mask = &mask.components();
            entity_mask & components_mask == prev_entity_mask & components_mask
        });
        self.ordered_entities_index_per_mask().retain(|(_,mask), _|{
            let components_mask = &mask.components();
            entity_mask & components_mask == prev_entity_mask & components_mask
        })
    }

    #[cfg(not(feature = "parallel_systems"))]
    pub fn refresh_indices_entity_changed(
        &self,
        entity_mask: &MaskType)
    {
        self.entities_index_per_mask().retain(|(mask, _), _|{
            entity_mask & mask.components() == MaskType::zero()
        });
        self.ordered_entities_index_per_mask().retain(|(_,mask), _|{
            entity_mask & mask.components() == MaskType::zero()
        });
    }

    pub fn creation_storage<C: ComponentThreadLocal>(&mut self) -> CreationSto<C>{
        #[cfg(feature = "parallel_systems")]
        let storage = self.storages.get(&C::id()).map(|s| {
            let storage: Option<&Lock<<C as Component>::Storage>> = s.storage.downcast_ref();
            if let Some(storage) = storage {
                WriteGuardRef::new(storage.write())
            }else{
                let storage: &RefCell<<C as Component>::Storage> = s.storage.downcast_ref().unwrap();
                WriteGuardRef::new(WriteGuard::ThreadLocal(storage.borrow_mut()))
            }
        })
        .unwrap_or_else(|| panic!("Trying to add component of type {} without registering first", C::type_name()));


        #[cfg(not(feature = "parallel_systems"))]
        let storage = self.storages.get(&C::id()).map(|s| {
            let storage: &RefCell<<C as Component>::Storage> = s.storage.downcast_ref().unwrap();
            WriteGuardRef::new(WriteGuard::ThreadLocal(storage.borrow_mut()))
        })
        .unwrap_or_else(|| panic!("Trying to add component of type {} without registering first", C::type_name()));

        CreationSto{
            storage,
            component_mask: self.storages[&C::id()].component_mask.clone(),
            storages: self.substorages_all(),
        }
    }

    pub fn storage_or_insert<'r, C: ComponentSend>(&mut self) -> &mut MaskedStorage
    where
        for<'s> <C as Component>::Storage: Storage<'s, C>,
        <<C as Component>::Storage as Storage<'r, C>>::Get: DebugParameter,
    {
        match self.storages.entry(C::id()) {
            Entry::Occupied(entry) => {
                entry.into_mut()
            }

            Entry::Vacant(vacant) => {
                let storage = Box::new(Lock::new(<C as Component>::Storage::new())) as Box<dyn Any>;
                let next_mask = self.registry.next_mask::<C>();
                let s = vacant.insert(MaskedStorage{
                    dependency_mask: self.resources_index.get(&C::id()).cloned().unwrap_or(MaskType::zero()),
                    component_mask: next_mask.clone(),
                    storage
                });
                self.registry.register_storage::<C>(next_mask);
                s
            }
        }
    }

    pub fn storage_thread_local_or_insert<'r, C: Component>(&mut self) -> &mut MaskedStorage
    where
        for<'s> <C as Component>::Storage: Storage<'s, C>,
        <<C as Component>::Storage as Storage<'r, C>>::Get: DebugParameter,
    {
        match self.storages.entry(C::id()) {
            Entry::Occupied(entry) => {
                entry.into_mut()
            }

            Entry::Vacant(vacant) => {
                let storage = Box::new(RefCell::new(<C as Component>::Storage::new())) as Box<dyn Any>;
                let next_mask = self.registry.next_mask::<C>();
                let s = vacant.insert(MaskedStorage{
                    dependency_mask: self.resources_index.get(&C::id()).cloned().unwrap_or(MaskType::zero()),
                    component_mask: next_mask.clone(),
                    storage
                });
                self.registry.register_storage_thread_local::<C>(next_mask);
                s
            }
        }
    }

    pub fn ref_storage_or_insert<'r, C: ComponentSend>(&mut self) -> &mut MaskedStorage
    where
        for<'s> <C as Component>::Storage: Storage<'s, C>,
        <<C as Component>::Storage as Storage<'r, C>>::Get: DebugParameter,
    {
        match self.storages.entry(C::id()) {
            Entry::Occupied(entry) => {
                entry.into_mut()
            }

            Entry::Vacant(vacant) => {
                let storage = Box::new(Lock::new(<C as Component>::Storage::new())) as Box<dyn Any>;
                let next_mask = self.registry.next_mask::<C>();
                let s = vacant.insert(MaskedStorage{
                    dependency_mask: self.resources_index.get(&C::id()).cloned().unwrap_or(MaskType::zero()),
                    component_mask: next_mask.clone(),
                    storage
                });
                self.registry.register_storage::<C>(next_mask);
                self.registry.component_references.insert(C::id(), C::references_other().unwrap());
                s
            }
        }
    }

    pub fn ref_storage_thread_local_or_insert<'r, C: Component>(&mut self) -> &mut MaskedStorage
    where
        for<'s> <C as Component>::Storage: Storage<'s, C>,
        <<C as Component>::Storage as Storage<'r, C>>::Get: DebugParameter,
    {
        match self.storages.entry(C::id()) {
            Entry::Occupied(entry) => {
                entry.into_mut()
            }

            Entry::Vacant(vacant) => {
                let storage = Box::new(RefCell::new(<C as Component>::Storage::new())) as Box<dyn Any>;
                let next_mask = self.registry.next_mask::<C>();
                let s = vacant.insert(MaskedStorage{
                    dependency_mask: self.resources_index.get(&C::id()).cloned().unwrap_or(MaskType::zero()),
                    component_mask: next_mask.clone(),
                    storage
                });
                self.registry.register_storage_thread_local::<C>(next_mask);
                self.registry.component_references.insert(C::id(), C::references_other().unwrap());
                s
            }
        }
    }

    pub fn tag_storage_or_insert<C: 'static>(&mut self) -> &mut MaskedStorage {
        match self.storages.entry(TypeId::of::<C>()) {
            Entry::Occupied(entry) => {
                entry.into_mut()
            }

            Entry::Vacant(vacant) => {
                let next_mask = self.registry.next_mask::<C>();
                let s = vacant.insert(MaskedStorage {
                    dependency_mask: MaskType::zero(),
                    component_mask: next_mask.clone(),
                    storage: Box::new(())
                });
                self.registry.register_tag_storage::<C>(next_mask);

                s
            }
        }
    }

    #[cfg(feature="parallel_systems")]
    pub fn add_component_to_guid<'r, C: ComponentSend>(&mut self, guid: usize, component: C) -> MaskType
    where
        for<'s> <C as Component>::Storage: Storage<'s, C>,
        <<C as Component>::Storage as Storage<'r, C>>::Get: DebugParameter,
    {
        let s = if C::is_reference() {
            self.ref_storage_or_insert::<C>()
        }else{
            self.storage_or_insert::<C>()
        };
        let storage: Option<&Lock<<C as Component>::Storage>> = s.storage.downcast_ref();
        if let Some(storage) = storage {
            storage.write().insert(guid, component);
            s.component_mask.clone()
        }else{
            panic!("Trying to add component {} as Send but it's registered as thread local", C::type_name())
        }
    }

    #[cfg(not(feature="parallel_systems"))]
    pub fn add_component_to_guid<'r, C: ComponentSend>(&mut self, guid: usize, component: C) -> MaskType
    where
        for<'s> <C as Component>::Storage: Storage<'s, C>,
        <<C as Component>::Storage as Storage<'r, C>>::Get: DebugParameter,
    {
        self.add_component_thread_local_to_guid(guid, component)
    }

    pub fn add_component_to<'r, C: ComponentSend>(&mut self, entity: &Entity, component: C)
    where
        for<'s> <C as Component>::Storage: Storage<'s, C>,
        <<C as Component>::Storage as Storage<'r, C>>::Get: DebugParameter,
    {
        let component_mask = self.add_component_to_guid::<C>(entity.guid(), component);
        let (_entity, mask) = &mut self.entities[entity.guid()];
        let prev_entity_mask = mask.clone();
        *mask |= &component_mask;
        let entity_mask = mask.clone();

        if entity_mask != prev_entity_mask {
            self.refresh_indices_component_changed(&entity_mask, &prev_entity_mask);
        }
    }

    pub fn add_component_thread_local_to_guid<'r, C: ComponentThreadLocal>(&mut self, guid: usize, component: C) -> MaskType
    where
        for<'s> <C as Component>::Storage: Storage<'s, C>,
        <<C as Component>::Storage as Storage<'r, C>>::Get: DebugParameter,
    {
        let s = if C::is_reference() {
            self.ref_storage_thread_local_or_insert::<C>()
        }else{
            self.storage_thread_local_or_insert::<C>()
        };
        let storage: Option<&RefCell<<C as Component>::Storage>> = s.storage.downcast_ref();
        if let Some(storage) = storage {
            storage.borrow_mut().insert(guid, component);
            s.component_mask.clone()
        }else{
            panic!("Trying to add component {} as thread local but it's registered as Send", C::type_name())
        }
    }

    pub fn add_component_to_thread_local<'r, C: ComponentThreadLocal>(&mut self, entity: &Entity, component: C)
    where
        for<'s> <C as Component>::Storage: Storage<'s, C>,
        <<C as Component>::Storage as Storage<'r, C>>::Get: DebugParameter,
    {
        let component_mask = self.add_component_thread_local_to_guid::<C>(entity.guid(), component);
        let (_entity, mask) = &mut self.entities[entity.guid()];
        let prev_entity_mask = mask.clone();
        *mask |= &component_mask;
        let entity_mask = mask.clone();

        if entity_mask != prev_entity_mask {
            self.refresh_indices_component_changed(&entity_mask, &prev_entity_mask);
        }
    }

    pub fn add_tag_to<C: 'static>(&mut self, entity: &Entity){
        let component_mask = self.tag_storage_or_insert::<C>().component_mask.clone();
        let (_entity, mask) = &mut self.entities[entity.guid()];
        let prev_entity_mask = mask.clone();
        *mask |= &component_mask;
        let entity_mask = mask.clone();
        if entity_mask != prev_entity_mask {
            self.refresh_indices_component_changed(&entity_mask, &prev_entity_mask);
        }
    }

    #[cfg(feature="parallel_systems")]
    pub fn add_child_component_to_guid<'r, C: ComponentSend>(
        &mut self,
        parent: &Entity,
        guid: usize,
        component: C) -> MaskType
    where
        for<'b> <C as Component>::Storage: HierarchicalStorage<'b,C>,
        <<C as Component>::Storage as Storage<'r, C>>::Get: DebugParameter,
    {
        let s = self.storage_or_insert::<C>();
        let storage: &Lock<<C as Component>::Storage> = s.storage.downcast_ref().unwrap();
        // TODO this shouldn't be unsafe, check in storage or here that parent exists?
        unsafe{
            storage.write().insert_child(parent.guid(), guid, component);
        }
        s.component_mask.clone()
    }

    #[cfg(not(feature="parallel_systems"))]
    pub fn add_child_component_to_guid<'r, C: ComponentSend>(
        &mut self,
        parent: &Entity,
        guid: usize,
        component: C) -> MaskType
    where
        for<'b> <C as Component>::Storage: HierarchicalStorage<'b,C>,
        <<C as Component>::Storage as Storage<'r, C>>::Get: DebugParameter,
    {
        self.add_child_component_thread_local_to_guid(parent, guid, component)
    }

    pub fn add_child_component_to<'r, C: ComponentSend>(
        &mut self,
        parent: &Entity,
        entity: &Entity,
        component: C)
    where
        for<'b> <C as Component>::Storage: HierarchicalStorage<'b,C>,
        <<C as Component>::Storage as Storage<'r, C>>::Get: DebugParameter,
    {
        let component_mask = self.add_child_component_to_guid::<C>(parent, entity.guid(), component);
        let &mut (_entity, ref mut mask) = &mut self.entities[entity.guid()];
        let prev_entity_mask = mask.clone();
        *mask |= &component_mask;
        let entity_mask = mask.clone();

        if entity_mask != prev_entity_mask {
            self.refresh_indices_component_changed(&entity_mask, &prev_entity_mask);
        }

        // self.clear_entities_per_mask_index();
        // self.clear_ordered_entities_per_mask_index();
    }

    pub fn add_child_component_thread_local_to_guid<'r, C: Component>(
        &mut self,
        parent: &Entity,
        guid: usize,
        component: C) -> MaskType
    where
        for<'b> <C as Component>::Storage: HierarchicalStorage<'b, C>,
        <<C as Component>::Storage as Storage<'r, C>>::Get: DebugParameter,
    {
        let s = self.storage_thread_local_or_insert::<C>();
        let storage: &RefCell<<C as Component>::Storage> = s.storage.downcast_ref().unwrap();
        // TODO this shouldn't be unsafe, check in storage or here that parent exists?
        unsafe{
            storage.borrow_mut().insert_child(parent.guid(), guid, component);
        }
        s.component_mask.clone()
    }

    pub fn add_child_component_to_thread_local<'r, C: ComponentThreadLocal>(
        &mut self,
        parent: &Entity,
        entity: &Entity,
        component: C)
    where
        for<'b> <C as Component>::Storage: HierarchicalStorage<'b, C>,
        <<C as Component>::Storage as Storage<'r, C>>::Get: DebugParameter,
    {
        let component_mask = self.add_child_component_thread_local_to_guid::<C>(parent, entity.guid(), component);
        let &mut (_entity, ref mut mask) = &mut self.entities[entity.guid()];
        let prev_entity_mask = mask.clone();
        *mask |= &component_mask;
        let entity_mask = mask.clone();

        if entity_mask != prev_entity_mask {
            self.refresh_indices_component_changed(&entity_mask, &prev_entity_mask);
        }

        // self.clear_entities_per_mask_index();
        // self.clear_ordered_entities_per_mask_index();
    }

    #[cfg(feature="parallel_systems")]
    pub fn add_slice_component_to_guid<'r, C, I>(&mut self, guid: usize, component: I) -> MaskType
    where
        C: OneToNComponentSend,
        for<'s> <C as Component>::Storage: OneToNStorage<'s, C>,
        <<C as Component>::Storage as Storage<'r, C>>::Get: DebugParameter,
        I: IntoIterator<Item = C>
    {
        let s = self.storage_or_insert::<C>();
        let storage: &Lock<<C as Component>::Storage> = s.storage.downcast_ref().unwrap();
        storage.write().insert_slice(guid, component);
        s.component_mask.clone()
    }

    #[cfg(not(feature="parallel_systems"))]
    pub fn add_slice_component_to_guid<'r, C, I>(&mut self, guid: usize, component: I) -> MaskType
    where
        C: OneToNComponentSend,
        for<'s> <C as Component>::Storage: OneToNStorage<'s, C>,
        <<C as Component>::Storage as Storage<'r, C>>::Get: DebugParameter,
        I: IntoIterator<Item = C>
    {
        self.add_slice_component_thread_local_to_guid(guid, component)
    }

    pub fn add_slice_component_to<'r, C, I>(&mut self, entity: &Entity, component: I)
    where
        C: OneToNComponentSend,
        for<'s> <C as Component>::Storage: OneToNStorage<'s, C>,
        <<C as Component>::Storage as Storage<'r, C>>::Get: DebugParameter,
        I: IntoIterator<Item = C>
    {
        let component_mask = self.add_slice_component_to_guid(entity.guid(), component);
        let &mut (_entity, ref mut mask) = &mut self.entities[entity.guid()];
        let prev_entity_mask = mask.clone();
        *mask |= &component_mask;
        let entity_mask = mask.clone();
        if entity_mask != prev_entity_mask {
            self.refresh_indices_component_changed(&entity_mask, &prev_entity_mask);
        }
    }

    pub fn add_slice_component_thread_local_to_guid<'r, C, I>(&mut self, guid: usize, component: I) -> MaskType
    where
        C: OneToNComponentThreadLocal,
        for<'s> <C as Component>::Storage: OneToNStorage<'s, C>,
        <<C as Component>::Storage as Storage<'r, C>>::Get: DebugParameter,
        I: IntoIterator<Item = C>
    {
        let s = self.storage_thread_local_or_insert::<C>();
        let storage: &RefCell<<C as Component>::Storage> = s.storage.downcast_ref().unwrap();
        storage.borrow_mut().insert_slice(guid, component);
        s.component_mask.clone()
    }

    pub fn add_slice_component_to_thread_local<'r, C, I>(&mut self, entity: &Entity, component: I)
    where
        C: OneToNComponentThreadLocal,
        for<'s> <C as Component>::Storage: OneToNStorage<'s, C>,
        <<C as Component>::Storage as Storage<'r, C>>::Get: DebugParameter,
        I: IntoIterator<Item = C>
    {
        let component_mask = self.add_slice_component_thread_local_to_guid(entity.guid(), component);
        let &mut (_entity, ref mut mask) = &mut self.entities[entity.guid()];
        let prev_entity_mask = mask.clone();
        *mask |= &component_mask;
        let entity_mask = mask.clone();
        if entity_mask != prev_entity_mask {
            self.refresh_indices_component_changed(&entity_mask, &prev_entity_mask);
        }
    }

    pub fn remove_component_from<'r, C: Component>(&mut self, entity: &Entity)
    where
        for<'s> <C as Component>::Storage: Storage<'s, C>,
        <<C as Component>::Storage as Storage<'r, C>>::Get: DebugParameter,
    {
        let component_mask = {
            let s = unwrap_or_return!(self.storages.get(&C::id()));

            #[cfg(feature = "parallel_systems")]
            {
                let storage: Option<&Lock<<C as Component>::Storage>> = s.storage.downcast_ref();
                if let Some(storage) = storage{
                    storage.write().remove(entity.guid());
                }else{
                    let storage: &RefCell<<C as Component>::Storage> = s.storage.downcast_ref().unwrap();
                    storage.borrow_mut().remove(entity.guid());
                }
            }

            #[cfg(not(feature = "parallel_systems"))]
            {
                let storage: &RefCell<<C as Component>::Storage> = s.storage.downcast_ref().unwrap();
                storage.borrow_mut().remove(entity.guid());
            }

            s.component_mask.clone()
        };
        let (_entity, mask) = &mut self.entities[entity.guid()];
        let prev_entity_mask = mask.clone();
        *mask ^= &component_mask;
        let entity_mask = mask.clone();

        if entity_mask != prev_entity_mask {
            self.refresh_indices_component_changed(&entity_mask, &prev_entity_mask);
        }

    }

    pub fn remove_entity(&mut self, entity: &Entity){
        let entity_mask = self.entities[entity.guid()].1.clone();
        self.refresh_indices_entity_changed(&entity_mask);

        let mut component_mask = bitmask::one();
        while component_mask < self.registry.next_component_mask.get(){
            if &entity_mask & &component_mask == component_mask{
                // let storage = &self.storages[&type_id];
                if let Some(remove_components) = self.registry.remove_components_mask_index.get(&component_mask){
                    let remove_component = unsafe{
                        delete_lifetime(remove_components)
                    };
                    remove_component(self, entity.guid());
                }
            }
            component_mask *= bitmask::two();
        }


        // TODO: can't remove entities since we rely on order for fast entitty search
        // mostly on ordered_ids_for. others are add / remove component which could be slower
        // without problem
        // Use DenseVec for entities storage?
        // self.entities.remove(pos);
        // self.masks.remove(pos)
        self.entities[entity.guid()] = (Entity{guid: std::usize::MAX}, bitmask::zero());
    }

    pub fn push_entity(&mut self, e: Entity, entity_mask: MaskType){
        self.entities.push((e, entity_mask.clone()));
        self.refresh_indices_entity_changed(&entity_mask);
    }

    pub fn component_mask<C: 'static>(&self) -> MaskType{
        self.mask_for_type_id(TypeId::of::<C>())
    }

    fn mask_for_type_id(&self, ty: TypeId) -> MaskType {
        self.storages.get(&ty)
            .map(|storage| storage.component_mask.clone())
        //     .unwrap_or_else(|| panic!("Trying to use component {} before registering", std::any::type_name::<C>()))
            .unwrap_or_else(|| MaskType::zero())
    }

    pub fn entities_ref(&self) -> &[(Entity, MaskType)]{
        &self.entities
    }

    pub(crate) fn check(&self, entity: &Entity, entity_mask: &MaskType, mask: &Bitmask) -> bool {
        match mask.check(entity_mask.clone()) {
            bitmask::Check::Yes => true,
            bitmask::Check::No => false,
            bitmask::Check::Refs(refs) => {
                refs.iter().all(|(storage, bitmask)| {
                    let check = &self.registry.component_references.get(&storage).unwrap_or_else(||{
                            panic!("Can't find reference component")
                    });
                    let ref_entity = check(self, entity);
                    let ref_mask = &self.entities[ref_entity.guid()].1;
                    self.check(&ref_entity, ref_mask, bitmask)
                })
            }
        }
    }

    // fn entities_ids<'a, 's, T, S: StorageRef<'s, T> + 'a>(&'a self, mask: &'a Bitmask, storage: &S) -> impl Iterator<Item = FastIndex> + 'a{
    //     self.entities.iter().filter_map(move |(e, entity_mask)| {
    //         if self.check(e, &entity_mask, mask) {
    //             Some(storage.fast_index(e.guid()))
    //         }else{
    //             None
    //         }
    //     })
    // }

    // Hack to check for system conditions, we use Self as storage id since the query is not
    // associated with any storage and store the guid instead of any fast ids.
    // Since we only ever want to query if there's at least one entity with that mask
    // we just find the first one and store it for future use
    #[cfg(feature = "parallel_systems")]
    pub fn any_has_mask(&self, mask: Bitmask) -> bool {
        let storage_id = std::any::TypeId::of::<Self>();
        let key = (mask.clone(), storage_id);
        let index = if let Some(index) = self.entities_index_per_mask.get(&key){
            index
        }else{
            let entities_ids = self.entities.iter().filter_map(|(e, entity_mask)| {
                if self.check(e, &entity_mask, &mask) {
                    Some(HiddenFastIndex::new(e.guid()))
                }else{
                    None
                }
            }).next().into_iter().collect();
            self.entities_index_per_mask.insert(key.clone(), entities_ids)
        };

        !index.value().is_empty()
    }

    // Hack to check for system conditions, we use Self as storage id since the query is not
    // associated with any storage and store the guid instead of any fast ids.
    // Since we only ever want to query if there's at least one entity with that mask
    // we just find the first one and store it for future use
    #[cfg(not(feature = "parallel_systems"))]
    pub fn any_has_mask(&self, mask: Bitmask) -> bool {
        let storage_id = std::any::TypeId::of::<Self>();
        let key = (mask.clone(), storage_id);
        let index = self.entities_index_per_mask().entry(key).or_insert_with(|| {
            self.entities.iter().filter_map(|(e, entity_mask)| {
                if self.check(e, &entity_mask, &mask) {
                    Some(HiddenFastIndex::new(e.guid()))
                }else{
                    None
                }
            }).next().into_iter().collect()
        });

        !index.is_empty()
    }

    #[cfg(feature = "parallel_systems")]
    pub fn entities_for_mask<S: FastIndexExt>(&self, query_mask: Bitmask, storage: &S) -> IndexGuard{
        let storage_id = std::any::TypeId::of::<S::StaticTypeId>();
        let key = (query_mask.clone(), storage_id);
        let index = if let Some(index) = self.entities_index_per_mask.get(&key) {
            index
        }else{
            let entities_ids = self.entities.iter().filter_map(|(e, entity_mask)| {
                if self.check(e, &entity_mask, &query_mask) {
                    let fast_indices = storage.fast_index(e.guid());
                    Some(HiddenFastIndex::new(fast_indices))
                }else{
                    None
                }
            }).collect();
            self.entities_index_per_mask.insert(key.clone(), entities_ids)
        };
        IndexGuard::new(index)
    }

    #[cfg(not(feature = "parallel_systems"))]
    pub fn entities_for_mask<S: FastIndexExt>(&self, query_mask: Bitmask, storage: &S) -> IndexGuard{
        let storage_id = std::any::TypeId::of::<S::StaticTypeId>();
        let key = (query_mask.clone(), storage_id);
        let index = self.entities_index_per_mask()
            .entry(key)
            .or_insert_with(|| self.entities.iter().filter_map(|(e, entity_mask)| {
                if self.check(e, &entity_mask, &query_mask) {
                    let fast_indices = storage.fast_index(e.guid());
                    Some(HiddenFastIndex::new(fast_indices))
                }else{
                    None
                }
            }).collect());
        IndexGuard::new(index)
    }

    #[cfg(feature = "parallel_systems")]
    pub fn ordered_entities_for_thread_local<C: Component, S: FastIndexExt>(&self, query_mask: Bitmask, unordered_storage: &S) -> OrderedIndexGuard
    where for<'a> <C as Component>::Storage: HierarchicalStorage<'a, C>
    {
        let regenerate_index = || {
            if let Some(storage) = self.storage_thread_local::<C>() {
                let ids = storage.ordered_ids();
                ids.iter()
                    .cloned()
                    .filter_map(|ordered_id| {
                        let (entity, entity_mask) = &self.entities[ordered_id.guid];
                        if self.check(entity, entity_mask, &query_mask) {
                            let fast_id = (ordered_id, unordered_storage.fast_index(ordered_id.guid));
                            Some(HiddenFastIndex::new(fast_id))
                        }else{
                            None
                        }
                    })
                    .collect::<Vec<_>>()
            }else{
                vec![]
            }
        };
        let index = if let Some(index) = self.ordered_entities_index_per_mask.get(&(C::id(), query_mask.clone())){
            index
        }else{
            self.ordered_entities_index_per_mask.insert(
                (C::id(), query_mask.clone()),
                regenerate_index()
            )
        };
        OrderedIndexGuard::new(index)
    }

    #[cfg(not(feature = "parallel_systems"))]
    pub fn ordered_entities_for_thread_local<C: Component, S: FastIndexExt>(&self, query_mask: Bitmask, unordered_storage: &S) -> OrderedIndexGuard
    where for<'a> <C as Component>::Storage: HierarchicalStorage<'a, C>
    {
        let regenerate_index = || {
            if let Some(storage) = self.storage_thread_local::<C>() {
                let ids = storage.ordered_ids();
                ids.iter()
                    .cloned()
                    .filter_map(|ordered_id| {
                        let (entity, entity_mask) = &self.entities[ordered_id.guid];
                        if self.check(entity, entity_mask, &query_mask) {
                            let fast_id = (ordered_id, unordered_storage.fast_index(ordered_id.guid));
                            Some(HiddenFastIndex::new(fast_id))
                        }else{
                            None
                        }
                    })
                    .collect::<Vec<_>>()
            }else{
                vec![]
            }
        };
        let index = self.ordered_entities_index_per_mask()
            .entry((C::id(), query_mask.clone()))
            .or_insert_with(|| regenerate_index());
        OrderedIndexGuard::new(index)
    }

    #[cfg(feature = "parallel_systems")]
    pub fn ordered_entities_for<C: Component, S: FastIndexExt>(&self, query_mask: Bitmask, unordered_storage: &S) -> OrderedIndexGuard
    where for<'a> <C as Component>::Storage: HierarchicalStorage<'a, C>
    {
        let regenerate_index = || {
            if let Some(storage) = self.storage::<C>() {
                let ids = storage.ordered_ids();
                ids.iter()
                    .cloned()
                    .filter_map(|ordered_id| {
                        let (entity, entity_mask) = &self.entities[ordered_id.guid];
                        if self.check(entity, entity_mask, &query_mask) {
                            let fast_id = (ordered_id, unordered_storage.fast_index(ordered_id.guid));
                            Some(HiddenFastIndex::new(fast_id))
                        }else{
                            None
                        }
                    })
                    .collect::<Vec<_>>()
            }else{
                vec![]
            }
        };
        if !self.ordered_entities_index_per_mask.contains_key(&(C::id(), query_mask.clone())){
            self.ordered_entities_index_per_mask.insert(
                (C::id(), query_mask.clone()),
                regenerate_index()
            );
        }
        let index = self.ordered_entities_index_per_mask.get(&(C::id(), query_mask.clone())).unwrap();
        OrderedIndexGuard::new(index)
    }

    #[cfg(not(feature = "parallel_systems"))]
    pub fn ordered_entities_for<C: Component, S: FastIndexExt>(&self, mask: Bitmask, unordered_storage: &S) -> IndexGuard
    where for<'a> <C as Component>::Storage: HierarchicalStorage<'a, C>
    {
        self.ordered_entities_for_thread_local::<C,_>(mask, unordered_storage)
    }

    pub fn update(&mut self){
        #[cfg(feature = "parallel_systems")]
        for update_storage in self.registry.update_storages.iter(){
            (*update_storage)(&mut self.storages)
        }

        for update_storages_thread_local in self.registry.update_storages.iter(){
            (*update_storages_thread_local)(&mut self.storages)
        }
    }

    pub fn check_storage_changed(&self, id: component::Id) -> bool{
        if let Some(storage_changed) = self.registry.storages_changed.get(&id) {
            storage_changed(&self.storages)
        }else{
            false
        }
    }

    pub fn is_registered<C: Component>(&self) -> bool{
        self.storages.contains_key(&C::id())
    }
}

pub struct CreationSto<'a, C>
where C: Component
{
    storage: WriteGuardRef<'a, C::Storage>,
    component_mask: MaskType,
    storages: SubStorages<'a>,
}

impl<'a, C> CreationSto<'a, C>
where
    C: Component,
    for<'s> C::Storage: Storage<'s, C>,
{
    pub fn add_component(&mut self, entity: &Entity, component: C) {
        // This is safe since creating the creation storage borrows mutably storages
        // and add_component borrows CreationSto mutably. We need a substorages to
        // be able to return the entities from the StorageWrapper impl
        // TODO: find a better way
        let storages = self.storages.storages as *const Storages as *mut Storages;
        let storages = unsafe{ &mut *storages };
        self.storage.insert(entity.guid(), component);

        let &mut (_entity, ref mut mask) = &mut storages.entities[entity.guid()];
        let prev_entity_mask = mask.clone();
        *mask |= &self.component_mask;
        let entity_mask = mask.clone();

        if entity_mask != prev_entity_mask {
            // TODO: can this be done in one step on drop? seems pretty fast as it is anyway
            storages.refresh_indices_component_changed(&entity_mask, &prev_entity_mask);
        }
    }

    pub fn remove_component(&mut self, entity: &Entity) {
        // This is safe since creating the creation storage borrows mutably storages
        // and add_component borrows CreationSto mutably. We need a substorages to
        // be able to return the entities from the StorageWrapper impl
        // TODO: find a better way
        let storages = self.storages.storages as *const Storages as *mut Storages;
        let storages = unsafe{ &mut *storages };
        let &mut (_entity, ref mut mask) = &mut storages.entities[entity.guid()];
        let prev_entity_mask = mask.clone();
        *mask ^= &self.component_mask;
        let entity_mask = mask.clone();

        self.storage.remove(entity.guid());
        if entity_mask != prev_entity_mask {
            storages.refresh_indices_component_changed(&entity_mask, &prev_entity_mask);
        }
    }

    pub fn get(&self, entity: &Entity) -> Option<<C::Storage as Storage<C>>::Get> {
        if self.storage.contains(entity.guid()) {
            Some(unsafe{ self.storage.get(entity.guid()) })
        }else{
            None
        }
    }

    pub fn get_mut(&mut self, entity: &Entity) -> Option<<C::Storage as Storage<C>>::GetMut> {
        if self.storage.contains(entity.guid()) {
            Some(unsafe{ self.storage.get_mut(entity.guid()) })
        }else{
            None
        }
    }
}

impl<'r, 'a, C> StorageEntitiesExt<'a, Write<'a, C>> for &'r mut CreationSto<'a, C>
where
    C: Component,
    for<'s> C::Storage: Storage<'s, C>,
{
    type Component = <C::Storage as Storage<'r, C>>::GetMut;
    type IntoStorage = StorageWrite<'a, C::Storage, C, &'r mut C::Storage>;

    fn into_storage(self) -> Option<Self::IntoStorage> {
        Some(StorageWrite{
            storage: &mut self.storage,
            _marker: PhantomData
        })
    }

    fn entities(&self) -> &Entities<'a> {
        unsafe{ std::mem::transmute(&self.storages) }
    }

    fn get(self, entity: &Entity) -> Option<Self::Component>{
        if self.storage.contains(entity.guid()) {
            unsafe{ Some(self.storage.get_mut(entity.guid())) }
        }else{
            None
        }
    }
}

impl<'r, 'a, C> StorageEntitiesExt<'a, Read<'a, C>> for &'r CreationSto<'a, C>
where
    C: Component,
    for<'s> C::Storage: Storage<'s, C>,
{
    type Component = <C::Storage as Storage<'r, C>>::Get;
    type IntoStorage = StorageRead<'a, C, &'r C::Storage>;

    fn into_storage(self) -> Option<Self::IntoStorage> {
        Some(StorageRead{
            storage: &self.storage,
            _marker_t: PhantomData,
        })
    }

    fn entities(&self) -> &Entities<'a> {
        unsafe{ std::mem::transmute(&self.storages) }
    }

    fn get(self, entity: &Entity) -> Option<Self::Component>{
        if self.storage.contains(entity.guid()) {
            unsafe{ Some(self.storage.get(entity.guid())) }
        }else{
            None
        }
    }
}