I think you need to split up your functions into smaller pieces, so you can test each one separately to verify they work. (Unit testing)

Seeing that you can handle dynamic memory management, why...