use na::{Vec3, Mat4, Unit, Mat3, FastInverse, vec3, zero, UnitQuat, ToPnt};
use angle::{Rad, Angle};
use blender;
use crate::utils::{Transformations, RotMode, transformations, ObjectId, LibraryId};
use crate::idtree;
use crate::utils::{self, Property};
use crate::curves;
use crate::scene::SceneData;

use std::mem;
use hashbrown::HashMap;


bitflags! {
    #[cfg_attr(feature = "serialize", derive(Serialize, Deserialize))]
    pub struct Flags: i32{
        const SELECTED               = (1 << 0);
        const ROOTSEL                = (1 << 1);
        const TIPSEL                 = (1 << 2);
        const TRANSFORM              = (1 << 3);   // Used instead of BONE_SELECTED during transform (clear before use)
        const CONNECTED              = (1 << 4);   // when bone has a parent, connect head of bone to parent's tail
        const HIDDEN_P               = (1 << 6);   // hidden Bones when drawing PoseChannels
        const DONE                   = (1 << 7);   // For detecting cyclic dependencies
        const DRAW_ACTIVE            = (1 << 8);   // active is on mouse clicks only - deprecated, ONLY USE FOR DRAWING
        const HINGE                  = (1 << 9);   // No parent rotation or scale
        const HIDDEN_A               = (1 << 10);  // hidden Bones when drawing Armature Editmode
        const MULT_VG_ENV            = (1 << 11);  // multiplies vgroup with envelope
        const NO_DEFORM              = (1 << 12);  // bone doesn't deform geometry
        const UNKEYED                = (1 << 13);  // set to prevent destruction of its unkeyframed pose (after transform)
        const HINGE_CHILD_TRANSFORM  = (1 << 14);  // set to prevent hinge child bones from influencing the transform center
        const NO_SCALE               = (1 << 15);  // No parent scale
        const HIDDEN_PG              = (1 << 16);  // hidden bone when drawing PoseChannels (for ghost drawing)
        const DRAWWIRE               = (1 << 17);  // bone should be drawn as OB_WIRE, regardless of draw-types of view+armature
        const NO_CYCLICOFFSET        = (1 << 18);  // when no parent, bone will not get cyclic offset
        const EDITMODE_LOCKED        = (1 << 19);  // bone transforms are locked in EditMode
        const TRANSFORM_CHILD        = (1 << 20);  // Indicates that a parent is also being transformed
        const UNSELECTABLE           = (1 << 21);  // bone cannot be selected
        const NO_LOCAL_LOCATION      = (1 << 22);  // bone location is in armature space
        const RELATIVE_PARENTING     = (1 << 23);  // object child will use relative transform (like deform)
        const ADD_PARENT_END_ROLL    = (1 << 24);   // it will add the parent end roll to the inroll
    }
}

// impl CLike for BoneFlag {
//      fn to_usize(&self) -> usize {
//          *self as usize
//      }

//      unsafe fn from_usize(v: usize) -> BoneFlag {
//          mem::transmute(v)
//      }
// }

#[cfg_attr(feature = "serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
pub struct Bone{
    name: String,
    head: Vec3,
    tail: Vec3,
    global_head: Vec3,
    global_tail: Vec3,
    head_radius: f32,
    tail_radius: f32,
    transformations: Transformations,
    animated_transformations: Transformations,
    rest_mat: Mat4<f32>,
    inv_rest_mat: Mat4<f32>,
    inv_rest_mat3: Mat3<f32>,
    roll: Rad<f32>,
    dir: Unit<Vec3>,
    flags: Flags,
    animated_id: usize,
    original_rot_mode: utils::RotMode,
}

