use std::error::Error;
use std::f64::consts::PI;

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

use crate::{
    entity::{get_entity_id, Entity, EntityHandler, EntityId},
    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, PartialEq)]
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,
    ) -> Result<EntityId, Box<dyn Error + Send + Sync>> {
        let mut entity_map = entities.entities.clone();

        let parent_entity = entity_map
            .get_mut(&parent)
            .ok_or("parent id does not exist")?;
        let parent_handle = match parent_entity {
            Entity::Player(player) => {
                if player.children[attachment_slot].is_some() {
                    return Err("already attached".into());
                }
                player.handle
            }
            Entity::AttachedModule(module) => {
                if module.children[attachment_slot].is_some() {
                    return Err("already attached".into());
                }
                module.handle
            }
            _ => {
                panic!("unexpected parent");
            }
        };

        let parent_body = data
            .rigid_body_set
            .get(parent_handle).ok_or("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();
        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.),
        };

        if let Some(id) = entities.get_from_module(&module) {
            let relative_pos = vector![
                anchor.x.mul_add((parent_body.rotation().angle()).cos(), anchor.y * -(parent_body.rotation().angle()).sin()),
                anchor.x.mul_add((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).ok_or("module body does not exist")?;
            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 = Self {
                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(&id);
            entity_map.insert(attached_id, Entity::AttachedModule(attached_module));
            entities.entities = entity_map;
            return Ok(attached_id);
        }
        Err("entity does not exist".into())
    }
    pub fn detach(
        data: &mut PhysicsData,
        entities: &mut EntityHandler,
        player_id: EntityId,
        module: Self,
    ) -> Option<EntityId> {
        let mut entity_map = entities.entities.clone();

        // player not in parent search
        // also no parents included in parent search
        let player = entities.get_player_from_id(player_id)?;
        let (slot, parent_id) = player.find_parent(&module, entities)?;
        let parent_entity = entity_map
            .get_mut(&parent_id)?;

        match parent_entity {
            Entity::Player(ref mut player) => {
                if let Some(child) = player.children[slot as usize].clone() {
                    data.impulse_joint_set.remove(child.connection, true);
                }
                player.children[slot as usize] = None;
            }
            Entity::AttachedModule(ref mut module) => {
                if let Some(child) = &module.children[slot as usize] {
                    data.impulse_joint_set.remove(child.connection, true);
                }
                module.children[slot as usize] = None;
            }
            _ => {
                panic!("unexpected parent");
            }
        };
        // remove joint
        let tree = module.search_modules(entities);
        let new_module = Module {
            handle: module.handle,
            module_type: module.module_type,
            lifetime: 0.,
            flags: 0,
        };
        entity_map.remove(&entities.get_id_from_attached(&module)?);
        let id = get_entity_id();
        entity_map.insert(id, Entity::Module(new_module));
        for element in tree {
            for child in element.clone().children {
                if let Some(child) = child {
                    data.impulse_joint_set.remove(child.connection, true);
                    let child_body = entities.get_attached_from_id(child.child)?;
                    let new_module = Module {
                        handle: child_body.handle,
                        module_type: child_body.module_type,
                        lifetime: 0.,
                        flags: 0,
                    };
                    entity_map.remove(&entities.get_id_from_attached(&child_body)?);
                    let attached_id = get_entity_id();
                    entity_map.insert(attached_id, Entity::Module(new_module));
                }
            }
        }
        entities.entities = entity_map;
        Some(id)
    }
    pub fn attach_new(
        data: &mut PhysicsData,
        entities: &mut EntityHandler,
        parent: EntityId,
        player_id: EntityId,
        module: ModuleTemplate,
        attachment_slot: usize,
    ) -> Option<EntityId> {
        let mut entity_map = entities.entities.clone();

        let parent_entity = entity_map
            .get_mut(&parent)?;
        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)?;
        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.mul_add((parent_body.rotation().angle()).cos(), anchor.y * -(parent_body.rotation().angle()).sin()),
            anchor.x.mul_add((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 = Self {
            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;
        Some(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) -> Option<(EntityId, Module)> {
        Some((
            entities.get_id_from_attached(self)?,
            Module {
                handle: self.handle,
                module_type: self.module_type,
                lifetime: 10.,
                flags: 1,
            },
        ))
    }

    pub fn to_protocol(
        &self,
        entities: &EntityHandler,
        data: &RigidBodySet,
    ) -> Option<starkingdoms_protocol::module::AttachedModule> {
        let body = data.get(self.handle)?;
        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(),
                });
            }
        }
        Some(starkingdoms_protocol::module::AttachedModule {
            module_type: self.module_type.into(),
            rotation: body.rotation().angle(),
            x: body.translation().x * SCALE,
            y: body.translation().y * SCALE,
            id: entities.get_id_from_attached(self)?,
            children: prot_children,
            special_fields: Default::default(),
        })
    }

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

    pub fn find_parent(
        &self,
        module: Self,
        entities: &EntityHandler,
    ) -> Option<(u8, EntityId)> {
        for (slot, attachment) in self.children.iter().enumerate() {
            if let Some(attachment) = attachment {
                if let Entity::AttachedModule(child_module) =
                    entities.entities.get(&attachment.child)?
                {
                    if *child_module == module {
                        return Some((slot as u8, entities.get_id_from_attached(self)?));
                    }
                    let parent = child_module.find_parent(module.clone(), entities);
                    if parent.is_some() {
                        return parent;
                    }
                }
            }
        }
        None
    }
}

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