import board
import digitalio
import simpleio
import time
import pwmio
from adafruit_motor import servo, motor
import busio
import adafruit_vl53l0x

# Initialize DC motors
m1a = pwmio.PWMOut(board.GP8, frequency=50)
m1b = pwmio.PWMOut(board.GP9, frequency=50)
motor1 = motor.DCMotor(m1a, m1b)
m2a = pwmio.PWMOut(board.GP10, frequency=50)
m2b = pwmio.PWMOut(board.GP11, frequency=50)
motor2 = motor.DCMotor(m2a, m2b)

#Initialize UART0(Bluetooth)
uart0 = busio.UART(tx=board.GP16, rx=board.GP17, baudrate=9600)

#Initialize Sensor
i2c = busio.I2C(scl=board.GP3, sda=board.GP2)
sensor = adafruit_vl53l0x.VL53L0X(i2c)


#モーターどっちも止める
motor1.throttle = 0
motor2.throttle = 0

#起動したことを知らせるためにBluetoothでなんかしゃべる
txData = b'hello world\n\r'
uart0.write(txData)


#Joystick用関数
#Initialize UART1(ESP-WROOM-02)
uart1 = busio.UART(tx=board.GP4, rx=board.GP5, baudrate=9600)
#WiFiモジュール通信用関数
def get_joystick():
    uart1.write(b'c')
    data = uart1.read(3)
    try:
        if data[0] == 99:#99 = c
            x = int.from_bytes(data[1:2],'big')
            if x > 127:
                x = (256-x) * (-1)
            y = int.from_bytes(data[2:3],'big')
            if y > 127:
                y = (256-y) * (-1)
            return x,y
        else:
            while(1):
                if uart1.read() == None:
                    break
            return 0,0
    except:
        return 0,0
#ESP-WROOM-02起動時に出るゴミデータを消去
while(1):
    if uart1.read() == None:
        break
#Joystick用関数 終わり

#txData = b'Range: {}mm'.format(sensor.range)

#print(b'Range: {}mm'.format(sensor.range))

#ポーリング用の変数初期化(前回実行した時間を格納
now = 0#センサー用
joytime = 0#ジョイスティック用


#以下無限ループ
while True:
    #モーター制御値ポーリング
    if time.monotonic() > (serialtime + 0.2):#ポーリング時間設定
        joytime = time.monotonic()#今の時間をポーリング用の変数に記録して次回実行の判断に使う。
        joy = get_joystick()#joyに最新のJoystick状態を取得(xとyの２つの値が来る)
        #print("X:" + str(joy[0]))#joystickのX座標(-100~100)
        #print("Y:" + str(joy[1]))#joystickのY座標(-100~100)
        inputX = joy[0]/100.00#わかりやすいようにしてみたつもり。joyの0にXが入って、1にYが入っている。
        inputY = joy[1]/100.00
        motorA = inputY + inputX
        motorB = inputY - inputX
        #print(motorA)
        #print(motorB)
        motor2.throttle = motorA
        motor1.throttle = motorB

    #センサー用ポーリング
    if time.monotonic() > (now + 5):
        now = time.monotonic()
        dist = sensor.range
        #print(dist)
        if dist < 200:
            motor1.throttle = 0
            motor2.throttle = 0
            time.sleep(1)
            uart0.read(128)
            motor2.throttle = -1
            motor1.throttle = -1
            time.sleep(1)
        uart0.write(b'Range: {}mm'.format(dist))
        uart0.write(b'\n\r')