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_array = read_data('8400_4direction_10Nor6N_8step-200step_continous_noise0.01_with_noise.txt')
print(data_array.shape)
print(data_array)

euler_x = []
euler_y = []
euler_z = []
force_x = []
force_z = []
for i in range(data_array.shape[0]):
    euler_x.append(data_array[i][3])
    euler_y.append(data_array[i][4])
    euler_z.append(data_array[i][5])
    force_x.append(data_array[i][35])
    force_z.append(data_array[i][37])
euler_x = np.array(euler_x)
euler_y = np.array(euler_y)
euler_z = np.array(euler_z)
force_x = np.array(force_x)
force_z = np.array(force_z)
print(euler_x)
print(euler_y)
print(euler_z)

time = np.arange(euler_x.shape[0])
time = time*0.008
print(euler_x.shape)
#x,y,z position

plt.rcParams['figure.figsize'] = (10.0, 4.0)

fig = plt.figure()
ax = fig.add_subplot(111)
#ax.plot(time,euler_x,label='euler_x',color='r')
#ax.plot(time,euler_y,label='euler_y',color='g')
ax.plot(time,euler_z,label='euler_z',color='b')
ax.plot(time,euler_x,label='euler_z',color='r')
ax.plot(time,euler_y,label='euler_z',color='g')
ax.legend(loc='upper left')
ax.set_xlabel('Time (s)')
ax.set_ylabel('Orientation (rad)')
ax.set_ylim(-12,32)

#ax.set_ylim(15, 35)
ax2 = ax.twinx()
ax2.plot(time,force_x,label='force_x',color='black')
ax2.plot(time,force_z,label='force_z',color='c')
#ax2.plot(time,force_z,label='force_z',color='c')
ax2.legend(loc='upper right')
#ax2.xlabel('Time (s)')
ax2.set_ylabel('External Force (N)')
ax2.set_ylim(-11, 11)
plt.show()
