from machine import Pin, PWM, I2C
import time
import framebuf

# SSD1306 driver using framebuf
class SSD1306_I2C(framebuf.FrameBuffer):
    def __init__(self, width, height, i2c, addr=0x3C):
        self.width = width
        self.height = height
        self.i2c = i2c
        self.addr = addr
        self.buffer = bytearray(self.width * self.height // 8)
        super().__init__(self.buffer, self.width, self.height, framebuf.MONO_VLSB)
        self.init_display()

    def init_display(self):
        for cmd in (
            0xAE, 0xA4, 0xD5, 0xF0, 0xA8, 0x3F, 0xD3, 0x00,
            0 | 0x0, 0x8D, 0x14, 0x20, 0x00, 0x21, 0, self.width - 1,
            0x22, 0, self.height // 8 - 1, 0xa0 | 0x1, 0xc8, 0xda, 0x12,
            0x81, 0xCF, 0xd9, 0xF1, 0xDB, 0x40, 0xA6, 0xD6, 0xA6, 0xAF):
            self.write_cmd(cmd)

    def write_cmd(self, cmd):
        self.i2c.writeto(self.addr, bytearray([0x80, cmd]))

    def show(self):
        self.write_cmd(0x21)
        self.write_cmd(0)
        self.write_cmd(self.width - 1)
        self.write_cmd(0x22)
        self.write_cmd(0)
        self.write_cmd(self.height // 8 - 1)
        self.i2c.writeto(self.addr, bytearray([0x40]) + self.buffer)

# Initialize pins and components
trigPin = Pin(5, Pin.OUT)
echoPin = Pin(18, Pin.IN)
servoPin = Pin(13, Pin.OUT)
servo = PWM(servoPin, freq=50)  # 50Hz for servo control

# I2C setup for OLED (SDA=21, SCL=22 for ESP32)
i2c = I2C(0, scl=Pin(22), sda=Pin(21), freq=200000)
oled = SSD1306_I2C(128, 64, i2c)  # 128x64 OLED display

def set_servo_angle(angle):
    minDuty, maxDuty = 40, 115
    duty = int((angle / 180) * (maxDuty - minDuty) + minDuty)
    servo.duty(duty)

def calculate_distance():
    trigPin.off()
    time.sleep_us(2)
    trigPin.on()
    time.sleep_us(10)
    trigPin.off()
    while echoPin.value() == 0:
        pass
    start = time.ticks_us()
    while echoPin.value() == 1:
        pass
    end = time.ticks_us()
    duration = time.ticks_diff(end, start)
    return (duration * 0.0343) / 2

oled.fill(0)  # Clear OLED display
oled.show()

while True:
    # Sweep from 15 to 165 degrees
    for angle in range(15, 166, 1):
        set_servo_angle(angle)
        time.sleep_ms(30)
        distance = calculate_distance()
        
        # Display angle and distance on OLED
        oled.fill(0)  # Clear the display
        oled.text('Angle: {} deg'.format(angle), 0, 0)
        oled.text('Distance: {:.2f} cm'.format(distance), 0, 20)
        oled.show()  # Update display
        time.sleep_ms(30)

    # Sweep back from 165 to 15 degrees
    for angle in range(165, 14, -1):
        set_servo_angle(angle)
        time.sleep_ms(30)
        distance = calculate_distance()
        
        # Display angle and distance on OLED
        oled.fill(0)  # Clear the display
        oled.text('Angle: {} deg'.format(angle), 0, 0)
        oled.text('Distance: {:.2f} cm'.format(distance), 0, 20)
        oled.show()  # Update display
        time.sleep_ms(30)