diff --git a/src/Algorithms/CavityAutophaser.cpp b/src/Algorithms/CavityAutophaser.cpp
index 11a9d4e9c1e38361cb8ef22f0bbd2c721d8dc592..84739022afe0c695162e9411fe15ae37b2d52006 100644
--- a/src/Algorithms/CavityAutophaser.cpp
+++ b/src/Algorithms/CavityAutophaser.cpp
@@ -71,7 +71,7 @@ double CavityAutophaser::getPhaseAtMaxEnergy(const Vector_t &R,
                    << std::left << std::setw(68) << std::setfill('*') << ss.str()
                    << std::setfill(' ') << endl);
     if (!apVeto) {
-        double initialEnergy = Util::getEnergy(P, itsReference_m.getM()) * 1e-6;
+        double initialEnergy = Util::getKineticEnergy(P, itsReference_m.getM()) * 1e-6;
         double AstraPhase    = 0.0;
         double designEnergy  = element->getDesignEnergy();
 
@@ -190,7 +190,7 @@ double CavityAutophaser::guessCavityPhase(double t) {
         return orig_phi;
     }
 
-    Phimax = element->getAutoPhaseEstimate(getEnergyMeV(refP),
+    Phimax = element->getAutoPhaseEstimate(Util::getKineticEnergy(refP, itsReference_m.getM()) * 1e-6,
                                            t,
                                            itsReference_m.getQ(),
                                            itsReference_m.getM() * 1e-6);
@@ -287,7 +287,7 @@ double CavityAutophaser::track(Vector_t /*R*/,
                                                             out);
     rfc->setPhasem(initialPhase);
 
-    double finalKineticEnergy = Util::getEnergy(Vector_t(0.0, 0.0, pe.first), itsReference_m.getM() * 1e-6);
+    double finalKineticEnergy = Util::getKineticEnergy(Vector_t(0.0, 0.0, pe.first), itsReference_m.getM() * 1e-6);
 
     return finalKineticEnergy;
 }
\ No newline at end of file
diff --git a/src/Algorithms/CavityAutophaser.h b/src/Algorithms/CavityAutophaser.h
index 29db048b24349a39a7e9b93b5518d18aafc5571a..a4e49c8da834b0c8cd8d8010aea35439c4315403 100644
--- a/src/Algorithms/CavityAutophaser.h
+++ b/src/Algorithms/CavityAutophaser.h
@@ -28,8 +28,6 @@ private:
                  const double dt,
                  const double phase,
                  std::ofstream *out = NULL) const;
-    double getEnergyMeV(const Vector_t &P);
-    double getMomentum(double kineticEnergyMeV);
 
     const PartData &itsReference_m;
     std::shared_ptr<Component> itsCavity_m;
@@ -39,14 +37,4 @@ private:
 
 };
 
-inline
-double CavityAutophaser::getEnergyMeV(const Vector_t &P) {
-    return itsReference_m.getM() * 1e-6 * (sqrt(dot(P,P) + 1) - 1);
-}
-
-inline
-double CavityAutophaser::getMomentum(double kineticEnergyMeV) {
-    return sqrt(std::pow(kineticEnergyMeV / (itsReference_m.getM() * 1e-6) + 1, 2) - 1);
-}
-
 #endif
