import bpy
import math

from mathutils import Vector, Matrix
from bpy_extras.object_utils import world_to_camera_view

def get_bone_head_tail_2d(posebone, scene=None, cam=None) -> tuple[Vector, Vector]:
    '''Get 2D vectors in camera view of bone head and tails
    return tuple of 2d vectors (head_2d and tail_2d) 
    '''
    scene = scene or bpy.context.scene
    cam = cam or scene.camera

    arm = posebone.id_data

    # Get 3D locations of head and tail
    head_3d = arm.matrix_world @ posebone.head
    tail_3d = arm.matrix_world @ posebone.tail

    # Convert 3D locations to 2D
    head_2d = world_to_camera_view(scene, cam, head_3d)
    tail_2d = world_to_camera_view(scene, cam, tail_3d)
    
    ratio = scene.render.resolution_y / scene.render.resolution_x
    head_2d.y *= ratio
    tail_2d.y *= ratio
    
    return Vector((head_2d.x, head_2d.y)), Vector((tail_2d.x, tail_2d.y))


def rotate_matrix_around_pivot(matrix, angle, pivot, axis):
    '''Rotate a given matrix by a CW angle around pivot on a given axis
    matrix (Matrix): the matrix to rotate
    angle (Float, Radians): the angle in radians
    pivot (Vector3): the pivot 3D coordinate
    axis (Vector3): the vector axis of rotation
    '''

    # Convert angle to radians ?
    # angle = math.radians(angle)

    # Create a rotation matrix
    rot_matrix = Matrix.Rotation(angle, 4, axis)

    # Create translation matrices
    translate_to_origin = Matrix.Translation(-pivot)
    translate_back = Matrix.Translation(pivot)

    # Combine the transformations : The order of multiplication is important
    new_matrix = translate_back @ rot_matrix @ translate_to_origin @ matrix

    return new_matrix