impl Bone{
    pub fn new(bone: &blender::Object, pose: &Option<blender::Object>, parent: Option<&Mat4>) -> Bone{
        let name = bone.name().unwrap();
        let head: &Vec3 = bone.get("arm_head").unwrap();
        let tail: &Vec3 = bone.get("arm_tail").unwrap();
        let _arm_mat: &Mat4 = bone.get("arm_mat").unwrap();
        let head_radius: f32 = *bone.get("rad_head").unwrap();
        let tail_radius: f32 = *bone.get("rad_tail").unwrap();
        let flags = *bone.get("flag").unwrap();
        let roll = *bone.get("roll").unwrap();
        let transformations;
        let dir;
        let rest_mat;
        let rot_mode = pose.as_ref().and_then(|pose| {
            let channel = pose.get_list("chanbase").unwrap().iter()
                .find(|channel| channel.name().unwrap() == name)
                .expect(&format!("Couldn't find channel for bone {}", name));
                channel.get("rotmode").ok().map(|r| *r)
        }).unwrap_or(RotMode::Quat);

        //TODO: Use rot_mod here instead of quaternion for everything??
        if let Some(parent) = parent {
            let inv_parent = parent.fast_orthonormal_inverse();
            let rel_head = (inv_parent * vec4!(head, 1.0)).xyz();
            let rel_tail = (inv_parent * vec4!(tail, 1.0)).xyz();
            dir = Unit::new_normalize(rel_tail-rel_head);
            let pos = rel_head.to_pnt();
            let orientation = bone_orientation(dir, roll);
            let scale = vec3!(1.);
            transformations = Transformations{
                position: pos,
                rotation: crate::Rotation::Quat(orientation),
                drotation: None,
                scale,
                parent_inv: None,
                const_inv: None,
                parent_bone: None,
            };
            rest_mat = parent * transformations.local_model_matrix();
        }else{
            dir = Unit::new_normalize(*tail - *head);
            let pos = head.to_pnt();
            let orientation = bone_orientation(dir, roll);
            let scale = vec3!(1.);
            transformations = Transformations{
                position: pos,
                rotation: crate::Rotation::Quat(orientation),
                drotation: None,
                scale,
                parent_inv: None,
                const_inv: None,
                parent_bone: None,
            };
            rest_mat = transformations.local_model_matrix();
        }

        let rel_tail = vec3(0., (*tail - *head).norm(), 0.);
        let inv_rest_mat = rest_mat.fast_affine_inverse().unwrap();
        let inv_rest_mat3 = Mat3::from_iterator(inv_rest_mat.columns(0,3).rows(0,3).iter().map(|v| *v));
        Bone{
            name: name.to_string(),
            head: zero(),
            tail: rel_tail,
            global_head: *head,
            global_tail: *tail,
            head_radius,
            tail_radius,
            animated_transformations: transformations.clone(),
            transformations,
            //original: original,
            roll: roll,
            rest_mat: rest_mat,
            inv_rest_mat: inv_rest_mat,
            inv_rest_mat3: inv_rest_mat3,
            dir: dir,
            flags: flags,
            animated_id: 0,
            original_rot_mode: rot_mode,
        }
    }

    pub fn vec(&self) -> Vec3{
        self.global_tail - self.global_head
    }

    pub fn bone_roll(&self) -> Rad<f32>{
        self.roll
    }

    pub fn head(&self) -> Vec3{
        self.head
    }

    pub fn tail(&self) -> Vec3{
        self.tail
    }

    pub fn skeleton_head(&self) -> Vec3{
        self.global_head
    }

    pub fn skeleton_tail(&self) -> Vec3{
        self.global_tail
    }

    pub fn head_radius(&self) -> f32{
        self.head_radius
    }

    pub fn tail_radius(&self) -> f32{
        self.tail_radius
    }

    pub fn name(&self) -> &str{
        &self.name
    }

    #[inline]
    pub fn rest_mat(&self) -> &Mat4{
        &self.rest_mat
    }

    pub fn transformations(&self) -> &Transformations{
        &self.transformations
    }

    pub fn inv_rest_mat(&self) -> &Mat4{
        &self.inv_rest_mat
    }

    pub fn inv_rest_mat3(&self) -> &Mat3{
        &self.inv_rest_mat3
    }

