import RPi.GPIO as GPIO
import time
from time import sleep
import cv2
import numpy as np
from PARAM import *
from motorm import turn
from ultdis import ultsound
#from LOCATE1 import locate
from FXGO2 import FXGO2
from LOCATE_goal import locate_g
from aimgoal import aimgoal

KSHOW=1

pin_servo = 26
GPIO.setmode(GPIO.BCM)
GPIO.setup(pin_servo, GPIO.OUT)
pwm_servo = GPIO.PWM(pin_servo, 50)  
pwm_servo.start(0)

pin1= 15
pin2= 14
GPIO.setup(pin1,GPIO.IN,pull_up_down=GPIO.PUD_UP)
GPIO.setup(pin2,GPIO.IN,pull_up_down=GPIO.PUD_UP)

trig_pin = 20
echo_pin = 21
GPIO.setup(trig_pin, GPIO.OUT)
GPIO.setup(echo_pin, GPIO.IN)


def destroy():
    pwm_servo.stop()
    GPIO.cleanup()


def setDirection(angle):
    duty = 2 + (angle / 18)
    pwm_servo.ChangeDutyCycle(duty)
    sleep(0.3) 
    pwm_servo.ChangeDutyCycle(0)

def distance1():
    GPIO.output(trig_pin, True)
    sleep(0.00001)
    GPIO.output(trig_pin, False)
    pulse_start = time.time()
    pulse_end = time.time()
    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
    sound_v = 34300 
    distance = pulse_duration*sound_v/2
    dis1=round(distance,1)
    return dis1

def moving(DR):
    if(DR==0 or DR==9): turn(0.0,0.0,0.05); turn(0,0,0.0)
    if(DR==1): turn(0.55,0.55,0.05); turn(0,0,0.02)
    if(DR==3): turn(-0.5,0.5,0.10); turn(0,0,0.1)
    if(DR==7): turn(0.5,-0.5,0.10); turn(0,0,0.1)
    
    
if(91==91):
    print('---軌道+轉動鏡頭+遇障停止+鏡頭發現目標,離開軌道追蹤目標---')
    cap=cv2.VideoCapture(0)
    cap.set(cv2.CAP_PROP_FRAME_WIDTH, Lx)
    cap.set(cv2.CAP_PROP_FRAME_HEIGHT, Ly)
    NR=0
    for j in range(80):
        ret, frame = cap.read()
        if(ret==False): break
        img2=np.copy(frame)
        NN,xs,ys,WDS,img24=locate_g(img2)
        cv2.imshow('img24',img24)
        if cv2.waitKey(1) & 0xFF == ord('q'): break
        if(j%15==0):
            NR+=1
            ang=NR%3*90; setDirection(ang); sleep(0.1)
        VP1=GPIO.input(pin1)
        VP2=GPIO.input(pin2)
        if(VP1==0 and VP2==0): DR=1
        if(VP1==0 and VP2==1): DR=1
        if(VP1==1 and VP2==0): DR=1
        if(VP1==1 and VP2==1): DR=1
        if(j%5==0):
            dis1 = distance1()
            if(dis1<20): print('dis1=',dis1); DR=9;
        moving(DR)
        print('j=',j,' DR=',DR,VP1,VP2,dis1)
    aimgoal(KSHOW,cap)
    cap.release()
    cv2.destroyAllWindows()
    input('IR,IR...')