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 FXGO2 import FXGO2
from LOCATE_goal import locate_g
from aimgoal import aimgoal

FL=open('/home/pi/P1-RED/CAR.txt','r')
car=FL.read()
car=int(car)
print('car=',car,type(car))


#---------------SERVO pin---------
pin_servo = 26
GPIO.setmode(GPIO.BCM)
GPIO.setup(pin_servo, GPIO.OUT)
pwm_servo = GPIO.PWM(pin_servo, 50)  # 50Hz frequency
pwm_servo.start(0)

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

#---------------ultra-sound pins--
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 #cm/s
    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.25,0.25,0.05); turn(0,0,0.0)
    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)
    







#----------------LOOP-1-------------
cap=cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, Lx)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, Ly)
setDirection(90)
print('LOOP-1, set initial direction as forward..')
jt=0; angle=0; dang=int(180/2); NF=5; ESC=0; NSTEP=12
while(car==3 and jt<NSTEP):
    jt+=1
    if(jt%NF==0):
        k=int(jt/NF)%3
        angle=k*90
        print('jt,k,angle=',jt,k,angle)
        setDirection(angle)
        sleep(0.1)
        ang=angle
        angle+=dang
    #sleep(0.5)
    #continue
    ret, frame = cap.read()
    if(ret==False): break
    #cv2.imshow('frame',frame)
    img2=np.copy(frame)
    NN,xs,ys,WDS,img24=locate_g(img2)
    #cv2.imshow('img24',img24)
    if cv2.waitKey(1) & 0xFF == ord('q'): break
    #continue
    if(NN>0): 
        ESC=1
        if(ang==90): 
            print('NN=',NN,'ang=90')
            sleep(1)
        else:
            print('ang,angle=',ang,angle)
            sleep(2)
            if(int(ang)==0): 
                print('a0,GOT it in side view! jt=',jt,NN,xs,ys,WDS); 
                setDirection(90); sleep(1)
                turn(0.5,-0.5,0.4);turn(0,0,0)
            if(int(ang)==180): 
                print('a180,GOT it in side view! jt=',jt,NN,xs,ys,WDS); 
                setDirection(90); sleep(1)
                turn(-0.5,0.5,0.5);turn(0,0,0)
            #turn(0.3,0.3,0.4); turn(-0.3,-0.3,0.5); turn(0,0,0.1)
            #sleep(1)
    if(ESC==1): break
    VP1=GPIO.input(pin1)
    VP2=GPIO.input(pin2)
    if(VP1==0 and VP2==0): DR=1
    if(VP1==0 and VP2==1): DR=7
    if(VP1==1 and VP2==0): DR=3
    if(VP1==1 and VP2==1): DR=9
    print(jt,VP1,VP2,' DR=',DR)
    VP1=GPIO.input(pin1)
    VP2=GPIO.input(pin2)
    print(jt)
    moving(DR)
    sleep(0.05)
FXGO2(cap)
input('iiiii-111111')
cap.release()
cv2.destroyAllWindows()