Lost wake-up in `concurrency_breadth_first_search`

[vil02]

Describe the bug

I run the tests with the thread sanitizer. The test_bfs takes forever to finish, which indicates that there might be some problem in concurrency_breadth_first_search. When I set num_threads to 1, the problem does not occur.

This seems to be the problematic code: https://github.com/ZigRazor/CXXGraph/blob/376d91bb74bf5f97cd6673c621ad63aa19ee70a1/include/CXXGraph/Graph/Algorithm/BestFirstSearch_impl.hpp#L230-L235

I think the problem is that the thread will not get notified by notify_all(), if it is not already in wait(). To Reproduce Steps to reproduce the behavior:

1. Replace https://github.com/ZigRazor/CXXGraph/blob/376d91bb74bf5f97cd6673c621ad63aa19ee70a1/CMakeLists.txt#L26-L35 with

	if(SANITIZE)
		add_compile_options(
			-fsanitize=thread
		)
		add_link_options(
			-fsanitize=thread
		)
	endif(SANITIZE)

2. Run the build with -DSANITIZE=ON and run the tests (cf. you might need this).

Expected behavior test_bfs takes reasonable amour of time.