Got rid of useless function, fixed distance formula, getting seg faults.