#-----------------------------------------------------------------------
# Import Required Libraries
#-----------------------------------------------------------------------
import numpy as np
import pygame
import pygame.gfxdraw
import sys, os
path_above = os.path.abspath(os.path.join(os.path.dirname( __file__ ), '..'))
sys.path.append(path_above)
from TBotTools import geometry, pgt
from datetime import datetime
#-----------------------------------------------------------------------
# Setup display and objects for drawing
#-----------------------------------------------------------------------
geom = geometry.geometry()
pygame.init()
screen = pygame.display.set_mode((1000, 700))
pygame.display.set_caption("Pendulum")
clock = pygame.time.Clock()
framerate = 60
origin = [500,180]
arrow = np.array([[1,0],[1,150],[0,152],[-1,150],[-1,0],[1,0]]).astype(float)
arrow = arrow/arrow.max()
scalefactor = 240
sbar = pgt.SliderBar(screen, (100,100), 1, 800, 2.00, 10, (170,170,170),(10,10,10),20)
textPrint = pgt.TextPrint((255,255,255))
textPrint.setfontsize(30)
#-----------------------------------------------------------------------
# Constants
#-----------------------------------------------------------------------
g = 9.81
#----------------------------------------------------------------------#
# sets the length of the trail
pts = deque(maxlen=10)
pts2 = deque(maxlen=10)
pathindex = 0
rotspeed = 200
speedfactor = 0.3
turnspeedfactor = 0.3
turntime = 0.01
bendscalefactor = 2
rdeadban = 2
tolerance = 30
#-------------------- Define functions ------------------------------#
geom = geometry.geometry(1) # scale factor to convert pixels to mm
#--------------------- Setup Bluetooth --------------------------------#
data = [0, 0, 0, 0]
sendcount = 0
#------------------------------------------------------------------
# For Linux / Raspberry Pi
#------------------------------------------------------------------
bd_addr = '98:D3:51:FD:81:AC' # use: 'hcitool scan' to scan for your T-Bot address
#bd_addr = '98:D3:51:FD:82:95' # George
#bd_addr = '98:D3:91:FD:46:C9' # B
#bd_addr = '98:D3:32:21:3D:77'
port = 1
btcom = tbt.bt_connect(bd_addr, port,
'PyBluez') # PyBluez works well for the Raspberry Pi