\ No newline at end of file
diff --git a/src/Algorithms/ParallelTTracker.cpp b/src/Algorithms/ParallelTTracker.cpp
index 49f56f3e87fbec86534b53c7719095938fa338d9..0d4c4abd6343cbcb0d311853c68b22bce318ceb3 100644
--- a/src/Algorithms/ParallelTTracker.cpp
+++ b/src/Algorithms/ParallelTTracker.cpp
@@ -1160,7 +1160,7 @@ void ParallelTTracker::findStartPosition(const BorisPusher &pusher) {
     selectDT();
 
     if ((back_track && itsOpalBeamline_m.containsSource()) ||
-        Util::getEnergy(itsBunch_m->RefPartP_m, itsBunch_m->getM()) < 1e-3) {
+        Util::getKineticEnergy(itsBunch_m->RefPartP_m, itsBunch_m->getM()) < 1e-3) {
         double gamma = 0.1 / itsBunch_m->getM() + 1.0;
         double beta = sqrt(1.0 - 1.0 / std::pow(gamma, 2));
         itsBunch_m->RefPartP_m = itsBunch_m->toLabTrafo_m.rotateTo(beta * gamma * Vector_t(0, 0, 1));
diff --git a/src/Classic/AbsBeamline/RFCavity.cpp b/src/Classic/AbsBeamline/RFCavity.cpp
index 5f08002c23f702ca573d735971b26e5c847e2fbe..55a0fe38572ceca70c8336fa281c725124437232 100644
--- a/src/Classic/AbsBeamline/RFCavity.cpp
+++ b/src/Classic/AbsBeamline/RFCavity.cpp
@@ -494,22 +494,22 @@ ElementBase::ElementType RFCavity::getType() const {
 double RFCavity::getAutoPhaseEstimateFallback(double E0, double t0, double q, double mass) {
 
     const double dt = 1e-13;
-    const double p0 = Util::getP(E0, mass);
+    const double p0 = Util::getBetaGamma(E0, mass);
     const double origPhase =getPhasem();
     double dphi = Physics::pi / 18;
 
     double phi = 0.0;
     setPhasem(phi);
-    std::pair<double, double> ret = trackOnAxisParticle(E0 / mass, t0, dt, q, mass);
+    std::pair<double, double> ret = trackOnAxisParticle(p0, t0, dt, q, mass);
     double phimax = 0.0;
-    double Emax = Util::getEnergy(Vector_t(0.0, 0.0, ret.first), mass);
+    double Emax = Util::getKineticEnergy(Vector_t(0.0, 0.0, ret.first), mass);
     phi += dphi;
 
     for (unsigned int j = 0; j < 2; ++ j) {
         for (unsigned int i = 0; i < 36; ++ i, phi += dphi) {
             setPhasem(phi);
             ret = trackOnAxisParticle(p0, t0, dt, q, mass);
-            double Ekin = Util::getEnergy(Vector_t(0.0, 0.0, ret.first), mass);
+            double Ekin = Util::getKineticEnergy(Vector_t(0.0, 0.0, ret.first), mass);
             if (Ekin > Emax) {
                 Emax = Ekin;
                 phimax = phi;
@@ -635,7 +635,7 @@ double RFCavity::getAutoPhaseEstimate(const double &E0, const double &t0, const
         }
 
         double cosine_part = 0.0, sine_part = 0.0;
-        double p0 = std::sqrt((E0 / mass + 1) * (E0 / mass + 1) - 1);
+        double p0 = Util::getBetaGamma(E0, mass);
         cosine_part += scale_m * std::cos(frequency_m * t0) * F[0];
         sine_part += scale_m * std::sin(frequency_m * t0) * F[0];
 
@@ -675,11 +675,11 @@ std::pair<double, double> RFCavity::trackOnAxisParticle(const double &p0,
     const double zend = length_m + startField_m;
 
     Vector_t z(0.0, 0.0, zbegin);
-    double dz = 0.5 * p(2) / std::sqrt(1.0 + dot(p, p)) * cdt;
+    double dz = 0.5 * p(2) / Util::getGamma(p) * cdt;
     Vector_t Ef(0.0), Bf(0.0);
 
     if (out) *out << std::setw(18) << z[2]
-                  << std::setw(18) << Util::getEnergy(p, mass)
+                  << std::setw(18) << Util::getKineticEnergy(p, mass)
                   << std::endl;
     while (z(2) + dz < zend && z(2) + dz > zbegin) {
         z /= cdt;
@@ -700,7 +700,7 @@ std::pair<double, double> RFCavity::trackOnAxisParticle(const double &p0,
         t += dt;
 
         if (out) *out << std::setw(18) << z[2]
-                      << std::setw(18) << Util::getEnergy(p, mass)
+                      << std::setw(18) << Util::getKineticEnergy(p, mass)
                       << std::endl;
     }
 
diff --git a/src/Classic/Algorithms/PartBunch.cpp b/src/Classic/Algorithms/PartBunch.cpp
index b475a680dd1b5f7604acef6ec212cf60b14f0d23..2be8fe9af747162e8cb6e4722b4b1cb65c3e7aa8 100644
--- a/src/Classic/Algorithms/PartBunch.cpp
+++ b/src/Classic/Algorithms/PartBunch.cpp
@@ -123,8 +123,7 @@ void PartBunch::computeSelfFields(int binNumber) {
 
     if(fs_m->hasValidSolver()) {
         /// Mesh the whole domain
-        if(fs_m->getFieldSolverType() == "SAAMG")
-            resizeMesh();
+        resizeMesh();
 
         /// Scatter charge onto space charge grid.
         this->Q *= this->dt;
@@ -328,12 +327,14 @@ void PartBunch::computeSelfFields(int binNumber) {
 }
 
 void PartBunch::resizeMesh() {
+    if (fs_m->getFieldSolverType() != "SAAMG") {
+        return;
+    }
+
     double xmin = fs_m->solver_m->getXRangeMin();
     double xmax = fs_m->solver_m->getXRangeMax();
     double ymin = fs_m->solver_m->getYRangeMin();
     double ymax = fs_m->solver_m->getYRangeMax();
-    double zmin = fs_m->solver_m->getZRangeMin();
-    double zmax = fs_m->solver_m->getZRangeMax();
 
     if(xmin > rmin_m[0] || xmax < rmax_m[0] ||
        ymin > rmin_m[1] || ymax < rmax_m[1]) {
@@ -354,14 +355,14 @@ void PartBunch::resizeMesh() {
         boundp();
         get_bounds(rmin_m, rmax_m);
     }
-    Vector_t mymin = Vector_t(xmin, ymin , zmin);
-    Vector_t mymax = Vector_t(xmax, ymax , zmax);
 
-    for (int i = 0; i < 3; i++)
-        hr_m[i]   = (mymax[i] - mymin[i])/nr_m[i];
+    Vector_t origin = Vector_t(0.0, 0.0, 0.0);
+
+    // update the mesh origin and mesh spacing hr_m
+    fs_m->solver_m->resizeMesh(origin, hr_m, rmin_m, rmax_m, dh_m);
 
     getMesh().set_meshSpacing(&(hr_m[0]));
-    getMesh().set_origin(mymin);
+    getMesh().set_origin(origin);
 
     rho_m.initialize(getMesh(),
                      getFieldLayout(),
@@ -384,8 +385,7 @@ void PartBunch::computeSelfFields() {
 
     if(fs_m->hasValidSolver()) {
         //mesh the whole domain
-        if(fs_m->getFieldSolverType() == "SAAMG")
-            resizeMesh();
+        resizeMesh();
 
         //scatter charges onto grid
         this->Q *= this->dt;
@@ -510,8 +510,7 @@ void PartBunch::computeSelfFields_cycl(double gamma) {
 
     if(fs_m->hasValidSolver()) {
         /// mesh the whole domain
-        if(fs_m->getFieldSolverType() == "SAAMG")
-            resizeMesh();
+        resizeMesh();
 
         /// scatter particles charge onto grid.
         this->Q.scatter(this->rho_m, this->R, IntrplCIC_t());
@@ -639,8 +638,7 @@ void PartBunch::computeSelfFields_cycl(int bin) {
 
     if(fs_m->hasValidSolver()) {
         /// mesh the whole domain
-        if(fs_m->getFieldSolverType() == "SAAMG")
-            resizeMesh();
+        resizeMesh();
 
         /// scatter particles charge onto grid.
         this->Q.scatter(this->rho_m, this->R, IntrplCIC_t());
diff --git a/src/Classic/Algorithms/PartBunchBase.h b/src/Classic/Algorithms/PartBunchBase.h
index 76807acfa037f60c17faccd72b4f51a3a7b43870..47b865bd6587ccaacf28de35295d892cb421313a 100644
--- a/src/Classic/Algorithms/PartBunchBase.h
+++ b/src/Classic/Algorithms/PartBunchBase.h
@@ -411,6 +411,8 @@ public:
 //     virtual void setFieldLayout(FieldLayout_t* fLayout) = 0;
     virtual FieldLayout_t &getFieldLayout() = 0;
 
+    virtual void resizeMesh() { };
+
     /*
      * Wrapped member functions of IpplParticleBase
      */
diff --git a/src/Classic/Utilities/Util.h b/src/Classic/Utilities/Util.h
index edf7178517c527c010c2bfcfd183278465f0fd1a..42479de4b024055ced8e0e5cc28a80a702a933e2 100644
--- a/src/Classic/Utilities/Util.h
+++ b/src/Classic/Utilities/Util.h
@@ -6,6 +6,7 @@
 
 #include <string>
 #include <cstring>
+#include <limits>
 #include <sstream>
 #include <type_traits>
 #include <functional>
@@ -28,16 +29,23 @@ namespace Util {
     }
 
     inline
-    double getEnergy(Vector_t p, double mass) {
+    double getKineticEnergy(Vector_t p, double mass) {
         return (getGamma(p) - 1.0) * mass;
     }
 
     inline
-    double getP(double E, double mass) {
-        double gamma = E / mass + 1;
-        return std::sqrt(std::pow(gamma, 2.0) - 1.0);
+    double getBetaGamma(double Ekin, double mass) {
+        double value = std::sqrt(std::pow(Ekin / mass + 1.0, 2) - 1.0);
+        if (value < std::numeric_limits<double>::epsilon())
+            value = std::sqrt(2 * Ekin / mass);
+        return value;
     }
 
+    inline
+    double convertMomentumeVToBetaGamma(double p, double mass) {
+        return p / mass;
+    }
+  
     inline
     std::string getTimeString(double time, unsigned int precision = 3) {
         std::string timeUnit(" [ps]");
@@ -162,7 +170,7 @@ namespace Util {
     }
 
     Vector_t getTaitBryantAngles(Quaternion rotation, const std::string &elementName = "");
-
+  
     std::string toUpper(const std::string &str);
 
     std::string combineFilePath(std::initializer_list<std::string>);
diff --git a/src/Distribution/CMakeLists.txt b/src/Distribution/CMakeLists.txt
index 81cf1cdc64d776363db893c99d65af9b3b1c6c81..0fa70c8865e63ef9354fc3143fc3bdde3cee322f 100644
--- a/src/Distribution/CMakeLists.txt
+++ b/src/Distribution/CMakeLists.txt
@@ -1,6 +1,8 @@
 set (_SRCS
     Distribution.cpp
     LaserProfile.cpp
+    RealDiracMatrix.cpp
+    SigmaGenerator.cpp
     )
 
 include_directories (
@@ -12,10 +14,10 @@ add_opal_sources(${_SRCS})
 set (HDRS
     ClosedOrbitFinder.h
     Distribution.h
-    Harmonics.h
     LaserProfile.h
     MapGenerator.h
     matrix_vector_operation.h
+    RealDiracMatrix.h
     SigmaGenerator.h
     )
 
diff --git a/src/Distribution/ClosedOrbitFinder.h b/src/Distribution/ClosedOrbitFinder.h
index 8c6ec78a5a437b48c9960d39dcf1d22b66b4d0ed..7a351afe91e4690d26ddf87c010c05c72651d8b9 100644
--- a/src/Distribution/ClosedOrbitFinder.h
+++ b/src/Distribution/ClosedOrbitFinder.h
@@ -40,6 +40,7 @@
 #include "Utilities/Options.h"
 #include "Utilities/Options.h"
 #include "Utilities/OpalException.h"
+#include "Physics/Physics.h"
 
 #include "AbstractObjects/OpalData.h"
 
diff --git a/src/Distribution/Distribution.cpp b/src/Distribution/Distribution.cpp
index e4e1648269780fa2a23fed21d75af2c7d709ac4e..53f20fb0116e25c6a0200b3257c5a5b8622dbc18 100644
--- a/src/Distribution/Distribution.cpp
+++ b/src/Distribution/Distribution.cpp
@@ -7,7 +7,7 @@
 // OPAL is licensed under GNU GPL version 3.
 
 #include "Distribution/Distribution.h"
-#include "Distribution/SigmaGenerator.h"
+#include "Distribution/ClosedOrbitFinder.h"
 #include "AbsBeamline/SpecificElementVisitor.h"
 
 #include <cmath>
@@ -17,7 +17,6 @@
 #include <string>
 #include <vector>
 #include <numeric>
-#include <limits>
 
 // IPPL
 #include "DataSource/DataConnect.h"
@@ -212,16 +211,6 @@ Distribution::~Distribution() {
     delete laserProfile_m;
 }
 
-/*
-  void Distribution::printSigma(SigmaGenerator<double,unsigned int>::matrix_type& M, Inform& out) {
-  for (int i=0; i<M.size1(); ++i) {
-  for (int j=0; j<M.size2(); ++j) {
-  *gmsg  << M(i,j) << " ";
-  }
-  *gmsg << endl;
-  }
-  }
-*/
 
 /**
  * Calculate the local number of particles evenly and adjust node 0
@@ -885,14 +874,6 @@ void Distribution::chooseInputMomentumUnits(InputMomentumUnitsT::InputMomentumUn
 
 }
 
-double Distribution::converteVToBetaGamma(double valueIneV, double massIneV) {
-    double value = std::copysign(std::sqrt(std::pow(std::abs(valueIneV) / massIneV + 1.0, 2) - 1.0), valueIneV);
-    if (std::abs(value) < std::numeric_limits<double>::epsilon())
-        value = std::copysign(std::sqrt(2 * std::abs(valueIneV) / massIneV), valueIneV);
-
-    return value;
-}
-
 void Distribution::createDistributionBinomial(size_t numberOfParticles, double massIneV) {
 
     setDistParametersBinomial(massIneV);
@@ -1079,9 +1060,9 @@ void Distribution::createDistributionFromFile(size_t /*numberOfParticles*/, doub
                 saveProcessor = 0;
 
             if (inputMoUnits_m == InputMomentumUnitsT::EV) {
-                P(0) = converteVToBetaGamma(P(0), massIneV);
-                P(1) = converteVToBetaGamma(P(1), massIneV);
-                P(2) = converteVToBetaGamma(P(2), massIneV);
+                P(0) = Util::convertMomentumeVToBetaGamma(P(0), massIneV);
+                P(1) = Util::convertMomentumeVToBetaGamma(P(1), massIneV);
+                P(2) = Util::convertMomentumeVToBetaGamma(P(2), massIneV);
             }
 
             pmean_m += P;
@@ -1278,18 +1259,18 @@ void Distribution::createMatchedGaussDistribution(size_t numberOfParticles, doub
 
     bool writeMap = true;
 
-    typedef SigmaGenerator<double, unsigned int> sGenerator_t;
-    sGenerator_t *siggen = new sGenerator_t(I_m,
-                                            Attributes::getReal(itsAttr[Attrib::Distribution::EX])*1E6,
-                                            Attributes::getReal(itsAttr[Attrib::Distribution::EY])*1E6,
-                                            Attributes::getReal(itsAttr[Attrib::Distribution::ET])*1E6,
-                                            E_m*1E-6,
-                                            massIneV*1E-6,
-                                            CyclotronElement,
-                                            Nint,
-                                            Nsectors,
-                                            Attributes::getReal(itsAttr[Attrib::Distribution::ORDERMAPS]),
-                                            writeMap);
+    std::unique_ptr<SigmaGenerator> siggen = std::unique_ptr<SigmaGenerator>(
+        new SigmaGenerator(I_m,
+                           Attributes::getReal(itsAttr[Attrib::Distribution::EX])*1E6,
+                           Attributes::getReal(itsAttr[Attrib::Distribution::EY])*1E6,
+                           Attributes::getReal(itsAttr[Attrib::Distribution::ET])*1E6,
+                           E_m*1E-6,
+                           massIneV*1E-6,
+                           CyclotronElement,
+                           Nint,
+                           Nsectors,
+                           Attributes::getReal(itsAttr[Attrib::Distribution::ORDERMAPS]),
+                           writeMap));
 
     if (siggen->match(accuracy,
                       Attributes::getReal(itsAttr[Attrib::Distribution::MAXSTEPSSI]),
@@ -1297,7 +1278,7 @@ void Distribution::createMatchedGaussDistribution(size_t numberOfParticles, doub
                       CyclotronElement,
                       denergy,
                       rguess,
-                      false, full))  {
+                      full))  {
 
         std::array<double,3> Emit = siggen->getEmittances();
 
@@ -1311,62 +1292,18 @@ void Distribution::createMatchedGaussDistribution(size_t numberOfParticles, doub
         for (unsigned int i = 0; i < siggen->getSigma().size1(); ++ i) {
             *gmsg << std::setprecision(4)  << std::setw(11) << siggen->getSigma()(i,0);
             for (unsigned int j = 1; j < siggen->getSigma().size2(); ++ j) {
-                if (siggen->getSigma()(i,j) < 10e-12){
+                if (std::abs(siggen->getSigma()(i,j)) < 1.0e-15) {
                     *gmsg << " & " <<  std::setprecision(4)  << std::setw(11) << 0.0;
                 }
                 else{
-                   *gmsg << " & " <<  std::setprecision(4)  << std::setw(11) << siggen->getSigma()(i,j);
+                    *gmsg << " & " <<  std::setprecision(4)  << std::setw(11) << siggen->getSigma()(i,j);
                 }
 
             }
             *gmsg << " \\\\" << endl;
         }
 
-        /*
-
-          Now setup the distribution generator
-          Units of the Sigma Matrix:
-
-          spatial: mm
-          moment:  rad
-
-        */
-        double gamma = E_m / massIneV + 1.0;
-        double beta = std::sqrt(1.0 - 1.0 / (gamma * gamma));
-
-        auto sigma = siggen->getSigma();
-        // change units from mm to m
-        for (unsigned int i = 0; i < 6; ++ i)
-            for (unsigned int j = 0; j < 6; ++ j) sigma(i, j) *= 1e-6;
-
-        for (unsigned int i = 0; i < 3; ++ i) {
-            if ( sigma(2 * i, 2 * i) < 0 || sigma(2 * i + 1, 2 * i + 1) < 0 )
-                throw OpalException("Distribution::CreateMatchedGaussDistribution()",
-                                    "Negative value on the diagonal of the sigma matrix.");
-        }
-
-        sigmaR_m[0] = std::sqrt(sigma(0, 0));
-        sigmaP_m[0] = std::sqrt(sigma(1, 1))*beta*gamma;
-        sigmaR_m[2] = std::sqrt(sigma(2, 2));
-        sigmaP_m[2] = std::sqrt(sigma(3, 3))*beta*gamma;
-        sigmaR_m[1] = std::sqrt(sigma(4, 4));
-
-        //p_l^2 = [(delta+1)*beta*gamma]^2 - px^2 - pz^2
-        double pl2 = (std::sqrt(sigma(5,5)) + 1)*(std::sqrt(sigma(5,5)) + 1)*beta*gamma*beta*gamma -
-                      sigmaP_m[0]*sigmaP_m[0] - sigmaP_m[2]*sigmaP_m[2];
-
-        double pl = std::sqrt(pl2);
-        sigmaP_m[1] = gamma*(pl - beta*gamma);
-
-        correlationMatrix_m(1, 0) = sigma(0, 1) / (std::sqrt(sigma(0, 0) * sigma(1, 1)));
-        correlationMatrix_m(3, 2) = sigma(2, 3) / (std::sqrt(sigma(2, 2) * sigma(3, 3)));
-        correlationMatrix_m(5, 4) = sigma(4, 5) / (std::sqrt(sigma(4, 4) * sigma(5, 5)));
-        correlationMatrix_m(4, 0) = sigma(0, 4) / (std::sqrt(sigma(0, 0) * sigma(4, 4)));
-        correlationMatrix_m(4, 1) = sigma(1, 4) / (std::sqrt(sigma(1, 1) * sigma(4, 4)));
-        correlationMatrix_m(5, 0) = sigma(0, 5) / (std::sqrt(sigma(0, 0) * sigma(5, 5)));
-        correlationMatrix_m(5, 1) = sigma(1, 5) / (std::sqrt(sigma(1, 1) * sigma(5, 5)));
-
-        createDistributionGauss(numberOfParticles, massIneV);
+        generateMatchedGauss(siggen->getSigma(), numberOfParticles, massIneV);
 
         // update injection radius and radial momentum
         CyclotronElement->setRinit(siggen->getInjectionRadius() * 1.0e3);
@@ -1375,13 +1312,9 @@ void Distribution::createMatchedGaussDistribution(size_t numberOfParticles, doub
     else {
         *gmsg << "* Not converged for " << E_m*1E-6 << " MeV" << endl;
 
-        delete siggen;
-
         throw OpalException("Distribution::CreateMatchedGaussDistribution",
                             "didn't find any matched distribution.");
     }
-
-    delete siggen;
 }
 
 void Distribution::createDistributionGauss(size_t numberOfParticles, double massIneV) {
@@ -1480,7 +1413,7 @@ void Distribution::createOpalT(PartBunchBase<double, 3> *beam,
     // This is PC from BEAM
     double deltaP = Attributes::getReal(itsAttr[Attrib::Distribution::OFFSETP]);
     if (inputMoUnits_m == InputMomentumUnitsT::EV) {
-        deltaP = converteVToBetaGamma(deltaP, beam->getM());
+        deltaP = Util::convertMomentumeVToBetaGamma(deltaP, beam->getM());
     }
 
     avrgpz_m = beam->getP()/beam->getM() + deltaP;
@@ -2421,6 +2354,136 @@ void Distribution::generateGaussZ(size_t numberOfParticles) {
     gsl_matrix_free(corMat);
 }
 
+void Distribution::generateMatchedGauss(const SigmaGenerator::matrix_t& sigma,
+                                        size_t numberOfParticles, double massIneV)
+{
+    /* This particle distribution generation is based on a
+     * symplectic method described in
+     * https://arxiv.org/abs/1205.3601
+     */
+
+    /* Units of the Sigma Matrix:
+     *  spatial: m
+     *  moment:  rad
+     *
+     * Attention: The vertical and longitudinal direction must be interchanged!
+     */
+    for (unsigned int i = 0; i < 3; ++ i) {
+        if ( sigma(2 * i, 2 * i) < 0 || sigma(2 * i + 1, 2 * i + 1) < 0 )
+            throw OpalException("Distribution::generateMatchedGauss()",
+                                "Negative value on the diagonal of the sigma matrix.");
+    }
+
+    double bgam = Util::getBetaGamma(E_m, massIneV);
+
+    /*
+     * only used for printing
+     */
+
+    // horizontal
+    sigmaR_m[0] = std::sqrt(sigma(0, 0));
+    sigmaP_m[0] = std::sqrt(sigma(1, 1)) * bgam;
+
+    // longitudinal
+    sigmaR_m[1] = std::sqrt(sigma(4, 4));
+    sigmaP_m[1] = std::sqrt(sigma(5, 5)) * bgam;
+
+    // vertical
+    sigmaR_m[2] = std::sqrt(sigma(2, 2));
+    sigmaP_m[2] = std::sqrt(sigma(3, 3)) * bgam;
+
+    correlationMatrix_m(1, 0) = sigma(0, 1) / (std::sqrt(sigma(0, 0) * sigma(1, 1)));
+    correlationMatrix_m(3, 2) = sigma(2, 3) / (std::sqrt(sigma(2, 2) * sigma(3, 3)));
+    correlationMatrix_m(5, 4) = sigma(4, 5) / (std::sqrt(sigma(4, 4) * sigma(5, 5)));
+    correlationMatrix_m(4, 0) = sigma(0, 4) / (std::sqrt(sigma(0, 0) * sigma(4, 4)));
+    correlationMatrix_m(4, 1) = sigma(1, 4) / (std::sqrt(sigma(1, 1) * sigma(4, 4)));
+    correlationMatrix_m(5, 0) = sigma(0, 5) / (std::sqrt(sigma(0, 0) * sigma(5, 5)));
+    correlationMatrix_m(5, 1) = sigma(1, 5) / (std::sqrt(sigma(1, 1) * sigma(5, 5)));
+
+    inputMoUnits_m = InputMomentumUnitsT::NONE;
+
+    /*
+     * decouple horizontal and longitudinal direction
+     */
+
+    // extract horizontal and longitudinal directions
+    RealDiracMatrix::matrix_t A(4, 4);
+    A(0, 0) = sigma(0, 0);
+    A(1, 1) = sigma(1, 1);
+    A(2, 2) = sigma(4, 4);
+    A(3, 3) = sigma(5, 5);
+
+    A(0, 1) = sigma(0, 1);
+    A(0, 2) = sigma(0, 4);
+    A(0, 3) = sigma(0, 5);
+    A(1, 0) = sigma(1, 0);
+    A(2, 0) = sigma(4, 0);
+    A(3, 0) = sigma(5, 0);
+
+    A(1, 2) = sigma(1, 4);
+    A(2, 1) = sigma(4, 1);
+    A(1, 3) = sigma(1, 5);
+    A(3, 1) = sigma(5, 1);
+    A(2, 3) = sigma(4, 5);
+    A(3, 2) = sigma(5, 4);
+
+
+    RealDiracMatrix rdm;
+    RealDiracMatrix::sparse_matrix_t R1 = rdm.diagonalize(A);
+
+    RealDiracMatrix::vector_t variances(8);
+    for (int i = 0; i < 4; ++i) {
+        variances(i) = std::sqrt(A(i, i));
+    }
+
+    /*
+     * decouple vertical direction
+     */
+    A *= 0.0;
+    A(0, 0) = 1;
+    A(1, 1) = 1;
+    A(2, 2) = sigma(2, 2);
+    A(3, 3) = sigma(3, 3);
+    A(2, 3) = sigma(2, 3);
+    A(3, 2) = sigma(3, 2);
+
+    RealDiracMatrix::sparse_matrix_t R2 = rdm.diagonalize(A);
+
+    for (int i = 0; i < 4; ++i) {
+        variances(4 + i) = std::sqrt(A(i, i));
+    }
+
+    int saveProcessor = -1;
+    const int myNode = Ippl::myNode();
+    const int numNodes = Ippl::getNodes();
+    const bool scalable = Attributes::getBool(itsAttr[Attrib::Distribution::SCALABLE]);
+
+    RealDiracMatrix::vector_t p1(4), p2(4);
+    for (size_t i = 0; i < numberOfParticles; i++) {
+        for (int j = 0; j < 4; j++) {
+            p1(j) = gsl_ran_gaussian(randGen_m, 1.0) * variances(j);
+            p2(j) = gsl_ran_gaussian(randGen_m, 1.0) * variances(4 + j);
+        }
+
+        p1 = boost::numeric::ublas::prod(R1, p1);
+        p2 = boost::numeric::ublas::prod(R2, p2);
+
+        // Save to each processor in turn.
+        saveProcessor++;
+        if (saveProcessor >= numNodes)
+            saveProcessor = 0;
+
+        if (scalable || myNode == saveProcessor) {
+            xDist_m.push_back(p1(0));
+            pxDist_m.push_back(p1(1) * bgam);
+            yDist_m.push_back(p1(2));
+            pyDist_m.push_back(p1(3) * bgam);
+            tOrZDist_m.push_back(p2(2));
+            pzDist_m.push_back(p2(3) * bgam);
+        }
+    }
+}
+
 void Distribution::generateLongFlattopT(size_t numberOfParticles) {
 
     double flattopTime = tPulseLengthFWHM_m
@@ -2980,7 +3043,7 @@ void Distribution::printDistMatchedGauss(Inform &os) const {
     os << "* SIGMAPX    = " << sigmaP_m[0] << " [Beta Gamma]" << endl;
     os << "* SIGMAPY    = " << sigmaP_m[1] << " [Beta Gamma]" << endl;
     os << "* SIGMAPZ    = " << sigmaP_m[2] << " [Beta Gamma]" << endl;
-    os << "* AVRGPZ     = " << avrgpz_m <<    " [Beta Gamma]" << endl;
+//     os << "* AVRGPZ     = " << avrgpz_m <<    " [Beta Gamma]" << endl;
 
     os << "* CORRX      = " << correlationMatrix_m(1, 0) << endl;
     os << "* CORRY      = " << correlationMatrix_m(3, 2) << endl;
@@ -2989,12 +3052,12 @@ void Distribution::printDistMatchedGauss(Inform &os) const {
     os << "* R62        = " << correlationMatrix_m(5, 1) << endl;
     os << "* R51        = " << correlationMatrix_m(4, 0) << endl;
     os << "* R52        = " << correlationMatrix_m(4, 1) << endl;
-    os << "* CUTOFFX    = " << cutoffR_m[0] << " [units of SIGMAX]" << endl;
-    os << "* CUTOFFY    = " << cutoffR_m[1] << " [units of SIGMAY]" << endl;
-    os << "* CUTOFFLONG = " << cutoffR_m[2] << " [units of SIGMAZ]" << endl;
-    os << "* CUTOFFPX   = " << cutoffP_m[0] << " [units of SIGMAPX]" << endl;
-    os << "* CUTOFFPY   = " << cutoffP_m[1] << " [units of SIGMAPY]" << endl;
-    os << "* CUTOFFPZ   = " << cutoffP_m[2] << " [units of SIGMAPY]" << endl;
+//     os << "* CUTOFFX    = " << cutoffR_m[0] << " [units of SIGMAX]" << endl;
+//     os << "* CUTOFFY    = " << cutoffR_m[1] << " [units of SIGMAY]" << endl;
+//     os << "* CUTOFFLONG = " << cutoffR_m[2] << " [units of SIGMAZ]" << endl;
+//     os << "* CUTOFFPX   = " << cutoffP_m[0] << " [units of SIGMAPX]" << endl;
+//     os << "* CUTOFFPY   = " << cutoffP_m[1] << " [units of SIGMAPY]" << endl;
+//     os << "* CUTOFFPZ   = " << cutoffP_m[2] << " [units of SIGMAPY]" << endl;
 }
 
 void Distribution::printDistGauss(Inform &os) const {
@@ -3577,9 +3640,9 @@ void Distribution::setSigmaP_m(double massIneV) {
 
     // Check what input units we are using for momentum.
     if (inputMoUnits_m == InputMomentumUnitsT::EV) {
-        sigmaP_m[0] = converteVToBetaGamma(sigmaP_m[0], massIneV);
-        sigmaP_m[1] = converteVToBetaGamma(sigmaP_m[1], massIneV);
-        sigmaP_m[2] = converteVToBetaGamma(sigmaP_m[2], massIneV);
+        sigmaP_m[0] = Util::convertMomentumeVToBetaGamma(sigmaP_m[0], massIneV);
+        sigmaP_m[1] = Util::convertMomentumeVToBetaGamma(sigmaP_m[1], massIneV);
+        sigmaP_m[2] = Util::convertMomentumeVToBetaGamma(sigmaP_m[2], massIneV);
     }
 }
 
@@ -3912,14 +3975,14 @@ void Distribution::setupEmissionModel(PartBunchBase<double, 3> *beam) {
 void Distribution::setupEmissionModelAstra(PartBunchBase<double, 3> *beam) {
 
     double wThermal = std::abs(Attributes::getReal(itsAttr[Attrib::Distribution::EKIN]));
-    pTotThermal_m = converteVToBetaGamma(wThermal, beam->getM());
+    pTotThermal_m = Util::getBetaGamma(wThermal, beam->getM());
     pmean_m = Vector_t(0.0, 0.0, 0.5 * pTotThermal_m);
 }
 
 void Distribution::setupEmissionModelNone(PartBunchBase<double, 3> *beam) {
 
     double wThermal = std::abs(Attributes::getReal(itsAttr[Attrib::Distribution::EKIN]));
-    pTotThermal_m = converteVToBetaGamma(wThermal, beam->getM());
+    pTotThermal_m = Util::getBetaGamma(wThermal, beam->getM());
     double avgPz = std::accumulate(pzDist_m.begin(), pzDist_m.end(), 0.0);
     size_t numParticles = pzDist_m.size();
     reduce(avgPz, avgPz, OpAddAssign());
@@ -4073,9 +4136,9 @@ void Distribution::shiftDistCoordinates(double massIneV) {
 
         // Check input momentum units.
         if (inputMoUnits_m == InputMomentumUnitsT::EV) {
-            deltaPx = converteVToBetaGamma(deltaPx, massIneV);
-            deltaPy = converteVToBetaGamma(deltaPy, massIneV);
-            deltaPz = converteVToBetaGamma(deltaPz, massIneV);
+            deltaPx = Util::convertMomentumeVToBetaGamma(deltaPx, massIneV);
+            deltaPy = Util::convertMomentumeVToBetaGamma(deltaPy, massIneV);
+            deltaPz = Util::convertMomentumeVToBetaGamma(deltaPz, massIneV);
         }
 
         size_t endIdx = startIdx + particlesPerDist_m[i];
@@ -4356,8 +4419,8 @@ void Distribution::adjustPhaseSpace(double massIneV) {
     double deltaPy = Attributes::getReal(itsAttr[Attrib::Distribution::OFFSETPY]);
     // Check input momentum units.
     if (inputMoUnits_m == InputMomentumUnitsT::EV) {
-        deltaPx = converteVToBetaGamma(deltaPx, massIneV);
-        deltaPy = converteVToBetaGamma(deltaPy, massIneV);
+        deltaPx = Util::convertMomentumeVToBetaGamma(deltaPx, massIneV);
+        deltaPy = Util::convertMomentumeVToBetaGamma(deltaPy, massIneV);
     }
 
     double avrg[6];
diff --git a/src/Distribution/Distribution.h b/src/Distribution/Distribution.h
index 159669467fcb19f09bfd6ec40acee7ad0a76cef0..31817e3c968886ab2d141f20d0e3c6876441a383 100644
--- a/src/Distribution/Distribution.h
+++ b/src/Distribution/Distribution.h
@@ -19,6 +19,8 @@
 #include "AppTypes/SymTenzor.h"
 #include "Attributes/Attributes.h"
 
+#include "Distribution/SigmaGenerator.h"
+
 #include <gsl/gsl_histogram.h>
 #include <gsl/gsl_qrng.h>
 #include <gsl/gsl_rng.h>
@@ -279,7 +281,6 @@ private:
     void eraseTOrZDist();
     void eraseBGzDist();
 
-    //    void printSigma(SigmaGenerator<double,unsigned int>::matrix_type& M, Inform& out);
     void addDistributions();
     void applyEmissionModel(double lowEnergyLimit, double &px, double &py, double &pz, std::vector<double> &additionalRNs);
     void applyEmissModelAstra(double &px, double &py, double &pz, std::vector<double> &additionalRNs);
@@ -291,7 +292,6 @@ private:
     void checkIfEmitted();
     void checkParticleNumber(size_t &numberOfParticles);
     void chooseInputMomentumUnits(InputMomentumUnitsT::InputMomentumUnitsT inputMoUnits);
-    double converteVToBetaGamma(double valueIneV, double massIneV);
     size_t getNumberOfParticlesInFile(std::ifstream &inputFile);
 
     class BinomialBehaviorSplitter {
@@ -337,6 +337,8 @@ private:
     void generateFlattopT(size_t numberOfParticles);
     void generateFlattopZ(size_t numberOfParticles);
     void generateGaussZ(size_t numberOfParticles);
+    void generateMatchedGauss(const SigmaGenerator::matrix_t&,
+                              size_t numberOfParticles, double massIneV);
     void generateLongFlattopT(size_t numberOfParticles);
     void generateTransverseGauss(size_t numberOfParticles);
     void initializeBeam(PartBunchBase<double, 3> *beam);
diff --git a/src/Distribution/Harmonics.h b/src/Distribution/Harmonics.h
deleted file mode 100644
index dbd7356ec7d8c85918aff78d1daece910b4bff10..0000000000000000000000000000000000000000
--- a/src/Distribution/Harmonics.h
+++ /dev/null
@@ -1,553 +0,0 @@
-//
-// Class Harmonics
-//   This class computes the cyclotron map based on harmonics.
-//   All functions are copied and translated to C++ from the original program inj2_ana.c of Dr. C. Baumgarten.
-//
-// Copyright (c) 2014 - 2015, Christian Baumgarten, Paul Scherrer Institut, Villigen PSI, Switzerland
-//                            Matthias Frey, ETH Zürich
-// All rights reserved
-//
-// This file is part of OPAL.
-//
-// OPAL 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 3 of the License, or
-// (at your option) any later version.
-//
-// You should have received a copy of the GNU General Public License
-// along with OPAL. If not, see <https://www.gnu.org/licenses/>.
-//
-#ifndef HARMONICS_H
-#define HARMONICS_H
-
-#include <cmath>
-#include <fstream>
-#include <string>
-#include <utility>
-#include <vector>
-
-#include "Physics/Physics.h"
-
-#include <boost/numeric/ublas/matrix.hpp>
-
-#include "matrix_vector_operation.h"
-
-template<typename Value_type, typename Size_type>
-class Harmonics
-{
-public:
-    /// Type of variable
-    typedef Value_type value_type;
-    /// Type of size
-    typedef Size_type size_type;
-    /// Type for specifying matrices
-    typedef boost::numeric::ublas::matrix<value_type> matrix_type;
-
-
-    /// Defines starting point of computation
-    enum START { SECTOR_ENTRANCE, SECTOR_CENTER, SECTOR_EXIT, VALLEY_CENTER };
-
-    /// Constructor
-    /*!
-     * @param wo is the nominal orbital frequency in Hz
-     * @param Emin is the minimal energy in MeV
-     * @param Emax is the maximal energy in MeV
-     * @param nth is the number of angle steps
-     * @param nr is the number of radial steps
-     * @param nSector is the number of sectors
-     * @param E is the kinetic energy [MeV]
-     * @param E0 is the potential energy [MeV]
-     */
-    Harmonics(value_type, value_type, value_type, size_type, size_type, size_type, value_type, value_type);
-
-    /// Compute all maps of the cyclotron using harmonics
-    /*!
-     * @param filename1 name of file 1
-     * @param filename2 name of file 2
-     * @param startmod (in center of valley, start of sector, ...)
-     */
-    std::vector<matrix_type> computeMap(const std::string, const std::string, const int);
-
-    /// Returns the radial and vertical tunes
-    std::pair<value_type, value_type> getTunes();
-
-    /// Returns the radius
-    value_type getRadius();
-
-    /// Returns step size
-    value_type getPathLength();
-
-private:
-
-    /// Stores the nominal orbital frequency in Hz
-    value_type wo_m;
-    /// Stores the minimum energy in MeV
-    value_type Emin_m;
-    /// Stores the maximum energy in MeV
-    value_type Emax_m;
-    /// Stores the number of angle splits
-    size_type nth_m;
-    /// Stores the number of radial splits
-    size_type nr_m;
-    /// Stores the number of sectors
-    size_type nSector_m;
-    /// Stores the tunes (radial, vertical)
-    std::pair<value_type, value_type> tunes_m;
-    /// Stores the radius
-    value_type radius_m;
-    /// Stores the path length
-    value_type ds_m;
-    /// Stores the energy for which we perform the computation
-    value_type E_m;
-    /// Stores the potential energy [MeV]
-    value_type E0_m;
-
-    /// Compute some matrix (ask Dr. C. Baumgarten)
-    matrix_type __Mix6(value_type, value_type, value_type);
-    /// Compute some matrix  (ask Dr. C. Baumgarten)
-    matrix_type __Md6(value_type, value_type);
-    /// Compute some matrix  (ask Dr. C. Baumgarten)
-    matrix_type __Mb6(value_type, value_type, value_type);
-    /// Compute some matrix  (ask Dr. C. Baumgarten)
-    matrix_type __Mb6k(value_type, value_type, value_type, value_type);
-};
-
-// -----------------------------------------------------------------------------------------------------------------------
-// PUBLIC MEMBER FUNCTIONS
-// -----------------------------------------------------------------------------------------------------------------------
-
-template<typename Value_type, typename Size_type>
-Harmonics<Value_type, Size_type>::Harmonics(value_type wo, value_type Emin, value_type Emax,
-    size_type nth, size_type nr, size_type nSector, value_type E, value_type E0)
-    : wo_m(wo), Emin_m(Emin), Emax_m(Emax), nth_m(nth), nr_m(nr), nSector_m(nSector), E_m(E), E0_m(E0)
-{}
-
-template<typename Value_type, typename Size_type>
-std::vector<typename Harmonics<Value_type, Size_type>::matrix_type> Harmonics<Value_type, Size_type>::computeMap(const std::string filename1, const std::string filename2, const int startmod) {
-    // i --> different energies
-    // j --> different angles
-
-    // ---------------
-
-
-//     unsigned int nth_m = 360; //270;
-    unsigned int N   = 4;
-    value_type two_pi    = 2.0 * M_PI;
-    value_type tpiN      = two_pi / value_type(N);
-    value_type piN       = M_PI / value_type(N);
-//     unsigned int nr_m  = 180;
-    unsigned int cnt = 0;
-    bool set = false;
-
-    value_type gap = 0.05;
-    value_type K1  = 0.45;
-
-    std::vector<matrix_type> Mcyc(nth_m);
-
-    value_type beta_m;
-
-    std::vector<value_type> gamma(nr_m), E(nr_m), PC(nr_m), nur(nr_m), nuz(nr_m);
-
-    std::vector<value_type> R(nr_m), r(nr_m);
-    std::vector<value_type> Bmag(nr_m), k(nr_m);
-    std::vector<value_type> alpha(nr_m), beta(nr_m);
-
-    //----------------------------------------------------------------------
-    // read files
-    std::ifstream infile2(filename2);
-    std::ifstream infile1(filename1);
-
-    if (!infile1.is_open() || !infile2.is_open()) {
-        std::cerr << "Couldn't open files!" << std::endl;
-        std::exit(0);
-    }
-
-    unsigned int n = 0;
-    value_type rN1, cN1, sN1, aN1, phN1, rN2, cN2, sN2, aN2, phN2;
-
-    while (infile1 >> rN1 >> cN1 >> sN1 >> aN1 >> phN1 &&
-           infile2 >> rN2 >> cN2 >> sN2 >> aN2 >> phN2) {
-
-        R[n]      = rN1 * 0.01;
-        alpha[n]  = 2.0 * std::acos(aN2 / aN1) / value_type(N);
-        beta[n]   = std::atan(sN1 / cN1) / value_type(N);
-        value_type f4 = aN1 * M_PI * 0.5 / std::sin(0.5 * alpha[n] * value_type(N));
-        value_type f8 = aN2 * M_PI / std::sin(alpha[n] * value_type(N));
-
-        Bmag[n] = 0.5 * (f4 + f8);
-
-        n++;
-    }
-
-    infile1.close();
-    infile2.close();
-    //----------------------------------------------------------------------
-
-    value_type s = std::sin(piN);
-    value_type c = std::cos(piN);
-
-    std::vector<value_type> dalp(nr_m), dbet(nr_m), len2(nr_m), len(nr_m), t1eff(nr_m), t2eff(nr_m);
-    std::vector<value_type> t1(nr_m), t2(nr_m), t3(nr_m);
-
-    dalp[0]   = (alpha[1]   - alpha[0])   / (R[1] - R[0]);
-    dalp[n-1] = (alpha[n-1] - alpha[n-2]) / (R[n-1] - R[n-2]);
-    dbet[0]   = (beta[1]    - alpha[0])   / (R[1] - R[0]);
-    dbet[n-1] = (beta[n-1]  - beta[n-2])  / (R[n-1] - R[n-2]);
-
-    for (unsigned int i = 1; i < n - 1; ++i) {
-        dalp[i] = R[i] * (alpha[i+1] - alpha[i-1]) / (R[i+1] - R[i-1]);
-        dbet[i] = R[i] * (beta[i+1]  - beta[i-1])  / (R[i+1] - R[i-1]);
-    }
-
-    for (unsigned int i = 0; i < n; ++i) {
-        value_type cot = 1.0 / std::tan(0.5 * alpha[i]);
-
-        value_type eps1 = std::atan(dbet[i] - 0.5 * dalp[i]);
-        value_type eps2 = std::atan(dbet[i] + 0.5 * dalp[i]);
-
-        value_type phi = piN - 0.5 * alpha[i];
-
-        // bending radius
-        r[i]    = R[i] * sin(0.5 * alpha[i]) / s;
-        len2[i] = R[i] * std::sin(phi);
-        len[i]  = two_pi * r[i] + 2.0 * len2[i] * value_type(N);
-
-        value_type g1 =   phi + eps1;
-        value_type g2 = - phi + eps2;
-
-        t1[i] = std::tan(g1);
-        t2[i] = std::tan(g2);
-        t3[i] = std::tan(phi);
-
-        value_type fac = gap / r[i] * K1;
-        value_type psi = fac * (1.0 + std::sin(g1) * std::sin(g1)) / std::cos(g1);
-
-        t1eff[i] = std::tan(g1 - psi);
-
-        psi = fac * (1.0 + std::sin(g2) * std::sin(g2)) / std::cos(g2);
-        t2eff[i] = std::tan(g2 + psi);
-
-        beta_m     = wo_m / Physics::c * len[i] / two_pi;
-        gamma[i] = 1.0 / std::sqrt(1.0 - beta_m * beta_m);
-        E[i]     = E0_m * (gamma[i] - 1.0);
-        PC[i]    = E0_m * gamma[i] * beta_m;
-        Bmag[i]  = E0_m * 1.0e6 / Physics::c * beta_m * gamma[i] / r[i] * 10.0;
-
-        if (!set && E[i] >= E_m) {
-            set = true;
-            cnt = i;
-        }
-    }
-
-    // (average) field gradient
-    for (unsigned int i = 0; i < n; ++i) {
-        value_type dBdr;
-        if (i == 0)
-            dBdr = (Bmag[1] - Bmag[0]) / (R[1] - R[0]);
-        else if (i == n - 1)
-            dBdr = (Bmag[n-1] - Bmag[n-2]) / (R[n-1] - R[n-2]);
-        else
-            dBdr = (Bmag[i+1] - Bmag[i-1]) / (R[i+1] - R[i-1]);
-
-        value_type bb  = std::sin(piN - 0.5 * alpha[i]) / std::sin(0.5 * alpha[i]);
-        value_type eps = 2.0 * bb / (1.0 + bb * bb);
-        value_type BaV = Bmag[i];
-
-        k[i] = r[i] * r[i] / (R[i] * BaV) * dBdr * (1.0 + bb * value_type(N) / M_PI * s);
-    }
-
-    for (unsigned int i = 0; i < n; ++i) {
-        if ((k[1] > -0.999) && (E[i] > Emin_m) && (E[i] < Emax_m + 1.0)) {
-            value_type kx = std::sqrt(1.0 + k[i]);
-            value_type Cx = std::cos(tpiN * kx);
-            value_type Sx = std::sin(tpiN * kx);
-            value_type pm, ky, Cy, Sy;
-
-            if (k[i] >= 0.0) {
-                pm = 1.0;
-                ky = std::sqrt(std::abs(k[i]));
-                Cy = std::cosh(tpiN * ky);
-                Sy = std::sinh(tpiN * ky);
-            } else {
-                pm = -1.0;
-                ky = std::sqrt(std::abs(k[i]));
-                Cy = std::cos(tpiN * ky);
-                Sy = std::sin(tpiN * ky);
-            }
-
-            value_type tav  = 0.5 * (t1[i] - t2[i]);
-            value_type tmul = t1[i] * t2[i];
-            value_type lrho = 2.0 * len2[i] / r[i];
-
-            nur[i] = std::acos(Cx * (1.0 + lrho * tav) + Sx / kx * (tav - lrho * 0.5 * (kx * kx + tmul))) / tpiN;
-
-            tav  = 0.5 * (t1eff[i] - t2eff[i]);
-            tmul = t1eff[i] * t2eff[i];
-
-            nuz[i] = std::acos(Cy * (1.0 - lrho * tav) + Sy / ky * (0.5 * lrho * (pm * ky * ky - tmul) - tav)) / tpiN;
-
-//             std::cout << E[i] << " " << R[i] << " " << nur[i] << " " << nuz[i] << std::endl;
-        }
-    }
-    // -------------------------------------------------------------------------
-
-    int i = cnt;
-
-    tunes_m = { nur[i], nuz[i] };
-    radius_m = R[i];
-
-//     for (int i = 9; i < 10; ++i) {       // n = 1 --> only for one energy
-        value_type smax, slim, ss, gam2;
-
-        smax = len[i] / value_type(N);
-        slim = r[i] * tpiN;
-        ds_m   = smax / value_type(nth_m);
-        ss = 0.0;
-        gam2 = gamma[i] * gamma[i];
-
-        matrix_type Mi = __Mix6(  t1[i],   t1eff[i], r[i]);
-        matrix_type Mx = __Mix6(- t2[i], - t2eff[i], r[i]);
-        matrix_type Md = __Md6(ds_m, gam2);
-        matrix_type Mb = __Mb6k(ds_m / r[i], r[i], k[i], gam2);
-
-        unsigned int j = 0;
-
-        matrix_type M0 = boost::numeric::ublas::zero_matrix<value_type>(6);
-        matrix_type M1 = boost::numeric::ublas::zero_matrix<value_type>(6);
-
-        switch (startmod) {
-            case 0: // SECTOR_ENTRANCE
-                ss = slim - ds_m;
-                Mcyc[j++] = boost::numeric::ublas::prod(Mb, Mi);
-
-                while (ss > ds_m) {
-                    Mcyc[j++] = Mb;
-                    ss -= ds_m;
-                }
-
-                M0 = __Mb6k(ss / r[i], r[i], k[i], gam2);
-                M1 = __Md6(ds_m - ss, gam2);
-
-                Mcyc[j++] = matt_boost::gemmm<matrix_type>(M1, Mx, M0);
-
-                ss = smax - slim - (ds_m - ss);
-
-                while ((ss > ds_m) && (j < nth_m)) {
-                    Mcyc[j++] = Md;
-                    ss -= ds_m;
-                }
-
-                if (j < nth_m)
-                    Mcyc[j++] = __Md6(ss, gam2);
-
-                break;
-
-            case 1: // SECTOR_CENTER
-                j  = 0;
-                ss = slim * 0.5;
-
-                while (ss > ds_m) {
-                    Mcyc[j++] = Mb;
-                    ss -= ds_m;
-                }
-
-                M0 = __Mb6k(ss / r[i], r[i], k[i], gam2);
-                M1 = __Md6(ds_m - ss, gam2);
-
-                Mcyc[j++] = matt_boost::gemmm<matrix_type>(M1, Mx, M0);
-
-                ss = smax - slim - (ds_m - ss);
-
-                while (ss > ds_m) {
-                    Mcyc[j++] = Md;
-                    ss -= ds_m;
-                }
-
-                M0 = __Md6(ss, gam2);
-                M1 = __Mb6k((ds_m - ss) / r[i], r[i], k[i], gam2);
-
-                Mcyc[j++] = matt_boost::gemmm<matrix_type>(M1, Mi, M0);
-
-                ss = slim * 0.5 - (ds_m - ss);
-
-                while ((ss > ds_m) && (j < nth_m)) {
-                    Mcyc[j++] = Mb;
-                    ss -= ds_m;
-                }
-
-                if (j < nth_m)
-                    Mcyc[j++] = __Mb6k(ss / r[i], r[i], k[i], gam2);
-
-                break;
-
-            case 2: // SECTOR_EXIT
-                j = 0;
-
-                Mcyc[j++] = boost::numeric::ublas::prod(Md,Mx);
-
-                ss = smax - slim - ds_m;
-
-                while (ss > ds_m) {
-                    Mcyc[j++] = Md;
-                    ss -= ds_m;
-                }
-
-                M0 = __Md6(ss, gam2);
-                M1 = __Mb6k((ds_m - ss) / r[i], r[i], k[i], gam2);
-
-                Mcyc[j++] = matt_boost::gemmm<matrix_type>(M1, Mi, M0);
-
-                ss = slim - (ds_m - ss);
-
-                while ((ss > ds_m) && (j < nth_m)) {
-                    Mcyc[j++] = Mb;
-                    ss -= ds_m;
-                }
-
-                if (j < nth_m)
-                    Mcyc[j++] = __Mb6k(ss / r[i], r[i], k[i], gam2);
-
-                break;
-
-            case 3: // VALLEY_CENTER
-            default:
-                j = 0;
-                ss = (smax - slim) * 0.5;
-
-                while (ss > ds_m) {
-                    Mcyc[j++] = Md;
-                    ss -= ds_m;
-                }
-
-                M0 = __Md6(ss, gam2);
-                M1 = __Mb6k((ds_m - ss) / r[i], r[i], k[i], gam2);
-
-                Mcyc[j++] = matt_boost::gemmm<matrix_type>(M1, Mi, M0);
-
-                ss = slim - (ds_m - ss);
-                while (ss > ds_m) {
-                    Mcyc[j++] = Mb;
-                    ss -= ds_m;
-                }
-
-                M0 = __Mb6k(ss / r[i], r[i], k[i], gam2);
-                M1 = __Md6(ds_m - ss, gam2);
-
-                Mcyc[j++] = matt_boost::gemmm<matrix_type>(M1, Mx, M0);
-
-                ss = (smax - slim) * 0.5 - (ds_m - ss);
-                while ((ss > ds_m) && (j < nth_m)) {
-                    Mcyc[j++] = Md;
-                    ss -= ds_m;
-                }
-
-                if (j < nth_m)
-                    Mcyc[j++] = __Md6(ss, gam2);
-
-                break;
-        } // end of switch
-//     } // end of for
-
-    return std::vector<matrix_type>(Mcyc);
-}
-
-template<typename Value_type, typename Size_type>
-inline std::pair<Value_type, Value_type> Harmonics<Value_type, Size_type>::getTunes() {
-    return tunes_m;
-}
-
-template<typename Value_type, typename Size_type>
-inline typename Harmonics<Value_type, Size_type>::value_type Harmonics<Value_type, Size_type>::getRadius() {
-    return radius_m;
-}
-
-template<typename Value_type, typename Size_type>
-inline typename Harmonics<Value_type, Size_type>::value_type Harmonics<Value_type, Size_type>::getPathLength() {
-    return ds_m;
-}
-
-
-// -----------------------------------------------------------------------------------------------------------------------
-// PRIVATE MEMBER FUNCTIONS
-// -----------------------------------------------------------------------------------------------------------------------
-
-template<typename Value_type, typename Size_type>
-typename Harmonics<Value_type, Size_type>::matrix_type Harmonics<Value_type, Size_type>::__Mix6(value_type tx, value_type ty, value_type r) {
-    matrix_type M = boost::numeric::ublas::identity_matrix<value_type>(6);
-
-    M(1,0) =   tx / r;
-    M(3,2) = - ty / r;
-
-    return M;
-}
-
-template<typename Value_type, typename Size_type>
-typename Harmonics<Value_type, Size_type>::matrix_type Harmonics<Value_type, Size_type>::__Md6(value_type l, value_type gam2) {
-    matrix_type M = boost::numeric::ublas::identity_matrix<value_type>(6);
-
-    M(0,1) = l;
-    M(2,3) = l;
-    M(4,5) = l / gam2;
-
-    return M;
-}
-
-template<typename Value_type, typename Size_type>
-typename Harmonics<Value_type, Size_type>::matrix_type Harmonics<Value_type, Size_type>::__Mb6(value_type phi, value_type r, value_type gam2) {
-    matrix_type M = boost::numeric::ublas::identity_matrix<value_type>(6);
-
-    value_type C = std::cos(phi);
-    value_type S = std::sin(phi);
-    value_type l =r * phi;
-
-    M(0,0) = M(1,1) = C;
-    M(0,1) =   S * r;
-    M(1,0) = - S / r;
-    M(2,3) = l;
-    M(4,5) = l / gam2 - l + r * S;
-    M(4,0) = - (M(1,5) = S);
-    M(4,1) = - (M(0,5) = r * (1.0 - C));
-
-    return M;
-}
-
-template<typename Value_type, typename Size_type>
-typename Harmonics<Value_type, Size_type>::matrix_type Harmonics<Value_type, Size_type>::__Mb6k(value_type phi, value_type r, value_type k, value_type gam2) {
-    matrix_type M = boost::numeric::ublas::identity_matrix<value_type>(6);
-
-    value_type C,S;
-
-    if (k == 0.0) return __Mb6(phi,r,gam2);
-
-    value_type fx = std::sqrt(1.0 + k);
-    value_type fy = std::sqrt(std::abs(k));
-    value_type c  = std::cos(phi * fx);
-    value_type s  = std::sin(phi * fx);
-    value_type l  = r * phi;
-
-
-    if (k > 0.0) {
-        C = std::cosh(phi * fy);
-        S = std::sinh(phi * fy);
-    } else {
-        C = std::cos(phi * fy);
-        S = std::sin(phi * fy);
-    }
-
-    M(0,0) = M(1,1) = c;
-    M(0,1) =   s * r / fx;
-    M(1,0) = - s * fx / r;
-    M(2,2) = M(3,3) = C;
-    M(2,3) = S * r / fy;
-
-    value_type sign = (std::signbit(k)) ? value_type(-1) : value_type(1);
-
-    M(3,2) = sign * S * fy / r;
-    M(4,5) = l / gam2 - r / (1.0 + k) * (phi - s / fx);
-    M(4,0)= - (M(1,5) = s / fx);
-    M(4,1)= - (M(0,5) = r * (1.0 - c) / (1.0 + k));
-
-    return M;
-}
-
-#endif
\ No newline at end of file
diff --git a/src/Distribution/RealDiracMatrix.cpp b/src/Distribution/RealDiracMatrix.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c90559a392bfdf16a6c2126ab494e1c1b706be6d
--- /dev/null
+++ b/src/Distribution/RealDiracMatrix.cpp
@@ -0,0 +1,281 @@
+//
+// Class: RealDiracMatrix
+//   Real Dirac matrix class.
+//   They're ordered after the paper of Dr. C. Baumgarten: "Use of real Dirac matrices in two-dimensional coupled linear optics".
+//   The diagonalizing method is based on the paper "Geometrical method of decoupling" (2012) of Dr. C. Baumgarten.
+//
+// Copyright (c) 2014, 2020 Christian Baumgarten, Paul Scherrer Institut, Villigen PSI, Switzerland
+//                          Matthias Frey, ETH Zürich
+// All rights reserved
+//
+// Implemented as part of the Semester thesis by Matthias Frey
+// "Matched Distributions in Cyclotrons"
+//
+// This file is part of OPAL.
+//
+// OPAL 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 3 of the License, or
+// (at your option) any later version.
+//
+// You should have received a copy of the GNU General Public License
+// along with OPAL. If not, see <https://www.gnu.org/licenses/>.
+//
+#include "RealDiracMatrix.h"
+
+#include "Utilities/OpalException.h"
+
+#include <cmath>
+#include <string>
+#include "matrix_vector_operation.h"
+
+RealDiracMatrix::RealDiracMatrix() {};
+
+typename RealDiracMatrix::sparse_matrix_t
+RealDiracMatrix::getRDM(short i) {
+    sparse_matrix_t rdm(4,4,4); // #nrows, #ncols, #non-zeros
+    switch(i) {
+        case 0:  rdm(0,1) = rdm(2,3) = 1; rdm(1,0) = rdm(3,2) = -1; break;
+        case 1:  rdm(0,1) = rdm(1,0) = -1; rdm(2,3) = rdm(3,2) = 1; break;
+        case 2:  rdm(0,3) = rdm(1,2) = rdm(2,1) = rdm(3,0) = 1;     break;
+        case 3:  rdm(0,0) = rdm(2,2) = -1; rdm(1,1) = rdm(3,3) = 1; break;
+        case 4:  rdm(0,0) = rdm(3,3) = -1; rdm(1,1) = rdm(2,2) = 1; break;
+        case 5:  rdm(0,2) = rdm(2,0) = 1; rdm(1,3) = rdm(3,1) = -1; break;
+        case 6:  rdm(0,1) = rdm(1,0) = rdm(2,3) = rdm(3,2) = 1;     break;
+        case 7:  rdm(0,3) = rdm(2,1) = 1; rdm(1,2) = rdm(3,0) = -1; break;
+        case 8:  rdm(0,1) = rdm(3,2) = 1; rdm(1,0) = rdm(2,3) = -1; break;
+        case 9:  rdm(0,2) = rdm(1,3) = -1; rdm(2,0) = rdm(3,1) = 1; break;
+        case 10: rdm(0,2) = rdm(3,1) = 1; rdm(1,3) = rdm(2,0) = -1; break;
+        case 11: rdm(0,2) = rdm(1,3) = rdm(2,0) = rdm(3,1) = -1;    break;
+        case 12: rdm(0,0) = rdm(1,1) = -1; rdm(2,2) = rdm(3,3) = 1; break;
+        case 13: rdm(0,3) = rdm(3,0) = -1; rdm(1,2) = rdm(2,1) = 1; break;
+        case 14: rdm(0,3) = rdm(1,2) = -1; rdm(2,1) = rdm(3,0) = 1; break;
+        case 15: rdm(0,0) = rdm(1,1) = rdm(2,2) = rdm(3,3) = 1;     break;
+        default: throw OpalException("RealDiracMatrix::getRDM()",
+                                     "Index (i = " + std::to_string(i)
+                                     + " out of range: 0 <= i <= 15"); break;
+    }
+    return rdm;
+}
+
+
+void RealDiracMatrix::diagonalize(matrix_t& Ms, sparse_matrix_t& R, sparse_matrix_t& invR) {
+
+    // R and invR store the total transformation
+    R = boost::numeric::ublas::identity_matrix<double>(4);
+    invR = R;
+
+    vector_t P(3), E(3), B(3), b;
+    double mr, mg, mb, eps;
+
+    // Lambda function to compute vectors E, P, B and scalar eps (it takes the current Ms as reference argument (--> [&])
+    auto mult = [&](short i) {
+        /*
+         * For computing E, P, B, eps according to formula (C4) from paper:
+         * Geometrical method of decoupling
+         */
+        matrix_t tmp = boost::numeric::ublas::prod(Ms,getRDM(i))+boost::numeric::ublas::prod(getRDM(i),Ms);
+        return 0.125*matt_boost::trace(tmp);
+    };
+
+    // 1. Transformation with \gamma_{0}
+    P = vector_t({ mult(1),  mult(2),  mult(3)});
+    E = vector_t({ mult(4),  mult(5),  mult(6)});
+    B = vector_t({-mult(7), -mult(8), -mult(9)});
+    mr = boost::numeric::ublas::inner_prod(E, B);        // formula (31), paper: Geometrical method of decoupling
+    mg = boost::numeric::ublas::inner_prod(B, P);        // formula (31), paper: Geometrical method of decoupling
+
+    transform(Ms, 0, 0.5 * std::atan2(mg,mr), R, invR);
+
+    // 2. Transformation with \gamma_{7}
+    eps =  -mult(0);
+    P   = vector_t({ mult(1),  mult(2),  mult(3)});
+    E   = vector_t({ mult(4),  mult(5),  mult(6)});
+    B   = vector_t({-mult(7), -mult(8), -mult(9)});
+    b   = eps * B + matt_boost::cross_prod(E, P);      // formula (32), paper: Geometrical method of decoupling
+
+    transform(Ms, 7, 0.5 * std::atan2(b(2), b(1)), R, invR);
+
+    // 3. Transformation with \gamma_{9}
+    eps =  -mult(0);
+    P   = vector_t({ mult(1),  mult(2),  mult(3)});
+    E   = vector_t({ mult(4),  mult(5),  mult(6)});
+    B   = vector_t({-mult(7), -mult(8), -mult(9)});
+    b   = eps * B + matt_boost::cross_prod(E, P);
+
+    transform(Ms, 9, -0.5 * std::atan2(b(0), b(1)), R, invR);
+
+    // 4. Transformation with \gamma_{2}
+    eps =  -mult(0);
+    P   = vector_t({ mult(1),  mult(2),  mult(3)});
+    E   = vector_t({ mult(4),  mult(5),  mult(6)});
+    B   = vector_t({-mult(7), -mult(8), -mult(9)});
+    mr  = boost::numeric::ublas::inner_prod(E, B);
+    b   = eps * B + matt_boost::cross_prod(E, P);
+
+    // Transformation distinction made according to function "rdm_Decouple_F"
+    // in rdm.c of Dr. Christian Baumgarten
+
+    if (std::fabs(mr) < std::fabs(b(1))) {
+        transform(Ms, 2, 0.5 * std::atanh(mr / b(1)), R, invR);
+    } else {
+        transform(Ms, 2, 0.5 * std::atanh(b(1) / mr), R, invR);
+    }
+
+    eps =  -mult(0);
+    P   = vector_t({ mult(1),  mult(2),  mult(3)});
+    E   = vector_t({ mult(4),  mult(5),  mult(6)});
+    B   = vector_t({-mult(7), -mult(8), -mult(9)});
+
+    // formula (31), paper: Geometrical method of decoupling
+    mr = boost::numeric::ublas::inner_prod(E, B);
+    mg = boost::numeric::ublas::inner_prod(B, P);
+    mb = boost::numeric::ublas::inner_prod(E, P);
+
+    double P2 = boost::numeric::ublas::inner_prod(P, P);
+    double E2 = boost::numeric::ublas::inner_prod(E, E);
+
+    // 5. Transform with \gamma_{0}
+    transform(Ms, 0, 0.25 * std::atan2(mb,0.5 * (E2 - P2)), R, invR);
+
+    // 6. Transformation with \gamma_{8}
+    P(0) = mult(1); P(2) = mult(3);
+
+    transform(Ms, 8, -0.5 * std::atan2(P(2), P(0)), R, invR);
+}
+
+
+RealDiracMatrix::sparse_matrix_t
+RealDiracMatrix::diagonalize(matrix_t& sigma) {
+    matrix_t S = boost::numeric::ublas::prod(sigma, getRDM(0));
+
+    sparse_matrix_t R     = boost::numeric::ublas::identity_matrix<double>(4);
+    sparse_matrix_t invR  = boost::numeric::ublas::identity_matrix<double>(4);
+    sparse_matrix_t iRtot = boost::numeric::ublas::identity_matrix<double>(4);
+
+    for (int i = 0; i < 6; ++i) {
+        update(sigma, i, R, invR);
+
+        iRtot = boost::numeric::ublas::prod(iRtot, invR);
+        S = matt_boost::gemmm<matrix_t>(R, S, invR);
+        sigma = - boost::numeric::ublas::prod(S, getRDM(0));
+    }
+
+    return iRtot;
+}
+
+
+typename RealDiracMatrix::matrix_t
+RealDiracMatrix::symplex(const matrix_t& M) {
+    /*
+     * formula(16), p. 3
+     */
+    sparse_matrix_t rdm0 = getRDM(0);
+    return 0.5 * (M + matt_boost::gemmm<matrix_t>(rdm0,boost::numeric::ublas::trans(M),rdm0));
+}
+
+
+void RealDiracMatrix::transform(matrix_t& M, short i, double phi,
+                                sparse_matrix_t& Rtot, sparse_matrix_t& invRtot)
+{
+    if (phi) {  // if phi == 0 --> nothing happens, since R and invR would be identity_matrix matrix
+        sparse_matrix_t R(4,4), invR(4,4);
+
+        transform(i, phi, R, invR);
+
+        // update matrices
+        M = matt_boost::gemmm<matrix_t>(R,M,invR);
+        Rtot = boost::numeric::ublas::prod(R,Rtot);
+        invRtot = boost::numeric::ublas::prod(invRtot,invR);
+    }
+}
+
+
+void RealDiracMatrix::transform(short i, double phi,
+                                sparse_matrix_t& R, sparse_matrix_t& invR)
+{
+    if (phi) {  // if phi == 0 --> nothing happens, since R and invR would be identity_matrix matrix
+        sparse_matrix_t I = boost::numeric::ublas::identity_matrix<double>(4);
+
+        if ((i < 7 && i != 0) || (i > 10 && i != 14)) {
+            R = I * std::cosh(phi) + getRDM(i) * std::sinh(phi);
+            invR = I * std::cosh(phi) - getRDM(i) * std::sinh(phi);
+        } else {
+            R = I * std::cos(phi) + getRDM(i) * std::sin(phi);
+            invR = I * std::cos(phi) - getRDM(i) * std::sin(phi);
+        }
+    }
+}
+
+
+void RealDiracMatrix::update(matrix_t& sigma, short i, sparse_matrix_t& R,
+                             sparse_matrix_t& invR)
+{
+    double s0 =  0.25 * ( sigma(0, 0) + sigma(1, 1) + sigma(2, 2) + sigma(3, 3) );
+    double s1 =  0.25 * (-sigma(0, 0) + sigma(1, 1) + sigma(2, 2) - sigma(3, 3) );
+    double s2 =  0.5  * ( sigma(0, 2) - sigma(1, 3) );
+    double s3 =  0.5  * ( sigma(0, 1) + sigma(2, 3) );
+    double s4 =  0.5  * ( sigma(0, 1) - sigma(2, 3) );
+    double s5 = -0.5  * ( sigma(0, 3) + sigma(1, 2) );
+    double s6 =  0.25 * ( sigma(0, 0) - sigma(1, 1) + sigma(2, 2) - sigma(3, 3) );
+    double s7 =  0.5  * ( sigma(0, 2) + sigma(1, 3) );
+    double s8 =  0.25 * ( sigma(0, 0) + sigma(1, 1) - sigma(2, 2) - sigma(3, 3) );
+    double s9 =  0.5  * ( sigma(0, 3) - sigma(1, 2) );
+
+    vector_t P(3); P(0) = s1; P(1) = s2; P(2) = s3;
+    vector_t E(3); E(0) = s4; E(1) = s5; E(2) = s6;
+    vector_t B(3); B(0) = s7; B(1) = s8; B(2) = s9;
+
+    double mr = boost::numeric::ublas::inner_prod(E, B);
+    double mg = boost::numeric::ublas::inner_prod(B, P);
+    double mb = boost::numeric::ublas::inner_prod(E, P);
+
+    vector_t b = -s0 * B + matt_boost::cross_prod(E, P);
+
+    switch (i) {
+        case 0:
+        {
+            double eps = std::atan2(mg, mr);
+            transform(0, 0.5 * eps, R, invR);
+            break;
+        }
+        case 1:
+        {
+            double eps = std::atan2(b(2), b(1));
+            transform(7, 0.5 * eps, R, invR);
+            break;
+        }
+        case 2:
+        {
+            double eps = - std::atan2(b(0), b(1));
+            transform(9, 0.5 * eps, R, invR);
+            break;
+        }
+        case 3:
+        {
+            double eps = - std::atanh(mr / b(1));
+            transform(2, 0.5 * eps, R, invR);
+            break;
+        }
+        case 4:
+        {
+            double eps = 0.5 * std::atan2(2.0 * mb,
+                                          boost::numeric::ublas::inner_prod(E, E) -
+                                          boost::numeric::ublas::inner_prod(P, P));
+            transform(0, 0.5 * eps, R, invR);
+            break;
+        }
+        case 5:
+        {
+            double eps = - std::atan2(P(2), P(0));
+            transform(8, 0.5 * eps, R, invR);
+            break;
+        }
+        default:
+        {
+            throw OpalException("RealDiracMatrix::update()",
+                                "Case " + std::to_string(i) +
+                                " not available.");
+        }
+    }
+}
\ No newline at end of file
diff --git a/src/Distribution/RealDiracMatrix.h b/src/Distribution/RealDiracMatrix.h
new file mode 100644
index 0000000000000000000000000000000000000000..ae6fa40ca86f4054377c8961078d9df63060eac9
--- /dev/null
+++ b/src/Distribution/RealDiracMatrix.h
@@ -0,0 +1,115 @@
+//
+// Class: RealDiracMatrix
+//   Real Dirac matrix class.
+//   They're ordered after the paper of Dr. C. Baumgarten: "Use of real Dirac matrices in two-dimensional coupled linear optics".
+//   The diagonalizing method is based on the paper "Geometrical method of decoupling" (2012) of Dr. C. Baumgarten.
+//
+// Copyright (c) 2014, 2020 Christian Baumgarten, Paul Scherrer Institut, Villigen PSI, Switzerland
+//                          Matthias Frey, ETH Zürich
+// All rights reserved
+//
+// Implemented as part of the Semester thesis by Matthias Frey
+// "Matched Distributions in Cyclotrons"
+//
+// This file is part of OPAL.
+//
+// OPAL 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 3 of the License, or
+// (at your option) any later version.
+//
+// You should have received a copy of the GNU General Public License
+// along with OPAL. If not, see <https://www.gnu.org/licenses/>.
+//
+#ifndef RDM_H
+#define RDM_H
+
+#include <boost/numeric/ublas/matrix_sparse.hpp>
+#include <boost/numeric/ublas/matrix.hpp>
+#include <boost/numeric/ublas/vector.hpp>
+
+class RealDiracMatrix
+{
+public:
+    /// Sparse matrix type definition
+    typedef boost::numeric::ublas::compressed_matrix<
+        double,
+        boost::numeric::ublas::row_major
+    > sparse_matrix_t;
+
+    /// Dense matrix type definition
+    typedef boost::numeric::ublas::matrix<double> matrix_t;
+    /// Dense vector type definition
+    typedef boost::numeric::ublas::vector<
+        double,
+        std::vector<double>
+    > vector_t;
+
+    /// Default constructor (sets only NumOfRDMs and DimOfRDMs)
+    RealDiracMatrix();
+
+    /*!
+        * @param i specifying the matrix (has to be in the range from 0 to 15)
+        * @returns the i-th Real Dirac matrix
+        */
+    sparse_matrix_t getRDM(short);
+
+    /*!
+     * Brings a 4x4 symplex matrix into Hamilton form and
+     * computes the transformation matrix and its inverse
+     *
+     * @param Ms is a 4x4 symplex matrix
+     * @param R is the 4x4 transformation matrix (gets computed)
+     * @param invR is the 4x4 inverse transformation matrix (gets computed)
+     */
+    void diagonalize(matrix_t&, sparse_matrix_t&, sparse_matrix_t&);
+
+    /*!
+     * Diagonalizes (in-place) the 4x4 sigma matrix. This algorithm
+     * is Implemented according to https://arxiv.org/abs/1205.3601
+     *
+     * @param sigma is the 4x4 sigma matrix
+     * @returns the inverse of the total transformation
+     */
+    sparse_matrix_t diagonalize(matrix_t&);
+
+    /*!
+     * @param M 4x4 real-valued matrix
+     * @returns the symplex part of a 4x4 real-valued matrix
+     */
+    matrix_t symplex(const matrix_t&);
+
+private:
+    /*!
+     * Applies a rotation to the matrix M by a given angle
+     *
+     * @param M is the matrix to be transformed
+     * @param i is the i-th RDM used for transformation
+     * @param phi is the angle of rotation
+     * @param Rtot is a reference to the current transformation matrix
+     * @param invRtot is a reference to the inverse of the current transformation matrix
+     */
+    void transform(matrix_t&, short, double, sparse_matrix_t&, sparse_matrix_t&);
+
+    /*!
+     * Obtain transformation matrices.
+     *
+     * @param i is the i-th RDM used for transformation
+     * @param phi is the angle of rotation
+     * @param R is a reference to the transformation matrix
+     * @param invR is a reference to the inverse of the transformation matrix
+     */
+    void transform(short, double, sparse_matrix_t&, sparse_matrix_t&);
+
+    /*!
+     * Update quantites to decouple the sigma-matrix
+     *
+     * @param sigma current state of sigma-matrix
+     * @param i is the i-th step
+     * @param R is a reference to the transformation matrix
+     * @param invR is a reference to the inverse of the transformation matrix
+     */
+    void update(matrix_t& sigma, short i, sparse_matrix_t& R, sparse_matrix_t& invR);
+};
+
+#endif
\ No newline at end of file
diff --git a/src/Distribution/SigmaGenerator.cpp b/src/Distribution/SigmaGenerator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..57ee37f2f51467eaf0c50214d792d26095f076fa
--- /dev/null
+++ b/src/Distribution/SigmaGenerator.cpp
@@ -0,0 +1,877 @@
+//
+// Class: SigmaGenerator
+// The SigmaGenerator class uses the class <b>ClosedOrbitFinder</b> to get the parameters(inverse bending radius, path length
+// field index and tunes) to initialize the sigma matrix.
+// The main function of this class is <b>match(double, unsigned int)</b>, where it iteratively tries to find a matched
+// distribution for given
+// emittances, energy and current. The computation stops when the L2-norm is smaller than a user-defined tolerance. \n
+// In default mode it prints all space charge maps, cyclotron maps and second moment matrices. The orbit properties, i.e.
+// tunes, average radius, orbit radius, inverse bending radius, path length, field index and frequency error, are printed
+// as well.
+//
+// Copyright (c) 2014, 2018, Matthias Frey, Cristopher Cortes, ETH Zürich
+// All rights reserved
+//
+// Implemented as part of the Semester thesis by Matthias Frey
+// "Matched Distributions in Cyclotrons"
+//
+// Some adaptations done as part of the Bachelor thesis by Cristopher Cortes
+// "Limitations of a linear transfer map method for finding matched distributions in high intensity cyclotrons"
+//
+// This file is part of OPAL.
+//
+// OPAL 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 3 of the License, or
+// (at your option) any later version.
+//
+// You should have received a copy of the GNU General Public License
+// along with OPAL. If not, see <https://www.gnu.org/licenses/>.
+//
+#include "SigmaGenerator.h"
+
+#include "AbstractObjects/OpalData.h"
+#include "Utilities/Options.h"
+#include "Utilities/Options.h"
+#include "Utilities/OpalException.h"
+#include "Utilities/Util.h"
+
+#include "matrix_vector_operation.h"
+#include "ClosedOrbitFinder.h"
+#include "MapGenerator.h"
+
+#include <cmath>
+#include <fstream>
+#include <functional>
+#include <iomanip>
+#include <iterator>
+#include <limits>
+#include <list>
+#include <numeric>
+#include <sstream>
+
+#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>
+
+#include <boost/numeric/ublas/vector_proxy.hpp>
+#include <boost/numeric/ublas/triangular.hpp>
+#include <boost/numeric/ublas/lu.hpp>
+#include <boost/numeric/ublas/io.hpp>
+
+#include <gsl/gsl_matrix.h>
+#include <gsl/gsl_math.h>
+#include <gsl/gsl_eigen.h>
+
+extern Inform *gmsg;
+
+SigmaGenerator::SigmaGenerator(double I,
+                               double ex,
+                               double ey,
+                               double ez,
+                               double E,
+                               double m,
+                               const Cyclotron* cycl,
+                               unsigned int N,
+                               unsigned int Nsectors,
+                               unsigned int truncOrder,
+                               bool write)
+    : I_m(I)
+    , wo_m(cycl->getRfFrequ(0)*1E6/cycl->getCyclHarm()*2.0*Physics::pi)
+    , E_m(E)
+    , gamma_m(E/m+1.0)
+    , gamma2_m(gamma_m*gamma_m)
+    , nh_m(cycl->getCyclHarm())
+    , beta_m(std::sqrt(1.0-1.0/gamma2_m))
+    , m_m(m)
+    , niterations_m(0)
+    , converged_m(false)
+    , Emin_m(cycl->getFMLowE())
+    , Emax_m(cycl->getFMHighE())
+    , N_m(N)
+    , nSectors_m(Nsectors)
+    , nStepsPerSector_m(N/cycl->getSymmetry())
+    , nSteps_m(N)
+    , error_m(std::numeric_limits<double>::max())
+    , truncOrder_m(truncOrder)
+    , write_m(write)
+    , sigmas_m(nStepsPerSector_m)
+    , rinit_m(0.0)
+    , prinit_m(0.0)
+{
+    // minimum beta*gamma
+    double bgam = Util::getBetaGamma(Emin_m, m_m);
+
+    // set emittances (initialization like that due to old compiler version)
+    // [ex] = [ey] = [ez] = pi*mm*mrad --> [emittance] = m rad
+    // normalized emittance (--> multiply with beta*gamma)
+    emittance_m[0] = ex * Physics::pi * 1.0e-6 * bgam;
+    emittance_m[1] = ey * Physics::pi * 1.0e-6 * bgam;
+    emittance_m[2] = ez * Physics::pi * 1.0e-6 * bgam;
+
+    // Define the Hamiltonian
+    Series::setGlobalTruncOrder(truncOrder_m);
+
+    // infinitesimal elements
+    x_m = Series::makeVariable(0);
+    px_m = Series::makeVariable(1);
+    z_m = Series::makeVariable(2);
+    pz_m = Series::makeVariable(3);
+    l_m = Series::makeVariable(4);
+    delta_m = Series::makeVariable(5);
+
+    H_m = [&](double h, double kx, double ky) {
+        return 0.5*px_m*px_m + 0.5*kx*x_m*x_m - h*x_m*delta_m +
+               0.5*pz_m*pz_m + 0.5*ky*z_m*z_m +
+               0.5*delta_m*delta_m/gamma2_m;
+    };
+
+    Hsc_m = [&](double sigx, double sigy, double sigz) {
+        // convert m from MeV/c^2 to eV/c^2
+        double m = m_m * 1.0e6;
+
+        // formula (57)
+        double lam = Physics::two_pi*Physics::c / (wo_m * nh_m); // wavelength, [lam] = m
+        // [K3] = m
+        double K3 = 3.0 * I_m * lam
+                  / (20.0 * std::sqrt(5.0) * Physics::pi * Physics::epsilon_0 * m
+                          * Physics::c * beta_m * beta_m * gamma_m * gamma2_m);
+
+        // formula (30), (31)
+        // [sigma(0,0)] = m^{2} --> [sx] = [sy] = [sz] = m
+        // In the cyclotron community z is the vertical and y the longitudinal
+        // direction.
+        // x: horizontal
+        // y/l: longitudinal
+        // z: vertical
+        double sx = std::sqrt(std::abs(sigx));
+        double sy = std::sqrt(std::abs(sigy));
+        double sz = std::sqrt(std::abs(sigz));
+
+        double tmp = sx * sy;                                           // [tmp] = m^{2}
+
+        double f = std::sqrt(tmp) / (3.0 * gamma_m * sz);               // [f] = 1
+        double kxy = K3 * std::abs(1.0 - f) / ((sx + sy) * sz); // [kxy] = 1/m
+
+        double Kx = kxy / sx;
+        double Ky = kxy / sy;
+        double Kz = K3 * f / (tmp * sz);
+
+        return -0.5 * Kx * x_m * x_m
+               -0.5 * Kz * z_m * z_m
+               -0.5 * Ky * l_m * l_m * gamma2_m;
+     };
+}
+
+
+bool SigmaGenerator::match(double accuracy,
+                           unsigned int maxit,
+                           unsigned int maxitOrbit,
+                           Cyclotron* cycl,
+                           double denergy,
+                           double rguess,
+                           bool full)
+{
+    /* compute the equilibrium orbit for energy E_
+     * and get the the following properties:
+     * - inverse bending radius h
+     * - step sizes of path ds
+     * - tune nuz
+     */
+
+    try {
+        if ( !full )
+            nSteps_m = nStepsPerSector_m;
+
+        // object for space charge map and cyclotron map
+        MapGenerator<double,
+                     unsigned int,
+                     Series,
+                     Map,
+                     Hamiltonian,
+                     SpaceCharge> mapgen(nSteps_m);
+
+        // compute cyclotron map and space charge map for each angle and store them into a vector
+        std::vector<matrix_t> Mcycs(nSteps_m), Mscs(nSteps_m);
+
+        ClosedOrbitFinder<double, unsigned int,
+            boost::numeric::odeint::runge_kutta4<container_t> > cof(m_m, N_m, cycl, false, nSectors_m);
+
+        if ( !cof.findOrbit(accuracy, maxitOrbit, E_m, denergy, rguess) ) {
+            throw OpalException("SigmaGenerator::match()",
+                                "Closed orbit finder didn't converge.");
+        }
+
+        cof.computeOrbitProperties(E_m);
+
+        double angle = cycl->getPHIinit();
+        container_t h    = cof.getInverseBendingRadius(angle);
+        container_t r    = cof.getOrbit(angle);
+        container_t peo  = cof.getMomentum(angle);
+        container_t ds   = cof.getPathLength(angle);
+        container_t fidx = cof.getFieldIndex(angle);
+
+        // write properties to file
+        writeOrbitOutput_m(r, peo, h, fidx, ds);
+
+        rinit_m = r[0];
+        prinit_m = peo[0];
+
+        std::string fpath = Util::combineFilePath({
+            OpalData::getInstance()->getAuxiliaryOutputDirectory(),
+            "maps"
+        });
+        if (!boost::filesystem::exists(fpath)) {
+            boost::filesystem::create_directory(fpath);
+        }
+
+        std::pair<double,double> tunes = cof.getTunes();
+        double ravg = cof.getAverageRadius();
+
+        // write to terminal
+        *gmsg << "* ----------------------------" << endl
+                << "* Closed orbit info:" << endl
+                << "*" << endl
+                << "* average radius: " << ravg << " [m]" << endl
+                << "* initial radius: " << r[0] << " [m]" << endl
+                << "* initial momentum: " << peo[0] << " [Beta Gamma]" << endl
+                << "* frequency error: " << cof.getFrequencyError()*100 <<" [ % ] "<< endl
+                << "* horizontal tune: " << tunes.first << endl
+                << "* vertical tune: " << tunes.second << endl
+                << "* ----------------------------" << endl << endl;
+
+        // initialize sigma matrices (for each angle one) (first guess)
+        initialize(tunes.second,ravg);
+
+        // for writing
+        std::ofstream writeMturn, writeMcyc, writeMsc;
+
+        if (write_m) {
+
+            std::string energy = float2string(E_m);
+
+            std::string fname = Util::combineFilePath({
+                OpalData::getInstance()->getAuxiliaryOutputDirectory(),
+                "maps",
+                "OneTurnMapsForEnergy" + energy + "MeV.dat"
+            });
+
+            writeMturn.open(fname, std::ios::out);
+
+            fname = Util::combineFilePath({
+                OpalData::getInstance()->getAuxiliaryOutputDirectory(),
+                "maps",
+                "SpaceChargeMapPerAngleForEnergy" + energy + "MeV_iteration_0.dat"
+            });
+
+            writeMsc.open(fname, std::ios::out);
+
+            fname = Util::combineFilePath({
+                OpalData::getInstance()->getAuxiliaryOutputDirectory(),
+                "maps",
+                "CyclotronMapPerAngleForEnergy" + energy + "MeV.dat"
+            });
+
+            writeMcyc.open(fname, std::ios::out);
+        }
+
+        // calculate only for a single sector (a nSector_-th) of the whole cyclotron
+        for (unsigned int i = 0; i < nSteps_m; ++i) {
+            Mcycs[i] = mapgen.generateMap(H_m(h[i],
+                                              h[i]*h[i]+fidx[i],
+                                              -fidx[i]),
+                                          ds[i],truncOrder_m);
+
+
+            Mscs[i]  = mapgen.generateMap(Hsc_m(sigmas_m[i](0,0),
+                                                sigmas_m[i](2,2),
+                                                sigmas_m[i](4,4)),
+                                          ds[i],truncOrder_m);
+
+            writeMatrix(writeMcyc, Mcycs[i]);
+            writeMatrix(writeMsc, Mscs[i]);
+        }
+
+        if (write_m)
+            writeMsc.close();
+
+        // one turn matrix
+        mapgen.combine(Mscs,Mcycs);
+        matrix_t Mturn = mapgen.getMap();
+
+        writeMatrix(writeMturn, Mturn);
+
+        // (inverse) transformation matrix
+        sparse_matrix_t R(6, 6), invR(6, 6);
+
+        // new initial sigma matrix
+        matrix_t newSigma(6,6);
+
+        // for exiting loop
+        bool stop = false;
+
+        double weight = 0.5;
+
+        while (error_m > accuracy && !stop) {
+            // decouple transfer matrix and compute (inverse) tranformation matrix
+           vector_t eigen = decouple(Mturn, R,invR);
+
+            // construct new initial sigma-matrix
+            newSigma = updateInitialSigma(Mturn, eigen, R, invR);
+
+            // compute new sigma matrices for all angles (except for initial sigma)
+            updateSigma(Mscs,Mcycs);
+
+            // compute error with mm^2 and (mrad)^2
+            error_m = L2ErrorNorm(sigmas_m[0] * 1e6, newSigma * 1e6);
+
+            // write new initial sigma-matrix into vector
+            sigmas_m[0] = weight*newSigma + (1.0-weight)*sigmas_m[0];
+
+            if (write_m) {
+
+                std::string energy = float2string(E_m);
+
+                std::string fname = Util::combineFilePath({
+                        OpalData::getInstance()->getAuxiliaryOutputDirectory(),
+                        "maps",
+                        "SpaceChargeMapPerAngleForEnergy" + energy + "MeV_iteration_"
+                        + std::to_string(niterations_m + 1)
+                        + ".dat"
+                });
+
+                writeMsc.open(fname, std::ios::out);
+            }
+
+            // compute new space charge maps
+            for (unsigned int i = 0; i < nSteps_m; ++i) {
+                Mscs[i] = mapgen.generateMap(Hsc_m(sigmas_m[i](0,0),
+                                                    sigmas_m[i](2,2),
+                                                    sigmas_m[i](4,4)),
+                                                ds[i],truncOrder_m);
+
+                writeMatrix(writeMsc, Mscs[i]);
+            }
+
+            if (write_m) {
+                writeMsc.close();
+            }
+
+            // construct new one turn transfer matrix M
+            mapgen.combine(Mscs,Mcycs);
+            Mturn = mapgen.getMap();
+
+            writeMatrix(writeMturn, Mturn);
+
+            // check if number of iterations has maxit exceeded.
+            stop = (niterations_m++ > maxit);
+        }
+
+        // store converged sigma-matrix
+        sigma_m.resize(6,6,false);
+        sigma_m.swap(newSigma);
+
+        // returns if the sigma matrix has converged
+        converged_m = error_m < accuracy;
+
+        // Close files
+        if (write_m) {
+            writeMturn.close();
+            writeMcyc.close();
+        }
+
+    } catch(const std::exception& e) {
+        throw OpalException("SigmaGenerator::match()", e.what());
+    }
+
+    if ( converged_m && write_m ) {
+        // write tunes
+        std::string fname = Util::combineFilePath({
+            OpalData::getInstance()->getAuxiliaryOutputDirectory(),
+            "MatchedDistributions.dat"
+        });
+
+        std::ofstream writeSigmaMatched(fname, std::ios::out);
+
+        std::array<double,3> emit = this->getEmittances();
+
+        writeSigmaMatched << "* Converged (Ex, Ey, Ez) = (" << emit[0]
+                          << ", " << emit[1] << ", " << emit[2]
+                          << ") pi mm mrad for E= " << E_m << " (MeV)"
+                          << std::endl << "* Sigma-Matrix " << std::endl;
+
+        for(unsigned int i = 0; i < sigma_m.size1(); ++ i) {
+            writeSigmaMatched << std::setprecision(4)  << std::setw(11)
+                              << sigma_m(i,0);
+            for(unsigned int j = 1; j < sigma_m.size2(); ++ j) {
+                writeSigmaMatched << " & " <<  std::setprecision(4)
+                                  << std::setw(11) << sigma_m(i,j);
+            }
+            writeSigmaMatched << " \\\\" << std::endl;
+        }
+
+        writeSigmaMatched << std::endl;
+        writeSigmaMatched.close();
+    }
+
+    return converged_m;
+}
+
+
+typename SigmaGenerator::vector_t
+SigmaGenerator::decouple(const matrix_t& Mturn,
+                         sparse_matrix_t& R,
+                         sparse_matrix_t& invR)
+{
+    // copy one turn matrix
+    matrix_t M(Mturn);
+
+    // reduce 6x6 matrix to 4x4 matrix
+    reduce<matrix_t>(M);
+
+    // compute symplex part
+    matrix_t Ms = rdm_m.symplex(M);
+
+    // diagonalize and compute transformation matrices
+    rdm_m.diagonalize(Ms,R,invR);
+
+    /*
+     * formula (38) in paper of Dr. Christian Baumgarten:
+     * Geometrical method of decoupling
+     *
+     *          [0,     alpha,  0,      0;
+     * F_{d} =  -beta,  0,      0,      0;
+     *          0,      0,      0,      gamma;
+     *          0,      0,      -delta, 0]
+     *
+     *
+     */
+    vector_t eigen(4);
+    eigen(0) =   Ms(0,1);       // alpha
+    eigen(1) = - Ms(1,0);       // beta
+    eigen(2) =   Ms(2,3);       // gamma
+    eigen(3) = - Ms(3,2);       // delta
+    return eigen;
+}
+
+
+double SigmaGenerator::isEigenEllipse(const matrix_t& Mturn,
+                                      const matrix_t& sigma)
+{
+    // compute sigma matrix after one turn
+    matrix_t newSigma = matt_boost::gemmm<matrix_t>(Mturn,
+                                                    sigma,
+                                                    boost::numeric::ublas::trans(Mturn));
+
+    // return L2 error
+    return L2ErrorNorm(sigma,newSigma);
+}
+
+
+void SigmaGenerator::initialize(double nuz, double ravg)
+{
+    /*
+     * The initialization is based on the analytical solution of
+     * using a spherical symmetric beam in the paper:
+     * Transverse-longitudinal coupling by space charge in cyclotrons
+     * by Dr. Christian Baumgarten
+     * (formulas: (46), (56), (57))
+     */
+
+
+    /* Units:
+     * ----------------------------------------------
+     * [wo] = 1/s
+     * [nh] = 1
+     * [q0] = 1 e
+     * [I] = A
+     * [eps0] = (A*s)^{2}/(N*m^{2})
+     * [E0] = MeV/(c^{2}) (with speed of light c)
+     * [beta] = 1
+     * [gamma] = 1
+     * [m] = eV/c^2
+     *
+     * [lam] = m
+     * [K3] = m
+     * [alpha] = 1
+     * ----------------------------------------------
+     */
+
+    // helper constants
+    double invbg = 1.0 / (beta_m * gamma_m);
+
+    // convert mass m_m from MeV/c^2 to eV*s^{2}/m^{2}
+    double m = m_m * 1.0e6;        // [m] = eV/c^2, [m_m] = MeV/c^2
+
+    // emittance [ex] = [ey] = [ez] = m rad
+    double ex = emittance_m[0] * invbg;                        // [ex] = m rad
+    double ey = emittance_m[1] * invbg;                        // [ey] = m rad
+    double ez = emittance_m[2] * invbg;                        // [ez] = m rad
+
+    // initial guess of emittance, [e] = m rad
+    double e = std::cbrt(ex * ey * ez);             // cbrt computes cubic root (C++11) <cmath>
+
+    // cyclotron radius [rcyc] = m
+    double rcyc = ravg / beta_m;
+
+    // "average" inverse bending radius
+    double h = 1.0 / ravg;            // [h] = 1/m
+
+    // formula (57)
+    double lam = Physics::two_pi * Physics::c / (wo_m * nh_m); // wavelength, [lam] = m
+
+    // m * c^3 --> c^2 in [m] = eV / c^2 cancel --> m * c in denominator
+    double K3 = 3.0 *  I_m * lam
+              / (20.0 * std::sqrt(5.0) * Physics::pi * Physics::epsilon_0 * m
+                      * Physics::c * beta_m * beta_m * gamma2_m * gamma_m);    // [K3] = m
+
+    // c in denominator cancels with unit of [m] = eV / c^2 --> we need to multiply
+    // with c in order to get dimensionless quantity
+    double alpha =  Physics::mu_0 * I_m * Physics::c / (5.0 * std::sqrt(10.0) * m
+                 * gamma_m * nh_m) * std::sqrt(rcyc * rcyc * rcyc / (e * e * e));   // [alpha] = 1/rad --> [alpha] = 1
+
+    double sig0 = std::sqrt(2.0 * rcyc * e) / gamma_m;                              // [sig0] = m*sqrt(rad) --> [sig0] = m
+
+    // formula (56)
+    double sig;                                     // [sig] = m
+    if (alpha >= 2.5) {
+        sig = sig0 * std::cbrt(1.0 + alpha);            // cbrt computes cubic root (C++11) <cmath>
+    } else if (alpha >= 0) {
+        sig = sig0 * (1 + alpha * (0.25 - 0.03125 * alpha));
+    } else {
+        throw OpalException("SigmaGenerator::initialize()",
+                            "Negative alpha value: " + std::to_string(alpha) + " < 0");
+    }
+
+    // K = Kx = Ky = Kz
+    double K = K3 * gamma_m / (3.0 * sig * sig * sig);   // formula (46), [K] = 1/m^{2}
+    double kx = h * h * gamma2_m;                        // formula (46) (assumption of an isochronous cyclotron), [kx] = 1/m^{2}
+
+    double a = 0.5 * kx - K;    // formula (22) (with K = Kx = Kz), [a] = 1/m^{2}
+    double b = K * K;           // formula (22) (with K = Kx = Kz and kx = h^2*gamma^2), [b] = 1/m^{4}
+
+
+    // b must be positive, otherwise no real-valued frequency
+    if (b < 0)
+        throw OpalException("SigmaGenerator::initialize()",
+                            "Negative value --> No real-valued frequency.");
+
+    double tmp = a * a - b;           // [tmp] = 1/m^{4}
+    if (tmp < 0)
+        throw OpalException("SigmaGenerator::initialize()",
+                            "Square root of negative number.");
+
+    tmp = std::sqrt(tmp);               // [tmp] = 1/m^{2}
+
+    if (a < tmp)
+        throw OpalException("SigmaGenerator::initialize()",
+                            "Square root of negative number.");
+
+    if (h * h * nuz * nuz <= K)
+        throw OpalException("SigmaGenerator::initialize()",
+                            "h^{2} * nu_{z}^{2} <= K (i.e. square root of negative number)");
+
+    double Omega = std::sqrt(a + tmp);                // formula (22), [Omega] = 1/m
+    double omega = std::sqrt(a - tmp);                // formula (22), [omega] = 1/m
+
+    double A = h / (Omega * Omega + K);           // formula (26), [A] = m
+    double B = h / (omega * omega + K);           // formula (26), [B] = m
+    double invAB = 1.0 / (B - A);                 // [invAB] = 1/m
+
+    // construct initial sigma-matrix (formula (29, 30, 31)
+    matrix_t sigma = boost::numeric::ublas::zero_matrix<double>(6);
+
+    // formula (30), [sigma(0,0)] = m^2 rad
+    sigma(0,0) = invAB * (B * ex / Omega + A * ez / omega);
+
+    // [sigma(0,5)] = [sigma(5,0)] = m rad
+    sigma(0,5) = sigma(5,0) = invAB * (ex / Omega + ez / omega);
+
+    // [sigma(1,1)] = rad
+    sigma(1,1) = invAB * (B * ex * Omega + A * ez * omega);
+
+    // [sigma(1,4)] = [sigma(4,1)] = m rad
+    sigma(1,4) = sigma(4,1) = invAB * (ex * Omega+ez * omega) / (K * gamma2_m);
+
+    // formula (31), [sigma(2,2)] = m rad
+    sigma(2,2) = ey / (std::sqrt(h * h * nuz * nuz - K));
+
+    sigma(3,3) = (ey * ey) / sigma(2,2);
+
+    // [sigma(4,4)] = m^{2} rad
+    sigma(4,4) = invAB * (A * ex * Omega + B * ez * omega) / (K * gamma2_m);
+
+    // formula (30), [sigma(5,5)] = rad
+    sigma(5,5) = invAB * (ex / (B * Omega) + ez / (A * omega));
+
+    // fill in initial guess of the sigma matrix (for each angle the same guess)
+    sigmas_m.resize(nSteps_m);
+    for (typename std::vector<matrix_t>::iterator it = sigmas_m.begin(); it != sigmas_m.end(); ++it) {
+        *it = sigma;
+    }
+
+    if (write_m) {
+        std::string energy = float2string(E_m);
+
+        std::string fname = Util::combineFilePath({
+            OpalData::getInstance()->getAuxiliaryOutputDirectory(),
+            "maps",
+            "InitialSigmaPerAngleForEnergy" + energy + "MeV.dat"
+        });
+
+        std::ofstream writeInit(fname, std::ios::out);
+        writeMatrix(writeInit, sigma);
+        writeInit.close();
+    }
+}
+
+
+typename SigmaGenerator::matrix_t
+SigmaGenerator::updateInitialSigma(const matrix_t& M,
+                                   const vector_t& eigen,
+                                   sparse_matrix_t& R,
+                                   sparse_matrix_t& invR)
+{
+    /*
+     * This function is based on the paper of Dr. Christian Baumgarten:
+     * Transverse-Longitudinal Coupling by Space Charge in Cyclotrons (2012)
+     */
+
+    /*
+     * Function input:
+     * - M: one turn transfer matrix
+     * - eigen = {alpha, beta, gamma, delta}
+     * - R: transformation matrix (in paper: E)
+     * - invR: inverse transformation matrix (in paper: E^{-1}
+     */
+
+    // normalize emittances
+    double invbg = 1.0 / (beta_m * gamma_m);
+    double ex = emittance_m[0] * invbg;
+    double ey = emittance_m[1] * invbg;
+    double ez = emittance_m[2] * invbg;
+
+    // alpha^2-beta*gamma = 1
+
+    /* 0        eigen(0) 0        0
+     * eigen(1) 0        0        0
+     * 0        0        0        eigen(2)
+     * 0        0        eigen(3) 0
+     *
+     * M = cos(mux)*[1, 0; 0, 1] + sin(mux)*[alpha, beta; -gamma, -alpha], Book, p. 242
+     *
+     * -----------------------------------------------------------------------------------
+     * X-DIRECTION and L-DIRECTION
+     * -----------------------------------------------------------------------------------
+     * --> eigen(0) = sin(mux)*betax
+     * --> eigen(1) = -gammax*sin(mux)
+     *
+     * What is sin(mux)?   --> alphax = 0 --> -alphax^2+betax*gammax = betax*gammax = 1
+     *
+     * Convention: betax > 0
+     *
+     * 1) betax = 1/gammax
+     * 2) eig0 = sin(mux)*betax
+     * 3) eig1 = -gammax*sin(mux)
+     *
+     * eig0 = sin(mux)/gammax
+     * eig1 = -gammax*sin(mux) <--> 1/gammax = -sin(mux)/eig1
+     *
+     * eig0 = -sin(mux)^2/eig1 --> -sin(mux)^2 = eig0*eig1      --> sin(mux) = sqrt(-eig0*eig1)
+     *                                                          --> gammax = -eig1/sin(mux)
+     *                                                          --> betax = eig0/sin(mux)
+     */
+
+
+    // x-direction
+    //double alphax = 0.0;
+    double betax  = std::sqrt(std::fabs(eigen(0) / eigen(1)));
+    double gammax = 1.0 / betax;
+
+    // l-direction
+    //double alphal = 0.0;
+    double betal  = std::sqrt(std::fabs(eigen(2) / eigen(3)));
+    double gammal = 1.0 / betal;
+
+    /*
+     * -----------------------------------------------------------------------------------
+     * Z-DIRECTION
+     * -----------------------------------------------------------------------------------
+     *
+     * m22 m23
+     * m32 m33
+     *
+     * m22 = cos(muz) + alpha*sin(muz)
+     * m33 = cos(muz) - alpha*sin(muz)
+     *
+     * --> cos(muz) = 0.5*(m22 + m33)
+     *     sin(muz) = sign(m32)*sqrt(1-cos(muz)^2)
+     *
+     * m22-m33 = 2*alpha*sin(muz) --> alpha = 0.5*(m22-m33)/sin(muz)
+     *
+     * m23 = betaz*sin(muz)     --> betaz = m23/sin(muz)
+     * m32 = -gammaz*sin(muz)   --> gammaz = -m32/sin(muz)
+     */
+
+    double cosz = 0.5 * (M(2,2) + M(3,3));
+
+    double sign = (std::signbit(M(2,3))) ? double(-1) : double(1);
+
+    double invsinz = sign / std::sqrt(std::fabs( 1.0 - cosz * cosz));
+
+    double alphaz = 0.5 * (M(2,2) - M(3,3)) * invsinz;
+    double betaz  =   M(2,3) * invsinz;
+    double gammaz = - M(3,2) * invsinz;
+
+    // Convention beta>0
+    if (std::signbit(betaz))    // singbit = true if beta<0, else false
+        betaz  *= -1.0;
+
+    // diagonal matrix with eigenvalues
+    matrix_t D = boost::numeric::ublas::zero_matrix<double>(6,6);
+    // x-direction
+    D(0,1) =   betax  * ex;
+    D(1,0) = - gammax * ex;
+    // z-direction
+    D(2,2) =   alphaz * ey;
+    D(3,3) = - alphaz * ey;
+    D(2,3) =   betaz  * ey;
+    D(3,2) = - gammaz * ey;
+    // l-direction
+    D(4,5) =   betal  * ez;
+    D(5,4) = - gammal * ez;
+
+    // expand 4x4 transformation matrices to 6x6
+    expand<sparse_matrix_t>(R);
+    expand<sparse_matrix_t>(invR);
+
+    // symplectic matrix
+    sparse_matrix_t S(6,6,6);
+    S(0,1) = S(2,3) = S(4,5) = 1;
+    S(1,0) = S(3,2) = S(5,4) = -1;
+
+    matrix_t sigma = matt_boost::gemmm<matrix_t>(-invR,D,R);
+    sigma = boost::numeric::ublas::prod(sigma,S);
+
+    if (write_m) {
+        std::string energy = float2string(E_m);
+
+        std::string fname = Util::combineFilePath({
+            OpalData::getInstance()->getAuxiliaryOutputDirectory(),
+            "maps",
+            "InitialSigmaPerAngleForEnergy" + energy + "MeV.dat"
+        });
+
+        std::ofstream writeInit(fname, std::ios::app);
+        writeMatrix(writeInit, sigma);
+        writeInit.close();
+    }
+
+    return sigma;
+}
+
+
+void SigmaGenerator::updateSigma(const std::vector<matrix_t>& Mscs,
+                                 const std::vector<matrix_t>& Mcycs)
+{
+    std::ofstream writeSigma;
+
+    if (write_m) {
+        std::string energy = float2string(E_m);
+
+        std::string fname = Util::combineFilePath({
+            OpalData::getInstance()->getAuxiliaryOutputDirectory(),
+            "maps",
+            "SigmaPerAngleForEnergy" + energy + "MeV_iteration_"
+            + std::to_string(niterations_m + 1)
+            + ".dat"
+        });
+
+        writeSigma.open(fname,std::ios::out);
+    }
+
+    // initial sigma is already computed
+    writeMatrix(writeSigma, sigmas_m[0]);
+
+    for (unsigned int i = 1; i < nSteps_m; ++i) {
+        // transfer matrix for one angle
+        matrix_t M = boost::numeric::ublas::prod(Mscs[i - 1],Mcycs[i - 1]);
+        // transfer the matrix sigma
+        sigmas_m[i] = matt_boost::gemmm<matrix_t>(M,sigmas_m[i - 1],
+                                                     boost::numeric::ublas::trans(M));
+
+        writeMatrix(writeSigma, sigmas_m[i]);
+    }
+
+    if (write_m) {
+        writeSigma.close();
+    }
+}
+
+
+double SigmaGenerator::L2ErrorNorm(const matrix_t& oldS,
+                                   const matrix_t& newS)
+{
+    // compute difference
+    matrix_t diff = newS - oldS;
+
+    // sum squared error up and take square root
+    return std::sqrt(std::inner_product(diff.data().begin(),
+                                        diff.data().end(),
+                                        diff.data().begin(),
+                                        0.0));
+}
+
+
+std::string SigmaGenerator::float2string(double val) {
+    std::ostringstream out;
+    out << std::setprecision(6) << val;
+    return out.str();
+}
+
+
+void SigmaGenerator::writeOrbitOutput_m(
+    const container_t& r,
+    const container_t& peo,
+    const container_t& h,
+    const container_t& fidx,
+    const container_t& ds)
+{
+    if (!write_m)
+        return;
+
+    std::string energy = float2string(E_m);
+    std::string fname = Util::combineFilePath({
+            OpalData::getInstance()->getAuxiliaryOutputDirectory(),
+            "PropertiesForEnergy" + energy + "MeV.dat"
+    });
+
+    std::ofstream writeProperties(fname, std::ios::out);
+
+    writeProperties << std::left
+                    << std::setw(25) << "orbit radius"
+                    << std::setw(25) << "orbit momentum"
+                    << std::setw(25) << "inverse bending radius"
+                    << std::setw(25) << "field index"
+                    << std::setw(25) << "path length" << std::endl;
+
+    for (unsigned int i = 0; i < r.size(); ++i) {
+        writeProperties << std::setprecision(10) << std::left
+                        << std::setw(25) << r[i]
+                        << std::setw(25) << peo[i]
+                        << std::setw(25) << h[i]
+                        << std::setw(25) << fidx[i]
+                        << std::setw(25) << ds[i] << std::endl;
+    }
+    writeProperties.close();
+}
+
+
+void SigmaGenerator::writeMatrix(std::ofstream& os, const matrix_t& m) {
+    if (!write_m)
+        return;
+
+    for (unsigned int i = 0; i < 6; ++i) {
+        for (unsigned int j = 0; j < 6; ++j)
+            os << m(i, j) << " ";
+    }
+    os << std::endl;
+}
diff --git a/src/Distribution/SigmaGenerator.h b/src/Distribution/SigmaGenerator.h
index a8d6e148de9358daceff6641d6b7cef2b12bfc6c..dc2035047dcf766b3d6c010534ae5311408952c3 100644
--- a/src/Distribution/SigmaGenerator.h
+++ b/src/Distribution/SigmaGenerator.h
@@ -1,8 +1,9 @@
 //
-// Class: SigmaGenerator.h
-// The SigmaGenerator class uses the class <b>ClosedOrbitFinder</b> to get the parameters (inverse bending radius, path length
+// Class: SigmaGenerator
+// The SigmaGenerator class uses the class <b>ClosedOrbitFinder</b> to get the parameters(inverse bending radius, path length
 // field index and tunes) to initialize the sigma matrix.
-// The main function of this class is <b>match(value_type, size_type)</b>, where it iteratively tries to find a matched distribution for given
+// The main function of this class is <b>match(double, unsigned int)</b>, where it iteratively tries to find a matched
+// distribution for given
 // emittances, energy and current. The computation stops when the L2-norm is smaller than a user-defined tolerance. \n
 // In default mode it prints all space charge maps, cyclotron maps and second moment matrices. The orbit properties, i.e.
 // tunes, average radius, orbit radius, inverse bending radius, path length, field index and frequency error, are printed
@@ -32,83 +33,35 @@
 #define SIGMAGENERATOR_H
 
 #include <array>
-#include <cmath>
-#include <fstream>
-#include <functional>
-#include <iomanip>
-#include <iterator>
-#include <limits>
-#include <list>
-#include <numeric>
-#include <sstream>
 #include <string>
-#include <utility>
 #include <vector>
 
-#include "AbstractObjects/OpalData.h"
+#include "AbsBeamline/Cyclotron.h"
+#include "FixedAlgebra/FTps.h"
 #include "Physics/Physics.h"
-#include "Utilities/Options.h"
-#include "Utilities/Options.h"
-#include "Utilities/OpalException.h"
-#include "Utilities/Util.h"
 
-#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>
+#include "Distribution/RealDiracMatrix.h"
 
-#include <boost/numeric/ublas/matrix.hpp>
-#include <boost/numeric/ublas/matrix_sparse.hpp>
-#include <boost/numeric/ublas/vector.hpp>
 
-#include <boost/numeric/ublas/vector_proxy.hpp>
-#include <boost/numeric/ublas/triangular.hpp>
-#include <boost/numeric/ublas/lu.hpp>
-#include <boost/numeric/ublas/io.hpp>
-
-#include <gsl/gsl_matrix.h>
-#include <gsl/gsl_math.h>
-#include <gsl/gsl_eigen.h>
-
-#include "matrix_vector_operation.h"
-#include "ClosedOrbitFinder.h"
-#include "MapGenerator.h"
-#include "Harmonics.h"
-
-extern Inform *gmsg;
-
-/// @brief This class computes the matched distribution
-template<typename Value_type, typename Size_type>
 class SigmaGenerator
 {
 public:
-    /// Type of variables
-    typedef Value_type value_type;
-    /// Type of constant variables
-    typedef const value_type const_value_type;
-    /// Type for specifying sizes
-    typedef Size_type size_type;
     /// Type for storing maps
-    typedef boost::numeric::ublas::matrix<value_type> matrix_type;
-
-    typedef std::complex<value_type> complex_t;
-
-    /// Type for storing complex matrices
-    typedef boost::numeric::ublas::matrix<complex_t> cmatrix_type;
-
+    typedef RealDiracMatrix::matrix_t matrix_t;
     /// Type for storing the sparse maps
-    typedef boost::numeric::ublas::compressed_matrix<complex_t,
-                                                     boost::numeric::ublas::row_major
-                                                     > sparse_matrix_type;
+    typedef RealDiracMatrix::sparse_matrix_t sparse_matrix_t;
     /// Type for storing vectors
-    typedef boost::numeric::ublas::vector<value_type> vector_type;
+    typedef RealDiracMatrix::vector_t vector_t;
     /// Container for storing the properties for each angle
-    typedef std::vector<value_type> container_type;
+    typedef std::vector<double> container_t;
     /// Type of the truncated powere series
-    typedef FTps<value_type,2*3> Series;
+    typedef FTps<double,2*3> Series;
     /// Type of a map
-    typedef FVps<value_type,2*3> Map;
+    typedef FVps<double,2*3> Map;
     /// Type of the Hamiltonian for the cyclotron
-    typedef std::function<Series(value_type,value_type,value_type)> Hamiltonian;
+    typedef std::function<Series(double,double,double)> Hamiltonian;
     /// Type of the Hamiltonian for the space charge
-    typedef std::function<Series(value_type,value_type,value_type)> SpaceCharge;
+    typedef std::function<Series(double,double,double)> SpaceCharge;
 
     /// Constructs an object of type SigmaGenerator
     /*!
@@ -125,9 +78,9 @@ public:
      * @param truncOrder is the truncation order for power series of the Hamiltonian
      * @param write is a boolean (default: true). If true all maps of all iterations are stored, otherwise not.
      */
-    SigmaGenerator(value_type I, value_type ex, value_type ey, value_type ez,
-                   value_type E, value_type m, const Cyclotron* cycl,
-                   size_type N, size_type Nsectors, size_type truncOrder, bool write = true);
+    SigmaGenerator(double I, double ex, double ey, double ez,
+                   double E, double m, const Cyclotron* cycl,
+                   unsigned int N, unsigned int Nsectors, unsigned int truncOrder, bool write = true);
 
     /// Searches for a matched distribution.
     /*!
@@ -140,26 +93,10 @@ public:
      * @param denergy energy step size for closed orbit finder [MeV]
      * @param rguess value of radius for closed orbit finder
      * @param type specifies the magnetic field format (e.g. CARBONCYCL)
-     * @param harmonic is a boolean. If "true" the harmonics are used instead of the closed orbit finder.
      * @param full match over full turn not just single sector
      */
-    bool match(value_type accuracy, size_type maxit, size_type maxitOrbit,
-               Cyclotron* cycl, value_type denergy, value_type rguess, bool harmonic, bool full);
-
-    /*!
-     * Eigenvalue / eigenvector solver
-     * @param Mturn is a 6x6 dimensional one turn transfer matrix
-     * @param R is the 6x6 dimensional transformation matrix (gets computed)
-     */
-    void eigsolve_m(const matrix_type& Mturn, sparse_matrix_type& R);
-
-    /*!
-     * @param R is the 6x6 dimensional transformation matrix
-     * @param invR is the 6x6 dimensional inverse transformation (gets computed)
-     * @return true if success
-     */
-    bool invertMatrix_m(const sparse_matrix_type& R,
-                        sparse_matrix_type& invR);
+    bool match(double accuracy, unsigned int maxit, unsigned int maxitOrbit,
+               Cyclotron* cycl, double denergy, double rguess, bool full);
 
     /// Block diagonalizes the symplex part of the one turn transfer matrix
     /*! It computes the transformation matrix <b>R</b> and its inverse <b>invR</b>.
@@ -168,7 +105,7 @@ public:
      * @param R is the 6x6 dimensional transformation matrix (gets computed)
      * @param invR is the 6x6 dimensional inverse transformation (gets computed)
      */
-    void decouple(const matrix_type& Mturn, sparse_matrix_type& R, sparse_matrix_type& invR);
+    vector_t decouple(const matrix_t& Mturn, sparse_matrix_t& R, sparse_matrix_t& invR);
 
     /// Checks if the sigma-matrix is an eigenellipse and returns the L2 error.
     /*!
@@ -176,19 +113,21 @@ public:
      * @param Mturn is the one turn transfer matrix
      * @param sigma is the sigma matrix to be tested
      */
-    value_type isEigenEllipse(const matrix_type& Mturn, const matrix_type& sigma);
+    double isEigenEllipse(const matrix_t& Mturn, const matrix_t& sigma);
 
     /// Returns the converged stationary distribution
-    matrix_type& getSigma();
+    matrix_t& getSigma();
 
     /// Returns the number of iterations needed for convergence (if not converged, it returns zero)
-    size_type getIterations() const;
+    unsigned int getIterations() const;
 
     /// Returns the error (if the program didn't converged, one can call this function to check the error)
-    value_type getError() const;
+    double getError() const;
 
-    /// Returns the emittances (ex,ey,ez) in \f$ \pi\ mm\ mrad \f$ for which the code converged (since the whole simulation is done on normalized emittances)
-    std::array<value_type,3> getEmittances() const;
+    /*! Returns the emittances (ex,ey,ez) in \f$ \pi\ mm\ mrad \f$ for which
+     * the code converged (since the whole simulation is done on normalized emittances)
+     */
+    std::array<double,3> getEmittances() const;
 
     const double& getInjectionRadius() const {
         return rinit_m;
@@ -198,57 +137,61 @@ public:
         return prinit_m;
     }
 
-    private:
+private:
     /// Stores the value of the current, \f$ \left[I\right] = A \f$
-    value_type I_m;
-    /// Stores the desired emittances, \f$ \left[\varepsilon_{x}\right] = \left[\varepsilon_{y}\right] = \left[\varepsilon_{z}\right] = mm \ mrad \f$
-    std::array<value_type,3> emittance_m;
+    double I_m;
+    /*! Stores the desired emittances,
+     * \f$ \left[\varepsilon_{x}\right] = \left[\varepsilon_{y}\right] = \left[\varepsilon_{z}\right] = m \ rad \f$
+     */
+    std::array<double,3> emittance_m;
     /// Is the orbital frequency, \f$ \left[\omega_{o}\right] = \frac{1}{s} \f$
-    value_type wo_m;
+    double wo_m;
     /// Stores the user-define energy, \f$ \left[E\right] = MeV \f$
-    value_type E_m;
-    /// Relativistic factor (which can be computed out ot the kinetic energy and rest mass (potential energy)), \f$ \left[\gamma\right] = 1 \f$
-    value_type gamma_m;
+    double E_m;
+    /*! Relativistic factor (which can be computed out ot the kinetic
+     * energy and rest mass (potential energy)), \f$ \left[\gamma\right] = 1 \f$
+     */
+    double gamma_m;
     /// Relativistic factor squared
-    value_type gamma2_m;
+    double gamma2_m;
     /// Harmonic number, \f$ \left[N_{h}\right] = 1 \f$
-    value_type nh_m;
+    double nh_m;
     /// Velocity (c/v), \f$ \left[\beta\right] = 1 \f$
-    value_type beta_m;
+    double beta_m;
     /// Is the mass of the particles, \f$ \left[m\right] = \frac{MeV}{c^{2}} \f$
-    value_type m_m;
+    double m_m;
     /// Is the number of iterations needed for convergence
-    size_type niterations_m;
+    unsigned int niterations_m;
     /// Is true if converged, false otherwise
     bool converged_m;
     /// Minimum energy needed in cyclotron, \f$ \left[E_{min}\right] = MeV \f$
-    value_type Emin_m;
+    double Emin_m;
     /// Maximum energy reached in cyclotron, \f$ \left[E_{max}\right] = MeV \f$
-    value_type Emax_m;
+    double Emax_m;
     /// Number of integration steps for closed orbit computation
-    size_type N_m;
+    unsigned int N_m;
     /// Number of (symmetric) sectors the field is averaged over
-    size_type nSectors_m;
+    unsigned int nSectors_m;
     /// Number of integration steps per sector (--> also: number of maps per sector)
-    size_type nStepsPerSector_m;
+    unsigned int nStepsPerSector_m;
 
     /// Number of integration steps in total
-    size_type nSteps_m;
+    unsigned int nSteps_m;
 
     /// Error of computation
-    value_type error_m;
+    double error_m;
 
     /// Truncation order of the power series for the Hamiltonian (cyclotron and space charge)
-    size_type truncOrder_m;
+    unsigned int truncOrder_m;
 
     /// Decides for writing output (default: true)
     bool write_m;
 
     /// Stores the stationary distribution (sigma matrix)
-    matrix_type sigma_m;
+    matrix_t sigma_m;
 
     /// Vector storing the second moment matrix for each angle
-    std::vector<matrix_type> sigmas_m;
+    std::vector<matrix_t> sigmas_m;
 
     /// Stores the Hamiltonian of the cyclotron
     Hamiltonian H_m;
@@ -256,8 +199,8 @@ public:
     /// Stores the Hamiltonian for the space charge
     SpaceCharge Hsc_m;
 
-    /// All variables x, px, y, py, z, delta
-    Series x_m, px_m, y_m, py_m, z_m, delta_m;
+    /// All variables x, px, z, pz, l, delta
+    Series x_m, px_m, z_m, pz_m, l_m, delta_m;
 
     double rinit_m;
     double prinit_m;
@@ -269,7 +212,7 @@ public:
      * @param nuz is the vertical tune
      * @param ravg is the average radius of the closed orbit
      */
-    void initialize(value_type, value_type);
+    void initialize(double, double);
 
     /// Computes the new initial sigma matrix
     /*!
@@ -277,1052 +220,158 @@ public:
      * @param R is the transformation matrix
      * @param invR is the inverse transformation matrix
      */
-    matrix_type updateInitialSigma(const matrix_type&,
-                                   sparse_matrix_type&,
-                                   sparse_matrix_type&);
+    matrix_t updateInitialSigma(const matrix_t&,
+                                   const vector_t&,
+                                   sparse_matrix_t&,
+                                   sparse_matrix_t&);
 
     /// Computes new sigma matrices (one for each angle)
     /*!
      * Mscs is a vector of all space charge maps
      * Mcycs is a vector of all cyclotron maps
      */
-    void updateSigma(const std::vector<matrix_type>&,
-                     const std::vector<matrix_type>&);
+    void updateSigma(const std::vector<matrix_t>&,
+                     const std::vector<matrix_t>&);
 
     /// Returns the L2-error norm between the old and new sigma-matrix
     /*!
      * @param oldS is the old sigma matrix
      * @param newS is the new sigma matrix
      */
-    value_type L2ErrorNorm(const matrix_type&, const matrix_type&);
-
-
-    /// Returns the Lp-error norm between the old and new sigma-matrix
-    /*!
-     * @param oldS is the old sigma matrix
-     * @param newS is the new sigma matrix
-     */
-    value_type L1ErrorNorm(const matrix_type&, const matrix_type&);
+    double L2ErrorNorm(const matrix_t&, const matrix_t&);
 
     /// Transforms a floating point value to a string
     /*!
      * @param val is the floating point value which is transformed to a string
      */
-    std::string float2string(value_type val);
+    std::string float2string(double val);
 
     /// Called within SigmaGenerator::match().
     /*!
-     * @param tunes
-     * @param ravg is the average radius [m]
      * @param r_turn is the radius [m]
      * @param peo is the momentum
      * @param h_turn is the inverse bending radius
      * @param fidx_turn is the field index
      * @param ds_turn is the path length element
      */
-    void writeOrbitOutput_m(const std::pair<value_type,value_type>& tunes,
-                            const value_type& ravg,
-                            const value_type& freqError,
-                            const container_type& r_turn,
-                            const container_type& peo,
-                            const container_type& h_turn,
-                            const container_type&  fidx_turn,
-                            const container_type&  ds_turn);
-};
-
-// -----------------------------------------------------------------------------------------------------------------------
-// PUBLIC MEMBER FUNCTIONS
-// -----------------------------------------------------------------------------------------------------------------------
-
-template<typename Value_type, typename Size_type>
-SigmaGenerator<Value_type, Size_type>::SigmaGenerator(value_type I,
-                                                      value_type ex,
-                                                      value_type ey,
-                                                      value_type ez,
-                                                      value_type E,
-                                                      value_type m,
-                                                      const Cyclotron* cycl,
-                                                      size_type N,
-                                                      size_type Nsectors,
-                                                      size_type truncOrder,
-                                                      bool write)
-    : I_m(I)
-    , wo_m(cycl->getRfFrequ(0)*1E6/cycl->getCyclHarm()*2.0*Physics::pi)
-    , E_m(E)
-    , gamma_m(E/m+1.0)
-    , gamma2_m(gamma_m*gamma_m)
-    , nh_m(cycl->getCyclHarm())
-    , beta_m(std::sqrt(1.0-1.0/gamma2_m))
-    , m_m(m)
-    , niterations_m(0)
-    , converged_m(false)
-    , Emin_m(cycl->getFMLowE())
-    , Emax_m(cycl->getFMHighE())
-    , N_m(N)
-    , nSectors_m(Nsectors)
-    , nStepsPerSector_m(N/cycl->getSymmetry())
-    , nSteps_m(N)
-    , error_m(std::numeric_limits<value_type>::max())
-    , truncOrder_m(truncOrder)
-    , write_m(write)
-    , sigmas_m(nStepsPerSector_m)
-    , rinit_m(0.0)
-    , prinit_m(0.0)
-{
-    // set emittances (initialization like that due to old compiler version)
-    // [ex] = [ey] = [ez] = pi*mm*mrad --> [emittance] = mm mrad
-    emittance_m[0] = ex * Physics::pi;
-    emittance_m[1] = ey * Physics::pi;
-    emittance_m[2] = ez * Physics::pi;
-
-    // minimum beta*gamma
-    value_type minGamma = Emin_m / m_m + 1.0;
-    value_type bgam = std::sqrt(minGamma * minGamma - 1.0);
-
-    // normalized emittance (--> multiply with beta*gamma)
-    // [emittance] = mm mrad
-    emittance_m[0] *= bgam;
-    emittance_m[1] *= bgam;
-    emittance_m[2] *= bgam;
-
-    // Define the Hamiltonian
-    Series::setGlobalTruncOrder(truncOrder_m);
-
-    // infinitesimal elements
-    x_m = Series::makeVariable(0);
-    px_m = Series::makeVariable(1);
-    y_m = Series::makeVariable(2);
-    py_m = Series::makeVariable(3);
-    z_m = Series::makeVariable(4);
-    delta_m = Series::makeVariable(5);
-
-    H_m = [&](value_type h, value_type kx, value_type ky) {
-        return 0.5*px_m*px_m + 0.5*kx*x_m*x_m - h*x_m*delta_m +
-               0.5*py_m*py_m + 0.5*ky*y_m*y_m +
-               0.5*delta_m*delta_m/gamma2_m;
-    };
-
-    Hsc_m = [&](value_type sigx, value_type sigy, value_type sigz) {
-        // convert m from MeV/c^2 to eV*s^{2}/m^{2}
-        value_type m = m_m * 1.0e6 / (Physics::c * Physics::c);
-
-        // formula (57)
-        value_type lam = Physics::two_pi*Physics::c / (wo_m * nh_m); // wavelength, [lam] = m
-        value_type K3 = 3.0 * /* physics::q0 */ 1.0 * I_m * lam / (20.0 * std::sqrt(5.0) * Physics::pi * Physics::epsilon_0 * m *
-                        Physics::c * Physics::c * Physics::c * beta_m * beta_m * gamma_m * gamma2_m);            // [K3] = m
-
-        value_type milli = 1.0e-3;
-
-        // formula (30), (31)
-        // [sigma(0,0)] = mm^{2} --> [sx] = [sy] = [sz] = mm
-        // multiply with 0.001 to get meter --> [sx] = [sy] = [sz] = m
-        value_type sx = std::sqrt(std::abs(sigx)) * milli;
-        value_type sy = std::sqrt(std::abs(sigy)) * milli;
-        value_type sz = std::sqrt(std::abs(sigz)) * milli;
-
-        value_type tmp = sx * sy;                                           // [tmp] = m^{2}
-
-        value_type f = std::sqrt(tmp) / (3.0 * gamma_m * sz);               // [f] = 1
-        value_type kxy = K3 * std::abs(1.0 - f) / ((sx + sy) * sz); // [kxy] = 1/m
-
-        value_type Kx = kxy / sx;
-        value_type Ky = kxy / sy;
-        value_type Kz = K3 * f / (tmp * sz);
-
-        return -0.5 * Kx * x_m * x_m
-               -0.5 * Ky * y_m * y_m
-               -0.5 * Kz * z_m * z_m * gamma2_m;
-     };
-}
-
-template<typename Value_type, typename Size_type>
-  bool SigmaGenerator<Value_type, Size_type>::match(value_type accuracy,
-                                                    size_type maxit,
-                                                    size_type maxitOrbit,
-                                                    Cyclotron* cycl,
-                                                    value_type denergy,
-                                                    value_type rguess,
-                                                    bool harmonic, bool full)
-{
-    /* compute the equilibrium orbit for energy E_
-     * and get the the following properties:
-     * - inverse bending radius h
-     * - step sizes of path ds
-     * - tune nuz
-     */
-
-    try {
-        if ( !full )
-            nSteps_m = nStepsPerSector_m;
-
-        // object for space charge map and cyclotron map
-        MapGenerator<value_type,
-                     size_type,
-                     Series,
-                     Map,
-                     Hamiltonian,
-                     SpaceCharge> mapgen(nSteps_m);
-
-        // compute cyclotron map and space charge map for each angle and store them into a vector
-        std::vector<matrix_type> Mcycs(nSteps_m), Mscs(nSteps_m);
-
-        container_type h(nSteps_m), r(nSteps_m), ds(nSteps_m), fidx(nSteps_m);
-        value_type ravg = 0.0, const_ds = 0.0;
-        std::pair<value_type,value_type> tunes;
-
-        if (!harmonic) {
-            ClosedOrbitFinder<value_type, size_type,
-                boost::numeric::odeint::runge_kutta4<container_type> > cof(m_m, N_m, cycl, false, nSectors_m);
-
-            if ( !cof.findOrbit(accuracy, maxitOrbit, E_m, denergy, rguess) ) {
-                throw OpalException("SigmaGenerator::match()",
-                                    "Closed orbit finder didn't converge.");
-            }
-
-            cof.computeOrbitProperties(E_m);
-
-            // properties of one turn
-            tunes = cof.getTunes();
-            ravg = cof.getAverageRadius();                   // average radius
-
-            value_type angle = cycl->getPHIinit();
-            container_type h_turn = cof.getInverseBendingRadius(angle);
-            container_type r_turn = cof.getOrbit(angle);
-            container_type ds_turn = cof.getPathLength(angle);
-            container_type fidx_turn = cof.getFieldIndex(angle);
-
-            container_type peo = cof.getMomentum(angle);
-
-            // write properties to file
-            if (write_m)
-                writeOrbitOutput_m(tunes, ravg, cof.getFrequencyError(),
-                                   r_turn, peo, h_turn, fidx_turn, ds_turn);
-
-            // write to terminal
-            *gmsg << "* ----------------------------" << endl
-                  << "* Closed orbit info:" << endl
-                  << "*" << endl
-                  << "* average radius: " << ravg << " [m]" << endl
-                  << "* initial radius: " << r_turn[0] << " [m]" << endl
-                  << "* initial momentum: " << peo[0] << " [Beta Gamma]" << endl
-                  << "* frequency error: " << cof.getFrequencyError()*100 <<" [ % ] "<< endl
-                  << "* horizontal tune: " << tunes.first << endl
-                  << "* vertical tune: " << tunes.second << endl
-                  << "* ----------------------------" << endl << endl;
-
-            // copy properties
-            std::copy_n(r_turn.begin(), nSteps_m, r.begin());
-            std::copy_n(h_turn.begin(), nSteps_m, h.begin());
-            std::copy_n(fidx_turn.begin(), nSteps_m, fidx.begin());
-            std::copy_n(ds_turn.begin(), nSteps_m, ds.begin());
-
-            rinit_m = r[0];
-            prinit_m = peo[0];
-
-        } else {
-            *gmsg << "Not yet supported." << endl;
-            return false;
-        }
-
-        // initialize sigma matrices (for each angle one) (first guess)
-        initialize(tunes.second,ravg);
+    void writeOrbitOutput_m(const container_t& r_turn,
+                            const container_t& peo,
+                            const container_t& h_turn,
+                            const container_t& fidx_turn,
+                            const container_t& ds_turn);
 
-        // for writing
-        std::ofstream writeMturn, writeMcyc, writeMsc;
+    void writeMatrix(std::ofstream&, const matrix_t&);
 
-        if (write_m) {
+    /// <b>RealDiracMatrix</b>-class member used for decoupling
+    RealDiracMatrix rdm_m;
 
-            std::string energy = float2string(E_m);
 
-            std::string fname = Util::combineFilePath({
-                OpalData::getInstance()->getAuxiliaryOutputDirectory(),
-                "OneTurnMapForEnergy" + energy + "MeV.dat"
-            });
+    template<class matrix>
+    void reduce(matrix&);
 
-            writeMturn.open(fname, std::ios::app);
-
-            fname = Util::combineFilePath({
-                OpalData::getInstance()->getAuxiliaryOutputDirectory(),
-                "SpaceChargeMapPerAngleForEnergy" + energy + "MeV.dat"
-            });
-
-            writeMsc.open(fname, std::ios::app);
-
-            fname = Util::combineFilePath({
-                OpalData::getInstance()->getAuxiliaryOutputDirectory(),
-                "CyclotronMapPerAngleForEnergy" + energy + "MeV.dat"
-            });
-
-            writeMcyc.open(fname, std::ios::app);
-
-            writeMturn << "--------------------------------" << std::endl;
-            writeMturn << "Iteration: 0 " << std::endl;
-            writeMturn << "--------------------------------" << std::endl;
-
-            writeMsc << "--------------------------------" << std::endl;
-            writeMsc << "Iteration: 0 " << std::endl;
-            writeMsc << "--------------------------------" << std::endl;
-        }
-
-        // calculate only for a single sector (a nSector_-th) of the whole cyclotron
-        for (size_type i = 0; i < nSteps_m; ++i) {
-            if (!harmonic) {
-                Mcycs[i] = mapgen.generateMap(H_m(h[i],
-                                                  h[i]*h[i]+fidx[i],
-                                                  -fidx[i]),
-                                              ds[i],truncOrder_m);
-
-                Mscs[i]  = mapgen.generateMap(Hsc_m(sigmas_m[i](0,0),
-                                                    sigmas_m[i](2,2),
-                                                    sigmas_m[i](4,4)),
-                                              ds[i],truncOrder_m);
-            } else {
-                Mscs[i] = mapgen.generateMap(Hsc_m(sigmas_m[i](0,0),
-                                                   sigmas_m[i](2,2),
-                                                   sigmas_m[i](4,4)),
-                                             const_ds,truncOrder_m);
-            }
-
-            if (write_m) {
-                writeMcyc << Mcycs[i] << std::endl;
-                writeMsc << Mscs[i] << std::endl;
-            }
-        }
-
-        // one turn matrix
-        mapgen.combine(Mscs,Mcycs);
-        matrix_type Mturn = mapgen.getMap();
-
-        if (write_m)
-            writeMturn << Mturn << std::endl;
-
-        // (inverse) transformation matrix
-        sparse_matrix_type R(6, 6), invR(6, 6);
-
-        // new initial sigma matrix
-        matrix_type newSigma(6,6);
-
-        // for exiting loop
-        bool stop = false;
-
-        value_type weight = 0.5;
-
-        while (error_m > accuracy && !stop) {
-            // decouple transfer matrix and compute (inverse) tranformation matrix
-            decouple(Mturn,R,invR);
-
-            // construct new initial sigma-matrix
-            newSigma = updateInitialSigma(Mturn, R, invR);
-
-            // compute new sigma matrices for all angles (except for initial sigma)
-            updateSigma(Mscs,Mcycs);
-
-            // compute error
-            error_m = L2ErrorNorm(sigmas_m[0],newSigma);
-
-            // write new initial sigma-matrix into vector
-            sigmas_m[0] = weight*newSigma + (1.0-weight)*sigmas_m[0];
-
-            if (write_m) {
-                writeMsc << "--------------------------------" << std::endl;
-                writeMsc << "Iteration: " << niterations_m + 1 << std::endl;
-                writeMsc << "--------------------------------" << std::endl;
-            }
-
-            // compute new space charge maps
-            for (size_type i = 0; i < nSteps_m; ++i) {
-                if (!harmonic) {
-                    Mscs[i] = mapgen.generateMap(Hsc_m(sigmas_m[i](0,0),
-                                                       sigmas_m[i](2,2),
-                                                       sigmas_m[i](4,4)),
-                                                 ds[i],truncOrder_m);
-                } else {
-                    Mscs[i] = mapgen.generateMap(Hsc_m(sigmas_m[i](0,0),
-                                                       sigmas_m[i](2,2),
-                                                       sigmas_m[i](4,4)),
-                                                 const_ds,truncOrder_m);
-                }
-
-                if (write_m)
-                    writeMsc << Mscs[i] << std::endl;
-            }
-
-            // construct new one turn transfer matrix M
-            mapgen.combine(Mscs,Mcycs);
-            Mturn = mapgen.getMap();
-
-            if (write_m) {
-                writeMturn << "--------------------------------" << std::endl;
-                writeMturn << "Iteration: " << niterations_m + 1 << std::endl;
-                writeMturn << "--------------------------------" << std::endl;
-                writeMturn << Mturn << std::endl;
-            }
-
-            // check if number of iterations has maxit exceeded.
-            stop = (niterations_m++ > maxit);
-        }
-
-        // store converged sigma-matrix
-        sigma_m.resize(6,6,false);
-        sigma_m.swap(newSigma);
-
-        // returns if the sigma matrix has converged
-        converged_m = error_m < accuracy;
-
-        // Close files
-        if (write_m) {
-            writeMturn.close();
-            writeMsc.close();
-            writeMcyc.close();
-        }
-
-    } catch(const std::exception& e) {
-        std::cerr << e.what() << std::endl;
-    }
-
-    if ( converged_m && write_m ) {
-        // write tunes
-        std::string fname = Util::combineFilePath({
-            OpalData::getInstance()->getAuxiliaryOutputDirectory(),
-            "MatchedDistributions.dat"
-        });
-
-        std::ofstream writeSigmaMatched(fname, std::ios::app);
-
-        std::array<double,3> emit = this->getEmittances();
-
-        writeSigmaMatched << "* Converged (Ex, Ey, Ez) = (" << emit[0]
-                          << ", " << emit[1] << ", " << emit[2]
-                          << ") pi mm mrad for E= " << E_m << " (MeV)"
-                          << std::endl << "* Sigma-Matrix " << std::endl;
-
-        for(unsigned int i = 0; i < sigma_m.size1(); ++ i) {
-            writeSigmaMatched << std::setprecision(4)  << std::setw(11)
-                              << sigma_m(i,0);
-            for(unsigned int j = 1; j < sigma_m.size2(); ++ j) {
-                writeSigmaMatched << " & " <<  std::setprecision(4)
-                                  << std::setw(11) << sigma_m(i,j);
-            }
-            writeSigmaMatched << " \\\\" << std::endl;
-        }
-
-        writeSigmaMatched << std::endl;
-        writeSigmaMatched.close();
-    }
-
-    return converged_m;
-}
-
-
-template<typename Value_type, typename Size_type>
-void SigmaGenerator<Value_type, Size_type>::eigsolve_m(const matrix_type& Mturn,
-                                                       sparse_matrix_type& R)
-{
-    typedef gsl_matrix*                   gsl_matrix_t;
-    typedef gsl_vector_complex*           gsl_cvector_t;
-    typedef gsl_matrix_complex*           gsl_cmatrix_t;
-    typedef gsl_eigen_nonsymmv_workspace* gsl_wspace_t;
-    typedef boost::numeric::ublas::vector<complex_t> complex_vector_type;
-
-    gsl_cvector_t evals = gsl_vector_complex_alloc(6);
-    gsl_cmatrix_t evecs = gsl_matrix_complex_alloc(6, 6);
-    gsl_wspace_t wspace = gsl_eigen_nonsymmv_alloc(6);
-    gsl_matrix_t      M = gsl_matrix_alloc(6, 6);
-
-    // go to GSL
-    for (size_type i = 0; i < 6; ++i){
-        for (size_type j = 0; j < 6; ++j) {
-            gsl_matrix_set(M, i, j, Mturn(i,j));
-        }
-    }
-
-    /*int status = */gsl_eigen_nonsymmv(M, evals, evecs, wspace);
-
-//     if ( !status )
-//         throw OpalException("SigmaGenerator::eigsolve_m()",
-//                             "Couldn't perform eigendecomposition!");
-
-    /*status = *///gsl_eigen_nonsymmv_sort(evals, evecs, GSL_EIGEN_SORT_ABS_ASC);
-
-//     if ( !status )
-//         throw OpalException("SigmaGenerator::eigsolve_m()",
-//                             "Couldn't sort eigenvalues and eigenvectors!");
-
-    // go to UBLAS
-    for( size_type i = 0; i < 6; i++){
-        gsl_vector_complex_view evec_i = gsl_matrix_complex_column(evecs, i);
-
-        for(size_type j = 0;j < 6; j++){
-            gsl_complex zgsl = gsl_vector_complex_get(&evec_i.vector, j);
-            complex_t z(GSL_REAL(zgsl), GSL_IMAG(zgsl));
-            R(i,j) = z;
-        }
-    }
-
-    // Sorting the Eigenvectors
-    // This is an arbitrary threshold that has worked for me. (We should fix this)
-    value_type threshold = 10e-12;
-    bool isZdirection = false;
-    std::vector<complex_vector_type> zVectors{};
-    std::vector<complex_vector_type> xyVectors{};
-
-    for(size_type i = 0; i < 6; i++){
-        complex_t z = R(i,0);
-        if(std::abs(z) < threshold) z = 0.;
-        if(z == 0.) isZdirection = true;
-        complex_vector_type v(6);
-        if(isZdirection){
-            for(size_type j = 0;j < 6; j++){
-                complex_t z = R(i,j);
-                v(j) = z;
-            }
-            zVectors.push_back(v);
-        }
-        else{
-            for(size_type j = 0; j < 6; j++){
-                complex_t z = R(i,j);
-                v(j) = z;
-            }
-            xyVectors.push_back(v);
-        }
-        isZdirection = false;
-    }
-
-    //if z-direction not found, then the system does not behave as expected
-    if(zVectors.size() != 2)
-        throw OpalException("SigmaGenerator::eigsolve_m()",
-                            "Couldn't find the vertical Eigenvectors.");
-
-    // Norms the Eigenvectors
-     for(size_type i = 0; i < 4; i++){
-        value_type norm{0};
-        for(size_type j = 0; j < 6; j++) norm += std::norm(xyVectors[i](j));
-        for(size_type j = 0; j < 6; j++) xyVectors[i](j) /= std::sqrt(norm);
-    }
-    for(size_type i = 0; i < 2; i++){
-        value_type norm{0};
-        for(size_type j = 0; j < 6; j++) norm += std::norm(zVectors[i](j));
-        for(size_type j = 0; j < 6; j++) zVectors[i](j) /= std::sqrt(norm);
-    }
-
-    for(value_type i = 0; i < 6; i++){
-        R(i,0) = xyVectors[0](i);
-        R(i,1) = xyVectors[1](i);
-        R(i,2) = zVectors[0](i);
-        R(i,3) = zVectors[1](i);
-        R(i,4) = xyVectors[2](i);
-        R(i,5) = xyVectors[3](i);
-    }
-
-    gsl_vector_complex_free(evals);
-    gsl_matrix_complex_free(evecs);
-    gsl_eigen_nonsymmv_free(wspace);
-    gsl_matrix_free(M);
-}
+    template<class matrix>
+    void expand(matrix&);
+};
 
 
-template<typename Value_type, typename Size_type>
-bool SigmaGenerator<Value_type, Size_type>::invertMatrix_m(const sparse_matrix_type& R,
-                                                           sparse_matrix_type& invR)
+inline
+typename SigmaGenerator::matrix_t&
+SigmaGenerator::getSigma()
 {
-    typedef boost::numeric::ublas::permutation_matrix<size_type> pmatrix_t;
-
-    //creates a working copy of R
-    cmatrix_type A(R);
-
-    //permutation matrix for the LU-factorization
-    pmatrix_t pm(A.size1());
-
-    //LU-factorization
-    int res = lu_factorize(A,pm);
-
-    if( res != 0)
-        return false;
-
-    // create identity matrix of invR
-    invR.assign(boost::numeric::ublas::identity_matrix<complex_t>(A.size1()));
-
-    // backsubstitute to get the inverse
-    boost::numeric::ublas::lu_substitute(A, pm, invR);
-
-    return true;
+    return sigma_m;
 }
 
 
-template<typename Value_type, typename Size_type>
-void SigmaGenerator<Value_type, Size_type>::decouple(const matrix_type& Mturn,
-                                                     sparse_matrix_type& R,
-                                                     sparse_matrix_type& invR)
+inline
+unsigned int SigmaGenerator::getIterations() const
 {
-    this->eigsolve_m(Mturn, R);
-
-    if ( !this->invertMatrix_m(R, invR) )
-        throw OpalException("SigmaGenerator::decouple()",
-                            "Couldn't compute inverse matrix!");
+    return (converged_m) ? niterations_m : 0;
 }
 
-template<typename Value_type, typename Size_type>
-typename SigmaGenerator<Value_type, Size_type>::value_type
-SigmaGenerator<Value_type, Size_type>::isEigenEllipse(const matrix_type& Mturn,
-                                                      const matrix_type& sigma)
-{
-    // compute sigma matrix after one turn
-    matrix_type newSigma = matt_boost::gemmm<matrix_type>(Mturn,
-                                                          sigma,
-                                                          boost::numeric::ublas::trans(Mturn));
-
-    // return L2 error
-    return L2ErrorNorm(sigma,newSigma);
-}
 
-template<typename Value_type, typename Size_type>
-inline typename SigmaGenerator<Value_type, Size_type>::matrix_type&
-SigmaGenerator<Value_type, Size_type>::getSigma()
-{
-    return sigma_m;
-}
-
-template<typename Value_type, typename Size_type>
-inline typename SigmaGenerator<Value_type, Size_type>::size_type
-SigmaGenerator<Value_type, Size_type>::getIterations() const
-{
-    return (converged_m) ? niterations_m : size_type(0);
-}
-
-template<typename Value_type, typename Size_type>
-inline typename SigmaGenerator<Value_type, Size_type>::value_type
-SigmaGenerator<Value_type, Size_type>::getError() const
+inline
+double SigmaGenerator::getError() const
 {
     return error_m;
 }
 
-template<typename Value_type, typename Size_type>
-inline std::array<Value_type,3>
-SigmaGenerator<Value_type, Size_type>::getEmittances() const
+
+inline
+std::array<double,3> SigmaGenerator::getEmittances() const
 {
-    value_type bgam = gamma_m*beta_m;
-    return std::array<value_type,3>{{
-        emittance_m[0]/Physics::pi/bgam,
-        emittance_m[1]/Physics::pi/bgam,
-        emittance_m[2]/Physics::pi/bgam
+    double bgam = gamma_m*beta_m;
+    return std::array<double,3>{{
+        emittance_m[0] / Physics::pi / bgam * 1.0e6,
+        emittance_m[1] / Physics::pi / bgam * 1.0e6,
+        emittance_m[2] / Physics::pi / bgam * 1.0e6
     }};
 }
 
-// -----------------------------------------------------------------------------------------------------------------------
-// PRIVATE MEMBER FUNCTIONS
-// -----------------------------------------------------------------------------------------------------------------------
 
-template<typename Value_type, typename Size_type>
-void SigmaGenerator<Value_type, Size_type>::initialize(value_type nuz,
-                                                       value_type ravg)
-{
-    /*
-     * The initialization is based on the analytical solution of
-     * using a spherical symmetric beam in the paper:
-     * Transverse-longitudinal coupling by space charge in cyclotrons
-     * by Dr. Christian Baumgarten
-     * (formulas: (46), (56), (57))
-     */
-
-
-    /* Units:
-     * ----------------------------------------------
-     * [wo] = 1/s
-     * [nh] = 1
-     * [q0] = e
-     * [I] = A
-     * [eps0] = (A*s)^{2}/(N*m^{2})
-     * [E0] = MeV/(c^{2}) (with speed of light c)
-     * [beta] = 1
-     * [gamma] = 1
-     * [m] = kg
+template<class matrix>
+void SigmaGenerator::reduce(matrix& M) {
+    /* The 6x6 matrix gets reduced to a 4x4 matrix in the following way:
      *
-     * [lam] = m
-     * [K3] = m
-     * [alpha] = 10^{3}/(pi*mrad)
-     * ----------------------------------------------
-     */
-
-    // helper constants
-    value_type invbg = 1.0 / (beta_m * gamma_m);
-    value_type micro = 1.0e-6;
-    value_type mega = 1.0e6;
-    //value_type kilo = 1.0e3;
-
-    // convert mass m_m from MeV/c^2 to eV*s^{2}/m^{2}
-    value_type m = m_m * mega/(Physics::c * Physics::c);        // [m] = eV*s^{2}/m^{2}, [m_m] = MeV/c^2
-
-    /* Emittance [ex] = [ey] = [ez] = mm mrad (emittance_m are normalized emittances
-     * (i.e. emittance multiplied with beta*gamma)
+     * a11 a12 a13 a14 a15 a16
+     * a21 a22 a23 a24 a25 a26          a11 a12 a15 a16
+     * a31 a32 a33 a34 a35 a36  -->     a21 a22 a25 a26
+     * a41 a42 a43 a44 a45 a46          a51 a52 a55 a56
+     * a51 a52 a53 a54 a55 a56          a61 a62 a65 a66
+     * a61 a62 a63 a64 a65 a66
      */
-    value_type ex = emittance_m[0] * invbg;                        // [ex] = mm mrad
-    value_type ey = emittance_m[1] * invbg;                        // [ey] = mm mrad
-    value_type ez = emittance_m[2] * invbg;                        // [ez] = mm mrad
 
-    // convert normalized emittances: mm mrad --> m rad (mm mrad: millimeter milliradian)
-    ex *= micro;
-    ey *= micro;
-    ez *= micro;
-
-    // initial guess of emittance, [e] = m rad
-    value_type e = std::cbrt(ex * ey * ez);             // cbrt computes cubic root (C++11) <cmath>
-
-    // cyclotron radius [rcyc] = m
-    value_type rcyc = ravg / beta_m;
-
-    // "average" inverse bending radius
-    value_type h = 1.0 / ravg;            // [h] = 1/m
-
-    // formula (57)
-    value_type lam = Physics::two_pi * Physics::c / (wo_m * nh_m); // wavelength, [lam] = m
-    value_type K3 = 3.0 * /* physics::q0 */ 1.0 * I_m * lam / (20.0 * std::sqrt(5.0) * Physics::pi * Physics::epsilon_0 * m *
-                    Physics::c * Physics::c * Physics::c * beta_m * beta_m * gamma2_m * gamma_m);               // [K3] = m
-
-    value_type alpha = /* physics::q0 */ 1.0 * Physics::mu_0 * I_m / (5.0 * std::sqrt(10.0) * m * Physics::c *
-                       gamma_m * nh_m) * std::sqrt(rcyc * rcyc * rcyc / (e * e * e));                           // [alpha] = 1/rad --> [alpha] = 1
-
-    value_type sig0 = std::sqrt(2.0 * rcyc * e) / gamma_m;                                                      // [sig0] = m*sqrt(rad) --> [sig0] = m
-
-    // formula (56)
-    value_type sig;                                     // [sig] = m
-    if (alpha >= 2.5) {
-        sig = sig0 * std::cbrt(1.0 + alpha);            // cbrt computes cubic root (C++11) <cmath>
-    } else if (alpha >= 0) {
-        sig = sig0 * (1 + alpha * (0.25 - 0.03125 * alpha));
-    } else {
-        throw OpalException("SigmaGenerator::initialize()",
-                            "Negative alpha value: " + std::to_string(alpha) + " < 0");
+    // copy x- and l-direction to a 4x4 matrix_t
+    matrix_t M4x4(4,4);
+    for (unsigned int i = 0; i < 2; ++i) {
+        // upper left 2x2 [a11,a12;a21,a22]
+        M4x4(i,0) = M(i,0);
+        M4x4(i,1) = M(i,1);
+        // lower left 2x2 [a51,a52;a61,a62]
+        M4x4(i + 2,0) = M(i + 4,0);
+        M4x4(i + 2,1) = M(i + 4,1);
+        // upper right 2x2 [a15,a16;a25,a26]
+        M4x4(i,2) = M(i,4);
+        M4x4(i,3) = M(i,5);
+        // lower right 2x2 [a55,a56;a65,a66]
+        M4x4(i + 2,2) = M(i + 4,4);
+        M4x4(i + 2,3) = M(i + 4,5);
     }
 
-    // K = Kx = Ky = Kz
-    value_type K = K3 * gamma_m / (3.0 * sig * sig * sig);   // formula (46), [K] = 1/m^{2}
-    value_type kx = h * h * gamma2_m;                        // formula (46) (assumption of an isochronous cyclotron), [kx] = 1/m^{2}
-
-    value_type a = 0.5 * kx - K;    // formula (22) (with K = Kx = Kz), [a] = 1/m^{2}
-    value_type b = K * K;           // formula (22) (with K = Kx = Kz and kx = h^2*gamma^2), [b] = 1/m^{4}
-
-
-    // b must be positive, otherwise no real-valued frequency
-    if (b < 0)
-        throw OpalException("SigmaGenerator::initialize()",
-                            "Negative value --> No real-valued frequency.");
-
-    value_type tmp = a * a - b;           // [tmp] = 1/m^{4}
-    if (tmp < 0)
-        throw OpalException("SigmaGenerator::initialize()",
-                            "Square root of negative number.");
-
-    tmp = std::sqrt(tmp);               // [tmp] = 1/m^{2}
-
-    if (a < tmp)
-        throw OpalException("Error in SigmaGenerator::initialize()",
-                            "Square root of negative number.");
-
-    if (h * h * nuz * nuz <= K)
-        throw OpalException("SigmaGenerator::initialize()",
-                            "h^{2} * nu_{z}^{2} <= K (i.e. square root of negative number)");
-
-    value_type Omega = std::sqrt(a + tmp);                // formula (22), [Omega] = 1/m
-    value_type omega = std::sqrt(a - tmp);                // formula (22), [omega] = 1/m
-
-    value_type A = h / (Omega * Omega + K);           // formula (26), [A] = m
-    value_type B = h / (omega * omega + K);           // formula (26), [B] = m
-    value_type invAB = 1.0 / (B - A);                 // [invAB] = 1/m
-
-    // construct initial sigma-matrix (formula (29, 30, 31)
-    // Remark: We multiply with 10^{6} (= mega) to convert emittances back.
-    // 1 m^{2} = 10^{6} mm^{2}
-    matrix_type sigma = boost::numeric::ublas::zero_matrix<value_type>(6);
-
-    // formula (30), [sigma(0,0)] = m^2 rad * 10^{6} = mm^{2} rad = mm mrad
-    sigma(0,0) = invAB * (B * ex / Omega + A * ez / omega) * mega;
-
-    // [sigma(0,5)] = [sigma(5,0)] = m rad * 10^{6} = mm mrad	// 1000: m --> mm and 1000 to promille
-    sigma(0,5) = sigma(5,0) = invAB * (ex / Omega + ez / omega) * mega;
-
-    // [sigma(1,1)] = rad * 10^{6} = mrad (and promille)
-    sigma(1,1) = invAB * (B * ex * Omega + A * ez * omega) * mega;
-
-    // [sigma(1,4)] = [sigma(4,1)] = m rad * 10^{6} = mm mrad
-    sigma(1,4) = sigma(4,1) = invAB * (ex * Omega+ez * omega) / (K * gamma2_m) * mega;
-
-    // formula (31), [sigma(2,2)] = m rad * 10^{6} = mm mrad
-    sigma(2,2) = ey / (std::sqrt(h * h * nuz * nuz - K)) * mega;
-
-    sigma(3,3) = (ey * mega) * (ey * mega) / sigma(2,2);
-
-    // [sigma(4,4)] = m^{2} rad * 10^{6} = mm^{2} rad = mm mrad
-    sigma(4,4) = invAB * (A * ex * Omega + B * ez * omega) / (K * gamma2_m) * mega;
-
-    // formula (30), [sigma(5,5)] = rad * 10^{6} = mrad (and promille)
-    sigma(5,5) = invAB * (ex / (B * Omega) + ez / (A * omega)) * mega;
-
-    // fill in initial guess of the sigma matrix (for each angle the same guess)
-    sigmas_m.resize(nSteps_m);
-    for (typename std::vector<matrix_type>::iterator it = sigmas_m.begin(); it != sigmas_m.end(); ++it) {
-        *it = sigma;
-    }
-
-    if (write_m) {
-        std::string energy = float2string(E_m);
-
-        std::string fname = Util::combineFilePath({
-            OpalData::getInstance()->getAuxiliaryOutputDirectory(),
-            "maps",
-            "InitialSigmaPerAngleForEnergy" + energy + "MeV.dat"
-        });
-
-        std::ofstream writeInit(fname, std::ios::app);
-        writeInit << sigma << std::endl;
-        writeInit.close();
-    }
+    M.resize(4,4,false);
+    M.swap(M4x4);
 }
 
-
-template<typename Value_type, typename Size_type>
-typename SigmaGenerator<Value_type, Size_type>::matrix_type
-SigmaGenerator<Value_type, Size_type>::updateInitialSigma(const matrix_type& /*M*/,
-                                                          sparse_matrix_type& R,
-                                                          sparse_matrix_type& invR)
-{
-    /*
-     * Function input:
-     * - M: one turn transfer matrix
-     * - R: transformation matrix (in paper: E)
-     * - invR: inverse transformation matrix (in paper: E^{-1}
-     */
-
-    /* formula (18):
-     * sigma = -E*D*E^{-1}*S
-     * with diagonal matrix D (stores eigenvalues of sigma*S (emittances apart from +- i),
-     * skew-symmetric matrix (formula (13)), and tranformation matrices E, E^{-1}
+template<class matrix>
+void SigmaGenerator::expand(matrix& M) {
+    /* The 4x4 matrix gets expanded to a 6x6 matrix in the following way:
+     *
+     *                          a11 a12 0 0 a13 a14
+     * a11 a12 a13 a14          a21 a22 0 0 a23 a24
+     * a21 a22 a23 a24  -->     0   0   1 0 0   0
+     * a31 a32 a33 a34          0   0   0 1 0   0
+     * a41 a42 a43 a44          a31 a32 0 0 a33 a34
+     *                          a41 a42 0 0 a43 a44
      */
 
-    cmatrix_type S = boost::numeric::ublas::zero_matrix<complex_t>(6,6);
-    S(0,1) = S(2,3) = S(4,5) = 1;
-    S(1,0) = S(3,2) = S(5,4) = -1;
-
-    // Build new D-Matrix
-    // Section 2.4 Eq. 18 in M. Frey Semester thesis
-    // D = diag(i*emx,-i*emx,i*emy,-i*emy,i*emz, -i*emz)
-
-
-    cmatrix_type D = boost::numeric::ublas::zero_matrix<complex_t>(6,6);
-    value_type invbg = 1.0 / (beta_m * gamma_m);
-    complex_t im(0,1);
-    for(size_type i = 0; i < 3; ++i){
-        D(2*i, 2*i) = emittance_m[i] * invbg * im;
-        D(2*i+1, 2*i+1) = -emittance_m[i] * invbg * im;
-    }
-
-    // Computing of new Sigma
-    // sigma = -R*D*R^{-1}*S
-    cmatrix_type csigma(6, 6);
-    csigma = boost::numeric::ublas::prod(invR, S);
-    csigma = boost::numeric::ublas::prod(D, csigma);
-    csigma = boost::numeric::ublas::prod(-R, csigma);
-
-    matrix_type sigma(6,6);
-    for (size_type i = 0; i < 6; ++i){
-        for (size_type j = 0; j < 6; ++j){
-            sigma(i,j) = csigma(i,j).real();
-        }
-    }
-
-    for (size_type i = 0; i < 6; ++i) {
-        if(sigma(i,i) < 0.)
-            sigma(i,i) *= -1.0;
-    }
-
-    if (write_m) {
-        std::string energy = float2string(E_m);
-
-        std::string fname = Util::combineFilePath({
-            OpalData::getInstance()->getAuxiliaryOutputDirectory(),
-            "maps",
-            "SigmaPerAngleForEnergy" + energy + "MeV.dat"
-        });
-
-        std::ofstream writeSigma(fname, std::ios::app);
-
-        writeSigma << "--------------------------------" << std::endl;
-        writeSigma << "Iteration: " << niterations_m + 1 << std::endl;
-        writeSigma << "--------------------------------" << std::endl;
-
-        writeSigma << sigma << std::endl;
-        writeSigma.close();
-    }
-
-    return sigma;
-}
-
-template<typename Value_type, typename Size_type>
-void SigmaGenerator<Value_type, Size_type>::updateSigma(const std::vector<matrix_type>& Mscs,
-                                                        const std::vector<matrix_type>& Mcycs)
-{
-    matrix_type M = boost::numeric::ublas::matrix<value_type>(6,6);
-
-    std::ofstream writeSigma;
-
-    if (write_m) {
-        std::string energy = float2string(E_m);
-
-        std::string fname = Util::combineFilePath({
-            OpalData::getInstance()->getAuxiliaryOutputDirectory(),
-            "maps",
-            "SigmaPerAngleForEnergy" + energy + "MeV.dat"
-        });
-
-        writeSigma.open(fname,std::ios::app);
-    }
-
-    // initial sigma is already computed
-    for (size_type i = 1; i < nSteps_m; ++i) {
-        // transfer matrix for one angle
-        M = boost::numeric::ublas::prod(Mscs[i - 1],Mcycs[i - 1]);
-        // transfer the matrix sigma
-        sigmas_m[i] = matt_boost::gemmm<matrix_type>(M,sigmas_m[i - 1],
-                                                     boost::numeric::ublas::trans(M));
-
-        if (write_m)
-            writeSigma << sigmas_m[i] << std::endl;
-    }
-
-    if (write_m) {
-        writeSigma << std::endl;
-        writeSigma.close();
-    }
-}
-
-template<typename Value_type, typename Size_type>
-typename SigmaGenerator<Value_type, Size_type>::value_type
-SigmaGenerator<Value_type, Size_type>::L2ErrorNorm(const matrix_type& oldS,
-                                                   const matrix_type& newS)
-{
-    // compute difference
-    matrix_type diff = newS - oldS;
-
-    // sum squared error up and take square root
-    return std::sqrt(std::inner_product(diff.data().begin(),
-                                        diff.data().end(),
-                                        diff.data().begin(),
-                                        0.0));
-}
-
-template<typename Value_type, typename Size_type>
-typename SigmaGenerator<Value_type, Size_type>::value_type
-    SigmaGenerator<Value_type, Size_type>::L1ErrorNorm(const matrix_type& oldS,
-                                                       const matrix_type& newS)
-{
-    // compute difference
-    matrix_type diff = newS - oldS;
-
-    std::for_each(diff.data().begin(), diff.data().end(),
-                  [](value_type& val) {
-                      return std::abs(val);
-                  });
-
-    // sum squared error up and take square root
-    return std::accumulate(diff.data().begin(), diff.data().end(), 0.0);
-}
-
-
-template<typename Value_type, typename Size_type>
-std::string SigmaGenerator<Value_type, Size_type>::float2string(value_type val) {
-    std::ostringstream out;
-    out << std::setprecision(6) << val;
-    return out.str();
-}
-
-
-template<typename Value_type, typename Size_type>
-void SigmaGenerator<Value_type, Size_type>::writeOrbitOutput_m(
-    const std::pair<value_type,value_type>& tunes,
-    const value_type& ravg,
-    const value_type& freqError,
-    const container_type& r_turn,
-    const container_type& peo,
-    const container_type& h_turn,
-    const container_type&  fidx_turn,
-    const container_type&  ds_turn)
-{
-    std::string fname = Util::combineFilePath({
-            OpalData::getInstance()->getAuxiliaryOutputDirectory(),
-            "Tunes.dat"
-    });
-
-    // write tunes
-    std::ofstream writeTunes(fname, std::ios::app);
-
-    if(writeTunes.tellp() == 0) // if nothing yet written --> write description
-        writeTunes << "energy [MeV]" << std::setw(15)
-                   << "nur" << std::setw(25)
-                   << "nuz" << std::endl;
-
-    writeTunes << E_m << std::setw(30) << std::setprecision(10)
-               << tunes.first << std::setw(25)
-               << tunes.second << std::endl;
-
-    // write average radius
-    fname = Util::combineFilePath({
-            OpalData::getInstance()->getAuxiliaryOutputDirectory(),
-            "AverageValues.dat"
-    });
-    std::ofstream writeAvgRadius(fname, std::ios::app);
-
-    if (writeAvgRadius.tellp() == 0) // if nothing yet written --> write description
-        writeAvgRadius << "energy [MeV]" << std::setw(15)
-                       << "avg. radius [m]" << std::setw(15)
-                       << "r [m]" << std::setw(15)
-                       << "pr [m]" << std::endl;
-
-    writeAvgRadius << E_m << std::setw(25) << std::setprecision(10)
-                   << ravg << std::setw(25) << std::setprecision(10)
-                   << r_turn[0] << std::setw(25) << std::setprecision(10)
-                   << peo[0] << std::endl;
-
-    // write frequency error
-    fname = Util::combineFilePath({
-            OpalData::getInstance()->getAuxiliaryOutputDirectory(),
-            "FrequencyError.dat"
-    });
-    std::ofstream writePhase(fname, std::ios::app);
-
-    if(writePhase.tellp() == 0) // if nothing yet written --> write description
-        writePhase << "energy [MeV]" << std::setw(15)
-                   << "freq. error" << std::endl;
-
-    writePhase << E_m << std::setw(30) << std::setprecision(10)
-               << freqError << std::endl;
-
-    // write other properties
-    std::string energy = float2string(E_m);
-    fname = Util::combineFilePath({
-            OpalData::getInstance()->getAuxiliaryOutputDirectory(),
-            "PropertiesForEnergy" + energy + "MeV.dat"
-    });
-    std::ofstream writeProperties(fname, std::ios::out);
-    writeProperties << std::left
-                    << std::setw(25) << "orbit radius"
-                    << std::setw(25) << "orbit momentum"
-                    << std::setw(25) << "inverse bending radius"
-                    << std::setw(25) << "field index"
-                    << std::setw(25) << "path length" << std::endl;
-
-    for (size_type i = 0; i < r_turn.size(); ++i) {
-        writeProperties << std::setprecision(10) << std::left
-                        << std::setw(25) << r_turn[i]
-                        << std::setw(25) << peo[i]
-                        << std::setw(25) << h_turn[i]
-                        << std::setw(25) << fidx_turn[i]
-                        << std::setw(25) << ds_turn[i] << std::endl;
+    matrix M6x6 = boost::numeric::ublas::identity_matrix<double>(6,6);
+
+    for (unsigned int i = 0; i < 2; ++i) {
+        // upper left 2x2 [a11,a12;a21,a22]
+        M6x6(i,0) = M(i,0);
+        M6x6(i,1) = M(i,1);
+        // lower left 2x2 [a31,a32;a41,a42]
+        M6x6(i + 4,0) = M(i + 2,0);
+        M6x6(i + 4,1) = M(i + 2,1);
+        // upper right 2x2 [a13,a14;a23,a24]
+        M6x6(i,4) = M(i,2);
+        M6x6(i,5) = M(i,3);
+        // lower right 2x2 [a22,a34;a43,a44]
+        M6x6(i + 4,4) = M(i + 2,2);
+        M6x6(i + 4,5) = M(i + 2,3);
     }
 
-    // close all files within this if-statement
-    writeTunes.close();
-    writeAvgRadius.close();
-    writePhase.close();
-    writeProperties.close();
+    // exchange
+    M.swap(M6x6);
 }
 
 #endif
\ No newline at end of file
diff --git a/src/Distribution/matrix_vector_operation.h b/src/Distribution/matrix_vector_operation.h
index 12be154050052d80faf5723c0d2b62d962504172..45f7aec46777eca24dfe27d1f69e2165a0e3f186 100644
--- a/src/Distribution/matrix_vector_operation.h
+++ b/src/Distribution/matrix_vector_operation.h
@@ -55,10 +55,10 @@ namespace matt_boost {
         }
 
     /// Computes the cross product \f$ v_{1}\times v_{2}\f$ of two vectors in \f$ \mathbb{R}^{3} \f$
-    template<class V>
+    template<class V, class A>
         BOOST_UBLAS_INLINE
-        ublas::vector<V> cross_prod(ublas::vector<V>& v1, ublas::vector<V>& v2) {
-            ublas::vector<V> v(v1.size());
+        ublas::vector<V, A> cross_prod(ublas::vector<V, A>& v1, ublas::vector<V, A>& v2) {
+            ublas::vector<V, A> v(v1.size());
             if (v1.size() == v2.size() && v1.size() == 3) {
                 v(0) = v1(1) * v2(2) - v1(2) * v2(1);
                 v(1) = v1(2) * v2(0) - v1(0) * v2(2);
diff --git a/src/Elements/OpalElement.cpp b/src/Elements/OpalElement.cpp
index 63bf8b771bf9200523104f97bbb7684f2ca46d29..684fc18f62354414ef97a6bbe5bb05471f603e96 100644
--- a/src/Elements/OpalElement.cpp
+++ b/src/Elements/OpalElement.cpp
@@ -568,7 +568,8 @@ void OpalElement::update() {
             rotation = rotTheta * (rotPhi * rotPsi);
         } else {
             if (itsAttr[ORIENTATION]) {
-                throw OpalException("Line::parse","Parameter orientation is array of 3 values (theta, phi, psi);\n" +
+                throw OpalException("OpalElement::update",
+                                    "Parameter orientation is array of 3 values (theta, phi, psi);\n" +
                                     std::to_string(dir.size()) + " values provided");
             }
         }
@@ -577,7 +578,8 @@ void OpalElement::update() {
             origin = Vector_t(ori[0], ori[1], ori[2]);
         } else {
             if (itsAttr[ORIGIN]) {
-                throw OpalException("Line::parse","Parameter origin is array of 3 values (x, y, z);\n" +
+                throw OpalException("OpalElement::update",
+                                    "Parameter origin is array of 3 values (x, y, z);\n" +
                                     std::to_string(ori.size()) + " values provided");
             }
         }
diff --git a/src/Elements/OpalRBend.cpp b/src/Elements/OpalRBend.cpp
index 1c4e61760f22033d4696e30bbfefb4e1b9c82f52..90fecb7d5b24992b880454af097b2ed594edac5d 100644
--- a/src/Elements/OpalRBend.cpp
+++ b/src/Elements/OpalRBend.cpp
@@ -24,7 +24,6 @@
 #include "Structure/OpalWake.h"
 #include "Structure/ParticleMatterInteraction.h"
 #include "Utilities/OpalException.h"
-#include <cmath>
 
 OpalRBend::OpalRBend():
     OpalBend("RBEND",
@@ -47,10 +46,8 @@ OpalRBend::OpalRBend(const std::string &name, OpalRBend *parent):
 
 
 OpalRBend::~OpalRBend() {
-    if(owk_m)
-        delete owk_m;
-    if(parmatint_m)
-        delete parmatint_m;
+    delete owk_m;
+    delete parmatint_m;
 }
 
 
@@ -131,7 +128,7 @@ void OpalRBend::update() {
     double k0s = itsAttr[K0S] ? Attributes::getReal(itsAttr[K0S]) : 0.0;
     //JMJ 4/10/2000: above line replaced
     //    length ? angle / length : angle;
-    // to avoid closed orbit created by RBEND with defalt K0.
+    // to avoid closed orbit created by RBEND with default K0.
     field.setNormalComponent(1, factor * k0);
     field.setSkewComponent(1, factor * Attributes::getReal(itsAttr[K0S]));
     field.setNormalComponent(2, factor * Attributes::getReal(itsAttr[K1]));
@@ -177,8 +174,11 @@ void OpalRBend::update() {
     }
 
     // Energy in eV.
-    if(itsAttr[DESIGNENERGY]) {
+    if(itsAttr[DESIGNENERGY] && Attributes::getReal(itsAttr[DESIGNENERGY]) != 0.0) {
         bend->setDesignEnergy(Attributes::getReal(itsAttr[DESIGNENERGY]), false);
+    } else if (bend->getName() != "RBEND") {
+        throw OpalException("OpalRBend::update",
+                            "RBend requires non-zero DESIGNENERGY");
     }
 
     bend->setFullGap(Attributes::getReal(itsAttr[GAP]));
diff --git a/src/Elements/OpalRBend3D.cpp b/src/Elements/OpalRBend3D.cpp
index f64500bfddc8945e4520fc85fbce6fe40bdd9ad1..e1175d63fcd64bce123bf92242e2bd0dfbb925b3 100644
--- a/src/Elements/OpalRBend3D.cpp
+++ b/src/Elements/OpalRBend3D.cpp
@@ -72,10 +72,8 @@ OpalRBend3D::OpalRBend3D(const std::string &name, OpalRBend3D *parent):
 }
 
 OpalRBend3D::~OpalRBend3D() {
-    if(owk_m)
-        delete owk_m;
-    if(parmatint_m)
-        delete parmatint_m;
+    delete owk_m;
+    delete parmatint_m;
 }
 
 OpalRBend3D *OpalRBend3D::clone(const std::string &name) {
@@ -130,8 +128,11 @@ void OpalRBend3D::update() {
     bend->setEntranceAngle(e1);
 
     // Energy in eV.
-    if(itsAttr[DESIGNENERGY]) {
+    if(itsAttr[DESIGNENERGY] && Attributes::getReal(itsAttr[DESIGNENERGY]) != 0.0) {
         bend->setDesignEnergy(Attributes::getReal(itsAttr[DESIGNENERGY]), false);
+    } else if (bend->getName() != "RBEND3D") {
+        throw OpalException("OpalRBend3D::update",
+                            "RBend3D requires non-zero DESIGNENERGY");
     }
 
     bend->setFullGap(Attributes::getReal(itsAttr[GAP]));
diff --git a/src/Elements/OpalSBend.cpp b/src/Elements/OpalSBend.cpp
index 785c1bf3d17951b734bc898323be340f58d297e2..2ec776a807aa5568bbd0e2005e294229f88bbb4a 100644
--- a/src/Elements/OpalSBend.cpp
+++ b/src/Elements/OpalSBend.cpp
@@ -24,7 +24,6 @@
 #include "Structure/OpalWake.h"
 #include "Structure/ParticleMatterInteraction.h"
 #include "Utilities/OpalException.h"
-#include <cmath>
 
 OpalSBend::OpalSBend():
     OpalBend("SBEND",
@@ -47,10 +46,8 @@ OpalSBend::OpalSBend(const std::string &name, OpalSBend *parent):
 
 
 OpalSBend::~OpalSBend() {
-    if(owk_m)
-        delete owk_m;
-    if(parmatint_m)
-        delete parmatint_m;
+    delete owk_m;
+    delete parmatint_m;
 }
 
 
@@ -131,7 +128,7 @@ void OpalSBend::update() {
     double k0s = itsAttr[K0S] ? Attributes::getReal(itsAttr[K0S]) : 0.0;
     //JMJ 4/10/2000: above line replaced
     //    length ? angle / length : angle;
-    // to avoid closed orbit created by SBEND with defalt K0.
+    // to avoid closed orbit created by SBEND with default K0.
     field.setNormalComponent(1, factor * k0);
     field.setSkewComponent(1, factor * Attributes::getReal(itsAttr[K0S]));
     field.setNormalComponent(2, factor * Attributes::getReal(itsAttr[K1]));
@@ -163,11 +160,11 @@ void OpalSBend::update() {
 
     if(itsAttr[GREATERTHANPI])
         throw OpalException("OpalSBend::update",
-                            "GREATERTHANPI not supportet any more");
+                            "GREATERTHANPI not supported anymore");
 
     if(itsAttr[ROTATION])
         throw OpalException("OpalSBend::update",
-                            "ROTATION not supportet any more; use PSI instead");
+                            "ROTATION not supported anymore; use PSI instead");
 
     if(itsAttr[FMAPFN])
         bend->setFieldMapFN(Attributes::getString(itsAttr[FMAPFN]));
@@ -183,15 +180,18 @@ void OpalSBend::update() {
     bend->setExitAngle(e2);
 
     // Units are eV.
-    if(itsAttr[DESIGNENERGY]) {
+    if(itsAttr[DESIGNENERGY] && Attributes::getReal(itsAttr[DESIGNENERGY]) != 0.0) {
         bend->setDesignEnergy(Attributes::getReal(itsAttr[DESIGNENERGY]), false);
+    } else if (bend->getName() != "SBEND") {
+        throw OpalException("OpalSBend::update",
+                            "SBend requires non-zero DESIGNENERGY");
     }
 
     bend->setFullGap(Attributes::getReal(itsAttr[GAP]));
 
     if(itsAttr[APERT])
-        throw OpalException("OpalRBend::fillRegisteredAttributes",
-                            "APERTURE in RBEND not supported; use GAP and HAPERT instead");
+        throw OpalException("OpalSBend::update",
+                            "APERTURE in SBEND not supported; use GAP and HAPERT instead");
 
     if(itsAttr[HAPERT]) {
         double hapert = Attributes::getReal(itsAttr[HAPERT]);
diff --git a/src/Solvers/EllipticDomain.cpp b/src/Solvers/EllipticDomain.cpp
index 28b6e8f2780ca407ad43de1898c5edfac73781a6..d3365cd53028bd7542eceaadb4ad370342b7e714 100644
--- a/src/Solvers/EllipticDomain.cpp
+++ b/src/Solvers/EllipticDomain.cpp
@@ -1,6 +1,8 @@
 //
 // Class EllipticDomain
-//   :FIXME: add brief description
+//   This class provides an elliptic beam pipe. The mesh adapts to the bunch size
+//   in the longitudinal direction. At the intersection of the mesh with the
+//   beam pipe, three stencil interpolation methods are available.
 //
 // Copyright (c) 2008,        Yves Ineichen, ETH Zürich,
 //               2013 - 2015, Tülin Kaman, Paul Scherrer Institut, Villigen PSI, Switzerland
@@ -38,52 +40,36 @@ EllipticDomain::EllipticDomain(Vector_t nr, Vector_t hr) {
     setHr(hr);
 }
 
-EllipticDomain::EllipticDomain(double semimajor, double semiminor, Vector_t nr, Vector_t hr, std::string interpl) {
-    SemiMajor = semimajor;
-    SemiMinor = semiminor;
+EllipticDomain::EllipticDomain(double semimajor, double semiminor, Vector_t nr,
+                               Vector_t hr, std::string interpl)
+    : semiMajor_m(semimajor)
+    , semiMinor_m(semiminor)
+{
     setNr(nr);
     setHr(hr);
 
-    if(interpl == "CONSTANT")
-        interpolationMethod = CONSTANT;
-    else if(interpl == "LINEAR")
-        interpolationMethod = LINEAR;
-    else if(interpl == "QUADRATIC")
-        interpolationMethod = QUADRATIC;
+    if (interpl == "CONSTANT")
+        interpolationMethod_m = CONSTANT;
+    else if (interpl == "LINEAR")
+        interpolationMethod_m = LINEAR;
+    else if (interpl == "QUADRATIC")
+        interpolationMethod_m = QUADRATIC;
 }
 
-EllipticDomain::EllipticDomain(BoundaryGeometry *bgeom, Vector_t nr, Vector_t hr, std::string interpl) {
-    SemiMajor = bgeom->getA();
-    SemiMinor = bgeom->getB();
+EllipticDomain::EllipticDomain(BoundaryGeometry *bgeom, Vector_t nr, Vector_t hr,
+                               std::string interpl)
+    : EllipticDomain(bgeom->getA(), bgeom->getB(), nr, hr, interpl)
+{
     setMinMaxZ(bgeom->getS(), bgeom->getLength());
-    setNr(nr);
-    setHr(hr);
-
-    if(interpl == "CONSTANT")
-        interpolationMethod = CONSTANT;
-    else if(interpl == "LINEAR")
-        interpolationMethod = LINEAR;
-    else if(interpl == "QUADRATIC")
-        interpolationMethod = QUADRATIC;
 }
 
-EllipticDomain::EllipticDomain(BoundaryGeometry *bgeom, Vector_t nr, std::string interpl) {
-    SemiMajor = bgeom->getA();
-    SemiMinor = bgeom->getB();
-    setMinMaxZ(bgeom->getS(), bgeom->getLength());
-    Vector_t hr_m;
-    hr_m[0] = (getXRangeMax()-getXRangeMin())/nr[0];
-    hr_m[1] = (getYRangeMax()-getYRangeMin())/nr[1];
-    hr_m[2] = (getZRangeMax()-getZRangeMin())/nr[2];
-    setHr(hr_m);
-
-    if(interpl == "CONSTANT")
-        interpolationMethod = CONSTANT;
-    else if(interpl == "LINEAR")
-        interpolationMethod = LINEAR;
-    else if(interpl == "QUADRATIC")
-        interpolationMethod = QUADRATIC;
-}
+EllipticDomain::EllipticDomain(BoundaryGeometry *bgeom, Vector_t nr, std::string interpl)
+    : EllipticDomain(bgeom, nr,
+                     Vector_t(2.0 * bgeom->getA() / nr[0],
+                              2.0 * bgeom->getB() / nr[1],
+                              (bgeom->getLength() - bgeom->getS()) / nr[2]),
+                     interpl)
+{ }
 
 EllipticDomain::~EllipticDomain() {
     //nothing so far
@@ -93,164 +79,115 @@ EllipticDomain::~EllipticDomain() {
 // for this geometry we only have to calculate the intersection on
 // one x-y-plane
 // for the moment we center the ellipse around the center of the grid
-void EllipticDomain::compute(Vector_t hr){
-    //there is nothing to be done if the mesh spacings have not changed
-    if(hr[0] == getHr()[0] && hr[1] == getHr()[1] && hr[2] == getHr()[2]) {
+void EllipticDomain::compute(Vector_t hr, NDIndex<3> localId){
+    // there is nothing to be done if the mesh spacings in x and y have not changed
+    if (hr[0] == getHr()[0] &&
+        hr[1] == getHr()[1])
+    {
         hasGeometryChanged_m = false;
         return;
     }
+
     setHr(hr);
     hasGeometryChanged_m = true;
     //reset number of points inside domain
     nxy_m = 0;
 
     // clear previous coordinate maps
-    IdxMap.clear();
-    CoordMap.clear();
+    idxMap_m.clear();
+    coordMap_m.clear();
     //clear previous intersection points
-    IntersectYDir.clear();
-    IntersectXDir.clear();
+    intersectYDir_m.clear();
+    intersectXDir_m.clear();
 
     // build a index and coordinate map
     int idx = 0;
     int x, y;
 
-    for(x = 0; x < nr[0]; x++) {
-        for(y = 0; y < nr[1]; y++) {
-            if(isInside(x, y, 1)) {
-                IdxMap[toCoordIdx(x, y)] = idx;
-                CoordMap[idx++] = toCoordIdx(x, y);
+    /* we need to scan the full x-y-plane on all cores
+     * in order to figure out the number of valid
+     * grid points per plane --> otherwise we might
+     * get not unique global indices in the Tpetra::CrsMatrix
+     */
+    for (x = 0; x < nr[0]; ++x) {
+        for (y = 0; y < nr[1]; ++y) {
+            if (isInside(x, y, 1)) {
+                idxMap_m[toCoordIdx(x, y)] = idx;
+                coordMap_m[idx++] = toCoordIdx(x, y);
                 nxy_m++;
             }
-
         }
     }
 
-    switch(interpolationMethod) {
+    switch (interpolationMethod_m) {
         case CONSTANT:
             break;
         case LINEAR:
         case QUADRATIC:
 
-            double smajsq = SemiMajor * SemiMajor;
-            double sminsq = SemiMinor * SemiMinor;
+            double smajsq = semiMajor_m * semiMajor_m;
+            double sminsq = semiMinor_m * semiMinor_m;
             double yd = 0.0;
             double xd = 0.0;
             double pos = 0.0;
-            double mx = (nr[0] - 1) * hr[0] / 2.0;
-            double my = (nr[1] - 1) * hr[1] / 2.0;
 
-            //calculate intersection with the ellipse
-            for(x = 0; x < nr[0]; x++) {
-                pos = x * hr[0] - mx;
-                if (pos <= -SemiMajor || pos >= SemiMajor)
+            // calculate intersection with the ellipse
+            for (x = localId[0].first(); x <= localId[0].last(); x++) {
+                pos = - semiMajor_m + hr[0] * (x + 0.5);
+                if (std::abs(pos) >= semiMajor_m)
                 {
-                    IntersectYDir.insert(std::pair<int, double>(x, 0));
-                    IntersectYDir.insert(std::pair<int, double>(x, 0));
-                }else{
-                    yd = std::abs(sqrt(sminsq - sminsq * pos * pos / smajsq)); // + 0.5*nr[1]*hr[1]);
-                    IntersectYDir.insert(std::pair<int, double>(x, yd));
-                    IntersectYDir.insert(std::pair<int, double>(x, -yd));
+                    intersectYDir_m.insert(std::pair<int, double>(x, 0));
+                    intersectYDir_m.insert(std::pair<int, double>(x, 0));
+                } else {
+                    yd = std::abs(std::sqrt(sminsq - sminsq * pos * pos / smajsq));
+                    intersectYDir_m.insert(std::pair<int, double>(x, yd));
+                    intersectYDir_m.insert(std::pair<int, double>(x, -yd));
                 }
 
             }
 
-            for(y = 0; y < nr[1]; y++) {
-                pos = y * hr[1] - my;
-                if (pos <= -SemiMinor || pos >= SemiMinor)
+            for (y = localId[0].first(); y < localId[1].last(); y++) {
+                pos = - semiMinor_m + hr[1] * (y + 0.5);
+                if (std::abs(pos) >= semiMinor_m)
                 {
-                    IntersectXDir.insert(std::pair<int, double>(y, 0));
-                    IntersectXDir.insert(std::pair<int, double>(y, 0));
-                }else{
-                    xd = std::abs(sqrt(smajsq - smajsq * pos * pos / sminsq)); // + 0.5*nr[0]*hr[0]);
-                    IntersectXDir.insert(std::pair<int, double>(y, xd));
-                    IntersectXDir.insert(std::pair<int, double>(y, -xd));
+                    intersectXDir_m.insert(std::pair<int, double>(y, 0));
+                    intersectXDir_m.insert(std::pair<int, double>(y, 0));
+                } else {
+                    xd = std::abs(std::sqrt(smajsq - smajsq * pos * pos / sminsq));
+                    intersectXDir_m.insert(std::pair<int, double>(y, xd));
+                    intersectXDir_m.insert(std::pair<int, double>(y, -xd));
                 }
             }
     }
 }
 
-void EllipticDomain::compute(Vector_t hr, NDIndex<3> localId){
-    //there is nothing to be done if the mesh spacings have not changed
-    if(hr[0] == getHr()[0] && hr[1] == getHr()[1] && hr[2] == getHr()[2]) {
-        hasGeometryChanged_m = false;
-        return;
-    }
-    setHr(hr);
-    hasGeometryChanged_m = true;
-    //reset number of points inside domain
-    nxy_m = 0;
-
-    // clear previous coordinate maps
-    IdxMap.clear();
-    CoordMap.clear();
-    //clear previous intersection points
-    IntersectYDir.clear();
-    IntersectXDir.clear();
-
-    // build a index and coordinate map
-    int idx = 0;
-    int x, y;
-
-    for(x = localId[0].first();  x<= localId[0].last(); x++) {
-        for(y = localId[1].first(); y <= localId[1].last(); y++) {
-            if(isInside(x, y, 1)) {
-                IdxMap[toCoordIdx(x, y)] = idx;
-                CoordMap[idx++] = toCoordIdx(x, y);
-                nxy_m++;
-            }
-        }
-    }
-
-    switch(interpolationMethod) {
-        case CONSTANT:
-            break;
-        case LINEAR:
-        case QUADRATIC:
+void EllipticDomain::resizeMesh(Vector_t& origin, Vector_t& hr, const Vector_t& rmin,
+                                const Vector_t& rmax, double dh)
+{
+    Vector_t mymax = Vector_t(0.0, 0.0, 0.0);
 
-            double smajsq = SemiMajor * SemiMajor;
-            double sminsq = SemiMinor * SemiMinor;
-            double yd = 0.0;
-            double xd = 0.0;
-            double pos = 0.0;
-            double mx = (nr[0] - 1) * hr[0] / 2.0;
-            double my = (nr[1] - 1) * hr[1] / 2.0;
+    // apply bounding box increment dh, i.e., "BBOXINCR" input argument
+    double zsize = rmax[2] - rmin[2];
 
-            //calculate intersection with the ellipse
-            for(x = localId[0].first(); x <= localId[0].last(); x++) {
-                pos = x * hr[0] - mx;
-                if (pos <= -SemiMajor || pos >= SemiMajor)
-                {
-                    IntersectYDir.insert(std::pair<int, double>(x, 0));
-                    IntersectYDir.insert(std::pair<int, double>(x, 0));
-                }else{
-                    yd = std::abs(sqrt(sminsq - sminsq * pos * pos / smajsq)); // + 0.5*nr[1]*hr[1]);
-                    IntersectYDir.insert(std::pair<int, double>(x, yd));
-                    IntersectYDir.insert(std::pair<int, double>(x, -yd));
-                }
+    setMinMaxZ(rmin[2] - zsize * (1.0 + dh),
+               rmax[2] + zsize * (1.0 + dh));
 
-            }
+    origin = Vector_t(-semiMajor_m, -semiMinor_m, getMinZ());
+    mymax  = Vector_t( semiMajor_m,  semiMinor_m, getMaxZ());
 
-            for(y = localId[0].first(); y < localId[1].last(); y++) {
-                pos = y * hr[1] - my;
-                if (pos <= -SemiMinor || pos >= SemiMinor)
-                {
-                    IntersectXDir.insert(std::pair<int, double>(y, 0));
-                    IntersectXDir.insert(std::pair<int, double>(y, 0));
-                }else{
-                    xd = std::abs(sqrt(smajsq - smajsq * pos * pos / sminsq)); // + 0.5*nr[0]*hr[0]);
-                    IntersectXDir.insert(std::pair<int, double>(y, xd));
-                    IntersectXDir.insert(std::pair<int, double>(y, -xd));
-                }
-            }
-    }
+    for (int i = 0; i < 3; ++i)
+        hr[i] = (mymax[i] - origin[i]) / nr[i];
 }
 
-void EllipticDomain::getBoundaryStencil(int x, int y, int z, double &W, double &E, double &S, double &N, double &F, double &B, double &C, double &scaleFactor) {
+void EllipticDomain::getBoundaryStencil(int x, int y, int z, double &W,
+                                        double &E, double &S, double &N,
+                                        double &F, double &B, double &C,
+                                        double &scaleFactor)
+{
     scaleFactor = 1.0;
 
-        // determine which interpolation method we use for points near the boundary
-    switch(interpolationMethod) {
+    // determine which interpolation method we use for points near the boundary
+    switch (interpolationMethod_m) {
         case CONSTANT:
             constantInterpolation(x, y, z, W, E, S, N, F, B, C, scaleFactor);
             break;
@@ -266,9 +203,10 @@ void EllipticDomain::getBoundaryStencil(int x, int y, int z, double &W, double &
     assert(C > 0);
 }
 
-void EllipticDomain::getBoundaryStencil(int idx, double &W, double &E, double &S, double &N, double &F, double &B, double &C, double &scaleFactor) {
-
-
+void EllipticDomain::getBoundaryStencil(int idx, double &W, double &E,
+                                        double &S, double &N, double &F,
+                                        double &B, double &C, double &scaleFactor)
+{
     int x = 0, y = 0, z = 0;
     getCoord(idx, x, y, z);
     getBoundaryStencil(x, y, z, W, E, S, N, F, B, C, scaleFactor);
@@ -283,113 +221,99 @@ void EllipticDomain::getNeighbours(int idx, int &W, int &E, int &S, int &N, int
 
 }
 
-void EllipticDomain::getNeighbours(int x, int y, int z, int &W, int &E, int &S, int &N, int &F, int &B) {
-
-    if(x > 0)
+void EllipticDomain::getNeighbours(int x, int y, int z, int &W, int &E,
+                                   int &S, int &N, int &F, int &B)
+{
+    if (x > 0)
         W = getIdx(x - 1, y, z);
     else
         W = -1;
-    if(x < nr[0] - 1)
+
+    if (x < nr[0] - 1)
         E = getIdx(x + 1, y, z);
     else
         E = -1;
 
-    if(y < nr[1] - 1)
+    if (y < nr[1] - 1)
         N = getIdx(x, y + 1, z);
     else
         N = -1;
-    if(y > 0)
+
+    if (y > 0)
         S = getIdx(x, y - 1, z);
     else
         S = -1;
 
-    if(z > 0)
+    if (z > 0)
         F = getIdx(x, y, z - 1);
     else
         F = -1;
-    if(z < nr[2] - 1)
+
+    if (z < nr[2] - 1)
         B = getIdx(x, y, z + 1);
     else
         B = -1;
 
 }
 
-void EllipticDomain::constantInterpolation(int x, int y, int z, double &WV, double &EV, double &SV, double &NV, double &FV, double &BV, double &CV, double &scaleFactor) {
-
+void EllipticDomain::constantInterpolation(int x, int y, int z, double &WV,
+                                           double &EV, double &SV, double &NV,
+                                           double &FV, double &BV, double &CV,
+                                           double &scaleFactor)
+{
     scaleFactor = 1.0;
 
-    WV = -1/(hr[0]*hr[0]);
-    EV = -1/(hr[0]*hr[0]);
-    NV = -1/(hr[1]*hr[1]);
-    SV = -1/(hr[1]*hr[1]);
-    FV = -1/(hr[2]*hr[2]);
-    BV = -1/(hr[2]*hr[2]);
-    CV = 2/(hr[0]*hr[0]) + 2/(hr[1]*hr[1]) + 2/(hr[2]*hr[2]);
+    WV = -1.0 / (hr[0] * hr[0]);
+    EV = -1.0 / (hr[0] * hr[0]);
+    NV = -1.0 / (hr[1] * hr[1]);
+    SV = -1.0 / (hr[1] * hr[1]);
+    FV = -1.0 / (hr[2] * hr[2]);
+    BV = -1.0 / (hr[2] * hr[2]);
+
+    CV =  2.0 / (hr[0] * hr[0])
+       +  2.0 / (hr[1] * hr[1])
+       +  2.0 / (hr[2] * hr[2]);
 
     // we are a right boundary point
-    if(!isInside(x + 1, y, z))
-        EV= 0.0;
+    if (!isInside(x + 1, y, z))
+        EV = 0.0;
 
     // we are a left boundary point
-    if(!isInside(x - 1, y, z))
+    if (!isInside(x - 1, y, z))
         WV = 0.0;
 
     // we are a upper boundary point
-    if(!isInside(x, y + 1, z))
+    if (!isInside(x, y + 1, z))
         NV = 0.0;
 
     // we are a lower boundary point
-    if(!isInside(x, y - 1, z))
+    if (!isInside(x, y - 1, z))
         SV = 0.0;
 
-    if(z == 1 || z == nr[2] - 2) {
-
-        // case where we are on the Robin BC in Z-direction
-        // where we distinguish two cases
-        // IFF: this values should not matter because they
-        // never make it into the discretization matrix
-        if(z == 1)
-            FV = 0.0;
-        else
-            BV = 0.0;
-
-        // add contribution of Robin discretization to center point
-        // d the distance between the center of the bunch and the boundary
-        //double cx = (x-(nr[0]-1)/2)*hr[0];
-        //double cy = (y-(nr[1]-1)/2)*hr[1];
-        //double cz = hr[2]*(nr[2]-1);
-        //double d = sqrt(cx*cx+cy*cy+cz*cz);
-        double d = hr[2] * (nr[2] - 1) / 2;
-        CV += 2 / (d * hr[2]);
-        //C += 2/((hr[2]*(nr[2]-1)/2.0) * hr[2]);
-
-        // scale all stencil-points in z-plane with 0.5 (Robin discretization)
-        WV /= 2.0;
-        EV /= 2.0;
-        NV /= 2.0;
-        SV /= 2.0;
-        CV /= 2.0;
-        scaleFactor *= 0.5;
-    }
-
+    // handle boundary condition in z direction
+    robinBoundaryStencil(z, FV, BV, CV);
 }
 
-void EllipticDomain::linearInterpolation(int x, int y, int z, double &W, double &E, double &S, double &N, double &F, double &B, double &C, double &scaleFactor) {
-
+void EllipticDomain::linearInterpolation(int x, int y, int z, double &W,
+                                         double &E, double &S, double &N,
+                                         double &F, double &B, double &C,
+                                         double &scaleFactor)
+{
     scaleFactor = 1.0;
 
-    double cx = x * hr[0] - (nr[0] - 1) * hr[0] / 2.0;
-    double cy = y * hr[1] - (nr[1] - 1) * hr[1] / 2.0;
+    double cx = - semiMajor_m + hr[0] * (x + 0.5);
+    double cy = - semiMinor_m + hr[1] * (y + 0.5);
 
     double dx = 0.0;
-    std::multimap<int, double>::iterator it = IntersectXDir.find(y);
-    if(cx < 0)
+    std::multimap<int, double>::iterator it = intersectXDir_m.find(y);
+
+    if (cx < 0)
         ++it;
     dx = it->second;
 
     double dy = 0.0;
-    it = IntersectYDir.find(x);
-    if(cy < 0)
+    it = intersectYDir_m.find(x);
+    if (cy < 0)
         ++it;
     dy = it->second;
 
@@ -400,73 +324,55 @@ void EllipticDomain::linearInterpolation(int x, int y, int z, double &W, double
     double ds = hr[1];
     C = 0.0;
 
-    //we are a right boundary point
-    if(!isInside(x + 1, y, z)) {
-        C += 1 / ((dx - cx) * de);
+    // we are a right boundary point
+    if (!isInside(x + 1, y, z)) {
+        C += 1.0 / ((dx - cx) * de);
         E = 0.0;
     } else {
-        C += 1 / (de * de);
-        E = -1 / (de * de);
+        C += 1.0 / (de * de);
+        E = -1.0 / (de * de);
     }
 
-    //we are a left boundary point
-    if(!isInside(x - 1, y, z)) {
-        C += 1 / ((std::abs(dx) - std::abs(cx)) * dw);
+    // we are a left boundary point
+    if (!isInside(x - 1, y, z)) {
+        C += 1.0 / ((std::abs(dx) - std::abs(cx)) * dw);
         W = 0.0;
     } else {
-        C += 1 / (dw * dw);
-        W = -1 / (dw * dw);
+        C += 1.0 / (dw * dw);
+        W = -1.0 / (dw * dw);
     }
 
-    //we are a upper boundary point
-    if(!isInside(x, y + 1, z)) {
-        C += 1 / ((dy - cy) * dn);
+    // we are a upper boundary point
+    if (!isInside(x, y + 1, z)) {
+        C += 1.0 / ((dy - cy) * dn);
         N = 0.0;
     } else {
-        C += 1 / (dn * dn);
-        N = -1 / (dn * dn);
+        C += 1.0 / (dn * dn);
+        N = -1.0 / (dn * dn);
     }
 
-    //we are a lower boundary point
-    if(!isInside(x, y - 1, z)) {
-        C += 1 / ((std::abs(dy) - std::abs(cy)) * ds);
+    // we are a lower boundary point
+    if (!isInside(x, y - 1, z)) {
+        C += 1.0 / ((std::abs(dy) - std::abs(cy)) * ds);
         S = 0.0;
     } else {
-        C += 1 / (ds * ds);
-        S = -1 / (ds * ds);
+        C += 1.0 / (ds * ds);
+        S = -1.0 / (ds * ds);
     }
 
-    F = -1 / (hr[2] * hr[2]);
-    B = -1 / (hr[2] * hr[2]);
-    C += 2 / (hr[2] * hr[2]);
-
     // handle boundary condition in z direction
-/*
-    if(z == 0 || z == nr[2] - 1) {
-
-        // case where we are on the NEUMAN BC in Z-direction
-        // where we distinguish two cases
-        if(z == 0)
-            F = 0.0;
-        else
-            B = 0.0;
-
-        //hr[2]*(nr2[2]-1)/2 = radius
-        double d = hr[2] * (nr[2] - 1) / 2;
-        C += 2 / (d * hr[2]);
-
-        W /= 2.0;
-        E /= 2.0;
-        N /= 2.0;
-        S /= 2.0;
-        C /= 2.0;
-        scaleFactor *= 0.5;
-
-    }
-*/
+    F = -1.0 / (hr[2] * hr[2]);
+    B = -1.0 / (hr[2] * hr[2]);
+    C += 2.0 / (hr[2] * hr[2]);
+    robinBoundaryStencil(z, F, B, C);
 }
 
-void EllipticDomain::quadraticInterpolation(int x, int y, int z, double &W, double &E, double &S, double &N, double &F, double &B, double &C, double &scaleFactor) {
+void EllipticDomain::quadraticInterpolation(int x, int y, int z,
+                                            double &W, double &E, double &S,
+                                            double &N, double &F, double &B, double &C,
+                                            double &scaleFactor)
+{
+    scaleFactor = 1.0;
 
     double cx = (x - (nr[0] - 1) / 2.0) * hr[0];
     double cy = (y - (nr[1] - 1) / 2.0) * hr[1];
@@ -474,14 +380,14 @@ void EllipticDomain::quadraticInterpolation(int x, int y, int z, double &W, doub
     // since every vector for elliptic domains has ALWAYS size 2 we
     // can catch all cases manually
     double dx = 0.0;
-    std::multimap<int, double>::iterator it = IntersectXDir.find(y);
-    if(cx < 0)
+    std::multimap<int, double>::iterator it = intersectXDir_m.find(y);
+    if (cx < 0)
         ++it;
     dx = it->second;
 
     double dy = 0.0;
-    it = IntersectYDir.find(x);
-    if(cy < 0)
+    it = intersectYDir_m.find(x);
+    if (cy < 0)
         ++it;
     dy = it->second;
 
@@ -489,197 +395,80 @@ void EllipticDomain::quadraticInterpolation(int x, int y, int z, double &W, doub
     double de = hr[0];
     double dn = hr[1];
     double ds = hr[1];
-    W = 1.0;
-    E = 1.0;
-    N = 1.0;
-    S = 1.0;
-    F = 1.0;
-    B = 1.0;
+
+    W = 0.0;
+    E = 0.0;
+    S = 0.0;
+    N = 0.0;
     C = 0.0;
 
-    //TODO: = cx+hr[0] > dx && cx > 0
-    //if((x-nr[0]/2.0+1)*hr[0] > dx && cx > 0) {
-    ////we are a right boundary point
-    ////if(!isInside(x+1,y,z)) {
-    //de = dx-cx;
-    //}
-
-    //if((x-nr[0]/2.0-1)*hr[0] < dx && cx < 0) {
-    ////we are a left boundary point
-    ////if(!isInside(x-1,y,z)) {
-    //dw = std::abs(dx)-std::abs(cx);
-    //}
-
-    //if((y-nr[1]/2.0+1)*hr[1] > dy && cy > 0) {
-    ////we are a upper boundary point
-    ////if(!isInside(x,y+1,z)) {
-    //dn = dy-cy;
-    //}
-
-    //if((y-nr[1]/2.0-1)*hr[1] < dy && cy < 0) {
-    ////we are a lower boundary point
-    ////if(!isInside(x,y-1,z)) {
-    //ds = std::abs(dy)-std::abs(cy);
-    //}
-
-    //TODO: = cx+hr[0] > dx && cx > 0
-    //if((x-nr[0]/2.0+1)*hr[0] > dx && cx > 0) {
-    //we are a right boundary point
-    if(!isInside(x + 1, y, z)) {
-        de = dx - cx;
+    // we are a right boundary point
+    if (!isInside(x + 1, y, z)) {
+        double s = dx - cx;
+        C -= 2.0 * (s - 1.0) / (s * de);
         E = 0.0;
+        W += (s - 1.0) / ((s + 1.0) * de);
+    } else {
+        C += 1.0 / (de * de);
+        E = -1.0 / (de * de);
     }
 
-    //if((x-nr[0]/2.0-1)*hr[0] < dx && cx < 0) {
-    //we are a left boundary point
-    if(!isInside(x - 1, y, z)) {
-        dw = std::abs(dx) - std::abs(cx);
+    // we are a left boundary point
+    if (!isInside(x - 1, y, z)) {
+        double s = std::abs(dx) - std::abs(cx);
+        C -= 2.0 * (s - 1.0) / (s * de);
         W = 0.0;
+        E += (s - 1.0) / ((s + 1.0) * de);
+    } else {
+        C += 1.0 / (dw * dw);
+        W = -1.0 / (dw * dw);
     }
 
-    //if((y-nr[1]/2.0+1)*hr[1] > dy && cy > 0) {
-    //we are a upper boundary point
-    if(!isInside(x, y + 1, z)) {
-        dn = dy - cy;
+    // we are a upper boundary point
+    if (!isInside(x, y + 1, z)) {
+        double s = dy - cy;
+        C -= 2.0 * (s - 1.0) / (s * dn);
         N = 0.0;
+        S += (s - 1.0) / ((s + 1.0) * dn);
+    } else {
+        C += 1.0 / (dn * dn);
+        N = -1.0 / (dn * dn);
     }
 
-    //if((y-nr[1]/2.0-1)*hr[1] < dy && cy < 0) {
-    //we are a lower boundary point
-    if(!isInside(x, y - 1, z)) {
-        ds = std::abs(dy) - std::abs(cy);
+    // we are a lower boundary point
+    if (!isInside(x, y - 1, z)) {
+        double s = std::abs(dy) - std::abs(cy);
+        C -= 2.0 * (s - 1.0) / (s * dn);
         S = 0.0;
+        N += (s - 1.0) / ((s + 1.0) * dn);
+    } else {
+        C += 1.0 / (ds * ds);
+        S = -1.0 / (ds * ds);
     }
 
-    //2/dw*(dw_de)
-    W *= -1.0 / (dw * (dw + de));
-    E *= -1.0 / (de * (dw + de));
-    N *= -1.0 / (dn * (dn + ds));
-    S *= -1.0 / (ds * (dn + ds));
-    F = -1 / (hr[2] * (hr[2] + hr[2]));
-    B = -1 / (hr[2] * (hr[2] + hr[2]));
-
-    //TODO: problem when de,dw,dn,ds == 0
-    //is NOT a regular BOUND PT
-    C += 1 / de * 1 / dw;
-    C += 1 / dn * 1 / ds;
-    C += 1 / hr[2] * 1 / hr[2];
-    scaleFactor = 0.5;
-
-
-    //for regular gridpoints no problem with symmetry, just boundary
-    //z direction is right
-    //implement isLastInside(dir)
-    //we have LOCAL x,y coordinates!
-
-    /*
-       if(dw != 0 && !wIsB)
-       W = -1/dw * (dn+ds) * 2*hr[2];
-       else
-       W = 0;
-       if(de != 0 && !eIsB)
-       E = -1/de * (dn+ds) * 2*hr[2];
-       else
-       E = 0;
-       if(dn != 0 && !nIsB)
-       N = -1/dn * (dw+de) * 2*hr[2];
-       else
-       N = 0;
-       if(ds != 0 && !sIsB)
-       S = -1/ds * (dw+de) * 2*hr[2];
-       else
-       S = 0;
-       F = -(dw+de)*(dn+ds)/hr[2];
-       B = -(dw+de)*(dn+ds)/hr[2];
-       */
-
-    //if(dw != 0)
-    //W = -2*hr[2]*(dn+ds)/dw;
-    //else
-    //W = 0;
-    //if(de != 0)
-    //E = -2*hr[2]*(dn+ds)/de;
-    //else
-    //E = 0;
-    //if(dn != 0)
-    //N = -2*hr[2]*(dw+de)/dn;
-    //else
-    //N = 0;
-    //if(ds != 0)
-    //S = -2*hr[2]*(dw+de)/ds;
-    //else
-    //S = 0;
-    //F = -(dw+de)*(dn+ds)/hr[2];
-    //B = -(dw+de)*(dn+ds)/hr[2];
-
-    //// RHS scaleFactor for current 3D index
-    //// Factor 0.5 results from the SW/quadratic extrapolation
-    //scaleFactor = 0.5*(dw+de)*(dn+ds)*(2*hr[2]);
-
-    // catch the case where a point lies on the boundary
-    //FIXME: do this more elegant!
-    //double m1 = dw*de;
-    //double m2 = dn*ds;
-    //if(de == 0)
-    //m1 = dw;
-    //if(dw == 0)
-    //m1 = de;
-    //if(dn == 0)
-    //m2 = ds;
-    //if(ds == 0)
-    //m2 = dn;
-    ////XXX: dn+ds || dw+de can be 0
-    ////C = 2*(dn+ds)*(dw+de)/hr[2];
-    //C = 2/hr[2];
-    //if(dw != 0 || de != 0)
-    //C *= (dw+de);
-    //if(dn != 0 || ds != 0)
-    //C *= (dn+ds);
-    //if(dw != 0 || de != 0)
-    //C += (2*hr[2])*(dn+ds)*(dw+de)/m1;
-    //if(dn != 0 || ds != 0)
-    //C += (2*hr[2])*(dw+de)*(dn+ds)/m2;
-
-    //handle Neumann case
-    //if(z == 0 || z == nr[2]-1) {
-
-    //if(z == 0)
-    //F = 0.0;
-    //else
-    //B = 0.0;
-
-    ////neumann stuff
-    //W = W/2.0;
-    //E = E/2.0;
-    //N = N/2.0;
-    //S = S/2.0;
-    //C /= 2.0;
-
-    //scaleFactor /= 2.0;
-    //}
-
     // handle boundary condition in z direction
-    if(z == 0 || z == nr[2] - 1) {
+    F = -1.0 / (hr[2] * hr[2]);
+    B = -1.0 / (hr[2] * hr[2]);
+    C += 2.0 / (hr[2] * hr[2]);
+    robinBoundaryStencil(z, F, B, C);
+}
+
+void EllipticDomain::robinBoundaryStencil(int z, double &F, double &B, double &C) {
+    if (z == 0 || z == nr[2] - 1) {
 
-        // case where we are on the NEUMAN BC in Z-direction
+        // case where we are on the Robin BC in Z-direction
         // where we distinguish two cases
-        if(z == 0)
+        // IFF: this values should not matter because they
+        // never make it into the discretization matrix
+        if (z == 0)
             F = 0.0;
         else
             B = 0.0;
 
-        //C += 2/((hr[2]*(nr[2]-1)/2.0) * hr[2]);
-        //hr[2]*(nr2[2]-1)/2 = radius
-        double d = hr[2] * (nr[2] - 1) / 2;
-        C += 2 / (d * hr[2]);
-
-        W /= 2.0;
-        E /= 2.0;
-        N /= 2.0;
-        S /= 2.0;
-        C /= 2.0;
-        scaleFactor /= 2.0;
-
+        // add contribution of Robin discretization to center point
+        // d the distance between the center of the bunch and the boundary
+        double d = 0.5 * hr[2] * (nr[2] - 1);
+        C += 2.0 / (d * hr[2]);
     }
 }
 
diff --git a/src/Solvers/EllipticDomain.h b/src/Solvers/EllipticDomain.h
index 691e82cffb148c0180103eef7fd127c843681fb2..e5478b5ae1fe0cdd90739b84cffa1c9232a5449b 100644
--- a/src/Solvers/EllipticDomain.h
+++ b/src/Solvers/EllipticDomain.h
@@ -1,6 +1,8 @@
 //
 // Class EllipticDomain
-//   :FIXME: add brief description
+//   This class provides an elliptic beam pipe. The mesh adapts to the bunch size
+//   in the longitudinal direction. At the intersection of the mesh with the
+//   beam pipe, three stencil interpolation methods are available.
 //
 // Copyright (c) 2008,        Yves Ineichen, ETH Zürich,
 //               2013 - 2015, Tülin Kaman, Paul Scherrer Institut, Villigen PSI, Switzerland
@@ -32,100 +34,127 @@
 #include <cmath>
 #include "IrregularDomain.h"
 #include "Structure/BoundaryGeometry.h"
+#include "Utilities/OpalException.h"
 
 class EllipticDomain : public IrregularDomain {
 
 public:
 
     EllipticDomain(Vector_t nr, Vector_t hr);
-    EllipticDomain(double semimajor, double semiminor, Vector_t nr, Vector_t hr, std::string interpl);
-    EllipticDomain(BoundaryGeometry *bgeom, Vector_t nr, Vector_t hr, std::string interpl);
+
+    EllipticDomain(double semimajor, double semiminor, Vector_t nr,
+                   Vector_t hr, std::string interpl);
+
+    EllipticDomain(BoundaryGeometry *bgeom, Vector_t nr,
+                   Vector_t hr, std::string interpl);
+
     EllipticDomain(BoundaryGeometry *bgeom, Vector_t nr, std::string interpl);
 
     ~EllipticDomain();
 
     /// returns discretization at (x,y,z)
-    void getBoundaryStencil(int x, int y, int z, double &W, double &E, double &S, double &N, double &F, double &B, double &C, double &scaleFactor);
+    void getBoundaryStencil(int x, int y, int z,
+                            double &W, double &E, double &S,
+                            double &N, double &F, double &B,
+                            double &C, double &scaleFactor);
+
     /// returns discretization at 3D index
-    void getBoundaryStencil(int idx, double &W, double &E, double &S, double &N, double &F, double &B, double &C, double &scaleFactor);
+    void getBoundaryStencil(int idx, double &W, double &E,
+                            double &S, double &N, double &F,
+                            double &B, double &C, double &scaleFactor);
+
     /// returns index of neighbours at (x,y,z)
-    void getNeighbours(int x, int y, int z, int &W, int &E, int &S, int &N, int &F, int &B);
+    void getNeighbours(int x, int y, int z, int &W, int &E,
+                       int &S, int &N, int &F, int &B);
+
     /// returns index of neighbours at 3D index
     void getNeighbours(int idx, int &W, int &E, int &S, int &N, int &F, int &B);
+
     /// returns type of boundary condition
     std::string getType() {return "Elliptic";}
+
     /// queries if a given (x,y,z) coordinate lies inside the domain
     inline bool isInside(int x, int y, int z) {
-        double xx = (x - (nr[0] - 1) / 2.0) * hr[0];
-        double yy = (y - (nr[1] - 1) / 2.0) * hr[1];
-        return ((xx * xx / (SemiMajor * SemiMajor) + yy * yy / (SemiMinor * SemiMinor) < 1) && z != 0 && z != nr[2] - 1);
+        double xx = - semiMajor_m + hr[0] * (x + 0.5);
+        double yy = - semiMinor_m + hr[1] * (y + 0.5);
+
+        bool isInsideEllipse = (xx * xx / (semiMajor_m * semiMajor_m) +
+                                yy * yy / (semiMinor_m * semiMinor_m) < 1);
+
+        return (isInsideEllipse && z > 0 && z < nr[2] - 1);
     }
 
     int getNumXY(int /*z*/) { return nxy_m; }
-    /// set semi-minor
-    void setSemiMinor(double sm) {SemiMinor = sm;}
-    /// set semi-major
-    void setSemiMajor(double sm) {SemiMajor = sm;}
 
-    /// calculates intersection 
-    void compute(Vector_t hr);
+
+    /// calculates intersection
+    void compute(Vector_t /*hr*/) {
+        throw OpalException("EllipticDomain::compute()", "This function is not available.");
+    }
+
     void compute(Vector_t hr, NDIndex<3> localId);
 
-    double getXRangeMin() { return -SemiMajor; }
-    double getXRangeMax() { return SemiMajor;  }
-    double getYRangeMin() { return -SemiMinor; }
-    double getYRangeMax() { return SemiMinor;  }
+    double getXRangeMin() { return -semiMajor_m; }
+    double getXRangeMax() { return semiMajor_m;  }
+    double getYRangeMin() { return -semiMinor_m; }
+    double getYRangeMax() { return semiMinor_m;  }
     double getZRangeMin() { return zMin_m; }
     double getZRangeMax() { return zMax_m; }
 
+    bool hasGeometryChanged() { return hasGeometryChanged_m; }
 
-    //TODO: ?
-    int getStartIdx() {return 0;}
 
-    bool hasGeometryChanged() { return hasGeometryChanged_m; }
+    void resizeMesh(Vector_t& origin, Vector_t& hr, const Vector_t& rmin,
+                    const Vector_t& rmax, double dh);
 
 private:
 
     /// Map from a single coordinate (x or y) to a list of intersection values with
     /// boundary.
-    typedef std::multimap<int, double> EllipticPointList;
+    typedef std::multimap<int, double> EllipticPointList_t;
 
     /// all intersection points with grid lines in X direction
-    EllipticPointList IntersectXDir;
+    EllipticPointList_t intersectXDir_m;
 
     /// all intersection points with grid lines in Y direction
-    EllipticPointList IntersectYDir;
+    EllipticPointList_t intersectYDir_m;
 
     /// mapping (x,y,z) -> idx
-    std::map<int, int> IdxMap;
+    std::map<int, int> idxMap_m;
 
     /// mapping idx -> (x,y,z)
-    std::map<int, int> CoordMap;
+    std::map<int, int> coordMap_m;
 
     /// semi-major of the ellipse
-    double SemiMajor;
+    double semiMajor_m;
+
     /// semi-minor of the ellipse
-    double SemiMinor;
+    double semiMinor_m;
+
     /// number of nodes in the xy plane (for this case: independent of the z coordinate)
     int nxy_m;
+
     /// interpolation type
-    int interpolationMethod;
+    int interpolationMethod_m;
+
     /// flag indicating if geometry has changed for the current time-step
     bool hasGeometryChanged_m;
 
     /// conversion from (x,y) to index in xy plane
     inline int toCoordIdx(int x, int y) { return y * nr[0] + x; }
+
     /// conversion from (x,y,z) to index on the 3D grid
     inline int getIdx(int x, int y, int z) {
-        if(isInside(x, y, z) && x >= 0 && y >= 0 && z >= 0)
-            return IdxMap[toCoordIdx(x, y)] + (z - 1) * nxy_m;
+        if (isInside(x, y, z))
+            return idxMap_m[toCoordIdx(x, y)] + (z - 1) * nxy_m;
         else
             return -1;
     }
+
     /// conversion from a 3D index to (x,y,z)
     inline void getCoord(int idx, int &x, int &y, int &z) {
         int ixy = idx % nxy_m;
-        int xy = CoordMap[ixy];
+        int xy = coordMap_m[ixy];
         int inr = (int)nr[0];
         x = xy % inr;
         y = (xy - x) / nr[0];
@@ -133,10 +162,23 @@ private:
     }
 
     /// different interpolation methods for boundary points
-    void constantInterpolation(int x, int y, int z, double &W, double &E, double &S, double &N, double &F, double &B, double &C, double &scaleFactor);
-    void linearInterpolation(int x, int y, int z, double &W, double &E, double &S, double &N, double &F, double &B, double &C, double &scaleFactor);
-    void quadraticInterpolation(int x, int y, int z, double &W, double &E, double &S, double &N, double &F, double &B, double &C, double &scaleFactor);
-
+    void constantInterpolation(int x, int y, int z,
+                               double &W, double &E, double &S,
+                               double &N, double &F, double &B,
+                               double &C, double &scaleFactor);
+
+    void linearInterpolation(int x, int y, int z, double &W,
+                             double &E, double &S, double &N,
+                             double &F, double &B, double &C,
+                             double &scaleFactor);
+
+    void quadraticInterpolation(int x, int y, int z, double &W,
+                                double &E, double &S, double &N,
+                                double &F, double &B, double &C,
+                                double &scaleFactor);
+
+    /// function to handle the open boundary condition in longitudinal direction
+    void robinBoundaryStencil(int z, double &F, double &B, double &C);
 };
 
 #endif //#ifdef ELLIPTICAL_DOMAIN_H
diff --git a/src/Solvers/IrregularDomain.h b/src/Solvers/IrregularDomain.h
index 4569f663d9cfbd9a3bdb78ce6875b5b69a438fd0..eb00435e1772f8dca9f019ebc0848650c29e0047 100644
--- a/src/Solvers/IrregularDomain.h
+++ b/src/Solvers/IrregularDomain.h
@@ -129,6 +129,22 @@ public:
     virtual bool hasGeometryChanged() = 0;
     virtual ~IrregularDomain() {};
 
+    virtual void resizeMesh(Vector_t& origin, Vector_t& hr,
+                            const Vector_t& /*rmin*/, const Vector_t& /*rmax*/, double /*dh*/) {
+        double xmin = getXRangeMin();
+        double xmax = getXRangeMax();
+        double ymin = getYRangeMin();
+        double ymax = getYRangeMax();
+        double zmin = getZRangeMin();
+        double zmax = getZRangeMax();
+
+        origin = Vector_t(xmin, ymin , zmin);
+        Vector_t mymax = Vector_t(xmax, ymax , zmax);
+
+        for (int i = 0; i < 3; i++)
+            hr[i] = (mymax[i] - origin[i]) / nr[i];
+    };
+
 protected:
 
     // a irregular domain is always defined on a grid
diff --git a/src/Solvers/MGPoissonSolver.cpp b/src/Solvers/MGPoissonSolver.cpp
index 57836bfd6ea0634b7c4b8ebb34c2525aefff80b8..ddcf129c7ebe3b38e3ae1374d1de41320d266817 100644
--- a/src/Solvers/MGPoissonSolver.cpp
+++ b/src/Solvers/MGPoissonSolver.cpp
@@ -353,8 +353,10 @@ void MGPoissonSolver::computePotential(Field_t &rho, Vector_t hr) {
     for (int idz = localId[2].first(); idz <= localId[2].last(); idz++) {
         for (int idy = localId[1].first(); idy <= localId[1].last(); idy++) {
             for (int idx = localId[0].first(); idx <= localId[0].last(); idx++) {
+                NDIndex<3> l(Index(idx, idx), Index(idy, idy), Index(idz, idz));
                 if (bp_m->isInside(idx, idy, idz))
-                        RHS->getDataNonConst()[id++] = 4*M_PI*rho[idx][idy][idz].get()/scaleFactor;
+                        RHS->replaceGlobalValue(bp_m->getIdx(idx, idy, idz),
+                                                4.0 * M_PI * rho.localElement(l) / scaleFactor);
             }
         }
     }
@@ -508,6 +510,7 @@ void MGPoissonSolver::IPPLToMap3D(NDIndex<3> localId) {
             }
         }
     }
+
     int indexbase = 0;
     map_p = Teuchos::rcp(new TpetraMap_t(Teuchos::OrdinalTraits<Tpetra::global_size_t>::invalid(),
                                          &MyGlobalElements[0], NumMyElements, indexbase, comm_mp));
diff --git a/src/Solvers/MGPoissonSolver.h b/src/Solvers/MGPoissonSolver.h
index 44bf05aca7449937087cb2a7c415721e9972b1fe..c2e1de9a9fe87dea819218f1b629574b29cd4234 100644
--- a/src/Solvers/MGPoissonSolver.h
+++ b/src/Solvers/MGPoissonSolver.h
@@ -125,8 +125,16 @@ public:
 
     void extrapolateLHS();
 
+    void resizeMesh(Vector_t& origin, Vector_t& hr, const Vector_t& rmin,
+                    const Vector_t& rmax, double dh)
+    {
+        bp_m->resizeMesh(origin, hr, rmin, rmax, dh);
+    }
+
     Inform &print(Inform &os) const;
 
+
+
 private:
 
     bool isMatrixfilled_m;
@@ -171,6 +179,7 @@ private:
 
     /// Map holding the processor distribution of data
     Teuchos::RCP<TpetraMap_t> map_p;
+
     /// communicator used by Trilinos
     Teuchos::RCP<const Comm_t> comm_mp;
 
diff --git a/src/Solvers/PoissonSolver.h b/src/Solvers/PoissonSolver.h
index e2dd5a350b3ec2c1f8a9c2f65e07f8c9592891d6..84aa754cee851194d88d2da0b47501c89b172ed4 100644
--- a/src/Solvers/PoissonSolver.h
+++ b/src/Solvers/PoissonSolver.h
@@ -58,6 +58,11 @@ public:
     virtual void test(PartBunchBase<double, 3> *bunch) = 0 ;
     virtual ~PoissonSolver(){};
 
+    virtual void resizeMesh(Vector_t& /*origin*/, Vector_t& /*hr*/,
+                            const Vector_t& /*rmin*/, const Vector_t& /*rmax*/,
+                            double /*dh*/)
+    { };
+
 };
 
 inline Inform &operator<<(Inform &os, const PoissonSolver &/*fs*/) {