import machine
import utime

# --- Pin Assignments ---
# 7-Segment Display Pins
SEGMENT_PINS = [
    machine.Pin(0, machine.Pin.OUT),  # Segment A
    machine.Pin(1, machine.Pin.OUT),  # Segment B
    machine.Pin(2, machine.Pin.OUT),  # Segment C
    machine.Pin(3, machine.Pin.OUT),  # Segment D
    machine.Pin(4, machine.Pin.OUT),  # Segment E
    machine.Pin(5, machine.Pin.OUT),  # Segment F
    machine.Pin(6, machine.Pin.OUT),  # Segment G
]
DP_PIN = machine.Pin(7, machine.Pin.OUT)  # Decimal Point
DIGIT_PINS = [
    machine.Pin(8, machine.Pin.OUT),  # Digit 1
    machine.Pin(9, machine.Pin.OUT),  # Digit 2
    machine.Pin(10, machine.Pin.OUT),  # Digit 3
    machine.Pin(11, machine.Pin.OUT),  # Digit 4
]

# Ultrasonic Sensor Pins
ULTRASONIC_TRIG = machine.Pin(12, machine.Pin.OUT)
ULTRASONIC_ECHO = machine.Pin(13, machine.Pin.IN)

# Motor LED Pins (for simulation)
MOTOR_LEFT_FORWARD = machine.Pin(14, machine.Pin.OUT)
MOTOR_LEFT_BACKWARD = machine.Pin(15, machine.Pin.OUT)
MOTOR_RIGHT_FORWARD = machine.Pin(16, machine.Pin.OUT)
MOTOR_RIGHT_BACKWARD = machine.Pin(17, machine.Pin.OUT)

# --- Constants ---
MULTIPLEX_DELAY = 10  # Multiplexing delay in ms
NUMBER_TEST_DELAY = 1000  # Delay between test numbers in ms


# --- Classes ---
class SevenSegmentDisplay:
    """Handles the 4x7-segment display."""
    NUMBER_MAP = [
        [1, 1, 1, 1, 1, 1, 0],  # 0
        [0, 1, 1, 0, 0, 0, 0],  # 1
        [1, 1, 0, 1, 1, 0, 1],  # 2
        [1, 1, 1, 1, 0, 0, 1],  # 3
        [0, 1, 1, 0, 0, 1, 1],  # 4
        [1, 0, 1, 1, 0, 1, 1],  # 5
        [1, 0, 1, 1, 1, 1, 1],  # 6
        [1, 1, 1, 0, 0, 0, 0],  # 7
        [1, 1, 1, 1, 1, 1, 1],  # 8
        [1, 1, 1, 1, 0, 1, 1],  # 9
    ]

    def __init__(self, segment_pins, digit_pins, dp_pin):
        self.segments = segment_pins
        self.digits = digit_pins
        self.dp = dp_pin

    def display_digit(self, number, digit_index):
        """Display a single number on a specific digit."""
        # Set segment states for the number (invert for common-anode)
        for i in range(len(self.segments)):
            self.segments[i].value(0 if self.NUMBER_MAP[number][i] else 1)  # Active-low logic

        # Turn off the decimal point (active-low)
        self.dp.value(1)

        # Activate only the selected digit (active-high)
        for i in range(len(self.digits)):
            self.digits[i].value(0)  # Turn off all digits
        self.digits[digit_index].value(1)  # Turn on the selected digit

    def display_number(self, number):
        """Display a full 4-digit number on the display."""
        number_str = "{:04d}".format(number)  # Zero-pad to 4 digits
        for digit_index, char in enumerate(number_str):
            self.display_digit(int(char), digit_index)
            utime.sleep_ms(MULTIPLEX_DELAY)  # Delay for multiplexing


class UltrasonicSensor:
    """Handles the HC-SR04 ultrasonic sensor."""
    def __init__(self, trig_pin, echo_pin):
        self.trig = trig_pin
        self.echo = echo_pin

    def get_distance(self):
        """Measure distance in cm."""
        # Send 10µs pulse to the trigger pin
        self.trig.low()
        utime.sleep_us(2)
        self.trig.high()
        utime.sleep_us(10)
        self.trig.low()

        # Measure pulse width
        start_time = utime.ticks_us()
        while self.echo.value() == 0:
            start_time = utime.ticks_us()
        while self.echo.value() == 1:
            end_time = utime.ticks_us()

        # Calculate distance in cm
        duration = utime.ticks_diff(end_time, start_time)
        distance = (duration / 2) / 29.1
        return max(0, min(9999, int(distance)))  # Clamp to 0-9999


# --- Testing Functions ---
def test_display(display):
    """Test the 7-segment display by cycling through numbers."""
    print("Testing 7-segment display...")
    for number in range(0, 10000, 1111):  # Test 0000, 1111, 2222, ..., 9999
        print(f"Displaying: {number}")
        for _ in range(100):  # Display for a short time
            display.display_number(number)
    print("Display test complete.")


def test_ultrasonic(sensor, display):
    """Test the ultrasonic sensor and display distances."""
    print("Testing ultrasonic sensor...")
    for _ in range(10):
        distance = sensor.get_distance()
        print(f"Measured distance: {distance} cm")
        for _ in range(50):  # Display distance for a short time
            display.display_number(distance)
    print("Ultrasonic sensor test complete.")


def test_motors():
    """Test motor control using LEDs to represent motor states."""
    print("Testing motors...")
    motors = {
        "left_forward": MOTOR_LEFT_FORWARD,
        "left_backward": MOTOR_LEFT_BACKWARD,
        "right_forward": MOTOR_RIGHT_FORWARD,
        "right_backward": MOTOR_RIGHT_BACKWARD,
    }
    # Simulate forward movement
    print("Simulating forward movement...")
    motors["left_forward"].value(1)
    motors["right_forward"].value(1)
    utime.sleep(1)
    # Stop
    print("Stopping...")
    for motor in motors.values():
        motor.value(0)
    utime.sleep(1)
    # Simulate backward movement
    print("Simulating backward movement...")
    motors["left_backward"].value(1)
    motors["right_backward"].value(1)
    utime.sleep(1)
    # Stop
    print("Stopping...")
    for motor in motors.values():
        motor.value(0)
    print("Motor test complete.")


# --- Main Program ---
if __name__ == "__main__":
    # Initialize components
    display = SevenSegmentDisplay(SEGMENT_PINS, DIGIT_PINS, DP_PIN)
    sensor = UltrasonicSensor(ULTRASONIC_TRIG, ULTRASONIC_ECHO)

    # Run tests
    test_display(display)
    test_ultrasonic(sensor, display)
    test_motors()
    print("All tests complete.")