import rp2                          # Import RP2040-specific PIO support
from rp2 import PIO                # Import PIO constants (like OUT_LOW)
from machine import Pin, time_pulse_us            # Import Pin class to control GPIOs
from time import sleep, sleep_us             # Import sleep for delay between counter updates
import math
# -------- #
# sevseg PIO program #
# -------- #

# --- PIO Program for 4-Digit 7-Segment Display --- #
@rp2.asm_pio(
    out_init=[PIO.OUT_LOW]*8,      # Initialize 8 output pins (segments A-G + DP) to LOW
    sideset_init=[PIO.OUT_LOW]*4   # Initialize 4 side-set pins (digit select lines) to LOW
)
def sevseg():
    wrap_target()                  # Mark the start of a loopable PIO block
    label("0")                     # Label this instruction as "0" for looping back later

    pull(noblock)           .side(0)
    """ Pull 32-bit data from FIFO into OSR (non-blocking); 
        while setting all digit-select lines LOW (avoid ghosting) """

    mov(x, osr)             .side(0)
    """ Move pulled data from OSR into register X; 
        keep digit lines LOW during transfer """

    out(pins, 8)            .side(1)
    """ Output 8 bits to segment pins (digit 1 data);
        Activate digit 1 by setting GPIO10 HIGH (side=1) """

    out(pins, 8)            .side(2)
    """ Output 8 bits for digit 2;
        Activate digit 2 by setting GPIO11 HIGH (side=2) """

    out(pins, 8)            .side(4)
    """ Output 8 bits for digit 3;
        Activate digit 3 by setting GPIO12 HIGH (side=4) """

    out(pins, 8)            .side(8)
    """ Output 8 bits for digit 4;
        Activate digit 4 by setting GPIO13 HIGH (side=8) """

    jmp("0")                .side(0)
    """ Loop back to label "0";
        Reset all digit select lines LOW before restarting """

    wrap()                         # End of the loopable PIO program

# --- Start State Machine for PIO --- #
sm = rp2.StateMachine(
    0,                # Use state machine 0
    sevseg,           # Load the sevseg PIO program
    freq=2000,        # Set clock frequency to 2kHz (controls refresh rate)
    out_base=Pin(2),  # Segment output pins start from GPIO2 (covers GPIO2–9)
    sideset_base=Pin(10)  # Digit select pins start from GPIO10 (GPIO10–13)
)


# 7-segment digit encodings (common cathode)
digits = [
  0b11000000,  # 0
  0b11111001,  # 1
  0b10100100,  # 2 
  0b10110000,  # 3
  0b10011001,  # 4
  0b10010010,  # 5
  0b10000010,  # 6
  0b11111000,  # 7
  0b10000000,  # 8
  0b10011000,  # 9
]


# Convert a number (0–9999) to 4-digit 7-segment display format
def segmentize(num):

    decimal_point_mask = 0b01111111 

    return (
        digits[num % 10]                             # Units digit
        | (digits[num // 10 % 10] & decimal_point_mask) << 8                # Tens digit
        | digits[num // 100 % 10] << 16              # Hundreds digit
        | digits[num // 1000 % 10] << 24             # Thousands digit
    )


def measure_distance():
    trig.low()                 # Ensure trigger is LOW
    sleep_us(4)          # Small delay to stabilize
    trig.high()               # Send 4µs HIGH pulse to start the measurement
    sleep_us(4)          # Hold HIGH for 4µs (sometimes 10µs is standard)
    trig.low()                # Pull trigger LOW again to complete the pulse

    # ⏱ Measure duration of HIGH signal on echo pin
    duration = time_pulse_us(echo, 1)  # Time echo pin stays HIGH (which is set by 1) (in µs)

    # 📐 Convert time to distance using the speed of sound
    return duration / 5.882  # Return distance in centimeters multiplied by 10

# 🎯 Setup TRIG as output pin and ECHO as input pin
trig = Pin(28, Pin.OUT)  # Trigger pin on GPIO 28
echo = Pin(27, Pin.IN)   # Echo pin on GPIO 27


sm.active(1)          # Start the state machine

while True:
    distance = int(measure_distance())

    sm.put(segmentize(distance))
    
    sleep(1)