diff --git a/dpsim-models/include/dpsim-models/MNAStampUtils.h b/dpsim-models/include/dpsim-models/MNAStampUtils.h
index fea77ed126..1dca3b4130 100644
--- a/dpsim-models/include/dpsim-models/MNAStampUtils.h
+++ b/dpsim-models/include/dpsim-models/MNAStampUtils.h
@@ -31,6 +31,18 @@ class MNAStampUtils {
       UInt node2Index, Bool isTerminal1NotGrounded, Bool isTerminal2NotGrounded,
       const Logger::Log &mSLog, Int maxFreq = 1, Int freqIdx = 0);
 
+  /// Stamps conductance as a 3x3 scalar matrix (a diagonal matrix, where all diagonal elements are equal to conductance).
+  static void stampConductanceAs3x3ScalarMatrix(
+      Real conductance, SparseMatrixRow &mat, UInt node1Index, UInt node2Index,
+      Bool isTerminal1NotGrounded, Bool isTerminal2NotGrounded,
+      const Logger::Log &mSLog);
+
+  /// Stamps admittance as a 3x3 scalar matrix (a diagonal matrix, where all diagonal elements are equal to admittance).
+  static void stampAdmittanceAs3x3ScalarMatrix(
+      Complex admittance, SparseMatrixRow &mat, UInt node1Index,
+      UInt node2Index, Bool isTerminal1NotGrounded, Bool isTerminal2NotGrounded,
+      const Logger::Log &mSLog, Int maxFreq = 1, Int freqIdx = 0);
+
 private:
   template <typename T>
   static void stampValue(T value, SparseMatrixRow &mat, UInt node1Index,
@@ -45,6 +57,14 @@ class MNAStampUtils {
                           Bool isTerminal2NotGrounded, Int maxFreq, Int freqIdx,
                           const Logger::Log &mSLog);
 
+  template <typename T>
+  static void stampValueAsScalarMatrix(T value, UInt sizeOfScalarMatrix,
+                                       SparseMatrixRow &mat, UInt node1Index,
+                                       UInt node2Index,
+                                       Bool isTerminal1NotGrounded,
+                                       Bool isTerminal2NotGrounded, Int maxFreq,
+                                       Int freqIdx, const Logger::Log &mSLog);
+
   template <typename T>
   static void stampValueNoConditions(T value, SparseMatrixRow &mat,
                                      UInt node1Index, UInt node2Index,
diff --git a/dpsim-models/src/DP/DP_Ph1_Switch.cpp b/dpsim-models/src/DP/DP_Ph1_Switch.cpp
index 5128001dbe..6f7f24d35d 100644
--- a/dpsim-models/src/DP/DP_Ph1_Switch.cpp
+++ b/dpsim-models/src/DP/DP_Ph1_Switch.cpp
@@ -54,38 +54,9 @@ void DP::Ph1::Switch::mnaCompApplySystemMatrixStamp(
   Complex conductance = (**mIsClosed) ? Complex(1. / **mClosedResistance, 0)
                                       : Complex(1. / **mOpenResistance, 0);
 
-  // Set diagonal entries
-  if (terminalNotGrounded(0))
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0),
-                             matrixNodeIndex(0), conductance);
-  if (terminalNotGrounded(1))
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1),
-                             matrixNodeIndex(1), conductance);
-  // Set off diagonal entries
-  if (terminalNotGrounded(0) && terminalNotGrounded(1)) {
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0),
-                             matrixNodeIndex(1), -conductance);
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1),
-                             matrixNodeIndex(0), -conductance);
-  }
-
-  SPDLOG_LOGGER_TRACE(mSLog, "-- Stamp ---");
-  if (terminalNotGrounded(0))
-    SPDLOG_LOGGER_TRACE(mSLog, "Add {:s} to system at ({:d},{:d})",
-                        Logger::complexToString(conductance),
-                        matrixNodeIndex(0), matrixNodeIndex(0));
-  if (terminalNotGrounded(1))
-    SPDLOG_LOGGER_TRACE(mSLog, "Add {:s} to system at ({:d},{:d})",
-                        Logger::complexToString(conductance),
-                        matrixNodeIndex(1), matrixNodeIndex(1));
-  if (terminalNotGrounded(0) && terminalNotGrounded(1)) {
-    SPDLOG_LOGGER_TRACE(mSLog, "Add {:s} to system at ({:d},{:d})",
-                        Logger::complexToString(-conductance),
-                        matrixNodeIndex(0), matrixNodeIndex(1));
-    SPDLOG_LOGGER_TRACE(mSLog, "Add {:s} to system at ({:d},{:d})",
-                        Logger::complexToString(-conductance),
-                        matrixNodeIndex(1), matrixNodeIndex(0));
-  }
+  MNAStampUtils::stampAdmittance(conductance, systemMatrix, matrixNodeIndex(0),
+                                 matrixNodeIndex(1), terminalNotGrounded(0),
+                                 terminalNotGrounded(1), mSLog);
 }
 
 void DP::Ph1::Switch::mnaCompApplySwitchSystemMatrixStamp(
@@ -93,39 +64,9 @@ void DP::Ph1::Switch::mnaCompApplySwitchSystemMatrixStamp(
   Complex conductance = (closed) ? Complex(1. / **mClosedResistance, 0)
                                  : Complex(1. / **mOpenResistance, 0);
 
-  // Set diagonal entries
-  if (terminalNotGrounded(0))
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0),
-                             matrixNodeIndex(0), conductance);
-  if (terminalNotGrounded(1))
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1),
-                             matrixNodeIndex(1), conductance);
-
-  // Set off diagonal entries
-  if (terminalNotGrounded(0) && terminalNotGrounded(1)) {
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0),
-                             matrixNodeIndex(1), -conductance);
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1),
-                             matrixNodeIndex(0), -conductance);
-  }
-
-  SPDLOG_LOGGER_TRACE(mSLog, "-- Stamp ---");
-  if (terminalNotGrounded(0))
-    SPDLOG_LOGGER_TRACE(mSLog, "Add {:s} to system at ({:d},{:d})",
-                        Logger::complexToString(conductance),
-                        matrixNodeIndex(0), matrixNodeIndex(0));
-  if (terminalNotGrounded(1))
-    SPDLOG_LOGGER_TRACE(mSLog, "Add {:s} to system at ({:d},{:d})",
-                        Logger::complexToString(conductance),
-                        matrixNodeIndex(1), matrixNodeIndex(1));
-  if (terminalNotGrounded(0) && terminalNotGrounded(1)) {
-    SPDLOG_LOGGER_TRACE(mSLog, "Add {:s} to system at ({:d},{:d})",
-                        Logger::complexToString(-conductance),
-                        matrixNodeIndex(0), matrixNodeIndex(1));
-    SPDLOG_LOGGER_TRACE(mSLog, "Add {:s} to system at ({:d},{:d})",
-                        Logger::complexToString(-conductance),
-                        matrixNodeIndex(1), matrixNodeIndex(0));
-  }
+  MNAStampUtils::stampAdmittance(conductance, systemMatrix, matrixNodeIndex(0),
+                                 matrixNodeIndex(1), terminalNotGrounded(0),
+                                 terminalNotGrounded(1), mSLog);
 }
 
 void DP::Ph1::Switch::mnaCompApplyRightSideVectorStamp(Matrix &rightVector) {}
