Refactor and correct timestamp calc.

Put switch statement into static inline inside of header, so it can be reused later if needed in other parts (i.e.: to write out timestamps in other clocks if needed). Simplify and correct timestamp calculations, these are tested and work.