# Ian Binstock
# 6/12/2026
# Merged cardemo.py and demotrack.py, added checkpoint and slowdown features

import pygame, sys, math
from pygame.locals import *

SCREEN_W = 1000
SCREEN_H = 600
MARGIN = 100
SPEED = 8
EASE = 0.75
TURN_SPEED = 4.5

RECT_W = 60
RECT_H = 34

# Helper function
def speedcap_circle(x1, y1, x2, y2, speed=8, ease=1.0):
    dx = x2 - x1
    dy = y2 - y1
    dist = math.sqrt(dx * dx + dy * dy)

    if dist > speed:
        # Accurate tracking using normalization and math
        x1 += (dx / dist) * speed * ease
        y1 += (dy / dist) * speed * ease
    else:
        # Snap to target
        x1 = x2
        y1 = y2

    return [x1, y1]

def main():

    # Basic set up
    pygame.init()
    screen = pygame.display.set_mode((SCREEN_W, SCREEN_H))
    pygame.display.set_caption("Car driving maze")

    clock = pygame.time.Clock()

    # This is our car
    base_surf = pygame.Surface((RECT_W, RECT_H), pygame.SRCALPHA)
    base_surf.fill("red")
    pygame.draw.rect(base_surf, "lightblue", (RECT_W - 16, RECT_H // 8, 12, RECT_H * 3 // 4))

    # Variables used for tracking
    # Start at (50,50)
    target = [50.0, 50.0]
    obj_x = 50.0
    obj_y = 50.0

    angle = 0.0

    # Load the maze
    myfile = open("track.txt", "r")

    # Get the grid size
    toks = myfile.readline().split()
    r = int(toks[0])
    c = int(toks[1])

    # Set up the size of the grid squares
    sqsize = [(SCREEN_W)//c, (SCREEN_H)//r]
    width = sqsize[0]*c
    height = sqsize[1]*r

    # Store the board here
    board = []
    check = 0

    # Read each line and add it to the board
    for i in range(r):
        tmp = myfile.readline().strip()
        board.append(tmp)

    # Close file
    myfile.close()
        
    # Game loop
    while True:

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

        screen.fill("white")

        ## THIS IS OUR MAZE CODE
        # Find starting location to center
        startx = (SCREEN_W - width)//2
        starty = (SCREEN_H - height)//2

        # Slow down flag
        SLOW = False

        # Loop through the input board indexes
        for i in range(r):
            for j in range(c):

                # Calculate the corresponding x y coordinates
                x = startx + j*sqsize[0]
                y = starty + i*sqsize[1]

                # Create square object
                cell_rect = pygame.Rect(x, y, sqsize[0], sqsize[1])

                # Color square
                if cell_rect.collidepoint((obj_x, obj_y)):
                    # If we are on a checkpoint...
                    if board[i][j] != 'X' and board[i][j]!= '_':
                        # ...and that checkpoint is the next in line
                        if int(board[i][j])%4 == (check+1)%4:
                            # Then increase Checkpoint counter by 1
                            check += 1
                            print(check)
                    if board[i][j] != 'X':
                        # slow down
                        SLOW = True
                    else:
                        # speed up
                        SLOW = False
                    color = "red"
                elif board[i][j] == 'X':
                    color = "green3"
                elif board[i][j] == '_':
                    color = "gray20"
                else:
                    color = "fuchsia"

                # Draw square
                pygame.draw.rect(screen, color, cell_rect, 0)

        ## THIS IS OUR CAR CODE
        # Target trails the mouse via speed cap
        mouse_pos = pygame.mouse.get_pos()
        new_speed = SPEED
        if not SLOW:
            new_speed = SPEED/4
        target = speedcap_circle(target[0], target[1], mouse_pos[0], mouse_pos[1], new_speed, EASE) 

        # Object trails the target with smoothing
        obj_x += (target[0] - obj_x) * 0.35
        obj_y += (target[1] - obj_y) * 0.35 
        dx = target[0] - obj_x
        dy = target[1] - obj_y

        # If moving, change angle as needed
        if abs(dx) > 0.5 or abs(dy) > 0.5:
            angle = math.degrees(math.atan2(-dy, dx))

        # Rotating alters the dimensions, re-center car object
        rotated_surf = pygame.transform.rotate(base_surf, angle)
        rotated_rect = rotated_surf.get_rect(center=(int(obj_x), int(obj_y)))   
    
        screen.blit(rotated_surf, rotated_rect) 
        pygame.display.update()
        clock.tick(60)

# Run it
main()