"""
Demonstrating oled and machine.Timer
"""

from machine import Pin, PWM, I2C, ADC, Timer
from hcsr04 import HCSR04
import utime as time
import urandom as random
from ssd1306 import SSD1306_I2C

# --- Servo Setup (Assumes SG90-style 360° Servos) ---
left_pwm = PWM(Pin(20))
right_pwm = PWM(Pin(21))

# 50 Hz PWM (20 ms period)
left_pwm.freq(50)
right_pwm.freq(50)

left_distance = HCSR04(trigger_pin=19, echo_pin=18, echo_timeout_us=10000)
right_distance = HCSR04(trigger_pin=17, echo_pin=16, echo_timeout_us=10000)

light = ADC(2)
"""
Observations:
 - sensor covered:  AO ~32k DO 1
 - sensor ambient:  AO ~15k DO 1
 - mobile phone:    AO ~11k DO 0
 - torch on LDR:    AO ~ 5k DO 0
"""

# --- Initialise OLED ---
i2c = I2C(0, scl=Pin(9), sda=Pin(8))
oled = SSD1306_I2C(128, 32, i2c)

def display_dict(oled, data):
    oled.fill(0)  # Clear display

    line_height = 8
    max_lines = oled.height // line_height  # 32 // 8 = 4

    for i, (key, value) in enumerate(data.items()):
        if i >= max_lines:
            break  # Avoid writing off the screen
        line_text = f"{key[:8]}:{value}"
        oled.text(line_text, 0, i * line_height)

    oled.show()


# Constants for servo control
STOP = 4915     # 1.5 ms pulse
FORWARD = 8192  # 1.7 ms pulse
REVERSE = 1628  # 1.3 ms pulse

def drive_servos(left_speed, right_speed):
    left_pwm.duty_u16(left_speed)
    right_pwm.duty_u16(right_speed)

def stop():
    drive_servos(STOP, STOP)

# --- Sensor Pins 
def read_left_distance():
    return left_distance.distance_cm()

def read_right_distance():
    return right_distance.distance_cm()

def read_light():
    return light.read_u16()

# --- State (dummy data) ---
sensor_data = {
    'left_distance': 0,
    'right_distance': 0,
    'light': 0
}

flags = {
    'distance_ready': False,
    'light_ready': False}

# --- Timer Callbacks ---
def distance_callback(timer):
    sensor_data['left_distance'] = read_left_distance()
    sensor_data['right_distance'] = read_right_distance()
    flags['distance_ready'] = True

def light_callback(timer):
    sensor_data['light'] = read_light()
    flags['light_ready'] = True


# --- Timer Setup ---
distance_timer = Timer()
light_timer = Timer()

# reads distance sensors every 200 ms
distance_timer.init(period=200, mode=Timer.PERIODIC, callback=distance_callback)

# read the light sensor every 500 ms
light_timer.init(period=500, mode=Timer.PERIODIC, callback=light_callback)


# --- Main Loop ---

while True:
    now = time.time()
    print(now)
    print(sensor_data)
    display_dict(oled, sensor_data)
 
    time.sleep(1)  # Short delay to avoid CPU spinning