from sdCard_Dev         import SDCard
from gc9107_0inch85_Dev import LCD_0inch85, RED, GREEN, BLUE, WHITE,BLACK
from ws2812_Dev         import WS2812
# from qmi8658a_Dev       import QMI8658A
from qmi8658a 			import QMI8658A
from machine            import I2C, Pin
import machine
import uos
import time



# 定义LED引脚
led = [0, 1, 2, 3,  8, 9, 10, 11, 26, 27, 28,29]

# 初始化LED引脚
led_pins = [Pin(pin_num, Pin.OUT) for pin_num in led]
i=0


#Initialize 0inch85 LCD
lcd = LCD_0inch85()

# Initialize ws2812 RGB
led = WS2812(pin_num=25, num_leds=1)

# Initialize I2C（GP21=SCL, GP20=SDA）
i2c = I2C(0, scl=Pin(21), sda=Pin(20), freq=400_000)

# Initialize QMI8658A
imu = QMI8658A(i2c)

# Intialize SPI peripheral (start with 1 MHz) for SD Card
spi = machine.SPI(0,
                  baudrate=100000000,
                  polarity=0,
                  phase=0,
                  bits=8,
                  firstbit=machine.SPI.MSB,
                  sck=machine.Pin(18),
                  mosi=machine.Pin(19),
                  miso=machine.Pin(16))
cs = machine.Pin(17, machine.Pin.OUT)

# Initialize SD card
# sd = SDCard(spi, cs)


# 预定义颜色
colors = [
    WS2812.create_color(0, 255, 0),    # Red
    WS2812.create_color(255,0, 0),     # Green
    WS2812.create_color(0, 0, 255),    # Blue
    WS2812.create_color(127, 127, 0),  # Yellow
    WS2812.create_color(0, 127, 127),  # Cyan
    WS2812.create_color(127, 0, 127),  # Magenta
    WS2812.create_color(255, 255, 255) # White
]

# led.start_cycle([RED, GREEN, BLUE, YELLOW, WHITE])
# led.start_cycle([RED,GREEN, BLUE])
# Mount filesystem
# vfs = uos.VfsFat(sd)
# uos.mount(vfs, "/sd")

sd_ok = False

try:
    sd = SDCard(spi, cs)
    vfs = uos.VfsFat(sd)
    uos.mount(vfs,"/sd")
    sd_ok = True
    print("SD mounted!!!")
except Exception as e:
    print("SD mount failed:",e)


def draw_arrow_lcd(lcd, roll, pitch):
    lcd.fill(0xFFFF)  # 清屏，黑底白字（或根据你驱动适配）
    time.sleep(0.05)

    lcd.text("Roll: {:.1f}".format(roll), 10, 10, 0)
    lcd.text("Pitch:{:.1f}".format(pitch), 10, 25, 0)

    arrow_up    = "Left" if pitch < -10 else " "
    arrow_down  = "Right" if pitch > 10 else " "
    arrow_left  = "Down" if roll < -10 else " "
    arrow_right = "UP" if roll > 10 else " "

    lcd.text("Orientation:", 10, 45, 0)
    lcd.text(arrow_right,    45, 60, 0)
    lcd.text("[ ]",          40, 70, 0)
    lcd.text(arrow_left,     40, 80, 0)
    lcd.text(arrow_up,       10, 70, 0)
    lcd.text(arrow_down,     60, 70, 0)

    lcd.show()

def file_exists(path):
    try:
        uos.stat(path)
        return True
    except:
        return False

def show_boot_image(lcd):
    sd_path    = "/sd/image.bmp"
    flash_path = "/flash_img.bmp"
    
    #优先使用SD卡
    if sd_ok and file_exists(sd_path):
        print("Load image from SD!")
        lcd.text("Load image ", 20, 0, WHITE)
        lcd.text("from SD!", 20, 20, WHITE)
        lcd.show()  # 刷新显示
        time.sleep(1.5)
        lcd.draw_bmp(sd_path)
    elif file_exists(flash_path):
        print("Load image from FLASH!")
        lcd.text("Load image ", 20, 0, WHITE)
        lcd.text("from FLASH!", 20, 20, WHITE)
        lcd.show()  # 刷新显示
        time.sleep(1.5)
        lcd.draw_bmp(flash_path)
    else:
        print("No image found!")
        lcd.text("No image found!", 10, 10, WHITE)
        lcd.show()  # 刷新显示
        time.sleep(1.5)
        lcd.fill_color(WHITE)

