/*
 * Project: MoleCuilder
 * Description: creates and alters molecular systems
 * Copyright (C)  2013 University of Bonn. All rights reserved.
 * Copyright (C)  2013 Frederik Heber. All rights reserved.
 * 
 *
 *   This file is part of MoleCuilder.
 *
 *    MoleCuilder is free software: you can redistribute it and/or modify
 *    it under the terms of the GNU General Public License as published by
 *    the Free Software Foundation, either version 2 of the License, or
 *    (at your option) any later version.
 *
 *    MoleCuilder is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *    GNU General Public License for more details.
 *
 *    You should have received a copy of the GNU General Public License
 *    along with MoleCuilder.  If not, see <http://www.gnu.org/licenses/>.
 */

/*
 * SaturatedFragment.cpp
 *
 *  Created on: Mar 3, 2013
 *      Author: heber
 */

// include config.h
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include "CodePatterns/MemDebug.hpp"

#include "SaturatedFragment.hpp"

#include <cmath>
#include <iostream>

#include "CodePatterns/Assert.hpp"
#include "CodePatterns/Log.hpp"

#include "LinearAlgebra/Exceptions.hpp"
#include "LinearAlgebra/Plane.hpp"
#include "LinearAlgebra/RealSpaceMatrix.hpp"
#include "LinearAlgebra/Vector.hpp"

#include "Atom/atom.hpp"
#include "Bond/bond.hpp"
#include "config.hpp"
#include "Descriptors/AtomIdDescriptor.hpp"
#include "Fragmentation/Exporters/HydrogenPool.hpp"
#include "Fragmentation/HydrogenSaturation_enum.hpp"
#include "Graph/BondGraph.hpp"
#include "World.hpp"

SaturatedFragment::SaturatedFragment(
    const KeySet &_set,
    KeySetsInUse_t &_container,
    HydrogenPool &_hydrogens,
    const enum HydrogenTreatment _treatment,
    const enum HydrogenSaturation _saturation,
    const GlobalSaturationPositions_t &_globalsaturationpositions) :
  container(_container),
  set(_set),
  hydrogens(_hydrogens),
  FullMolecule(set),
  treatment(_treatment),
  saturation(_saturation)
{
  // add to in-use container
  ASSERT( container.find(set) == container.end(),
      "SaturatedFragment::SaturatedFragment() - the set "
      +toString(set)+" is already marked as in use.");
  container.insert(set);

  // prepare saturation hydrogens, either using global information
  // or if not given, local information (created in the function)
  if (_globalsaturationpositions.empty())
    saturate();
  else
    saturate(_globalsaturationpositions);
}

SaturatedFragment::~SaturatedFragment()
{
  // release all saturation hydrogens if present
  for (KeySet::iterator iter = SaturationHydrogens.begin();
      !SaturationHydrogens.empty();
      iter = SaturationHydrogens.begin()) {
    hydrogens.releaseHydrogen(*iter);
    SaturationHydrogens.erase(iter);
  }

  // remove ourselves from in-use container
  KeySetsInUse_t::iterator iter = container.find(set);
  ASSERT( container.find(set) != container.end(),
      "SaturatedFragment::SaturatedFragment() - the set "
      +toString(set)+" is not marked as in use.");
  container.erase(iter);
}

typedef std::vector<atom *> atoms_t;

atoms_t gatherAllAtoms(const KeySet &_FullMolecule)
{
  atoms_t atoms;
  for (KeySet::const_iterator iter = _FullMolecule.begin();
      iter != _FullMolecule.end();
      ++iter) {
    atom * const Walker = World::getInstance().getAtom(AtomById(*iter));
    ASSERT( Walker != NULL,
        "gatherAllAtoms() - id "
        +toString(*iter)+" is unknown to World.");
    atoms.push_back(Walker);
  }

  return atoms;
}

typedef std::map<atom *, BondList > CutBonds_t;

