conv

import math

# Module turtlebot_basic

# Convert a theta value in radians to degrees
def thetaRad2Deg(theta_rad):
    if theta_rad < 0.0:
        theta_deg = 180.0 + 180.0 * (math.pi + theta_rad) / math.pi
    else:
        theta_deg = 180.0 * theta_rad / math.pi

    return theta_deg

# Convert a theta value in degrees to radians
def thetaDeg2Rad(theta_deg):
    if theta_deg > 180.0:
        theta_rad = (theta_deg - 180.0) * math.pi / 180.0 - math.pi
    else:
        theta_rad = theta_deg * math.pi / 180.0

    return theta_rad

# Sum between two theta values in radians
def getThetaSum(theta1, theta2):
    theta = (thetaRad2Deg(theta1) + thetaRad2Deg(theta2)) % 360

    return thetaDeg2Rad(theta)

def getThetaDegDiff(thetaRbt, thetaGoal):
    d = abs(thetaRbt - thetaGoal) % 360
    if d > 180:
        r = 360 - d
    else:
        r = d

    #calculate sign
    if (thetaRbt - thetaGoal >= 0 and thetaRbt - thetaGoal <= 180) or (thetaRbt - thetaGoal <=-180 and thetaRbt - thetaGoal >= -360):
        sign = 1
    else:
        sign = -1

    r *= sign

    return r

def getDiffThetaObj2Robot(rbt_x, rbt_y, rbt_theta, obj_x, obj_y, obj_z):
    if obj_z != -1:
        x = obj_x - rbt_x
        y = obj_y - rbt_y

        thetaObj = math.atan2(y, x)

        diff = getThetaDegDiff(thetaRad2Deg(rbt_theta), thetaRad2Deg(thetaObj))

        return diff
    else:
        return 0.0