# ===== 主程序 =====
if __name__ == "__main__":
    
    color_list = [RED, GREEN, BLUE, WHITE]
    index = 0
    if(sd_ok):
        print("SD card sectors:", sd.ioctl(4, 0))
        print("SD card capacity (bytes):", sd.ioctl(4, 0) * 512)
        # Create a file and write something to it
        with open("/sd/test01.txt", "w") as file:
            file.write("Hello, SD World!\r\n")
            file.write("This is a test\r\n")
        # Open the file we just created and read from it
        with open("/sd/test01.txt", "r") as file:
            data = file.read()
            print(data)
    
    ##################################################

    # lcd.draw_rgb565("/image.bin")  # 文件需提前上传
    # lcd2.draw_rgb565("/sd/image.bin")

    lcd.init_lcd()
  #  led.show(colors[0])
   # lcd.fill_color(RED)
#     time.sleep(0.2)
    # led.show(colors[1])
    # lcd.fill_color(GREEN)
#     time.sleep(0.2)
    # led.show(colors[2])
    # lcd.fill_color(BLUE)
#     time.sleep(0.2)
    # led.show(colors[6])
    # lcd.fill_color(WHITE)
#     time.sleep(0.2)
    
    led.show(colors[6])
    
    #lcd.draw_bmp("/sd/picture.bmp")
    #time.sleep(1)
    #lcd.draw_bmp("/sd/image.bmp")
    #time.sleep(1)
#     lcd.draw_bmp("/sd/B4.bmp")
#     lcd.draw_bmp("/flash_img.bmp")
    show_boot_image(lcd)
    time.sleep(2)
#     lcd.fill(1)

    # 显示文字
#     lcd.text("ABC", 10, 10, WHITE)  # 在 (10,10) 位置显示黑色 ABC 字符
#     lcd.show()  # 刷新显示
#     time.sleep(2)
    while True:
        i=i+1
        led.show(color_list[index])

        # 循环渐变显示
#         led.cycle(colors, steps_per_transition=100, delay_ms=10)
              # 主循环：打印传感器数据
#         lcd.fill(1)  # 清屏
#         ax, ay, az, gx, gy, gz = imu.read()
#         print("Accel: X={:.2f}g Y={:.2f}g Z={:.2f}g".format(ax, ay, az))
#         print("Gyro:  X={:.2f}°/s Y={:.2f}°/s Z={:.2f}°/s\n".format(gx, gy, gz))
#         time.sleep(0.2)
        # 显示文字
#         lcd.text("Accel: X={:.2f}g Y={:.2f}g Z={:.2f}g".format(ax, ay, az), 10, 10, BLACK)  # 在 (10,10) 位置显示黑色 ABC 字符
#         lcd.show()  # 刷新显示
        # 主循环：打印传感器数据
#         ax, ay, az, gx, gy, gz = imu.read()
#         print("Accel: X={:.2f}g Y={:.2f}g Z={:.2f}g".format(ax, ay, az))
#         print("Gyro:  X={:.2f}°/s Y={:.2f}°/s Z={:.2f}°/s\n".format(gx, gy, gz))
#         time.sleep(0.2)
#         lcd.fill(1)  # 清屏

#         ax, ay, az, gx, gy, gz = imu.read()
#         imu.update_orientation()
#         imu.draw_orientation()

#         lcd.text("Accel:", 10, 10, 0)
#         lcd.text("X={:.2f}g".format(ax), 10, 25, 0)
#         lcd.text("Y={:.2f}g".format(ay), 10, 40, 0)
#         lcd.text("Z={:.2f}g".format(az), 10, 55, 0)

#         lcd.text("Gyro:", 10, 75, 0)
#         lcd.text("X={:.2f}".format(gx), 10, 90, 0)
#         lcd.text("Y={:.2f}".format(gy), 10, 105, 0)
#         lcd.text("Z={:.2f}".format(gz), 10, 120, 0)

#         lcd.show()
#         time.sleep(0.05)
        imu.update_orientation()
        roll, pitch = imu.get_orientation()
        draw_arrow_lcd(lcd, roll, pitch)
          # 同时点亮所有LED
        for pin in led_pins:
            pin.value(i&1)  # 点亮LED
#             time.sleep(1)  # 等待1秒钟
        time.sleep(0.02)
        index = (index + 1) % len(color_list)
        
#         # 同时点亮所有LED
#         for pin in led_pins:
#             pin.value(1)  # 点亮LED
#             time.sleep(1)  # 等待1秒钟

            # 同时关闭所有LED
#         for pin in led_pins:
#             pin.value(0)  # 关闭LED
#             time.sleep(1)  # 等待1秒
#             pass  # 图片保持显示
            # print("AA")


