Added Scott's code in, still in java. Fixed bugs. Modified equation for inv_distance map