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


##################GET the color code on the bottom line########
def botclr(img):
    img1=np.copy(img)
    img2=cv2.resize(img1,(160,120))
    hsv=cv2.cvtColor(img2,cv2.COLOR_BGR2HSV)
    Y=119; 
    for x in range(160):
        print(x,img2[Y,x,:],hsv[Y,x,:])
    cv2.line(img2,(0,Y-6),(159,Y-6),(0,255,0),1)
    cv2.imshow('botclr',img2)
    cv2.waitKey(0)


#-------------road or floor color-------
low_rd=np.array([0,  0,   50])
upp_rd=np.array([255,20,  255])
#-------------red target @ 180 ---------
low1=np.array([170, 100,   100])
upp1=np.array([180,255,  255])
low2=np.array([0, 100,   100])
upp2=np.array([10,255,  255])
MSK=2


def targ(MSK,low1,upp1,low2,upp2,img):
    Y=img.shape[0]-1;
    img1=np.copy(img)
    hsv=cv2.cvtColor(img1,cv2.COLOR_BGR2HSV)
    mask1=cv2.inRange(hsv,low1,upp1)
    if(MSK==2):
        mask3=cv2.inRange(hsv,low2,upp2)
        mask2=mask1 | mask3
    else:
        mask2=np.copy(mask1)
    img2=cv2.bitwise_and(img1, img1, mask=mask2)
    thresh=np.copy(mask2)
    contours,hierarchy=cv2.findContours(thresh, cv2.RETR_EXTERNAL,
                cv2.CHAIN_APPROX_NONE)
    #print('lencont=',len(contours))
    #for t in contours: print(cv2.contourArea(t))
    img3=np.copy(img2)
    box=[0,0,0,0]; lencontr=len(contours)
    if(lencontr != 0):
        cx=max(contours,key=cv2.contourArea);cax=cv2.contourArea(cx);
        nc=0; boxes=[]
        for c in contours:
            ca=cv2.contourArea(c)
            if(ca<60): continue
            nc+=1; cv2.drawContours(img2,c,-1,(0,255,0),2)
            x,y,w,h = cv2.boundingRect(c)
            img3=cv2.rectangle(img3,(x,y),(x+w,y+h),(255,0,0),2)
            A=w*h; box=[x,y,w,h,ca,A]; boxes.append(box)
            print(nc,box)
        print('lencontr=',lencontr,'max_c_area=',cax)
    else: print('lencontour=0')
    h2=np.hstack((img1,img2,img3))
    #cv2.imshow('h2',h2)
    #cv2.imwrite('targ_red.png',h2)
    #cv2.waitKey(0)
    return boxes,h2


#********************** MAIN ************************
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH,640)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT,480)
sleep(2)
ret,img=cap.read()

if(1==1):
    for j in range(10):
        ret,img=cap.read()
        boxes,h2=targs(MSK,low1,upp1,low2,upp2,img)
        print(j,len(boxes),boxes)
        cv2.imshow('h2',h2)
        cv2.imwrite('targ_red_'+str(j)+'.png',h2)
        cv2.waitKey(0)

if(1==2):
    #botclr(img)
    for j in range(10):
        ret,img=cap.read()
        floor(img)