Try to support all screen modes
fix mode 0 and implement mode 2
Merge branch 'feature/screen-modes' into develop
revert changes to main.cpp