import pygame
from kingdon import Algebra
import math

ERROR = 0.00001
DISK_RADIUS = 250
WIDTH, HEIGHT = 800, 600
CENTER_X, CENTER_Y = 400, 300

alg = Algebra(2,1,0)
locals().update(alg.blades)

points = []
lines  = []

def point(x,y):
    #Construct a point.
    #Hyperbolic space is modeled as a hyperboloid embedded in 3D Minkowski space.
    #Find w such that x,y,w is a point on the hyperboloid.
    w_sqr = x*x + y*y + 1
    w = math.sqrt(w_sqr)
    return x*e23 -y*e13 + w*e12

def draw_point(point):
    #Normalize the point
    x_0 = point.e12
    x_1 = -point.e23
    x_2 = point.e13
    sqr = x_0*x_0 - x_1*x_1 - x_2*x_2
    if sqr < 0:
        return
    norm = math.sqrt(sqr)
    x_0 = x_0/norm
    x_1 = x_1/norm
    x_2 = x_2/norm
    #Transform it into the Poincare Space
    if x_0 == -1:
        return
    u = x_1/(1+x_0)
    v = x_2/(1+x_0)

    pygame.draw.circle(screen, RED, (int(u*DISK_RADIUS+CENTER_X), int(v*DISK_RADIUS+CENTER_Y)), 5)

def draw_line(line):
    delta = line.e3
    if abs(delta) < ERROR:
        # The geodesic is a line through the origin.
        theta   = math.atan2(line.e1, -line.e2)
        start_x = int(CENTER_X-DISK_RADIUS*math.cos(theta))
        start_y = int(CENTER_Y-DISK_RADIUS*math.sin(theta))
        end_x   = int(CENTER_X+DISK_RADIUS*math.cos(theta))
        end_y   = int(CENTER_Y+DISK_RADIUS*math.sin(theta))
        pygame.draw.line(screen, BLUE, (start_x, start_y), (end_x, end_y), 1)
    else:
        #The geodesic is a circle
        a = line.e1
        b = line.e2
        sqr = a*a + b*b
        norm = math.sqrt(sqr)
        x_c = a/delta
        y_c = b/delta
        r = math.sqrt(x_c*x_c+y_c*y_c - 1)
        c_x = int(CENTER_X + x_c*DISK_RADIUS)
        c_y = int(CENTER_Y + y_c*DISK_RADIUS)
        pygame.draw.circle(screen, BLUE, (c_x, c_y), int(r*DISK_RADIUS), 1)

def simulation(t):
    lines.clear()
    points.clear()
    #lines.append(y_axis)
    #lines.append(x_axis)
    #points.append(origin)

    x_motor = tx_generator.exp()
    y_motor = ty_generator.exp()

    #generator = math.sin(t)*tx_generator + math.sin(math.sqrt(2)*t)*ty_generator + math.sin(math.sqrt(3)*t)*origin
    y_tick = motor*y_axis*motor.reverse()
    x_tick = motor*x_axis*motor.reverse()
    #lines.append(y_tick)
    #lines.append(x_tick)
    #points.append(y_tick^x_tick)

    mirror_one = e1
    mirror_two = e2
    mirror_three = -math.sqrt(3)/2 * e1 - math.sqrt(2)/2 * e2 + 1/2 * e3

    generated = [ e1 ]
    gen_next = []

    for loop in range(8):
        for line in generated:
            gen_next.append(mirror_one * line * mirror_one)
            gen_next.append(mirror_two * line * mirror_two)
            gen_next.append(mirror_three * line * mirror_three)
        generated = gen_next
        gen_next = []

    for line in generated:
        lines.append(motor*line*motor.reverse())
    '''
    for i in range(9):
        for j in range(9):
            motor_x = ((i-4)*tx_generator/10).exp()
            motor_y = ((j-4)*ty_generator/10).exp()
            y_tick = motor * motor_x * y_axis * motor_x.reverse() * motor.reverse()
            x_tick = motor * motor_y * x_axis * motor_y.reverse() * motor.reverse()
            #lines.append(y_tick)
            #lines.append(x_tick)
            #points.append(y_tick^x_tick)
    '''


y_axis = e1 #x = 0 line
x_axis = e2 #y = 0 line

tx_generator = e13 #A translation in the x direction
ty_generator = e23 #A translation in the y direction
origin = e12 #The origin, and a rotation about the origin

# Initialize Pygame
pygame.init()

screen = pygame.display.set_mode((WIDTH, HEIGHT))
pygame.display.set_caption("Pygame Example")

# Colors
BLACK = (0, 0, 0)
WHITE = (255, 255, 255)
RED = (255, 0, 0)
BLUE = (0, 0, 255)


# Game loop
running = True
clock = pygame.time.Clock()
font = pygame.font.SysFont("Arial", 18)
t = 0

lines.append(x_axis)
lines.append(y_axis)

pos = 0*tx_generator + 0*ty_generator + 0*origin
motor = 1*e
tx_motor = (tx_generator/100).exp()
ty_motor = (ty_generator/100).exp()
while running:
    t += 0.001

    keys = pygame.key.get_pressed()

    #point_gen = pos.e13*e13 + pos.e23*e23
    #motor = point_gen.exp()
    point = motor * origin * motor.reverse()
    point_motor = (point/100).exp()

    if keys[pygame.K_a]:
        motor = tx_motor * motor 
    if keys[pygame.K_d]:
        motor = tx_motor.reverse() * motor
    if keys[pygame.K_w]:
        motor = ty_motor * motor 
    if keys[pygame.K_s]:
        motor = ty_motor.reverse() * motor
    if keys[pygame.K_q]:
        motor = point_motor*motor
    if keys[pygame.K_e]:
        motor = point_motor.reverse()*motor

    # Clear the screen
    screen.fill(WHITE)

    clock.tick()
    fps = clock.get_fps()
    fps_text = font.render(f"FPS: {int(fps)}", True, (0, 0, 0))
    screen.blit(fps_text, (10, 10))

    #Poincare Disk Boundary
    pygame.draw.circle(screen, BLACK, (CENTER_X, CENTER_Y), DISK_RADIUS, 1)

    simulation(t)
    for line in lines:
        draw_line(line)
    for point in points:
        draw_point(point)

    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            running = False

    # Update the display
    pygame.display.flip()

# Quit Pygame
pygame.quit()