    pub fn animated_id(&self) -> usize{
        self.animated_id
    }

    pub fn flags(&self) -> Flags{
        self.flags
    }

    pub fn original_rot_mode(&self) -> utils::RotMode{
        self.original_rot_mode
    }

    // #[inline]
    // pub fn pose_mat(&self) -> Mat4{
    //     self.node.global_transformation()
    // }

    // #[inline]
    // pub fn local_mat(&self) -> Mat4{
    //     self.pose_mat().fast_mul(&self.inv_rest_mat)
    // }
    //
    // #[inline]
    // pub fn normal_mat(&self) -> Mat3{
    //     let p = Mat3::from_iterator(self.pose_mat().columns(0,3).rows(0,3).iter().map(|v| *v));
    //     p.fast_mul(&self.inv_rest_mat3)
    // }

    // #[inline]
    // pub fn global_head(&self) -> Vec3{
    //     self.global_transformation().fast_mul(&vec4!(self.head(), 1.0)).xyz()
    // }
    //
    // #[inline]
    // pub fn global_tail(&self) -> Vec3{
    //     self.global_transformation().fast_mul(&vec4!(self.tail(), 1.0)).xyz()
    // }
}


fn bone_orientation(dir: Unit<Vec3>, roll: Rad<f32>) -> UnitQuat{
    let target = vec3(0.,1.,0.);
    let axis = target.cross(dir.as_ref());
    if axis.dot(&axis) > 0.0000001{
        let axis = Unit::new_normalize(axis);
        let theta = target.dot(dir.as_ref()).acos();
        UnitQuat::from_axis_angle(&dir, roll.value()) * UnitQuat::from_axis_angle(&axis, theta)
    }else{
        if target.dot(dir.as_ref()) < 0. {
            let z = Unit::new_unchecked(Vec3::z());
            UnitQuat::from_axis_angle(&dir, roll.value()) * UnitQuat::from_axis_angle(&z, Rad::pi().value())
        }else{
            UnitQuat::from_axis_angle(&dir, roll.value())
        }
    }
}

// https://math.stackexchange.com/questions/382760/composition-of-two-axis-angle-rotations
// TODO: check parenthesis are correct
// fn combine_angle_axis(axis1: Unit<Vec3>, alpha: Rad<f32>, axis2: Unit<Vec3>, beta: Rad<f32>) -> (Unit<Vec3>, Rad<f32>){
//     let c = (alpha / 2.).cos() * (beta / 2.).cos() - (alpha / 2.).sin() * ((beta / 2.).sin() * axis1.unwrap()).dot(&axis2.unwrap());
//     let angle = (c * 2.).acos();

//     let sn = (alpha / 2.).sin() * (beta / 2.).cos() * axis1.unwrap() + (alpha / 2.).cos() * (beta / 2.).sin() * axis2.unwrap() + ((alpha / 2.).sin() * (beta / 2.).sin() * axis1.unwrap()).component_mul(&axis2.unwrap());
//     let axis = sn / (angle / 2.).asin();

//     (Unit::new_unchecked(axis), Rad(angle))
// }

// fn bone_axis_angle(dir: Unit<Vec3>, roll: Rad<f32>) -> (Unit<Vec3>, Rad<f32>){
//     let target = vec3(0.,1.,0.);
//     let axis = target.cross(dir.as_ref());
//     if axis.dot(&axis) > 0.0000001{
//         let axis = Unit::new_normalize(axis);
//         let theta = target.dot(dir.as_ref()).acos();
//         combine_angle_axis(dir, roll, axis, Rad(theta))
//     }else{
//         if target.dot(dir.as_ref()) < 0. {
//             let z = Unit::new_unchecked(Vec3::z());
//             combine_angle_axis(dir, roll, z, Rad::pi())
//         }else{
//             (dir, roll)
//         }
//     }
// }


