/*!
 * Model skinning. When added to the scene this module will apply skinning to any visible model
 * in the system that has a Skeleton and AnimatedGeometry
 */

mod components;
pub use self::components::*;

use rin_scene::{DeferredScene, components::Visible, geometry::{GeometryRef, Geometry, VertexGroups}, geometry::AnimatedGeometry, renderer::memory::AllocatorsIndex, transformation::{Bone, BoneParts, SkinningUpToDate}};
use rinecs::{Entities, EntitiesThreadLocal, Entity, Has, HasOption, Read, ReadOption, ReadOr, ReadRef, Ref, Resources, ResourcesThreadLocal, Sto, System, SystemThreadLocal, Write, WriteAndParent, WriteOption, system, system_storages, system_thread_local, StorageRef};
use rin_graphics::{self as graphics, Vertex, vertex, Vertex3D, Node, NodeParts};
use rin_math::{
    Vec4, Mat4, Mat3, vec3, ToPnt, zero, vec4, ToVec, FastInverse, one, AsPnt,
    Quaternion, DualQuat
};
#[cfg(feature="rayon")]
use rayon::prelude::*;
use std::marker::PhantomData;
use std::fmt::Debug;
use densevec::DenseVec;
use std::mem;
use super::SceneIndex;
use rin_events::{Property, Parameter};
#[cfg(glsl_debug)]
use rin_util::LogErr;
#[cfg(glsl_debug)]
use std::path::Path;

#[cfg(gl)]
use rin_scene::renderer::{
    geometry::{
        VertexBuffer, GpuGeometryRef,
    },
    Allocator,
};

#[cfg(gl)]
use rin_gl as gl;
#[cfg(gl)]
use crate::components::OriginalGeometryBuffer;

use serde::{Serialize, Deserialize};

#[system(name = "skeleton changed reset")]
#[after("SkeletonGeometryUpdater")]
#[after("skeleton_cache_updater")]
#[writes("Skeleton")]
pub fn skeleton_changed_reseter(mut entities: Entities, _: Resources){
    for skeleton in entities.iter_for_mut::<Write<Skeleton>>(){
        skeleton.changed = false;
    }
}

// This updates the bones transformations used for objects parented to bones,
// those transformations are not updated by the normal transformation updater systems
// instead we copy them from the animated bones with the skeleton as parent for each bone.
// The normal transformation updaters have to run first so the skeleton global transformation
// is up to date when running this one
#[system(name = "bone updater")]
#[needs("BoneParts")]
#[updates("Bone", "NodeParts")]
#[writes("Node")]
#[before(
    "rin_graphics::node::update_all",
    "rin_graphics::node::update_dynamic",
    "rin_graphics::node::update_static"
)]
#[after_name("animation entities", "animation scenes")]
// TODO: this should work through dependencies, the order doesn't really matter but they can't
// run simultaneously cause both write to trafo
pub fn bone_updater(mut entities: Entities, _resources: Resources){
    for ((bone, parent), bone_trafo) in entities.ordered_iter_for_mut::<(
        WriteAndParent<Bone>,
        Write<Node>)>()
    {
        if !bone.used { continue }

        if bone.update_with_parent(parent) {
            *bone_trafo = graphics::Node::new(
                    bone.animated.global_position(),
                    bone.animated.global_orientation(),
                    bone.animated.global_scale());

            // Until 0.13.9 we were updating the node here with it's parent as skeleton cause
            // transformations where created with the same hierarchy as bones.
            // Now bone trafos are parented to it's skeleton and updated in transformation_update
            // This is not tested with objects parented to bones but should work ok
            // otherwise 427d74496cafb184eafd856a68dfea72504e07d6 is the commit that changed this
        }
    }
}

#[system_storages(name = "feet bones updater")]
#[needs("Bone")] // TODO "TransformationParts", loops with path walker, should specify it's only with skeleton
#[updates("FootBones")]
#[after_name("animation entities", "animation scenes")]
// TODO: this should work through dependencies, the order doesn't really matter but they can't
// run simultaneously cause both animation* write to trafo and this reads it
pub fn feet_updater(
    mut footbones: Sto<(Write<FootBones>, Read<Node>, Read<Skeleton>)>,
    bones: Sto<Read<Bone>>,
){
    for (foot_bones, skeleton_trafo, skeleton) in footbones.iter_mut() {
        if skeleton.changed {
            let inverse_skeleton = skeleton_trafo.local_transformation();
            for foot in foot_bones.left.iter_mut(){
                let bone = bones.get(&foot.entity).unwrap();
                let local_foot = inverse_skeleton * bone.animated.global_transformation();
                let new_foot_position = local_foot.column(3).xyz();
                foot.delta = vec3!(0., new_foot_position.y - foot.position.y, 0.);
                foot.position = new_foot_position.to_pnt();
            }
            for foot in foot_bones.right.iter_mut(){
                let bone = bones.get(&foot.entity).unwrap();
                let local_foot = inverse_skeleton * bone.animated.global_transformation();
                let new_foot_position = local_foot.column(3).xyz();
                foot.delta = vec3!(0., new_foot_position.y - foot.position.y, 0.);
                foot.position = new_foot_position.to_pnt();
            }
        }else{
            for foot in foot_bones.left.iter_mut(){
                foot.delta = vec3!(0.);
            }
            for foot in foot_bones.right.iter_mut(){
                foot.delta = vec3!(0.);
            }
        }
    }
}