diff --git a/dpsim-models/src/DP/DP_Ph1_varResSwitch.cpp b/dpsim-models/src/DP/DP_Ph1_varResSwitch.cpp
index b1667b69fb..c233ea6d09 100644
--- a/dpsim-models/src/DP/DP_Ph1_varResSwitch.cpp
+++ b/dpsim-models/src/DP/DP_Ph1_varResSwitch.cpp
@@ -47,20 +47,9 @@ void DP::Ph1::varResSwitch::mnaCompApplySystemMatrixStamp(
   Complex conductance = (**mIsClosed) ? Complex(1. / **mClosedResistance, 0)
                                       : Complex(1. / **mOpenResistance, 0);
 
-  // Set diagonal entries
-  if (terminalNotGrounded(0))
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0),
-                             matrixNodeIndex(0), conductance);
-  if (terminalNotGrounded(1))
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1),
-                             matrixNodeIndex(1), conductance);
-  // Set off diagonal entries
-  if (terminalNotGrounded(0) && terminalNotGrounded(1)) {
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0),
-                             matrixNodeIndex(1), -conductance);
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1),
-                             matrixNodeIndex(0), -conductance);
-  }
+  MNAStampUtils::stampAdmittance(conductance, systemMatrix, matrixNodeIndex(0),
+                                 matrixNodeIndex(1), terminalNotGrounded(0),
+                                 terminalNotGrounded(1), mSLog);
 }
 
 void DP::Ph1::varResSwitch::mnaCompApplySwitchSystemMatrixStamp(
@@ -68,39 +57,9 @@ void DP::Ph1::varResSwitch::mnaCompApplySwitchSystemMatrixStamp(
   Complex conductance = (closed) ? Complex(1. / **mClosedResistance, 0)
                                  : Complex(1. / **mOpenResistance, 0);
 
-  // Set diagonal entries
-  if (terminalNotGrounded(0))
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0),
-                             matrixNodeIndex(0), conductance);
-  if (terminalNotGrounded(1))
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1),
-                             matrixNodeIndex(1), conductance);
-
-  // Set off diagonal entries
-  if (terminalNotGrounded(0) && terminalNotGrounded(1)) {
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0),
-                             matrixNodeIndex(1), -conductance);
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1),
-                             matrixNodeIndex(0), -conductance);
-  }
-
-  SPDLOG_LOGGER_INFO(mSLog, "-- Stamp ---");
-  if (terminalNotGrounded(0))
-    SPDLOG_LOGGER_INFO(mSLog, "Add {:s} to system at ({:d},{:d})",
-                       Logger::complexToString(conductance), matrixNodeIndex(0),
-                       matrixNodeIndex(0));
-  if (terminalNotGrounded(1))
-    SPDLOG_LOGGER_INFO(mSLog, "Add {:s} to system at ({:d},{:d})",
-                       Logger::complexToString(conductance), matrixNodeIndex(1),
-                       matrixNodeIndex(1));
-  if (terminalNotGrounded(0) && terminalNotGrounded(1)) {
-    SPDLOG_LOGGER_INFO(mSLog, "Add {:s} to system at ({:d},{:d})",
-                       Logger::complexToString(-conductance),
-                       matrixNodeIndex(0), matrixNodeIndex(1));
-    SPDLOG_LOGGER_INFO(mSLog, "Add {:s} to system at ({:d},{:d})",
-                       Logger::complexToString(-conductance),
-                       matrixNodeIndex(1), matrixNodeIndex(0));
-  }
+  MNAStampUtils::stampAdmittance(conductance, systemMatrix, matrixNodeIndex(0),
+                                 matrixNodeIndex(1), terminalNotGrounded(0),
+                                 terminalNotGrounded(1), mSLog);
 }
 
 void DP::Ph1::varResSwitch::mnaCompApplyRightSideVectorStamp(
diff --git a/dpsim-models/src/DP/DP_Ph3_SeriesSwitch.cpp b/dpsim-models/src/DP/DP_Ph3_SeriesSwitch.cpp
index f46382681f..2f2e36ecdf 100644
--- a/dpsim-models/src/DP/DP_Ph3_SeriesSwitch.cpp
+++ b/dpsim-models/src/DP/DP_Ph3_SeriesSwitch.cpp
@@ -50,33 +50,9 @@ void DP::Ph3::SeriesSwitch::mnaCompApplySystemMatrixStamp(
   Complex conductance = (**mIsClosed) ? Complex(1. / **mClosedResistance, 0)
                                       : Complex(1. / **mOpenResistance, 0);
 
-  // Set diagonal entries
-  if (terminalNotGrounded(0))
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndices(0),
-                             matrixNodeIndices(0), conductance);
-  if (terminalNotGrounded(1))
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndices(1),
-                             matrixNodeIndices(1), conductance);
-  // Set off diagonal entries
-  if (terminalNotGrounded(0) && terminalNotGrounded(1)) {
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndices(0),
-                             matrixNodeIndices(1), -conductance);
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndices(1),
-                             matrixNodeIndices(0), -conductance);
-  }
-
-  if (terminalNotGrounded(0))
-    SPDLOG_LOGGER_INFO(mSLog, "Add {} to {}, {}", conductance,
-                       matrixNodeIndices(0)[0], matrixNodeIndices(0)[0]);
-  if (terminalNotGrounded(1))
-    SPDLOG_LOGGER_INFO(mSLog, "Add {} to {}, {}", conductance,
-                       matrixNodeIndices(1)[0], matrixNodeIndices(1)[0]);
-  if (terminalNotGrounded(0) && terminalNotGrounded(1)) {
-    SPDLOG_LOGGER_INFO(mSLog, "Add {} to {}, {}", -conductance,
-                       matrixNodeIndices(0)[0], matrixNodeIndices(1)[0]);
-    SPDLOG_LOGGER_INFO(mSLog, "Add {} to {}, {}", -conductance,
-                       matrixNodeIndices(1)[0], matrixNodeIndices(0)[0]);
-  }
+  MNAStampUtils::stampAdmittanceAs3x3ScalarMatrix(
+      conductance, systemMatrix, matrixNodeIndex(0), matrixNodeIndex(1),
+      terminalNotGrounded(0), terminalNotGrounded(1), mSLog);
 }
 
 void DP::Ph3::SeriesSwitch::mnaCompApplySwitchSystemMatrixStamp(
@@ -84,33 +60,9 @@ void DP::Ph3::SeriesSwitch::mnaCompApplySwitchSystemMatrixStamp(
   Complex conductance = (closed) ? Complex(1. / **mClosedResistance, 0)
                                  : Complex(1. / **mOpenResistance, 0);
 
-  // Set diagonal entries
-  if (terminalNotGrounded(0))
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndices(0),
-                             matrixNodeIndices(0), conductance);
-  if (terminalNotGrounded(1))
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndices(1),
-                             matrixNodeIndices(1), conductance);
-  // Set off diagonal entries
-  if (terminalNotGrounded(0) && terminalNotGrounded(1)) {
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndices(0),
-                             matrixNodeIndices(1), -conductance);
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndices(1),
-                             matrixNodeIndices(0), -conductance);
-  }
-
-  if (terminalNotGrounded(0))
-    SPDLOG_LOGGER_INFO(mSLog, "Add {} to {}, {}", conductance,
-                       matrixNodeIndices(0)[0], matrixNodeIndices(0)[0]);
-  if (terminalNotGrounded(1))
-    SPDLOG_LOGGER_INFO(mSLog, "Add {} to {}, {}", conductance,
-                       matrixNodeIndices(1)[0], matrixNodeIndices(1)[0]);
-  if (terminalNotGrounded(0) && terminalNotGrounded(1)) {
-    SPDLOG_LOGGER_INFO(mSLog, "Add {} to {}, {}", -conductance,
-                       matrixNodeIndices(0)[0], matrixNodeIndices(1)[0]);
-    SPDLOG_LOGGER_INFO(mSLog, "Add {} to {}, {}", -conductance,
-                       matrixNodeIndices(1)[0], matrixNodeIndices(0)[0]);
-  }
+  MNAStampUtils::stampAdmittanceAs3x3ScalarMatrix(
+      conductance, systemMatrix, matrixNodeIndex(0), matrixNodeIndex(1),
+      terminalNotGrounded(0), terminalNotGrounded(1), mSLog);
 }
 
 void DP::Ph3::SeriesSwitch::mnaCompAddPostStepDependencies(
diff --git a/dpsim-models/src/EMT/EMT_Ph3_SeriesSwitch.cpp b/dpsim-models/src/EMT/EMT_Ph3_SeriesSwitch.cpp
index 77060d828a..7a75c76950 100644
--- a/dpsim-models/src/EMT/EMT_Ph3_SeriesSwitch.cpp
+++ b/dpsim-models/src/EMT/EMT_Ph3_SeriesSwitch.cpp
@@ -65,33 +65,9 @@ void EMT::Ph3::SeriesSwitch::mnaCompApplySystemMatrixStamp(
   Real conductance =
       (**mIsClosed) ? 1. / **mClosedResistance : 1. / **mOpenResistance;
 
-  // Set diagonal entries
-  if (terminalNotGrounded(0))
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndices(0),
-                             matrixNodeIndices(0), conductance);
-  if (terminalNotGrounded(1))
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndices(1),
-                             matrixNodeIndices(1), conductance);
-  // Set off diagonal entries
-  if (terminalNotGrounded(0) && terminalNotGrounded(1)) {
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndices(0),
-                             matrixNodeIndices(1), -conductance);
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndices(1),
-                             matrixNodeIndices(0), -conductance);
-  }
-
-  if (terminalNotGrounded(0))
-    SPDLOG_LOGGER_INFO(mSLog, "Add {} to {}, {}", conductance,
-                       matrixNodeIndices(0)[0], matrixNodeIndices(0)[0]);
-  if (terminalNotGrounded(1))
-    SPDLOG_LOGGER_INFO(mSLog, "Add {} to {}, {}", conductance,
-                       matrixNodeIndices(1)[0], matrixNodeIndices(1)[0]);
-  if (terminalNotGrounded(0) && terminalNotGrounded(1)) {
-    SPDLOG_LOGGER_INFO(mSLog, "Add {} to {}, {}", -conductance,
-                       matrixNodeIndices(0)[0], matrixNodeIndices(1)[0]);
-    SPDLOG_LOGGER_INFO(mSLog, "Add {} to {}, {}", -conductance,
-                       matrixNodeIndices(1)[0], matrixNodeIndices(0)[0]);
-  }
+  MNAStampUtils::stampConductanceAs3x3ScalarMatrix(
+      conductance, systemMatrix, matrixNodeIndex(0), matrixNodeIndex(1),
+      terminalNotGrounded(0), terminalNotGrounded(1), mSLog);
 }
 
 void EMT::Ph3::SeriesSwitch::mnaCompApplySwitchSystemMatrixStamp(
@@ -99,33 +75,9 @@ void EMT::Ph3::SeriesSwitch::mnaCompApplySwitchSystemMatrixStamp(
   Real conductance =
       (closed) ? 1. / **mClosedResistance : 1. / **mOpenResistance;
 
-  // Set diagonal entries
-  if (terminalNotGrounded(0))
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndices(0),
-                             matrixNodeIndices(0), conductance);
-  if (terminalNotGrounded(1))
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndices(1),
-                             matrixNodeIndices(1), conductance);
-  // Set off diagonal entries
-  if (terminalNotGrounded(0) && terminalNotGrounded(1)) {
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndices(0),
-                             matrixNodeIndices(1), -conductance);
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndices(1),
-                             matrixNodeIndices(0), -conductance);
-  }
-
-  if (terminalNotGrounded(0))
-    SPDLOG_LOGGER_INFO(mSLog, "Add {} to {}, {}", conductance,
-                       matrixNodeIndices(0)[0], matrixNodeIndices(0)[0]);
-  if (terminalNotGrounded(1))
-    SPDLOG_LOGGER_INFO(mSLog, "Add {} to {}, {}", conductance,
-                       matrixNodeIndices(1)[0], matrixNodeIndices(1)[0]);
-  if (terminalNotGrounded(0) && terminalNotGrounded(1)) {
-    SPDLOG_LOGGER_INFO(mSLog, "Add {} to {}, {}", -conductance,
-                       matrixNodeIndices(0)[0], matrixNodeIndices(1)[0]);
-    SPDLOG_LOGGER_INFO(mSLog, "Add {} to {}, {}", -conductance,
-                       matrixNodeIndices(1)[0], matrixNodeIndices(0)[0]);
-  }
+  MNAStampUtils::stampConductanceAs3x3ScalarMatrix(
+      conductance, systemMatrix, matrixNodeIndex(0), matrixNodeIndex(1),
+      terminalNotGrounded(0), terminalNotGrounded(1), mSLog);
 }
 
 void EMT::Ph3::SeriesSwitch::mnaCompAddPostStepDependencies(
diff --git a/dpsim-models/src/EMT/EMT_Ph3_Switch.cpp b/dpsim-models/src/EMT/EMT_Ph3_Switch.cpp
index 23e725e679..8eeb521d27 100644
--- a/dpsim-models/src/EMT/EMT_Ph3_Switch.cpp
+++ b/dpsim-models/src/EMT/EMT_Ph3_Switch.cpp
@@ -65,89 +65,10 @@ void EMT::Ph3::Switch::mnaCompApplySystemMatrixStamp(
                                           ? (**mClosedResistance).inverse()
                                           : (**mOpenResistance).inverse();
 
-  // Set diagonal entries
-  if (terminalNotGrounded(0)) {
-    // set upper left block, 3x3 entries
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 0),
-                             matrixNodeIndex(0, 0), conductance(0, 0));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 0),
-                             matrixNodeIndex(0, 1), conductance(0, 1));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 0),
-                             matrixNodeIndex(0, 2), conductance(0, 2));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 1),
-                             matrixNodeIndex(0, 0), conductance(1, 0));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 1),
-                             matrixNodeIndex(0, 1), conductance(1, 1));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 1),
-                             matrixNodeIndex(0, 2), conductance(1, 2));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 2),
-                             matrixNodeIndex(0, 0), conductance(2, 0));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 2),
-                             matrixNodeIndex(0, 1), conductance(2, 1));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 2),
-                             matrixNodeIndex(0, 2), conductance(2, 2));
-  }
-  if (terminalNotGrounded(1)) {
-    // set buttom right block, 3x3 entries
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 0),
-                             matrixNodeIndex(1, 0), conductance(0, 0));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 0),
-                             matrixNodeIndex(1, 1), conductance(0, 1));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 0),
-                             matrixNodeIndex(1, 2), conductance(0, 2));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 1),
-                             matrixNodeIndex(1, 0), conductance(1, 0));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 1),
-                             matrixNodeIndex(1, 1), conductance(1, 1));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 1),
-                             matrixNodeIndex(1, 2), conductance(1, 2));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 2),
-                             matrixNodeIndex(1, 0), conductance(2, 0));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 2),
-                             matrixNodeIndex(1, 1), conductance(2, 1));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 2),
-                             matrixNodeIndex(1, 2), conductance(2, 2));
-  }
-  // Set off diagonal blocks, 2x3x3 entries
-  if (terminalNotGrounded(0) && terminalNotGrounded(1)) {
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 0),
-                             matrixNodeIndex(1, 0), -conductance(0, 0));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 0),
-                             matrixNodeIndex(1, 1), -conductance(0, 1));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 0),
-                             matrixNodeIndex(1, 2), -conductance(0, 2));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 1),
-                             matrixNodeIndex(1, 0), -conductance(1, 0));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 1),
-                             matrixNodeIndex(1, 1), -conductance(1, 1));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 1),
-                             matrixNodeIndex(1, 2), -conductance(1, 2));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 2),
-                             matrixNodeIndex(1, 0), -conductance(2, 0));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 2),
-                             matrixNodeIndex(1, 1), -conductance(2, 1));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 2),
-                             matrixNodeIndex(1, 2), -conductance(2, 2));
+  MNAStampUtils::stampConductanceMatrix(
+      conductance, systemMatrix, matrixNodeIndex(0), matrixNodeIndex(1),
+      terminalNotGrounded(0), terminalNotGrounded(1), mSLog);
 
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 0),
-                             matrixNodeIndex(0, 0), -conductance(0, 0));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 0),
-                             matrixNodeIndex(0, 1), -conductance(0, 1));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 0),
-                             matrixNodeIndex(0, 2), -conductance(0, 2));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 1),
-                             matrixNodeIndex(0, 0), -conductance(1, 0));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 1),
-                             matrixNodeIndex(0, 1), -conductance(1, 1));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 1),
-                             matrixNodeIndex(0, 2), -conductance(1, 2));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 2),
-                             matrixNodeIndex(0, 0), -conductance(2, 0));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 2),
-                             matrixNodeIndex(0, 1), -conductance(2, 1));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 2),
-                             matrixNodeIndex(0, 2), -conductance(2, 2));
-  }
   SPDLOG_LOGGER_TRACE(mSLog, "\nConductance matrix: {:s}",
                       Logger::matrixToString(conductance));
 }
