import numpy
import json
import steamvr
import time
import cv2
import glfw
from scipy.spatial.transform import Rotation as R
import numpy as np
from alignment import align_init, align_show_horizon

CANTING = 6.17

lh = steamvr.LighthouseConsole()
lh.open()
config = lh.download_config()
#video = cv2.VideoCapture(1)

def read_eye_to_head():
    print("read_eye_to_head")

    rot = numpy.array(config['tracking_to_eye_transform'][0]['eye_to_head'])
    left_eye_pitch = R.from_matrix(rot).as_euler("XYZ", True)[0]
    left_eye_yaw = R.from_matrix(rot).as_euler("XYZ", True)[1]
    left_eye_roll = R.from_matrix(rot).as_euler("XYZ", True)[2]
    print("left eye value read")
    print(left_eye_pitch, "|", left_eye_yaw, "|", left_eye_roll)

    rot = numpy.array(config['tracking_to_eye_transform'][1]['eye_to_head'])
    right_eye_pitch = R.from_matrix(rot).as_euler("XYZ", True)[0]
    right_eye_yaw = R.from_matrix(rot).as_euler("XYZ", True)[1]
    right_eye_roll = R.from_matrix(rot).as_euler("XYZ", True)[2]
    print("right eye value read")
    print(right_eye_pitch, "|", right_eye_yaw, "|", right_eye_roll)

def write_eye_to_head():
    print ("write_eye_to_head")

    rot = R.from_euler("XYZ", [0, CANTING, 0], degrees=True)
    config['tracking_to_eye_transform'][0]['eye_to_head'] = rot.as_matrix().tolist()

    rot = R.from_euler("XYZ", [0, -CANTING, 0], degrees=True)
    config['tracking_to_eye_transform'][1]['eye_to_head'] = rot.as_matrix().tolist()

    lh.upload_config(config)


def reset_eye_to_head():
    print("reset_eye_to_head")

    rot = R.from_euler("XYZ", [0, CANTING, 0], degrees=True)
    config['tracking_to_eye_transform'][0]['eye_to_head'] = rot.as_matrix().tolist()

    rot = R.from_euler("XYZ", [0, -CANTING, 0], degrees=True)
    config['tracking_to_eye_transform'][1]['eye_to_head'] = rot.as_matrix().tolist()

    lh.upload_config(config)

def read_ipd():
    print(config['ipd']['high_mm'])

def dump_json():
    with open(f'C:/Users/jyakk/Desktop/config.json', 'w') as file:
        json.dump(config, file)

def view_camera():
    video = cv2.VideoCapture(1)

    if not video.isOpened():
        print("Error: Could not open video.")
        return

    video_width = int(video.get(cv2.CAP_PROP_FRAME_WIDTH))
    video_height = int(video.get(cv2.CAP_PROP_FRAME_HEIGHT))
    cv2.namedWindow("Bigscreen VAP Tool", cv2.WINDOW_NORMAL)
    cv2.resizeWindow("Bigscreen VAP Tool", video_width, video_height)
    video.set(cv2.CAP_PROP_FRAME_WIDTH, video_width)
    video.set(cv2.CAP_PROP_FRAME_HEIGHT, video_height)
    video.set(cv2.CAP_PROP_EXPOSURE, -1)  # this appears to also disable auto-exposure
    video.set(cv2.CAP_PROP_FPS, 45)


    while True:
        ret, img = video.read()

        if not ret:
            print("Error: Failed to capture image")
            break

        cv2.imshow('Bigscreen VAP Tool', img)

        # break the loop on 'q' key press
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break

    video.release()
    cv2.destroyAllWindows()

def unroll_marker_ids(ids) -> np.ndarray:
    return (4 * np.array(ids)[:, None] + np.arange(4)).reshape(-1)


def unroll_marker_corners(marker_corners) -> np.ndarray:
    return np.array(marker_corners).reshape(-1, 2)

def charuco_sandbox():
    print("Charuco sandbox")
    # Load the predefined dictionary
    aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_1000)

    # Create the Charuco board
    board = cv2.aruco.CharucoBoard(
        [28, 14], 0.045454545454545, 0.022727272727272, aruco_dict
    )

    # Read the image
    image = cv2.imread("ChArUco_Marker.png")

    # Detect markers
    parameters = cv2.aruco.CharucoParameters()
    detector = cv2.aruco.CharucoDetector(board, parameters)
    detections = detector.detectBoard(image=image)

    detections = list(detections)
    detections[0] = [] if detections[0] is None else detections[0]
    detections[1] = [] if detections[1] is None else detections[1]
    detections[2] = [] if detections[2] is None else detections[2]
    detections[3] = [] if detections[3] is None else detections[3]

    charucoCorners, charucoIds, markerCorners, markerIds = detections

    charucoCorners = np.array(charucoCorners).reshape(-1, 2)
    charucoIds = np.array(charucoIds).squeeze()
    markerCorners = np.array(markerCorners).reshape(-1, 2)
    markerIds = (4 * np.array(markerIds)[:, None] + np.arange(4)).reshape(-1)

    # Get the index of charuco ID 153 (right eye center)
    index = np.where(charucoIds == 181)[0]
    coords = charucoCorners[index]
    cv2.circle(image, tuple(coords[0].astype(int)), 50, (255, 0, 0), -1)

    # Get the index of charuco ID 169 (left eye center)
    index = np.where(charucoIds == 169)[0]
    coords = charucoCorners[index]
    cv2.circle(image, tuple(coords[0].astype(int)), 50, (255, 0, 0), -1)

    cv2.imwrite("detected.png", image)


# MAIN

#read_eye_to_head()
#write_eye_to_head()
#align_init("BS21C010002", 64)
#align_show_horizon()
#view_camera()
#dump_json()
charuco_sandbox()

# TODO add test case for writing new values, reading values, restarting steamvr, and reading again