use std::f32::consts::PI;

use crate::attachment::{Joint, JointOf, Joints, PartInShip, Peer, SnapOf, SnapOfJoint};
use crate::ecs::Me;
use crate::client::colors::GREEN;
use crate::ecs::{DragRequestEvent, Part, MAIN_LAYER};
use crate::client::input::CursorWorldCoordinates;
use bevy::color::palettes::css::{ORANGE, PURPLE, RED, YELLOW};
use crate::client::ship::attachment::AttachmentDebugRes;
use crate::prelude::*;

pub fn parts_plugin(app: &mut App) {
    app.insert_resource(DragResource(None));
    app.insert_resource(SnapResource(None, None));
    app.add_systems(PreUpdate, update_drag_ghosts);
    app.add_systems(
        Update,
        (
            handle_incoming_parts,
            handle_updated_parts,
            update_part_sprites,
        ),
    );
    app.add_observer(on_part_release);
}

fn handle_incoming_parts(
    mut commands: Commands,
    new_parts: Query<(Entity, &Part, Option<&PartInShip>), Added<Part>>,
    asset_server: Res<AssetServer>,
) {
    for (new_entity, new_part, is_connected) in new_parts.iter() {
        let mut sprite = Sprite::from_image(asset_server.load(if is_connected.is_some() {
            &new_part.strong_config.part.sprite_connected
        } else {
            &new_part.strong_config.part.sprite_disconnected
        }));
        sprite.custom_size = Some(Vec2::new(
            new_part.strong_config.physics.width,
            new_part.strong_config.physics.height,
        ));

        commands
            .entity(new_entity)
            .insert(MAIN_LAYER)
            .insert(sprite)
            .insert(Pickable::default())
            .observe(on_part_click);
    }
}
fn handle_updated_parts(
    mut commands: Commands,
    updated_parts: Query<(Entity, &Part, Option<&PartInShip>), Changed<Part>>,
    asset_server: Res<AssetServer>,
) {
    for (updated_entity, updated_part, is_connected) in updated_parts.iter() {
        let mut sprite = Sprite::from_image(asset_server.load(if is_connected.is_some() {
            &updated_part.strong_config.part.sprite_connected
        } else {
            &updated_part.strong_config.part.sprite_disconnected
        }));
        sprite.custom_size = Some(Vec2::new(
            updated_part.strong_config.physics.width,
            updated_part.strong_config.physics.height,
        ));

        commands
            .entity(updated_entity)
            .remove::<Sprite>()
            .insert(sprite);
    }
}

fn update_part_sprites(
    added: Query<Entity, Added<PartInShip>>,
    mut removed: RemovedComponents<PartInShip>,
    parts: Query<(&Part, Option<&PartInShip>)>,
    asset_server: Res<AssetServer>,
    mut commands: Commands,
) {
    for e in added.into_iter().chain(removed.read()) {
        let Ok((part, connected_to)) = parts.get(e) else {
            continue;
        };

        let sprite = if connected_to.is_some() {
            &part.strong_config.part.sprite_connected
        } else {
            &part.strong_config.part.sprite_disconnected
        };

        let mut sprite = Sprite::from_image(asset_server.load(sprite));
        sprite.custom_size = Some(Vec2::new(
            part.strong_config.physics.width,
            part.strong_config.physics.height,
        ));

        commands.entity(e).insert(sprite);
    }
}

#[derive(Resource)]
struct DragResource(Option<Entity>);
#[derive(Resource)]
struct SnapResource(Option<Entity>, Option<Entity>);

#[derive(Component)]
struct DragGhost;
#[derive(Component)]
struct Ghost {
    pub rot: Quat,
    pub last_target_pos: Vec3,
    pub vel: Vec3,
    pub part: Entity
}

const ROTATION_SMOOTH: f32 = 0.1;

fn on_part_click(
    ev: On<Pointer<Press>>,
    sprites: Query<(&Sprite, &Transform, &Joints), Without<Me>>,
    joints: Query<&Joint, Without<Peer>>,
    mut drag: ResMut<DragResource>,
    mut commands: Commands,
) {
    if ev.button != PointerButton::Primary {
        return;
    }
    let Ok(sprite) = sprites.get(ev.event_target()) else {
        return;
    };
    // make sure it has at least 1 valid unpeered joint
    let mut valid_peers = 0;
    for joint in &**sprite.2 {
        if joints.get(*joint).is_ok() {
            valid_peers += 1;
        }
    }
    if valid_peers == 0 {
        return; // ignore
    }
    let mut s = sprite.0.clone();
    s.color = Color::srgba(0.7, 0.7, 0.7, 1.0);
    commands.spawn((DragGhost, Ghost {
        rot: sprite.1.rotation,
        last_target_pos: sprite.1.translation,
        vel: Vec3::ZERO,
        part: ev.event_target(),
    }, *sprite.1, s));

    drag.0 = Some(ev.event().event_target());
}

