Complete and debugged code. Fixed energy normalization
Merge origin/master