Closing #16, the camera is working now, but this code still need some refactory