added lcd_cursor_goto function, refactor some code