"""
Sample PID calculations.

Tests the algorithm for updating output frequency based on the current SPM.
"""
import matplotlib.pyplot as plt

# Nameplate
nameplate_hz = 120.0
nameplate_rpm = 1200.0

# Setpoints
starting_hz = 20.0
spm_target = 4.0

pid_ramp_limit_large = 1.0
pid_ramp_limit_small = 0.2

pid_ramp_hz_large = 5.0
pid_ramp_hz_small = 1.0

hz_setpoint = starting_hz


def stroke(last_stroke_spm):
    """
    Calculate the new output frequency.

    Input Parameters:
    last_stroke_spm: last strokes SPM
    """
    global hz_setpoint, spm_target

    error = abs(spm_target - last_stroke_spm)
    error_sign = 1 if (spm_target > last_stroke_spm) else -1

    if error >= pid_ramp_limit_large:
        print("Large change")
        hz_setpoint += error_sign * pid_ramp_hz_large
    elif error >= pid_ramp_limit_small:
        print("Small change")
        hz_setpoint += error_sign * pid_ramp_hz_small

    return hz_setpoint


def loop():
    """Loop and calculate."""
    new_spm = 1.0
    spm_arr = []
    rpm_arr = []

    while new_spm != 0.0:
        new_spm = float(raw_input("New SPM: "))
        hz_sp = stroke(new_spm)
        new_rpm = (nameplate_rpm / nameplate_hz) * hz_sp
        spm_arr.append(new_spm)
        rpm_arr.append(new_rpm)
        print("{} SPM, {} RPM".format(new_spm, new_rpm))
    fig, ax1 = plt.subplots()
    ax1.plot(spm_arr, "b-")
    ax1.set_ylabel("SPM", color="b")
    ax2 = ax1.twinx()
    ax2.plot(rpm_arr, "r-")
    ax2.set_ylabel("RPM", color="r")
    fig.tight_layout()
    plt.show()


loop()
