bevy_lagrange 0.0.3

use bevy::prelude::*;
use bevy_kana::Position;

use super::constants::EPSILON;
use super::constants::MIN_ORBIT_RADIUS;
use super::constants::PERSPECTIVE_NEAR_MIN;
use super::constants::PERSPECTIVE_NEAR_RADIUS_FACTOR;
use super::constants::SMOOTHNESS_EXPONENT;

pub(crate) fn calculate_from_translation_and_focus(
    translation: impl Into<Position>,
    focus: impl Into<Position>,
    axis: [Vec3; 3],
) -> (f32, f32, f32) {
    let translation = translation.into();
    let focus = focus.into();
    let axis = Mat3::from_cols(axis[0], axis[1], axis[2]);
    let comp_vec = *translation - *focus;
    let mut radius = comp_vec.length();
    if radius < f32::EPSILON {
        radius = MIN_ORBIT_RADIUS;
    }
    let comp_vec = axis * comp_vec;
    let yaw = comp_vec.x.atan2(comp_vec.z);
    let pitch = (comp_vec.y / radius).asin();
    (yaw, pitch, radius)
}

/// Update `transform` based on yaw, pitch, and the camera's focus and radius
pub(crate) fn update_orbit_transform(
    yaw: f32,
    pitch: f32,
    mut radius: f32,
    focus: impl Into<Position>,
    transform: &mut Transform,
    projection: &mut Projection,
    axis: [Vec3; 3],
) {
    let focus = focus.into();
    let mut new_transform = Transform::IDENTITY;
    match &mut *projection {
        Projection::Orthographic(p) => {
            p.scale = radius;
            // IMPORTANT: Do NOT replace this with `f32::midpoint()`.
            // On aarch64, `midpoint()` promotes to f64 intermediate precision:
            //   ((self as f64 + other as f64) / 2.0) as f32
            // This produces a subtly different camera distance than plain f32 arithmetic.
            // That tiny difference shifts the projected screen-space bounds just enough
            // to flip the fit overlay balance check (tolerance: 0.001) — causing
            // all margin labels to show green/balanced when they should show red/unbalanced.
            #[expect(
                clippy::manual_midpoint,
                reason = "f32::midpoint uses f64 on aarch64, breaking fit visualization balance detection"
            )]
            {
                radius = (p.near + p.far) / 2.0;
            }
        },
        Projection::Perspective(p) => sync_perspective_near_clip(p, radius),
        Projection::Custom(_) => {},
    }
    let yaw_rot = Quat::from_axis_angle(axis[1], yaw);
    let pitch_rot = Quat::from_axis_angle(axis[0], -pitch);
    new_transform.rotation *= yaw_rot * pitch_rot;
    new_transform.translation += *focus + new_transform.rotation * Vec3::new(0.0, 0.0, radius);
    *transform = new_transform;
}

fn sync_perspective_near_clip(projection: &mut PerspectiveProjection, radius: f32) {
    let new_near = (radius * PERSPECTIVE_NEAR_RADIUS_FACTOR)
        .max(PERSPECTIVE_NEAR_MIN)
        .min(projection.far);
    projection.near = new_near;
    projection.near_clip_plane = Vec4::new(0.0, 0.0, -1.0, -new_near);
}

pub(crate) const fn approx_equal(a: f32, b: f32) -> bool { (a - b).abs() < EPSILON }

pub(crate) fn lerp_and_snap_f32(from: f32, to: f32, smoothness: f32, dt: f32) -> f32 {
    let t = smoothness.powi(SMOOTHNESS_EXPONENT);
    let mut new_value = from.lerp(to, 1.0 - t.powf(dt));
    if smoothness < 1.0 && approx_equal(new_value, to) {
        new_value = to;
    }
    new_value
}

pub(crate) fn lerp_and_snap_position(
    from: impl Into<Position>,
    to: impl Into<Position>,
    smoothness: f32,
    dt: f32,
) -> Position {
    let from = from.into();
    let to = to.into();
    let t = smoothness.powi(SMOOTHNESS_EXPONENT);
    let mut new_value = (*from).lerp(*to, 1.0 - t.powf(dt));
    if smoothness < 1.0 && approx_equal((new_value - *to).length(), 0.0) {
        new_value.x = to.x;
    }
    Position(new_value)
}

#[cfg(test)]
#[allow(
    clippy::unreadable_literal,
    clippy::float_cmp,
    reason = "test assertions verify deterministic bitwise-exact float results"
)]
mod calculate_from_translation_and_focus_tests {
    use std::f32::consts::PI;

