import math


def heading_from_camera(bbox, image_shape):
    """
    Calculates the heading angle (in degree) of the object from the camera.

    PARMS
    bbox: Bounding box [px]
    image_shape: Size of the image (width, height) [px]
    """
    # GoPro Intrensic Camera Settings #
    ###################################
    focal_length_mm = 5.21
    unit_pixel_length = 1.12
    sen_res = (5663, 4223) 
    sensor_height_mm = (unit_pixel_length*sen_res[1])/1000 
    sensor_width_mm = (unit_pixel_length*sen_res[0])/1000
    ###################################

    # Image Center
    (cX, cY) = image_shape[1]/2, image_shape[0]/2 
    # Object Center
    (startX, startY, endX, endY) = bbox
    (centerX, centerY) = (startX+endX)/2, (startY+endY)/2

    # Distance between the two points
    distance = math.sqrt((centerX - cX)**2 + (centerY - cY)**2)

    # Focal Length in px
    img_width_px = image_shape[1]
    f_px = (focal_length_mm * img_width_px)/ (sensor_width_mm)

    # Heading Angle
    angle = math.degrees(math.asin(distance/f_px))
    if centerX > cX:
        return angle
    else:
        return -angle