import os.path

import cv2
import numpy
import struct
import atexit
import subprocess
import win32pipe
import win32file
import steamvr
import logging
from scipy.spatial.transform import Rotation
import time
import matplotlib.pyplot as plt
from skimage.draw import line
from collections import deque
# from mes_logger import mes_add

# GLOBALS
LEFT_EYE = 0
RIGHT_EYE = 1
DOWNWARD_CANT = -5
ANGLE_TEST_LIMIT = 2
ANGLE_TEST_INCREMENT = 0.035
SHOW_DEBUG_VIEW = False

horizontal_cant = 0  # gets set to 2 or 4.5 depending on HMD IPD
serial_number = ""
is_first_run = True
refine_delay_counter = 0
best_pitch_left = 0
best_pitch_right = 0
best_roll_left = 0
best_roll_right = 0
pixels_per_degree = 0

current_roll_iterations = 0
max_roll_iterations = 150
should_apply_roll = [True, True]
should_find_transitions = True

left_transition_point = [0, 0]
right_transition_point = [0, 0]
transition_delta = 0

video = cv2.VideoCapture(0)
pipe = win32pipe.CreateNamedPipe(r'\\.\pipe\vap_pipe',
                                 win32pipe.PIPE_ACCESS_DUPLEX,
                                 win32pipe.PIPE_TYPE_MESSAGE | win32pipe.PIPE_WAIT,
                                 1, 65536, 65536, 300, None)
lh = steamvr.LighthouseConsole()
lh.open()
try:
    config = lh.download_config()
except:
    config = None

subprocess_path = "vap_unity_build/VAP.exe"
if os.path.exists("_internal/") is True:  # If we're in a Pyinstaller build, find assets in the _internal folder
    subprocess_path = "_internal/" + subprocess_path

logger = logging.getLogger("ALIGN")
logger.setLevel(logging.INFO)

# MES values for alignment
MES_OP = "20101883"
MES_STATION_ID = "T060 VAP02"
MES_SW_VER = "0.0.1"
uut_start = 0
uut_stop = 0
mes_string_sent = False


class Align:
    p_unity = None
    eye_areas = []
    eye_bounds = []
    hmd_centers = numpy.zeros((2, 2), dtype=int)
    hmd_horizon = numpy.zeros((2, 2), dtype=int)
    best_pitch = numpy.zeros((2, 1))
    best_roll = numpy.zeros((2, 1))
    best_yaw = numpy.zeros((2, 1))


def gaussian(x, a, x0, sigma):
    return a * numpy.exp(-(x - x0) ** 2 / (2 * sigma ** 2))


def wait_for_unity():
    # This will block until we've received something written to us over the pipe.
    _, data = win32file.ReadFile(pipe, 256)


def align_init(serial, ipd, canting_override):
    global horizontal_cant, is_first_run, video, pipe, lh, config, serial_number, refine_delay_counter
    global best_pitch_right, best_pitch_left, pixels_per_degree, uut_start

    uut_start = time.time()

    serial_number = serial
    refine_delay_counter = 0
    best_pitch_left = 0
    best_pitch_right = 0
    pixels_per_degree = 0

    logger.handlers = []
    file_handler = logging.FileHandler(time.strftime(f"ALIGN_%Y_%m_%d") + f"_%s.log" % serial_number)
    formatter = logging.Formatter('%(asctime)s - %(name)s - %(levelname)s - %(message)s')
    file_handler.setFormatter(formatter)
    logger.addHandler(file_handler)

    # Log the serial number with the LHR value from the config
    lhr_serial = config['device_serial_number']
    logger.info(f"Serial Number: {serial_number}, LHR: {lhr_serial}")

    if is_first_run is False:
        video = cv2.VideoCapture(0)
        pipe = win32pipe.CreateNamedPipe(r'\\.\pipe\vap_pipe',
                                         win32pipe.PIPE_ACCESS_DUPLEX,
                                         win32pipe.PIPE_TYPE_MESSAGE | win32pipe.PIPE_WAIT,
                                         1, 65536, 65536, 300, None)
        lh = steamvr.LighthouseConsole()
        lh.open()
        config = lh.download_config()

    if canting_override != 0:
        horizontal_cant = canting_override
    else:
        align_determine_horizontal_canting(ipd)

    video_width = int(video.get(cv2.CAP_PROP_FRAME_WIDTH)) * 2
    video_height = int(video.get(cv2.CAP_PROP_FRAME_HEIGHT)) * 2
    cv2.namedWindow("Bigscreen VAP Tool")
    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)
    video.set(cv2.CAP_PROP_FPS, 45)
    align_reset_config_values()
    ret, img = video.read()
    cv2.imshow('Bigscreen VAP Tool', img)
    Align.p_unity = subprocess.Popen(subprocess_path, stdin=subprocess.PIPE, stdout=subprocess.PIPE)
    atexit.register(align_destroy)
    win32pipe.ConnectNamedPipe(pipe, None)
    win32file.WriteFile(pipe, "1".encode())  # Switch to solid white mode to start.
    wait_for_unity()
    is_first_run = False


