#!/usr/bin/python3
import RPi.GPIO as GPIO
import turtle
import time
oldX=0
oldY=0
x_coord = 0
y_coord = 0
# GPIO pins for the encoders
encoder1_pins = (23, 24)  # A and B pins for encoder 2 (x-axis)
encoder2_pins = (14, 15)  # A and B pins for encoder 1 (y-axis)


# Set up GPIO
button_pin = 12
GPIO.setmode(GPIO.BCM)
GPIO.setup(button_pin, GPIO.IN, pull_up_down=GPIO.PUD_UP)
GPIO.setup(encoder1_pins[0], GPIO.IN, pull_up_down=GPIO.PUD_UP)
GPIO.setup(encoder1_pins[1], GPIO.IN, pull_up_down=GPIO.PUD_UP)
GPIO.setup(encoder2_pins[0], GPIO.IN, pull_up_down=GPIO.PUD_UP)
GPIO.setup(encoder2_pins[1], GPIO.IN, pull_up_down=GPIO.PUD_UP)

# Variables to store encoder state
encoder1_last_state = (GPIO.input(encoder1_pins[0]) << 1) | GPIO.input(encoder1_pins[1])
encoder2_last_state = (GPIO.input(encoder2_pins[0]) << 1) | GPIO.input(encoder2_pins[1])

# Create a turtle object
t = turtle.Turtle()

# Set up the turtle screen
screen = turtle.Screen()
screen.setup(width=1.0, height=1.0)  # Set the window to full-screen mode
canvas = screen.getcanvas()

# Create a RawTurtle object with the canvas
raw_turtle = turtle.RawTurtle(canvas)
root = canvas.winfo_toplevel()
root.overrideredirect(1)
    
# Define a callback function to run when the button is pressed
def button_callback(channel):
    global x_coord, y_coord 
    print("Clear Screen")
    raw_turtle.reset()  # Reset the turtle
    x_coord = 0  # Reset the x coordinate to 0
    y_coord = 0  # Reset the y coordinate to 0 
        
# Callback function for encoder 1 (x-axis)
def encoder1_callback(channel):
    global encoder1_last_state, x_coord
    a = GPIO.input(encoder1_pins[0])
    b = GPIO.input(encoder1_pins[1])
    new_state = (a << 1) | b
    if (encoder1_last_state == 0b00 and new_state == 0b10) or (encoder1_last_state == 0b11 and new_state == 0b01):
        x_coord += 1
    elif (encoder1_last_state == 0b10 and new_state == 0b00) or (encoder1_last_state == 0b01 and new_state == 0b11):
        x_coord -= 1
    encoder1_last_state = new_state

# Callback function for encoder 2 (y-axis)
def encoder2_callback(channel):
    global encoder2_last_state, y_coord
    a = GPIO.input(encoder2_pins[0])
    b = GPIO.input(encoder2_pins[1])
    new_state = (a << 1) | b
    if (encoder2_last_state == 0b00 and new_state == 0b10) or (encoder2_last_state == 0b11 and new_state == 0b01):
        y_coord -= 1
    elif (encoder2_last_state == 0b10 and new_state == 0b00) or (encoder2_last_state == 0b01 and new_state == 0b11):
        y_coord += 1
    encoder2_last_state = new_state

# Add event detection for encoder 1 (x-axis)
GPIO.add_event_detect(encoder1_pins[0], GPIO.BOTH, callback=encoder1_callback)
GPIO.add_event_detect(encoder1_pins[1], GPIO.BOTH, callback=encoder1_callback)

# Add event detection for encoder 2 (y-axis)
GPIO.add_event_detect(encoder2_pins[0], GPIO.BOTH, callback=encoder2_callback)
GPIO.add_event_detect(encoder2_pins[1], GPIO.BOTH, callback=encoder2_callback)

# Add event listener to the button pin
GPIO.add_event_detect(button_pin, GPIO.FALLING, callback=button_callback, bouncetime=300)

# Function to update the turtle's position based on encoder callbacks
def update_position():
    global x_coord, y_coord, oldX, oldY
    if (x_coord != oldX or y_coord != oldY):
        print(f"X: {x_coord}, Y: {y_coord}")
        raw_turtle.goto(x_coord, y_coord)
    oldX = x_coord
    oldY = y_coord
    screen.ontimer(update_position, 100)  # Schedule the next update


# Main loop
# Start updating the position
update_position()
# Start the turtle main loop
turtle.mainloop()

# Clean up GPIO on keyboard interrupt
GPIO.cleanup() #Press Ctrl+C to exit (terminal window... maybe add a screen listener for ctrl+c or Q)
turtle.done()