CutBonds_t gatherCutBonds(
    const atoms_t &_atoms,
    const KeySet &_set,
    const enum HydrogenTreatment _treatment)
{
  //  bool LonelyFlag = false;
  CutBonds_t CutBonds;
  for (atoms_t::const_iterator iter = _atoms.begin();
      iter != _atoms.end();
      ++iter) {
    atom * const Walker = *iter;

    // go through all bonds
    const BondList& ListOfBonds = Walker->getListOfBonds();
    for (BondList::const_iterator BondRunner = ListOfBonds.begin();
        BondRunner != ListOfBonds.end();
        ++BondRunner) {
      atom * const OtherWalker = (*BondRunner)->GetOtherAtom(Walker);
      // if other atom is in key set or is a specially treated hydrogen
      if (_set.find(OtherWalker->getId()) != _set.end()) {
        LOG(4, "DEBUG: Walker " << *Walker << " is bound to " << *OtherWalker << ".");
      } else if ((_treatment == ExcludeHydrogen)
          && (OtherWalker->getElementNo() == (atomicNumber_t)1)) {
        LOG(4, "DEBUG: Walker " << *Walker << " is bound to specially treated hydrogen " <<
            *OtherWalker << ".");
      } else {
        LOG(4, "DEBUG: Walker " << *Walker << " is bound to "
            << *OtherWalker << ", who is not in this fragment molecule.");
          if (CutBonds.count(Walker) == 0)
            CutBonds.insert( std::make_pair(Walker, BondList() ));
          CutBonds[Walker].push_back(*BondRunner);
      }
    }
  }

  return CutBonds;
}

typedef std::vector<atomId_t> atomids_t;

atomids_t gatherPresentExcludedHydrogens(
    const atoms_t &_atoms,
    const KeySet &_set,
    const enum HydrogenTreatment _treatment)
{
  //  bool LonelyFlag = false;
  atomids_t ExcludedHydrogens;
  for (atoms_t::const_iterator iter = _atoms.begin();
      iter != _atoms.end();
      ++iter) {
    atom * const Walker = *iter;

    // go through all bonds
    const BondList& ListOfBonds = Walker->getListOfBonds();
    for (BondList::const_iterator BondRunner = ListOfBonds.begin();
        BondRunner != ListOfBonds.end();
        ++BondRunner) {
      atom * const OtherWalker = (*BondRunner)->GetOtherAtom(Walker);
      // if other atom is in key set or is a specially treated hydrogen
      if (_set.find(OtherWalker->getId()) != _set.end()) {
        LOG(6, "DEBUG: OtherWalker " << *OtherWalker << " is in set already.");
      } else if ((_treatment == ExcludeHydrogen)
          && (OtherWalker->getElementNo() == (atomicNumber_t)1)) {
        LOG(5, "DEBUG: Adding excluded hydrogen OtherWalker " << *OtherWalker << ".");
        ExcludedHydrogens.push_back(OtherWalker->getId());
      } else {
        LOG(6, "DEBUG: OtherWalker " << *Walker << " is not in this fragment molecule and no hydrogen.");
      }
    }
  }

  return ExcludedHydrogens;
}

void SaturatedFragment::saturate()
{
  // so far, we just have a set of keys. Hence, convert to atom refs
  // and gather all atoms in a vector
  std::vector<atom *> atoms = gatherAllAtoms(FullMolecule);

  // go through each atom of the fragment and gather all cut bonds in list
  CutBonds_t CutBonds = gatherCutBonds(atoms, set, treatment);

  // add excluded hydrogens to FullMolecule if treated specially
  if (treatment == ExcludeHydrogen) {
    atomids_t ExcludedHydrogens = gatherPresentExcludedHydrogens(atoms, set, treatment);
    FullMolecule.insert(ExcludedHydrogens.begin(), ExcludedHydrogens.end());
  }

  // go through all cut bonds and replace with a hydrogen
  for (CutBonds_t::const_iterator atomiter = CutBonds.begin();
      atomiter != CutBonds.end(); ++atomiter) {
    atom * const Walker = atomiter->first;
    if (!saturateAtom(Walker, atomiter->second))
      exit(1);
  }
}