#[system_storages(name = "skeleton cache updater")]
#[needs("Node", "Bone", "BoneBase", "GeometryWeights")]
#[updates("ArmatureCache", "ArmatureMatrices", "ArmatureDualQuats")]
#[reads("SkeletonRef", "Skeleton")]
pub fn skeleton_cache_updater(
    mut skeletons: Sto<(
        Read<Node>,
        ReadRef<SkeletonRef, Skeleton>,
        ReadRef<SkeletonRef, Node>,
        Write<ArmatureCache>,
        WriteOption<ArmatureMatrices>,
        WriteOption<ArmatureDualQuats>)>,
    bones: Sto<Read<Bone>>,
){
    for (object_trafo, skeleton, skeleton_trafo, armature_cache, matrices, dualquats) in skeletons.iter_mut() {
        if skeleton.changed {
            let skeleton_scale = skeleton_trafo.global_scale();
            let object_scale = object_trafo.global_scale();
            let range_one = 0.9999 .. 1.0001;
            let needs_prepost_mats = !range_one.contains(&skeleton_scale.x)
                || !range_one.contains(&skeleton_scale.y)
                || !range_one.contains(&skeleton_scale.z)
                // TODO: do we need to take into account the object scale, or only the skeleton
                || !range_one.contains(&object_scale.x)
                || !range_one.contains(&object_scale.y)
                || !range_one.contains(&object_scale.z);

            if needs_prepost_mats {
                let object_inv = object_trafo.inv_global_transformation();

                let postmat = object_inv * skeleton_trafo.global_transformation();
                let premat = postmat.fast_affine_inverse().unwrap();

                let postmat3 = Mat3::from_iterator(
                    postmat.adjoint()
                        .columns(0,3)
                        .rows(0,3)
                        .iter()
                        .copied()
                );
                let premat3 = Mat3::from_iterator(
                    premat.adjoint()
                        .columns(0,3)
                        .rows(0,3)
                        .iter()
                        .copied()
                );
                armature_cache.postmat = Some(postmat);
                armature_cache.postmat3 = Some(postmat3);
                armature_cache.premat = Some(premat);
                armature_cache.premat3 = Some(premat3);
            }

            if let Some(mut matrices) = matrices {
                for bone in bones.iter_for_entities(&armature_cache.skinning_bones_index) {
                    if let Some(premat) = armature_cache.premat.as_ref() {
                        matrices[bone.optimized_id].bone_mat = bone.local_mat() * premat;
                    }else{
                        matrices[bone.optimized_id].bone_mat = bone.local_mat();
                    }
                }
            }else if let Some(mut dualquats) = dualquats {
                for bone in bones.iter_for_entities(&armature_cache.skinning_bones_index) {
                    let (dq, scale) = bone.to_dual_quaternion();
                    dualquats[bone.optimized_id].dual_quat = dq;
                    dualquats[bone.optimized_id].scale = scale;
                }
            }

            armature_cache.changed = true;
        }
    }
}

pub struct SkeletonGeometryUpdater{
    skeletons_rendering: Parameter<'static, bool>,
}

impl SkeletonGeometryUpdater{
    pub fn new(skeletons_rendering: Parameter<'static, bool>) -> SkeletonGeometryUpdater{
        SkeletonGeometryUpdater{ skeletons_rendering }
    }
}

#[system(name = "skeleton geometry updater")]
#[needs("GeometryRef", "Visible", "BoneParts", "BoneBase")]
#[updates("Geometry<Vertex3D>")]
impl System for SkeletonGeometryUpdater {
    fn run(&mut self, entities: Entities, _resources: Resources){
        if *self.skeletons_rendering.get() {
            let skeletons = entities
                .iter_for::<(Entity, Read<Skeleton>, Read<GeometryRef>, Read<Visible>)>()
                .filter(|(_,skeleton,_, visible)| visible.is_visible() && skeleton.changed);
            for (entity, _, geometryref, _) in skeletons {
                //TODO recover BoneBase directly from iter no need for entity?
                let bones = entities.component_for::<BoneBase>(&entity).unwrap();
                let mut geometry = entities.component_for_mut::<Geometry<Vertex3D>>(geometryref).unwrap();
                geometry.clear_vertices();
                geometry.extend(bones.iter().flat_map(|bone_base|{
                    if let Some(bone) = entities.tree_node_for::<Bone>(&bone_base.0) {
                        bone.descendants_ref().flat_map(|bone|{
                            if bone.used {
                                vec![graphics::vertex3d(bone.global_head().to_vec()),
                                    graphics::vertex3d(bone.global_tail().to_vec())]
                            }else{
                                vec![]
                            }
                        }).collect::<Vec<_>>()
                    }else{
                        vec![]
                    }
                }));
            }
        }
    }
}

