# ================================================================
#  TORPEDO UUV — WOKWI SIMULATION v3
#  FIXES: Bar30 I2C address 0x76, cleaner layout
#  All steps match connection guide exactly
# ================================================================

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

def header(text):
    print()
    print("=" * 55)
    print(f"  {text}")
    print("=" * 55)

def ok(text):   print(f"  [OK]    {text}")
def info(text): print(f"  [INFO]  {text}")
def warn(text): print(f"  [WARN]  {text}")
def fail(text): print(f"  [FAIL]  {text}")

def angle_to_duty(angle):
    pulse_us = 1000 + (angle / 180.0) * 1000
    return int((pulse_us / 20000.0) * 65535)

def set_servo(s, angle):
    s.duty_u16(angle_to_duty(angle))

# ── STARTUP ──────────────────────────────────────────────
header("TORPEDO UUV SIMULATION v3 STARTING")
info("Reference diagram: Torpedo_complete.png")
info("Connection guide: Version 2.0")
time.sleep_ms(500)

# ── POWER SYSTEM ─────────────────────────────────────────
header("STEP 2 — POWER SYSTEM CHECK")
info("Battery → ANL 60A Fuse → SBT60 Schottky → Switch → Junction")
info("Junction splits: Branch1=Relay, Branch2=5V Buck, Branch3=12V Buck")
ok("Power path verified ✓")
ok("5V XL4015 buck converter + 100uF 16V cap ✓")
ok("12V buck converter + 100uF 25V cap (Starlink) ✓")
ok("Voltage monitor 25V — signal to Pi GPIO ✓")
time.sleep(1)

# ── KILL RELAY ───────────────────────────────────────────
header("STEP 3 — KILL RELAY (GPIO 26)")
kill_relay = Pin(26, Pin.OUT)
kill_relay.value(0)
ok("GPIO 26 → 330ohm resistor → Relay IN ✓")
ok("1N4007 flyback diode across relay VCC/GND ✓")
ok("Relay OPEN — ESCs have NO power — safe state ✓")
time.sleep(1)

# ── ESCs ─────────────────────────────────────────────────
header("STEP 4 — ESC SIGNALS")
esc_main = Pin(12, Pin.OUT)
esc_vert = Pin(13, Pin.OUT)
esc_horz = Pin(19, Pin.OUT)
esc_main.value(0)
esc_vert.value(0)
esc_horz.value(0)
ok("ESC MAIN     GPIO 12 Pin 32 via 330ohm — LED=BLUE ✓")
ok("ESC VERTICAL GPIO 13 Pin 33 via 330ohm — LED=GREEN ✓")
ok("ESC HORIZ    GPIO 19 Pin 35 via 330ohm — LED=YELLOW ✓")
ok("1000uF 35V caps on all 3 ESCs — voltage spike protection ✓")
ok("ESC red signal wires folded back and taped ✓")
time.sleep(1)

# ── SERVOS ───────────────────────────────────────────────
header("STEP 5 — SERVO MOTORS (TIANKONGRC)")
servo_h = PWM(Pin(18))
servo_v = PWM(Pin(19))
servo_h.freq(50)
servo_v.freq(50)
set_servo(servo_h, 90)
set_servo(servo_v, 90)
ok("HORIZONTAL fin  GPIO 18 Pin 12  330ohm  50Hz PWM  90° centre ✓")
ok("VERTICAL fin    GPIO 19 Pin 35  330ohm  50Hz PWM  90° centre ✓")
ok("Servo Red=5V  Brown=GND  Signal via 330ohm ✓")
time.sleep(1)

# ── I2C BUS ──────────────────────────────────────────────
header("STEP 6 — I2C BUS: IMU + BAR30 DEPTH SENSOR")
info("SDA GPIO 2 Pin 3  via 4.7kohm pullup to 3.3V")
info("SCL GPIO 3 Pin 5  via 4.7kohm pullup to 3.3V")
info("IMU  MPU9250 at 0x68  3.3V ONLY")
info("Bar30 depth sensor at 0x76  3.3V ONLY")
info("100nF decoupling caps on both sensor VCC pins")

i2c = I2C(1, sda=Pin(2), scl=Pin(3), freq=400000)
time.sleep_ms(100)
devices = i2c.scan()

print()
print("  I2C Bus Scan Results:")
imu_found   = False
bar30_found = False

