import pandas as pd
from model import Car
import numpy as np
import os

def get_parameters(path):
    parameters = pd.read_csv(path)
    return parameters


def F(m1,longth1,width1,Izz1,v1x,v1y,omi1z,theta1,ksi1,m2,longth2,width2,Izz2,v2x,v2y,omi2z,theta2,ksi2,e,miu,miu_sliding,tire_radius,tire_inertia,):
    records1 = []  # 记录模型1
    records2 = []
    def collision():
        "前车"
        X = np.array([v1x, v2x, v1y, v2y, omi1z, omi2z])
        # 碰撞系数
        Gamma = ksi1
        d1 = longth1/2 / np.cos(ksi1)
        d2 = longth2/2 / np.cos(ksi2)

        da = d2 * np.sin(theta2 + ksi2)
        db = d2 * np.cos(theta2 + ksi2)
        dc = d1 * np.sin(theta1 + ksi1)
        dd = d1 * np.cos(theta1 + ksi1)

        C1 = np.array(
            [[m1, m2, 0, 0, 0, 0, ],
             [0, 0, m1, m2, 0, 0, ],
             [0, m2 * dc, m1 * dd, 0, Izz1, 0, ],
             [0, m2 * da, m1 * db, 0, 0, Izz2, ],
             [np.cos(Gamma), -np.cos(Gamma), np.sin(Gamma), -np.sin(Gamma), dc * np.cos(Gamma) - dd * np.sin(Gamma),
              da * np.cos(Gamma) - db * np.sin(Gamma), ],
             [0, m2 * (np.sin(Gamma) + miu * np.cos(Gamma)), m1 * (np.cos(Gamma) - miu * np.sin(Gamma)), 0, 0, 0, ]])

        C2 = np.array(
            [[m1, m2, 0, 0, 0, 0, ],
             [0, 0, m1, m2, 0, 0, ],
             [0, m2 * dc, m1 * dd, 0, Izz1, 0, ],
             [0, m2 * da, m1 * db, 0, 0, Izz2, ],
             [-e * np.cos(Gamma), e * np.cos(Gamma), -e * np.sin(Gamma), e * np.sin(Gamma),
              -e * (dc * np.cos(Gamma) - dd * np.sin(Gamma)),
              -e * (da * np.cos(Gamma) - db * np.sin(Gamma)), ],
             [0, m2 * (np.sin(Gamma) + miu * np.cos(Gamma)), m1 * (np.cos(Gamma) - miu * np.sin(Gamma)), 0, 0, 0, ]])
        C1_inv = np.linalg.inv(C1)
        V1x, V2x, V1y, V2y, Omi1z, Omi2z = np.dot(C1_inv,(np.dot(C2,X)))
        records1 = [i,V1x, V2x, V1y, V2y, Omi1z, Omi2z]
        records1 = pd.DataFrame([records1],columns=columns1)
        records1.to_csv(path1,mode='a',header=False)
        return V1x, V1y, Omi1z

    def give_me_a_car(V1x, V1y, Omi1z):
        V = np.array([V1x,V1y], dtype='float')
        OMEGA = Omi1z
        theta = theta1
        omegas = -(v1x / tire_radius) * np.ones(4)
        pos = np.zeros(2)
        car = Car(m1, V, OMEGA, theta, omegas, pos, miu_sliding, width1, longth1, tire_radius, Izz1, tire_inertia)
        return car

    def test_car(car):
        N_max = 10000
        t = 0.01  #!!!!
        threshold = 1e-1
        iter_times = 0
        Ymin = car.pos[0]
        Ymax = car.pos[1]
        records2 = []
        while np.linalg.norm(car.V) > threshold and iter_times < N_max:
            car.gao(t)
            record = [
                i,
                car.pos[0],
                car.pos[1],
                car.V[0],
                car.V[1],
                car.theta,
                car.OMEGA,
                car.omegas[0],
                car.omegas[1],
                car.omegas[2],
                car.omegas[3],
            ]
            Ynow = car.pos[1]
            if Ynow > Ymax :
                Ymax = Ynow
            elif Ynow < Ymin :
                Ymin = Ynow
            iter_times += 1
            # if iter_times % 1000 == 0 :
            #     print(iter_times,' ms')
            records2.append(record)
        records2 = pd.DataFrame(records2,columns=columns2)
        records2.to_csv(path2, mode='a')
        return iter_times,Ymin,Ymax

    def f():
        """
        v2x,v2y,omi2z,ksi1,ksi2,theta2
        :param args:
        :return:
        """
        tmp1 = collision()
        tmp2 = give_me_a_car(*tmp1)
        t,Ymin,Ymax = test_car(tmp2)
        record3 = [i,t,Ymin,Ymax]
        record3 = pd.DataFrame([record3],columns=columns3)
        record3.to_csv+(path3, mode='a',header=False)
        return t,Ymin,Ymax
    f()

if __name__ == '__main__':
    path = './paras/2021-06-25-04-34-43-PARAMETERS.csv'

    path1 = path[:-14] + 'RECORDS1' + '.csv'
    # path2 = path[:-14] + 'RECORDS2' + '.csv'
    path3 = path[:-14] + 'RECORDS3' + '.csv'
    columns1 = ['index','V1x', 'V2x', 'V1y', 'V2y', 'Omi1z', 'Omi2z']
    columns2 = ['index', 'posX', 'posY', 'Vx', 'Vy', 'theta', 'OMEGA', 'omega1', 'omega2', 'omega3', 'omega4']
    columns3 = ['index','time','Ymin','Ymax']
    if os.path.exists(path1):
        header1 = pd.DataFrame(columns=columns1)
        header1.to_csv(path1)
    # header2 = pd.DataFrame(columns=columns2)
    if os.path.exists(path3):
        header3 = pd.DataFrame(columns=columns3)
    # header2.to_csv(path2) # 打印一个标题
        header3.to_csv(path3)
    parameters = get_parameters(path)
    para_E = parameters.loc[parameters['e'] == 0.6]
    N = len(parameters)


    for i in range(4000,N):
        i = int(i)
        path2 = path[:-14] + 'RECORDS2-' + str(i) + '.csv'
        parameter = parameters.iloc[i][1:]
        F(*parameter)
        # print("第",i,"迭代发生错误:\n",e)
        print(i)
        # if i % 100 == 0:
        #     print(i/N,"%")