#!/usr/bin/python3
# Crop row navigation for autonomous field robot

import cv2
import numpy as np
import matplotlib.pyplot as plt
import sys

def rescale_frame(frame, percent): # rescale image considering aspect ratio
    width = int(frame.shape[1] * percent/ 100)
    height = int(frame.shape[0] * percent/ 100)
    dim = (width, height)
    return cv2.resize(frame, dim, interpolation =cv2.INTER_AREA)

image = cv2.imread('/home/caroline/Pictures/first.jpg')
image=rescale_frame(image, 20)

### TBD: add Gaussian Blurr

### Image Segmentation

## RGB
# apply mask that generates grayscale image highlighting green ROIs
b,g,r = cv2.split(image)
gscale = 2*g-r-b

""" cv2.imshow("Image after ExG mask is applied", gscale)
cv2.waitKey(0) """


# obtain binary image by applying intensity threshold
""" color = ('b','g','r')
for i,col in enumerate(color):
    hist = cv2.calcHist(image,[i],None,[256], [0,256])# image histogram
    plt.plot(hist, color=col)
    plt.title('Histogram of initial image')
plt.show() 
cv2.waitKey(0)  """


""" hist2 = cv2.calcHist(gscale,[0],None,[256], [0,256])# image histogram
plt.plot(hist2,'k')
plt.title('Histogram of grayscale image')
plt.show() 
cv2.waitKey(0)  """

ret2,gscale2 = cv2.threshold(gscale,127,225,0) 
ret,gscale = cv2.threshold(gscale,180,200,0) 
""" cv2.imshow("Binary image", gscale)
cv2.waitKey(0) """

while True:
    Hori = np.concatenate((gscale, gscale2), axis=1) # left and right image
    cv2.imshow('Comparing Different Thresholds', Hori)
    cv2.waitKey(1)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

cv2.destroyAllWindows()

## YUV
# get U and V Layer
image_yuv = cv2.cvtColor(image, cv2.COLOR_BGR2YUV)
(Y,U,V) = cv2.split(image_yuv)
task=2 #1: Layers; 2: histogram, 3: binary image, 4: combined layers
if task==1: 
    while True:
        cv2.imshow("U-Layer",U)
        cv2.imshow("V-Layer",V)
        cv2.waitKey(1)
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break
    cv2.destroyAllWindows()

elif task==2:
    for i in range(2):
        hist_yuv = cv2.calcHist(image_yuv,[i],None,[256], [0,256])
        plt.plot(hist_yuv)
        plt.legend('U' 'V')
        plt.title('Histogram of U and V Layer')
        plt.show() 
        cv2.waitKey(0)

elif task==3:
    ret_u,binary_u = cv2.threshold(U,120,130,0) 
    ret_v,binary_v = cv2.threshold(V,125,135,0) 
    while True:
        binary_uv = np.concatenate((binary_u, binary_v), axis=1) 
        cv2.imshow('U and V layer as binary image', binary_uv)
        cv2.waitKey(1)
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break
    cv2.destroyAllWindows()

elif task==4:
    uv_combi = cv2.bitwise_and(binary_u,binary_v)
    cv2.imshow('U and V layer as binary image', uv_combi)
    cv2.waitKey(0)