use std::f32::consts::PI;
use std::time::{Duration};
use avian2d::parry::transformation::utils::transform;
use crate::prelude::*;

pub fn interpolation_plugin(app: &mut App) {
    app
        .add_systems(Update, update_interpolation_info)
        .add_systems(Update, sync_non_interpolated_transforms)
        .add_systems(Update, do_interpolation);
}

#[derive(Component, Debug)]
pub struct TranslationInterpolationInfo {
    pub last_dt: Duration,
    pub this_tick_start: bevy::platform::time::Instant,
    pub latest_position: Vec2,
    pub last_position: Vec2,
}
#[derive(Component, Debug)]
pub struct RotationInterpolationInfo {
    pub last_dt: Duration,
    pub this_tick_start: bevy::platform::time::Instant,
    pub last_angular_velocity: f32,
    pub latest_rotation: f32,
    pub last_rotation: f32,
}

fn update_interpolation_info(
    mut interpolation_pos_query: Query<(Entity, &Position, &mut TranslationInterpolationInfo), Changed<Position>>,
    mut interpolation_rot_query: Query<(&Rotation, &AngularVelocity, &mut RotationInterpolationInfo), Changed<Rotation>>,
) {
    for (entity, position, mut info) in interpolation_pos_query.iter_mut() {
        info.last_dt = info.this_tick_start.elapsed();
        info.this_tick_start = bevy::platform::time::Instant::now();
        info.last_position = info.latest_position;
        info.latest_position = position.as_vec2();
    }
    for (rotation, angular_velocity, mut info) in interpolation_rot_query.iter_mut() {
        info.last_dt = info.this_tick_start.elapsed();
        info.this_tick_start = bevy::platform::time::Instant::now();
        let avg_angular_velocity = (angular_velocity.0.val_num_f32() + info.last_angular_velocity) / 2.0;
        let delta_rotation = avg_angular_velocity * info.last_dt.as_secs_f32();
        let num_revolutions = (delta_rotation / (2.0*PI)).round();
        info.last_rotation = info.latest_rotation;
        let mut rotation_offset = 0.0;
        let rotation = rotation.as_radians().val_num_f32();
        if delta_rotation + info.last_rotation >= PI {
            rotation_offset = 2.0*PI;
        } else if delta_rotation + info.last_rotation < -PI {
            rotation_offset = -2.0*PI;
        }
        info.latest_rotation = rotation + rotation_offset + num_revolutions*2.0*PI;
        info.last_angular_velocity = angular_velocity.0.val_num_f32();
    }
}
fn sync_non_interpolated_transforms(
    mut query: Query<(&mut Transform, &Position), (Changed<Position>, Without<TranslationInterpolationInfo>)>,
) {
    for (mut transform, position) in &mut query {
        transform.translation = position.as_vec2().extend(0.0);
    }
}
fn do_interpolation(
    mut interpolation_query: Query<(Entity, &mut Transform, &TranslationInterpolationInfo, &RotationInterpolationInfo)>,
) {
    for (entity, mut transform, pos_info, rot_info) in &mut interpolation_query {
        let dt = bevy::platform::time::Instant::now() - pos_info.this_tick_start;
        let progress = dt.as_secs_f32() / pos_info.last_dt.as_secs_f32(); // should be between 0.0 and 1.0
        transform.translation = (pos_info.last_position + progress * (pos_info.latest_position - pos_info.last_position)).extend(0.0);

        let dt = bevy::platform::time::Instant::now() - rot_info.this_tick_start;
        let progress = dt.as_secs_f32() / rot_info.last_dt.as_secs_f32(); // should be between 0.0 and 1.0
        transform.rotation = Quat::from_rotation_z(rot_info.last_rotation + progress * (rot_info.latest_rotation - rot_info.last_rotation));
    }
}