Rewrite the pseudo code to real Coq

Fix some type mistakes in the process.