#!/usr/bin/env python
import rospy
from geometry_msgs.msg  import Twist
from turtlesim.msg import Pose
from math import pow,atan2,sqrt
from enum import Enum

class turtle():
    def __init__(self):
        #Creating our node,publisher and subscriber
        rospy.init_node('turtlebot_controller', anonymous=True)
        self.velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
        self.pose_subscriber = rospy.Subscriber('/turtle1/pose', Pose, self.callback)
        self.pose = Pose()
        self.rate = rospy.Rate(10)
    #This is the tolerance of the controller, because the turtlesim is not the most accurate simulator
    self.tolerance = 0.1

    #Callback function implementing the pose value received
    #This saves the current position of the turtle in global coordinate system
    def callback(self, data):
        self.pose = data
        self.pose.x = round(self.pose.x, 4)
        self.pose.y = round(self.pose.y, 4)

    #Distance to the actual goal
    def get_distance(self, goal_x, goal_y):
        distance = sqrt(pow((goal_x - self.pose.x), 2) + pow((goal_y - self.pose.y), 2))
        return distance

    #This function drives the correct behavior of the robot in the turtle sim map
    def move2goal(self,posX,posY):
        goal_pose = Pose()
        goal_pose.x = posX
        goal_pose.y = posY
        distance_tolerance = self.tolerance
        vel_msg = Twist()
        while sqrt(pow((goal_pose.x - self.pose.x), 2) + pow((goal_pose.y - self.pose.y), 2)) >= distance_tolerance:
            #Porportional Controller
            #linear velocity in the x-axis:
            vel_msg.linear.x = 1.5 * sqrt(pow((goal_pose.x - self.pose.x), 2) + pow((goal_pose.y - self.pose.y), 2))
            vel_msg.linear.y = 0
            vel_msg.linear.z = 0

            #angular velocity in the z-axis:
            vel_msg.angular.x = 0
            vel_msg.angular.y = 0
            vel_msg.angular.z = 4 * (atan2(goal_pose.y - self.pose.y, goal_pose.x - self.pose.x) - self.pose.theta)

            #Publishing our vel_msg
            self.velocity_publisher.publish(vel_msg)
            self.rate.sleep()
        #Stopping our robot after the movement is over
        vel_msg.linear.x = 0
        vel_msg.angular.z =0
        self.velocity_publisher.publish(vel_msg)

def create_list_from_mission_plan(mission_plan):
    coordinates = []
    for mission in mission_plan :
        currTask = mission[0]
        currWayPoints = mission[1]
        if currTask is 'SHORTESTPATH' :
            currWayPoints = currWayPoints
        for wp in currWayPoints :
            coordinates.append(wp)

    return coordinates

if __name__ == '__main__':
    try:
    #creating a turtle object
        tb = turtle()
    #list of coordinates to visit
    #This can is the only thing to change in the model to text
        mission = [['START', [(1,1)]], ['SHORTESTPATH', [(1,2),(3,4)]], ['RETURNTOSTART', [(1,1)]], ['SHORTESTPATH', [(1,2),(10,5)]], ['LINE', [(3,4)]], ['RETURNTOSTART', [(1,1)]]]
        coordinate_list = create_list_from_mission_plan(mission)
    #making the the turtle follow the coordinates in sequence
        for coordinate in coordinate_list:
           x,y = coordinate
           tb.move2goal(x,y)

    except rospy.ROSInterruptException: pass