#[cfg(all(gl, not(target_os="macos"), not(target_arch = "wasm32")))]
pub struct GpuSkinningSystem<V> {
    linear_program: glin::Program,
    dualquats_program: glin::Program,
    #[cfg(glsl_debug)]
    linear_loader: rin_util::AutoLoader<glin::Program>,
    #[cfg(glsl_debug)]
    dualquats_loader: rin_util::AutoLoader<glin::Program>,
    marker: PhantomData<V>,
}

#[cfg(all(gl, not(target_os="macos"), not(target_arch = "wasm32")))]
impl<V> GpuSkinningSystem<V>{
    #[cfg(not(glsl_debug))]
    pub fn new(gl: &gl::Renderer) -> GpuSkinningSystem<V> {
        let linear_program = gl.new_program()
            .from_src(&[(gl::COMPUTE_SHADER, include_str!("shaders/linear_skinning.comp"))])
            .unwrap();
        let dualquats_program = gl.new_program()
            .from_src(&[(gl::COMPUTE_SHADER, include_str!("shaders/dualquats_skinning.comp"))])
            .unwrap();


        GpuSkinningSystem{
            linear_program,
            dualquats_program,
            marker: PhantomData,
        }
    }

    #[cfg(glsl_debug)]
    pub fn new(gl: &gl::Renderer) -> GpuSkinningSystem<V> {
        let parent_folder = Path::new(file!()).parent().unwrap();

        let mut settings = gl::autoload::ProgramSettings{
            version: 440,
            precision: glin::program::ShaderPrecision::High,
            shaders: vec![
                (gl::COMPUTE_SHADER, parent_folder.join("shaders/linear_skinning.comp"))
            ],
            extensions: vec![],
            defines: vec![],
            shader_replace: vec![],
            includes_search_paths: vec![],
            includes_dictionary: Default::default(),
        };

        let mut linear_loader = gl.new_auto_program(settings.clone());
        let linear_program = linear_loader.load()
            .log_err("")
            .unwrap_or_else(|_| panic!());

        settings.shaders = vec![
            (gl::COMPUTE_SHADER, parent_folder.join("shaders/dualquats_skinning.comp"))
        ];

        let mut dualquats_loader = gl.new_auto_program(settings);
        let dualquats_program = dualquats_loader.load()
            .log_err("")
            .unwrap_or_else(|_| panic!());

        GpuSkinningSystem{
            linear_loader,
            dualquats_loader,
            linear_program,
            dualquats_program,
            marker: PhantomData,
        }
    }
}


