"""
将所有向量转换为(x,y,z)的形式, 以减少计算叉乘时的负担, 并且加入画图函数
"""
import numpy as np
import matplotlib.pyplot as plt
import calculate as cal
import matplotlib
import matplotlib.animation as animation

class Car:
    def __init__(self, m, V, OMEGA, theta, omegas, pos, miu, w, l, r, J,j):
        self.m = m  # 质量
        self.V = V  # 质心速度
        self.OMEGA = OMEGA  # 车身角速度
        self.theta = theta  # 偏转角  车前后中心轴与x轴夹角
        self.omegas = np.ones((4)) * omegas  # 四个车轮的角速度
        self.pos = pos  # 初始质心位置
        self.miu = miu  # 摩擦系数
        self.w = w  # 汽车宽度
        self.l = l  # 汽车长度
        self.R = (w**2+l**2)**0.5  # 外接圆半径
        self.r = r # 轮胎半径
        self.ksi = cal.angles_relative(self.l, self.w)
        self.VM = np.zeros((4,2))  # 轮胎中心速度
        self.VP = np.zeros((4,2))  # 轮胎接触点速度
        self.frottements = np.zeros((4,2))  # 摩擦力
        self.moments = np.zeros(4)
        self.g = 9.8
        self.J = cal.Momentum_of_Inertia(self.l, self.w, self.m)  # 转动惯量
        self.OM_angle = cal.angles_relative(self.l,self.w)  # 每个轮胎与质心连线与x轴的角度

        self.j = j # 单个轮胎的转动惯量

    def calculate_vitesses(self):
        Vx = self.V[0]
        Vy = self.V[1]
        for k in range(4):
            # 轮胎中心速度
            # 质心坐标系速度大小:
            V_module = self.OMEGA * self.R
            ang = self.theta + self.ksi[k] + np.pi/2
            V_direction = cal.angle2vector(ang)
            V_MK = self.V + V_direction * V_module
            # 轮胎接触点速度:
            Vpk = V_MK + self.omegas[k] * self.r * cal.angle2vector(self.theta)
            self.VM[k, :] = V_MK
            self.VP[k, :] = Vpk
        pass


    def calculate_forces(self):
        f = 1/4 * self.m * self.g * self.miu # 下压力
        for k in range(4):
            direction = -1 * self.VP[k] / np.linalg.norm(self.VP[k])
            frottement = f * direction
            self.frottements[k,:] = frottement


    def calculate_moments(self):
        for k in range(4):
            # 质心到轮子中心的位矢
            OM = cal.angle2vector(self.theta+self.ksi[k]) * self.R
            F = self.frottements[k]
            M_f = F[1] * OM[0] - F[0] * OM[1]
            self.moments[k] = M_f  # 右手右手, 逆时针为正方向

    def move(self, t):
        a = np.sum(self.frottements,axis=0) / self.m
        a_angle = np.sum(self.moments) / self.J
        direction = cal.angle2vector(self.theta)
        for k in range(4):
            self.omegas[k] = self.omegas[k] + np.dot(self.frottements[k], direction) * self.r / self.j * t
        self.theta = self.theta + 1 / 2 * a_angle * t ** 2 + self.OMEGA * t
        self.pos = self.pos + 1/2 * a * t**2 + self.V * t
        # 中心点位置更新
        # 角加速度
        self.V = self.V + a * t - 0.5 * self.V / (self.V[0] ** 2 + self.V[1] ** 2)**0.5 * t
        self.OMEGA = self.OMEGA + a_angle * t



    def gao(self,t):
        self.VM = np.zeros((4, 2))  # 轮胎中心速度
        self.VP = np.zeros((4, 2))  # 轮胎接触点速度
        self.frottements = np.zeros((4, 2))  # 摩擦力
        self.moments = np.zeros(4)
        self.calculate_vitesses()
        self.calculate_forces()
        self.calculate_moments()
        self.move(t)
        return self

def bande_dessiner(X1,Y1,X2,Y2, *args):
    N = len(args) / 2
    if N % 1 != 0:
        print("数据不成对")

    def update_points(num):
        '''
        更新数据点
        '''
        point_ani.set_data(X1[num], Y1[num])
        point_ani2.set_data(X2[num], Y2[num])
        return point_ani, point_ani2

    fig = plt.figure(tight_layout=True)
    plt.plot(X1, Y1, )
    plt.axis('equal')
    point_ani, = plt.plot(X1[0], Y1[0], "ro")
    point_ani2, = plt.plot(X2[0], Y2[0], "bo")
    plt.grid(ls="--")
    # 开始制作动画
    ani = animation.FuncAnimation(fig, update_points, np.arange(0, 10000, 100), interval=5, blit=True)

    # ani.save('sin_test2.gif', writer='imagemagick', fps=10)
    plt.show()





if __name__ == '__main__':
    bande_dessiner([1,2],[3,4])
    # m = 2450
    # V = np.array([2,18])
    # OMEGA = 1*np.pi / 3
    # theta = np.pi / 2
    # omegas = -np.ones(4) * 60
    # pos = np.zeros((1, 2))
    # miu = 0.3
    # w = 1.8
    # l = 4.8
    # r = 0.3
    # J = cal.Momentum_of_Inertia(l,w,m)
    # j = 30 * r**2
    # print(J)
    #
    # A = Car(m, V, OMEGA, theta, omegas, pos, miu,w,l,r, J, j)
    # # B = A.gao(t=1)
    # N = 10000
    # interval = 0.001