BoxedMan/main.py

"""Main application entry point for BoxedMan: integrates Vision, Physics, Agent, and Display."""

import sys
import os
import argparse
import pygame
import cv2
import numpy as np

from src.config import Settings, CalibrationData
from src.vision import VisionPipeline
from src.physics import PhysicsWorld
from src.pegman import PegMan
from src.calibration import Calibrator


def main():
    parser = argparse.ArgumentParser(description="BoxedMan - Interactive Projector Installation")
    parser.add_argument("--calibrate", action="store_true", help="Run 4-point perspective camera calibration")
    parser.add_argument("--cam", type=int, default=None, help="Camera index override (e.g. 0, 1, 2)")
    parser.add_argument("--fullscreen", action="store_true", help="Launch in fullscreen mode on projector")
    parser.add_argument("--sim", action="store_true", help="Simulation mode without webcam (use mouse to draw obstacles)")
    args = parser.parse_args()

    settings = Settings.load()
    if args.cam is not None:
        settings.camera_index = args.cam
    if args.fullscreen:
        settings.fullscreen = True

    # 1. Calibration mode if requested or no calibration file exists
    calibration = CalibrationData.load()
    if args.calibrate or (calibration is None and not args.sim):
        print("[BoxedMan] Starting calibration utility...")
        calibrator = Calibrator(settings)
        calibration = calibrator.run()

    # 2. Initialize Vision Pipeline (unless running pure simulation)
    vision: VisionPipeline = None
    if not args.sim:
        vision = VisionPipeline(settings, calibration)
        if not vision.start_camera():
            print("[BoxedMan] Could not start camera. Falling back to mouse simulation mode (--sim).")
            args.sim = True

    # 3. Initialize Pygame Display
    pygame.init()
    flags = pygame.FULLSCREEN if settings.fullscreen else 0
    screen = pygame.display.set_mode((settings.projector_width, settings.projector_height), flags)
    pygame.display.set_caption("BoxedMan")
    clock = pygame.time.Clock()

    # 4. Initialize Physics & Peg Man
    physics = PhysicsWorld(settings)
    pegman = PegMan(physics, settings, start_pos=(settings.projector_width // 2, settings.projector_height - 100))

    # Mouse simulation obstacles
    mouse_polygons = []
    drawing_poly = []

    # State variables
    show_debug_vision = False
    running = True

    print("\n--- BOXEDMAN KØRER ---")
    print("Taster:")
    print("  'b' : Tag nyt baggrundsreferencebillede (uden skygger/hænder på lærredet)")
    print("  'd' : Vis / skjul vision debug overlay")
    print("  'c' : Genkalibrér 4 hjørner")
    print("  'r' : Nulstil Peg Mans position")
    print("  'ESC' eller luk vindue for at afslutte.")
    if args.sim:
        print("SIMULATIONSTILSTAND AKTIV: Venstreklik og træk musen for at tegne forhindringer, Højreklik for at rydde.")
    print("----------------------\n")

    while running:
        dt = clock.tick(settings.fps) / 1000.0
        dt = min(dt, 0.05)  # Prevent physics explosion on lag spike

        # --- Event Handling ---
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                running = False
            elif event.type == pygame.KEYDOWN:
                if event.key == pygame.K_ESCAPE:
                    running = False
                elif event.key == pygame.K_b and vision:
                    vision.capture_reference_background()
                elif event.key == pygame.K_d:
                    show_debug_vision = not show_debug_vision
                elif event.key == pygame.K_r:
                    pegman.body.position = (settings.projector_width // 2, settings.projector_height - 100)
                    pegman.body.velocity = (0, 0)
                elif event.key == pygame.K_c and vision:
                    vision.stop_camera()
                    calibrator = Calibrator(settings)
                    new_calib = calibrator.run()
                    if new_calib:
                        calibration = new_calib
                        vision.calibration = calibration
                        vision._update_homography()
                    vision.start_camera()

            # Mouse obstacle drawing for simulation mode
            elif args.sim:
                if event.type == pygame.MOUSEBUTTONDOWN and event.button == 1:
                    drawing_poly = [event.pos]
                elif event.type == pygame.MOUSEMOTION and event.buttons[0]:
                    if drawing_poly and np.hypot(event.pos[0] - drawing_poly[-1][0], event.pos[1] - drawing_poly[-1][1]) > 10:
                        drawing_poly.append(event.pos)
                elif event.type == pygame.MOUSEBUTTONUP and event.button == 1:
                    if len(drawing_poly) >= 3:
                        mouse_polygons.append(np.array(drawing_poly, dtype=np.int32))
                    drawing_poly = []
                elif event.type == pygame.MOUSEBUTTONDOWN and event.button == 3:
                    mouse_polygons.clear()

        # --- Vision Processing ---
        detected_polygons = []
        debug_mask = None
        if vision:
            raw_frame = vision.read_raw_frame()
            if raw_frame is not None:
                warped = vision.warp_frame(raw_frame)
                detected_polygons, debug_mask = vision.extract_shadow_polygons(warped)

        if args.sim:
            detected_polygons = mouse_polygons

        # Update dynamic physics colliders based on detected shapes
        physics.update_shadow_geometry(detected_polygons)

        # Step physics and agent behavior
        pegman.update(dt)
        physics.step(dt)

        # --- Rendering ---
        # 1. Clear with clean white projector canvas
        screen.fill((255, 255, 255))

        # 2. Render borders / bounding box
        pygame.draw.rect(screen, (30, 30, 30), (0, 0, settings.projector_width, settings.projector_height), 4)

        # 3. Draw shadow outlines or solids (subtle styling on the white projector)
        for poly in detected_polygons:
            if len(poly) >= 3:
                pts = [(int(p[0]), int(p[1])) for p in poly]
                # Light neutral fill / outline
                pygame.draw.polygon(screen, (230, 230, 230), pts)
                pygame.draw.lines(screen, (100, 100, 100), True, pts, 2)

        # Draw currently drawn sim line
        if drawing_poly and len(drawing_poly) > 1:
            pygame.draw.lines(screen, (150, 150, 150), False, drawing_poly, 2)

        # 4. Render Peg Man
        pegman.render(screen)

        # 5. Optional Debug Overlay for Vision
        if show_debug_vision and debug_mask is not None:
            mask_rgb = cv2.cvtColor(debug_mask, cv2.COLOR_GRAY2RGB)
            mask_surf = pygame.surfarray.make_surface(np.transpose(mask_rgb, (1, 0, 2)))
            mask_surf.set_alpha(128)
            screen.blit(mask_surf, (0, 0))

        pygame.display.flip()

    if vision:
        vision.stop_camera()
    pygame.quit()
    sys.exit(0)


if __name__ == "__main__":
    main()