from machine import Pin, I2C
import time
import math
import ssd1306

# ESP32 Pin assignment 
i2c = I2C(0, scl=Pin(22), sda=Pin(21), freq=1000000)

oled_width = 128
oled_height = 64
oled = ssd1306.SSD1306_I2C(oled_width, oled_height, i2c)

# World map (1 is a wall, 0 is empty space)
world_map = [
    [1, 1, 1, 1, 1, 1, 1],
    [1, 0, 1, 0, 0, 0, 1],
    [1, 0, 1, 0, 0, 0, 1],
    [1, 0, 0, 0, 0, 0, 1],
    [1, 0, 1, 1, 1, 0, 1],
    [1, 0, 0, 0, 0, 0, 1],
    [1, 1, 1, 1, 1, 1, 1]
]
rays = [0] * 43
old_rays = [1] * 43
# Player's starting position and angle
player_x, player_y = 3.5, 3.5  # Player starts at the center of the map
player_angle = 90  # The player is facing upwards (90 degrees)

# FOV settings
fov = 86  
num_rays = 43 
screen_height = 64 
# Raycasting constants
map_width = len(world_map[0])
map_height = len(world_map)
max_distance = 50 

# Precompute the sin and cos of angles
def precompute_sin_cos():
    global sin_table, cos_table
    sin_table = [math.sin(math.radians(i)) for i in range(360)]
    cos_table = [math.cos(math.radians(i)) for i in range(360)]

# Keep Angle Legal
def normalize_angle(angle):
    return angle % 360

# Cast a single ray and return the distance to the wall
def cast_ray(angle):
    angle = normalize_angle(angle)
    sin_a = sin_table[int(angle)] * 0.25
    cos_a = cos_table[int(angle)] * 0.25

    # Raycasting loop
    for depth in range(1, max_distance):
        ray_x = player_x + cos_a * depth
        ray_y = player_y + sin_a * depth

        map_x = int(ray_x)
        map_y = int(ray_y)

        # Ensure that we're within the world map boundaries
        if map_x < 0 or map_x >= map_width or map_y < 0 or map_y >= map_height:
            break  # Out of bounds, stop the ray

        # Check if the ray hits a wall (1)
        if world_map[map_y][map_x] == 1:
            return depth * 0.25 # Hit a wall, return distance

    return max_distance  # Return max distance if no collision

# Main raycasting function
def raycast():
    rays = []
    half_fov = fov / 2
    start_angle = player_angle - half_fov
    end_angle = player_angle + half_fov

    # Loop over all rays
    for i in range(num_rays):
        # Spread rays evenly across the FOV
        ray_angle = start_angle + (i * fov / (num_rays - 1))  # Evenly distribute the angles
        ray_angle = normalize_angle(ray_angle)  # Normalize the angle
        distance = cast_ray(ray_angle)  # Get the distance to the wall
        rays.append(distance)

    return rays

# Function to rotate the player
def rotate_player():
    global player_angle
    player_angle += 5  # Rotate 10 degrees clockwise every cycle
    player_angle = normalize_angle(player_angle)  # Normalize the angle to stay within 0-360 degrees

def render(screen_width, screen_height):
    global rays, old_rays  # Declare them as global here to access and modify them


    # Start profiling
    render_start_time = time.ticks_us()

    # Check if the rays data has changed to avoid unnecessary rendering
    if rays == old_rays:
        render_end_time = time.ticks_us()
        render_elapsed_time = time.ticks_diff(render_end_time, render_start_time)
        print(f"Render skipped - Time: {render_elapsed_time / 1000} ms")
        return None  # Skip render if rays haven't changed

    oled.fill(0)

    half_height = screen_height // 2  # Center height of the screen

    for i, ray in enumerate(rays):
            
        # Calculate the height of the column based on the ray distance (projection)
        column_height = int(screen_height / ray)
        # Determine the X-coordinate of the column on the screen (multiply by 2 for two pixels per ray)

        # Calculate the vertical start and end positions for the column on the screen
        line_start_y = half_height - (column_height // 2)
        line_end_y = half_height + (column_height // 2)

        # Profiling the draw operation
        draw_start_time = time.ticks_us()
        if line_start_y < 0:
            line_start_y = 0
        if line_end_y > 63:
            line_end_y = 63
        
        oled.pixel(i*2, line_start_y, 1)
        oled.pixel(i*2, line_end_y, 1)
        draw_end_time = time.ticks_us()
        draw_elapsed_time = time.ticks_diff(draw_end_time, draw_start_time)
        print(f"Draw time for column {i}: {draw_elapsed_time / 1000} ms")

    oled.text(str(player_angle), 100, 10)
    # Profiling show operation
    show_start_time = time.ticks_us()
    #oled.show()
    oled.write_framebuf()
    show_end_time = time.ticks_us()
    show_elapsed_time = time.ticks_diff(show_end_time, show_start_time)
    print(f"Show time: {show_elapsed_time / 1000} ms")


    # End of render profiling
    render_end_time = time.ticks_us()
    render_elapsed_time = time.ticks_diff(render_end_time, render_start_time)
    print(f"Render total time: {render_elapsed_time / 1000} ms")
    
    # Optionally, you can draw pixels in between if needed
    # You mentioned drawing both top and bottom pixels, so these two are essential
    # For a more filled column, you could loop through the range between ys and ye
    # but for now we draw just two pixels as specified.


# Main loop (this would be run continuously in your application)
def main_loop():
    global rays, old_rays
    rays = [0] * 43
    precompute_sin_cos()  # Precompute the sine and cosine tables
    # Run the loop at 10fps (simulate 10 frames per second)
    while True:
        start_time = time.ticks_us()
        old_rays = rays
        # Rotate the player
        rotate_player()
        # Cast rays and get the distances to walls
        rays = raycast()
        
        # Print ray distances (for debugging)
        #print(rays)

        # Add your drawing or rendering code here
        
        render(86, 64)
        
        end_time = time.ticks_us()    # End time

        elapsed_time = time.ticks_diff(end_time, start_time)
        elapsed_time = elapsed_time / 1000
        fps = 1000 / elapsed_time
        print(f"Raycasting time: {elapsed_time} milliseconds")
        print(f"FPS: {fps}")

main_loop()