Changed function signature and added debug calculations (currently commented)