"""Compile/run the actual hardware guard without ROS or a robot connection."""
import subprocess
import tempfile
from pathlib import Path
from unittest import TestCase

class PositionCommandFenceTest(TestCase):
    def test_reject_old_targets_after_enable_and_native_movel(self):
        root = Path(__file__).resolve().parents[2]
        include = root / 'src/flexiv_ros2/flexiv_hardware/include'
        code = r"""
#include <cassert>
#include <limits>
#include "flexiv_hardware/position_command_fence.hpp"
int main() {
    flexiv_hardware::MotionExecutionLatch latch;
    assert(!latch.observe(true, true, false)); // not activated
    latch.activate(); assert(latch.observe(true, true, false));
    assert(!latch.observe(true, false, false)); // physical E-stop
    for (int i=0; i<2000; ++i) assert(!latch.observe(true, true, false)); // Enable cannot resume
    latch.activate(); assert(latch.observe(true, true, false));
    assert(!latch.observe(true, true, true)); // fault
    assert(!latch.observe(true, true, false)); // clear fault cannot resume
    latch.activate(); assert(!latch.observe(false, true, false)); // disconnect
    assert(!latch.observe(true, true, false));
    flexiv_hardware::PositionCommandFence fence;
    std::vector<double> current(7, .74), old(7, 0.);
    for (int i=0; i<2000; ++i) assert(!fence.accept(old, current));
    assert(!fence.accept({}, current));
    auto invalid = current; invalid[0] = std::numeric_limits<double>::quiet_NaN();
    assert(!fence.accept(invalid, current));
    assert(fence.accept(current, current));
    assert(fence.accept(old, current)); // normal trajectory after handshake
    fence.reset(); // native MoveL / controller activation
    for (int i=0; i<2000; ++i) assert(!fence.accept(old, current));
    auto start = current; start[0] += .0001;
    assert(fence.accept(start, current));
    assert(fence.accept(old, current));
}
"""
        with tempfile.TemporaryDirectory() as folder:
            source = Path(folder)/'fence.cpp'; binary = Path(folder)/'fence'
            source.write_text(code)
            built = subprocess.run(['g++','-std=c++17','-I',str(include),str(source),'-o',str(binary)],capture_output=True,text=True,timeout=30)
            self.assertEqual(built.returncode,0,built.stderr)
            result = subprocess.run([str(binary)],capture_output=True,text=True,timeout=5)
            self.assertEqual(result.returncode,0,result.stderr)