use std::f64::consts::PI;

use log::debug;
use nalgebra::{Vector2, point, vector, Isometry, Isometry2};
use rapier2d_f64::prelude::{RigidBodyHandle, Real, MassProperties, ColliderBuilder, RigidBodyBuilder, FixedJointBuilder, ImpulseJointHandle};
use starkingdoms_protocol::module::ModuleType;

use crate::{entity::{EntityId, EntityHandler, Entity, get_entity_id}, manager::PhysicsData, SCALE};

#[derive(Debug, Clone)]
pub struct Module {
    pub handle: RigidBodyHandle,
    pub module_type: ModuleType,
    pub lifetime: f64,
}

#[derive(Clone)]
pub struct ModuleTemplate {
    pub translation: Vector2<Real>,
    pub mass_properties: MassProperties,
    pub module_type: ModuleType,
}

#[derive(Debug, Clone)]
pub struct AttachedModule {
    pub handle: RigidBodyHandle,
    pub module_type: ModuleType,
    pub player_id: EntityId,
    pub children: [Option<Attachment>; 4],
}
impl AttachedModule {
    pub fn attach(
        data: &mut PhysicsData,
        entities: &mut EntityHandler,
        parent: EntityId,
        player_id: EntityId,
        module: Module,
        attachment_slot: usize,
    ) {
        let mut entity_map = entities.entities.clone();

        let loose_id = entities
            .get_from_module(&module)
            .expect("loose module does not exist");
        let loose_body = data
            .rigid_body_set
            .get(module.handle)
            .expect("loose module does not exist");
        let parent_entity = entity_map
            .get_mut(&parent)
            .expect("parent id does not exist");
        let parent_handle = match parent_entity {
            Entity::Player(player) => player.handle,
            Entity::AttachedModule(module) => module.handle,
            _ => {
                panic!("unexpected parent");
            }
        };
        //let parent_body = data.rigid_body_set
        //    .get(parent_handle).unwrap();
        // create attachment module
        let module_collider = ColliderBuilder::cuboid(25.0 / SCALE, 25.0 / SCALE)
            .mass_properties(loose_body.mass_properties().local_mprops)
            .build();
        let module_body = RigidBodyBuilder::dynamic()
            .translation(*loose_body.translation())
            .rotation(loose_body.rotation().angle())
            .build();
        let attached_handle = data.rigid_body_set.insert(module_body);
        data.collider_set.insert_with_parent(
            module_collider,
            attached_handle,
            &mut data.rigid_body_set,
        );
        let attach_joint = FixedJointBuilder::new()
            .local_anchor1(point![0.0, 0.0])
            .local_anchor2(point![0.0, 0.0])
            .build();
        let attach_joint_handle =
            data.impulse_joint_set
                .insert(parent_handle, attached_handle, attach_joint, true);
        let attached_module = AttachedModule {
            handle: attached_handle,
            module_type: module.module_type,
            player_id,
            children: [None, None, None, None],
        };
        let attached_id = get_entity_id();
        match parent_entity {
            Entity::Player(ref mut player) => {
                player.children[attachment_slot] = Some(Attachment {
                    child: attached_id,
                    connection: attach_joint_handle,
                });
            }
            Entity::AttachedModule(ref mut module) => {
                module.children[attachment_slot] = Some(Attachment {
                    child: attached_id,
                    connection: attach_joint_handle,
                });
            }
            _ => {
                panic!("unexpected parent");
            }
        };
        entity_map.insert(attached_id, Entity::AttachedModule(attached_module));
        // delete loose module
        data.rigid_body_set.remove(
            module.handle,
            &mut data.island_manager,
            &mut data.collider_set,
            &mut data.impulse_joint_set,
            &mut data.multibody_joint_set,
            true,
        );
        entities.entities.remove(&loose_id);
    }
    pub fn attach_new(
        data: &mut PhysicsData,
        entities: &mut EntityHandler,
        parent: EntityId,
        player_id: EntityId,
        module: ModuleTemplate,
        attachment_slot: usize,
    ) -> EntityId {
        let mut entity_map = entities.entities.clone();

        //let loose_id = entities.get_from_module(&module).expect("loose module does not exist");
        //let loose_body = data.rigid_body_set.get(module.handle).expect("loose module does not exist");
        let parent_entity = entity_map
            .get_mut(&parent)
            .expect("parent id does not exist");
        let parent_handle = match parent_entity {
            Entity::Player(player) => player.handle,
            Entity::AttachedModule(module) => module.handle,
            _ => {
                panic!("unexpected parent");
            }
        };

        let parent_body = data.rigid_body_set.get(parent_handle).expect("Parent body does not exist");
        let parent_pos = vector![parent_body.translation().x, parent_body.translation().y];

        let (anchor, rotation) = match attachment_slot {
            0 => {
                (point![
                    0. / SCALE,
                    53. / SCALE
                ], PI)
            }
            1 => {
                (point![
                    -53. / SCALE,
                     0. / SCALE
                ], -PI/2.)
            }
            2 => {
                (point![
                     0. / SCALE,
                    -53. / SCALE
                ], 0.)
            }
            3 => {
                (point![
                    53. / SCALE,
                    0. / SCALE
                ], PI/2.)
            }
            _ => {
                (point![
                    0. / SCALE,
                    53. / SCALE
                ], 0.)
            }
        };

        let relative_pos = 
            vector![anchor.x *  (parent_body.rotation().angle()).cos() + 
                    anchor.y * -(parent_body.rotation().angle()).sin(),
                    anchor.x *  (parent_body.rotation().angle()).sin() +
                    anchor.y *  (parent_body.rotation().angle()).cos()];
        let module_pos = parent_pos + relative_pos;

        // create attachment module
        let module_collider = ColliderBuilder::cuboid(25.0 / SCALE, 25.0 / SCALE)
            .mass_properties(module.mass_properties)
            .build();
        let module_body = RigidBodyBuilder::dynamic()
            .translation(module_pos)
            .rotation(parent_body.rotation().angle() + rotation)
            .build();
        let attached_handle = data.rigid_body_set.insert(module_body);
        data.collider_set.insert_with_parent(
            module_collider,
            attached_handle,
            &mut data.rigid_body_set,
        );

        let attach_joint = FixedJointBuilder::new()
            .local_anchor1(anchor)
            .local_anchor2(point![0.0, 0.0 / SCALE])
            .local_frame2(Isometry2::rotation(rotation))
            .build();
        let attach_joint_handle =
            data.impulse_joint_set
                .insert(parent_handle, attached_handle, attach_joint, true);
        let attached_module = AttachedModule {
            handle: attached_handle,
            module_type: module.module_type,
            player_id,
            children: [None, None, None, None],
        };
        let attached_id = get_entity_id();
        match parent_entity {
            Entity::Player(ref mut player) => {
                player.children[attachment_slot] = Some(Attachment {
                    child: attached_id,
                    connection: attach_joint_handle,
                });
            }
            Entity::AttachedModule(ref mut module) => {
                module.children[attachment_slot] = Some(Attachment {
                    child: attached_id,
                    connection: attach_joint_handle,
                });
            }
            _ => {
                panic!("unexpected parent");
            }
        };
        entity_map.insert(attached_id, Entity::AttachedModule(attached_module));
        entities.entities = entity_map;
        attached_id
    }
    // TODO: remove this function
    pub fn to_module(&self) -> Module {
        Module {
            handle: self.handle,
            module_type: self.module_type,
            lifetime: 10.,
        }
    }

    pub fn search_modules(&self, entities: &EntityHandler) -> Vec<AttachedModule> {
        let mut modules = vec![self.clone()];
        for attachment in self.children.iter().flatten() {
            if let Entity::AttachedModule(child_module) =
                entities.entities.get(&attachment.child).unwrap()
            {
                modules.append(&mut child_module.search_modules(entities));
            }
        }
        modules
    }
}

#[derive(Debug, Clone)]
pub struct Attachment {
    pub child: EntityId,
    pub connection: ImpulseJointHandle,
}