Fixed issue with screen not being restored and cleaned up code