#[cfg(all(gl, not(target_os="macos"), not(target_arch = "wasm32")))]
#[system_thread_local(name = "skinning (gpu)")]
#[cfg_attr(gl, after(
    "rin_scene::renderer::geometry::geometry_changed_updater<V>",
    "rin_scene::renderer::geometry::geometryref_changed_updater"
))]
#[needs("Visible", "Geometry<V>", "ArmatureMatrices", "ArmatureDualQuats", "ArmatureCache")]
#[updates("AnimatedGeometry<V>")]
#[reads(
    SceneIndex,
    BoneWeightsAndIndicesBuffer,
    GpuGeometryRef,
    VertexBuffer,
    gl::Renderer,
    AllocatorsIndex,
)]
#[writes(
    ArmatureCache,
    ArmatureMatricesBuffer,
    ArmatureDualQuatsBuffer,
    OriginalGeometryBuffer<V>,
    SkinningUpToDate
)]
#[gpu_stats]
impl<V> SystemThreadLocal for GpuSkinningSystem<V>
where V: Send + Clone + Sync + Debug + Serialize + Deserialize<'static> +
         Vertex<Position = Vec4> + vertex::Normal<Normal = Vec4> + DefaultWeight +
         gl::VertexFormat + 'static
{
    fn run(&mut self, mut entities: EntitiesThreadLocal, resources: ResourcesThreadLocal) {
        let mut force_reskin = false;
        let gl = resources.get::<gl::Renderer>().unwrap();
        #[cfg(gl_debug_groups)]
        let _debug_group = gl.new_debug_group(0, "Skinning");

        #[cfg(glsl_debug)]
        {
            match self.linear_loader.update(){
                Ok(Some(program)) => {
                    self.linear_program = program;
                    force_reskin = true;
                    log::info!("Reloaded linear skinning compute program")
                },
                Ok(None) => (),
                Err(err) => log::error!("Error reloading linear skinning compute program: {}", err)
            }
            match self.dualquats_loader.update(){
                Ok(Some(program)) => {
                    self.dualquats_program = program;
                    force_reskin = true;
                    log::info!("Reloaded dual quaternions skinning compute program")
                },
                Ok(None) => (),
                Err(err) => log::error!("Error reloading dual quaternions skinning compute program: {}", err)
            }
        }

        let iter = entities.iter_for_mut::<(
            (
                // Ref<GeometryRef, Read<Geometry<V>>>,
                Write<OriginalGeometryBuffer<V>>,
                Write<AnimatedGeometry<V>>,
                Ref<GpuGeometryRef, Read<VertexBuffer>>
            ),
            Read<Visible>,
            Read<BoneWeightsAndIndicesBuffer>,
            Write<ArmatureCache>,
            (WriteOption<ArmatureMatricesBuffer>, WriteOption<ArmatureDualQuatsBuffer>),
            ReadOr<(ArmatureMatrices, ArmatureDualQuats)>,
            Write<SkinningUpToDate>,
            HasOption<crate::ShapeKey>,
        )>()
        .filter(|(_, visible, _, armature_cache, _ ,_ , _, _)| visible.is_visible() && (armature_cache.changed || force_reskin));
        for (
                (original_geom, animated_geom, vertex_buffer),
                _,
                weights,
                armature_cache,
                (gpu_matrices, gpu_dualquats),
                (matrices, dualquats),
                skinning_uptodate,
                has_shapekey
            ) in iter
        {
            if has_shapekey && animated_geom.changed {
                original_geom.update(&animated_geom.geom);
            }

            let program;
            let uniforms;
            let gl = if let (Some(gpu_matrices), Some(matrices)) = (gpu_matrices, matrices) {
                gpu_matrices.update(&matrices);
                program = &self.linear_program;
                uniforms = gl::uniforms! {
                    postmat: armature_cache.postmat.unwrap_or(one()),
                    postmat3: armature_cache.postmat3.unwrap_or(one()),
                };
                gl.with_properties(&[
                    glin::Property::BufferBaseBinding(gl::UNIFORM_BUFFER, 0, &gpu_matrices.0),
                ])
            }else if let (Some(gpu_dualquats), Some(dualquats)) = (gpu_dualquats, dualquats) {
                gpu_dualquats.update(&dualquats);
                program = &self.dualquats_program;
                uniforms = gl::uniforms! {
                    premat: armature_cache.premat.unwrap_or(one()),
                    premat3: armature_cache.premat3.unwrap_or(one()),
                    postmat: armature_cache.postmat.unwrap_or(one()),
                    postmat3: armature_cache.postmat3.unwrap_or(one()),
                };
                gl.with_properties(&[
                    glin::Property::BufferBaseBinding(gl::UNIFORM_BUFFER, 0, &gpu_dualquats.0),
                ])
            }else{
                continue;
            };

            let allocators = resources.get::<AllocatorsIndex>().unwrap();
            let allocator = allocators.by_type::<V, glin::SharedBufferStorage<u8>>().unwrap();
            let geom_buffer = allocator
                .dynamic_vertex_buffer_range(&vertex_buffer.0);
            let gl = gl.with_properties(&[
                glin::Property::BufferBaseBinding(gl::SHADER_STORAGE_BUFFER, 0, &original_geom.0),
                glin::Property::BufferBaseBinding(gl::SHADER_STORAGE_BUFFER, 1, geom_buffer),
                glin::Property::BufferBaseBinding(gl::SHADER_STORAGE_BUFFER, 2, &weights.0),
            ]);

            let _ = gl.dispatch_compute(
                program,
                (animated_geom.geom.len() as f32 / 1024.).ceil()  as u32, 1, 1,
                uniforms
            );

            skinning_uptodate.0 = true;
            armature_cache.changed = false;
            animated_geom.changed = false;
        }

        for (entity, scene_index, _) in entities.iter_for::<(Entity, Read<SceneIndex>, Has<SkinningUpToDate>)>(){
            let all_uptodate = entities
                .iter_for_entities::<Read<SkinningUpToDate>, _>(&scene_index.models)
                .all(|uptodate| uptodate.0);

            entities.component_for_mut::<SkinningUpToDate>(&entity).unwrap().0 = all_uptodate;
        }
    }
}


#[inline]
fn fast_add(a: &Vec4, b: &Vec4, c: &Vec4, d: &Vec4) -> (Vec4, Vec4){
    (a + c, b + d)
}