    use float_cmp::approx_eq;

    use super::*;
    const AXIS: [Vec3; 3] = [Vec3::X, Vec3::Y, Vec3::Z];
    const AXIS_Z_UP: [Vec3; 3] = [Vec3::X, Vec3::Z, Vec3::Y];

    #[test]
    fn zero() {
        let translation = Position::new(0.0, 0.0, 0.0);
        let focus = Position::default();
        let (yaw, pitch, radius) = calculate_from_translation_and_focus(translation, focus, AXIS);
        assert_eq!(yaw, 0.0);
        assert_eq!(pitch, 0.0);
        assert_eq!(radius, MIN_ORBIT_RADIUS);
    }

    #[test]
    fn zero_z_up_axis() {
        let translation = Position::new(0.0, 0.0, 0.0);
        let focus = Position::default();
        let (yaw, pitch, radius) =
            calculate_from_translation_and_focus(translation, focus, AXIS_Z_UP);
        assert_eq!(yaw, 0.0);
        assert_eq!(pitch, 0.0);
        assert_eq!(radius, MIN_ORBIT_RADIUS);
    }

    #[test]
    fn in_front() {
        let translation = Position::new(0.0, 0.0, 5.0);
        let focus = Position::default();
        let (yaw, pitch, radius) = calculate_from_translation_and_focus(translation, focus, AXIS);
        assert_eq!(yaw, 0.0);
        assert_eq!(pitch, 0.0);
        assert_eq!(radius, 5.0);
    }

    #[test]
    fn in_front_z_up_axis() {
        let translation = Position::new(0.0, 5.0, 0.0);
        let axis = [Vec3::X, Vec3::Z, Vec3::Y];
        let focus = Position::default();
        let (yaw, pitch, radius) = calculate_from_translation_and_focus(translation, focus, axis);
        assert_eq!(yaw, 0.0);
        assert_eq!(pitch, 0.0);
        assert_eq!(radius, 5.0);
    }

    #[test]
    fn to_the_side() {
        let translation = Position::new(5.0, 0.0, 0.0);
        let focus = Position::default();
        let (yaw, pitch, radius) = calculate_from_translation_and_focus(translation, focus, AXIS);
        assert!(approx_eq!(f32, yaw, PI / 2.0));
        assert_eq!(pitch, 0.0);
        assert_eq!(radius, 5.0);
    }

    #[test]
    fn above() {
        let translation = Position::new(0.0, 5.0, 0.0);
        let focus = Position::default();
        let (yaw, pitch, radius) = calculate_from_translation_and_focus(translation, focus, AXIS);
        assert_eq!(yaw, 0.0);
        assert!(approx_eq!(f32, pitch, PI / 2.0));
        assert_eq!(radius, 5.0);
    }

    #[test]
    fn above_z_as_up_axis() {
        let translation = Position::new(0.0, 0.0, 5.0);
        let focus = Position::default();
        let (yaw, pitch, radius) =
            calculate_from_translation_and_focus(translation, focus, AXIS_Z_UP);
        assert_eq!(yaw, 0.0);
        assert!(approx_eq!(f32, pitch, PI / 2.0));
        assert_eq!(radius, 5.0);
    }

    #[test]
    fn arbitrary() {
        let translation = Position::new(0.92563736, 3.864204, -1.0105048);
        let focus = Position::default();
        let (yaw, pitch, radius) = calculate_from_translation_and_focus(translation, focus, AXIS);
        assert!(approx_eq!(f32, yaw, 2.4));
        assert!(approx_eq!(f32, pitch, 1.23));
        assert_eq!(radius, 4.1);
    }

    #[test]
    fn negative_x() {
        let translation = Position::new(-5.0, 5.0, 9.0);
        let focus = Position::default();
        let (yaw, pitch, radius) = calculate_from_translation_and_focus(translation, focus, AXIS);
        assert!(approx_eq!(f32, yaw, -0.5070985));
        assert!(approx_eq!(f32, pitch, 0.45209613));
        assert!(approx_eq!(f32, radius, 11.445523));
    }
}

#[cfg(test)]
#[allow(
    clippy::unreadable_literal,
    clippy::float_cmp,
    reason = "test assertions verify deterministic bitwise-exact float results"
)]
mod approx_equal_tests {
    use super::*;

    #[test]
    fn same_value_is_approx_equal() {
        assert!(approx_equal(1.0, 1.0));
    }