@@ -157,89 +78,9 @@ void EMT::Ph3::Switch::mnaCompApplySwitchSystemMatrixStamp(
   MatrixFixedSize<3, 3> conductance = (closed) ? (**mClosedResistance).inverse()
                                                : (**mOpenResistance).inverse();
 
-  // Set diagonal entries
-  if (terminalNotGrounded(0)) {
-    // set upper left block, 3x3 entries
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 0),
-                             matrixNodeIndex(0, 0), conductance(0, 0));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 0),
-                             matrixNodeIndex(0, 1), conductance(0, 1));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 0),
-                             matrixNodeIndex(0, 2), conductance(0, 2));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 1),
-                             matrixNodeIndex(0, 0), conductance(1, 0));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 1),
-                             matrixNodeIndex(0, 1), conductance(1, 1));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 1),
-                             matrixNodeIndex(0, 2), conductance(1, 2));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 2),
-                             matrixNodeIndex(0, 0), conductance(2, 0));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 2),
-                             matrixNodeIndex(0, 1), conductance(2, 1));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 2),
-                             matrixNodeIndex(0, 2), conductance(2, 2));
-  }
-  if (terminalNotGrounded(1)) {
-    // set buttom right block, 3x3 entries
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 0),
-                             matrixNodeIndex(1, 0), conductance(0, 0));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 0),
-                             matrixNodeIndex(1, 1), conductance(0, 1));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 0),
-                             matrixNodeIndex(1, 2), conductance(0, 2));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 1),
-                             matrixNodeIndex(1, 0), conductance(1, 0));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 1),
-                             matrixNodeIndex(1, 1), conductance(1, 1));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 1),
-                             matrixNodeIndex(1, 2), conductance(1, 2));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 2),
-                             matrixNodeIndex(1, 0), conductance(2, 0));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 2),
-                             matrixNodeIndex(1, 1), conductance(2, 1));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 2),
-                             matrixNodeIndex(1, 2), conductance(2, 2));
-  }
-  // Set off diagonal blocks, 2x3x3 entries
-  if (terminalNotGrounded(0) && terminalNotGrounded(1)) {
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 0),
-                             matrixNodeIndex(1, 0), -conductance(0, 0));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 0),
-                             matrixNodeIndex(1, 1), -conductance(0, 1));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 0),
-                             matrixNodeIndex(1, 2), -conductance(0, 2));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 1),
-                             matrixNodeIndex(1, 0), -conductance(1, 0));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 1),
-                             matrixNodeIndex(1, 1), -conductance(1, 1));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 1),
-                             matrixNodeIndex(1, 2), -conductance(1, 2));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 2),
-                             matrixNodeIndex(1, 0), -conductance(2, 0));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 2),
-                             matrixNodeIndex(1, 1), -conductance(2, 1));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0, 2),
-                             matrixNodeIndex(1, 2), -conductance(2, 2));
-
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 0),
-                             matrixNodeIndex(0, 0), -conductance(0, 0));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 0),
-                             matrixNodeIndex(0, 1), -conductance(0, 1));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 0),
-                             matrixNodeIndex(0, 2), -conductance(0, 2));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 1),
-                             matrixNodeIndex(0, 0), -conductance(1, 0));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 1),
-                             matrixNodeIndex(0, 1), -conductance(1, 1));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 1),
-                             matrixNodeIndex(0, 2), -conductance(1, 2));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 2),
-                             matrixNodeIndex(0, 0), -conductance(2, 0));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 2),
-                             matrixNodeIndex(0, 1), -conductance(2, 1));
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1, 2),
-                             matrixNodeIndex(0, 2), -conductance(2, 2));
-  }
+  MNAStampUtils::stampConductanceMatrix(
+      conductance, systemMatrix, matrixNodeIndex(0), matrixNodeIndex(1),
+      terminalNotGrounded(0), terminalNotGrounded(1), mSLog);
 
   SPDLOG_LOGGER_TRACE(mSLog, "\nConductance matrix: {:s}",
                       Logger::matrixToString(conductance));
