#!/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")

def talker():
	
	# open state.csv
	state_file = open(state_csv_path, newline='')
	reader_s = csv.DictReader(state_file)
	
	#start node and publishers 
	rospy.init_node('csv_2_rviz')
	br = tf2_ros.TransformBroadcaster()
	path = rospy.Publisher('riptide_path', Marker, queue_size=10)
	joint = rospy.Publisher('joint_states', JointState, queue_size=10)
	boat_surface = rospy.Publisher('boat_surface', Marker, queue_size=10)
	boat_elli = rospy.Publisher('boat_elli', 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
	
	F = Marker()
	F.type = 4 # LINE_STRIP
	F.action = 0 # ADD
	F.scale.x = 0.2 # line width
	F.color.g = 1.0
	F.color.a = 1.0 
	F.header.frame_id = "fixed_frame"
	F.id = 1
	F.pose.orientation.w = 1.0
	
	J = JointState()
	J.name = ["base_to_propel","base_to_top_fin","base_to_right_fin","base_to_left_fin"]	
	
	Elli = Marker()
	Elli.type = 2 # SPHERE
	Elli.action = 0
	Elli.color.r = 1.0
	Elli.color.a = 0.3
	Elli.header.frame_id = "fixed_frame"
	Elli.id = 20
	Elli.pose.orientation.w = 1.0
	Elli.scale.x = 20.0*2
	Elli.scale.y = 3.0*2
	Elli.scale.z = 5.0*2
	
	# 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)
		
		f = geometry_msgs.msg.Point()
		f.x = float(row['px'])
		f.y = float(row['py'])
		f.z = float(row['pz']) + float(row['h'])
		F.header.stamp = rospy.Time.now()
		F.points.append(f)
		
		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)
		joint.publish(J)
		boat_surface.publish(F)
		boat_elli.publish(Elli)
		
		
		rospy.sleep(0.1)
		if rospy.is_shutdown():
			break

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