"""
Pico2ROMEmu - ROM Emulator for Raspberry Pi Pico 2 (RP2350)
Uses PIO and DMA for high-performance ROM emulation
"""

import machine
import rp2
import array
import time
from machine import Pin, mem32
import _thread
import gc

class Pico2ROMEmu:
    def __init__(self):
        # GPIO Pin assignments
        self.ADDR_PINS = list(range(0, 16))  # GP0-GP15 for A0-A15
        self.DATA_PINS = [16, 17, 18, 19, 20, 21, 22, 26]  # GP16-GP22, GP26 for D0-D7
        self.CE_PIN = 27  # GP27 for Chip Enable (active low)
        self.OE_PIN = 28  # GP28 for Output Enable (active low)
        
        # ROM storage
        self.rom_data = bytearray(65536)  # 64KB ROM space
        self.rom_images = {}  # Multiple ROM images
        self.current_image = 0
        
        # Performance counters
        self.access_count = 0
        self.last_access_time = 0
        
        self.setup_pins()
        self.setup_pio()
        self.load_default_rom()
        
    def setup_pins(self):
        """Initialize GPIO pins"""
        # Address pins as input with pull-down
        self.addr_pins = []
        for pin_num in self.ADDR_PINS:
            pin = Pin(pin_num, Pin.IN, Pin.PULL_DOWN)
            self.addr_pins.append(pin)
        
        # Data pins as output (initially high-Z)
        self.data_pins = []
        for pin_num in self.DATA_PINS:
            pin = Pin(pin_num, Pin.OUT, value=0)
            self.data_pins.append(pin)
        
        # Control pins as input with pull-up
        self.ce_pin = Pin(self.CE_PIN, Pin.IN, Pin.PULL_UP)
        self.oe_pin = Pin(self.OE_PIN, Pin.IN, Pin.PULL_UP)
        
        print("GPIO pins initialized")
    
    @rp2.asm_pio(set_init=rp2.PIO.OUT_LOW, out_init=rp2.PIO.OUT_LOW)
    def rom_emulator_pio():
        """PIO program for high-speed ROM emulation"""
        # Wait for chip enable and output enable
        wait(0, pin, 0)  # Wait for CE# low
        wait(0, pin, 1)  # Wait for OE# low
        
        # Read address from pins
        in_(pins, 16)    # Read 16-bit address
        mov(x, isr)      # Move address to X register
        
        # Calculate ROM data address
        mov(isr, x)      # Move address back to ISR
        push()           # Push to FIFO for CPU processing
        
        # Wait for data from CPU
        pull()           # Pull data from FIFO
        out(pins, 8)     # Output 8-bit data
        
        # Wait for access to complete
        wait(1, pin, 0)  # Wait for CE# high
        
    def setup_pio(self):
        """Setup PIO state machine for ROM emulation"""
        self.sm = rp2.StateMachine(0, self.rom_emulator_pio, 
                                  freq=125_000_000,  # 125MHz for fast response
                                  in_base=Pin(0),    # Address input base
                                  out_base=Pin(16),  # Data output base
                                  set_base=Pin(16))  # Data set base
        
        # Start the state machine
        self.sm.active(1)
        print("PIO state machine initialized")
    
    def load_default_rom(self):
        """Load a default ROM image for testing"""
        # Create a simple test ROM
        for i in range(len(self.rom_data)):
            if i < 256:
                self.rom_data[i] = i & 0xFF  # Simple counter pattern
            elif i < 512:
                self.rom_data[i] = 0xFF - (i & 0xFF)  # Inverted pattern
            else:
                self.rom_data[i] = 0x55 if (i // 256) % 2 else 0xAA  # Alternating pattern
        
        print("Default ROM loaded (64KB)")
    
    def load_rom_image(self, image_id, filename):
        """Load ROM image from file"""
        try:
            with open(filename, 'rb') as f:
                data = f.read()
                self.rom_images[image_id] = bytearray(data)
                print(f"ROM image {image_id} loaded: {len(data)} bytes")
                return True
        except Exception as e:
            print(f"Error loading ROM image {image_id}: {e}")
            return False
    
    def switch_rom_image(self, image_id):
        """Switch to different ROM image"""
        if image_id in self.rom_images:
            self.rom_data = self.rom_images[image_id]
            self.current_image = image_id
            print(f"Switched to ROM image {image_id}")
            return True
        else:
            print(f"ROM image {image_id} not found")
            return False
    
    def read_address(self):
        """Read current address from GPIO pins"""
        address = 0
        for i, pin in enumerate(self.addr_pins):
            if pin.value():
                address |= (1 << i)
        return address
    
    def write_data(self, data):
        """Write data to GPIO data pins"""
        for i, pin in enumerate(self.data_pins):
            pin.value((data >> i) & 1)
    
    def set_data_direction(self, output=True):
        """Set data pins direction (output/input)"""
        for pin_num in self.DATA_PINS:
            if output:
                Pin(pin_num, Pin.OUT)
            else:
                Pin(pin_num, Pin.IN, Pin.PULL_DOWN)
    
    def rom_access_handler(self):
        """Main ROM access handling loop (runs on core 1)"""
        print("ROM access handler started on core 1")
        
        while True:
            try:
                # Check if PIO has address data
                if self.sm.rx_fifo():
                    address = self.sm.get() & 0xFFFF  # Get 16-bit address
                    
                    # Bounds check
                    if address < len(self.rom_data):
                        data = self.rom_data[address]
                    else:
                        data = 0xFF  # Default value for out-of-bounds
                    
                    # Send data back to PIO
                    self.sm.put(data)
                    
                    # Update statistics
                    self.access_count += 1
                    self.last_access_time = time.ticks_ms()
                
                # Small delay to prevent CPU overload
                time.sleep_us(1)
                
            except Exception as e:
                print(f"ROM access handler error: {e}")
                time.sleep_ms(10)
    
    def start_emulation(self):
        """Start ROM emulation on second core"""
        _thread.start_new_thread(self.rom_access_handler, ())
        print("ROM emulation started")
    
    def get_statistics(self):
        """Get emulation statistics"""
        return {
            'access_count': self.access_count,
            'last_access_time': self.last_access_time,
            'current_image': self.current_image,
            'rom_size': len(self.rom_data),
            'free_memory': gc.mem_free()
        }
    
    def dump_rom_section(self, start_addr=0, length=256):
        """Dump ROM section for debugging"""
        print(f"ROM dump from 0x{start_addr:04X}:")
        for i in range(0, length, 16):
            addr = start_addr + i
            if addr >= len(self.rom_data):
                break
            
            # Address
            line = f"{addr:04X}: "
            
            # Hex data
            hex_data = []
            ascii_data = ""
            for j in range(16):
                if addr + j < len(self.rom_data):
                    byte = self.rom_data[addr + j]
                    hex_data.append(f"{byte:02X}")
                    ascii_data += chr(byte) if 32 <= byte <= 126 else "."
                else:
                    hex_data.append("  ")
                    ascii_data += " "
            
            line += " ".join(hex_data) + "  " + ascii_data
            print(line)

# Web interface for remote control (if using Pico W)
class ROMEmuWebInterface:
    def __init__(self, rom_emu):
        self.rom_emu = rom_emu
        self.setup_wifi()
        self.setup_web_server()
    
    def setup_wifi(self):
        """Setup WiFi connection (for Pico W only)"""
        try:
            import network
            self.wlan = network.WLAN(network.STA_IF)
            self.wlan.active(True)
            # Configure your WiFi credentials here
            # self.wlan.connect('YOUR_SSID', 'YOUR_PASSWORD')
            print("WiFi interface ready")
        except ImportError:
            print("WiFi not available (not Pico W)")
    
    def setup_web_server(self):
        """Setup simple web server for ROM management"""
        # Implementation would go here for web-based ROM management
        pass

# Main execution
def main():
    print("Pico2ROMEmu - ROM Emulator for RP2350")
    print("=====================================")
    
    # Create ROM emulator instance
    rom_emu = Pico2ROMEmu()
    
    # Start emulation
    rom_emu.start_emulation()
    
    # Interactive command loop
    print("\nROM Emulator ready!")
    print("Commands: stats, dump, switch , load  , help, quit")
    
    while True:
        try:
            command = input("> ").strip().lower()
            
            if command == "quit":
                break
            elif command == "stats":
                stats = rom_emu.get_statistics()
                print("ROM Emulator Statistics:")
                for key, value in stats.items():
                    print(f"  {key}: {value}")
            elif command == "dump":
                rom_emu.dump_rom_section(0, 256)
            elif command.startswith("switch "):
                try:
                    image_id = int(command.split()[1])
                    rom_emu.switch_rom_image(image_id)
                except (IndexError, ValueError):
                    print("Usage: switch ")
            elif command.startswith("load "):
                try:
                    parts = command.split()
                    image_id = int(parts[1])
                    filename = parts[2]
                    rom_emu.load_rom_image(image_id, filename)
                except (IndexError, ValueError):
                    print("Usage: load  ")
            elif command == "help":
                print("Available commands:")
                print("  stats - Show emulation statistics")
                print("  dump - Dump first 256 bytes of ROM")
                print("  switch  - Switch to ROM image ")
                print("  load   - Load ROM image from file")
                print("  help - Show this help")
                print("  quit - Exit emulator")
            else:
                print("Unknown command. Type 'help' for available commands.")
        
        except KeyboardInterrupt:
            print("\nShutting down...")
            break
        except Exception as e:
            print(f"Error: {e}")

if __name__ == "__main__":
    main()