# Simulation of a PID-controller
import matplotlib.pyplot as plt

class PIDController:
    def __init__(self, Kp, Ki, Kd, setpoint):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
        self.setpoint = setpoint
        self.prev_error = 0
        self.integral = 0

    def update(self, measured_value):
        error = self.setpoint - measured_value
        self.integral += error
        derivative = error - self.prev_error
        output = self.Kp * error + self.Ki * self.integral + self.Kd * derivative
        self.prev_error = error
        return output
    
    def set_setpoint(self, setpoint):
        self.setpoint = setpoint

# Simulation parameters
Kp = 4 # Proportional gain
Ki = 1/4 # Integral gain
Kd = 1/2 # Derivative gain
setpoint = 100.0
initial_value = 0.0
simulation_time = 100
time_step = 0.1

# Create PID controller
pid_controller = PIDController(Kp, Ki, Kd, setpoint)

# Initialize lists to store data for plotting
time_points = []
control_signals = []
current_values = []

# Simulate PID controller
current_value = initial_value
for t in range(simulation_time):
    if t == simulation_time // 3:
        pid_controller.set_setpoint(50.0)
    if t == 2 * simulation_time // 3:
        pid_controller.set_setpoint(150.0)
    time_points.append(t)
    control_signal = pid_controller.update(current_value)
    control_signals.append(control_signal)
    # Apply control signal (simulate process)
    current_value += control_signal * time_step
    current_values.append(current_value)

# Plotting
plt.figure(figsize=(10, 6))
plt.subplot(2, 1, 1)
plt.plot(time_points, current_values, label='Current Value')
plt.plot([0, simulation_time // 3], [100, 100], 'r--', label='Setpoint 100')
plt.plot([simulation_time // 3, 2 * simulation_time // 3], [50, 50], 'r--', label='Setpoint 50')
plt.plot([2 * simulation_time // 3, simulation_time], [150, 150], 'r--', label='Setpoint 150')
plt.xlabel('Time')
plt.ylabel('Value')
plt.title('PID Controller Simulation')
plt.legend()

plt.subplot(2, 1, 2)
plt.plot(time_points, control_signals, 'g', label='Control Signal')
plt.xlabel('Time')
plt.ylabel('Control Signal')
plt.legend()

plt.tight_layout()
# plt.savefig('pid_simulation.png') # Save to file
plt.show() # Show immediately