#-- AI4.py

import cv2
import numpy as np
import time
from time import sleep
from motorm import turn

def cw(k): cv2.waitKey(k)
def sh0(w,img): cv2.imshow(w,img)
def cntr(img):
    cntsT,_=cv2.findContours(img,cv2.RETR_LIST,cv2.CHAIN_APPROX_SIMPLE)
    return cntsT

##############################################################
def takepic():    
    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
    for k in LAB:
        #if(k=='T' or k=='H'): continue
        jt=0; NP=0;  
        while(jt < 80 and cap.isOpened()):
            ret,img=cap.read(); jt+=1; 
            fn='PNG/'+k+'1_'+str(NP).zfill(3)+'.png'  
            (x,y,w,h),(cx,cy),masked,hst1=CNTX(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)
            sh0('hst1',hst1); #cw(0)
            cv2.imwrite(fn,img3)
            km=jt%5; NP+=1; print(jt,NP,km,fn,(x,y,w,h)); 
            if cv2.waitKey(10) & 0xFF == ord('q'): break
            if(km==0): turn(0.3,0.3,0.07); turn(0,0,0.3) 
            if(km==1): turn(0.3,-0.3,0.07); turn(0,0,0.5) 
            if(km==2 or km==3): turn(-0.3,0.3,0.07); turn(0,0,0.5) 
            if(km==4): turn(0.3,-0.3,0.07); turn(0,0,0.5) 
        input('change card...')
    cv2.destroyAllWindows()

###############################################################
def CNTX(img):
    imgin=np.copy(img); 
    img1=cv2.resize(imgin,(Lx2,Ly2))
    Ac=Lx2*Ly2*0.0001
    hsv=cv2.cvtColor(img1, cv2.COLOR_BGR2HSV)
    mask1=cv2.inRange(hsv,lower1,upper1)
    masked=cv2.bitwise_and(img1, img1, mask=mask1)
    hst1=np.vstack((img1,masked))
    cnts=cntr(mask1)
    if(len(cnts)==0): return (0,0,0,0),(0,0),masked,hst1
    C=max(cnts,key=cv2.contourArea)  #max() max contours
    (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)
    hst1=np.vstack((img1,masked))
    return (x,y,w,h),(cx,cy),masked,hst1

#------------------------------------- main ----------------
Lx=800; Ly=600; F=2; Lx2=Lx//F; Ly2=Ly//F;
color='green'; h=60
lower1=np.array([50, 30, 90])        # green
upper1=np.array([80, 255, 255])      # green
LAB=['2','4','6','8']; 
LABEL=['two','four','six','eight']
takepic(); 
cv2.destroyAllWindows()