Added new implementation to main and alos TestMain to test the function that count the energy