Here is a draft patch, it now works. But some part may need polish.
Can you give me some suggestions? Thanks.