#!/usr/bin/env python

# ME133b - Lab 1b    Move the Bebop along some basic closed shape
#                     (so that it theoretically will end where it started)

# A basic script to make the Bebop to rotate for 4 seconds. Press CTRL + C to stop.  To run:
# On laptop:
# python me133b_lab1_template.py

import rospy
from geometry_msgs.msg import Twist
from geometry_msgs.msg import PoseStamped,Pose
from nav_msgs.msg import Odometry
import time
import math
import csv

class DrawShape():
    def __init__(self):
        # initiliaze
        rospy.init_node('ME133b_Lab_1', anonymous=False)

        # tell user how to stop Bebop
        rospy.loginfo("To stop Bebop CTRL + C")

        # What function to call when you ctrl + c    
        rospy.on_shutdown(self.shutdown)
            
        # Create a publisher which can "talk" to Bebop and tell it to move
        # Tip: You may need to change cmd_vel_mux/input/navi to /cmd_vel if you're not using Bebop2
        self.cmd_vel = rospy.Publisher('/bebop/cmd_vel', Twist, queue_size=10)
        
        # Open a file to write all the sensor values to
        self.file = open('bebop.csv','w')
        self.file.write('Time,  Odom x, Odom y, Odom z,Linear Command X Y Z,Angular Command Z, OptiTrack Pos X,Pos Y,Pos Z,Quat x,Quat y,Quat z,Quat w,\n')
        
        # Declare empty move command and pose from OptiTrack
        self.move_cmd = Twist()
        self.optiTrackPose = Pose()
        
        # Subscribe to the sensor core topic to read the encoder values
        self.odomSubscriber = rospy.Subscriber("/bebop/odom", Odometry, self.writeData)
        self.optiTrackSubscriber = rospy.Subscriber("/vrpn_client_node/ME133_Bebop320/pose",PoseStamped,self.readOptitrack)
        
        # Bebop will stop if we don't keep telling it to move.  How often should we tell it to move? 10 HZ
        r = rospy.Rate(10)
        t0 = time.time()
        
        ######################################################
        #----------------Modify this code---------------------
        # Draw out some basic shape
        ######################################################
        # Here we'll loop through a series of commands
        while (time.time() - t0 < 5.0): #outer loop to make sure we send empty velocity commands to stop the vehicle, keep at least a 2 sec margin
            self.move_cmd = Twist()
            if (time.time() - t0 < 1.0): # inner loop to execute the path
                self.move_cmd.linear.x = 0.0 # m/s
				self.move_cmd.linear.y = 0.0 # m/s
				self.move_cmd.linear.z = 0.0 # m/s
                self.move_cmd.angular.z = 1.0  # rad/s
            # Publish the commands to the node then wait for the next cycle
            self.cmd_vel.publish(self.move_cmd)
            r.sleep()
        
        ######################################################
    
    # Write data from the encoders to the csv we opened
    def writeData(self,data):
        self.file.write('%5.3f,%5.3f,%5.3f,%5.3f,%5.3f,%5.3f,%5.3f,%5.3f,' % ( rospy.get_time(),data.pose.pose.position.x, data.pose.pose.position.y, data.pose.pose.position.z, self.move_cmd.linear.x, self.move_cmd.linear.y, self.move_cmd.linear.z, self.move_cmd.angular.z))
        self.file.write('%5.3f,%5.3f,%5.3f,%5.3f,%5.3f,%5.3f,%5.3f \n' % (self.optiTrackPose.position.x,self.optiTrackPose.position.y,self.optiTrackPose.position.z,self.optiTrackPose.orientation.x,self.optiTrackPose.orientation.y,self.optiTrackPose.orientation.z,self.optiTrackPose.orientation.w))

    # Store the OptiTrack pose data whenever it is received
    def readOptitrack(self,data):
        self.optiTrackPose = data.pose

    # Unsubscribe from the sensor topic and tell the Bebop to stop moving
    def shutdown(self):
        self.odomSubscriber.unregister()
        self.optiTrackSubscriber.unregister()
        self.file.close()
        rospy.loginfo("Stop Bebop")
        self.cmd_vel.publish(Twist())
        rospy.sleep(1)
 
if __name__ == '__main__':
    try:
        DrawShape()
    except:
        rospy.loginfo("DrawShape node terminated.")