import numpy as np
import matplotlib.pyplot as plt


def read_data(dir_str):
    '''
    此函数读取txt文件中的数据
    数据内容：科学计数法保存的多行两列数据
    输入：txt文件的路径
    输出：小数格式的数组，行列与txt文件中相同
    '''
    data_temp=[]
    with open(dir_str) as fdata:
        while True:
            line=fdata.readline()
            if not line:
                break
            #data_temp.append([])
            #print(line)
            #print(i for i in line.split(" "))
            data_temp.append([float(i) for i in line.split(' ')])
    return np.array(data_temp)


data_array1 = read_data('6400_symmetry-refmotion-and-state_0.62_0.38_track0.005.txt')
data_array2 = read_data('9300_symmetry-refmotion-and-state_0.62_0.38_track0.01.txt')
data_array3 = read_data('9300_symmetry-refmotion-and-state_0.62_0.38_track0.01.txt')
print(data_array1.shape)
print(data_array2.shape)
print(data_array3.shape)
length1 = data_array1.shape[0]
length2 = data_array2.shape[0]
length3 = data_array3.shape[0]

min_length = min(length1,length2,length3)


timeline = np.arange(min_length)
timeline = timeline * 0.032
ref_vel = timeline * 0.313
# plt.plot(data_array1[:,0])
# plt.plot(data_array1[:,1])
# plt.plot(timeline, data_array1[0:min_length,2], label='Tracking0.01_without_symmetry')
plt.plot(timeline, data_array2[0:min_length,2], label='symmetry_ref_motion_and_state_track0.01')
# plt.plot(timeline, data_array3[0:min_length,2], label='no_symmetry')
plt.plot(timeline, ref_vel, label='ref_vel')
plt.legend(loc='best')
plt.show()
# plt.savefig('comparison_robot_forward_distance.svg',dpi=600,format='svg')