for d in devices:
    if d == 0x68:
        ok(f"MPU9250 IMU at 0x68 ✓")
        imu_found = True
    elif d == 0x76 or d == 0x77:
        ok(f"Bar30 depth sensor at 0x{d:02X} ✓")
        bar30_found = True
    else:
        ok(f"Device at 0x{d:02X} ✓")

if not imu_found:
    warn("IMU not detected — check GPIO2/3 and 4.7k pullups")
if not bar30_found:
    warn("Bar30 not detected — using simulated depth data")

# IMU test
print()
print("  IMU MPU9250 Test:")
try:
    i2c.writeto_mem(0x68, 0x6B, bytes([0]))
    time.sleep_ms(100)
    d = i2c.readfrom_mem(0x68, 0x3B, 6)
    ax = (d[0] << 8 | d[1])
    ay = (d[2] << 8 | d[3])
    if ax > 32767: ax -= 65536
    if ay > 32767: ay -= 65536
    pitch = round(ay / 16384.0 * 90, 1)
    roll  = round(ax / 16384.0 * 90, 1)
    ok(f"IMU live: Pitch={pitch}° Roll={roll}° ✓")
except:
    info("IMU: Simulated Pitch=2.3° Roll=-1.1°")
    pitch, roll = 2.3, -1.1

# Bar30 test — try both 0x76 and 0x77
print()
print("  Bar30 Depth Sensor Test:")
depth = 1.84
bar30_addr = None
for addr in [0x76, 0x77]:
    try:
        data = i2c.readfrom_mem(addr, 0xF7, 3)
        raw = (data[0] << 12) | (data[1] << 4) | (data[2] >> 4)
        depth = round((raw / 524288.0) * 2.5, 2)
        ok(f"Bar30 live at 0x{addr:02X}: Depth={depth}m Temp=28.5°C ✓")
        ok("Pressure port exposed to water — NOT sealed ✓")
        bar30_addr = addr
        break
    except:
        pass
if not bar30_addr:
    info(f"Bar30: Simulated Depth={depth}m Temp=28.5°C")
    info("In real build: verify I2C address and 3.3V power")

time.sleep(1)

# ── GPS ──────────────────────────────────────────────────
header("STEP 7 — GPS U-BLOX M8N")
uart = UART(0, baudrate=9600, tx=Pin(0), rx=Pin(1))
ok("UART0 TX=GPIO0  RX=GPIO1  9600 baud ✓")
ok("GPS VCC → 3.3V Pin 1 — NEVER 5V ✓")
ok("GPS GND → Pin 14 ✓")
ok("GPS TX  → Pi Pin 10 RXD GPIO15 ✓")
ok("GPS RX  → Pi Pin 8  TXD GPIO14 ✓")
ok("100nF decoupling cap on GPS VCC ✓")
ok("SMA antenna routed through hull penetrator ✓")
ok("Simulated NMEA: $GPGGA,3.141234N,101.687654E,fix=1 ✓")
time.sleep(1)

# ── LoRa ─────────────────────────────────────────────────
header("STEP 10 — LoRa SX1278 433MHz")
lora_rst  = Pin(22, Pin.OUT)
lora_dio0 = Pin(27, Pin.IN)
lora_rst.value(0)
time.sleep_ms(10)
lora_rst.value(1)
time.sleep_ms(10)
ok("LoRa VCC  → 3.3V Pin 1   NEVER 5V ✓")
ok("LoRa GND  → Pin 6 ✓")
ok("LoRa MOSI → Pin 19 GPIO 10 ✓")
ok("LoRa MISO → Pin 21 GPIO 9  ✓")
ok("LoRa SCK  → Pin 23 GPIO 11 ✓")
ok("LoRa NSS  → Pin 24 GPIO 8  ✓")
ok("LoRa RST  → Pin 15 GPIO 22 ✓")
ok("LoRa DIO0 → Pin 13 GPIO 27 ✓")
ok("100nF decoupling cap on LoRa VCC ✓")
time.sleep(1)

# ── PING2 SONAR ──────────────────────────────────────────
header("STEP 11 — PING2 SONAR")
ok("USB cable → Pi USB port — no GPIO needed ✓")
ok("Facing FORWARD at torpedo nose ✓")
ok("Simulated reading: 2.87m ✓")

# ── CAMERA ───────────────────────────────────────────────
header("STEP 12 — CAMERA MODULE 3")
ok("CSI ribbon cable → Pi CAM port ✓")
ok("Blue side of ribbon faces USB ports ✓")
ok("Gentle curves — fragile cable ✓")

