import math
import unittest

import carla

from global_route_planner import GlobalRoutePlanner
from global_route_planner import NavEnum
from global_route_planner_dao import GlobalRoutePlannerDAO


class Test_GlobalRoutePlanner(unittest.TestCase):
    """
    Test class for GlobalRoutePlanner class
    """

    def setUp(self):
        # == Utilities test instance without DAO == #
        self.simple_grp = GlobalRoutePlanner(None)

        # == Integration test instance == #
        client = carla.Client("localhost", 2000)
        world = client.get_world()
        integ_dao = GlobalRoutePlannerDAO(world.get_map())
        self.integ_grp = GlobalRoutePlanner(integ_dao)
        self.integ_grp.setup()
        pass

    def tearDown(self):
        self.simple_grp = None
        self.dao_grp = None
        self.integ_grp = None
        pass

    def test_plan_route(self):
        """
        Test for GlobalROutePlanner.plan_route()
        Run this test with carla server running Town03
        """
        plan = self.integ_grp.plan_route((-60, -5), (-77.65, 72.72))
        self.assertEqual(
            plan,
            [
                NavEnum.START,
                NavEnum.LEFT,
                NavEnum.LEFT,
                NavEnum.GO_STRAIGHT,
                NavEnum.LEFT,
                NavEnum.STOP,
            ],
        )

    def test_path_search(self):
        """
        Test for GlobalRoutePlanner.path_search()
        Run this test with carla server running Town03
        """
        self.integ_grp.path_search((191.947, -5.602), (78.730, -50.091))
        self.assertEqual(
            self.integ_grp.path_search((196.947, -5.602), (78.730, -50.091)),
            [256, 157, 158, 117, 118, 59, 55, 230],
        )

    def test_localise(self):
        """
        Test for GlobalRoutePlanner.localise()
        Run this test with carla server running Town03
        """
        x, y = (200, -250)
        segment = self.integ_grp.localise(x, y)
        self.assertEqual(self.integ_grp._id_map[segment["entry"]], 5)
        self.assertEqual(self.integ_grp._id_map[segment["exit"]], 225)

    def test_unit_vector(self):
        """
        Test for GlobalROutePlanner.unit_vector()
        """
        vector = self.simple_grp.unit_vector((1, 1), (2, 2))
        self.assertAlmostEquals(vector[0], 1 / math.sqrt(2))
        self.assertAlmostEquals(vector[1], 1 / math.sqrt(2))

    def test_dot(self):
        """
        Test for GlobalROutePlanner.test_dot()
        """
        self.assertAlmostEqual(self.simple_grp.dot((1, 0), (0, 1)), 0)
        self.assertAlmostEqual(self.simple_grp.dot((1, 0), (1, 0)), 1)


def suite():
    """
    Gathering all tests
    """

    suite = unittest.TestSuite()
    suite.addTest(Test_GlobalRoutePlanner("test_unit_vector"))
    suite.addTest(Test_GlobalRoutePlanner("test_dot"))
    suite.addTest(Test_GlobalRoutePlanner("test_localise"))
    suite.addTest(Test_GlobalRoutePlanner("test_path_search"))
    suite.addTest(Test_GlobalRoutePlanner("test_plan_route"))

    return suite


if __name__ == "__main__":
    """
    Running test suite
    """
    mySuit = suite()
    runner = unittest.TextTestRunner()
    runner.run(mySuit)