// fn bone_euler_angles(dir: Unit<Vec3>, roll: Rad<f32>, rot_order: ::RotOrder) -> (Rad<f32>, Rad<f32>, Rad<f32>){
//     let target = vec3(0.,1.,0.);
//     let axis = target.cross(dir.as_ref());
//     if axis.dot(&axis) > 0.0000001{
//         let axis = Unit::new_normalize(axis);
//         let theta = target.dot(dir.as_ref()).acos();
//         let q = UnitQuat::from_axis_angle(&dir, roll.value()) * UnitQuat::from_axis_angle(&axis, theta);
//         utils::to_euler(&q, rot_order)
//     }else{
//         if target.dot(dir.as_ref()) < 0. {
//             let z = Unit::new_unchecked(Vec3::z());
//             let q = UnitQuat::from_axis_angle(&dir, roll.value()) * UnitQuat::from_axis_angle(&z, Rad::pi().value());
//             utils::to_euler(&q, rot_order)
//         }else{
//             let q = UnitQuat::from_axis_angle(&dir, roll.value());
//             utils::to_euler(&q, rot_order)
//         }
//     }
// }

// fn bone_rotation(dir: Unit<Vec3>, roll: Rad<f32>, rot_mode: utils::RotMode) -> ::Rotation{
//     match rot_mode{
//         utils::RotMode::Quat =>  ::Rotation::Quat(bone_orientation(dir, roll)),
// 	    utils::RotMode::AxisAngle => {
//             let (axis, angle) = bone_axis_angle(dir, roll);
//             ::Rotation::AxisAngle(axis, angle)
//         }
//         euler_rotation => {
//             let rot_order = euler_rotation.to_rot_order();
//             let (x,y,z) = bone_euler_angles(dir, roll, rot_order);
//             rot_mode.to_euler_rotation(vec3(x,y,z))
//         }
//     }
// }

#[cfg_attr(feature = "serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
pub struct Skeleton{
    name: ObjectId,
    arena: idtree::Arena<Bone>,
    bones: HashMap<String, idtree::NodeId>,
    bone_base: Vec<idtree::NodeId>,
    armature_mat: Mat4<f32>,
    transformations: Transformations,
    animated_index: HashMap<String, usize>,
    animated: Vec<Mat4<f32>>,
    animated_normals: Vec<Mat3<f32>>,
    changed: bool,
    custom_properties: Vec<Property>,
    default_action_name: Option<ObjectId>,
}

pub type Pose = Skeleton;

fn parse_skeleton(bone: blender::Object,
    library_id: &LibraryId,
    pose: &Option<blender::Object>,
    parent: Option<idtree::NodeId>,
    arena: &mut idtree::Arena<Bone>) -> Vec<idtree::NodeId>
{
    let rest_bone = {
        let parent_rest_bone = parent.map(|id| &arena[id].rest_mat);
        Bone::new(&bone, &pose, parent_rest_bone)
    };
    let rest_bone = if let Some(parent_rest) = parent{
        parent_rest.append_new(rest_bone, arena).id()
    }else{
        arena.new_node(rest_bone).id()
    };

    // children recursively
    let mut res = bone.get_list("childbase").unwrap().iter().flat_map(|bone|{
        parse_skeleton(bone, library_id, pose, Some(rest_bone), arena).into_iter()
    }).collect::<Vec<_>>();

    res.push(rest_bone);
    res
}


