import cv2
import numpy as np
from  motorm import turn
##################GET the color code on the bottom line########
def bottclr(img):
    img1=np.copy(img)
    img2=cv2.resize(img1,(80,60))
    hsv=cv2.cvtColor(img2,cv2.COLOR_BGR2HSV)
    Y1=59; Y2=40; Y3=20
    for x in range(80):
        print(x,img2[Y1,x,:],hsv[Y1,x,:],img2[Y2,x,:],hsv[Y2,x,:],
          img2[Y3,x,:],hsv[Y3,x,:])
    cv2.line(img2,(0,Y1-6),(159,Y1-6),(0,255,0),1)
    cv2.line(img2,(0,Y2-6),(159,Y2-6),(0,255,0),1)
    cv2.line(img2,(0,Y3-6),(159,Y3-6),(0,255,0),1)
    img2=cv2.resize(img2,(320,240))
    cv2.imshow('fcolor',img2)
    cv2.waitKey(0)


def tarbox(image):
    Lx=image.shape[1]; Ly=image.shape[0];
    Lxg=Lx; Lyg=Ly; Fg=1
    img2 = cv2.resize(image, (Lxg,Lyg), interpolation=cv2.INTER_AREA)
    img2=np.copy(image)
    gray = cv2.cvtColor(img3, cv2.COLOR_BGR2GRAY)
    cnts1,_ = cv2.findContours(gray.copy(), cv2.RETR_EXTERNAL, 
          cv2.CHAIN_APPROX_SIMPLE) # cv2.RETR_LIST
    lcnts1=len(cnts1); #print('lcnts1=',lcnts1)
    N1=0; ax=0; cx=0; px=0; mx1=0; Kc1=0; xx1=[0 for i in range(5)]
    clone1=img2.copy(); data1=[]
    for c in cnts1:
        M=cv2.moments(c)
        if(M['m00']==0): M['m00']=1
        cX=int(M['m10']/M['m00']); cY=int(M['m01']/M['m00'])
        area = cv2.contourArea(c); area3=round(area,3); #print(j,area3)
        area=int(area)
        if(area<1000): continue
        Kc1=1; N1+=1; (x,y,w,h)=cv2.boundingRect(c);
        xx1=(x,y,w,h); mx1=(cX,cY)
        d=[area,xx1,mx1]; data1.append(d)
        cv2.circle(clone1,mx1,6,(0,125,255),-1)
        x,y,w,h=xx1[0],xx1[1],xx1[2],xx1[3]
        cv2.rectangle(clone1,(x,y),(x+w,y+h),(255,125,0),1)
        #cv2.imshow('tarbox',clone1)
    lenda=len(data1)
    #print(lenda,data1)
    #cv2.waitKey(0)
    return lenda,data1,clone1



if __name__ == '__main__':
    from time import sleep
    print('fcolor.py as a main')
    low=np.array([105,120 ,120 ])
    upp=np.array([115,255 ,255 ])
    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()
    #bottclr(img)
    Ni=0
    for j in range(60):
        ret,imgi=cap.read()
        img=cv2.resize(imgi,(320,240))
        hsv=cv2.cvtColor(img,cv2.COLOR_BGR2HSV)
        mask1=cv2.inRange(hsv,low,upp)
        imgmask=cv2.bitwise_and(img,img,mask=mask1)
        img3=cv2.resize(imgmask,(640,480))
        lenda,data1,imgt=tarbox(img3)
        cv2.imshow('mask1',mask1)
        cv2.imshow('imgt',imgt)
        if(lenda>0):
            print(j,data1)
            for i in range(lenda):
                Ni+=1; sn=str(Ni).zfill(3); fn='NUM4_'+sn+'.png'
                (x,y,w,h)=data1[i][1]
                imgi=img3[y:y+h,x:x+w,:]
                imgir=cv2.resize(imgi,(100,130))
                imgis=cv2.resize(imgi,(200,260))
                #cv2.imshow('tar'+sn,imgis)
                cv2.imshow('tar',imgis)
                cv2.imwrite(fn,imgir)
            #cv2.waitKey(0)
        MJ=j%8
        if(MJ<2): turn(0.2,0.2,0.15); turn(0,0,1.5)
        elif(2<=MJ<4): turn(0.3,-0.0,0.10); turn(0,0,1.5)
        elif(4<=MJ<6): turn(0.2,0.2,0.10); turn(0,0,1.5)
        else: turn(-0.0,0.3,0.10); turn(0,0,1.5)
        if cv2.waitKey(1) & 0xFF == ord('q'): break
        print('j=',j)

    #main()