#!/usr/bin/env python
import roslib; roslib.load_manifest('control_node')
import rospy
from geometry_msgs.msg import Twist, Vector3, PoseWithCovariance
from tf.transformations import euler_from_quaternion

        
def distance2(a, b):
    return (a[0]-b[0])^2 + (a[1]-b[1])^2
    
def add(a, b):
    return [a[0]+b[0], a[1]+b[1]]
    
def neg(a):
    return [-a[0], -a[1]]
    
def normalize(a):
    length = sqrt(a[0]^2+a[1]^2)
    return [a[0]/length, a[1]/length]

class talker(object):
    global posprint
    def __init__(self):
        self.path  = [[0, 0], [0.2, 0], [0.4, -0.1], [0.6, 0]]
        self.followDistance = 0.05
        self.command_map = {
            's'     : (self.cmd_stop,   'Stop!')}
        self.publisher = rospy.Publisher('cmd_vel', Twist)
        rospy.init_node('ControlNode')
        
    def isPassed(self, currentPos, point, pointBack, pointFront):
        vectorBack = normalize(poinBack - point)
        vectorFront = normalize(pointFront - point)
        distFront = distance2(add(point, vectorBack), currentPos)
        distBack = distance2(add(point, vectorFront), currentPos)
        if(distBack > distFront):
            return False
        else:
            return True
        
    def control_angle(self, robotPose):
        robotCoord = [robotPose.pose.position.x, robotPose.pose.position.y]
        (roll, pitch, robotYaw) = euler_from_quaternion(robotPose.orientation)
        #remove passed points
        while len(self.path < 3) or self.isPassed(robotCoord, self.path[1], self.path[0], self.path[2]):
            self.path.pop(0)
        #now the first point in path is the one behind us, the second is the one ahead
        
        
    def send_cmd_msg(self):
        msg = Twist()
        msg.linear = Vector3()
        msg.angular = Vector3()
        msg.linear.x = self.x
        msg.linear.y = 0
        msg.linear.z = 0
        msg.angular.x = 0
        msg.angular.y = 0
        msg.angular.z = self.omega
        self.publisher.publish(msg)        

    def cmd_x(self, args):
        usage = "usage: x [speed m/s]"
        if len(args) < 2:
            print(usage)
        self.x = float(args[1])
        self.send_cmd_msg()
        
    def cmd_stop(self, args):
        self.x = 0
        self.omega = 0
        self.send_cmd_msg()
        
    def cmd_omega(self, args):
        usage = "usage: omega [angular speed rad/s]"
        if len(args) < 2:
            print(usage)
        self.omega = float(args[1])
        self.send_cmd_msg()
                 
    def process_stdin(self, line):
        '''handle commands from user'''
        if line is None:
            sys.exit(0)
        line = line.strip()
    
        if not line:
            return
    
        args = line.split()
        cmd = args[0]
            
        if cmd in ['help', 'h']:
            k = self.command_map.keys()
            k.sort()
            for cmd in k:
                (fn, help) = self.command_map[cmd]
                print("%-15s : %s" % (cmd, help))
            return
        if not cmd in self.command_map:
            print("Unknown command '%s'" % line)
            return
        (fn, help) = self.command_map[cmd]
        try:
            fn(args)
        except Exception as e:
            print("ERROR in command: %s" % str(e))
        
    def run(self):
        self.publisher = rospy.Publisher('cmd_vel', Twist)
        rospy.init_node('ControlNode')
        while(True):
            input_command = raw_input('>')        
            cmds = input_command.split(';')
            for c in cmds:
                self.process_stdin(c)


if __name__ == '__main__':
    talkR = talker()
    try:
        talkR.run()
    except rospy.ROSInterruptException:
        pass
       
