import cv2
import numpy as np
import time
from time import sleep
import RPi.GPIO as GPIO

def distance1():
    GPIO.output(trig_pin,True)
    sleep(0.0001)
    GPIO.output(trig_pin,False)
    pulse_start = time.time()
    pulse_end = time.time()
    while GPIO.input(echo_pin) == 0: pulse_start=round(time.time(),3)
    while GPIO.input(echo_pin) == 1: pulse_end=round(time.time(),3)
    pulse_duration = pulse_end - pulse_start
    sound_v = 34300 #cm/s
    distance = round(pulse_duration*sound_v/2,1)
    print('pulse:',pulse_start,pulse_end,pulse_duration,distance)
    return distance

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 measure_distance():
    GPIO.output(TRIG_PIN, True)
    time.sleep(0.00005)  
    GPIO.output(TRIG_PIN, False)
    while GPIO.input(ECHO_PIN) == 0:
        pulse_start = time.time()
    while GPIO.input(ECHO_PIN) == 1:
        pulse_end = time.time()
    pulse_duration = pulse_end - pulse_start
    distance = pulse_duration * 34300 / 2
    return round(distance, 2)

#######################################################
def CNTX(img):
    imgin=np.copy(img); 
    img1=np.copy(imgin)
    Ac=Lx*Ly*0.0001
    hsv=cv2.cvtColor(img1, cv2.COLOR_BGR2HSV)
    mask1=cv2.inRange(hsv,lower1,upper1)
    masked=cv2.bitwise_and(img1, img1, mask=mask1)
    img2=cv2.resize(img1,(rx,ry)); mask2=cv2.resize(masked,(rx,ry)); 
    hst1=np.vstack((img2,mask2))
    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)
    img2=cv2.resize(img1,(rx,ry)); mask2=cv2.resize(masked,(rx,ry)); 
    hst1=np.vstack((img2,mask2))
    return (x,y,w,h),(cx,cy),masked,hst1

#------------------------------------- main ------------

GPIO.setmode(GPIO.BCM)
TRIG_PIN = 26
ECHO_PIN = 19
GPIO.setup(TRIG_PIN, GPIO.OUT)
GPIO.setup(ECHO_PIN, GPIO.IN)
Lx=1920; Ly=1080; rx=Lx//3; ry=Ly//3
color='green'; h=60
lower1=np.array([40, 10, 10])        # green
upper1=np.array([80, 255, 255])      # green
if(1==1):
    cap = cv2.VideoCapture(0)
    cap.set(cv2.CAP_PROP_FRAME_WIDTH,Lx)
    cap.set(cv2.CAP_PROP_FRAME_HEIGHT,Ly)
    sleep(2)  
    jt=0; NP=0;  
    t0=time.time()
    for t in range(1000):
        t1=time.time(); dt=round(t1-t0,2)
        dist = measure_distance()
        time.sleep(1)  
        ret,img=cap.read(); jt+=1; 
        (x,y,w,h),(cx,cy),masked,hst1=CNTX(img) 
        Dcam=round(132/w*60,1)
        x1=x; x2=x+w; y1=y; y2=y+h
        NP+=1; print(t,'dist,Dcam=',dist,Dcam,(cx,cy,x,y,w,h)); 
        sh0('hst1',hst1); cw(10)
    cv2.destroyAllWindows()
    GPIO.cleanup()
cv2.destroyAllWindows()