#! usr#!/usr/bin/env python

import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist

import time
import math

def shutdown():
	rospy.loginfo("Stop TurtleBot")
	pub.publish(Twist())
	rospy.sleep(1)

rospy.init_node('move_forward')
rospy.loginfo("To stop TurtleBot CTRL + C")
rospy.on_shutdown(shutdown)
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
move = Twist()
r = rospy.Rate(10)

#
# 1.2s for 90 deg
#
def fwd(t=3, v=0.22):
    move.linear.x = v
    move.angular.z = 0.0
    print("making forward")
    curr_time = time.time()
    while time.time()-curr_time <= t:
	    pub.publish(move)
	    r.sleep()
    move.linear.x = 0.0
    move.angular.z = 0.0
    pub.publish(move)

def rgt():
    print("biglang liko")
    move.linear.x = 0.0
    move.angular.z = -1*math.pi/2
    curr_time = time.time()
    while time.time()-curr_time <= 1.2:
	    pub.publish(move)
	    r.sleep()
    move.linear.x = 0.0
    move.angular.z = 0.0
    pub.publish(move)

def lft():
    print("biglang liko")
    move.linear.x = 0.0
    move.angular.z = 1*math.pi/2
    curr_time = time.time()
    while time.time()-curr_time <= 1.05:
	    pub.publish(move)
	    r.sleep()
    pub.publish(Twist())
    move.linear.x = 0.0
    move.angular.z = 0.0
    pub.publish(move)

def e():
    print("\n--------------------------------------------")
    print("Drawing E")
    print("--------------------------------------------\n")
    fwd(1)
    lft()
    fwd(1)
    lft()
    fwd(1)
    lft()
    fwd(2)
    lft()
    fwd(1)

def i():
    print("\n--------------------------------------------")
    print("Drawing I")
    print("--------------------------------------------\n")
    fwd(2)

def l():
    print("\n--------------------------------------------")
    print("Drawing L")
    print("--------------------------------------------\n")
    fwd(2, -0.22)

def c():
    print("\n--------------------------------------------")
    print("Drawing C")
    print("--------------------------------------------\n")
    lft()
    fwd(1)


#e()
#l()
#e()
#c()
#t()
#r()
#i()
#c()
#a()
#l()
#a()
#n()
#d()
#e()
#l()
#e()
#c()
#t()
#r()
#i()
#c()
#a()
#l()
#e()
#n()
#g()
#i()
#n()
#e()
#e()
#r()
#i()
#n()
#g()