def align_determine_horizontal_canting(ipd):
    global horizontal_cant

    if ipd >= 70:
        horizontal_cant = 2
    else:
        horizontal_cant = 4.5

    steamvr.steamvr.set_ipd_default_mm(config, ipd)
    lh.upload_config(config)


def align_find_hmd_horizon():
    ret, img = video.read()
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    ret, thresh = cv2.threshold(gray, 127, 255, 0)
    thresh = cv2.erode(thresh, None, iterations=2)
    thresh = cv2.dilate(thresh, None, iterations=2)
    # Find contours to determine the extents of both lenses.
    contours, hierarchy = cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
    cv2.drawContours(img, contours, -1, (0, 255, 0), 3)
    centers = []
    Align.eye_areas.clear()
    Align.eye_bounds.clear()
    large_areas = []
    for c in contours:
        m = cv2.moments(c)
        area = cv2.contourArea(c)

        if (m["m00"] != 0) and (area > 55000):
            centers.append((int(m["m10"] / m["m00"]), int(m["m01"] / m["m00"])))
            x, y, w, h = cv2.boundingRect(c)
            Align.eye_bounds.append([x, y, w, h])
            eye_region = gray[y: y + h, x: x + w]
            Align.eye_areas.append(numpy.sum(eye_region))
            large_areas.append(area)

    # Only continue if we've found at least two centers
    if len(centers) >= 2:
        # Sort the centers based on their x-coordinates
        sorted_centers = sorted(centers, key=lambda pt: pt[0])

        Align.hmd_centers[LEFT_EYE] = sorted_centers[0]  # left-most center
        Align.hmd_centers[RIGHT_EYE] = sorted_centers[1]  # right-most center
        Align.hmd_horizon = [Align.hmd_centers[LEFT_EYE], Align.hmd_centers[RIGHT_EYE]]

        cv2.drawContours(img, contours, -1, (0, 0, 255), 3)
        cv2.imshow('Bigscreen VAP Tool', img)
        return True

    cv2.line(img, Align.hmd_centers[LEFT_EYE] if len(Align.hmd_centers) > 0 else (0, 0),
             Align.hmd_centers[RIGHT_EYE] if len(Align.hmd_centers) > 1 else (0, 0), (255, 0, 0), 5)
    cv2.drawContours(img, contours, -1, (0, 255, 0), 3)
    cv2.imshow('Bigscreen VAP Tool', img)

    return False


def align_get_hmd_horizon():
    return Align.hmd_horizon


