Fix memory allocation and add unit tests