Part of C code for harry potter exercise
Finish code and test, but need optimize