bevy_a5 0.1.1

//! ECS systems for transform propagation and floating origin management.

use bevy_ecs::prelude::*;
use bevy_math::{DVec3, Quat, Vec3};
use bevy_transform::components::{GlobalTransform, Transform};

use crate::cell::GeoCell;
use crate::coord;
use crate::origin::FloatingOrigin;
use crate::orientation;
use crate::planet::PlanetSettings;

/// Get the 3D center position and orientation for a cell on a planet.
///
/// The orientation is the **vertex-aligned** frame
/// ([`orientation::cell_vertex_orientation`]): the cell centre is the local
/// origin, +Y is radial-up, and -Z (Bevy "forward") points from the cell
/// centre to the cell's first boundary vertex.
///
/// This means a cell-anchored entity with `Transform::from_xyz(0.0, 0.0, -d)`
/// sits `d` metres along the line from the cell centre to its first vertex —
/// the canonical "north" of the cell.
fn cell_frame(cell: u64, radius: f64) -> Option<(DVec3, Quat)> {
    let center = coord::cell_to_dvec3(cell, radius)?;
    let orient = orientation::cell_vertex_orientation(cell, radius)?;
    Some((center, orient))
}

/// Recentre the floating origin when it drifts too far from its cell centre.
///
/// When `transform.translation.length()` exceeds the origin's
/// [`FloatingOrigin::recenter_threshold`], this system:
/// 1. Computes the origin's current sphere-space world position.
/// 2. Looks up the cell at that lon/lat at the same resolution.
/// 3. Re-expresses both translation **and** rotation in the new cell's
///    vertex-aligned frame.
/// 4. Writes the new `GeoCell` and the rebased `Transform`.
///
/// Because the world position and the world-space rotation are both
/// preserved across the rebasing, a recentre is *visually invisible* to a
/// camera attached to the origin: only the `(GeoCell, Transform)`
/// decomposition changes.
pub fn recenter_floating_origin(
    planet: Res<PlanetSettings>,
    mut origin_query: Query<(&FloatingOrigin, &mut GeoCell, &mut Transform)>,
) {
    for (origin, mut geo_cell, mut transform) in origin_query.iter_mut() {
        let distance = transform.translation.length();
        if distance <= origin.recenter_threshold {
            continue;
        }

        let Some((old_center, old_orient)) = cell_frame(geo_cell.raw(), planet.radius) else {
            continue;
        };

        // Origin's current absolute (sphere-space) world position and rotation.
        let world_pos = old_center + DVec3::from(old_orient * transform.translation);
        let world_rot = old_orient * transform.rotation;

        // Find the nearest cell at the same resolution.
        let Some(ll) = coord::dvec3_to_lonlat(world_pos) else {
            continue;
        };
        let resolution = geo_cell.resolution();
        let Some(new_cell) =
            GeoCell::from_lon_lat(ll.longitude(), ll.latitude(), resolution)
        else {
            continue;
        };
        let Some((new_center, new_orient)) = cell_frame(new_cell.raw(), planet.radius) else {
            continue;
        };

        // Rebase translation and rotation into the new cell's local frame.
        let inv_new_orient = new_orient.inverse();
        let local_offset = inv_new_orient * (world_pos - new_center).as_vec3();
        let local_rot = inv_new_orient * world_rot;

        geo_cell.set_if_neq(new_cell);
        transform.translation = local_offset;
        transform.rotation = local_rot;
    }
}

/// System that computes [`GlobalTransform`] for entities with a [`GeoCell`],
/// relative to the floating origin.
///
/// This is the core transform propagation system. It converts each entity's
/// cell position + local transform into a world-space position relative to
/// the floating origin entity.
pub fn propagate_geo_transforms(
    planet: Res<PlanetSettings>,
    origin_query: Query<(&GeoCell, &Transform), With<FloatingOrigin>>,
    mut entity_query: Query<
        (&GeoCell, &Transform, &mut GlobalTransform),
        Without<FloatingOrigin>,
    >,
) {
    let Ok((origin_cell, origin_transform)) = origin_query.single() else {
        return;
    };

    let Some((origin_center, origin_orient)) = cell_frame(origin_cell.raw(), planet.radius)
    else {
        return;
    };
    let inv_origin_orient = origin_orient.inverse();
    let origin_world_offset = DVec3::from(origin_orient * origin_transform.translation);

    for (entity_cell, entity_transform, mut global_transform) in entity_query.iter_mut() {
        let Some((entity_center, entity_orient)) =
            cell_frame(entity_cell.raw(), planet.radius)
        else {
            continue;
        };

        let entity_world_offset = DVec3::from(entity_orient * entity_transform.translation);

        // Position relative to the origin, in origin's local frame
        let relative_pos =
            (entity_center + entity_world_offset) - (origin_center + origin_world_offset);
        let local_pos: Vec3 = inv_origin_orient * relative_pos.as_vec3();

        let relative_rotation =
            inv_origin_orient * entity_orient * entity_transform.rotation;

        *global_transform = GlobalTransform::from(Transform {
            translation: local_pos,
            rotation: relative_rotation,
            scale: entity_transform.scale,
        });
    }
}

