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



def frame_2_msg(msg, image):

    msg.utime = int(time.time()*1000000)
    msg.width = image.shape[1]
    msg.height = image.shape[0]
    msg.data = image.tobytes()

    msg.size = len(msg.data)
    msg.pixelformat = image_t.PIXEL_FORMAT_BE_GRAY16
    
    return msg

lc = lcm.LCM("udpm://239.255.76.67:7667?ttl=2")

# 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')
        #f = 
        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])
        msg = frame_2_msg(image_t(), img1[::2,::2])
        lc.publish('RAW_IMG_'+str(sys.argv[1]),msg.encode())
        
    except Exception as e:
        print(e)

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

cap.release()
cv2.destroyAllWindows()