import time
import cv2
import numpy as np

has_cuda = cv2.cuda.getCudaEnabledDeviceCount()
print(has_cuda)  # if one cuda is getCudaEnabledDeviceCount

left = "Left.mp4"
right = "Right.mp4"
final = "Final.mp4"
WIDTH = 2000 * 2
HEIGHT = 750 * 2
delay = 0
clip1 = None
clip2 = None
CLIP_START_FROM = 0
CLIP_OFFSET = 20

map1_left = None
map2_left = None

map1_right = None
map2_right = None

def shift_image_vertically(image, shift):
    """
    Shifts an image along the y-axis.

    Parameters:
    - image: The input image (2D or 3D array).
    - shift: The number of pixels to shift. Positive values shift downward, negative values shift upward.

    Returns:
    - Shifted image.
    """

    if len(image.shape) == 2:  # For grayscale images
        pad_width = ((abs(shift), 0), (0, 0))
    elif len(image.shape) == 3:  # For RGB images
        pad_width = ((abs(shift), 0), (0, 0), (0, 0))
    else:
        raise ValueError("Invalid image shape")

    if shift > 0:  # Shift downward
        return np.pad(image, pad_width=pad_width, mode="constant")[:-shift]
    elif shift < 0:  # Shift upward
        return np.pad(image, pad_width=pad_width, mode="constant")[-shift:]
    else:  # No shift
        return image


delay = int(1000 / 20)


def stitch_images(image_left, image_right, right_x_shift, right_y_shift):
    image_right = shift_image_vertically(image_right, right_y_shift)
    return np.concatenate((image_left, image_right), axis=1)


def straighten_image(image, coefficients):
    DIST_COEFF = np.array([coefficients[0], coefficients[1], coefficients[2], 0, 0])
    camera_matrix = np.array(
        [[3000, 0, image.shape[1] / 2], [0, 3000, image.shape[0] / 2], [0, 0, 1]],
        dtype="double",
    )
    return cv2.undistort(image, camera_matrix, DIST_COEFF)


clip1 = cv2.VideoCapture(left)
clip2 = cv2.VideoCapture(right)
fps = clip1.get(cv2.CAP_PROP_FPS)
delay = int(1000 / 20)

clip1.set(cv2.CAP_PROP_POS_FRAMES, CLIP_START_FROM + CLIP_OFFSET)
clip2.set(cv2.CAP_PROP_POS_FRAMES, CLIP_START_FROM)

def init_undistort():
import cv2
import numpy as np

has_cuda = cv2.cuda.getCudaEnabledDeviceCount()
print(has_cuda)  # if one cuda is getCudaEnabledDeviceCount

left = "Left.mp4"
right = "Right.mp4"
final = "Final.mp4"
WIDTH = 2000 * 2
HEIGHT = 750 * 2
delay = 0
clip1 = None
clip2 = None
CLIP_START_FROM = 0
CLIP_OFFSET = 20


def shift_image_vertically(image, shift):
    """
    Shifts an image along the y-axis.

    Parameters:
    - image: The input image (2D or 3D array).
    - shift: The number of pixels to shift. Positive values shift downward, negative values shift upward.

    Returns:
    - Shifted image.
    """

    if len(image.shape) == 2:  # For grayscale images
        pad_width = ((abs(shift), 0), (0, 0))
    elif len(image.shape) == 3:  # For RGB images
        pad_width = ((abs(shift), 0), (0, 0), (0, 0))
    else:
        raise ValueError("Invalid image shape")

    if shift > 0:  # Shift downward
        return np.pad(image, pad_width=pad_width, mode="constant")[:-shift]
    elif shift < 0:  # Shift upward
        return np.pad(image, pad_width=pad_width, mode="constant")[-shift:]
    else:  # No shift
        return image


delay = int(1000 / 20)


def stitch_images(image_left, image_right, right_x_shift, right_y_shift):
    image_right = shift_image_vertically(image_right, right_y_shift)
    return np.concatenate((image_left, image_right), axis=1)


def straighten_image(image, coefficients):
    DIST_COEFF = np.array([coefficients[0], coefficients[1], coefficients[2], 0, 0])
    camera_matrix = np.array(
       DIST_COEFF = np.array([coefficients[0], coefficients[1], coefficients[2], 0, 0])
    camera_matrix = np.array(
        [[3000, 0, image.shape[1] / 2], [0, 3000, image.shape[0] / 2], [0, 0, 1]],
        dtype="double",
    ) [[3000, 0, image.shape[1] / 2], [0, 3000, image.shape[0] / 2], [0, 0, 1]],
        dtype="double",
    )
    return cv2.undistort(image, camera_matrix, DIST_COEFF)


clip1 = cv2.VideoCapture(left)
clip2 = cv2.VideoCapture(right)
fps = clip1.get(cv2.CAP_PROP_FPS)
delay = int(1000 / 20)

clip1.set(cv2.CAP_PROP_POS_FRAMES, CLIP_START_FROM + CLIP_OFFSET)
clip2.set(cv2.CAP_PROP_POS_FRAMES, CLIP_START_FROM)


def start():
    index = 0
    while True:
        index += 1
        # clip2.    )

        ret1, frame1 = clip1.read()
        ret2, frame2 = clip2.read()

        if not ret1 or not ret2:
            # either 1 of the video's ended, break out of it
            break

        w = frame1.shape[0]
        h = frame1.shape[1]

        COEFFICIENTS_1 = [-0.199, -0.1300, -0.0150]
        frame1 = straighten_image(frame1, COEFFICIENTS_1)

        COEFFICIENTS_2 = [-0.4190, 0.0780, -0.0460]
        frame2 = straighten_image(frame2, COEFFICIENTS_2)

        final_frame = stitch_images(frame1, frame2, 0, 20)

        final_frame = final_frame[50:]

        # Resize final frame and write to final clip
        final_frame = cv2.resize(
            final_frame, (WIDTH, HEIGHT), interpolation=cv2.INTER_LINEAR
        )

        # final_clip.write(final_frame)
        cv2.imshow("Video", final_frame)
        key = cv2.waitKey(delay) & 0xFF


start()

def start():
    index = 0
    while True:
        index += 1
        frame_start = time.time()
        # clip2.    )

        ret1, frame1 = clip1.read()
        ret2, frame2 = clip2.read()

        if not ret1 or not ret2:
            # either 1 of the video's ended, break out of it
            break

        w = frame1.shape[0]
        h = frame1.shape[1]

        COEFFICIENTS_1 = [-0.199, -0.1300, -0.0150]
        frame1 = straighten_image(frame1, COEFFICIENTS_1)

        COEFFICIENTS_2 = [-0.4190, 0.0780, -0.0460]
        frame2 = straighten_image(frame2, COEFFICIENTS_2)

        final_frame = stitch_images(frame1, frame2, 0, 20)

        final_frame = final_frame[50:]

        # Resize final frame and write to final clip
        final_frame = cv2.resize(
            final_frame, (), interpolation=cv2.INTER_LINEAR
        )

        # final_clip.write(final_frame)
        cv2.imshow("Video", final_frame)
        frame_end = time.time()
        frame_time = frame_end - frame_start
        print(frame_time)
        key = cv2.waitKey(delay) & 0xFF


start()