import time
import math


class PIDController:
    def __init__(
        self, kp=1.0, ki=0.0, kd=0.0, setpoint=0.0, min_output=-100, max_output=100
    ):
        # PID constants
        self.kp = kp  # Proportional gain
        self.ki = ki  # Integral gain
        self.kd = kd  # Derivative gain

        # Control variables
        self.setpoint = setpoint  # Desired target value
        self.prev_error = 0.0  # Previous error for derivative calculation
        self.integral = 0.0  # Accumulated error for integral calculation
        self.last_time = time.time()  # Time of last update
        self.last_input = float("nan")  # Store the last process variable

        # Output constraints
        self.min_output = min_output
        self.max_output = max_output

        # For visualization/debugging
        self.p_term = 0.0
        self.i_term = 0.0
        self.d_term = 0.0

    def compute(self, process_variable):
        """Calculate PID output value based on current process variable"""
        # Store the current input value
        self.last_input = process_variable

        # Handle non-numeric or invalid input
        if math.isnan(process_variable):
            return 0.0, 0.0, 0.0, 0.0

        # Calculate time delta
        current_time = time.time()
        dt = current_time - self.last_time
        if dt <= 0:
            dt = 0.001  # Avoid division by zero

        # Calculate error
        error = self.setpoint - process_variable

        # Proportional term
        self.p_term = self.kp * error

        # Integral term - with anti-windup
        self.integral += error * dt
        self.i_term = self.ki * self.integral

        # Derivative term - on measurement, not error
        if dt > 0:
            self.d_term = self.kd * (error - self.prev_error) / dt
        else:
            self.d_term = 0.0

        # Calculate output
        output = self.p_term + self.i_term + self.d_term

        # Constrain output
        output = max(self.min_output, min(self.max_output, output))

        # Save state
        self.prev_error = error
        self.last_time = current_time

        return output, self.p_term, self.i_term, self.d_term

    def reset(self):
        """Reset the controller state"""
        self.prev_error = 0.0
        self.integral = 0.0
        self.last_time = time.time()
        self.p_term = 0.0
        self.i_term = 0.0
        self.d_term = 0.0
        self.last_input = float("nan")  # Reset the last input to NaN