import machine
import ssd1306
import math
import time

# 1. Setup Display (I2C0, GP4=SDA, GP5=SCL)
i2c = machine.I2C(0, sda=machine.Pin(4), scl=machine.Pin(5))
display = ssd1306.SSD1306_I2C(128, 64, i2c)

# 2. Define the Model (Vertices & Edges)
vertices = [
    [-1, -1, 1], [1, -1, 1], [1, 1, 1], [-1, 1, 1],
    [-1, -1, -1], [1, -1, -1], [1, 1, -1], [-1, 1, -1]
]

edges = [
    (0, 1), (1, 2), (2, 3), (3, 0), # Front face
    (4, 5), (5, 6), (6, 7), (7, 4), # Back face
    (0, 4), (1, 5), (2, 6), (3, 7) # Connecting lines
]

# 3. Projection Variables
scale = 40
distance = 4
angle_x = 0
angle_y = 0

while True:
    # STEP 1: Clear the display
    display.fill(0)
    
    # Update rotation angles
    angle_x += 0.1
    angle_y += 0.1
    
    # Temporary list to store projected 2D points
    projected_points = []
    
    # STEP 2 & 3: Transform (Rotate) and Project
    for v in vertices:
        x, y, z = v[0], v[1], v[2]
        
        # Rotation around Y-axis
        nx = x * math.cos(angle_y) - z * math.sin(angle_y)
        nz = x * math.sin(angle_y) + z * math.cos(angle_y)
        x, z = nx, nz
        
        # Rotation around X-axis
        ny = y * math.cos(angle_x) - z * math.sin(angle_x)
        nz = y * math.sin(angle_x) + z * math.cos(angle_x)
        y, z = ny, nz
        
        # Perspective Projection Formula
        factor = scale / (z + distance)
        px = int(64 + x * factor) # Center X: 128/2 = 64
        py = int(32 + y * factor) # Center Y: 64/2 = 32
        
        projected_points.append((px, py))
        
    # STEP 4: Rasterization (Draw the Edges)
    for edge in edges:
        p1 = projected_points[edge[0]]
        p2 = projected_points[edge[1]]
        display.line(p1[0], p1[1], p2[0], p2[1], 1)
        
    # Push data to OLED
    display.show()
    
    # Optimization: Small sleep to prevent CPU overload
    time.sleep(0.01)