Finished mathematic part of BHF addition, everything works, refactoring coming up