def align_minimize_pitch():
    global pixels_per_degree, pipe, best_pitch_right, best_pitch_left
    win32file.WriteFile(pipe, "2".encode())  # Switch to horizon line mode.
    wait_for_unity()
    my_bytes = bytearray("p".encode())
    my_bytes += bytearray(struct.pack("f", 90.0))  # Reset pitch
    win32file.WriteFile(pipe, my_bytes)
    wait_for_unity()
    my_bytes = bytearray("r".encode())
    my_bytes += bytearray(struct.pack("f", 0.0))  # Reset roll
    win32file.WriteFile(pipe, my_bytes)
    wait_for_unity()
    my_bytes = bytearray("y".encode())
    my_bytes += bytearray(struct.pack("f", 0.0))  # Reset yaw
    win32file.WriteFile(pipe, my_bytes)
    wait_for_unity()

    pivot = 0.0
    i = pivot + ANGLE_TEST_LIMIT
    angles, left_offsets, right_offsets = [], [], []
    line_length = 75  # Search area extends by this value above and below the center

    # Flags and counters
    left_zero_found = False
    right_zero_found = False
    left_zero_post_count = 0
    right_zero_post_count = 0
    post_crossing_margin = 5

    while i > pivot - ANGLE_TEST_LIMIT:
        angle = float(i)
        print(angle, flush=True)

        # Apply the proposed pitch angle and visualize it.
        pitch = angle
        my_bytes = bytearray("p".encode())
        my_bytes += bytearray(struct.pack("f", pitch))
        win32file.WriteFile(pipe, my_bytes)
        wait_for_unity()

        # Grab the latest camera frame.
        ret, new_img = video.read()  # Capture the new image.
        angle_str = "Angle: {:.2f}".format(angle)
        new_gray = cv2.cvtColor(new_img, cv2.COLOR_BGR2GRAY)
        cv2.putText(new_img, angle_str, (0, 50), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2, cv2.LINE_AA)

        # LEFT EYE calculations
        left_eye_top = (Align.hmd_centers[LEFT_EYE][1] - line_length, Align.hmd_centers[LEFT_EYE][0])
        left_eye_bottom = (Align.hmd_centers[LEFT_EYE][1] + line_length, Align.hmd_centers[LEFT_EYE][0])
        left_rr, left_cc = line(*left_eye_bottom, *left_eye_top)
        left_intensities = new_gray[left_rr, left_cc].astype('float64')
        left_intensities -= numpy.average(left_intensities)  # Normalize
        left_step_filter = numpy.hstack((numpy.ones(len(left_intensities)), -1 * numpy.ones(len(left_intensities))))
        left_step_conv = numpy.convolve(left_intensities, left_step_filter, mode='valid')
        left_step_index = numpy.argmax(left_step_conv)
        left_pixel_offset = left_step_index - line_length

        # RIGHT EYE calculations
        right_eye_top = (Align.hmd_centers[RIGHT_EYE][1] - line_length, Align.hmd_centers[RIGHT_EYE][0])
        right_eye_bottom = (Align.hmd_centers[RIGHT_EYE][1] + line_length, Align.hmd_centers[RIGHT_EYE][0])
        right_rr, right_cc = line(*right_eye_bottom, *right_eye_top)
        right_intensities = new_gray[right_rr, right_cc].astype('float64')
        right_intensities -= numpy.average(right_intensities)  # Normalize
        right_step_filter = numpy.hstack((numpy.ones(len(right_intensities)), -1 * numpy.ones(len(right_intensities))))
        right_step_conv = numpy.convolve(right_intensities, right_step_filter, mode='valid')
        right_step_index = numpy.argmax(right_step_conv)
        right_pixel_offset = right_step_index - line_length

        if SHOW_DEBUG_VIEW is True:
            cv2.line(new_img, Align.hmd_centers[LEFT_EYE] if len(Align.hmd_centers) > 0 else (0, 0),
                     Align.hmd_centers[RIGHT_EYE] if len(Align.hmd_centers) > 1 else (0, 0), (255, 0, 0), 5)
            cv2.line(new_img, Align.hmd_centers[LEFT_EYE],
                     (Align.hmd_centers[LEFT_EYE][0], Align.hmd_centers[LEFT_EYE][1] - left_pixel_offset),
                     (0, 255, 0), 2)
            cv2.line(new_img, Align.hmd_centers[RIGHT_EYE],
                     (Align.hmd_centers[RIGHT_EYE][0], Align.hmd_centers[RIGHT_EYE][1] - right_pixel_offset),
                     (0, 255, 0), 2)

        angles.append(angle)
        left_offsets.append(left_pixel_offset)
        right_offsets.append(right_pixel_offset)

        # Check if zero-crossing is found for left eye and update the post zero-crossing counter
        if left_zero_found:
            left_zero_post_count += 1

        # Check for zero-crossing for left eye if not found yet
        if not left_zero_found:
            for idx in range(1, len(left_offsets)):
                y1, y2 = left_offsets[idx - 1], left_offsets[idx]
                x1, x2 = angles[idx - 1], angles[idx]
                if y1 * y2 <= 0 and abs(y2 - y1) < 20:
                    left_zero_crossing = x1 + (0 - y1) * (x2 - x1) / (y2 - y1)
                    left_zero_crossing_index = idx
                    left_zero_found = True
                    break

        # Check if zero-crossing is found for right eye and update the post zero-crossing counter
        if right_zero_found:
            right_zero_post_count += 1

        # Check for zero-crossing for right eye if not found yet
        if not right_zero_found:
            for idx in range(1, len(right_offsets)):
                y1, y2 = right_offsets[idx - 1], right_offsets[idx]
                x1, x2 = angles[idx - 1], angles[idx]
                if y1 * y2 <= 0 and abs(y2 - y1) < 20:
                    right_zero_crossing = x1 + (0 - y1) * (x2 - x1) / (y2 - y1)
                    right_zero_crossing_index = idx
                    right_zero_found = True
                    break

        # Break the loop if we've passed both crossings by the post-crossing margin
        if left_zero_found and right_zero_found and left_zero_post_count >= post_crossing_margin and right_zero_post_count >= post_crossing_margin:
            break

        i -= ANGLE_TEST_INCREMENT

        key = cv2.waitKey(1) & 0xFF
        if key == ord('s'):
            return angles[-1]

        cv2.imshow("Bigscreen VAP Tool", new_img)
        time.sleep(0.25)

    # Calculate average slope, pixels per degree (search a 10-value range around zero crossing)
    start_index = left_zero_crossing_index - post_crossing_margin
    end_index = left_zero_crossing_index + post_crossing_margin
    total_slope = 0
    n = end_index - start_index
    for i in range(start_index, end_index):
        total_slope += (left_offsets[i] - left_offsets[i - 1]) / (angles[i] - angles[i - 1])
    avg_slope = total_slope / (n - 1)
    pixels_per_degree = 1 / avg_slope

    print("Pixels per degree: ", pixels_per_degree)
    print("Average slope: ", avg_slope)
    print("Left crossing: ", left_zero_crossing)
    print("Right crossing: ", right_zero_crossing)
    logger.info(f"Pixels per degree: {pixels_per_degree}")
    logger.info(f"Average slope: {avg_slope}")
    logger.info(f"Left crossing: {left_zero_crossing}")
    logger.info(f"Right crossing: {right_zero_crossing}")

    formatted_angles = ', '.join(f'{num:.2f}' for num in angles)
    formatted_left_offsets = ', '.join(f'{num:.2f}' for num in left_offsets)
    formatted_right_offsets = ', '.join(f'{num:.2f}' for num in right_offsets)
    logger.info(f"Angles: {formatted_angles}")
    logger.info(f"Left Offsets: {formatted_left_offsets}")
    logger.info(f"Right Offsets: {formatted_right_offsets}")

    # Plot original data points
    plt.scatter(angles, left_offsets, color='blue', label='Data Points')
    plt.scatter(angles, right_offsets, color='red', label='Data Points')
    plt.scatter(angles[left_zero_crossing_index-post_crossing_margin:left_zero_crossing_index+post_crossing_margin],
                left_offsets[left_zero_crossing_index-post_crossing_margin:left_zero_crossing_index+post_crossing_margin],
                color='green', label='Left Crossing')
    plt.scatter(angles[right_zero_crossing_index-post_crossing_margin:right_zero_crossing_index+post_crossing_margin],
                right_offsets[right_zero_crossing_index-post_crossing_margin:right_zero_crossing_index+post_crossing_margin],
                color='green', label='Right Crossing')

    plt.xlabel('Angles')
    plt.ylabel('Offsets')
    plt.legend()
    plt.grid(True)
    plt.savefig(f"{serial_number}_alignment_coarse.jpg")
    #plt.close()
    plt.show()

    best_pitch_left = -left_zero_crossing
    best_pitch_right = -right_zero_crossing
    align_set_best_pitch(best_pitch_left, best_pitch_right)
    align_restart_steamvr()

    refine_stage_passed = False
    while refine_stage_passed is False:
        if align_refine_best_pitch():
            refine_stage_passed = True

    return best_pitch_left, best_pitch_right


