import serial,timeit,time
import math
radar = serial.Serial('/dev/tty.usbmodem144301', 19200, timeout=1)
print("conncted to " + radar.portstr)


arduino = serial.Serial('/dev/tty.usbmodem144101', 19200, timeout=1)
print("conncted to " + arduino.portstr)

cur_speed, cur_distance=0,0


dist_max = 100
dist_min = 2
speed_max = 31
speed_min = 0.5


speed_deteced=timeit.default_timer()


while True:
    radar_input_raw = radar.readline()
    print(radar_input_raw)
    radar_input_decoded=radar_input_raw.decode("utf-8")
    print(radar_input_decoded)

    state, value = radar_input_decoded.split(',')

    state = state.strip('\"')
    tmp = value
    value = value.rstrip()
    print("input",state,value)
    print("current",cur_speed,cur_distance)

    # dist_range = 2 - 100m
    # speed_range = 4 - 31 mps

    v = float(value)
    if state == 'mps':      # velocity
        if v>=0 and v<= speed_max:
            cur_speed = v
            speed_deteced=timeit.default_timer()
            flag=1


    elif state == 'm':
        if v>=dist_min and v<=dist_max:
            cur_distance = v
            flag=0

    intensity = ((dist_max - float(cur_distance))/1000+float(cur_speed)/31)/2


    # if no speed update for 2 sec, reset to 0
    tt=timeit.default_timer()
    #print(tt)
    t=(tt-speed_deteced)
    if t >= 1:
        cur_speed=0


    print("intensity----------------------",intensity,)

    threshold=0.07
    if intensity>= threshold:
        arduino.write(bytes('r','utf-8'))
        time.sleep(0.5)
        print('trigger')