#!/usr/bin/env python3
"""BCM4500 indirect register loopback test.

Write a known value via indirect write, then read it back.
Tests multiple approaches:
1. Multi-byte A6+A7+A8 in one transaction (0xB2 uses bcm_indirect_write)
2. Separate writes via 0xB6 diagnostic
3. Read via 0xB6 with various delays
4. Read using 0xB1 (bcm_indirect_read wrapper)

If we can write-then-read a value back, the DSP command processor works.
If not, we need to look at the I2C transaction structure more carefully.
"""
import sys
import time
sys.path.insert(0, 'tools')
from skywalker_lib import SkyWalker1

CMD_RAW_DEMOD_READ = 0xB1
CMD_RAW_DEMOD_WRITE = 0xB2
CMD_I2C_RAW_READ = 0xB5
CMD_I2C_DIAG = 0xB6
BCM4500_ADDR = 0x08

sw = SkyWalker1()
sw.open()
print('=== BCM4500 Indirect Register Loopback Test ===')
print(f'Firmware: {sw.get_fw_version()}')

# Boot with full sequence
print('\n--- Booting BCM4500 (full boot) ---')
result = sw._vendor_in(0x89, value=1, index=0, length=3)
cfg, stage = result[0], result[1]
print(f'  Config: 0x{cfg:02X}, Stage: 0x{stage:02X}')
time.sleep(0.1)

# ============================================================
# Test 1: Read default indirect register values
# ============================================================
print('\n=== Test 1: Default indirect register values (0xB1) ===')
for page in [0x00, 0x01, 0x06, 0x07, 0x0A, 0x0F]:
    try:
        data = sw._vendor_in(CMD_RAW_DEMOD_READ, value=page, index=0, length=1)
        print(f'  Page 0x{page:02X}: 0x{data[0]:02X}')
    except Exception as e:
        print(f'  Page 0x{page:02X}: FAILED ({e})')

# ============================================================
# Test 2: Write via 0xB2 (multi-byte A6+A7+A8), then read via 0xB1
# ============================================================
print('\n=== Test 2: Write 0x42 to page 0x00, read back (0xB2 write, 0xB1 read) ===')
try:
    # 0xB2: bcm_indirect_write(page=0x00, val=0x42)
    # Writes A6=0x00, A7=0x42, A8=0x03 in one 3-byte I2C transaction
    sw._vendor_in(CMD_RAW_DEMOD_WRITE, value=0x00, index=0x42, length=0)
except Exception:
    pass  # May not return data

time.sleep(0.05)

# Read back via 0xB1 (bcm_indirect_read with 1ms delay)
try:
    data = sw._vendor_in(CMD_RAW_DEMOD_READ, value=0x00, index=0, length=1)
    print(f'  Read back page 0x00: 0x{data[0]:02X}')
    if data[0] == 0x42:
        print('  >> LOOPBACK SUCCESS! DSP is processing commands!')
    elif data[0] == 0x00:
        print('  >> Got 0x00 — either DSP not running or write/read protocol broken')
    else:
        print(f'  >> Unexpected: 0x{data[0]:02X}')
except Exception as e:
    print(f'  Read failed: {e}')

# ============================================================
# Test 3: Direct register reads before/after indirect write
# ============================================================
print('\n=== Test 3: Direct register state before/after indirect commands ===')
print('  Before indirect write:')
for reg in [0xA6, 0xA7, 0xA8]:
    data = sw._vendor_in(CMD_I2C_RAW_READ, value=BCM4500_ADDR,
                          index=reg, length=1)
    print(f'    0x{reg:02X}: 0x{data[0]:02X}')

# Write via 0xB2
try:
    sw._vendor_in(CMD_RAW_DEMOD_WRITE, value=0x06, index=0xAB, length=0)
except Exception:
    pass

time.sleep(0.01)
print('  After indirect write (page=0x06, data=0xAB):')
for reg in [0xA6, 0xA7, 0xA8]:
    data = sw._vendor_in(CMD_I2C_RAW_READ, value=BCM4500_ADDR,
                          index=reg, length=1)
    print(f'    0x{reg:02X}: 0x{data[0]:02X}')

