import turtle
import time
from networktables import NetworkTables
import logging
import math

turtle.setup(1920,900)
wn=turtle.Screen()
wn.title("GUI")
turtle.speed("fastest")
turtle.penup()

logging.basicConfig(level=logging.DEBUG)
NetworkTables.initialize(server="roborio-1540-frc.local")

sd = NetworkTables.getTable("Odometry")
xValue = sd.getAutoUpdateValue("Debug/WheelOdometry/position/x", 0)
yValue = sd.getAutoUpdateValue("Debug/WheelOdometry/position/y", 0)
yawValue = sd.getAutoUpdateValue("Debug/WheelOdometry/orientation/z", 0)

def x():
    return math.degrees(xValue.value)

def y():
    return math.degrees(yValue.value)

def rotation():
    return math.degrees(yawValue.value)

print("Zeroing...")
time.sleep(2)
zeroX = x()
zeroY = y()
zeroR = rotation()
print("ZEROES: (", x(), ",", y(), ") ", rotation())
turtle.pendown()

while True:
    turtle.setheading(rotation())
    turtle.goto(x(), y())

wn.listen()
wn.mainloop()