import cv2 as cv
import numpy as np

# imgWidth=564
# imgHeight=650
imgWidth=640
imgHeight=480

def processFrameforDisplay(frame):
    processed_frame=np.ndarray(shape=imgWidth*imgHeight,  dtype='<h',buffer=frame)    
    processed_frame=processed_frame.reshape((imgHeight,imgWidth))  
    return processed_frame
#%%
 
cap = cv.VideoCapture(1) # change to Miramar camera index
cap.set(cv.CAP_PROP_CONVERT_RGB,0)
 
framenum=0
 
if not cap.isOpened():
    print("Cannot open camera")
    exit()
 
MAX=34000
MIN=31500
gain=-(255)/(MAX-MIN)
offset=-MAX*gain
 
while True:
    ret, frame = cap.read()     
    framenum+=1
 
    if not ret:
        print("Can't receive frame (stream end?). Exiting ...")
        break
    
    #frame=frame.reshape((1,imgWidth*imgHeight))
    display_frame=processFrameforDisplay(frame)
    
 
    # to periodically recalculate gain and offset

    test_frame=display_frame
    
    # test_frame=processFrameFlatten(test_frame)
    
    # test_frame=test_frame*invgain
    
    added=test_frame+32768
    
    addedCopy=added.copy()
    addedCopy[added>MAX]=MAX
    addedCopy[added<MIN]=MIN
 
    test=addedCopy*gain+offset
    
    display_frame=test.astype(np.uint8)
    if framenum>80:
        framenum=0
        
        added_mean=added[:,0:590].mean()
        added_std=added[:,0:590].std()
        print('mean: '+str(added_mean))
        print(added_std)
        # added_std=100
        MAX=added_mean+2*added_std
        MIN=added_mean-3*added_std
        gain=-(255)/(MAX-MIN)
        offset=-MAX*gain
        
        
    cv.imshow('final',display_frame)
    
    k = cv.waitKey(5) & 0xFF
    if k == 27:
        break
 
cap.release()
cv.destroyAllWindows()