import cv2
import sys
import os
import time
import numpy as np

# open /dev/video0
cap = cv2.VideoCapture(int(sys.argv[1]), cv2.CAP_V4L2)

# set output format
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 5440)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 3648)
cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc('R', 'G', '1', '2'))

# turn off automatic debayering becuase we have to reshape first
cap.set(cv2.CAP_PROP_CONVERT_RGB, 0)

# These no not work, need to use v4l2-ctrl
# cap.set(cv2.CAP_PROP_EXPOSURE,30000)
# cap.set(cv2.CAP_PROP_GAIN,1700)

os.system("v4l2-ctl -d /dev/video" + sys.argv[1] + " --set-ctrl exposure=25000")
os.system("v4l2-ctl -d /dev/video" + sys.argv[1] + " --set-ctrl gain=" + sys.argv[2])
os.system("v4l2-ctl -d /dev/video" + sys.argv[1] + " --set-ctrl sensor_mode=1")

x_m = 2720
y_m = 1824
s = 100

counter = 0

while True:
    try:
        r,f = cap.read()
        #f = np.frombuffer(np.ndarray.tobytes(f), 'uint16')
        img1 = np.reshape(f[0,1::2].copy(),(3648,5504))
        #img2 = np.reshape(f[0,0::2].copy(),(3648,5504))
        #img = img.astype('uint8')
        #img_color = cv2.cvtColor(img, cv2.COLOR_BayerRG2RGB)
        #i2 = cv2.resize(img_color, (0,0), fx=0.5, fy=0.5)
        img_mid = cv2.resize(2*img1[(x_m-s):(x_m+s),(y_m-s):(y_m+s)], (0,0), fx=6.0, fy=6.0, interpolation=cv2.INTER_NEAREST)
        #img_top = cv2.resize(5*img1[0:(2*s),0:(2*s)], (0,0), fx=6.0, fy=6.0, interpolation=cv2.INTER_NEAREST)
        #img_bottom = cv2.resize(5*img1[0:(2*s),-(2*s):], (0,0), fx=6.0, fy=6.0, interpolation=cv2.INTER_NEAREST)
        #img = np.hstack([img_top, img_mid, img_bottom])
        cv2.imshow('Cam ' + sys.argv[1], img_mid)
        #with open('/NVMEDATA/cam_' + sys.argv[1] + '_' + str(int(time.time()*1000000)) + '.raw',"wb") as out:
        #    out.write(f.tobytes())
        
        #cv2.imshow('Video2!', 2*img2[0:1024,0:1024])
        if counter % 100 == 0:
            print("Camera " + sys.argv[1] + " Frame: " + str(counter))
        counter += 1
    
    except Exception as e:
        print(e)

    if cv2.waitKey(1) & 0xFF == ord('q'):
        break


os.system("v4l2-ctl -d /dev/video" + sys.argv[1] + " --set-ctrl sensor_mode=0")

cap.release()
cv2.destroyAllWindows()