import math

DEBUG = True

# GUI
SCREEN_DIM = (800, 600)
FONTSIZE = 24
BG_COL = (255,255,255) #white
FONT_COL = (0,0,0) #black
MENU_FPS = 15 # enough to poll keys efficiently
CTR_FPS = 60

# IP of the robot.
UR5_IP = "129.241.187.119"

# Enables force constraints on the tool. Note: Setting this to "True" involves
# polling the UR5 through a real-time monitor that runs at 125Hz. If the controller
# runs slow, try disabling this first.
USE_FORCE_MONITOR = True
FORCE_CONSTRAINT = 60
FORCE_T = 0.1

# Control parameters.
VELOCITY = 0.1
ACCELERATION = 0.5
DECELERATION = 0.5
CHCMD_DECEL = 0.8
CHCMD_T = 0.01 # cooldown between commands to smooth out the change in motion

# Loop parameters
DEFAULT_LOOP_SPEED = 0.1
LOOP_SPEED_MAX = 0.7
LOOP_SPEED_MIN = 0.05

# Block parameters
BLOCK_DIM = 0.0975 #TODO measure

# Table cylinder parameters
TABLE_QUADRANT = 0 # In range [0..4). Quadrant 0 is along the x-axis of the robot.
TABLE_ORIGO_OFFSET = -0.25 # TODO measure
Z_TABLE = -0.336
R_MIN = 0.6 # relative to TABLE center
R_MAX = 0.9 # relative to TABLE center
THETA_MIN= -math.pi/4 # table edge left
THETA_MAX = math.pi/4 # table edge right
Z_MIN = Z_TABLE + BLOCK_DIM/2
Z_MAX = Z_MIN + 5*BLOCK_DIM

# Block move parameters
R_LVL0 = 0.7 # tip of tool at table edge
Z_LVL0 = Z_MIN
THETA_EDGE_LEFT = -math.pi/4 + math.pi/68
THETA_EDGE_RIGHT = math.pi/4 - math.pi/68
R_BLOCKMOVE_MARGIN = BLOCK_DIM + 0.05 # r margin before and after pick/place
THETA_BLOCKMOVE_MARGIN = 0 # theta margin before and after pick/place
Z_BLOCKMOVE_MARGIN = 0 # z margin before and after pick/place
R_BLOCKMOVE_OFFSET = -0.01 # r start/end in place/pick. lift slightly inwards to avoid collision with other blocks
THETA_BLOCKMOVE_OFFSET = 0 # theta start/end in place/pick
Z_BLOCKMOVE_OFFSET = BLOCK_DIM*0.5 # z start/end in place/pick



# Home quadrant joint configuration.
# We need this expressed in joint space to ensure that the robot does not
# collide with itself when moving to the base configuration.
# This is also for fine-tuning the tool orientation
QUADRANT_JOINT_CONFIG = [TABLE_QUADRANT*math.pi/2,
			   	   		 -3*math.pi/4,
			   	   		 -3*math.pi/4,
			   	   		 -math.pi/2,
			   	  		 -math.pi/2,
			  	  		 49*math.pi/50]