Code cleanup and add headers