import pygame
from pygame.locals import *
from OpenGL.GL import *
from OpenGL.GLU import *
import pybullet as p
import pybullet_data
import numpy as np
import sys

# Initialize Pygame and OpenGL
def init_pygame_opengl():
    pygame.init()
    display = (800, 600)
    pygame.display.set_mode(display, DOUBLEBUF | OPENGL)
    gluPerspective(45, (display[0] / display[1]), 0.1, 100.0)
    glEnable(GL_DEPTH_TEST)

# Initialize PyBullet
def init_pybullet():
    physicsClient = p.connect(p.DIRECT)  # Use p.GUI for graphical version
    p.setGravity(0, -9.81, 0)
    p.setAdditionalSearchPath(pybullet_data.getDataPath())
    
    # Load ground plane
    planeId = p.loadURDF("plane.urdf")
    
    return physicsClient

# Create a simple soft body using multiple spheres connected by constraints
def create_soft_body():
    # Main body position
    main_body_position = [0, 1, 0]
    main_body_radius = 0.5
    main_body_mass = 1.0

    # Create main body (a simple sphere)
    main_body_col = p.createCollisionShape(p.GEOM_SPHERE, radius=main_body_radius)
    main_body_vis = p.createVisualShape(p.GEOM_SPHERE, radius=main_body_radius, rgbaColor=[1, 0, 0, 1])
    main_body = p.createMultiBody(main_body_mass, main_body_col, main_body_vis, main_body_position)

    # Create two "boobs" as smaller spheres attached to the main body
    boob_radius = 0.2
    boob_mass = 0.3
    boob_distance = 0.6  # Distance from the main body center
    
    # Left boob
    left_boob_pos = [main_body_position[0] - boob_distance, main_body_position[1] + 0.2, main_body_position[2]]
    left_boob_col = p.createCollisionShape(p.GEOM_SPHERE, radius=boob_radius)
    left_boob_vis = p.createVisualShape(p.GEOM_SPHERE, radius=boob_radius, rgbaColor=[0, 0, 1, 1])
    left_boob = p.createMultiBody(boob_mass, left_boob_col, left_boob_vis, left_boob_pos)

    # Right boob
    right_boob_pos = [main_body_position[0] + boob_distance, main_body_position[1] + 0.2, main_body_position[2]]
    right_boob_col = p.createCollisionShape(p.GEOM_SPHERE, radius=boob_radius)
    right_boob_vis = p.createVisualShape(p.GEOM_SPHERE, radius=boob_radius, rgbaColor=[0, 0, 1, 1])
    right_boob = p.createMultiBody(boob_mass, right_boob_col, right_boob_vis, right_boob_pos)

    # Create constraints (invisible springs) between main body and boobs
    stiffness = 0.5
    damping = 0.1

    # Left constraint
    p.createConstraint(
        parentBodyUniqueId=main_body,
        parentLinkIndex=-1,
        childBodyUniqueId=left_boob,
        childLinkIndex=-1,
        jointType=p.JOINT_SPRING,
        jointAxis=[0, 0, 0],
        parentFramePosition=[-boob_distance, 0.2, 0],
        childFramePosition=[0, 0, 0]
    )
    # Note: PyBullet does not have built-in spring joints; alternative approaches needed for spring-like behavior

    # Right constraint
    p.createConstraint(
        parentBodyUniqueId=main_body,
        parentLinkIndex=-1,
        childBodyUniqueId=right_boob,
        childLinkIndex=-1,
        jointType=p.JOINT_SPHERICAL,
        jointAxis=[0, 0, 0],
        parentFramePosition=[boob_distance, 0.2, 0],
        childFramePosition=[0, 0, 0]
    )

    return main_body, left_boob, right_boob

# Draw a sphere using OpenGL
def draw_sphere(position, radius=0.1, color=[1, 0, 0, 1]):
    glPushMatrix()
    glTranslatef(*position)
    glColor4f(*color)
    quad = gluNewQuadric()
    gluSphere(quad, radius, 32, 32)
    gluDeleteQuadric(quad)
    glPopMatrix()

# Convert PyBullet position to OpenGL coordinates
def get_body_position(body_id):
    pos, orn = p.getBasePositionAndOrientation(body_id)
    return pos

# Main loop
def main():
    init_pygame_opengl()
    physicsClient = init_pybullet()
    main_body, left_boob, right_boob = create_soft_body()
    
    clock = pygame.time.Clock()
    rot_y = 0

    try:
        while True:
            for event in pygame.event.get():
                if event.type == pygame.QUIT:
                    raise KeyboardInterrupt

            # Step simulation
            p.stepSimulation()

            # Clear buffers
            glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)

            # Reset transformation
            glLoadIdentity()
            gluPerspective(45, (800 / 600), 0.1, 100.0)
            glTranslatef(0.0, -1.5, -10)

            # Rotate view for better visualization
            glRotatef(rot_y, 0, 1, 0)
            rot_y += 0.1  # Rotate slightly each frame

            # Get positions
            main_pos = get_body_position(main_body)
            left_pos = get_body_position(left_boob)
            right_pos = get_body_position(right_boob)

            # Draw main body
            draw_sphere(main_pos, radius=0.5, color=[1, 0, 0, 1])

            # Draw boobs
            draw_sphere(left_pos, radius=0.2, color=[0, 0, 1, 1])
            draw_sphere(right_pos, radius=0.2, color=[0, 0, 1, 1])

            # Swap buffers
            pygame.display.flip()
            clock.tick(60)

    except KeyboardInterrupt:
        print("Exiting...")
    finally:
        p.disconnect()
        pygame.quit()
        sys.exit()

if __name__ == "__main__":
    main()

analyse for errors