/// Update the floating origin entity's own [`GlobalTransform`].
///
/// The floating origin is the rendering reference point. In the big-space
/// convention used by `bevy_a5` (and `big_space`), the origin always renders
/// at world `(0, 0, 0)`: we set its `GlobalTransform.translation` to zero and
/// keep its `Transform.rotation` and `Transform.scale`. Other cell-anchored
/// entities are placed by [`propagate_geo_transforms`] in the origin's local
/// frame relative to the origin's *true* sphere-space world position
/// (`origin_center + origin_orient * origin_transform.translation`), so the
/// camera-to-entity geometry comes out correct without the origin itself
/// drifting through world space.
pub fn update_origin_global_transform(
    mut origin_query: Query<(&Transform, &mut GlobalTransform), With<FloatingOrigin>>,
) {
    for (transform, mut global_transform) in origin_query.iter_mut() {
        *global_transform = GlobalTransform::from(Transform {
            translation: Vec3::ZERO,
            rotation: transform.rotation,
            scale: transform.scale,
        });
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use bevy_ecs::schedule::Schedule;
    use bevy_ecs::world::World;

    fn run_systems(world: &mut World) {
        let mut schedule = Schedule::default();
        schedule.add_systems(
            (
                recenter_floating_origin,
                propagate_geo_transforms,
                update_origin_global_transform,
            )
                .chain(),
        );
        schedule.run(world);
    }

    fn fresh_world() -> World {
        let mut world = World::new();
        world.insert_resource(PlanetSettings::earth().with_radius(100.0));
        world
    }

    #[test]
    fn origin_renders_at_world_zero_translation() {
        let mut world = fresh_world();
        let origin_cell = GeoCell::from_lon_lat(2.3522, 48.8566, 5).unwrap();
        let origin = world
            .spawn((
                FloatingOrigin::with_threshold(1_000.0),
                origin_cell,
                Transform::from_xyz(7.5, -2.0, 3.0),
                GlobalTransform::default(),
            ))
            .id();

        run_systems(&mut world);

        let gt = world.get::<GlobalTransform>(origin).unwrap().translation();
        assert!(
            gt.length() < 1e-4,
            "FloatingOrigin must render at world zero (got {gt:?})"
        );
    }

    #[test]
    fn entity_in_same_cell_renders_relative_to_origin() {
        let mut world = fresh_world();
        let cell = GeoCell::from_lon_lat(2.3522, 48.8566, 5).unwrap();
        let origin_offset = Vec3::new(5.0, 1.0, -2.0);
        world.spawn((
            FloatingOrigin::with_threshold(1_000.0),
            cell,
            Transform::from_translation(origin_offset),
            GlobalTransform::default(),
        ));
        let entity = world
            .spawn((cell, Transform::default(), GlobalTransform::default()))
            .id();

        run_systems(&mut world);

        // The entity sits at the cell centre; the origin sits at +origin_offset
        // within the same cell. From the origin's frame, the entity should be
        // at -origin_offset.
        let gt = world.get::<GlobalTransform>(entity).unwrap().translation();
        assert!(
            (gt + origin_offset).length() < 1e-3,
            "expected ~{:?}, got {gt:?}",
            -origin_offset
        );
    }

    #[test]
    fn recenter_preserves_world_pose() {
        let mut world = fresh_world();
        let cell = GeoCell::from_lon_lat(2.3522, 48.8566, 5).unwrap();
        let pre_translation = Vec3::new(60.0, 0.0, 0.0);
        let pre_rotation = Quat::from_rotation_y(0.7);
        let origin = world
            .spawn((
                FloatingOrigin::with_threshold(50.0),
                cell,
                Transform {
                    translation: pre_translation,
                    rotation: pre_rotation,
                    scale: Vec3::ONE,
                },
                GlobalTransform::default(),
            ))
            .id();

        let planet = world.resource::<PlanetSettings>().clone();
        let (pre_centre, pre_orient) = cell_frame(cell.raw(), planet.radius).unwrap();
        let pre_world_pos = pre_centre + DVec3::from(pre_orient * pre_translation);
        let pre_world_rot = pre_orient * pre_rotation;

        run_systems(&mut world);

        let new_cell = *world.get::<GeoCell>(origin).unwrap();
        assert_ne!(new_cell.raw(), cell.raw(), "expected a recentre");

        let post_transform = *world.get::<Transform>(origin).unwrap();
        let (post_centre, post_orient) =
            cell_frame(new_cell.raw(), planet.radius).unwrap();
        let post_world_pos =
            post_centre + DVec3::from(post_orient * post_transform.translation);
        let post_world_rot = post_orient * post_transform.rotation;

        assert!(
            (post_world_pos - pre_world_pos).length() < 1e-3,
            "world position drifted: {} -> {}",
            pre_world_pos,
            post_world_pos
        );
        assert!(
            (post_world_rot.dot(pre_world_rot).abs() - 1.0).abs() < 1e-4,
            "world rotation drifted",
        );
    }
}

/// Compute the 3D world position of a [`GeoCell`] with a local offset,
/// relative to an origin cell and its offset.
///
/// This is a utility function for manual position computation outside of ECS systems.
pub fn relative_position(
    cell: &GeoCell,
    local_offset: Vec3,
    origin_cell: &GeoCell,
    origin_offset: Vec3,
    planet: &PlanetSettings,
) -> Option<Vec3> {
    let (cell_center, cell_orient) = cell_frame(cell.raw(), planet.radius)?;
    let (origin_center, origin_orient) = cell_frame(origin_cell.raw(), planet.radius)?;

    let cell_world = cell_center + DVec3::from(cell_orient * local_offset);
    let origin_world = origin_center + DVec3::from(origin_orient * origin_offset);

    let relative = cell_world - origin_world;
    let inv_origin = origin_orient.inverse();
    Some(inv_origin * relative.as_vec3())
}