Refctored a bit, fixed cursor position calculation