Sfortunatamente non dispongo del tempo necessario alla revisione e correzione del tuo codice, ma ti segnalo una delle più diffuse reference implementation, basata quasi pedissequamente sugli algoritmi del CLRS. Anche se ingegneristicamente migliorabile sotto vari aspetti, è ottima per impieghi studenteschi.