import matplotlib.pyplot as plt
import numpy as np
import scipy.integrate as sp
import math

#Table 1 Parameters
M=1280
m=120
g=9.8
a=1.15
b=1.35
h=0.4
r=0.35
J=1.8
mudry=0.8
muwet=0.4
ab=a+b
k=0.3

#Table 2 Parameters (dry asphalt)
c1=1.28
c2=24
c3=0.52

#Initial Conditions
#72 km/h = 20 m/s
x0list=[0,20,20/.35,20/.35]

t=np.linspace(0.5,1.5,51)
C2=5000
C3=k*C2

def ODE1(X,t,C2,C3):
    [x, dotx, omega2, omega3]=X


    if omega2 == omega3:
        omega3=omega2+0.000001

    s2=1-((r*omega2)/dotx)
    s3=1-((r*omega3)/dotx)
    mu_s2=c1*(1-math.exp(-c2*s2))-(c3*s2)
    mu_s3=c1*(1-math.exp(-c2*s3))-(c3*s3)

    ddotx1=(((mu_s3-mu_s2)*(C2+C3+(M*g*b)+(m*g*(a+b))-r*g*mu_s3*(M+2*m)))-(g*mu_s3*(M+2*m)*(mu_s2*r-mu_s3*r+ab)))
    ddotx2=(((M+2*m)*(mu_s2*r-mu_s3*r+a+b))+((2*m*r+M*h)*(mu_s3-mu_s2)))
    ddotx=ddotx1/ddotx2
    N2=((M+2*m)*((ddotx)+mu_s3*g))/(mu_s3-mu_s2)
    N3=((M+2*m)*g)-N2
    omegadot2=((r*mu_s2*(N2))-C2)/J
    omegadot3=((r*mu_s3*(N3))-C3)/J

    return [dotx, ddotx, omegadot2, omegadot3]

sol=sp.odeint(ODE1,x0list,t,args=(C2,C3))
print(sol)

plt.plot(t,sol[:,1], color='r')
plt.plot(t,sol[:,2], color='g')
plt.plot(t,sol[:,3], color='b')
plt.axis([0,3.5,-100,100])
plt.show()