Organized code
Added the #ifdef __cplusplus stuff