from sports import MeasurementUnit
from sports.basketball import CourtConfiguration, League, draw_court, draw_points_on_court
import numpy as np
import supervision as sv
import cv2

CONFIG = CourtConfiguration(league=League.NBA, measurement_unit=MeasurementUnit.FEET).vertices

def frame_xy_to_court_xy(frame_xy: np.ndarray, H: np.ndarray):

    assert frame_xy.shape[1] == 2
    n_points = frame_xy.shape[0]

    court_xy = np.hstack((frame_xy, np.ones(shape=(n_points, 1)))) @ H.T
    court_xy_norm = court_xy[:, :2] / court_xy[:, [-1]]
    return court_xy_norm

def get_players_court_xy(frame, detections, model, use_bottom_center=True, normalize=False):
    KEYPOINT_DETECTION_MODEL_CONFIDENCE = 0.3
    KEYPOINT_DETECTION_MODEL_ANCHOR_CONFIDENCE = 0.5

    # Locate court keypoints (or reference points)
    result = model.infer(frame, confidence=KEYPOINT_DETECTION_MODEL_CONFIDENCE)[0]
    key_points = sv.KeyPoints.from_inference(result)
    filter_mask = key_points.confidence[0] > KEYPOINT_DETECTION_MODEL_ANCHOR_CONFIDENCE

    # Compute homography matrix H
    court_landmarks = np.array(CONFIG)[filter_mask]
    frame_landmarks = key_points[:, filter_mask].xy[0]
    H, _ = cv2.findHomography(frame_landmarks, court_landmarks)

    # From the player detections, retrieve their position on the court
    x1 = detections.xyxy[:, 0]
    x2 = detections.xyxy[:, 2]
    y1 = detections.xyxy[:, 1]
    y2 = detections.xyxy[:, 3]
    if use_bottom_center:
        # Take the bottom center of the bounding box as the (x,y) coordinate
        frame_xy = np.vstack(
        (x1 + (x2 - x1) / 2, y2)
        ).T
    else:
        frame_xy = np.vstack(
        (x1 + (x2 - x1) / 2, y1 + (y2 - y1) / 2)
        ).T
    # apply homographic transformation
    court_xy = frame_xy_to_court_xy(frame_xy, H)

    if normalize:
        court_xy = court_xy / np.array([94.0, 50.0])

    return court_xy

def show_positions_on_court(court_xy):
    court = draw_court(config=CONFIG)
    court = draw_points_on_court(
        config=CONFIG,
        xy=court_xy,
        court=court
    )
    sv.plot_image(court)