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.array([1.0, 0.0, 0.0, 0.0]).reshape(1, 4), np.zeros((1, 1))

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

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

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

bode = Bode(freqs, sys)

mag, phase = bode.mag_phase_system(sys)

bode.plot_system(sys, mag, phase, decibel=False, degree=False)