Added MemoryUtils, containing unique_dynamic_cast

unique_dynamic_cast gives us a dynamic_cast-like function for
unique_ptrs.
This is very useful for casting messages to their desired type
after receiving them from a message queue.
Started writing tests for the state machine