from machine import Pin, ADC, PWM, Timer

# Define LED Pins
D5 = Pin(5, Pin.OUT)   # Heartbeat
D4 = Pin(32, Pin.OUT)  # Binary Bit 4 (MSB for a 5-bit display)
D3 = Pin(33, Pin.OUT)  # Binary Bit 3
D2 = Pin(14, Pin.OUT)  # Binary Bit 2
D1 = Pin(26, Pin.OUT)  # Binary Bit 1
D0 = Pin(27, Pin.OUT)  # Binary Bit 0 (LSB)

# Setup PWM for Servo
pwm0 = PWM(Pin(25))
pwm0.freq(50)

# Setup ADC (9-bit: 0-511)
adc = ADC(Pin(2))
adc.atten(ADC.ATTN_11DB)
adc.width(ADC.WIDTH_9BIT)

# Heartbeat Handler
def handler(time0):
    D5.value(not D5.value())

time0 = Timer(0)
time0.init(period=150, mode=Timer.PERIODIC, callback=handler)

# Improved Map function for both Servo and Binary Display
def map_and_display(x, in_min, in_max, out_min, out_max):
    # 1. Update Binary LEDs based on the raw value
    # We use the 5 most significant bits of the 9-bit ADC value
    # so the display changes visibly across the whole pot range.
    val_5bit = int((x / in_max) * 31) 
    binary = '{0:05b}'.format(val_5bit)
    
    D4.value(int(binary[0])) # MSB
    D3.value(int(binary[1]))
    D2.value(int(binary[2]))
    D1.value(int(binary[3]))
    D0.value(int(binary[4])) # LSB
    
    # 2. Return the mapped value for the Servo
    return int((x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min)
    
while True:
    adc_value = adc.read()
    
    # Use the raw adc_value (0-511) for smooth mapping
    servo_duty = map_and_display(adc_value, 0, 511, 25, 125)
    pwm0.duty(servo_duty)
    
    volts = (3.3 * adc_value) / 511
    print("Voltage: {:0.2f}V | Raw: {:3d} | Duty: {:3d}".format(volts, adc_value, servo_duty), end ="\r")