from math import pi as π, radians


def theoretical_peak_angle_velocity(stride_freq_Hz: float = 3.,
                                    total_angle_deg: float = 180.,
                                    disp: bool = True):
    """Cheetah leg moves from 0⁰ -> 90⁰ -> 0⁰ in about 1/3 of a second. Ie, follows the shape:
         position(t) = 90/2 * sin(radians(t/0.3 * 360))
    where t = 0..0.3

    Differentiating with respect to time:
        velocity(t) = 90/2 * cos(radians(t/0.3 * 360)) * 360/0.3
    Giving a max velocity of
        velocity(0) -> 90/2 * 360/0.3 = 

    Example code:

    ```python
        from math import pi as π
        total_angle_deg = 180.
        stride_freq_Hz = 3.

        t = np.linspace(0, 1/stride_freq_Hz)
        pos = lambda t: total_angle_deg/2 * np.sin(t*stride_freq_Hz * 2*π)
        plt.plot(t, 10*pos(t), label='position [deg] scaled by 10')

        vel = lambda t: total_angle_deg/2 * np.cos(t*stride_freq_Hz * 2*π) * stride_freq_Hz * 2*π
        plt.plot(t, vel(t), label='velocity [deg]')

        max_ω_deg = total_angle_deg/2 * stride_freq_Hz * 2*π
        plt.title(f'total angle change = {total_angle_deg} deg\nmax angular velocity = {max_ω_deg:.1f} deg/s = {np.radians(max_ω_deg):.1f} rad/s')
        plt.legend(); plt.show()
    ```
    """
    peak = total_angle_deg/2 * stride_freq_Hz * 2*π

    if disp:
        print(f'Expected peak angular velocity of a leg moving though '
              f'{total_angle_deg} degrees at {stride_freq_Hz} Hz:\n'
              f'total_angle_deg/2 * stride_freq_Hz * 2*π '
              f'= {peak:.2f} deg/s = {radians(peak):.2f} rad/s')

    return peak