# C2.py
import cv2
import numpy as np
#from gethsv import hsvcolor
from  motorm import turn
from time import sleep
from picamera2 import Picamera2



#----------------------------------------------
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]-10, h[1]-40, 50])
    uppT=np.array([h[0]+10, h[1]+40, 255])
    #print('h=',h,' lowT=',lowT,uppT)
    return h,lowT,uppT
#--------------------------------------------------------
#sleep(5)




#curve1()
#input('000')


#------- 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)

print('run picam2...')
picam2 = Picamera2()
picam2.configure(picam2.create_preview_configuration(main={"format": 'RGB888', "size": (Lx,Ly)}))
picam2.start()
for j in range(200):
    frame=picam2.capture_array()
    cv2.imshow("Camera Preview", frame)
    if cv2.waitKey(1) & 0xFF == ord('q'): break
cv2.destroyAllWindows()
#input('picam2 preview done...')
print('picam2 preview done...')

#ret,frame=cap.read(); cw(2000);
frame=picam2.capture_array(); cw(2000)
h,lowT,uppT=setlowup(); print('h,low,up=',h,lowT,uppT)
print('Lx...=',(Lx,Ly),XL,XR,XC,YC,R)
t=0
while(t<2501):
    t+=1; 
    #ret,frame=cap.read(); imgb=np.copy(frame);
    #if ret == False: break
    frame=picam2.capture_array(); imgb=np.copy(frame);
    #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)==0): (cx,cy,x,y,w,h)=(-100,-100,0,0,0,0); DR=0
    else: 
        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); 
        if(w<5 or h<5): DR=0
        else:
            if(XL<=cx<=XR): DR=1
            elif(cx<XL): DR=3
            elif(cx>XR): DR=7
    sh0('imgb',imgb); cw(0)
    if(cv2.waitKey(2) & 0xFF == ord('q')): break
    #continue

    print(t,(cx,cy,w,h),DR,XL,XR)
    if(DR==0): turn(-0.3,-0.3,0.05); turn(0,0,0.1);
    if(DR==1): turn(0.3,0.3,0.10); turn(0,0,0.3);
    if(DR==3): turn(-0.3,0.3,0.10); turn(0,0,0.3);
    if(DR==7): turn(0.3,-0.3,0.10); turn(0,0,0.3);
    if(DR != 1): print('*****',t,cx,DR,XL,XR)
    if(cv2.waitKey(1) & 0xFF == ord('q')): break
cap.release()
cv2.destroyAllWindows()
picam2.close()