import itertools

import pytest

from iiwa_web.ros_node import CobotWebNode
from iiwa_web import ros_node


class DoneFuture:
    def __init__(self, result):
        self._result = result

    def done(self):
        return True

    def result(self):
        return self._result


class PendingFuture:
    def done(self):
        return False

    def result(self):
        return None


class GoalHandle:
    accepted = True

    def __init__(self):
        self.cancel_calls = 0

    def get_result_async(self):
        return PendingFuture()

    def cancel_goal_async(self):
        self.cancel_calls += 1
        return DoneFuture(None)


class ActionClientStub:
    def __init__(self, goal_handle):
        self.goal_handle = goal_handle

    def wait_for_server(self, timeout_sec):
        return True

    def send_goal_async(self, goal):
        return DoneFuture(self.goal_handle)


def test_send_action_cancels_goal_when_result_times_out(monkeypatch):
    node = object.__new__(CobotWebNode)
    goal_handle = GoalHandle()
    node._action_clients = {"cobot/move_to_pose": ActionClientStub(goal_handle)}

    timestamps = itertools.chain([0.0, 0.3, 0.6], itertools.repeat(0.9))
    monkeypatch.setattr(ros_node.time, "monotonic", lambda: next(timestamps))
    monkeypatch.setattr(ros_node.time, "sleep", lambda _seconds: None)

    with pytest.raises(TimeoutError):
        node.send_action(object, "cobot/move_to_pose", object(), timeout=0.5)

    assert goal_handle.cancel_calls == 1