import machine
import ssd1306
import time
import math

# I2C on GP4=SDA, GP5=SCL (hardware I2C bus 0)
i2c = machine.I2C(0,
                  sda=machine.Pin(4),
                  scl=machine.Pin(5),
                  freq=400000)

# Scan and confirm devices
found = i2c.scan()
print("I2C devices:", [hex(x) for x in found])
# Should print: ['0x3c', '0x68']

# OLED 128x64
oled = ssd1306.SSD1306_I2C(128, 64, i2c, addr=0x3C)

# MPU-6050 registers
MPU      = 0x68
PWR_REG  = 0x6B
ACCEL_REG = 0x3B

def mpu_wake():
    i2c.writeto_mem(MPU, PWR_REG, b'\x00')

def read_accel():
    d = i2c.readfrom_mem(MPU, ACCEL_REG, 6)
    def s16(h, l):
        v = (h << 8) | l
        return v - 65536 if v > 32767 else v
    ax = s16(d[0], d[1]) / 16384.0
    ay = s16(d[2], d[3]) / 16384.0
    az = s16(d[4], d[5]) / 16384.0
    return ax, ay, az

# Step detection config
THRESHOLD    = 1.2   # g-force peak
MIN_INTERVAL = 300   # ms between steps

steps        = 0
last_step_ms = 0
above         = False
t0            = time.ticks_ms()

mpu_wake()

def draw(steps, elapsed):
    oled.fill(0)
    # Header bar
    oled.fill_rect(0, 0, 128, 13, 1)
    oled.text("PEDOMETER", 24, 3, 0)
    # Steps
    oled.text("Steps:", 4, 18)
    oled.text(str(steps), 80, 18)
    # Distance
    dist = steps * 0.75
    oled.text("Dist: ", 4, 30)
    if dist < 1000:
        oled.text("{:.1f}m".format(dist), 80, 30)
    else:
        oled.text("{:.2f}km".format(dist / 1000), 72, 30)
    # Time
    m, s = divmod(int(elapsed), 60)
    oled.text("Time: ", 4, 42)
    oled.text("{:02d}:{:02d}".format(m, s), 80, 42)
    # Footer
    oled.hline(0, 55, 128, 1)
    oled.text("SDA:GP4 SCL:GP5", 2, 57)
    oled.show()

print("Pedometer started!")

while True:
    ax, ay, az = read_accel()
    mag = math.sqrt(ax*ax + ay*ay + az*az)
    now = time.ticks_ms()

    if mag > THRESHOLD and not above:
        if time.ticks_diff(now, last_step_ms) > MIN_INTERVAL:
            steps += 1
            last_step_ms = now
            print("Step:", steps, " mag={:.3f}g".format(mag))
        above = True
    elif mag <= THRESHOLD:
        above = False

    elapsed = time.ticks_diff(now, t0) / 1000.0
    draw(steps, elapsed)
    time.sleep_ms(50)