use std::fmt::Debug;

use bevy::prelude::*;
use bevy_rapier3d::{
    geometry::Group,
    prelude::{
        Ccd, Collider, ColliderMassProperties, CollisionGroups, Damping, ExternalForce, Friction,
        ImpulseJoint, PrismaticJointBuilder, Restitution, RigidBody, Sleeping,
        TransformInterpolation, Velocity,
    },
};

use crate::planet::PLANET_RADIUS;

pub(crate) const SPAWN_ALTITUDE: f32 = PLANET_RADIUS * 0.2;

#[derive(Component)]
pub struct CyberBikeBody;

#[derive(Component)]
pub struct CyberBikeCollider;

#[derive(Component, Debug)]
pub struct CyberBikeModel;

#[derive(Component, Debug, Default, Clone, Copy)]
pub struct CyberBikeControl {
    pub error_sum: f32,
    pub prev_error: f32,
}

#[derive(Resource, Reflect)]
#[reflect(Resource)]
pub struct WheelConfig {
    pub front_forward: f32,
    pub front_stance: f32,
    pub rear_back: f32,
    pub y: f32,
    pub limits: [f32; 2],
    pub stiffness: f32,
    pub damping: f32,
}

impl Default for WheelConfig {
    fn default() -> Self {
        Self {
            front_forward: 0.9,
            front_stance: 1.2,
            rear_back: 1.1,
            y: -1.0,
            limits: [0.1, 1.0],
            stiffness: 10.0,
            damping: 0.7,
        }
    }
}

const BIKE_BODY_COLLISION_GROUP: (Group, Group) = (Group::GROUP_1, Group::GROUP_1);
const BIKE_WHEEL_COLLISION_GROUP: (Group, Group) = (Group::GROUP_10, Group::GROUP_10);

fn spawn_cyberbike(mut commands: Commands, asset_server: Res<AssetServer>) {
    let xform = Transform::from_translation(Vec3::Y * SPAWN_ALTITUDE);
    //.with_rotation(Quat::from_axis_angle(Vec3::Z, -89.0f32.to_radians()));

    let damping = Damping {
        angular_damping: 0.5,
        linear_damping: 0.1,
    };
    let not_sleeping = Sleeping::disabled();

    let ccd = Ccd { enabled: true };

    let bcollider_shape =
        Collider::capsule(Vec3::new(0.0, 0.0, -1.0), Vec3::new(0.0, 0.0, 1.0), 0.7);

    let friction = Friction {
        coefficient: 0.0,
        ..Default::default()
    };

    let restitution = Restitution {
        coefficient: 0.0,
        ..Default::default()
    };

    let mass_properties = ColliderMassProperties::Density(0.2);

    let (membership, filter) = BIKE_BODY_COLLISION_GROUP;
    let bike_collision_group = CollisionGroups::new(membership, filter);

    let scene = asset_server.load("cb-no-y_up.glb#Scene0");

    let spatialbundle = SpatialBundle {
        transform: xform,
        ..Default::default()
    };

    let bike = commands
        .spawn(RigidBody::Dynamic)
        .insert(spatialbundle)
        .insert((
            bcollider_shape,
            bike_collision_group,
            mass_properties,
            damping,
            restitution,
            friction,
            not_sleeping,
            ccd,
        ))
        .insert(TransformInterpolation {
            start: None,
            end: None,
        })
        .insert(Velocity::zero())
        .insert(ExternalForce::default())
        .insert(CyberBikeCollider)
        .with_children(|rider| {
            rider.spawn(SceneBundle {
                scene,
                ..Default::default()
            });
        })
        .insert(CyberBikeModel)
        .insert(CyberBikeBody)
        .insert(CyberBikeControl::default())
        .id();

    //return;
    let wheel_z_positions = vec![-1.0, 1.2, -1.0];
    let wheel_y = -1.0f32;

    // re-set the collision group
    let (membership, filter) = BIKE_WHEEL_COLLISION_GROUP;
    let wheels_collision_group = CollisionGroups::new(membership, filter);

    for (i, &wheel_z) in wheel_z_positions.iter().enumerate() {
        let (wheel_x, wheel_rad, stiffness) = match i {
            0 => (-1.1, 0.5, 2.0),
            2 => (1.1, 0.5, 2.0),
            1 => (0.0, 0.5, 1.8),
            _ => unreachable!(),
        };
        let offset = Vec3::new(wheel_x, wheel_y, wheel_z);
        let trans = xform.translation + offset;
        let wheel_pos_in_world = Transform::from_rotation(xform.rotation).with_translation(trans);
        let wheel_damping = Damping {
            linear_damping: 0.8,
            ..Default::default()
        };
        let wheel_collider = Collider::ball(wheel_rad);
        let mass_props = ColliderMassProperties::Density(0.001);

        let damping = 0.3;
        let prismatic = PrismaticJointBuilder::new(Vec3::Y)
            .local_anchor2(offset)
            .limits([-1.0, 0.9])
            .motor_position(0.0, stiffness, damping);
        let joint = ImpulseJoint::new(bike, prismatic);

        let _wheel_rb = commands
            .spawn(RigidBody::Dynamic)
            .insert((wheel_pos_in_world, GlobalTransform::default()))
            .insert((
                wheel_collider,
                mass_props,
                wheel_damping,
                ccd,
                not_sleeping,
                joint,
                wheels_collision_group,
            ))
            .id();
    }
}

pub struct CyberBikePlugin;
impl Plugin for CyberBikePlugin {
    fn build(&self, app: &mut App) {
        app.insert_resource(WheelConfig::default())
            .register_type::<WheelConfig>()
            .add_startup_system_to_stage(StartupStage::PostStartup, spawn_cyberbike);
    }
}