Index: src/Fragmentation/Exporters/SaturationDistanceMaximizer.cpp
===================================================================
--- src/Fragmentation/Exporters/SaturationDistanceMaximizer.cpp	(revision a1d1dd8b043f6987e14d6d37973e71bd8ccad587)
+++ src/Fragmentation/Exporters/SaturationDistanceMaximizer.cpp	(revision a1d1dd8b043f6987e14d6d37973e71bd8ccad587)
@@ -0,0 +1,302 @@
+/*
+ * Project: MoleCuilder
+ * Description: creates and alters molecular systems
+ * Copyright (C)  2014 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/>.
+ */
+
+/*
+ * SaturationDistanceMaximizer.cpp
+ *
+ *  Created on: Jul 27, 2014
+ *      Author: heber
+ */
+
+// include config.h
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include "CodePatterns/MemDebug.hpp"
+
+#include "SaturationDistanceMaximizer.hpp"
+
+#include <cmath>
+#include <gsl/gsl_multimin.h>
+#include <gsl/gsl_vector.h>
+
+#include "CodePatterns/Log.hpp"
+
+double
+func(const gsl_vector *x, void *adata)
+{
+  // get the object whose functions we call
+  SaturationDistanceMaximizer::Advocate *maximizer =
+      static_cast<SaturationDistanceMaximizer::Advocate *>(adata);
+  // set alphas
+  maximizer->setAlphas(x);
+  // calculate function value and return
+  return maximizer->calculatePenality();
+}
+
+void
+jacf(const gsl_vector *x, void *adata, gsl_vector *g)
+{
+  // get the object whose functions we call
+  SaturationDistanceMaximizer::Advocate *maximizer =
+      static_cast<SaturationDistanceMaximizer::Advocate *>(adata);
+  // set alphas
+  maximizer->setAlphas(x);
+  // calculate function gradient and return
+  std::vector<double> gradient = maximizer->calculatePenalityGradient();
+  for (unsigned int i=0;i<gradient.size();++i)
+    gsl_vector_set(g,i,gradient[i]);
+}
+
+void
+funcjacf(const gsl_vector *x, void *adata, double *f, gsl_vector *g)
+{
+  // get the object whose functions we call
+  SaturationDistanceMaximizer::Advocate *maximizer =
+      static_cast<SaturationDistanceMaximizer::Advocate *>(adata);
+  // set alphas
+  maximizer->setAlphas(x);
+  // calculate function value and return
+  *f = maximizer->calculatePenality();
+  std::vector<double> gradient = maximizer->calculatePenalityGradient();
+  for (unsigned int i=0;i<gradient.size();++i)
+    gsl_vector_set(g,i,gradient[i]);
+}
+
+std::vector<double> SaturationDistanceMaximizer::getAlphas() const
+{
+  std::vector<double> alphas;
+  PositionContainers_t::iterator containeriter = PositionContainers.begin();
+  for (unsigned int i=0; i<PositionContainers.size(); ++i, ++containeriter)
+    alphas.push_back( (*containeriter)->alpha );
+  return alphas;
+}
+
+void SaturationDistanceMaximizer::setAlphas(const gsl_vector *x)
+{
+  PositionContainers_t::iterator containeriter = PositionContainers.begin();
+  for (unsigned int i=0; i<PositionContainers.size(); ++i, ++containeriter)
+    (*containeriter)->alpha = gsl_vector_get(x,i);
+}
+
+void SaturationDistanceMaximizer::operator()()
+{
+  // some control constants
+  const double tolerance = 1e-6;
+  const unsigned int MAXITERATIONS = 100;
+
+  const gsl_multimin_fdfminimizer_type *T;
+  gsl_multimin_fdfminimizer *s;
+
+  gsl_vector *x;
+  gsl_multimin_function_fdf my_func;
+
+  const unsigned int N = PositionContainers.size();
+  my_func.n = N;
+  my_func.f = &func;
+  my_func.df = &jacf;
+  my_func.fdf = &funcjacf;
+  my_func.params = getAdvocate();
+
+  // allocate argument and set to zero
+  x = gsl_vector_alloc(N);
+  for (unsigned int i=0;i<N;++i)
+    gsl_vector_set(x, i, 0.);
+
+  // set minimizer and allocate workspace
+  T = gsl_multimin_fdfminimizer_vector_bfgs;
+  s = gsl_multimin_fdfminimizer_alloc (T, N);
+
+  // initialize minimizer
+  gsl_multimin_fdfminimizer_set(s, &my_func, x, 0.1, tolerance); /* tolerance */
+
+  size_t iter = 0;
+  int status = 0;
+  do {
+    ++iter;
+    status = gsl_multimin_fdfminimizer_iterate(s);
+
+    if (status)
+      break;
+
+    status = gsl_multimin_test_gradient(s->gradient, tolerance);
+
+  } while ((status = GSL_CONTINUE) && (iter < MAXITERATIONS));
+
+  // set to solution
+  setAlphas(s->x);
+
+  // print solution
+  if (DoLog(4)) {
+    std::stringstream sstream;
+    sstream << "DEBUG: Minimal alphas are ";
+    for (unsigned int i=0;i<N;++i)
+      sstream << gsl_vector_get(s->x,i) << ((i!= N-1) ? "," : "");
+    LOG(4, sstream.str());
+  }
+
+  // free memory
+  gsl_multimin_fdfminimizer_free(s);
+  gsl_vector_free(x);
+}
+
+SaturationDistanceMaximizer::Advocate* SaturationDistanceMaximizer::getAdvocate()
+{
+  return new Advocate(*this);
+}
+
+SaturationDistanceMaximizer::position_bins_t
+SaturationDistanceMaximizer::getAllPositionBins() const
+{
+  position_bins_t position_bins;
+  position_bins.reserve(PositionContainers.size());
+  for (PositionContainers_t::const_iterator containeriter = PositionContainers.begin();
+      containeriter != PositionContainers.end(); ++containeriter)
+    position_bins.push_back( (*containeriter)->getPositions() );
+
+  return position_bins;
+}
+
+double SaturationDistanceMaximizer::calculatePenality() const
+{
+  double penalty = 0.;
+
+  LOG(6, "DEBUG: Current alphas are " << getAlphas());
+
+  // gather all positions
+  position_bins_t position_bins = getAllPositionBins();
+
+  // go through both bins (but with i<j)
+  for (position_bins_t::const_iterator firstbiniter = position_bins.begin();
+      firstbiniter != position_bins.end(); ++firstbiniter) {
+    for (position_bins_t::const_iterator secondbiniter = firstbiniter;
+        secondbiniter != position_bins.end(); ++secondbiniter) {
+      if (firstbiniter == secondbiniter)
+        continue;
+
+      // then in each bin take each position
+      for (SaturatedBond::positions_t::const_iterator firstpositioniter = firstbiniter->begin();
+          firstpositioniter != firstbiniter->end(); ++firstpositioniter) {
+        for (SaturatedBond::positions_t::const_iterator secondpositioniter = secondbiniter->begin();
+            secondpositioniter != secondbiniter->end(); ++secondpositioniter) {
+          // Both iters are from different bins, can never be the same.
+          // We do not penalize over positions from same bin as their positions
+          // are fixed.
+
+          // We penalize by one over the squared distance
+          penalty += 1./(firstpositioniter->DistanceSquared(*secondpositioniter));
+        }
+      }
+    }
+  }
+
+  LOG(4, "DEBUG: Penalty is " << penalty);
+
+  return penalty;
+}
+
+#ifdef HAVE_INLINE
+inline
+#else
+static
+#endif
+size_t calculateHydrogenNo(
+    const SaturatedBond::positions_t::const_iterator &_start,
+    const SaturatedBond::positions_t::const_iterator &_current)
+{
+  const size_t HydrogenNo = std::distance(_start, _current);
+  ASSERT( (HydrogenNo >= 0) && (HydrogenNo <= 2),
+      "calculatePenalityGradient() - hydrogen no not in [0,2].");
+  return HydrogenNo;
+}
+
+std::vector<double> SaturationDistanceMaximizer::calculatePenalityGradient() const
+{
+  // gather all positions
+  const position_bins_t position_bins = getAllPositionBins();
+  LOG(6, "DEBUG: Current alphas are " << getAlphas());
+
+  std::vector<double> gradient(position_bins.size(), 0.);
+
+  std::vector<double>::iterator biniter = gradient.begin();
+  PositionContainers_t::const_iterator bonditer = PositionContainers.begin();
+  position_bins_t::const_iterator firstbiniter = position_bins.begin();
+  // go through each bond/gradient component/alpha
+  for(; biniter != gradient.end(); ++biniter, ++bonditer, ++firstbiniter) {
+    LOG(5, "DEBUG: Current bond is " << **bonditer << ", current bin is #"
+        << std::distance(gradient.begin(), biniter) << ", set of positions are "
+        << *firstbiniter);
+    // skip bin if it belongs to a degree-1 bond (no alpha dependency here)
+    if ((*bonditer)->saturated_bond.getDegree() == 1) {
+      LOG(6, "DEBUG: Skipping due to degree 1.");
+      continue;
+    }
+
+    // in the bin go through each position
+    for (SaturatedBond::positions_t::const_iterator firstpositioniter = firstbiniter->begin();
+        firstpositioniter != firstbiniter->end(); ++firstpositioniter) {
+      LOG(6, "DEBUG: Current position is " << *firstpositioniter);
+
+      // count the hydrogen we are looking at: Each is placed at a different position!
+      const size_t HydrogenNo =
+          calculateHydrogenNo(firstbiniter->begin(), firstpositioniter);
+      const double alpha = (*bonditer)->alpha
+          + (double)HydrogenNo * 2.*M_PI/(double)(*bonditer)->saturated_bond.getDegree();
+      LOG(6, "DEBUG: HydrogenNo is " << HydrogenNo << ", alpha is " << alpha);
+
+      // and go through each other bin
+      for (position_bins_t::const_iterator secondbiniter = position_bins.begin();
+          secondbiniter != position_bins.end(); ++secondbiniter) {
+        // distance between hydrogens in same bin is not affected by the angle
+//        if (firstbiniter == secondbiniter)
+//          continue;
+
+        // in the other bin go through each position
+        for (SaturatedBond::positions_t::const_iterator secondpositioniter = secondbiniter->begin();
+            secondpositioniter != secondbiniter->end(); ++secondpositioniter) {
+          if (firstpositioniter == secondpositioniter) {
+            LOG(7, "DEBUG: Skipping due to same positions.");
+            continue;
+          }
+          LOG(7, "DEBUG: Second position is " << *secondpositioniter);
+
+          // iters are from different bins, can never be the same
+          const Vector distance = *firstpositioniter - *secondpositioniter;
+          const double temp = -2./pow(distance.NormSquared(), 2);
+          const Vector tempVector =
+              (-sin(alpha)*(*bonditer)->vector_a)
+              +(cos(alpha)*(*bonditer)->vector_b);
+          const double result = temp * (distance.ScalarProduct(tempVector));
+          *biniter += 2.*result; //for x_i and x_j
+          LOG(7, "DEBUG: Total is " << result << ", temp is " << temp << ", tempVector is " << tempVector
+              << ", and bondVector is " << distance << ": bin = " << *biniter);
+        }
+      }
+    }
+  }
+
+  LOG(4, "DEBUG: Gradient of penalty is " << gradient);
+
+  return gradient;
+}
+
