import numpy as np
from dtk.bicycle import benchmark_matrices, benchmark_state_space
from dtk.control import StateSpace, Bode

speed = 4.6  # m/s
A, B = benchmark_state_space(*benchmark_matrices(), speed, 9.81)
C, D = np.eye(4), np.zeros((4, 2))

states = ['Roll Angle', 'Steer Angle', 'Roll Rate', 'Steer Rate']
inputs = ['Roll Torque', 'Steer Torque']

sys = StateSpace(A, B, C, D,
    name='Carvallo-Whipple Bicycle',
    stateNames=states,
    inputNames=inputs,
    outputNames=states,
)

freqs = np.logspace(0.0, 3.0, num=400)

bode = Bode(freqs, sys)

bode.plot()