use std::collections::VecDeque;
use std::f32::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 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, &ConfirmHistory), Changed<Position>>,
    mut interpolation_rot_query: Query<(&Rotation, &AngularVelocity, &mut RotationInterpolationInfo), 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/* - server_clock.rtt/2.0 - server_clock.time_offset*/;
        //let now = confirm_history.last_tick().get() as f32 / TICK_RATE as f32;
        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 {
            //debug!("pop");
            info.positions.drain(..last_over_time);
        }
        //debug!("{:?}", info.positions)
        /*if let Some((time, _)) = info.positions.get(0) && now.duration_since(*time) > INTERP {
            debug!("pop");
            pos_info.positions.pop_front();
        }*/
    }
    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, &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 }
        //debug!("{:?} {:?} {:?} {}", entity, first_time.1, second_time.1, progress);
        transform.translation = (first_time.1 + progress as f32 * (second_time.1 - first_time.1)).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));
    }
}