Added energy evaluation part and fixed a few bugs