Rearrange code
Fix Cmake