# ============================================================
# Test 4: Manual step-by-step with individual I2C writes
# ============================================================
print('\n=== Test 4: Manual indirect read using individual raw I2C writes ===')
print('  Writing A6=0x06 via direct I2C write...')

# We don't have a raw I2C write command, but we can use the 0xB6 diagnostic
# which does individual writes and reads for us.

# Test 4a: 0xB6 with READ command (wIndex=0x01)
print('\n  4a: 0xB6 diagnostic — READ page 0x06:')
diag = sw._vendor_in(CMD_I2C_DIAG, value=0x06, index=0x01, length=8)
print(f'    A6 write ok:  {diag[0]}')
print(f'    A6 readback:  0x{diag[1]:02X}')
print(f'    A8 write ok:  {diag[2]}')
print(f'    A8 immediate: 0x{diag[3]:02X}')
print(f'    A8 after 2ms: 0x{diag[4]:02X}')
print(f'    A7 data:      0x{diag[5]:02X}')
print(f'    A6 final:     0x{diag[6]:02X}')

# ============================================================
# Test 5: Try reading A7 with longer delays
# ============================================================
print('\n=== Test 5: Indirect read with longer delays ===')
print('  Maybe the DSP needs more time to process the command...')

# Use 0xB6 to write A6=0x06 and A8=0x01
sw._vendor_in(CMD_I2C_DIAG, value=0x06, index=0x01, length=8)

for delay_ms in [10, 50, 100, 500]:
    time.sleep(delay_ms / 1000.0)
    # Read A7 via raw I2C
    try:
        data = sw._vendor_in(CMD_I2C_RAW_READ, value=BCM4500_ADDR,
                              index=0xA7, length=1)
        print(f'  After {delay_ms:4d}ms: A7=0x{data[0]:02X}')
    except Exception as e:
        print(f'  After {delay_ms:4d}ms: FAILED ({e})')

# ============================================================
# Test 6: Direct register dump after all tests
# ============================================================
print('\n=== Test 6: Register state after all tests ===')
print('  A0-AF:')
for reg in range(0xA0, 0xB0):
    data = sw._vendor_in(CMD_I2C_RAW_READ, value=BCM4500_ADDR,
                          index=reg, length=1)
    print(f'  0x{reg:02X}=0x{data[0]:02X}', end='')
    if (reg % 8) == 7:
        print()
print()

# ============================================================
# Test 7: Power cycle BCM4500 and check pre-command register state
# ============================================================
print('\n=== Test 7: Reboot and check BEFORE any indirect commands ===')
sw._vendor_in(0x89, value=0, index=0, length=3)  # shutdown
time.sleep(0.5)
sw._vendor_in(0x89, value=1, index=0, length=3)  # full boot
time.sleep(0.1)
print('  Fresh boot — direct reg reads (no indirect commands issued):')
for reg in [0xA0, 0xA2, 0xA4, 0xA6, 0xA7, 0xA8, 0xA9, 0xAA, 0xAB]:
    data = sw._vendor_in(CMD_I2C_RAW_READ, value=BCM4500_ADDR,
                          index=reg, length=1)
    print(f'    0x{reg:02X}: 0x{data[0]:02X}')

# Now issue ONE indirect read command and check if registers change
print('\n  After ONE indirect read (page 0x06):')
diag = sw._vendor_in(CMD_I2C_DIAG, value=0x06, index=0x01, length=8)
print(f'    A7 data: 0x{diag[5]:02X} (this is the indirect read result)')

# Check if direct registers changed
print('  Direct register check after indirect command:')
for reg in [0xA0, 0xA2, 0xA4, 0xA6, 0xA7, 0xA8]:
    data = sw._vendor_in(CMD_I2C_RAW_READ, value=BCM4500_ADDR,
                          index=reg, length=1)
    print(f'    0x{reg:02X}: 0x{data[0]:02X}')

sw.close()
print('\n=== Done ===')