Your pseudo code looks pretty good, why don't you post your actual code?