#!/usr/bin/env python3

import numpy as np
import cv2 as cv

width, height = 2048, 768

cap0 = cv.VideoCapture(0)
cap1 = cv.VideoCapture(1)

if not (cap0.isOpened() and cap1.isOpened()):
    print("Could not open video device")

#Set the resolution
cap0.set(cv.CAP_PROP_FRAME_WIDTH, width)
cap0.set(cv.CAP_PROP_FRAME_HEIGHT, height)

cap1.set(cv.CAP_PROP_FRAME_WIDTH, width)
cap1.set(cv.CAP_PROP_FRAME_HEIGHT, height)

# Set the FourCC
# fourcc = cv.VideoWriter_fourcc('B','G','R','3')
# cap.set(cv.CAP_PROP_FOURCC, fourcc)

# Capture frame-by-frame
while(True):
    ret0, frm0 = cap0.read()
    ret1, frm1 = cap1.read()

    b0, g0, r0 = cv.split(frm0)
    b1, g1, r1 = cv.split(frm1)

    b, g, r = [np.zeros([height*2, width], np.uint16) for _ in range(3)]

    # interleave color channels from captured frames
    b[0::2], b[1::2] = b0, b1
    g[0::2], g[1::2] = g0, g1
    r[0::2], r[1::2] = r0, r1

    # decode raw encoding from RGB
    dat = np.zeros((width*2, height*2), np.uint16)
    dat[0::2] = (r.T << 4) | (g.T >> 4)
    dat[1::2] = ((g.T & 0xF) << 8) | (b.T)

    # cv.imwrite("/tmp/dat.png", dat*16)

    # split out sensel from mapping
    dat00, dat10 = dat[0::2,0::2], dat[1::2,0::2]
    dat01, dat11 = dat[0::2,1::2], dat[1::2,1::2]

    # cv.imwrite("/tmp/dat00.png", dat00*16)
    # cv.imwrite("/tmp/dat01.png", dat01*16)
    # cv.imwrite("/tmp/dat10.png", dat10*16)
    # cv.imwrite("/tmp/dat11.png", dat11*16)

    # recombine sensel into binned image
    vis = np.zeros((height*4, width), np.uint16)
    vis[0::4] = dat01.T
    vis[1::4] = dat11.T
    vis[2::4] = dat00.T
    vis[3::4] = dat10.T

    # split the two sequential frames
    vis0, vis1 = np.split(vis, 2)

    # Display the resulting frame(s)
    cv.imshow("preview", vis0*16)
    cv.imwrite("/tmp/vis0.png", vis0*16)

    #cv.imshow("preview", vis1*16)
    #cv.imwrite("/tmp/vis1.png", vis1*16)

    #Waits for a user input to quit the application
    if cv.waitKey(1) & 0xFF == ord('q'):
        break

# When everything done, release the capture
cap.release()
cv.destroyAllWindows()