#[cfg(feature="rayon")]
fn par_deform_geom_mut<'a, T: Send>(geom: &'a mut AnimatedGeometry<T>, weights: &'a GeometryWeights)
    -> impl IndexedParallelIterator<Item = Option<(&'a [rinblender::mesh::MDeformWeight], &'a mut T)>>
{
    let dweight = &weights.final_weights;
    weights.final_dverts.par_iter()
        .zip(geom.geom.par_iter_mut())
        .map(move |(dvert, animated)| dvert.as_ref().map(move |dvert| {
            (&dweight[dvert.dw_start .. dvert.dw_end], animated)
        }))
}

#[cfg(not(feature="rayon"))]
fn deform_geom_mut<'a, T: Send>(geom: &'a mut AnimatedGeometry<T>, weights: &'a GeometryWeights)
    -> impl Iterator<Item = Option<(&'a [rinblender::mesh::MDeformWeight], &'a mut T)>>
{
    let dweight = &weights.final_weights;
    weights.final_dverts.iter()
        .zip(geom.geom.iter_mut())
        .map(move |(dvert, animated)| dvert.as_ref().map(move |dvert| {
            (&dweight[dvert.dw_start .. dvert.dw_end], animated)
        }))
}

fn deform_geom<'a, T: Send>(geom: &'a AnimatedGeometry<T>, weights: &'a GeometryWeights)
    -> impl Iterator<Item = Option<(&'a [rinblender::mesh::MDeformWeight], &'a T)>>
{
    let dweight = &weights.final_weights;
    weights.final_dverts.iter()
        .zip(geom.geom.iter())
        .map(move |(dvert, animated)| dvert.as_ref().map(move |dvert| {
            (&dweight[dvert.dw_start .. dvert.dw_end], animated)
        }))
}

pub struct SkinningSystem<V: Clone + 'static> {
    sender: std::sync::mpsc::Sender<(Entity, Geometry<V>, AnimatedGeometry<V>, GeometryWeights, ArmatureCache, Option<Vec<ArmatureMatrices>>, Option<Vec<ArmatureDualQuats>>)>,
    receiver: std::sync::mpsc::Receiver<(Entity, Geometry<V>, AnimatedGeometry<V>, GeometryWeights, ArmatureCache, Option<Vec<ArmatureMatrices>>, Option<Vec<ArmatureDualQuats>>)>,
    cache: DenseVec<(Geometry<V>, AnimatedGeometry<V>, GeometryWeights, ArmatureCache, Option<Vec<ArmatureMatrices>>, Option<Vec<ArmatureDualQuats>>)>,
    defer: Property<'static, bool>,
}

impl<V: Clone> SkinningSystem<V> {
    pub fn new(defer: Property<'static, bool>) -> SkinningSystem<V> {
        let (sender, receiver) = std::sync::mpsc::channel();
        SkinningSystem {
            sender,
            receiver,
            cache: DenseVec::new(),
            defer,
        }
    }
}

