import cv2
import sys
import os
import numpy as np
import lcm
from eyeris.image_t import image_t 

# 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=8000")
os.system("v4l2-ctl -d /dev/video" + sys.argv[1] + " --set-ctrl gain=1600")
os.system("v4l2-ctl -d /dev/video" + sys.argv[1] + " --set-ctrl sensor_mode=1")

x_m = 2720
y_m = 1824

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_top = cv2.resize(2*img1[(x_m-100):(x_m+100):2,(y_m-100):(y_m+100):2], (0,0), fx=6.0, fy=6.0, interpolation=cv2.INTER_NEAREST)
        cv2.imshow('Cam ' + sys.argv[1], img_top)
        #cv2.imshow('Video2!', 2*img2[0:1024,0:1024])
    except Exception as e:
        print(e)

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

cap.release()
cv2.destroyAllWindows()