#!/usr/bin/env python

# Listen to /bebop/cmd_vel . Press CTRL + C to stop.  To run:
# On laptop before sending cmd_vel commands:
# python bebop_s.py


import rospy
from geometry_msgs.msg import Twist, Point
import time
import math
from std_msgs.msg import Empty
from visualization_msgs.msg import Marker

class bebopSimulation():    
    def __init__(self):
	
        # initiliaze        
        rospy.init_node('fake_bebop', anonymous=False)
        rospy.loginfo("Starting fake bebop")
        
        # tell user how to stop Bebop
        rospy.loginfo("To stop CTRL + C")
        
        # What function to call when you ctrl + c    
        rospy.on_shutdown(self.shutdown)
        
        self.position = Point()    
        self.position.x  = 0
        self.position.y  = 0
        self.position.z  = 1
        self.orientation = 0
        self.flyingState = 'air'
        self.count = 0  # for marker id
        
        self.cmd_vel_subscriber  = rospy.Subscriber('/bebop/cmd_vel', Twist, self.readCmd_vel)        
        self.takeoff_subscriber  = rospy.Subscriber('/bebop/takeoff', Empty, self.readTakeoff)
        self.land_subscriber  = rospy.Subscriber('/bebop/land', Empty, self.readLand)
        self.pub_marker = rospy.Publisher('/robotMarker', Marker, queue_size=10)
        
        self.frequencyUpdate = 10;
        r = rospy.Rate(self.frequencyUpdate)
        t0 = time.time()
        
        self.cmd_vel = Twist()
        
        rospy.loginfo('Ready to takeoff')
        ######################################################
        while not rospy.is_shutdown():
            if  self.flyingState == 'air' :
                self.updatePosition()
            self.publishPoint()
            r.sleep()
        
        ######################################################
    def updatePosition(self):
        self.position.x = self.position.x + 1.0/self.frequencyUpdate*( math.cos(self.orientation)*self.cmd_vel.linear.x - math.sin(self.orientation)*self.cmd_vel.linear.y )
        self.position.y = self.position.y + 1.0/self.frequencyUpdate*( math.sin(self.orientation)*self.cmd_vel.linear.x + math.cos(self.orientation)*self.cmd_vel.linear.y )
        self.position.z = self.position.z + 1.0/self.frequencyUpdate*self.cmd_vel.linear.z
        self.orientation = self.orientation+1.0/self.frequencyUpdate*self.cmd_vel.angular.z
    
    def readCmd_vel(self,data):
        self.cmd_vel = data
        rospy.loginfo('Cmd_vel command recieved')
        
        #rospy.loginfo(self.position.z)
        
    def readLand(self,data):
        self.flyingState = 'ground'
        self.position.z = 0
        rospy.loginfo("Landed")
        
    def readTakeoff(self,data):
        self.flyingState = 'air'
        self.position.z = 1
        rospy.loginfo("Take Off")

    def publishPoint(self):
        self.robotMarker = Marker()
        self.robotMarker.header.frame_id = "world"
        self.robotMarker.header.stamp    = rospy.get_rostime()
        self.robotMarker.id = self.count
        self.robotMarker.type = 2 # sphere
        self.robotMarker.scale.x = 0.3
        self.robotMarker.scale.y = 0.3
        self.robotMarker.scale.z = 0.2
        
        self.robotMarker.pose.position.x = self.position.x
        self.robotMarker.pose.position.y = self.position.y
        self.robotMarker.pose.position.z = self.position.z
        self.robotMarker.pose.orientation.w = 1 
	self.robotMarker.pose.orientation.x = 0 
	self.robotMarker.pose.orientation.y = 0 
	self.robotMarker.pose.orientation.z = 0 
        
        self.robotMarker.color.r = 0.0
        self.robotMarker.color.g = 1.0
        self.robotMarker.color.b = 0.0
        self.robotMarker.color.a = 1.0

        self.robotMarker.lifetime = rospy.Duration(60)
        self.pub_marker.publish(self.robotMarker)
        self.count = self.count + 1
        
    def shutdown(self):
        self.takeoff_subscriber.unregister()
        self.land_subscriber.unregister() 
        self.cmd_vel_subscriber.unregister()
       
        
        rospy.loginfo("Stop Bebop")
        rospy.sleep(1)
 
if __name__ == '__main__':
    try:
        bebopSimulation()
    except rospy.ROSInterruptException:
        pass