Final code for IMAV 2014 Task D; No bug exists!