#I smoke weedz every day for da memez
import numpy as np
import cv2
import sys
import serial

#Determines the port for serial communication (Arduino)
ser=serial.Serial('/dev/ttyACM0',9600)
#Using Python 2.7
sys.path.append('/usr/local/lib/python2.7/site-packages')


#x max = 650
#y max = 480

cam = cv2.VideoCapture(0)
cam.set(3,320)
cam.set(4,240)

cv2.namedWindow('frame')

cv2.namedWindow('frame2')

cv2.moveWindow('frame', 0,0)
cv2.moveWindow('frame2', 645,35)


while True:
    ballcollected = False
    firstcone = False
    secondcone = False
    lastcone = False
    greenL = (30, 22, 80)
    greenH = (50, 200, 255)
    while True:
        ballfound = False
        ret, image =cam.read()
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
        greencolor = cv2.dilate(cv2.erode(cv2.inRange(hsv, greenL, greenH), None, iterations=2), None, iterations=2)



        contour = cv2.findContours(greencolor.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]


        #initialize center
        c = None

        #if there is at least one green object
        if len(contour) > 0:
            #biggest green object
            largest = max(contour, key=cv2.contourArea)
            #radius and center
            ((x,y), r) = cv2.minEnclosingCircle(largest)
            #finds center of mass
            #moments- help finds things such as area, center, etc.
            M = cv2.moments(largest)
            xx = (int(M["m10"] / M["m00"]))
            yy = (int(M["m01"] / M["m00"]))
            c = (int(M["m10"] / M["m00"]),int(M["m01"] / M["m00"]))

            #draw circle
            #make sure object is sufficiently large
            if r > 15:
                cv2.circle(image, c, 5, (0,255,255), -1)
                cv2.circle(image, (int(x), int(y)), int(r), (255,0,255), 2)
                ballfound = True
        cv2.imshow('frame',image)
        cv2.imshow('frame2',greencolor)
        if ballfound:
            #when ball stops moving

            #if cbefore == None:


            #deltad = xx
            #if delta < 15:
            #   ballstopped = True


            #print(int(x))
            #print(int(r))


            #replace with moments center

            if lastcone: #oriented towards last cone
                break
            elif int(yy) > 180 and firstcone: #reach first cone and look for second
                secondcone = True
                ser.write('5')
            elif int(yy) > 180:
                ser.write('4')
                ballcollected = True
            elif int(xx) > 180:
                ser.write('2')
            elif int(xx) < 140:
                ser.write('3')
            else:
                ser.write('7')

            ballfound = False

        else:
            ser.write('1')
        if ballcollected:
            #change to color of first cone
            firstcone = True
            greenL = (30, 22, 80)
            greenH = (50, 200, 255)
            ballcollected = False






        #cv2.waitKey(500)
        key = cv2.waitKey(5) & 0xFF
        if key == ord("q"):
            break

        if secondcone:
            #change to color of second cone
            greenL = (30, 22, 80)
            greenH = (50, 200, 255)
            lastcone = True
            secondcone = False

    #ser.write('6')
cam.release()
cv2.destroyAllWindows()