impl Skeleton{
    pub fn parse(
        name: &str,
        obj: &blender::Object,
        library_id: &LibraryId,
        armature: &blender::Object,
        libraries: &HashMap<LibraryId, blender::File>) -> Skeleton
    {
        let transfos = transformations(&obj);
        let mut arena = idtree::Arena::new();

        let pose = obj.get_object("pose").ok();
        let bone_ids = armature.get_list("bonebase").unwrap().iter().flat_map(|bone| {
            parse_skeleton(bone, library_id, &pose, None, &mut arena)
        }).collect::<Vec<_>>();

        let animated_index = bone_ids.iter().enumerate().map(|(id,bone)| {
            let name = arena[*bone].name.clone();
            arena[*bone].animated_id = id;
            (name, id)
        }).collect::<HashMap<_,_>>();


        let bones = bone_ids.into_iter().map(|bone| {
            let name = arena[bone].name.clone();
            (name, bone)
        }).collect::<HashMap<_,_>>();

        let bone_base = armature.get_list("bonebase").unwrap()
            .iter().map(|b| bones[b.name().unwrap()])
            .collect();

        let custom_properties = utils::custom_properties(&armature);

        let default_action_name = curves::default_action_name_for(
            obj,
            library_id,
            libraries
        );

        Skeleton{
            name: ObjectId::new(library_id.clone(), name),
            arena,
            bones,
            bone_base,
            transformations: transfos,
            armature_mat: *obj.get("obmat").unwrap(),
            animated: unsafe{ vec![mem::MaybeUninit::uninit().assume_init(); animated_index.len()] },
            animated_normals: unsafe{ vec![mem::MaybeUninit::uninit().assume_init(); animated_index.len()] },
            animated_index,
            changed: true,
            custom_properties,
            default_action_name
        }
    }

    pub fn name(&self) -> &ObjectId{
        &self.name
    }

    pub fn bones(&self) -> &HashMap<String, idtree::NodeId>{
        &self.bones
    }

    pub fn bones_mut(&mut self) -> &mut HashMap<String, idtree::NodeId>{
        &mut self.bones
    }

    pub fn bone(&self, name: &str) -> Option<&Bone>{
        self.bones.get(name).map(|id|{
            self.arena[*id].data()
        })
    }

    pub fn bone_base(&self) -> &[idtree::NodeId]{
        &self.bone_base
    }

    pub fn bones_tree(&self) -> &idtree::Arena<Bone>{
        &self.arena
    }

    pub fn animated_index(&self) -> &HashMap<String, usize>{
        &self.animated_index
    }

    pub fn armature_mat(&self) -> &Mat4<f32>{
        &self.armature_mat
    }

    pub fn deform_mat_by_name(&self, name: &str) -> Option<&Mat4<f32>>{
        self.animated_index.get(name).and_then(|idx| self.animated.get(*idx))
    }

    pub fn deform_normal_mat_by_name(&self, name: &str) -> Option<&Mat3<f32>>{
        self.animated_index.get(name).and_then(|idx| self.animated_normals.get(*idx))
    }

    pub fn deform_mat(&self, id: usize) -> Option<&Mat4<f32>>{
        self.animated.get(id)
    }

    pub fn deform_normal_mat(&self, id: usize) -> Option<&Mat3<f32>>{
        self.animated_normals.get(id)
    }

    pub fn transformations(&self) -> &Transformations{
        &self.transformations
    }

    pub fn default_action(&self) -> Option<&ObjectId>{
        self.default_action_name.as_ref()
    }

    pub fn animate(&mut self, t: f64, action: &crate::Action, doloop: bool){
        for (name, bone) in self.bones.iter(){
            let bone = &mut self.arena[*bone];

            let q = bone.animated_transformations.rotation.into_quat();
            let q = match bone.original_rot_mode{
                RotMode::Quat =>
                    action.orientation_at(&name, t as f32, &q, doloop),
                RotMode::AxisAngle =>
                    action.orientation_from_axis_angle_at(&name, t as f32, &q, doloop),
                euler_rotation =>
                    action.orientation_from_euler_at(&name, t as f32, &q, euler_rotation.to_rot_order(), doloop),
            };
            if let Some(q) = q{
                bone.animated_transformations.rotation = crate::Rotation::Quat(q);
            }

            let p = action.location_at(name, t as f32, &bone.animated_transformations.position, doloop);
            if let Some(p) = p{
                bone.animated_transformations.position = p;
            }
        }
    }

