Add rp_math.h
Add some comments