In the new file `cost.py` implement function `add_energy`. The input should be similar to the `add_gkci`. It should take wavefunction and energies as input.

[RichRick1]

In the new file `cost.py` implement function `add_energy`. The input should be similar to the `add_gkci`. It should take wavefunction and energies as input.