How to implement a tilt compensated compass?
Inspecting your code, it does not appear you are compensating the raw magnetometer values for soft and hard iron effects.  These terms roughly refer to adjusting the range and magnitude of each of the individual magnetometers (X, Y and Z).  Some magnetometer manufactures allow writing these compensation values into the chip.  Even then the developer and user will still need to perform some form of calibration as the magnetometer can not predict under what conditions the magnetometer will be used.  Here is another explanation using matrices of hard and soft iron compensation.
Once the compass produces expected results on a flat surface parallel to the surface of the earth (in a place on earth where the magnetic lines roughly align with true north).  Consider adding tilt compensation.  Accelerometers usually do not need to be compensated as extensively as magnetometers.
This can be done using trigonometric math and Euler angles.  However, as with physical gambles, this can lead to a situation similar to gimbal lock.  Where the trigonometric math goes to infinity.
To avoid a gimbal lock like situation, most tilt compensated compasses use quaternions.  Quaternions use imaginary numbers and matrix math to describe 3 dimensional rotation.