from machine import Pin, I2C, UART, SPI
import time

# ---------------- I2C : MPU6050 ----------------
i2c = I2C(0, scl=Pin(22), sda=Pin(21))

MPU_ADDR = 0x68
i2c.writeto_mem(MPU_ADDR, 0x6B, b'\x00')  # wake up MPU

# ---------------- GPS (UART1) ----------------
gps = UART(1, baudrate=9600, tx=Pin(13), rx=Pin(12))

# ---------------- SIM800L (UART2) ----------------
gsm = UART(2, baudrate=9600, tx=Pin(17), rx=Pin(16))

# ---------------- MCP2515 (SPI) ----------------
spi = SPI(2, baudrate=1000000, polarity=0, phase=0,
          sck=Pin(18), mosi=Pin(23), miso=Pin(19))
cs = Pin(5, Pin.OUT)
cs.value(1)

print("=== SYSTEM START ===")

# ---------------- FUNCTIONS ----------------

def read_mpu():
    try:
        data = i2c.readfrom_mem(MPU_ADDR, 0x3B, 6)

        ax = (data[0] << 8 | data[1])
        ay = (data[2] << 8 | data[3])
        az = (data[4] << 8 | data[5])

        if ax > 32768: ax -= 65536
        if ay > 32768: ay -= 65536
        if az > 32768: az -= 65536

        print("MPU -> AX:", ax, "AY:", ay, "AZ:", az)
    except:
        print("MPU ERROR")


def read_gps():
    if gps.any():
        try:
            line = gps.readline()
            if line:
                print("GPS ->", line.decode().strip())
        except:
            pass


def read_can():
    # simulation SPI (MCP2515)
    cs.value(0)
    spi.write(b'\xAA')
    cs.value(1)
    print("CAN -> SPI OK")


def test_gsm():
    gsm.write("AT\r\n")
    time.sleep(0.5)

    if gsm.any():
        try:
            print("GSM ->", gsm.read().decode())
        except:
            pass


# ---------------- LOOP ----------------
while True:
    read_mpu()
    read_gps()
    read_can()
    test_gsm()

    print("-----------------------------")
    time.sleep(2)