import numpy as np
import pygame as pg
from mediapipe.python.solutions.drawing_utils import _normalized_to_pixel_coordinates

# def blitRotate(surf, image, topleft, angle):
#     """Rotate an image while drawing it."""
#     rotated_image = pg.transform.rotate(image, angle)
#     new_rect = rotated_image.get_rect(center=image.get_rect(topleft=topleft).center)
#     surf.blit(rotated_image, new_rect.topleft)


# https://stackoverflow.com/questions/70819750/rotating-and-scaling-an-image-around-a-pivot-while-scaling-width-and-height-sep/70820034#70820034
def blitRotate(surf, image, origin, angle, pivot=None):

    if pivot is None:
        pivot = pg.math.Vector2(image.get_size()) / 2

    image_rect = image.get_rect(topleft=(origin[0] - pivot[0], origin[1] - pivot[1]))
    offset_center_to_pivot = pg.math.Vector2(origin) - image_rect.center
    rotated_offset = offset_center_to_pivot.rotate(-angle)
    rotated_image_center = (origin[0] - rotated_offset.x, origin[1] - rotated_offset.y)
    rotated_image = pg.transform.rotate(image, angle)
    rect = rotated_image.get_rect(center=rotated_image_center)

    surf.blit(rotated_image, rect)


def landmark2vec(landmark, screen):
    """Convert a landmark to a pygame Vector2."""
    return pg.Vector2(
        _normalized_to_pixel_coordinates(
            np.clip(landmark.x, 0, 1),
            np.clip(landmark.y, 0, 1),
            screen.get_width(),
            screen.get_height(),
        )  # type: ignore
    )


def landmark3vec(landmark, screen):
    """Convert a landmark to a pygame Vector3."""
    return pg.Vector3(
        np.clip(landmark.x, 0, 1),
        np.clip(landmark.y, 0, 1),
        np.clip(landmark.z, 0, 1),
    )