exoskeleton/code/test/test_energy_audit_allocator.py

#!/usr/bin/env python3
"""Tests for final accepted-port allocation and independent H4 audit."""

from pathlib import Path
import sys
import unittest

import numpy as np


CODE_ROOT = Path(__file__).resolve().parents[1]
sys.path.insert(0, str(CODE_ROOT))

from core.command_allocator import CommandAllocator  # noqa: E402
from core.energy_audit import audit_haptic_energy  # noqa: E402
from core.time_domain_popc import TimeDomainPOPC  # noqa: E402


class AllocatorAuditTest(unittest.TestCase):
    def test_reserved_headroom_avoids_total_command_clipping(self):
        allocator = CommandAllocator(np.array([5.0, 4.0]))
        prepared = allocator.prepare(
            compensation=np.array([4.0, -3.5]),
            haptic_raw=np.array([3.0, -2.0]),
        )
        np.testing.assert_allclose(prepared.haptic_candidate, [1.0, -0.5])
        final = allocator.finalize(
            prepared, 0.5 * prepared.haptic_candidate
        )
        self.assertFalse(final.downstream_modified)
        np.testing.assert_allclose(
            final.haptic_accepted, 0.5 * prepared.haptic_candidate
        )

    def test_quantization_is_reported_and_accepted_increment_reconstructed(self):
        allocator = CommandAllocator(
            np.array([5.0]), quantization_step=np.array([0.2])
        )
        prepared = allocator.prepare(np.array([1.0]), np.array([0.34]))
        final = allocator.finalize(prepared, np.array([0.34]))
        self.assertTrue(final.quantization_active)
        self.assertTrue(final.downstream_modified)
        np.testing.assert_allclose(final.total_accepted, [1.4])
        np.testing.assert_allclose(final.haptic_accepted, [0.4])

    def test_independent_audit_catches_preclip_defect(self):
        candidate = np.array([[2.0], [2.0]])
        projected = np.array([[0.5], [0.0]])
        qd = np.ones((2, 1))
        clean = audit_haptic_energy(
            tau_candidate=candidate,
            tau_projected=projected,
            tau_accepted=projected,
            qd_master=qd,
            dt=0.5,
            energy_initial=1.25,
            energy_min=1.0,
            energy_max=5.0,
        )
        self.assertAlmostEqual(clean.max_floor_deficit, 0.0)
        self.assertGreater(clean.delta_B, 0.0)
        self.assertGreater(clean.projection_distortion, 0.0)

        defective = audit_haptic_energy(
            tau_candidate=candidate,
            tau_projected=projected,
            tau_accepted=np.array([[0.7], [0.0]]),
            qd_master=qd,
            dt=0.5,
            energy_initial=1.25,
            energy_min=1.0,
            energy_max=5.0,
            logged_preclip=np.array([1.0, 1.0]),
        )
        self.assertGreater(defective.max_floor_deficit, 0.0)
        self.assertGreater(defective.downstream_modification_max, 0.0)
        self.assertGreater(defective.preclip_log_max_error, 0.0)

    def test_projection_distortion_is_zero_without_intervention(self):
        torque = np.array([[1.0, -2.0], [0.5, 0.25]])
        result = audit_haptic_energy(
            tau_candidate=torque,
            tau_projected=torque,
            tau_accepted=torque,
            qd_master=np.zeros_like(torque),
            dt=0.01,
            energy_initial=2.0,
            energy_min=1.0,
            energy_max=3.0,
        )
        self.assertAlmostEqual(result.projection_distortion, 0.0)
        self.assertAlmostEqual(result.delta_B, 0.0)


class TimeDomainPOPCTest(unittest.TestCase):
    def test_no_intervention_for_passive_candidate(self):
        controller = TimeDomainPOPC(
            initial_energy=0.0, minimum_energy=0.0, maximum_energy=5.0
        )
        applied, diagnostics = controller.apply(
            np.array([-2.0]), np.array([1.0]), 0.1
        )
        np.testing.assert_allclose(applied, [-2.0])
        self.assertFalse(diagnostics.intervention_active)
        self.assertAlmostEqual(controller.energy, 0.2)

    def test_active_candidate_receives_damping_injection(self):
        controller = TimeDomainPOPC(
            initial_energy=0.1, minimum_energy=0.0, maximum_energy=5.0
        )
        applied, diagnostics = controller.apply(
            np.array([2.0]), np.array([1.0]), 0.1
        )
        np.testing.assert_allclose(applied, [1.0])
        self.assertTrue(diagnostics.intervention_active)
        self.assertAlmostEqual(diagnostics.damping_gain, 1.0)
        self.assertAlmostEqual(controller.energy, 0.0)


if __name__ == "__main__":
    unittest.main()