import unittest

import lanelet2  # if we fail here, there is something wrong with lanelet2 registration
from lanelet2.core import (
    getId,
    Point3d,
    LineString3d,
    Lanelet,
    LaneletMap,
    ConstLanelet,
)
from lanelet2.matching import ConstLaneletMatch, ConstLaneletMatchProbabilistic
from lanelet2.matching import (
    Pose2d,
    getDeterministicMatches,
    getProbabilisticMatches,
    Object2d,
    ObjectWithCovariance2d,
    PositionCovariance2d,
    removeNonRuleCompliantMatches,
)


def get_sample_lanelet_map():
    mymap = LaneletMap()
    x_left = 2
    x_right = 0
    ls_left = LineString3d(getId(), [Point3d(getId(), x_left, i, 0) for i in range(0, 3)])
    ls_right = LineString3d(getId(), [Point3d(getId(), x_right, i, 0) for i in range(0, 3)])
    llet = Lanelet(getId(), ls_left, ls_right)
    mymap.add(llet)
    return mymap


class ObjectsApiTestCase(unittest.TestCase):
    def test_default_init(self):
        obj_2d = Object2d()
        self.assertTrue(isinstance(obj_2d, Object2d))
        self.assertEqual(len(obj_2d.absoluteHull), 0)

        obj_with_cov_2d = ObjectWithCovariance2d()
        self.assertTrue(isinstance(obj_with_cov_2d, ObjectWithCovariance2d))
        self.assertEqual(len(obj_with_cov_2d.absoluteHull), 0)
        self.assertEqual(obj_with_cov_2d.vonMisesKappa, 0)

        pos_cov_2d = PositionCovariance2d()
        self.assertTrue(isinstance(pos_cov_2d, PositionCovariance2d))

        pose_2d = Pose2d()
        self.assertTrue(isinstance(pose_2d, Pose2d))

    def test_custom_init(self):
        obj_2d = Object2d(1, Pose2d(), [])
        self.assertTrue(isinstance(obj_2d, Object2d))
        self.assertEqual(len(obj_2d.absoluteHull), 0)

        obj_with_cov_2d = ObjectWithCovariance2d(1, Pose2d(), [], PositionCovariance2d(), 2)
        self.assertTrue(isinstance(obj_with_cov_2d, ObjectWithCovariance2d))
        self.assertEqual(len(obj_with_cov_2d.absoluteHull), 0)
        self.assertEqual(obj_with_cov_2d.vonMisesKappa, 2)

        pos_cov_2d = PositionCovariance2d(1.0, 2.0, 3.0)
        self.assertTrue(isinstance(pos_cov_2d, PositionCovariance2d))
        self.assertEqual("1 3\n3 2", str(pos_cov_2d))

        pose_2d = Pose2d(1.0, 2.0, 3.0)
        self.assertTrue(isinstance(pose_2d, Pose2d))
        self.assertEqual("x: 1\ny:  2\nphi: 3", str(pose_2d))


class MatchingApiTestCase(unittest.TestCase):
    def test_matching(self):
        mymap = get_sample_lanelet_map()

        obj = Object2d(1, Pose2d(1, 1, 0), [])
        obj_with_cov = ObjectWithCovariance2d(1, Pose2d(1, 1, 0), [], PositionCovariance2d(1.0, 1.0, 0.0), 2)

        obj_matches = getDeterministicMatches(mymap, obj, 1.0)
        self.assertEqual(len(obj_matches), 2)  # lanelet in both directions
        self.assertTrue(isinstance(obj_matches[0], ConstLaneletMatch))

        obj_with_cov_matches = getProbabilisticMatches(mymap, obj_with_cov, 1.0)
        self.assertEqual(len(obj_with_cov_matches), 2)  # lanelet in both directions
        self.assertTrue(isinstance(obj_with_cov_matches[0], ConstLaneletMatchProbabilistic))

        traffic_rules = lanelet2.traffic_rules.create(
            lanelet2.traffic_rules.Locations.Germany,
            lanelet2.traffic_rules.Participants.Vehicle,
        )

        obj_matches_rule_compliant = removeNonRuleCompliantMatches(obj_matches, traffic_rules)
        self.assertEqual(len(obj_matches_rule_compliant), 1)  # lanelet only in one direction
        self.assertTrue(isinstance(obj_matches_rule_compliant[0], ConstLaneletMatch))

        obj_with_cov_matches_rule_compliant = removeNonRuleCompliantMatches(obj_with_cov_matches, traffic_rules)
        self.assertEqual(len(obj_with_cov_matches_rule_compliant), 1)  # lanelet only in one direction
        self.assertTrue(isinstance(obj_with_cov_matches_rule_compliant[0], ConstLaneletMatchProbabilistic))
        self.assertTrue(isinstance(obj_with_cov_matches_rule_compliant[0].lanelet, ConstLanelet))
        self.assertEqual(obj_with_cov_matches_rule_compliant[0].distance, 0)
        self.assertTrue(isinstance(obj_with_cov_matches_rule_compliant[0].mahalanobisDistSq, float))

        # empty matches list
        empty_matches_rule_compliant = removeNonRuleCompliantMatches(list(), traffic_rules)
        self.assertEqual(len(empty_matches_rule_compliant), 0)

        # list of neither matches nor probabilistic matches
        self.assertRaises(RuntimeError, removeNonRuleCompliantMatches, [obj], traffic_rules)


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