#!/usr/bin/env python

'''This python file runs a ROS-node of name drone_control which holds the position of e-Drone on the given dummy as well as of Real drone.
This node publishes and subsribes the following topics:

		PUBLICATIONS			SUBSCRIPTIONS
		/drone_command			/whycon/poses
		/alt_error				/pid_tuning_altitude
		/pitch_error			/pid_tuning_pitch
		/roll_error				/pid_tuning_roll
		/yaw_error				/pid_tuning_yaw
								/drone_yaw
'''

# Importing the required libraries

from plutodrone.msg import *
from geometry_msgs.msg import PoseArray
from std_msgs.msg import Int16
from std_msgs.msg import Int64
from std_msgs.msg import Float64
from pid_tune.msg import PidTune
import rospy
import time


global Iterm 
global error
global start_time
global lasttime
global kp	
global ki
global kd
start_time=0
lasttime=0
start_time=time.time()
last=start_time

Iterm = [0,0,0,0]
error = [0,0,0,0]
kp = [0,0,0,0]
ki = [0,0,0,0]
kd = [0,0,0,0]
	
class Edrone():
	"""docstring for Edrone"""
	def __init__(self):
		
		#rospy.init_node('drone_control')	# initializing ros node with name drone_control
		rospy.init_node('pid_node')
		# This corresponds to your current position of drone. This value must be updated each time in your whycon callback
		# [x,y,z,yaw_value]
		self.drone_position = [0,0,0,0]	

		# [x_setpoint, y_setpoint, z_setpoint, yaw_value_setpoint]
		self.setpoint = [0.0,0.0,20.0,0.0]


		#Declaring a cmd of message type PlutoMsg and initializing values
		self.cmd = PlutoMsg()
		self.cmd.rcRoll = 1500
		self.cmd.rcPitch = 1500
		self.cmd.rcYaw = 1500
		self.cmd.rcThrottle = 1500
		self.cmd.rcAUX1 = 1500
		self.cmd.rcAUX2 = 1500
		self.cmd.rcAUX3 = 1500
		self.cmd.rcAUX4 = 1500


		#initial setting of Kp, Kd and ki for [throttle, pitch, roll, yaw]. 
		self.Kp = [10.02,7.26,7.38,2.7]
		self.Ki = [0,0,0,0]
		self.Kd = [26.4,0,1.5,3.6]
			
		self.max_values = [1800,1800,1800,1800]
		self.prev_values = [0,0,0,0]
		self.min_values = [1200,1200,1200,1200]

		self.sample_time = 0.7 # in seconds	

		# Publishing /drone_command, /alt_error, /pitch_error, /roll_error, /yaw_error
		self.command_pub = rospy.Publisher('/drone_command', PlutoMsg, queue_size=1)
		self.command_pub1 = rospy.Publisher('/alt_error',Float64, queue_size=1)
		self.command_pub2 = rospy.Publisher('/pitch_error',Float64, queue_size=1)
		self.command_pub3 = rospy.Publisher('/roll_error', Float64, queue_size=1)
		self.command_pub4 = rospy.Publisher('/yaw_error', Float64, queue_size=1)
		



		#-----------------------------------------------------------------------------------------------------------


		# Subscribing to /whycon/poses, /drone_yaw, /pid_tuning_altitude, /pid_tuning_pitch, pid_tuning_roll
		rospy.Subscriber('whycon/poses', PoseArray, self.whycon_callback)
		rospy.Subscriber('/pid_tuning_altitude',PidTune,self.altitude_set_pid)
		rospy.Subscriber('/pid_tuning_pitch',PidTune,self.pitch_set_pid)
		rospy.Subscriber('/pid_tuning_roll',PidTune,self.roll_set_pid)
		rospy.Subscriber('/pid_tuning_yaw',PidTune,self.yaw_set_pid1)
		rospy.Subscriber('/drone_yaw',Float64,self.yaw_set_pid)
		
		#------------------------------------------------------------------------------------------------------------

		self.arm() # ARMING THE DRONE


	# Disarming condition of the drone
	def disarm(self):
		self.cmd.rcAUX4 = 1100
		self.command_pub.publish(self.cmd)
		rospy.sleep(1)


	# Arming condition of the drone 
	def arm(self):

		self.disarm()

		self.cmd.rcRoll = 1500
		self.cmd.rcYaw = 1500
		self.cmd.rcPitch = 1500
		self.cmd.rcThrottle = 1000
		self.cmd.rcAUX4 = 1500
		self.command_pub.publish(self.cmd)	# Publishing /drone_command
		rospy.sleep(1)



	# Whycon callback function
	# The function gets executed each time when /whycon node publishes /whycon/poses 
	def whycon_callback(self,msg):
		self.drone_position[0] = msg.poses[0].position.x
		self.drone_position[1] = msg.poses[0].position.y
		self.drone_position[2] = msg.poses[0].position.z
			
		#---------------------------------------------------------------------------------------------------------------



	# Callback function for /pid_tuning_altitude
	# This function gets executed each time when /tune_pid publishes /pid_tuning_altitude
	def altitude_set_pid(self,alt):
		self.Kp[0] = alt.Kp * 0.06 
		self.Ki[0] = alt.Ki * 0.008
		self.Kd[0] = alt.Kd * 0.3
		
	def pitch_set_pid(self,pitch):
		self.Kp[1] = pitch.Kp * 0.06  
		self.Ki[1] = pitch.Ki * 0.008
		self.Kd[1] = pitch.Kd * 0.3
		
	
	def roll_set_pid(self,roll):
		self.Kp[2] = roll.Kp * 0.06 
		self.Ki[2] = roll.Ki * 0.08
		self.Kd[2] = roll.Kd * 0.3
		

	def yaw_set_pid(self,msg):
		self.drone_position[3] = msg.data
		

	

	def yaw_set_pid1(self,yaw):
		self.Kp[3] = yaw.Kp * 0.06 
		self.Ki[3] = yaw.Ki * 0.008
		self.Kd[3] = yaw.Kd * 0.3


	#----------------------------------------------------------------------------------------------------------------------


	def pid(self):
		
			
	
		error[0] = self.drone_position[0] - self.setpoint[0]
		error[1] = self.drone_position[1] - self.setpoint[1]
		error[2] = self.drone_position[2] - self.setpoint[2]
		error[3] = self.drone_position[3] - self.setpoint[3]
			
		kp[0] = self.Kp[0]
		ki[0] = self.Ki[0] * (self.sample_time) 
		kd[0] = self.Kd[0] / (self.sample_time )
			
		kp[1] = self.Kp[1]
		ki[1] = self.Ki[1] * (self.sample_time)
		kd[1] = self.Kd[1] / self.sample_time 

		kp[2] = self.Kp[2]
		ki[2] = self.Ki[2] * (self.sample_time)  
		kd[2] = self.Kd[2] / self.sample_time
			
		kp[3] = self.Kp[3]
		ki[3] = self.Ki[3] * (self.sample_time) 
		kd[3] = self.Kd[3] / (self.sample_time) 

			
				
			
		Iterm[0]=(error[0]*ki[1]+Iterm[0])
		self.out_pitch=error[0]*kp[1]+(error[0]-self.prev_values[0])*kd[1]+Iterm[0]
			
			
		
		Iterm[1]=(error[1]*ki[2]+Iterm[1])
		self.out_roll=error[1]*(kp[2])+(error[1]-self.prev_values[1])*kd[2]+Iterm[1]
			
		
		Iterm[2]=(error[2]*ki[0]+Iterm[2])
		self.out_throttle=error[2]*kp[0]+(error[2]-self.prev_values[2])*kd[0]+Iterm[2]


		Iterm[3]=(error[3]*ki[3]+Iterm[3])
		self.out_yaw=-error[3]*kp[3]-(error[3]-self.prev_values[3])*kd[3]+Iterm[3]
		
		self.cmd.rcRoll = 1500 + self.out_roll
		self.cmd.rcPitch = 1500 + self.out_pitch
		
				
		self.cmd.rcThrottle =1500 +self.out_throttle
				
		self.cmd.rcYaw=1500+self.out_yaw
			
		
			

				
			
			
		if self.cmd.rcPitch > self.max_values[1]:
			self.cmd.rcPitch = self.max_values[1]
		if self.cmd.rcRoll > self.max_values[2]:
			self.cmd.rcRoll = self.max_values[2]
		if self.cmd.rcThrottle > self.max_values[0]:
			self.cmd.rcThrottle = self.max_values[0]
		if self.cmd.rcYaw > self.max_values[3]:
			self.cmd.rcYaw = self.max_values[3]



		if self.cmd.rcPitch < self.min_values[1]:
			self.cmd.rcPitch = self.min_values[1]
		if self.cmd.rcRoll < self.min_values[2]:
			self.cmd.rcRoll = self.min_values[2]
		if self.cmd.rcThrottle < self.min_values[0]:
			self.cmd.rcThrottle = self.min_values[0]
		if self.cmd.rcYaw < self.min_values[3]:
			self.cmd.rcYaw = self.min_values[3]
			


		self.command_pub1.publish(self.out_throttle)
		self.command_pub2.publish(self.out_pitch)
		self.command_pub3.publish(self.out_roll)
		self.command_pub4.publish(self.out_yaw)
		



		self.prev_values[0]=error[0]
		self.prev_values[1]=error[1]
		self.prev_values[2]=error[2]
		self.prev_values[3]=error[3]
			
		self.command_pub.publish(self.cmd)

			


		#------------------------------------------------------------------------------------------------------------------------
    
if __name__ == '__main__':

	e_drone = Edrone()

	while not rospy.is_shutdown():
		e_drone.pid()
		rospy.sleep(0.7)
