[2/2] Fix build errors & clean up code

Thank you to CM, PA, Mahdi-Rom source codes. Let's see if it builds now.