#! /usr/bin/env python
# coding=utf-8

import rospy

from nav_msgs.msg import Odometry
from geometry_msgs.msg import Quaternion
from geometry_msgs.msg import Twist
from tf.transformations import euler_from_quaternion

from math import sin, cos, pi
from numpy import sign


#######################################################################################################################
class KlancarLine:
#######################################################################################################################

	###################################################################################################################

	def __init__(self):


		##### Inicialização do nó #####


		rospy.init_node("klancar_line")


		##### Parâmetros #####


		# Frequência

		self.frequency = rospy.get_param("~frequency", 10)
		self.Ts = 1.0 / self.frequency

		# Parâmetros da reta a ser seguida

		self.distance = rospy.get_param("~distance", 1.0)
		self.base_speed = rospy.get_param("~base_speed", 0.1)

		# Dimensões

		self.wheel_diameter = rospy.get_param("~wheel_diameter", 0.17)
		self.wheel_separation = rospy.get_param("~wheel_separation", 0.405)

		# Parâmetros do controlador 	<-----------------------------------------------------------#################

		self.ksi = rospy.get_param("~ksi", 0.3)
		self.w_n = rospy.get_param("~w_n", 1)
		self.g = rospy.get_param("~g", 40)

		# Valores máximos de velocidade

		self.Vb_max = rospy.get_param("~Vb_max", 0.15)
		self.Wb_max = rospy.get_param("~Wb_max", 0.50)


		##### Arquivo para salvar os resultados #####

		path = "/home/nanook/nanook_ros/src/nanook_path_tracking/controlador_klancar/ensaios/ensaio_reta.txt"
		self.data_file = open(path, 'w')


		##### Publishers #####


		self.speed_ref_pub = rospy.Publisher("cmd_vel", Twist, queue_size = 10)


		##### Subscribers #####


		rospy.Subscriber("odom", Odometry, self.get_odometry)


		##### Geração da trajetória #####


		# Inicializando as listas

		self.x_ref = []
		self.y_ref = []
		self.theta_ref = []

		self.Vb_ref = []
		self.Wb_ref = []

		# Gerando a trajetória

		self.generate_line(self.distance, self.base_speed, self.Ts)

		# Número de iterações

		self.N_iters = len(self.x_ref)


		##### Variáveis #####

		# Índice da trajetória de referência

		self.index = 0

		# Pose e velocidade da base do robô

		self.x = 0.0
		self.y = 0.0
		self.theta = 0.0

		self.Vb = 0.0
		self.Wb = 0.0

		# Nova pose e velocidade da base recebida (válida na próxima iteração)

		self.new_x = 0.0
		self.new_y = 0.0
		self.new_theta = 0.0

		self.new_Vb = 0.0
		self.new_Wb = 0.0

		# Sinais de controle de velocidades linear e angular a serem enviados para a base

		self.Vb_u = 0.0
		self.Wb_u = 0.0

	###################################################################################################################


	###################################################################################################################

	def generate_line(self, distance, base_speed, Ts):

		# Gera uma trajetória reta com velocidade da base constante

		# Número de pontos

		N = int(distance / (base_speed * Ts)) + 1

		# Parâmetros constantes

		self.y_ref = [0.0] * N
		self.theta_ref = [0.0] * N

		self.Vb_ref = [base_speed] * N
		self.Wb_ref = [0.0] * N

		# Gerando pontos no eixo x

		for i in range(N):

			self.x_ref.append(base_speed * i * Ts)

	###################################################################################################################


	###################################################################################################################

	def get_odometry(self, odometry):

		# Recebe informação de odometria do nó controlador de base

		# Convertendo a orientação de Quaternion para Euler

		orientation = odometry.pose.pose.orientation

		(row, pitch, yaw) = euler_from_quaternion([orientation.x, orientation.y, orientation.z, orientation.w])		

		# Atualizando a pose

		self.new_x = odometry.pose.pose.position.x
		self.new_y = odometry.pose.pose.position.y
		self.new_theta = yaw

		# Convertendo a velocidade para o frame da base

		Vb = odometry.twist.twist.linear.x / cos(self.new_theta)

		# Atualizando as velocidades

		self.new_Vb = Vb
		self.new_Wb = odometry.twist.twist.angular.z

	###################################################################################################################


	###################################################################################################################

	def control_pose(self):

		# Controlador de trajetória de Klancar

		# Cálculo dos erros

		x_error = self.x_ref[self.index] - self.x
		y_error = self.y_ref[self.index] - self.y
		theta_error = self.theta_ref[self.index] - self.theta

		if theta_error < - pi:
			theta_error = - theta_error

		# Fatores e

		e1 = cos(self.theta) * x_error + sin(self.theta) * y_error
		e2 = cos(self.theta) * y_error - sin(self.theta) * x_error
		e3 = theta_error

		# Ganhos

		k1 = 2 * self.ksi * self.w_n
		k2 = self.g * abs(self.Vb_ref[self.index])
		k3 = k1

		v1 = -k1 * e1
		v2 = -sign(self.Vb_ref[self.index]) * k2 * e2 - k3 * e3

		# Velocidades linear e angular de referência para a base

		self.Vb_u = self.Vb_ref[self.index] * cos(e3) - v1
		self.Wb_u = self.Wb_ref[self.index] - v2

		# Limitação das velocidades

		if self.Vb_u > self.Vb_max:
			self.Vb_u = self.Vb_max

		if self.Vb_u < -self.Vb_max:
			self.Vb_u = -self.Vb_max

		if self.Wb_u > self.Wb_max:
			self.Wb_u = self.Wb_max

		if self.Wb_u < -self.Wb_max:
			self.Wb_u = -self.Wb_max

	###################################################################################################################


	###################################################################################################################

	def publish_speed_refs(self):

		# Publica as referências de velocidade linear e angular de base para o nó controlador de base

		# Criação da mensagem tipo Twist

		reference = Twist()
		reference.linear.x = self.Vb_u		# Velocidade linear em x (m/s)
		reference.angular.z	= self.Wb_u		# Velocidade angular em z (rad/s)

		# Publicação da referência

		self.speed_ref_pub.publish(reference)

	###################################################################################################################


	###################################################################################################################

	def spin(self):

		# Laço principal

		while not rospy.is_shutdown() and (self.index < self.N_iters):

			# Tempo no início da iteração

			t_iter = rospy.get_time()

			print("*** Iteração % d ***" % (self.index + 1))

			# Atualizando a pose e velocidade da base para as últimas recebidas

			self.x = self.new_x
			self.y = self.new_y
			self.theta = self.new_theta

			self.Vb = self.new_Vb
			self.Wb = self.new_Wb

			print("* Pose de Referência:")
			print("- x_ref = %.5f m" % self.x_ref[self.index])
			print("- y_ref = %.5f m" % self.y_ref[self.index])
			print("- theta_ref = %.5f rad" % self.theta_ref[self.index])
			print("- Vb_ref = %.5f m/s" % self.Vb_ref[self.index])
			print("- Wb_ref = %.5f rad/s" % self.Wb_ref[self.index])

			print("* Pose Atual:")
			print("- x = %.5f m" % self.x)
			print("- y = %.5f m" % self.y)
			print("- theta = %.5f rad" % self.theta)
			print("- Vb = %.5f m/s" % self.Vb)
			print("- Wb = %.5f rad/s" % self.Wb)

			# Iteração do controle de pose
		
			self.control_pose()

			# Publicação das velocidades de referência para o nó controlador de base

			self.publish_speed_refs()

			# Salvando os dados no arquivo

			# ÍNDICE | TEMPO | X-REF | Y-REF | 0-REF | Vb-REF | Wb-REF | X | Y | 0 | Vb | Wb | Vb-u | Wb-u

			index_str = "%d" % self.index
			time_str = "%.5f" % t_iter

			x_ref_str = "%.5f" % self.x_ref[self.index]
			y_ref_str = "%.5f" % self.y_ref[self.index]
			theta_ref_str = "%.5f" % self.theta_ref[self.index]
			Vb_ref_str = "%.5f" % self.Vb_ref[self.index]
			Wb_ref_str = "%.5f" % self.Wb_ref[self.index]

			x_str = "%.5f" % self.x
			y_str = "%.5f" % self.y
			theta_str = "%.5f" % self.theta
			Vb_str = "%.5f" % self.Vb
			Wb_str = "%.5f" % self.Wb

			Vb_u_str = "%.5f" % self.Vb_u
			Wb_u_str = "%.5f" % self.Wb_u

			self.data_file.write(index_str + " " + 
								 time_str + " " +
								 x_ref_str + " " +
								 y_ref_str + " " +
								 theta_ref_str + " " +
								 Vb_ref_str + " " + 
								 Wb_ref_str + " " +
								 x_str + " " +
								 y_str + " " +
								 theta_str + " " + 
								 Vb_str + " " +
								 Wb_str + " " +
								 Vb_u_str + " " +
								 Wb_u_str + "\n")

			# Incremento do índice

			self.index += 1

			# Duração da iteração

			dt_iter = rospy.get_time() - t_iter

			print("* Duração da Iteração: %.5f s" % dt_iter)
			print

			# Aguarda o tempo restante para completar o período de amostragem

			rospy.sleep(self.Ts - dt_iter)

	###################################################################################################################


	###################################################################################################################

	def finalize(self):

		# Finalização

		# Parar o robô

		self.publish_speed_refs(0.0, 0.0)

		# Fechar o arquivo com os dados

		self.data_file.close()

	###################################################################################################################


#######################################################################################################################
### EXECUÇÃO
#######################################################################################################################

if __name__ == '__main__':

	# Instanciação do controlador de trajetória

	klancar_line = KlancarLine()

	# Execução do laço principal

	klancar_line.spin()

	# Finalização

	klancar_line.finalize()

	print("[INFO] Trajetória concluída.")
