Worked out bugs in FMM energy part