Refactored and more code cleanup
Comitting for backup
Added Test board
Merge branch 'master' into Issue2
Don't feel like debugging