Best next step, then, is to add printf's so you can see what various values are and what the code is consequently doing.

HTH.