import machine
import time
from machine import Pin, I2C, PWM
import ssd1306
import math

# Inicjalizacja ekranu OLED
WIDTH = 128
HEIGHT = 64
i2c = I2C(-1, scl=Pin(22), sda=Pin(21))
oled = ssd1306.SSD1306_I2C(WIDTH,HEIGHT,i2c)

# Inicjalizacja serwomechanizmu SG90
sg90 = PWM(Pin(13, mode=Pin.OUT))
sg90.freq(50)
sg90.duty(20)

# Inicjalizacja czujnika odległości HCSR04
class HCSR04:
    def __init__(self, trigger_pin, echo_pin, echo_timeout_us=500*2*30):
        self.echo_timeout_us = echo_timeout_us
        # Init trigger pin (out)
        self.trigger = Pin(trigger_pin, mode=Pin.OUT, pull=None)
        self.trigger.value(0)
        # Init echo pin (in)
        self.echo = Pin(echo_pin, mode=Pin.IN, pull=None)

    def _send_pulse_and_wait(self):
        self.trigger.value(0) # Stabilize the sensor
        time.sleep_us(5)
        self.trigger.value(1)
        # Send a 10us pulse.
        time.sleep_us(10)
        self.trigger.value(0)
        try:
            pulse_time = machine.time_pulse_us(self.echo, 1, self.echo_timeout_us)
            return pulse_time
        except OSError as ex:
            if ex.args[0] == 110: # 110 = ETIMEDOUT
                raise OSError('Out of range')
            raise ex

    def distance_mm(self):
        pulse_time = self._send_pulse_and_wait()
        mm = pulse_time * 100 // 582
        return mm

    def distance_cm(self):
        pulse_time = self._send_pulse_and_wait()
        cms = (pulse_time / 2) / 29.1
        return cms

sensor = HCSR04(trigger_pin=14, echo_pin=12, echo_timeout_us=10000)

while True:
    for angle in range(20, 130, 5):
        if angle == 20:
            oled.fill(0)
        sg90.duty(angle)
        suma = 0
        for i in range(10):
            suma += sensor.distance_cm()
            time.sleep(0.1)
        suma = suma / 10
        
        # Przekształć odległość na współrzędną na ekranie
        angle_rad = math.radians(180-(18/11)*angle+20)
        x = WIDTH // 2 + int(suma * math.cos(angle_rad))
        y = 0 + int(suma * math.sin(angle_rad))

        # Rysuj linię na ekranie w odpowiednim kolorze
        if suma > 100:
            oled.line(WIDTH // 2, 0, x, y, 1)  # Rysuj linię białą
        else:
            oled.line(WIDTH // 2, 0, x, y, 2)  # Rysuj linię czerwoną

        oled.show()  # Wyświetl obraz na ekranie
        time.sleep(0.1)  # Odczekaj krótką chwilę przed narysowaniem kolejnej linii