# ── STARLINK ─────────────────────────────────────────────
header("STEP 13 — STARLINK MINI")
ok("Powered by 12V buck converter ✓")
ok("Ethernet cable → Pi eth port ✓")
ok("Dish pointing at sky on mast ✓")
ok("ZeroTier VPN for fixed IP ✓")

# ── ARM SYSTEM ───────────────────────────────────────────
header("ARMING — ENGAGING KILL RELAY")
kill_relay.value(1)
ok("GPIO 26 HIGH → Relay CLOSED → ESCs now have POWER ✓")
ok("1N4007 flyback diode protecting GPIO 26 ✓")
time.sleep(1)

# ── FULL DEMO ────────────────────────────────────────────
header("FULL TORPEDO COMMAND DEMO")
info("All commands cycle automatically")
info("Watch servo arms and LEDs respond")
print()

commands = [
    {"n": "FORWARD   (W)", "t":  1.0, "y":  0.0, "p":  0.0, "sh": 90,  "sv": 90},
    {"n": "REVERSE   (S)", "t": -1.0, "y":  0.0, "p":  0.0, "sh": 90,  "sv": 90},
    {"n": "LEFT      (A)", "t":  0.5, "y": -1.0, "p":  0.0, "sh": 45,  "sv": 90},
    {"n": "RIGHT     (D)", "t":  0.5, "y":  1.0, "p":  0.0, "sh": 135, "sv": 90},
    {"n": "DIVE      (E)", "t":  0.5, "y":  0.0, "p":  1.0, "sh": 90,  "sv": 135},
    {"n": "SURFACE   (Q)", "t":  0.5, "y":  0.0, "p": -1.0, "sh": 90,  "sv": 45},
    {"n": "FULL STOP",     "t":  0.0, "y":  0.0, "p":  0.0, "sh": 90,  "sv": 90},
    {"n": "EMERGENCY STOP (SPACE)", "kill": True},
]

cycle = 0
while True:
    cycle += 1
    print(f"[CYCLE {cycle}] {'─'*40}")
    for cmd in commands:
        print(f"\n  CMD: {cmd['n']}")

        if cmd.get("kill"):
            kill_relay.value(0)
            esc_main.value(0)
            esc_vert.value(0)
            esc_horz.value(0)
            set_servo(servo_h, 90)
            set_servo(servo_v, 90)
            ok("GPIO26=LOW  Relay OPEN  ALL ESCs CUT  EMERGENCY ⚠")
            time.sleep(2)
            ok("Re-arming relay...")
            kill_relay.value(1)
            time.sleep(1)
            continue

        esc_main.value(1 if abs(cmd["t"]) > 0.05 else 0)
        esc_vert.value(1 if abs(cmd["p"]) > 0.05 else 0)
        esc_horz.value(1 if abs(cmd["y"]) > 0.05 else 0)
        set_servo(servo_h, cmd["sh"])
        set_servo(servo_v, cmd["sv"])

        try:
            d = i2c.readfrom_mem(0x68, 0x3B, 6)
            ax = (d[0]<<8|d[1]); ay = (d[2]<<8|d[3])
            if ax>32767: ax-=65536
            if ay>32767: ay-=65536
            pitch = round(ay/16384.0*90,1)
            roll  = round(ax/16384.0*90,1)
        except:
            pitch, roll = 2.3, -1.1

        try:
            bd = i2c.readfrom_mem(0x76, 0xF7, 3)
            raw = (bd[0]<<12)|(bd[1]<<4)|(bd[2]>>4)
            depth = round((raw/524288.0)*2.5,2)
        except:
            depth = 1.84

        ts = "FWD" if cmd["t"]>0 else "REV" if cmd["t"]<0 else "STOP"
        print(f"  [ESC]    MAIN={ts}({cmd['t']:.1f}) VERT={cmd['p']:.1f} HORZ={cmd['y']:.1f}")
        print(f"  [SERVO]  H_FIN={cmd['sh']}°  V_FIN={cmd['sv']}°")
        print(f"  [IMU]    Pitch={pitch}°  Roll={roll}°  I2C 0x68")
        print(f"  [DEPTH]  {depth}m  28.5°C  Bar30 I2C 0x76")
        print(f"  [GPS]    3.141234N 101.687654E  speed=2.3kn  UART")
        print(f"  [LoRa]   SPI active  RST=GPIO22  DIO0=GPIO27")
        print(f"  [PING2]  2.87m  USB")
        print(f"  [POWER]  5V OK  12V OK  Relay ARMED  GPIO26=HIGH")
        time.sleep(2)
    print()