def align_refine_best_pitch():
    global refine_delay_counter, pipe, best_pitch_left, best_pitch_right

    ret, img = video.read()
    new_gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    cv2.putText(img, "Refining", (0, 50), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2, cv2.LINE_AA)
    cv2.imshow('Bigscreen VAP Tool', img)

    line_length = 75  # Search area extends by this value above and below the center

    # Left eye calculation
    left_eye_top = (Align.hmd_centers[LEFT_EYE][1] - line_length, Align.hmd_centers[LEFT_EYE][0])
    left_eye_bottom = (Align.hmd_centers[LEFT_EYE][1] + line_length, Align.hmd_centers[LEFT_EYE][0])
    left_rr, left_cc = line(*left_eye_bottom, *left_eye_top)
    left_intensities = new_gray[left_rr, left_cc].astype('float64')
    left_intensities -= numpy.average(left_intensities)  # Normalize
    left_step_filter = numpy.hstack((numpy.ones(len(left_intensities)), -1 * numpy.ones(len(left_intensities))))
    left_step_conv = numpy.convolve(left_intensities, left_step_filter, mode='valid')
    left_step_index = numpy.argmax(left_step_conv)

    # Right eye calculations
    right_eye_top = (Align.hmd_centers[RIGHT_EYE][1] - line_length, Align.hmd_centers[RIGHT_EYE][0])
    right_eye_bottom = (Align.hmd_centers[RIGHT_EYE][1] + line_length, Align.hmd_centers[RIGHT_EYE][0])
    right_rr, right_cc = line(*right_eye_bottom, *right_eye_top)
    right_intensities = new_gray[right_rr, right_cc].astype('float64')
    right_intensities -= numpy.average(right_intensities)  # Normalize
    right_step_filter = numpy.hstack((numpy.ones(len(right_intensities)), -1 * numpy.ones(len(right_intensities))))
    right_step_conv = numpy.convolve(right_intensities, right_step_filter, mode='valid')
    right_step_index = numpy.argmax(right_step_conv)

    # Plotting
    plt.plot(left_intensities, label='Original Values (Left Eye)')
    plt.plot(left_step_conv / 10, label='Convolution Result (Left Eye)')
    plt.plot((left_step_index, left_step_index), (left_step_conv[left_step_index] / 10, 0), 'r',
             label='Step Transition (Left Eye)')
    plt.plot(right_intensities, label='Original Values (Right Eye)')
    plt.plot(right_step_conv / 10, label='Convolution Result (Right Eye)')
    plt.plot((right_step_index, right_step_index), (right_step_conv[right_step_index] / 10, 0), 'g',
             label='Step Transition (Right Eye)')

    plt.xlabel('Index along the Line')
    plt.ylabel('Pixel Intensity')
    plt.legend()
    plt.grid(True)
    plt.savefig(f"{serial_number}_alignment_fine.jpg")
    plt.close()

    if left_step_index != 0 and right_step_index != 0:
        refine_delay_counter += 1  # TODO: Handle CV camera warm-up more gracefully
        if refine_delay_counter < 20:
            cv2.waitKey(1)
            cv2.putText(img, "Refining", (0, 50), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2, cv2.LINE_AA)
            cv2.imshow('Bigscreen VAP Tool', img)
            return False

        cv2.imwrite(f"{serial_number}_before_refinement.jpg", img)

        left_offset = left_step_index - line_length
        right_offset = right_step_index - line_length
        left_offset *= pixels_per_degree
        right_offset *= pixels_per_degree

        best_pitch_left += left_offset
        best_pitch_right += right_offset
        align_set_best_pitch(best_pitch_left, best_pitch_right)
        align_restart_steamvr()
        return True

    return False