    #[test]
    fn value_within_threshold_is_approx_equal() {
        assert!(approx_equal(1.0, 1.0000001));
    }

    #[test]
    fn value_outside_threshold_is_not_approx_equal() {
        assert!(!approx_equal(1.0, 1.01));
    }
}

#[cfg(test)]
#[allow(
    clippy::float_cmp,
    reason = "tests verify exact near-plane sync behavior"
)]
mod sync_perspective_near_clip_tests {
    use super::*;

    #[test]
    fn near_plane_tracks_radius() {
        let mut projection = PerspectiveProjection::default();
        sync_perspective_near_clip(&mut projection, 2.0);
        assert_eq!(projection.near, 0.002);
        assert_eq!(
            projection.near_clip_plane,
            Vec4::new(0.0, 0.0, -1.0, -0.002)
        );
    }

    #[test]
    fn near_plane_respects_absolute_minimum() {
        let mut projection = PerspectiveProjection::default();
        sync_perspective_near_clip(&mut projection, 1e-9);
        assert_eq!(projection.near, PERSPECTIVE_NEAR_MIN);
        assert_eq!(
            projection.near_clip_plane,
            Vec4::new(0.0, 0.0, -1.0, -PERSPECTIVE_NEAR_MIN)
        );
    }

    #[test]
    fn near_plane_never_exceeds_far_plane() {
        let mut projection = PerspectiveProjection {
            far: 0.01,
            ..default()
        };
        sync_perspective_near_clip(&mut projection, 20.0);
        assert_eq!(projection.near, 0.01);
        assert_eq!(projection.near_clip_plane, Vec4::new(0.0, 0.0, -1.0, -0.01));
    }
}

#[cfg(test)]
#[allow(
    clippy::unreadable_literal,
    clippy::float_cmp,
    reason = "test assertions verify deterministic bitwise-exact float results"
)]
mod lerp_and_snap_f32_tests {
    use super::*;

    #[test]
    fn lerps_when_output_outside_snap_threshold() {
        let out = lerp_and_snap_f32(1.0, 2.0, 0.5, 1.0);
        // Due to the frame rate independence, this value is not easily predictable
        assert_eq!(out, 1.9921875);
    }

    #[test]
    fn snaps_to_target_when_inside_threshold() {
        let out = lerp_and_snap_f32(1.9991, 2.0, 0.5, 1.0);
        assert_eq!(out, 2.0);
        let out = lerp_and_snap_f32(1.9991, 2.0, 0.1, 1.0);
        assert_eq!(out, 2.0);
        let out = lerp_and_snap_f32(1.9991, 2.0, 0.9, 1.0);
        assert_eq!(out, 2.0);
    }

    #[test]
    fn does_not_snap_if_smoothness_is_one() {
        // Smoothness of one results in the value not changing, so it doesn't make sense to snap
        let out = lerp_and_snap_f32(1.9991, 2.0, 1.0, 1.0);
        assert_eq!(out, 1.9991);
    }
}

#[cfg(test)]
#[allow(
    clippy::unreadable_literal,
    clippy::float_cmp,
    reason = "test assertions verify deterministic bitwise-exact float results"
)]
mod lerp_and_snap_position_tests {
    use super::*;

    #[test]
    fn lerps_when_output_outside_snap_threshold() {
        let out = lerp_and_snap_position(Position::default(), Position(Vec3::X), 0.5, 1.0);
        // Due to the frame rate independence, this value is not easily predictable
        assert_eq!(out, Position::new(0.9921875, 0.0, 0.0));
    }

    #[test]
    fn snaps_to_target_when_inside_threshold() {
        let out = lerp_and_snap_position(Position(Vec3::X * 0.9991), Position(Vec3::X), 0.5, 1.0);
        assert_eq!(out, Position(Vec3::X));
        let out = lerp_and_snap_position(Position(Vec3::X * 0.9991), Position(Vec3::X), 0.1, 1.0);
        assert_eq!(out, Position(Vec3::X));
        let out = lerp_and_snap_position(Position(Vec3::X * 0.9991), Position(Vec3::X), 0.9, 1.0);
        assert_eq!(out, Position(Vec3::X));
    }

    #[test]
    fn does_not_snap_if_smoothness_is_one() {
        // Smoothness of one results in the value not changing, so it doesn't make sense to snap
        let out = lerp_and_snap_position(Position(Vec3::X * 0.9991), Position(Vec3::X), 1.0, 1.0);
        assert_eq!(out, Position(Vec3::X * 0.9991));
    }
}