import compLib.CompLib_pb2 as CompLib_pb2
from compLib.CompLibClient import CompLibClient


class Movement(object):
    """High level class to control movement of the robot
    """

    @staticmethod
    def drive_distance(distance: float, speed: float):
        """
        Drive a given distance with a certain speed.
        Positive distance and speed with result in forward motion. Everything else will move backwards.
        :param distance: Distance in meters
        :param speed: Speed in meters per second
        :return: None
        """
        request = CompLib_pb2.DriveDistanceRequest()
        request.header.message_type = request.DESCRIPTOR.full_name
        request.distance_m = distance
        request.velocity_m_s = speed

        response = CompLib_pb2.GenericResponse()
        response.ParseFromString(CompLibClient.send(request.SerializeToString(), request.ByteSize()))

    @staticmethod
    def turn_degrees(degrees: float, speed: float):
        """
        Turn specified degrees with a given speed.
        Positive degrees and speed with result in counter-clockwise motion. Everything else will be clockwise
        :param degrees: Degrees between -180 and 180
        :param speed: Speed in radians per second
        :return: None
        """
        request = CompLib_pb2.TurnDegreesRequest()
        request.header.message_type = request.DESCRIPTOR.full_name
        request.angle_degrees = degrees
        request.velocity_rad_s = speed

        response = CompLib_pb2.GenericResponse()
        response.ParseFromString(CompLibClient.send(request.SerializeToString(), request.ByteSize()))

    @staticmethod
    def drive(linear: float, angular: float):
        """
        Non-blocking way to perform a linear and angular motion at the same time.
        :param linear: Linear speed in meters per second
        :param angular: Angular speed in radians per second
        :return: None
        """
        request = CompLib_pb2.DriveDistanceRequest()
        request.header.message_type = request.DESCRIPTOR.full_name
        request.linear_velocity_m_s = linear
        request.angular_velocity_rad_s = angular

        response = CompLib_pb2.GenericResponse()
        response.ParseFromString(CompLibClient.send(request.SerializeToString(), request.ByteSize()))