#[system_thread_local(name = "skinning")]
#[cfg_attr(gl, after(
    "rin_scene::renderer::geometry::geometry_changed_updater<V>",
    "rin_scene::renderer::geometry::geometryref_changed_updater"
))]
#[needs("Visible", "Geometry<V>", "ArmatureMatrices", "ArmatureCache")]
#[updates("AnimatedGeometry<V>")]
#[writes("GeometryWeights", "ArmatureCache", "ArmatureMatrices", "VertexGroups")]
// TODO: separate vertex groups into vertex groups and index? so we can depend on the vertex groups
// and update the index? It's really only needed for this system so it's probably not needed
impl<V> SystemThreadLocal for SkinningSystem<V>
    where V: Send + Clone + Sync + Debug + Serialize + Deserialize<'static> +
             Vertex<Position = Vec4> + vertex::Normal<Normal = Vec4> + DefaultWeight +
             'static
{
    // let then = time::Instant::now();
    fn run(&mut self, mut entities: EntitiesThreadLocal, _resources: ResourcesThreadLocal) {
        fn skinning<'a, V>(
            geom: &Geometry<V>,
            animated_geom: &mut AnimatedGeometry<V>,
            weights: &GeometryWeights,
            armature_cache: &ArmatureCache,
            matrices: Option<&[ArmatureMatrices]>,
            dualquats: Option<&[ArmatureDualQuats]>,
            has_shapekey: bool)
        where
            V: Send + Clone + Sync + Debug + Serialize + Deserialize<'static> +
             Vertex<Position = Vec4> + vertex::Normal<Normal = Vec4> + DefaultWeight +
             'static
        {
            let animated_changed = animated_geom.changed;
            animated_geom.changed = true;

            #[cfg(feature="rayon")]
            let deform_iter = geom.vertices().par_iter()
                .zip(par_deform_geom_mut(animated_geom, weights))
                .filter_map(|(vertex, animated)| if let Some((weights, animated)) = animated{
                    // TODO: Create an intermediate mesh for blendshaped geometry
                    // so we can animate from there instead of having everything on
                    // animated geometry, otherwise if blendshape is not applied one frame
                    // skinning would be applied over original
                    if has_shapekey && animated_changed {
                        Some((animated.clone(), weights, animated))
                    }else{
                        Some((vertex.clone(), weights, animated))
                    }
                }else{
                    None
                });

            #[cfg(not(feature="rayon"))]
            let deform_iter = geom.vertices().iter()
                .zip(deform_geom_mut(animated_geom, weights))
                .filter_map(|(vertex, animated)| if let Some((weights, animated)) = animated{
                    // TODO: Create an intermediate mesh for blendshaped geometry
                    // so we can animate from there instead of having everything on
                    // animated geometry, otherwise if blendshape is not applied one frame
                    // skinning would be applied over original
                    if has_shapekey && animated_changed {
                        Some((animated.clone(), weights, animated))
                    }else{
                        Some((vertex.clone(), weights, animated))
                    }
                }else{
                    None
                });


            if let Some(dualquats) = dualquats {
                deform_iter.for_each(|(vertex, weights, animated)| {
                    let vpos;
                    let vnor;
                    if let (Some(premat), Some(premat3)) = (armature_cache.premat.as_ref(), armature_cache.premat3.as_ref())
                    {
                        vpos = premat * vertex.position();
                        vnor = vec4!(premat3 * vertex.normal().xyz(), 0.);
                    }else{
                        vpos = *vertex.position();
                        vnor = vec4!(vertex.normal().xyz(), 0.);
                    }

                    // let w0 = &weights[0];
                    // let w1 = &weights[1];
                    // let w2 = &weights[2];
                    // let w3 = &weights[3];

                    // let dq0 = unsafe{ dualquats.get_unchecked(w0.def_nr as usize) };
                    // let dq1 = unsafe{ dualquats.get_unchecked(w1.def_nr as usize) };
                    // let dq2 = unsafe{ dualquats.get_unchecked(w2.def_nr as usize) };
                    // let dq3 = unsafe{ dualquats.get_unchecked(w3.def_nr as usize) };

                    // let dual_q0 = dq0.dual_quat * w0.weight;
                    // let dual_q1 = dq1.dual_quat * w1.weight;
                    // let dual_q2 = dq2.dual_quat * w2.weight;
                    // let dual_q3 = dq3.dual_quat * w3.weight;
                    // let dual_q = dual_q0 + dual_q1 + dual_q2 + dual_q3;
                    // let scale0 = dq0.scale * w0.weight;
                    // let scale1 = dq1.scale * w1.weight;
                    // let scale2 = dq2.scale * w2.weight;
                    // let scale3 = dq3.scale * w3.weight;
                    // let scale = scale0 + scale1 + scale2 + scale3;
                    // let total_weight = w0.weight + w1.weight + w2.weight + w3.weight;

                    let mut iter = weights.iter().map(|weight| {
                        let mat = unsafe{ dualquats.get_unchecked(weight.def_nr as usize) };
                        (mat.dual_quat, mat.scale, weight.weight)
                    });
                    let (dq0, s0, w0) = if !weights.is_empty() {
                        iter.next().unwrap()
                    }else{
                        (one(), one(), 0.)
                    };

                    let (dual_q, scale, total_weight): (DualQuat, Mat4, f32) = iter
                        .fold((dq0 * w0, s0 * w0, w0), |(dual_q, scale, total_weight), (bone_dq, bone_scale, weight)|{
                            (
                                dual_q + bone_dq * (weight * dual_q.r.dot(&bone_dq.r).signum()),
                                scale + bone_scale * weight,
                                total_weight + weight,
                            )
                        });

                    if total_weight > 0.{
                        let weight_factor = animated.default_weight() / total_weight;

                        let wscale = scale * weight_factor;
                        let position = wscale * vpos;
                        let scalenormal = wscale.adjoint();
                        let scalenormal = Mat3::from_iterator(scalenormal.columns(0, 3).rows(0, 3).iter().copied());
                        let normal = scalenormal * vnor.xyz();
                        let (position, normal) = dual_q.transform_position_and_normal(position.xyz().as_pnt(), &normal);

                        if let (Some(postmat), Some(postmat3)) = (armature_cache.postmat.as_ref(), armature_cache.postmat3.as_ref())
                        {
                            *animated.position_mut() = postmat * position.to_homogeneous();
                            *animated.normal_mut() = vec4!(postmat3 * normal, 0.);
                        }else{
                            *animated.position_mut() = position.to_homogeneous();
                            *animated.normal_mut() = vec4!(normal, 0.);
                        }
                    }
                });
            }else if let Some(matrices) = matrices{
                deform_iter.for_each(|(vertex, weights, animated)| {
                    let vpos = vertex.position();
                    let vnor = vertex.normal();

                    // let (mut position, mut normal, total_weight) = weights.iter()
                    //     .map(|weight| {
                    //         let mat = unsafe{ matrices.get_unchecked(weight.def_nr as usize) };
                    //         (weight, mat)
                    //     })
                    //     .fold((zero(), zero(), 0.), |(position, normal, total_weight), (weight, mat)| {
                    //         let cpos = mat.vertices * vpos;
                    //         let cnor = vec4!(mat.normals * vnor.xyz(), 0.);

                    //         let c_v_pos = (cpos - vpos) * weight.weight;
                    //         let c_v_nor = (cnor - vnor) * weight.weight;

                    //         // Doing this is slightly faster than individually adding the vectors!?
                    //         let (pos, nor) = fast_add(&c_v_pos, &c_v_nor, &position, &normal);
                    //         // let pos = c_v_pos + position;
                    //         // let nor = c_v_nor + normal;

                    //         (pos, nor, total_weight + weight.weight)
                    //     });

                    let (mat_pos, total_weight): (Mat4, f32) = weights.iter()
                        .map(|weight| {
                            let mat = unsafe{ matrices.get_unchecked(weight.def_nr as usize) };
                            (weight, mat)
                        })
                        .fold((zero(), 0.), |(mat_pos, total_weight), (weight, mat)| {
                            let cpos = mat_pos + mat.bone_mat * weight.weight;
                            (cpos, total_weight + weight.weight)
                        });

                    let mut position = mat_pos * vpos;
                    let mat_nor = Mat3::from_iterator(mat_pos
                        .adjoint()
                        .columns(0, 3)
                        .rows(0, 3)
                        .iter()
                        .copied()
                    );

                    let mut normal = vec4!(mat_nor * vnor.xyz(), 0.);


                    // let w0 = &weights[0];
                    // let w1 = &weights[1];
                    // let w2 = &weights[2];
                    // let w3 = &weights[3];

                    // let mat0 = unsafe{ matrices.get_unchecked(w0.def_nr as usize) };
                    // let mat1 = unsafe{ matrices.get_unchecked(w1.def_nr as usize) };
                    // let mat2 = unsafe{ matrices.get_unchecked(w2.def_nr as usize) };
                    // let mat3 = unsafe{ matrices.get_unchecked(w3.def_nr as usize) };

                    // let mat_pos0 = mat0.vertices * w0.weight;
                    // let mat_pos1 = mat1.vertices * w1.weight;
                    // let mat_pos2 = mat2.vertices * w2.weight;
                    // let mat_pos3 = mat3.vertices * w3.weight;
                    // let mat_pos = mat_pos0 + mat_pos1 + mat_pos2 + mat_pos3;

                    // let mat_nor = Mat3::from_iterator(mat_pos
                    //     .adjoint()
                    //     .columns(0, 3)
                    //     .rows(0, 3)
                    //     .iter()
                    //     .copied()
                    // );

                    // let mut position = mat_pos * vpos;
                    // let mut normal = vec4!(mat_nor * vnor.xyz(), 0.);
                    // let total_weight = w0.weight + w1.weight + w2.weight + w3.weight;

                    if total_weight > 0.{
                        let weight_factor = animated.default_weight() / total_weight;
                        position *= weight_factor;
                        normal *= weight_factor;

                        // Doing this is slightly faster than individually adding the vectors!?
                        // let (new_position, new_normal) = fast_add(&position, &normal, vpos, vnor);

                        if let (Some(postmat), Some(postmat3)) = (armature_cache.postmat.as_ref(), armature_cache.postmat3.as_ref())
                        {
                            *animated.position_mut() = postmat * position;
                            *animated.normal_mut() = vec4!(postmat3 * normal.xyz(), 0.);
                        }else{
                            *animated.position_mut() = position;
                            *animated.normal_mut() = normal;
                        }
                    }
                });
            }

            //animated_geom.recalculate_normals(&geom.total_indices);
            // skeleton.changed = false;
        }

        if *self.defer.get() {
            #[cfg(feature="rayon")]
            {
                use rinecs::StreamingIteratorMut;
                let iter = entities.iter_for_mut::<(
                    (Ref<GeometryRef, Write<Geometry<V>>>, Entity),
                    Read<Visible>,
                    Write<AnimatedGeometry<V>>,
                    Write<GeometryWeights>,
                    Write<ArmatureCache>,
                    ReadOr<(ArmatureMatrices, ArmatureDualQuats)>,
                    HasOption<crate::ShapeKey>,
                )>()
                    // .filter(|(_,  visible, _,_, armature_cache, _, _)| visible.is_visible() && armature_cache.changed)
                    .fold_mut(vec![], |mut acc, ((geom, e), _, animated_geom, weights, armature_cache, (matrices, dualquats), has_shapekey)|{
                        armature_cache.changed = false;
                        if let Some((mut c_geom, mut c_animated_geom, mut c_weights, mut c_armature_cache, mut c_matrices, mut c_dualquats)) = self.cache.remove(e.guid()){
                            mem::swap(&mut c_geom, geom);
                            mem::swap(&mut c_animated_geom, animated_geom);
                            mem::swap(&mut c_weights, weights);
                            mem::swap(&mut c_armature_cache, armature_cache);
                            if let Some(matrices) = matrices {
                                let c_matrices = c_matrices.as_mut().unwrap();
                                if c_matrices.len() != matrices.len() {
                                    c_matrices.clear();
                                    c_matrices.reserve(matrices.len());
                                    unsafe{
                                        c_matrices.set_len(matrices.len());
                                    }
                                }
                                c_matrices.copy_from_slice(&matrices);
                            }else if let Some(dualquats) = dualquats {
                                let c_dualquats = c_dualquats.as_mut().unwrap();
                                if c_dualquats.len() != dualquats.len() {
                                    c_dualquats.clear();
                                    c_dualquats.reserve(dualquats.len());
                                    unsafe{
                                        c_dualquats.set_len(dualquats.len());
                                    }
                                }
                                c_dualquats.copy_from_slice(&dualquats);
                            }
                            acc.push((*e, c_geom, c_animated_geom, c_weights, c_armature_cache, c_matrices, c_dualquats, *has_shapekey));
                            acc
                        }else{
                            let matrices = matrices.map(|m| m.to_vec());
                            let dualquats = dualquats.map(|d| d.to_vec());
                            acc.push((*e, geom.clone(), animated_geom.clone(), weights.clone(), armature_cache.clone(), matrices, dualquats, *has_shapekey));
                            acc
                        }
                    });

                let sender = self.sender.clone();
                rayon::spawn(move || for(e, geom, mut animated_geom, weights, armature_cache, matrices, dualquats, has_shapekey) in iter{
                    skinning(
                        &geom,
                        &mut animated_geom,
                        &weights,
                        &armature_cache,
                        matrices.as_ref().map(|m| m.as_slice()),
                        dualquats.as_ref().map(|d| d.as_slice()),
                        has_shapekey
                    );
                    if sender.send((e, geom, animated_geom, weights, armature_cache, matrices, dualquats)).is_err() {
                        return
                    }
                });

                for (e, geom, mut animated_geom, weights, armature_cache, matrices, dualquats) in self.receiver.try_iter() {
                    mem::swap(*entities.component_for_mut::<AnimatedGeometry<V>>(&e).unwrap(), &mut animated_geom);
                    entities.component_for_mut::<SkinningUpToDate>(&e).unwrap().0 = true;
                    self.cache.insert(e.guid(), (geom, animated_geom, weights, armature_cache, matrices, dualquats));
                }

                for (entity, scene_index, _) in entities.iter_for::<(Entity, Read<SceneIndex>, Has<SkinningUpToDate>)>(){
                    let all_uptodate = entities
                        .iter_for_entities::<Read<SkinningUpToDate>, _>(&scene_index.models)
                        .all(|uptodate| uptodate.0);

                    entities.component_for_mut::<SkinningUpToDate>(&entity).unwrap().0 = all_uptodate;
                }
            }
        }else{
            let iter = entities.iter_for_mut::<(
                (Entity, Ref<GeometryRef, Read<Geometry<V>>>, Write<SkinningUpToDate>),
                Read<Visible>,
                Write<AnimatedGeometry<V>>,
                Read<GeometryWeights>,
                Read<ArmatureCache>,
                ReadOr<(ArmatureMatrices, ArmatureDualQuats)>,
                HasOption<crate::ShapeKey>,
            )>()
            .filter(|(_,  visible, _,_, armature_cache, _, _)| visible.is_visible() && armature_cache.changed);

            for((e, geom, skinning_uptodate), _, animated_geom, weights, armature_cache, (matrices, dualquats), has_shapekey) in iter{
                skinning(
                    geom,
                    animated_geom,
                    weights,
                    armature_cache,
                    matrices.as_ref().map(|m| &**m),
                    dualquats.as_ref().map(|d| &**d),
                    has_shapekey
                );
                skinning_uptodate.0 = true;
            }

            for (entity, scene_index, _) in entities.iter_for::<(Entity, Read<SceneIndex>, Has<SkinningUpToDate>)>(){
                let all_uptodate = entities
                    .iter_for_entities::<Read<SkinningUpToDate>, _>(&scene_index.models)
                    .all(|uptodate| uptodate.0);

                entities.component_for_mut::<SkinningUpToDate>(&entity).unwrap().0 = all_uptodate;
            }
        }
    }
}