fn on_part_release(
    ev: On<Pointer<Release>>,
    mut drag: ResMut<DragResource>,
    mut events: MessageWriter<DragRequestEvent>,
    cursor: Res<CursorWorldCoordinates>,
    mut commands: Commands,
    ghost: Single<(Entity, &Transform), With<DragGhost>>,
    snap: Res<SnapResource>,
) {
    if ev.button != PointerButton::Primary {
        return;
    }

    if let Some(e) = drag.0 {
        let rotation = ghost.1.rotation;
        commands.entity(ghost.0).despawn();

        if let Some(c) = cursor.0 {
            debug!(?e, ?c, "sending drag request");
            events.write(DragRequestEvent {
                drag_target: e,
                drag_to: c,
                set_rotation: rotation,
                snap_target: snap.0,
                peer_snap: snap.1,
            });
        }
    }

    drag.0 = None;
}

const F: f32 = 2.0; // frequency (Hz)
// 0.0 is undamped, 0.0-1.0 is underdamped, 1.0 is critical, and >1.0 is over
const ZETA: f32 = 0.7; // damping
const R: f32 = -0.6; // initial response speed
const K1: f32 = ZETA/(PI*F);
const K2: f32 = 1.0/((2.0*PI*F)*(2.0*PI*F));
const K3: f32 = (R*ZETA)/(2.0*PI*F);

fn smoothing_a(x: Vec3, x_dot: Vec3, r: Vec3, v: Vec3) -> Vec3 {
    x/K2 + K3/K2*x_dot - r/K2 - K1/K2*v
}

// y + K1*y' + K2*y'' = x + K3*x'
// K2*y'' = x - y + K3*x' - K1*y'
// y''(t) = (x - y + K3*x' - K1*y')/K2
// r(t) = y(t)
// v(t) = y'(t)
// r'(t) = y'(t) = v(t)
// v'(t) = y''(t) = (x - y(t) + K3*x' - K1*y'(t))/K2
//                = x/K2 + K3/K2*x' - r(t)/K2 - K1/K2*v(t)
// [ r' ] = [ 0    1     ] * [ r(t) ] + [ 0 ]
// [ v' ] = [ 1/K2 K1/K2 ] * [ v(t) ] + [ 1 ] * x/K2 + K3/K2*x'
fn rk4_k_values(x: Vec3, x_dot:Vec3, r: Vec3, v: Vec3, dt: f32)
    -> (Vec3, Vec3, Vec3, Vec3, Vec3, Vec3, Vec3, Vec3) {
    let a = smoothing_a(x, x_dot, r, v);

    let r_k1 = v;
    let v_k1 = a;

    let r_1 = r + r_k1*dt/2.0;
    let v_1 = v + v_k1*dt/2.0;

    let r_k2 = v_1;
    let v_k2 = smoothing_a(x, x_dot, r_1, v_1);

    let r_2 = r + r_k2*dt/2.0;
    let v_2 = v + v_k2*dt/2.0;

    let r_k3 = v_2;
    let v_k3 = smoothing_a(x, x_dot, r_2, v_2);

    let r_3 = r + r_k3*dt;
    let v_3 = v + v_k3*dt;

    let r_k4 = v_3;
    let v_k4 = smoothing_a(x, x_dot, r_3, v_3);

    (r_k1, v_k1, r_k2, v_k2, r_k3, v_k3, r_k4, v_k4)
}