    /*pub fn animate_loop_by(&mut self, delta_s: f64, action: &Action, loop_bone: &str){
        let t = self.last_t + delta_s;
        let max_t = action.max_time_s();
        let loop_t = t % max_t;
        if t > self.last_t && loop_t < self.last_t{
            let bone = &self.bones_group[self.bones[loop_bone]];
            let p = bone.position().coords;
            let p = action.value_at(loop_bone, "location", max_t as f32, p.as_slice());
            if let Some(p) = p{
                let length = math::norm(&vec3(p[0], p[1], p[2]));
                let advance = -self.animation_node.y_axis().as_ref() * length;
                self.animation_node.translate(&advance);
            }
        }
        self.animate(loop_t, action);
    }*/

    pub fn animate_rot(&mut self, t: f64, action: &crate::Action, doloop: bool){
        for (name, bone) in self.bones.iter_mut(){
            let bone = &mut self.arena[*bone];
            let q = bone.animated_transformations.rotation.into_quat();
            let q = match bone.original_rot_mode{
                RotMode::Quat =>
                    action.orientation_at(&name, t as f32, &q, doloop),
                RotMode::AxisAngle =>
                    action.orientation_from_axis_angle_at(&name, t as f32, &q, doloop),
                euler_rotation =>
                    action.orientation_from_euler_at(&name, t as f32, &q, euler_rotation.to_rot_order(), doloop),
            };
            if let Some(q) = q{
                bone.animated_transformations.rotation = crate::Rotation::Quat(q);
            }
        }
    }

    pub fn animate_loc(&mut self, t: f64, action: &crate::Action, doloop: bool){
        for (name, bone) in self.bones.iter_mut(){
            let bone = &mut self.arena[*bone];
            let p = action.location_at(name, t as f32, &bone.animated_transformations.position, doloop);
            if let Some(p) = p{
                bone.animated_transformations.position = p;
            }
        }
    }

    pub fn changed(&self) -> bool{
        self.changed
    }

    pub fn custom_properties(&self) -> &[Property]{
        &self.custom_properties
    }

    // fn update_cache(&mut self){
    //     for (_name, bone) in self.bones.iter(){
    //         let bone = &self.arena[*bone];
    //         self.animated[bone.animated_id] = bone.local_mat();
    //         self.animated_normals[bone.animated_id] = bone.normal_mat();
    //     }
    // }

    // pub fn update_geometry(&mut self){
    //     if !*self.uptodate_geom {
    //         let geometry = self.bones.iter()
    //             .flat_map(|(_name, bone)|{
    //                 let bone = &self.bones_group[*bone];
    //                 vec![
    //                     vertex3dcolor(bone.global_head(), &WHITE),
    //                     vertex3dcolor(bone.global_tail(), &WHITE),
    //                 ]
    //             })
    //             .collect::<Vec<_>>();
    //         self.vao.update_vertices(&geometry);
    //         self.updated_geom_sender.send(true);
    //     }
    // }
    //
    // pub fn update_skeleton_with_parent(&mut self, parent: Option<&Node>) -> bool{
    //     self.skeleton_node.update_with_parent(parent)
    // }
    //
    // pub fn update_bones_with_parent(&mut self, parent: Option<&Node>) -> bool{
    //     self.changed = self.bones_group.update_with_parent(parent);
    //
    //     if self.changed {
    //         self.update_cache();
    //         self.updated_mat_sender.send(true);
    //     }
    //     self.changed
    // }
}

// impl NodeT for Skeleton{
//     fn node(&self) -> &Node{
//     	self.skeleton_node.node()
//     }
//
//     fn node_mut(&mut self) -> &mut Node{
//         self.skeleton_node.node_mut()
//     }
//
//     fn update_with_parent(&mut self, parent: Option<&Node>) -> bool{
//         self.changed = self.skeleton_node.update_with_parent(parent) |
//                        self.bones_group.update_with_parent(parent);
//
//         if self.changed {
//             self.update_cache();
//             self.updated_mat_sender.send(true);
//         }
//         self.changed
//     }
// }