# TBD: automatic thresholding, feature map (transformation), PLC library
#!/usr/bin/env python3
import sys
import argparse
import rospy
import time
import rospy
import cv2
import numpy as np
import time

from cv_bridge import CvBridge, CvBridgeError
from geometry_msgs.msg import Twist
from sensor_msgs.msg import Image

image_ros = True

max_value = 255
max_value_H = 360//2
low_H = 0
low_S = 0
low_V = 0
high_H = max_value_H
high_S = max_value
high_V = max_value
window_capture_name = 'Video Capture'
window_detection_name = 'Object Detection'
low_H_name = 'Low H'
low_S_name = 'Low S'
low_V_name = 'Low V'
high_H_name = 'High H'
high_S_name = 'High S'
high_V_name = 'High V'
bridge_object = CvBridge()
def on_low_H_thresh_trackbar(val):
    global low_H
    global high_H
    low_H = val
    low_H = min(high_H-1, low_H)
    cv2.setTrackbarPos(low_H_name, window_detection_name, low_H)
def on_high_H_thresh_trackbar(val):
    global low_H
    global high_H
    high_H = val
    high_H = max(high_H, low_H+1)
    cv2.setTrackbarPos(high_H_name, window_detection_name, high_H)
def on_low_S_thresh_trackbar(val):
    global low_S
    global high_S
    low_S = val
    low_S = min(high_S-1, low_S)
    cv2.setTrackbarPos(low_S_name, window_detection_name, low_S)
def on_high_S_thresh_trackbar(val):
    global low_S
    global high_S
    high_S = val
    high_S = max(high_S, low_S+1)
    cv2.setTrackbarPos(high_S_name, window_detection_name, high_S)
def on_low_V_thresh_trackbar(val):
    global low_V
    global high_V
    low_V = val
    low_V = min(high_V-1, low_V)
    cv2.setTrackbarPos(low_V_name, window_detection_name, low_V)
def on_high_V_thresh_trackbar(val):
    global low_V
    global high_V
    high_V = val
    high_V = max(high_V, low_V+1)
    cv2.setTrackbarPos(high_V_name, window_detection_name, high_V)
def camera_callback(data):
    rospy.loginfo("callback started")
    

    try:
        # Select bgr8 because its the OpenCV encoding by default
        cv_image = bridge_object.imgmsg_to_cv2(data, desired_encoding="passthrough")
        #cv2.imshow("Original_image",cv_image)    
    except CvBridgeError as e:
        print(e)
    cv2.waitKey(10)
    #ret1, cv_image = capture.read()

    frame_HSV = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
    frame_threshold = cv2.inRange(frame_HSV, (low_H, low_S, low_V), (high_H, high_S, high_V))
    
    
    cv2.imshow(window_capture_name, cv_image)
    cv2.imshow(window_detection_name, frame_threshold)

if not image_ros:
    capture = cv2.VideoCapture("/home/andrew/Desktop/webcam_data/webcam/harvest2.mp4")
    if (capture.isOpened()== False):
        print("Error opening file.Please check path or name")
else:
    bridge_object = CvBridge()
    image_sub = rospy.Subscriber("/zed/zed_node/left/image_rect_color",Image,camera_callback) # rectified image from left camerarate = rospy.Rate(10) # 10hz


cv2.namedWindow(window_capture_name)
cv2.namedWindow(window_detection_name)
cv2.createTrackbar(low_H_name, window_detection_name , low_H, max_value_H, on_low_H_thresh_trackbar)
cv2.createTrackbar(high_H_name, window_detection_name , high_H, max_value_H, on_high_H_thresh_trackbar)
cv2.createTrackbar(low_S_name, window_detection_name , low_S, max_value, on_low_S_thresh_trackbar)
cv2.createTrackbar(high_S_name, window_detection_name , high_S, max_value, on_high_S_thresh_trackbar)
cv2.createTrackbar(low_V_name, window_detection_name , low_V, max_value, on_low_V_thresh_trackbar)
cv2.createTrackbar(high_V_name, window_detection_name , high_V, max_value, on_high_V_thresh_trackbar)
rospy.init_node('hsv_toolbar', anonymous=True)
rospy.loginfo("Node is starting ...")


#------------------#
while True:
    time.sleep(0.05)
    
cv2.destroyAllWindows()