/// !IMPORTANT!
/// This function forms the bulk of the attachment system.
/// PLEASE DO NOT MODIFY OR MOVE
///
/// This code is super cursed, and it will break at the lightest breeze
fn update_drag_ghosts(
    ghost: Single<
        (&mut Transform, &mut Ghost),
        (
            With<DragGhost>,
            Without<SnapOf>,
            Without<JointOf>,
            Without<Part>,
        ),
    >,
    cursor: Res<CursorWorldCoordinates>,
    snaps: Query<(&Transform, &SnapOfJoint, &SnapOf, Entity)>,
    joints: Query<(&Transform, &JointOf, Entity), Without<Peer>>,
    parts: Query<(&GlobalTransform, Option<&Me>, Option<&PartInShip>), With<Part>>,
    linvel: Query<&LinearVelocity, With<Me>>,
    dt: Res<Time<Fixed>>,
    me: Single<Entity, With<Me>>,
    debug: Res<AttachmentDebugRes>,
    mut rsnap: ResMut<SnapResource>,
    drag: Res<DragResource>,
    mut gizmos: Gizmos,
    keys: Res<ButtonInput<KeyCode>>,
    time: Res<Time>,
) {
    const CUTOFF: f32 = 25.0; // px

    let (mut ghost, mut ghost_info) = ghost.into_inner();
    let Some(cursor) = cursor.0 else { return };

    let mut best_distance = f32::INFINITY;
    let mut best_target = Transform::from_xyz(cursor.x, cursor.y, 0.0);
    best_target.rotation = ghost_info.rot;
    let mut snap = None;
    let mut best_parent_position = None;
    let mut best_snap_position = None;
    let mut best_self_snap = None;
    let mut best_self_snap_position = None;
    let mut best_self_position = None;

    for (snap_local_transform, snap_joint, snap_part, snap_id) in &snaps {
        if Some(snap_part.0) == drag.0 {
            continue;
        }

        // only snap to ourselves

        let Ok((part_being_snapped_to, maybe_me, maybe_parent_ship)) = parts.get(snap_part.0)
        else {
            continue;
        };

        let allowed = maybe_me.is_some() || maybe_parent_ship.is_some_and(|u| u.0 == *me);
        if !allowed {
            continue;
        }

        let snap_global_translation = part_being_snapped_to.mul_transform(*snap_local_transform);

        let distance_to_cursor = cursor.distance(snap_global_translation.translation().xy());

        if distance_to_cursor > best_distance {
            if debug.0 {
                gizmos.circle_2d(snap_global_translation.translation().xy(), 3.0, RED);
            }
            continue;
        }
        if distance_to_cursor > CUTOFF {
            if debug.0 {
                gizmos.circle_2d(snap_global_translation.translation().xy(), 3.0, ORANGE);
            }
            continue;
        }

        if debug.0 {
            gizmos.circle_2d(snap_global_translation.translation().xy(), 3.0, GREEN);
        }

        let Ok((offset, _, _)) = joints.get(snap_joint.0) else {
            continue;
        };

        let joint_target = part_being_snapped_to.transform_point(offset.translation);

        if debug.0 {
            gizmos.circle_2d(joint_target.xy(), 3.0, GREEN);
        }

        snap = Some(snap_id);
        let mut target_transform = Transform {
            translation: joint_target,
            rotation: ghost.rotation,
            scale: offset.scale,
        };
        best_distance = distance_to_cursor;
        best_parent_position = Some(part_being_snapped_to.translation().xy());
        best_snap_position = Some(snap_global_translation);

        // find, in this particular situation, the best peer on ourselves
        let mut best_peer_snap_distance = f32::INFINITY;
        let mut best_peer_snap = None;
        let mut best_peer_snap_pos = None;
        let mut best_peer_target_pos = None;
        let mut best_joint_transform = None;
        for (our_snap_local_transform, our_snap_joint, our_snap_part, our_snap_id) in &snaps {
            if Some(our_snap_part.0) != drag.0 {
                continue;
            }
            let our_snap_global_translation =
                target_transform.mul_transform(*our_snap_local_transform);
            let distance = our_snap_global_translation
                .translation
                .distance(snap_global_translation.translation());
            if distance > best_peer_snap_distance {
                continue;
            }

            let Ok((our_joint, _, _)) = joints.get(our_snap_joint.0) else {
                continue;
            };

            best_peer_snap = Some(our_snap_id);
            best_peer_snap_distance = distance;
            best_peer_snap_pos = Some(our_snap_global_translation.translation.xy());
            best_peer_target_pos = Some(target_transform.translation.xy());
            best_joint_transform = Some(our_joint);
        }
        target_transform.rotation = part_being_snapped_to.rotation()
            * (offset.rotation * best_joint_transform.unwrap().rotation.inverse())
            * Quat::from_rotation_z(180.0f32.to_radians());
        best_target = target_transform;
        best_self_snap = best_peer_snap;
        best_self_snap_position = best_peer_snap_pos;
        best_self_position = best_peer_target_pos;
    }

    if debug.0
        && let Some(part) = best_parent_position
        && let Some(snap) = best_snap_position
    {
        gizmos.arrow_2d(part, snap.translation().xy(), YELLOW);
    }
    if debug.0
        && let Some(part) = best_self_position
        && let Some(snap) = best_self_snap_position
    {
        gizmos.arrow_2d(part, snap, PURPLE);
    }

    if keys.pressed(KeyCode::KeyQ) {
        best_target.rotation = best_target.rotation.mul_quat(Quat::from_rotation_z(
            180.0_f32.to_radians() * time.delta_secs(),
        ));
    }
    if keys.pressed(KeyCode::KeyE) {
        best_target.rotation = best_target.rotation.mul_quat(Quat::from_rotation_z(
            -180.0_f32.to_radians() * time.delta_secs(),
        ));
    }
    ghost_info.rot = best_target.rotation;
    ghost.rotation = ghost.rotation.slerp(ghost_info.rot, ROTATION_SMOOTH);

    let target_vel = (best_target.translation - ghost_info.last_target_pos)
        /time.delta_secs();


    let a = (best_target.translation - ghost.translation
        + K3*target_vel - K1*ghost_info.vel)/K2;
    let (r_k1, v_k1, r_k2, v_k2, r_k3, v_k3, r_k4, v_k4) = rk4_k_values(best_target.translation, target_vel, ghost.translation, ghost_info.vel, time.delta_secs());

    ghost.translation += (r_k1 + 2.0*r_k2 + 2.0*r_k3 + r_k4)/6.0*time.delta_secs();
    ghost_info.vel += (v_k1 + 2.0*v_k2 + 2.0*v_k3 + v_k4)/6.0*time.delta_secs();

    ghost_info.last_target_pos = best_target.translation;

    let partvel = linvel.single().unwrap();
    let t_dt = dt.delta_secs();
    ghost.translation.x += partvel.x * t_dt;
    ghost.translation.y += partvel.y * t_dt;

    rsnap.0 = snap;
    rsnap.1 = best_self_snap;
}