Fixed path fitting. Working pretty well now... Harder problem than I thought. Function is ugly though, will refactor some time.