use std::collections::VecDeque;
use std::f64::consts::PI;
use std::time::{Duration};
use avian2d::parry::transformation::utils::transform;
use bevy_replicon::client::confirm_history::ConfirmHistory;
use crate::client::components::{ServerClock, ServerTimeOffset};
use crate::prelude::*;
use crate::shared::plugins::TICK_RATE;

/// interpolation period in seconds
const INTERP: f64 = 0.150;

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 positions: VecDeque<(f64, Vec2)>,
}
#[derive(Component, Debug)]
pub struct RotationInterpolationInfo {
    pub rotations: VecDeque<(f64, f64)> // time and rotation
}

fn update_interpolation_info(
    mut interpolation_pos_query: Query<(Entity, &Position, &mut TranslationInterpolationInfo, &ConfirmHistory), Changed<Position>>,
    mut interpolation_rot_query: Query<(&Rotation, &AngularVelocity, &mut RotationInterpolationInfo, &ConfirmHistory), Changed<Rotation>>,
    server_clock: Res<ServerClock>,
    server_time_offset: Res<ServerTimeOffset>,
) {
    for (entity, position, mut info, confirm_history) in interpolation_pos_query.iter_mut() {
        let now = confirm_history.last_tick().get() as f64 / TICK_RATE - **server_time_offset;
        info.positions.push_back((now, position.as_vec2()));

        let mut last_over_time = 0;
        for (i, (time, _)) in info.positions.iter().enumerate() {
            if *time < now - INTERP {
                last_over_time = i;
            }
        }
        if last_over_time > 0 {
            info.positions.drain(..last_over_time);
        }
    }
    for (rotation, angular_velocity, mut info, confirm_history) in interpolation_rot_query.iter_mut() {
        let now = confirm_history.last_tick().get() as f64 / TICK_RATE - **server_time_offset;
        let delta_rotation = angular_velocity.0.to_radians() / TICK_RATE;
        let num_revolutions = (delta_rotation / (2.0*PI)).round();
        let mut rotation_offset = 0.0;
        let rotation = rotation.as_radians() as f64;
        if let Some((_, rotation)) = info.rotations.back() {
            if delta_rotation + rotation >= PI {
                rotation_offset = 2.0 * PI;
            } else if delta_rotation + rotation < -PI {
                rotation_offset = -2.0 * PI;
            }
        }
        info.rotations.push_back((now, rotation + rotation_offset + num_revolutions*2.0*PI));

        let mut last_over_time = 0;
        for (i, (time, _)) in info.rotations.iter().enumerate() {
            if *time < now - INTERP {
                last_over_time = i;
            }
        }
        if last_over_time > 0 {
            info.rotations.drain(..last_over_time);
        }
    }
}
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, &mut TranslationInterpolationInfo, &RotationInterpolationInfo)>,
    time: Res<Time>,
) {
    for (entity, mut transform, mut pos_info, rot_info) in &mut interpolation_query {
        let now = time.elapsed().as_secs_f64();

        let mut time_after_now = 0;
        for (i, (time, _)) in pos_info.positions.iter().enumerate() {
            if *time > now - INTERP {
                time_after_now = i;
                break;
            }
        }
        // this should not happen, but is necessary to prevent a panic
        if time_after_now == 0 { continue }
        // we need to have 2 packets to reference, so continuing if we don't have that is a-ok
        let Some(first_time) = pos_info.positions.get(time_after_now-1) else { continue };
        let Some(second_time) = pos_info.positions.get(time_after_now) else { continue };
        let elapsed = now - INTERP - first_time.0;
        let dt = second_time.0 - first_time.0;
        let progress = elapsed / dt; // should be between 0.0 and 1.0
        if progress < 0.0 || progress > 1.0 { continue }
        transform.translation = (first_time.1 + progress as f32 * (second_time.1 - first_time.1)).extend(0.0);

        let mut time_after_now = 0;
        for (i, (time, _)) in rot_info.rotations.iter().enumerate() {
            if *time > now - INTERP {
                time_after_now = i;
                break;
            }
        }
        // this should not happen, but is necessary to prevent a panic
        if time_after_now == 0 { continue }
        // we need to have 2 packets to reference, so continuing if we don't have that is a-ok
        let Some(first_time) = rot_info.rotations.get(time_after_now-1) else { continue };
        let Some(second_time) = rot_info.rotations.get(time_after_now) else { continue };
        let elapsed = now - INTERP - first_time.0;
        let dt = second_time.0 - first_time.0;
        let progress = elapsed / dt; // should be between 0.0 and 1.0
        if progress < 0.0 || progress > 1.0 { continue }
        transform.rotation = Quat::from_rotation_z((first_time.1 + progress * (second_time.1 - first_time.1)) as f32);
    }
}