import numpy as np
from constants import ARM_OFFSET

# Convert radian measurements to degrees.
def deg(radians):
    return radians * 180 / np.pi

# Make sure a variable is between -bound and +bound.
def bound_by(variable, bound):
    return max(-bound, min(bound, variable))

# Calculate the euclidean distance between two points.
# Only uses the x and y coordinates.
def euclidean(u, v):
    return np.sqrt( (u[0]-v[0])**2 + (u[1]-v[1])**2 )

# Calculate the position to which the robot should move to grab the jar, given
# the position of the jar provided by the camera and the robot's current
# position.
def grasp_point(jar_pos, robot_pos, jar_offset=0.):
    d_dist = jar_pos - robot_pos[:2] # dx,dy between the jar and the robot.
    distance = np.linalg.norm(d_dist) # Euclidean distance.
    d_dist *= (distance - ARM_OFFSET) / distance # Account for ARM_OFFSET.
    d_dist[0] += jar_offset # Add the jar_offset to the x-axis.
    return robot_pos[:2] + d_dist

# Make sure angles are between -pi and +pi.
def fix_arctan(radians):
    radians += np.pi
    radians %= 2 * np.pi
    return radians - np.pi

# Convert from robot coordinates to world coordinates.
def robot_to_world_coords(robot_coor, gps_pos, theta):
    c, s = np.cos(theta), np.sin(theta)
    matrix = np.array([[c, -s], [s, c]]) # Rotation matrix for theta.
    # Adjust for angle and add the robot's position to convert to world coords.
    return matrix @ robot_coor[:2] + gps_pos[:2]