import cv2
from datetime import datetime
import math
import numpy as np
import time

WIDTH = 800
HEIGHT = 600
cap = cv2.VideoCapture(0) 
cap.set(cv2.CAP_PROP_FRAME_WIDTH, WIDTH)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, HEIGHT)
#If there is no camera connected, exit
if not cap.isOpened():
    print("Camera Not Found")
    sys.exit()

stagenumber = 0

#Capture and circle detection
while True:
#    sleep(1)
    #Capture
    _, frame = cap.read()
    img = frame.copy()
    # cut the sample for field size
    # img[top : bottom, left : right]
    img1 = img[300 : 600, 140: 620]
    #cv2.imwrite("out_sample1.jpg", img1)
    
    #
    today = datetime.today()
    if today.second % 2 ==0:
        twosecloop = 0
    else:
        twosecloop = 1
    #Inport background
    if stagenumber == 1:#Pokemon
        stage1=cv2.imread("stage_p.png")
    elif stagenumber == 2:#water
        if today.microsecond < 200000:
            stage1=cv2.imread("stage_water01.png")
        elif today.microsecond < 400000:
            stage1=cv2.imread("stage_water02.png")
        elif today.microsecond < 600000:
            stage1=cv2.imread("stage_water03.png")
        elif today.microsecond < 800000:
            stage1=cv2.imread("stage_water04.png")
        else:
            stage1=cv2.imread("stage_water05.png")
    elif stagenumber == 3:
        stage1=cv2.imread("stage_c.png")
    elif stagenumber == 4:
        stage1=cv2.imread("stage_w.png") 
    elif stagenumber == 5:
        stage1= img1
    else:
        stage1= cv2.imread("stage_b.png")
    stage = stage1

    
    gray = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY) #conver the picture to gray
    #gray = cv2.medianBlur(gray ,7)
    #gray = cv2.equalizeHist(gray)
    
    
    #Circle detection
    circles = cv2.HoughCircles(gray , #i8-bit, single-channel, grayscale input image
                               cv2.HOUGH_GRADIENT, #detection method
                               dp=7, #inverse ratio of the accumulator resolution
                               minDist=50, #minimum distance between the centers of the detected circles
                               param1=240, #first method-specific parameter
                               param2=60, #second method-specific parameter
                               minRadius=6, #minimum circle radius
                               maxRadius=15) #maximum circle radius

    #Draw effect to the field
    if circles is not None:
        if stagenumber == 1:
            for i in circles[0].astype('uint16'):
                timeloop=(twosecloop+today.microsecond)/2000000 #loop from 0 to 1, every 2 sec
                for n in range(6):
                    petalx=int((i[2]+40)*math.cos(6.28/6*(n+timeloop*2)))
                    petaly=int((i[2]+40)*math.sin(6.28/6*(n+timeloop*2)))
                    cv2.circle(stage,(i[0]+petalx,i[1]+petaly),10,(255,255,255),-1)
                    cv2.ellipse(stage,(i[0]+petalx,i[1]+petaly),(10,10),0,180,360,(0,0,255),-1)
                    cv2.circle(stage,(i[0]+petalx,i[1]+petaly),10,(0,0,0),1,cv2.LINE_AA)
                    cv2.circle(stage,(i[0]+petalx,i[1]+petaly),4,(255,255,255),-1)
                    cv2.circle(stage,(i[0]+petalx,i[1]+petaly),4,(0,0,0),1,cv2.LINE_AA)
                    
        elif stagenumber == 2:#wave
            for i in circles[0].astype('uint16'):
                timeloop=today.microsecond/1000000 #loop from 0 to 1, every 1 sec
                rad=int(15*timeloop)
                col_b=int(255)
                col_g=int(250)
                col_r=int(120)
                #col_b2=int(col_b*(1-timeloop))
                #col_g2=int(col_g*(1-timeloop))
                #col_r2=int(col_r*(1-timeloop))
                cv2.circle(stage,(i[0],i[1]),15+rad,(col_b,col_g,col_r),int(6*timeloop),cv2.LINE_AA)
                cv2.circle(stage,(i[0],i[1]),30+rad,(col_b,col_g,col_r),6,cv2.LINE_AA)
                cv2.circle(stage,(i[0],i[1]),45+rad,(col_b,col_g,col_r),int(6*(1-timeloop)),cv2.LINE_AA)


                    
        else:
            for i in circles[0].astype('uint16'):
                timeloop=today.microsecond/1000000 #loop from 0 to 1, every 1 sec
                rad=int(30*timeloop) 
                rads=int(15*timeloop) 
                col_b=int(255)
                col_g=int(250)
                col_r=int(120)
                if timeloop < 0.8:
                    col_b=int(255)
                    col_g=int(250)
                    col_r=int(120)
                else:
                    col_b=int(col_b-(col_b)*(timeloop-0.8)*5)
                    col_g=int(col_g-(col_g)*(timeloop-0.8)*5)
                    col_r=int(col_r-(col_r)*(timeloop-0.8)*5)
                #col_b=int(255*(1-timeloop))
                #col_g=int(250*(1-timeloop))
                #col_r=int(120*(1-timeloop))
                cv2.circle(stage,(i[0],i[1]),i[2]+rad,(col_b,col_g,col_r),6)
                cv2.circle(stage,(i[0],i[1]),i[2]+rad*2,(col_b,col_g,col_r),6)
                for n in range(9):
                    petalx=int((i[2]+10+rad*1.5)*math.cos(6.28/9*(n+timeloop*2)))
                    petaly=int((i[2]+10+rad*1.5)*math.sin(6.28/9*(n+timeloop*2)))                
                    cv2.circle(stage,(i[0]+petalx,i[1]+petaly),rads,(col_b,col_g,col_r),-1)

    cv2.imshow('screen', stage1)

    # waiting keyboard input
    k = cv2.waitKey(2) 
    # save the picture
    if k == ord("s"):
        cv2.imwrite("sample.png", stage)
    
    #Background selection
    if k == ord("0") or k == 158:
        stagenumber = 0
    elif k == ord("1") or k == 156:
        print (chr(k))
        stagenumber = 1
    elif k == ord("2") or k == 153:
        stagenumber = 2
    elif k == ord("3") or k == 155:
        stagenumber = 3
    elif k == ord("4") or k == 150:
        stagenumber = 4
    elif k == ord("5") or k == 157:
        stagenumber = 5
        
    # Exit at Esc
    if k == 27: #27=Esc
        break
    
    #print (k)
    
    
cv2.destroyAllWindows() #close all window
cap.release()