#!/usr/bin/env python3
"""Independent numerical endpoint tests for H1--H4."""

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 analysis.metrics import (  # noqa: E402
    audit_h4_energy,
    compute_h1_composite,
    compute_h2_wrench_metrics,
    compute_h3_power_mismatch,
)


class IndependentExperimentMetricsTest(unittest.TestCase):
    def test_h1_detects_only_eligible_discontinuity(self):
        metrics = compute_h1_composite(
            mapping_valid=[True, True, True],
            position_error_m=[0.0, 0.0, 0.0],
            orientation_error_rad=[0.0, 0.0, 0.0],
            q_slave=np.array(
                [
                    [0.0, 0.0],
                    [0.05, 0.0],
                    [0.50, 0.0],
                ]
            ),
            swivel_angle_rad=[0.0, 0.05, 0.10],
            master_step_norm=[0.0, 0.05, 0.05],
            position_threshold_m=0.01,
            orientation_threshold_rad=0.1,
            joint_step_threshold_rad=0.2,
            swivel_step_threshold_rad=0.2,
            input_step_threshold_rad=0.1,
        )
        self.assertEqual(metrics["h1_F_r"], 0)
        self.assertEqual(metrics["h1_D_r"], 1)
        self.assertEqual(metrics["h1_C_r"], 1)

    def test_h2_separates_force_and_moment_rmse(self):
        reference = np.zeros((3, 6))
        estimate = np.tile([3.0, 4.0, 0.0, 0.0, 0.0, 2.0], (3, 1))
        metrics = compute_h2_wrench_metrics(estimate, reference)
        self.assertAlmostEqual(metrics["h2_force_rmse_N"], 5.0)
        self.assertAlmostEqual(metrics["h2_moment_rmse_Nm"], 2.0)

    def test_h3_is_zero_for_identical_aligned_ports(self):
        torque = np.array([[1.0, 2.0], [-2.0, 1.0], [0.5, -0.5]])
        velocity = np.array([[0.2, 0.1], [0.3, -0.1], [0.4, 0.2]])
        metrics = compute_h3_power_mismatch(
            tau_master_raw=torque,
            qd_master=velocity,
            tau_slave_source=torque,
            qd_slave_source=velocity,
            dt=0.002,
        )
        self.assertAlmostEqual(metrics["h3_epsilon_P_act"], 0.0)

    def test_h4_reconstructs_floor_and_projection_distortion(self):
        metrics = audit_h4_energy(
            energy_before_J=[1.2, 1.1],
            energy_after_J=[1.1, 1.0],
            tau_candidate=np.array([[0.4], [0.4]]),
            tau_applied=np.array([[0.1], [0.1]]),
            qd_master=np.ones((2, 1)),
            dt=1.0,
            energy_min_J=1.0,
            energy_max_J=2.0,
            audit_tolerance_J=1e-12,
        )
        self.assertTrue(metrics["h4_energy_audit_pass"])
        self.assertAlmostEqual(metrics["h4_projected_floor_deficit_J"], 0.0)
        self.assertAlmostEqual(metrics["h4_shadow_floor_deficit_J"], 0.6)
        self.assertAlmostEqual(metrics["h4_delta_B_J"], 0.6)
        self.assertAlmostEqual(metrics["h4_D_proj"], 0.75, places=10)

    def test_h4_detects_downstream_drive_modification(self):
        metrics = audit_h4_energy(
            energy_before_J=[1.1],
            energy_after_J=[1.0],
            tau_candidate=np.array([[0.2]]),
            tau_applied=np.array([[0.1]]),
            tau_accepted=np.array([[0.15]]),
            qd_master=np.ones((1, 1)),
            dt=1.0,
            energy_min_J=1.0,
            energy_max_J=2.0,
            software_preclip_J=[1.0],
            audit_tolerance_J=1e-12,
        )
        self.assertFalse(metrics["h4_energy_audit_pass"])
        self.assertAlmostEqual(
            metrics["h4_downstream_modification_max_Nm"], 0.05
        )
        self.assertGreater(metrics["h4_software_preclip_max_error_J"], 0.0)


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