import machine
import time
import ssd1306

# ==========================================
# HARDWARE CONFIGURATION (ESP32)
# ==========================================

# 1. Analog Input: Slider Potentiometer (Throttle/Position)
pot_pin = machine.ADC(machine.Pin(34))
pot_pin.atten(machine.ADC.ATTN_11DB)

# 2. Digital Inputs: Preset Switches
sw_min = machine.Pin(25, machine.Pin.IN, machine.Pin.PULL_UP) # Force to 0 deg
sw_max = machine.Pin(26, machine.Pin.IN, machine.Pin.PULL_UP) # Force to 180 deg

# 3. Output: Servo Motor (PWM)
# In Wokwi/MicroPython, Servos usually operate at 50Hz
# Duty cycle for 180 deg servos is roughly 40 (0 deg) to 115 (180 deg)
servo = machine.PWM(machine.Pin(12), freq=50)

# 4. OLED Display (I2C)
i2c = machine.I2C(0, scl=machine.Pin(22), sda=machine.Pin(21))
oled = ssd1306.SSD1306_I2C(128, 64, i2c)

# ==========================================
# LOGIC FUNCTIONS
# ==========================================

def get_pot_angle(samples=10):
    """
    Reads slider and maps 0-4095 to 0-180 degrees.
    Includes averaging to handle signal noise (PDF Page 11).
    """
    total = 0
    for _ in range(samples):
        total += pot_pin.read()
        time.sleep_ms(1)

    avg_raw = total / samples
    # Mapping formula: (raw / 4095) * 180
    return int((avg_raw / 4095) * 180)

def angle_to_duty(angle):
    """
    Converts 0-180 degrees to PWM duty cycle for standard servos.
    Typically: 0.5ms (duty ~26) to 2.4ms (duty ~123) at 50Hz.
    """
    # Standard MicroPython PWM duty is 0-1023
    # Duty = ((angle/180) * (max_duty - min_duty)) + min_duty
    return int(((angle / 180) * (123 - 26)) + 26)

def update_display(angle, source):
    """
    Visual feedback on the OLED.
    """
    oled.fill(0)
    oled.text("SERVO MONITOR", 10, 0)
    oled.text("-" * 15, 5, 10)

    oled.text("SOURCE:", 0, 25)
    oled.text(source, 60, 25)

    oled.text("ANGLE:", 0, 40)
    oled.text(f"{angle} deg", 60, 40)

    # Simple Visual Gauge
    oled.rect(10, 55, 108, 6, 1)
    fill_width = int((angle / 180) * 108)
    oled.fill_rect(10, 55, fill_width, 6, 1)

    oled.show()

# ==========================================
# MAIN CONTROL LOOP
# ==========================================

def main():
    print("--- Servo System Starting (Wokwi Compatible) ---")

    while True:
        try:
            # 1. SENSE
            pot_angle = get_pot_angle()
            min_btn = sw_min.value()
            max_btn = sw_max.value()

            # 2. THINK (MIMO Priority Logic)
            # If button is pressed, it overrides the potentiometer
            target_angle = pot_angle
            source = "SLIDER"

            if not min_btn: # Active Low
                target_angle = 0
                source = "PRESET MIN"
            elif not max_btn:
                target_angle = 180
                source = "PRESET MAX"

            # 3. ACT
            duty = angle_to_duty(target_angle)
            servo.duty(duty)

            # 4. MONITOR
            update_display(target_angle, source)

            time.sleep(0.05)

        except Exception as e:
            print("Error:", e)
            break

if __name__ == "__main__":
    main()