Updated debug version of algorithm to be essentially equivalent with the one on the microccontroller