#--AI1.py--
import time
from PARAM3 import *  #攝影鏡頭擷取影片的參數
import cv2            #open cv是影像分析所使用的模組
import numpy as np    #矩陣運算的模組
from  motorm import turn  #控制馬達運轉的函數
from LOCREC import locate  #取出畫面中特定顏色(green)的位置和大小所形成的框架
from time import sleep     #sleep是讓程式的執行休息若干秒鐘
from gethsv import hsvcolor

def ptext(img1,s,pos,sz):
    img2=cv2.putText(img=img1,text=s,org=pos,
      fontFace = cv2.FONT_HERSHEY_DUPLEX,
      fontScale=sz,color=(125,246,55),thickness=1)
    return img2

#--------PICTURE------
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH,Lx)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT,Ly)
sleep(2)  #wait 2 s for camera warm-up
print('Lx,Ly,Lx2,Ly2=',Lx,Ly,Lx2,Ly2)
for t in range(30):
    ret,img=cap.read();
    h,img1=hsvcolor(img)
    a=str(h)
    fn='green_hsv_'+str(t).zfill(2)+'.png'
    img2=ptext(img1,a,(50,50),1.0)
    cv2.imshow('img2',img2)
    cv2.imwrite(fn,img2)
    print(t,h)
    #if cv2.waitKey(1000) & 0xFF == ord('q'): break
    cv2.waitKey(0)
input('get the color of card is done...')
print('get the color of card is done...')


if(color=='blue'):
    #-------blue----------------------
    lower1=np.array([105, 100, 100])        # blue
    upper1=np.array([125, 255, 255])      # blue
if(color=='green'):
    #-------green----------------------
    lower1=np.array([50, 30, 90])        # blue-green
    upper1=np.array([80, 255, 255])      # blue-green
if(color=='red'):
    #-------red----------------------
    lower_red1=np.array([0, 150, 120])
    upper_red1=np.array([10, 255, 255])
    lower_red2=np.array([160, 150, 120])
    upper_red2=np.array([180, 255, 255])


color='green'
lower1=np.array([h[0]-5, h[1]-50, h[2]-50])        # blue-green
upper1=np.array([h[0]+5, 255, 255])      # blue-green
print('lower1=',lower1)
input('Is the color hsv OK? ctl-c to stop; or Hit the enter to run ...')

#############################################
def PLATS(img):
    imgin=np.copy(img); 
    img1=cv2.resize(imgin,(Lx2,Ly2))
    Ac=Lx2*Ly2*0.01
    hsv=cv2.cvtColor(img1, cv2.COLOR_BGR2HSV)
    if(color == 'red'): 
        mask1=cv2.inRange(hsv,lower_red1,upper_red1)
        mask2=cv2.inRange(hsv,lower_red2,upper_red2)
        mask=mask1 | mask2
    else:
        mask1=cv2.inRange(hsv,lower1,upper1)
        mask=mask1
    cnts,_=cv2.findContours(np.copy(mask),cv2.RETR_LIST,
          cv2.CHAIN_APPROX_SIMPLE)
    cs=[cv2.contourArea(c) for c in cnts]
    print('lenc=',len(cnts),'cs=',cs)
    masked=cv2.bitwise_and(img1, img1, mask=mask)
    hst1=np.vstack((img1,masked))
    #cv2.imshow('masked',masked); cv2.waitKey(0)
    #if(len(cs)==0): return 0,(0,0),0,masked
    C=max(cnts,key=cv2.contourArea)  #max()所有輪廓中取最大的
    NC=0; BOX=[]
    for C in cnts:
        (x,y,w,h)=cv2.boundingRect(C);
        A=w*h; cx=int(x+w/2); cy=int(y+h/2); rat=w/h
        if(A<Ac): continue
        if(rat>1.2 or rat<0.1): continue
        NC+=1
        cv2.rectangle(masked,(x,y),(x+w,y+h),(200,200,200),2)
        cv2.circle(masked,(cx,cy),5,(255,255,255),-1)
        #if(cx<masked.shape[1]*0.40): DR=3
        #elif(cx>masked.shape[1]*0.60): DR=7
        #else: DR=1
        BOX.append([(x,y,w,h),(cx,cy)])
        print(NC,(x,y,w,h),(cx,cy),A,Ac)
    cv2.imshow('hst1',hst1); cv2.waitKey(0)
    #return DR,(cx,cy),A,masked

