"""
All the robot devices as well as the blackboard are defined and initialised
here for an important reason - imported modules in python are run only once.
This means that there will be no redundant re-execution and all the variables
will be accessible from the other files as necessary. Modifications to the
blackboard will be visible from all files, which is the intended effect.
"""
from controller import Robot

robot = Robot()
timestep = int(robot.getBasicTimeStep())
blackboard = {} # Defined as a dictionary as it is much simpler.

# Setup the motors and sensors as dictionaries, with the relevant joint names
# as keys.
joints = [
    "gripper_left_finger_joint", "gripper_right_finger_joint", "head_2_joint",
    "arm_1_joint", "arm_2_joint", "arm_4_joint", "arm_7_joint",
    "torso_lift_joint",
]
finger_sensor_names = {
    "gripper_left_finger_joint": "gripper_left_sensor_finger_joint",
    "gripper_right_finger_joint": "gripper_right_sensor_finger_joint",
}
sensors = {}
motors = {}
for joint in joints:
    motors[joint] = robot.getDevice(joint)
    # If the name is in finger_sensor_names, use it. Fallback to %s_sensor.
    sensor = finger_sensor_names.get(joint, "%s_sensor" % joint)
    sensor = robot.getDevice(sensor)
    sensor.enable(timestep)
    sensors[joint] = sensor

# Enable force feedback for the grippers.
for finger in finger_sensor_names:
    motors[finger].enableForceFeedback(timestep)

# Initialise the wheel motors.
left_motor = robot.getDevice("wheel_left_joint")
right_motor = robot.getDevice("wheel_right_joint")
left_motor.setPosition(float("inf"))
right_motor.setPosition(float("inf"))
left_motor.setVelocity(.0)
right_motor.setVelocity(.0)

# Enable the gps and compass to gain better situational awareness.
gps = robot.getDevice("gps")
gps.enable(timestep)
compass = robot.getDevice("compass")
compass.enable(timestep)

# Setup and enable the camera, including the recognition functionality to
# obtain the jar locations.
camera = robot.getDevice("Astra rgb")
camera.enable(timestep)
camera.recognitionEnable(timestep)