import copy
import sys
import unittest
from pathlib import Path

sys.path.insert(0, str(Path(__file__).resolve().parents[1] / 'tools'))
from physics_delivery_probe import verify_delivery


class DeliveryPredicateTests(unittest.TestCase):
    def setUp(self):
        self.frames = [dict(time=i / 240, body_id=4, destination_id=2,
                            aabb=[[-.02, -.02, .01], [.02, .02, .05]],
                            linear_speed=0., angular_speed=0.,
                            floor_contact=True, held=False) for i in range(121)]
        self.bounds = [[-.2, -.2, 0.], [.2, .2, .3]]

    def verify(self, frames=None):
        return verify_delivery(self.frames if frames is None else frames,
                               self.bounds, 4, 2)

    def test_settled_released_object(self):
        self.assertTrue(self.verify()['verified'])

    def test_reject_each_unsafe_condition(self):
        for update in ({'held': True}, {'floor_contact': False},
                       {'linear_speed': .1}, {'angular_speed': .3},
                       {'body_id': 8}, {'destination_id': 9},
                       {'aabb': [[.19, 0., .01], [.23, .04, .05]]},
                       {'linear_speed': float('nan')}, {'held': None},
                       {'floor_contact': 'true'}):
            with self.subTest(update=update):
                frames = copy.deepcopy(self.frames)
                frames[-1].update(update)
                self.assertFalse(self.verify(frames)['verified'])

    def test_missing_or_short_evidence(self):
        for frames in ([], self.frames[-20:], [{}]):
            self.assertFalse(self.verify(frames)['verified'])

    def test_time_must_increase_without_sampling_gap(self):
        for value in (self.frames[-2]['time'], 1., float('nan')):
            frames = copy.deepcopy(self.frames)
            frames[-1]['time'] = value
            self.assertFalse(self.verify(frames)['verified'])

    def test_stability_requires_whole_window(self):
        frames = copy.deepcopy(self.frames)
        frames[-40]['held'] = True
        self.assertFalse(self.verify(frames)['verified'])

    def test_invalid_earlier_timestamp_cannot_be_filtered_out(self):
        frames = copy.deepcopy(self.frames)
        frames[-30]['time'] = float('nan')
        self.assertFalse(self.verify(frames)['verified'])


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