#!/usr/bin/env python3
import rospy
from std_msgs.msg import String


received_command = ""
last_received_command = ""

def listener():
    rospy.init_node("motor_driver", anonymous=True)
    rospy.Subscriber("/command", String, command_callback)
    rate = rospy.spin()

def command_callback(message):
    global received_command
    global last_received_command

    received_command = message.data

    COMMAND_MAP.get(received_command, unknown_command)()

    if received_command != last_received_command:
        rospy.loginfo("Received command: {}".format(received_command))
        last_received_command = received_command

    
def forward():
    pass


def back():
    pass


def left():
    pass


def right():
    pass


def stop_motors():
    pass


def unknown_command():
    pass

COMMAND_MAP = {
    "forward": forward,
    "stop": stop_motors,
    "left": left,
    "right": right,
    "back": back,
}

if __name__ == "__main__":
    print("Ready to recieve commands!")
    listener()
    print("Node is shutting down. Stopping Motors")
    stop_motors()