diff --git a/dpsim-models/src/MNAStampUtils.cpp b/dpsim-models/src/MNAStampUtils.cpp
index 4bb7b6bb0f..de3159f9ad 100644
--- a/dpsim-models/src/MNAStampUtils.cpp
+++ b/dpsim-models/src/MNAStampUtils.cpp
@@ -22,8 +22,7 @@ void MNAStampUtils::stampAdmittance(Complex admittance, SparseMatrixRow &mat,
                                     const Logger::Log &mSLog, Int maxFreq,
                                     Int freqIdx) {
   SPDLOG_LOGGER_DEBUG(
-      mSLog, "Start stamping admittance for frequency index {:d}...",
-      freqIdx);
+      mSLog, "Start stamping admittance for frequency index {:d}...", freqIdx);
 
   stampValue(admittance, mat, node1Index, node2Index, isTerminal1NotGrounded,
              isTerminal2NotGrounded, maxFreq, freqIdx, mSLog);
@@ -50,8 +49,7 @@ void MNAStampUtils::stampAdmittanceMatrix(
     UInt node2Index, Bool isTerminal1NotGrounded, Bool isTerminal2NotGrounded,
     const Logger::Log &mSLog, Int maxFreq, Int freqIdx) {
   SPDLOG_LOGGER_DEBUG(
-      mSLog,
-      "Start stamping admittance matrix for frequency index {:d}...",
+      mSLog, "Start stamping admittance matrix for frequency index {:d}...",
       freqIdx);
 
   stampMatrix(admittanceMat, mat, node1Index, node2Index,
@@ -61,6 +59,36 @@ void MNAStampUtils::stampAdmittanceMatrix(
   SPDLOG_LOGGER_DEBUG(mSLog, "Stamping completed.");
 }
 
+void MNAStampUtils::stampConductanceAs3x3ScalarMatrix(
+    Real conductance, SparseMatrixRow &mat, UInt node1Index, UInt node2Index,
+    Bool isTerminal1NotGrounded, Bool isTerminal2NotGrounded,
+    const Logger::Log &mSLog) {
+  SPDLOG_LOGGER_DEBUG(mSLog,
+                      "Start stamping conductance as 3x3 scalar matrix...");
+
+  stampValueAsScalarMatrix(conductance, 3, mat, node1Index, node2Index,
+                           isTerminal1NotGrounded, isTerminal2NotGrounded, 1, 0,
+                           mSLog);
+
+  SPDLOG_LOGGER_DEBUG(mSLog, "Stamping completed.");
+}
+
+void MNAStampUtils::stampAdmittanceAs3x3ScalarMatrix(
+    Complex admittance, SparseMatrixRow &mat, UInt node1Index, UInt node2Index,
+    Bool isTerminal1NotGrounded, Bool isTerminal2NotGrounded,
+    const Logger::Log &mSLog, Int maxFreq, Int freqIdx) {
+  SPDLOG_LOGGER_DEBUG(mSLog,
+                      "Start stamping admittance as 3x3 scalar matrix for "
+                      "frequency index {:d}...",
+                      freqIdx);
+
+  stampValueAsScalarMatrix(admittance, 3, mat, node1Index, node2Index,
+                           isTerminal1NotGrounded, isTerminal2NotGrounded,
+                           maxFreq, freqIdx, mSLog);
+
+  SPDLOG_LOGGER_DEBUG(mSLog, "Stamping completed.");
+}
+
 template <typename T>
 void MNAStampUtils::stampValue(T value, SparseMatrixRow &mat, UInt node1Index,
                                UInt node2Index, Bool isTerminal1NotGrounded,
@@ -114,6 +142,29 @@ void MNAStampUtils::stampMatrix(const MatrixVar<T> &matrix,
   }
 }
 
+template <typename T>
+void MNAStampUtils::stampValueAsScalarMatrix(
+    T value, UInt sizeOfScalarMatrix, SparseMatrixRow &mat, UInt node1Index,
+    UInt node2Index, Bool isTerminal1NotGrounded, Bool isTerminal2NotGrounded,
+    Int maxFreq, Int freqIdx, const Logger::Log &mSLog) {
+  if (isTerminal1NotGrounded && isTerminal2NotGrounded) {
+    for (UInt i = 0; i < sizeOfScalarMatrix; i++) {
+      stampValueNoConditions(value, mat, node1Index + i, node2Index + i,
+                             maxFreq, freqIdx, mSLog);
+    }
+  } else if (isTerminal1NotGrounded) {
+    for (UInt i = 0; i < sizeOfScalarMatrix; i++) {
+      stampValueOnDiagonalNoConditions(value, mat, node1Index + i, maxFreq,
+                                       freqIdx, mSLog);
+    }
+  } else if (isTerminal2NotGrounded) {
+    for (UInt i = 0; i < sizeOfScalarMatrix; i++) {
+      stampValueOnDiagonalNoConditions(value, mat, node2Index + i, maxFreq,
+                                       freqIdx, mSLog);
+    }
+  }
+}
+
 template <typename T>
 void MNAStampUtils::stampValueNoConditions(T value, SparseMatrixRow &mat,
                                            UInt node1Index, UInt node2Index,
@@ -152,7 +203,8 @@ void MNAStampUtils::addToMatrixElement(SparseMatrixRow &mat, Matrix::Index row,
                                        Matrix::Index column, Real value,
                                        Int maxFreq, Int freqIdx,
                                        const Logger::Log &mSLog) {
-  SPDLOG_LOGGER_DEBUG(mSLog, "- Adding {:s} to system matrix element ({:d},{:d})",
+  SPDLOG_LOGGER_DEBUG(mSLog,
+                      "- Adding {:s} to system matrix element ({:d},{:d})",
                       Logger::realToString(value), row, column);
 
   Math::addToMatrixElement(mat, row, column, value);
@@ -162,7 +214,8 @@ void MNAStampUtils::addToMatrixElement(SparseMatrixRow &mat, Matrix::Index row,
                                        Matrix::Index column, Complex value,
                                        Int maxFreq, Int freqIdx,
                                        const Logger::Log &mSLog) {
-  SPDLOG_LOGGER_DEBUG(mSLog, "- Adding {:s} to system matrix element ({:d},{:d})",
+  SPDLOG_LOGGER_DEBUG(mSLog,
+                      "- Adding {:s} to system matrix element ({:d},{:d})",
                       Logger::complexToString(value), row, column);
 
   Math::addToMatrixElement(mat, row, column, value, maxFreq, freqIdx);
diff --git a/dpsim-models/src/SP/SP_Ph1_Switch.cpp b/dpsim-models/src/SP/SP_Ph1_Switch.cpp
index c8708b4089..d10ffc2bb1 100644
--- a/dpsim-models/src/SP/SP_Ph1_Switch.cpp
+++ b/dpsim-models/src/SP/SP_Ph1_Switch.cpp
@@ -56,38 +56,9 @@ void SP::Ph1::Switch::mnaCompApplySystemMatrixStamp(
   Complex conductance = (**mIsClosed) ? Complex(1. / **mClosedResistance, 0)
                                       : Complex(1. / **mOpenResistance, 0);
 
-  // Set diagonal entries
-  if (terminalNotGrounded(0))
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0),
-                             matrixNodeIndex(0), conductance);
-  if (terminalNotGrounded(1))
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1),
-                             matrixNodeIndex(1), conductance);
-  // Set off diagonal entries
-  if (terminalNotGrounded(0) && terminalNotGrounded(1)) {
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0),
-                             matrixNodeIndex(1), -conductance);
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1),
-                             matrixNodeIndex(0), -conductance);
-  }
-
-  SPDLOG_LOGGER_TRACE(mSLog, "-- Stamp ---");
-  if (terminalNotGrounded(0))
-    SPDLOG_LOGGER_TRACE(mSLog, "Add {:s} to system at ({:d},{:d})",
-                        Logger::complexToString(conductance),
-                        matrixNodeIndex(0), matrixNodeIndex(0));
-  if (terminalNotGrounded(1))
-    SPDLOG_LOGGER_TRACE(mSLog, "Add {:s} to system at ({:d},{:d})",
-                        Logger::complexToString(conductance),
-                        matrixNodeIndex(1), matrixNodeIndex(1));
-  if (terminalNotGrounded(0) && terminalNotGrounded(1)) {
-    SPDLOG_LOGGER_TRACE(mSLog, "Add {:s} to system at ({:d},{:d})",
-                        Logger::complexToString(-conductance),
-                        matrixNodeIndex(0), matrixNodeIndex(1));
-    SPDLOG_LOGGER_TRACE(mSLog, "Add {:s} to system at ({:d},{:d})",
-                        Logger::complexToString(-conductance),
-                        matrixNodeIndex(1), matrixNodeIndex(0));
-  }
+  MNAStampUtils::stampAdmittance(conductance, systemMatrix, matrixNodeIndex(0),
+                                 matrixNodeIndex(1), terminalNotGrounded(0),
+                                 terminalNotGrounded(1), mSLog);
 }
 
 void SP::Ph1::Switch::mnaCompApplySwitchSystemMatrixStamp(
@@ -95,39 +66,9 @@ void SP::Ph1::Switch::mnaCompApplySwitchSystemMatrixStamp(
   Complex conductance = (closed) ? Complex(1. / **mClosedResistance, 0)
                                  : Complex(1. / **mOpenResistance, 0);
 
-  // Set diagonal entries
-  if (terminalNotGrounded(0))
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0),
-                             matrixNodeIndex(0), conductance);
-  if (terminalNotGrounded(1))
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1),
-                             matrixNodeIndex(1), conductance);
-
-  // Set off diagonal entries
-  if (terminalNotGrounded(0) && terminalNotGrounded(1)) {
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0),
-                             matrixNodeIndex(1), -conductance);
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1),
-                             matrixNodeIndex(0), -conductance);
-  }
-
-  SPDLOG_LOGGER_TRACE(mSLog, "-- Stamp ---");
-  if (terminalNotGrounded(0))
-    SPDLOG_LOGGER_TRACE(mSLog, "Add {:s} to system at ({:d},{:d})",
-                        Logger::complexToString(conductance),
-                        matrixNodeIndex(0), matrixNodeIndex(0));
-  if (terminalNotGrounded(1))
-    SPDLOG_LOGGER_TRACE(mSLog, "Add {:s} to system at ({:d},{:d})",
-                        Logger::complexToString(conductance),
-                        matrixNodeIndex(1), matrixNodeIndex(1));
-  if (terminalNotGrounded(0) && terminalNotGrounded(1)) {
-    SPDLOG_LOGGER_TRACE(mSLog, "Add {:s} to system at ({:d},{:d})",
-                        Logger::complexToString(-conductance),
-                        matrixNodeIndex(0), matrixNodeIndex(1));
-    SPDLOG_LOGGER_TRACE(mSLog, "Add {:s} to system at ({:d},{:d})",
-                        Logger::complexToString(-conductance),
-                        matrixNodeIndex(1), matrixNodeIndex(0));
-  }
+  MNAStampUtils::stampAdmittance(conductance, systemMatrix, matrixNodeIndex(0),
+                                 matrixNodeIndex(1), terminalNotGrounded(0),
+                                 terminalNotGrounded(1), mSLog);
 }
 
 void SP::Ph1::Switch::mnaCompApplyRightSideVectorStamp(Matrix &rightVector) {}