void SaturatedFragment::saturate(
    const GlobalSaturationPositions_t &_globalsaturationpositions)
{
  // so far, we just have a set of keys. Hence, convert to atom refs
  // and gather all atoms in a vector
  std::vector<atom *> atoms = gatherAllAtoms(FullMolecule);

  // go through each atom of the fragment and gather all cut bonds in list
  CutBonds_t CutBonds = gatherCutBonds(atoms, set, treatment);

  // add excluded hydrogens to FullMolecule if treated specially
  if (treatment == ExcludeHydrogen) {
    atomids_t ExcludedHydrogens = gatherPresentExcludedHydrogens(atoms, set, treatment);
    FullMolecule.insert(ExcludedHydrogens.begin(), ExcludedHydrogens.end());
  }

  // go through all cut bonds and replace with a hydrogen
  if (saturation == DoSaturate) {
    for (CutBonds_t::const_iterator atomiter = CutBonds.begin();
        atomiter != CutBonds.end(); ++atomiter) {
      atom * const Walker = atomiter->first;
      ASSERT( set.find(Walker->getId()) != set.end(),
          "SaturatedFragment::saturate() - key "
          +toString(*Walker)+"not in set?");
      LOG(4, "DEBUG: We are now saturating dangling bonds of " << *Walker);

      // gather set of positions for this atom from global map
      GlobalSaturationPositions_t::const_iterator mapiter =
          _globalsaturationpositions.find(Walker->getId());
      ASSERT( mapiter != _globalsaturationpositions.end(),
          "SaturatedFragment::saturate() - no global information for "
          +toString(*Walker));
      const SaturationsPositionsPerNeighbor_t &saturationpositions =
          mapiter->second;

      // go through all cut bonds for this atom
      for (BondList::const_iterator bonditer = atomiter->second.begin();
          bonditer != atomiter->second.end(); ++bonditer) {
        atom * const OtherWalker = (*bonditer)->GetOtherAtom(Walker);

        // get positions from global map
        SaturationsPositionsPerNeighbor_t::const_iterator positionsiter =
            saturationpositions.find(OtherWalker->getId());
        ASSERT(positionsiter != saturationpositions.end(),
            "SaturatedFragment::saturate() - no information on bond neighbor "
            +toString(*OtherWalker)+" to atom "+toString(*Walker));
        ASSERT(!positionsiter->second.empty(),
            "SaturatedFragment::saturate() - no positions for saturating bond to"
            +toString(*OtherWalker)+" to atom "+toString(*Walker));

//        // get typical bond distance from elements database
//        double BondDistance = Walker->getType()->getHBondDistance(positionsiter->second.size()-1);
//        if (BondDistance < 0.) {
//          ELOG(2, "saturateAtoms() - no typical hydrogen bond distance of degree "
//              +toString(positionsiter->second.size())+" for element "
//              +toString(Walker->getType()->getName()));
//          // try bond degree 1 distance
//          BondDistance = Walker->getType()->getHBondDistance(1-1);
//          if (BondDistance < 0.) {
//            ELOG(1, "saturateAtoms() - no typical hydrogen bond distance for element "
//                +toString(Walker->getType()->getName()));
//            BondDistance = 1.;
//          }
//        }

        // place hydrogen at each point
        LOG(4, "DEBUG: Places to saturate for atom " << *OtherWalker
            << " are " << positionsiter->second);
        atom * const father = Walker;
        for (SaturationsPositions_t::const_iterator positer = positionsiter->second.begin();
            positer != positionsiter->second.end(); ++positer) {
          const atom& hydrogen =
              setHydrogenReplacement(Walker, *positer, 1., father);
          FullMolecule.insert(hydrogen.getId());
        }
      }
    }
  } else
    LOG(3, "INFO: We are not saturating cut bonds.");
}

