"""This module asserts correct runtime behaviour of the pyrate.act.pid module."""

# Test environment
from unittest import TestCase

# Mathematics
from numpy import array
from numpy import Inf
from numpy.linalg import norm
from numpy import vstack

# Package under test
from pyrate.act.control import AntiWindupPid


class TestAntiWindupPid(TestCase):

    """Test for correct runtime behaviour in pyrate.act.control.anti_windup_pid."""

    # In this context, we reproduce a common PID notation
    # pylint: disable=invalid-name

    def setUp(self) -> None:
        """Set up the PID specification for testing."""

        # PID specification
        self.P = array([[1.0]])
        self.I = array([[1.0]])  # noqa: 741
        self.D = array([[1.0]])
        self.max_control = 1.0
        self.dt = 0.5

        # State and target
        # Target is already reached in this example
        self.state = vstack([0.0])
        self.state_large = vstack([0.5])
        self.state_small_neg = vstack([-0.01])
        self.state_derivative = vstack([0.0])
        self.desired = vstack([0.0])

    def test_anti_windup(self) -> None:
        """Assert control signal in allowed range, PID responsive by limited integral part."""
        # Initialize PID controller
        pid = AntiWindupPid(self.P, self.I, self.D, self.max_control, self.dt, keep_trace=True)

        # Execute a few control steps
        for _ in range(10):
            control_signal = pid.control(
                desired=self.desired, state=self.state_large, state_derivative=self.state_derivative
            )
            assert abs(control_signal) <= self.max_control, "control limits are not applied"
        assert abs(control_signal) == self.max_control, "control limits not reached"

        # test reactiveness
        control_signal_back = pid.control(
            desired=self.desired, state=self.state_small_neg, state_derivative=self.state_derivative
        )
        assert abs(control_signal_back) < self.max_control + 1e-4, "anti wind up is not working"

    def test_process_tracking(self) -> None:
        """Assert that a pandas.DataFrame with process data is created."""

        # Initialize PID controller
        pid = AntiWindupPid(self.P, self.I, self.D, self.max_control, self.dt, keep_trace=True)

        # Execute a few control steps
        # Execute a few control steps
        state = self.state.copy()
        previous_error = Inf
        for _ in range(10):
            control_signal = pid.control(
                desired=self.desired, state=self.state, state_derivative=self.state_derivative
            )
            state += control_signal
            error = norm(state - self.desired).item()  # Convert from numpy scalar to Python float
            assert error < previous_error or error == 0, "Error did not decrease"
            previous_error = error

        # Assert correct process tracing with PID controller
        assert pid.process is not None, "PID did not keep trace of process"
        assert len(pid.process.index) == 10, "PID has not traced enough steps"

        # Reset PID
        pid.reset()

        # Assert correct reset to initial state
        assert pid.summed_error == 0.0, "PID did not reset summed error properly"
        assert len(pid.process.index) == 0, "PID has not dropped process trace properly"