Fixed some bugs and wrote a test for ControlColidateKing