#!/usr/bin/env python3

import rospy 
import tf_conversions
import tf2_ros
import csv
import geometry_msgs.msg 
from visualization_msgs.msg import Marker, MarkerArray
from sensor_msgs.msg import JointState
from math import sqrt

state_csv_path = rospy.get_param("state_csv_path")
landmark_csv_path = rospy.get_param("landmark_csv_path")

def talker():
	
	# open state.csv
	state_file = open(state_csv_path, newline='')
	reader_s = csv.DictReader(state_file)
	
	# open landmark.csv
	landmark_file = open(landmark_csv_path, newline='')
	reader_l = csv.DictReader(landmark_file)
	
	#start node and publishers 
	rospy.init_node('csv_2_rviz')
	real = rospy.Publisher('real_position', Marker, queue_size=10)
	estim_int = rospy.Publisher('int_estimated_position', Marker, queue_size=10)
	estim_kal = rospy.Publisher('kal_estimated_position', Marker, queue_size=10)
	lm = rospy.Publisher('landmarks', Marker, queue_size=10)
	lake_pub = rospy.Publisher('lake', Marker, queue_size=10)
	cov_ellipse = rospy.Publisher('estim_ellipse', Marker, queue_size=10)
	particles = rospy.Publisher('estim_particles', Marker, queue_size=10)
	init_kal = rospy.Publisher('init_estim', Marker, queue_size=10)
	
	locs = []
	for i in range(30):
		locs.append( rospy.Publisher('position_interval_'+str(i), Marker, queue_size=10) )
	
	# init msgs
	
	P = Marker()
	P.type = 2 # SPHERE
	P.scale.x = 0.2 # line width
	P.color.r = 1.0 
	P.color.g = 1.0
	P.color.a = 1.0 
	P.scale.x = 1
	P.scale.y = 1
	P.scale.z = 1
	P.header.frame_id = "fixed_frame"
	P.id = 0
	P.pose.orientation.w = 1.0
	
	P0 = Marker()
	P0.type = 2 # SPHERE
	P0.color.r = 0.0 
	P0.color.g = 0.0
	P0.color.b = 0.0
	P0.color.a = 1.0 
	P0.scale.x = 1
	P0.scale.y = 1
	P0.scale.z = 1
	P0.header.frame_id = "fixed_frame"
	P0.id = 200
	P0.pose.orientation.w = 1.0
	
	P_hat_int = Marker()
	P_hat_int.type = 2 # SPHERE
	P_hat_int.color.r = 1.0
	P_hat_int.color.b = 1.0
	P_hat_int.color.a = 1.0 
	P_hat_int.scale.x = 1
	P_hat_int.scale.y = 1
	P_hat_int.scale.z = 1
	P_hat_int.header.frame_id = "fixed_frame"
	P_hat_int.pose.orientation.w = 1.0
	P_hat_int.id = 1
	
	P_hat_kal = Marker()
	P_hat_kal.type = 2 # SPHERE
	P_hat_kal.color.g = 1.0
	P_hat_kal.color.a = 1.0 
	P_hat_kal.scale.x = 1
	P_hat_kal.scale.y = 1
	P_hat_kal.scale.z = 1
	P_hat_kal.header.frame_id = "fixed_frame"
	P_hat_kal.pose.orientation.w = 1.0
	P_hat_kal.id = 2
	
	Locs = []        # interval boxes for position
	for i in range(30):
		Locs.append(Marker())
		Locs[i].type = 1 # CUBE
		Locs[i].action = 0 # ADD
		Locs[i].color.r = 1.0
		Locs[i].color.b = 1.0 # color
		Locs[i].color.a = 0.5 
		Locs[i].header.frame_id = "fixed_frame"
		Locs[i].pose.orientation.w = 1.0
		Locs[i].id = 100+i
	
	Lm = Marker()
	Lm.type = 7 # SPHERE_LIST 
	Lm.action = 0 # ADD
	Lm.color.r = 1.0
	Lm.color.a = 1.0 
	Lm.scale.x = 1
	Lm.scale.y = 1
	Lm.scale.z = 1
	Lm.header.frame_id = "fixed_frame"
	Lm.pose.orientation.w = 1.0
	Lm.id = 5
	
	Lake = Marker()
	Lake.type = 1 # CUBE
	Lake.color.b = 1.0 # color
	Lake.color.a = 0.2 
	Lake.header.frame_id = "fixed_frame"
	Lake.pose.orientation.w = 1.0
	Lake.id = 6
	
	Elli = Marker()
	Elli.type = 2 # SPHERE
	Elli.action = 0
	Elli.color.g = 1.0
	Elli.color.a = 0.7
	Elli.header.frame_id = "fixed_frame"
	Elli.id = 20
	
	Part = Marker()
	Part.type = 7 # SPHERE LIST
	Part.action = 0 # ADD
	Part.color.b = 1.0 
	Part.color.a = 1.0 
	Part.scale.x = 0.5
	Part.scale.y = 0.5
	Part.scale.z = 0.5
	Part.header.frame_id = "fixed_frame"
	Part.id = 100
	Part.pose.orientation.w = 1.0
	
	# read landmarks
	for row in reader_l:
		p = geometry_msgs.msg.Point()
		p.x = float(row['lmx'])
		p.y = float(row['lmy'])
		p.z = float(0)
		Lm.points.append(p)
	
	
	# read simulation
	for row in reader_s:
		P.pose.position.x = float(row['px'])
		P.pose.position.y = float(row['py'])
		P.pose.position.z = float(0)
		
		P0.pose.position.x = float(row['p0_x'])
		P0.pose.position.y = float(row['p0_y'])
		P0.pose.position.z = float(0)

		P_hat_int.pose.position.x = float(row['px_hat_int'])
		P_hat_int.pose.position.y = float(row['py_hat_int'])
		P_hat_int.pose.position.z = float(0)
		
		P_hat_kal.pose.position.x = float(row['px_hat_kal'])
		P_hat_kal.pose.position.y = float(row['py_hat_kal'])
		P_hat_kal.pose.position.z = float(0)

		for i in range(30):
			Locs[i].pose.position.x = float(row['x_box_'+str(i)])
			Locs[i].pose.position.y = float(row['y_box_'+str(i)])
			Locs[i].pose.position.z = float(0)
			Locs[i].scale.x = float(row['lx_box_'+str(i)])
			Locs[i].scale.y = float(row['ly_box_'+str(i)])
			Locs[i].scale.z = float(0.1)
			
		Lake.pose.position.x = float(row['lake_px'])
		Lake.pose.position.y = float(row['lake_py'])
		Lake.pose.position.z = float(0)
		Lake.scale.x = float(row['lake_lx'])
		Lake.scale.y = float(row['lake_ly'])
		Lake.scale.z = float(0.05)
		
		Elli.pose.position = P_hat_kal.pose.position
		Elli.pose.orientation.w = float(row['cov_q1'])
		Elli.pose.orientation.x = float(row['cov_q2'])
		Elli.pose.orientation.y = float(row['cov_q3'])
		Elli.pose.orientation.z = float(row['cov_q4'])
		Elli.scale.x = sqrt(float(row['cov_l1']))
		Elli.scale.y = sqrt(float(row['cov_l2']))
		Elli.scale.z = sqrt(float(row['cov_l3']))
		
		for j in range(int(row['n_part'])):
			part = geometry_msgs.msg.Point()
			part.x = float(row['x_part_'+str(j)])
			part.y = float(row['y_part_'+str(j)])
			part.z = float(0)
			Part.points.append(part)

	while not rospy.is_shutdown():
		real.publish(P)
		estim_int.publish(P_hat_int)
		estim_kal.publish(P_hat_kal)
		cov_ellipse.publish(Elli)
		lm.publish(Lm)
		for i in range(30):
			locs[i].publish(Locs[i])
		lake_pub.publish(Lake)
		particles.publish(Part)
		init_kal.publish(P0)
		rospy.sleep(0.1)

if __name__ == '__main__':
	
	try:
		talker()
	except rospy.ROSInterruptException:
		pass
