import sys
import serial
import pygame
from pygame.locals import *
from OpenGL.GL import *
from OpenGL.GLU import *

# CONFIGURATION: Change this to match your actual Mac Serial Port!
SERIAL_PORT = '/dev/cu.usbmodem1101' 
BAUD_RATE = 115200

# Initialize Serial Connection
try:
    ser = serial.Serial(SERIAL_PORT, BAUD_RATE, timeout=1)
except Exception as e:
    print(f"Error opening serial port: {e}")
    sys.exit()

# Define vertices and edges for a simple 3D rectangular box
vertices = (
    (1, -0.2, -1), (1, 0.2, -1), (-1, 0.2, -1), (-1, -0.2, -1),
    (1, -0.2, 1), (1, 0.2, 1), (-1, -0.2, 1), (-1, 0.2, 1)
)
edges = ((0,1), (0,3), (0,4), (2,1), (2,3), (2,7), (6,3), (6,4), (6,7), (5,1), (5,4), (5,7))
surfaces = ((0,1,2,3), (3,2,7,6), (6,7,5,4), (4,5,1,0), (1,5,7,2), (4,0,3,6))
colors = ((1,0,0), (0,1,0), (0,0,1), (1,1,0), (1,0,1), (0,1,1))

def draw_cube():
    glBegin(GL_QUADS)
    for i, surface in enumerate(surfaces):
        glColor3fv(colors[i])
        for vertex in surface:
            glVertex3fv(vertices[vertex])
    glEnd()
    
    glColor3fv((0,0,0)) # Black outlines
    glBegin(GL_LINES)
    for edge in edges:
        for vertex in edge:
            glVertex3fv(vertices[vertex])
    glEnd()

def main():
    pygame.init()
    display = (800, 600)
    pygame.display.set_mode(display, DOUBLEBUF | OPENGL)
    pygame.display.set_caption("XIAO ESP32-C3 3D IMU Visualizer")

    gluPerspective(45, (display[0]/display[1]), 0.1, 50.0)
    glTranslatef(0.0, 0.0, -5.0)

    pitch, roll = 0.0, 0.0

    while True:
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                ser.close()
                pygame.quit()
                sys.exit()

        # Read serial data from ESP32
        if ser.in_waiting > 0:
            try:
                line = ser.readline().decode('utf-8').strip()
                data = line.split(',')
                if len(data) == 2:
                    pitch = float(data[0])
                    roll = float(data[1])
            except:
                pass # Ignore corrupt/incomplete serial packets

        # Rendering
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
        glEnable(GL_DEPTH_TEST)
        
        glPushMatrix()
        # Apply rotations matching the IMU physics
        glRotatef(pitch, 1, 0, 0)
        glRotatef(roll, 0, 0, 1)
        
        draw_cube()
        glPopMatrix()

        pygame.display.flip()
        pygame.time.wait(10)

if __name__ == '__main__':
    main()