#############################################
def PLATX(img):
    imgin=np.copy(img); 
    img1=cv2.resize(imgin,(Lx2,Ly2))
    Ac=Lx2*Ly2*0.01
    hsv=cv2.cvtColor(img1, cv2.COLOR_BGR2HSV)
    if(color == 'red'): 
        mask1=cv2.inRange(hsv,lower_red1,upper_red1)
        mask2=cv2.inRange(hsv,lower_red2,upper_red2)
        mask=mask1 | mask2
    else:
        mask1=cv2.inRange(hsv,lower1,upper1)
        mask=mask1
    masked=cv2.bitwise_and(img1, img1, mask=mask)
    hst1=np.vstack((img1,masked))
    cnts,_=cv2.findContours(np.copy(mask),cv2.RETR_LIST,
          cv2.CHAIN_APPROX_SIMPLE)
    cs=[cv2.contourArea(c) for c in cnts]
    #print('lenc=',len(cnts),'cs=',cs)
    if(len(cnts)==0): return (0,0,0,0),(0,0),masked,hst1
    C=max(cnts,key=cv2.contourArea)  #max()所有輪廓中取最大的
    (x,y,w,h)=cv2.boundingRect(C);
    A=w*h; cx=int(x+w/2); cy=int(y+h/2); rat=w/h
    if(A<Ac): return (0,0,0,0),(0,0),masked,hst1
    cv2.rectangle(masked,(x,y),(x+w,y+h),(200,200,200),2)
    cv2.circle(masked,(cx,cy),5,(255,255,255),-1)
    print((x,y,w,h),(cx,cy),A,Ac)
    hst1=np.vstack((img1,masked))
    #cv2.imshow('hst1',hst1); cv2.waitKey(0)
    return (x,y,w,h),(cx,cy),masked,hst1





jt=0; NP=0;  
while(jt < 60 and cap.isOpened()):
    sleep(1)
    ret,img=cap.read(); jt+=1; 
    (x,y,w,h),(cx,cy),masked,hst1=PLATX(img)
    x1=x; x2=x+w; y1=y; y2=y+h
    x1=x1*F; y1=y1*F; x2=x2*F; y2=y2*F
    img3=img[y1:y2,x1:x2,:]
    img3=cv2.cvtColor(img3, cv2.COLOR_BGR2GRAY)
    cv2.imshow('img3',img3)
    cv2.imshow('hst1',hst1); cv2.waitKey(0)
    fn='PNG/A1_'+str(NP).zfill(3)+'.png'
    cv2.imwrite(fn,img3)
    NP+=1
    continue

    frame= cv2.resize(frame, (Lx,Ly), interpolation=cv2.INTER_AREA)
    imgc= cv2.resize(frame, (Lx2*2,Ly2*2), interpolation=cv2.INTER_AREA)
    if(ret==False): break
    img=np.copy(frame)
    img2=np.copy(img)
    NN,BOX,img24=locate(color,img2)
    print(jt,NN,BOX)
    if(NN==0): WDSX=0; print('jt=',jt,'null NN=',NN,BOX); 
    for t in range(NN):
        NP+=1
        x1=BOX[t][0]*F; x2=BOX[t][1]*F; y1=BOX[t][2]*F; y2=BOX[t][3]*F;
        cv2.rectangle(img2,(x1,y1),(x2,y2),(5,0,255),6)
        cv2.rectangle(img24,(x1,y1),(x2,y2),(0,120,0),2)
        img3=img[y1:y2,x1:x2,:]
        img3=cv2.cvtColor(img3, cv2.COLOR_BGR2GRAY)
        fn='PNG/D1_'+str(NP).zfill(3)+'.png'
        cv2.imwrite(fn,img3)
        #cv2.imshow('img3NN'+str(t),img3)
    DR=1
    imgb=cv2.resize(img2, (Lx2*2,Ly2*2))
    print('jt=',jt,' NN=',NN,BOX,img3.shape)
    cv2.imshow('imgb',imgb)
    if cv2.waitKey(1) & 0xFF == ord('q'): break
    #turn(0.2,0.2,0.1); turn(0,0,0.5) #自駕車前進一些準備取得新的相片
    #cv2.waitKey(0)
    #if(jt%10==0): sleep(4)
cv2.destroyAllWindows()