const atom& SaturatedFragment::setHydrogenReplacement(
  const atom * const _OwnerAtom,
  const Vector &_position,
  const double _distance,
  atom * const _father)
{
  atom * const _atom = hydrogens.leaseHydrogen();    // new atom
  _atom->setPosition( _OwnerAtom->getPosition() + _distance * _position );
  // always set as fixed ion (not moving during molecular dynamics simulation)
  _atom->setFixedIon(true);
  // if we replace hydrogen, we mark it as our father, otherwise we are just an added hydrogen with no father
  _atom->setFather(_father);
  SaturationHydrogens.insert(_atom->getId());

  return *_atom;
}

bool SaturatedFragment::saturateAtom(
    atom * const _atom,
    const BondList &_cutbonds)
{
  // go through each bond and replace
  for (BondList::const_iterator bonditer = _cutbonds.begin();
      bonditer != _cutbonds.end(); ++bonditer) {
    atom * const OtherWalker = (*bonditer)->GetOtherAtom(_atom);
    if (!AddHydrogenReplacementAtom(
        (*bonditer),
        _atom,
        OtherWalker,
        World::getInstance().getConfig()->IsAngstroem == 1))
      return false;
  }
  return true;
}

bool SaturatedFragment::OutputConfig(
    std::ostream &out,
    const ParserTypes _type) const
{
  // gather all atoms in a vector
  std::vector<const atom *> atoms;
  for (KeySet::const_iterator iter = FullMolecule.begin();
      iter != FullMolecule.end();
      ++iter) {
    const atom * const Walker = const_cast<const World &>(World::getInstance()).
        getAtom(AtomById(*iter));
    ASSERT( Walker != NULL,
        "SaturatedFragment::OutputConfig() - id "
        +toString(*iter)+" is unknown to World.");
    atoms.push_back(Walker);
  }

  // TODO: ScanForPeriodicCorrection() is missing so far!
  // note however that this is not straight-forward for the following reasons:
  // - we do not copy all atoms anymore, hence we are forced to shift the real
  //   atoms hither and back again
  // - we use a long-range potential that supports periodic boundary conditions.
  //   Hence, there we would like the original configuration (split through the
  //   the periodic boundaries). Otherwise, we would have to shift (and probably
  //   interpolate) the potential with OBCs applying.

  // list atoms in fragment for debugging
  {
    std::stringstream output;
    output << "INFO: Contained atoms: ";
    for (std::vector<const atom *>::const_iterator iter = atoms.begin();
        iter != atoms.end(); ++iter) {
      output << (*iter)->getName() << " ";
    }
    LOG(3, output.str());
  }

  // store to stream via FragmentParser
  const bool intermediateResult =
      FormatParserStorage::getInstance().save(
          out,
          FormatParserStorage::getInstance().getSuffixFromType(_type),
          atoms);

  return intermediateResult;
}

atom * const SaturatedFragment::getHydrogenReplacement(atom * const replacement)
{
  atom * const _atom = hydrogens.leaseHydrogen();    // new atom
  _atom->setAtomicVelocity(replacement->getAtomicVelocity()); // copy velocity
  _atom->setFixedIon(replacement->getFixedIon());
  // if we replace hydrogen, we mark it as our father, otherwise we are just an added hydrogen with no father
  _atom->setFather(replacement);
  SaturationHydrogens.insert(_atom->getId());
  return _atom;
}