diff --git a/dpsim-models/src/SP/SP_Ph1_varResSwitch.cpp b/dpsim-models/src/SP/SP_Ph1_varResSwitch.cpp
index 039dd88858..23daa31cef 100644
--- a/dpsim-models/src/SP/SP_Ph1_varResSwitch.cpp
+++ b/dpsim-models/src/SP/SP_Ph1_varResSwitch.cpp
@@ -49,20 +49,9 @@ void SP::Ph1::varResSwitch::mnaCompApplySystemMatrixStamp(
   Complex conductance = (**mIsClosed) ? Complex(1. / **mClosedResistance, 0)
                                       : Complex(1. / **mOpenResistance, 0);
 
-  // Set diagonal entries
-  if (terminalNotGrounded(0))
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0),
-                             matrixNodeIndex(0), conductance);
-  if (terminalNotGrounded(1))
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1),
-                             matrixNodeIndex(1), conductance);
-  // Set off diagonal entries
-  if (terminalNotGrounded(0) && terminalNotGrounded(1)) {
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0),
-                             matrixNodeIndex(1), -conductance);
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1),
-                             matrixNodeIndex(0), -conductance);
-  }
+  MNAStampUtils::stampAdmittance(conductance, systemMatrix, matrixNodeIndex(0),
+                                 matrixNodeIndex(1), terminalNotGrounded(0),
+                                 terminalNotGrounded(1), mSLog);
 }
 
 void SP::Ph1::varResSwitch::mnaCompApplySwitchSystemMatrixStamp(
@@ -70,39 +59,9 @@ void SP::Ph1::varResSwitch::mnaCompApplySwitchSystemMatrixStamp(
   Complex conductance = (closed) ? Complex(1. / **mClosedResistance, 0)
                                  : Complex(1. / **mOpenResistance, 0);
 
-  // Set diagonal entries
-  if (terminalNotGrounded(0))
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0),
-                             matrixNodeIndex(0), conductance);
-  if (terminalNotGrounded(1))
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1),
-                             matrixNodeIndex(1), conductance);
-
-  // Set off diagonal entries
-  if (terminalNotGrounded(0) && terminalNotGrounded(1)) {
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(0),
-                             matrixNodeIndex(1), -conductance);
-    Math::addToMatrixElement(systemMatrix, matrixNodeIndex(1),
-                             matrixNodeIndex(0), -conductance);
-  }
-
-  SPDLOG_LOGGER_INFO(mSLog, "-- Stamp ---");
-  if (terminalNotGrounded(0))
-    SPDLOG_LOGGER_INFO(mSLog, "Add {:s} to system at ({:d},{:d})",
-                       Logger::complexToString(conductance), matrixNodeIndex(0),
-                       matrixNodeIndex(0));
-  if (terminalNotGrounded(1))
-    SPDLOG_LOGGER_INFO(mSLog, "Add {:s} to system at ({:d},{:d})",
-                       Logger::complexToString(conductance), matrixNodeIndex(1),
-                       matrixNodeIndex(1));
-  if (terminalNotGrounded(0) && terminalNotGrounded(1)) {
-    SPDLOG_LOGGER_INFO(mSLog, "Add {:s} to system at ({:d},{:d})",
-                       Logger::complexToString(-conductance),
-                       matrixNodeIndex(0), matrixNodeIndex(1));
-    SPDLOG_LOGGER_INFO(mSLog, "Add {:s} to system at ({:d},{:d})",
-                       Logger::complexToString(-conductance),
-                       matrixNodeIndex(1), matrixNodeIndex(0));
-  }
+  MNAStampUtils::stampAdmittance(conductance, systemMatrix, matrixNodeIndex(0),
+                                 matrixNodeIndex(1), terminalNotGrounded(0),
+                                 terminalNotGrounded(1), mSLog);
 }
 
 void SP::Ph1::varResSwitch::mnaCompApplyRightSideVectorStamp(