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

use log::debug;
use nalgebra::{Vector2, point, vector, Isometry, Isometry2, Complex, Unit};
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,
    pub flags: u32,
}

#[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,
    ) -> EntityId {
        let mut entity_map = entities.entities.clone();

        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 parent_angle = parent_body.rotation().angle();
        let parent_linvel = parent_body.linvel().clone();
        let parent_angvel = parent_body.angvel();

        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;
        let module_body = data.rigid_body_set.get_mut(module.handle).unwrap();
        module_body.set_translation(module_pos, true);
        module_body.set_rotation(Unit::from_angle(parent_angle + rotation), true);
        module_body.set_linvel(parent_linvel, true);
        module_body.set_angvel(parent_angvel, true);

        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, module.handle, attach_joint, true);
        let attached_module = AttachedModule {
            handle: module.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.remove(&entities.get_from_module(&module).unwrap());
        entity_map.insert(attached_id, Entity::AttachedModule(attached_module));
        entities.entities = entity_map;
        attached_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 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.,
            flags: 0,
        }
    }
    // TODO: this one too
    pub fn to_module_id(&self, entities: &EntityHandler) -> (EntityId, Module) {
        (entities.get_id_from_attached(self.clone()).unwrap(), Module {
            handle: self.handle,
            module_type: self.module_type,
            lifetime: 10.,
            flags: 0,
        })
    }

    pub fn to_protocol(&self, entities: &EntityHandler, data: &mut PhysicsData) -> starkingdoms_protocol::module::AttachedModule {
        let body = data.rigid_body_set.get(self.handle).unwrap();
        let children = self.children.to_vec();
        let mut prot_children = Vec::new();
        for i in 1..children.len() {
            if let Some(Some(child)) = children.get(i) {
                prot_children.push(starkingdoms_protocol::module::Attachment {
                    id: child.child,
                    slot: i as u32,
                    special_fields: Default::default(),
                });
            }
        }
        starkingdoms_protocol::module::AttachedModule {
            module_type: self.module_type.into(),
            rotation: body.rotation().angle(),
            x: body.translation().x,
            y: body.translation().y,
            id: entities.get_id_from_attached(self.clone()).unwrap(),
            children: prot_children,
            special_fields: Default::default(),
        }
    }

    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,
}