A bunch of debugging code and prints added.
Added the fudge for length and width offset to work well. Tested and works really well with rectangles.