import time, math, gc
import board, busio, displayio, terminalio
from fourwire import FourWire
from adafruit_ili9341 import ILI9341
from adafruit_display_text import label
from adafruit_display_shapes.rect import Rect
from adafruit_mpu6050 import MPU6050

# =======================================================
# CONFIG
# =======================================================
HOURGLASS_DURATION = 10.0
FRAME_INTERVAL     = 0.016
IMU_INTERVAL       = 0.2
PITCH_ENTER_THRESH = -50.0  # enter hourglass
PITCH_EXIT_THRESH  = -20.0  # back to clock
ENTER_DEBOUNCE_READS = 2
CLOCK_X_NUDGE = 10

RED = 0xFF0000
BLACK = 0x000000
CLOCK_W, CLOCK_H = 320, 240
HOUR_W,  HOUR_H  = 320, 240

# =======================================================
# DISPLAY SETUP
# =======================================================
displayio.release_displays()
spi = busio.SPI(clock=board.GP14, MOSI=board.GP15)
tft_cs, tft_dc, tft_rst = board.GP13, board.GP12, board.GP11
bus = FourWire(spi, command=tft_dc, chip_select=tft_cs, reset=tft_rst)
display = ILI9341(bus, width=CLOCK_W, height=CLOCK_H, rotation=0)

# Clock UI
clock_group = displayio.Group()
display.root_group = clock_group
clock_group.append(Rect(0, 0, CLOCK_W, CLOCK_H, fill=BLACK))
clock_label = label.Label(
    terminalio.FONT, text="09:41", color=RED, scale=10,
    anchor_point=(0.5, 0.5),
    anchored_position=(CLOCK_W // 2 + CLOCK_X_NUDGE, CLOCK_H // 2)
)
clock_group.append(clock_label)

# Hourglass UI
hourglass_group = displayio.Group()
hg_bitmap = displayio.Bitmap(HOUR_W, HOUR_H, 2)
hg_palette = displayio.Palette(2)
hg_palette[0] = BLACK
hg_palette[1] = RED
hg_tile = displayio.TileGrid(hg_bitmap, pixel_shader=hg_palette)
hourglass_group.append(hg_tile)

fill_rect = None

def hg_refill_fast():
    """Instant fill using Rect, and sync it into bitmap in one step."""
    global fill_rect
    if fill_rect is not None and fill_rect in hourglass_group:
        hourglass_group.remove(fill_rect)
    fill_rect = Rect(0, 0, HOUR_W, HOUR_H, fill=RED)
    hourglass_group.append(fill_rect)
    hg_bitmap.fill(1)
    gc.collect()

def hg_switch_to_bitmap():
    """Remove rect so bitmap underneath is now visible."""
    global fill_rect
    if fill_rect is not None and fill_rect in hourglass_group:
        hourglass_group.remove(fill_rect)
        fill_rect = None

def hg_clear_columns(prev_cols, new_cols):
    """Smooth continuous depletion left→right."""
    if new_cols <= prev_cols:
        return
    if new_cols > HOUR_W:
        new_cols = HOUR_W
    # flipped direction: left→right
    for x in range(prev_cols, new_cols):
        if x >= HOUR_W:
            break
        for y in range(HOUR_H):
            hg_bitmap[x, y] = 0

# =======================================================
# IMU SETUP (MPU6050)
# =======================================================
i2c = busio.I2C(board.GP17, board.GP16)
mpu = MPU6050(i2c)

def read_orientation():
    """Return roll, pitch, yaw (deg) computed from accelerometer only."""
    try:
        ax, ay, az = mpu.acceleration
        roll = math.degrees(math.atan2(ay, az))
        pitch = math.degrees(math.atan2(-ax, math.sqrt(ay * ay + az * az)))
        yaw = math.degrees(math.atan2(ay, ax))  # pseudo yaw
        return roll, pitch, yaw
    except Exception:
        return None, None, None

# =======================================================
# STATE
# =======================================================
mode = "clock"
start_hour, start_minute = 9, 41
manual_time_start = time.monotonic()
print("System initialized — global reference active.")

last_imu_read = 0
enter_consec = 0
hourglass_start = 0
cleared_cols = 0
can_enter = True
can_exit = False
bitmap_visible = False

# =======================================================
# MAIN LOOP
# =======================================================
while True:
    now = time.monotonic()

    if mode == "clock":
        if now - last_imu_read > IMU_INTERVAL:
            roll, pitch, yaw = read_orientation()
            if pitch is not None:
                print("Roll: {:6.1f}°, Pitch: {:6.1f}°, Yaw: {:6.1f}°".format(roll, pitch, yaw))
                if can_enter and pitch <= PITCH_ENTER_THRESH:
                    enter_consec += 1
                else:
                    enter_consec = 0
                if can_enter and enter_consec >= ENTER_DEBOUNCE_READS:
                    mode = "hourglass"
                    display.root_group = hourglass_group
                    hg_refill_fast()
                    bitmap_visible = False
                    cleared_cols = 0
                    hourglass_start = now
                    can_enter = False
                    can_exit = True
                    print("→ Hourglass started (Pitch {:.1f}°)".format(pitch))
            last_imu_read = now

        # update clock display
        mins = int((now - manual_time_start) / 60)
        h = (start_hour + (start_minute + mins) // 60) % 24
        m = (start_minute + mins) % 60
        clock_label.text = f"{h:02d}:{m:02d}"

    elif mode == "hourglass":
        elapsed = now - hourglass_start

        if not bitmap_visible and elapsed > 0.3:
            hg_switch_to_bitmap()
            bitmap_visible = True

        if elapsed >= HOURGLASS_DURATION:
            hg_clear_columns(cleared_cols, HOUR_W)
            cleared_cols = HOUR_W
            roll, pitch, yaw = read_orientation()
            if pitch is not None:
                print("[Hourglass] Roll: {:6.1f}°, Pitch: {:6.1f}°, Yaw: {:6.1f}°".format(roll, pitch, yaw))
                if can_exit and pitch >= PITCH_EXIT_THRESH:
                    display.root_group = clock_group
                    mode = "clock"
                    can_enter = True
                    can_exit = False
                    print("→ Back to clock (Pitch {:.1f}°)".format(pitch))
        else:
            if bitmap_visible:
                target_cols = int((elapsed / HOURGLASS_DURATION) * HOUR_W)
                if target_cols > cleared_cols:
                    hg_clear_columns(cleared_cols, target_cols)
                    cleared_cols = target_cols

    time.sleep(FRAME_INTERVAL)