# C2.py
import cv2
import numpy as np
#from gethsv import hsvcolor
from  motorm import turn
#----------------------------------------------
def cntr(img):
    cntsT,_=cv2.findContours(img,cv2.RETR_LIST,cv2.CHAIN_APPROX_SIMPLE)
    return cntsT
#----------------------------------------------
def curve1():
    ts=0.1
    turn( 0.6, 0.0,0.40);    turn(0,0,ts);
    turn( 0.2, 0.9,1.20);    turn(0,0,ts);
    turn( 0.4,-0.4,0.35);    turn(0,0,ts);
#----------------------------------------------
def sh0(win,img): cv2.imshow(win,img);
def cw(k): cv2.waitKey(k)
#----------------------------------------------------------
def hsvcolor(img,XC,YC,R):
    Lx=img.shape[1]; Ly=img.shape[0]; #XC=int(Lx/2); YC=int(Ly/2); R=Lx//40;
    img1=np.copy(img)
    x1=XC-R; x2=XC+R; y1=YC-R; y2=YC+R
    img2=img[y1:y2,x1:x2,:]
    cv2.rectangle(img1,(x1,y1),(x2,y2),(255,255,255),4)
    h=[0,0,0]
    hsv=cv2.cvtColor(img2, cv2.COLOR_BGR2HSV)
    for i in range(3): h[i]=int(np.average(hsv[:,:,i]))
    return h,img1
#--------------------------------------------------------
def setlowup():
    if(1==2):
        for j in range(6):
            ret,frame=cap.read()
            h,img1=hsvcolor(frame,XC,YC,R)
            print(j,h)
            cv2.imshow('img1',img1); cv2.waitKey(0)
    else:
        h=np.array([56, 121, 149])
    lowT=np.array([h[0]-15, 50, 50])
    uppT=np.array([h[0]+15, 255, 255])
    #print('h=',h,' lowT=',lowT,uppT)
    return h,lowT,uppT
#--------------------------------------------------------


def curve():
    turn(0.6,0.0,0.40); turn(0,0,0.3);
    turn(0.3,0.3,0.40); turn(0,0,0.3);
    turn(0.0,0.6,0.40); turn(0,0,0.3);
    turn(0.3,0.3,0.80); turn(0,0,0.3);
    turn(0.0,0.6,0.40); turn(0,0,0.3);
    turn(0.3,0.3,0.40); turn(0,0,0.3);
    turn(0.3,-0.3,0.30); turn(0,0,0.3);

#curve()
#input('111')


#------- main -------------------
Lx=640; Ly=480; F=8; Lx2=int(Lx/F); Ly2=int(Ly/F); 
XL=Lx//5*2; XR=Lx//5*3; XC=int(Lx*0.5); YC=int(Ly*0.5); R=Lx//40;
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, Lx)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, Ly)
ret,frame=cap.read(); cw(2000);
h,lowT,uppT=setlowup(); print('h,low,up=',h,lowT,uppT)
print('Lx...=',(Lx,Ly),XL,XR,XC,YC,R)
t=0; NC=0
while(t<2501 and cap.isOpened()):
    t+=1; ret,frame=cap.read(); imgb=np.copy(frame);
    if ret == False: break
    img2=cv2.resize(frame,(Lx2,Ly2)); sh0('img2',img2)
    #continue
    #.........................................
    hsv=cv2.cvtColor(frame,cv2.COLOR_BGR2HSV);
    mask1=cv2.inRange(hsv,lowT,uppT)
    #sh0('mask1',mask1); #cw(0);
    imgc=cv2.bitwise_and(imgb,imgb,mask=mask1)
    maskc=cv2.cvtColor(mask1,cv2.COLOR_GRAY2BGR)
    #.........................................
    if(cv2.waitKey(1) & 0xFF == ord('q')): break
    cnt=cntr(mask1);
    if(len(cnt)<1): continue
    C=max(cnt,key=cv2.contourArea)
    x,y,w,h=cv2.boundingRect(C); cx=x+w//2; cy=y+h//2
    cv2.rectangle(imgb,(x,y),(x+w,y+h),(0,255,0),4)
    hst=np.hstack((frame,maskc,imgb))
    hst2=cv2.resize(hst,(int(Lx*3*0.8),int(Ly*0.8))); #sh0('hst2',hst2); 
    sh0('imgb',imgb); #cw(0)
    if(cv2.waitKey(2) & 0xFF == ord('q')): break
    if(XL<=cx<=XR): DR=1
    elif(cx<XL): DR=3
    elif(cx>XR): DR=7
    print(t,(cx,cy),'w=',w,'h=',h,DR,XL,XR)
    if(w>150):
        if(NC==0): NC+=1; curve1()
        else: turn(0,0,2)
    if(DR==1): turn(0.3,0.3,0.05); turn(0,0,0.3);
    if(DR==3): turn(-0.3,0.3,0.05); turn(0,0,0.3);
    if(DR==7): turn(0.3,-0.3,0.05); turn(0,0,0.3);
    if(DR != 1): print('*****',t,cx,DR,XL,XR)
    #cv2.waitKey(0)
    if(cv2.waitKey(1) & 0xFF == ord('q')): break
cap.release()
cv2.destroyAllWindows()