import cv2
import argparse
import subprocess
import time
import numpy as np

import numpy as np
import cv2
import threading
from copy import deepcopy

thread_lock = threading.Lock()
thread_exit = False

class VideoThread(threading.Thread):
    def __init__(self, camera_id, img_height, img_width):
        super(VideoThread, self).__init__()
        self.camera_id = camera_id
        self.img_height = img_height
        self.img_width = img_width
        self.frame = np.zeros((img_height, img_width, 3), dtype=np.uint8)

    def get_frame(self):
        return deepcopy(self.frame)

    def run(self):
        global thread_exit
        cap = cv2.VideoCapture(self.camera_id)
        cap.set(cv2.CAP_PROP_FRAME_WIDTH, self.img_width)
        cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self.img_height)
        frame_num = 0
        timestampes = []
        while not thread_exit:
            ret, frame = cap.read()
            if ret:
                #thread_lock.acquire()
                self.frame = frame
                if frame_num == 0:
                    time_start = time.perf_counter()
                frame_num = frame_num + 1
                #thread_lock.release()
                print('get' + self.camera_id[-6:] + f'frame {frame_num}')
            else:
                thread_exit = True
            time_end = time.perf_counter()
            time_ms = (time_end - time_start)*1000
            timestampes.append(time_ms)
            if frame_num >= 3000:
                thread_exit = True
                timestampes = np.array(timestampes)
                np.save(self.camera_id[-6:] + '.npy', timestampes)
        cap.release()


def main():
    # Set up command-line argument parser
    parser = argparse.ArgumentParser(description='Real-time display of GREY image from mv-cam')
    parser.add_argument('--roix', type=int, default=0, help='roi start x (default: 0)')
    parser.add_argument('--roiy', type=int, default=0, help='roi start y (default: 0)')
    parser.add_argument('--width', type=int, default=704, help='image width (default: 640)')
    parser.add_argument('--height', type=int, default=544, help='image height (default: 480)')
    parser.add_argument('--fps', type=int, default=320, help='frame rate (default: 30)')
    parser.add_argument('--pixelformat', type=int, default=12, help='pixel format (default: 12)')

    args = parser.parse_args()
    
    v4l2_cmd = f"v4l2-ctl --set-ctrl roi_x={args.roix}"
    subprocess.run(v4l2_cmd, shell=True)
    
    v4l2_cmd = f"v4l2-ctl --set-ctrl roi_y={args.roiy}"
    subprocess.run(v4l2_cmd, shell=True)
    
    v4l2_cmd = f"v4l2-ctl --set-fmt-video=width={args.width},height={args.height}"
    subprocess.run(v4l2_cmd, shell=True)
    
    v4l2_cmd = f"v4l2-ctl --set-ctrl frame_rate={args.fps}"
    subprocess.run(v4l2_cmd, shell=True)

    v4l2_cmd = "v4l2-ctl --set-ctrl low_latency_mode=1"
    subprocess.run(v4l2_cmd, shell=True)

    v4l2_cmd = f"v4l2-ctl --set-ctrl pixelformat=XY{args.pixelformat}"
    subprocess.run(v4l2_cmd, shell=True)

    #v4l2_cmd = "v4l2-ctl --set-ctrl exposure_auto=1"
    #subprocess.run(v4l2_cmd, shell=True)

    #v4l2_cmd = "v4l2-ctl --set-ctrl exposure_absulute=20"
    #subprocess.run(v4l2_cmd, shell=True)

    v4l2_cmd = "v4l2-ctl --set-ctrl trigger_mode=1"
    subprocess.run(v4l2_cmd, shell=True)

    v4l2_cmd = "v4l2-ctl --set-ctrl trigger_src=1"
    subprocess.run(v4l2_cmd, shell=True)

    v4l2_cmd = "v4l2-ctl --set-ctrl vi_time_out_disable=1"
    subprocess.run(v4l2_cmd, shell=True)


    global thread_exit
    img_height = args.height
    img_width = args.width

    thread_0 = VideoThread('/dev/video0', img_height, img_width)
    thread_1 = VideoThread('/dev/video1', img_height, img_width)
    thread_0.start()
    thread_1.start()
    #frame_num = 0
    while not thread_exit:
        #thread_lock.acquire()
        #frame_0 = thread_0.get_frame()
        #print(f'get cam0 {frame_num}')
        #frame_1 = thread_1.get_frame()
        #print(f'get cam1 {frame_num}')
        #thread_lock.release()
        #thread_0.join()
        #thread_1.join()
        #frame_num = frame_num + 1
        #if frame_num >= 3000:
            #thread_exit = True
        pass
    print('done!')
    # Release resources
    v4l2_cmd = "v4l2-ctl --set-ctrl vi_time_out_disable=0"
    subprocess.run(v4l2_cmd, shell=True)

if __name__ == '__main__':
    main()