bool SaturatedFragment::AddHydrogenReplacementAtom(
    bond::ptr TopBond,
    atom *Origin,
    atom *Replacement,
    bool IsAngstroem)
{
//  Info info(__func__);
  bool AllWentWell = true;    // flag gathering the boolean return value of molecule::AddAtom and other functions, as return value on exit
  double bondlength;  // bond length of the bond to be replaced/cut
  double bondangle;  // bond angle of the bond to be replaced/cut
  double BondRescale;   // rescale value for the hydrogen bond length
  bond::ptr FirstBond;
  bond::ptr SecondBond; // Other bonds in double bond case to determine "other" plane
  atom *FirstOtherAtom = NULL, *SecondOtherAtom = NULL, *ThirdOtherAtom = NULL; // pointer to hydrogen atoms to be added
  double b,l,d,f,g, alpha, factors[NDIM];    // hold temporary values in triple bond case for coordination determination
  Vector Orthovector1, Orthovector2;  // temporary vectors in coordination construction
  Vector InBondvector;    // vector in direction of *Bond
  const RealSpaceMatrix &matrix =  World::getInstance().getDomain().getM();
  bond::ptr Binder;

  // create vector in direction of bond
  InBondvector = Replacement->getPosition() - Origin->getPosition();
  bondlength = InBondvector.Norm();

   // is greater than typical bond distance? Then we have to correct periodically
   // the problem is not the H being out of the box, but InBondvector have the wrong direction
   // due to Replacement or Origin being on the wrong side!
  const BondGraph * const BG = World::getInstance().getBondGraph();
  const range<double> MinMaxBondDistance(
      BG->getMinMaxDistance(Origin,Replacement));
  if (!MinMaxBondDistance.isInRange(bondlength)) {
//    LOG(4, "InBondvector is: " << InBondvector << ".");
    Orthovector1.Zero();
    for (int i=NDIM;i--;) {
      l = Replacement->at(i) - Origin->at(i);
      if (fabs(l) > MinMaxBondDistance.last) { // is component greater than bond distance (check against min not useful here)
        Orthovector1[i] = (l < 0) ? -1. : +1.;
      } // (signs are correct, was tested!)
    }
    Orthovector1 *= matrix;
    InBondvector -= Orthovector1; // subtract just the additional translation
    bondlength = InBondvector.Norm();
//    LOG(4, "INFO: Corrected InBondvector is now: " << InBondvector << ".");
  } // periodic correction finished

  InBondvector.Normalize();
  // get typical bond length and store as scale factor for later
  ASSERT(Origin->getType() != NULL,
      "SaturatedFragment::AddHydrogenReplacementAtom() - element of Origin is not given.");
  BondRescale = Origin->getType()->getHBondDistance(TopBond->getDegree()-1);
  if (BondRescale == -1) {
    ELOG(1, "There is no typical hydrogen bond distance in replacing bond (" << Origin->getName() << "<->" << Replacement->getName() << ") of degree " << TopBond->getDegree() << "!");
    BondRescale = Origin->getType()->getHBondDistance(TopBond->getDegree());
    if (BondRescale == -1) {
      ELOG(1, "There is no typical hydrogen bond distance in replacing bond (" << Origin->getName() << "<->" << Replacement->getName() << ") of any degree!");
      return false;
      BondRescale = bondlength;
    }
  } else {
    if (!IsAngstroem)
      BondRescale /= (1.*AtomicLengthToAngstroem);
  }

  // discern single, double and triple bonds
  switch(TopBond->getDegree()) {
    case 1:
      // check whether replacement has been an excluded hydrogen
      if (Replacement->getType()->getAtomicNumber() == HydrogenPool::HYDROGEN) { // neither rescale nor replace if it's already hydrogen
        FirstOtherAtom = Replacement;
        BondRescale = bondlength;
      } else {
        FirstOtherAtom = getHydrogenReplacement(Replacement);
        InBondvector *= BondRescale;   // rescale the distance vector to Hydrogen bond length
        FirstOtherAtom->setPosition(Origin->getPosition() + InBondvector); // set coordination to origin and add distance vector to replacement atom
      }
      FullMolecule.insert(FirstOtherAtom->getId());
//      LOG(4, "INFO: Added " << *FirstOtherAtom << " at: " << FirstOtherAtom->x << ".");
      break;
    case 2:
      {
        // determine two other bonds (warning if there are more than two other) plus valence sanity check
        const BondList& ListOfBonds = Origin->getListOfBonds();
        for (BondList::const_iterator Runner = ListOfBonds.begin();
            Runner != ListOfBonds.end();
            ++Runner) {
          if ((*Runner) != TopBond) {
            if (FirstBond == NULL) {
              FirstBond = (*Runner);
              FirstOtherAtom = (*Runner)->GetOtherAtom(Origin);
            } else if (SecondBond == NULL) {
              SecondBond = (*Runner);
              SecondOtherAtom = (*Runner)->GetOtherAtom(Origin);
            } else {
              ELOG(2, "Detected more than four bonds for atom " << Origin->getName());
            }
          }
        }
      }
      if (SecondOtherAtom == NULL) {  // then we have an atom with valence four, but only 3 bonds: one to replace and one which is TopBond (third is FirstBond)
        SecondBond = TopBond;
        SecondOtherAtom = Replacement;
      }
      if (FirstOtherAtom != NULL) { // then we just have this double bond and the plane does not matter at all
//        LOG(3, "Regarding the double bond (" << Origin->Name << "<->" << Replacement->Name << ") to be constructed: Taking " << FirstOtherAtom->Name << " and " << SecondOtherAtom->Name << " along with " << Origin->Name << " to determine orthogonal plane.");

        // determine the plane of these two with the *origin
        try {
          Orthovector1 = Plane(Origin->getPosition(), FirstOtherAtom->getPosition(), SecondOtherAtom->getPosition()).getNormal();
        }
        catch(LinearDependenceException &excp){
          LOG(0, boost::diagnostic_information(excp));
          // TODO: figure out what to do with the Orthovector in this case
          AllWentWell = false;
        }
      } else {
        Orthovector1.GetOneNormalVector(InBondvector);
      }
      //LOG(3, "INFO: Orthovector1: " << Orthovector1 << ".");
      // orthogonal vector and bond vector between origin and replacement form the new plane
      Orthovector1.MakeNormalTo(InBondvector);
      Orthovector1.Normalize();
      //LOG(3, "ReScaleCheck: " << Orthovector1.Norm() << " and " << InBondvector.Norm() << ".");

      // create the two Hydrogens ...
      FirstOtherAtom = getHydrogenReplacement(Replacement);
      SecondOtherAtom = getHydrogenReplacement(Replacement);
      FullMolecule.insert(FirstOtherAtom->getId());
      FullMolecule.insert(SecondOtherAtom->getId());
      bondangle = Origin->getType()->getHBondAngle(1);
      if (bondangle == -1) {
        ELOG(1, "There is no typical hydrogen bond angle in replacing bond (" << Origin->getName() << "<->" << Replacement->getName() << ") of degree " << TopBond->getDegree() << "!");
        return false;
        bondangle = 0;
      }
      bondangle *= M_PI/180./2.;
//      LOG(3, "INFO: ReScaleCheck: InBondvector " << InBondvector << ", " << Orthovector1 << ".");
//      LOG(3, "Half the bond angle is " << bondangle << ", sin and cos of it: " << sin(bondangle) << ", " << cos(bondangle));
      FirstOtherAtom->Zero();
      SecondOtherAtom->Zero();
      for(int i=NDIM;i--;) { // rotate by half the bond angle in both directions (InBondvector is bondangle = 0 direction)
        FirstOtherAtom->set(i, InBondvector[i] * cos(bondangle) + Orthovector1[i] * (sin(bondangle)));
        SecondOtherAtom->set(i, InBondvector[i] * cos(bondangle) + Orthovector1[i] * (-sin(bondangle)));
      }
      FirstOtherAtom->Scale(BondRescale);  // rescale by correct BondDistance
      SecondOtherAtom->Scale(BondRescale);
      //LOG(3, "ReScaleCheck: " << FirstOtherAtom->x.Norm() << " and " << SecondOtherAtom->x.Norm() << ".");
      *FirstOtherAtom += Origin->getPosition();
      *SecondOtherAtom += Origin->getPosition();
      // ... and add to molecule
//      LOG(4, "INFO: Added " << *FirstOtherAtom << " at: " << FirstOtherAtom->x << ".");
//      LOG(4, "INFO: Added " << *SecondOtherAtom << " at: " << SecondOtherAtom->x << ".");
      break;
    case 3:
      // take the "usual" tetraoidal angle and add the three Hydrogen in direction of the bond (height of the tetraoid)
      FirstOtherAtom = getHydrogenReplacement(Replacement);
      SecondOtherAtom = getHydrogenReplacement(Replacement);
      ThirdOtherAtom = getHydrogenReplacement(Replacement);
      FullMolecule.insert(FirstOtherAtom->getId());
      FullMolecule.insert(SecondOtherAtom->getId());
      FullMolecule.insert(ThirdOtherAtom->getId());

      // we need to vectors orthonormal the InBondvector
      AllWentWell = AllWentWell && Orthovector1.GetOneNormalVector(InBondvector);
//      LOG(3, "INFO: Orthovector1: " << Orthovector1 << ".");
      try{
        Orthovector2 = Plane(InBondvector, Orthovector1,0).getNormal();
      }
      catch(LinearDependenceException &excp) {
        LOG(0, boost::diagnostic_information(excp));
        AllWentWell = false;
      }
//      LOG(3, "INFO: Orthovector2: " << Orthovector2 << ".")

      // create correct coordination for the three atoms
      alpha = (Origin->getType()->getHBondAngle(2))/180.*M_PI/2.;  // retrieve triple bond angle from database
      l = BondRescale;        // desired bond length
      b = 2.*l*sin(alpha);    // base length of isosceles triangle
      d = l*sqrt(cos(alpha)*cos(alpha) - sin(alpha)*sin(alpha)/3.);   // length for InBondvector
      f = b/sqrt(3.);   // length for Orthvector1
      g = b/2.;         // length for Orthvector2
//      LOG(3, "Bond length and half-angle: " << l << ", " << alpha << "\t (b,d,f,g) = " << b << ", " << d << ", " << f << ", " << g << ", ");
//      LOG(3, "The three Bond lengths: " << sqrt(d*d+f*f) << ", " << sqrt(d*d+(-0.5*f)*(-0.5*f)+g*g) << ", "  << sqrt(d*d+(-0.5*f)*(-0.5*f)+g*g));
      factors[0] = d;
      factors[1] = f;
      factors[2] = 0.;
      FirstOtherAtom->LinearCombinationOfVectors(InBondvector, Orthovector1, Orthovector2, factors);
      factors[1] = -0.5*f;
      factors[2] = g;
      SecondOtherAtom->LinearCombinationOfVectors(InBondvector, Orthovector1, Orthovector2, factors);
      factors[2] = -g;
      ThirdOtherAtom->LinearCombinationOfVectors(InBondvector, Orthovector1, Orthovector2, factors);

      // rescale each to correct BondDistance
//      FirstOtherAtom->x.Scale(&BondRescale);
//      SecondOtherAtom->x.Scale(&BondRescale);
//      ThirdOtherAtom->x.Scale(&BondRescale);

      // and relative to *origin atom
      *FirstOtherAtom += Origin->getPosition();
      *SecondOtherAtom += Origin->getPosition();
      *ThirdOtherAtom += Origin->getPosition();

      // ... and add to molecule
//      LOG(4, "INFO: Added " << *FirstOtherAtom << " at: " << FirstOtherAtom->x << ".");
//      LOG(4, "INFO: Added " << *SecondOtherAtom << " at: " << SecondOtherAtom->x << ".");
//      LOG(4, "INFO: Added " << *ThirdOtherAtom << " at: " << ThirdOtherAtom->x << ".");
      break;
    default:
      ELOG(1, "BondDegree does not state single, double or triple bond!");
      AllWentWell = false;
      break;
  }

  return AllWentWell;
};