Liberally use ASSERT macros within your code. If your system doesn't already have an ASSERT macros mechanism, develop one and use it.
An ASSERT(expression) macro will
if (FALSE == expression) then
output a warning, trap to a debugger etc.
endif
Ref: Chap 2, Assert Yourself, Writing Solid Code by Steve Maguire