use crate::diagnostics::LookupError;

use super::{NodeKey, Quat, Scene, Transform, Vec3};

impl Scene {
    pub fn set_transform(
        &mut self,
        node: NodeKey,
        transform: Transform,
    ) -> Result<(), LookupError> {
        let node = self
            .nodes
            .get_mut(node)
            .ok_or(LookupError::NodeNotFound(node))?;
        if node.transform != transform {
            node.transform = transform;
            self.transform_revision = self.transform_revision.saturating_add(1);
        }
        Ok(())
    }

    pub fn world_transform(&self, node: NodeKey) -> Option<Transform> {
        let mut chain = Vec::new();
        let mut current = node;
        loop {
            let node = self.nodes.get(current)?;
            chain.push(node.transform);
            let Some(parent) = node.parent else {
                break;
            };
            current = parent;
        }
        Some(
            chain
                .into_iter()
                .rev()
                .fold(Transform::IDENTITY, compose_transform),
        )
    }
}

pub(super) fn compose_transform(parent: Transform, child: Transform) -> Transform {
    let scaled_child_translation = Vec3::new(
        child.translation.x * parent.scale.x,
        child.translation.y * parent.scale.y,
        child.translation.z * parent.scale.z,
    );
    Transform {
        translation: add_vec3(
            parent.translation,
            rotate_vec3(parent.rotation, scaled_child_translation),
        ),
        rotation: multiply_quat(parent.rotation, child.rotation),
        scale: Vec3::new(
            parent.scale.x * child.scale.x,
            parent.scale.y * child.scale.y,
            parent.scale.z * child.scale.z,
        ),
    }
}

pub(super) fn local_transform_from_world(parent: Transform, world: Transform) -> Option<Transform> {
    if !is_invertible_scale(parent.scale) {
        return None;
    }
    let inverse_parent_rotation = inverse_unit_quat(parent.rotation);
    let local_translation = rotate_vec3(
        inverse_parent_rotation,
        subtract_vec3(world.translation, parent.translation),
    );
    Some(Transform {
        translation: Vec3::new(
            local_translation.x / parent.scale.x,
            local_translation.y / parent.scale.y,
            local_translation.z / parent.scale.z,
        ),
        rotation: multiply_quat(inverse_parent_rotation, world.rotation),
        scale: Vec3::new(
            world.scale.x / parent.scale.x,
            world.scale.y / parent.scale.y,
            world.scale.z / parent.scale.z,
        ),
    })
}

pub(super) fn rotate_vec3(rotation: Quat, vector: Vec3) -> Vec3 {
    let length_squared = rotation.x * rotation.x
        + rotation.y * rotation.y
        + rotation.z * rotation.z
        + rotation.w * rotation.w;
    if length_squared <= f32::EPSILON || !length_squared.is_finite() {
        return vector;
    }
    let inverse_length = length_squared.sqrt().recip();
    let qx = rotation.x * inverse_length;
    let qy = rotation.y * inverse_length;
    let qz = rotation.z * inverse_length;
    let qw = rotation.w * inverse_length;
    let tx = 2.0 * (qy * vector.z - qz * vector.y);
    let ty = 2.0 * (qz * vector.x - qx * vector.z);
    let tz = 2.0 * (qx * vector.y - qy * vector.x);
    Vec3::new(
        vector.x + qw * tx + (qy * tz - qz * ty),
        vector.y + qw * ty + (qz * tx - qx * tz),
        vector.z + qw * tz + (qx * ty - qy * tx),
    )
}

pub(super) fn multiply_quat(left: Quat, right: Quat) -> Quat {
    normalize_quat(Quat::from_xyzw(
        left.w * right.x + left.x * right.w + left.y * right.z - left.z * right.y,
        left.w * right.y - left.x * right.z + left.y * right.w + left.z * right.x,
        left.w * right.z + left.x * right.y - left.y * right.x + left.z * right.w,
        left.w * right.w - left.x * right.x - left.y * right.y - left.z * right.z,
    ))
}

fn inverse_unit_quat(rotation: Quat) -> Quat {
    let normalized = normalize_quat(rotation);
    Quat::from_xyzw(-normalized.x, -normalized.y, -normalized.z, normalized.w)
}

fn normalize_quat(value: Quat) -> Quat {
    let length_squared =
        value.x * value.x + value.y * value.y + value.z * value.z + value.w * value.w;
    if length_squared <= f32::EPSILON || !length_squared.is_finite() {
        return Quat::IDENTITY;
    }
    let inverse_length = length_squared.sqrt().recip();
    Quat::from_xyzw(
        value.x * inverse_length,
        value.y * inverse_length,
        value.z * inverse_length,
        value.w * inverse_length,
    )
}

fn subtract_vec3(left: Vec3, right: Vec3) -> Vec3 {
    Vec3::new(left.x - right.x, left.y - right.y, left.z - right.z)
}

fn add_vec3(left: Vec3, right: Vec3) -> Vec3 {
    Vec3::new(left.x + right.x, left.y + right.y, left.z + right.z)
}

fn is_invertible_scale(scale: Vec3) -> bool {
    [scale.x, scale.y, scale.z]
        .into_iter()
        .all(|component| component.is_finite() && component.abs() > f32::EPSILON)
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn set_transform_marks_transform_dirty_without_structure_change() {
        let mut scene = Scene::new();
        let structure_before = scene.structure_revision;
        let transform_before = scene.transform_revision;

        scene
            .set_transform(scene.root(), Transform::at(Vec3::new(1.0, 0.0, 0.0)))
            .expect("root transform updates");

        assert_eq!(
            scene.structure_revision, structure_before,
            "moving an existing node must not invalidate static scene structure"
        );
        assert!(
            scene.transform_revision > transform_before,
            "moving an existing node must mark transform-dependent prepared data dirty"
        );
    }
}
