#coding:utf-8
import os
import time
import RPi.GPIO as GPIO
from base_ctrl import BaseController


GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
#底盘连接
def is_raspberry_pi5():
    with open('/proc/cpuinfo', 'r') as file:
        for line in file:
            if 'Model' in line:
                if 'Raspberry Pi 5' in line:
                    return True
                else:
                    return False

#云台连接

base = BaseController('/dev/ttyAMA0', 115200)

class CAR:
    input_x = 0
    input_y = 0
    input_speed = 0
    input_acc = 0

    def stop(self):                 # 停止运行
        base.send_command({"T":1,"L":0,"R":0})
        print("run: stop move")

    def Q(self):                    # 停止的快捷键
        self.stop()
    def q(self):
        self.stop()
    
    def forward(self):              # 前进
        base.send_command({"T":1,"L":0.2,"R":0.2})
        time.sleep(1)
        base.send_command({"T":1,"L":0,"R":0})
        print("run: move !!!!forward")

    def W(self):                    # 前进的快捷键
        self.forward()
    def w(self):
        self.forward()

    def back(self):                 # 后退
        base.send_command({"T":1,"L":-0.2,"R":-0.2})
        time.sleep(1)
        base.send_command({"T":1,"L":0,"R":0})
        print("run: move back")

    def S(self):                    # 后退的快捷键
        self.back()
    def s(self):
        self.back()

    def left(self):                 # 左转
        base.send_command({"T":1,"L":-0.2,"R":0.2})
        time.sleep(0.5)
        base.send_command({"T":1,"L":0,"R":0})
        print("run: move left")
    
    def A(self):                    # 左转的快捷键
        self.left()
    def a(self):
        self.left()

    def right(self):                # 右转
        base.send_command({"T":1,"L":0.2,"R":-0.2})
        time.sleep(0.5)
        base.send_command({"T":1,"L":0,"R":0})
        print("run: move right")

    def D(self):                    # 右转的快捷键
        self.right()
    def d(self):
        self.right()

    def up(self):
        self.input_y+=10
        base.gimbal_ctrl(self.input_x, self.input_y, self.input_speed, self.input_acc)

    def down(self):
        self.input_y-=10
        base.gimbal_ctrl(self.input_x, self.input_y, self.input_speed, self.input_acc)

    def yunleft(self):
        self.input_x-=10
        base.gimbal_ctrl(self.input_x, self.input_y, self.input_speed, self.input_acc)

    def yunright(self):
        self.input_x+=10
        base.gimbal_ctrl(self.input_x, self.input_y, self.input_speed, self.input_acc)

    def guiwei(self):
        self.input_x = 2
        self.input_y = 2
        self.input_speed = 0
        base.gimbal_base_ctrl(self.input_x, self.input_y, self.input_speed)