#!/usr/bin/env python3

import rospy
import time
from geometry_msgs.msg import Twist, Vector3
from math import pi
from std_srvs.srv import Empty

class rect:
    '''
    Publishes:  geometry_msgs/Twist message to cmd_vel
    Services: unpause_physics_client - unpauses Gazebo simulation
    Parameters: len - length of rectangle
                wid - width of rectangle
    '''
    def __init__(self):
        '''
        constructor of rect class
        initializes publisher and parameters
        '''
        #import rectangle parameters
        self.len = rospy.get_param('follow_rect/rect_L')
        self.wid = rospy.get_param('follow_rect/rect_W')

        #initialize publisher
        self.pub = rospy.Publisher('cmd_vel',Twist,queue_size=10)
        self.prate = rospy.Rate(300)

        #initialize services
        rospy.wait_for_service('/gazebo/unpause_physics')
        self.unpause_physics_client = rospy.ServiceProxy('/gazebo/unpause_physics',Empty)

    def create_twist(self,xdot,omega):
        '''
        converts velocity and angular velocity to geometry_msgs/Twist message
        args:       xdot - velocity
                    omega - angular velocity
        reuturns:   Twist message
        '''
        return Twist(linear = Vector3(x = xdot, y = 0, z = 0),
                    angular = Vector3(x = 0, y = 0, z = omega))

    def turn(self):
        '''
        turn robot 90 degrees
        '''
        now = rospy.get_time()
        while rospy.get_time() < now+2.5:
            self.pub.publish(self.create_twist(0,pi/4))
            self.prate.sleep()
        self.pub.publish(self.create_twist(0,0))


    def follow(self):
        '''
        follow rectangle trajectory defined by rectangle parameters in rect_dims.yaml
        '''
        #first side
        now = rospy.get_time()
        while rospy.get_time() < now+self.len*2:
            self.pub.publish(self.create_twist(0.5,0))
            self.prate.sleep()
        self.pub.publish(self.create_twist(0,0))

        time.sleep(2)

        #turn
        self.turn()

        #second side
        now = rospy.get_time()
        while rospy.get_time() < now+self.wid*2:
            self.pub.publish(self.create_twist(0.5,0))
            self.prate.sleep()
        self.pub.publish(self.create_twist(0,0))

        time.sleep(2)

        #turn
        self.turn()

        #third side
        now = rospy.get_time()
        while rospy.get_time() < now+self.len*2:
            self.pub.publish(self.create_twist(0.5,0))
            self.prate.sleep()
        self.pub.publish(self.create_twist(0,0))

        time.sleep(2)

        #turn
        self.turn()

        #fourth side
        now = rospy.get_time()
        while rospy.get_time() < now+self.wid*2:
            self.pub.publish(self.create_twist(0.5,0))
            self.prate.sleep()
        self.pub.publish(self.create_twist(0,0))

        time.sleep(2)

        #turn
        self.turn()


        


def main():
    '''
    main function
    creates rect object
    uses rect object to continuously follow rectangle trajectory
    '''
    rospy.init_node('follow_rect')
    r = rect()
    r.unpause_physics_client()
    time.sleep(2)
    while not rospy.is_shutdown():
        r.follow()
    rospy.spin()

if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        pass