use argh::FromArgs;
use bevy::prelude::*;

// toid
const SPEED: f32 = 10.0;
const SPEED_DIFF_RANGE: f32 = 0.08; // +/- 8%

// how far from origin before really wanting to come back
const RADIUS: f32 = 800.0;

// how close to try to stay to your buddies
const BUDDY_RADIUS: f32 = 15.0;

const MIN_ALTITUDE: f32 = 1.5;

pub type Point = [f32; 3];
pub type Toint = rstar::primitives::GeomWithData<Point, Entity>;

#[derive(Debug, FromArgs, Resource)]
/// Toid Watching
pub struct Config {
    /// how many Toids to spawn
    #[argh(option, short = 't', default = "100")]
    pub toids: usize,
}

#[derive(Resource, Deref, DerefMut, Default)]
pub struct Index(pub rstar::RTree<Toint>);

#[derive(Component, Debug, Clone, Deref, DerefMut, Default)]
pub struct Buddies(Vec<Toint>);

#[derive(Component, Debug, Clone, Deref, DerefMut, Default)]
pub struct Velocity(Vec3);

#[derive(Component)]
pub struct Toid {
    pub speed: f32,
    pub buddies: usize,
}

pub fn turkey_time(
    commands: &mut Commands,
    scene: &Handle<Scene>,
    r: &mut impl rand::prelude::Rng,
) -> Entity {
    let speed_diff = r.gen_range(-SPEED_DIFF_RANGE..=SPEED_DIFF_RANGE);
    let speed = SPEED + (SPEED * speed_diff);
    let vel = unit_vec(r) * speed;
    let buddies = r.gen_range(6..=8);
    let spatial_bundle = SpatialBundle {
        transform: Transform::from_xyz(0.0, MIN_ALTITUDE + 0.1, 0.0),
        ..Default::default()
    };
    commands
        .spawn(spatial_bundle)
        .insert((Velocity(vel), Buddies::default(), Toid { speed, buddies }))
        .with_children(|t| {
            t.spawn(SceneBundle {
                scene: scene.to_owned(),
                ..Default::default()
            })
            .insert(Transform::from_rotation(Quat::from_axis_angle(
                Vec3::Y,
                -std::f32::consts::FRAC_PI_2,
            )));
        })
        .id()
}

pub fn add_gizmos(mut gizmos: Gizmos, toids: Query<(&Transform, Entity), With<Toid>>) {
    let gizmos = &mut gizmos;
    for (pos, _entity) in toids.iter() {
        let nudge = pos.up() * 0.15;
        let rpos = pos.translation + nudge;
        gizmos.ray(rpos, pos.forward(), Color::RED);
    }
}

pub fn update_pos(
    mut toids: Query<(&mut Transform, &Velocity, Entity)>,
    time: Res<Time>,
    mut index: ResMut<Index>,
) {
    let mut positions = Vec::with_capacity(toids.iter().len());

    let dt = time.delta_seconds();
    for (mut xform, vel, entity) in toids.iter_mut() {
        let look_at = xform.translation + vel.0;
        xform.translation += vel.0 * dt;
        xform.look_at(look_at, Vec3::Y);
        let toint = Toint::new(xform.translation.to_array(), entity);
        positions.push(toint);
    }
    let tree = rstar::RTree::bulk_load(positions);
    **index = tree;
}

pub fn update_vel(
    mut toids: Query<(&Transform, &mut Velocity, &Toid), With<Toid>>,
    time: Res<Time>,
) {
    let dt = time.delta_seconds();
    for (xform, mut vel, toid) in toids.iter_mut() {
        let speed = toid.speed;
        let mut dir = vel.normalize();
        let pos = xform.translation;

        // nudge up if too low
        if pos.y < MIN_ALTITUDE {
            let dh = MIN_ALTITUDE - pos.y;
            let pct = (dh / MIN_ALTITUDE).min(1.0).powi(2);
            let rot = Quat::from_rotation_arc(dir, Vec3::Y);
            let (axis, ang) = rot.to_axis_angle();
            let rot = Quat::from_axis_angle(axis, ang * pct).normalize();
            dir = rot.mul_vec3(dir);
        }

        // nudge toward origin if too far
        {
            let dist = pos.length();
            let toward_origin = -pos.normalize();
            let forward = dir.normalize();
            let rot = Quat::from_rotation_arc(forward, toward_origin);
            let pct = (dist / RADIUS).min(1.0);
            let (axis, ang) = rot.to_axis_angle();
            let rot = Quat::from_axis_angle(axis, ang * pct).normalize();
            dir = rot.mul_vec3(dir);
        }
        // find buddies and orient: point more towards further-away buddies

        **vel = dir * speed;
    }
}

pub fn update_buddies(mut toids: Query<(&Transform, Entity, &mut Buddies)>, index: Res<Index>) {}

//-************************************************************************
// util
//-************************************************************************

pub fn unit_vec(r: &mut impl rand::Rng) -> Vec3 {
    let mut x1: f32 = 0.0;
    let mut x2: f32 = 0.0;
    let mut ssum = std::f32::MAX;
    while ssum >= 1.0 {
        x1 = r.gen_range(-1.0..=1.0);
        x2 = r.gen_range(-1.0..=1.0);
        ssum = x1.powi(2) + x2.powi(2);
    }
    let sqrt = (1.0 - ssum).sqrt();
    let x = 2.0 * x1 * sqrt;
    let y = 2.0 * x2 * sqrt;
    let z = 1.0 - 2.0 * ssum;
    Vec3::new(x, y, z).normalize()
}