import numpy as np
from scipy.signal import lti, lsim
from dtk.bicycle import (benchmark_matrices, benchmark_state_space,
                         meijaard_figure_four)
t = np.linspace(0.0, 5.0)
x0 = np.array([0.0, 0.0, 0.5, 0.0])
speed = 4.6  # m/s
A, B = benchmark_state_space(*benchmark_matrices(), speed, 9.81)
C, D = np.eye(4), np.zeros((4, 2))
system = lti(A, B, C, D)
t, y, _ = lsim(system, 0.0, t, X0=x0)
meijaard_figure_four(t, y[:, 2], y[:, 3], speed*np.ones_like(t))