#!/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")
planning_csv_path = rospy.get_param("planning_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 planning.csv
	planning_file = open(planning_csv_path, newline='')
	reader_p = csv.DictReader(planning_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')
	br = tf2_ros.TransformBroadcaster()
	path = rospy.Publisher('riptide_path', Marker, queue_size=10)
	way = rospy.Publisher('waypoint_line', Marker, queue_size=10)
	att = rospy.Publisher('orientation_vector', Marker, queue_size=10)
	joint = rospy.Publisher('joint_states', JointState, queue_size=10)
	lm = rospy.Publisher('landmarks', Marker, queue_size=10)
	
	# init msgs
	t = geometry_msgs.msg.TransformStamped()
	
	P = Marker()
	P.type = 4 # LINE_STRIP
	P.action = 0 # ADD
	P.scale.x = 0.2 # line width
	P.color.r = 1.0
	P.color.g = 1.0 
	P.color.a = 1.0 
	P.header.frame_id = "fixed_frame"
	P.id = 0
	P.pose.orientation.w = 1.0
	
	way_list = Marker()
	way_list.type = 4 # LINE_STRIP
	way_list.action = 0 # ADD
	way_list.scale.x = 0.1 # line width
	way_list.color.g = 1.0 # green color
	way_list.color.a = 1.0 
	way_list.header.frame_id = "fixed_frame"
	way_list.id = 2
	way_list.pose.orientation.w = 1.0
	
	Wk = Marker()
	Wk.type = 5 # LINE_LIST
	Wk.action = 0 # ADD
	Wk.scale.x = 0.05 # line width
	Wk.color.r = 1.0 # red color
	Wk.color.a = 1.0 
	Wk.header.frame_id = "fixed_frame"
	Wk.id = 3
	Wk.pose.orientation.w = 1.0
	
	J = JointState()
	J.name = ["base_to_propel","base_to_top_fin","base_to_right_fin","base_to_left_fin"]
	
	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
	
	# read planning
	for row in reader_p:
		p = geometry_msgs.msg.Point()
		p.x = float(row['akx'])
		p.y = float(row['aky'])
		p.z = float(row['akz'])
		way_list.points.append(p)
		Wk.points.append(p)
		
		# add the second point of the line for W
		p2 = geometry_msgs.msg.Point()
		p2.x = p.x + float(row['wkx'])
		p2.y = p.y + float(row['wky'])
		p2.z = p.z + float(row['wkz'])
		Wk.points.append(p2)
	
	# 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(row['lmz'])
		Lm.points.append(p)
	
	
	# read simulation
	for row in reader_s:
		t.header.stamp = rospy.Time.now()
		t.header.frame_id = "fixed_frame"
		t.child_frame_id = "base_link"

		t.transform.translation.x = float(row['px'])
		t.transform.translation.y = float(row['py'])
		t.transform.translation.z = float(row['pz'])
		t.transform.rotation.w = float(row['q1'])
		t.transform.rotation.x = float(row['q2'])
		t.transform.rotation.y = float(row['q3'])
		t.transform.rotation.z = float(row['q4'])
		
		p = geometry_msgs.msg.Point()
		p.x = float(row['px'])
		p.y = float(row['py'])
		p.z = float(row['pz'])
		P.header.stamp = rospy.Time.now()
		P.points.append(p)
		
		J.position = [float(row['b']),float(row['u2']),float(row['u3']),float(row['u4'])] 
		J.header.stamp = rospy.Time.now()
	
		br.sendTransform(t)
		path.publish(P)
		way.publish(way_list)
		att.publish(Wk)
		joint.publish(J)
		lm.publish(Lm)

		
		rospy.sleep(0.1)
		if rospy.is_shutdown():
			break

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