#! /usr/bin/env python3

import argparse
import sys
import logging
import cv2
import numpy as np

logger = logging.getLogger(__name__)


def main() -> int:
    # Handle program arguments
    ap = argparse.ArgumentParser(
        prog="leap-view",
        description="View the camera feeds from a Leap Motion controller",
    )
    ap.add_argument("device", help="Path to the video device")
    ap.add_argument(
        "-r",
        "--resolution",
        help="Resolution of the camera",
        choices=["640x120", "640x240", "640x480", "752x120", "752x240", "752x480"],
        default="640x480",
    )
    ap.add_argument(
        "--only", help="Only show the left or right camera", choices=["left", "right"]
    )
    ap.add_argument("--no-led", help="Disable the LEDs", action="store_true")
    ap.add_argument(
        "--no-brightness-normalization",
        help="Do not normalize the brightness of the frames",
        action="store_true",
    )
    ap.add_argument(
        "-v", "--verbose", help="Enable verbose logging", action="store_true"
    )
    args = ap.parse_args()

    # Configure logging
    logging.basicConfig(
        level=logging.DEBUG if args.verbose else logging.INFO,
        format="%(levelname)s:	%(message)s",
    )

    # Open the video device
    cap = cv2.VideoCapture(args.device)

    if not cap.isOpened():
        logger.error("Failed to open video device")
        return 1

    # Set the resolution
    width, height = map(int, args.resolution.split("x"))
    cap.set(cv2.CAP_PROP_FRAME_WIDTH, width)
    cap.set(cv2.CAP_PROP_FRAME_HEIGHT, height)
    cap.set(cv2.CAP_PROP_CONVERT_RGB, 0)
    
    # Configure the LEDs
    # NOTE: See the libuvc for leap documentation for info about this
    # https://github.com/ewpratten/leapuvc/blob/master/LeapUVC-Manual.pdf
    cap.set(cv2.CAP_PROP_CONTRAST, (2 | (int(not args.no_led) << 6)))
    cap.set(cv2.CAP_PROP_CONTRAST, (3 | (int(not args.no_led) << 6)))
    cap.set(cv2.CAP_PROP_CONTRAST, (4 | (int(not args.no_led) << 6)))

    # Read frames
    while True:
        ret, frame = cap.read()

        if not ret:
            logger.error("Failed to read frame")
            break

        # Reshape the frame
        frame = np.reshape(frame, (height, width * 2))

        # Split into left and right frames (every other byte)
        left_frame = frame[:, 0::2]
        right_frame = frame[:, 1::2]

        # Ignore the last row of the frames
        left_frame = left_frame[:-1]
        right_frame = right_frame[:-1]

        # Normalize the frames so that the brightest pixel is 255
        if not args.no_brightness_normalization:
            left_frame = cv2.normalize(left_frame, None, 0, 255, cv2.NORM_MINMAX)
            right_frame = cv2.normalize(right_frame, None, 0, 255, cv2.NORM_MINMAX)

        # Show the frame
        if not args.only or args.only == "left":
            cv2.imshow("Left", left_frame)
        if not args.only or args.only == "right":
            cv2.imshow("Right", right_frame)

        # Check if one of the windows was closed
        if cv2.waitKey(1) & 0xFF == ord("q"):
            break

    return 0


if __name__ == "__main__":
    sys.exit(main())