def align_minimize_yaw(eye):
    win32file.WriteFile(pipe, "3".encode())  # Switch to vertical line mode.
    wait_for_unity()
    my_bytes = bytearray("p".encode())
    my_bytes += bytearray(struct.pack("f", Align.best_pitch[eye]))  # Reset pitch
    win32file.WriteFile(pipe, my_bytes)
    wait_for_unity()
    my_bytes = bytearray("y".encode())
    my_bytes += bytearray(struct.pack("f", 20.0))  # Reset yaw
    win32file.WriteFile(pipe, my_bytes)
    wait_for_unity()
    my_bytes = bytearray("r".encode())
    my_bytes += bytearray(struct.pack("f", 0.0))  # Reset roll
    win32file.WriteFile(pipe, my_bytes)
    wait_for_unity()
    # Minimize the error for yaw from the left to right.
    maximum = 0
    pivot = 0.0
    for i in numpy.arange(pivot + ANGLE_TEST_LIMIT, pivot - ANGLE_TEST_LIMIT, -ANGLE_TEST_INCREMENT):
        angle = float(i)
        print(angle, flush=True)
        # Apply the proposed roll and visualize it.
        yaw = angle
        my_bytes = bytearray("y".encode())
        my_bytes += bytearray(struct.pack("f", yaw))
        win32file.WriteFile(pipe, my_bytes)
        wait_for_unity()
        # Grab the latest camera frame.
        ret, new_img = video.read()  # Capture the new image.
        new_gray = cv2.cvtColor(new_img, cv2.COLOR_BGR2GRAY)
        new_gray_copy = numpy.copy(new_gray)
        # Mask out the other eye.
        full_screen = [int(new_img.shape[1]), int(new_img.shape[0])]
        mid_screen = [int(new_img.shape[1] / 2), int(new_img.shape[0] / 2)]
        mask = numpy.array([[0, 0], [0, 0], [0, 0], [0, 0]])
        if eye == LEFT_EYE:
            mask = numpy.array([[mid_screen[0], 0], [mid_screen[0], full_screen[1]],
                                [full_screen[0], full_screen[1]], [full_screen[0], 0]])
        elif eye == RIGHT_EYE:
            mask = numpy.array([[0, 0], [0, full_screen[1]],
                                [mid_screen[0], full_screen[1]], [mid_screen[0], 0]])
        cv2.fillPoly(new_img, pts=[mask], color=(0, 0, 0))
        cv2.fillPoly(new_gray_copy, pts=[mask], color=(0, 0, 0))
        # Calculate area which we want to maximize for a given rotation.
        x = Align.eye_bounds[eye][0]
        y = Align.eye_bounds[eye][1]
        width = Align.eye_bounds[eye][2]
        height = Align.eye_bounds[eye][3]
        eye_region = new_gray[y: y + height, x: x + width]
        total_area = numpy.sum(eye_region)
        if total_area > maximum:
            maximum = total_area
            Align.best_yaw[eye] = angle
        cv2.waitKey(1)
        cv2.imshow("Bigscreen VAP Tool", new_img)
    return Align.best_yaw[eye, 0]


