# -*- coding: utf-8 -*-
"""
Created on Thu Mar  1 22:13:35 2018

@author: Marin Čereg
Za Izradu sustava skupljanja loptica robotskim manipulatorom
program je zamišljen raditi na Raspbery PI
"""

# Ucitavanje biblioteka
from collections import deque
import numpy as np
import argparse
import imutils
import cv2
import urllib #UCITAVANJE URL-a

Loop=1

# ARGUMENTI
ap = argparse.ArgumentParser()
ap.add_argument("-v", "--video",
    help="path to the (optional) video file")
ap.add_argument("-b", "--buffer", type=int, default=64,
    help="max buffer size")
args = vars(ap.parse_args())
#SEARCH CONDITION AND CORRECTION OF WRONG INPUT
print("please select color : GREEN,RED,BLUE,ALL")
Search=input()
while(Search!="red" and Search!="RED" and Search!="green" and Search!="GREEN" and Search!="blue" and Search!="BLUE"and Search!="ALL"and Search!="all"):
	Search=input()


# COLOR SPACE AND SEARCH OPTION
if (Search==("Red") or Search ==("RED")):
    lower = {'red':(166, 84, 141)}
    upper = {'red':(186,255,255)}
if (Search==("blue") or Search ==("BLUE")):
    lower = {'blue':(97, 100, 117)}
    upper = {'blue':(117,255,255)}
if (Search==("green") or Search ==("GREEN")):
    lower = {'green':(66, 122, 129)}
    upper = {'green':(86,255,255)}
if (Search==("ALL") or Search ==("all")):
    lower = {'red':(166, 84, 141), 'green':(66, 122, 129), 'blue':(97, 100, 117), 'yellow':(23, 59, 119), 'orange':(0, 50, 80)}
    upper = {'red':(186,255,255), 'green':(86,255,255), 'blue':(117,255,255), 'yellow':(54,255,255), 'orange':(20,255,255)}
# STANDRARD COLORS , LABEL
colors = {'red':(0,0,255), 'green':(0,255,0), 'blue':(255,0,0)}
if (Search==("ALL") or Search ==("all")):
    colors = {'red':(0,0,255), 'green':(0,255,0), 'blue':(255,0,0), 'yellow':(0, 255, 217), 'orange':(0,140,255)}
# REFERENCA AKO NEMA VIDEO SIGNALA
if not args.get("video", False):
    camera = cv2.VideoCapture(1)
# PROGRAM
else:
    #camera = cv2.VideoCapture(args["video"])
    camera = cv2.VideoCapture(1)

    (grabbed, frame) = camera.read()
while (Loop==1):
    # FRAME CAMPTURE
    (grabbed, frame) = camera.read()
    if args.get("video") and not grabbed:
        break

    # FRAME WIDTH
    frame = imutils.resize(frame, width=1000)
	#oznaka objekta
    blurred = cv2.GaussianBlur(frame, (11, 11), 0)
    hsv = cv2.cvtColor(blurred, cv2.COLOR_BGR2HSV)
    #for each color in dictionary check object in frame
    for key, value in upper.items():
        kernel = np.ones((9,9),np.uint8)
        mask = cv2.inRange(hsv, lower[key], upper[key])
        mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
        mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)
        # PRONADJI CENTAR OBJEKTA
        cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
            cv2.CHAIN_APPROX_SIMPLE)[-2]
        center = None
        # AKO PRONADJE BOJU
        if len(cnts) > 0:
            # pronadji kontur i napravi krug CENTROID
            c = max(cnts, key=cv2.contourArea)
            ((x, y), radius) = cv2.minEnclosingCircle(c)
            M = cv2.moments(c)
            center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
            # only proceed if the radius meets a minimum size. Correct this value for your obect's size
            if radius > 0.5:
                # NACRTAJ KRUG I UPDEJTAJ FRAME
                # FILTRIRAJ SCREEN
                cv2.circle(frame, (int(x), int(y)), int(radius), colors[key], 2)
                cv2.putText(frame,key , (int(x-radius),int(y-radius)), cv2.FONT_HERSHEY_SIMPLEX, 0.6,colors[key],2)
                print(key)
                print("X-os",x,"_____","Y-os",y)

    # SHOW FRAME
    cv2.imshow("Frame", frame)
    Loop=2

# CLEANUP
cv2.waitKey(0)
cv2.destroyAllWindows()
camera.release()
cv2.destroyAllWindows()