Added energy computation. Sort of works