Documentation and improved calculations

Removed some crazy math and added 3D plane error compensation to
improve touch detection. Also added comments :)