#!/usr/bin/python3
import sys
sys.path.append('master_plot')
sys.path.append('../')

from plot_lib import animate_path

import numpy as np
import rospy
from gazebo_msgs.msg import ModelStates
import matplotlib.pyplot as plt
import threading
from master_msgs.msg import node_frame2

# fig, ax = plt.subplots(2, 2)
# ax = plt.subplot(2, 1, 2)

fig, axs = plt.subplots()

ax_agents = axs
# ax_agents = plt.subplot(221)
# ax_agent = plt.subplot(222)
# ax_dis_anchor = plt.subplot(212)

anp_agents = animate_path(fig, ax_agents, 8, name="robot", keep = True)
# anp_agent = animate_path(fig, ax_agent, 2, keep=True)
# anp_dis_anchor = animate_path(fig, ax_dis_anchor, 5, mode='1d', name ='agent2toAnchor', ylim = [-70, -90])

def callback(data):

    coordinate_list = []

    agent_names = data.name
    agent_poses = data.pose
    agent_twists= data.twist

    for i, pose in enumerate(agent_poses):
        if agent_names[i] != "ground_plane":
            x = pose.position.x
            y = pose.position.y

            coordinate_list.append([x, y])

    anp_agents.update_coordinate(coordinate_list)
    # anp_agent.update_coordinate(coordinate_list)

def uwb_callback(data):

    data_list = []

    for node in data.nodes:
        # data_list.append(node.distance)
        data_list.append(node.fpRssi)

    if len(data_list) == 5:
        anp_dis_anchor.update_data_1d(data_list)

def path_plot():

    rospy.init_node('rt_path_node')
    rospy.Subscriber('/gazebo/model_states', ModelStates, callback)
    # rospy.Subscriber('/agent3/nlink_linktrack_nodeframe2', node_frame2, uwb_callback)
    
    anp_agents.ani_plot()
    # anp_agent.ani_plot()
    # anp_dis_anchor.ani_plot()

    plt.show()

def spin_callback():
    rospy.spin()
    
# def plot_sub():
#     anp_1.ani_plot()

if __name__ == "__main__":

    thread_callback = threading.Thread(target = spin_callback, name='ros_callback', daemon=True)
    path_plot()
    thread_callback.start()
    
    # thread_subplot = threading.Thread(target = plot_sub, name='subplot', daemon=True)
    # thread_subplot.start()

    
    
    