def align_minimize_roll(eye):
    global current_roll_iterations, max_roll_iterations, should_apply_roll
    win32file.WriteFile(pipe, "3".encode())  # Switch to grid mode
    wait_for_unity()
    my_bytes = bytearray("p".encode())
    my_bytes += bytearray(struct.pack("f", Align.best_pitch[eye]))  # Reset pitch
    win32file.WriteFile(pipe, my_bytes)
    wait_for_unity()
    my_bytes = bytearray("y".encode())
    my_bytes += bytearray(struct.pack("f", Align.best_yaw[eye]))  # Reset yaw
    win32file.WriteFile(pipe, my_bytes)
    wait_for_unity()
    my_bytes = bytearray("r".encode())
    my_bytes += bytearray(struct.pack("f", 0))  # Reset roll
    win32file.WriteFile(pipe, my_bytes)
    wait_for_unity()

    hough_param = 65
    angle_offsets = deque(maxlen=25)
    pitch_angle = -15
    while True:
        current_roll_iterations += 1

        # If roll is taking too long, stop and skip applying correction
        if current_roll_iterations > max_roll_iterations:
            # Save the current image for debugging purposes
            ret, img = video.read()
            cv2.imwrite(f"{serial_number}_roll_fail_{eye}.jpg", img)
            logger.error(f"Roll took too long for eye {eye}, skipping correction")
            current_roll_iterations = 0
            avg_angle = 0
            should_apply_roll[eye] = False
            break

        pitch_angle += 0.005
        my_bytes = bytearray("p".encode())
        my_bytes += bytearray(struct.pack("f", pitch_angle))
        win32file.WriteFile(pipe, my_bytes)
        wait_for_unity()

        ret, img = video.read()
        # Mask out the other eye.
        full_screen = [int(img.shape[1]), int(img.shape[0])]
        mid_screen = [int(img.shape[1] / 2), int(img.shape[0] / 2)]
        mask = numpy.array([[0, 0], [0, 0], [0, 0], [0, 0]])
        if eye == LEFT_EYE:
            mask = numpy.array([[mid_screen[0], 0], [mid_screen[0], full_screen[1]],
                                [full_screen[0], full_screen[1]], [full_screen[0], 0]])
        elif eye == RIGHT_EYE:
            mask = numpy.array([[0, 0], [0, full_screen[1]],
                                [mid_screen[0], full_screen[1]], [mid_screen[0], 0]])
        cv2.fillPoly(img, pts=[mask], color=(0, 0, 0))

        img = cv2.GaussianBlur(img, (11, 11), 3)
        edges = cv2.Canny(img, 255 * 0.25, 255 * 0.40, apertureSize=3)
        lines = cv2.HoughLinesP(edges, 1, numpy.pi / 180, threshold=hough_param, minLineLength=50, maxLineGap=5)

        if SHOW_DEBUG_VIEW is True:
            if eye == 0:
                mid_col = img.shape[1] // 2
                for i in range(img.shape[0]):
                    for j in range(mid_col, img.shape[1]):
                        img[i, j] = edges[i, j - mid_col]
            else:
                mid_col = img.shape[1] // 2
                for i in range(img.shape[0]):
                    for j in range(mid_col):
                        img[i, j] = edges[i, j + mid_col]

        if lines is not None:
            avg_angle = 0
            for found_line in lines:
                x1, y1, x2, y2 = found_line[0]
                slope = (y2 - y1) / (x2 - x1 + 1e-10)
                theta = numpy.arctan(slope)
                angle_in_degrees = numpy.rad2deg(theta)
                avg_angle += angle_in_degrees

                if SHOW_DEBUG_VIEW is True:
                    cv2.line(img, (x1, y1), (x2, y2), (0, 255, 0), 2)
                    if eye == 0:
                        cv2.line(img, (x1 + mid_col, y1), (x2 + mid_col, y2), (0, 255, 0), 2)
                    else:
                        cv2.line(img, (x1 - mid_col, y1), (x2 - mid_col, y2), (0, 255, 0), 2)

            avg_angle /= len(lines)
            angle_offsets.append(avg_angle)

            if len(angle_offsets) == angle_offsets.maxlen:
                print("Average angle: ", numpy.mean(angle_offsets))
                plt.title("Roll Angle")
                plt.xlabel("Frames")
                plt.ylabel("Angle (degrees)")
                avg_angle = numpy.mean(angle_offsets)
                variance = numpy.var(angle_offsets)
                std_dev = numpy.std(angle_offsets)
                plt.plot(angle_offsets)
                plt.axhline(y=avg_angle + variance, color='g', linestyle='--')
                plt.axhline(y=avg_angle - variance, color='g', linestyle='--')
                title = f"Variance: {variance:.2f} | Std Dev: {std_dev:.2f} | Avg Angle: {avg_angle:.2f}"
                plt.axhline(y=avg_angle + std_dev, color='r', linestyle='-')
                plt.axhline(y=avg_angle - std_dev, color='r', linestyle='-')
                plt.axhline(y=avg_angle, color='r', linestyle='-')
                plt.title(title)
                logger.info(f" Applying roll to eye: {eye} | " + title)
                plt.savefig(f"{serial_number}_roll_angle_{eye}.jpg")
                plt.clf()
                break

        cv2.putText(img, f"Progress: {len(angle_offsets)*4}%", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1)
        if SHOW_DEBUG_VIEW is True:
            avg_angle = numpy.mean(angle_offsets) if angle_offsets else 0
            cv2.putText(img, f"Angle: {avg_angle:.2f}", (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
            cv2.putText(img, f"Hough Param: {hough_param}", (10, 90), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1)

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

    # Set to best (for this eye)
    my_bytes = bytearray("p".encode())
    my_bytes += bytearray(struct.pack("f", Align.best_pitch[eye]))  # Reset pitch
    win32file.WriteFile(pipe, my_bytes)
    wait_for_unity()
    my_bytes = bytearray("y".encode())
    my_bytes += bytearray(struct.pack("f", Align.best_yaw[eye]))  # Reset yaw
    win32file.WriteFile(pipe, my_bytes)
    wait_for_unity()
    my_bytes = bytearray("r".encode())
    my_bytes += bytearray(struct.pack("f", Align.best_roll[eye]))  # Reset roll
    win32file.WriteFile(pipe, my_bytes)
    wait_for_unity()
    ret, new_img = video.read()  # Capture the new image.
    cv2.waitKey(1)
    cv2.imshow("Bigscreen VAP Tool", new_img)
    current_roll_iterations = 0
    return avg_angle


def align_show_horizon():
    global should_find_transitions, left_transition_point, right_transition_point, transition_delta, mes_string_sent, uut_start, uut_stop
    if video.isOpened():
        ret, img = video.read()
        gray_img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
        cv2.line(img, Align.hmd_centers[LEFT_EYE], Align.hmd_centers[RIGHT_EYE], (255, 0, 0), 1)
        # If transition points aren't 0, determine pass/fail
        if left_transition_point != (0, 0) and right_transition_point != (0, 0):
            if transition_delta < 10:
                cv2.putText(img, "PASS", (0, 50), cv2.FONT_HERSHEY_SIMPLEX, 2, (0, 255, 0), 2, cv2.LINE_AA)
                if mes_string_sent is False:
                    uut_stop = time.time()
                    formatted_string = mes_add(serial_number, "PASS", uut_start, uut_stop, MES_SW_VER)
                    logger.info("Writing MES...")
                    logger.info(formatted_string)
                    mes_string_sent = True
            else:
                cv2.putText(img, "FAIL", (0, 50), cv2.FONT_HERSHEY_SIMPLEX, 2, (255, 0, 0), 2, cv2.LINE_AA)
                if mes_string_sent is False:
                    uut_stop = time.time()
                    formatted_string = mes_add(serial_number, "FAIL", uut_start, uut_stop, MES_SW_VER)
                    logger.info("Writing MES...")
                    logger.info(formatted_string)
                    mes_string_sent = True
        cv2.imshow('Bigscreen VAP Tool', img)

        if should_find_transitions is True:
            should_find_transitions = False
            left_transition_point = find_transition_point(Align.hmd_centers[LEFT_EYE], gray_img)
            right_transition_point = find_transition_point(Align.hmd_centers[RIGHT_EYE], gray_img)
            print("Left transition point: ", left_transition_point)
            logger.info(f"Left transition point: {left_transition_point}")
            print("Right transition point: ", right_transition_point)
            logger.info(f"Right transition point: {right_transition_point}")
            transition_delta = abs(left_transition_point[1] - right_transition_point[1])

    # If Enter is pressed, close window
    key = cv2.waitKey(1) & 0xFF
    if key == 13:  # Enter key
        align_destroy()


def find_transition_point(coord, img, scan_range=25):
    x, y = coord
    for i in range(y - scan_range, y + scan_range):
        if i >= img.shape[0]:  # Ensure we don't go out of image bounds
            break
        if img[i, x] < 128:  # Consider pixel values below 128 as black
            return (x, i)
    return (x, y)


def align_destroy():
    global current_roll_iterations, should_apply_roll, mes_string_sent, should_find_transitions
    global left_transition_point, right_transition_point
    if Align.p_unity is not None:
        Align.p_unity.kill()
        Align.p_unity = None
        win32file.CloseHandle(pipe)
    current_roll_iterations = 0
    should_apply_roll = [True, True]
    should_find_transitions = True
    left_transition_point = (0, 0)
    right_transition_point = (0, 0)
    mes_string_sent = False
    video.release()
    cv2.destroyAllWindows()


def align_set_best_pitch(pitch_left, pitch_right):
    global best_pitch_left, best_pitch_right

    best_pitch_right = pitch_right
    best_pitch_left = pitch_left
    rot = Rotation.from_euler("XYZ", [pitch_left + DOWNWARD_CANT, horizontal_cant, best_roll_left], degrees=True)
    config['tracking_to_eye_transform'][0]['eye_to_head'] = rot.as_matrix().tolist()
    rot = Rotation.from_euler("XYZ", [pitch_right + DOWNWARD_CANT, -horizontal_cant, best_roll_right], degrees=True)
    config['tracking_to_eye_transform'][1]['eye_to_head'] = rot.as_matrix().tolist()

    lh.upload_config(config)


def align_set_best_roll(roll_left, roll_right):
    global best_roll_left, best_roll_right

    best_roll_right = roll_right
    best_roll_left = roll_left
    rot = Rotation.from_euler("XYZ", [best_pitch_left, horizontal_cant, roll_left], degrees=True)
    if should_apply_roll[0]:
        config['tracking_to_eye_transform'][0]['eye_to_head'] = rot.as_matrix().tolist()
    rot = Rotation.from_euler("XYZ", [best_pitch_right, -horizontal_cant, roll_right], degrees=True)
    if should_apply_roll[1]:
        config['tracking_to_eye_transform'][1]['eye_to_head'] = rot.as_matrix().tolist()

    lh.upload_config(config)
    align_restart_steamvr()


def align_reset_config_values():
    rot = Rotation.from_euler("XYZ", [0, horizontal_cant, 0], degrees=True)
    config['tracking_to_eye_transform'][0]['eye_to_head'] = rot.as_matrix().tolist()
    rot = Rotation.from_euler("XYZ", [0, -horizontal_cant, 0], degrees=True)
    config['tracking_to_eye_transform'][1]['eye_to_head'] = rot.as_matrix().tolist()

    lh.upload_config(config)
    align_restart_steamvr()


def align_restart_steamvr():
    global pipe

    is_unity_open = False
    if Align.p_unity is not None:
        is_unity_open = True
        Align.p_unity.kill()
        Align.p_unity = None
        win32file.CloseHandle(pipe)

    subprocess.call(['taskkill', '/IM', 'vrmonitor.exe', '/F'])
    subprocess.call(['taskkill', '/IM', 'vrserver.exe', '/F'])
    subprocess.call(['taskkill', '/IM', 'vrwebhelper.exe', '/F'])
    time.sleep(2)

    subprocess.Popen(["C:\\Program Files (x86)\\Steam\\steamapps\\common\\SteamVR\\bin\\win64\\vrmonitor.exe"])
    time.sleep(2)

    if is_unity_open is True:
        pipe = win32pipe.CreateNamedPipe(r'\\.\pipe\vap_pipe',
                                         win32pipe.PIPE_ACCESS_DUPLEX,
                                         win32pipe.PIPE_TYPE_MESSAGE | win32pipe.PIPE_WAIT,
                                         1, 65536, 65536, 300, None)

        Align.p_unity = subprocess.Popen(subprocess_path, stdin=subprocess.PIPE, stdout=subprocess.PIPE)
        atexit.register(align_destroy)
        win32pipe.ConnectNamedPipe(pipe, None)
        win32file.WriteFile(pipe, "4".encode())
        _, data = win32file.ReadFile(pipe, 256)


def align_get_left_config_string():
    rot = numpy.array(config['tracking_to_eye_transform'][0]['eye_to_head'])
    left_eye_pitch = round(Rotation.from_matrix(rot).as_euler("XYZ", True)[0], 2)
    left_eye_yaw = round(Rotation.from_matrix(rot).as_euler("XYZ", True)[1], 2)
    left_eye_roll = round(Rotation.from_matrix(rot).as_euler("XYZ", True)[2], 2)
    config_string = f"Left eye offset: {left_eye_pitch} | {left_eye_yaw} | {left_eye_roll}"
    return config_string


def align_get_right_config_string():
    rot = numpy.array(config['tracking_to_eye_transform'][1]['eye_to_head'])
    right_eye_pitch = round(Rotation.from_matrix(rot).as_euler("XYZ", True)[0], 2)
    right_eye_yaw = round(Rotation.from_matrix(rot).as_euler("XYZ", True)[1], 2)
    right_eye_roll = round(Rotation.from_matrix(rot).as_euler("XYZ", True)[2], 2)
    config_string = f"Right eye offset: {right_eye_pitch} | {right_eye_yaw} | {right_eye_roll}"
    return config_string


def confirm_distortion():
    return True
    # Read through config, make sure there are exactly 2 distortion values
    print("Confirming distortion values...")
    logger.info("Confirming distortion values...")
    distortion_values = []
    try:
        distortion_values.append(config['tracking_to_eye_transform'][0]['distortion_red'])
        distortion_values.append(config['tracking_to_eye_transform'][1]['distortion_red'])
    except KeyError:
        print("Failed to detect calibration, run calibration upload again")
        return False

    print(f"distortion_red values: {distortion_values}")
    logger.info(f"distortion_red values: {distortion_values}")
    return True


def confirm_tracking_calibration() -> bool:
    return True
    if config is None:
        return False

    return True