From 2a1dab0624515f040d082adc8f98f36e03762bd9 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Tue, 19 Oct 2021 17:12:28 -0600
Subject: [PATCH 01/95] Very broken

---
 include/kkt_system.h           | 45 ++++++++++------
 source/kkt_system.cc           | 94 ++++++++++++++++++++++------------
 source/schur_preconditioner.cc | 30 +++++------
 source/watchdog_main.cc        | 48 ++++++++---------
 4 files changed, 128 insertions(+), 89 deletions(-)

diff --git a/include/kkt_system.h b/include/kkt_system.h
index 89f1d48..ac0af74 100644
--- a/include/kkt_system.h
+++ b/include/kkt_system.h
@@ -18,6 +18,7 @@
 #include <deal.II/lac/packaged_operation.h>
 #include <deal.II/lac/sparse_direct.h>
 #include <deal.II/lac/affine_constraints.h>
+#include <deal.II/lac/generic_linear_algebra.h>
 
 #include <deal.II/grid/tria.h>
 #include <deal.II/grid/grid_generator.h>
@@ -38,6 +39,9 @@
 #include <deal.II/numerics/error_estimator.h>
 #include <deal.II/hp/fe_collection.h>
 
+#include <deal.II/distributed/tria.h>
+#include <deal.II/distributed/grid_refinement.h>
+
 #include "../include/schur_preconditioner.h"
 #include "../include/density_filter.h"
 
@@ -45,12 +49,21 @@
 #include <fstream>
 #include <algorithm>
 namespace SAND {
+    namespace LA
+    {
+        using namespace dealii::LinearAlgebraPETSc;
+    }
     using namespace dealii;
 
     template<int dim>
     class KktSystem {
 
     public:
+        MPI_Comm mpi_communicator;
+
+        std::vector<IndexSet> owned_partitioning;
+        std::vector<IndexSet> relevant_partitioning;
+
         KktSystem();
 
         void
@@ -66,47 +79,47 @@ namespace SAND {
         setup_block_system();
 
         void
-        assemble_block_system(const BlockVector<double> &state, const double barrier_size);
+        assemble_block_system(const LA::MPI::BlockVector &state, const double barrier_size);
 
-        BlockVector<double>
-        solve(const BlockVector<double> &state, double barrier_size);
+        LA::MPI::BlockVector
+        solve(const LA::MPI::BlockVector &state, double barrier_size);
 
-        BlockVector<double>
+        LA::MPI::BlockVector
         get_initial_state();
 
         double
-        calculate_objective_value(const BlockVector<double> &state) const;
+        calculate_objective_value(const LA::MPI::BlockVector &state) const;
 
         double
-        calculate_barrier_distance(const BlockVector<double> &state) const;
+        calculate_barrier_distance(const LA::MPI::BlockVector &state) const;
 
         double
-        calculate_feasibility(const BlockVector<double> &state, const double barrier_size) const;
+        calculate_feasibility(const LA::MPI::BlockVector &state, const double barrier_size) const;
 
         double
-        calculate_convergence(const BlockVector<double> &state) const;
+        calculate_convergence(const LA::MPI::BlockVector &state) const;
 
         void
-        output(const BlockVector<double> &state, const unsigned int j) const;
+        output(const LA::MPI::BlockVector &state, const unsigned int j) const;
 
         void
         calculate_initial_rhs_error();
 
         double
-        calculate_rhs_norm(const BlockVector<double> &state, const double barrier_size) const;
+        calculate_rhs_norm(const LA::MPI::BlockVector &state, const double barrier_size) const;
 
         void
-        output_stl(const BlockVector<double> &state);
+        output_stl(const LA::MPI::BlockVector &state);
 
     private:
 
-        BlockVector<double>
-        calculate_rhs(const BlockVector<double> &test_solution, const double barrier_size) const;
+        LA::MPI::BlockVector
+        calculate_rhs(const LA::MPI::BlockVector &test_solution, const double barrier_size) const;
 
         BlockSparsityPattern sparsity_pattern;
-        BlockSparseMatrix<double> system_matrix;
-        BlockVector<double> linear_solution;
-        BlockVector<double> system_rhs;
+        LA::MPI::BlockSparseMatrix system_matrix;
+        LA::MPI::BlockVector linear_solution;
+        LA::MPI::BlockVector system_rhs;
         Triangulation<dim> triangulation;
         DoFHandler<dim> dof_handler;
         AffineConstraints<double> constraints;
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index e505e36..2c0c06b 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -560,6 +560,32 @@ namespace SAND {
         const unsigned int n_p = dofs_per_block[0];
         const unsigned int n_u = dofs_per_block[1];
         std::cout << "n_p:  " << n_p << "   n_u:  " << n_u << std::endl;
+
+        owned_partitioning.resize(10);
+        owned_partitioning[0] = dof_handler.locally_owned_dofs().get_view(0, n_p);
+        owned_partitioning[1] = dof_handler.locally_owned_dofs().get_view(n_p, 2*n_p);
+        owned_partitioning[2] = dof_handler.locally_owned_dofs().get_view(2*n_p, 3*n_p);
+        owned_partitioning[3] = dof_handler.locally_owned_dofs().get_view(3*n_p, 4*n_p);
+        owned_partitioning[4] = dof_handler.locally_owned_dofs().get_view(4*n_p, 5*n_p);
+        owned_partitioning[5] = dof_handler.locally_owned_dofs().get_view(5*n_p, 5*n_p+n_u);
+        owned_partitioning[6] = dof_handler.locally_owned_dofs().get_view(5*n_p+n_u, 5*n_p+2*n_u);
+        owned_partitioning[7] = dof_handler.locally_owned_dofs().get_view(5*n_p+2*n_u, 6*n_p+2*n_u);
+        owned_partitioning[8] = dof_handler.locally_owned_dofs().get_view(6*n_p+2*n_u, 7*n_p+2*n_u);
+        owned_partitioning[9] = dof_handler.locally_owned_dofs().get_view(7*n_p+2*n_u, 7*n_p+2*n_u + 1);
+        IndexSet locally_relevant_dofs;
+        DoFTools::extract_locally_relevant_dofs(dof_handler, locally_relevant_dofs);
+        relevant_partitioning.resize(2);
+        relevant_partitioning[0] = locally_relevant_dofs.get_view(0, n_p);
+        relevant_partitioning[1] = locally_relevant_dofs.get_view(n_p, 2*n_p);
+        relevant_partitioning[2] = locally_relevant_dofs.get_view(2*n_p, 3*n_p);
+        relevant_partitioning[3] = locally_relevant_dofs.get_view(3*n_p, 4*n_p);
+        relevant_partitioning[4] = locally_relevant_dofs.get_view(4*n_p, 5*n_p);
+        relevant_partitioning[5] = locally_relevant_dofs.get_view(5*n_p, 5*n_p+n_u);
+        relevant_partitioning[6] = locally_relevant_dofs.get_view(5*n_p+n_u, 5*n_p+2*n_u);
+        relevant_partitioning[7] = locally_relevant_dofs.get_view(5*n_p+2*n_u, 6*n_p+2*n_u);
+        relevant_partitioning[8] = locally_relevant_dofs.get_view(6*n_p+2*n_u, 7*n_p+2*n_u);
+        relevant_partitioning[9] = locally_relevant_dofs.get_view(7*n_p+2*n_u, 7*n_p+2*n_u + 1);
+
         const std::vector<unsigned int> block_sizes = {n_p, n_p, n_p, n_p, n_p, n_u, n_u, n_p, n_p, 1};
 
         BlockDynamicSparsityPattern dsp(10, 10);
@@ -627,7 +653,7 @@ namespace SAND {
         coupling[SolutionComponents::density_upper_slack_multiplier<dim>][SolutionComponents::density_upper_slack<dim>] = DoFTools::always;
 
         coupling[SolutionComponents::density_upper_slack_multiplier<dim>][SolutionComponents::density_upper_slack_multiplier<dim>] = DoFTools::always;
-
+        constraints.reinit(locally_relevant_dofs);
         constraints.clear();
         constraints.close();
 
@@ -653,15 +679,15 @@ namespace SAND {
         std::ofstream out("sparsity.plt");
         sparsity_pattern.print_gnuplot(out);
 
-        system_matrix.reinit(sparsity_pattern);
+        system_matrix.reinit(owned_partitioning, sparsity_pattern, mpi_communicator);
 
 
-        linear_solution.reinit(block_sizes);
-        system_rhs.reinit(block_sizes);
+        linear_solution.reinit(owned_partitioning, relevant_partitioning, mpi_communicator);
+        system_rhs.reinit(owned_partitioning,mpi_communicator);
 
         for (unsigned int j = 0; j < 10; j++) {
-            linear_solution.block(j).reinit(block_sizes[j]);
-            system_rhs.block(j).reinit(block_sizes[j]);
+            linear_solution.block(j).reinit(block_sizes[j],mpi_communicator);
+            system_rhs.block(j).reinit(block_sizes[j],mpi_communicator);
         }
 
         linear_solution.collect_sizes();
@@ -673,7 +699,7 @@ namespace SAND {
 
     template<int dim>
     void
-    KktSystem<dim>::assemble_block_system(const BlockVector<double> &state, const double barrier_size) {
+    KktSystem<dim>::assemble_block_system(const LA::MPI::BlockVector &state, const double barrier_size) {
         /*Remove any values from old iterations*/
         system_matrix.reinit(sparsity_pattern);
         linear_solution = 0;
@@ -703,7 +729,7 @@ namespace SAND {
                                              update_values);
 
         FullMatrix<double> cell_matrix;
-        Vector<double> cell_rhs;
+        LA::MPI::Vector cell_rhs;
         std::vector<types::global_dof_index> local_dof_indices;
 
         const FEValuesExtractors::Scalar densities(SolutionComponents::density<dim>);
@@ -723,8 +749,8 @@ namespace SAND {
 
         const Functions::ConstantFunction<dim> lambda(1.), mu(1.);
 
-        BlockVector<double> filtered_unfiltered_density_solution = state;
-        BlockVector<double> filter_adjoint_unfiltered_density_multiplier_solution = state;
+        LA::MPI::BlockVector filtered_unfiltered_density_solution(state);
+        LA::MPI::BlockVector filter_adjoint_unfiltered_density_multiplier_solution = state;
         filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = 0;
         filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier) = 0;
 
@@ -1044,7 +1070,7 @@ namespace SAND {
 
     template<int dim>
     double
-    KktSystem<dim>::calculate_objective_value(const BlockVector<double> &state) const {
+    KktSystem<dim>::calculate_objective_value(const LA::MPI::BlockVector &state) const {
         /*Remove any values from old iterations*/
 
         QGauss<dim> nine_quadrature(fe_nine.degree + 1);
@@ -1071,7 +1097,7 @@ namespace SAND {
                                              update_values);
 
         FullMatrix<double> cell_matrix;
-        Vector<double> cell_rhs;
+        LA::MPI::Vector cell_rhs;
 
         const FEValuesExtractors::Vector displacements(SolutionComponents::displacement<dim>);
 
@@ -1126,7 +1152,7 @@ namespace SAND {
     //As the KKT System know which vectors correspond to the slack variables, the sum of the logs of the slacks is computed here for use in the filter.
     template<int dim>
     double
-    KktSystem<dim>::calculate_barrier_distance(const BlockVector<double> &state) const {
+    KktSystem<dim>::calculate_barrier_distance(const LA::MPI::BlockVector &state) const {
         double barrier_distance_log_sum = 0;
         unsigned int vect_size = state.block(SolutionBlocks::density_lower_slack).size();
         for (unsigned int k = 0; k < vect_size; k++) {
@@ -1140,7 +1166,7 @@ namespace SAND {
 
     template<int dim>
     double
-    KktSystem<dim>::calculate_rhs_norm(const BlockVector<double> &state, const double barrier_size) const {
+    KktSystem<dim>::calculate_rhs_norm(const LA::MPI::BlockVector &state, const double barrier_size) const {
         return calculate_rhs(state, barrier_size).l2_norm();
     }
 
@@ -1148,8 +1174,8 @@ namespace SAND {
     //Feasibility conditions appear on the RHS of the linear system, so I compute the RHS to find it. Could probably be combined with the objective value finding part to make it faster.
     template<int dim>
     double
-    KktSystem<dim>::calculate_feasibility(const BlockVector<double> &state, const double barrier_size) const {
-        BlockVector<double> test_rhs = calculate_rhs(state, barrier_size);
+    KktSystem<dim>::calculate_feasibility(const LA::MPI::BlockVector &state, const double barrier_size) const {
+        LA::MPI::BlockVector test_rhs = calculate_rhs(state, barrier_size);
 //        double feasibility = 0;
 //        feasibility +=
 //                test_rhs.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm() +
@@ -1190,8 +1216,8 @@ namespace SAND {
 
     template<int dim>
     double
-    KktSystem<dim>::calculate_convergence(const BlockVector<double> &state) const {
-        BlockVector<double> test_rhs = calculate_rhs(state, Input::min_barrier_size);
+    KktSystem<dim>::calculate_convergence(const LA::MPI::BlockVector &state) const {
+        LA::MPI::BlockVector test_rhs = calculate_rhs(state, Input::min_barrier_size);
         double norm = 0;
 
         norm += std::pow(test_rhs.block(SolutionBlocks::displacement).l2_norm(), 2);
@@ -1223,9 +1249,9 @@ namespace SAND {
     }
 
     template<int dim>
-    BlockVector<double>
-    KktSystem<dim>::calculate_rhs(const BlockVector<double> &state, const double barrier_size) const {
-        BlockVector<double> test_rhs;
+    LA::MPI::BlockVector
+    KktSystem<dim>::calculate_rhs(const LA::MPI::BlockVector &state, const double barrier_size) const {
+        LA::MPI::BlockVector test_rhs;
         test_rhs = system_rhs;
         test_rhs = 0;
 
@@ -1254,7 +1280,7 @@ namespace SAND {
                                              update_values);
 
         FullMatrix<double> cell_matrix;
-        Vector<double> cell_rhs;
+        LA::MPI::Vector cell_rhs;
         std::vector<types::global_dof_index> local_dof_indices;
 
         const FEValuesExtractors::Scalar densities(SolutionComponents::density<dim>);
@@ -1277,8 +1303,8 @@ namespace SAND {
 
         const Functions::ConstantFunction<dim> lambda(1.), mu(1.);
 
-        BlockVector<double> filtered_unfiltered_density_solution = state;
-        BlockVector<double> filter_adjoint_unfiltered_density_multiplier_solution = state;
+        LA::MPI::BlockVector filtered_unfiltered_density_solution = state;
+        LA::MPI::BlockVector filter_adjoint_unfiltered_density_multiplier_solution = state;
         filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = 0;
         filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier) = 0;
 
@@ -1553,8 +1579,8 @@ namespace SAND {
 
     ///A  direct solver, for now. The complexity of the system means that an iterative solver algorithm will take some more work in the future.
     template<int dim>
-    BlockVector<double>
-    KktSystem<dim>::solve(const BlockVector<double> &state, double barrier_size) {
+    LA::MPI::BlockVector
+    KktSystem<dim>::solve(const LA::MPI::BlockVector &state, double barrier_size) {
         constraints.condense(system_matrix);
         double gmres_tolerance;
         if (Input::use_eisenstat_walker) {
@@ -1579,21 +1605,21 @@ namespace SAND {
             }
             case SolverOptions::exact_preconditioner_with_gmres: {
                 preconditioner.initialize(system_matrix, boundary_values, dof_handler, barrier_size, state);
-                SolverFGMRES<BlockVector<double>> A_fgmres(solver_control);
+                SolverFGMRES<LA::MPI::BlockVector> A_fgmres(solver_control);
                 A_fgmres.solve(system_matrix, linear_solution, system_rhs, preconditioner);
                 std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
                 break;
             }
             case SolverOptions::inexact_K_with_exact_A_gmres: {
                 preconditioner.initialize(system_matrix, boundary_values, dof_handler, barrier_size, state);
-                SolverFGMRES<BlockVector<double>> B_fgmres(solver_control);
+                SolverFGMRES<LA::MPI::BlockVector> B_fgmres(solver_control);
                 B_fgmres.solve(system_matrix, linear_solution, system_rhs, preconditioner);
                 std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
                 break;
             }
             case SolverOptions::inexact_K_with_inexact_A_gmres: {
                 preconditioner.initialize(system_matrix, boundary_values, dof_handler, barrier_size, state);
-                SolverFGMRES<BlockVector<double>> C_fgmres(solver_control);
+                SolverFGMRES<LA::MPI::BlockVector> C_fgmres(solver_control);
                 C_fgmres.solve(system_matrix, linear_solution, system_rhs, preconditioner);
                 std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
                 break;
@@ -1638,7 +1664,7 @@ namespace SAND {
     }
 
     template<int dim>
-    BlockVector<double>
+    LA::MPI::BlockVector
     KktSystem<dim>::get_initial_state() {
 
         std::vector<unsigned int> block_component(10, 2);
@@ -1650,7 +1676,7 @@ namespace SAND {
         const unsigned int n_u = dofs_per_block[1];
         const std::vector<unsigned int> block_sizes = {n_p, n_p, n_p, n_p, n_p, n_u, n_u, n_p, n_p, 1};
 
-        BlockVector<double> state(block_sizes);
+        LA::MPI::BlockVector state(block_sizes);
         {
             using namespace SolutionBlocks;
             state.block(density).add(density_ratio);
@@ -1671,7 +1697,7 @@ namespace SAND {
 
     template<int dim>
     void
-    KktSystem<dim>::output(const BlockVector<double> &state, const unsigned int j) const {
+    KktSystem<dim>::output(const LA::MPI::BlockVector &state, const unsigned int j) const {
         std::vector<std::string> solution_names(1, "low_slack_multiplier");
         std::vector<DataComponentInterpretation::DataComponentInterpretation> data_component_interpretation(
                 1, DataComponentInterpretation::component_is_scalar);
@@ -1718,7 +1744,7 @@ namespace SAND {
 
     template<>
     void
-    KktSystem<2>::output_stl(const BlockVector<double> &state) {
+    KktSystem<2>::output_stl(const LA::MPI::BlockVector &state) {
         double height = .25;
         const int dim = 2;
         std::ofstream stlfile;
@@ -1937,7 +1963,7 @@ namespace SAND {
 
     template<>
     void
-    KktSystem<3>::output_stl(const BlockVector<double> &state)
+    KktSystem<3>::output_stl(const LA::MPI::BlockVector &state)
     {
         std::ofstream stlfile;
         stlfile.open("bridge.stl");
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 5711bd1..001a0f8 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -42,7 +42,7 @@ namespace SAND {
     }
 
     template<int dim>
-    void TopOptSchurPreconditioner<dim>::initialize(BlockSparseMatrix<double> &matrix, const std::map<types::global_dof_index, double> &boundary_values,const DoFHandler<dim> &dof_handler, const double barrier_size, const BlockVector<double> &state)
+    void TopOptSchurPreconditioner<dim>::initialize(BlockSparseMatrix<double> &matrix, const std::map<types::global_dof_index, double> &boundary_values,const DoFHandler<dim> &dof_handler, const double barrier_size, const LA::MPI::BlockVector &state)
     {
         TimerOutput::Scope t(timer, "initialize");
         {
@@ -213,8 +213,8 @@ namespace SAND {
 
 
     template<int dim>
-    void TopOptSchurPreconditioner<dim>::vmult(BlockVector<double> &dst, const BlockVector<double> &src) const {
-        BlockVector<double> temp_src;
+    void TopOptSchurPreconditioner<dim>::vmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
+        LA::MPI::BlockVector temp_src;
         {
             TimerOutput::Scope t(timer, "part 1");
             vmult_step_1(dst, src);
@@ -241,24 +241,24 @@ namespace SAND {
     }
 
     template<int dim>
-    void TopOptSchurPreconditioner<dim>::Tvmult(BlockVector<double> &dst, const BlockVector<double> &src) const {
+    void TopOptSchurPreconditioner<dim>::Tvmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
         dst = src;
     }
 
     template<int dim>
-    void TopOptSchurPreconditioner<dim>::vmult_add(BlockVector<double> &dst, const BlockVector<double> &src) const {
-        BlockVector<double> dst_temp = dst;
+    void TopOptSchurPreconditioner<dim>::vmult_add(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
+        LA::MPI::BlockVector dst_temp = dst;
         vmult(dst_temp, src);
         dst += dst_temp;
     }
 
     template<int dim>
-    void TopOptSchurPreconditioner<dim>::Tvmult_add(BlockVector<double> &dst, const BlockVector<double> &src) const {
+    void TopOptSchurPreconditioner<dim>::Tvmult_add(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
         dst = src;
     }
 
     template<int dim>
-    void TopOptSchurPreconditioner<dim>::vmult_step_1(BlockVector<double> &dst, const BlockVector<double> &src) const {
+    void TopOptSchurPreconditioner<dim>::vmult_step_1(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
         dst = src;
         dst.block(SolutionBlocks::unfiltered_density) += -1 * linear_operator(d_5_mat)*src.block(SolutionBlocks::density_lower_slack_multiplier) +
                 linear_operator(d_6_mat) * src.block(SolutionBlocks::density_upper_slack_multiplier) + src.block(SolutionBlocks::density_lower_slack)
@@ -266,13 +266,13 @@ namespace SAND {
     }
 
     template<int dim>
-    void TopOptSchurPreconditioner<dim>::vmult_step_2(BlockVector<double> &dst, const BlockVector<double> &src) const {
+    void TopOptSchurPreconditioner<dim>::vmult_step_2(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
         dst = src;
         dst.block(SolutionBlocks::unfiltered_density_multiplier) += -1 * linear_operator(f_mat) * linear_operator(d_8_mat) * src.block(SolutionBlocks::unfiltered_density);
     }
 
     template<int dim>
-    void TopOptSchurPreconditioner<dim>::vmult_step_3(BlockVector<double> &dst, const BlockVector<double> &src) const {
+    void TopOptSchurPreconditioner<dim>::vmult_step_3(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
         dst = src;
         if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
         {
@@ -293,7 +293,7 @@ namespace SAND {
     }
 
     template<int dim>
-    void TopOptSchurPreconditioner<dim>::vmult_step_4(BlockVector<double> &dst, const BlockVector<double> &src) const {
+    void TopOptSchurPreconditioner<dim>::vmult_step_4(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
         dst = src;
         Vector<double> k_density_mult;
         k_density_mult.reinit(src.block(SolutionBlocks::density).size());
@@ -407,7 +407,7 @@ namespace SAND {
 
 
     template<int dim>
-    void TopOptSchurPreconditioner<dim>::vmult_step_5(BlockVector<double> &dst, const BlockVector<double> &src) const {
+    void TopOptSchurPreconditioner<dim>::vmult_step_5(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
         {
             //First Block Inverse
             TimerOutput::Scope t(timer, "inverse 1");
@@ -567,7 +567,7 @@ namespace SAND {
     }
 
     template<int dim>
-    void TopOptSchurPreconditioner<dim>::assemble_mass_matrix(const BlockVector<double> &state,
+    void TopOptSchurPreconditioner<dim>::assemble_mass_matrix(const LA::MPI::BlockVector &state,
                                                               const hp::FECollection<dim> &fe_collection,
                                                               const DoFHandler<dim> &dof_handler,
                                                               const AffineConstraints<double> &constraints,
@@ -609,8 +609,8 @@ namespace SAND {
 
         const Functions::ConstantFunction<dim> lambda(1.), mu(1.);
 
-        BlockVector<double> filtered_unfiltered_density_solution = state;
-        BlockVector<double> filter_adjoint_unfiltered_density_multiplier_solution = state;
+        LA::MPI::BlockVector filtered_unfiltered_density_solution = state;
+        LA::MPI::BlockVector filter_adjoint_unfiltered_density_multiplier_solution = state;
         filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = 0;
         filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier) = 0;
 
diff --git a/source/watchdog_main.cc b/source/watchdog_main.cc
index e2134cd..9aa8879 100644
--- a/source/watchdog_main.cc
+++ b/source/watchdog_main.cc
@@ -32,19 +32,19 @@ namespace SAND {
     private:
 
         std::pair<double,double>
-        calculate_max_step_size(const BlockVector<double> &state, const BlockVector<double> &step) const;
+        calculate_max_step_size(const LA::MPI::BlockVector &state, const LA::MPI::BlockVector &step) const;
 
-        const BlockVector<double>
-        find_max_step(const BlockVector<double> &state);
+        const LA::MPI::BlockVector
+        find_max_step(const LA::MPI::BlockVector &state);
 
-        BlockVector<double>
-        take_scaled_step(const BlockVector<double> &state,const BlockVector<double> &max_step) const;
+        LA::MPI::BlockVector
+        take_scaled_step(const LA::MPI::BlockVector &state,const LA::MPI::BlockVector &max_step) const;
 
         bool
-        check_convergence(const BlockVector<double> &state) const;
+        check_convergence(const LA::MPI::BlockVector &state) const;
 
         void
-        update_barrier(BlockVector<double> &current_state);
+        update_barrier(LA::MPI::BlockVector &current_state);
 
         KktSystem<dim> kkt_system;
         MarkovFilter markov_filter;
@@ -63,7 +63,7 @@ namespace SAND {
 
     template<int dim>
     std::pair<double,double>
-    SANDTopOpt<dim>::calculate_max_step_size(const BlockVector<double> &state, const BlockVector<double> &step) const {
+    SANDTopOpt<dim>::calculate_max_step_size(const LA::MPI::BlockVector &state, const LA::MPI::BlockVector &step) const {
 
         double step_size_s_low = 0;
         double step_size_z_low = 0;
@@ -77,10 +77,10 @@ namespace SAND {
 
             step_size_s = (step_size_s_low + step_size_s_high) / 2;
             step_size_z = (step_size_z_low + step_size_z_high) / 2;
-            const BlockVector<double> state_test_s =
+            const LA::MPI::BlockVector state_test_s =
                     (Input::fraction_to_boundary * state) + (step_size_s * step);
 
-            const BlockVector<double> state_test_z =
+            const LA::MPI::BlockVector state_test_z =
                     (Input::fraction_to_boundary * state) + (step_size_z * step);
 
             const bool accept_s = (state_test_s.block(SolutionBlocks::density_lower_slack).is_non_negative())
@@ -105,16 +105,16 @@ namespace SAND {
 ///Creates a rhs vector that we can use to look at the magnitude of the KKT conditions.  This is then used for testing the convergence before shrinking barrier size, as well as in the calculation of the l1 merit.
 
     template<int dim>
-    const BlockVector<double>
-    SANDTopOpt<dim>::find_max_step(const BlockVector<double> &state)
+    const LA::MPI::BlockVector
+    SANDTopOpt<dim>::find_max_step(const LA::MPI::BlockVector &state)
     {
         kkt_system.assemble_block_system(state, barrier_size);
-        const BlockVector<double> step = kkt_system.solve(state,barrier_size);
+        const LA::MPI::BlockVector step = kkt_system.solve(state,barrier_size);
 
         const auto max_step_sizes= calculate_max_step_size(state,step);
         const double step_size_s = max_step_sizes.first;
         const double step_size_z = max_step_sizes.second;
-        BlockVector<double> max_step(10);
+        LA::MPI::BlockVector max_step(10);
 
         max_step.block(SolutionBlocks::density) = step_size_s * step.block(SolutionBlocks::density);
         max_step.block(SolutionBlocks::displacement) = step_size_s * step.block(SolutionBlocks::displacement);
@@ -133,8 +133,8 @@ namespace SAND {
     ///This is my back-stepping algorithm for a line search - keeps shrinking step size until it finds a step where the merit is decreased.
 
     template<int dim>
-    BlockVector<double>
-    SANDTopOpt<dim>::take_scaled_step(const BlockVector<double> &state,const BlockVector<double> &max_step) const
+    LA::MPI::BlockVector
+    SANDTopOpt<dim>::take_scaled_step(const LA::MPI::BlockVector &state,const LA::MPI::BlockVector &max_step) const
     {
         double step_size = 1;
             for(unsigned int k = 0; k<10; k++)
@@ -157,7 +157,7 @@ namespace SAND {
     ///Checks to see if the KKT conditions are sufficiently met to lower barrier size.
     template<int dim>
     bool
-    SANDTopOpt<dim>::check_convergence(const BlockVector<double> &state) const
+    SANDTopOpt<dim>::check_convergence(const LA::MPI::BlockVector &state) const
     {
               if (kkt_system.calculate_convergence(state) < Input::required_norm)
               {
@@ -171,7 +171,7 @@ namespace SAND {
 
     template<int dim>
     void
-    SANDTopOpt<dim>::update_barrier(BlockVector<double> &current_state)
+    SANDTopOpt<dim>::update_barrier(LA::MPI::BlockVector &current_state)
     {
         if (Input::barrier_reduction == BarrierOptions::loqo)
         {
@@ -306,8 +306,8 @@ namespace SAND {
         const unsigned int max_uphill_steps = 8;
         unsigned int iteration_number = 0;
         //while barrier value above minimal value and total iterations under some value
-        BlockVector<double> current_state = kkt_system.get_initial_state();
-        BlockVector<double> current_step;
+        LA::MPI::BlockVector current_state = kkt_system.get_initial_state();
+        LA::MPI::BlockVector current_step;
         markov_filter.setup(kkt_system.calculate_objective_value(current_state), kkt_system.calculate_barrier_distance(current_state), kkt_system.calculate_feasibility(current_state,barrier_size), barrier_size);
 
         while((barrier_size > Input::min_barrier_size || !check_convergence(current_state)) && iteration_number < Input::max_steps)
@@ -319,8 +319,8 @@ namespace SAND {
                 bool found_step = false;
                 //save current state as watchdog state
 
-                const BlockVector<double> watchdog_state = current_state;
-                BlockVector<double> watchdog_step;
+                const LA::MPI::BlockVector watchdog_state = current_state;
+                LA::MPI::BlockVector watchdog_step;
                 //for 1-8 steps - this is the number of steps away we will let it go uphill before demanding downhill
                 for(unsigned int k = 0; k<max_uphill_steps; k++)
                 {
@@ -363,7 +363,7 @@ namespace SAND {
                     current_step = find_max_step(current_state);
                     //find step length so that merit of stretch state - sized step from current length - is less than merit of (current state + descent requirement * linear derivative of merit of current state in direction of current step)
                     //update stretch state with found step length
-                    const BlockVector<double> stretch_state = take_scaled_step(current_state, current_step);
+                    const LA::MPI::BlockVector stretch_state = take_scaled_step(current_state, current_step);
                     //if current merit is less than watchdog merit, or if stretch merit is less than earlier goal merit
                     if(markov_filter.check_filter(kkt_system.calculate_objective_value(current_state), kkt_system.calculate_barrier_distance(current_state), kkt_system.calculate_feasibility(current_state,barrier_size)))
                     {
@@ -386,7 +386,7 @@ namespace SAND {
                         else
                         {
                             //calculate direction from stretch state
-                            const BlockVector<double> stretch_step = find_max_step(stretch_state);
+                            const LA::MPI::BlockVector stretch_step = find_max_step(stretch_state);
                             //find step length from stretch state that meets descent requirement
                             current_state = take_scaled_step(stretch_state, stretch_step);
                             //update iteration count

From 60a35b75f32cd192d1fbd0f6bd5c4ec1440af974 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Wed, 20 Oct 2021 12:38:02 -0600
Subject: [PATCH 02/95] Still Very broken

---
 include/density_filter.h            |  2 +-
 include/kkt_system.h                |  8 +++-
 include/parameters_and_components.h |  2 +-
 include/schur_preconditioner.h      | 71 +++++++++++++++++------------
 source/density_filter.cc            |  2 +-
 source/kkt_system.cc                | 48 +++++++++++--------
 source/schur_preconditioner.cc      | 63 +++++++++++++++----------
 7 files changed, 117 insertions(+), 79 deletions(-)

diff --git a/include/density_filter.h b/include/density_filter.h
index 0a23cc9..479201f 100644
--- a/include/density_filter.h
+++ b/include/density_filter.h
@@ -53,7 +53,7 @@ namespace SAND {
     public:
 
         DensityFilter()=default;
-
+        DynamicSparsityPattern filter_dsp;
         SparseMatrix<double> filter_matrix;
         SparsityPattern filter_sparsity_pattern;
         void initialize(Triangulation<dim> &triangulation);
diff --git a/include/kkt_system.h b/include/kkt_system.h
index ac0af74..6ca8897 100644
--- a/include/kkt_system.h
+++ b/include/kkt_system.h
@@ -19,6 +19,10 @@
 #include <deal.II/lac/sparse_direct.h>
 #include <deal.II/lac/affine_constraints.h>
 #include <deal.II/lac/generic_linear_algebra.h>
+#include <deal.II/lac/petsc_block_sparse_matrix.h>
+#include <deal.II/lac/petsc_sparse_matrix.h>
+#include <deal.II/lac/petsc_vector.h>
+#include <deal.II/lac/petsc_solver.h>
 
 #include <deal.II/grid/tria.h>
 #include <deal.II/grid/grid_generator.h>
@@ -59,8 +63,7 @@ namespace SAND {
     class KktSystem {
 
     public:
-        MPI_Comm mpi_communicator;
-
+        MPI_Comm  mpi_communicator;
         std::vector<IndexSet> owned_partitioning;
         std::vector<IndexSet> relevant_partitioning;
 
@@ -116,6 +119,7 @@ namespace SAND {
         LA::MPI::BlockVector
         calculate_rhs(const LA::MPI::BlockVector &test_solution, const double barrier_size) const;
 
+        BlockDynamicSparsityPattern dsp;
         BlockSparsityPattern sparsity_pattern;
         LA::MPI::BlockSparseMatrix system_matrix;
         LA::MPI::BlockVector linear_solution;
diff --git a/include/parameters_and_components.h b/include/parameters_and_components.h
index 192b085..978cd96 100644
--- a/include/parameters_and_components.h
+++ b/include/parameters_and_components.h
@@ -63,6 +63,6 @@ namespace SAND {
         static constexpr unsigned int mbb = 1;
         static constexpr unsigned int l_shape = 2;
     }
-
+    static constexpr unsigned int block_number = 10;
 }
 #endif //SAND_PARAMETERS_AND_COMPONENTS_H
diff --git a/include/schur_preconditioner.h b/include/schur_preconditioner.h
index 0d6ef4e..aad0770 100644
--- a/include/schur_preconditioner.h
+++ b/include/schur_preconditioner.h
@@ -19,6 +19,13 @@
 #include <deal.II/lac/solver_gmres.h>
 #include <deal.II/lac/solver_cg.h>
 #include <deal.II/lac/solver_bicgstab.h>
+#include <deal.II/lac/generic_linear_algebra.h>
+#include <deal.II/lac/petsc_block_sparse_matrix.h>
+#include <deal.II/lac/petsc_solver.h>
+#include <deal.II/lac/petsc_sparse_matrix.h>
+#include <deal.II/lac/petsc_vector.h>
+#include <deal.II/lac/petsc_solver.h>
+#include <deal.II/lac/matrix_out.h>
 
 #include <deal.II/grid/tria.h>
 #include <deal.II/grid/grid_generator.h>
@@ -62,24 +69,28 @@
 
 namespace SAND
 {
+    namespace LA
+    {
+        using namespace dealii::LinearAlgebraPETSc;
+    }
     using namespace dealii;
     template<int dim>
     class TopOptSchurPreconditioner: public Subscriptor {
     public:
-        TopOptSchurPreconditioner(BlockSparseMatrix<double> &matrix_in);
-        void initialize (BlockSparseMatrix<double> &matrix, const std::map<types::global_dof_index, double> &boundary_values, const DoFHandler<dim> &dof_handler, const double barrier_size, const BlockVector<double> &state);
-        void vmult(BlockVector<double> &dst, const BlockVector<double> &src) const;
-        void Tvmult(BlockVector<double> &dst, const BlockVector<double> &src) const;
-        void vmult_add(BlockVector<double> &dst, const BlockVector<double> &src) const;
-        void Tvmult_add(BlockVector<double> &dst, const BlockVector<double> &src) const;
+        TopOptSchurPreconditioner(LA::MPI::BlockSparseMatrix &matrix_in);
+        void initialize (LA::MPI::BlockSparseMatrix &matrix, const std::map<types::global_dof_index, double> &boundary_values, const DoFHandler<dim> &dof_handler, const double barrier_size, const LA::MPI::BlockVector &state);
+        void vmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
+        void Tvmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
+        void vmult_add(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
+        void Tvmult_add(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
         void clear();
         unsigned int m() const;
         unsigned int n() const;
         void get_sparsity_pattern(BlockDynamicSparsityPattern &bdsp);
 
-        void assemble_mass_matrix(const BlockVector<double> &state, const hp::FECollection<dim> &fe_system, const DoFHandler<dim> &dof_handler, const AffineConstraints<double> &constraints,   const BlockSparsityPattern &bsp);
+        void assemble_mass_matrix(const LA::MPI::BlockVector &state, const hp::FECollection<dim> &fe_system, const DoFHandler<dim> &dof_handler, const AffineConstraints<double> &constraints,   const BlockSparsityPattern &bsp);
 
-        void print_stuff(const BlockSparseMatrix<double> &matrix);
+        void print_stuff(const LA::MPI::BlockSparseMatrix &matrix);
 
         BlockSparseMatrix<double> &system_matrix;
 
@@ -88,11 +99,11 @@ namespace SAND
         unsigned int n_columns;
         unsigned int n_block_rows;
         unsigned int n_block_columns;
-        void vmult_step_1(BlockVector<double> &dst, const BlockVector<double> &src) const;
-        void vmult_step_2(BlockVector<double> &dst, const BlockVector<double> &src) const;
-        void vmult_step_3(BlockVector<double> &dst, const BlockVector<double> &src) const;
-        void vmult_step_4(BlockVector<double> &dst, const BlockVector<double> &src) const;
-        void vmult_step_5(BlockVector<double> &dst, const BlockVector<double> &src) const;
+        void vmult_step_1(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
+        void vmult_step_2(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
+        void vmult_step_3(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
+        void vmult_step_4(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
+        void vmult_step_5(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
 
         BlockSparsityPattern mass_sparsity;
         BlockSparseMatrix<double> approx_h_mat;
@@ -102,23 +113,23 @@ namespace SAND
         mutable SolverGMRES<Vector<double>> other_gmres;
         mutable SolverCG<Vector<double>> other_cg;
 
-        SparseMatrix<double> &a_mat;
-        const SparseMatrix<double> &b_mat;
-        const SparseMatrix<double> &c_mat;
-        const SparseMatrix<double> &e_mat;
-        const SparseMatrix<double> &f_mat;
-        const SparseMatrix<double> &d_m_mat;
-        const SparseMatrix<double> &d_1_mat;
-        const SparseMatrix<double> &d_2_mat;
-        const SparseMatrix<double> &m_vect;
-
-        SparseMatrix<double> d_3_mat;
-        SparseMatrix<double> d_4_mat;
-        SparseMatrix<double> d_5_mat;
-        SparseMatrix<double> d_6_mat;
-        SparseMatrix<double> d_7_mat;
-        SparseMatrix<double> d_8_mat;
-        SparseMatrix<double> d_m_inv_mat;
+        LA::MPI::SparseMatrix &a_mat;
+        const LA::MPI::SparseMatrix &b_mat;
+        const LA::MPI::SparseMatrix &c_mat;
+        const LA::MPI::SparseMatrix &e_mat;
+        const LA::MPI::SparseMatrix &f_mat;
+        const LA::MPI::SparseMatrix &d_m_mat;
+        const LA::MPI::SparseMatrix &d_1_mat;
+        const LA::MPI::SparseMatrix &d_2_mat;
+        const LA::MPI::SparseMatrix &m_vect;
+
+        LA::MPI::SparseMatrix d_3_mat;
+        LA::MPI::SparseMatrix d_4_mat;
+        LA::MPI::SparseMatrix d_5_mat;
+        LA::MPI::SparseMatrix d_6_mat;
+        LA::MPI::SparseMatrix d_7_mat;
+        LA::MPI::SparseMatrix d_8_mat;
+        LA::MPI::SparseMatrix d_m_inv_mat;
 
         FullMatrix<double> g_mat;
         FullMatrix<double> h_mat;
diff --git a/source/density_filter.cc b/source/density_filter.cc
index 184b4a6..1c92354 100644
--- a/source/density_filter.cc
+++ b/source/density_filter.cc
@@ -18,7 +18,7 @@ namespace SAND {
     template<int dim>
     void
     DensityFilter<dim>::initialize(Triangulation<dim> &triangulation) {
-        DynamicSparsityPattern filter_dsp;
+
         filter_dsp.reinit(triangulation.n_active_cells(),
                           triangulation.n_active_cells());
 
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index 2c0c06b..825b27f 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -13,7 +13,13 @@
 #include <deal.II/lac/precondition.h>
 #include <deal.II/lac/linear_operator.h>
 #include <deal.II/lac/solver_gmres.h>
+#include <deal.II/lac/generic_linear_algebra.h>
+#include <deal.II/lac/petsc_block_sparse_matrix.h>
+#include <deal.II/lac/petsc_solver.h>
 
+#include <deal.II/lac/petsc_sparse_matrix.h>
+#include <deal.II/lac/petsc_vector.h>
+#include <deal.II/lac/petsc_solver.h>
 #include <deal.II/lac/matrix_out.h>
 
 #include <deal.II/grid/tria.h>
@@ -63,7 +69,9 @@ namespace SAND {
                    FE_DGQ<dim>(0) ^ 1),
             density_ratio(Input::volume_percentage),
             density_penalty_exponent(Input::density_penalty_exponent),
-            density_filter() {
+            density_filter(),
+            mpi_communicator(MPI_COMM_WORLD)
+    {
         fe_collection.push_back(fe_nine);
         fe_collection.push_back(fe_ten);
     }
@@ -550,7 +558,6 @@ namespace SAND {
         //MAKE n_u and n_P*****************************************************************
 
         /*Setup 10 by 10 block matrix*/
-
         std::vector<unsigned int> block_component(10, 2);
         block_component[0] = 0;
         block_component[5] = 1;
@@ -561,6 +568,8 @@ namespace SAND {
         const unsigned int n_u = dofs_per_block[1];
         std::cout << "n_p:  " << n_p << "   n_u:  " << n_u << std::endl;
 
+        dsp.reinit(10,10);
+
         owned_partitioning.resize(10);
         owned_partitioning[0] = dof_handler.locally_owned_dofs().get_view(0, n_p);
         owned_partitioning[1] = dof_handler.locally_owned_dofs().get_view(n_p, 2*n_p);
@@ -588,8 +597,6 @@ namespace SAND {
 
         const std::vector<unsigned int> block_sizes = {n_p, n_p, n_p, n_p, n_p, n_u, n_u, n_p, n_p, 1};
 
-        BlockDynamicSparsityPattern dsp(10, 10);
-
         for (unsigned int k = 0; k < 10; k++) {
             for (unsigned int j = 0; j < 10; j++) {
                 dsp.block(j, k).reinit(block_sizes[j], block_sizes[k]);
@@ -673,23 +680,25 @@ namespace SAND {
         sparsity_pattern.block(SolutionBlocks::unfiltered_density,
                                SolutionBlocks::unfiltered_density_multiplier).copy_from(
                 density_filter.filter_sparsity_pattern);
+        dsp.block(SolutionBlocks::unfiltered_density, SolutionBlocks::unfiltered_density_multiplier)=density_filter.filter_dsp;
         sparsity_pattern.block(SolutionBlocks::unfiltered_density_multiplier,
                                SolutionBlocks::unfiltered_density).copy_from(density_filter.filter_sparsity_pattern);
+        dsp.block(SolutionBlocks::unfiltered_density_multiplier,SolutionBlocks::unfiltered_density)=density_filter.filter_dsp;
 
         std::ofstream out("sparsity.plt");
         sparsity_pattern.print_gnuplot(out);
-
-        system_matrix.reinit(owned_partitioning, sparsity_pattern, mpi_communicator);
+        SparsityTools::distribute_sparsity_pattern(
+                dsp,
+                Utilities::MPI::all_gather(mpi_communicator,
+                                           dof_handler.locally_owned_dofs()),
+                mpi_communicator,
+                locally_relevant_dofs);
+        system_matrix.reinit(owned_partitioning, dsp, mpi_communicator);
 
 
         linear_solution.reinit(owned_partitioning, relevant_partitioning, mpi_communicator);
         system_rhs.reinit(owned_partitioning,mpi_communicator);
 
-        for (unsigned int j = 0; j < 10; j++) {
-            linear_solution.block(j).reinit(block_sizes[j],mpi_communicator);
-            system_rhs.block(j).reinit(block_sizes[j],mpi_communicator);
-        }
-
         linear_solution.collect_sizes();
         system_rhs.collect_sizes();
     }
@@ -701,7 +710,7 @@ namespace SAND {
     void
     KktSystem<dim>::assemble_block_system(const LA::MPI::BlockVector &state, const double barrier_size) {
         /*Remove any values from old iterations*/
-        system_matrix.reinit(sparsity_pattern);
+        system_matrix.reinit(owned_partitioning, dsp, mpi_communicator);
         linear_solution = 0;
         system_rhs = 0;
 
@@ -729,7 +738,7 @@ namespace SAND {
                                              update_values);
 
         FullMatrix<double> cell_matrix;
-        LA::MPI::Vector cell_rhs;
+        Vector<double> cell_rhs;
         std::vector<types::global_dof_index> local_dof_indices;
 
         const FEValuesExtractors::Scalar densities(SolutionComponents::density<dim>);
@@ -1097,7 +1106,7 @@ namespace SAND {
                                              update_values);
 
         FullMatrix<double> cell_matrix;
-        LA::MPI::Vector cell_rhs;
+        Vector<double> cell_rhs;
 
         const FEValuesExtractors::Vector displacements(SolutionComponents::displacement<dim>);
 
@@ -1280,7 +1289,7 @@ namespace SAND {
                                              update_values);
 
         FullMatrix<double> cell_matrix;
-        LA::MPI::Vector cell_rhs;
+        Vector<double> cell_rhs;
         std::vector<types::global_dof_index> local_dof_indices;
 
         const FEValuesExtractors::Scalar densities(SolutionComponents::density<dim>);
@@ -1598,9 +1607,10 @@ namespace SAND {
         TopOptSchurPreconditioner<dim> preconditioner(system_matrix);
         switch (Input::solver_choice) {
             case SolverOptions::direct_solve: {
-                SparseDirectUMFPACK A_direct;
-                A_direct.initialize(system_matrix);
-                A_direct.vmult(linear_solution, system_rhs);
+//                SolverControl cn;
+//                PETScWrappers::SparseDirectMUMPS solver(cn, mpi_communicator);
+//                solver.set_symmetric_mode(true);
+//                solver.solve(system_matrix, linear_solution, system_rhs);
                 break;
             }
             case SolverOptions::exact_preconditioner_with_gmres: {
@@ -1676,7 +1686,7 @@ namespace SAND {
         const unsigned int n_u = dofs_per_block[1];
         const std::vector<unsigned int> block_sizes = {n_p, n_p, n_p, n_p, n_p, n_u, n_u, n_p, n_p, 1};
 
-        LA::MPI::BlockVector state(block_sizes);
+        LA::MPI::BlockVector state(owned_partitioning, relevant_partitioning,mpi_communicator);
         {
             using namespace SolutionBlocks;
             state.block(density).add(density_ratio);
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 001a0f8..ce13643 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -6,16 +6,17 @@
 #include <deal.II/lac/sparse_matrix.h>
 #include <deal.II/base/timer.h>
 #include <deal.II/lac/generic_linear_algebra.h>
+#include <deal.II/lac/petsc_solver.h>
+#include <deal.II/lac/petsc_precondition.h>
 #include "../include/schur_preconditioner.h"
 #include "../include/input_information.h"
 #include "../include/sand_tools.h"
-#include <deal.II/lac/generic_linear_algebra.h>
 #include <fstream>
 
 namespace SAND {
     using namespace dealii;
     template<int dim>
-    TopOptSchurPreconditioner<dim>::TopOptSchurPreconditioner(BlockSparseMatrix<double> &matrix_in)
+    TopOptSchurPreconditioner<dim>::TopOptSchurPreconditioner(LA::MPI::BlockSparseMatrix &matrix_in)
             :
             system_matrix(matrix_in),
             n_rows(0),
@@ -42,7 +43,7 @@ namespace SAND {
     }
 
     template<int dim>
-    void TopOptSchurPreconditioner<dim>::initialize(BlockSparseMatrix<double> &matrix, const std::map<types::global_dof_index, double> &boundary_values,const DoFHandler<dim> &dof_handler, const double barrier_size, const LA::MPI::BlockVector &state)
+    void TopOptSchurPreconditioner<dim>::initialize(LA::MPI::BlockSparseMatrix &matrix, const std::map<types::global_dof_index, double> &boundary_values,const DoFHandler<dim> &dof_handler, const double barrier_size, const LA::MPI::BlockVector &state)
     {
         TimerOutput::Scope t(timer, "initialize");
         {
@@ -99,13 +100,20 @@ namespace SAND {
         }
         {
             TimerOutput::Scope t(timer, "reinit diag matrices");
-            d_3_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density).get_sparsity_pattern());
-            d_4_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density).get_sparsity_pattern());
-            d_5_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density).get_sparsity_pattern());
-            d_6_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density).get_sparsity_pattern());
-            d_7_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density).get_sparsity_pattern());
-            d_8_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density).get_sparsity_pattern());
-            d_m_inv_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density).get_sparsity_pattern());
+            d_3_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
+            d_4_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
+            d_5_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
+            d_6_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
+            d_7_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
+            d_8_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
+            d_m_inv_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
+            d_3_mat=0;
+            d_4_mat=0;
+            d_5_mat=0;
+            d_6_mat=0;
+            d_7_mat=0;
+            d_8_mat=0;
+            d_m_inv_mat=0;
         }
         {
             TimerOutput::Scope t(timer, "build diag matrices");
@@ -153,11 +161,10 @@ namespace SAND {
                     transpose_operator(linear_operator(f_mat));
 
         auto op_h = op_g;
-        PreconditionSSOR<SparseMatrix<double>> preconditioner;
-        preconditioner.initialize(a_mat, 1.2);
+
         SolverControl            solver_control(1000, 1e-12);
         SolverCG<Vector<double>> a_solver_cg(solver_control);
-        auto a_inv_op = inverse_operator(linear_operator(a_mat),a_solver_cg,preconditioner);
+        auto a_inv_op = inverse_operator(linear_operator(a_mat),a_solver_cg,PreconditionIdentity());
 
         if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
         {
@@ -260,34 +267,40 @@ namespace SAND {
     template<int dim>
     void TopOptSchurPreconditioner<dim>::vmult_step_1(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
         dst = src;
-        dst.block(SolutionBlocks::unfiltered_density) += -1 * linear_operator(d_5_mat)*src.block(SolutionBlocks::density_lower_slack_multiplier) +
-                linear_operator(d_6_mat) * src.block(SolutionBlocks::density_upper_slack_multiplier) + src.block(SolutionBlocks::density_lower_slack)
-                - src.block(SolutionBlocks::density_upper_slack);
+        d_5_mat.vmult_add(dst.block(SolutionBlocks::unfiltered_density),-1 * src.block(SolutionBlocks::density_lower_slack_multiplier));
+        d_6_mat.vmult_add(dst.block(SolutionBlocks::unfiltered_density),-1 * src.block(SolutionBlocks::density_upper_slack_multiplier));
+        dst.block(SolutionBlocks::unfiltered_density)+=src.block(SolutionBlocks::density_lower_slack);
+        dst.block(SolutionBlocks::unfiltered_density) -=src.block(SolutionBlocks::density_upper_slack);
     }
 
     template<int dim>
     void TopOptSchurPreconditioner<dim>::vmult_step_2(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
         dst = src;
-        dst.block(SolutionBlocks::unfiltered_density_multiplier) += -1 * linear_operator(f_mat) * linear_operator(d_8_mat) * src.block(SolutionBlocks::unfiltered_density);
+        auto dst_temp = dst;
+                d_8_mat.vmult(dst_temp.block(SolutionBlocks::unfiltered_density_multiplier), src.block(SolutionBlocks::unfiltered_density));
+                f_mat.vmult(dst.block(SolutionBlocks::unfiltered_density_multiplier),-1*dst_temp.block(SolutionBlocks::unfiltered_density_multiplier));
     }
 
     template<int dim>
     void TopOptSchurPreconditioner<dim>::vmult_step_3(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
         dst = src;
+        auto dst_temp = dst;
         if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
         {
-            PreconditionSSOR<SparseMatrix<double>> preconditioner;
-            preconditioner.initialize(a_mat, 1.2);
             SolverControl            solver_control(1000, 1e-12);
-            SolverCG<Vector<double>> a_solver_cg(solver_control);
-            auto a_inv_op = inverse_operator(linear_operator(a_mat),a_solver_cg,preconditioner);
-            dst.block(SolutionBlocks::density)+= -1 * transpose_operator(linear_operator(e_mat)) * a_inv_op * src.block(SolutionBlocks::displacement)
-                                                 - transpose_operator(linear_operator(c_mat)) * a_inv_op * src.block(SolutionBlocks::displacement_multiplier);
+            LA::SolverCG a_solver_cg(solver_control);
+            a_solver_cg.solve(a_mat,dst_temp.block(SolutionBlocks::density),src.block(SolutionBlocks::displacement),dealii::PETScWrappers::PreconditionNone);
+            e_mat.Tvmult_add(dst.block(SolutionBlocks::density),-1*dst_temp.block(SolutionBlocks::density));
+
+            a_solver_cg.solve(a_mat,dst_temp.block(SolutionBlocks::density),src.block(SolutionBlocks::displacement_multiplier));
+            c_mat.Tvmult_add(dst.block(SolutionBlocks::density),-1*dst_temp.block(SolutionBlocks::density));
         }
         else
         {
-            dst.block(SolutionBlocks::density)+= -1 * transpose_operator(linear_operator(e_mat)) * linear_operator(a_inv_direct) * src.block(SolutionBlocks::displacement)
-                                                 - transpose_operator(linear_operator(c_mat)) * linear_operator(a_inv_direct) * src.block(SolutionBlocks::displacement_multiplier);
+            a_inv_direct.vmult(dst_temp.block(SolutionBlocks::density),src.block(SolutionBlocks::displacement));
+            e_mat.Tvmult_add(dst.block(SolutionBlocks::density),-1 *dst_temp.block(SolutionBlocks::density));
+            a_inv_direct.vmult(dst_temp.block(SolutionBlocks::density),src.block(SolutionBlocks::displacement_multiplier));
+            e_mat.Tvmult_add(dst.block(SolutionBlocks::density),-1 *dst_temp.block(SolutionBlocks::density));
         }
 
     }

From 7d2b0af7b28abaf083ca61944fbbccb15b1911ae Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Mon, 25 Oct 2021 16:22:53 -0600
Subject: [PATCH 03/95] Still Very broken. Problem with distribute local to
 global?

---
 include/density_filter.h       |  13 +-
 include/input_information.h    |   2 +-
 include/kkt_system.h           |   2 +-
 include/sand_tools.h           |   6 +-
 include/schur_preconditioner.h |  27 +--
 source/density_filter.cc       |   2 +-
 source/kkt_system.cc           | 210 ++++++++--------
 source/schur_preconditioner.cc | 431 ++++++++++++++++++---------------
 source/watchdog_main.cc        |   8 +-
 9 files changed, 375 insertions(+), 326 deletions(-)

diff --git a/include/density_filter.h b/include/density_filter.h
index 479201f..e853b8a 100644
--- a/include/density_filter.h
+++ b/include/density_filter.h
@@ -20,6 +20,8 @@
 #include <deal.II/lac/packaged_operation.h>
 #include <deal.II/lac/sparse_direct.h>
 #include <deal.II/lac/affine_constraints.h>
+#include <deal.II/lac/trilinos_parallel_block_vector.h>
+#include <deal.II/lac/generic_linear_algebra.h>
 
 #include <deal.II/grid/tria.h>
 #include <deal.II/grid/grid_generator.h>
@@ -47,20 +49,23 @@
  * Once formed, we have F*\sigma = \rho*/
 namespace SAND {
     using namespace dealii;
-
+    namespace LA
+    {
+        using namespace dealii::LinearAlgebraTrilinos;
+    }
     template<int dim>
     class DensityFilter {
     public:
 
         DensityFilter()=default;
         DynamicSparsityPattern filter_dsp;
-        SparseMatrix<double> filter_matrix;
+        LA::MPI::SparseMatrix filter_matrix;
         SparsityPattern filter_sparsity_pattern;
         void initialize(Triangulation<dim> &triangulation);
-
+        std::set<typename Triangulation<dim>::cell_iterator> find_relevant_neighbors(typename Triangulation<dim>::cell_iterator cell) const;
 
     private:
-        std::set<typename Triangulation<dim>::cell_iterator> find_relevant_neighbors(typename Triangulation<dim>::cell_iterator cell) const;
+
 
     };
 }
diff --git a/include/input_information.h b/include/input_information.h
index 51e796e..9b7be58 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -39,7 +39,7 @@ namespace SAND {
         constexpr bool output_parts_of_matrix = false;
 
         //Linear solver options
-        constexpr unsigned int solver_choice = SolverOptions::inexact_K_with_exact_A_gmres;
+        constexpr unsigned int solver_choice = SolverOptions::inexact_K_with_inexact_A_gmres;
         constexpr bool use_eisenstat_walker = false;
         constexpr double default_gmres_tolerance = 1e-6;
 
diff --git a/include/kkt_system.h b/include/kkt_system.h
index 6ca8897..5e76e07 100644
--- a/include/kkt_system.h
+++ b/include/kkt_system.h
@@ -55,7 +55,7 @@
 namespace SAND {
     namespace LA
     {
-        using namespace dealii::LinearAlgebraPETSc;
+        using namespace dealii::LinearAlgebraTrilinos;
     }
     using namespace dealii;
 
diff --git a/include/sand_tools.h b/include/sand_tools.h
index 767a203..54ba5f2 100644
--- a/include/sand_tools.h
+++ b/include/sand_tools.h
@@ -12,10 +12,10 @@ namespace SAND{
     void build_matrix_element_by_element (const auto &X,
                                           FullMatrix<double>   &X_matrix)
     {
-
+        auto op_X = linear_operator(X);
         Threads::TaskGroup<void> tasks;
         for (unsigned int j=0; j<X_matrix.n(); ++j)
-            tasks += Threads::new_task ([&X, &X_matrix, j]()
+            tasks += Threads::new_task ([&op_X, &X_matrix, j]()
                                         {
                                             Vector<double> e_j (X_matrix.m());
                                             Vector<double> r_j (X_matrix.n());
@@ -23,7 +23,7 @@ namespace SAND{
                                             e_j = 0;
                                             e_j(j) = 1;
 
-                                            X.vmult (r_j, e_j);
+                                            r_j = op_X * e_j;
 
                                             for (unsigned int i=0; i<X_matrix.m(); ++i)
                                                 X_matrix(i,j) = r_j(i);
diff --git a/include/schur_preconditioner.h b/include/schur_preconditioner.h
index aad0770..aa9fdda 100644
--- a/include/schur_preconditioner.h
+++ b/include/schur_preconditioner.h
@@ -14,18 +14,16 @@
 #include <deal.II/lac/block_sparse_matrix.h>
 #include <deal.II/lac/precondition.h>
 #include <deal.II/lac/linear_operator.h>
+#include <deal.II/lac/linear_operator_tools.h>
 #include <deal.II/lac/packaged_operation.h>
 #include <deal.II/lac/sparse_direct.h>
 #include <deal.II/lac/solver_gmres.h>
 #include <deal.II/lac/solver_cg.h>
 #include <deal.II/lac/solver_bicgstab.h>
 #include <deal.II/lac/generic_linear_algebra.h>
-#include <deal.II/lac/petsc_block_sparse_matrix.h>
-#include <deal.II/lac/petsc_solver.h>
-#include <deal.II/lac/petsc_sparse_matrix.h>
-#include <deal.II/lac/petsc_vector.h>
-#include <deal.II/lac/petsc_solver.h>
 #include <deal.II/lac/matrix_out.h>
+#include <deal.II/lac/trilinos_sparse_matrix.h>
+#include <deal.II/lac/trilinos_block_sparse_matrix.h>
 
 #include <deal.II/grid/tria.h>
 #include <deal.II/grid/grid_generator.h>
@@ -71,7 +69,7 @@ namespace SAND
 {
     namespace LA
     {
-        using namespace dealii::LinearAlgebraPETSc;
+        using namespace dealii::LinearAlgebraTrilinos;
     }
     using namespace dealii;
     template<int dim>
@@ -92,7 +90,7 @@ namespace SAND
 
         void print_stuff(const LA::MPI::BlockSparseMatrix &matrix);
 
-        BlockSparseMatrix<double> &system_matrix;
+        LA::MPI::BlockSparseMatrix &system_matrix;
 
     private:
         unsigned int n_rows;
@@ -106,7 +104,7 @@ namespace SAND
         void vmult_step_5(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
 
         BlockSparsityPattern mass_sparsity;
-        BlockSparseMatrix<double> approx_h_mat;
+        LA::MPI::BlockSparseMatrix approx_h_mat;
 
         SolverControl other_solver_control;
         mutable SolverBicgstab<Vector<double>> other_bicgstab;
@@ -136,12 +134,13 @@ namespace SAND
         FullMatrix<double> k_inv_mat;
         LAPACKFullMatrix<double> k_mat;
 
-        mutable Vector<double> pre_j;
-        mutable Vector<double> pre_k;
-        mutable Vector<double> g_d_m_inv_density;
-        mutable Vector<double> k_g_d_m_inv_density;
-
-        SparseDirectUMFPACK a_inv_direct;
+        mutable LA::MPI::Vector pre_j;
+        mutable LA::MPI::Vector pre_k;
+        mutable LA::MPI::Vector g_d_m_inv_density;
+        mutable LA::MPI::Vector k_g_d_m_inv_density;
+//
+//        SolverControl direct_solver_control;
+//        mutable TrilinosWrappers::SolverDirect a_inv_direct;
 
         mutable TimerOutput timer;
 
diff --git a/source/density_filter.cc b/source/density_filter.cc
index 1c92354..975be83 100644
--- a/source/density_filter.cc
+++ b/source/density_filter.cc
@@ -57,7 +57,7 @@ namespace SAND {
         {
             const unsigned int i = cell->active_cell_index();
             double denominator = 0;
-            typename SparseMatrix<double>::iterator iter = filter_matrix.begin(
+            typename LA::MPI::SparseMatrix::iterator iter = filter_matrix.begin(
                     i);
             for (; iter != filter_matrix.end(i); iter++)
             {
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index 825b27f..2fc6f37 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -74,6 +74,7 @@ namespace SAND {
     {
         fe_collection.push_back(fe_nine);
         fe_collection.push_back(fe_ten);
+
     }
 
 
@@ -559,41 +560,44 @@ namespace SAND {
 
         /*Setup 10 by 10 block matrix*/
         std::vector<unsigned int> block_component(10, 2);
+
         block_component[0] = 0;
         block_component[5] = 1;
+
         const std::vector<types::global_dof_index> dofs_per_block =
                 DoFTools::count_dofs_per_fe_block(dof_handler, block_component);
-
         const unsigned int n_p = dofs_per_block[0];
         const unsigned int n_u = dofs_per_block[1];
+
         std::cout << "n_p:  " << n_p << "   n_u:  " << n_u << std::endl;
 
-        dsp.reinit(10,10);
+        IndexSet locally_owned_dofs = dof_handler.locally_owned_dofs();
+        IndexSet locally_relevant_dofs;
+        DoFTools::extract_locally_relevant_dofs(dof_handler, locally_relevant_dofs);
 
+        dsp.reinit(10, 10);
         owned_partitioning.resize(10);
         owned_partitioning[0] = dof_handler.locally_owned_dofs().get_view(0, n_p);
-        owned_partitioning[1] = dof_handler.locally_owned_dofs().get_view(n_p, 2*n_p);
-        owned_partitioning[2] = dof_handler.locally_owned_dofs().get_view(2*n_p, 3*n_p);
-        owned_partitioning[3] = dof_handler.locally_owned_dofs().get_view(3*n_p, 4*n_p);
-        owned_partitioning[4] = dof_handler.locally_owned_dofs().get_view(4*n_p, 5*n_p);
-        owned_partitioning[5] = dof_handler.locally_owned_dofs().get_view(5*n_p, 5*n_p+n_u);
-        owned_partitioning[6] = dof_handler.locally_owned_dofs().get_view(5*n_p+n_u, 5*n_p+2*n_u);
-        owned_partitioning[7] = dof_handler.locally_owned_dofs().get_view(5*n_p+2*n_u, 6*n_p+2*n_u);
-        owned_partitioning[8] = dof_handler.locally_owned_dofs().get_view(6*n_p+2*n_u, 7*n_p+2*n_u);
-        owned_partitioning[9] = dof_handler.locally_owned_dofs().get_view(7*n_p+2*n_u, 7*n_p+2*n_u + 1);
-        IndexSet locally_relevant_dofs;
-        DoFTools::extract_locally_relevant_dofs(dof_handler, locally_relevant_dofs);
-        relevant_partitioning.resize(2);
+        owned_partitioning[1] = dof_handler.locally_owned_dofs().get_view(n_p, 2 * n_p);
+        owned_partitioning[2] = dof_handler.locally_owned_dofs().get_view(2 * n_p, 3 * n_p);
+        owned_partitioning[3] = dof_handler.locally_owned_dofs().get_view(3 * n_p, 4 * n_p);
+        owned_partitioning[4] = dof_handler.locally_owned_dofs().get_view(4 * n_p, 5 * n_p);
+        owned_partitioning[5] = dof_handler.locally_owned_dofs().get_view(5 * n_p, 5 * n_p + n_u);
+        owned_partitioning[6] = dof_handler.locally_owned_dofs().get_view(5 * n_p + n_u, 5 * n_p + 2 * n_u);
+        owned_partitioning[7] = dof_handler.locally_owned_dofs().get_view(5 * n_p + 2 * n_u, 6 * n_p + 2 * n_u);
+        owned_partitioning[8] = dof_handler.locally_owned_dofs().get_view(6 * n_p + 2 * n_u, 7 * n_p + 2 * n_u);
+        owned_partitioning[9] = dof_handler.locally_owned_dofs().get_view(7 * n_p + 2 * n_u, 7 * n_p + 2 * n_u + 1);
+        relevant_partitioning.resize(10);
         relevant_partitioning[0] = locally_relevant_dofs.get_view(0, n_p);
-        relevant_partitioning[1] = locally_relevant_dofs.get_view(n_p, 2*n_p);
-        relevant_partitioning[2] = locally_relevant_dofs.get_view(2*n_p, 3*n_p);
-        relevant_partitioning[3] = locally_relevant_dofs.get_view(3*n_p, 4*n_p);
-        relevant_partitioning[4] = locally_relevant_dofs.get_view(4*n_p, 5*n_p);
-        relevant_partitioning[5] = locally_relevant_dofs.get_view(5*n_p, 5*n_p+n_u);
-        relevant_partitioning[6] = locally_relevant_dofs.get_view(5*n_p+n_u, 5*n_p+2*n_u);
-        relevant_partitioning[7] = locally_relevant_dofs.get_view(5*n_p+2*n_u, 6*n_p+2*n_u);
-        relevant_partitioning[8] = locally_relevant_dofs.get_view(6*n_p+2*n_u, 7*n_p+2*n_u);
-        relevant_partitioning[9] = locally_relevant_dofs.get_view(7*n_p+2*n_u, 7*n_p+2*n_u + 1);
+        relevant_partitioning[1] = locally_relevant_dofs.get_view(n_p, 2 * n_p);
+        relevant_partitioning[2] = locally_relevant_dofs.get_view(2 * n_p, 3 * n_p);
+        relevant_partitioning[3] = locally_relevant_dofs.get_view(3 * n_p, 4 * n_p);
+        relevant_partitioning[4] = locally_relevant_dofs.get_view(4 * n_p, 5 * n_p);
+        relevant_partitioning[5] = locally_relevant_dofs.get_view(5 * n_p, 5 * n_p + n_u);
+        relevant_partitioning[6] = locally_relevant_dofs.get_view(5 * n_p + n_u, 5 * n_p + 2 * n_u);
+        relevant_partitioning[7] = locally_relevant_dofs.get_view(5 * n_p + 2 * n_u, 6 * n_p + 2 * n_u);
+        relevant_partitioning[8] = locally_relevant_dofs.get_view(6 * n_p + 2 * n_u, 7 * n_p + 2 * n_u);
+        relevant_partitioning[9] = locally_relevant_dofs.get_view(7 * n_p + 2 * n_u, 7 * n_p + 2 * n_u + 1);
 
         const std::vector<unsigned int> block_sizes = {n_p, n_p, n_p, n_p, n_p, n_u, n_u, n_p, n_p, 1};
 
@@ -602,105 +606,108 @@ namespace SAND {
                 dsp.block(j, k).reinit(block_sizes[j], block_sizes[k]);
             }
         }
-
         dsp.collect_sizes();
-
         Table<2, DoFTools::Coupling> coupling(2 * dim + 8, 2 * dim + 8);
 //Coupling for density
-        coupling[SolutionComponents::density<dim>][SolutionComponents::density<dim>] = DoFTools::always;
+            coupling[SolutionComponents::density<dim>][SolutionComponents::density<dim>] = DoFTools::always;
 
-        for (unsigned int i = 0; i < dim; i++) {
-            coupling[SolutionComponents::density<dim>][SolutionComponents::displacement<dim> + i] = DoFTools::always;
-            coupling[SolutionComponents::displacement<dim> + i][SolutionComponents::density<dim>] = DoFTools::always;
-        }
+            for (unsigned int i = 0; i < dim; i++) {
+                coupling[SolutionComponents::density<dim>][SolutionComponents::displacement<dim> +
+                                                           i] = DoFTools::always;
+                coupling[SolutionComponents::displacement<dim> +
+                         i][SolutionComponents::density<dim>] = DoFTools::always;
+            }
 
-        coupling[SolutionComponents::density<dim>][SolutionComponents::unfiltered_density_multiplier<dim>] = DoFTools::always;
-        coupling[SolutionComponents::unfiltered_density_multiplier<dim>][SolutionComponents::density<dim>] = DoFTools::always;
+            coupling[SolutionComponents::density<dim>][SolutionComponents::unfiltered_density_multiplier<dim>] = DoFTools::always;
+            coupling[SolutionComponents::unfiltered_density_multiplier<dim>][SolutionComponents::density<dim>] = DoFTools::always;
 
-        for (unsigned int i = 0; i < dim; i++) {
-            coupling[SolutionComponents::density<dim>][SolutionComponents::displacement_multiplier<dim> +
-                                                       i] = DoFTools::always;
-            coupling[SolutionComponents::displacement_multiplier<dim> +
-                     i][SolutionComponents::density<dim>] = DoFTools::always;
-        }
+            for (unsigned int i = 0; i < dim; i++) {
+                coupling[SolutionComponents::density<dim>][SolutionComponents::displacement_multiplier<dim> +
+                                                           i] = DoFTools::always;
+                coupling[SolutionComponents::displacement_multiplier<dim> +
+                         i][SolutionComponents::density<dim>] = DoFTools::always;
+            }
 
 //Coupling for displacement
-        for (unsigned int i = 0; i < dim; i++) {
-
-            for (unsigned int k = 0; k < dim; k++) {
-                coupling[SolutionComponents::displacement<dim> + i][SolutionComponents::displacement_multiplier<dim> +
-                                                                    k] = DoFTools::always;
-                coupling[SolutionComponents::displacement_multiplier<dim> + k][SolutionComponents::displacement<dim> +
-                                                                               i] = DoFTools::always;
+            for (unsigned int i = 0; i < dim; i++) {
+
+                for (unsigned int k = 0; k < dim; k++) {
+                    coupling[SolutionComponents::displacement<dim> + i][
+                            SolutionComponents::displacement_multiplier<dim> +
+                            k] = DoFTools::always;
+                    coupling[SolutionComponents::displacement_multiplier<dim> + k][
+                            SolutionComponents::displacement<dim> +
+                            i] = DoFTools::always;
+                }
             }
-        }
 
 // coupling for unfiltered density
-        coupling[SolutionComponents::unfiltered_density<dim>][SolutionComponents::density_lower_slack_multiplier<dim>] = DoFTools::always;
-        coupling[SolutionComponents::density_lower_slack_multiplier<dim>][SolutionComponents::unfiltered_density<dim>] = DoFTools::always;
+            coupling[SolutionComponents::unfiltered_density<dim>][SolutionComponents::density_lower_slack_multiplier<dim>] = DoFTools::always;
+            coupling[SolutionComponents::density_lower_slack_multiplier<dim>][SolutionComponents::unfiltered_density<dim>] = DoFTools::always;
 
-        coupling[SolutionComponents::unfiltered_density<dim>][SolutionComponents::density_upper_slack_multiplier<dim>] = DoFTools::always;
-        coupling[SolutionComponents::density_upper_slack_multiplier<dim>][SolutionComponents::unfiltered_density<dim>] = DoFTools::always;
+            coupling[SolutionComponents::unfiltered_density<dim>][SolutionComponents::density_upper_slack_multiplier<dim>] = DoFTools::always;
+            coupling[SolutionComponents::density_upper_slack_multiplier<dim>][SolutionComponents::unfiltered_density<dim>] = DoFTools::always;
 
-        coupling[SolutionComponents::unfiltered_density<dim>][SolutionComponents::unfiltered_density_multiplier<dim>] = DoFTools::always;
-        coupling[SolutionComponents::unfiltered_density_multiplier<dim>][SolutionComponents::unfiltered_density<dim>] = DoFTools::always;
+            coupling[SolutionComponents::unfiltered_density<dim>][SolutionComponents::unfiltered_density_multiplier<dim>] = DoFTools::always;
+            coupling[SolutionComponents::unfiltered_density_multiplier<dim>][SolutionComponents::unfiltered_density<dim>] = DoFTools::always;
 
 
 
 
 //        Coupling for lower slack
-        coupling[SolutionComponents::density_lower_slack<dim>][SolutionComponents::density_lower_slack<dim>] = DoFTools::always;
+            coupling[SolutionComponents::density_lower_slack<dim>][SolutionComponents::density_lower_slack<dim>] = DoFTools::always;
 
-        coupling[SolutionComponents::density_lower_slack<dim>][SolutionComponents::density_lower_slack_multiplier<dim>] = DoFTools::always;
-        coupling[SolutionComponents::density_lower_slack_multiplier<dim>][SolutionComponents::density_lower_slack<dim>] = DoFTools::always;
+            coupling[SolutionComponents::density_lower_slack<dim>][SolutionComponents::density_lower_slack_multiplier<dim>] = DoFTools::always;
+            coupling[SolutionComponents::density_lower_slack_multiplier<dim>][SolutionComponents::density_lower_slack<dim>] = DoFTools::always;
 
 //
-        coupling[SolutionComponents::density_upper_slack<dim>][SolutionComponents::density_upper_slack<dim>] = DoFTools::always;
-        coupling[SolutionComponents::density_upper_slack<dim>][SolutionComponents::density_upper_slack_multiplier<dim>] = DoFTools::always;
-        coupling[SolutionComponents::density_upper_slack_multiplier<dim>][SolutionComponents::density_upper_slack<dim>] = DoFTools::always;
-
-        coupling[SolutionComponents::density_upper_slack_multiplier<dim>][SolutionComponents::density_upper_slack_multiplier<dim>] = DoFTools::always;
-        constraints.reinit(locally_relevant_dofs);
-        constraints.clear();
-        constraints.close();
-
-        DoFTools::make_sparsity_pattern(dof_handler, coupling, dsp, constraints);
-
-        //adds the row into the sparsity pattern for the total volume constraint
-        for (const auto &cell: dof_handler.active_cell_iterators()) {
-            const unsigned int i = cell->active_cell_index();
-            dsp.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier).add(i, 0);
-            dsp.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).add(0, i);
-        }
+            coupling[SolutionComponents::density_upper_slack<dim>][SolutionComponents::density_upper_slack<dim>] = DoFTools::always;
+            coupling[SolutionComponents::density_upper_slack<dim>][SolutionComponents::density_upper_slack_multiplier<dim>] = DoFTools::always;
+            coupling[SolutionComponents::density_upper_slack_multiplier<dim>][SolutionComponents::density_upper_slack<dim>] = DoFTools::always;
+
+            coupling[SolutionComponents::density_upper_slack_multiplier<dim>][SolutionComponents::density_upper_slack_multiplier<dim>] = DoFTools::always;
+            constraints.reinit(locally_relevant_dofs);
+            constraints.clear();
+            constraints.close();
+
+            system_matrix.clear();
+
+            DoFTools::make_sparsity_pattern(dof_handler, coupling, dsp, constraints, false);
+            SparsityTools::distribute_sparsity_pattern(dsp, dof_handler.locally_owned_dofs(), mpi_communicator,
+                                                       locally_relevant_dofs);
+            //adds the row into the sparsity pattern for the total volume constraint
+            for (const auto &cell: dof_handler.active_cell_iterators())
+            {
+                const unsigned int i = cell->active_cell_index();
+                dsp.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier).add(i, 0);
+                dsp.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).add(0, i);
+            }
 
-        constraints.condense(dsp);
-        sparsity_pattern.copy_from(dsp);
-
-        //adds the row into the sparsity pattern for the total volume constraint
-        sparsity_pattern.block(SolutionBlocks::unfiltered_density,
-                               SolutionBlocks::unfiltered_density_multiplier).copy_from(
-                density_filter.filter_sparsity_pattern);
-        dsp.block(SolutionBlocks::unfiltered_density, SolutionBlocks::unfiltered_density_multiplier)=density_filter.filter_dsp;
-        sparsity_pattern.block(SolutionBlocks::unfiltered_density_multiplier,
-                               SolutionBlocks::unfiltered_density).copy_from(density_filter.filter_sparsity_pattern);
-        dsp.block(SolutionBlocks::unfiltered_density_multiplier,SolutionBlocks::unfiltered_density)=density_filter.filter_dsp;
-
-        std::ofstream out("sparsity.plt");
-        sparsity_pattern.print_gnuplot(out);
-        SparsityTools::distribute_sparsity_pattern(
-                dsp,
-                Utilities::MPI::all_gather(mpi_communicator,
-                                           dof_handler.locally_owned_dofs()),
-                mpi_communicator,
-                locally_relevant_dofs);
-        system_matrix.reinit(owned_partitioning, dsp, mpi_communicator);
+            /*finds neighbors whose values would be relevant, and adds them to the sparsity pattern of the matrix*/
+            for (const auto &cell: triangulation.active_cell_iterators()) {
+                const unsigned int i = cell->active_cell_index();
+                for (const auto &neighbor_cell: density_filter.find_relevant_neighbors(cell))
+                {
+                    const unsigned int j = neighbor_cell->active_cell_index();
+                    dsp.block(SolutionBlocks::unfiltered_density_multiplier, SolutionBlocks::unfiltered_density).add(i,
+                                                                                                                     j);
+                    dsp.block(SolutionBlocks::unfiltered_density, SolutionBlocks::unfiltered_density_multiplier).add(i,
+                                                                                                                     j);
+                }
+            }
 
 
-        linear_solution.reinit(owned_partitioning, relevant_partitioning, mpi_communicator);
-        system_rhs.reinit(owned_partitioning,mpi_communicator);
+            SparsityTools::distribute_sparsity_pattern(
+                    dsp,
+                    Utilities::MPI::all_gather(mpi_communicator,
+                                               dof_handler.locally_owned_dofs()),
+                    mpi_communicator,
+                    locally_relevant_dofs);
+            DoFTools::extract_locally_relevant_dofs(dof_handler, locally_relevant_dofs);
+            system_matrix.reinit(owned_partitioning, dsp, mpi_communicator);
 
-        linear_solution.collect_sizes();
-        system_rhs.collect_sizes();
+            linear_solution.reinit(owned_partitioning, relevant_partitioning, mpi_communicator);
+            system_rhs.reinit(owned_partitioning, mpi_communicator);
     }
 
     ///This  is  where  the  magic  happens.   The  equations  describing  the newtons method for finding 0s in the KKT conditions are implemented here.
@@ -710,6 +717,7 @@ namespace SAND {
     void
     KktSystem<dim>::assemble_block_system(const LA::MPI::BlockVector &state, const double barrier_size) {
         /*Remove any values from old iterations*/
+
         system_matrix.reinit(owned_partitioning, dsp, mpi_communicator);
         linear_solution = 0;
         system_rhs = 0;
@@ -1048,16 +1056,21 @@ namespace SAND {
             MatrixTools::local_apply_boundary_values(boundary_values, local_dof_indices,
                                                      cell_matrix, cell_rhs, true);
 
+            std::cout << "here" << std::endl;
+
             constraints.distribute_local_to_global(
                     cell_matrix, cell_rhs, local_dof_indices, system_matrix, system_rhs);
 
+            std::cout << "not here" << std::endl;
+
         }
+        system_matrix.compress(VectorOperation::add);
         system_rhs = calculate_rhs(state, barrier_size);
 
         for (const auto &cell: dof_handler.active_cell_iterators()) {
             const unsigned int i = cell->active_cell_index();
 
-            typename SparseMatrix<double>::iterator iter = density_filter.filter_matrix.begin(
+            typename LA::MPI::SparseMatrix::iterator iter = density_filter.filter_matrix.begin(
                     i);
             for (; iter != density_filter.filter_matrix.end(i); iter++) {
                 unsigned int j = iter->column();
@@ -1590,7 +1603,6 @@ namespace SAND {
     template<int dim>
     LA::MPI::BlockVector
     KktSystem<dim>::solve(const LA::MPI::BlockVector &state, double barrier_size) {
-        constraints.condense(system_matrix);
         double gmres_tolerance;
         if (Input::use_eisenstat_walker) {
             gmres_tolerance = std::max(
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index ce13643..5c3bd34 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -6,14 +6,18 @@
 #include <deal.II/lac/sparse_matrix.h>
 #include <deal.II/base/timer.h>
 #include <deal.II/lac/generic_linear_algebra.h>
-#include <deal.II/lac/petsc_solver.h>
-#include <deal.II/lac/petsc_precondition.h>
+#include <deal.II/lac/trilinos_solver.h>
 #include "../include/schur_preconditioner.h"
 #include "../include/input_information.h"
 #include "../include/sand_tools.h"
 #include <fstream>
 
 namespace SAND {
+    using MatrixType  = dealii::TrilinosWrappers::SparseMatrix;
+    using VectorType  = dealii::TrilinosWrappers::MPI::Vector;
+    using PayloadType = dealii::TrilinosWrappers::internal::LinearOperatorImplementation::TrilinosPayload;
+    using PayloadVectorType = typename PayloadType::VectorType;
+    using size_type         = dealii::types::global_dof_index;
     using namespace dealii;
     template<int dim>
     TopOptSchurPreconditioner<dim>::TopOptSchurPreconditioner(LA::MPI::BlockSparseMatrix &matrix_in)
@@ -24,6 +28,7 @@ namespace SAND {
             n_block_rows(0),
             n_block_columns(0),
             other_solver_control(100000, 1e-10),
+//            direct_solver_control(1, 0),
             other_bicgstab(other_solver_control),
             other_gmres(other_solver_control),
             other_cg(other_solver_control),
@@ -38,6 +43,7 @@ namespace SAND {
             m_vect(matrix_in.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier)),
             timer(std::cout, TimerOutput::summary,
                   TimerOutput::wall_times)
+//            a_inv_direct(direct_solver_control)
     {
 
     }
@@ -96,7 +102,7 @@ namespace SAND {
         else
         {
             TimerOutput::Scope t(timer, "build A inv");
-            a_inv_direct.initialize(a_mat);
+//            a_inv_direct.initialize(a_mat);
         }
         {
             TimerOutput::Scope t(timer, "reinit diag matrices");
@@ -152,31 +158,34 @@ namespace SAND {
             }
         }
 
-        pre_j.reinit(matrix.block(SolutionBlocks::density,SolutionBlocks::density).n());
-        pre_k.reinit(matrix.block(SolutionBlocks::density,SolutionBlocks::density).n());
-        g_d_m_inv_density.reinit(matrix.block(SolutionBlocks::density,SolutionBlocks::density).n());
-        k_g_d_m_inv_density.reinit(matrix.block(SolutionBlocks::density,SolutionBlocks::density).n());
+        pre_j=state.block(SolutionBlocks::density);
+        pre_k=state.block(SolutionBlocks::density);
+        g_d_m_inv_density=state.block(SolutionBlocks::density);
+        k_g_d_m_inv_density=state.block(SolutionBlocks::density);
+        LinearOperator<VectorType,VectorType,PayloadType> op_g;
+        LinearOperator<VectorType,VectorType,PayloadType> op_h;
+        LinearOperator<VectorType,VectorType,PayloadType> op_f;
+        LinearOperator<VectorType,VectorType,PayloadType> op_d_8;
+        op_f = linear_operator<VectorType,VectorType,PayloadType>(f_mat);
+        op_d_8 = linear_operator<VectorType,VectorType,PayloadType>(d_8_mat);
+        op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) * transpose_operator<VectorType, VectorType, PayloadType>(linear_operator<VectorType,VectorType,PayloadType>(f_mat));
 
-        auto op_g = linear_operator(f_mat) * linear_operator(d_8_mat) *
-                    transpose_operator(linear_operator(f_mat));
-
-        auto op_h = op_g;
 
         SolverControl            solver_control(1000, 1e-12);
-        SolverCG<Vector<double>> a_solver_cg(solver_control);
-        auto a_inv_op = inverse_operator(linear_operator(a_mat),a_solver_cg,PreconditionIdentity());
+        LA::SolverCG a_solver_cg(solver_control);
+        auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg,PreconditionIdentity());
 
         if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
         {
-            op_h = linear_operator(b_mat)
-                   - transpose_operator(linear_operator(c_mat)) * a_inv_op * linear_operator(e_mat)
-                   - transpose_operator(linear_operator(e_mat)) * a_inv_op * linear_operator(c_mat);
+            op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
+                   - transpose_operator<VectorType, VectorType, PayloadType>(linear_operator<VectorType, VectorType, PayloadType>(c_mat)) * a_inv_op * linear_operator<VectorType,VectorType,PayloadType>(e_mat)
+                   - transpose_operator<VectorType, VectorType, PayloadType>(linear_operator<VectorType, VectorType, PayloadType>(e_mat)) * a_inv_op * linear_operator<VectorType,VectorType,PayloadType>(c_mat);
         }
         else
         {
-            op_h = linear_operator(b_mat)
-                   - transpose_operator(linear_operator(c_mat)) * linear_operator(a_inv_direct) * linear_operator(e_mat)
-                   - transpose_operator(linear_operator(e_mat)) * linear_operator(a_inv_direct) * linear_operator(c_mat);
+//            op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
+//                   - transpose_operator<VectorType, VectorType, PayloadType>(e_mat) * linear_operator<VectorType, VectorType, PayloadType>(a_inv_direct) * transpose_operator<VectorType, VectorType, PayloadType>(c_mat)
+//                   - transpose_operator<VectorType, VectorType, PayloadType>(c_mat) * linear_operator<VectorType, VectorType, PayloadType>(a_inv_direct) * transpose_operator<VectorType, VectorType, PayloadType>(e_mat);
         }
 
         if(Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres || Input::solver_choice == SolverOptions::inexact_K_with_exact_A_gmres)
@@ -185,26 +194,26 @@ namespace SAND {
         }
         else
         {
-            {
-                TimerOutput::Scope t(timer, "build g_mat");
-                g_mat.reinit(b_mat.n(), b_mat.n());
-                build_matrix_element_by_element(op_g, g_mat);
-            }
-
-
-            {
-                TimerOutput::Scope t(timer, "build h_mat");
-                h_mat.reinit(b_mat.n(), b_mat.n());
-                build_matrix_element_by_element(op_h, h_mat);
-            }
-
-            {
-                TimerOutput::Scope t(timer, "build k_inv_mat");
-                auto op_k_inv = -1 * op_g * linear_operator(d_m_inv_mat) * op_h -
-                                linear_operator(d_m_mat);
-                k_inv_mat.reinit(b_mat.n(), b_mat.n());
-                build_matrix_element_by_element(op_k_inv, k_inv_mat);
-            }
+//            {
+//                TimerOutput::Scope t(timer, "build g_mat");
+//                g_mat.reinit(b_mat.n(), b_mat.n());
+//                build_matrix_element_by_element(op_g, g_mat);
+//            }
+//
+//
+//            {
+//                TimerOutput::Scope t(timer, "build h_mat");
+//                h_mat.reinit(b_mat.n(), b_mat.n());
+//                build_matrix_element_by_element(op_h, h_mat);
+//            }
+//
+//            {
+//                TimerOutput::Scope t(timer, "build k_inv_mat");
+//                auto op_k_inv = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h -
+//                                linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
+//                k_inv_mat.reinit(b_mat.n(), b_mat.n());
+//                build_matrix_element_by_element(op_k_inv, k_inv_mat);
+//            }
         }
 
 
@@ -267,40 +276,53 @@ namespace SAND {
     template<int dim>
     void TopOptSchurPreconditioner<dim>::vmult_step_1(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
         dst = src;
-        d_5_mat.vmult_add(dst.block(SolutionBlocks::unfiltered_density),-1 * src.block(SolutionBlocks::density_lower_slack_multiplier));
-        d_6_mat.vmult_add(dst.block(SolutionBlocks::unfiltered_density),-1 * src.block(SolutionBlocks::density_upper_slack_multiplier));
-        dst.block(SolutionBlocks::unfiltered_density)+=src.block(SolutionBlocks::density_lower_slack);
-        dst.block(SolutionBlocks::unfiltered_density) -=src.block(SolutionBlocks::density_upper_slack);
+        auto dst_temp = dst;
+        auto op_d_5 = linear_operator<VectorType,VectorType,PayloadType>(d_5_mat);
+        auto op_d_6 = linear_operator<VectorType,VectorType,PayloadType>(d_6_mat);
+        dst_temp.block(SolutionBlocks::density_upper_slack) = -1 * op_d_5 * src.block(SolutionBlocks::density_lower_slack_multiplier);
+        dst_temp.block(SolutionBlocks::density_lower_slack) = op_d_6 * src.block(SolutionBlocks::density_upper_slack_multiplier);
+        dst.block(SolutionBlocks::unfiltered_density) = dst_temp.block(SolutionBlocks::unfiltered_density) +
+                                                                   dst_temp.block(SolutionBlocks::density_upper_slack) +
+                                                                   dst_temp.block(SolutionBlocks::density_lower_slack) +
+                                                                   src.block(SolutionBlocks::density_lower_slack) -
+                                                                   src.block(SolutionBlocks::density_upper_slack);
     }
 
     template<int dim>
     void TopOptSchurPreconditioner<dim>::vmult_step_2(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
         dst = src;
         auto dst_temp = dst;
-                d_8_mat.vmult(dst_temp.block(SolutionBlocks::unfiltered_density_multiplier), src.block(SolutionBlocks::unfiltered_density));
-                f_mat.vmult(dst.block(SolutionBlocks::unfiltered_density_multiplier),-1*dst_temp.block(SolutionBlocks::unfiltered_density_multiplier));
+        dst_temp.block(SolutionBlocks::density) = linear_operator<VectorType,VectorType,PayloadType>(f_mat)*linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) * src.block(SolutionBlocks::unfiltered_density);
+        dst.block(SolutionBlocks::unfiltered_density_multiplier) = dst_temp.block(SolutionBlocks::density) + dst_temp.block(SolutionBlocks::unfiltered_density_multiplier);
+
     }
 
     template<int dim>
     void TopOptSchurPreconditioner<dim>::vmult_step_3(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
         dst = src;
-        auto dst_temp = dst;
+
         if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
         {
-            SolverControl            solver_control(1000, 1e-12);
-            LA::SolverCG a_solver_cg(solver_control);
-            a_solver_cg.solve(a_mat,dst_temp.block(SolutionBlocks::density),src.block(SolutionBlocks::displacement),dealii::PETScWrappers::PreconditionNone);
-            e_mat.Tvmult_add(dst.block(SolutionBlocks::density),-1*dst_temp.block(SolutionBlocks::density));
+            SolverControl            solver_control_1(1000, 1e-12);
+            SolverControl            solver_control_2(1000, 1e-12);
+            LA::SolverCG a_solver_cg_1(solver_control_1);
+            LA::SolverCG a_solver_cg_2(solver_control_2);
+            auto dst_temp = dst;
+            a_solver_cg_1.solve(a_mat,dst_temp.block(SolutionBlocks::displacement_multiplier),src.block(SolutionBlocks::displacement_multiplier),dealii::TrilinosWrappers::PreconditionIdentity());
+            a_solver_cg_2.solve(a_mat,dst_temp.block(SolutionBlocks::displacement),src.block(SolutionBlocks::displacement),dealii::TrilinosWrappers::PreconditionIdentity());
+            c_mat.Tvmult(dst_temp.block(SolutionBlocks::density_upper_slack),dst_temp.block(SolutionBlocks::displacement_multiplier));
+            e_mat.Tvmult(dst_temp.block(SolutionBlocks::density_lower_slack),dst_temp.block(SolutionBlocks::displacement));
+
+            dst.block(SolutionBlocks::density) = dst_temp.block(SolutionBlocks::density) - dst_temp.block(SolutionBlocks::density_upper_slack) - dst_temp.block(SolutionBlocks::density_lower_slack);
+
 
-            a_solver_cg.solve(a_mat,dst_temp.block(SolutionBlocks::density),src.block(SolutionBlocks::displacement_multiplier));
-            c_mat.Tvmult_add(dst.block(SolutionBlocks::density),-1*dst_temp.block(SolutionBlocks::density));
         }
         else
         {
-            a_inv_direct.vmult(dst_temp.block(SolutionBlocks::density),src.block(SolutionBlocks::displacement));
-            e_mat.Tvmult_add(dst.block(SolutionBlocks::density),-1 *dst_temp.block(SolutionBlocks::density));
-            a_inv_direct.vmult(dst_temp.block(SolutionBlocks::density),src.block(SolutionBlocks::displacement_multiplier));
-            e_mat.Tvmult_add(dst.block(SolutionBlocks::density),-1 *dst_temp.block(SolutionBlocks::density));
+//            a_inv_direct.vmult(dst_temp.block(SolutionBlocks::density),src.block(SolutionBlocks::displacement));
+//            e_mat.Tvmult_add(dst.block(SolutionBlocks::density),-1 *dst_temp.block(SolutionBlocks::density));
+//            a_inv_direct.vmult(dst_temp.block(SolutionBlocks::density),src.block(SolutionBlocks::displacement_multiplier));
+//            e_mat.Tvmult_add(dst.block(SolutionBlocks::density),-1 *dst_temp.block(SolutionBlocks::density));
         }
 
     }
@@ -308,80 +330,81 @@ namespace SAND {
     template<int dim>
     void TopOptSchurPreconditioner<dim>::vmult_step_4(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
         dst = src;
-        Vector<double> k_density_mult;
-        k_density_mult.reinit(src.block(SolutionBlocks::density).size());
+        auto dst_temp = dst;
+        auto k_density_mult =  src.block(SolutionBlocks::density);
 
 
 
         if (Input::solver_choice == SolverOptions::exact_preconditioner_with_gmres)
         {
-            g_d_m_inv_density = linear_operator(g_mat) * linear_operator(d_m_inv_mat) * src.block(SolutionBlocks::density);
-            k_g_d_m_inv_density = linear_operator(k_mat) * g_d_m_inv_density;
-            k_density_mult = linear_operator(k_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
+//            g_d_m_inv_density = linear_operator<VectorType,VectorType,PayloadType>(g_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
+//            k_g_d_m_inv_density = linear_operator<VectorType,VectorType,PayloadType>(k_mat) * g_d_m_inv_density;
+//            k_density_mult = linear_operator<VectorType,VectorType,PayloadType>(k_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
         }
 
         else if (Input::solver_choice == SolverOptions::inexact_K_with_exact_A_gmres)
         {
-            auto op_g = linear_operator(f_mat) * linear_operator(d_8_mat) *
-                    transpose_operator(linear_operator(f_mat));
-
-            auto op_h = linear_operator(b_mat)
-                   - transpose_operator(linear_operator(c_mat)) * linear_operator(a_inv_direct) * linear_operator(e_mat)
-                   - transpose_operator(linear_operator(e_mat)) * linear_operator(a_inv_direct) * linear_operator(c_mat);
-
-            auto op_k_inv = -1 * op_g * linear_operator(d_m_inv_mat) * op_h - linear_operator(d_m_mat);
-
-            g_d_m_inv_density = op_g * linear_operator(d_m_inv_mat) * src.block(SolutionBlocks::density);
-
-            SolverControl step_4_gmres_control_1 (10000, g_d_m_inv_density.l2_norm()*1e-6);
-            SolverGMRES<Vector<double>> step_4_gmres_1 (step_4_gmres_control_1);
-            try {
-                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
-                                      g_d_m_inv_density;
-            } catch (std::exception &exc)
-            {
-                std::cerr << "Failure of linear solver step_4_gmres_1" << std::endl;
-                std::cout << "first residual: " << step_4_gmres_control_1.initial_value() << std::endl;
-                std::cout << "last residual: " << step_4_gmres_control_1.last_value() << std::endl;
-                throw;
-            }
-
-            SolverControl step_4_gmres_control_2 (10000, src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6);
-            SolverGMRES<Vector<double>> step_4_gmres_2 (step_4_gmres_control_2);
-            try {
-                k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, PreconditionIdentity()) *
-                                 src.block(SolutionBlocks::unfiltered_density_multiplier);
-            } catch (std::exception &exc)
-            {
-                std::cerr << "Failure of linear solver step_4_gmres_2" << std::endl;
-                std::cout << "first residual: " << step_4_gmres_control_2.initial_value() << std::endl;
-                std::cout << "last residual: " << step_4_gmres_control_2.last_value() << std::endl;
-                throw;
-            }
+//            auto op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) *
+//                    transpose_operator<VectorType, VectorType, PayloadType>(linear_operator<VectorType,VectorType,PayloadType>(f_mat));
+//
+//            auto op_h = linear_operator(b_mat)
+//                   - transpose_operator<VectorType, VectorType, PayloadType>(linear_operator(c_mat)) * linear_operator(a_inv_direct) * linear_operator(e_mat)
+//                   - transpose_operator<VectorType, VectorType, PayloadType>(linear_operator(e_mat)) * linear_operator(a_inv_direct) * linear_operator(c_mat);
+//
+//            auto op_k_inv = -1 * op_g * linear_operator(d_m_inv_mat) * op_h - linear_operator(d_m_mat);
+//
+//            g_d_m_inv_density = op_g * linear_operator(d_m_inv_mat) * src.block(SolutionBlocks::density);
+//
+//            SolverControl step_4_gmres_control_1 (10000, g_d_m_inv_density.l2_norm()*1e-6);
+//            SolverGMRES<Vector<double>> step_4_gmres_1 (step_4_gmres_control_1);
+//            try {
+//                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
+//                                      g_d_m_inv_density;
+//            } catch (std::exception &exc)
+//            {
+//                std::cerr << "Failure of linear solver step_4_gmres_1" << std::endl;
+//                std::cout << "first residual: " << step_4_gmres_control_1.initial_value() << std::endl;
+//                std::cout << "last residual: " << step_4_gmres_control_1.last_value() << std::endl;
+//                throw;
+//            }
+//
+//            SolverControl step_4_gmres_control_2 (10000, src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6);
+//            SolverGMRES<Vector<double>> step_4_gmres_2 (step_4_gmres_control_2);
+//            try {
+//                k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, PreconditionIdentity()) *
+//                                 src.block(SolutionBlocks::unfiltered_density_multiplier);
+//            } catch (std::exception &exc)
+//            {
+//                std::cerr << "Failure of linear solver step_4_gmres_2" << std::endl;
+//                std::cout << "first residual: " << step_4_gmres_control_2.initial_value() << std::endl;
+//                std::cout << "last residual: " << step_4_gmres_control_2.last_value() << std::endl;
+//                throw;
+//            }
         }
         else if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
         {
-            PreconditionSSOR<SparseMatrix<double>> preconditioner;
-            preconditioner.initialize(a_mat, 1.2);
+
             SolverControl            solver_control(1000, 1e-12);
-            SolverCG<Vector<double>> a_solver_cg(solver_control);
-            auto a_inv_op = inverse_operator(linear_operator(a_mat),a_solver_cg,preconditioner);
+            LA::SolverCG a_solver_cg(solver_control);
+
+            auto preconditioner = dealii::TrilinosWrappers::PreconditionIdentity();
 
-            auto op_g = linear_operator(f_mat) * linear_operator(d_8_mat) *
-                        transpose_operator(linear_operator(f_mat));
+            auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg, preconditioner);
 
-            auto op_h = linear_operator(b_mat)
-                        - transpose_operator(linear_operator(c_mat)) * a_inv_op * linear_operator(e_mat)
-                        - transpose_operator(linear_operator(e_mat)) * a_inv_op * linear_operator(c_mat);
 
-            auto op_k_inv = -1 * op_g * linear_operator(d_m_inv_mat) * op_h - linear_operator(d_m_mat);
+            auto op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) *
+                        transpose_operator<VectorType, VectorType, PayloadType>(f_mat);
 
-            g_d_m_inv_density = op_g * linear_operator(d_m_inv_mat) * src.block(SolutionBlocks::density);
+            auto op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
+                        - transpose_operator<VectorType, VectorType, PayloadType>(c_mat) * a_inv_op * linear_operator<VectorType,VectorType,PayloadType>(e_mat)
+                        - transpose_operator<VectorType, VectorType, PayloadType>(e_mat) * a_inv_op * linear_operator<VectorType,VectorType,PayloadType>(c_mat);
+
+            auto op_k_inv = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
 
             SolverControl step_4_gmres_control_1 (10000, g_d_m_inv_density.l2_norm()*1e-6);
-            SolverGMRES<Vector<double>> step_4_gmres_1 (step_4_gmres_control_1);
+            LA::SolverGMRES step_4_gmres_1 (step_4_gmres_control_1);
             try {
-                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
+                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, preconditioner) *
                                       g_d_m_inv_density;
             } catch (std::exception &exc)
             {
@@ -392,9 +415,9 @@ namespace SAND {
             }
 
             SolverControl step_4_gmres_control_2 (10000, src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6);
-            SolverGMRES<Vector<double>> step_4_gmres_2 (step_4_gmres_control_2);
+            LA::SolverGMRES step_4_gmres_2 (step_4_gmres_control_2);
             try {
-                k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, PreconditionIdentity()) *
+                k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, preconditioner) *
                                  src.block(SolutionBlocks::unfiltered_density_multiplier);
             } catch (std::exception &exc)
             {
@@ -410,10 +433,11 @@ namespace SAND {
             throw;
         }
 
+        auto holder_1 = transpose_operator<VectorType, VectorType, PayloadType>(m_vect)*k_g_d_m_inv_density;
+        auto holder_2 = transpose_operator<VectorType, VectorType, PayloadType>(m_vect)*k_density_mult;
 
-        dst.block(SolutionBlocks::total_volume_multiplier) += transpose_operator(linear_operator(m_vect))*k_g_d_m_inv_density;
-        dst.block(SolutionBlocks::total_volume_multiplier) -= transpose_operator(linear_operator(m_vect))*k_density_mult;
-
+        dst.block(SolutionBlocks::total_volume_multiplier) = holder_1 - holder_2
+                                                        +dst_temp.block(SolutionBlocks::total_volume_multiplier);
     }
 
 
@@ -424,12 +448,12 @@ namespace SAND {
         {
             //First Block Inverse
             TimerOutput::Scope t(timer, "inverse 1");
-            dst.block(SolutionBlocks::density_lower_slack_multiplier) = linear_operator(d_3_mat) * src.block(SolutionBlocks::density_lower_slack_multiplier) +
-                    linear_operator(d_m_inv_mat) * src.block(SolutionBlocks::density_lower_slack);
-            dst.block(SolutionBlocks::density_upper_slack_multiplier) = linear_operator(d_4_mat) * src.block(SolutionBlocks::density_upper_slack_multiplier) +
-                    linear_operator(d_m_inv_mat) * src.block(SolutionBlocks::density_upper_slack);
-            dst.block(SolutionBlocks::density_lower_slack) = linear_operator(d_m_inv_mat) * src.block(SolutionBlocks::density_lower_slack_multiplier);
-            dst.block(SolutionBlocks::density_upper_slack) = linear_operator(d_m_inv_mat) * src.block(SolutionBlocks::density_upper_slack_multiplier);
+            dst.block(SolutionBlocks::density_lower_slack_multiplier) = linear_operator<VectorType,VectorType,PayloadType>(d_3_mat) * src.block(SolutionBlocks::density_lower_slack_multiplier) +
+                    linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density_lower_slack);
+            dst.block(SolutionBlocks::density_upper_slack_multiplier) = linear_operator<VectorType,VectorType,PayloadType>(d_4_mat) * src.block(SolutionBlocks::density_upper_slack_multiplier) +
+                    linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density_upper_slack);
+            dst.block(SolutionBlocks::density_lower_slack) = linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density_lower_slack_multiplier);
+            dst.block(SolutionBlocks::density_upper_slack) = linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density_upper_slack_multiplier);
         }
 
 
@@ -437,7 +461,7 @@ namespace SAND {
             //Second Block Inverse
             TimerOutput::Scope t(timer, "inverse 2");
             dst.block(SolutionBlocks::unfiltered_density) =
-                    linear_operator(d_8_mat) * src.block(SolutionBlocks::unfiltered_density);
+                    linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) * src.block(SolutionBlocks::unfiltered_density);
         }
 
 
@@ -446,17 +470,19 @@ namespace SAND {
             TimerOutput::Scope t(timer, "inverse 3");
             if(Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
             {
-                PreconditionSSOR<SparseMatrix<double>> preconditioner;
-                preconditioner.initialize(a_mat, 1.2);
                 SolverControl            solver_control(1000, 1e-12);
-                SolverCG<Vector<double>> a_solver_cg(solver_control);
-                auto a_inv_op = inverse_operator(linear_operator(a_mat),a_solver_cg,preconditioner);
+                LA::SolverCG a_solver_cg(solver_control);
+
+                auto preconditioner = dealii::TrilinosWrappers::PreconditionIdentity();
+
+                auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg, preconditioner);
+
                 dst.block(SolutionBlocks::displacement) = a_inv_op * src.block(SolutionBlocks::displacement_multiplier);
                 dst.block(SolutionBlocks::displacement_multiplier) = a_inv_op * src.block(SolutionBlocks::displacement);
             } else
             {
-                dst.block(SolutionBlocks::displacement) = linear_operator(a_inv_direct) * src.block(SolutionBlocks::displacement_multiplier);
-                dst.block(SolutionBlocks::displacement_multiplier) = linear_operator(a_inv_direct) * src.block(SolutionBlocks::displacement);
+//                dst.block(SolutionBlocks::displacement) = linear_operator(a_inv_direct) * src.block(SolutionBlocks::displacement_multiplier);
+//                dst.block(SolutionBlocks::displacement_multiplier) = linear_operator(a_inv_direct) * src.block(SolutionBlocks::displacement);
             }
 
         }
@@ -470,76 +496,79 @@ namespace SAND {
 
             if (Input::solver_choice == SolverOptions::exact_preconditioner_with_gmres)
             {
-                pre_j = src.block(SolutionBlocks::density) + linear_operator(h_mat) * linear_operator(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
-                pre_k = -1* linear_operator(g_mat) * linear_operator(d_m_inv_mat) * src.block(SolutionBlocks::density) + src.block(SolutionBlocks::unfiltered_density_multiplier);
-                dst.block(SolutionBlocks::unfiltered_density_multiplier) = transpose_operator(linear_operator(k_mat)) * pre_j;
-                dst.block(SolutionBlocks::density) = linear_operator(k_mat) * pre_k;
+//                pre_j = src.block(SolutionBlocks::density) + linear_operator<VectorType,VectorType,PayloadType>(h_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
+//                pre_k = -1* linear_operator<VectorType,VectorType,PayloadType>(g_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density) + src.block(SolutionBlocks::unfiltered_density_multiplier);
+//                dst.block(SolutionBlocks::unfiltered_density_multiplier) = transpose_operator<VectorType, VectorType, PayloadType>(k_mat) * pre_j;
+//                dst.block(SolutionBlocks::density) = linear_operator<VectorType,VectorType,PayloadType>(k_mat) * pre_k;
             }
 
             else if (Input::solver_choice == SolverOptions::inexact_K_with_exact_A_gmres)
             {
-                auto op_g = linear_operator(f_mat) * linear_operator(d_8_mat) *
-                            transpose_operator(linear_operator(f_mat));
-
-                auto op_h = linear_operator(b_mat)
-                            - transpose_operator(linear_operator(c_mat)) * linear_operator(a_inv_direct) * linear_operator(e_mat)
-                            - transpose_operator(linear_operator(e_mat)) * linear_operator(a_inv_direct) * linear_operator(c_mat);
-
-                auto op_k_inv = -1 * op_g * linear_operator(d_m_inv_mat) * op_h - linear_operator(d_m_mat);
-
-                pre_j = src.block(SolutionBlocks::density) + op_h * linear_operator(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
-                pre_k = -1* op_g * linear_operator(d_m_inv_mat) * src.block(SolutionBlocks::density) + src.block(SolutionBlocks::unfiltered_density_multiplier);
-
-                SolverControl step_5_gmres_control_1 (10000, pre_j.l2_norm()*1e-6);
-                SolverGMRES<Vector<double>> step_5_gmres_1 (step_5_gmres_control_1);
-                try {
-                    dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
-                                          pre_j;
-                } catch (std::exception &exc)
-                {
-                    std::cerr << "Failure of linear solver step_5_gmres_1" << std::endl;
-                    std::cout << "first residual: " << step_5_gmres_control_1.initial_value() << std::endl;
-                    std::cout << "last residual: " << step_5_gmres_control_1.last_value() << std::endl;
-                    throw;
-                }
 
-                SolverControl step_5_gmres_control_2 (10000, pre_k.l2_norm()*1e-6);
-                SolverGMRES<Vector<double>> step_5_gmres_2 (step_5_gmres_control_2);
-                try {
-                    dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) *
-                                                                               pre_k;
-                } catch (std::exception &exc)
-                {
-                    std::cerr << "Failure of linear solver step_5_gmres_2" << std::endl;
-                    std::cout << "first residual: " << step_5_gmres_control_2.initial_value() << std::endl;
-                    std::cout << "last residual: " << step_5_gmres_control_2.last_value() << std::endl;
-                    throw;
-                }
+//                a_inv_direct.initialize(a_mat);
+//
+//                auto op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) *
+//                            transpose_operator<VectorType, VectorType, PayloadType>(f_mat);
+//
+//                auto op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
+//                            - transpose_operator<VectorType, VectorType, PayloadType>(c_mat) * inverse_operator(
+//                                    linear_operator<VectorType,VectorType,PayloadType>(a_mat), a_inv_direct, TrilinosWrappers::PreconditionIdentity()) * linear_operator<VectorType,VectorType,PayloadType>(e_mat)
+//                            - transpose_operator<VectorType, VectorType, PayloadType>(e_mat) * linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct) * linear_operator<VectorType,VectorType,PayloadType>(c_mat);
+//
+//                auto op_k_inv = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
+//
+//                pre_j = src.block(SolutionBlocks::density) + op_h * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
+//                pre_k = -1* op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density) + src.block(SolutionBlocks::unfiltered_density_multiplier);
+//
+//                SolverControl step_5_gmres_control_1 (10000, pre_j.l2_norm()*1e-6);
+//                SolverGMRES<Vector<double>> step_5_gmres_1 (step_5_gmres_control_1);
+//                try {
+//                    dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
+//                                          pre_j;
+//                } catch (std::exception &exc)
+//                {
+//                    std::cerr << "Failure of linear solver step_5_gmres_1" << std::endl;
+//                    std::cout << "first residual: " << step_5_gmres_control_1.initial_value() << std::endl;
+//                    std::cout << "last residual: " << step_5_gmres_control_1.last_value() << std::endl;
+//                    throw;
+//                }
+//
+//                SolverControl step_5_gmres_control_2 (10000, pre_k.l2_norm()*1e-6);
+//                SolverGMRES<Vector<double>> step_5_gmres_2 (step_5_gmres_control_2);
+//                try {
+//                    dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) *
+//                                                                               pre_k;
+//                } catch (std::exception &exc)
+//                {
+//                    std::cerr << "Failure of linear solver step_5_gmres_2" << std::endl;
+//                    std::cout << "first residual: " << step_5_gmres_control_2.initial_value() << std::endl;
+//                    std::cout << "last residual: " << step_5_gmres_control_2.last_value() << std::endl;
+//                    throw;
+//                }
             }
             else if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
             {
-                PreconditionSSOR<SparseMatrix<double>> preconditioner;
-                preconditioner.initialize(a_mat, 1.2);
+
                 SolverControl            solver_control(1000, 1e-12);
-                SolverCG<Vector<double>> a_solver_cg(solver_control);
-                auto a_inv_op = inverse_operator(linear_operator(a_mat),a_solver_cg,preconditioner);
+                LA::SolverCG a_solver_cg (solver_control);
+                auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg,PreconditionIdentity());
 
-                auto op_g = linear_operator(f_mat) * linear_operator(d_8_mat) *
-                            transpose_operator(linear_operator(f_mat));
+                auto op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) *
+                            transpose_operator<VectorType, VectorType, PayloadType>(f_mat);
 
-                auto op_h = linear_operator(b_mat)
-                            - transpose_operator(linear_operator(c_mat)) * a_inv_op * linear_operator(e_mat)
-                            - transpose_operator(linear_operator(e_mat)) * a_inv_op * linear_operator(c_mat);
+                auto op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
+                            - transpose_operator<VectorType, VectorType, PayloadType>(c_mat) * a_inv_op * linear_operator<VectorType,VectorType,PayloadType>(e_mat)
+                            - transpose_operator<VectorType, VectorType, PayloadType>(e_mat) * a_inv_op * linear_operator<VectorType,VectorType,PayloadType>(c_mat);
 
-                auto op_k_inv = -1 * op_g * linear_operator(d_m_inv_mat) * op_h - linear_operator(d_m_mat);
+                auto op_k_inv = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
 
-                pre_j = src.block(SolutionBlocks::density) + op_h * linear_operator(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
-                pre_k = -1* op_g * linear_operator(d_m_inv_mat) * src.block(SolutionBlocks::density) + src.block(SolutionBlocks::unfiltered_density_multiplier);
+                pre_j = src.block(SolutionBlocks::density) + op_h * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
+                pre_k = -1* op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density) + src.block(SolutionBlocks::unfiltered_density_multiplier);
 
                 SolverControl step_5_gmres_control_1 (10000, pre_j.l2_norm()*1e-6);
-                SolverGMRES<Vector<double>> step_5_gmres_1 (step_5_gmres_control_1);
+                LA::SolverGMRES step_5_gmres_1 (step_5_gmres_control_1);
                 try {
-                    dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
+                    dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType,VectorType,PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
                                                                                pre_j;
                 } catch (std::exception &exc)
                 {
@@ -550,7 +579,7 @@ namespace SAND {
                 }
 
                 SolverControl step_5_gmres_control_2 (10000, pre_k.l2_norm()*1e-6);
-                SolverGMRES<Vector<double>> step_5_gmres_2 (step_5_gmres_control_2);
+                LA::SolverGMRES step_5_gmres_2 (step_5_gmres_control_2);
                 try {
                     dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) *
                                                          pre_k;
@@ -724,7 +753,7 @@ namespace SAND {
 
 
     template<int dim>
-    void TopOptSchurPreconditioner<dim>::print_stuff(const BlockSparseMatrix<double> &matrix)
+    void TopOptSchurPreconditioner<dim>::print_stuff(const LA::MPI::BlockSparseMatrix &matrix)
     {
 
 //        print_matrix("OAmat.csv",a_mat);
@@ -732,23 +761,23 @@ namespace SAND {
 //        print_matrix("OCmat.csv",c_mat);
 //        print_matrix("OEmat.csv",e_mat);
 //        print_matrix("OFmat.csv",f_mat);
-        FullMatrix<double> g_mat;
-        FullMatrix<double> h_mat;
-        FullMatrix<double> k_inv_mat;
-        g_mat.reinit(b_mat.m(),b_mat.n());
-        h_mat.reinit(b_mat.m(),b_mat.n());
-        k_inv_mat.reinit(b_mat.m(),b_mat.n());
-        auto op_g = linear_operator(f_mat) * linear_operator(d_8_mat) *
-                    transpose_operator(linear_operator(f_mat));
-
-        auto op_h = linear_operator(b_mat)
-                    - transpose_operator(linear_operator(c_mat)) * linear_operator(a_inv_direct) * linear_operator(e_mat)
-                    - transpose_operator(linear_operator(e_mat)) * linear_operator(a_inv_direct) * linear_operator(c_mat);
-
-        auto op_k_inv = -1 * op_g * linear_operator(d_m_inv_mat) * op_h - linear_operator(d_m_mat);
-        build_matrix_element_by_element(op_g,g_mat);
-        build_matrix_element_by_element(op_h,h_mat);
-        build_matrix_element_by_element(op_k_inv,k_inv_mat);
+//        FullMatrix<double> g_mat;
+//        FullMatrix<double> h_mat;
+//        FullMatrix<double> k_inv_mat;
+//        g_mat.reinit(b_mat.m(),b_mat.n());
+//        h_mat.reinit(b_mat.m(),b_mat.n());
+//        k_inv_mat.reinit(b_mat.m(),b_mat.n());
+//        auto op_g = linear_operator(f_mat) * linear_operator(d_8_mat) *
+//                    transpose_operator(linear_operator(f_mat));
+//
+//        auto op_h = linear_operator(b_mat)
+//                    - transpose_operator(linear_operator(c_mat)) * linear_operator(a_inv_direct) * linear_operator(e_mat)
+//                    - transpose_operator(linear_operator(e_mat)) * linear_operator(a_inv_direct) * linear_operator(c_mat);
+//
+//        auto op_k_inv = -1 * op_g * linear_operator(d_m_inv_mat) * op_h - linear_operator(d_m_mat);
+//        build_matrix_element_by_element(op_g,g_mat);
+//        build_matrix_element_by_element(op_h,h_mat);
+//        build_matrix_element_by_element(op_k_inv,k_inv_mat);
 //        print_matrix("OGmat.csv",g_mat);
 //        print_matrix("OHmat.csv",h_mat);
 //        print_matrix("OKinvmat.csv",k_inv_mat);
diff --git a/source/watchdog_main.cc b/source/watchdog_main.cc
index 9aa8879..e51d25e 100644
--- a/source/watchdog_main.cc
+++ b/source/watchdog_main.cc
@@ -7,6 +7,8 @@
 #include "../include/kkt_system.h"
 #include "../include/input_information.h"
 #include <deal.II/lac/generic_linear_algebra.h>
+#include <deal.II/lac/trilinos_parallel_block_vector.h>
+#include <deal.II/lac/generic_linear_algebra.h>
 
 ///Above are fairly normal files to include.  I also use the sparse direct package, which requiresBLAS/LAPACK
 /// to  perform  a  direct  solve  while  I  work  on  a  fast  iterative  solver  for  this problem.
@@ -15,7 +17,7 @@
 namespace SAND {
     namespace LA
     {
-        using namespace dealii::LinearAlgebraPETSc;
+        using namespace dealii::LinearAlgebraTrilinos;
     }
 
 
@@ -296,6 +298,7 @@ namespace SAND {
         kkt_system.create_triangulation();
         kkt_system.setup_boundary_values();
         kkt_system.setup_filter_matrix();
+        std::cout << "setup kkt system" << std::endl;
         kkt_system.setup_block_system();
 
         if (Input::barrier_reduction==BarrierOptions::mixed)
@@ -416,8 +419,9 @@ namespace SAND {
 } // namespace SAND
 
 int
-main() {
+main(int argc, char *argv[]) {
     try {
+        Utilities::MPI::MPI_InitFinalize mpi_initialization(argc, argv, 1);
         SAND::SANDTopOpt<SAND::Input::dim> elastic_problem_2d;
         elastic_problem_2d.run();
     }

From aed10fd1fced764757791cc45077250a6c00e232 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Wed, 27 Oct 2021 10:17:02 -0600
Subject: [PATCH 04/95] Still Very broken. trying to distribute triangulation

---
 include/density_filter.h |  2 +-
 include/kkt_system.h     | 19 +++++++++++++------
 source/density_filter.cc |  2 +-
 source/kkt_system.cc     | 24 ++++++++++++++----------
 4 files changed, 29 insertions(+), 18 deletions(-)

diff --git a/include/density_filter.h b/include/density_filter.h
index e853b8a..ccd539a 100644
--- a/include/density_filter.h
+++ b/include/density_filter.h
@@ -61,7 +61,7 @@ namespace SAND {
         DynamicSparsityPattern filter_dsp;
         LA::MPI::SparseMatrix filter_matrix;
         SparsityPattern filter_sparsity_pattern;
-        void initialize(Triangulation<dim> &triangulation);
+        void initialize(parallel::distributed::Triangulation<dim> &triangulation);
         std::set<typename Triangulation<dim>::cell_iterator> find_relevant_neighbors(typename Triangulation<dim>::cell_iterator cell) const;
 
     private:
diff --git a/include/kkt_system.h b/include/kkt_system.h
index 5e76e07..a2015a6 100644
--- a/include/kkt_system.h
+++ b/include/kkt_system.h
@@ -5,11 +5,15 @@
 #ifndef SAND_KKT_SYSTEM_H
 #define SAND_KKT_SYSTEM_H
 #include <deal.II/base/quadrature_lib.h>
+#include <deal.II/base/utilities.h>
+#include <deal.II/base/conditional_ostream.h>
+#include <deal.II/base/index_set.h>
 #include <deal.II/base/function.h>
 #include <deal.II/base/tensor.h>
 #include <deal.II/base/timer.h>
 
 #include <deal.II/lac/block_vector.h>
+#include <deal.II/lac/vector.h>
 #include <deal.II/lac/full_matrix.h>
 #include <deal.II/lac/block_sparse_matrix.h>
 #include <deal.II/lac/solver_cg.h>
@@ -19,14 +23,17 @@
 #include <deal.II/lac/sparse_direct.h>
 #include <deal.II/lac/affine_constraints.h>
 #include <deal.II/lac/generic_linear_algebra.h>
-#include <deal.II/lac/petsc_block_sparse_matrix.h>
-#include <deal.II/lac/petsc_sparse_matrix.h>
-#include <deal.II/lac/petsc_vector.h>
-#include <deal.II/lac/petsc_solver.h>
+#include <deal.II/lac/dynamic_sparsity_pattern.h>
+#include <deal.II/lac/sparsity_tools.h>
+#include <deal.II/lac/trilinos_sparse_matrix.h>
+#include <deal.II/lac/trilinos_block_sparse_matrix.h>
+#include <deal.II/lac/trilinos_parallel_block_vector.h>
+#include <deal.II/lac/trilinos_vector.h>
 
-#include <deal.II/grid/tria.h>
 #include <deal.II/grid/grid_generator.h>
+#include <deal.II/grid/manifold_lib.h>
 #include <deal.II/grid/grid_refinement.h>
+#include <deal.II/grid/grid_tools.h>
 
 #include <deal.II/dofs/dof_handler.h>
 #include <deal.II/dofs/dof_renumbering.h>
@@ -124,7 +131,7 @@ namespace SAND {
         LA::MPI::BlockSparseMatrix system_matrix;
         LA::MPI::BlockVector linear_solution;
         LA::MPI::BlockVector system_rhs;
-        Triangulation<dim> triangulation;
+        parallel::distributed::Triangulation<dim> triangulation;
         DoFHandler<dim> dof_handler;
         AffineConstraints<double> constraints;
         FESystem<dim> fe_nine;
diff --git a/source/density_filter.cc b/source/density_filter.cc
index 975be83..6b2e780 100644
--- a/source/density_filter.cc
+++ b/source/density_filter.cc
@@ -17,7 +17,7 @@ namespace SAND {
      * convolution being applied to a piecewise constant function on that triangulation  */
     template<int dim>
     void
-    DensityFilter<dim>::initialize(Triangulation<dim> &triangulation) {
+    DensityFilter<dim>::initialize(parallel::distributed::Triangulation<dim> &triangulation) {
 
         filter_dsp.reinit(triangulation.n_active_cells(),
                           triangulation.n_active_cells());
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index 2fc6f37..f03a37e 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -14,15 +14,12 @@
 #include <deal.II/lac/linear_operator.h>
 #include <deal.II/lac/solver_gmres.h>
 #include <deal.II/lac/generic_linear_algebra.h>
-#include <deal.II/lac/petsc_block_sparse_matrix.h>
-#include <deal.II/lac/petsc_solver.h>
+#include <deal.II/lac/trilinos_sparse_matrix.h>
+#include <deal.II/lac/trilinos_block_sparse_matrix.h>
+
 
-#include <deal.II/lac/petsc_sparse_matrix.h>
-#include <deal.II/lac/petsc_vector.h>
-#include <deal.II/lac/petsc_solver.h>
 #include <deal.II/lac/matrix_out.h>
 
-#include <deal.II/grid/tria.h>
 #include <deal.II/grid/grid_generator.h>
 #include <deal.II/grid/grid_refinement.h>
 
@@ -43,6 +40,10 @@
 #include <deal.II/numerics/data_out.h>
 #include <deal.II/numerics/error_estimator.h>
 
+#include <deal.II/distributed/tria.h>
+#include <deal.II/distributed/grid_refinement.h>
+
+
 #include "../include/input_information.h"
 #include "../include/sand_tools.h"
 
@@ -55,6 +56,11 @@ namespace SAND {
     template<int dim>
     KktSystem<dim>::KktSystem()
             :
+            mpi_communicator(MPI_COMM_WORLD),
+            triangulation(mpi_communicator,
+                          typename Triangulation<dim>::MeshSmoothing(
+                                  Triangulation<dim>::smoothing_on_refinement |
+                                  Triangulation<dim>::smoothing_on_coarsening)),
             dof_handler(triangulation),
             /*fe should have 1 FE_DGQ<dim>(0) element for density, dim FE_Q finite elements for displacement,
              * another dim FE_Q elements for the lagrange multiplier on the FE constraint, and 2 more FE_DGQ<dim>(0)
@@ -69,8 +75,7 @@ namespace SAND {
                    FE_DGQ<dim>(0) ^ 1),
             density_ratio(Input::volume_percentage),
             density_penalty_exponent(Input::density_penalty_exponent),
-            density_filter(),
-            mpi_communicator(MPI_COMM_WORLD)
+            density_filter()
     {
         fe_collection.push_back(fe_nine);
         fe_collection.push_back(fe_ten);
@@ -1056,7 +1061,6 @@ namespace SAND {
             MatrixTools::local_apply_boundary_values(boundary_values, local_dof_indices,
                                                      cell_matrix, cell_rhs, true);
 
-            std::cout << "here" << std::endl;
 
             constraints.distribute_local_to_global(
                     cell_matrix, cell_rhs, local_dof_indices, system_matrix, system_rhs);
@@ -1066,7 +1070,7 @@ namespace SAND {
         }
         system_matrix.compress(VectorOperation::add);
         system_rhs = calculate_rhs(state, barrier_size);
-
+        system_rhs.compress(VectorOperation::add);
         for (const auto &cell: dof_handler.active_cell_iterators()) {
             const unsigned int i = cell->active_cell_index();
 

From a113ccb59769802adf0ed85f3e6260f8bf6d8988 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Wed, 27 Oct 2021 14:16:38 -0600
Subject: [PATCH 05/95] Works with trilinos vectors. Not sure how to use them
 in parallel

---
 source/kkt_system.cc           | 14 ++++++++------
 source/schur_preconditioner.cc | 22 ++++++++++------------
 2 files changed, 18 insertions(+), 18 deletions(-)

diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index f03a37e..a36d036 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -677,7 +677,8 @@ namespace SAND {
 
             system_matrix.clear();
 
-            DoFTools::make_sparsity_pattern(dof_handler, coupling, dsp, constraints, false);
+//            DoFTools::make_sparsity_pattern(dof_handler, coupling, dsp, constraints, false);
+            DoFTools::make_sparsity_pattern(dof_handler, dsp, constraints, false);
             SparsityTools::distribute_sparsity_pattern(dsp, dof_handler.locally_owned_dofs(), mpi_communicator,
                                                        locally_relevant_dofs);
             //adds the row into the sparsity pattern for the total volume constraint
@@ -723,7 +724,7 @@ namespace SAND {
     KktSystem<dim>::assemble_block_system(const LA::MPI::BlockVector &state, const double barrier_size) {
         /*Remove any values from old iterations*/
 
-        system_matrix.reinit(owned_partitioning, dsp, mpi_communicator);
+        system_matrix=0;
         linear_solution = 0;
         system_rhs = 0;
 
@@ -1065,12 +1066,10 @@ namespace SAND {
             constraints.distribute_local_to_global(
                     cell_matrix, cell_rhs, local_dof_indices, system_matrix, system_rhs);
 
-            std::cout << "not here" << std::endl;
 
         }
         system_matrix.compress(VectorOperation::add);
         system_rhs = calculate_rhs(state, barrier_size);
-        system_rhs.compress(VectorOperation::add);
         for (const auto &cell: dof_handler.active_cell_iterators()) {
             const unsigned int i = cell->active_cell_index();
 
@@ -1588,7 +1587,7 @@ namespace SAND {
                     cell_rhs, local_dof_indices, test_rhs);
 
         }
-
+        test_rhs.compress(VectorOperation::add);
         double total_volume = 0;
         double goal_volume = 0;
         for (const auto &cell: dof_handler.active_cell_iterators()) {
@@ -1598,6 +1597,8 @@ namespace SAND {
 
         test_rhs.block(SolutionBlocks::total_volume_multiplier)[0] = goal_volume - total_volume;
 
+
+
         return test_rhs;
 
     }
@@ -1615,7 +1616,8 @@ namespace SAND {
                             .001
                     ),
                     Input::default_gmres_tolerance);
-        } else {
+        }
+        else {
             gmres_tolerance = Input::default_gmres_tolerance;
         }
         SolverControl solver_control(10000, gmres_tolerance * system_rhs.l2_norm());
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 5c3bd34..d6b2182 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -303,10 +303,10 @@ namespace SAND {
 
         if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
         {
-            SolverControl            solver_control_1(1000, 1e-12);
-            SolverControl            solver_control_2(1000, 1e-12);
-            LA::SolverCG a_solver_cg_1(solver_control_1);
-            LA::SolverCG a_solver_cg_2(solver_control_2);
+            SolverControl            solver_control_1(100000, 1e-12);
+            SolverControl            solver_control_2(100000, 1e-12);
+            SolverCG<LA::MPI::Vector> a_solver_cg_1(solver_control_1);
+            SolverCG<LA::MPI::Vector> a_solver_cg_2(solver_control_2);
             auto dst_temp = dst;
             a_solver_cg_1.solve(a_mat,dst_temp.block(SolutionBlocks::displacement_multiplier),src.block(SolutionBlocks::displacement_multiplier),dealii::TrilinosWrappers::PreconditionIdentity());
             a_solver_cg_2.solve(a_mat,dst_temp.block(SolutionBlocks::displacement),src.block(SolutionBlocks::displacement),dealii::TrilinosWrappers::PreconditionIdentity());
@@ -471,11 +471,9 @@ namespace SAND {
             if(Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
             {
                 SolverControl            solver_control(1000, 1e-12);
-                LA::SolverCG a_solver_cg(solver_control);
+                SolverCG<LA::MPI::Vector> a_solver_cg(solver_control);
 
-                auto preconditioner = dealii::TrilinosWrappers::PreconditionIdentity();
-
-                auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg, preconditioner);
+                auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg, PreconditionIdentity());
 
                 dst.block(SolutionBlocks::displacement) = a_inv_op * src.block(SolutionBlocks::displacement_multiplier);
                 dst.block(SolutionBlocks::displacement_multiplier) = a_inv_op * src.block(SolutionBlocks::displacement);
@@ -550,8 +548,8 @@ namespace SAND {
             {
 
                 SolverControl            solver_control(1000, 1e-12);
-                LA::SolverCG a_solver_cg (solver_control);
-                auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg,PreconditionIdentity());
+                SolverCG<LA::MPI::Vector> a_solver_cg (solver_control);
+                auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg, PreconditionIdentity());
 
                 auto op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) *
                             transpose_operator<VectorType, VectorType, PayloadType>(f_mat);
@@ -566,7 +564,7 @@ namespace SAND {
                 pre_k = -1* op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density) + src.block(SolutionBlocks::unfiltered_density_multiplier);
 
                 SolverControl step_5_gmres_control_1 (10000, pre_j.l2_norm()*1e-6);
-                LA::SolverGMRES step_5_gmres_1 (step_5_gmres_control_1);
+                SolverGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
                 try {
                     dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType,VectorType,PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
                                                                                pre_j;
@@ -579,7 +577,7 @@ namespace SAND {
                 }
 
                 SolverControl step_5_gmres_control_2 (10000, pre_k.l2_norm()*1e-6);
-                LA::SolverGMRES step_5_gmres_2 (step_5_gmres_control_2);
+                SolverGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
                 try {
                     dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) *
                                                          pre_k;

From 807b6f3b401d1983f378135f0338553b4a3e6ff5 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Thu, 28 Oct 2021 15:07:41 -0600
Subject: [PATCH 06/95] Working on vmult for trilinos direct solver

---
 include/input_information.h    |   2 +-
 include/schur_preconditioner.h |  20 ++++-
 source/schur_preconditioner.cc | 152 +++++++++++++++++++--------------
 3 files changed, 107 insertions(+), 67 deletions(-)

diff --git a/include/input_information.h b/include/input_information.h
index 9b7be58..51e796e 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -39,7 +39,7 @@ namespace SAND {
         constexpr bool output_parts_of_matrix = false;
 
         //Linear solver options
-        constexpr unsigned int solver_choice = SolverOptions::inexact_K_with_inexact_A_gmres;
+        constexpr unsigned int solver_choice = SolverOptions::inexact_K_with_exact_A_gmres;
         constexpr bool use_eisenstat_walker = false;
         constexpr double default_gmres_tolerance = 1e-6;
 
diff --git a/include/schur_preconditioner.h b/include/schur_preconditioner.h
index aa9fdda..c170b5b 100644
--- a/include/schur_preconditioner.h
+++ b/include/schur_preconditioner.h
@@ -72,6 +72,18 @@ namespace SAND
         using namespace dealii::LinearAlgebraTrilinos;
     }
     using namespace dealii;
+
+    class VmultTrilinosSolverDirect : public TrilinosWrappers::PreconditionBase {
+    public:
+        VmultTrilinosSolverDirect(SolverControl &cn,
+                                  const TrilinosWrappers::SolverDirect::AdditionalData &data);
+        void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
+        void initialize(LA::MPI::SparseMatrix &a_mat) const;
+
+    private:
+        TrilinosWrappers::SolverDirect solver_direct;
+    };
+
     template<int dim>
     class TopOptSchurPreconditioner: public Subscriptor {
     public:
@@ -138,9 +150,11 @@ namespace SAND
         mutable LA::MPI::Vector pre_k;
         mutable LA::MPI::Vector g_d_m_inv_density;
         mutable LA::MPI::Vector k_g_d_m_inv_density;
-//
-//        SolverControl direct_solver_control;
-//        mutable TrilinosWrappers::SolverDirect a_inv_direct;
+
+        std::string solver_type;
+        TrilinosWrappers::SolverDirect::AdditionalData additional_data;
+        SolverControl direct_solver_control;
+        mutable VmultTrilinosSolverDirect a_inv_direct;
 
         mutable TimerOutput timer;
 
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index d6b2182..2de263f 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -27,7 +27,7 @@ namespace SAND {
             n_columns(0),
             n_block_rows(0),
             n_block_columns(0),
-            other_solver_control(100000, 1e-10),
+            other_solver_control(100000, 1e-6),
 //            direct_solver_control(1, 0),
             other_bicgstab(other_solver_control),
             other_gmres(other_solver_control),
@@ -42,8 +42,10 @@ namespace SAND {
             d_2_mat(matrix_in.block(SolutionBlocks::density_upper_slack, SolutionBlocks::density_upper_slack)),
             m_vect(matrix_in.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier)),
             timer(std::cout, TimerOutput::summary,
-                  TimerOutput::wall_times)
-//            a_inv_direct(direct_solver_control)
+                  TimerOutput::wall_times),
+            solver_type("Amesos_Lapack"),
+            additional_data(false, solver_type),
+            a_inv_direct(direct_solver_control, additional_data)
     {
 
     }
@@ -102,7 +104,7 @@ namespace SAND {
         else
         {
             TimerOutput::Scope t(timer, "build A inv");
-//            a_inv_direct.initialize(a_mat);
+            a_inv_direct.initialize(a_mat);
         }
         {
             TimerOutput::Scope t(timer, "reinit diag matrices");
@@ -171,21 +173,23 @@ namespace SAND {
         op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) * transpose_operator<VectorType, VectorType, PayloadType>(linear_operator<VectorType,VectorType,PayloadType>(f_mat));
 
 
-        SolverControl            solver_control(1000, 1e-12);
-        LA::SolverCG a_solver_cg(solver_control);
-        auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg,PreconditionIdentity());
+
 
         if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
         {
+            SolverControl            solver_control(10000, 1e-6);
+            SolverCG<LA::MPI::Vector> a_solver_cg(solver_control);
+            auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg,PreconditionIdentity());
             op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
                    - transpose_operator<VectorType, VectorType, PayloadType>(linear_operator<VectorType, VectorType, PayloadType>(c_mat)) * a_inv_op * linear_operator<VectorType,VectorType,PayloadType>(e_mat)
                    - transpose_operator<VectorType, VectorType, PayloadType>(linear_operator<VectorType, VectorType, PayloadType>(e_mat)) * a_inv_op * linear_operator<VectorType,VectorType,PayloadType>(c_mat);
         }
         else
         {
-//            op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
-//                   - transpose_operator<VectorType, VectorType, PayloadType>(e_mat) * linear_operator<VectorType, VectorType, PayloadType>(a_inv_direct) * transpose_operator<VectorType, VectorType, PayloadType>(c_mat)
-//                   - transpose_operator<VectorType, VectorType, PayloadType>(c_mat) * linear_operator<VectorType, VectorType, PayloadType>(a_inv_direct) * transpose_operator<VectorType, VectorType, PayloadType>(e_mat);
+            auto a_inv_op =linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct);
+            op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
+                   - transpose_operator<VectorType, VectorType, PayloadType>(c_mat) * a_inv_op * linear_operator<VectorType, VectorType, PayloadType>(e_mat)
+                   - transpose_operator<VectorType, VectorType, PayloadType>(e_mat) * a_inv_op * linear_operator<VectorType, VectorType, PayloadType>(c_mat);
         }
 
         if(Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres || Input::solver_choice == SolverOptions::inexact_K_with_exact_A_gmres)
@@ -303,13 +307,13 @@ namespace SAND {
 
         if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
         {
-            SolverControl            solver_control_1(100000, 1e-12);
-            SolverControl            solver_control_2(100000, 1e-12);
+            SolverControl            solver_control_1(100000, 1e-6);
+            SolverControl            solver_control_2(100000, 1e-6);
             SolverCG<LA::MPI::Vector> a_solver_cg_1(solver_control_1);
             SolverCG<LA::MPI::Vector> a_solver_cg_2(solver_control_2);
             auto dst_temp = dst;
-            a_solver_cg_1.solve(a_mat,dst_temp.block(SolutionBlocks::displacement_multiplier),src.block(SolutionBlocks::displacement_multiplier),dealii::TrilinosWrappers::PreconditionIdentity());
-            a_solver_cg_2.solve(a_mat,dst_temp.block(SolutionBlocks::displacement),src.block(SolutionBlocks::displacement),dealii::TrilinosWrappers::PreconditionIdentity());
+            a_solver_cg_1.solve(a_mat,dst_temp.block(SolutionBlocks::displacement_multiplier),src.block(SolutionBlocks::displacement_multiplier),PreconditionIdentity());
+            a_solver_cg_2.solve(a_mat,dst_temp.block(SolutionBlocks::displacement),src.block(SolutionBlocks::displacement),PreconditionIdentity());
             c_mat.Tvmult(dst_temp.block(SolutionBlocks::density_upper_slack),dst_temp.block(SolutionBlocks::displacement_multiplier));
             e_mat.Tvmult(dst_temp.block(SolutionBlocks::density_lower_slack),dst_temp.block(SolutionBlocks::displacement));
 
@@ -319,10 +323,14 @@ namespace SAND {
         }
         else
         {
-//            a_inv_direct.vmult(dst_temp.block(SolutionBlocks::density),src.block(SolutionBlocks::displacement));
-//            e_mat.Tvmult_add(dst.block(SolutionBlocks::density),-1 *dst_temp.block(SolutionBlocks::density));
-//            a_inv_direct.vmult(dst_temp.block(SolutionBlocks::density),src.block(SolutionBlocks::displacement_multiplier));
-//            e_mat.Tvmult_add(dst.block(SolutionBlocks::density),-1 *dst_temp.block(SolutionBlocks::density));
+            auto dst_temp = dst;
+            a_inv_direct.vmult(dst_temp.block(SolutionBlocks::displacement_multiplier),src.block(SolutionBlocks::displacement_multiplier));
+            a_inv_direct.vmult(dst_temp.block(SolutionBlocks::displacement),src.block(SolutionBlocks::displacement));
+
+            c_mat.Tvmult(dst_temp.block(SolutionBlocks::density_upper_slack),dst_temp.block(SolutionBlocks::displacement_multiplier));
+            e_mat.Tvmult(dst_temp.block(SolutionBlocks::density_lower_slack),dst_temp.block(SolutionBlocks::displacement));
+
+            dst.block(SolutionBlocks::density) = dst_temp.block(SolutionBlocks::density) - dst_temp.block(SolutionBlocks::density_upper_slack) - dst_temp.block(SolutionBlocks::density_lower_slack);
         }
 
     }
@@ -344,50 +352,50 @@ namespace SAND {
 
         else if (Input::solver_choice == SolverOptions::inexact_K_with_exact_A_gmres)
         {
-//            auto op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) *
-//                    transpose_operator<VectorType, VectorType, PayloadType>(linear_operator<VectorType,VectorType,PayloadType>(f_mat));
-//
-//            auto op_h = linear_operator(b_mat)
-//                   - transpose_operator<VectorType, VectorType, PayloadType>(linear_operator(c_mat)) * linear_operator(a_inv_direct) * linear_operator(e_mat)
-//                   - transpose_operator<VectorType, VectorType, PayloadType>(linear_operator(e_mat)) * linear_operator(a_inv_direct) * linear_operator(c_mat);
-//
-//            auto op_k_inv = -1 * op_g * linear_operator(d_m_inv_mat) * op_h - linear_operator(d_m_mat);
-//
-//            g_d_m_inv_density = op_g * linear_operator(d_m_inv_mat) * src.block(SolutionBlocks::density);
-//
-//            SolverControl step_4_gmres_control_1 (10000, g_d_m_inv_density.l2_norm()*1e-6);
-//            SolverGMRES<Vector<double>> step_4_gmres_1 (step_4_gmres_control_1);
-//            try {
-//                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
-//                                      g_d_m_inv_density;
-//            } catch (std::exception &exc)
-//            {
-//                std::cerr << "Failure of linear solver step_4_gmres_1" << std::endl;
-//                std::cout << "first residual: " << step_4_gmres_control_1.initial_value() << std::endl;
-//                std::cout << "last residual: " << step_4_gmres_control_1.last_value() << std::endl;
-//                throw;
-//            }
-//
-//            SolverControl step_4_gmres_control_2 (10000, src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6);
-//            SolverGMRES<Vector<double>> step_4_gmres_2 (step_4_gmres_control_2);
-//            try {
-//                k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, PreconditionIdentity()) *
-//                                 src.block(SolutionBlocks::unfiltered_density_multiplier);
-//            } catch (std::exception &exc)
-//            {
-//                std::cerr << "Failure of linear solver step_4_gmres_2" << std::endl;
-//                std::cout << "first residual: " << step_4_gmres_control_2.initial_value() << std::endl;
-//                std::cout << "last residual: " << step_4_gmres_control_2.last_value() << std::endl;
-//                throw;
-//            }
+            auto op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) *
+                    transpose_operator<VectorType, VectorType, PayloadType>(f_mat);
+
+            auto op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
+                   - transpose_operator<VectorType, VectorType, PayloadType>(c_mat) * linear_operator<VectorType, VectorType, PayloadType>(a_inv_direct) * linear_operator<VectorType,VectorType,PayloadType>(e_mat)
+                   - transpose_operator<VectorType, VectorType, PayloadType>(e_mat) * linear_operator<VectorType, VectorType, PayloadType>(a_inv_direct) * linear_operator<VectorType,VectorType,PayloadType>(c_mat);
+
+            auto op_k_inv = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
+
+            g_d_m_inv_density = op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
+
+            SolverControl step_4_gmres_control_1 (10000, g_d_m_inv_density.l2_norm()*1e-6);
+            SolverGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
+            try {
+                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
+                                      g_d_m_inv_density;
+            } catch (std::exception &exc)
+            {
+                std::cerr << "Failure of linear solver step_4_gmres_1" << std::endl;
+                std::cout << "first residual: " << step_4_gmres_control_1.initial_value() << std::endl;
+                std::cout << "last residual: " << step_4_gmres_control_1.last_value() << std::endl;
+                throw;
+            }
+
+            SolverControl step_4_gmres_control_2 (10000, src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6);
+            SolverGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
+            try {
+                k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, PreconditionIdentity()) *
+                                 src.block(SolutionBlocks::unfiltered_density_multiplier);
+            } catch (std::exception &exc)
+            {
+                std::cerr << "Failure of linear solver step_4_gmres_2" << std::endl;
+                std::cout << "first residual: " << step_4_gmres_control_2.initial_value() << std::endl;
+                std::cout << "last residual: " << step_4_gmres_control_2.last_value() << std::endl;
+                throw;
+            }
         }
         else if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
         {
 
-            SolverControl            solver_control(1000, 1e-12);
-            LA::SolverCG a_solver_cg(solver_control);
+            SolverControl            solver_control(10000, 1e-6);
+            SolverCG<LA::MPI::Vector> a_solver_cg(solver_control);
 
-            auto preconditioner = dealii::TrilinosWrappers::PreconditionIdentity();
+            auto preconditioner = PreconditionIdentity();
 
             auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg, preconditioner);
 
@@ -402,7 +410,7 @@ namespace SAND {
             auto op_k_inv = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
 
             SolverControl step_4_gmres_control_1 (10000, g_d_m_inv_density.l2_norm()*1e-6);
-            LA::SolverGMRES step_4_gmres_1 (step_4_gmres_control_1);
+            SolverGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
             try {
                 k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, preconditioner) *
                                       g_d_m_inv_density;
@@ -415,7 +423,7 @@ namespace SAND {
             }
 
             SolverControl step_4_gmres_control_2 (10000, src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6);
-            LA::SolverGMRES step_4_gmres_2 (step_4_gmres_control_2);
+            SolverGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
             try {
                 k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, preconditioner) *
                                  src.block(SolutionBlocks::unfiltered_density_multiplier);
@@ -470,7 +478,7 @@ namespace SAND {
             TimerOutput::Scope t(timer, "inverse 3");
             if(Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
             {
-                SolverControl            solver_control(1000, 1e-12);
+                SolverControl            solver_control(1000, 1e-6);
                 SolverCG<LA::MPI::Vector> a_solver_cg(solver_control);
 
                 auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg, PreconditionIdentity());
@@ -479,8 +487,8 @@ namespace SAND {
                 dst.block(SolutionBlocks::displacement_multiplier) = a_inv_op * src.block(SolutionBlocks::displacement);
             } else
             {
-//                dst.block(SolutionBlocks::displacement) = linear_operator(a_inv_direct) * src.block(SolutionBlocks::displacement_multiplier);
-//                dst.block(SolutionBlocks::displacement_multiplier) = linear_operator(a_inv_direct) * src.block(SolutionBlocks::displacement);
+                dst.block(SolutionBlocks::displacement) = linear_operator<VectorType>(a_inv_direct) * src.block(SolutionBlocks::displacement_multiplier);
+                dst.block(SolutionBlocks::displacement_multiplier) = linear_operator<VectorType>(a_inv_direct) * src.block(SolutionBlocks::displacement);
             }
 
         }
@@ -547,7 +555,7 @@ namespace SAND {
             else if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
             {
 
-                SolverControl            solver_control(1000, 1e-12);
+                SolverControl            solver_control(1000, 1e-6);
                 SolverCG<LA::MPI::Vector> a_solver_cg (solver_control);
                 auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg, PreconditionIdentity());
 
@@ -782,6 +790,24 @@ namespace SAND {
 
 
     }
+
+    void VmultTrilinosSolverDirect::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+    {
+        solver_direct.solve(dst, src);
+    }
+
+    void VmultTrilinosSolverDirect::initialize(LA::MPI::SparseMatrix &a_mat) const
+    {
+        solver_direct.initialize(a_mat);
+    }
+
+    VmultTrilinosSolverDirect::VmultTrilinosSolverDirect(SolverControl &cn,
+                                                         const TrilinosWrappers::SolverDirect::AdditionalData &data)
+    : solver_direct(cn, data)
+    {
+
+    }
+
 }
 template class SAND::TopOptSchurPreconditioner<2>;
 template class SAND::TopOptSchurPreconditioner<3>;
\ No newline at end of file

From 893efb6e10c4f36807be06ecfb3f0c4005652671 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Mon, 1 Nov 2021 11:43:57 -0600
Subject: [PATCH 07/95] Solves in preconditioner failing - unclear what is
 different from dealii vector implementation

---
 CMakeCache.txt                 |  16 ++--
 cmake_install.cmake            |   5 ++
 include/input_information.h    |   8 +-
 include/schur_preconditioner.h |  31 +++++---
 source/kkt_system.cc           |   7 +-
 source/schur_preconditioner.cc | 136 +++++++++++++++++++--------------
 6 files changed, 121 insertions(+), 82 deletions(-)

diff --git a/CMakeCache.txt b/CMakeCache.txt
index df96834..7a7f87a 100644
--- a/CMakeCache.txt
+++ b/CMakeCache.txt
@@ -1,6 +1,6 @@
 # This is the CMakeCache file.
 # For build in directory: /home/justin/Thesis/topo code/SAND
-# It was generated by CMake: /usr/bin/cmake
+# It was generated by CMake: /snap/clion/169/bin/cmake/linux/bin/cmake
 # You can edit this file to change values found and used by cmake.
 # If you do not want to change any of the values, simply exit the editor.
 # If you do want to change a value, simply edit, save, and exit the editor.
@@ -21,7 +21,7 @@ CMAKE_ADDR2LINE:FILEPATH=/usr/bin/addr2line
 CMAKE_AR:FILEPATH=/usr/bin/ar
 
 //Choose the type of build, options are: Debug, Release
-CMAKE_BUILD_TYPE:STRING=Debug
+CMAKE_BUILD_TYPE:STRING=Release
 
 //Enable/Disable color output during build.
 CMAKE_COLOR_MAKEFILE:BOOL=ON
@@ -265,17 +265,17 @@ CMAKE_CACHEFILE_DIR:INTERNAL=/home/justin/Thesis/topo code/SAND
 //Major version of cmake used to create the current loaded cache
 CMAKE_CACHE_MAJOR_VERSION:INTERNAL=3
 //Minor version of cmake used to create the current loaded cache
-CMAKE_CACHE_MINOR_VERSION:INTERNAL=16
+CMAKE_CACHE_MINOR_VERSION:INTERNAL=20
 //Patch version of cmake used to create the current loaded cache
-CMAKE_CACHE_PATCH_VERSION:INTERNAL=3
+CMAKE_CACHE_PATCH_VERSION:INTERNAL=2
 //ADVANCED property for variable: CMAKE_COLOR_MAKEFILE
 CMAKE_COLOR_MAKEFILE-ADVANCED:INTERNAL=1
 //Path to CMake executable.
-CMAKE_COMMAND:INTERNAL=/usr/bin/cmake
+CMAKE_COMMAND:INTERNAL=/snap/clion/169/bin/cmake/linux/bin/cmake
 //Path to cpack program executable.
-CMAKE_CPACK_COMMAND:INTERNAL=/usr/bin/cpack
+CMAKE_CPACK_COMMAND:INTERNAL=/snap/clion/169/bin/cmake/linux/bin/cpack
 //Path to ctest program executable.
-CMAKE_CTEST_COMMAND:INTERNAL=/usr/bin/ctest
+CMAKE_CTEST_COMMAND:INTERNAL=/snap/clion/169/bin/cmake/linux/bin/ctest
 //ADVANCED property for variable: CMAKE_CXX_COMPILER
 CMAKE_CXX_COMPILER-ADVANCED:INTERNAL=1
 //ADVANCED property for variable: CMAKE_CXX_COMPILER_AR
@@ -382,7 +382,7 @@ CMAKE_RANLIB-ADVANCED:INTERNAL=1
 //ADVANCED property for variable: CMAKE_READELF
 CMAKE_READELF-ADVANCED:INTERNAL=1
 //Path to CMake installation.
-CMAKE_ROOT:INTERNAL=/usr/share/cmake-3.16
+CMAKE_ROOT:INTERNAL=/snap/clion/169/bin/cmake/linux/share/cmake-3.20
 //ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS
 CMAKE_SHARED_LINKER_FLAGS-ADVANCED:INTERNAL=1
 //ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_DEBUG
diff --git a/cmake_install.cmake b/cmake_install.cmake
index 417d8e3..554a30e 100644
--- a/cmake_install.cmake
+++ b/cmake_install.cmake
@@ -37,6 +37,11 @@ if(NOT DEFINED CMAKE_CROSSCOMPILING)
   set(CMAKE_CROSSCOMPILING "FALSE")
 endif()
 
+# Set default install directory permissions.
+if(NOT DEFINED CMAKE_OBJDUMP)
+  set(CMAKE_OBJDUMP "/usr/bin/objdump")
+endif()
+
 if(CMAKE_INSTALL_COMPONENT)
   set(CMAKE_INSTALL_MANIFEST "install_manifest_${CMAKE_INSTALL_COMPONENT}.txt")
 else()
diff --git a/include/input_information.h b/include/input_information.h
index 51e796e..09edaf0 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -17,12 +17,12 @@ namespace SAND {
         //geometry options
         constexpr unsigned int geometry_base = GeometryOptions::mbb;
         constexpr unsigned int dim = 2;
-        constexpr unsigned int refinements =  3;
+        constexpr unsigned int refinements =  5;
 
         //nonlinear algorithm options
         constexpr double initial_barrier_size = 25;
-        constexpr double min_barrier_size = .00000;
-        constexpr double fraction_to_boundary = .9;
+        constexpr double min_barrier_size = .000001;
+        constexpr double fraction_to_boundary = .8;
         constexpr unsigned int max_steps=75;
         constexpr unsigned int barrier_reduction=BarrierOptions::loqo;
         constexpr double required_norm = .0001;
@@ -31,7 +31,7 @@ namespace SAND {
         constexpr double filter_r = .251;
 
         //other options
-        constexpr double density_penalty_exponent = 3;
+        constexpr double density_penalty_exponent = 5;
 
         //output options
         constexpr bool output_full_preconditioned_matrix = false;
diff --git a/include/schur_preconditioner.h b/include/schur_preconditioner.h
index c170b5b..2da1542 100644
--- a/include/schur_preconditioner.h
+++ b/include/schur_preconditioner.h
@@ -73,15 +73,26 @@ namespace SAND
     }
     using namespace dealii;
 
-    class VmultTrilinosSolverDirect : public TrilinosWrappers::PreconditionBase {
-    public:
-        VmultTrilinosSolverDirect(SolverControl &cn,
-                                  const TrilinosWrappers::SolverDirect::AdditionalData &data);
-        void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
-        void initialize(LA::MPI::SparseMatrix &a_mat) const;
-
-    private:
-        TrilinosWrappers::SolverDirect solver_direct;
+    class VmultTrilinosSolverDirect : public TrilinosWrappers::SparseMatrix {
+        public:
+            VmultTrilinosSolverDirect(SolverControl &cn,
+                                      const TrilinosWrappers::SolverDirect::AdditionalData &data,
+                                      LA::MPI::SparseMatrix &a_mat
+                                      );
+            void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
+            void vmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const;
+            void Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
+            void Tvmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const;
+            void initialize(LA::MPI::SparseMatrix &a_mat);
+            unsigned int m() const;
+            unsigned int n() const;
+            int get_size()
+            {
+                return size;
+            }
+        private:
+            mutable TrilinosWrappers::SolverDirect solver_direct;
+            int size;
     };
 
     template<int dim>
@@ -155,7 +166,7 @@ namespace SAND
         TrilinosWrappers::SolverDirect::AdditionalData additional_data;
         SolverControl direct_solver_control;
         mutable VmultTrilinosSolverDirect a_inv_direct;
-
+//        TrilinosWrappers::SolverDirect a_inv_direct;
         mutable TimerOutput timer;
 
     };
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index a36d036..f7b83b9 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -1626,9 +1626,10 @@ namespace SAND {
         switch (Input::solver_choice) {
             case SolverOptions::direct_solve: {
 //                SolverControl cn;
-//                PETScWrappers::SparseDirectMUMPS solver(cn, mpi_communicator);
-//                solver.set_symmetric_mode(true);
-//                solver.solve(system_matrix, linear_solution, system_rhs);
+//                std::string solver_type;
+//                TrilinosWrappers::SolverDirect::AdditionalData additional_data(false, "Amesos_Klu");
+//                TrilinosWrappers::SolverDirect solver(cn, mpi_communicator, system_matrix);
+//                solver.solve(linear_solution, system_rhs);
                 break;
             }
             case SolverOptions::exact_preconditioner_with_gmres: {
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 2de263f..8dcaa84 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -28,7 +28,7 @@ namespace SAND {
             n_block_rows(0),
             n_block_columns(0),
             other_solver_control(100000, 1e-6),
-//            direct_solver_control(1, 0),
+            direct_solver_control(1, 0),
             other_bicgstab(other_solver_control),
             other_gmres(other_solver_control),
             other_cg(other_solver_control),
@@ -43,9 +43,9 @@ namespace SAND {
             m_vect(matrix_in.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier)),
             timer(std::cout, TimerOutput::summary,
                   TimerOutput::wall_times),
-            solver_type("Amesos_Lapack"),
+            solver_type("Amesos_Klu"),
             additional_data(false, solver_type),
-            a_inv_direct(direct_solver_control, additional_data)
+            a_inv_direct(direct_solver_control, additional_data, a_mat)
     {
 
     }
@@ -160,6 +160,8 @@ namespace SAND {
             }
         }
 
+
+
         pre_j=state.block(SolutionBlocks::density);
         pre_k=state.block(SolutionBlocks::density);
         g_d_m_inv_density=state.block(SolutionBlocks::density);
@@ -172,12 +174,12 @@ namespace SAND {
         op_d_8 = linear_operator<VectorType,VectorType,PayloadType>(d_8_mat);
         op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) * transpose_operator<VectorType, VectorType, PayloadType>(linear_operator<VectorType,VectorType,PayloadType>(f_mat));
 
-
+        std::cout << "here" << std::endl;
 
 
         if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
         {
-            SolverControl            solver_control(10000, 1e-6);
+            SolverControl            solver_control(100000, 1e-6);
             SolverCG<LA::MPI::Vector> a_solver_cg(solver_control);
             auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg,PreconditionIdentity());
             op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
@@ -192,6 +194,8 @@ namespace SAND {
                    - transpose_operator<VectorType, VectorType, PayloadType>(e_mat) * a_inv_op * linear_operator<VectorType, VectorType, PayloadType>(c_mat);
         }
 
+        std::cout << "here" << std::endl;
+
         if(Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres || Input::solver_choice == SolverOptions::inexact_K_with_exact_A_gmres)
         {
 
@@ -235,6 +239,7 @@ namespace SAND {
     template<int dim>
     void TopOptSchurPreconditioner<dim>::vmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
         LA::MPI::BlockVector temp_src;
+        std::cout << "starting vmult" << std::endl;
         {
             TimerOutput::Scope t(timer, "part 1");
             vmult_step_1(dst, src);
@@ -363,7 +368,7 @@ namespace SAND {
 
             g_d_m_inv_density = op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
 
-            SolverControl step_4_gmres_control_1 (10000, g_d_m_inv_density.l2_norm()*1e-6);
+            SolverControl step_4_gmres_control_1 (100000, std::max(g_d_m_inv_density.l2_norm()*1e-6,1e-6));
             SolverGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
             try {
                 k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
@@ -376,7 +381,7 @@ namespace SAND {
                 throw;
             }
 
-            SolverControl step_4_gmres_control_2 (10000, src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6);
+            SolverControl step_4_gmres_control_2 (100000, std::max(src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6,1e-6));
             SolverGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
             try {
                 k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, PreconditionIdentity()) *
@@ -392,7 +397,7 @@ namespace SAND {
         else if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
         {
 
-            SolverControl            solver_control(10000, 1e-6);
+            SolverControl            solver_control(100000, 1e-6);
             SolverCG<LA::MPI::Vector> a_solver_cg(solver_control);
 
             auto preconditioner = PreconditionIdentity();
@@ -409,7 +414,7 @@ namespace SAND {
 
             auto op_k_inv = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
 
-            SolverControl step_4_gmres_control_1 (10000, g_d_m_inv_density.l2_norm()*1e-6);
+            SolverControl step_4_gmres_control_1 (10000, std::max(g_d_m_inv_density.l2_norm()*1e-9,1e-9));
             SolverGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
             try {
                 k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, preconditioner) *
@@ -422,7 +427,7 @@ namespace SAND {
                 throw;
             }
 
-            SolverControl step_4_gmres_control_2 (10000, src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6);
+            SolverControl step_4_gmres_control_2 (10000, std::max(src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-9,1e-9));
             SolverGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
             try {
                 k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, preconditioner) *
@@ -478,7 +483,7 @@ namespace SAND {
             TimerOutput::Scope t(timer, "inverse 3");
             if(Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
             {
-                SolverControl            solver_control(1000, 1e-6);
+                SolverControl            solver_control(100000, 1e-6);
                 SolverCG<LA::MPI::Vector> a_solver_cg(solver_control);
 
                 auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg, PreconditionIdentity());
@@ -510,52 +515,48 @@ namespace SAND {
 
             else if (Input::solver_choice == SolverOptions::inexact_K_with_exact_A_gmres)
             {
+                auto op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) *
+                            transpose_operator<VectorType, VectorType, PayloadType>(f_mat);
 
-//                a_inv_direct.initialize(a_mat);
-//
-//                auto op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) *
-//                            transpose_operator<VectorType, VectorType, PayloadType>(f_mat);
-//
-//                auto op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
-//                            - transpose_operator<VectorType, VectorType, PayloadType>(c_mat) * inverse_operator(
-//                                    linear_operator<VectorType,VectorType,PayloadType>(a_mat), a_inv_direct, TrilinosWrappers::PreconditionIdentity()) * linear_operator<VectorType,VectorType,PayloadType>(e_mat)
-//                            - transpose_operator<VectorType, VectorType, PayloadType>(e_mat) * linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct) * linear_operator<VectorType,VectorType,PayloadType>(c_mat);
-//
-//                auto op_k_inv = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
-//
-//                pre_j = src.block(SolutionBlocks::density) + op_h * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
-//                pre_k = -1* op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density) + src.block(SolutionBlocks::unfiltered_density_multiplier);
-//
-//                SolverControl step_5_gmres_control_1 (10000, pre_j.l2_norm()*1e-6);
-//                SolverGMRES<Vector<double>> step_5_gmres_1 (step_5_gmres_control_1);
-//                try {
-//                    dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
-//                                          pre_j;
-//                } catch (std::exception &exc)
-//                {
-//                    std::cerr << "Failure of linear solver step_5_gmres_1" << std::endl;
-//                    std::cout << "first residual: " << step_5_gmres_control_1.initial_value() << std::endl;
-//                    std::cout << "last residual: " << step_5_gmres_control_1.last_value() << std::endl;
-//                    throw;
-//                }
-//
-//                SolverControl step_5_gmres_control_2 (10000, pre_k.l2_norm()*1e-6);
-//                SolverGMRES<Vector<double>> step_5_gmres_2 (step_5_gmres_control_2);
-//                try {
-//                    dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) *
-//                                                                               pre_k;
-//                } catch (std::exception &exc)
-//                {
-//                    std::cerr << "Failure of linear solver step_5_gmres_2" << std::endl;
-//                    std::cout << "first residual: " << step_5_gmres_control_2.initial_value() << std::endl;
-//                    std::cout << "last residual: " << step_5_gmres_control_2.last_value() << std::endl;
-//                    throw;
-//                }
+                auto op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
+                            - transpose_operator<VectorType, VectorType, PayloadType>(c_mat) *
+                                    linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct) * linear_operator<VectorType,VectorType,PayloadType>(e_mat)
+                            - transpose_operator<VectorType, VectorType, PayloadType>(e_mat) * linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct) * linear_operator<VectorType,VectorType,PayloadType>(c_mat);
+
+                auto op_k_inv = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
+
+                pre_j = src.block(SolutionBlocks::density) + op_h * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
+                pre_k = -1* op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density) + src.block(SolutionBlocks::unfiltered_density_multiplier);
+
+                SolverControl step_5_gmres_control_1 (100000, std::max(pre_j.l2_norm()*1e-6,1e-6));
+                SolverGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
+                try {
+                    dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
+                                          pre_j;
+                } catch (std::exception &exc)
+                {
+                    std::cerr << "Failure of linear solver step_5_gmres_1" << std::endl;
+                    std::cout << "first residual: " << step_5_gmres_control_1.initial_value() << std::endl;
+                    std::cout << "last residual: " << step_5_gmres_control_1.last_value() << std::endl;
+                    throw;
+                }
+
+                SolverControl step_5_gmres_control_2 (100000, std::max(pre_k.l2_norm()*1e-6,1e-6));
+                SolverGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
+                try {
+                    dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) * pre_k;
+                } catch (std::exception &exc)
+                {
+                    std::cerr << "Failure of linear solver step_5_gmres_2" << std::endl;
+                    std::cout << "first residual: " << step_5_gmres_control_2.initial_value() << std::endl;
+                    std::cout << "last residual: " << step_5_gmres_control_2.last_value() << std::endl;
+                    throw;
+                }
             }
             else if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
             {
 
-                SolverControl            solver_control(1000, 1e-6);
+                SolverControl            solver_control(100000, 1e-6);
                 SolverCG<LA::MPI::Vector> a_solver_cg (solver_control);
                 auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg, PreconditionIdentity());
 
@@ -571,7 +572,7 @@ namespace SAND {
                 pre_j = src.block(SolutionBlocks::density) + op_h * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
                 pre_k = -1* op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density) + src.block(SolutionBlocks::unfiltered_density_multiplier);
 
-                SolverControl step_5_gmres_control_1 (10000, pre_j.l2_norm()*1e-6);
+                SolverControl step_5_gmres_control_1 (10000, std::max(pre_j.l2_norm()*1e-6,1e-9));
                 SolverGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
                 try {
                     dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType,VectorType,PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
@@ -584,7 +585,7 @@ namespace SAND {
                     throw;
                 }
 
-                SolverControl step_5_gmres_control_2 (10000, pre_k.l2_norm()*1e-6);
+                SolverControl step_5_gmres_control_2 (10000, std::max(pre_k.l2_norm()*1e-6,1e-9));
                 SolverGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
                 try {
                     dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) *
@@ -791,18 +792,39 @@ namespace SAND {
 
     }
 
+    void VmultTrilinosSolverDirect::initialize(LA::MPI::SparseMatrix &a_mat)
+    {
+        reinit(a_mat);
+        solver_direct.initialize(a_mat);
+        size = a_mat.n();
+    }
+
+    void
+    VmultTrilinosSolverDirect::vmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
+    {
+        solver_direct.solve(dst, src);
+    }
+
     void VmultTrilinosSolverDirect::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
     {
         solver_direct.solve(dst, src);
     }
 
-    void VmultTrilinosSolverDirect::initialize(LA::MPI::SparseMatrix &a_mat) const
+    void
+    VmultTrilinosSolverDirect::Tvmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
     {
-        solver_direct.initialize(a_mat);
+        solver_direct.solve(dst, src);
+    }
+
+    void VmultTrilinosSolverDirect::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+    {
+        solver_direct.solve(dst, src);
     }
 
+
     VmultTrilinosSolverDirect::VmultTrilinosSolverDirect(SolverControl &cn,
-                                                         const TrilinosWrappers::SolverDirect::AdditionalData &data)
+                     const TrilinosWrappers::SolverDirect::AdditionalData &data,
+                     LA::MPI::SparseMatrix &a_mat)
     : solver_direct(cn, data)
     {
 

From 3dfad854f9f21bc96dc250f174c26ac91d807617 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Tue, 2 Nov 2021 09:52:19 -0600
Subject: [PATCH 08/95] Solves in preconditioner failing - unclear what is
 different from dealii vector implementation

---
 source/kkt_system.cc           |  7 ++++++-
 source/schur_preconditioner.cc | 35 ++++++++++++++++++++++++----------
 2 files changed, 31 insertions(+), 11 deletions(-)

diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index f7b83b9..a34de1c 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -673,6 +673,7 @@ namespace SAND {
             coupling[SolutionComponents::density_upper_slack_multiplier<dim>][SolutionComponents::density_upper_slack_multiplier<dim>] = DoFTools::always;
             constraints.reinit(locally_relevant_dofs);
             constraints.clear();
+            DoFTools::make_hanging_node_constraints(dof_handler,constraints);
             constraints.close();
 
             system_matrix.clear();
@@ -714,6 +715,10 @@ namespace SAND {
 
             linear_solution.reinit(owned_partitioning, relevant_partitioning, mpi_communicator);
             system_rhs.reinit(owned_partitioning, mpi_communicator);
+
+            linear_solution.collect_sizes();
+            system_rhs.collect_sizes();
+            system_matrix.collect_sizes();
     }
 
     ///This  is  where  the  magic  happens.   The  equations  describing  the newtons method for finding 0s in the KKT conditions are implemented here.
@@ -772,7 +777,7 @@ namespace SAND {
 
         const Functions::ConstantFunction<dim> lambda(1.), mu(1.);
 
-        LA::MPI::BlockVector filtered_unfiltered_density_solution(state);
+        LA::MPI::BlockVector filtered_unfiltered_density_solution = state;
         LA::MPI::BlockVector filter_adjoint_unfiltered_density_multiplier_solution = state;
         filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = 0;
         filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier) = 0;
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 8dcaa84..18c4355 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -7,6 +7,7 @@
 #include <deal.II/base/timer.h>
 #include <deal.II/lac/generic_linear_algebra.h>
 #include <deal.II/lac/trilinos_solver.h>
+#include <deal.II/lac/trilinos_precondition.h>
 #include "../include/schur_preconditioner.h"
 #include "../include/input_information.h"
 #include "../include/sand_tools.h"
@@ -366,11 +367,17 @@ namespace SAND {
 
             auto op_k_inv = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
 
+            TrilinosWrappers::PreconditionIdentity preconditioner;
+            preconditioner.initialize(b_mat);
+
             g_d_m_inv_density = op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
 
             SolverControl step_4_gmres_control_1 (100000, std::max(g_d_m_inv_density.l2_norm()*1e-6,1e-6));
             SolverGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
+//            LA::SolverGMRES step_4_gmres_1_Trilinos (step_4_gmres_control_1);
             try {
+
+//                step_4_gmres_1_Trilinos.solve(op_k_inv, k_g_d_m_inv_density,g_d_m_inv_density,preconditioner);
                 k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
                                       g_d_m_inv_density;
             } catch (std::exception &exc)
@@ -383,7 +390,9 @@ namespace SAND {
 
             SolverControl step_4_gmres_control_2 (100000, std::max(src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6,1e-6));
             SolverGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
+            LA::SolverGMRES step_4_gmres_2_Trilinos (step_4_gmres_control_2);
             try {
+//                step_4_gmres_2_Trilinos.solve(op_k_inv, k_density_mult,src.block(SolutionBlocks::unfiltered_density_multiplier),preconditioner);
                 k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, PreconditionIdentity()) *
                                  src.block(SolutionBlocks::unfiltered_density_multiplier);
             } catch (std::exception &exc)
@@ -414,10 +423,10 @@ namespace SAND {
 
             auto op_k_inv = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
 
-            SolverControl step_4_gmres_control_1 (10000, std::max(g_d_m_inv_density.l2_norm()*1e-9,1e-9));
+            SolverControl step_4_gmres_control_1 (10000, std::max(g_d_m_inv_density.l2_norm()*1e-6,1e-6));
             SolverGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
             try {
-                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, preconditioner) *
+                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
                                       g_d_m_inv_density;
             } catch (std::exception &exc)
             {
@@ -427,10 +436,11 @@ namespace SAND {
                 throw;
             }
 
-            SolverControl step_4_gmres_control_2 (10000, std::max(src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-9,1e-9));
+            SolverControl step_4_gmres_control_2 (10000, std::max(src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6,1e-6));
             SolverGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
             try {
-                k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, preconditioner) *
+
+                k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, PreconditionIdentity()) *
                                  src.block(SolutionBlocks::unfiltered_density_multiplier);
             } catch (std::exception &exc)
             {
@@ -446,10 +456,8 @@ namespace SAND {
             throw;
         }
 
-        auto holder_1 = transpose_operator<VectorType, VectorType, PayloadType>(m_vect)*k_g_d_m_inv_density;
-        auto holder_2 = transpose_operator<VectorType, VectorType, PayloadType>(m_vect)*k_density_mult;
-
-        dst.block(SolutionBlocks::total_volume_multiplier) = holder_1 - holder_2
+        dst.block(SolutionBlocks::total_volume_multiplier) = transpose_operator<VectorType, VectorType, PayloadType>(m_vect)*k_g_d_m_inv_density
+                                                        - transpose_operator<VectorType, VectorType, PayloadType>(m_vect)*k_density_mult
                                                         +dst_temp.block(SolutionBlocks::total_volume_multiplier);
     }
 
@@ -528,9 +536,14 @@ namespace SAND {
                 pre_j = src.block(SolutionBlocks::density) + op_h * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
                 pre_k = -1* op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density) + src.block(SolutionBlocks::unfiltered_density_multiplier);
 
+                TrilinosWrappers::PreconditionIdentity preconditioner;
+                preconditioner.initialize(b_mat);
+
                 SolverControl step_5_gmres_control_1 (100000, std::max(pre_j.l2_norm()*1e-6,1e-6));
                 SolverGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
+//                LA::SolverGMRES step_5_gmres_1_Trilinos (step_5_gmres_control_1);
                 try {
+//                    step_5_gmres_1_Trilinos.solve(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv),dst.block(SolutionBlocks::unfiltered_density_multiplier), pre_j, preconditioner);
                     dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
                                           pre_j;
                 } catch (std::exception &exc)
@@ -543,7 +556,9 @@ namespace SAND {
 
                 SolverControl step_5_gmres_control_2 (100000, std::max(pre_k.l2_norm()*1e-6,1e-6));
                 SolverGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
+//                LA::SolverGMRES step_5_gmres_2_Trilinos (step_5_gmres_control_2);
                 try {
+//                    step_5_gmres_2_Trilinos.solve(op_k_inv,dst.block(SolutionBlocks::density), pre_k, preconditioner);
                     dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) * pre_k;
                 } catch (std::exception &exc)
                 {
@@ -572,7 +587,7 @@ namespace SAND {
                 pre_j = src.block(SolutionBlocks::density) + op_h * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
                 pre_k = -1* op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density) + src.block(SolutionBlocks::unfiltered_density_multiplier);
 
-                SolverControl step_5_gmres_control_1 (10000, std::max(pre_j.l2_norm()*1e-6,1e-9));
+                SolverControl step_5_gmres_control_1 (10000, std::max(pre_j.l2_norm()*1e-6,1e-6));
                 SolverGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
                 try {
                     dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType,VectorType,PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
@@ -585,7 +600,7 @@ namespace SAND {
                     throw;
                 }
 
-                SolverControl step_5_gmres_control_2 (10000, std::max(pre_k.l2_norm()*1e-6,1e-9));
+                SolverControl step_5_gmres_control_2 (10000, std::max(pre_k.l2_norm()*1e-6,1e-6));
                 SolverGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
                 try {
                     dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) *

From b3abbb9983722b1efe7e14d57a66164a69a94096 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Tue, 2 Nov 2021 10:05:03 -0600
Subject: [PATCH 09/95] added check in preconditioner step 4

---
 source/schur_preconditioner.cc | 1 +
 1 file changed, 1 insertion(+)

diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 18c4355..3110d51 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -372,6 +372,7 @@ namespace SAND {
 
             g_d_m_inv_density = op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
 
+            std::cout << g_d_m_inv_density.l2_norm() << " 4-1" << std::endl;
             SolverControl step_4_gmres_control_1 (100000, std::max(g_d_m_inv_density.l2_norm()*1e-6,1e-6));
             SolverGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
 //            LA::SolverGMRES step_4_gmres_1_Trilinos (step_4_gmres_control_1);

From b80192a075ac1c347b779b9a68c92db8644e8248 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Tue, 2 Nov 2021 10:30:35 -0600
Subject: [PATCH 10/95] added check in preconditioner step 4

---
 source/schur_preconditioner.cc | 8 ++------
 1 file changed, 2 insertions(+), 6 deletions(-)

diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 3110d51..9135207 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -175,8 +175,6 @@ namespace SAND {
         op_d_8 = linear_operator<VectorType,VectorType,PayloadType>(d_8_mat);
         op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) * transpose_operator<VectorType, VectorType, PayloadType>(linear_operator<VectorType,VectorType,PayloadType>(f_mat));
 
-        std::cout << "here" << std::endl;
-
 
         if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
         {
@@ -195,7 +193,6 @@ namespace SAND {
                    - transpose_operator<VectorType, VectorType, PayloadType>(e_mat) * a_inv_op * linear_operator<VectorType, VectorType, PayloadType>(c_mat);
         }
 
-        std::cout << "here" << std::endl;
 
         if(Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres || Input::solver_choice == SolverOptions::inexact_K_with_exact_A_gmres)
         {
@@ -240,7 +237,6 @@ namespace SAND {
     template<int dim>
     void TopOptSchurPreconditioner<dim>::vmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
         LA::MPI::BlockVector temp_src;
-        std::cout << "starting vmult" << std::endl;
         {
             TimerOutput::Scope t(timer, "part 1");
             vmult_step_1(dst, src);
@@ -371,8 +367,6 @@ namespace SAND {
             preconditioner.initialize(b_mat);
 
             g_d_m_inv_density = op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
-
-            std::cout << g_d_m_inv_density.l2_norm() << " 4-1" << std::endl;
             SolverControl step_4_gmres_control_1 (100000, std::max(g_d_m_inv_density.l2_norm()*1e-6,1e-6));
             SolverGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
 //            LA::SolverGMRES step_4_gmres_1_Trilinos (step_4_gmres_control_1);
@@ -388,6 +382,8 @@ namespace SAND {
                 std::cout << "last residual: " << step_4_gmres_control_1.last_value() << std::endl;
                 throw;
             }
+            std::cout << "first residual 4-1: " << step_4_gmres_control_1.initial_value() << std::endl;
+            std::cout << "last residual 4-1: " << step_4_gmres_control_1.last_value() << std::endl;
 
             SolverControl step_4_gmres_control_2 (100000, std::max(src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6,1e-6));
             SolverGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);

From bb7a630546f8215f6b4643c0cc09360ac54bc1f8 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Tue, 2 Nov 2021 10:36:23 -0600
Subject: [PATCH 11/95] added many checks

---
 source/schur_preconditioner.cc | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 9135207..4e04849 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -399,6 +399,8 @@ namespace SAND {
                 std::cout << "last residual: " << step_4_gmres_control_2.last_value() << std::endl;
                 throw;
             }
+            std::cout << "first residual 4-2: " << step_4_gmres_control_2.initial_value() << std::endl;
+            std::cout << "last residual 4-2: " << step_4_gmres_control_2.last_value() << std::endl;
         }
         else if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
         {
@@ -550,6 +552,8 @@ namespace SAND {
                     std::cout << "last residual: " << step_5_gmres_control_1.last_value() << std::endl;
                     throw;
                 }
+                std::cout << "first residual 5-1: " << step_5_gmres_control_1.initial_value() << std::endl;
+                std::cout << "last residual 5-1: " << step_5_gmres_control_1.last_value() << std::endl;
 
                 SolverControl step_5_gmres_control_2 (100000, std::max(pre_k.l2_norm()*1e-6,1e-6));
                 SolverGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
@@ -564,6 +568,8 @@ namespace SAND {
                     std::cout << "last residual: " << step_5_gmres_control_2.last_value() << std::endl;
                     throw;
                 }
+                std::cout << "first residual 5-2: " << step_5_gmres_control_2.initial_value() << std::endl;
+                std::cout << "last residual 5-2: " << step_5_gmres_control_2.last_value() << std::endl;
             }
             else if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
             {

From 3b4eef205a65fcaa9d25d48d3f1ef6967d1d8540 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Tue, 2 Nov 2021 10:42:46 -0600
Subject: [PATCH 12/95] added many checks

---
 source/schur_preconditioner.cc | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 4e04849..84cfbf0 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -367,7 +367,7 @@ namespace SAND {
             preconditioner.initialize(b_mat);
 
             g_d_m_inv_density = op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
-            SolverControl step_4_gmres_control_1 (100000, std::max(g_d_m_inv_density.l2_norm()*1e-6,1e-6));
+            SolverControl step_4_gmres_control_1 (100000, g_d_m_inv_density.l2_norm()*1e-6);
             SolverGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
 //            LA::SolverGMRES step_4_gmres_1_Trilinos (step_4_gmres_control_1);
             try {
@@ -385,7 +385,7 @@ namespace SAND {
             std::cout << "first residual 4-1: " << step_4_gmres_control_1.initial_value() << std::endl;
             std::cout << "last residual 4-1: " << step_4_gmres_control_1.last_value() << std::endl;
 
-            SolverControl step_4_gmres_control_2 (100000, std::max(src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6,1e-6));
+            SolverControl step_4_gmres_control_2 (100000, src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6);
             SolverGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
             LA::SolverGMRES step_4_gmres_2_Trilinos (step_4_gmres_control_2);
             try {
@@ -538,7 +538,7 @@ namespace SAND {
                 TrilinosWrappers::PreconditionIdentity preconditioner;
                 preconditioner.initialize(b_mat);
 
-                SolverControl step_5_gmres_control_1 (100000, std::max(pre_j.l2_norm()*1e-6,1e-6));
+                SolverControl step_5_gmres_control_1 (100000, pre_j.l2_norm()*1e-6);
                 SolverGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
 //                LA::SolverGMRES step_5_gmres_1_Trilinos (step_5_gmres_control_1);
                 try {
@@ -555,7 +555,7 @@ namespace SAND {
                 std::cout << "first residual 5-1: " << step_5_gmres_control_1.initial_value() << std::endl;
                 std::cout << "last residual 5-1: " << step_5_gmres_control_1.last_value() << std::endl;
 
-                SolverControl step_5_gmres_control_2 (100000, std::max(pre_k.l2_norm()*1e-6,1e-6));
+                SolverControl step_5_gmres_control_2 (100000, pre_k.l2_norm()*1e-6);
                 SolverGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
 //                LA::SolverGMRES step_5_gmres_2_Trilinos (step_5_gmres_control_2);
                 try {

From 645b24efd8949fd82ef08c3ae595830808f01107 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Tue, 2 Nov 2021 11:03:50 -0600
Subject: [PATCH 13/95] more specific checks

---
 source/schur_preconditioner.cc | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 84cfbf0..04d9775 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -523,7 +523,7 @@ namespace SAND {
             else if (Input::solver_choice == SolverOptions::inexact_K_with_exact_A_gmres)
             {
                 auto op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) *
-                            transpose_operator<VectorType, VectorType, PayloadType>(f_mat);
+                            transpose_operator(linear_operator<VectorType,VectorType,PayloadType>(f_mat));
 
                 auto op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
                             - transpose_operator<VectorType, VectorType, PayloadType>(c_mat) *
@@ -533,7 +533,10 @@ namespace SAND {
                 auto op_k_inv = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
 
                 pre_j = src.block(SolutionBlocks::density) + op_h * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
-                pre_k = -1* op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density) + src.block(SolutionBlocks::unfiltered_density_multiplier);
+                pre_k = src.block(SolutionBlocks::unfiltered_density_multiplier) - op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
+                std::cout << "5-2 density " << src.block(SolutionBlocks::density).l2_norm() << " unfiltered_density_multiplier " << src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()
+                        << " pre_k norm " << pre_k.l2_norm();
+
 
                 TrilinosWrappers::PreconditionIdentity preconditioner;
                 preconditioner.initialize(b_mat);

From 0d35df69234352fd12506f2c64f2d8671462e0d9 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Tue, 2 Nov 2021 11:07:16 -0600
Subject: [PATCH 14/95] more specific checks

---
 source/schur_preconditioner.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 04d9775..814e3ea 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -535,7 +535,7 @@ namespace SAND {
                 pre_j = src.block(SolutionBlocks::density) + op_h * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
                 pre_k = src.block(SolutionBlocks::unfiltered_density_multiplier) - op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
                 std::cout << "5-2 density " << src.block(SolutionBlocks::density).l2_norm() << " unfiltered_density_multiplier " << src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()
-                        << " pre_k norm " << pre_k.l2_norm();
+                        << " pre_k norm " << pre_k.l2_norm() << std::endl;
 
 
                 TrilinosWrappers::PreconditionIdentity preconditioner;

From 13dc8e00f86a2af18a02fc99918aefbd4bca151b Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Tue, 2 Nov 2021 11:26:21 -0600
Subject: [PATCH 15/95] more specific checks

---
 source/schur_preconditioner.cc | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 814e3ea..046832b 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -533,8 +533,10 @@ namespace SAND {
                 auto op_k_inv = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
 
                 pre_j = src.block(SolutionBlocks::density) + op_h * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
-                pre_k = src.block(SolutionBlocks::unfiltered_density_multiplier) - op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
-                std::cout << "5-2 density " << src.block(SolutionBlocks::density).l2_norm() << " unfiltered_density_multiplier " << src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()
+                auto pre_pre_k = pre_k;
+                pre_pre_k = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
+                pre_k =  pre_pre_k + src.block(SolutionBlocks::unfiltered_density_multiplier);
+                std::cout << "pre_pre_k norm " << pre_pre_k.l2_norm() << " unfiltered_density_multiplier " << src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()
                         << " pre_k norm " << pre_k.l2_norm() << std::endl;
 
 

From 9b61c4b415a4bfead268685814304195ec56bb0a Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Tue, 2 Nov 2021 13:22:26 -0600
Subject: [PATCH 16/95] One fix down...

---
 source/schur_preconditioner.cc | 26 ++++++++++++++------------
 1 file changed, 14 insertions(+), 12 deletions(-)

diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 046832b..c8563a4 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -283,23 +283,19 @@ namespace SAND {
     void TopOptSchurPreconditioner<dim>::vmult_step_1(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
         dst = src;
         auto dst_temp = dst;
-        auto op_d_5 = linear_operator<VectorType,VectorType,PayloadType>(d_5_mat);
-        auto op_d_6 = linear_operator<VectorType,VectorType,PayloadType>(d_6_mat);
-        dst_temp.block(SolutionBlocks::density_upper_slack) = -1 * op_d_5 * src.block(SolutionBlocks::density_lower_slack_multiplier);
-        dst_temp.block(SolutionBlocks::density_lower_slack) = op_d_6 * src.block(SolutionBlocks::density_upper_slack_multiplier);
-        dst.block(SolutionBlocks::unfiltered_density) = dst_temp.block(SolutionBlocks::unfiltered_density) +
-                                                                   dst_temp.block(SolutionBlocks::density_upper_slack) +
-                                                                   dst_temp.block(SolutionBlocks::density_lower_slack) +
-                                                                   src.block(SolutionBlocks::density_lower_slack) -
-                                                                   src.block(SolutionBlocks::density_upper_slack);
+        dst.block(SolutionBlocks::unfiltered_density) = dst_temp.block(SolutionBlocks::unfiltered_density)
+                - linear_operator<VectorType,VectorType,PayloadType>(d_5_mat) * src.block(SolutionBlocks::density_lower_slack_multiplier)
+                + linear_operator<VectorType,VectorType,PayloadType>(d_6_mat) * src.block(SolutionBlocks::density_upper_slack_multiplier)
+                + src.block(SolutionBlocks::density_lower_slack)
+                - src.block(SolutionBlocks::density_upper_slack);
     }
 
     template<int dim>
     void TopOptSchurPreconditioner<dim>::vmult_step_2(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
         dst = src;
         auto dst_temp = dst;
-        dst_temp.block(SolutionBlocks::density) = linear_operator<VectorType,VectorType,PayloadType>(f_mat)*linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) * src.block(SolutionBlocks::unfiltered_density);
-        dst.block(SolutionBlocks::unfiltered_density_multiplier) = dst_temp.block(SolutionBlocks::density) + dst_temp.block(SolutionBlocks::unfiltered_density_multiplier);
+        dst.block(SolutionBlocks::unfiltered_density_multiplier) = dst_temp.block(SolutionBlocks::unfiltered_density_multiplier)
+                - linear_operator<VectorType,VectorType,PayloadType>(f_mat)*linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) * src.block(SolutionBlocks::unfiltered_density);
 
     }
 
@@ -332,7 +328,9 @@ namespace SAND {
             c_mat.Tvmult(dst_temp.block(SolutionBlocks::density_upper_slack),dst_temp.block(SolutionBlocks::displacement_multiplier));
             e_mat.Tvmult(dst_temp.block(SolutionBlocks::density_lower_slack),dst_temp.block(SolutionBlocks::displacement));
 
-            dst.block(SolutionBlocks::density) = dst_temp.block(SolutionBlocks::density) - dst_temp.block(SolutionBlocks::density_upper_slack) - dst_temp.block(SolutionBlocks::density_lower_slack);
+            dst.block(SolutionBlocks::density) = dst_temp.block(SolutionBlocks::density)
+                    - transpose_operator<VectorType,VectorType,PayloadType>(e_mat) * linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct)* src.block(SolutionBlocks::displacement)
+                    - transpose_operator<VectorType,VectorType,PayloadType>(c_mat) * linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct)* src.block(SolutionBlocks::displacement_multiplier);
         }
 
     }
@@ -384,6 +382,7 @@ namespace SAND {
             }
             std::cout << "first residual 4-1: " << step_4_gmres_control_1.initial_value() << std::endl;
             std::cout << "last residual 4-1: " << step_4_gmres_control_1.last_value() << std::endl;
+            std::cout << "last residual 4-1: " << step_4_gmres_control_1.last_step() << std::endl;
 
             SolverControl step_4_gmres_control_2 (100000, src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6);
             SolverGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
@@ -401,6 +400,7 @@ namespace SAND {
             }
             std::cout << "first residual 4-2: " << step_4_gmres_control_2.initial_value() << std::endl;
             std::cout << "last residual 4-2: " << step_4_gmres_control_2.last_value() << std::endl;
+            std::cout << "last residual 4-2: " << step_4_gmres_control_2.last_step() << std::endl;
         }
         else if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
         {
@@ -559,6 +559,7 @@ namespace SAND {
                 }
                 std::cout << "first residual 5-1: " << step_5_gmres_control_1.initial_value() << std::endl;
                 std::cout << "last residual 5-1: " << step_5_gmres_control_1.last_value() << std::endl;
+                std::cout << "last residual 5-1: " << step_5_gmres_control_1.last_step() << std::endl;
 
                 SolverControl step_5_gmres_control_2 (100000, pre_k.l2_norm()*1e-6);
                 SolverGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
@@ -575,6 +576,7 @@ namespace SAND {
                 }
                 std::cout << "first residual 5-2: " << step_5_gmres_control_2.initial_value() << std::endl;
                 std::cout << "last residual 5-2: " << step_5_gmres_control_2.last_value() << std::endl;
+                std::cout << "last residual 5-2: " << step_5_gmres_control_2.last_step() << std::endl;
             }
             else if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
             {

From f942b5a3e9c4a3788e8b05bf1f5453ab7f3151aa Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Wed, 3 Nov 2021 09:23:51 -0600
Subject: [PATCH 17/95] Outputs A Matrix

---
 include/input_information.h    |  6 ++--
 source/kkt_system.cc           |  2 ++
 source/schur_preconditioner.cc | 55 ++++++++++++++++++++++------------
 3 files changed, 41 insertions(+), 22 deletions(-)

diff --git a/include/input_information.h b/include/input_information.h
index 09edaf0..b95de06 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -17,12 +17,12 @@ namespace SAND {
         //geometry options
         constexpr unsigned int geometry_base = GeometryOptions::mbb;
         constexpr unsigned int dim = 2;
-        constexpr unsigned int refinements =  5;
+        constexpr unsigned int refinements =  4;
 
         //nonlinear algorithm options
         constexpr double initial_barrier_size = 25;
-        constexpr double min_barrier_size = .000001;
-        constexpr double fraction_to_boundary = .8;
+        constexpr double min_barrier_size = .00000;
+        constexpr double fraction_to_boundary = .9;
         constexpr unsigned int max_steps=75;
         constexpr unsigned int barrier_reduction=BarrierOptions::loqo;
         constexpr double required_norm = .0001;
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index a34de1c..a129c6c 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -1625,6 +1625,8 @@ namespace SAND {
         else {
             gmres_tolerance = Input::default_gmres_tolerance;
         }
+        linear_solution=0;
+        constraints.distribute(linear_solution);
         SolverControl solver_control(10000, gmres_tolerance * system_rhs.l2_norm());
 
         TopOptSchurPreconditioner<dim> preconditioner(system_matrix);
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index c8563a4..4491e48 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -97,6 +97,22 @@ namespace SAND {
                             dof_index - disp_mult_start_index, dof_index - disp_mult_start_index, 0);
                 }
             }
+
+
+            const unsigned int m = a_mat.m();
+            const unsigned int n = a_mat.n();
+            std::ofstream Xmat("a_mat_par.csv");
+            for (unsigned int i = 0; i < m; i++)
+            {
+                Xmat << a_mat.el(i, 0);
+                for (unsigned int j = 1; j < n; j++)
+                {
+                    Xmat << "," << a_mat.el(i, j);
+                }
+                Xmat << "\n";
+            }
+            Xmat.close();
+
         }
         if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
         {
@@ -322,12 +338,6 @@ namespace SAND {
         else
         {
             auto dst_temp = dst;
-            a_inv_direct.vmult(dst_temp.block(SolutionBlocks::displacement_multiplier),src.block(SolutionBlocks::displacement_multiplier));
-            a_inv_direct.vmult(dst_temp.block(SolutionBlocks::displacement),src.block(SolutionBlocks::displacement));
-
-            c_mat.Tvmult(dst_temp.block(SolutionBlocks::density_upper_slack),dst_temp.block(SolutionBlocks::displacement_multiplier));
-            e_mat.Tvmult(dst_temp.block(SolutionBlocks::density_lower_slack),dst_temp.block(SolutionBlocks::displacement));
-
             dst.block(SolutionBlocks::density) = dst_temp.block(SolutionBlocks::density)
                     - transpose_operator<VectorType,VectorType,PayloadType>(e_mat) * linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct)* src.block(SolutionBlocks::displacement)
                     - transpose_operator<VectorType,VectorType,PayloadType>(c_mat) * linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct)* src.block(SolutionBlocks::displacement_multiplier);
@@ -367,12 +377,14 @@ namespace SAND {
             g_d_m_inv_density = op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
             SolverControl step_4_gmres_control_1 (100000, g_d_m_inv_density.l2_norm()*1e-6);
             SolverGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
-//            LA::SolverGMRES step_4_gmres_1_Trilinos (step_4_gmres_control_1);
+            TrilinosWrappers::SolverGMRES step_4_gmres_1_Trilinos (step_4_gmres_control_1);
             try {
 
 //                step_4_gmres_1_Trilinos.solve(op_k_inv, k_g_d_m_inv_density,g_d_m_inv_density,preconditioner);
-                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
+                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1_Trilinos, PreconditionIdentity()) *
                                       g_d_m_inv_density;
+//                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
+//                                      g_d_m_inv_density;
             } catch (std::exception &exc)
             {
                 std::cerr << "Failure of linear solver step_4_gmres_1" << std::endl;
@@ -382,15 +394,17 @@ namespace SAND {
             }
             std::cout << "first residual 4-1: " << step_4_gmres_control_1.initial_value() << std::endl;
             std::cout << "last residual 4-1: " << step_4_gmres_control_1.last_value() << std::endl;
-            std::cout << "last residual 4-1: " << step_4_gmres_control_1.last_step() << std::endl;
+            std::cout << "Iterations 4-1: " << step_4_gmres_control_1.last_step() << std::endl;
 
             SolverControl step_4_gmres_control_2 (100000, src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6);
             SolverGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
             LA::SolverGMRES step_4_gmres_2_Trilinos (step_4_gmres_control_2);
             try {
 //                step_4_gmres_2_Trilinos.solve(op_k_inv, k_density_mult,src.block(SolutionBlocks::unfiltered_density_multiplier),preconditioner);
-                k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, PreconditionIdentity()) *
+                k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2_Trilinos, PreconditionIdentity()) *
                                  src.block(SolutionBlocks::unfiltered_density_multiplier);
+//                k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, PreconditionIdentity()) *
+//                                 src.block(SolutionBlocks::unfiltered_density_multiplier);
             } catch (std::exception &exc)
             {
                 std::cerr << "Failure of linear solver step_4_gmres_2" << std::endl;
@@ -400,7 +414,7 @@ namespace SAND {
             }
             std::cout << "first residual 4-2: " << step_4_gmres_control_2.initial_value() << std::endl;
             std::cout << "last residual 4-2: " << step_4_gmres_control_2.last_value() << std::endl;
-            std::cout << "last residual 4-2: " << step_4_gmres_control_2.last_step() << std::endl;
+            std::cout << "Iterations 4-2: " << step_4_gmres_control_2.last_step() << std::endl;
         }
         else if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
         {
@@ -499,8 +513,8 @@ namespace SAND {
                 dst.block(SolutionBlocks::displacement_multiplier) = a_inv_op * src.block(SolutionBlocks::displacement);
             } else
             {
-                dst.block(SolutionBlocks::displacement) = linear_operator<VectorType>(a_inv_direct) * src.block(SolutionBlocks::displacement_multiplier);
-                dst.block(SolutionBlocks::displacement_multiplier) = linear_operator<VectorType>(a_inv_direct) * src.block(SolutionBlocks::displacement);
+                dst.block(SolutionBlocks::displacement) = linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct) * src.block(SolutionBlocks::displacement_multiplier);
+                dst.block(SolutionBlocks::displacement_multiplier) = linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct) * src.block(SolutionBlocks::displacement);
             }
 
         }
@@ -545,11 +559,13 @@ namespace SAND {
 
                 SolverControl step_5_gmres_control_1 (100000, pre_j.l2_norm()*1e-6);
                 SolverGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
-//                LA::SolverGMRES step_5_gmres_1_Trilinos (step_5_gmres_control_1);
+                LA::SolverGMRES step_5_gmres_1_Trilinos (step_5_gmres_control_1);
                 try {
 //                    step_5_gmres_1_Trilinos.solve(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv),dst.block(SolutionBlocks::unfiltered_density_multiplier), pre_j, preconditioner);
-                    dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
+                    dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), step_5_gmres_1_Trilinos, PreconditionIdentity()) *
                                           pre_j;
+//                    dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
+//                                                                               pre_j;
                 } catch (std::exception &exc)
                 {
                     std::cerr << "Failure of linear solver step_5_gmres_1" << std::endl;
@@ -559,14 +575,15 @@ namespace SAND {
                 }
                 std::cout << "first residual 5-1: " << step_5_gmres_control_1.initial_value() << std::endl;
                 std::cout << "last residual 5-1: " << step_5_gmres_control_1.last_value() << std::endl;
-                std::cout << "last residual 5-1: " << step_5_gmres_control_1.last_step() << std::endl;
+                std::cout << "Iterations 5-1: " << step_5_gmres_control_1.last_step() << std::endl;
 
                 SolverControl step_5_gmres_control_2 (100000, pre_k.l2_norm()*1e-6);
                 SolverGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
-//                LA::SolverGMRES step_5_gmres_2_Trilinos (step_5_gmres_control_2);
+                LA::SolverGMRES step_5_gmres_2_Trilinos (step_5_gmres_control_2);
                 try {
 //                    step_5_gmres_2_Trilinos.solve(op_k_inv,dst.block(SolutionBlocks::density), pre_k, preconditioner);
-                    dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) * pre_k;
+                    dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2_Trilinos, PreconditionIdentity()) * pre_k;
+//                    dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) * pre_k;
                 } catch (std::exception &exc)
                 {
                     std::cerr << "Failure of linear solver step_5_gmres_2" << std::endl;
@@ -576,7 +593,7 @@ namespace SAND {
                 }
                 std::cout << "first residual 5-2: " << step_5_gmres_control_2.initial_value() << std::endl;
                 std::cout << "last residual 5-2: " << step_5_gmres_control_2.last_value() << std::endl;
-                std::cout << "last residual 5-2: " << step_5_gmres_control_2.last_step() << std::endl;
+                std::cout << "Iterations 5-2: " << step_5_gmres_control_2.last_step() << std::endl;
             }
             else if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
             {

From 8e5fefcb989f11490dea2f0cbe8ba70923df560c Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Wed, 3 Nov 2021 10:37:40 -0600
Subject: [PATCH 18/95] Outputs cell Matrix

---
 source/kkt_system.cc | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index a129c6c..0203fc8 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -1070,6 +1070,10 @@ namespace SAND {
 
             constraints.distribute_local_to_global(
                     cell_matrix, cell_rhs, local_dof_indices, system_matrix, system_rhs);
+            if (cell->active_cell_index()==10)
+            {
+                cell_matrix.print(std::cout);
+            }
 
 
         }

From 19d60e075609248f0765cb3227aedea602661133 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Wed, 3 Nov 2021 11:40:52 -0600
Subject: [PATCH 19/95] Changed Parameter

---
 include/input_information.h | 2 +-
 source/kkt_system.cc        | 5 +++++
 2 files changed, 6 insertions(+), 1 deletion(-)

diff --git a/include/input_information.h b/include/input_information.h
index b95de06..628ea65 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -31,7 +31,7 @@ namespace SAND {
         constexpr double filter_r = .251;
 
         //other options
-        constexpr double density_penalty_exponent = 5;
+        constexpr double density_penalty_exponent = 3;
 
         //output options
         constexpr bool output_full_preconditioned_matrix = false;
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index 0203fc8..3c058c5 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -1076,6 +1076,11 @@ namespace SAND {
             }
 
 
+            if (cell->active_cell_index()==10)
+            {
+                cell_matrix.print(std::cout);
+            }
+
         }
         system_matrix.compress(VectorOperation::add);
         system_rhs = calculate_rhs(state, barrier_size);

From 5834d5fc093f6a78ef4a2b0a9ace4df4d08512b5 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Thu, 4 Nov 2021 09:02:01 -0600
Subject: [PATCH 20/95] Changed Parameter

---
 source/kkt_system.cc | 9 +--------
 1 file changed, 1 insertion(+), 8 deletions(-)

diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index 3c058c5..8667bcb 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -1070,16 +1070,9 @@ namespace SAND {
 
             constraints.distribute_local_to_global(
                     cell_matrix, cell_rhs, local_dof_indices, system_matrix, system_rhs);
-            if (cell->active_cell_index()==10)
-            {
-                cell_matrix.print(std::cout);
-            }
 
-
-            if (cell->active_cell_index()==10)
-            {
+            if (cell->active_cell_index() == 23)
                 cell_matrix.print(std::cout);
-            }
 
         }
         system_matrix.compress(VectorOperation::add);

From aee26196ff1a69fcac8db0f4c719ab5e0d4f5b65 Mon Sep 17 00:00:00 2001
From: Justin OConnor <64496813+juleoc02@users.noreply.github.com>
Date: Thu, 4 Nov 2021 09:17:45 -0600
Subject: [PATCH 21/95] Delete CMakeCache.txt

---
 CMakeCache.txt | 423 -------------------------------------------------
 1 file changed, 423 deletions(-)
 delete mode 100644 CMakeCache.txt

diff --git a/CMakeCache.txt b/CMakeCache.txt
deleted file mode 100644
index 7a7f87a..0000000
--- a/CMakeCache.txt
+++ /dev/null
@@ -1,423 +0,0 @@
-# This is the CMakeCache file.
-# For build in directory: /home/justin/Thesis/topo code/SAND
-# It was generated by CMake: /snap/clion/169/bin/cmake/linux/bin/cmake
-# You can edit this file to change values found and used by cmake.
-# If you do not want to change any of the values, simply exit the editor.
-# If you do want to change a value, simply edit, save, and exit the editor.
-# The syntax for the file is as follows:
-# KEY:TYPE=VALUE
-# KEY is the name of a variable in the cache.
-# TYPE is a hint to GUIs for the type of VALUE, DO NOT EDIT TYPE!.
-# VALUE is the current value for the KEY.
-
-########################
-# EXTERNAL cache entries
-########################
-
-//Path to a program.
-CMAKE_ADDR2LINE:FILEPATH=/usr/bin/addr2line
-
-//Path to a program.
-CMAKE_AR:FILEPATH=/usr/bin/ar
-
-//Choose the type of build, options are: Debug, Release
-CMAKE_BUILD_TYPE:STRING=Release
-
-//Enable/Disable color output during build.
-CMAKE_COLOR_MAKEFILE:BOOL=ON
-
-//CXX compiler
-CMAKE_CXX_COMPILER:STRING=/usr/bin/c++
-
-//A wrapper around 'ar' adding the appropriate '--plugin' option
-// for the GCC compiler
-CMAKE_CXX_COMPILER_AR:FILEPATH=/usr/bin/gcc-ar-9
-
-//A wrapper around 'ranlib' adding the appropriate '--plugin' option
-// for the GCC compiler
-CMAKE_CXX_COMPILER_RANLIB:FILEPATH=/usr/bin/gcc-ranlib-9
-
-//Flags used by the compiler during all build types.
-CMAKE_CXX_FLAGS:STRING=
-
-//Flags used by the compiler during debug builds.
-CMAKE_CXX_FLAGS_DEBUG:STRING=
-
-//Flags used by the CXX compiler during MINSIZEREL builds.
-CMAKE_CXX_FLAGS_MINSIZEREL:STRING=-Os -DNDEBUG
-
-//Flags used by the compiler during release builds.
-CMAKE_CXX_FLAGS_RELEASE:STRING=
-
-//Flags used by the CXX compiler during RELWITHDEBINFO builds.
-CMAKE_CXX_FLAGS_RELWITHDEBINFO:STRING=-O2 -g -DNDEBUG
-
-//C compiler
-CMAKE_C_COMPILER:STRING=/usr/bin/cc
-
-//A wrapper around 'ar' adding the appropriate '--plugin' option
-// for the GCC compiler
-CMAKE_C_COMPILER_AR:FILEPATH=/usr/bin/gcc-ar-9
-
-//A wrapper around 'ranlib' adding the appropriate '--plugin' option
-// for the GCC compiler
-CMAKE_C_COMPILER_RANLIB:FILEPATH=/usr/bin/gcc-ranlib-9
-
-//Flags used by the C compiler during all build types.
-CMAKE_C_FLAGS:STRING=
-
-//Flags used by the C compiler during DEBUG builds.
-CMAKE_C_FLAGS_DEBUG:STRING=-g
-
-//Flags used by the C compiler during MINSIZEREL builds.
-CMAKE_C_FLAGS_MINSIZEREL:STRING=-Os -DNDEBUG
-
-//Flags used by the C compiler during RELEASE builds.
-CMAKE_C_FLAGS_RELEASE:STRING=-O3 -DNDEBUG
-
-//Flags used by the C compiler during RELWITHDEBINFO builds.
-CMAKE_C_FLAGS_RELWITHDEBINFO:STRING=-O2 -g -DNDEBUG
-
-//No help, variable specified on the command line.
-CMAKE_DEPENDS_USE_COMPILER:UNINITIALIZED=FALSE
-
-//Path to a program.
-CMAKE_DLLTOOL:FILEPATH=CMAKE_DLLTOOL-NOTFOUND
-
-//The Eclipse executable
-CMAKE_ECLIPSE_EXECUTABLE:FILEPATH=CMAKE_ECLIPSE_EXECUTABLE-NOTFOUND
-
-//If disabled, CMake will not generate linked resource to the subprojects
-// and to the source files within targets
-CMAKE_ECLIPSE_GENERATE_LINKED_RESOURCES:BOOL=TRUE
-
-//If enabled, CMake will generate a source project for Eclipse
-// in CMAKE_SOURCE_DIR
-CMAKE_ECLIPSE_GENERATE_SOURCE_PROJECT:BOOL=FALSE
-
-//Additional command line arguments when Eclipse invokes make.
-// Enter e.g. -j<some_number> to get parallel builds
-CMAKE_ECLIPSE_MAKE_ARGUMENTS:STRING=-j8
-
-//The version of Eclipse. If Eclipse has not been found, 3.6 (Helios)
-// is assumed.
-CMAKE_ECLIPSE_VERSION:STRING=3.6 (Helios)
-
-//Flags used by the linker during all build types.
-CMAKE_EXE_LINKER_FLAGS:STRING=
-
-//Flags used by the linker during DEBUG builds.
-CMAKE_EXE_LINKER_FLAGS_DEBUG:STRING=
-
-//Flags used by the linker during MINSIZEREL builds.
-CMAKE_EXE_LINKER_FLAGS_MINSIZEREL:STRING=
-
-//Flags used by the linker during RELEASE builds.
-CMAKE_EXE_LINKER_FLAGS_RELEASE:STRING=
-
-//Flags used by the linker during RELWITHDEBINFO builds.
-CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO:STRING=
-
-//Enable/Disable output of compile commands during generation.
-CMAKE_EXPORT_COMPILE_COMMANDS:BOOL=OFF
-
-//Install path prefix, prepended onto install directories.
-CMAKE_INSTALL_PREFIX:PATH=/usr/local
-
-//Path to a program.
-CMAKE_LINKER:FILEPATH=/usr/bin/ld
-
-//Path to a program.
-CMAKE_MAKE_PROGRAM:FILEPATH=/usr/bin/make
-
-//Flags used by the linker during the creation of modules during
-// all build types.
-CMAKE_MODULE_LINKER_FLAGS:STRING=
-
-//Flags used by the linker during the creation of modules during
-// DEBUG builds.
-CMAKE_MODULE_LINKER_FLAGS_DEBUG:STRING=
-
-//Flags used by the linker during the creation of modules during
-// MINSIZEREL builds.
-CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL:STRING=
-
-//Flags used by the linker during the creation of modules during
-// RELEASE builds.
-CMAKE_MODULE_LINKER_FLAGS_RELEASE:STRING=
-
-//Flags used by the linker during the creation of modules during
-// RELWITHDEBINFO builds.
-CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO:STRING=
-
-//Path to a program.
-CMAKE_NM:FILEPATH=/usr/bin/nm
-
-//Path to a program.
-CMAKE_OBJCOPY:FILEPATH=/usr/bin/objcopy
-
-//Path to a program.
-CMAKE_OBJDUMP:FILEPATH=/usr/bin/objdump
-
-//Value Computed by CMake
-CMAKE_PROJECT_DESCRIPTION:STATIC=
-
-//Value Computed by CMake
-CMAKE_PROJECT_HOMEPAGE_URL:STATIC=
-
-//Value Computed by CMake
-CMAKE_PROJECT_NAME:STATIC=SAND
-
-//Path to a program.
-CMAKE_RANLIB:FILEPATH=/usr/bin/ranlib
-
-//Path to a program.
-CMAKE_READELF:FILEPATH=/usr/bin/readelf
-
-//Flags used by the linker during the creation of shared libraries
-// during all build types.
-CMAKE_SHARED_LINKER_FLAGS:STRING=
-
-//Flags used by the linker during the creation of shared libraries
-// during DEBUG builds.
-CMAKE_SHARED_LINKER_FLAGS_DEBUG:STRING=
-
-//Flags used by the linker during the creation of shared libraries
-// during MINSIZEREL builds.
-CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL:STRING=
-
-//Flags used by the linker during the creation of shared libraries
-// during RELEASE builds.
-CMAKE_SHARED_LINKER_FLAGS_RELEASE:STRING=
-
-//Flags used by the linker during the creation of shared libraries
-// during RELWITHDEBINFO builds.
-CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO:STRING=
-
-//If set, runtime paths are not added when installing shared libraries,
-// but are added when building.
-CMAKE_SKIP_INSTALL_RPATH:BOOL=NO
-
-//If set, runtime paths are not added when using shared libraries.
-CMAKE_SKIP_RPATH:BOOL=NO
-
-//Flags used by the linker during the creation of static libraries
-// during all build types.
-CMAKE_STATIC_LINKER_FLAGS:STRING=
-
-//Flags used by the linker during the creation of static libraries
-// during DEBUG builds.
-CMAKE_STATIC_LINKER_FLAGS_DEBUG:STRING=
-
-//Flags used by the linker during the creation of static libraries
-// during MINSIZEREL builds.
-CMAKE_STATIC_LINKER_FLAGS_MINSIZEREL:STRING=
-
-//Flags used by the linker during the creation of static libraries
-// during RELEASE builds.
-CMAKE_STATIC_LINKER_FLAGS_RELEASE:STRING=
-
-//Flags used by the linker during the creation of static libraries
-// during RELWITHDEBINFO builds.
-CMAKE_STATIC_LINKER_FLAGS_RELWITHDEBINFO:STRING=
-
-//Path to a program.
-CMAKE_STRIP:FILEPATH=/usr/bin/strip
-
-//If this value is on, makefiles will be generated without the
-// .SILENT directive, and all commands will be echoed to the console
-// during the make.  This is useful for debugging only. With Visual
-// Studio IDE projects all commands are done without /nologo.
-CMAKE_VERBOSE_MAKEFILE:BOOL=FALSE
-
-//No help, variable specified on the command line.
-DEAL_II_DIR:UNINITIALIZED=/home/justin/Dealii/installed
-
-//Path to a program.
-PERL_EXECUTABLE:FILEPATH=/usr/bin/perl
-
-//Path to a program.
-ProcessorCount_cmd_nproc:FILEPATH=/usr/bin/nproc
-
-//Path to a program.
-ProcessorCount_cmd_sysctl:FILEPATH=/usr/sbin/sysctl
-
-//Value Computed by CMake
-SAND_BINARY_DIR:STATIC=/home/justin/Thesis/topo code/SAND
-
-//Value Computed by CMake
-SAND_SOURCE_DIR:STATIC=/home/justin/Thesis/topo code/SAND
-
-//The directory containing a CMake configuration file for deal.II.
-deal.II_DIR:PATH=/home/justin/Dealii/installed/lib/cmake/deal.II
-
-
-########################
-# INTERNAL cache entries
-########################
-
-//ADVANCED property for variable: CMAKE_ADDR2LINE
-CMAKE_ADDR2LINE-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_AR
-CMAKE_AR-ADVANCED:INTERNAL=1
-//This is the directory where this CMakeCache.txt was created
-CMAKE_CACHEFILE_DIR:INTERNAL=/home/justin/Thesis/topo code/SAND
-//Major version of cmake used to create the current loaded cache
-CMAKE_CACHE_MAJOR_VERSION:INTERNAL=3
-//Minor version of cmake used to create the current loaded cache
-CMAKE_CACHE_MINOR_VERSION:INTERNAL=20
-//Patch version of cmake used to create the current loaded cache
-CMAKE_CACHE_PATCH_VERSION:INTERNAL=2
-//ADVANCED property for variable: CMAKE_COLOR_MAKEFILE
-CMAKE_COLOR_MAKEFILE-ADVANCED:INTERNAL=1
-//Path to CMake executable.
-CMAKE_COMMAND:INTERNAL=/snap/clion/169/bin/cmake/linux/bin/cmake
-//Path to cpack program executable.
-CMAKE_CPACK_COMMAND:INTERNAL=/snap/clion/169/bin/cmake/linux/bin/cpack
-//Path to ctest program executable.
-CMAKE_CTEST_COMMAND:INTERNAL=/snap/clion/169/bin/cmake/linux/bin/ctest
-//ADVANCED property for variable: CMAKE_CXX_COMPILER
-CMAKE_CXX_COMPILER-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_CXX_COMPILER_AR
-CMAKE_CXX_COMPILER_AR-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_CXX_COMPILER_RANLIB
-CMAKE_CXX_COMPILER_RANLIB-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_CXX_FLAGS
-CMAKE_CXX_FLAGS-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_CXX_FLAGS_DEBUG
-CMAKE_CXX_FLAGS_DEBUG-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_CXX_FLAGS_MINSIZEREL
-CMAKE_CXX_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_CXX_FLAGS_RELEASE
-CMAKE_CXX_FLAGS_RELEASE-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_CXX_FLAGS_RELWITHDEBINFO
-CMAKE_CXX_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_C_COMPILER
-CMAKE_C_COMPILER-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_C_COMPILER_AR
-CMAKE_C_COMPILER_AR-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_C_COMPILER_RANLIB
-CMAKE_C_COMPILER_RANLIB-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_C_FLAGS
-CMAKE_C_FLAGS-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_C_FLAGS_DEBUG
-CMAKE_C_FLAGS_DEBUG-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_C_FLAGS_MINSIZEREL
-CMAKE_C_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_C_FLAGS_RELEASE
-CMAKE_C_FLAGS_RELEASE-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_C_FLAGS_RELWITHDEBINFO
-CMAKE_C_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_DLLTOOL
-CMAKE_DLLTOOL-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_ECLIPSE_GENERATE_SOURCE_PROJECT
-CMAKE_ECLIPSE_GENERATE_SOURCE_PROJECT-ADVANCED:INTERNAL=1
-//STRINGS property for variable: CMAKE_ECLIPSE_VERSION
-CMAKE_ECLIPSE_VERSION-STRINGS:INTERNAL=3.2 (Callisto);3.3 (Europa);3.4 (Ganymede);3.5 (Galileo);3.6 (Helios);3.7 (Indigo);4.2 (Juno);4.3 (Kepler);4.4 (Luna);4.5 (Mars)
-//Executable file format
-CMAKE_EXECUTABLE_FORMAT:INTERNAL=ELF
-//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS
-CMAKE_EXE_LINKER_FLAGS-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_DEBUG
-CMAKE_EXE_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_MINSIZEREL
-CMAKE_EXE_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_RELEASE
-CMAKE_EXE_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO
-CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_EXPORT_COMPILE_COMMANDS
-CMAKE_EXPORT_COMPILE_COMMANDS-ADVANCED:INTERNAL=1
-//Name of external makefile project generator.
-CMAKE_EXTRA_GENERATOR:INTERNAL=Eclipse CDT4
-//CXX compiler system defined macros
-CMAKE_EXTRA_GENERATOR_CXX_SYSTEM_DEFINED_MACROS:INTERNAL=__STDC__;1;__STDC_VERSION__;201710L;__STDC_UTF_16__;1;__STDC_UTF_32__;1;__STDC_HOSTED__;1;__GNUC__;9;__GNUC_MINOR__;3;__GNUC_PATCHLEVEL__;0;__VERSION__;"9.3.0";__ATOMIC_RELAXED;0;__ATOMIC_SEQ_CST;5;__ATOMIC_ACQUIRE;2;__ATOMIC_RELEASE;3;__ATOMIC_ACQ_REL;4;__ATOMIC_CONSUME;1;__pic__;2;__PIC__;2;__pie__;2;__PIE__;2;__FINITE_MATH_ONLY__;0;_LP64;1;__LP64__;1;__SIZEOF_INT__;4;__SIZEOF_LONG__;8;__SIZEOF_LONG_LONG__;8;__SIZEOF_SHORT__;2;__SIZEOF_FLOAT__;4;__SIZEOF_DOUBLE__;8;__SIZEOF_LONG_DOUBLE__;16;__SIZEOF_SIZE_T__;8;__CHAR_BIT__;8;__BIGGEST_ALIGNMENT__;16;__ORDER_LITTLE_ENDIAN__;1234;__ORDER_BIG_ENDIAN__;4321;__ORDER_PDP_ENDIAN__;3412;__BYTE_ORDER__;__ORDER_LITTLE_ENDIAN__;__FLOAT_WORD_ORDER__;__ORDER_LITTLE_ENDIAN__;__SIZEOF_POINTER__;8;__SIZE_TYPE__;long unsigned int;__PTRDIFF_TYPE__;long int;__WCHAR_TYPE__;int;__WINT_TYPE__;unsigned int;__INTMAX_TYPE__;long int;__UINTMAX_TYPE__;long unsigned int;__CHAR16_TYPE__;short unsigned int;__CHAR32_TYPE__;unsigned int;__SIG_ATOMIC_TYPE__;int;__INT8_TYPE__;signed char;__INT16_TYPE__;short int;__INT32_TYPE__;int;__INT64_TYPE__;long int;__UINT8_TYPE__;unsigned char;__UINT16_TYPE__;short unsigned int;__UINT32_TYPE__;unsigned int;__UINT64_TYPE__;long unsigned int;__INT_LEAST8_TYPE__;signed char;__INT_LEAST16_TYPE__;short int;__INT_LEAST32_TYPE__;int;__INT_LEAST64_TYPE__;long int;__UINT_LEAST8_TYPE__;unsigned char;__UINT_LEAST16_TYPE__;short unsigned int;__UINT_LEAST32_TYPE__;unsigned int;__UINT_LEAST64_TYPE__;long unsigned int;__INT_FAST8_TYPE__;signed char;__INT_FAST16_TYPE__;long int;__INT_FAST32_TYPE__;long int;__INT_FAST64_TYPE__;long int;__UINT_FAST8_TYPE__;unsigned char;__UINT_FAST16_TYPE__;long unsigned int;__UINT_FAST32_TYPE__;long unsigned int;__UINT_FAST64_TYPE__;long unsigned int;__INTPTR_TYPE__;long int;__UINTPTR_TYPE__;long unsigned int;__has_include(STR);__has_include__(STR);__has_include_next(STR);__has_include_next__(STR);__GXX_ABI_VERSION;1013;__SCHAR_MAX__;0x7f;__SHRT_MAX__;0x7fff;__INT_MAX__;0x7fffffff;__LONG_MAX__;0x7fffffffffffffffL;__LONG_LONG_MAX__;0x7fffffffffffffffLL;__WCHAR_MAX__;0x7fffffff;__WCHAR_MIN__;(-__WCHAR_MAX__ - 1);__WINT_MAX__;0xffffffffU;__WINT_MIN__;0U;__PTRDIFF_MAX__;0x7fffffffffffffffL;__SIZE_MAX__;0xffffffffffffffffUL;__SCHAR_WIDTH__;8;__SHRT_WIDTH__;16;__INT_WIDTH__;32;__LONG_WIDTH__;64;__LONG_LONG_WIDTH__;64;__WCHAR_WIDTH__;32;__WINT_WIDTH__;32;__PTRDIFF_WIDTH__;64;__SIZE_WIDTH__;64;__INTMAX_MAX__;0x7fffffffffffffffL;__INTMAX_C(c);c ## L;__UINTMAX_MAX__;0xffffffffffffffffUL;__UINTMAX_C(c);c ## UL;__INTMAX_WIDTH__;64;__SIG_ATOMIC_MAX__;0x7fffffff;__SIG_ATOMIC_MIN__;(-__SIG_ATOMIC_MAX__ - 1);__SIG_ATOMIC_WIDTH__;32;__INT8_MAX__;0x7f;__INT16_MAX__;0x7fff;__INT32_MAX__;0x7fffffff;__INT64_MAX__;0x7fffffffffffffffL;__UINT8_MAX__;0xff;__UINT16_MAX__;0xffff;__UINT32_MAX__;0xffffffffU;__UINT64_MAX__;0xffffffffffffffffUL;__INT_LEAST8_MAX__;0x7f;__INT8_C(c);c;__INT_LEAST8_WIDTH__;8;__INT_LEAST16_MAX__;0x7fff;__INT16_C(c);c;__INT_LEAST16_WIDTH__;16;__INT_LEAST32_MAX__;0x7fffffff;__INT32_C(c);c;__INT_LEAST32_WIDTH__;32;__INT_LEAST64_MAX__;0x7fffffffffffffffL;__INT64_C(c);c ## L;__INT_LEAST64_WIDTH__;64;__UINT_LEAST8_MAX__;0xff;__UINT8_C(c);c;__UINT_LEAST16_MAX__;0xffff;__UINT16_C(c);c;__UINT_LEAST32_MAX__;0xffffffffU;__UINT32_C(c);c ## U;__UINT_LEAST64_MAX__;0xffffffffffffffffUL;__UINT64_C(c);c ## UL;__INT_FAST8_MAX__;0x7f;__INT_FAST8_WIDTH__;8;__INT_FAST16_MAX__;0x7fffffffffffffffL;__INT_FAST16_WIDTH__;64;__INT_FAST32_MAX__;0x7fffffffffffffffL;__INT_FAST32_WIDTH__;64;__INT_FAST64_MAX__;0x7fffffffffffffffL;__INT_FAST64_WIDTH__;64;__UINT_FAST8_MAX__;0xff;__UINT_FAST16_MAX__;0xffffffffffffffffUL;__UINT_FAST32_MAX__;0xffffffffffffffffUL;__UINT_FAST64_MAX__;0xffffffffffffffffUL;__INTPTR_MAX__;0x7fffffffffffffffL;__INTPTR_WIDTH__;64;__UINTPTR_MAX__;0xffffffffffffffffUL;__GCC_IEC_559;2;__GCC_IEC_559_COMPLEX;2;__FLT_EVAL_METHOD__;0;__FLT_EVAL_METHOD_TS_18661_3__;0;__DEC_EVAL_METHOD__;2;__FLT_RADIX__;2;__FLT_MANT_DIG__;24;__FLT_DIG__;6;__FLT_MIN_EXP__;(-125);__FLT_MIN_10_EXP__;(-37);__FLT_MAX_EXP__;128;__FLT_MAX_10_EXP__;38;__FLT_DECIMAL_DIG__;9;__FLT_MAX__;3.40282346638528859811704183484516925e+38F;__FLT_MIN__;1.17549435082228750796873653722224568e-38F;__FLT_EPSILON__;1.19209289550781250000000000000000000e-7F;__FLT_DENORM_MIN__;1.40129846432481707092372958328991613e-45F;__FLT_HAS_DENORM__;1;__FLT_HAS_INFINITY__;1;__FLT_HAS_QUIET_NAN__;1;__DBL_MANT_DIG__;53;__DBL_DIG__;15;__DBL_MIN_EXP__;(-1021);__DBL_MIN_10_EXP__;(-307);__DBL_MAX_EXP__;1024;__DBL_MAX_10_EXP__;308;__DBL_DECIMAL_DIG__;17;__DBL_MAX__;((double)1.79769313486231570814527423731704357e+308L);__DBL_MIN__;((double)2.22507385850720138309023271733240406e-308L);__DBL_EPSILON__;((double)2.22044604925031308084726333618164062e-16L);__DBL_DENORM_MIN__;((double)4.94065645841246544176568792868221372e-324L);__DBL_HAS_DENORM__;1;__DBL_HAS_INFINITY__;1;__DBL_HAS_QUIET_NAN__;1;__LDBL_MANT_DIG__;64;__LDBL_DIG__;18;__LDBL_MIN_EXP__;(-16381);__LDBL_MIN_10_EXP__;(-4931);__LDBL_MAX_EXP__;16384;__LDBL_MAX_10_EXP__;4932;__DECIMAL_DIG__;21;__LDBL_DECIMAL_DIG__;21;__LDBL_MAX__;1.18973149535723176502126385303097021e+4932L;__LDBL_MIN__;3.36210314311209350626267781732175260e-4932L;__LDBL_EPSILON__;1.08420217248550443400745280086994171e-19L;__LDBL_DENORM_MIN__;3.64519953188247460252840593361941982e-4951L;__LDBL_HAS_DENORM__;1;__LDBL_HAS_INFINITY__;1;__LDBL_HAS_QUIET_NAN__;1;__FLT32_MANT_DIG__;24;__FLT32_DIG__;6;__FLT32_MIN_EXP__;(-125);__FLT32_MIN_10_EXP__;(-37);__FLT32_MAX_EXP__;128;__FLT32_MAX_10_EXP__;38;__FLT32_DECIMAL_DIG__;9;__FLT32_MAX__;3.40282346638528859811704183484516925e+38F32;__FLT32_MIN__;1.17549435082228750796873653722224568e-38F32;__FLT32_EPSILON__;1.19209289550781250000000000000000000e-7F32;__FLT32_DENORM_MIN__;1.40129846432481707092372958328991613e-45F32;__FLT32_HAS_DENORM__;1;__FLT32_HAS_INFINITY__;1;__FLT32_HAS_QUIET_NAN__;1;__FLT64_MANT_DIG__;53;__FLT64_DIG__;15;__FLT64_MIN_EXP__;(-1021);__FLT64_MIN_10_EXP__;(-307);__FLT64_MAX_EXP__;1024;__FLT64_MAX_10_EXP__;308;__FLT64_DECIMAL_DIG__;17;__FLT64_MAX__;1.79769313486231570814527423731704357e+308F64;__FLT64_MIN__;2.22507385850720138309023271733240406e-308F64;__FLT64_EPSILON__;2.22044604925031308084726333618164062e-16F64;__FLT64_DENORM_MIN__;4.94065645841246544176568792868221372e-324F64;__FLT64_HAS_DENORM__;1;__FLT64_HAS_INFINITY__;1;__FLT64_HAS_QUIET_NAN__;1;__FLT128_MANT_DIG__;113;__FLT128_DIG__;33;__FLT128_MIN_EXP__;(-16381);__FLT128_MIN_10_EXP__;(-4931);__FLT128_MAX_EXP__;16384;__FLT128_MAX_10_EXP__;4932;__FLT128_DECIMAL_DIG__;36;__FLT128_MAX__;1.18973149535723176508575932662800702e+4932F128;__FLT128_MIN__;3.36210314311209350626267781732175260e-4932F128;__FLT128_EPSILON__;1.92592994438723585305597794258492732e-34F128;__FLT128_DENORM_MIN__;6.47517511943802511092443895822764655e-4966F128;__FLT128_HAS_DENORM__;1;__FLT128_HAS_INFINITY__;1;__FLT128_HAS_QUIET_NAN__;1;__FLT32X_MANT_DIG__;53;__FLT32X_DIG__;15;__FLT32X_MIN_EXP__;(-1021);__FLT32X_MIN_10_EXP__;(-307);__FLT32X_MAX_EXP__;1024;__FLT32X_MAX_10_EXP__;308;__FLT32X_DECIMAL_DIG__;17;__FLT32X_MAX__;1.79769313486231570814527423731704357e+308F32x;__FLT32X_MIN__;2.22507385850720138309023271733240406e-308F32x;__FLT32X_EPSILON__;2.22044604925031308084726333618164062e-16F32x;__FLT32X_DENORM_MIN__;4.94065645841246544176568792868221372e-324F32x;__FLT32X_HAS_DENORM__;1;__FLT32X_HAS_INFINITY__;1;__FLT32X_HAS_QUIET_NAN__;1;__FLT64X_MANT_DIG__;64;__FLT64X_DIG__;18;__FLT64X_MIN_EXP__;(-16381);__FLT64X_MIN_10_EXP__;(-4931);__FLT64X_MAX_EXP__;16384;__FLT64X_MAX_10_EXP__;4932;__FLT64X_DECIMAL_DIG__;21;__FLT64X_MAX__;1.18973149535723176502126385303097021e+4932F64x;__FLT64X_MIN__;3.36210314311209350626267781732175260e-4932F64x;__FLT64X_EPSILON__;1.08420217248550443400745280086994171e-19F64x;__FLT64X_DENORM_MIN__;3.64519953188247460252840593361941982e-4951F64x;__FLT64X_HAS_DENORM__;1;__FLT64X_HAS_INFINITY__;1;__FLT64X_HAS_QUIET_NAN__;1;__DEC32_MANT_DIG__;7;__DEC32_MIN_EXP__;(-94);__DEC32_MAX_EXP__;97;__DEC32_MIN__;1E-95DF;__DEC32_MAX__;9.999999E96DF;__DEC32_EPSILON__;1E-6DF;__DEC32_SUBNORMAL_MIN__;0.000001E-95DF;__DEC64_MANT_DIG__;16;__DEC64_MIN_EXP__;(-382);__DEC64_MAX_EXP__;385;__DEC64_MIN__;1E-383DD;__DEC64_MAX__;9.999999999999999E384DD;__DEC64_EPSILON__;1E-15DD;__DEC64_SUBNORMAL_MIN__;0.000000000000001E-383DD;__DEC128_MANT_DIG__;34;__DEC128_MIN_EXP__;(-6142);__DEC128_MAX_EXP__;6145;__DEC128_MIN__;1E-6143DL;__DEC128_MAX__;9.999999999999999999999999999999999E6144DL;__DEC128_EPSILON__;1E-33DL;__DEC128_SUBNORMAL_MIN__;0.000000000000000000000000000000001E-6143DL;__REGISTER_PREFIX__; ;__USER_LABEL_PREFIX__; ;__GNUC_STDC_INLINE__;1;__NO_INLINE__;1;__GCC_HAVE_SYNC_COMPARE_AND_SWAP_1;1;__GCC_HAVE_SYNC_COMPARE_AND_SWAP_2;1;__GCC_HAVE_SYNC_COMPARE_AND_SWAP_4;1;__GCC_HAVE_SYNC_COMPARE_AND_SWAP_8;1;__GCC_ATOMIC_BOOL_LOCK_FREE;2;__GCC_ATOMIC_CHAR_LOCK_FREE;2;__GCC_ATOMIC_CHAR16_T_LOCK_FREE;2;__GCC_ATOMIC_CHAR32_T_LOCK_FREE;2;__GCC_ATOMIC_WCHAR_T_LOCK_FREE;2;__GCC_ATOMIC_SHORT_LOCK_FREE;2;__GCC_ATOMIC_INT_LOCK_FREE;2;__GCC_ATOMIC_LONG_LOCK_FREE;2;__GCC_ATOMIC_LLONG_LOCK_FREE;2;__GCC_ATOMIC_TEST_AND_SET_TRUEVAL;1;__GCC_ATOMIC_POINTER_LOCK_FREE;2;__HAVE_SPECULATION_SAFE_VALUE;1;__GCC_HAVE_DWARF2_CFI_ASM;1;__PRAGMA_REDEFINE_EXTNAME;1;__SSP_STRONG__;3;__SIZEOF_INT128__;16;__SIZEOF_WCHAR_T__;4;__SIZEOF_WINT_T__;4;__SIZEOF_PTRDIFF_T__;8;__amd64;1;__amd64__;1;__x86_64;1;__x86_64__;1;__SIZEOF_FLOAT80__;16;__SIZEOF_FLOAT128__;16;__ATOMIC_HLE_ACQUIRE;65536;__ATOMIC_HLE_RELEASE;131072;__GCC_ASM_FLAG_OUTPUTS__;1;__k8;1;__k8__;1;__code_model_small__;1;__MMX__;1;__SSE__;1;__SSE2__;1;__FXSR__;1;__SSE_MATH__;1;__SSE2_MATH__;1;__SEG_FS;1;__SEG_GS;1;__CET__;3;__gnu_linux__;1;__linux;1;__linux__;1;linux;1;__unix;1;__unix__;1;unix;1;__ELF__;1;__DECIMAL_BID_FORMAT__;1;_STDC_PREDEF_H;1;__STDC_IEC_559__;1;__STDC_IEC_559_COMPLEX__;1;__STDC_ISO_10646__;201706L;__STDC__;1;__cplusplus;201402L;__STDC_UTF_16__;1;__STDC_UTF_32__;1;__STDC_HOSTED__;1;__GNUC__;9;__GNUC_MINOR__;3;__GNUC_PATCHLEVEL__;0;__VERSION__;"9.3.0";__ATOMIC_RELAXED;0;__ATOMIC_SEQ_CST;5;__ATOMIC_ACQUIRE;2;__ATOMIC_RELEASE;3;__ATOMIC_ACQ_REL;4;__ATOMIC_CONSUME;1;__pic__;2;__PIC__;2;__pie__;2;__PIE__;2;__FINITE_MATH_ONLY__;0;_LP64;1;__LP64__;1;__SIZEOF_INT__;4;__SIZEOF_LONG__;8;__SIZEOF_LONG_LONG__;8;__SIZEOF_SHORT__;2;__SIZEOF_FLOAT__;4;__SIZEOF_DOUBLE__;8;__SIZEOF_LONG_DOUBLE__;16;__SIZEOF_SIZE_T__;8;__CHAR_BIT__;8;__BIGGEST_ALIGNMENT__;16;__ORDER_LITTLE_ENDIAN__;1234;__ORDER_BIG_ENDIAN__;4321;__ORDER_PDP_ENDIAN__;3412;__BYTE_ORDER__;__ORDER_LITTLE_ENDIAN__;__FLOAT_WORD_ORDER__;__ORDER_LITTLE_ENDIAN__;__SIZEOF_POINTER__;8;__GNUG__;9;__SIZE_TYPE__;long unsigned int;__PTRDIFF_TYPE__;long int;__WCHAR_TYPE__;int;__WINT_TYPE__;unsigned int;__INTMAX_TYPE__;long int;__UINTMAX_TYPE__;long unsigned int;__CHAR16_TYPE__;short unsigned int;__CHAR32_TYPE__;unsigned int;__SIG_ATOMIC_TYPE__;int;__INT8_TYPE__;signed char;__INT16_TYPE__;short int;__INT32_TYPE__;int;__INT64_TYPE__;long int;__UINT8_TYPE__;unsigned char;__UINT16_TYPE__;short unsigned int;__UINT32_TYPE__;unsigned int;__UINT64_TYPE__;long unsigned int;__INT_LEAST8_TYPE__;signed char;__INT_LEAST16_TYPE__;short int;__INT_LEAST32_TYPE__;int;__INT_LEAST64_TYPE__;long int;__UINT_LEAST8_TYPE__;unsigned char;__UINT_LEAST16_TYPE__;short unsigned int;__UINT_LEAST32_TYPE__;unsigned int;__UINT_LEAST64_TYPE__;long unsigned int;__INT_FAST8_TYPE__;signed char;__INT_FAST16_TYPE__;long int;__INT_FAST32_TYPE__;long int;__INT_FAST64_TYPE__;long int;__UINT_FAST8_TYPE__;unsigned char;__UINT_FAST16_TYPE__;long unsigned int;__UINT_FAST32_TYPE__;long unsigned int;__UINT_FAST64_TYPE__;long unsigned int;__INTPTR_TYPE__;long int;__UINTPTR_TYPE__;long unsigned int;__has_include(STR);__has_include__(STR);__has_include_next(STR);__has_include_next__(STR);__GXX_WEAK__;1;__DEPRECATED;1;__GXX_RTTI;1;__cpp_rtti;199711;__GXX_EXPERIMENTAL_CXX0X__;1;__cpp_binary_literals;201304;__cpp_hex_float;201603;__cpp_runtime_arrays;198712;__cpp_unicode_characters;200704;__cpp_raw_strings;200710;__cpp_unicode_literals;200710;__cpp_user_defined_literals;200809;__cpp_lambdas;200907;__cpp_range_based_for;200907;__cpp_static_assert;200410;__cpp_decltype;200707;__cpp_attributes;200809;__cpp_rvalue_reference;200610;__cpp_rvalue_references;200610;__cpp_variadic_templates;200704;__cpp_initializer_lists;200806;__cpp_delegating_constructors;200604;__cpp_nsdmi;200809;__cpp_inheriting_constructors;201511;__cpp_ref_qualifiers;200710;__cpp_alias_templates;200704;__cpp_return_type_deduction;201304;__cpp_init_captures;201304;__cpp_generic_lambdas;201304;__cpp_constexpr;201304;__cpp_decltype_auto;201304;__cpp_aggregate_nsdmi;201304;__cpp_variable_templates;201304;__cpp_digit_separators;201309;__cpp_sized_deallocation;201309;__cpp_threadsafe_static_init;200806;__EXCEPTIONS;1;__cpp_exceptions;199711;__GXX_ABI_VERSION;1013;__SCHAR_MAX__;0x7f;__SHRT_MAX__;0x7fff;__INT_MAX__;0x7fffffff;__LONG_MAX__;0x7fffffffffffffffL;__LONG_LONG_MAX__;0x7fffffffffffffffLL;__WCHAR_MAX__;0x7fffffff;__WCHAR_MIN__;(-__WCHAR_MAX__ - 1);__WINT_MAX__;0xffffffffU;__WINT_MIN__;0U;__PTRDIFF_MAX__;0x7fffffffffffffffL;__SIZE_MAX__;0xffffffffffffffffUL;__SCHAR_WIDTH__;8;__SHRT_WIDTH__;16;__INT_WIDTH__;32;__LONG_WIDTH__;64;__LONG_LONG_WIDTH__;64;__WCHAR_WIDTH__;32;__WINT_WIDTH__;32;__PTRDIFF_WIDTH__;64;__SIZE_WIDTH__;64;__GLIBCXX_TYPE_INT_N_0;__int128;__GLIBCXX_BITSIZE_INT_N_0;128;__INTMAX_MAX__;0x7fffffffffffffffL;__INTMAX_C(c);c ## L;__UINTMAX_MAX__;0xffffffffffffffffUL;__UINTMAX_C(c);c ## UL;__INTMAX_WIDTH__;64;__SIG_ATOMIC_MAX__;0x7fffffff;__SIG_ATOMIC_MIN__;(-__SIG_ATOMIC_MAX__ - 1);__SIG_ATOMIC_WIDTH__;32;__INT8_MAX__;0x7f;__INT16_MAX__;0x7fff;__INT32_MAX__;0x7fffffff;__INT64_MAX__;0x7fffffffffffffffL;__UINT8_MAX__;0xff;__UINT16_MAX__;0xffff;__UINT32_MAX__;0xffffffffU;__UINT64_MAX__;0xffffffffffffffffUL;__INT_LEAST8_MAX__;0x7f;__INT8_C(c);c;__INT_LEAST8_WIDTH__;8;__INT_LEAST16_MAX__;0x7fff;__INT16_C(c);c;__INT_LEAST16_WIDTH__;16;__INT_LEAST32_MAX__;0x7fffffff;__INT32_C(c);c;__INT_LEAST32_WIDTH__;32;__INT_LEAST64_MAX__;0x7fffffffffffffffL;__INT64_C(c);c ## L;__INT_LEAST64_WIDTH__;64;__UINT_LEAST8_MAX__;0xff;__UINT8_C(c);c;__UINT_LEAST16_MAX__;0xffff;__UINT16_C(c);c;__UINT_LEAST32_MAX__;0xffffffffU;__UINT32_C(c);c ## U;__UINT_LEAST64_MAX__;0xffffffffffffffffUL;__UINT64_C(c);c ## UL;__INT_FAST8_MAX__;0x7f;__INT_FAST8_WIDTH__;8;__INT_FAST16_MAX__;0x7fffffffffffffffL;__INT_FAST16_WIDTH__;64;__INT_FAST32_MAX__;0x7fffffffffffffffL;__INT_FAST32_WIDTH__;64;__INT_FAST64_MAX__;0x7fffffffffffffffL;__INT_FAST64_WIDTH__;64;__UINT_FAST8_MAX__;0xff;__UINT_FAST16_MAX__;0xffffffffffffffffUL;__UINT_FAST32_MAX__;0xffffffffffffffffUL;__UINT_FAST64_MAX__;0xffffffffffffffffUL;__INTPTR_MAX__;0x7fffffffffffffffL;__INTPTR_WIDTH__;64;__UINTPTR_MAX__;0xffffffffffffffffUL;__GCC_IEC_559;2;__GCC_IEC_559_COMPLEX;2;__FLT_EVAL_METHOD__;0;__FLT_EVAL_METHOD_TS_18661_3__;0;__DEC_EVAL_METHOD__;2;__FLT_RADIX__;2;__FLT_MANT_DIG__;24;__FLT_DIG__;6;__FLT_MIN_EXP__;(-125);__FLT_MIN_10_EXP__;(-37);__FLT_MAX_EXP__;128;__FLT_MAX_10_EXP__;38;__FLT_DECIMAL_DIG__;9;__FLT_MAX__;3.40282346638528859811704183484516925e+38F;__FLT_MIN__;1.17549435082228750796873653722224568e-38F;__FLT_EPSILON__;1.19209289550781250000000000000000000e-7F;__FLT_DENORM_MIN__;1.40129846432481707092372958328991613e-45F;__FLT_HAS_DENORM__;1;__FLT_HAS_INFINITY__;1;__FLT_HAS_QUIET_NAN__;1;__DBL_MANT_DIG__;53;__DBL_DIG__;15;__DBL_MIN_EXP__;(-1021);__DBL_MIN_10_EXP__;(-307);__DBL_MAX_EXP__;1024;__DBL_MAX_10_EXP__;308;__DBL_DECIMAL_DIG__;17;__DBL_MAX__;double(1.79769313486231570814527423731704357e+308L);__DBL_MIN__;double(2.22507385850720138309023271733240406e-308L);__DBL_EPSILON__;double(2.22044604925031308084726333618164062e-16L);__DBL_DENORM_MIN__;double(4.94065645841246544176568792868221372e-324L);__DBL_HAS_DENORM__;1;__DBL_HAS_INFINITY__;1;__DBL_HAS_QUIET_NAN__;1;__LDBL_MANT_DIG__;64;__LDBL_DIG__;18;__LDBL_MIN_EXP__;(-16381);__LDBL_MIN_10_EXP__;(-4931);__LDBL_MAX_EXP__;16384;__LDBL_MAX_10_EXP__;4932;__DECIMAL_DIG__;21;__LDBL_DECIMAL_DIG__;21;__LDBL_MAX__;1.18973149535723176502126385303097021e+4932L;__LDBL_MIN__;3.36210314311209350626267781732175260e-4932L;__LDBL_EPSILON__;1.08420217248550443400745280086994171e-19L;__LDBL_DENORM_MIN__;3.64519953188247460252840593361941982e-4951L;__LDBL_HAS_DENORM__;1;__LDBL_HAS_INFINITY__;1;__LDBL_HAS_QUIET_NAN__;1;__FLT32_MANT_DIG__;24;__FLT32_DIG__;6;__FLT32_MIN_EXP__;(-125);__FLT32_MIN_10_EXP__;(-37);__FLT32_MAX_EXP__;128;__FLT32_MAX_10_EXP__;38;__FLT32_DECIMAL_DIG__;9;__FLT32_MAX__;3.40282346638528859811704183484516925e+38F32;__FLT32_MIN__;1.17549435082228750796873653722224568e-38F32;__FLT32_EPSILON__;1.19209289550781250000000000000000000e-7F32;__FLT32_DENORM_MIN__;1.40129846432481707092372958328991613e-45F32;__FLT32_HAS_DENORM__;1;__FLT32_HAS_INFINITY__;1;__FLT32_HAS_QUIET_NAN__;1;__FLT64_MANT_DIG__;53;__FLT64_DIG__;15;__FLT64_MIN_EXP__;(-1021);__FLT64_MIN_10_EXP__;(-307);__FLT64_MAX_EXP__;1024;__FLT64_MAX_10_EXP__;308;__FLT64_DECIMAL_DIG__;17;__FLT64_MAX__;1.79769313486231570814527423731704357e+308F64;__FLT64_MIN__;2.22507385850720138309023271733240406e-308F64;__FLT64_EPSILON__;2.22044604925031308084726333618164062e-16F64;__FLT64_DENORM_MIN__;4.94065645841246544176568792868221372e-324F64;__FLT64_HAS_DENORM__;1;__FLT64_HAS_INFINITY__;1;__FLT64_HAS_QUIET_NAN__;1;__FLT128_MANT_DIG__;113;__FLT128_DIG__;33;__FLT128_MIN_EXP__;(-16381);__FLT128_MIN_10_EXP__;(-4931);__FLT128_MAX_EXP__;16384;__FLT128_MAX_10_EXP__;4932;__FLT128_DECIMAL_DIG__;36;__FLT128_MAX__;1.18973149535723176508575932662800702e+4932F128;__FLT128_MIN__;3.36210314311209350626267781732175260e-4932F128;__FLT128_EPSILON__;1.92592994438723585305597794258492732e-34F128;__FLT128_DENORM_MIN__;6.47517511943802511092443895822764655e-4966F128;__FLT128_HAS_DENORM__;1;__FLT128_HAS_INFINITY__;1;__FLT128_HAS_QUIET_NAN__;1;__FLT32X_MANT_DIG__;53;__FLT32X_DIG__;15;__FLT32X_MIN_EXP__;(-1021);__FLT32X_MIN_10_EXP__;(-307);__FLT32X_MAX_EXP__;1024;__FLT32X_MAX_10_EXP__;308;__FLT32X_DECIMAL_DIG__;17;__FLT32X_MAX__;1.79769313486231570814527423731704357e+308F32x;__FLT32X_MIN__;2.22507385850720138309023271733240406e-308F32x;__FLT32X_EPSILON__;2.22044604925031308084726333618164062e-16F32x;__FLT32X_DENORM_MIN__;4.94065645841246544176568792868221372e-324F32x;__FLT32X_HAS_DENORM__;1;__FLT32X_HAS_INFINITY__;1;__FLT32X_HAS_QUIET_NAN__;1;__FLT64X_MANT_DIG__;64;__FLT64X_DIG__;18;__FLT64X_MIN_EXP__;(-16381);__FLT64X_MIN_10_EXP__;(-4931);__FLT64X_MAX_EXP__;16384;__FLT64X_MAX_10_EXP__;4932;__FLT64X_DECIMAL_DIG__;21;__FLT64X_MAX__;1.18973149535723176502126385303097021e+4932F64x;__FLT64X_MIN__;3.36210314311209350626267781732175260e-4932F64x;__FLT64X_EPSILON__;1.08420217248550443400745280086994171e-19F64x;__FLT64X_DENORM_MIN__;3.64519953188247460252840593361941982e-4951F64x;__FLT64X_HAS_DENORM__;1;__FLT64X_HAS_INFINITY__;1;__FLT64X_HAS_QUIET_NAN__;1;__DEC32_MANT_DIG__;7;__DEC32_MIN_EXP__;(-94);__DEC32_MAX_EXP__;97;__DEC32_MIN__;1E-95DF;__DEC32_MAX__;9.999999E96DF;__DEC32_EPSILON__;1E-6DF;__DEC32_SUBNORMAL_MIN__;0.000001E-95DF;__DEC64_MANT_DIG__;16;__DEC64_MIN_EXP__;(-382);__DEC64_MAX_EXP__;385;__DEC64_MIN__;1E-383DD;__DEC64_MAX__;9.999999999999999E384DD;__DEC64_EPSILON__;1E-15DD;__DEC64_SUBNORMAL_MIN__;0.000000000000001E-383DD;__DEC128_MANT_DIG__;34;__DEC128_MIN_EXP__;(-6142);__DEC128_MAX_EXP__;6145;__DEC128_MIN__;1E-6143DL;__DEC128_MAX__;9.999999999999999999999999999999999E6144DL;__DEC128_EPSILON__;1E-33DL;__DEC128_SUBNORMAL_MIN__;0.000000000000000000000000000000001E-6143DL;__REGISTER_PREFIX__; ;__USER_LABEL_PREFIX__; ;__GNUC_STDC_INLINE__;1;__NO_INLINE__;1;__GCC_HAVE_SYNC_COMPARE_AND_SWAP_1;1;__GCC_HAVE_SYNC_COMPARE_AND_SWAP_2;1;__GCC_HAVE_SYNC_COMPARE_AND_SWAP_4;1;__GCC_HAVE_SYNC_COMPARE_AND_SWAP_8;1;__GCC_ATOMIC_BOOL_LOCK_FREE;2;__GCC_ATOMIC_CHAR_LOCK_FREE;2;__GCC_ATOMIC_CHAR16_T_LOCK_FREE;2;__GCC_ATOMIC_CHAR32_T_LOCK_FREE;2;__GCC_ATOMIC_WCHAR_T_LOCK_FREE;2;__GCC_ATOMIC_SHORT_LOCK_FREE;2;__GCC_ATOMIC_INT_LOCK_FREE;2;__GCC_ATOMIC_LONG_LOCK_FREE;2;__GCC_ATOMIC_LLONG_LOCK_FREE;2;__GCC_ATOMIC_TEST_AND_SET_TRUEVAL;1;__GCC_ATOMIC_POINTER_LOCK_FREE;2;__HAVE_SPECULATION_SAFE_VALUE;1;__GCC_HAVE_DWARF2_CFI_ASM;1;__PRAGMA_REDEFINE_EXTNAME;1;__SSP_STRONG__;3;__SIZEOF_INT128__;16;__SIZEOF_WCHAR_T__;4;__SIZEOF_WINT_T__;4;__SIZEOF_PTRDIFF_T__;8;__amd64;1;__amd64__;1;__x86_64;1;__x86_64__;1;__SIZEOF_FLOAT80__;16;__SIZEOF_FLOAT128__;16;__ATOMIC_HLE_ACQUIRE;65536;__ATOMIC_HLE_RELEASE;131072;__GCC_ASM_FLAG_OUTPUTS__;1;__k8;1;__k8__;1;__code_model_small__;1;__MMX__;1;__SSE__;1;__SSE2__;1;__FXSR__;1;__SSE_MATH__;1;__SSE2_MATH__;1;__SEG_FS;1;__SEG_GS;1;__CET__;3;__gnu_linux__;1;__linux;1;__linux__;1;linux;1;__unix;1;__unix__;1;unix;1;__ELF__;1;__DECIMAL_BID_FORMAT__;1;_GNU_SOURCE;1;_STDC_PREDEF_H;1;__STDC_IEC_559__;1;__STDC_IEC_559_COMPLEX__;1;__STDC_ISO_10646__;201706L
-//CXX compiler system include directories
-CMAKE_EXTRA_GENERATOR_CXX_SYSTEM_INCLUDE_DIRS:INTERNAL=/usr/local/cgns/4.1.1_gcc-8.3.0/include;/usr/local/hdf5/1.10.6_gcc-8.3.0/include;/usr/local/szip/2.1.1/include;/usr/include/c++/9;/usr/include/x86_64-linux-gnu/c++/9;/usr/include/c++/9/backward;/usr/lib/gcc/x86_64-linux-gnu/9/include;/usr/local/include;/usr/include/x86_64-linux-gnu;/usr/include
-//C compiler system defined macros
-CMAKE_EXTRA_GENERATOR_C_SYSTEM_DEFINED_MACROS:INTERNAL=__STDC__;1;__STDC_VERSION__;201710L;__STDC_UTF_16__;1;__STDC_UTF_32__;1;__STDC_HOSTED__;1;__GNUC__;9;__GNUC_MINOR__;3;__GNUC_PATCHLEVEL__;0;__VERSION__;"9.3.0";__ATOMIC_RELAXED;0;__ATOMIC_SEQ_CST;5;__ATOMIC_ACQUIRE;2;__ATOMIC_RELEASE;3;__ATOMIC_ACQ_REL;4;__ATOMIC_CONSUME;1;__pic__;2;__PIC__;2;__pie__;2;__PIE__;2;__FINITE_MATH_ONLY__;0;_LP64;1;__LP64__;1;__SIZEOF_INT__;4;__SIZEOF_LONG__;8;__SIZEOF_LONG_LONG__;8;__SIZEOF_SHORT__;2;__SIZEOF_FLOAT__;4;__SIZEOF_DOUBLE__;8;__SIZEOF_LONG_DOUBLE__;16;__SIZEOF_SIZE_T__;8;__CHAR_BIT__;8;__BIGGEST_ALIGNMENT__;16;__ORDER_LITTLE_ENDIAN__;1234;__ORDER_BIG_ENDIAN__;4321;__ORDER_PDP_ENDIAN__;3412;__BYTE_ORDER__;__ORDER_LITTLE_ENDIAN__;__FLOAT_WORD_ORDER__;__ORDER_LITTLE_ENDIAN__;__SIZEOF_POINTER__;8;__SIZE_TYPE__;long unsigned int;__PTRDIFF_TYPE__;long int;__WCHAR_TYPE__;int;__WINT_TYPE__;unsigned int;__INTMAX_TYPE__;long int;__UINTMAX_TYPE__;long unsigned int;__CHAR16_TYPE__;short unsigned int;__CHAR32_TYPE__;unsigned int;__SIG_ATOMIC_TYPE__;int;__INT8_TYPE__;signed char;__INT16_TYPE__;short int;__INT32_TYPE__;int;__INT64_TYPE__;long int;__UINT8_TYPE__;unsigned char;__UINT16_TYPE__;short unsigned int;__UINT32_TYPE__;unsigned int;__UINT64_TYPE__;long unsigned int;__INT_LEAST8_TYPE__;signed char;__INT_LEAST16_TYPE__;short int;__INT_LEAST32_TYPE__;int;__INT_LEAST64_TYPE__;long int;__UINT_LEAST8_TYPE__;unsigned char;__UINT_LEAST16_TYPE__;short unsigned int;__UINT_LEAST32_TYPE__;unsigned int;__UINT_LEAST64_TYPE__;long unsigned int;__INT_FAST8_TYPE__;signed char;__INT_FAST16_TYPE__;long int;__INT_FAST32_TYPE__;long int;__INT_FAST64_TYPE__;long int;__UINT_FAST8_TYPE__;unsigned char;__UINT_FAST16_TYPE__;long unsigned int;__UINT_FAST32_TYPE__;long unsigned int;__UINT_FAST64_TYPE__;long unsigned int;__INTPTR_TYPE__;long int;__UINTPTR_TYPE__;long unsigned int;__has_include(STR);__has_include__(STR);__has_include_next(STR);__has_include_next__(STR);__GXX_ABI_VERSION;1013;__SCHAR_MAX__;0x7f;__SHRT_MAX__;0x7fff;__INT_MAX__;0x7fffffff;__LONG_MAX__;0x7fffffffffffffffL;__LONG_LONG_MAX__;0x7fffffffffffffffLL;__WCHAR_MAX__;0x7fffffff;__WCHAR_MIN__;(-__WCHAR_MAX__ - 1);__WINT_MAX__;0xffffffffU;__WINT_MIN__;0U;__PTRDIFF_MAX__;0x7fffffffffffffffL;__SIZE_MAX__;0xffffffffffffffffUL;__SCHAR_WIDTH__;8;__SHRT_WIDTH__;16;__INT_WIDTH__;32;__LONG_WIDTH__;64;__LONG_LONG_WIDTH__;64;__WCHAR_WIDTH__;32;__WINT_WIDTH__;32;__PTRDIFF_WIDTH__;64;__SIZE_WIDTH__;64;__INTMAX_MAX__;0x7fffffffffffffffL;__INTMAX_C(c);c ## L;__UINTMAX_MAX__;0xffffffffffffffffUL;__UINTMAX_C(c);c ## UL;__INTMAX_WIDTH__;64;__SIG_ATOMIC_MAX__;0x7fffffff;__SIG_ATOMIC_MIN__;(-__SIG_ATOMIC_MAX__ - 1);__SIG_ATOMIC_WIDTH__;32;__INT8_MAX__;0x7f;__INT16_MAX__;0x7fff;__INT32_MAX__;0x7fffffff;__INT64_MAX__;0x7fffffffffffffffL;__UINT8_MAX__;0xff;__UINT16_MAX__;0xffff;__UINT32_MAX__;0xffffffffU;__UINT64_MAX__;0xffffffffffffffffUL;__INT_LEAST8_MAX__;0x7f;__INT8_C(c);c;__INT_LEAST8_WIDTH__;8;__INT_LEAST16_MAX__;0x7fff;__INT16_C(c);c;__INT_LEAST16_WIDTH__;16;__INT_LEAST32_MAX__;0x7fffffff;__INT32_C(c);c;__INT_LEAST32_WIDTH__;32;__INT_LEAST64_MAX__;0x7fffffffffffffffL;__INT64_C(c);c ## L;__INT_LEAST64_WIDTH__;64;__UINT_LEAST8_MAX__;0xff;__UINT8_C(c);c;__UINT_LEAST16_MAX__;0xffff;__UINT16_C(c);c;__UINT_LEAST32_MAX__;0xffffffffU;__UINT32_C(c);c ## U;__UINT_LEAST64_MAX__;0xffffffffffffffffUL;__UINT64_C(c);c ## UL;__INT_FAST8_MAX__;0x7f;__INT_FAST8_WIDTH__;8;__INT_FAST16_MAX__;0x7fffffffffffffffL;__INT_FAST16_WIDTH__;64;__INT_FAST32_MAX__;0x7fffffffffffffffL;__INT_FAST32_WIDTH__;64;__INT_FAST64_MAX__;0x7fffffffffffffffL;__INT_FAST64_WIDTH__;64;__UINT_FAST8_MAX__;0xff;__UINT_FAST16_MAX__;0xffffffffffffffffUL;__UINT_FAST32_MAX__;0xffffffffffffffffUL;__UINT_FAST64_MAX__;0xffffffffffffffffUL;__INTPTR_MAX__;0x7fffffffffffffffL;__INTPTR_WIDTH__;64;__UINTPTR_MAX__;0xffffffffffffffffUL;__GCC_IEC_559;2;__GCC_IEC_559_COMPLEX;2;__FLT_EVAL_METHOD__;0;__FLT_EVAL_METHOD_TS_18661_3__;0;__DEC_EVAL_METHOD__;2;__FLT_RADIX__;2;__FLT_MANT_DIG__;24;__FLT_DIG__;6;__FLT_MIN_EXP__;(-125);__FLT_MIN_10_EXP__;(-37);__FLT_MAX_EXP__;128;__FLT_MAX_10_EXP__;38;__FLT_DECIMAL_DIG__;9;__FLT_MAX__;3.40282346638528859811704183484516925e+38F;__FLT_MIN__;1.17549435082228750796873653722224568e-38F;__FLT_EPSILON__;1.19209289550781250000000000000000000e-7F;__FLT_DENORM_MIN__;1.40129846432481707092372958328991613e-45F;__FLT_HAS_DENORM__;1;__FLT_HAS_INFINITY__;1;__FLT_HAS_QUIET_NAN__;1;__DBL_MANT_DIG__;53;__DBL_DIG__;15;__DBL_MIN_EXP__;(-1021);__DBL_MIN_10_EXP__;(-307);__DBL_MAX_EXP__;1024;__DBL_MAX_10_EXP__;308;__DBL_DECIMAL_DIG__;17;__DBL_MAX__;((double)1.79769313486231570814527423731704357e+308L);__DBL_MIN__;((double)2.22507385850720138309023271733240406e-308L);__DBL_EPSILON__;((double)2.22044604925031308084726333618164062e-16L);__DBL_DENORM_MIN__;((double)4.94065645841246544176568792868221372e-324L);__DBL_HAS_DENORM__;1;__DBL_HAS_INFINITY__;1;__DBL_HAS_QUIET_NAN__;1;__LDBL_MANT_DIG__;64;__LDBL_DIG__;18;__LDBL_MIN_EXP__;(-16381);__LDBL_MIN_10_EXP__;(-4931);__LDBL_MAX_EXP__;16384;__LDBL_MAX_10_EXP__;4932;__DECIMAL_DIG__;21;__LDBL_DECIMAL_DIG__;21;__LDBL_MAX__;1.18973149535723176502126385303097021e+4932L;__LDBL_MIN__;3.36210314311209350626267781732175260e-4932L;__LDBL_EPSILON__;1.08420217248550443400745280086994171e-19L;__LDBL_DENORM_MIN__;3.64519953188247460252840593361941982e-4951L;__LDBL_HAS_DENORM__;1;__LDBL_HAS_INFINITY__;1;__LDBL_HAS_QUIET_NAN__;1;__FLT32_MANT_DIG__;24;__FLT32_DIG__;6;__FLT32_MIN_EXP__;(-125);__FLT32_MIN_10_EXP__;(-37);__FLT32_MAX_EXP__;128;__FLT32_MAX_10_EXP__;38;__FLT32_DECIMAL_DIG__;9;__FLT32_MAX__;3.40282346638528859811704183484516925e+38F32;__FLT32_MIN__;1.17549435082228750796873653722224568e-38F32;__FLT32_EPSILON__;1.19209289550781250000000000000000000e-7F32;__FLT32_DENORM_MIN__;1.40129846432481707092372958328991613e-45F32;__FLT32_HAS_DENORM__;1;__FLT32_HAS_INFINITY__;1;__FLT32_HAS_QUIET_NAN__;1;__FLT64_MANT_DIG__;53;__FLT64_DIG__;15;__FLT64_MIN_EXP__;(-1021);__FLT64_MIN_10_EXP__;(-307);__FLT64_MAX_EXP__;1024;__FLT64_MAX_10_EXP__;308;__FLT64_DECIMAL_DIG__;17;__FLT64_MAX__;1.79769313486231570814527423731704357e+308F64;__FLT64_MIN__;2.22507385850720138309023271733240406e-308F64;__FLT64_EPSILON__;2.22044604925031308084726333618164062e-16F64;__FLT64_DENORM_MIN__;4.94065645841246544176568792868221372e-324F64;__FLT64_HAS_DENORM__;1;__FLT64_HAS_INFINITY__;1;__FLT64_HAS_QUIET_NAN__;1;__FLT128_MANT_DIG__;113;__FLT128_DIG__;33;__FLT128_MIN_EXP__;(-16381);__FLT128_MIN_10_EXP__;(-4931);__FLT128_MAX_EXP__;16384;__FLT128_MAX_10_EXP__;4932;__FLT128_DECIMAL_DIG__;36;__FLT128_MAX__;1.18973149535723176508575932662800702e+4932F128;__FLT128_MIN__;3.36210314311209350626267781732175260e-4932F128;__FLT128_EPSILON__;1.92592994438723585305597794258492732e-34F128;__FLT128_DENORM_MIN__;6.47517511943802511092443895822764655e-4966F128;__FLT128_HAS_DENORM__;1;__FLT128_HAS_INFINITY__;1;__FLT128_HAS_QUIET_NAN__;1;__FLT32X_MANT_DIG__;53;__FLT32X_DIG__;15;__FLT32X_MIN_EXP__;(-1021);__FLT32X_MIN_10_EXP__;(-307);__FLT32X_MAX_EXP__;1024;__FLT32X_MAX_10_EXP__;308;__FLT32X_DECIMAL_DIG__;17;__FLT32X_MAX__;1.79769313486231570814527423731704357e+308F32x;__FLT32X_MIN__;2.22507385850720138309023271733240406e-308F32x;__FLT32X_EPSILON__;2.22044604925031308084726333618164062e-16F32x;__FLT32X_DENORM_MIN__;4.94065645841246544176568792868221372e-324F32x;__FLT32X_HAS_DENORM__;1;__FLT32X_HAS_INFINITY__;1;__FLT32X_HAS_QUIET_NAN__;1;__FLT64X_MANT_DIG__;64;__FLT64X_DIG__;18;__FLT64X_MIN_EXP__;(-16381);__FLT64X_MIN_10_EXP__;(-4931);__FLT64X_MAX_EXP__;16384;__FLT64X_MAX_10_EXP__;4932;__FLT64X_DECIMAL_DIG__;21;__FLT64X_MAX__;1.18973149535723176502126385303097021e+4932F64x;__FLT64X_MIN__;3.36210314311209350626267781732175260e-4932F64x;__FLT64X_EPSILON__;1.08420217248550443400745280086994171e-19F64x;__FLT64X_DENORM_MIN__;3.64519953188247460252840593361941982e-4951F64x;__FLT64X_HAS_DENORM__;1;__FLT64X_HAS_INFINITY__;1;__FLT64X_HAS_QUIET_NAN__;1;__DEC32_MANT_DIG__;7;__DEC32_MIN_EXP__;(-94);__DEC32_MAX_EXP__;97;__DEC32_MIN__;1E-95DF;__DEC32_MAX__;9.999999E96DF;__DEC32_EPSILON__;1E-6DF;__DEC32_SUBNORMAL_MIN__;0.000001E-95DF;__DEC64_MANT_DIG__;16;__DEC64_MIN_EXP__;(-382);__DEC64_MAX_EXP__;385;__DEC64_MIN__;1E-383DD;__DEC64_MAX__;9.999999999999999E384DD;__DEC64_EPSILON__;1E-15DD;__DEC64_SUBNORMAL_MIN__;0.000000000000001E-383DD;__DEC128_MANT_DIG__;34;__DEC128_MIN_EXP__;(-6142);__DEC128_MAX_EXP__;6145;__DEC128_MIN__;1E-6143DL;__DEC128_MAX__;9.999999999999999999999999999999999E6144DL;__DEC128_EPSILON__;1E-33DL;__DEC128_SUBNORMAL_MIN__;0.000000000000000000000000000000001E-6143DL;__REGISTER_PREFIX__; ;__USER_LABEL_PREFIX__; ;__GNUC_STDC_INLINE__;1;__NO_INLINE__;1;__GCC_HAVE_SYNC_COMPARE_AND_SWAP_1;1;__GCC_HAVE_SYNC_COMPARE_AND_SWAP_2;1;__GCC_HAVE_SYNC_COMPARE_AND_SWAP_4;1;__GCC_HAVE_SYNC_COMPARE_AND_SWAP_8;1;__GCC_ATOMIC_BOOL_LOCK_FREE;2;__GCC_ATOMIC_CHAR_LOCK_FREE;2;__GCC_ATOMIC_CHAR16_T_LOCK_FREE;2;__GCC_ATOMIC_CHAR32_T_LOCK_FREE;2;__GCC_ATOMIC_WCHAR_T_LOCK_FREE;2;__GCC_ATOMIC_SHORT_LOCK_FREE;2;__GCC_ATOMIC_INT_LOCK_FREE;2;__GCC_ATOMIC_LONG_LOCK_FREE;2;__GCC_ATOMIC_LLONG_LOCK_FREE;2;__GCC_ATOMIC_TEST_AND_SET_TRUEVAL;1;__GCC_ATOMIC_POINTER_LOCK_FREE;2;__HAVE_SPECULATION_SAFE_VALUE;1;__GCC_HAVE_DWARF2_CFI_ASM;1;__PRAGMA_REDEFINE_EXTNAME;1;__SSP_STRONG__;3;__SIZEOF_INT128__;16;__SIZEOF_WCHAR_T__;4;__SIZEOF_WINT_T__;4;__SIZEOF_PTRDIFF_T__;8;__amd64;1;__amd64__;1;__x86_64;1;__x86_64__;1;__SIZEOF_FLOAT80__;16;__SIZEOF_FLOAT128__;16;__ATOMIC_HLE_ACQUIRE;65536;__ATOMIC_HLE_RELEASE;131072;__GCC_ASM_FLAG_OUTPUTS__;1;__k8;1;__k8__;1;__code_model_small__;1;__MMX__;1;__SSE__;1;__SSE2__;1;__FXSR__;1;__SSE_MATH__;1;__SSE2_MATH__;1;__SEG_FS;1;__SEG_GS;1;__CET__;3;__gnu_linux__;1;__linux;1;__linux__;1;linux;1;__unix;1;__unix__;1;unix;1;__ELF__;1;__DECIMAL_BID_FORMAT__;1;_STDC_PREDEF_H;1;__STDC_IEC_559__;1;__STDC_IEC_559_COMPLEX__;1;__STDC_ISO_10646__;201706L
-//C compiler system include directories
-CMAKE_EXTRA_GENERATOR_C_SYSTEM_INCLUDE_DIRS:INTERNAL=/usr/local/cgns/4.1.1_gcc-8.3.0/include;/usr/local/hdf5/1.10.6_gcc-8.3.0/include;/usr/local/szip/2.1.1/include;/usr/lib/gcc/x86_64-linux-gnu/9/include;/usr/local/include;/usr/include/x86_64-linux-gnu;/usr/include
-//Name of generator.
-CMAKE_GENERATOR:INTERNAL=Unix Makefiles
-//Generator instance identifier.
-CMAKE_GENERATOR_INSTANCE:INTERNAL=
-//Name of generator platform.
-CMAKE_GENERATOR_PLATFORM:INTERNAL=
-//Name of generator toolset.
-CMAKE_GENERATOR_TOOLSET:INTERNAL=
-//Source directory with the top level CMakeLists.txt file for this
-// project
-CMAKE_HOME_DIRECTORY:INTERNAL=/home/justin/Thesis/topo code/SAND
-//ADVANCED property for variable: CMAKE_INSTALL_PREFIX
-CMAKE_INSTALL_PREFIX-ADVANCED:INTERNAL=1
-//Install .so files without execute permission.
-CMAKE_INSTALL_SO_NO_EXE:INTERNAL=1
-//ADVANCED property for variable: CMAKE_LINKER
-CMAKE_LINKER-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_MAKE_PROGRAM
-CMAKE_MAKE_PROGRAM-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS
-CMAKE_MODULE_LINKER_FLAGS-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_DEBUG
-CMAKE_MODULE_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL
-CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_RELEASE
-CMAKE_MODULE_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO
-CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_NM
-CMAKE_NM-ADVANCED:INTERNAL=1
-//number of local generators
-CMAKE_NUMBER_OF_MAKEFILES:INTERNAL=1
-//ADVANCED property for variable: CMAKE_OBJCOPY
-CMAKE_OBJCOPY-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_OBJDUMP
-CMAKE_OBJDUMP-ADVANCED:INTERNAL=1
-//Platform information initialized
-CMAKE_PLATFORM_INFO_INITIALIZED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_RANLIB
-CMAKE_RANLIB-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_READELF
-CMAKE_READELF-ADVANCED:INTERNAL=1
-//Path to CMake installation.
-CMAKE_ROOT:INTERNAL=/snap/clion/169/bin/cmake/linux/share/cmake-3.20
-//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS
-CMAKE_SHARED_LINKER_FLAGS-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_DEBUG
-CMAKE_SHARED_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL
-CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_RELEASE
-CMAKE_SHARED_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO
-CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_SKIP_INSTALL_RPATH
-CMAKE_SKIP_INSTALL_RPATH-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_SKIP_RPATH
-CMAKE_SKIP_RPATH-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS
-CMAKE_STATIC_LINKER_FLAGS-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_DEBUG
-CMAKE_STATIC_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_MINSIZEREL
-CMAKE_STATIC_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_RELEASE
-CMAKE_STATIC_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_RELWITHDEBINFO
-CMAKE_STATIC_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_STRIP
-CMAKE_STRIP-ADVANCED:INTERNAL=1
-//uname command
-CMAKE_UNAME:INTERNAL=/usr/bin/uname
-//ADVANCED property for variable: CMAKE_VERBOSE_MAKEFILE
-CMAKE_VERBOSE_MAKEFILE-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: PERL_EXECUTABLE
-PERL_EXECUTABLE-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: ProcessorCount_cmd_nproc
-ProcessorCount_cmd_nproc-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: ProcessorCount_cmd_sysctl
-ProcessorCount_cmd_sysctl-ADVANCED:INTERNAL=1
-USAGE_PRINTED:INTERNAL=TRUE
-

From a2921f846f71c6987e01cff9bc52a808dc2ad629 Mon Sep 17 00:00:00 2001
From: Justin OConnor <64496813+juleoc02@users.noreply.github.com>
Date: Thu, 4 Nov 2021 09:17:52 -0600
Subject: [PATCH 22/95] Delete cmake_install.cmake

---
 cmake_install.cmake | 54 ---------------------------------------------
 1 file changed, 54 deletions(-)
 delete mode 100644 cmake_install.cmake

diff --git a/cmake_install.cmake b/cmake_install.cmake
deleted file mode 100644
index 554a30e..0000000
--- a/cmake_install.cmake
+++ /dev/null
@@ -1,54 +0,0 @@
-# Install script for directory: /home/justin/Thesis/topo code/SAND
-
-# Set the install prefix
-if(NOT DEFINED CMAKE_INSTALL_PREFIX)
-  set(CMAKE_INSTALL_PREFIX "/usr/local")
-endif()
-string(REGEX REPLACE "/$" "" CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}")
-
-# Set the install configuration name.
-if(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME)
-  if(BUILD_TYPE)
-    string(REGEX REPLACE "^[^A-Za-z0-9_]+" ""
-           CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}")
-  else()
-    set(CMAKE_INSTALL_CONFIG_NAME "Release")
-  endif()
-  message(STATUS "Install configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"")
-endif()
-
-# Set the component getting installed.
-if(NOT CMAKE_INSTALL_COMPONENT)
-  if(COMPONENT)
-    message(STATUS "Install component: \"${COMPONENT}\"")
-    set(CMAKE_INSTALL_COMPONENT "${COMPONENT}")
-  else()
-    set(CMAKE_INSTALL_COMPONENT)
-  endif()
-endif()
-
-# Install shared libraries without execute permission?
-if(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE)
-  set(CMAKE_INSTALL_SO_NO_EXE "1")
-endif()
-
-# Is this installation the result of a crosscompile?
-if(NOT DEFINED CMAKE_CROSSCOMPILING)
-  set(CMAKE_CROSSCOMPILING "FALSE")
-endif()
-
-# Set default install directory permissions.
-if(NOT DEFINED CMAKE_OBJDUMP)
-  set(CMAKE_OBJDUMP "/usr/bin/objdump")
-endif()
-
-if(CMAKE_INSTALL_COMPONENT)
-  set(CMAKE_INSTALL_MANIFEST "install_manifest_${CMAKE_INSTALL_COMPONENT}.txt")
-else()
-  set(CMAKE_INSTALL_MANIFEST "install_manifest.txt")
-endif()
-
-string(REPLACE ";" "\n" CMAKE_INSTALL_MANIFEST_CONTENT
-       "${CMAKE_INSTALL_MANIFEST_FILES}")
-file(WRITE "/home/justin/Thesis/topo code/SAND/${CMAKE_INSTALL_MANIFEST}"
-     "${CMAKE_INSTALL_MANIFEST_CONTENT}")

From 0238483726297a12aaa85a770531f9f69fb7869a Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Thu, 4 Nov 2021 11:18:39 -0600
Subject: [PATCH 23/95] Added check to filter matrix

---
 CMakeLists.txt           | 4 ++--
 source/density_filter.cc | 1 +
 2 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6625bfd..b32c19c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -21,9 +21,9 @@ SET(  TARGET_SRC ${TARGET_SRC} ${TARGET_INC}
   
 # Usually, you will not need to modify anything beyond this point...
 
-CMAKE_MINIMUM_REQUIRED(VERSION 2.8.12)
+CMAKE_MINIMUM_REQUIRED(VERSION 3.1.0)
 
-FIND_PACKAGE(deal.II 9.2.0 QUIET
+FIND_PACKAGE(deal.II 10.0.0 QUIET
   HINTS ${deal.II_DIR} ${DEAL_II_DIR} ../ ../../ $ENV{DEAL_II_DIR}
   )
 IF(NOT ${deal.II_FOUND})
diff --git a/source/density_filter.cc b/source/density_filter.cc
index 6b2e780..f148634 100644
--- a/source/density_filter.cc
+++ b/source/density_filter.cc
@@ -69,6 +69,7 @@ namespace SAND {
                 iter->value() = iter->value() / denominator;
             }
         }
+        std::cout << "density nonzero entries" << filter_matrix.n_nonzero_elements() << std::endl;
     }
 
     /*This function finds which neighbors are within a certain radius of the initial cell.*/

From fb7d9a0eb13fb363fcec3295f5bc4a85f0ec6874 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Fri, 5 Nov 2021 12:15:16 -0600
Subject: [PATCH 24/95] Getting rid of errors

---
 include/input_information.h    |  4 ++--
 source/kkt_system.cc           |  3 ---
 source/schur_preconditioner.cc | 33 +++++----------------------------
 3 files changed, 7 insertions(+), 33 deletions(-)

diff --git a/include/input_information.h b/include/input_information.h
index 628ea65..00515e0 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -17,13 +17,13 @@ namespace SAND {
         //geometry options
         constexpr unsigned int geometry_base = GeometryOptions::mbb;
         constexpr unsigned int dim = 2;
-        constexpr unsigned int refinements =  4;
+        constexpr unsigned int refinements =  2;
 
         //nonlinear algorithm options
         constexpr double initial_barrier_size = 25;
         constexpr double min_barrier_size = .00000;
         constexpr double fraction_to_boundary = .9;
-        constexpr unsigned int max_steps=75;
+        constexpr unsigned int max_steps=1;
         constexpr unsigned int barrier_reduction=BarrierOptions::loqo;
         constexpr double required_norm = .0001;
 
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index 8667bcb..2ba4d1b 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -1071,9 +1071,6 @@ namespace SAND {
             constraints.distribute_local_to_global(
                     cell_matrix, cell_rhs, local_dof_indices, system_matrix, system_rhs);
 
-            if (cell->active_cell_index() == 23)
-                cell_matrix.print(std::cout);
-
         }
         system_matrix.compress(VectorOperation::add);
         system_rhs = calculate_rhs(state, barrier_size);
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 4491e48..068a85a 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -371,20 +371,12 @@ namespace SAND {
 
             auto op_k_inv = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
 
-            TrilinosWrappers::PreconditionIdentity preconditioner;
-            preconditioner.initialize(b_mat);
-
             g_d_m_inv_density = op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
             SolverControl step_4_gmres_control_1 (100000, g_d_m_inv_density.l2_norm()*1e-6);
             SolverGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
-            TrilinosWrappers::SolverGMRES step_4_gmres_1_Trilinos (step_4_gmres_control_1);
             try {
-
-//                step_4_gmres_1_Trilinos.solve(op_k_inv, k_g_d_m_inv_density,g_d_m_inv_density,preconditioner);
-                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1_Trilinos, PreconditionIdentity()) *
+                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
                                       g_d_m_inv_density;
-//                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
-//                                      g_d_m_inv_density;
             } catch (std::exception &exc)
             {
                 std::cerr << "Failure of linear solver step_4_gmres_1" << std::endl;
@@ -398,13 +390,9 @@ namespace SAND {
 
             SolverControl step_4_gmres_control_2 (100000, src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6);
             SolverGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
-            LA::SolverGMRES step_4_gmres_2_Trilinos (step_4_gmres_control_2);
             try {
-//                step_4_gmres_2_Trilinos.solve(op_k_inv, k_density_mult,src.block(SolutionBlocks::unfiltered_density_multiplier),preconditioner);
-                k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2_Trilinos, PreconditionIdentity()) *
+                k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, PreconditionIdentity()) *
                                  src.block(SolutionBlocks::unfiltered_density_multiplier);
-//                k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, PreconditionIdentity()) *
-//                                 src.block(SolutionBlocks::unfiltered_density_multiplier);
             } catch (std::exception &exc)
             {
                 std::cerr << "Failure of linear solver step_4_gmres_2" << std::endl;
@@ -550,22 +538,14 @@ namespace SAND {
                 auto pre_pre_k = pre_k;
                 pre_pre_k = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
                 pre_k =  pre_pre_k + src.block(SolutionBlocks::unfiltered_density_multiplier);
-                std::cout << "pre_pre_k norm " << pre_pre_k.l2_norm() << " unfiltered_density_multiplier " << src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()
-                        << " pre_k norm " << pre_k.l2_norm() << std::endl;
-
-
                 TrilinosWrappers::PreconditionIdentity preconditioner;
                 preconditioner.initialize(b_mat);
 
                 SolverControl step_5_gmres_control_1 (100000, pre_j.l2_norm()*1e-6);
                 SolverGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
-                LA::SolverGMRES step_5_gmres_1_Trilinos (step_5_gmres_control_1);
                 try {
-//                    step_5_gmres_1_Trilinos.solve(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv),dst.block(SolutionBlocks::unfiltered_density_multiplier), pre_j, preconditioner);
-                    dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), step_5_gmres_1_Trilinos, PreconditionIdentity()) *
-                                          pre_j;
-//                    dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
-//                                                                               pre_j;
+                    dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
+                                                                               pre_j;
                 } catch (std::exception &exc)
                 {
                     std::cerr << "Failure of linear solver step_5_gmres_1" << std::endl;
@@ -579,11 +559,8 @@ namespace SAND {
 
                 SolverControl step_5_gmres_control_2 (100000, pre_k.l2_norm()*1e-6);
                 SolverGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
-                LA::SolverGMRES step_5_gmres_2_Trilinos (step_5_gmres_control_2);
                 try {
-//                    step_5_gmres_2_Trilinos.solve(op_k_inv,dst.block(SolutionBlocks::density), pre_k, preconditioner);
-                    dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2_Trilinos, PreconditionIdentity()) * pre_k;
-//                    dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) * pre_k;
+                    dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) * pre_k;
                 } catch (std::exception &exc)
                 {
                     std::cerr << "Failure of linear solver step_5_gmres_2" << std::endl;

From c9677442b553dd58d937b3ac69be28db8473d511 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Sun, 7 Nov 2021 10:06:42 -0700
Subject: [PATCH 25/95] Warning Killing

---
 include/kkt_system.h           |   2 +-
 include/sand_tools.h           | 153 ++++++++++++++++++++++++---------
 include/schur_preconditioner.h |   7 +-
 source/kkt_system.cc           |  22 ++---
 source/schur_preconditioner.cc |  91 +++++++++++---------
 source/watchdog_main.cc        |   2 +-
 6 files changed, 173 insertions(+), 104 deletions(-)

diff --git a/include/kkt_system.h b/include/kkt_system.h
index a2015a6..db9aab4 100644
--- a/include/kkt_system.h
+++ b/include/kkt_system.h
@@ -92,7 +92,7 @@ namespace SAND {
         assemble_block_system(const LA::MPI::BlockVector &state, const double barrier_size);
 
         LA::MPI::BlockVector
-        solve(const LA::MPI::BlockVector &state, double barrier_size);
+        solve(const LA::MPI::BlockVector &state);
 
         LA::MPI::BlockVector
         get_initial_state();
diff --git a/include/sand_tools.h b/include/sand_tools.h
index 54ba5f2..4d19664 100644
--- a/include/sand_tools.h
+++ b/include/sand_tools.h
@@ -4,25 +4,79 @@
 
 #ifndef SAND_MY_TOOLS_H
 #define SAND_MY_TOOLS_H
+#include <deal.II/base/quadrature_lib.h>
+#include <deal.II/base/function.h>
+#include <deal.II/base/tensor.h>
+#include <deal.II/base/timer.h>
+
+#include <deal.II/lac/block_vector.h>
+#include <deal.II/lac/full_matrix.h>
+#include <deal.II/lac/block_sparse_matrix.h>
+#include <deal.II/lac/precondition.h>
+#include <deal.II/lac/linear_operator.h>
+#include <deal.II/lac/linear_operator_tools.h>
+#include <deal.II/lac/packaged_operation.h>
+#include <deal.II/lac/sparse_direct.h>
+#include <deal.II/lac/solver_gmres.h>
+#include <deal.II/lac/solver_cg.h>
+#include <deal.II/lac/solver_bicgstab.h>
+#include <deal.II/lac/generic_linear_algebra.h>
+#include <deal.II/lac/matrix_out.h>
+#include <deal.II/lac/trilinos_sparse_matrix.h>
+#include <deal.II/lac/trilinos_block_sparse_matrix.h>
+
+#include <deal.II/grid/tria.h>
+#include <deal.II/grid/grid_generator.h>
+#include <deal.II/grid/grid_refinement.h>
+
+#include <deal.II/dofs/dof_handler.h>
+#include <deal.II/dofs/dof_renumbering.h>
+#include <deal.II/dofs/dof_tools.h>
+
+#include <deal.II/fe/fe_values.h>
+#include <deal.II/fe/fe_dgq.h>
+#include <deal.II/fe/fe_system.h>
+#include <deal.II/fe/fe_q.h>
+
+#include <deal.II/numerics/vector_tools.h>
+#include <deal.II/numerics/matrix_tools.h>
+#include <deal.II/numerics/data_out.h>
+#include <deal.II/numerics/error_estimator.h>
+#include <deal.II/base/config.h>
+#include <deal.II/base/numbers.h>
+#include <deal.II/base/table.h>
+#include <deal.II/base/tensor.h>
+#include <deal.II/base/timer.h>
+
+#include <deal.II/differentiation/ad/ad_number_traits.h>
+
+#include <deal.II/lac/exceptions.h>
+#include <deal.II/lac/identity_matrix.h>
+
 
 
 namespace SAND{
+    namespace LA
+    {
+        using namespace dealii::LinearAlgebraTrilinos;
+    }
+
     using namespace dealii;
 
-    void build_matrix_element_by_element (const auto &X,
-                                          FullMatrix<double>   &X_matrix)
+
+    void build_matrix_element_by_element (const LinearOperator<LA::MPI::Vector,LA::MPI::Vector,dealii::TrilinosWrappers::internal::LinearOperatorImplementation::TrilinosPayload> &op_X,
+                                          FullMatrix<double>   &X_matrix,
+                                          LA::MPI::Vector &exemplar_vector)
     {
-        auto op_X = linear_operator(X);
         Threads::TaskGroup<void> tasks;
         for (unsigned int j=0; j<X_matrix.n(); ++j)
-            tasks += Threads::new_task ([&op_X, &X_matrix, j]()
+            tasks += Threads::new_task ([&op_X, &X_matrix, &exemplar_vector, j]()
                                         {
-                                            Vector<double> e_j (X_matrix.m());
-                                            Vector<double> r_j (X_matrix.n());
+                                            LA::MPI::Vector e_j (exemplar_vector);
+                                            LA::MPI::Vector r_j (exemplar_vector);
 
                                             e_j = 0;
                                             e_j(j) = 1;
-
                                             r_j = op_X * e_j;
 
                                             for (unsigned int i=0; i<X_matrix.m(); ++i)
@@ -32,41 +86,60 @@ namespace SAND{
         tasks.join_all();
     }
 
-//    void print_matrix (std::string &name, FullMatrix<double>   &X_matrix)
-//    {
-////        const unsigned int n = X_matrix.n();
-////        const unsigned int m = X_matrix.m();
-////        std::ofstream Xmat(name);
-////        for (unsigned int i = 0; i < m; i++)
-////        {
-////            Xmat << X_matrix(i, 0);
-////            for (unsigned int j = 1; j < n; j++)
-////            {
-////                Xmat << "," << X_matrix(i, j);
-////            }
-////            Xmat << "\n";
-////        }
-////        Xmat.close();
-//    }
-
-//    void print_matrix (std::string &name, SparseMatrix<double>   &X_matrix)
-//    {
-////        const unsigned int n = X_matrix.n();
-////        const unsigned int m = X_matrix.m();
-////        std::ofstream Xmat(name);
-////        for (unsigned int i = 0; i < m; i++)
-////        {
-////            Xmat << X_matrix.el(i, 0);
-////            for (unsigned int j = 1; j < n; j++)
-////            {
-////                Xmat << "," << X_matrix.el(i, j);
-////            }
-////            Xmat << "\n";
-////        }
-////        Xmat.close();
-//    }
+    void print_matrix (std::string name, FullMatrix<double>   &X_matrix)
+    {
+        const unsigned int n = X_matrix.n();
+        const unsigned int m = X_matrix.m();
+        std::ofstream Xmat(name);
+        for (unsigned int i = 0; i < m; i++)
+        {
+            Xmat << X_matrix(i, 0);
+            for (unsigned int j = 1; j < n; j++)
+            {
+                Xmat << "," << X_matrix(i, j);
+            }
+            Xmat << "\n";
+        }
+        Xmat.close();
+    }
+
+    void print_matrix (std::string &name, SparseMatrix<double>   &X_matrix)
+    {
+        const unsigned int n = X_matrix.n();
+        const unsigned int m = X_matrix.m();
+        std::ofstream Xmat(name);
+        for (unsigned int i = 0; i < m; i++)
+        {
+            Xmat << X_matrix.el(i, 0);
+            for (unsigned int j = 1; j < n; j++)
+            {
+                Xmat << "," << X_matrix.el(i, j);
+            }
+            Xmat << "\n";
+        }
+        Xmat.close();
+    }
+
+    void print_matrix (std::string &name, LA::MPI::SparseMatrix &X_matrix)
+    {
+        const unsigned int n = X_matrix.n();
+        const unsigned int m = X_matrix.m();
+        std::ofstream Xmat(name);
+        for (unsigned int i = 0; i < m; i++)
+        {
+            Xmat << X_matrix.el(i, 0);
+            for (unsigned int j = 1; j < n; j++)
+            {
+                Xmat << "," << X_matrix.el(i, j);
+            }
+            Xmat << "\n";
+        }
+        Xmat.close();
+    }
 
 }
 
 
+
+
 #endif //SAND_MY_TOOLS_H
diff --git a/include/schur_preconditioner.h b/include/schur_preconditioner.h
index 2da1542..c4d1749 100644
--- a/include/schur_preconditioner.h
+++ b/include/schur_preconditioner.h
@@ -76,8 +76,7 @@ namespace SAND
     class VmultTrilinosSolverDirect : public TrilinosWrappers::SparseMatrix {
         public:
             VmultTrilinosSolverDirect(SolverControl &cn,
-                                      const TrilinosWrappers::SolverDirect::AdditionalData &data,
-                                      LA::MPI::SparseMatrix &a_mat
+                                      const TrilinosWrappers::SolverDirect::AdditionalData &data
                                       );
             void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
             void vmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const;
@@ -99,7 +98,7 @@ namespace SAND
     class TopOptSchurPreconditioner: public Subscriptor {
     public:
         TopOptSchurPreconditioner(LA::MPI::BlockSparseMatrix &matrix_in);
-        void initialize (LA::MPI::BlockSparseMatrix &matrix, const std::map<types::global_dof_index, double> &boundary_values, const DoFHandler<dim> &dof_handler, const double barrier_size, const LA::MPI::BlockVector &state);
+        void initialize (LA::MPI::BlockSparseMatrix &matrix, const std::map<types::global_dof_index, double> &boundary_values, const DoFHandler<dim> &dof_handler, const LA::MPI::BlockVector &state);
         void vmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
         void Tvmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
         void vmult_add(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
@@ -111,7 +110,7 @@ namespace SAND
 
         void assemble_mass_matrix(const LA::MPI::BlockVector &state, const hp::FECollection<dim> &fe_system, const DoFHandler<dim> &dof_handler, const AffineConstraints<double> &constraints,   const BlockSparsityPattern &bsp);
 
-        void print_stuff(const LA::MPI::BlockSparseMatrix &matrix);
+        void print_stuff();
 
         LA::MPI::BlockSparseMatrix &system_matrix;
 
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index 2ba4d1b..ed5a586 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -7,21 +7,16 @@
 #include <deal.II/base/tensor.h>
 #include <deal.II/base/timer.h>
 
-#include <deal.II/lac/block_vector.h>
 #include <deal.II/lac/full_matrix.h>
-#include <deal.II/lac/block_sparse_matrix.h>
-#include <deal.II/lac/precondition.h>
 #include <deal.II/lac/linear_operator.h>
 #include <deal.II/lac/solver_gmres.h>
 #include <deal.II/lac/generic_linear_algebra.h>
 #include <deal.II/lac/trilinos_sparse_matrix.h>
-#include <deal.II/lac/trilinos_block_sparse_matrix.h>
 
 
 #include <deal.II/lac/matrix_out.h>
 
 #include <deal.II/grid/grid_generator.h>
-#include <deal.II/grid/grid_refinement.h>
 
 #include <deal.II/dofs/dof_renumbering.h>
 #include <deal.II/dofs/dof_tools.h>
@@ -40,12 +35,7 @@
 #include <deal.II/numerics/data_out.h>
 #include <deal.II/numerics/error_estimator.h>
 
-#include <deal.II/distributed/tria.h>
-#include <deal.II/distributed/grid_refinement.h>
-
-
 #include "../include/input_information.h"
-#include "../include/sand_tools.h"
 
 #include <iostream>
 #include <algorithm>
@@ -690,7 +680,7 @@ namespace SAND {
                 dsp.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).add(0, i);
             }
 
-            /*finds neighbors whose values would be relevant, and adds them to the sparsity pattern of the matrix*/
+            /*This finds neighbors whose values would be relevant, and adds them to the sparsity pattern of the matrix*/
             for (const auto &cell: triangulation.active_cell_iterators()) {
                 const unsigned int i = cell->active_cell_index();
                 for (const auto &neighbor_cell: density_filter.find_relevant_neighbors(cell))
@@ -1611,7 +1601,7 @@ namespace SAND {
     ///A  direct solver, for now. The complexity of the system means that an iterative solver algorithm will take some more work in the future.
     template<int dim>
     LA::MPI::BlockVector
-    KktSystem<dim>::solve(const LA::MPI::BlockVector &state, double barrier_size) {
+    KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
         double gmres_tolerance;
         if (Input::use_eisenstat_walker) {
             gmres_tolerance = std::max(
@@ -1639,21 +1629,21 @@ namespace SAND {
                 break;
             }
             case SolverOptions::exact_preconditioner_with_gmres: {
-                preconditioner.initialize(system_matrix, boundary_values, dof_handler, barrier_size, state);
+                preconditioner.initialize(system_matrix, boundary_values, dof_handler, state);
                 SolverFGMRES<LA::MPI::BlockVector> A_fgmres(solver_control);
                 A_fgmres.solve(system_matrix, linear_solution, system_rhs, preconditioner);
                 std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
                 break;
             }
             case SolverOptions::inexact_K_with_exact_A_gmres: {
-                preconditioner.initialize(system_matrix, boundary_values, dof_handler, barrier_size, state);
+                preconditioner.initialize(system_matrix, boundary_values, dof_handler, state);
                 SolverFGMRES<LA::MPI::BlockVector> B_fgmres(solver_control);
                 B_fgmres.solve(system_matrix, linear_solution, system_rhs, preconditioner);
                 std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
                 break;
             }
             case SolverOptions::inexact_K_with_inexact_A_gmres: {
-                preconditioner.initialize(system_matrix, boundary_values, dof_handler, barrier_size, state);
+                preconditioner.initialize(system_matrix, boundary_values, dof_handler, state);
                 SolverFGMRES<LA::MPI::BlockVector> C_fgmres(solver_control);
                 C_fgmres.solve(system_matrix, linear_solution, system_rhs, preconditioner);
                 std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
@@ -1665,7 +1655,7 @@ namespace SAND {
         constraints.distribute(linear_solution);
 
         if (Input::output_parts_of_matrix) {
-            preconditioner.print_stuff(system_matrix);
+            preconditioner.print_stuff();
         }
 
         if (Input::output_full_preconditioned_matrix) {
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 068a85a..9a59b5c 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -29,7 +29,6 @@ namespace SAND {
             n_block_rows(0),
             n_block_columns(0),
             other_solver_control(100000, 1e-6),
-            direct_solver_control(1, 0),
             other_bicgstab(other_solver_control),
             other_gmres(other_solver_control),
             other_cg(other_solver_control),
@@ -42,17 +41,17 @@ namespace SAND {
             d_1_mat(matrix_in.block(SolutionBlocks::density_lower_slack, SolutionBlocks::density_lower_slack)),
             d_2_mat(matrix_in.block(SolutionBlocks::density_upper_slack, SolutionBlocks::density_upper_slack)),
             m_vect(matrix_in.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier)),
-            timer(std::cout, TimerOutput::summary,
-                  TimerOutput::wall_times),
             solver_type("Amesos_Klu"),
             additional_data(false, solver_type),
-            a_inv_direct(direct_solver_control, additional_data, a_mat)
+            direct_solver_control(1, 0),
+            a_inv_direct(direct_solver_control, additional_data),
+            timer(std::cout, TimerOutput::summary, TimerOutput::wall_times)
     {
 
     }
 
     template<int dim>
-    void TopOptSchurPreconditioner<dim>::initialize(LA::MPI::BlockSparseMatrix &matrix, const std::map<types::global_dof_index, double> &boundary_values,const DoFHandler<dim> &dof_handler, const double barrier_size, const LA::MPI::BlockVector &state)
+    void TopOptSchurPreconditioner<dim>::initialize(LA::MPI::BlockSparseMatrix &matrix, const std::map<types::global_dof_index, double> &boundary_values,const DoFHandler<dim> &dof_handler, const LA::MPI::BlockVector &state)
     {
         TimerOutput::Scope t(timer, "initialize");
         {
@@ -362,16 +361,26 @@ namespace SAND {
 
         else if (Input::solver_choice == SolverOptions::inexact_K_with_exact_A_gmres)
         {
-            auto op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) *
-                    transpose_operator<VectorType, VectorType, PayloadType>(f_mat);
+            auto op_d_8 = linear_operator<VectorType,VectorType,PayloadType>(d_8_mat);
+            auto op_f = linear_operator<VectorType,VectorType,PayloadType>(f_mat);
+            auto op_b = linear_operator<VectorType,VectorType,PayloadType>(b_mat);
+            auto op_c = linear_operator<VectorType,VectorType,PayloadType>(c_mat);
+            auto op_a_inv = linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct);
+            auto op_e = linear_operator<VectorType,VectorType,PayloadType>(e_mat);
+            auto op_d_m= linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
+            auto op_d_m_inv= linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat);
 
-            auto op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
-                   - transpose_operator<VectorType, VectorType, PayloadType>(c_mat) * linear_operator<VectorType, VectorType, PayloadType>(a_inv_direct) * linear_operator<VectorType,VectorType,PayloadType>(e_mat)
-                   - transpose_operator<VectorType, VectorType, PayloadType>(e_mat) * linear_operator<VectorType, VectorType, PayloadType>(a_inv_direct) * linear_operator<VectorType,VectorType,PayloadType>(c_mat);
+            auto op_g = op_f * op_d_8 *
+                    transpose_operator(op_f);
 
-            auto op_k_inv = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
 
-            g_d_m_inv_density = op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
+            auto op_h = op_b
+                   - transpose_operator(op_c) * op_a_inv * op_e
+                   - transpose_operator(op_e) * op_a_inv * op_c;
+
+            auto op_k_inv = -1 * op_g *op_d_m_inv * op_h - op_d_m;
+
+            g_d_m_inv_density = op_g * op_d_m_inv * src.block(SolutionBlocks::density);
             SolverControl step_4_gmres_control_1 (100000, g_d_m_inv_density.l2_norm()*1e-6);
             SolverGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
             try {
@@ -538,8 +547,7 @@ namespace SAND {
                 auto pre_pre_k = pre_k;
                 pre_pre_k = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
                 pre_k =  pre_pre_k + src.block(SolutionBlocks::unfiltered_density_multiplier);
-                TrilinosWrappers::PreconditionIdentity preconditioner;
-                preconditioner.initialize(b_mat);
+
 
                 SolverControl step_5_gmres_control_1 (100000, pre_j.l2_norm()*1e-6);
                 SolverGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
@@ -779,34 +787,34 @@ namespace SAND {
 
 
     template<int dim>
-    void TopOptSchurPreconditioner<dim>::print_stuff(const LA::MPI::BlockSparseMatrix &matrix)
+    void TopOptSchurPreconditioner<dim>::print_stuff()
     {
 
-//        print_matrix("OAmat.csv",a_mat);
-//        print_matrix("OBmat.csv",b_mat);
-//        print_matrix("OCmat.csv",c_mat);
-//        print_matrix("OEmat.csv",e_mat);
-//        print_matrix("OFmat.csv",f_mat);
-//        FullMatrix<double> g_mat;
-//        FullMatrix<double> h_mat;
-//        FullMatrix<double> k_inv_mat;
-//        g_mat.reinit(b_mat.m(),b_mat.n());
-//        h_mat.reinit(b_mat.m(),b_mat.n());
-//        k_inv_mat.reinit(b_mat.m(),b_mat.n());
-//        auto op_g = linear_operator(f_mat) * linear_operator(d_8_mat) *
-//                    transpose_operator(linear_operator(f_mat));
-//
-//        auto op_h = linear_operator(b_mat)
-//                    - transpose_operator(linear_operator(c_mat)) * linear_operator(a_inv_direct) * linear_operator(e_mat)
-//                    - transpose_operator(linear_operator(e_mat)) * linear_operator(a_inv_direct) * linear_operator(c_mat);
-//
-//        auto op_k_inv = -1 * op_g * linear_operator(d_m_inv_mat) * op_h - linear_operator(d_m_mat);
-//        build_matrix_element_by_element(op_g,g_mat);
-//        build_matrix_element_by_element(op_h,h_mat);
-//        build_matrix_element_by_element(op_k_inv,k_inv_mat);
-//        print_matrix("OGmat.csv",g_mat);
-//        print_matrix("OHmat.csv",h_mat);
-//        print_matrix("OKinvmat.csv",k_inv_mat);
+//        print_matrix(std::string("OAmat.csv"),a_mat);
+//        print_matrix(std::string("OBmat.csv"),b_mat);
+//        print_matrix(std::string("OCmat.csv"),c_mat);
+//        print_matrix(std::string("OEmat.csv"),e_mat);
+//        print_matrix(std::string("OFmat.csv"),f_mat);
+        FullMatrix<double> g_mat;
+        FullMatrix<double> h_mat;
+        FullMatrix<double> k_inv_mat;
+        g_mat.reinit(b_mat.m(),b_mat.n());
+        h_mat.reinit(b_mat.m(),b_mat.n());
+        k_inv_mat.reinit(b_mat.m(),b_mat.n());
+        auto op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) *
+                    transpose_operator(linear_operator<VectorType,VectorType,PayloadType>(f_mat));
+
+        auto op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
+                    - transpose_operator(linear_operator<VectorType,VectorType,PayloadType>(c_mat)) * linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct) * linear_operator<VectorType,VectorType,PayloadType>(e_mat)
+                    - transpose_operator(linear_operator<VectorType,VectorType,PayloadType>(e_mat)) * linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct) * linear_operator<VectorType,VectorType,PayloadType>(c_mat);
+
+        auto op_k_inv = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
+//        build_matrix_element_by_element(op_g,g_mat, src.block(SolutionBlocks::density));
+//        build_matrix_element_by_element(op_h,h_mat, src.block(SolutionBlocks::density));
+//        build_matrix_element_by_element(op_k_inv,k_inv_mat, src.block(SolutionBlocks::density));
+//        print_matrix(std::string("OGmat.csv"),g_mat);
+//        print_matrix(std::string("OHmat.csv"),h_mat);
+//        print_matrix(std::string("OKinvmat.csv"),k_inv_mat);
 
 
     }
@@ -842,8 +850,7 @@ namespace SAND {
 
 
     VmultTrilinosSolverDirect::VmultTrilinosSolverDirect(SolverControl &cn,
-                     const TrilinosWrappers::SolverDirect::AdditionalData &data,
-                     LA::MPI::SparseMatrix &a_mat)
+                     const TrilinosWrappers::SolverDirect::AdditionalData &data)
     : solver_direct(cn, data)
     {
 
diff --git a/source/watchdog_main.cc b/source/watchdog_main.cc
index e51d25e..5b58cb0 100644
--- a/source/watchdog_main.cc
+++ b/source/watchdog_main.cc
@@ -111,7 +111,7 @@ namespace SAND {
     SANDTopOpt<dim>::find_max_step(const LA::MPI::BlockVector &state)
     {
         kkt_system.assemble_block_system(state, barrier_size);
-        const LA::MPI::BlockVector step = kkt_system.solve(state,barrier_size);
+        const LA::MPI::BlockVector step = kkt_system.solve(state);
 
         const auto max_step_sizes= calculate_max_step_size(state,step);
         const double step_size_s = max_step_sizes.first;

From 59ea6a1398acfef18cbc6188e14a65ebfec00dab Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Thu, 11 Nov 2021 16:54:14 -0700
Subject: [PATCH 26/95] RHS calculated

---
 include/kkt_system.h           |    7 +-
 source/kkt_system.cc           | 1188 +++++++++++++++++---------------
 source/schur_preconditioner.cc |   13 +-
 3 files changed, 631 insertions(+), 577 deletions(-)

diff --git a/include/kkt_system.h b/include/kkt_system.h
index db9aab4..389d4a2 100644
--- a/include/kkt_system.h
+++ b/include/kkt_system.h
@@ -128,8 +128,9 @@ namespace SAND {
 
         BlockDynamicSparsityPattern dsp;
         BlockSparsityPattern sparsity_pattern;
-        LA::MPI::BlockSparseMatrix system_matrix;
-        LA::MPI::BlockVector linear_solution;
+        mutable LA::MPI::BlockSparseMatrix system_matrix;
+        mutable LA::MPI::BlockVector locally_relevant_solution;
+        mutable LA::MPI::BlockVector distributed_solution;
         LA::MPI::BlockVector system_rhs;
         parallel::distributed::Triangulation<dim> triangulation;
         DoFHandler<dim> dof_handler;
@@ -140,7 +141,7 @@ namespace SAND {
         const double density_ratio;
         const double density_penalty_exponent;
 
-        DensityFilter<dim> density_filter;
+        mutable DensityFilter<dim> density_filter;
 
         std::map<types::global_dof_index, double> boundary_values;
 
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index ed5a586..6ca50ca 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -109,30 +109,33 @@ namespace SAND {
 
                 /*Set BCIDs   */
                 for (const auto &cell: dof_handler.active_cell_iterators()) {
-                    cell->set_active_fe_index(0);
-                    cell->set_material_id(MaterialIds::without_multiplier);
-                    for (unsigned int face_number = 0;
-                         face_number < GeometryInfo<dim>::faces_per_cell;
-                         ++face_number) {
-                        if (cell->face(face_number)->at_boundary()) {
-                            const auto center = cell->face(face_number)->center();
-
-                            if (std::fabs(center(1) - downforce_y) < 1e-12) {
-                                if (std::fabs(center(0) - downforce_x) < downforce_size) {
-                                    cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
-                                } else {
-                                    cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
+                    if(cell->is_locally_owned())
+                    {
+                        cell->set_active_fe_index(0);
+                        cell->set_material_id(MaterialIds::without_multiplier);
+                        for (unsigned int face_number = 0;
+                             face_number < GeometryInfo<dim>::faces_per_cell;
+                             ++face_number) {
+                            if (cell->face(face_number)->at_boundary()) {
+                                const auto center = cell->face(face_number)->center();
+
+                                if (std::fabs(center(1) - downforce_y) < 1e-12) {
+                                    if (std::fabs(center(0) - downforce_x) < downforce_size) {
+                                        cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
+                                    } else {
+                                        cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
+                                    }
                                 }
                             }
                         }
-                    }
-                    for (unsigned int vertex_number = 0;
-                         vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                         ++vertex_number) {
-                        if (std::abs(cell->vertex(vertex_number)(0)) + std::abs(cell->vertex(vertex_number)(1)) <
-                            1e-10) {
-                            cell->set_active_fe_index(1);
-                            cell->set_material_id(MaterialIds::with_multiplier);
+                        for (unsigned int vertex_number = 0;
+                             vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                             ++vertex_number) {
+                            if (std::abs(cell->vertex(vertex_number)(0)) + std::abs(cell->vertex(vertex_number)(1)) <
+                                1e-10) {
+                                cell->set_active_fe_index(1);
+                                cell->set_material_id(MaterialIds::with_multiplier);
+                            }
                         }
                     }
                 }
@@ -149,30 +152,33 @@ namespace SAND {
                 triangulation.refine_global(Input::refinements);
 
                 for (const auto &cell: dof_handler.active_cell_iterators()) {
-                    cell->set_active_fe_index(0);
-                    cell->set_material_id(MaterialIds::without_multiplier);
-                    for (unsigned int face_number = 0;
-                         face_number < GeometryInfo<dim>::faces_per_cell;
-                         ++face_number) {
-                        if (cell->face(face_number)->at_boundary()) {
-                            const auto center = cell->face(face_number)->center();
-
-                            if (std::fabs(center(1) - downforce_y) < 1e-12) {
-                                if (std::fabs(center(0) - downforce_x) < downforce_size) {
-                                    cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
-                                } else {
-                                    cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
+                    if (cell->is_locally_owned())
+                    {
+                        cell->set_active_fe_index(0);
+                        cell->set_material_id(MaterialIds::without_multiplier);
+                        for (unsigned int face_number = 0;
+                             face_number < GeometryInfo<dim>::faces_per_cell;
+                             ++face_number) {
+                            if (cell->face(face_number)->at_boundary()) {
+                                const auto center = cell->face(face_number)->center();
+
+                                if (std::fabs(center(1) - downforce_y) < 1e-12) {
+                                    if (std::fabs(center(0) - downforce_x) < downforce_size) {
+                                        cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
+                                    } else {
+                                        cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
+                                    }
                                 }
                             }
                         }
-                    }
-                    for (unsigned int vertex_number = 0;
-                         vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                         ++vertex_number) {
-                        if (std::abs(cell->vertex(vertex_number)(0)) + std::abs(cell->vertex(vertex_number)(1))
-                            + std::abs(cell->vertex(vertex_number)(2)) < 1e-10) {
-                            cell->set_active_fe_index(1);
-                            cell->set_material_id(MaterialIds::with_multiplier);
+                        for (unsigned int vertex_number = 0;
+                             vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                             ++vertex_number) {
+                            if (std::abs(cell->vertex(vertex_number)(0)) + std::abs(cell->vertex(vertex_number)(1))
+                                + std::abs(cell->vertex(vertex_number)(2)) < 1e-10) {
+                                cell->set_active_fe_index(1);
+                                cell->set_material_id(MaterialIds::with_multiplier);
+                            }
                         }
                     }
                 }
@@ -207,30 +213,33 @@ namespace SAND {
 
                 /*Set BCIDs   */
                 for (const auto &cell: dof_handler.active_cell_iterators()) {
-                    cell->set_active_fe_index(0);
-                    cell->set_material_id(MaterialIds::without_multiplier);
-                    for (unsigned int face_number = 0;
-                         face_number < GeometryInfo<dim>::faces_per_cell;
-                         ++face_number) {
-                        if (cell->face(face_number)->at_boundary()) {
-                            const auto center = cell->face(face_number)->center();
-
-                            if (std::fabs(center(0) - downforce_x) < 1e-12) {
-                                if (std::fabs(center(1) - downforce_y) < downforce_size) {
-                                    cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
-                                } else {
-                                    cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
+                    if (cell->is_locally_owned())
+                    {
+                        cell->set_active_fe_index(0);
+                        cell->set_material_id(MaterialIds::without_multiplier);
+                        for (unsigned int face_number = 0;
+                             face_number < GeometryInfo<dim>::faces_per_cell;
+                             ++face_number) {
+                            if (cell->face(face_number)->at_boundary()) {
+                                const auto center = cell->face(face_number)->center();
+
+                                if (std::fabs(center(0) - downforce_x) < 1e-12) {
+                                    if (std::fabs(center(1) - downforce_y) < downforce_size) {
+                                        cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
+                                    } else {
+                                        cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
+                                    }
                                 }
                             }
                         }
-                    }
-                    for (unsigned int vertex_number = 0;
-                         vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                         ++vertex_number) {
-                        if (std::abs(cell->vertex(vertex_number)(0)) + std::abs(cell->vertex(vertex_number)(1)) <
-                            1e-10) {
-                            cell->set_active_fe_index(1);
-                            cell->set_material_id(MaterialIds::with_multiplier);
+                        for (unsigned int vertex_number = 0;
+                             vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                             ++vertex_number) {
+                            if (std::abs(cell->vertex(vertex_number)(0)) + std::abs(cell->vertex(vertex_number)(1)) <
+                                1e-10) {
+                                cell->set_active_fe_index(1);
+                                cell->set_material_id(MaterialIds::with_multiplier);
+                            }
                         }
                     }
                 }
@@ -250,35 +259,38 @@ namespace SAND {
 
                 /*Set BCIDs   */
                 for (const auto &cell: dof_handler.active_cell_iterators()) {
-                    cell->set_active_fe_index(0);
-                    cell->set_material_id(MaterialIds::without_multiplier);
-                    for (unsigned int face_number = 0;
-                         face_number < GeometryInfo<dim>::faces_per_cell;
-                         ++face_number) {
-                        if (cell->face(face_number)->at_boundary()) {
-                            const auto center = cell->face(face_number)->center();
-
-                            if (std::fabs(center(0) - downforce_x) < 1e-12) {
-                                if (std::fabs(center(1) - downforce_y) < downforce_size) {
-                                    cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
-                                    if (std::fabs(center(2) - downforce_z) < downforce_size) {
+                    if(cell->is_locally_owned())
+                    {
+                        cell->set_active_fe_index(0);
+                        cell->set_material_id(MaterialIds::without_multiplier);
+                        for (unsigned int face_number = 0;
+                             face_number < GeometryInfo<dim>::faces_per_cell;
+                             ++face_number) {
+                            if (cell->face(face_number)->at_boundary()) {
+                                const auto center = cell->face(face_number)->center();
+
+                                if (std::fabs(center(0) - downforce_x) < 1e-12) {
+                                    if (std::fabs(center(1) - downforce_y) < downforce_size) {
                                         cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
+                                        if (std::fabs(center(2) - downforce_z) < downforce_size) {
+                                            cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
+                                        } else {
+                                            cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
+                                        }
                                     } else {
                                         cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
                                     }
-                                } else {
-                                    cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
                                 }
                             }
                         }
-                    }
-                    for (unsigned int vertex_number = 0;
-                         vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                         ++vertex_number) {
-                        if (std::abs(cell->vertex(vertex_number)(0)) + std::abs(cell->vertex(vertex_number)(1)) <
-                            1e-10) {
-                            cell->set_active_fe_index(1);
-                            cell->set_material_id(MaterialIds::with_multiplier);
+                        for (unsigned int vertex_number = 0;
+                             vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                             ++vertex_number) {
+                            if (std::abs(cell->vertex(vertex_number)(0)) + std::abs(cell->vertex(vertex_number)(1)) <
+                                1e-10) {
+                                cell->set_active_fe_index(1);
+                                cell->set_material_id(MaterialIds::with_multiplier);
+                            }
                         }
                     }
                 }
@@ -304,113 +316,119 @@ namespace SAND {
         if (Input::geometry_base == GeometryOptions::mbb) {
             if (dim == 2) {
                 for (const auto &cell: dof_handler.active_cell_iterators()) {
-
-                    for (unsigned int face_number = 0;
-                         face_number < GeometryInfo<dim>::faces_per_cell;
-                         ++face_number) {
-                        if (cell->face(face_number)->at_boundary()) {
-                            for (unsigned int vertex_number = 0;
-                                 vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                                 ++vertex_number) {
-                                const auto vert = cell->vertex(vertex_number);
-                                /*Find bottom left corner*/
-                                if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
-                                        vert(1) - 0) < 1e-12) {
-
-                                    const unsigned int x_displacement =
-                                            cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
-                                    const unsigned int y_displacement =
-                                            cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
-                                    const unsigned int x_displacement_multiplier =
-                                            cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
-                                    const unsigned int y_displacement_multiplier =
-                                            cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
-                                    /*set bottom left BC*/
-                                    boundary_values[x_displacement] = 0;
-                                    boundary_values[y_displacement] = 0;
-                                    boundary_values[x_displacement_multiplier] = 0;
-                                    boundary_values[y_displacement_multiplier] = 0;
-                                }
-                                /*Find bottom right corner*/
-                                if (std::fabs(vert(0) - 6) < 1e-12 && std::fabs(
-                                        vert(1) - 0) < 1e-12) {
+                    if(cell->is_locally_owned())
+                    {
+                        for (unsigned int face_number = 0;
+                             face_number < GeometryInfo<dim>::faces_per_cell;
+                             ++face_number) {
+                            if (cell->face(face_number)->at_boundary()) {
+                                for (unsigned int vertex_number = 0;
+                                     vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                                     ++vertex_number) {
+                                    const auto vert = cell->vertex(vertex_number);
+                                    /*Find bottom left corner*/
+                                    if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
+                                            vert(1) - 0) < 1e-12) {
+
+                                        const unsigned int x_displacement =
+                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
+                                        const unsigned int y_displacement =
+                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
+                                        const unsigned int x_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
+                                        const unsigned int y_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
+                                        /*set bottom left BC*/
+                                        boundary_values[x_displacement] = 0;
+                                        boundary_values[y_displacement] = 0;
+                                        boundary_values[x_displacement_multiplier] = 0;
+                                        boundary_values[y_displacement_multiplier] = 0;
+                                    }
+                                    /*Find bottom right corner*/
+                                    if (std::fabs(vert(0) - 6) < 1e-12 && std::fabs(
+                                            vert(1) - 0) < 1e-12) {
 //                            const unsigned int x_displacement =
 //                                    cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
-                                    const unsigned int y_displacement =
-                                            cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
+                                        const unsigned int y_displacement =
+                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
 //                            const unsigned int x_displacement_multiplier =
 //                                    cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
-                                    const unsigned int y_displacement_multiplier =
-                                            cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
+                                        const unsigned int y_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
 //                            boundary_values[x_displacement] = 0;
-                                    boundary_values[y_displacement] = 0;
+                                        boundary_values[y_displacement] = 0;
 //                            boundary_values[x_displacement_multiplier] = 0;
-                                    boundary_values[y_displacement_multiplier] = 0;
+                                        boundary_values[y_displacement_multiplier] = 0;
+                                    }
                                 }
                             }
                         }
                     }
+
                 }
             } else if (dim == 3) {
                 for (const auto &cell: dof_handler.active_cell_iterators()) {
-                    for (unsigned int face_number = 0;
-                         face_number < GeometryInfo<dim>::faces_per_cell;
-                         ++face_number) {
-                        if (cell->face(face_number)->at_boundary()) {
-                            for (unsigned int vertex_number = 0;
-                                 vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                                 ++vertex_number) {
-                                const auto vert = cell->vertex(vertex_number);
-                                /*Find bottom left corner*/
-                                if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
-                                        vert(1) - 0) < 1e-12 && ((std::fabs(
-                                        vert(2) - 0) < 1e-12) || (std::fabs(
-                                        vert(2) - 1) < 1e-12))) {
-
-
-                                    const unsigned int x_displacement =
-                                            cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
-                                    const unsigned int y_displacement =
-                                            cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
-                                    const unsigned int z_displacement =
-                                            cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
-                                    const unsigned int x_displacement_multiplier =
-                                            cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
-                                    const unsigned int y_displacement_multiplier =
-                                            cell->vertex_dof_index(vertex_number, 4, cell->active_fe_index());
-                                    const unsigned int z_displacement_multiplier =
-                                            cell->vertex_dof_index(vertex_number, 5, cell->active_fe_index());
-
-                                    boundary_values[x_displacement] = 0;
-                                    boundary_values[y_displacement] = 0;
-                                    boundary_values[z_displacement] = 0;
-                                    boundary_values[x_displacement_multiplier] = 0;
-                                    boundary_values[y_displacement_multiplier] = 0;
-                                    boundary_values[z_displacement_multiplier] = 0;
-                                }
-                                /*Find bottom right corner*/
-                                if (std::fabs(vert(0) - 6) < 1e-12 && std::fabs(
-                                        vert(1) - 0) < 1e-12 && ((std::fabs(
-                                        vert(2) - 0) < 1e-12) || (std::fabs(
-                                        vert(2) - 1) < 1e-12))) {
+                    if(cell->is_locally_owned())
+                    {
+                        for (unsigned int face_number = 0;
+                             face_number < GeometryInfo<dim>::faces_per_cell;
+                             ++face_number) {
+                            if (cell->face(face_number)->at_boundary()) {
+                                for (unsigned int vertex_number = 0;
+                                     vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                                     ++vertex_number) {
+                                    const auto vert = cell->vertex(vertex_number);
+                                    /*Find bottom left corner*/
+                                    if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
+                                            vert(1) - 0) < 1e-12 && ((std::fabs(
+                                            vert(2) - 0) < 1e-12) || (std::fabs(
+                                            vert(2) - 1) < 1e-12))) {
+
+
+                                        const unsigned int x_displacement =
+                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
+                                        const unsigned int y_displacement =
+                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
+                                        const unsigned int z_displacement =
+                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
+                                        const unsigned int x_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
+                                        const unsigned int y_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 4, cell->active_fe_index());
+                                        const unsigned int z_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 5, cell->active_fe_index());
+
+                                        boundary_values[x_displacement] = 0;
+                                        boundary_values[y_displacement] = 0;
+                                        boundary_values[z_displacement] = 0;
+                                        boundary_values[x_displacement_multiplier] = 0;
+                                        boundary_values[y_displacement_multiplier] = 0;
+                                        boundary_values[z_displacement_multiplier] = 0;
+                                    }
+                                    /*Find bottom right corner*/
+                                    if (std::fabs(vert(0) - 6) < 1e-12 && std::fabs(
+                                            vert(1) - 0) < 1e-12 && ((std::fabs(
+                                            vert(2) - 0) < 1e-12) || (std::fabs(
+                                            vert(2) - 1) < 1e-12))) {
 //                              const unsigned int x_displacement =
 //                                    cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
-                                    const unsigned int y_displacement =
-                                            cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
-                                    const unsigned int z_displacement =
-                                            cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
+                                        const unsigned int y_displacement =
+                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
+                                        const unsigned int z_displacement =
+                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
 //                              const unsigned int x_displacement_multiplier =
 //                                    cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
-                                    const unsigned int y_displacement_multiplier =
-                                            cell->vertex_dof_index(vertex_number, 4, cell->active_fe_index());
-                                    const unsigned int z_displacement_multiplier =
-                                            cell->vertex_dof_index(vertex_number, 5, cell->active_fe_index());
+                                        const unsigned int y_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 4, cell->active_fe_index());
+                                        const unsigned int z_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 5, cell->active_fe_index());
 //                              boundary_values[x_displacement] = 0;
-                                    boundary_values[y_displacement] = 0;
-                                    boundary_values[z_displacement] = 0;
+                                        boundary_values[y_displacement] = 0;
+                                        boundary_values[z_displacement] = 0;
 //                              boundary_values[x_displacement_multiplier] = 0;
-                                    boundary_values[y_displacement_multiplier] = 0;
-                                    boundary_values[z_displacement_multiplier] = 0;
+                                        boundary_values[y_displacement_multiplier] = 0;
+                                        boundary_values[z_displacement_multiplier] = 0;
+                                    }
                                 }
                             }
                         }
@@ -422,48 +440,50 @@ namespace SAND {
         } else if (Input::geometry_base == GeometryOptions::l_shape) {
             if (dim == 2) {
                 for (const auto &cell: dof_handler.active_cell_iterators()) {
-
-                    for (unsigned int face_number = 0;
-                         face_number < GeometryInfo<dim>::faces_per_cell;
-                         ++face_number) {
-                        if (cell->face(face_number)->at_boundary()) {
-                            for (unsigned int vertex_number = 0;
-                                 vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                                 ++vertex_number) {
-                                const auto vert = cell->vertex(vertex_number);
-                                /*Find top left corner*/
-                                if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
-                                        vert(1) - 2) < 1e-12) {
-
-                                    const unsigned int x_displacement =
-                                            cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
-                                    const unsigned int y_displacement =
-                                            cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
-                                    const unsigned int x_displacement_multiplier =
-                                            cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
-                                    const unsigned int y_displacement_multiplier =
-                                            cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
-                                    /*set bottom left BC*/
-                                    boundary_values[x_displacement] = 0;
-                                    boundary_values[y_displacement] = 0;
-                                    boundary_values[x_displacement_multiplier] = 0;
-                                    boundary_values[y_displacement_multiplier] = 0;
-                                }
-                                /*Find top right corner*/
-                                if (std::fabs(vert(0) - 1) < 1e-12 && std::fabs(
-                                        vert(1) - 2) < 1e-12) {
-                                    const unsigned int x_displacement =
-                                            cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
-                                    const unsigned int y_displacement =
-                                            cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
-                                    const unsigned int x_displacement_multiplier =
-                                            cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
-                                    const unsigned int y_displacement_multiplier =
-                                            cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
-                                    boundary_values[x_displacement] = 0;
-                                    boundary_values[y_displacement] = 0;
-                                    boundary_values[x_displacement_multiplier] = 0;
-                                    boundary_values[y_displacement_multiplier] = 0;
+                    if(cell->is_locally_owned())
+                    {
+                        for (unsigned int face_number = 0;
+                             face_number < GeometryInfo<dim>::faces_per_cell;
+                             ++face_number) {
+                            if (cell->face(face_number)->at_boundary()) {
+                                for (unsigned int vertex_number = 0;
+                                     vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                                     ++vertex_number) {
+                                    const auto vert = cell->vertex(vertex_number);
+                                    /*Find top left corner*/
+                                    if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
+                                            vert(1) - 2) < 1e-12) {
+
+                                        const unsigned int x_displacement =
+                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
+                                        const unsigned int y_displacement =
+                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
+                                        const unsigned int x_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
+                                        const unsigned int y_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
+                                        /*set bottom left BC*/
+                                        boundary_values[x_displacement] = 0;
+                                        boundary_values[y_displacement] = 0;
+                                        boundary_values[x_displacement_multiplier] = 0;
+                                        boundary_values[y_displacement_multiplier] = 0;
+                                    }
+                                    /*Find top right corner*/
+                                    if (std::fabs(vert(0) - 1) < 1e-12 && std::fabs(
+                                            vert(1) - 2) < 1e-12) {
+                                        const unsigned int x_displacement =
+                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
+                                        const unsigned int y_displacement =
+                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
+                                        const unsigned int x_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
+                                        const unsigned int y_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
+                                        boundary_values[x_displacement] = 0;
+                                        boundary_values[y_displacement] = 0;
+                                        boundary_values[x_displacement_multiplier] = 0;
+                                        boundary_values[y_displacement_multiplier] = 0;
+                                    }
                                 }
                             }
                         }
@@ -471,64 +491,67 @@ namespace SAND {
                 }
             } else if (dim == 3) {
                 for (const auto &cell: dof_handler.active_cell_iterators()) {
-                    for (unsigned int face_number = 0;
-                         face_number < GeometryInfo<dim>::faces_per_cell;
-                         ++face_number) {
-                        if (cell->face(face_number)->at_boundary()) {
-                            for (unsigned int vertex_number = 0;
-                                 vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                                 ++vertex_number) {
-                                const auto vert = cell->vertex(vertex_number);
-                                /*Find bottom left corner*/
-                                if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
-                                        vert(1) - 2) < 1e-12 && ((std::fabs(
-                                        vert(2) - 0) < 1e-12) || (std::fabs(
-                                        vert(2) - 1) < 1e-12))) {
-
-
-                                    const unsigned int x_displacement =
-                                            cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
-                                    const unsigned int y_displacement =
-                                            cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
-                                    const unsigned int z_displacement =
-                                            cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
-                                    const unsigned int x_displacement_multiplier =
-                                            cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
-                                    const unsigned int y_displacement_multiplier =
-                                            cell->vertex_dof_index(vertex_number, 4, cell->active_fe_index());
-                                    const unsigned int z_displacement_multiplier =
-                                            cell->vertex_dof_index(vertex_number, 5, cell->active_fe_index());
-
-                                    boundary_values[x_displacement] = 0;
-                                    boundary_values[y_displacement] = 0;
-                                    boundary_values[z_displacement] = 0;
-                                    boundary_values[x_displacement_multiplier] = 0;
-                                    boundary_values[y_displacement_multiplier] = 0;
-                                    boundary_values[z_displacement_multiplier] = 0;
-                                }
-                                /*Find bottom right corner*/
-                                if (std::fabs(vert(0) - 1) < 1e-12 && std::fabs(
-                                        vert(1) - 2) < 1e-12 && ((std::fabs(
-                                        vert(2) - 0) < 1e-12) || (std::fabs(
-                                        vert(2) - 1) < 1e-12))) {
-                                    const unsigned int x_displacement =
-                                            cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
-                                    const unsigned int y_displacement =
-                                            cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
-                                    const unsigned int z_displacement =
-                                            cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
-                                    const unsigned int x_displacement_multiplier =
-                                            cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
-                                    const unsigned int y_displacement_multiplier =
-                                            cell->vertex_dof_index(vertex_number, 4, cell->active_fe_index());
-                                    const unsigned int z_displacement_multiplier =
-                                            cell->vertex_dof_index(vertex_number, 5, cell->active_fe_index());
-                                    boundary_values[x_displacement] = 0;
-                                    boundary_values[y_displacement] = 0;
-                                    boundary_values[z_displacement] = 0;
-                                    boundary_values[x_displacement_multiplier] = 0;
-                                    boundary_values[y_displacement_multiplier] = 0;
-                                    boundary_values[z_displacement_multiplier] = 0;
+                    if(cell->is_locally_owned())
+                    {
+                        for (unsigned int face_number = 0;
+                             face_number < GeometryInfo<dim>::faces_per_cell;
+                             ++face_number) {
+                            if (cell->face(face_number)->at_boundary()) {
+                                for (unsigned int vertex_number = 0;
+                                     vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                                     ++vertex_number) {
+                                    const auto vert = cell->vertex(vertex_number);
+                                    /*Find bottom left corner*/
+                                    if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
+                                            vert(1) - 2) < 1e-12 && ((std::fabs(
+                                            vert(2) - 0) < 1e-12) || (std::fabs(
+                                            vert(2) - 1) < 1e-12))) {
+
+
+                                        const unsigned int x_displacement =
+                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
+                                        const unsigned int y_displacement =
+                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
+                                        const unsigned int z_displacement =
+                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
+                                        const unsigned int x_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
+                                        const unsigned int y_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 4, cell->active_fe_index());
+                                        const unsigned int z_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 5, cell->active_fe_index());
+
+                                        boundary_values[x_displacement] = 0;
+                                        boundary_values[y_displacement] = 0;
+                                        boundary_values[z_displacement] = 0;
+                                        boundary_values[x_displacement_multiplier] = 0;
+                                        boundary_values[y_displacement_multiplier] = 0;
+                                        boundary_values[z_displacement_multiplier] = 0;
+                                    }
+                                    /*Find bottom right corner*/
+                                    if (std::fabs(vert(0) - 1) < 1e-12 && std::fabs(
+                                            vert(1) - 2) < 1e-12 && ((std::fabs(
+                                            vert(2) - 0) < 1e-12) || (std::fabs(
+                                            vert(2) - 1) < 1e-12))) {
+                                        const unsigned int x_displacement =
+                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
+                                        const unsigned int y_displacement =
+                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
+                                        const unsigned int z_displacement =
+                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
+                                        const unsigned int x_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
+                                        const unsigned int y_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 4, cell->active_fe_index());
+                                        const unsigned int z_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 5, cell->active_fe_index());
+                                        boundary_values[x_displacement] = 0;
+                                        boundary_values[y_displacement] = 0;
+                                        boundary_values[z_displacement] = 0;
+                                        boundary_values[x_displacement_multiplier] = 0;
+                                        boundary_values[y_displacement_multiplier] = 0;
+                                        boundary_values[z_displacement_multiplier] = 0;
+                                    }
                                 }
                             }
                         }
@@ -551,7 +574,7 @@ namespace SAND {
     KktSystem<dim>::setup_block_system() {
         const FEValuesExtractors::Scalar densities(SolutionComponents::density<dim>);
 
-        //MAKE n_u and n_P*****************************************************************
+        //MAKE n_u and n_P
 
         /*Setup 10 by 10 block matrix*/
         std::vector<unsigned int> block_component(10, 2);
@@ -703,10 +726,12 @@ namespace SAND {
             DoFTools::extract_locally_relevant_dofs(dof_handler, locally_relevant_dofs);
             system_matrix.reinit(owned_partitioning, dsp, mpi_communicator);
 
-            linear_solution.reinit(owned_partitioning, relevant_partitioning, mpi_communicator);
+            locally_relevant_solution.reinit(owned_partitioning, relevant_partitioning, mpi_communicator);
+            distributed_solution.reinit(owned_partitioning, mpi_communicator);
             system_rhs.reinit(owned_partitioning, mpi_communicator);
 
-            linear_solution.collect_sizes();
+            locally_relevant_solution.collect_sizes();
+            distributed_solution.collect_sizes();
             system_rhs.collect_sizes();
             system_matrix.collect_sizes();
     }
@@ -720,7 +745,7 @@ namespace SAND {
         /*Remove any values from old iterations*/
 
         system_matrix=0;
-        linear_solution = 0;
+        locally_relevant_solution = 0;
         system_rhs = 0;
 
         QGauss<dim> nine_quadrature(fe_nine.degree + 1);
@@ -767,15 +792,37 @@ namespace SAND {
 
         const Functions::ConstantFunction<dim> lambda(1.), mu(1.);
 
+        for (const auto &cell: dof_handler.active_cell_iterators()) {
+            const unsigned int i = cell->active_cell_index();
+
+            typename LA::MPI::SparseMatrix::iterator iter = density_filter.filter_matrix.begin(
+                    i);
+            for (; iter != density_filter.filter_matrix.end(i); iter++) {
+                unsigned int j = iter->column();
+                double value = iter->value() * cell->measure();
+
+                system_matrix.block(SolutionBlocks::unfiltered_density_multiplier,
+                                    SolutionBlocks::unfiltered_density).add(i, j, value);
+                system_matrix.block(SolutionBlocks::unfiltered_density,
+                                    SolutionBlocks::unfiltered_density_multiplier).add(j, i, value);
+            }
+
+            system_matrix.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).add(0, i,
+                                                                                                      cell->measure());
+            system_matrix.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier).add(i, 0,
+                                                                                                      cell->measure());
+        }
+
+
         LA::MPI::BlockVector filtered_unfiltered_density_solution = state;
         LA::MPI::BlockVector filter_adjoint_unfiltered_density_multiplier_solution = state;
         filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = 0;
         filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier) = 0;
 
-        density_filter.filter_matrix.vmult(
+        system_matrix.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).vmult(
                 filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density),
                 state.block(SolutionBlocks::unfiltered_density));
-        density_filter.filter_matrix.Tvmult(filter_adjoint_unfiltered_density_multiplier_solution.block(
+        system_matrix.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).Tvmult(filter_adjoint_unfiltered_density_multiplier_solution.block(
                                                     SolutionBlocks::unfiltered_density_multiplier),
                                             state.block(SolutionBlocks::unfiltered_density_multiplier));
 
@@ -1064,26 +1111,7 @@ namespace SAND {
         }
         system_matrix.compress(VectorOperation::add);
         system_rhs = calculate_rhs(state, barrier_size);
-        for (const auto &cell: dof_handler.active_cell_iterators()) {
-            const unsigned int i = cell->active_cell_index();
-
-            typename LA::MPI::SparseMatrix::iterator iter = density_filter.filter_matrix.begin(
-                    i);
-            for (; iter != density_filter.filter_matrix.end(i); iter++) {
-                unsigned int j = iter->column();
-                double value = iter->value() * cell->measure();
-
-                system_matrix.block(SolutionBlocks::unfiltered_density_multiplier,
-                                    SolutionBlocks::unfiltered_density).add(i, j, value);
-                system_matrix.block(SolutionBlocks::unfiltered_density,
-                                    SolutionBlocks::unfiltered_density_multiplier).add(j, i, value);
-            }
 
-            system_matrix.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).add(0, i,
-                                                                                                      cell->measure());
-            system_matrix.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier).add(i, 0,
-                                                                                                      cell->measure());
-        }
         std::cout << "assembled" << std::endl;
     }
 
@@ -1195,19 +1223,6 @@ namespace SAND {
     double
     KktSystem<dim>::calculate_feasibility(const LA::MPI::BlockVector &state, const double barrier_size) const {
         LA::MPI::BlockVector test_rhs = calculate_rhs(state, barrier_size);
-//        double feasibility = 0;
-//        feasibility +=
-//                test_rhs.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm() +
-//                test_rhs.block(SolutionBlocks::density_lower_slack_multiplier).l2_norm() +
-//                test_rhs.block(SolutionBlocks::density_upper_slack_multiplier).l2_norm() +
-//                test_rhs.block(SolutionBlocks::displacement_multiplier).l2_norm() +
-//                test_rhs.block(SolutionBlocks::density_lower_slack).l2_norm() +
-//                test_rhs.block(SolutionBlocks::density_upper_slack).l2_norm() +
-//                test_rhs.block(SolutionBlocks::total_volume_multiplier).l2_norm()+
-//                test_rhs.block(SolutionBlocks::density).l2_norm()+
-//                test_rhs.block(SolutionBlocks::unfiltered_density).l2_norm()+
-//                test_rhs.block(SolutionBlocks::displacement).l2_norm();
-//        return feasibility;
 
         double norm = 0;
         norm += std::pow(test_rhs.block(SolutionBlocks::displacement).l2_norm(), 2);
@@ -1218,6 +1233,7 @@ namespace SAND {
         norm += std::pow(test_rhs.block(SolutionBlocks::total_volume_multiplier).l2_norm(), 2);
         norm += std::pow(test_rhs.block(SolutionBlocks::density_upper_slack_multiplier).l2_norm(), 2);
         norm += std::pow(test_rhs.block(SolutionBlocks::density_lower_slack_multiplier).l2_norm(), 2);
+        std::cout << "!!!!!!!!!!!!!!!!!!" << std::endl;
         for (unsigned int k = 0; k < state.block(SolutionBlocks::density_upper_slack).size(); k++) {
             norm += state.block(SolutionBlocks::density_upper_slack)[k] *
                     state.block(SolutionBlocks::density_upper_slack_multiplier)[k]
@@ -1270,8 +1286,7 @@ namespace SAND {
     template<int dim>
     LA::MPI::BlockVector
     KktSystem<dim>::calculate_rhs(const LA::MPI::BlockVector &state, const double barrier_size) const {
-        LA::MPI::BlockVector test_rhs;
-        test_rhs = system_rhs;
+        LA::MPI::BlockVector test_rhs (system_rhs);
         test_rhs = 0;
 
 
@@ -1322,277 +1337,324 @@ namespace SAND {
 
         const Functions::ConstantFunction<dim> lambda(1.), mu(1.);
 
-        LA::MPI::BlockVector filtered_unfiltered_density_solution = state;
-        LA::MPI::BlockVector filter_adjoint_unfiltered_density_multiplier_solution = state;
+        locally_relevant_solution = state;
+        distributed_solution = state;
+        LA::MPI::BlockVector filtered_unfiltered_density_solution (system_rhs);
+        LA::MPI::BlockVector filter_adjoint_unfiltered_density_multiplier_solution (system_rhs);
         filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = 0;
         filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier) = 0;
+        system_matrix.block(SolutionBlocks::unfiltered_density_multiplier,
+                            SolutionBlocks::unfiltered_density) = 0;
+        system_matrix.block(SolutionBlocks::unfiltered_density_multiplier,
+                            SolutionBlocks::unfiltered_density) = 0;
+        for (const auto &cell: dof_handler.active_cell_iterators()) {
+            if(cell->is_locally_owned())
+            {
+                const unsigned int i = cell->active_cell_index();
 
-        density_filter.filter_matrix.vmult(
-                filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density),
-                state.block(SolutionBlocks::unfiltered_density));
-        density_filter.filter_matrix.Tvmult(filter_adjoint_unfiltered_density_multiplier_solution.block(
-                                                    SolutionBlocks::unfiltered_density_multiplier),
-                                            state.block(SolutionBlocks::unfiltered_density_multiplier));
-        const double old_volume_multiplier = state.block(SolutionBlocks::total_volume_multiplier)[0];
+                typename LA::MPI::SparseMatrix::iterator iter = density_filter.filter_matrix.begin(
+                        i);
+                for (; iter != density_filter.filter_matrix.end(i); iter++)
+                {
+                    unsigned int j = iter->column();
+                    double value = iter->value() * cell->measure();
 
-        for (const auto &cell: dof_handler.active_cell_iterators()) {
-            hp_fe_values.reinit(cell);
-            const FEValues<dim> &fe_values = hp_fe_values.get_present_fe_values();
-            cell_matrix.reinit(cell->get_fe().n_dofs_per_cell(),
-                               cell->get_fe().n_dofs_per_cell());
-            cell_rhs.reinit(cell->get_fe().n_dofs_per_cell());
+                    system_matrix.block(SolutionBlocks::unfiltered_density_multiplier,
+                                        SolutionBlocks::unfiltered_density).add(i, j, value);
+                    system_matrix.block(SolutionBlocks::unfiltered_density,
+                                        SolutionBlocks::unfiltered_density_multiplier).add(j, i, value);
+                }
+            }
+        }
 
-            const unsigned int n_q_points = fe_values.n_quadrature_points;
+        auto op_f = linear_operator<LA::MPI::Vector>(system_matrix.block(SolutionBlocks::unfiltered_density_multiplier,
+                                                                         SolutionBlocks::unfiltered_density));
+        filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = op_f * distributed_solution.block(SolutionBlocks::unfiltered_density);
+        filter_adjoint_unfiltered_density_multiplier_solution.block(
+                                                    SolutionBlocks::unfiltered_density_multiplier)
+                                                    = transpose_operator(op_f) * distributed_solution.block(SolutionBlocks::unfiltered_density_multiplier);
 
-            std::vector<double> old_density_values(n_q_points);
-            std::vector<Tensor<1, dim>> old_displacement_values(n_q_points);
-            std::vector<double> old_displacement_divs(n_q_points);
-            std::vector<SymmetricTensor<2, dim>> old_displacement_symmgrads(
-                    n_q_points);
-            std::vector<Tensor<1, dim>> old_displacement_multiplier_values(
-                    n_q_points);
-            std::vector<double> old_displacement_multiplier_divs(n_q_points);
-            std::vector<SymmetricTensor<2, dim>> old_displacement_multiplier_symmgrads(
-                    n_q_points);
-            std::vector<double> old_lower_slack_multiplier_values(n_q_points);
-            std::vector<double> old_upper_slack_multiplier_values(n_q_points);
-            std::vector<double> old_lower_slack_values(n_q_points);
-            std::vector<double> old_upper_slack_values(n_q_points);
-            std::vector<double> old_unfiltered_density_values(n_q_points);
-            std::vector<double> old_unfiltered_density_multiplier_values(n_q_points);
-            std::vector<double> filtered_unfiltered_density_values(n_q_points);
-            std::vector<double> filter_adjoint_unfiltered_density_multiplier_values(n_q_points);
-            std::vector<double> lambda_values(n_q_points);
-            std::vector<double> mu_values(n_q_points);
+        LA::MPI::BlockVector relevant_filtered_unfiltered_density_solution (locally_relevant_solution);
+        LA::MPI::BlockVector relevant_filter_adjoint_unfiltered_density_multiplier_solution (locally_relevant_solution);
+        relevant_filtered_unfiltered_density_solution = filtered_unfiltered_density_solution;
+        relevant_filter_adjoint_unfiltered_density_multiplier_solution = filter_adjoint_unfiltered_density_multiplier_solution;
 
-            const unsigned int dofs_per_cell = cell->get_fe().n_dofs_per_cell();
+        double old_volume_multiplier;
+        if(state.block(SolutionBlocks::total_volume_multiplier).in_local_range(0))
+        {
+            old_volume_multiplier = state.block(SolutionBlocks::total_volume_multiplier)[0];
+        }
 
-            cell_matrix = 0;
-            cell_rhs = 0;
-            local_dof_indices.resize(cell->get_fe().n_dofs_per_cell());
+        std::cout << "here" << std::endl;
 
-            cell->get_dof_indices(local_dof_indices);
+        for (const auto &cell: dof_handler.active_cell_iterators()) {
+            if(cell->is_locally_owned())
+            {
+                hp_fe_values.reinit(cell);
+                const FEValues<dim> &fe_values = hp_fe_values.get_present_fe_values();
+                cell_matrix.reinit(cell->get_fe().n_dofs_per_cell(),
+                                   cell->get_fe().n_dofs_per_cell());
+                cell_rhs.reinit(cell->get_fe().n_dofs_per_cell());
+
+                const unsigned int n_q_points = fe_values.n_quadrature_points;
+
+                std::vector<double> old_density_values(n_q_points);
+                std::vector<Tensor<1, dim>> old_displacement_values(n_q_points);
+                std::vector<double> old_displacement_divs(n_q_points);
+                std::vector<SymmetricTensor<2, dim>> old_displacement_symmgrads(
+                        n_q_points);
+                std::vector<Tensor<1, dim>> old_displacement_multiplier_values(
+                        n_q_points);
+                std::vector<double> old_displacement_multiplier_divs(n_q_points);
+                std::vector<SymmetricTensor<2, dim>> old_displacement_multiplier_symmgrads(
+                        n_q_points);
+                std::vector<double> old_lower_slack_multiplier_values(n_q_points);
+                std::vector<double> old_upper_slack_multiplier_values(n_q_points);
+                std::vector<double> old_lower_slack_values(n_q_points);
+                std::vector<double> old_upper_slack_values(n_q_points);
+                std::vector<double> old_unfiltered_density_values(n_q_points);
+                std::vector<double> old_unfiltered_density_multiplier_values(n_q_points);
+                std::vector<double> filtered_unfiltered_density_values(n_q_points);
+                std::vector<double> filter_adjoint_unfiltered_density_multiplier_values(n_q_points);
+                std::vector<double> lambda_values(n_q_points);
+                std::vector<double> mu_values(n_q_points);
+
+                const unsigned int dofs_per_cell = cell->get_fe().n_dofs_per_cell();
+
+                cell_matrix = 0;
+                cell_rhs = 0;
+                local_dof_indices.resize(cell->get_fe().n_dofs_per_cell());
+
+                cell->get_dof_indices(local_dof_indices);
+
+                lambda.value_list(fe_values.get_quadrature_points(), lambda_values);
+                mu.value_list(fe_values.get_quadrature_points(), mu_values);
+
+                fe_values[densities].get_function_values(state,
+                                                         old_density_values);
+                fe_values[displacements].get_function_values(state,
+                                                             old_displacement_values);
+                fe_values[displacements].get_function_divergences(state,
+                                                                  old_displacement_divs);
+                fe_values[displacements].get_function_symmetric_gradients(
+                        state, old_displacement_symmgrads);
+                fe_values[displacement_multipliers].get_function_values(
+                        state, old_displacement_multiplier_values);
+                fe_values[displacement_multipliers].get_function_divergences(
+                        state, old_displacement_multiplier_divs);
+                fe_values[displacement_multipliers].get_function_symmetric_gradients(
+                        state, old_displacement_multiplier_symmgrads);
+                fe_values[density_lower_slacks].get_function_values(
+                        state, old_lower_slack_values);
+                fe_values[density_lower_slack_multipliers].get_function_values(
+                        state, old_lower_slack_multiplier_values);
+                fe_values[density_upper_slacks].get_function_values(
+                        state, old_upper_slack_values);
+                fe_values[density_upper_slack_multipliers].get_function_values(
+                        state, old_upper_slack_multiplier_values);
+                fe_values[unfiltered_densities].get_function_values(
+                        state, old_unfiltered_density_values);
+                fe_values[unfiltered_density_multipliers].get_function_values(
+                        state, old_unfiltered_density_multiplier_values);
+                fe_values[unfiltered_densities].get_function_values(
+                        relevant_filtered_unfiltered_density_solution, filtered_unfiltered_density_values);
+                fe_values[unfiltered_density_multipliers].get_function_values(
+                        relevant_filter_adjoint_unfiltered_density_multiplier_solution,
+                        filter_adjoint_unfiltered_density_multiplier_values);
+
+
+                Tensor<1, dim> traction;
+                traction[1] = -1;
+
+                for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
+
+                    for (unsigned int i = 0; i < dofs_per_cell; ++i) {
+                        const SymmetricTensor<2, dim> displacement_phi_i_symmgrad =
+                                fe_values[displacements].symmetric_gradient(i, q_point);
+                        const double displacement_phi_i_div =
+                                fe_values[displacements].divergence(i, q_point);
+
+                        const SymmetricTensor<2, dim> displacement_multiplier_phi_i_symmgrad =
+                                fe_values[displacement_multipliers].symmetric_gradient(i,
+                                                                                       q_point);
+                        const double displacement_multiplier_phi_i_div =
+                                fe_values[displacement_multipliers].divergence(i,
+                                                                               q_point);
 
-            lambda.value_list(fe_values.get_quadrature_points(), lambda_values);
-            mu.value_list(fe_values.get_quadrature_points(), mu_values);
 
-            fe_values[densities].get_function_values(state,
-                                                     old_density_values);
-            fe_values[displacements].get_function_values(state,
-                                                         old_displacement_values);
-            fe_values[displacements].get_function_divergences(state,
-                                                              old_displacement_divs);
-            fe_values[displacements].get_function_symmetric_gradients(
-                    state, old_displacement_symmgrads);
-            fe_values[displacement_multipliers].get_function_values(
-                    state, old_displacement_multiplier_values);
-            fe_values[displacement_multipliers].get_function_divergences(
-                    state, old_displacement_multiplier_divs);
-            fe_values[displacement_multipliers].get_function_symmetric_gradients(
-                    state, old_displacement_multiplier_symmgrads);
-            fe_values[density_lower_slacks].get_function_values(
-                    state, old_lower_slack_values);
-            fe_values[density_lower_slack_multipliers].get_function_values(
-                    state, old_lower_slack_multiplier_values);
-            fe_values[density_upper_slacks].get_function_values(
-                    state, old_upper_slack_values);
-            fe_values[density_upper_slack_multipliers].get_function_values(
-                    state, old_upper_slack_multiplier_values);
-            fe_values[unfiltered_densities].get_function_values(
-                    state, old_unfiltered_density_values);
-            fe_values[unfiltered_density_multipliers].get_function_values(
-                    state, old_unfiltered_density_multiplier_values);
-            fe_values[unfiltered_densities].get_function_values(
-                    filtered_unfiltered_density_solution, filtered_unfiltered_density_values);
-            fe_values[unfiltered_density_multipliers].get_function_values(
-                    filter_adjoint_unfiltered_density_multiplier_solution,
-                    filter_adjoint_unfiltered_density_multiplier_values);
+                        const double density_phi_i = fe_values[densities].value(i,
+                                                                                q_point);
+                        const double unfiltered_density_phi_i = fe_values[unfiltered_densities].value(i,
+                                                                                                      q_point);
+                        const double unfiltered_density_multiplier_phi_i = fe_values[unfiltered_density_multipliers].value(
+                                i, q_point);
 
+                        const double lower_slack_multiplier_phi_i =
+                                fe_values[density_lower_slack_multipliers].value(i,
+                                                                                 q_point);
 
-            Tensor<1, dim> traction;
-            traction[1] = -1;
+                        const double lower_slack_phi_i =
+                                fe_values[density_lower_slacks].value(i, q_point);
 
-            for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
+                        const double upper_slack_phi_i =
+                                fe_values[density_upper_slacks].value(i, q_point);
 
-                for (unsigned int i = 0; i < dofs_per_cell; ++i) {
-                    const SymmetricTensor<2, dim> displacement_phi_i_symmgrad =
-                            fe_values[displacements].symmetric_gradient(i, q_point);
-                    const double displacement_phi_i_div =
-                            fe_values[displacements].divergence(i, q_point);
+                        const double upper_slack_multiplier_phi_i =
+                                fe_values[density_upper_slack_multipliers].value(i,
+                                                                                 q_point);
 
-                    const SymmetricTensor<2, dim> displacement_multiplier_phi_i_symmgrad =
-                            fe_values[displacement_multipliers].symmetric_gradient(i,
-                                                                                   q_point);
-                    const double displacement_multiplier_phi_i_div =
-                            fe_values[displacement_multipliers].divergence(i,
-                                                                           q_point);
 
+                        //rhs eqn 0
+                        cell_rhs(i) +=
+                                -1 * fe_values.JxW(q_point) * (
+                                        -1 * density_penalty_exponent *
+                                        std::pow(old_density_values[q_point], density_penalty_exponent - 1) * density_phi_i
+                                        * (old_displacement_multiplier_divs[q_point] * old_displacement_divs[q_point]
+                                           * lambda_values[q_point]
+                                           + 2 * mu_values[q_point] * (old_displacement_symmgrads[q_point]
+                                                                       * old_displacement_multiplier_symmgrads[q_point]))
+                                        - density_phi_i * old_unfiltered_density_multiplier_values[q_point]
+                                        + old_volume_multiplier * density_phi_i
+                                );
 
-                    const double density_phi_i = fe_values[densities].value(i,
-                                                                            q_point);
-                    const double unfiltered_density_phi_i = fe_values[unfiltered_densities].value(i,
-                                                                                                  q_point);
-                    const double unfiltered_density_multiplier_phi_i = fe_values[unfiltered_density_multipliers].value(
-                            i, q_point);
+                        //rhs eqn 1 - boundary terms counted later
+                        cell_rhs(i) +=
+                                -1 * fe_values.JxW(q_point) * (
+                                        -1 * std::pow(old_density_values[q_point], density_penalty_exponent)
+                                        * (old_displacement_multiplier_divs[q_point] * displacement_phi_i_div
+                                           * lambda_values[q_point]
+                                           + 2 * mu_values[q_point] * (old_displacement_multiplier_symmgrads[q_point]
+                                                                       * displacement_phi_i_symmgrad))
+                                );
 
-                    const double lower_slack_multiplier_phi_i =
-                            fe_values[density_lower_slack_multipliers].value(i,
-                                                                             q_point);
+                        //rhs eqn 2
+                        cell_rhs(i) +=
+                                -1 * fe_values.JxW(q_point) * (
+                                        unfiltered_density_phi_i *
+                                        filter_adjoint_unfiltered_density_multiplier_values[q_point]
+                                        + unfiltered_density_phi_i * old_upper_slack_multiplier_values[q_point]
+                                        + -1 * unfiltered_density_phi_i * old_lower_slack_multiplier_values[q_point]
+                                );
 
-                    const double lower_slack_phi_i =
-                            fe_values[density_lower_slacks].value(i, q_point);
 
-                    const double upper_slack_phi_i =
-                            fe_values[density_upper_slacks].value(i, q_point);
 
-                    const double upper_slack_multiplier_phi_i =
-                            fe_values[density_upper_slack_multipliers].value(i,
-                                                                             q_point);
 
+                        //rhs eqn 3 - boundary terms counted later
+                        cell_rhs(i) +=
+                                -1 * fe_values.JxW(q_point) * (
+                                        -1 * std::pow(old_density_values[q_point], density_penalty_exponent)
+                                        * (old_displacement_divs[q_point] * displacement_multiplier_phi_i_div
+                                           * lambda_values[q_point]
+                                           + 2 * mu_values[q_point] * (displacement_multiplier_phi_i_symmgrad
+                                                                       * old_displacement_symmgrads[q_point]))
+                                );
 
-                    //rhs eqn 0
-                    cell_rhs(i) +=
-                            -1 * fe_values.JxW(q_point) * (
-                                    -1 * density_penalty_exponent *
-                                    std::pow(old_density_values[q_point], density_penalty_exponent - 1) * density_phi_i
-                                    * (old_displacement_multiplier_divs[q_point] * old_displacement_divs[q_point]
-                                       * lambda_values[q_point]
-                                       + 2 * mu_values[q_point] * (old_displacement_symmgrads[q_point]
-                                                                   * old_displacement_multiplier_symmgrads[q_point]))
-                                    - density_phi_i * old_unfiltered_density_multiplier_values[q_point]
-                                    + old_volume_multiplier * density_phi_i
-                            );
-
-                    //rhs eqn 1 - boundary terms counted later
-                    cell_rhs(i) +=
-                            -1 * fe_values.JxW(q_point) * (
-                                    -1 * std::pow(old_density_values[q_point], density_penalty_exponent)
-                                    * (old_displacement_multiplier_divs[q_point] * displacement_phi_i_div
-                                       * lambda_values[q_point]
-                                       + 2 * mu_values[q_point] * (old_displacement_multiplier_symmgrads[q_point]
-                                                                   * displacement_phi_i_symmgrad))
-                            );
-
-                    //rhs eqn 2
-                    cell_rhs(i) +=
-                            -1 * fe_values.JxW(q_point) * (
-                                    unfiltered_density_phi_i *
-                                    filter_adjoint_unfiltered_density_multiplier_values[q_point]
-                                    + unfiltered_density_phi_i * old_upper_slack_multiplier_values[q_point]
-                                    + -1 * unfiltered_density_phi_i * old_lower_slack_multiplier_values[q_point]
-                            );
-
-
-
-
-                    //rhs eqn 3 - boundary terms counted later
-                    cell_rhs(i) +=
-                            -1 * fe_values.JxW(q_point) * (
-                                    -1 * std::pow(old_density_values[q_point], density_penalty_exponent)
-                                    * (old_displacement_divs[q_point] * displacement_multiplier_phi_i_div
-                                       * lambda_values[q_point]
-                                       + 2 * mu_values[q_point] * (displacement_multiplier_phi_i_symmgrad
-                                                                   * old_displacement_symmgrads[q_point]))
-                            );
-
-                    //rhs eqn 4
-                    cell_rhs(i) +=
-                            -1 * fe_values.JxW(q_point) *
-                            (-1 * lower_slack_multiplier_phi_i
-                             * (old_unfiltered_density_values[q_point] - old_lower_slack_values[q_point])
-                            );
-
-                    //rhs eqn 5
-                    cell_rhs(i) +=
-                            -1 * fe_values.JxW(q_point) * (
-                                    -1 * upper_slack_multiplier_phi_i
-                                    * (1 - old_unfiltered_density_values[q_point]
-                                       - old_upper_slack_values[q_point]));
-
-                    //rhs eqn 6
-                    cell_rhs(i) +=
-                            -1 * fe_values.JxW(q_point) * (
-                                    -1 * unfiltered_density_multiplier_phi_i
-                                    * (old_density_values[q_point] - filtered_unfiltered_density_values[q_point])
-                            );
-
-                    //rhs eqn 7
-                    cell_rhs(i) +=
-                            -1 * fe_values.JxW(q_point) *
-                            (lower_slack_phi_i *
-                             (old_lower_slack_multiplier_values[q_point] -
-                              barrier_size / old_lower_slack_values[q_point]));
-
-                    //rhs eqn 8
-                    cell_rhs(i) +=
-                            -1 * fe_values.JxW(q_point) *
-                            (upper_slack_phi_i *
-                             (old_upper_slack_multiplier_values[q_point] -
-                              barrier_size / old_upper_slack_values[q_point]));
+                        //rhs eqn 4
+                        cell_rhs(i) +=
+                                -1 * fe_values.JxW(q_point) *
+                                (-1 * lower_slack_multiplier_phi_i
+                                 * (old_unfiltered_density_values[q_point] - old_lower_slack_values[q_point])
+                                );
+
+                        //rhs eqn 5
+                        cell_rhs(i) +=
+                                -1 * fe_values.JxW(q_point) * (
+                                        -1 * upper_slack_multiplier_phi_i
+                                        * (1 - old_unfiltered_density_values[q_point]
+                                           - old_upper_slack_values[q_point]));
+
+                        //rhs eqn 6
+                        cell_rhs(i) +=
+                                -1 * fe_values.JxW(q_point) * (
+                                        -1 * unfiltered_density_multiplier_phi_i
+                                        * (old_density_values[q_point] - filtered_unfiltered_density_values[q_point])
+                                );
+
+                        //rhs eqn 7
+                        cell_rhs(i) +=
+                                -1 * fe_values.JxW(q_point) *
+                                (lower_slack_phi_i *
+                                 (old_lower_slack_multiplier_values[q_point] -
+                                  barrier_size / old_lower_slack_values[q_point]));
+
+                        //rhs eqn 8
+                        cell_rhs(i) +=
+                                -1 * fe_values.JxW(q_point) *
+                                (upper_slack_phi_i *
+                                 (old_upper_slack_multiplier_values[q_point] -
+                                  barrier_size / old_upper_slack_values[q_point]));
+
+                    }
 
                 }
 
-            }
+                for (unsigned int face_number = 0;
+                     face_number < GeometryInfo<dim>::faces_per_cell;
+                     ++face_number) {
+                    if (cell->face(face_number)->at_boundary() && cell->face(
+                            face_number)->boundary_id() == BoundaryIds::down_force) {
+                        for (unsigned int face_q_point = 0;
+                             face_q_point < n_face_q_points; ++face_q_point) {
+                            for (unsigned int i = 0; i < dofs_per_cell; ++i) {
+                                if (cell->material_id() == MaterialIds::without_multiplier) {
+                                    fe_nine_face_values.reinit(cell, face_number);
+                                    cell_rhs(i) += -1
+                                                   * traction
+                                                   * fe_nine_face_values[displacements].value(i,
+                                                                                              face_q_point)
+                                                   * fe_nine_face_values.JxW(face_q_point);
 
+                                    cell_rhs(i) += -1 * traction
+                                                   * fe_nine_face_values[displacement_multipliers].value(
+                                            i, face_q_point)
+                                                   * fe_nine_face_values.JxW(face_q_point);
+                                } else {
+                                    fe_ten_face_values.reinit(cell, face_number);
+                                    cell_rhs(i) += -1
+                                                   * traction
+                                                   * fe_ten_face_values[displacements].value(i,
+                                                                                             face_q_point)
+                                                   * fe_ten_face_values.JxW(face_q_point);
 
-            for (unsigned int face_number = 0;
-                 face_number < GeometryInfo<dim>::faces_per_cell;
-                 ++face_number) {
-                if (cell->face(face_number)->at_boundary() && cell->face(
-                        face_number)->boundary_id() == BoundaryIds::down_force) {
-                    for (unsigned int face_q_point = 0;
-                         face_q_point < n_face_q_points; ++face_q_point) {
-                        for (unsigned int i = 0; i < dofs_per_cell; ++i) {
-                            if (cell->material_id() == MaterialIds::without_multiplier) {
-                                fe_nine_face_values.reinit(cell, face_number);
-                                cell_rhs(i) += -1
-                                               * traction
-                                               * fe_nine_face_values[displacements].value(i,
-                                                                                          face_q_point)
-                                               * fe_nine_face_values.JxW(face_q_point);
-
-                                cell_rhs(i) += -1 * traction
-                                               * fe_nine_face_values[displacement_multipliers].value(
-                                        i, face_q_point)
-                                               * fe_nine_face_values.JxW(face_q_point);
-                            } else {
-                                fe_ten_face_values.reinit(cell, face_number);
-                                cell_rhs(i) += -1
-                                               * traction
-                                               * fe_ten_face_values[displacements].value(i,
-                                                                                         face_q_point)
-                                               * fe_ten_face_values.JxW(face_q_point);
-
-                                cell_rhs(i) += -1 * traction
-                                               * fe_ten_face_values[displacement_multipliers].value(
-                                        i, face_q_point)
-                                               * fe_ten_face_values.JxW(face_q_point);
+                                    cell_rhs(i) += -1 * traction
+                                                   * fe_ten_face_values[displacement_multipliers].value(
+                                            i, face_q_point)
+                                                   * fe_ten_face_values.JxW(face_q_point);
+                                }
                             }
                         }
                     }
                 }
-            }
-
-
-            MatrixTools::local_apply_boundary_values(boundary_values, local_dof_indices,
-                                                     cell_matrix, cell_rhs, true);
 
-            constraints.distribute_local_to_global(
-                    cell_rhs, local_dof_indices, test_rhs);
 
+                MatrixTools::local_apply_boundary_values(boundary_values, local_dof_indices,
+                                                         cell_matrix, cell_rhs, true);
+                constraints.distribute_local_to_global(
+                        cell_rhs, local_dof_indices, test_rhs);
+            }
         }
+        std::cout << "++++++++++++++++" << std::endl;
         test_rhs.compress(VectorOperation::add);
         double total_volume = 0;
         double goal_volume = 0;
+
+        distributed_solution = state;
         for (const auto &cell: dof_handler.active_cell_iterators()) {
-            total_volume += cell->measure() * state.block(SolutionBlocks::density)[cell->active_cell_index()];
-            goal_volume += cell->measure() * Input::volume_percentage;
+            if(cell->is_locally_owned())
+            {
+                if (distributed_solution.block(SolutionBlocks::density).in_local_range(cell->active_cell_index()))
+                {
+                    total_volume += cell->measure() * state.block(SolutionBlocks::density)[cell->active_cell_index()];
+                    goal_volume += cell->measure() * Input::volume_percentage;
+                }
+            }
+        }
+        if (test_rhs.block(SolutionBlocks::total_volume_multiplier).in_local_range(0))
+        {
+            test_rhs.block(SolutionBlocks::total_volume_multiplier)[0] = goal_volume - total_volume;
         }
 
-        test_rhs.block(SolutionBlocks::total_volume_multiplier)[0] = goal_volume - total_volume;
-
-
-
+        test_rhs.print(std::cout);
         return test_rhs;
 
     }
@@ -1614,8 +1676,8 @@ namespace SAND {
         else {
             gmres_tolerance = Input::default_gmres_tolerance;
         }
-        linear_solution=0;
-        constraints.distribute(linear_solution);
+        locally_relevant_solution=0;
+        constraints.distribute(locally_relevant_solution);
         SolverControl solver_control(10000, gmres_tolerance * system_rhs.l2_norm());
 
         TopOptSchurPreconditioner<dim> preconditioner(system_matrix);
@@ -1625,34 +1687,34 @@ namespace SAND {
 //                std::string solver_type;
 //                TrilinosWrappers::SolverDirect::AdditionalData additional_data(false, "Amesos_Klu");
 //                TrilinosWrappers::SolverDirect solver(cn, mpi_communicator, system_matrix);
-//                solver.solve(linear_solution, system_rhs);
+//                solver.solve(locally_relevant_solution, system_rhs);
                 break;
             }
             case SolverOptions::exact_preconditioner_with_gmres: {
                 preconditioner.initialize(system_matrix, boundary_values, dof_handler, state);
                 SolverFGMRES<LA::MPI::BlockVector> A_fgmres(solver_control);
-                A_fgmres.solve(system_matrix, linear_solution, system_rhs, preconditioner);
+                A_fgmres.solve(system_matrix, locally_relevant_solution, system_rhs, preconditioner);
                 std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
                 break;
             }
             case SolverOptions::inexact_K_with_exact_A_gmres: {
                 preconditioner.initialize(system_matrix, boundary_values, dof_handler, state);
                 SolverFGMRES<LA::MPI::BlockVector> B_fgmres(solver_control);
-                B_fgmres.solve(system_matrix, linear_solution, system_rhs, preconditioner);
+                B_fgmres.solve(system_matrix, locally_relevant_solution, system_rhs, preconditioner);
                 std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
                 break;
             }
             case SolverOptions::inexact_K_with_inexact_A_gmres: {
                 preconditioner.initialize(system_matrix, boundary_values, dof_handler, state);
                 SolverFGMRES<LA::MPI::BlockVector> C_fgmres(solver_control);
-                C_fgmres.solve(system_matrix, linear_solution, system_rhs, preconditioner);
+                C_fgmres.solve(system_matrix, locally_relevant_solution, system_rhs, preconditioner);
                 std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
                 break;
             }
             default:
                 throw;
         }
-        constraints.distribute(linear_solution);
+        constraints.distribute(locally_relevant_solution);
 
         if (Input::output_parts_of_matrix) {
             preconditioner.print_stuff();
@@ -1679,7 +1741,7 @@ namespace SAND {
 //            }
 //            Mat.close();
         }
-        return linear_solution;
+        return locally_relevant_solution;
     }
 
     template<int dim>
@@ -1693,8 +1755,8 @@ namespace SAND {
     KktSystem<dim>::get_initial_state() {
 
         std::vector<unsigned int> block_component(10, 2);
-        block_component[0] = 0;
-        block_component[5] = 1;
+        block_component[SolutionBlocks::density] = 0;
+        block_component[SolutionBlocks::displacement] = 1;
         const std::vector<types::global_dof_index> dofs_per_block =
                 DoFTools::count_dofs_per_fe_block(dof_handler, block_component);
         const unsigned int n_p = dofs_per_block[0];
@@ -1704,17 +1766,19 @@ namespace SAND {
         LA::MPI::BlockVector state(owned_partitioning, relevant_partitioning,mpi_communicator);
         {
             using namespace SolutionBlocks;
-            state.block(density).add(density_ratio);
-            state.block(unfiltered_density).add(density_ratio);
-            state.block(unfiltered_density_multiplier)
+            distributed_solution=state;
+            distributed_solution.block(density).add(density_ratio);
+            distributed_solution.block(unfiltered_density).add(density_ratio);
+            distributed_solution.block(unfiltered_density_multiplier)
                     .add(density_ratio);
-            state.block(density_lower_slack).add(density_ratio);
-            state.block(density_lower_slack_multiplier).add(50);
-            state.block(density_upper_slack).add(1 - density_ratio);
-            state.block(density_upper_slack_multiplier).add(50);
-            state.block(total_volume_multiplier).add(1);
-            state.block(displacement).add(0);
-            state.block(displacement_multiplier).add(0);
+            distributed_solution.block(density_lower_slack).add(density_ratio);
+            distributed_solution.block(density_lower_slack_multiplier).add(50);
+            distributed_solution.block(density_upper_slack).add(1 - density_ratio);
+            distributed_solution.block(density_upper_slack_multiplier).add(50);
+            distributed_solution.block(total_volume_multiplier).add(1);
+            distributed_solution.block(displacement).add(0);
+            distributed_solution.block(displacement_multiplier).add(0);
+            state=distributed_solution;
         }
         return state;
 
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 9a59b5c..620ce10 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -393,9 +393,6 @@ namespace SAND {
                 std::cout << "last residual: " << step_4_gmres_control_1.last_value() << std::endl;
                 throw;
             }
-            std::cout << "first residual 4-1: " << step_4_gmres_control_1.initial_value() << std::endl;
-            std::cout << "last residual 4-1: " << step_4_gmres_control_1.last_value() << std::endl;
-            std::cout << "Iterations 4-1: " << step_4_gmres_control_1.last_step() << std::endl;
 
             SolverControl step_4_gmres_control_2 (100000, src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6);
             SolverGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
@@ -409,9 +406,6 @@ namespace SAND {
                 std::cout << "last residual: " << step_4_gmres_control_2.last_value() << std::endl;
                 throw;
             }
-            std::cout << "first residual 4-2: " << step_4_gmres_control_2.initial_value() << std::endl;
-            std::cout << "last residual 4-2: " << step_4_gmres_control_2.last_value() << std::endl;
-            std::cout << "Iterations 4-2: " << step_4_gmres_control_2.last_step() << std::endl;
         }
         else if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
         {
@@ -561,9 +555,7 @@ namespace SAND {
                     std::cout << "last residual: " << step_5_gmres_control_1.last_value() << std::endl;
                     throw;
                 }
-                std::cout << "first residual 5-1: " << step_5_gmres_control_1.initial_value() << std::endl;
-                std::cout << "last residual 5-1: " << step_5_gmres_control_1.last_value() << std::endl;
-                std::cout << "Iterations 5-1: " << step_5_gmres_control_1.last_step() << std::endl;
+
 
                 SolverControl step_5_gmres_control_2 (100000, pre_k.l2_norm()*1e-6);
                 SolverGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
@@ -576,9 +568,6 @@ namespace SAND {
                     std::cout << "last residual: " << step_5_gmres_control_2.last_value() << std::endl;
                     throw;
                 }
-                std::cout << "first residual 5-2: " << step_5_gmres_control_2.initial_value() << std::endl;
-                std::cout << "last residual 5-2: " << step_5_gmres_control_2.last_value() << std::endl;
-                std::cout << "Iterations 5-2: " << step_5_gmres_control_2.last_step() << std::endl;
             }
             else if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
             {

From c2c2ea005e0c7971024881d9f9117c85e34bb444 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Thu, 11 Nov 2021 17:16:21 -0700
Subject: [PATCH 27/95] RHS calculated, other outputs trashed, RHS norm output

---
 source/kkt_system.cc | 33 ++++++++++++++++++++++-----------
 1 file changed, 22 insertions(+), 11 deletions(-)

diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index 6ca50ca..0e96f59 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -1233,19 +1233,30 @@ namespace SAND {
         norm += std::pow(test_rhs.block(SolutionBlocks::total_volume_multiplier).l2_norm(), 2);
         norm += std::pow(test_rhs.block(SolutionBlocks::density_upper_slack_multiplier).l2_norm(), 2);
         norm += std::pow(test_rhs.block(SolutionBlocks::density_lower_slack_multiplier).l2_norm(), 2);
-        std::cout << "!!!!!!!!!!!!!!!!!!" << std::endl;
-        for (unsigned int k = 0; k < state.block(SolutionBlocks::density_upper_slack).size(); k++) {
-            norm += state.block(SolutionBlocks::density_upper_slack)[k] *
-                    state.block(SolutionBlocks::density_upper_slack_multiplier)[k]
-                    * state.block(SolutionBlocks::density_upper_slack)[k] *
-                    state.block(SolutionBlocks::density_upper_slack_multiplier)[k];
+        distributed_solution = state;
+        for (unsigned int k = 0; k < state.block(SolutionBlocks::density_upper_slack).size(); k++)
+        {
+            if(distributed_solution.block(SolutionBlocks::density_upper_slack).in_local_range(k))
+            {
+                norm += distributed_solution.block(SolutionBlocks::density_upper_slack)[k] *
+                    distributed_solution.block(SolutionBlocks::density_upper_slack_multiplier)[k]
+                    * distributed_solution.block(SolutionBlocks::density_upper_slack)[k] *
+                    distributed_solution.block(SolutionBlocks::density_upper_slack_multiplier)[k];
+            }
         }
-        for (unsigned int k = 0; k < state.block(SolutionBlocks::density_lower_slack).size(); k++) {
-            norm += state.block(SolutionBlocks::density_lower_slack)[k] *
-                    state.block(SolutionBlocks::density_lower_slack_multiplier)[k]
-                    * state.block(SolutionBlocks::density_lower_slack)[k] *
-                    state.block(SolutionBlocks::density_lower_slack_multiplier)[k];
+        for (unsigned int k = 0; k < state.block(SolutionBlocks::density_lower_slack).size(); k++)
+        {
+            if(distributed_solution.block(SolutionBlocks::density_upper_slack).in_local_range(k))
+            {
+                norm += distributed_solution.block(SolutionBlocks::density_lower_slack)[k] *
+                        distributed_solution.block(SolutionBlocks::density_lower_slack_multiplier)[k]
+                        * distributed_solution.block(SolutionBlocks::density_lower_slack)[k] *
+                        distributed_solution.block(SolutionBlocks::density_lower_slack_multiplier)[k];
+            }
         }
+
+        std::cout << "norm " << norm << std::endl;
+
         return norm;
     }
 

From c17c3027850edfb0eb2df9a0ade598698757ce74 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Thu, 11 Nov 2021 19:09:20 -0700
Subject: [PATCH 28/95] To Schur Preconditioner Before Breaking!!!!!!!!!!

---
 source/kkt_system.cc | 620 ++++++++++++++++++++++---------------------
 1 file changed, 321 insertions(+), 299 deletions(-)

diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index 0e96f59..8ba8166 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -813,300 +813,312 @@ namespace SAND {
                                                                                                       cell->measure());
         }
 
-
-        LA::MPI::BlockVector filtered_unfiltered_density_solution = state;
-        LA::MPI::BlockVector filter_adjoint_unfiltered_density_multiplier_solution = state;
+        distributed_solution = state;
+        LA::MPI::BlockVector filtered_unfiltered_density_solution = distributed_solution;
+        LA::MPI::BlockVector filter_adjoint_unfiltered_density_multiplier_solution = distributed_solution;
         filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = 0;
         filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier) = 0;
 
-        system_matrix.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).vmult(
+        std::cout << "******************BEFORE" << std::endl;
+
+        system_matrix.block(SolutionBlocks::unfiltered_density_multiplier,
+                            SolutionBlocks::unfiltered_density).vmult(
                 filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density),
-                state.block(SolutionBlocks::unfiltered_density));
-        system_matrix.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).Tvmult(filter_adjoint_unfiltered_density_multiplier_solution.block(
+                distributed_solution.block(SolutionBlocks::unfiltered_density));
+        system_matrix.block(SolutionBlocks::unfiltered_density_multiplier,
+                            SolutionBlocks::unfiltered_density).Tvmult(filter_adjoint_unfiltered_density_multiplier_solution.block(
                                                     SolutionBlocks::unfiltered_density_multiplier),
-                                            state.block(SolutionBlocks::unfiltered_density_multiplier));
-
+                                            distributed_solution.block(SolutionBlocks::unfiltered_density_multiplier));
+        std::cout << "******************AFTER" << std::endl;
 
+        LA::MPI::BlockVector relevant_filtered_unfiltered_density_solution = locally_relevant_solution;
+        LA::MPI::BlockVector relevant_filter_adjoint_unfiltered_density_multiplier_solution = locally_relevant_solution;
+        relevant_filtered_unfiltered_density_solution =filtered_unfiltered_density_solution;
+        relevant_filter_adjoint_unfiltered_density_multiplier_solution = filter_adjoint_unfiltered_density_multiplier_solution;
         for (const auto &cell: dof_handler.active_cell_iterators()) {
-            hp_fe_values.reinit(cell);
-            const FEValues<dim> &fe_values = hp_fe_values.get_present_fe_values();
-            cell_matrix.reinit(cell->get_fe().n_dofs_per_cell(),
-                               cell->get_fe().n_dofs_per_cell());
-            cell_rhs.reinit(cell->get_fe().n_dofs_per_cell());
-
-            const unsigned int n_q_points = fe_values.n_quadrature_points;
-
-            std::vector<double> old_density_values(n_q_points);
-            std::vector<Tensor<1, dim>> old_displacement_values(n_q_points);
-            std::vector<double> old_displacement_divs(n_q_points);
-            std::vector<SymmetricTensor<2, dim>> old_displacement_symmgrads(
-                    n_q_points);
-            std::vector<Tensor<1, dim>> old_displacement_multiplier_values(
-                    n_q_points);
-            std::vector<double> old_displacement_multiplier_divs(n_q_points);
-            std::vector<SymmetricTensor<2, dim>> old_displacement_multiplier_symmgrads(
-                    n_q_points);
-            std::vector<double> old_lower_slack_multiplier_values(n_q_points);
-            std::vector<double> old_upper_slack_multiplier_values(n_q_points);
-            std::vector<double> old_lower_slack_values(n_q_points);
-            std::vector<double> old_upper_slack_values(n_q_points);
-            std::vector<double> old_unfiltered_density_values(n_q_points);
-            std::vector<double> old_unfiltered_density_multiplier_values(n_q_points);
-            std::vector<double> filtered_unfiltered_density_values(n_q_points);
-            std::vector<double> filter_adjoint_unfiltered_density_multiplier_values(n_q_points);
-            std::vector<double> lambda_values(n_q_points);
-            std::vector<double> mu_values(n_q_points);
-
-            const unsigned int dofs_per_cell = cell->get_fe().n_dofs_per_cell();
-
-            cell_matrix = 0;
-            cell_rhs = 0;
-            local_dof_indices.resize(cell->get_fe().n_dofs_per_cell());
-            cell->get_dof_indices(local_dof_indices);
-
-            lambda.value_list(fe_values.get_quadrature_points(), lambda_values);
-            mu.value_list(fe_values.get_quadrature_points(), mu_values);
-
-            fe_values[densities].get_function_values(state,
-                                                     old_density_values);
-            fe_values[displacements].get_function_values(state,
-                                                         old_displacement_values);
-            fe_values[displacements].get_function_divergences(state,
-                                                              old_displacement_divs);
-            fe_values[displacements].get_function_symmetric_gradients(
-                    state, old_displacement_symmgrads);
-            fe_values[displacement_multipliers].get_function_values(
-                    state, old_displacement_multiplier_values);
-            fe_values[displacement_multipliers].get_function_divergences(
-                    state, old_displacement_multiplier_divs);
-            fe_values[displacement_multipliers].get_function_symmetric_gradients(
-                    state, old_displacement_multiplier_symmgrads);
-            fe_values[density_lower_slacks].get_function_values(
-                    state, old_lower_slack_values);
-            fe_values[density_lower_slack_multipliers].get_function_values(
-                    state, old_lower_slack_multiplier_values);
-            fe_values[density_upper_slacks].get_function_values(
-                    state, old_upper_slack_values);
-            fe_values[density_upper_slack_multipliers].get_function_values(
-                    state, old_upper_slack_multiplier_values);
-            fe_values[unfiltered_densities].get_function_values(
-                    state, old_unfiltered_density_values);
-            fe_values[unfiltered_density_multipliers].get_function_values(
-                    state, old_unfiltered_density_multiplier_values);
-            fe_values[unfiltered_densities].get_function_values(
-                    filtered_unfiltered_density_solution, filtered_unfiltered_density_values);
-            fe_values[unfiltered_density_multipliers].get_function_values(
-                    filter_adjoint_unfiltered_density_multiplier_solution,
-                    filter_adjoint_unfiltered_density_multiplier_values);
-
-            Tensor<1, dim> traction;
-            traction[1] = -1;
-
-            for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
-
-                for (unsigned int i = 0; i < dofs_per_cell; ++i) {
-                    const SymmetricTensor<2, dim> displacement_phi_i_symmgrad =
-                            fe_values[displacements].symmetric_gradient(i, q_point);
-                    const double displacement_phi_i_div =
-                            fe_values[displacements].divergence(i, q_point);
-
-                    const SymmetricTensor<2, dim> displacement_multiplier_phi_i_symmgrad =
-                            fe_values[displacement_multipliers].symmetric_gradient(i,
-                                                                                   q_point);
-                    const double displacement_multiplier_phi_i_div =
-                            fe_values[displacement_multipliers].divergence(i,
-                                                                           q_point);
+            if(cell->is_locally_owned())
+            {
+                hp_fe_values.reinit(cell);
+                const FEValues<dim> &fe_values = hp_fe_values.get_present_fe_values();
+                cell_matrix.reinit(cell->get_fe().n_dofs_per_cell(),
+                                   cell->get_fe().n_dofs_per_cell());
+                cell_rhs.reinit(cell->get_fe().n_dofs_per_cell());
 
+                const unsigned int n_q_points = fe_values.n_quadrature_points;
 
-                    const double density_phi_i = fe_values[densities].value(i,
-                                                                            q_point);
-                    const double unfiltered_density_phi_i = fe_values[unfiltered_densities].value(i,
-                                                                                                  q_point);
-                    const double unfiltered_density_multiplier_phi_i = fe_values[unfiltered_density_multipliers].value(
-                            i, q_point);
+                std::vector<double> old_density_values(n_q_points);
+                std::vector<Tensor<1, dim>> old_displacement_values(n_q_points);
+                std::vector<double> old_displacement_divs(n_q_points);
+                std::vector<SymmetricTensor<2, dim>> old_displacement_symmgrads(
+                        n_q_points);
+                std::vector<Tensor<1, dim>> old_displacement_multiplier_values(
+                        n_q_points);
+                std::vector<double> old_displacement_multiplier_divs(n_q_points);
+                std::vector<SymmetricTensor<2, dim>> old_displacement_multiplier_symmgrads(
+                        n_q_points);
+                std::vector<double> old_lower_slack_multiplier_values(n_q_points);
+                std::vector<double> old_upper_slack_multiplier_values(n_q_points);
+                std::vector<double> old_lower_slack_values(n_q_points);
+                std::vector<double> old_upper_slack_values(n_q_points);
+                std::vector<double> old_unfiltered_density_values(n_q_points);
+                std::vector<double> old_unfiltered_density_multiplier_values(n_q_points);
+                std::vector<double> filtered_unfiltered_density_values(n_q_points);
+                std::vector<double> filter_adjoint_unfiltered_density_multiplier_values(n_q_points);
+                std::vector<double> lambda_values(n_q_points);
+                std::vector<double> mu_values(n_q_points);
 
-                    const double lower_slack_multiplier_phi_i =
-                            fe_values[density_lower_slack_multipliers].value(i,
-                                                                             q_point);
+                const unsigned int dofs_per_cell = cell->get_fe().n_dofs_per_cell();
 
-                    const double lower_slack_phi_i =
-                            fe_values[density_lower_slacks].value(i, q_point);
+                cell_matrix = 0;
+                cell_rhs = 0;
+                local_dof_indices.resize(cell->get_fe().n_dofs_per_cell());
+                cell->get_dof_indices(local_dof_indices);
 
-                    const double upper_slack_phi_i =
-                            fe_values[density_upper_slacks].value(i, q_point);
+                lambda.value_list(fe_values.get_quadrature_points(), lambda_values);
+                mu.value_list(fe_values.get_quadrature_points(), mu_values);
 
-                    const double upper_slack_multiplier_phi_i =
-                            fe_values[density_upper_slack_multipliers].value(i,
-                                                                             q_point);
+                fe_values[densities].get_function_values(state,
+                                                         old_density_values);
+                fe_values[displacements].get_function_values(state,
+                                                             old_displacement_values);
+                fe_values[displacements].get_function_divergences(state,
+                                                                  old_displacement_divs);
+                fe_values[displacements].get_function_symmetric_gradients(
+                        state, old_displacement_symmgrads);
+                fe_values[displacement_multipliers].get_function_values(
+                        state, old_displacement_multiplier_values);
+                fe_values[displacement_multipliers].get_function_divergences(
+                        state, old_displacement_multiplier_divs);
+                fe_values[displacement_multipliers].get_function_symmetric_gradients(
+                        state, old_displacement_multiplier_symmgrads);
+                fe_values[density_lower_slacks].get_function_values(
+                        state, old_lower_slack_values);
+                fe_values[density_lower_slack_multipliers].get_function_values(
+                        state, old_lower_slack_multiplier_values);
+                fe_values[density_upper_slacks].get_function_values(
+                        state, old_upper_slack_values);
+                fe_values[density_upper_slack_multipliers].get_function_values(
+                        state, old_upper_slack_multiplier_values);
+                fe_values[unfiltered_densities].get_function_values(
+                        state, old_unfiltered_density_values);
+                fe_values[unfiltered_density_multipliers].get_function_values(
+                        state, old_unfiltered_density_multiplier_values);
+                fe_values[unfiltered_densities].get_function_values(
+                        relevant_filtered_unfiltered_density_solution, filtered_unfiltered_density_values);
+                fe_values[unfiltered_density_multipliers].get_function_values(
+                        relevant_filter_adjoint_unfiltered_density_multiplier_solution,
+                        filter_adjoint_unfiltered_density_multiplier_values);
 
+                Tensor<1, dim> traction;
+                traction[1] = -1;
 
-                    for (unsigned int j = 0; j < dofs_per_cell; ++j) {
-                        const SymmetricTensor<2, dim> displacement_phi_j_symmgrad =
-                                fe_values[displacements].symmetric_gradient(j,
-                                                                            q_point);
-                        const double displacement_phi_j_div =
-                                fe_values[displacements].divergence(j, q_point);
+                for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
 
-                        const SymmetricTensor<2, dim> displacement_multiplier_phi_j_symmgrad =
-                                fe_values[displacement_multipliers].symmetric_gradient(
-                                        j, q_point);
-                        const double displacement_multiplier_phi_j_div =
-                                fe_values[displacement_multipliers].divergence(j,
+                    for (unsigned int i = 0; i < dofs_per_cell; ++i) {
+                        const SymmetricTensor<2, dim> displacement_phi_i_symmgrad =
+                                fe_values[displacements].symmetric_gradient(i, q_point);
+                        const double displacement_phi_i_div =
+                                fe_values[displacements].divergence(i, q_point);
+
+                        const SymmetricTensor<2, dim> displacement_multiplier_phi_i_symmgrad =
+                                fe_values[displacement_multipliers].symmetric_gradient(i,
+                                                                                       q_point);
+                        const double displacement_multiplier_phi_i_div =
+                                fe_values[displacement_multipliers].divergence(i,
                                                                                q_point);
 
-                        const double density_phi_j = fe_values[densities].value(
-                                j, q_point);
 
-                        const double unfiltered_density_phi_j = fe_values[unfiltered_densities].value(j,
+                        const double density_phi_i = fe_values[densities].value(i,
+                                                                                q_point);
+                        const double unfiltered_density_phi_i = fe_values[unfiltered_densities].value(i,
                                                                                                       q_point);
-                        const double unfiltered_density_multiplier_phi_j = fe_values[unfiltered_density_multipliers].value(
-                                j, q_point);
-
-
-                        const double lower_slack_phi_j =
-                                fe_values[density_lower_slacks].value(j, q_point);
-
-                        const double upper_slack_phi_j =
-                                fe_values[density_upper_slacks].value(j, q_point);
-
-                        const double lower_slack_multiplier_phi_j =
-                                fe_values[density_lower_slack_multipliers].value(j,
-                                                                                 q_point);
+                        const double unfiltered_density_multiplier_phi_i = fe_values[unfiltered_density_multipliers].value(
+                                i, q_point);
 
-                        const double upper_slack_multiplier_phi_j =
-                                fe_values[density_upper_slack_multipliers].value(j,
+                        const double lower_slack_multiplier_phi_i =
+                                fe_values[density_lower_slack_multipliers].value(i,
                                                                                  q_point);
 
-                        //Equation 0
-                        cell_matrix(i, j) +=
-                                fe_values.JxW(q_point) *
-                                (
-                                        -density_phi_i * unfiltered_density_multiplier_phi_j
-
-                                        - density_penalty_exponent * (density_penalty_exponent - 1)
-                                          * std::pow(
-                                                old_density_values[q_point],
-                                                density_penalty_exponent - 2)
-                                          * density_phi_i
-                                          * density_phi_j
-                                          * (old_displacement_multiplier_divs[q_point] * old_displacement_divs[q_point]
-                                             * lambda_values[q_point]
-                                             + 2 * mu_values[q_point]
-                                               * (old_displacement_symmgrads[q_point] *
-                                                  old_displacement_multiplier_symmgrads[q_point]))
-
-                                        - density_penalty_exponent * std::pow(
-                                                old_density_values[q_point],
-                                                density_penalty_exponent - 1)
-                                          * density_phi_i
-                                          * (displacement_multiplier_phi_j_div * old_displacement_divs[q_point]
-                                             * lambda_values[q_point]
-                                             + 2 * mu_values[q_point]
-                                               *
-                                               (old_displacement_symmgrads[q_point] *
-                                                displacement_multiplier_phi_j_symmgrad))
-
-                                        - density_penalty_exponent * std::pow(
-                                                old_density_values[q_point],
-                                                density_penalty_exponent - 1)
-                                          * density_phi_i
-                                          * (displacement_phi_j_div * old_displacement_multiplier_divs[q_point]
-                                             * lambda_values[q_point]
-                                             + 2 * mu_values[q_point]
-                                               * (old_displacement_multiplier_symmgrads[q_point] *
-                                                  displacement_phi_j_symmgrad)));
-                        //Equation 1
-
-                        cell_matrix(i, j) +=
-                                fe_values.JxW(q_point) * (
-                                        -density_penalty_exponent * std::pow(
-                                                old_density_values[q_point],
-                                                density_penalty_exponent - 1)
-                                        * density_phi_j
-                                        * (old_displacement_multiplier_divs[q_point] * displacement_phi_i_div
-                                           * lambda_values[q_point]
-                                           + 2 * mu_values[q_point]
-                                             * (old_displacement_multiplier_symmgrads[q_point] *
-                                                displacement_phi_i_symmgrad))
+                        const double lower_slack_phi_i =
+                                fe_values[density_lower_slacks].value(i, q_point);
 
-                                        - std::pow(old_density_values[q_point],
-                                                   density_penalty_exponent)
-                                          * (displacement_multiplier_phi_j_div * displacement_phi_i_div
-                                             * lambda_values[q_point]
-                                             + 2 * mu_values[q_point]
-                                               * (displacement_multiplier_phi_j_symmgrad * displacement_phi_i_symmgrad))
+                        const double upper_slack_phi_i =
+                                fe_values[density_upper_slacks].value(i, q_point);
 
-                                );
+                        const double upper_slack_multiplier_phi_i =
+                                fe_values[density_upper_slack_multipliers].value(i,
+                                                                                 q_point);
 
-                        //Equation 2 has to do with the filter, which is calculated elsewhere.
-                        cell_matrix(i, j) +=
-                                fe_values.JxW(q_point) * (
-                                        -1 * unfiltered_density_phi_i * lower_slack_multiplier_phi_j
-                                        + unfiltered_density_phi_i * upper_slack_multiplier_phi_j);
 
-                        //Equation 3 - Primal Feasibility
+                        for (unsigned int j = 0; j < dofs_per_cell; ++j) {
+                            const SymmetricTensor<2, dim> displacement_phi_j_symmgrad =
+                                    fe_values[displacements].symmetric_gradient(j,
+                                                                                q_point);
+                            const double displacement_phi_j_div =
+                                    fe_values[displacements].divergence(j, q_point);
+
+                            const SymmetricTensor<2, dim> displacement_multiplier_phi_j_symmgrad =
+                                    fe_values[displacement_multipliers].symmetric_gradient(
+                                            j, q_point);
+                            const double displacement_multiplier_phi_j_div =
+                                    fe_values[displacement_multipliers].divergence(j,
+                                                                                   q_point);
 
-                        cell_matrix(i, j) +=
-                                fe_values.JxW(q_point) * (
+                            const double density_phi_j = fe_values[densities].value(
+                                    j, q_point);
+
+                            const double unfiltered_density_phi_j = fe_values[unfiltered_densities].value(j,
+                                                                                                          q_point);
+                            const double unfiltered_density_multiplier_phi_j = fe_values[unfiltered_density_multipliers].value(
+                                    j, q_point);
+
+
+                            const double lower_slack_phi_j =
+                                    fe_values[density_lower_slacks].value(j, q_point);
+
+                            const double upper_slack_phi_j =
+                                    fe_values[density_upper_slacks].value(j, q_point);
+
+                            const double lower_slack_multiplier_phi_j =
+                                    fe_values[density_lower_slack_multipliers].value(j,
+                                                                                     q_point);
+
+                            const double upper_slack_multiplier_phi_j =
+                                    fe_values[density_upper_slack_multipliers].value(j,
+                                                                                     q_point);
+
+                            //Equation 0
+                            cell_matrix(i, j) +=
+                                    fe_values.JxW(q_point) *
+                                    (
+                                            -density_phi_i * unfiltered_density_multiplier_phi_j
+
+                                            - density_penalty_exponent * (density_penalty_exponent - 1)
+                                              * std::pow(
+                                                    old_density_values[q_point],
+                                                    density_penalty_exponent - 2)
+                                              * density_phi_i
+                                              * density_phi_j
+                                              * (old_displacement_multiplier_divs[q_point] * old_displacement_divs[q_point]
+                                                 * lambda_values[q_point]
+                                                 + 2 * mu_values[q_point]
+                                                   * (old_displacement_symmgrads[q_point] *
+                                                      old_displacement_multiplier_symmgrads[q_point]))
+
+                                            - density_penalty_exponent * std::pow(
+                                                    old_density_values[q_point],
+                                                    density_penalty_exponent - 1)
+                                              * density_phi_i
+                                              * (displacement_multiplier_phi_j_div * old_displacement_divs[q_point]
+                                                 * lambda_values[q_point]
+                                                 + 2 * mu_values[q_point]
+                                                   *
+                                                   (old_displacement_symmgrads[q_point] *
+                                                    displacement_multiplier_phi_j_symmgrad))
+
+                                            - density_penalty_exponent * std::pow(
+                                                    old_density_values[q_point],
+                                                    density_penalty_exponent - 1)
+                                              * density_phi_i
+                                              * (displacement_phi_j_div * old_displacement_multiplier_divs[q_point]
+                                                 * lambda_values[q_point]
+                                                 + 2 * mu_values[q_point]
+                                                   * (old_displacement_multiplier_symmgrads[q_point] *
+                                                      displacement_phi_j_symmgrad)));
+                            //Equation 1
+
+                            cell_matrix(i, j) +=
+                                    fe_values.JxW(q_point) * (
+                                            -density_penalty_exponent * std::pow(
+                                                    old_density_values[q_point],
+                                                    density_penalty_exponent - 1)
+                                            * density_phi_j
+                                            * (old_displacement_multiplier_divs[q_point] * displacement_phi_i_div
+                                               * lambda_values[q_point]
+                                               + 2 * mu_values[q_point]
+                                                 * (old_displacement_multiplier_symmgrads[q_point] *
+                                                    displacement_phi_i_symmgrad))
+
+                                            - std::pow(old_density_values[q_point],
+                                                       density_penalty_exponent)
+                                              * (displacement_multiplier_phi_j_div * displacement_phi_i_div
+                                                 * lambda_values[q_point]
+                                                 + 2 * mu_values[q_point]
+                                                   * (displacement_multiplier_phi_j_symmgrad * displacement_phi_i_symmgrad))
+
+                                    );
+
+                            //Equation 2 has to do with the filter, which is calculated elsewhere.
+                            cell_matrix(i, j) +=
+                                    fe_values.JxW(q_point) * (
+                                            -1 * unfiltered_density_phi_i * lower_slack_multiplier_phi_j
+                                            + unfiltered_density_phi_i * upper_slack_multiplier_phi_j);
+
+                            //Equation 3 - Primal Feasibility
+
+                            cell_matrix(i, j) +=
+                                    fe_values.JxW(q_point) * (
+
+                                            -1 * density_penalty_exponent * std::pow(
+                                                    old_density_values[q_point],
+                                                    density_penalty_exponent - 1)
+                                            * density_phi_j
+                                            * (old_displacement_divs[q_point] * displacement_multiplier_phi_i_div
+                                               * lambda_values[q_point]
+                                               + 2 * mu_values[q_point]
+                                                 * (old_displacement_symmgrads[q_point] *
+                                                    displacement_multiplier_phi_i_symmgrad))
+
+                                            + -1 * std::pow(old_density_values[q_point],
+                                                            density_penalty_exponent)
+                                              * (displacement_phi_j_div * displacement_multiplier_phi_i_div
+                                                 * lambda_values[q_point]
+                                                 + 2 * mu_values[q_point]
+                                                   *
+                                                   (displacement_phi_j_symmgrad * displacement_multiplier_phi_i_symmgrad)));
+
+                            //Equation 4 - more primal feasibility
+                            cell_matrix(i, j) +=
+                                    -1 * fe_values.JxW(q_point) * lower_slack_multiplier_phi_i *
+                                    (unfiltered_density_phi_j - lower_slack_phi_j);
+
+                            //Equation 5 - more primal feasibility
+                            cell_matrix(i, j) +=
+                                    -1 * fe_values.JxW(q_point) * upper_slack_multiplier_phi_i * (
+                                            -1 * unfiltered_density_phi_j - upper_slack_phi_j);
+
+                            //Equation 6 - more primal feasibility - part with filter added later
+                            cell_matrix(i, j) +=
+                                    -1 * fe_values.JxW(q_point) * unfiltered_density_multiplier_phi_i * (
+                                            density_phi_j);
+
+                            //Equation 7 - complementary slackness
+                            cell_matrix(i, j) += fe_values.JxW(q_point) *
+                                                 (lower_slack_phi_i * lower_slack_multiplier_phi_j
+                                                  + lower_slack_phi_i * lower_slack_phi_j *
+                                                    old_lower_slack_multiplier_values[q_point] /
+                                                    old_lower_slack_values[q_point]);
+                            //Equation 8 - complementary slackness
+                            cell_matrix(i, j) += fe_values.JxW(q_point) *
+                                                 (upper_slack_phi_i * upper_slack_multiplier_phi_j
+                                                  + upper_slack_phi_i * upper_slack_phi_j *
+                                                    old_upper_slack_multiplier_values[q_point] /
+                                                    old_upper_slack_values[q_point]);
+                        }
 
-                                        -1 * density_penalty_exponent * std::pow(
-                                                old_density_values[q_point],
-                                                density_penalty_exponent - 1)
-                                        * density_phi_j
-                                        * (old_displacement_divs[q_point] * displacement_multiplier_phi_i_div
-                                           * lambda_values[q_point]
-                                           + 2 * mu_values[q_point]
-                                             * (old_displacement_symmgrads[q_point] *
-                                                displacement_multiplier_phi_i_symmgrad))
-
-                                        + -1 * std::pow(old_density_values[q_point],
-                                                        density_penalty_exponent)
-                                          * (displacement_phi_j_div * displacement_multiplier_phi_i_div
-                                             * lambda_values[q_point]
-                                             + 2 * mu_values[q_point]
-                                               *
-                                               (displacement_phi_j_symmgrad * displacement_multiplier_phi_i_symmgrad)));
-
-                        //Equation 4 - more primal feasibility
-                        cell_matrix(i, j) +=
-                                -1 * fe_values.JxW(q_point) * lower_slack_multiplier_phi_i *
-                                (unfiltered_density_phi_j - lower_slack_phi_j);
-
-                        //Equation 5 - more primal feasibility
-                        cell_matrix(i, j) +=
-                                -1 * fe_values.JxW(q_point) * upper_slack_multiplier_phi_i * (
-                                        -1 * unfiltered_density_phi_j - upper_slack_phi_j);
-
-                        //Equation 6 - more primal feasibility - part with filter added later
-                        cell_matrix(i, j) +=
-                                -1 * fe_values.JxW(q_point) * unfiltered_density_multiplier_phi_i * (
-                                        density_phi_j);
-
-                        //Equation 7 - complementary slackness
-                        cell_matrix(i, j) += fe_values.JxW(q_point) *
-                                             (lower_slack_phi_i * lower_slack_multiplier_phi_j
-                                              + lower_slack_phi_i * lower_slack_phi_j *
-                                                old_lower_slack_multiplier_values[q_point] /
-                                                old_lower_slack_values[q_point]);
-                        //Equation 8 - complementary slackness
-                        cell_matrix(i, j) += fe_values.JxW(q_point) *
-                                             (upper_slack_phi_i * upper_slack_multiplier_phi_j
-                                              + upper_slack_phi_i * upper_slack_phi_j *
-                                                old_upper_slack_multiplier_values[q_point] /
-                                                old_upper_slack_values[q_point]);
                     }
 
                 }
 
-            }
 
+                MatrixTools::local_apply_boundary_values(boundary_values, local_dof_indices,
+                                                         cell_matrix, cell_rhs, true);
 
-            MatrixTools::local_apply_boundary_values(boundary_values, local_dof_indices,
-                                                     cell_matrix, cell_rhs, true);
 
+                constraints.distribute_local_to_global(
+                        cell_matrix, cell_rhs, local_dof_indices, system_matrix, system_rhs);
 
-            constraints.distribute_local_to_global(
-                    cell_matrix, cell_rhs, local_dof_indices, system_matrix, system_rhs);
+            }
 
         }
         system_matrix.compress(VectorOperation::add);
@@ -1150,48 +1162,53 @@ namespace SAND {
 
         Tensor<1, dim> traction;
         traction[1] = -1;
-
+        distributed_solution = state;
         double objective_value = 0;
         for (const auto &cell: dof_handler.active_cell_iterators()) {
-            hp_fe_values.reinit(cell);
-            const FEValues<dim> &fe_values = hp_fe_values.get_present_fe_values();
-            const unsigned int dofs_per_cell = cell->get_fe().n_dofs_per_cell();
-            const unsigned int n_q_points = fe_values.n_quadrature_points;
-            const unsigned int n_face_q_points = common_face_quadrature.size();
-
-            std::vector<Tensor<1, dim>> old_displacement_values(n_q_points);
-            fe_values[displacements].get_function_values(
-                    state, old_displacement_values);
-
-            for (unsigned int face_number = 0;
-                 face_number < GeometryInfo<dim>::faces_per_cell;
-                 ++face_number) {
-                if (cell->face(face_number)->at_boundary() && cell->face(face_number)->boundary_id()
-                                                              == BoundaryIds::down_force) {
-
-
-                    for (unsigned int face_q_point = 0;
-                         face_q_point < n_face_q_points; ++face_q_point) {
-                        for (unsigned int i = 0; i < dofs_per_cell; ++i) {
-                            if (cell->material_id() == MaterialIds::without_multiplier) {
-                                fe_nine_face_values.reinit(cell, face_number);
-                                objective_value += traction
-                                                   * fe_nine_face_values[displacements].value(i,
-                                                                                              face_q_point)
-                                                   * fe_nine_face_values.JxW(face_q_point);
-                            } else {
-                                fe_ten_face_values.reinit(cell, face_number);
-                                objective_value += traction
-                                                   * fe_ten_face_values[displacements].value(i,
-                                                                                             face_q_point)
-                                                   * fe_ten_face_values.JxW(face_q_point);
+            if(cell->is_locally_owned())
+            {
+                hp_fe_values.reinit(cell);
+                const FEValues<dim> &fe_values = hp_fe_values.get_present_fe_values();
+                const unsigned int dofs_per_cell = cell->get_fe().n_dofs_per_cell();
+                const unsigned int n_q_points = fe_values.n_quadrature_points;
+                const unsigned int n_face_q_points = common_face_quadrature.size();
+
+                std::vector<Tensor<1, dim>> old_displacement_values(n_q_points);
+                fe_values[displacements].get_function_values(
+                        state, old_displacement_values);
+
+                for (unsigned int face_number = 0;
+                     face_number < GeometryInfo<dim>::faces_per_cell;
+                     ++face_number) {
+                    if (cell->face(face_number)->at_boundary() && cell->face(face_number)->boundary_id()
+                                                                  == BoundaryIds::down_force) {
+
+
+                        for (unsigned int face_q_point = 0;
+                             face_q_point < n_face_q_points; ++face_q_point) {
+                            for (unsigned int i = 0; i < dofs_per_cell; ++i) {
+                                if (cell->material_id() == MaterialIds::without_multiplier) {
+                                    fe_nine_face_values.reinit(cell, face_number);
+                                    objective_value += traction
+                                                       * fe_nine_face_values[displacements].value(i,
+                                                                                                  face_q_point)
+                                                       * fe_nine_face_values.JxW(face_q_point);
+                                } else {
+                                    fe_ten_face_values.reinit(cell, face_number);
+                                    objective_value += traction
+                                                       * fe_ten_face_values[displacements].value(i,
+                                                                                                 face_q_point)
+                                                       * fe_ten_face_values.JxW(face_q_point);
+                                }
                             }
                         }
                     }
-                }
 
+                }
             }
+
         }
+        std::cout << "objective value: - NEED TO ADD THESE " << objective_value << std::endl;
         return objective_value;
     }
 
@@ -1202,12 +1219,17 @@ namespace SAND {
     KktSystem<dim>::calculate_barrier_distance(const LA::MPI::BlockVector &state) const {
         double barrier_distance_log_sum = 0;
         unsigned int vect_size = state.block(SolutionBlocks::density_lower_slack).size();
+        distributed_solution = state;
         for (unsigned int k = 0; k < vect_size; k++) {
+            if (distributed_solution.block(SolutionBlocks::density_lower_slack).in_local_range(k))
             barrier_distance_log_sum += std::log(state.block(SolutionBlocks::density_lower_slack)[k]);
         }
         for (unsigned int k = 0; k < vect_size; k++) {
+            if (distributed_solution.block(SolutionBlocks::density_upper_slack).in_local_range(k))
             barrier_distance_log_sum += std::log(state.block(SolutionBlocks::density_upper_slack)[k]);
         }
+        std::cout << "Barrier distance log sum - NEED TO ADD THESE TOO " << barrier_distance_log_sum << std::endl;
+
         return barrier_distance_log_sum;
     }
 
@@ -1255,7 +1277,7 @@ namespace SAND {
             }
         }
 
-        std::cout << "norm " << norm << std::endl;
+        std::cout << "norm - NEED TO ADD THESE!!!" << norm << std::endl;
 
         return norm;
     }
@@ -1688,9 +1710,9 @@ namespace SAND {
             gmres_tolerance = Input::default_gmres_tolerance;
         }
         locally_relevant_solution=0;
-        constraints.distribute(locally_relevant_solution);
+        distributed_solution = state;
         SolverControl solver_control(10000, gmres_tolerance * system_rhs.l2_norm());
-
+        std::cout << "into solve" << std::endl;
         TopOptSchurPreconditioner<dim> preconditioner(system_matrix);
         switch (Input::solver_choice) {
             case SolverOptions::direct_solve: {
@@ -1698,34 +1720,34 @@ namespace SAND {
 //                std::string solver_type;
 //                TrilinosWrappers::SolverDirect::AdditionalData additional_data(false, "Amesos_Klu");
 //                TrilinosWrappers::SolverDirect solver(cn, mpi_communicator, system_matrix);
-//                solver.solve(locally_relevant_solution, system_rhs);
+//                solver.solve(distributed_solution, system_rhs);
                 break;
             }
             case SolverOptions::exact_preconditioner_with_gmres: {
                 preconditioner.initialize(system_matrix, boundary_values, dof_handler, state);
                 SolverFGMRES<LA::MPI::BlockVector> A_fgmres(solver_control);
-                A_fgmres.solve(system_matrix, locally_relevant_solution, system_rhs, preconditioner);
+                A_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
                 std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
                 break;
             }
             case SolverOptions::inexact_K_with_exact_A_gmres: {
                 preconditioner.initialize(system_matrix, boundary_values, dof_handler, state);
                 SolverFGMRES<LA::MPI::BlockVector> B_fgmres(solver_control);
-                B_fgmres.solve(system_matrix, locally_relevant_solution, system_rhs, preconditioner);
+                B_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
                 std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
                 break;
             }
             case SolverOptions::inexact_K_with_inexact_A_gmres: {
                 preconditioner.initialize(system_matrix, boundary_values, dof_handler, state);
                 SolverFGMRES<LA::MPI::BlockVector> C_fgmres(solver_control);
-                C_fgmres.solve(system_matrix, locally_relevant_solution, system_rhs, preconditioner);
+                C_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
                 std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
                 break;
             }
             default:
                 throw;
         }
-        constraints.distribute(locally_relevant_solution);
+        constraints.distribute(distributed_solution);
 
         if (Input::output_parts_of_matrix) {
             preconditioner.print_stuff();

From 22a85b81fa22e1856770dfd3450383ff80bc959f Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Fri, 12 Nov 2021 13:33:40 -0700
Subject: [PATCH 29/95] KKT seems to be working - Preconditioner Diagonals
 Giving Me Trouble.

---
 source/kkt_system.cc           | 67 +++++++++++++------------
 source/schur_preconditioner.cc | 92 +++++++++++++++++-----------------
 source/watchdog_main.cc        |  2 +
 3 files changed, 84 insertions(+), 77 deletions(-)

diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index 8ba8166..0b39187 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -819,8 +819,6 @@ namespace SAND {
         filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = 0;
         filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier) = 0;
 
-        std::cout << "******************BEFORE" << std::endl;
-
         system_matrix.block(SolutionBlocks::unfiltered_density_multiplier,
                             SolutionBlocks::unfiltered_density).vmult(
                 filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density),
@@ -829,7 +827,6 @@ namespace SAND {
                             SolutionBlocks::unfiltered_density).Tvmult(filter_adjoint_unfiltered_density_multiplier_solution.block(
                                                     SolutionBlocks::unfiltered_density_multiplier),
                                             distributed_solution.block(SolutionBlocks::unfiltered_density_multiplier));
-        std::cout << "******************AFTER" << std::endl;
 
         LA::MPI::BlockVector relevant_filtered_unfiltered_density_solution = locally_relevant_solution;
         LA::MPI::BlockVector relevant_filter_adjoint_unfiltered_density_multiplier_solution = locally_relevant_solution;
@@ -1164,7 +1161,8 @@ namespace SAND {
         traction[1] = -1;
         distributed_solution = state;
         double objective_value = 0;
-        for (const auto &cell: dof_handler.active_cell_iterators()) {
+        for (const auto &cell: dof_handler.active_cell_iterators())
+        {
             if(cell->is_locally_owned())
             {
                 hp_fe_values.reinit(cell);
@@ -1179,11 +1177,11 @@ namespace SAND {
 
                 for (unsigned int face_number = 0;
                      face_number < GeometryInfo<dim>::faces_per_cell;
-                     ++face_number) {
+                     ++face_number)
+                {
                     if (cell->face(face_number)->at_boundary() && cell->face(face_number)->boundary_id()
-                                                                  == BoundaryIds::down_force) {
-
-
+                                                                  == BoundaryIds::down_force)
+                    {
                         for (unsigned int face_q_point = 0;
                              face_q_point < n_face_q_points; ++face_q_point) {
                             for (unsigned int i = 0; i < dofs_per_cell; ++i) {
@@ -1203,12 +1201,14 @@ namespace SAND {
                             }
                         }
                     }
-
                 }
             }
-
         }
-        std::cout << "objective value: - NEED TO ADD THESE " << objective_value << std::endl;
+        double objective_value_out;
+        MPI_Reduce(&objective_value, &objective_value_out, 1, MPI_DOUBLE, MPI_SUM, 0, MPI_COMM_WORLD);
+        MPI_Bcast(&objective_value_out, 1, MPI_DOUBLE, 0, MPI_COMM_WORLD);
+
+        std::cout << "objective value: " << objective_value_out << std::endl;
         return objective_value;
     }
 
@@ -1228,9 +1228,13 @@ namespace SAND {
             if (distributed_solution.block(SolutionBlocks::density_upper_slack).in_local_range(k))
             barrier_distance_log_sum += std::log(state.block(SolutionBlocks::density_upper_slack)[k]);
         }
-        std::cout << "Barrier distance log sum - NEED TO ADD THESE TOO " << barrier_distance_log_sum << std::endl;
+        double out_barrier_distance_log_sum;
+        MPI_Reduce(&barrier_distance_log_sum, &out_barrier_distance_log_sum, 1, MPI_DOUBLE, MPI_SUM, 0, MPI_COMM_WORLD);
+        MPI_Bcast(&out_barrier_distance_log_sum, 1, MPI_DOUBLE, 0, MPI_COMM_WORLD);
 
-        return barrier_distance_log_sum;
+        std::cout << "Barrier distance log sum: " << out_barrier_distance_log_sum << std::endl;
+
+        return out_barrier_distance_log_sum;
     }
 
     template<int dim>
@@ -1247,14 +1251,7 @@ namespace SAND {
         LA::MPI::BlockVector test_rhs = calculate_rhs(state, barrier_size);
 
         double norm = 0;
-        norm += std::pow(test_rhs.block(SolutionBlocks::displacement).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::density).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::unfiltered_density).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::displacement_multiplier).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::total_volume_multiplier).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::density_upper_slack_multiplier).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::density_lower_slack_multiplier).l2_norm(), 2);
+
         distributed_solution = state;
         for (unsigned int k = 0; k < state.block(SolutionBlocks::density_upper_slack).size(); k++)
         {
@@ -1268,7 +1265,7 @@ namespace SAND {
         }
         for (unsigned int k = 0; k < state.block(SolutionBlocks::density_lower_slack).size(); k++)
         {
-            if(distributed_solution.block(SolutionBlocks::density_upper_slack).in_local_range(k))
+            if(distributed_solution.block(SolutionBlocks::density_lower_slack).in_local_range(k))
             {
                 norm += distributed_solution.block(SolutionBlocks::density_lower_slack)[k] *
                         distributed_solution.block(SolutionBlocks::density_lower_slack_multiplier)[k]
@@ -1276,8 +1273,20 @@ namespace SAND {
                         distributed_solution.block(SolutionBlocks::density_lower_slack_multiplier)[k];
             }
         }
+        double pre_norm;
+        MPI_Reduce(&norm, &pre_norm, 1, MPI_DOUBLE, MPI_SUM, 0, MPI_COMM_WORLD);
+        MPI_Bcast(&pre_norm, 1, MPI_DOUBLE, 0, MPI_COMM_WORLD);
+        norm = pre_norm;
+        norm += std::pow(test_rhs.block(SolutionBlocks::displacement).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::density).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::unfiltered_density).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::displacement_multiplier).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::total_volume_multiplier).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::density_upper_slack_multiplier).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::density_lower_slack_multiplier).l2_norm(), 2);
 
-        std::cout << "norm - NEED TO ADD THESE!!!" << norm << std::endl;
+        std::cout << "norm: " << norm << std::endl;
 
         return norm;
     }
@@ -1418,8 +1427,6 @@ namespace SAND {
             old_volume_multiplier = state.block(SolutionBlocks::total_volume_multiplier)[0];
         }
 
-        std::cout << "here" << std::endl;
-
         for (const auto &cell: dof_handler.active_cell_iterators()) {
             if(cell->is_locally_owned())
             {
@@ -1666,7 +1673,7 @@ namespace SAND {
                         cell_rhs, local_dof_indices, test_rhs);
             }
         }
-        std::cout << "++++++++++++++++" << std::endl;
+
         test_rhs.compress(VectorOperation::add);
         double total_volume = 0;
         double goal_volume = 0;
@@ -1687,7 +1694,6 @@ namespace SAND {
             test_rhs.block(SolutionBlocks::total_volume_multiplier)[0] = goal_volume - total_volume;
         }
 
-        test_rhs.print(std::cout);
         return test_rhs;
 
     }
@@ -1712,7 +1718,6 @@ namespace SAND {
         locally_relevant_solution=0;
         distributed_solution = state;
         SolverControl solver_control(10000, gmres_tolerance * system_rhs.l2_norm());
-        std::cout << "into solve" << std::endl;
         TopOptSchurPreconditioner<dim> preconditioner(system_matrix);
         switch (Input::solver_choice) {
             case SolverOptions::direct_solve: {
@@ -1731,14 +1736,14 @@ namespace SAND {
                 break;
             }
             case SolverOptions::inexact_K_with_exact_A_gmres: {
-                preconditioner.initialize(system_matrix, boundary_values, dof_handler, state);
+                preconditioner.initialize(system_matrix, boundary_values, dof_handler,  state);
                 SolverFGMRES<LA::MPI::BlockVector> B_fgmres(solver_control);
                 B_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
                 std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
                 break;
             }
             case SolverOptions::inexact_K_with_inexact_A_gmres: {
-                preconditioner.initialize(system_matrix, boundary_values, dof_handler, state);
+                preconditioner.initialize(system_matrix, boundary_values, dof_handler,  state);
                 SolverFGMRES<LA::MPI::BlockVector> C_fgmres(solver_control);
                 C_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
                 std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
@@ -2204,4 +2209,4 @@ namespace SAND {
 }
 
 template class SAND::KktSystem<2>;
-template class SAND::KktSystem<3>;
\ No newline at end of file
+template class SAND::KktSystem<3>;
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 620ce10..e3a3416 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -55,12 +55,14 @@ namespace SAND {
     {
         TimerOutput::Scope t(timer, "initialize");
         {
+
             TimerOutput::Scope t(timer, "diag stuff");
+            const types::global_dof_index disp_start_index = system_matrix.get_row_indices().block_start(
+                    SolutionBlocks::displacement);
+            const types::global_dof_index disp_mult_start_index = system_matrix.get_row_indices().block_start(
+                    SolutionBlocks::displacement_multiplier);
             for (auto&[dof_index, boundary_value]: boundary_values) {
-                const types::global_dof_index disp_start_index = system_matrix.get_row_indices().block_start(
-                        SolutionBlocks::displacement);
-                const types::global_dof_index disp_mult_start_index = system_matrix.get_row_indices().block_start(
-                        SolutionBlocks::displacement_multiplier);
+
                 const types::global_dof_index n_u = system_matrix.block(SolutionBlocks::displacement,
                                                                         SolutionBlocks::displacement).m();
                 if ((dof_index >= disp_start_index) && (dof_index < disp_start_index + n_u)) {
@@ -139,45 +141,47 @@ namespace SAND {
             d_8_mat=0;
             d_m_inv_mat=0;
         }
+        std::cout << "*************************** Diag Matrix Stuff Needs To Go Back In" << std::endl;
         {
             TimerOutput::Scope t(timer, "build diag matrices");
-            for (const auto cell: dof_handler.active_cell_iterators())
-            {
-                const double i = cell->active_cell_index();
-                const double m = cell->measure();
-                double d_3_value = -1 * state.block(SolutionBlocks::density_lower_slack_multiplier)[i] /
-                                   (m * state.block(SolutionBlocks::density_lower_slack)[i]);
-                double d_4_value = -1 * state.block(SolutionBlocks::density_upper_slack_multiplier)[i] /
-                                   (m * state.block(SolutionBlocks::density_upper_slack)[i]);
-                double d_5_value = state.block(SolutionBlocks::density_lower_slack_multiplier)[i] /
-                                   (state.block(SolutionBlocks::density_lower_slack)[i]);
-                double d_6_value = state.block(SolutionBlocks::density_upper_slack_multiplier)[i] /
-                                   (state.block(SolutionBlocks::density_upper_slack)[i]);
-                double d_7_value = (m * (state.block(SolutionBlocks::density_lower_slack_multiplier)[i] *
-                                         state.block(SolutionBlocks::density_upper_slack)[i] +
-                                         state.block(SolutionBlocks::density_upper_slack_multiplier)[i] *
-                                         state.block(SolutionBlocks::density_lower_slack)[i]))
-                                   / (state.block(SolutionBlocks::density_lower_slack)[i] *
-                                      state.block(SolutionBlocks::density_upper_slack)[i]);
-                double d_8_value = (state.block(SolutionBlocks::density_lower_slack)[i] *
-                                    state.block(SolutionBlocks::density_upper_slack)[i])
-                                   / (m * (state.block(SolutionBlocks::density_lower_slack_multiplier)[i] *
-                                           state.block(SolutionBlocks::density_upper_slack)[i] +
-                                           state.block(SolutionBlocks::density_upper_slack_multiplier)[i] *
-                                           state.block(SolutionBlocks::density_lower_slack)[i]));
-                d_3_mat.set(i, i, d_3_value);
-                d_4_mat.set(i, i, d_4_value);
-                d_5_mat.set(i, i, d_5_value);
-                d_6_mat.set(i, i, d_6_value);
-                d_7_mat.set(i, i, d_7_value);
-                d_8_mat.set(i, i, d_8_value);
-                d_m_inv_mat.set(i, i, 1 / m);
-
-            }
+//            for (const auto cell: dof_handler.active_cell_iterators())
+//            {
+//                if(cell->is_locally_owned())
+//                {
+//                    const unsigned int i = cell->active_cell_index();
+//                    std::cout << i << std::endl;
+//                    const double m = cell->measure();
+//                    double d_3_value = -1 * state.block(SolutionBlocks::density_lower_slack_multiplier)[i] /
+//                                       (m * state.block(SolutionBlocks::density_lower_slack)[i]);
+//                    double d_4_value = -1 * state.block(SolutionBlocks::density_upper_slack_multiplier)[i] /
+//                                       (m * state.block(SolutionBlocks::density_upper_slack)[i]);
+//                    double d_5_value = state.block(SolutionBlocks::density_lower_slack_multiplier)[i] /
+//                                       (state.block(SolutionBlocks::density_lower_slack)[i]);
+//                    double d_6_value = state.block(SolutionBlocks::density_upper_slack_multiplier)[i] /
+//                                       (state.block(SolutionBlocks::density_upper_slack)[i]);
+//                    double d_7_value = (m * (state.block(SolutionBlocks::density_lower_slack_multiplier)[i] *
+//                                             state.block(SolutionBlocks::density_upper_slack)[i] +
+//                                             state.block(SolutionBlocks::density_upper_slack_multiplier)[i] *
+//                                             state.block(SolutionBlocks::density_lower_slack)[i]))
+//                                       / (state.block(SolutionBlocks::density_lower_slack)[i] *
+//                                          state.block(SolutionBlocks::density_upper_slack)[i]);
+//                    double d_8_value = (state.block(SolutionBlocks::density_lower_slack)[i] *
+//                                        state.block(SolutionBlocks::density_upper_slack)[i])
+//                                       / (m * (state.block(SolutionBlocks::density_lower_slack_multiplier)[i] *
+//                                               state.block(SolutionBlocks::density_upper_slack)[i] +
+//                                               state.block(SolutionBlocks::density_upper_slack_multiplier)[i] *
+//                                               state.block(SolutionBlocks::density_lower_slack)[i]));
+//                    d_3_mat.set(i, i, d_3_value);
+//                    d_4_mat.set(i, i, d_4_value);
+//                    d_5_mat.set(i, i, d_5_value);
+//                    d_6_mat.set(i, i, d_6_value);
+//                    d_7_mat.set(i, i, d_7_value);
+//                    d_8_mat.set(i, i, d_8_value);
+//                    d_m_inv_mat.set(i, i, 1 / m);
+//                }
+//            }
         }
 
-
-
         pre_j=state.block(SolutionBlocks::density);
         pre_k=state.block(SolutionBlocks::density);
         g_d_m_inv_density=state.block(SolutionBlocks::density);
@@ -190,7 +194,6 @@ namespace SAND {
         op_d_8 = linear_operator<VectorType,VectorType,PayloadType>(d_8_mat);
         op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) * transpose_operator<VectorType, VectorType, PayloadType>(linear_operator<VectorType,VectorType,PayloadType>(f_mat));
 
-
         if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
         {
             SolverControl            solver_control(100000, 1e-6);
@@ -237,15 +240,12 @@ namespace SAND {
 //            }
         }
 
-
-
         if (Input::solver_choice == SolverOptions::exact_preconditioner_with_gmres)
         {
             TimerOutput::Scope t(timer, "invert k_mat");
             k_mat.copy_from(k_inv_mat);
             k_mat.invert();
         }
-
     }
 
 
@@ -386,14 +386,14 @@ namespace SAND {
             try {
                 k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
                                       g_d_m_inv_density;
-            } catch (std::exception &exc)
+            }
+            catch (std::exception &exc)
             {
                 std::cerr << "Failure of linear solver step_4_gmres_1" << std::endl;
                 std::cout << "first residual: " << step_4_gmres_control_1.initial_value() << std::endl;
                 std::cout << "last residual: " << step_4_gmres_control_1.last_value() << std::endl;
                 throw;
             }
-
             SolverControl step_4_gmres_control_2 (100000, src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6);
             SolverGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
             try {
@@ -847,4 +847,4 @@ namespace SAND {
 
 }
 template class SAND::TopOptSchurPreconditioner<2>;
-template class SAND::TopOptSchurPreconditioner<3>;
\ No newline at end of file
+template class SAND::TopOptSchurPreconditioner<3>;
diff --git a/source/watchdog_main.cc b/source/watchdog_main.cc
index 5b58cb0..1c4c7a6 100644
--- a/source/watchdog_main.cc
+++ b/source/watchdog_main.cc
@@ -85,6 +85,8 @@ namespace SAND {
             const LA::MPI::BlockVector state_test_z =
                     (Input::fraction_to_boundary * state) + (step_size_z * step);
 
+            std::cout << "********** NEED TO FIND A WAY TO CHECK NONNEGATIVE HERE ***************" << std::endl;
+
             const bool accept_s = (state_test_s.block(SolutionBlocks::density_lower_slack).is_non_negative())
                                   && (state_test_s.block(SolutionBlocks::density_upper_slack).is_non_negative());
             const bool accept_z = (state_test_z.block(SolutionBlocks::density_lower_slack_multiplier).is_non_negative())

From 11ece70d8d67d5e89498816454b1673da47500fd Mon Sep 17 00:00:00 2001
From: juleoc02 <juleoc@colostate.edu>
Date: Mon, 15 Nov 2021 17:15:07 -0700
Subject: [PATCH 30/95] Closer... Always closer...

---
 include/schur_preconditioner.h |   2 +-
 source/kkt_system.cc           | 102 +++++++++++++-----------
 source/schur_preconditioner.cc | 137 +++++++++++++++++++--------------
 source/watchdog_main.cc        |   9 ++-
 4 files changed, 142 insertions(+), 108 deletions(-)

diff --git a/include/schur_preconditioner.h b/include/schur_preconditioner.h
index c4d1749..11b8255 100644
--- a/include/schur_preconditioner.h
+++ b/include/schur_preconditioner.h
@@ -98,7 +98,7 @@ namespace SAND
     class TopOptSchurPreconditioner: public Subscriptor {
     public:
         TopOptSchurPreconditioner(LA::MPI::BlockSparseMatrix &matrix_in);
-        void initialize (LA::MPI::BlockSparseMatrix &matrix, const std::map<types::global_dof_index, double> &boundary_values, const DoFHandler<dim> &dof_handler, const LA::MPI::BlockVector &state);
+        void initialize (LA::MPI::BlockSparseMatrix &matrix, const std::map<types::global_dof_index, double> &boundary_values, const DoFHandler<dim> &dof_handler, const LA::MPI::BlockVector &state, const LA::MPI::BlockVector &distributed_state);
         void vmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
         void Tvmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
         void vmult_add(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index 0b39187..f3d1e0d 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -572,7 +572,6 @@ namespace SAND {
     template<int dim>
     void
     KktSystem<dim>::setup_block_system() {
-        const FEValuesExtractors::Scalar densities(SolutionComponents::density<dim>);
 
         //MAKE n_u and n_P
 
@@ -741,9 +740,12 @@ namespace SAND {
 
     template<int dim>
     void
-    KktSystem<dim>::assemble_block_system(const LA::MPI::BlockVector &state, const double barrier_size) {
+    KktSystem<dim>::assemble_block_system(const LA::MPI::BlockVector &distributed_state, const double barrier_size) {
         /*Remove any values from old iterations*/
 
+        LA::MPI::BlockVector relevant_state(owned_partitioning, relevant_partitioning, mpi_communicator);
+        relevant_state = distributed_state;
+
         system_matrix=0;
         locally_relevant_solution = 0;
         system_rhs = 0;
@@ -813,7 +815,7 @@ namespace SAND {
                                                                                                       cell->measure());
         }
 
-        distributed_solution = state;
+        distributed_solution = distributed_state;
         LA::MPI::BlockVector filtered_unfiltered_density_solution = distributed_solution;
         LA::MPI::BlockVector filter_adjoint_unfiltered_density_multiplier_solution = distributed_solution;
         filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = 0;
@@ -874,32 +876,32 @@ namespace SAND {
                 lambda.value_list(fe_values.get_quadrature_points(), lambda_values);
                 mu.value_list(fe_values.get_quadrature_points(), mu_values);
 
-                fe_values[densities].get_function_values(state,
+                fe_values[densities].get_function_values(relevant_state,
                                                          old_density_values);
-                fe_values[displacements].get_function_values(state,
+                fe_values[displacements].get_function_values(relevant_state,
                                                              old_displacement_values);
-                fe_values[displacements].get_function_divergences(state,
+                fe_values[displacements].get_function_divergences(relevant_state,
                                                                   old_displacement_divs);
                 fe_values[displacements].get_function_symmetric_gradients(
-                        state, old_displacement_symmgrads);
+                        relevant_state, old_displacement_symmgrads);
                 fe_values[displacement_multipliers].get_function_values(
-                        state, old_displacement_multiplier_values);
+                        relevant_state, old_displacement_multiplier_values);
                 fe_values[displacement_multipliers].get_function_divergences(
-                        state, old_displacement_multiplier_divs);
+                        relevant_state, old_displacement_multiplier_divs);
                 fe_values[displacement_multipliers].get_function_symmetric_gradients(
-                        state, old_displacement_multiplier_symmgrads);
+                        relevant_state, old_displacement_multiplier_symmgrads);
                 fe_values[density_lower_slacks].get_function_values(
-                        state, old_lower_slack_values);
+                        relevant_state, old_lower_slack_values);
                 fe_values[density_lower_slack_multipliers].get_function_values(
-                        state, old_lower_slack_multiplier_values);
+                        relevant_state, old_lower_slack_multiplier_values);
                 fe_values[density_upper_slacks].get_function_values(
-                        state, old_upper_slack_values);
+                        relevant_state, old_upper_slack_values);
                 fe_values[density_upper_slack_multipliers].get_function_values(
-                        state, old_upper_slack_multiplier_values);
+                        relevant_state, old_upper_slack_multiplier_values);
                 fe_values[unfiltered_densities].get_function_values(
-                        state, old_unfiltered_density_values);
+                        relevant_state, old_unfiltered_density_values);
                 fe_values[unfiltered_density_multipliers].get_function_values(
-                        state, old_unfiltered_density_multiplier_values);
+                        relevant_state, old_unfiltered_density_multiplier_values);
                 fe_values[unfiltered_densities].get_function_values(
                         relevant_filtered_unfiltered_density_solution, filtered_unfiltered_density_values);
                 fe_values[unfiltered_density_multipliers].get_function_values(
@@ -1104,10 +1106,11 @@ namespace SAND {
                         }
 
                     }
-
                 }
 
 
+                if (cell->active_cell_index()==23)
+                    cell_matrix.print(std::cout);
                 MatrixTools::local_apply_boundary_values(boundary_values, local_dof_indices,
                                                          cell_matrix, cell_rhs, true);
 
@@ -1119,16 +1122,18 @@ namespace SAND {
 
         }
         system_matrix.compress(VectorOperation::add);
-        system_rhs = calculate_rhs(state, barrier_size);
-
+        system_rhs = calculate_rhs(distributed_state, barrier_size);
         std::cout << "assembled" << std::endl;
     }
 
     template<int dim>
     double
-    KktSystem<dim>::calculate_objective_value(const LA::MPI::BlockVector &state) const {
+    KktSystem<dim>::calculate_objective_value(const LA::MPI::BlockVector &distributed_state) const {
         /*Remove any values from old iterations*/
 
+        locally_relevant_solution = distributed_state;
+
+
         QGauss<dim> nine_quadrature(fe_nine.degree + 1);
         QGauss<dim> ten_quadrature(fe_ten.degree + 1);
 
@@ -1159,7 +1164,7 @@ namespace SAND {
 
         Tensor<1, dim> traction;
         traction[1] = -1;
-        distributed_solution = state;
+        distributed_solution = distributed_state;
         double objective_value = 0;
         for (const auto &cell: dof_handler.active_cell_iterators())
         {
@@ -1173,7 +1178,7 @@ namespace SAND {
 
                 std::vector<Tensor<1, dim>> old_displacement_values(n_q_points);
                 fe_values[displacements].get_function_values(
-                        state, old_displacement_values);
+                        locally_relevant_solution, old_displacement_values);
 
                 for (unsigned int face_number = 0;
                      face_number < GeometryInfo<dim>::faces_per_cell;
@@ -1327,8 +1332,10 @@ namespace SAND {
 
     template<int dim>
     LA::MPI::BlockVector
-    KktSystem<dim>::calculate_rhs(const LA::MPI::BlockVector &state, const double barrier_size) const {
+    KktSystem<dim>::calculate_rhs(const LA::MPI::BlockVector &distributed_state, const double barrier_size) const {
         LA::MPI::BlockVector test_rhs (system_rhs);
+        LA::MPI::BlockVector state (locally_relevant_solution);
+        state = distributed_state;
         test_rhs = 0;
 
 
@@ -1693,7 +1700,7 @@ namespace SAND {
         {
             test_rhs.block(SolutionBlocks::total_volume_multiplier)[0] = goal_volume - total_volume;
         }
-
+        test_rhs.compress(VectorOperation::insert);
         return test_rhs;
 
     }
@@ -1715,7 +1722,8 @@ namespace SAND {
         else {
             gmres_tolerance = Input::default_gmres_tolerance;
         }
-        locally_relevant_solution=0;
+        system_rhs.print(std::cout);
+        locally_relevant_solution=state;
         distributed_solution = state;
         SolverControl solver_control(10000, gmres_tolerance * system_rhs.l2_norm());
         TopOptSchurPreconditioner<dim> preconditioner(system_matrix);
@@ -1729,21 +1737,21 @@ namespace SAND {
                 break;
             }
             case SolverOptions::exact_preconditioner_with_gmres: {
-                preconditioner.initialize(system_matrix, boundary_values, dof_handler, state);
+                preconditioner.initialize(system_matrix, boundary_values, dof_handler, locally_relevant_solution, distributed_solution);
                 SolverFGMRES<LA::MPI::BlockVector> A_fgmres(solver_control);
                 A_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
                 std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
                 break;
             }
             case SolverOptions::inexact_K_with_exact_A_gmres: {
-                preconditioner.initialize(system_matrix, boundary_values, dof_handler,  state);
+                preconditioner.initialize(system_matrix, boundary_values, dof_handler,  state, distributed_solution);
                 SolverFGMRES<LA::MPI::BlockVector> B_fgmres(solver_control);
                 B_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
                 std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
                 break;
             }
             case SolverOptions::inexact_K_with_inexact_A_gmres: {
-                preconditioner.initialize(system_matrix, boundary_values, dof_handler,  state);
+                preconditioner.initialize(system_matrix, boundary_values, dof_handler,  state, distributed_solution);
                 SolverFGMRES<LA::MPI::BlockVector> C_fgmres(solver_control);
                 C_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
                 std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
@@ -1779,7 +1787,7 @@ namespace SAND {
 //            }
 //            Mat.close();
         }
-        return locally_relevant_solution;
+        return distributed_solution;
     }
 
     template<int dim>
@@ -1801,30 +1809,29 @@ namespace SAND {
         const unsigned int n_u = dofs_per_block[1];
         const std::vector<unsigned int> block_sizes = {n_p, n_p, n_p, n_p, n_p, n_u, n_u, n_p, n_p, 1};
 
-        LA::MPI::BlockVector state(owned_partitioning, relevant_partitioning,mpi_communicator);
+        LA::MPI::BlockVector state(owned_partitioning, mpi_communicator);
         {
             using namespace SolutionBlocks;
-            distributed_solution=state;
-            distributed_solution.block(density).add(density_ratio);
-            distributed_solution.block(unfiltered_density).add(density_ratio);
-            distributed_solution.block(unfiltered_density_multiplier)
+            state.block(density).add(density_ratio);
+            state.block(unfiltered_density).add(density_ratio);
+            state.block(unfiltered_density_multiplier)
                     .add(density_ratio);
-            distributed_solution.block(density_lower_slack).add(density_ratio);
-            distributed_solution.block(density_lower_slack_multiplier).add(50);
-            distributed_solution.block(density_upper_slack).add(1 - density_ratio);
-            distributed_solution.block(density_upper_slack_multiplier).add(50);
-            distributed_solution.block(total_volume_multiplier).add(1);
-            distributed_solution.block(displacement).add(0);
-            distributed_solution.block(displacement_multiplier).add(0);
-            state=distributed_solution;
+            state.block(density_lower_slack).add(density_ratio);
+            state.block(density_lower_slack_multiplier).add(50);
+            state.block(density_upper_slack).add(1 - density_ratio);
+            state.block(density_upper_slack_multiplier).add(50);
+            state.block(total_volume_multiplier).add(1);
+            state.block(displacement).add(0);
+            state.block(displacement_multiplier).add(0);
         }
+        state.compress(VectorOperation::add);
         return state;
-
     }
 
     template<int dim>
     void
     KktSystem<dim>::output(const LA::MPI::BlockVector &state, const unsigned int j) const {
+        locally_relevant_solution = state;
         std::vector<std::string> solution_names(1, "low_slack_multiplier");
         std::vector<DataComponentInterpretation::DataComponentInterpretation> data_component_interpretation(
                 1, DataComponentInterpretation::component_is_scalar);
@@ -1861,11 +1868,14 @@ namespace SAND {
                 DataComponentInterpretation::component_is_scalar);
         DataOut<dim> data_out;
         data_out.attach_dof_handler(dof_handler);
-        data_out.add_data_vector(state, solution_names,
-                                 DataOut<dim>::type_dof_data, data_component_interpretation);
+        std::cout << "+++++++++++++++++++++++++HERE++++++++++" << std::endl;
+        data_out.add_data_vector(locally_relevant_solution,
+                                 solution_names,
+                                 DataOut<dim>::type_dof_data,
+                                 data_component_interpretation);
         data_out.build_patches();
         std::ofstream output("solution" + std::to_string(j) + ".vtk");
-        data_out.write_vtk(output);
+//        data_out.write_vtk(output);
 
     }
 
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index e3a3416..ac95a0b 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -51,7 +51,7 @@ namespace SAND {
     }
 
     template<int dim>
-    void TopOptSchurPreconditioner<dim>::initialize(LA::MPI::BlockSparseMatrix &matrix, const std::map<types::global_dof_index, double> &boundary_values,const DoFHandler<dim> &dof_handler, const LA::MPI::BlockVector &state)
+    void TopOptSchurPreconditioner<dim>::initialize(LA::MPI::BlockSparseMatrix &matrix, const std::map<types::global_dof_index, double> &boundary_values,const DoFHandler<dim> &dof_handler, const LA::MPI::BlockVector &locally_relevant_state, const LA::MPI::BlockVector &distributed_state)
     {
         TimerOutput::Scope t(timer, "initialize");
         {
@@ -80,7 +80,6 @@ namespace SAND {
                 }
             }
 
-
             //set diagonal to 0?
             for (auto&[dof_index, boundary_value]: boundary_values) {
                 const types::global_dof_index disp_start_index = system_matrix.get_row_indices().block_start(
@@ -99,20 +98,21 @@ namespace SAND {
                 }
             }
 
+//            system_matrix.compress(VectorOperation::insert);
 
-            const unsigned int m = a_mat.m();
-            const unsigned int n = a_mat.n();
-            std::ofstream Xmat("a_mat_par.csv");
-            for (unsigned int i = 0; i < m; i++)
-            {
-                Xmat << a_mat.el(i, 0);
-                for (unsigned int j = 1; j < n; j++)
-                {
-                    Xmat << "," << a_mat.el(i, j);
-                }
-                Xmat << "\n";
-            }
-            Xmat.close();
+//            const unsigned int m = a_mat.m();
+//            const unsigned int n = a_mat.n();
+//            std::ofstream Xmat("a_mat_par.csv");
+//            for (unsigned int i = 0; i < m; i++)
+//            {
+//                Xmat << a_mat.el(i, 0);
+//                for (unsigned int j = 1; j < n; j++)
+//                {
+//                    Xmat << "," << a_mat.el(i, j);
+//                }
+//                Xmat << "\n";
+//            }
+//            Xmat.close();
 
         }
         if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
@@ -141,51 +141,73 @@ namespace SAND {
             d_8_mat=0;
             d_m_inv_mat=0;
         }
-        std::cout << "*************************** Diag Matrix Stuff Needs To Go Back In" << std::endl;
         {
             TimerOutput::Scope t(timer, "build diag matrices");
-//            for (const auto cell: dof_handler.active_cell_iterators())
-//            {
-//                if(cell->is_locally_owned())
-//                {
-//                    const unsigned int i = cell->active_cell_index();
-//                    std::cout << i << std::endl;
-//                    const double m = cell->measure();
-//                    double d_3_value = -1 * state.block(SolutionBlocks::density_lower_slack_multiplier)[i] /
-//                                       (m * state.block(SolutionBlocks::density_lower_slack)[i]);
-//                    double d_4_value = -1 * state.block(SolutionBlocks::density_upper_slack_multiplier)[i] /
-//                                       (m * state.block(SolutionBlocks::density_upper_slack)[i]);
-//                    double d_5_value = state.block(SolutionBlocks::density_lower_slack_multiplier)[i] /
-//                                       (state.block(SolutionBlocks::density_lower_slack)[i]);
-//                    double d_6_value = state.block(SolutionBlocks::density_upper_slack_multiplier)[i] /
-//                                       (state.block(SolutionBlocks::density_upper_slack)[i]);
-//                    double d_7_value = (m * (state.block(SolutionBlocks::density_lower_slack_multiplier)[i] *
-//                                             state.block(SolutionBlocks::density_upper_slack)[i] +
-//                                             state.block(SolutionBlocks::density_upper_slack_multiplier)[i] *
-//                                             state.block(SolutionBlocks::density_lower_slack)[i]))
-//                                       / (state.block(SolutionBlocks::density_lower_slack)[i] *
-//                                          state.block(SolutionBlocks::density_upper_slack)[i]);
-//                    double d_8_value = (state.block(SolutionBlocks::density_lower_slack)[i] *
-//                                        state.block(SolutionBlocks::density_upper_slack)[i])
-//                                       / (m * (state.block(SolutionBlocks::density_lower_slack_multiplier)[i] *
-//                                               state.block(SolutionBlocks::density_upper_slack)[i] +
-//                                               state.block(SolutionBlocks::density_upper_slack_multiplier)[i] *
-//                                               state.block(SolutionBlocks::density_lower_slack)[i]));
-//                    d_3_mat.set(i, i, d_3_value);
-//                    d_4_mat.set(i, i, d_4_value);
-//                    d_5_mat.set(i, i, d_5_value);
-//                    d_6_mat.set(i, i, d_6_value);
-//                    d_7_mat.set(i, i, d_7_value);
-//                    d_8_mat.set(i, i, d_8_value);
-//                    d_m_inv_mat.set(i, i, 1 / m);
-//                }
-//            }
-        }
+            for (const auto cell: dof_handler.active_cell_iterators())
+            {
+                const unsigned int i = cell->active_cell_index();
+                const double m = cell->measure();
+                double l = 0;
+                double lm = 0;
+                double u = 0;
+                double um = 0;
+
+
+                double l_global;
+                double lm_global;
+                double u_global;
+                double um_global;
+
 
-        pre_j=state.block(SolutionBlocks::density);
-        pre_k=state.block(SolutionBlocks::density);
-        g_d_m_inv_density=state.block(SolutionBlocks::density);
-        k_g_d_m_inv_density=state.block(SolutionBlocks::density);
+                if(distributed_state.block(SolutionBlocks::density_lower_slack_multiplier).in_local_range(i))
+                {
+                    lm = distributed_state.block(SolutionBlocks::density_lower_slack_multiplier)[i];
+                }
+
+                MPI_Allreduce(&lm, &lm_global, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+
+                if(distributed_state.block(SolutionBlocks::density_lower_slack).in_local_range(i))
+                {
+                    l =  distributed_state.block(SolutionBlocks::density_lower_slack)[i];
+                }
+                MPI_Allreduce(&l, &l_global, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+
+                if(distributed_state.block(SolutionBlocks::density_upper_slack_multiplier).in_local_range(i))
+                {
+                    um= distributed_state.block(SolutionBlocks::density_upper_slack_multiplier)[i];
+                }
+                MPI_Allreduce(&um, &um_global, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+
+                if(distributed_state.block(SolutionBlocks::density_upper_slack).in_local_range(i))
+                {
+                    u = distributed_state.block(SolutionBlocks::density_upper_slack)[i];
+                }
+                MPI_Allreduce(&u, &u_global, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+
+                if(distributed_state.block(SolutionBlocks::density_lower_slack_multiplier).in_local_range(i))
+                {
+                    d_3_mat.set(i, i, -1 * lm_global/(m*l_global));
+                    d_4_mat.set(i, i, -1 * um_global/(m*u_global));
+                    d_5_mat.set(i, i, lm_global/l_global);
+                    d_6_mat.set(i, i, um_global/u_global);
+                    d_7_mat.set(i, i, m*(lm_global*u_global + um_global*l_global)/(l_global*u_global));
+                    d_8_mat.set(i, i, l_global*u_global/(m*(lm_global*u_global + um_global*l_global)));
+                    d_m_inv_mat.set(i, i, 1 / m);
+                }
+            }
+        }
+        d_3_mat.compress(VectorOperation::insert);
+        d_4_mat.compress(VectorOperation::insert);
+        d_5_mat.compress(VectorOperation::insert);
+        d_6_mat.compress(VectorOperation::insert);
+        d_7_mat.compress(VectorOperation::insert);
+        d_8_mat.compress(VectorOperation::insert);
+        d_m_inv_mat.compress(VectorOperation::insert);
+
+        pre_j=distributed_state.block(SolutionBlocks::density);
+        pre_k=distributed_state.block(SolutionBlocks::density);
+        g_d_m_inv_density=distributed_state.block(SolutionBlocks::density);
+        k_g_d_m_inv_density=distributed_state.block(SolutionBlocks::density);
         LinearOperator<VectorType,VectorType,PayloadType> op_g;
         LinearOperator<VectorType,VectorType,PayloadType> op_h;
         LinearOperator<VectorType,VectorType,PayloadType> op_f;
@@ -252,6 +274,7 @@ namespace SAND {
     template<int dim>
     void TopOptSchurPreconditioner<dim>::vmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
         LA::MPI::BlockVector temp_src;
+        std::cout << "vmult" << std::endl;
         {
             TimerOutput::Scope t(timer, "part 1");
             vmult_step_1(dst, src);
diff --git a/source/watchdog_main.cc b/source/watchdog_main.cc
index 1c4c7a6..fee7e6a 100644
--- a/source/watchdog_main.cc
+++ b/source/watchdog_main.cc
@@ -72,8 +72,10 @@ namespace SAND {
         double step_size_s_high = 1;
         double step_size_z_high = 1;
         double step_size_s, step_size_z;
-
-
+        LA::MPI::BlockVector state_test_s = state;
+        state_test_s = 0;
+        LA::MPI::BlockVector state_test_z = state;
+        state_test_z = 0;
         for (unsigned int k = 0; k < 50; k++)
         {
 
@@ -85,8 +87,6 @@ namespace SAND {
             const LA::MPI::BlockVector state_test_z =
                     (Input::fraction_to_boundary * state) + (step_size_z * step);
 
-            std::cout << "********** NEED TO FIND A WAY TO CHECK NONNEGATIVE HERE ***************" << std::endl;
-
             const bool accept_s = (state_test_s.block(SolutionBlocks::density_lower_slack).is_non_negative())
                                   && (state_test_s.block(SolutionBlocks::density_upper_slack).is_non_negative());
             const bool accept_z = (state_test_z.block(SolutionBlocks::density_lower_slack_multiplier).is_non_negative())
@@ -103,6 +103,7 @@ namespace SAND {
                 step_size_z_high = step_size_z;
             }
         }
+        std::cout << "s step : " << step_size_s_low << " z size : " << step_size_z_low << std::endl;
         return {step_size_s_low, step_size_z_low};
     }
 

From eb18c303d245b3ac2fbcddf8686c86807b13f452 Mon Sep 17 00:00:00 2001
From: juleoc02 <juleoc@colostate.edu>
Date: Mon, 15 Nov 2021 18:53:14 -0700
Subject: [PATCH 31/95] changed outputs

---
 source/kkt_system.cc | 27 +++++++++++++--------------
 1 file changed, 13 insertions(+), 14 deletions(-)

diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index f3d1e0d..0292b5b 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -1210,8 +1210,7 @@ namespace SAND {
             }
         }
         double objective_value_out;
-        MPI_Reduce(&objective_value, &objective_value_out, 1, MPI_DOUBLE, MPI_SUM, 0, MPI_COMM_WORLD);
-        MPI_Bcast(&objective_value_out, 1, MPI_DOUBLE, 0, MPI_COMM_WORLD);
+        MPI_Allreduce(&objective_value, &objective_value_out, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
 
         std::cout << "objective value: " << objective_value_out << std::endl;
         return objective_value;
@@ -1234,8 +1233,7 @@ namespace SAND {
             barrier_distance_log_sum += std::log(state.block(SolutionBlocks::density_upper_slack)[k]);
         }
         double out_barrier_distance_log_sum;
-        MPI_Reduce(&barrier_distance_log_sum, &out_barrier_distance_log_sum, 1, MPI_DOUBLE, MPI_SUM, 0, MPI_COMM_WORLD);
-        MPI_Bcast(&out_barrier_distance_log_sum, 1, MPI_DOUBLE, 0, MPI_COMM_WORLD);
+        MPI_Allreduce(&barrier_distance_log_sum, &out_barrier_distance_log_sum, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
 
         std::cout << "Barrier distance log sum: " << out_barrier_distance_log_sum << std::endl;
 
@@ -1279,8 +1277,7 @@ namespace SAND {
             }
         }
         double pre_norm;
-        MPI_Reduce(&norm, &pre_norm, 1, MPI_DOUBLE, MPI_SUM, 0, MPI_COMM_WORLD);
-        MPI_Bcast(&pre_norm, 1, MPI_DOUBLE, 0, MPI_COMM_WORLD);
+        MPI_Allreduce(&norm, &pre_norm, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
         norm = pre_norm;
         norm += std::pow(test_rhs.block(SolutionBlocks::displacement).l2_norm(), 2);
         norm += std::pow(test_rhs.block(SolutionBlocks::density).l2_norm(), 2);
@@ -1428,11 +1425,13 @@ namespace SAND {
         relevant_filtered_unfiltered_density_solution = filtered_unfiltered_density_solution;
         relevant_filter_adjoint_unfiltered_density_multiplier_solution = filter_adjoint_unfiltered_density_multiplier_solution;
 
+        double old_volume_multiplier_temp = 0;
         double old_volume_multiplier;
-        if(state.block(SolutionBlocks::total_volume_multiplier).in_local_range(0))
+        if(distributed_state.block(SolutionBlocks::total_volume_multiplier).in_local_range(0))
         {
-            old_volume_multiplier = state.block(SolutionBlocks::total_volume_multiplier)[0];
+            old_volume_multiplier_temp = state.block(SolutionBlocks::total_volume_multiplier)[0];
         }
+        MPI_Allreduce(&old_volume_multiplier_temp, &old_volume_multiplier, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
 
         for (const auto &cell: dof_handler.active_cell_iterators()) {
             if(cell->is_locally_owned())
@@ -1610,12 +1609,12 @@ namespace SAND {
                                         * (1 - old_unfiltered_density_values[q_point]
                                            - old_upper_slack_values[q_point]));
 
-                        //rhs eqn 6
-                        cell_rhs(i) +=
-                                -1 * fe_values.JxW(q_point) * (
-                                        -1 * unfiltered_density_multiplier_phi_i
-                                        * (old_density_values[q_point] - filtered_unfiltered_density_values[q_point])
-                                );
+//                        //rhs eqn 6
+//                        cell_rhs(i) +=
+//                                -1 * fe_values.JxW(q_point) * (
+//                                        -1 * unfiltered_density_multiplier_phi_i
+//                                        * (old_density_values[q_point] - filtered_unfiltered_density_values[q_point])
+//                                );
 
                         //rhs eqn 7
                         cell_rhs(i) +=

From 6d2ed7310aaa73ba552eb5e627b4744242188de1 Mon Sep 17 00:00:00 2001
From: juleoc02 <juleoc@colostate.edu>
Date: Tue, 16 Nov 2021 13:11:40 -0700
Subject: [PATCH 32/95] Density filter matrix acting strange

---
 include/density_filter.h |   4 +-
 source/density_filter.cc |  94 ++++++++++++++++------------
 source/kkt_system.cc     | 128 ++++++++++++++++++---------------------
 3 files changed, 116 insertions(+), 110 deletions(-)

diff --git a/include/density_filter.h b/include/density_filter.h
index ccd539a..3fdde45 100644
--- a/include/density_filter.h
+++ b/include/density_filter.h
@@ -61,8 +61,8 @@ namespace SAND {
         DynamicSparsityPattern filter_dsp;
         LA::MPI::SparseMatrix filter_matrix;
         SparsityPattern filter_sparsity_pattern;
-        void initialize(parallel::distributed::Triangulation<dim> &triangulation);
-        std::set<typename Triangulation<dim>::cell_iterator> find_relevant_neighbors(typename Triangulation<dim>::cell_iterator cell) const;
+        void initialize(DoFHandler<dim> &dof_handler);
+        std::set<typename DoFHandler<dim>::cell_iterator> find_relevant_neighbors(typename DoFHandler<dim>::cell_iterator cell) const;
 
     private:
 
diff --git a/source/density_filter.cc b/source/density_filter.cc
index f148634..4ebc55a 100644
--- a/source/density_filter.cc
+++ b/source/density_filter.cc
@@ -17,56 +17,74 @@ namespace SAND {
      * convolution being applied to a piecewise constant function on that triangulation  */
     template<int dim>
     void
-    DensityFilter<dim>::initialize(parallel::distributed::Triangulation<dim> &triangulation) {
-
-        filter_dsp.reinit(triangulation.n_active_cells(),
-                          triangulation.n_active_cells());
-
+    DensityFilter<dim>::initialize(DoFHandler<dim> &dof_handler) {
+        const std::vector<types::global_dof_index> dofs_per_block = DoFTools::count_dofs_per_fe_block(dof_handler);
+        filter_dsp.reinit(dofs_per_block[0],
+                          dofs_per_block[0]);
         std::set<unsigned int> neighbor_ids;
-        std::set<typename Triangulation<dim>::cell_iterator> cells_to_check;
-        std::set<typename Triangulation<dim>::cell_iterator> cells_to_check_temp;
+        std::set<typename DoFHandler<dim>::cell_iterator> cells_to_check;
+        std::set<typename DoFHandler<dim>::cell_iterator> cells_to_check_temp;
 
         /*finds neighbors whose values would be relevant, and adds them to the sparsity pattern of the matrix*/
-        for (const auto &cell : triangulation.active_cell_iterators()) {
-            const unsigned int i = cell->active_cell_index();
-            for (const auto &neighbor_cell : find_relevant_neighbors(cell)) {
-                const unsigned int j = neighbor_cell->active_cell_index();
-                filter_dsp.add(i, j);
+        for (const auto &cell : dof_handler.active_cell_iterators()) {
+            if(cell->is_locally_owned())
+            {
+                std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
+                cell->get_dof_indices(i);
+
+                std::cout << i[16];
+
+                std::cout << std::endl;
+                for (const auto &neighbor_cell : find_relevant_neighbors(cell))
+                {
+                    std::vector<types::global_dof_index> j(neighbor_cell->get_fe().n_dofs_per_cell());
+                    neighbor_cell->get_dof_indices(j);
+                    filter_dsp.add(i[16], j[16]);
+                }
             }
-        }
 
+        }
         filter_sparsity_pattern.copy_from(filter_dsp);
         filter_matrix.reinit(filter_sparsity_pattern);
 
         /*adds values to the matrix corresponding to the max radius - */
-        for (const auto &cell : triangulation.active_cell_iterators())
+        for (const auto &cell : dof_handler.active_cell_iterators())
         {
-            const unsigned int i = cell->active_cell_index();
-            for (const auto &neighbor_cell : find_relevant_neighbors(cell)) {
-                const unsigned int j = neighbor_cell->active_cell_index();
-                const double d =
-                        cell->center().distance(neighbor_cell->center());
-                /*value should be (max radius - distance between cells)*cell measure */
-                double value = (Input::filter_r - d)*neighbor_cell->measure();
-                filter_matrix.add(i, j, value);
+            if(cell->is_locally_owned())
+            {
+                std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
+                cell->get_dof_indices(i);
+                for (const auto &neighbor_cell : find_relevant_neighbors(cell)) {
+                    std::vector<types::global_dof_index> j(neighbor_cell->get_fe().n_dofs_per_cell());
+                    neighbor_cell->get_dof_indices(j);
+                    const double d =
+                            cell->center().distance(neighbor_cell->center());
+                    /*value should be (max radius - distance between cells)*cell measure */
+                    double value = (Input::filter_r - d)*neighbor_cell->measure();
+                    filter_matrix.add(i[16], j[16], value);
+                }
             }
         }
 
         //here we normalize the filter so it computes an average. Sum of values in a row should be 1
-        for (const auto &cell : triangulation.active_cell_iterators())
+        for (const auto &cell : dof_handler.active_cell_iterators())
         {
-            const unsigned int i = cell->active_cell_index();
-            double denominator = 0;
-            typename LA::MPI::SparseMatrix::iterator iter = filter_matrix.begin(
-                    i);
-            for (; iter != filter_matrix.end(i); iter++)
+            if(cell->is_locally_owned())
             {
-                denominator = denominator + iter->value();
-            }
-            iter = filter_matrix.begin(i);
-            for (; iter != filter_matrix.end(i); iter++)
-            {
-                iter->value() = iter->value() / denominator;
+                std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
+                cell->get_dof_indices(i);
+                double denominator = 0;
+                typename LA::MPI::SparseMatrix::iterator iter = filter_matrix.begin(
+                        i[16]);
+                for (; iter != filter_matrix.end(i[16]); iter++)
+                {
+                    denominator = denominator + iter->value();
+                }
+                iter = filter_matrix.begin(i[16]);
+                for (; iter != filter_matrix.end(i[16]); iter++)
+                {
+                    iter->value() = iter->value() / denominator;
+                }
             }
         }
         std::cout << "density nonzero entries" << filter_matrix.n_nonzero_elements() << std::endl;
@@ -74,17 +92,17 @@ namespace SAND {
 
     /*This function finds which neighbors are within a certain radius of the initial cell.*/
     template<int dim>
-    std::set<typename Triangulation<dim>::cell_iterator>
-    DensityFilter<dim>::find_relevant_neighbors(typename Triangulation<dim>::cell_iterator cell) const {
+    std::set<typename DoFHandler<dim>::cell_iterator>
+    DensityFilter<dim>::find_relevant_neighbors(typename DoFHandler<dim>::cell_iterator cell) const {
         std::set<unsigned int> neighbor_ids;
-        std::set<typename Triangulation<dim>::cell_iterator> cells_to_check;
+        std::set<typename DoFHandler<dim>::cell_iterator> cells_to_check;
         neighbor_ids.insert(cell->active_cell_index());
         cells_to_check.insert(cell);
         bool new_neighbors_found;
         do {
             new_neighbors_found = false;
             for (const auto &check_cell :
-                    std::vector<typename Triangulation<dim>::cell_iterator>(
+                    std::vector<typename DoFHandler<dim>::cell_iterator>(
                             cells_to_check.begin(), cells_to_check.end())) {
                 for (const auto n : check_cell->face_indices()) {
                     if (!(check_cell->face(n)->at_boundary())) {
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index 0292b5b..fc44eb5 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -79,7 +79,7 @@ namespace SAND {
     void
     KktSystem<dim>::setup_filter_matrix() {
 
-        density_filter.initialize(triangulation);
+        density_filter.initialize(dof_handler);
     }
 
     //This triangulation matches the problem description -
@@ -697,21 +697,31 @@ namespace SAND {
             //adds the row into the sparsity pattern for the total volume constraint
             for (const auto &cell: dof_handler.active_cell_iterators())
             {
-                const unsigned int i = cell->active_cell_index();
-                dsp.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier).add(i, 0);
-                dsp.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).add(0, i);
+                if (cell->is_locally_owned())
+                {
+                    std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
+                    cell->get_dof_indices(i);
+                    dsp.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier).add(i[16], 0);
+                    dsp.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).add(0, i[16]);
+                }
+
             }
 
             /*This finds neighbors whose values would be relevant, and adds them to the sparsity pattern of the matrix*/
-            for (const auto &cell: triangulation.active_cell_iterators()) {
-                const unsigned int i = cell->active_cell_index();
-                for (const auto &neighbor_cell: density_filter.find_relevant_neighbors(cell))
+            for (const auto &cell : dof_handler.active_cell_iterators()) {
+                if (cell->is_locally_owned())
                 {
-                    const unsigned int j = neighbor_cell->active_cell_index();
-                    dsp.block(SolutionBlocks::unfiltered_density_multiplier, SolutionBlocks::unfiltered_density).add(i,
-                                                                                                                     j);
-                    dsp.block(SolutionBlocks::unfiltered_density, SolutionBlocks::unfiltered_density_multiplier).add(i,
-                                                                                                                     j);
+                    std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
+                    cell->get_dof_indices(i);
+                    for (const auto &neighbor_cell : density_filter.find_relevant_neighbors(cell)) {
+                        std::vector<types::global_dof_index> j(neighbor_cell->get_fe().n_dofs_per_cell());
+                        neighbor_cell->get_dof_indices(j);
+
+                        dsp.block(SolutionBlocks::unfiltered_density_multiplier,
+                                  SolutionBlocks::unfiltered_density).add(i[16], j[16]);
+                        dsp.block(SolutionBlocks::unfiltered_density,
+                                  SolutionBlocks::unfiltered_density_multiplier).add(i[16], j[16]);
+                    }
                 }
             }
 
@@ -795,40 +805,41 @@ namespace SAND {
         const Functions::ConstantFunction<dim> lambda(1.), mu(1.);
 
         for (const auto &cell: dof_handler.active_cell_iterators()) {
-            const unsigned int i = cell->active_cell_index();
-
-            typename LA::MPI::SparseMatrix::iterator iter = density_filter.filter_matrix.begin(
-                    i);
-            for (; iter != density_filter.filter_matrix.end(i); iter++) {
-                unsigned int j = iter->column();
-                double value = iter->value() * cell->measure();
-
-                system_matrix.block(SolutionBlocks::unfiltered_density_multiplier,
-                                    SolutionBlocks::unfiltered_density).add(i, j, value);
-                system_matrix.block(SolutionBlocks::unfiltered_density,
-                                    SolutionBlocks::unfiltered_density_multiplier).add(j, i, value);
+            if(cell->is_locally_owned())
+            {
+                std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
+                cell->get_dof_indices(i);
+                typename LA::MPI::SparseMatrix::iterator iter = density_filter.filter_matrix.begin(
+                        i[16]);
+                for (; iter != density_filter.filter_matrix.end(i[16]); iter++) {
+                    unsigned int j = iter->column();
+                    double value = iter->value() * cell->measure();
+
+                    system_matrix.block(SolutionBlocks::unfiltered_density_multiplier,
+                                        SolutionBlocks::unfiltered_density).add(i[16], j, value);
+                    system_matrix.block(SolutionBlocks::unfiltered_density,
+                                        SolutionBlocks::unfiltered_density_multiplier).add(j, i[16], value);
+                }
+
+                system_matrix.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).add(0, i[16],
+                                                                                                          cell->measure());
+                system_matrix.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier).add(i[16], 0,
+                                                                                                          cell->measure());
             }
 
-            system_matrix.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).add(0, i,
-                                                                                                      cell->measure());
-            system_matrix.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier).add(i, 0,
-                                                                                                      cell->measure());
         }
 
+
+        std::cout << ":) :) :) :) " << std::endl;
+
         distributed_solution = distributed_state;
         LA::MPI::BlockVector filtered_unfiltered_density_solution = distributed_solution;
         LA::MPI::BlockVector filter_adjoint_unfiltered_density_multiplier_solution = distributed_solution;
         filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = 0;
         filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier) = 0;
-
-        system_matrix.block(SolutionBlocks::unfiltered_density_multiplier,
-                            SolutionBlocks::unfiltered_density).vmult(
-                filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density),
-                distributed_solution.block(SolutionBlocks::unfiltered_density));
-        system_matrix.block(SolutionBlocks::unfiltered_density_multiplier,
-                            SolutionBlocks::unfiltered_density).Tvmult(filter_adjoint_unfiltered_density_multiplier_solution.block(
-                                                    SolutionBlocks::unfiltered_density_multiplier),
-                                            distributed_solution.block(SolutionBlocks::unfiltered_density_multiplier));
+        auto op_f = linear_operator<LA::MPI::Vector>(density_filter.filter_matrix);
+        filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = op_f * distributed_solution.block(SolutionBlocks::unfiltered_density);
+        filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier) = transpose_operator(op_f) *  distributed_solution.block(SolutionBlocks::unfiltered_density_multiplier);
 
         LA::MPI::BlockVector relevant_filtered_unfiltered_density_solution = locally_relevant_solution;
         LA::MPI::BlockVector relevant_filter_adjoint_unfiltered_density_multiplier_solution = locally_relevant_solution;
@@ -1385,41 +1396,19 @@ namespace SAND {
 
         locally_relevant_solution = state;
         distributed_solution = state;
-        LA::MPI::BlockVector filtered_unfiltered_density_solution (system_rhs);
-        LA::MPI::BlockVector filter_adjoint_unfiltered_density_multiplier_solution (system_rhs);
+        LA::MPI::BlockVector filtered_unfiltered_density_solution (distributed_solution);
+        LA::MPI::BlockVector filter_adjoint_unfiltered_density_multiplier_solution (distributed_solution);
         filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = 0;
         filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier) = 0;
         system_matrix.block(SolutionBlocks::unfiltered_density_multiplier,
                             SolutionBlocks::unfiltered_density) = 0;
         system_matrix.block(SolutionBlocks::unfiltered_density_multiplier,
                             SolutionBlocks::unfiltered_density) = 0;
-        for (const auto &cell: dof_handler.active_cell_iterators()) {
-            if(cell->is_locally_owned())
-            {
-                const unsigned int i = cell->active_cell_index();
-
-                typename LA::MPI::SparseMatrix::iterator iter = density_filter.filter_matrix.begin(
-                        i);
-                for (; iter != density_filter.filter_matrix.end(i); iter++)
-                {
-                    unsigned int j = iter->column();
-                    double value = iter->value() * cell->measure();
 
-                    system_matrix.block(SolutionBlocks::unfiltered_density_multiplier,
-                                        SolutionBlocks::unfiltered_density).add(i, j, value);
-                    system_matrix.block(SolutionBlocks::unfiltered_density,
-                                        SolutionBlocks::unfiltered_density_multiplier).add(j, i, value);
-                }
-            }
-        }
-
-        auto op_f = linear_operator<LA::MPI::Vector>(system_matrix.block(SolutionBlocks::unfiltered_density_multiplier,
-                                                                         SolutionBlocks::unfiltered_density));
+        auto op_f = linear_operator<LA::MPI::Vector>(density_filter.filter_matrix);
         filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = op_f * distributed_solution.block(SolutionBlocks::unfiltered_density);
-        filter_adjoint_unfiltered_density_multiplier_solution.block(
-                                                    SolutionBlocks::unfiltered_density_multiplier)
+        filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier)
                                                     = transpose_operator(op_f) * distributed_solution.block(SolutionBlocks::unfiltered_density_multiplier);
-
         LA::MPI::BlockVector relevant_filtered_unfiltered_density_solution (locally_relevant_solution);
         LA::MPI::BlockVector relevant_filter_adjoint_unfiltered_density_multiplier_solution (locally_relevant_solution);
         relevant_filtered_unfiltered_density_solution = filtered_unfiltered_density_solution;
@@ -1609,12 +1598,12 @@ namespace SAND {
                                         * (1 - old_unfiltered_density_values[q_point]
                                            - old_upper_slack_values[q_point]));
 
-//                        //rhs eqn 6
-//                        cell_rhs(i) +=
-//                                -1 * fe_values.JxW(q_point) * (
-//                                        -1 * unfiltered_density_multiplier_phi_i
-//                                        * (old_density_values[q_point] - filtered_unfiltered_density_values[q_point])
-//                                );
+                        //rhs eqn 6
+                        cell_rhs(i) +=
+                                -1 * fe_values.JxW(q_point) * (
+                                        -1 * unfiltered_density_multiplier_phi_i
+                                        * (old_density_values[q_point] - filtered_unfiltered_density_values[q_point])
+                                );
 
                         //rhs eqn 7
                         cell_rhs(i) +=
@@ -1867,7 +1856,6 @@ namespace SAND {
                 DataComponentInterpretation::component_is_scalar);
         DataOut<dim> data_out;
         data_out.attach_dof_handler(dof_handler);
-        std::cout << "+++++++++++++++++++++++++HERE++++++++++" << std::endl;
         data_out.add_data_vector(locally_relevant_solution,
                                  solution_names,
                                  DataOut<dim>::type_dof_data,

From 3930252b0f82d7424c1bd9ce48b0f62330e54fae Mon Sep 17 00:00:00 2001
From: juleoc02 <juleoc@colostate.edu>
Date: Tue, 16 Nov 2021 14:04:30 -0700
Subject: [PATCH 33/95] more debugging

---
 source/density_filter.cc |  6 +++++-
 source/kkt_system.cc     | 10 ++--------
 source/watchdog_main.cc  |  1 -
 3 files changed, 7 insertions(+), 10 deletions(-)

diff --git a/source/density_filter.cc b/source/density_filter.cc
index 4ebc55a..0a546a2 100644
--- a/source/density_filter.cc
+++ b/source/density_filter.cc
@@ -21,6 +21,7 @@ namespace SAND {
         const std::vector<types::global_dof_index> dofs_per_block = DoFTools::count_dofs_per_fe_block(dof_handler);
         filter_dsp.reinit(dofs_per_block[0],
                           dofs_per_block[0]);
+        std::cout << "initialized with " << dofs_per_block[0] << std::endl;
         std::set<unsigned int> neighbor_ids;
         std::set<typename DoFHandler<dim>::cell_iterator> cells_to_check;
         std::set<typename DoFHandler<dim>::cell_iterator> cells_to_check_temp;
@@ -45,7 +46,10 @@ namespace SAND {
 
         }
         filter_sparsity_pattern.copy_from(filter_dsp);
-        filter_matrix.reinit(filter_sparsity_pattern);
+
+        const auto owned_dofs = dof_handler.locally_owned_dofs().get_view(0, dofs_per_block[0]);
+
+        filter_matrix.reinit(owned_dofs, filter_sparsity_pattern, MPI_COMM_WORLD);
 
         /*adds values to the matrix corresponding to the max radius - */
         for (const auto &cell : dof_handler.active_cell_iterators())
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index fc44eb5..20c73e7 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -708,6 +708,7 @@ namespace SAND {
             }
 
             /*This finds neighbors whose values would be relevant, and adds them to the sparsity pattern of the matrix*/
+            setup_filter_matrix();
             for (const auto &cell : dof_handler.active_cell_iterators()) {
                 if (cell->is_locally_owned())
                 {
@@ -720,7 +721,7 @@ namespace SAND {
                         dsp.block(SolutionBlocks::unfiltered_density_multiplier,
                                   SolutionBlocks::unfiltered_density).add(i[16], j[16]);
                         dsp.block(SolutionBlocks::unfiltered_density,
-                                  SolutionBlocks::unfiltered_density_multiplier).add(i[16], j[16]);
+                                  SolutionBlocks::unfiltered_density_multiplier).add(j[16], i[16]);
                     }
                 }
             }
@@ -830,8 +831,6 @@ namespace SAND {
         }
 
 
-        std::cout << ":) :) :) :) " << std::endl;
-
         distributed_solution = distributed_state;
         LA::MPI::BlockVector filtered_unfiltered_density_solution = distributed_solution;
         LA::MPI::BlockVector filter_adjoint_unfiltered_density_multiplier_solution = distributed_solution;
@@ -1119,13 +1118,9 @@ namespace SAND {
                     }
                 }
 
-
-                if (cell->active_cell_index()==23)
-                    cell_matrix.print(std::cout);
                 MatrixTools::local_apply_boundary_values(boundary_values, local_dof_indices,
                                                          cell_matrix, cell_rhs, true);
 
-
                 constraints.distribute_local_to_global(
                         cell_matrix, cell_rhs, local_dof_indices, system_matrix, system_rhs);
 
@@ -1144,7 +1139,6 @@ namespace SAND {
 
         locally_relevant_solution = distributed_state;
 
-
         QGauss<dim> nine_quadrature(fe_nine.degree + 1);
         QGauss<dim> ten_quadrature(fe_ten.degree + 1);
 
diff --git a/source/watchdog_main.cc b/source/watchdog_main.cc
index fee7e6a..23534ed 100644
--- a/source/watchdog_main.cc
+++ b/source/watchdog_main.cc
@@ -300,7 +300,6 @@ namespace SAND {
         barrier_size = Input::initial_barrier_size;
         kkt_system.create_triangulation();
         kkt_system.setup_boundary_values();
-        kkt_system.setup_filter_matrix();
         std::cout << "setup kkt system" << std::endl;
         kkt_system.setup_block_system();
 

From c9014a7a77aa21137a641887116559bcca180eb1 Mon Sep 17 00:00:00 2001
From: juleoc02 <juleoc@colostate.edu>
Date: Thu, 18 Nov 2021 16:32:03 -0700
Subject: [PATCH 34/95] Running. Second solve takes long time.

---
 include/input_information.h    |  2 +-
 source/density_filter.cc       |  4 --
 source/kkt_system.cc           | 80 +++++++++++++++++++++++---------
 source/schur_preconditioner.cc | 84 ++++++++++++++++++----------------
 source/watchdog_main.cc        | 52 +++++++++++++++++----
 5 files changed, 147 insertions(+), 75 deletions(-)

diff --git a/include/input_information.h b/include/input_information.h
index 00515e0..f1fd96c 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -23,7 +23,7 @@ namespace SAND {
         constexpr double initial_barrier_size = 25;
         constexpr double min_barrier_size = .00000;
         constexpr double fraction_to_boundary = .9;
-        constexpr unsigned int max_steps=1;
+        constexpr unsigned int max_steps=5;
         constexpr unsigned int barrier_reduction=BarrierOptions::loqo;
         constexpr double required_norm = .0001;
 
diff --git a/source/density_filter.cc b/source/density_filter.cc
index 0a546a2..155120e 100644
--- a/source/density_filter.cc
+++ b/source/density_filter.cc
@@ -32,10 +32,6 @@ namespace SAND {
             {
                 std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
                 cell->get_dof_indices(i);
-
-                std::cout << i[16];
-
-                std::cout << std::endl;
                 for (const auto &neighbor_cell : find_relevant_neighbors(cell))
                 {
                     std::vector<types::global_dof_index> j(neighbor_cell->get_fe().n_dofs_per_cell());
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index 20c73e7..d338a08 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -88,6 +88,27 @@ namespace SAND {
     template<int dim>
     void
     KktSystem<dim>::create_triangulation() {
+
+        std::vector<unsigned int> sub_blocks(2*dim+8, 0);
+
+        sub_blocks[0]=0;
+        sub_blocks[1]=1;
+        sub_blocks[2]=2;
+        sub_blocks[3]=3;
+        sub_blocks[4]=4;
+        for(int i=0; i<dim; i++)
+        {
+            sub_blocks[5+i]=5;
+        }
+        for(int i=0; i<dim; i++)
+        {
+            sub_blocks[5+dim+i]=6;
+        }
+        sub_blocks[5+2*dim]=7;
+        sub_blocks[6+2*dim]=8;
+        sub_blocks[7+2*dim]=9;
+
+
         if (Input::geometry_base == GeometryOptions::mbb) {
             const double width = 6;
             const unsigned int width_refine = 6;
@@ -99,7 +120,8 @@ namespace SAND {
             const double downforce_x = 3;
             const double downforce_size = .3;
 
-            if (dim == 2) {
+            if (dim == 2)
+            {
                 GridGenerator::subdivided_hyper_rectangle(triangulation,
                                                           {width_refine, height_refine},
                                                           Point<dim>(0, 0),
@@ -140,10 +162,14 @@ namespace SAND {
                     }
                 }
 
+
+
                 dof_handler.distribute_dofs(fe_collection);
 
-                DoFRenumbering::component_wise(dof_handler);
-            } else if (dim == 3) {
+                DoFRenumbering::component_wise(dof_handler, sub_blocks);
+            }
+            else if (dim == 3)
+            {
                 GridGenerator::subdivided_hyper_rectangle(triangulation,
                                                           {width_refine, height_refine, depth_refine},
                                                           Point<dim>(0, 0, 0),
@@ -185,7 +211,7 @@ namespace SAND {
 
                 dof_handler.distribute_dofs(fe_collection);
 
-                DoFRenumbering::component_wise(dof_handler);
+                DoFRenumbering::component_wise(dof_handler, sub_blocks);
 
             } else {
                 throw;
@@ -246,7 +272,7 @@ namespace SAND {
 
                 dof_handler.distribute_dofs(fe_collection);
 
-                DoFRenumbering::component_wise(dof_handler);
+                DoFRenumbering::component_wise(dof_handler, sub_blocks);
 
             } else if (dim == 3) {
                 GridGenerator::subdivided_hyper_L(triangulation,
@@ -297,7 +323,7 @@ namespace SAND {
 
                 dof_handler.distribute_dofs(fe_collection);
 
-                DoFRenumbering::component_wise(dof_handler);
+                DoFRenumbering::component_wise(dof_handler, sub_blocks);
             } else {
                 throw;
             }
@@ -1121,6 +1147,7 @@ namespace SAND {
                 MatrixTools::local_apply_boundary_values(boundary_values, local_dof_indices,
                                                          cell_matrix, cell_rhs, true);
 
+
                 constraints.distribute_local_to_global(
                         cell_matrix, cell_rhs, local_dof_indices, system_matrix, system_rhs);
 
@@ -1139,6 +1166,7 @@ namespace SAND {
 
         locally_relevant_solution = distributed_state;
 
+
         QGauss<dim> nine_quadrature(fe_nine.degree + 1);
         QGauss<dim> ten_quadrature(fe_ten.degree + 1);
 
@@ -1304,6 +1332,29 @@ namespace SAND {
         LA::MPI::BlockVector test_rhs = calculate_rhs(state, Input::min_barrier_size);
         double norm = 0;
 
+        for (unsigned int k = 0; k < state.block(SolutionBlocks::density_upper_slack).size(); k++) {
+            if(state.block(SolutionBlocks::density_upper_slack).in_local_range(k))
+            {
+                norm += state.block(SolutionBlocks::density_upper_slack)[k] *
+                        state.block(SolutionBlocks::density_upper_slack_multiplier)[k]
+                        * state.block(SolutionBlocks::density_upper_slack)[k] *
+                        state.block(SolutionBlocks::density_upper_slack_multiplier)[k];
+            }
+        }
+        for (unsigned int k = 0; k < state.block(SolutionBlocks::density_lower_slack).size(); k++) {
+            if(state.block(SolutionBlocks::density_lower_slack).in_local_range(k))
+            {
+                norm += state.block(SolutionBlocks::density_lower_slack)[k] *
+                        state.block(SolutionBlocks::density_lower_slack_multiplier)[k]
+                        * state.block(SolutionBlocks::density_lower_slack)[k] *
+                        state.block(SolutionBlocks::density_lower_slack_multiplier)[k];
+            }
+        }
+
+        double pre_norm;
+        MPI_Allreduce(&norm, &pre_norm, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+        norm = pre_norm;
+
         norm += std::pow(test_rhs.block(SolutionBlocks::displacement).l2_norm(), 2);
         norm += std::pow(test_rhs.block(SolutionBlocks::density).l2_norm(), 2);
         norm += std::pow(test_rhs.block(SolutionBlocks::unfiltered_density).l2_norm(), 2);
@@ -1313,18 +1364,6 @@ namespace SAND {
         norm += std::pow(test_rhs.block(SolutionBlocks::density_upper_slack_multiplier).l2_norm(), 2);
         norm += std::pow(test_rhs.block(SolutionBlocks::density_lower_slack_multiplier).l2_norm(), 2);
 
-        for (unsigned int k = 0; k < state.block(SolutionBlocks::density_upper_slack).size(); k++) {
-            norm += state.block(SolutionBlocks::density_upper_slack)[k] *
-                    state.block(SolutionBlocks::density_upper_slack_multiplier)[k]
-                    * state.block(SolutionBlocks::density_upper_slack)[k] *
-                    state.block(SolutionBlocks::density_upper_slack_multiplier)[k];
-        }
-        for (unsigned int k = 0; k < state.block(SolutionBlocks::density_lower_slack).size(); k++) {
-            norm += state.block(SolutionBlocks::density_lower_slack)[k] *
-                    state.block(SolutionBlocks::density_lower_slack_multiplier)[k]
-                    * state.block(SolutionBlocks::density_lower_slack)[k] *
-                    state.block(SolutionBlocks::density_lower_slack_multiplier)[k];
-        }
         norm = std::pow(norm, .5);
 
         std::cout << "l2 norm: " << system_rhs.l2_norm() << std::endl;
@@ -1704,7 +1743,6 @@ namespace SAND {
         else {
             gmres_tolerance = Input::default_gmres_tolerance;
         }
-        system_rhs.print(std::cout);
         locally_relevant_solution=state;
         distributed_solution = state;
         SolverControl solver_control(10000, gmres_tolerance * system_rhs.l2_norm());
@@ -1855,8 +1893,8 @@ namespace SAND {
                                  DataOut<dim>::type_dof_data,
                                  data_component_interpretation);
         data_out.build_patches();
-        std::ofstream output("solution" + std::to_string(j) + ".vtk");
-//        data_out.write_vtk(output);
+        std::string output("solution" + std::to_string(j) + ".vtu");
+        data_out.write_vtu_in_parallel(output, MPI_COMM_WORLD);
 
     }
 
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index ac95a0b..f4dc59f 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -145,54 +145,58 @@ namespace SAND {
             TimerOutput::Scope t(timer, "build diag matrices");
             for (const auto cell: dof_handler.active_cell_iterators())
             {
-                const unsigned int i = cell->active_cell_index();
-                const double m = cell->measure();
-                double l = 0;
-                double lm = 0;
-                double u = 0;
-                double um = 0;
-
+                if(cell->is_locally_owned())
+                {
+                    std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
+                    cell->get_dof_indices(i);
+                    const double m = cell->measure();
+                    double l = 0;
+                    double lm = 0;
+                    double u = 0;
+                    double um = 0;
 
-                double l_global;
-                double lm_global;
-                double u_global;
-                double um_global;
 
+                    double l_global;
+                    double lm_global;
+                    double u_global;
+                    double um_global;
 
-                if(distributed_state.block(SolutionBlocks::density_lower_slack_multiplier).in_local_range(i))
-                {
-                    lm = distributed_state.block(SolutionBlocks::density_lower_slack_multiplier)[i];
-                }
 
-                MPI_Allreduce(&lm, &lm_global, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+                    if(distributed_state.block(SolutionBlocks::density_lower_slack_multiplier).in_local_range(i[16]))
+                    {
+                        lm = distributed_state.block(SolutionBlocks::density_lower_slack_multiplier)[i[16]];
+                    }
 
-                if(distributed_state.block(SolutionBlocks::density_lower_slack).in_local_range(i))
-                {
-                    l =  distributed_state.block(SolutionBlocks::density_lower_slack)[i];
-                }
-                MPI_Allreduce(&l, &l_global, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+                    MPI_Allreduce(&lm, &lm_global, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
 
-                if(distributed_state.block(SolutionBlocks::density_upper_slack_multiplier).in_local_range(i))
-                {
-                    um= distributed_state.block(SolutionBlocks::density_upper_slack_multiplier)[i];
-                }
-                MPI_Allreduce(&um, &um_global, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+                    if(distributed_state.block(SolutionBlocks::density_lower_slack).in_local_range(i[16]))
+                    {
+                        l =  distributed_state.block(SolutionBlocks::density_lower_slack)[i[16]];
+                    }
+                    MPI_Allreduce(&l, &l_global, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
 
-                if(distributed_state.block(SolutionBlocks::density_upper_slack).in_local_range(i))
-                {
-                    u = distributed_state.block(SolutionBlocks::density_upper_slack)[i];
-                }
-                MPI_Allreduce(&u, &u_global, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+                    if(distributed_state.block(SolutionBlocks::density_upper_slack_multiplier).in_local_range(i[16]))
+                    {
+                        um= distributed_state.block(SolutionBlocks::density_upper_slack_multiplier)[i[16]];
+                    }
+                    MPI_Allreduce(&um, &um_global, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
 
-                if(distributed_state.block(SolutionBlocks::density_lower_slack_multiplier).in_local_range(i))
-                {
-                    d_3_mat.set(i, i, -1 * lm_global/(m*l_global));
-                    d_4_mat.set(i, i, -1 * um_global/(m*u_global));
-                    d_5_mat.set(i, i, lm_global/l_global);
-                    d_6_mat.set(i, i, um_global/u_global);
-                    d_7_mat.set(i, i, m*(lm_global*u_global + um_global*l_global)/(l_global*u_global));
-                    d_8_mat.set(i, i, l_global*u_global/(m*(lm_global*u_global + um_global*l_global)));
-                    d_m_inv_mat.set(i, i, 1 / m);
+                    if(distributed_state.block(SolutionBlocks::density_upper_slack).in_local_range(i[16]))
+                    {
+                        u = distributed_state.block(SolutionBlocks::density_upper_slack)[i[16]];
+                    }
+                    MPI_Allreduce(&u, &u_global, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+
+                    if(distributed_state.block(SolutionBlocks::density_lower_slack_multiplier).in_local_range(i[16]))
+                    {
+                        d_3_mat.set(i[16], i[16], -1 * lm_global/(m*l_global));
+                        d_4_mat.set(i[16], i[16], -1 * um_global/(m*u_global));
+                        d_5_mat.set(i[16], i[16], lm_global/l_global);
+                        d_6_mat.set(i[16], i[16], um_global/u_global);
+                        d_7_mat.set(i[16], i[16], m*(lm_global*u_global + um_global*l_global)/(l_global*u_global));
+                        d_8_mat.set(i[16], i[16], l_global*u_global/(m*(lm_global*u_global + um_global*l_global)));
+                        d_m_inv_mat.set(i[16], i[16], 1 / m);
+                    }
                 }
             }
         }
diff --git a/source/watchdog_main.cc b/source/watchdog_main.cc
index 23534ed..3225a77 100644
--- a/source/watchdog_main.cc
+++ b/source/watchdog_main.cc
@@ -182,16 +182,34 @@ namespace SAND {
         {
             double loqo_min = 1000;
             double loqo_average;
+            double lower_prod;
+            double full_lower_prod;
+            double upper_prod;
+            double full_upper_prod;
             unsigned int vect_size = current_state.block(SolutionBlocks::density_lower_slack).size();
             for(unsigned int k = 0; k < vect_size; k++)
             {
-                if (current_state.block(SolutionBlocks::density_lower_slack)[k]*current_state.block(SolutionBlocks::density_lower_slack_multiplier)[k] < loqo_min)
+                lower_prod = 1;
+                if (current_state.block(SolutionBlocks::density_lower_slack).in_local_range(k))
+                    lower_prod=lower_prod * current_state.block(SolutionBlocks::density_lower_slack)[k];
+                if (current_state.block(SolutionBlocks::density_lower_slack_multiplier).in_local_range(k))
+                    lower_prod=lower_prod * current_state.block(SolutionBlocks::density_lower_slack_multiplier)[k];
+
+                upper_prod=1;
+                if (current_state.block(SolutionBlocks::density_upper_slack).in_local_range(k))
+                    upper_prod=upper_prod * current_state.block(SolutionBlocks::density_upper_slack)[k];
+                if (current_state.block(SolutionBlocks::density_upper_slack_multiplier).in_local_range(k))
+                    upper_prod=upper_prod * current_state.block(SolutionBlocks::density_upper_slack_multiplier)[k];
+
+                MPI_Allreduce(&lower_prod, &full_lower_prod, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
+                MPI_Allreduce(&upper_prod, &full_upper_prod, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
+                if (full_lower_prod < loqo_min)
                 {
-                    loqo_min = current_state.block(SolutionBlocks::density_lower_slack)[k]*current_state.block(SolutionBlocks::density_lower_slack_multiplier)[k];
+                    loqo_min = full_lower_prod;
                 }
-                if (current_state.block(SolutionBlocks::density_upper_slack)[k]*current_state.block(SolutionBlocks::density_upper_slack_multiplier)[k] < loqo_min)
+                if (full_upper_prod < loqo_min)
                 {
-                    loqo_min = current_state.block(SolutionBlocks::density_upper_slack)[k]*current_state.block(SolutionBlocks::density_upper_slack_multiplier)[k];
+                    loqo_min = full_upper_prod;
                 }
             }
             loqo_average = (current_state.block(SolutionBlocks::density_lower_slack)*current_state.block(SolutionBlocks::density_lower_slack_multiplier)
@@ -249,15 +267,31 @@ namespace SAND {
                 double loqo_min = 1000;
                 double loqo_average;
                 unsigned int vect_size = current_state.block(SolutionBlocks::density_lower_slack).size();
+                double lower_prod, full_lower_prod, upper_prod, full_upper_prod;
                 for(unsigned int k = 0; k < vect_size; k++)
                 {
-                    if (current_state.block(SolutionBlocks::density_lower_slack)[k]*current_state.block(SolutionBlocks::density_lower_slack_multiplier)[k] < loqo_min)
+                    lower_prod = 1;
+                    if (current_state.block(SolutionBlocks::density_lower_slack).in_local_range(k))
+                        lower_prod=lower_prod * current_state.block(SolutionBlocks::density_lower_slack)[k];
+                    if (current_state.block(SolutionBlocks::density_lower_slack_multiplier).in_local_range(k))
+                        lower_prod=lower_prod * current_state.block(SolutionBlocks::density_lower_slack_multiplier)[k];
+
+                    upper_prod=1;
+                    if (current_state.block(SolutionBlocks::density_upper_slack).in_local_range(k))
+                        upper_prod=upper_prod * current_state.block(SolutionBlocks::density_upper_slack)[k];
+                    if (current_state.block(SolutionBlocks::density_upper_slack_multiplier).in_local_range(k))
+                        upper_prod=upper_prod * current_state.block(SolutionBlocks::density_upper_slack_multiplier)[k];
+
+                    MPI_Allreduce(&lower_prod, &full_lower_prod, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
+                    MPI_Allreduce(&upper_prod, &full_upper_prod, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
+
+                    if (full_lower_prod < loqo_min)
                     {
-                        loqo_min = current_state.block(SolutionBlocks::density_lower_slack)[k]*current_state.block(SolutionBlocks::density_lower_slack_multiplier)[k];
+                        loqo_min = full_lower_prod;
                     }
-                    if (current_state.block(SolutionBlocks::density_upper_slack)[k]*current_state.block(SolutionBlocks::density_upper_slack_multiplier)[k] < loqo_min)
+                    if (full_upper_prod < loqo_min)
                     {
-                        loqo_min = current_state.block(SolutionBlocks::density_upper_slack)[k]*current_state.block(SolutionBlocks::density_upper_slack_multiplier)[k];
+                        loqo_min = full_upper_prod;
                     }
                 }
                 loqo_average = (current_state.block(SolutionBlocks::density_lower_slack)*current_state.block(SolutionBlocks::density_lower_slack_multiplier)
@@ -415,7 +449,7 @@ namespace SAND {
             }
 
         }
-        kkt_system.output_stl(current_state);
+//        kkt_system.output_stl(current_state);
     }
 
 } // namespace SAND

From 30ade24a3582e6daac4c1f49ee2183f0aa5f4d3a Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Tue, 23 Nov 2021 11:04:54 -0700
Subject: [PATCH 35/95] even more outputs

---
 source/kkt_system.cc | 63 +++++++++++++++++++++++++-------------------
 1 file changed, 36 insertions(+), 27 deletions(-)

diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index d338a08..2da49f3 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -1284,34 +1284,43 @@ namespace SAND {
     template<int dim>
     double
     KktSystem<dim>::calculate_feasibility(const LA::MPI::BlockVector &state, const double barrier_size) const {
-        LA::MPI::BlockVector test_rhs = calculate_rhs(state, barrier_size);
+       LA::MPI::BlockVector test_rhs = calculate_rhs(state, barrier_size);
+
+       double norm = 0;
+
+       distributed_solution = state;
+       double full_prod1 =1;
+       double full_prod2 = 1;
+
+       for (unsigned int k = 0; k < state.block(SolutionBlocks::density_upper_slack).size(); k++) {
+           double prod1 = 1;
+           double prod2 = 1;
+           if(state.block(SolutionBlocks::density_upper_slack).in_local_range(k))
+           {
+               prod1 = prod1 * state.block(SolutionBlocks::density_upper_slack)[k]
+                       * state.block(SolutionBlocks::density_upper_slack)[k];
+           }
+           if(state.block(SolutionBlocks::density_lower_slack).in_local_range(k))
+           {
+              prod2 = prod2 *  state.block(SolutionBlocks::density_lower_slack)[k]
+                      * state.block(SolutionBlocks::density_lower_slack)[k];
+           }
+           if(state.block(SolutionBlocks::density_upper_slack_multiplier).in_local_range(k))
+           {
+               prod1 = prod1 * state.block(SolutionBlocks::density_upper_slack_multiplier)[k]
+                       * state.block(SolutionBlocks::density_upper_slack_multiplier)[k];
+           }
+           if(state.block(SolutionBlocks::density_lower_slack_multiplier).in_local_range(k))
+           {
+               prod2 = prod2 *  state.block(SolutionBlocks::density_lower_slack_multiplier)[k]
+                       * state.block(SolutionBlocks::density_lower_slack_multiplier)[k];
+           }
+           MPI_Allreduce(&prod1, &full_prod1, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
+           MPI_Allreduce(&prod2, &full_prod2, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
+           norm = norm + full_prod1 + full_prod2;
+       }
+       std::cout << "pre-norm: " << norm << std::endl;
 
-        double norm = 0;
-
-        distributed_solution = state;
-        for (unsigned int k = 0; k < state.block(SolutionBlocks::density_upper_slack).size(); k++)
-        {
-            if(distributed_solution.block(SolutionBlocks::density_upper_slack).in_local_range(k))
-            {
-                norm += distributed_solution.block(SolutionBlocks::density_upper_slack)[k] *
-                    distributed_solution.block(SolutionBlocks::density_upper_slack_multiplier)[k]
-                    * distributed_solution.block(SolutionBlocks::density_upper_slack)[k] *
-                    distributed_solution.block(SolutionBlocks::density_upper_slack_multiplier)[k];
-            }
-        }
-        for (unsigned int k = 0; k < state.block(SolutionBlocks::density_lower_slack).size(); k++)
-        {
-            if(distributed_solution.block(SolutionBlocks::density_lower_slack).in_local_range(k))
-            {
-                norm += distributed_solution.block(SolutionBlocks::density_lower_slack)[k] *
-                        distributed_solution.block(SolutionBlocks::density_lower_slack_multiplier)[k]
-                        * distributed_solution.block(SolutionBlocks::density_lower_slack)[k] *
-                        distributed_solution.block(SolutionBlocks::density_lower_slack_multiplier)[k];
-            }
-        }
-        double pre_norm;
-        MPI_Allreduce(&norm, &pre_norm, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-        norm = pre_norm;
         norm += std::pow(test_rhs.block(SolutionBlocks::displacement).l2_norm(), 2);
         norm += std::pow(test_rhs.block(SolutionBlocks::density).l2_norm(), 2);
         norm += std::pow(test_rhs.block(SolutionBlocks::unfiltered_density).l2_norm(), 2);

From 8e10b5f36f71ec2a4ab35ea0085ba03fd1821950 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Tue, 23 Nov 2021 12:19:06 -0700
Subject: [PATCH 36/95] small fix

---
 include/input_information.h |  2 +-
 source/kkt_system.cc        | 51 ++++++++++++++++++++++++-------------
 source/watchdog_main.cc     |  1 +
 3 files changed, 35 insertions(+), 19 deletions(-)

diff --git a/include/input_information.h b/include/input_information.h
index f1fd96c..c1010b5 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -21,7 +21,7 @@ namespace SAND {
 
         //nonlinear algorithm options
         constexpr double initial_barrier_size = 25;
-        constexpr double min_barrier_size = .00000;
+        constexpr double min_barrier_size = 0;
         constexpr double fraction_to_boundary = .9;
         constexpr unsigned int max_steps=5;
         constexpr unsigned int barrier_reduction=BarrierOptions::loqo;
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index 2da49f3..7b91d1d 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -1341,28 +1341,38 @@ namespace SAND {
         LA::MPI::BlockVector test_rhs = calculate_rhs(state, Input::min_barrier_size);
         double norm = 0;
 
+        distributed_solution = state;
+        double full_prod1 =1;
+        double full_prod2 = 1;
+
         for (unsigned int k = 0; k < state.block(SolutionBlocks::density_upper_slack).size(); k++) {
+            double prod1 = 1;
+            double prod2 = 1;
             if(state.block(SolutionBlocks::density_upper_slack).in_local_range(k))
             {
-                norm += state.block(SolutionBlocks::density_upper_slack)[k] *
-                        state.block(SolutionBlocks::density_upper_slack_multiplier)[k]
-                        * state.block(SolutionBlocks::density_upper_slack)[k] *
-                        state.block(SolutionBlocks::density_upper_slack_multiplier)[k];
+                prod1 = prod1 * state.block(SolutionBlocks::density_upper_slack)[k]
+                        * state.block(SolutionBlocks::density_upper_slack)[k];
             }
-        }
-        for (unsigned int k = 0; k < state.block(SolutionBlocks::density_lower_slack).size(); k++) {
             if(state.block(SolutionBlocks::density_lower_slack).in_local_range(k))
             {
-                norm += state.block(SolutionBlocks::density_lower_slack)[k] *
-                        state.block(SolutionBlocks::density_lower_slack_multiplier)[k]
-                        * state.block(SolutionBlocks::density_lower_slack)[k] *
-                        state.block(SolutionBlocks::density_lower_slack_multiplier)[k];
+               prod2 = prod2 *  state.block(SolutionBlocks::density_lower_slack)[k]
+                       * state.block(SolutionBlocks::density_lower_slack)[k];
+            }
+            if(state.block(SolutionBlocks::density_upper_slack_multiplier).in_local_range(k))
+            {
+                prod1 = prod1 * state.block(SolutionBlocks::density_upper_slack_multiplier)[k]
+                        * state.block(SolutionBlocks::density_upper_slack_multiplier)[k];
             }
+            if(state.block(SolutionBlocks::density_lower_slack_multiplier).in_local_range(k))
+            {
+                prod2 = prod2 *  state.block(SolutionBlocks::density_lower_slack_multiplier)[k]
+                        * state.block(SolutionBlocks::density_lower_slack_multiplier)[k];
+            }
+            MPI_Allreduce(&prod1, &full_prod1, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
+            MPI_Allreduce(&prod2, &full_prod2, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
+            norm = norm + full_prod1 + full_prod2;
         }
-
-        double pre_norm;
-        MPI_Allreduce(&norm, &pre_norm, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-        norm = pre_norm;
+        std::cout << "pre-norm: " << norm << std::endl;
 
         norm += std::pow(test_rhs.block(SolutionBlocks::displacement).l2_norm(), 2);
         norm += std::pow(test_rhs.block(SolutionBlocks::density).l2_norm(), 2);
@@ -1712,8 +1722,9 @@ namespace SAND {
         }
 
         test_rhs.compress(VectorOperation::add);
-        double total_volume = 0;
-        double goal_volume = 0;
+        double total_volume_temp = 0;
+        double goal_volume_temp = 0;
+        double total_volume, goal_volume;
 
         distributed_solution = state;
         for (const auto &cell: dof_handler.active_cell_iterators()) {
@@ -1721,11 +1732,15 @@ namespace SAND {
             {
                 if (distributed_solution.block(SolutionBlocks::density).in_local_range(cell->active_cell_index()))
                 {
-                    total_volume += cell->measure() * state.block(SolutionBlocks::density)[cell->active_cell_index()];
-                    goal_volume += cell->measure() * Input::volume_percentage;
+                    total_volume_temp += cell->measure() * state.block(SolutionBlocks::density)[cell->active_cell_index()];
+                    goal_volume_temp += cell->measure() * Input::volume_percentage;
                 }
             }
         }
+
+        MPI_Allreduce(&total_volume_temp, &total_volume, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+        MPI_Allreduce(&goal_volume_temp, &goal_volume, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+
         if (test_rhs.block(SolutionBlocks::total_volume_multiplier).in_local_range(0))
         {
             test_rhs.block(SolutionBlocks::total_volume_multiplier)[0] = goal_volume - total_volume;
diff --git a/source/watchdog_main.cc b/source/watchdog_main.cc
index 3225a77..4d5a8c2 100644
--- a/source/watchdog_main.cc
+++ b/source/watchdog_main.cc
@@ -216,6 +216,7 @@ namespace SAND {
                             + current_state.block(SolutionBlocks::density_upper_slack)*current_state.block(SolutionBlocks::density_upper_slack_multiplier)
                            )/(2*vect_size);
             double loqo_complimentarity_deviation = loqo_min/loqo_average;
+            std::cout << "loqo cd: " << loqo_complimentarity_deviation << std::endl;
             double loqo_multiplier;
             if((.05 * (1-loqo_complimentarity_deviation)/loqo_complimentarity_deviation)<2)
             {

From b4fa45f24c5b331ffcf5c0c0bd2bba8655385b0e Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Tue, 23 Nov 2021 12:47:59 -0700
Subject: [PATCH 37/95] more out

---
 source/kkt_system.cc    | 4 ++++
 source/watchdog_main.cc | 1 +
 2 files changed, 5 insertions(+)

diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index 7b91d1d..ba0c612 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -1746,6 +1746,10 @@ namespace SAND {
             test_rhs.block(SolutionBlocks::total_volume_multiplier)[0] = goal_volume - total_volume;
         }
         test_rhs.compress(VectorOperation::insert);
+       std::cout << std::endl;
+       test_rhs.print(std::cout);
+       std::cout << std::endl;
+
         return test_rhs;
 
     }
diff --git a/source/watchdog_main.cc b/source/watchdog_main.cc
index 4d5a8c2..06ca3e0 100644
--- a/source/watchdog_main.cc
+++ b/source/watchdog_main.cc
@@ -226,6 +226,7 @@ namespace SAND {
             {
                 loqo_multiplier = .8;
             }
+            std::cout << "loqo mult: " << loqo_multiplier << std::endl;
             if (loqo_multiplier< 0)
             {
                 barrier_size = .2 * loqo_average;

From 6ef05047843a557cb3226429fe1f85336b32b8a2 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Tue, 23 Nov 2021 16:31:43 -0700
Subject: [PATCH 38/95] fdsa

---
 source/kkt_system.cc | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index ba0c612..fe74357 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -1746,9 +1746,6 @@ namespace SAND {
             test_rhs.block(SolutionBlocks::total_volume_multiplier)[0] = goal_volume - total_volume;
         }
         test_rhs.compress(VectorOperation::insert);
-       std::cout << std::endl;
-       test_rhs.print(std::cout);
-       std::cout << std::endl;
 
         return test_rhs;
 
@@ -1775,6 +1772,12 @@ namespace SAND {
         distributed_solution = state;
         SolverControl solver_control(10000, gmres_tolerance * system_rhs.l2_norm());
         TopOptSchurPreconditioner<dim> preconditioner(system_matrix);
+
+        std::cout << std::endl;
+        system_rhs.print(std::cout);
+        std::cout << std::endl;
+
+
         switch (Input::solver_choice) {
             case SolverOptions::direct_solve: {
 //                SolverControl cn;

From b76c4395f90657d8fe7b5e61b265e5ac29e364b3 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Fri, 26 Nov 2021 21:51:27 -0700
Subject: [PATCH 39/95] push...

---
 source/kkt_system.cc | 55 +++++++++++++++++++++++---------------------
 1 file changed, 29 insertions(+), 26 deletions(-)

diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index fe74357..1729a63 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -831,32 +831,6 @@ namespace SAND {
 
         const Functions::ConstantFunction<dim> lambda(1.), mu(1.);
 
-        for (const auto &cell: dof_handler.active_cell_iterators()) {
-            if(cell->is_locally_owned())
-            {
-                std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
-                cell->get_dof_indices(i);
-                typename LA::MPI::SparseMatrix::iterator iter = density_filter.filter_matrix.begin(
-                        i[16]);
-                for (; iter != density_filter.filter_matrix.end(i[16]); iter++) {
-                    unsigned int j = iter->column();
-                    double value = iter->value() * cell->measure();
-
-                    system_matrix.block(SolutionBlocks::unfiltered_density_multiplier,
-                                        SolutionBlocks::unfiltered_density).add(i[16], j, value);
-                    system_matrix.block(SolutionBlocks::unfiltered_density,
-                                        SolutionBlocks::unfiltered_density_multiplier).add(j, i[16], value);
-                }
-
-                system_matrix.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).add(0, i[16],
-                                                                                                          cell->measure());
-                system_matrix.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier).add(i[16], 0,
-                                                                                                          cell->measure());
-            }
-
-        }
-
-
         distributed_solution = distributed_state;
         LA::MPI::BlockVector filtered_unfiltered_density_solution = distributed_solution;
         LA::MPI::BlockVector filter_adjoint_unfiltered_density_multiplier_solution = distributed_solution;
@@ -1155,8 +1129,37 @@ namespace SAND {
 
         }
         system_matrix.compress(VectorOperation::add);
+
+
+        for (const auto &cell: dof_handler.active_cell_iterators()) {
+            if(cell->is_locally_owned())
+            {
+                std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
+                cell->get_dof_indices(i);
+                typename LA::MPI::SparseMatrix::iterator iter = density_filter.filter_matrix.begin(
+                        i[16]);
+                for (; iter != density_filter.filter_matrix.end(i[16]); iter++) {
+                    unsigned int j = iter->column();
+                    double value = iter->value() * cell->measure();
+
+                    system_matrix.block(SolutionBlocks::unfiltered_density_multiplier,
+                                        SolutionBlocks::unfiltered_density).add(i[16], j, value);
+                    system_matrix.block(SolutionBlocks::unfiltered_density,
+                                        SolutionBlocks::unfiltered_density_multiplier).add(j, i[16], value);
+                }
+
+                system_matrix.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).add(0, i[16],
+                                                                                                          cell->measure());
+                system_matrix.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier).add(i[16], 0,
+                                                                                                          cell->measure());
+                std::cout << "i16: " << i[16] << std::endl;
+            }
+
+        }
+        system_matrix.compress(VectorOperation::add);
         system_rhs = calculate_rhs(distributed_state, barrier_size);
         std::cout << "assembled" << std::endl;
+
     }
 
     template<int dim>

From 088a2f1313062be4d626c437b73f7545e65b96b0 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Thu, 2 Dec 2021 09:48:51 -0700
Subject: [PATCH 40/95] More outputs...

preconditioner vmult seems to do the same thing, but somehow solves are
different?
---
 include/input_information.h    |  2 +-
 source/kkt_system.cc           | 96 +++++++++++++++++++---------------
 source/schur_preconditioner.cc | 10 ++++
 source/watchdog_main.cc        |  2 +-
 4 files changed, 65 insertions(+), 45 deletions(-)

diff --git a/include/input_information.h b/include/input_information.h
index c1010b5..c8858d1 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -41,7 +41,7 @@ namespace SAND {
         //Linear solver options
         constexpr unsigned int solver_choice = SolverOptions::inexact_K_with_exact_A_gmres;
         constexpr bool use_eisenstat_walker = false;
-        constexpr double default_gmres_tolerance = 1e-6;
+        constexpr double default_gmres_tolerance = 1e-9;
 
 
     }
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index 1729a63..cdf2c10 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -1118,6 +1118,7 @@ namespace SAND {
                     }
                 }
 
+
                 MatrixTools::local_apply_boundary_values(boundary_values, local_dof_indices,
                                                          cell_matrix, cell_rhs, true);
 
@@ -1129,7 +1130,7 @@ namespace SAND {
 
         }
         system_matrix.compress(VectorOperation::add);
-
+        system_rhs = calculate_rhs(distributed_state, barrier_size);
 
         for (const auto &cell: dof_handler.active_cell_iterators()) {
             if(cell->is_locally_owned())
@@ -1150,14 +1151,14 @@ namespace SAND {
 
                 system_matrix.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).add(0, i[16],
                                                                                                           cell->measure());
+
                 system_matrix.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier).add(i[16], 0,
                                                                                                           cell->measure());
-                std::cout << "i16: " << i[16] << std::endl;
             }
-
         }
         system_matrix.compress(VectorOperation::add);
-        system_rhs = calculate_rhs(distributed_state, barrier_size);
+
+
         std::cout << "assembled" << std::endl;
 
     }
@@ -1455,10 +1456,6 @@ namespace SAND {
         LA::MPI::BlockVector filter_adjoint_unfiltered_density_multiplier_solution (distributed_solution);
         filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = 0;
         filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier) = 0;
-        system_matrix.block(SolutionBlocks::unfiltered_density_multiplier,
-                            SolutionBlocks::unfiltered_density) = 0;
-        system_matrix.block(SolutionBlocks::unfiltered_density_multiplier,
-                            SolutionBlocks::unfiltered_density) = 0;
 
         auto op_f = linear_operator<LA::MPI::Vector>(density_filter.filter_matrix);
         filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = op_f * distributed_solution.block(SolutionBlocks::unfiltered_density);
@@ -1755,7 +1752,6 @@ namespace SAND {
     }
 
 
-    ///A  direct solver, for now. The complexity of the system means that an iterative solver algorithm will take some more work in the future.
     template<int dim>
     LA::MPI::BlockVector
     KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
@@ -1776,44 +1772,53 @@ namespace SAND {
         SolverControl solver_control(10000, gmres_tolerance * system_rhs.l2_norm());
         TopOptSchurPreconditioner<dim> preconditioner(system_matrix);
 
-        std::cout << std::endl;
+        std::cout << " rhs " << std::endl;
         system_rhs.print(std::cout);
         std::cout << std::endl;
 
+        preconditioner.initialize(system_matrix, boundary_values, dof_handler, locally_relevant_solution, distributed_solution);
+        preconditioner.vmult(distributed_solution,system_rhs);
+
+        std::cout << "solution" <<  std::endl;
+        distributed_solution.print(std::cout);
+        std::cout << std::endl;
+
+//        switch (Input::solver_choice) {
+//            case SolverOptions::direct_solve: {
+////                SolverControl cn;
+////                std::string solver_type;
+////                TrilinosWrappers::SolverDirect::AdditionalData additional_data(false, "Amesos_Lapack");
+////                TrilinosWrappers::SolverDirect solver(cn, additional_data);
+////                solver.initialize(system_matrix);
+////                solver.solve(distributed_solution, system_rhs);
+//                break;
+//            }
+//            case SolverOptions::exact_preconditioner_with_gmres: {
+//                preconditioner.initialize(system_matrix, boundary_values, dof_handler, locally_relevant_solution, distributed_solution);
+//                SolverFGMRES<LA::MPI::BlockVector> A_fgmres(solver_control);
+//                A_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
+//                std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
+//                break;
+//            }
+//            case SolverOptions::inexact_K_with_exact_A_gmres: {
+//                preconditioner.initialize(system_matrix, boundary_values, dof_handler,  locally_relevant_solution, distributed_solution);
+//                SolverFGMRES<LA::MPI::BlockVector> B_fgmres(solver_control);
+//                B_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
+//                std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
+//                break;
+//            }
+//            case SolverOptions::inexact_K_with_inexact_A_gmres: {
+//                preconditioner.initialize(system_matrix, boundary_values, dof_handler,  locally_relevant_solution, distributed_solution);
+//                SolverFGMRES<LA::MPI::BlockVector> C_fgmres(solver_control);
+//                C_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
+//                std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
+//                break;
+//            }
+//            default:
+//                throw;
+//        }
+
 
-        switch (Input::solver_choice) {
-            case SolverOptions::direct_solve: {
-//                SolverControl cn;
-//                std::string solver_type;
-//                TrilinosWrappers::SolverDirect::AdditionalData additional_data(false, "Amesos_Klu");
-//                TrilinosWrappers::SolverDirect solver(cn, mpi_communicator, system_matrix);
-//                solver.solve(distributed_solution, system_rhs);
-                break;
-            }
-            case SolverOptions::exact_preconditioner_with_gmres: {
-                preconditioner.initialize(system_matrix, boundary_values, dof_handler, locally_relevant_solution, distributed_solution);
-                SolverFGMRES<LA::MPI::BlockVector> A_fgmres(solver_control);
-                A_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
-                std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
-                break;
-            }
-            case SolverOptions::inexact_K_with_exact_A_gmres: {
-                preconditioner.initialize(system_matrix, boundary_values, dof_handler,  state, distributed_solution);
-                SolverFGMRES<LA::MPI::BlockVector> B_fgmres(solver_control);
-                B_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
-                std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
-                break;
-            }
-            case SolverOptions::inexact_K_with_inexact_A_gmres: {
-                preconditioner.initialize(system_matrix, boundary_values, dof_handler,  state, distributed_solution);
-                SolverFGMRES<LA::MPI::BlockVector> C_fgmres(solver_control);
-                C_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
-                std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
-                break;
-            }
-            default:
-                throw;
-        }
         constraints.distribute(distributed_solution);
 
         if (Input::output_parts_of_matrix) {
@@ -1841,6 +1846,11 @@ namespace SAND {
 //            }
 //            Mat.close();
         }
+
+        std::cout << "solution" << std::endl;
+        distributed_solution.print(std::cout);
+        std::cout << std::endl;
+
         return distributed_solution;
     }
 
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index f4dc59f..fa61bee 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -284,6 +284,8 @@ namespace SAND {
             vmult_step_1(dst, src);
             temp_src = dst;
         }
+        std::cout << std::endl << "vmult stuff pt 1:" << std::endl;
+        dst.print(std::cout);
 
         {
             TimerOutput::Scope t(timer, "part 2");
@@ -302,6 +304,7 @@ namespace SAND {
             temp_src = dst;
         }
         vmult_step_5(dst, temp_src);
+
     }
 
     template<int dim>
@@ -336,6 +339,13 @@ namespace SAND {
     void TopOptSchurPreconditioner<dim>::vmult_step_2(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
         dst = src;
         auto dst_temp = dst;
+//        auto temp = src.block(SolutionBlocks::unfiltered_density);
+//        temp = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) * src.block(SolutionBlocks::unfiltered_density);
+//        std::cout <<std::endl <<std::endl<< "temp output:" << std::endl;
+//        temp.print(std::cout);
+//        std::cout << std::endl;
+
+
         dst.block(SolutionBlocks::unfiltered_density_multiplier) = dst_temp.block(SolutionBlocks::unfiltered_density_multiplier)
                 - linear_operator<VectorType,VectorType,PayloadType>(f_mat)*linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) * src.block(SolutionBlocks::unfiltered_density);
 
diff --git a/source/watchdog_main.cc b/source/watchdog_main.cc
index 06ca3e0..880aeb0 100644
--- a/source/watchdog_main.cc
+++ b/source/watchdog_main.cc
@@ -229,7 +229,7 @@ namespace SAND {
             std::cout << "loqo mult: " << loqo_multiplier << std::endl;
             if (loqo_multiplier< 0)
             {
-                barrier_size = .2 * loqo_average;
+                barrier_size = std::abs(loqo_multiplier) * loqo_average;
             }
             else
             {

From 5a432256794baf1eda4aecfd94f07abc58d0c79a Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Thu, 2 Dec 2021 14:05:20 -0700
Subject: [PATCH 41/95] frobenius norms of system matrix output

---
 source/kkt_system.cc           | 118 ++++++++++++++++++---------------
 source/schur_preconditioner.cc |   5 +-
 2 files changed, 67 insertions(+), 56 deletions(-)

diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index cdf2c10..1a2161e 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -727,8 +727,8 @@ namespace SAND {
                 {
                     std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
                     cell->get_dof_indices(i);
-                    dsp.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier).add(i[16], 0);
-                    dsp.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).add(0, i[16]);
+                    dsp.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier).add(i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)], 0);
+                    dsp.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).add(0, i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)]);
                 }
 
             }
@@ -745,9 +745,9 @@ namespace SAND {
                         neighbor_cell->get_dof_indices(j);
 
                         dsp.block(SolutionBlocks::unfiltered_density_multiplier,
-                                  SolutionBlocks::unfiltered_density).add(i[16], j[16]);
+                                  SolutionBlocks::unfiltered_density).add(i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)], j[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)]);
                         dsp.block(SolutionBlocks::unfiltered_density,
-                                  SolutionBlocks::unfiltered_density_multiplier).add(j[16], i[16]);
+                                  SolutionBlocks::unfiltered_density_multiplier).add(j[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)], i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)]);
                     }
                 }
             }
@@ -1137,22 +1137,26 @@ namespace SAND {
             {
                 std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
                 cell->get_dof_indices(i);
+
+                const types::global_dof_index dof_index_lower_slack =
+                        i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)];
+                Assert (cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>,0)==16, ExcInternalError());
                 typename LA::MPI::SparseMatrix::iterator iter = density_filter.filter_matrix.begin(
-                        i[16]);
-                for (; iter != density_filter.filter_matrix.end(i[16]); iter++) {
+                        i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)]);
+                for (; iter != density_filter.filter_matrix.end(i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)]); iter++) {
                     unsigned int j = iter->column();
                     double value = iter->value() * cell->measure();
 
                     system_matrix.block(SolutionBlocks::unfiltered_density_multiplier,
-                                        SolutionBlocks::unfiltered_density).add(i[16], j, value);
+                                        SolutionBlocks::unfiltered_density).add(i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)], j, value);
                     system_matrix.block(SolutionBlocks::unfiltered_density,
-                                        SolutionBlocks::unfiltered_density_multiplier).add(j, i[16], value);
+                                        SolutionBlocks::unfiltered_density_multiplier).add(j, i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)], value);
                 }
 
-                system_matrix.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).add(0, i[16],
+                system_matrix.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).add(0, i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)],
                                                                                                           cell->measure());
 
-                system_matrix.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier).add(i[16], 0,
+                system_matrix.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier).add(i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)], 0,
                                                                                                           cell->measure());
             }
         }
@@ -1772,51 +1776,61 @@ namespace SAND {
         SolverControl solver_control(10000, gmres_tolerance * system_rhs.l2_norm());
         TopOptSchurPreconditioner<dim> preconditioner(system_matrix);
 
-        std::cout << " rhs " << std::endl;
-        system_rhs.print(std::cout);
-        std::cout << std::endl;
+//        std::cout << " rhs " << std::endl;
+//        system_rhs.print(std::cout);
+//        std::cout << std::endl;
 
-        preconditioner.initialize(system_matrix, boundary_values, dof_handler, locally_relevant_solution, distributed_solution);
-        preconditioner.vmult(distributed_solution,system_rhs);
+//        preconditioner.initialize(system_matrix, boundary_values, dof_handler, locally_relevant_solution, distributed_solution);
+//        preconditioner.vmult(distributed_solution,system_rhs);
 
-        std::cout << "solution" <<  std::endl;
-        distributed_solution.print(std::cout);
-        std::cout << std::endl;
+//        std::cout << "solution" <<  std::endl;
+//        distributed_solution.print(std::cout);
+//        std::cout << std::endl;
 
-//        switch (Input::solver_choice) {
-//            case SolverOptions::direct_solve: {
-////                SolverControl cn;
-////                std::string solver_type;
-////                TrilinosWrappers::SolverDirect::AdditionalData additional_data(false, "Amesos_Lapack");
-////                TrilinosWrappers::SolverDirect solver(cn, additional_data);
-////                solver.initialize(system_matrix);
-////                solver.solve(distributed_solution, system_rhs);
-//                break;
-//            }
-//            case SolverOptions::exact_preconditioner_with_gmres: {
-//                preconditioner.initialize(system_matrix, boundary_values, dof_handler, locally_relevant_solution, distributed_solution);
-//                SolverFGMRES<LA::MPI::BlockVector> A_fgmres(solver_control);
-//                A_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
-//                std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
-//                break;
-//            }
-//            case SolverOptions::inexact_K_with_exact_A_gmres: {
-//                preconditioner.initialize(system_matrix, boundary_values, dof_handler,  locally_relevant_solution, distributed_solution);
-//                SolverFGMRES<LA::MPI::BlockVector> B_fgmres(solver_control);
-//                B_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
-//                std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
-//                break;
-//            }
-//            case SolverOptions::inexact_K_with_inexact_A_gmres: {
-//                preconditioner.initialize(system_matrix, boundary_values, dof_handler,  locally_relevant_solution, distributed_solution);
-//                SolverFGMRES<LA::MPI::BlockVector> C_fgmres(solver_control);
-//                C_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
-//                std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
-//                break;
-//            }
-//            default:
-//                throw;
-//        }
+
+        for (unsigned int k=0; k<10; k++)
+        {
+            for (unsigned int j=0; j<10; j++)
+            {
+                std::cout << k << ", " << j << std::endl;
+                std::cout << system_matrix.block(k,j).frobenius_norm() << std::endl;
+            }
+        }
+
+        switch (Input::solver_choice) {
+            case SolverOptions::direct_solve: {
+//                SolverControl cn;
+//                std::string solver_type;
+//                TrilinosWrappers::SolverDirect::AdditionalData additional_data(false, "Amesos_Lapack");
+//                TrilinosWrappers::SolverDirect solver(cn, additional_data);
+//                solver.initialize(system_matrix);
+//                solver.solve(distributed_solution, system_rhs);
+                break;
+            }
+            case SolverOptions::exact_preconditioner_with_gmres: {
+                preconditioner.initialize(system_matrix, boundary_values, dof_handler, locally_relevant_solution, distributed_solution);
+                SolverFGMRES<LA::MPI::BlockVector> A_fgmres(solver_control);
+                A_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
+                std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
+                break;
+            }
+            case SolverOptions::inexact_K_with_exact_A_gmres: {
+                preconditioner.initialize(system_matrix, boundary_values, dof_handler,  locally_relevant_solution, distributed_solution);
+                SolverFGMRES<LA::MPI::BlockVector> B_fgmres(solver_control);
+                B_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
+                std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
+                break;
+            }
+            case SolverOptions::inexact_K_with_inexact_A_gmres: {
+                preconditioner.initialize(system_matrix, boundary_values, dof_handler,  locally_relevant_solution, distributed_solution);
+                SolverFGMRES<LA::MPI::BlockVector> C_fgmres(solver_control);
+                C_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
+                std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
+                break;
+            }
+            default:
+                throw;
+        }
 
 
         constraints.distribute(distributed_solution);
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index fa61bee..5a5af7d 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -284,8 +284,6 @@ namespace SAND {
             vmult_step_1(dst, src);
             temp_src = dst;
         }
-        std::cout << std::endl << "vmult stuff pt 1:" << std::endl;
-        dst.print(std::cout);
 
         {
             TimerOutput::Scope t(timer, "part 2");
@@ -303,8 +301,7 @@ namespace SAND {
             vmult_step_4(dst, temp_src);
             temp_src = dst;
         }
-        vmult_step_5(dst, temp_src);
-
+        vmult_step_5(dst, temp_src);        
     }
 
     template<int dim>

From aa339503a2a5be3e6b2e00f89db1711b2bbf46f4 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Wed, 8 Dec 2021 17:45:01 -0700
Subject: [PATCH 42/95] works - need to fix outputs

---
 include/input_information.h    |   4 +-
 include/kkt_system.h           |   2 +-
 source/density_filter.cc       |   2 +-
 source/kkt_system.cc           |  22 ++--
 source/schur_preconditioner.cc | 230 ++++++++-------------------------
 5 files changed, 66 insertions(+), 194 deletions(-)

diff --git a/include/input_information.h b/include/input_information.h
index c8858d1..6ceda9c 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -17,13 +17,13 @@ namespace SAND {
         //geometry options
         constexpr unsigned int geometry_base = GeometryOptions::mbb;
         constexpr unsigned int dim = 2;
-        constexpr unsigned int refinements =  2;
+        constexpr unsigned int refinements =  3;
 
         //nonlinear algorithm options
         constexpr double initial_barrier_size = 25;
         constexpr double min_barrier_size = 0;
         constexpr double fraction_to_boundary = .9;
-        constexpr unsigned int max_steps=5;
+        constexpr unsigned int max_steps=25;
         constexpr unsigned int barrier_reduction=BarrierOptions::loqo;
         constexpr double required_norm = .0001;
 
diff --git a/include/kkt_system.h b/include/kkt_system.h
index 389d4a2..9db4a82 100644
--- a/include/kkt_system.h
+++ b/include/kkt_system.h
@@ -132,7 +132,7 @@ namespace SAND {
         mutable LA::MPI::BlockVector locally_relevant_solution;
         mutable LA::MPI::BlockVector distributed_solution;
         LA::MPI::BlockVector system_rhs;
-        parallel::distributed::Triangulation<dim> triangulation;
+        parallel::shared::Triangulation<dim> triangulation;
         DoFHandler<dim> dof_handler;
         AffineConstraints<double> constraints;
         FESystem<dim> fe_nine;
diff --git a/source/density_filter.cc b/source/density_filter.cc
index 155120e..78bddc9 100644
--- a/source/density_filter.cc
+++ b/source/density_filter.cc
@@ -25,11 +25,11 @@ namespace SAND {
         std::set<unsigned int> neighbor_ids;
         std::set<typename DoFHandler<dim>::cell_iterator> cells_to_check;
         std::set<typename DoFHandler<dim>::cell_iterator> cells_to_check_temp;
-
         /*finds neighbors whose values would be relevant, and adds them to the sparsity pattern of the matrix*/
         for (const auto &cell : dof_handler.active_cell_iterators()) {
             if(cell->is_locally_owned())
             {
+
                 std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
                 cell->get_dof_indices(i);
                 for (const auto &neighbor_cell : find_relevant_neighbors(cell))
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index 1a2161e..1ad22ed 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -1788,14 +1788,14 @@ namespace SAND {
 //        std::cout << std::endl;
 
 
-        for (unsigned int k=0; k<10; k++)
-        {
-            for (unsigned int j=0; j<10; j++)
-            {
-                std::cout << k << ", " << j << std::endl;
-                std::cout << system_matrix.block(k,j).frobenius_norm() << std::endl;
-            }
-        }
+//        for (unsigned int k=0; k<10; k++)
+//        {
+//            for (unsigned int j=0; j<10; j++)
+//            {
+//                std::cout << k << ", " << j << std::endl;
+//                std::cout << system_matrix.block(k,j).frobenius_norm() << std::endl;
+//            }
+//        }
 
         switch (Input::solver_choice) {
             case SolverOptions::direct_solve: {
@@ -1861,9 +1861,9 @@ namespace SAND {
 //            Mat.close();
         }
 
-        std::cout << "solution" << std::endl;
-        distributed_solution.print(std::cout);
-        std::cout << std::endl;
+//        std::cout << "solution" << std::endl;
+//        distributed_solution.print(std::cout);
+//        std::cout << std::endl;
 
         return distributed_solution;
     }
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 5a5af7d..f0f8b60 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -140,8 +140,27 @@ namespace SAND {
             d_7_mat=0;
             d_8_mat=0;
             d_m_inv_mat=0;
+            std::cout << "diag reinit" << std::endl;
         }
         {
+            const types::global_dof_index n_p = system_matrix.block(SolutionBlocks::density,
+                                                                    SolutionBlocks::density).m();
+
+            std::cout << "np: " << n_p << std::endl;
+
+
+            double l_global[n_p]= {0};
+            double lm_global[n_p]= {0};
+            double u_global[n_p]= {0};
+            double um_global[n_p]= {0};
+            double m_global[n_p]= {0};
+
+            double l[n_p]= {0};
+            double lm[n_p]= {0};
+            double u[n_p]= {0};
+            double um[n_p]= {0};
+            double m[n_p]= {0};
+
             TimerOutput::Scope t(timer, "build diag matrices");
             for (const auto cell: dof_handler.active_cell_iterators())
             {
@@ -149,55 +168,52 @@ namespace SAND {
                 {
                     std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
                     cell->get_dof_indices(i);
-                    const double m = cell->measure();
-                    double l = 0;
-                    double lm = 0;
-                    double u = 0;
-                    double um = 0;
 
 
-                    double l_global;
-                    double lm_global;
-                    double u_global;
-                    double um_global;
 
+                    const int i_ind = cell->get_fe().component_to_system_index(0,0);
 
-                    if(distributed_state.block(SolutionBlocks::density_lower_slack_multiplier).in_local_range(i[16]))
+                    if(distributed_state.block(SolutionBlocks::density_lower_slack_multiplier).in_local_range(i[i_ind]))
                     {
-                        lm = distributed_state.block(SolutionBlocks::density_lower_slack_multiplier)[i[16]];
+                        lm[i[i_ind]] = distributed_state.block(SolutionBlocks::density_lower_slack_multiplier)[i[i_ind]];
+                        m[i[i_ind]] = cell->measure();
                     }
 
-                    MPI_Allreduce(&lm, &lm_global, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-
-                    if(distributed_state.block(SolutionBlocks::density_lower_slack).in_local_range(i[16]))
+                    if(distributed_state.block(SolutionBlocks::density_lower_slack).in_local_range(i[i_ind]))
                     {
-                        l =  distributed_state.block(SolutionBlocks::density_lower_slack)[i[16]];
+                        l[i[i_ind]] =  distributed_state.block(SolutionBlocks::density_lower_slack)[i[i_ind]];
                     }
-                    MPI_Allreduce(&l, &l_global, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
 
-                    if(distributed_state.block(SolutionBlocks::density_upper_slack_multiplier).in_local_range(i[16]))
+                    if(distributed_state.block(SolutionBlocks::density_upper_slack_multiplier).in_local_range(i[i_ind]))
                     {
-                        um= distributed_state.block(SolutionBlocks::density_upper_slack_multiplier)[i[16]];
+                        um[i[i_ind]]= distributed_state.block(SolutionBlocks::density_upper_slack_multiplier)[i[i_ind]];
                     }
-                    MPI_Allreduce(&um, &um_global, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
 
-                    if(distributed_state.block(SolutionBlocks::density_upper_slack).in_local_range(i[16]))
+                    if(distributed_state.block(SolutionBlocks::density_upper_slack).in_local_range(i[i_ind]))
                     {
-                        u = distributed_state.block(SolutionBlocks::density_upper_slack)[i[16]];
+                        u[i[i_ind]] = distributed_state.block(SolutionBlocks::density_upper_slack)[i[i_ind]];
                     }
-                    MPI_Allreduce(&u, &u_global, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+                }
+            }
+
+            MPI_Allreduce(&lm, &lm_global, n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+            MPI_Allreduce(&l, &l_global, n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+            MPI_Allreduce(&um, &um_global, n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+            MPI_Allreduce(&u, &u_global, n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+            MPI_Allreduce(&m, &m_global, n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
 
-                    if(distributed_state.block(SolutionBlocks::density_lower_slack_multiplier).in_local_range(i[16]))
+            for (unsigned int k=0; k< n_p; k++)
+            {
+                    if(distributed_state.block(0).in_local_range(k))
                     {
-                        d_3_mat.set(i[16], i[16], -1 * lm_global/(m*l_global));
-                        d_4_mat.set(i[16], i[16], -1 * um_global/(m*u_global));
-                        d_5_mat.set(i[16], i[16], lm_global/l_global);
-                        d_6_mat.set(i[16], i[16], um_global/u_global);
-                        d_7_mat.set(i[16], i[16], m*(lm_global*u_global + um_global*l_global)/(l_global*u_global));
-                        d_8_mat.set(i[16], i[16], l_global*u_global/(m*(lm_global*u_global + um_global*l_global)));
-                        d_m_inv_mat.set(i[16], i[16], 1 / m);
+                        d_3_mat.set(k, k, -1 * lm_global[k]/(m_global[k]*l_global[k]));
+                        d_4_mat.set(k, k, -1 * um_global[k]/(m_global[k]*u_global[k]));
+                        d_5_mat.set(k, k, lm_global[k]/l_global[k]);
+                        d_6_mat.set(k, k, um_global[k]/u_global[k]);
+                        d_7_mat.set(k, k, m_global[k]*(lm_global[k]*u_global[k] + um_global[k]*l_global[k])/(l_global[k]*u_global[k]));
+                        d_8_mat.set(k, k, l_global[k]*u_global[k]/(m_global[k]*(lm_global[k]*u_global[k] + um_global[k]*l_global[k])));
+                        d_m_inv_mat.set(k, k, 1 / m_global[k]);
                     }
-                }
             }
         }
         d_3_mat.compress(VectorOperation::insert);
@@ -208,6 +224,8 @@ namespace SAND {
         d_8_mat.compress(VectorOperation::insert);
         d_m_inv_mat.compress(VectorOperation::insert);
 
+        std::cout << "compressed" << std::endl;
+
         pre_j=distributed_state.block(SolutionBlocks::density);
         pre_k=distributed_state.block(SolutionBlocks::density);
         g_d_m_inv_density=distributed_state.block(SolutionBlocks::density);
@@ -220,6 +238,8 @@ namespace SAND {
         op_d_8 = linear_operator<VectorType,VectorType,PayloadType>(d_8_mat);
         op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) * transpose_operator<VectorType, VectorType, PayloadType>(linear_operator<VectorType,VectorType,PayloadType>(f_mat));
 
+        std::cout << "ops made" << std::endl;
+
         if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
         {
             SolverControl            solver_control(100000, 1e-6);
@@ -660,154 +680,6 @@ namespace SAND {
         }
     }
 
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::get_sparsity_pattern(BlockDynamicSparsityPattern &bdsp) {
-        mass_sparsity.copy_from(bdsp);
-    }
-
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::assemble_mass_matrix(const LA::MPI::BlockVector &state,
-                                                              const hp::FECollection<dim> &fe_collection,
-                                                              const DoFHandler<dim> &dof_handler,
-                                                              const AffineConstraints<double> &constraints,
-                                                              const BlockSparsityPattern &bsp) {
-        timer.reset();
-
-        approx_h_mat.reinit(bsp);
-
-        /*Remove any values from old iterations*/
-        QGauss<dim> nine_quadrature(fe_collection[0].degree + 1);
-        QGauss<dim> ten_quadrature(fe_collection[1].degree + 1);
-
-        hp::QCollection<dim> q_collection;
-        q_collection.push_back(nine_quadrature);
-        q_collection.push_back(ten_quadrature);
-
-        hp::FEValues<dim> hp_fe_values(fe_collection,
-                                       q_collection,
-                                       update_values | update_quadrature_points |
-                                       update_JxW_values | update_gradients);
-        FullMatrix<double> cell_matrix;
-        Vector<double>     cell_rhs;
-        std::vector<types::global_dof_index> local_dof_indices;
-
-        const FEValuesExtractors::Scalar densities(SolutionComponents::density<dim>);
-        const FEValuesExtractors::Vector displacements(SolutionComponents::displacement<dim>);
-        const FEValuesExtractors::Scalar unfiltered_densities(SolutionComponents::unfiltered_density<dim>);
-        const FEValuesExtractors::Vector displacement_multipliers(SolutionComponents::displacement_multiplier<dim>);
-        const FEValuesExtractors::Scalar unfiltered_density_multipliers(
-                SolutionComponents::unfiltered_density_multiplier<dim>);
-        const FEValuesExtractors::Scalar density_lower_slacks(SolutionComponents::density_lower_slack<dim>);
-        const FEValuesExtractors::Scalar density_lower_slack_multipliers(
-                SolutionComponents::density_lower_slack_multiplier<dim>);
-        const FEValuesExtractors::Scalar density_upper_slacks(SolutionComponents::density_upper_slack<dim>);
-        const FEValuesExtractors::Scalar density_upper_slack_multipliers(
-                SolutionComponents::density_upper_slack_multiplier<dim>);
-        const FEValuesExtractors::Scalar total_volume_multiplier(
-                SolutionComponents::total_volume_multiplier<dim>);
-
-        const Functions::ConstantFunction<dim> lambda(1.), mu(1.);
-
-        LA::MPI::BlockVector filtered_unfiltered_density_solution = state;
-        LA::MPI::BlockVector filter_adjoint_unfiltered_density_multiplier_solution = state;
-        filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = 0;
-        filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier) = 0;
-
-
-        for (const auto &cell : dof_handler.active_cell_iterators()) {
-            hp_fe_values.reinit(cell);
-            const FEValues<dim> &fe_values = hp_fe_values.get_present_fe_values();
-            cell_matrix.reinit(cell->get_fe().n_dofs_per_cell(),
-                               cell->get_fe().n_dofs_per_cell());
-            cell_rhs.reinit(cell->get_fe().n_dofs_per_cell());
-
-            const unsigned int n_q_points = fe_values.n_quadrature_points;
-
-            std::vector<double> old_density_values(n_q_points);
-            std::vector<double> old_displacement_divs(n_q_points);
-            std::vector<SymmetricTensor<2, dim>> old_displacement_symmgrads(
-                    n_q_points);
-            std::vector<double> old_displacement_multiplier_divs(n_q_points);
-            std::vector<SymmetricTensor<2, dim>> old_displacement_multiplier_symmgrads(
-                    n_q_points);
-            std::vector<double> lambda_values(n_q_points);
-            std::vector<double> mu_values(n_q_points);
-
-            const unsigned int dofs_per_cell = cell->get_fe().n_dofs_per_cell();
-
-            cell_matrix = 0;
-            cell_rhs = 0;
-            local_dof_indices.resize(cell->get_fe().n_dofs_per_cell());
-            cell->get_dof_indices(local_dof_indices);
-
-            lambda.value_list(fe_values.get_quadrature_points(), lambda_values);
-            mu.value_list(fe_values.get_quadrature_points(), mu_values);
-
-            fe_values[densities].get_function_values(state,
-                                                     old_density_values);
-            fe_values[displacements].get_function_divergences(state,
-                                                              old_displacement_divs);
-            fe_values[displacements].get_function_symmetric_gradients(
-                    state, old_displacement_symmgrads);
-            fe_values[displacement_multipliers].get_function_divergences(
-                    state, old_displacement_multiplier_divs);
-            fe_values[displacement_multipliers].get_function_symmetric_gradients(
-                    state, old_displacement_multiplier_symmgrads);
-
-            Tensor<1, dim> traction;
-            traction[1] = -1;
-
-            for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
-
-                for (unsigned int i = 0; i < dofs_per_cell; ++i) {
-
-                    const double unfiltered_density_phi_i = fe_values[unfiltered_densities].value(i,
-                                                                                                  q_point);
-                    const double density_phi_i = fe_values[densities].value(i,q_point);
-
-                    for (unsigned int j = 0; j < dofs_per_cell; ++j) {
-
-                        const double unfiltered_density_phi_j = fe_values[unfiltered_densities].value(j,
-                                                                                                      q_point);
-                        const double density_phi_j = fe_values[densities].value(j,q_point);
-
-
-
-                        double value =   unfiltered_density_phi_i
-                                       * unfiltered_density_phi_j
-                                       * (-1 * Input::density_penalty_exponent * Input::density_penalty_exponent - Input::density_penalty_exponent)
-                                       * std::pow(old_density_values[q_point],Input::density_penalty_exponent - 2)
-                                       *
-                                       (old_displacement_divs[q_point] * old_displacement_multiplier_divs[q_point]
-                                        * lambda_values[q_point]
-                                        +
-                                        2 * mu_values[q_point] * (old_displacement_symmgrads[q_point] *
-                                                                  old_displacement_multiplier_symmgrads[q_point]));
-
-
-                        value +=   density_phi_i
-                                * density_phi_j
-                                * (-2 * Input::density_penalty_exponent * Input::density_penalty_exponent)
-                                * std::pow(old_density_values[q_point],Input::density_penalty_exponent - 2)
-                                * old_displacement_symmgrads[q_point].norm() *
-                                old_displacement_multiplier_symmgrads[q_point].norm();
-
-                        if (value != 0)
-                        {
-                            cell_matrix(i, j) +=
-                                    fe_values.JxW(q_point) * value ;
-                        }
-                    }
-
-                }
-
-            }
-
-            constraints.distribute_local_to_global(cell_matrix, local_dof_indices, approx_h_mat);
-        }
-
-    }
-
 
     template<int dim>
     void TopOptSchurPreconditioner<dim>::print_stuff()

From 90dc7f3f1c9f14571ed37fa64b9cd6289829bd3f Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Thu, 9 Dec 2021 09:58:18 -0700
Subject: [PATCH 43/95] getting there

---
 source/schur_preconditioner.cc | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index f0f8b60..a936565 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -149,6 +149,8 @@ namespace SAND {
             std::cout << "np: " << n_p << std::endl;
 
 
+
+
             double l_global[n_p]= {0};
             double lm_global[n_p]= {0};
             double u_global[n_p]= {0};

From 04ff1fffe42695683a6f7319b378c5656358137b Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Fri, 10 Dec 2021 08:57:11 -0700
Subject: [PATCH 44/95] mod

---
 source/density_filter.cc.autosave |  128 ++
 source/kkt_system.cc              |    4 +-
 source/kkt_system.cc.autosave     | 2300 +++++++++++++++++++++++++++++
 3 files changed, 2431 insertions(+), 1 deletion(-)
 create mode 100644 source/density_filter.cc.autosave
 create mode 100644 source/kkt_system.cc.autosave

diff --git a/source/density_filter.cc.autosave b/source/density_filter.cc.autosave
new file mode 100644
index 0000000..706552e
--- /dev/null
+++ b/source/density_filter.cc.autosave
@@ -0,0 +1,128 @@
+//
+// Created by justin on 5/13/21.
+//
+#include "../include/density_filter.h"
+#include "../include/input_information.h"
+#include <deal.II/base/tensor.h>
+#include <deal.II/lac/block_sparse_matrix.h>
+#include <deal.II/lac/precondition.h>
+#include <deal.II/grid/tria.h>
+#include <deal.II/numerics/vector_tools.h>
+#include <deal.II/numerics/matrix_tools.h>
+
+namespace SAND {
+    using namespace dealii;
+
+    /* When initialized, this function takes the current triangulation and creates a matrix corresponding to a
+     * convolution being applied to a piecewise constant function on that triangulation  */
+    template<int dim>
+    void
+    DensityFilter<dim>::initialize(DoFHandler<dim> &dof_handler) {
+        const std::vector<types::global_dof_index> dofs_per_block = DoFTools::count_dofs_per_fe_block(dof_handler);
+        filter_dsp.reinit(dofs_per_block[0],
+                          dofs_per_block[0]);
+        std::cout << "initialized with " << dofs_per_block[0] << std::endl;
+        std::set<unsigned int> neighbor_ids;
+        std::set<typename DoFHandler<dim>::cell_iterator> cells_to_check;
+        std::set<typename DoFHandler<dim>::cell_iterator> cells_to_check_temp;
+        /*finds neighbors whose values would be relevant, and adds them to the sparsity pattern of the matrix*/
+        for (const auto &cell : dof_handler.active_cell_iterators()) {
+            if(cell->is_locally_owned())
+            {
+
+                std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
+                cell->get_dof_indices(i);
+                for (const auto &neighbor_cell : find_relevant_neighbors(cell))
+                {
+                    std::vector<types::global_dof_index> j(neighbor_cell->get_fe().n_dofs_per_cell());
+                    neighbor_cell->get_dof_indices(j);
+                    filter_dsp.add(i[cell->get_fe().component_to_system_index(0,0)], j[cell->get_fe().component_to_system_index(0,0)]);
+                }
+            }
+
+        }
+        filter_sparsity_pattern.copy_from(filter_dsp);
+
+        const auto owned_dofs = dof_handler.locally_owned_dofs().get_view(0, dofs_per_block[0]);
+
+        filter_matrix.reinit(owned_dofs, filter_sparsity_pattern, MPI_COMM_WORLD);
+
+        /*adds values to the matrix corresponding to the max radius - */
+        for (const auto &cell : dof_handler.active_cell_iterators())
+        {
+            if(cell->is_locally_owned())
+            {
+                std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
+                cell->get_dof_indices(i);
+                for (const auto &neighbor_cell : find_relevant_neighbors(cell)) {
+                    std::vector<types::global_dof_index> j(neighbor_cell->get_fe().n_dofs_per_cell());
+                    neighbor_cell->get_dof_indices(j);
+                    const double d =
+                            cell->center().distance(neighbor_cell->center());
+                    /*value should be (max radius - distance between cells)*cell measure */
+                    double value = (Input::filter_r - d)*neighbor_cell->measure();
+                    filter_matrix.add(i[cell->get_fe().component_to_system_index(0,0)], j[cell->get_fe().component_to_system_index(0,0)], value);
+                }
+            }
+        }
+
+        //here we normalize the filter so it computes an average. Sum of values in a row should be 1
+        for (const auto &cell : dof_handler.active_cell_iterators())
+        {
+            if(cell->is_locally_owned())
+            {
+                std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
+                cell->get_dof_indices(i);
+                double denominator = 0;
+                typename LA::MPI::SparseMatrix::iterator iter = filter_matrix.begin(
+                        i[cell->get_fe().component_to_system_index(0,0)]);
+                for (; iter != filter_matrix.end(i[cell->get_fe().component_to_system_index(0,0)]); iter++)
+                {
+                    denominator = denominator + iter->value();
+                }
+                iter = filter_matrix.begin(i[cell->get_fe().component_to_system_index(0,0)]);
+                for (; iter != filter_matrix.end(i[cell->get_fe().component_to_system_index(0,0)]); iter++)
+                {
+                    iter->value() = iter->value() / denominator;
+                }
+            }
+        }
+        std::cout << "density nonzero entries" << filter_matrix.n_nonzero_elements() << std::endl;
+    }
+
+    /*This function finds which neighbors are within a certain radius of the initial cell.*/
+    template<int dim>
+    std::set<typename DoFHandler<dim>::cell_iterator>
+    DensityFilter<dim>::find_relevant_neighbors(typename DoFHandler<dim>::cell_iterator cell) const {
+        std::set<unsigned int> neighbor_ids;
+        std::set<typename DoFHandler<dim>::cell_iterator> cells_to_check;
+        neighbor_ids.insert(cell->active_cell_index());
+        cells_to_check.insert(cell);
+        bool new_neighbors_found;
+        do {
+            new_neighbors_found = false;
+            for (const auto &check_cell :
+                    std::vector<typename DoFHandler<dim>::cell_iterator>(
+                            cells_to_check.begin(), cells_to_check.end())) {
+                for (const auto n : check_cell->face_indices()) {
+                    if (!(check_cell->face(n)->at_boundary())) {
+                        const auto &neighbor = check_cell->neighbor(n);
+                        const double distance =
+                                cell->center().distance(neighbor->center());
+                        if ((distance < Input::filter_r) &&
+                            !(neighbor_ids.count(neighbor->active_cell_index())))
+                        {
+                            cells_to_check.insert(neighbor);
+                            neighbor_ids.insert(neighbor->active_cell_index());
+                            new_neighbors_found = true;
+                        }
+                    }
+                }
+            }
+        } while (new_neighbors_found);
+        return cells_to_check;
+    }
+
+}//SAND namespace
+    template class SAND::DensityFilter<2>;
+    template class SAND::DensityFilter<3>;
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index 1ad22ed..6b22358 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -783,7 +783,7 @@ namespace SAND {
         LA::MPI::BlockVector relevant_state(owned_partitioning, relevant_partitioning, mpi_communicator);
         relevant_state = distributed_state;
 
-        system_matrix=0;
+        system_matrix.reinit(owned_partitioning, dsp, mpi_communicator);
         locally_relevant_solution = 0;
         system_rhs = 0;
 
@@ -1129,6 +1129,7 @@ namespace SAND {
             }
 
         }
+        std::cout << "compress 1" << std::endl;
         system_matrix.compress(VectorOperation::add);
         system_rhs = calculate_rhs(distributed_state, barrier_size);
 
@@ -1160,6 +1161,7 @@ namespace SAND {
                                                                                                           cell->measure());
             }
         }
+        std::cout << "compress 2" << std::endl;
         system_matrix.compress(VectorOperation::add);
 
 
diff --git a/source/kkt_system.cc.autosave b/source/kkt_system.cc.autosave
new file mode 100644
index 0000000..1b5a8b4
--- /dev/null
+++ b/source/kkt_system.cc.autosave
@@ -0,0 +1,2300 @@
+//
+// Created by justin on 2/17/21.
+//
+#include "../include/kkt_system.h"
+#include <deal.II/base/quadrature_lib.h>
+#include <deal.II/base/function.h>
+#include <deal.II/base/tensor.h>
+#include <deal.II/base/timer.h>
+
+#include <deal.II/lac/full_matrix.h>
+#include <deal.II/lac/linear_operator.h>
+#include <deal.II/lac/solver_gmres.h>
+#include <deal.II/lac/generic_linear_algebra.h>
+#include <deal.II/lac/trilinos_sparse_matrix.h>
+
+
+#include <deal.II/lac/matrix_out.h>
+
+#include <deal.II/grid/grid_generator.h>
+
+#include <deal.II/dofs/dof_renumbering.h>
+#include <deal.II/dofs/dof_tools.h>
+
+#include <deal.II/fe/fe_values.h>
+#include <deal.II/fe/fe_dgq.h>
+#include <deal.II/fe/fe_system.h>
+#include <deal.II/fe/fe_q.h>
+#include <deal.II/fe/fe_nothing.h>
+
+#include <deal.II/hp/fe_collection.h>
+#include <deal.II/hp/fe_values.h>
+
+#include <deal.II/numerics/vector_tools.h>
+#include <deal.II/numerics/matrix_tools.h>
+#include <deal.II/numerics/data_out.h>
+#include <deal.II/numerics/error_estimator.h>
+
+#include "../include/input_information.h"
+
+#include <iostream>
+#include <algorithm>
+
+// This problem initializes with a FESystem composed of 2×dim FE_Q(1) elements, and 8 FE_DGQ(0)  elements.
+// The  piecewise  constant  functions  are  for  density-related  variables,and displacement-related variables are assigned to the FE_Q(1) elements.
+namespace SAND {
+    template<int dim>
+    KktSystem<dim>::KktSystem()
+            :
+            mpi_communicator(MPI_COMM_WORLD),
+            triangulation(mpi_communicator,
+                          typename Triangulation<dim>::MeshSmoothing(
+                                  Triangulation<dim>::smoothing_on_refinement |
+                                  Triangulation<dim>::smoothing_on_coarsening)),
+            dof_handler(triangulation),
+            /*fe should have 1 FE_DGQ<dim>(0) element for density, dim FE_Q finite elements for displacement,
+             * another dim FE_Q elements for the lagrange multiplier on the FE constraint, and 2 more FE_DGQ<dim>(0)
+             * elements for the upper and lower bound constraints */
+            fe_nine(FE_DGQ<dim>(0) ^ 5,
+                    (FESystem<dim>(FE_Q<dim>(1) ^ dim)) ^ 2,
+                    FE_DGQ<dim>(0) ^ 2,
+                    FE_Nothing<dim>() ^ 1),
+            fe_ten(FE_DGQ<dim>(0) ^ 5,
+                   (FESystem<dim>(FE_Q<dim>(1) ^ dim)) ^ 2,
+                   FE_DGQ<dim>(0) ^ 2,
+                   FE_DGQ<dim>(0) ^ 1),
+            density_ratio(Input::volume_percentage),
+            density_penalty_exponent(Input::density_penalty_exponent),
+            density_filter()
+    {
+        fe_collection.push_back(fe_nine);
+        fe_collection.push_back(fe_ten);
+
+    }
+
+
+//A  function  used  once  at  the  beginning  of  the  program,  this  creates  a  matrix  H  so  that H* unfiltered density = filtered density
+
+    template<int dim>
+    void
+    KktSystem<dim>::setup_filter_matrix() {
+
+        density_filter.initialize(dof_handler);
+    }
+
+    //This triangulation matches the problem description -
+    // a 6-by-1 rectangle where a force will be applied in the top center.
+
+    template<int dim>
+    void
+    KktSystem<dim>::create_triangulation() {
+
+        std::vector<unsigned int> sub_blocks(2*dim+8, 0);
+
+        sub_blocks[0]=0;
+        sub_blocks[1]=1;
+        sub_blocks[2]=2;
+        sub_blocks[3]=3;
+        sub_blocks[4]=4;
+        for(int i=0; i<dim; i++)
+        {
+            sub_blocks[5+i]=5;
+        }
+        for(int i=0; i<dim; i++)
+        {
+            sub_blocks[5+dim+i]=6;
+        }
+        sub_blocks[5+2*dim]=7;
+        sub_blocks[6+2*dim]=8;
+        sub_blocks[7+2*dim]=9;
+
+
+        if (Input::geometry_base == GeometryOptions::mbb) {
+            const double width = 6;
+            const unsigned int width_refine = 6;
+            const double height = 1;
+            const unsigned int height_refine = 1;
+            const double depth = 1;
+            const unsigned int depth_refine = 1;
+            const double downforce_y = 1;
+            const double downforce_x = 3;
+            const double downforce_size = .3;
+
+            if (dim == 2)
+            {
+                GridGenerator::subdivided_hyper_rectangle(triangulation,
+                                                          {width_refine, height_refine},
+                                                          Point<dim>(0, 0),
+                                                          Point<dim>(width, height));
+
+                triangulation.refine_global(Input::refinements);
+
+                /*Set BCIDs   */
+                for (const auto &cell: dof_handler.active_cell_iterators()) {
+                    if(cell->is_locally_owned())
+                    {
+                        cell->set_active_fe_index(0);
+                        cell->set_material_id(MaterialIds::without_multiplier);
+                        for (unsigned int face_number = 0;
+                             face_number < GeometryInfo<dim>::faces_per_cell;
+                             ++face_number) {
+                            if (cell->face(face_number)->at_boundary()) {
+                                const auto center = cell->face(face_number)->center();
+
+                                if (std::fabs(center(1) - downforce_y) < 1e-12) {
+                                    if (std::fabs(center(0) - downforce_x) < downforce_size) {
+                                        cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
+                                    } else {
+                                        cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
+                                    }
+                                }
+                            }
+                        }
+                        for (unsigned int vertex_number = 0;
+                             vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                             ++vertex_number) {
+                            if (std::abs(cell->vertex(vertex_number)(0)) + std::abs(cell->vertex(vertex_number)(1)) <
+                                1e-10) {
+                                cell->set_active_fe_index(1);
+                                cell->set_material_id(MaterialIds::with_multiplier);
+                            }
+                        }
+                    }
+                }
+
+
+
+                dof_handler.distribute_dofs(fe_collection);
+
+                DoFRenumbering::component_wise(dof_handler, sub_blocks);
+            }
+            else if (dim == 3)
+            {
+                GridGenerator::subdivided_hyper_rectangle(triangulation,
+                                                          {width_refine, height_refine, depth_refine},
+                                                          Point<dim>(0, 0, 0),
+                                                          Point<dim>(width, height, depth));
+
+                triangulation.refine_global(Input::refinements);
+
+                for (const auto &cell: dof_handler.active_cell_iterators()) {
+                    if (cell->is_locally_owned())
+                    {
+                        cell->set_active_fe_index(0);
+                        cell->set_material_id(MaterialIds::without_multiplier);
+                        for (unsigned int face_number = 0;
+                             face_number < GeometryInfo<dim>::faces_per_cell;
+                             ++face_number) {
+                            if (cell->face(face_number)->at_boundary()) {
+                                const auto center = cell->face(face_number)->center();
+
+                                if (std::fabs(center(1) - downforce_y) < 1e-12) {
+                                    if (std::fabs(center(0) - downforce_x) < downforce_size) {
+                                        cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
+                                    } else {
+                                        cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
+                                    }
+                                }
+                            }
+                        }
+                        for (unsigned int vertex_number = 0;
+                             vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                             ++vertex_number) {
+                            if (std::abs(cell->vertex(vertex_number)(0)) + std::abs(cell->vertex(vertex_number)(1))
+                                + std::abs(cell->vertex(vertex_number)(2)) < 1e-10) {
+                                cell->set_active_fe_index(1);
+                                cell->set_material_id(MaterialIds::with_multiplier);
+                            }
+                        }
+                    }
+                }
+
+                dof_handler.distribute_dofs(fe_collection);
+
+                DoFRenumbering::component_wise(dof_handler, sub_blocks);
+
+            } else {
+                throw;
+            }
+        } else if (Input::geometry_base == GeometryOptions::l_shape) {
+            const double width = 2;
+            const unsigned int width_refine = 2;
+            const double height = 2;
+            const unsigned int height_refine = 2;
+            const double depth = 1;
+            const unsigned int depth_refine = 1;
+            const double downforce_x = 2;
+            const double downforce_y = 1;
+            const double downforce_z = .5;
+            const double downforce_size = .3;
+
+            if (dim == 2) {
+                GridGenerator::subdivided_hyper_L(triangulation,
+                                                  {width_refine, height_refine},
+                                                  Point<dim>(0, 0),
+                                                  Point<dim>(width, height),
+                                                  {-1, -1});
+
+                triangulation.refine_global(Input::refinements);
+
+                /*Set BCIDs   */
+                for (const auto &cell: dof_handler.active_cell_iterators()) {
+                    if (cell->is_locally_owned())
+                    {
+                        cell->set_active_fe_index(0);
+                        cell->set_material_id(MaterialIds::without_multiplier);
+                        for (unsigned int face_number = 0;
+                             face_number < GeometryInfo<dim>::faces_per_cell;
+                             ++face_number) {
+                            if (cell->face(face_number)->at_boundary()) {
+                                const auto center = cell->face(face_number)->center();
+
+                                if (std::fabs(center(0) - downforce_x) < 1e-12) {
+                                    if (std::fabs(center(1) - downforce_y) < downforce_size) {
+                                        cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
+                                    } else {
+                                        cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
+                                    }
+                                }
+                            }
+                        }
+                        for (unsigned int vertex_number = 0;
+                             vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                             ++vertex_number) {
+                            if (std::abs(cell->vertex(vertex_number)(0)) + std::abs(cell->vertex(vertex_number)(1)) <
+                                1e-10) {
+                                cell->set_active_fe_index(1);
+                                cell->set_material_id(MaterialIds::with_multiplier);
+                            }
+                        }
+                    }
+                }
+
+                dof_handler.distribute_dofs(fe_collection);
+
+                DoFRenumbering::component_wise(dof_handler, sub_blocks);
+
+            } else if (dim == 3) {
+                GridGenerator::subdivided_hyper_L(triangulation,
+                                                  {width_refine, height_refine, depth_refine},
+                                                  Point<dim>(0, 0, 0),
+                                                  Point<dim>(width, height, depth),
+                                                  {-1, -1, depth_refine});
+
+                triangulation.refine_global(Input::refinements);
+
+                /*Set BCIDs   */
+                for (const auto &cell: dof_handler.active_cell_iterators()) {
+                    if(cell->is_locally_owned())
+                    {
+                        cell->set_active_fe_index(0);
+                        cell->set_material_id(MaterialIds::without_multiplier);
+                        for (unsigned int face_number = 0;
+                             face_number < GeometryInfo<dim>::faces_per_cell;
+                             ++face_number) {
+                            if (cell->face(face_number)->at_boundary()) {
+                                const auto center = cell->face(face_number)->center();
+
+                                if (std::fabs(center(0) - downforce_x) < 1e-12) {
+                                    if (std::fabs(center(1) - downforce_y) < downforce_size) {
+                                        cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
+                                        if (std::fabs(center(2) - downforce_z) < downforce_size) {
+                                            cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
+                                        } else {
+                                            cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
+                                        }
+                                    } else {
+                                        cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
+                                    }
+                                }
+                            }
+                        }
+                        for (unsigned int vertex_number = 0;
+                             vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                             ++vertex_number) {
+                            if (std::abs(cell->vertex(vertex_number)(0)) + std::abs(cell->vertex(vertex_number)(1)) <
+                                1e-10) {
+                                cell->set_active_fe_index(1);
+                                cell->set_material_id(MaterialIds::with_multiplier);
+                            }
+                        }
+                    }
+                }
+
+                dof_handler.distribute_dofs(fe_collection);
+
+                DoFRenumbering::component_wise(dof_handler, sub_blocks);
+            } else {
+                throw;
+            }
+        } else {
+            throw;
+        }
+
+    }
+
+// The  bottom  corners  are  kept  in  place  in  the  y  direction  -  the  bottom  left  also  in  the  x direction.
+// Because deal.ii is formulated to enforce boundary conditions along regions of the boundary,
+// we do this to ensure these BCs are only enforced at points.
+    template<int dim>
+    void
+    KktSystem<dim>::setup_boundary_values() {
+        if (Input::geometry_base == GeometryOptions::mbb) {
+            if (dim == 2) {
+                for (const auto &cell: dof_handler.active_cell_iterators()) {
+                    if(cell->is_locally_owned())
+                    {
+                        for (unsigned int face_number = 0;
+                             face_number < GeometryInfo<dim>::faces_per_cell;
+                             ++face_number) {
+                            if (cell->face(face_number)->at_boundary()) {
+                                for (unsigned int vertex_number = 0;
+                                     vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                                     ++vertex_number) {
+                                    const auto vert = cell->vertex(vertex_number);
+                                    /*Find bottom left corner*/
+                                    if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
+                                            vert(1) - 0) < 1e-12) {
+
+                                        const unsigned int x_displacement =
+                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
+                                        const unsigned int y_displacement =
+                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
+                                        const unsigned int x_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
+                                        const unsigned int y_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
+                                        /*set bottom left BC*/
+                                        boundary_values[x_displacement] = 0;
+                                        boundary_values[y_displacement] = 0;
+                                        boundary_values[x_displacement_multiplier] = 0;
+                                        boundary_values[y_displacement_multiplier] = 0;
+                                    }
+                                    /*Find bottom right corner*/
+                                    if (std::fabs(vert(0) - 6) < 1e-12 && std::fabs(
+                                            vert(1) - 0) < 1e-12) {
+//                            const unsigned int x_displacement =
+//                                    cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
+                                        const unsigned int y_displacement =
+                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
+//                            const unsigned int x_displacement_multiplier =
+//                                    cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
+                                        const unsigned int y_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
+//                            boundary_values[x_displacement] = 0;
+                                        boundary_values[y_displacement] = 0;
+//                            boundary_values[x_displacement_multiplier] = 0;
+                                        boundary_values[y_displacement_multiplier] = 0;
+                                    }
+                                }
+                            }
+                        }
+                    }
+
+                }
+            } else if (dim == 3) {
+                for (const auto &cell: dof_handler.active_cell_iterators()) {
+                    if(cell->is_locally_owned())
+                    {
+                        for (unsigned int face_number = 0;
+                             face_number < GeometryInfo<dim>::faces_per_cell;
+                             ++face_number) {
+                            if (cell->face(face_number)->at_boundary()) {
+                                for (unsigned int vertex_number = 0;
+                                     vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                                     ++vertex_number) {
+                                    const auto vert = cell->vertex(vertex_number);
+                                    /*Find bottom left corner*/
+                                    if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
+                                            vert(1) - 0) < 1e-12 && ((std::fabs(
+                                            vert(2) - 0) < 1e-12) || (std::fabs(
+                                            vert(2) - 1) < 1e-12))) {
+
+
+                                        const unsigned int x_displacement =
+                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
+                                        const unsigned int y_displacement =
+                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
+                                        const unsigned int z_displacement =
+                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
+                                        const unsigned int x_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
+                                        const unsigned int y_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 4, cell->active_fe_index());
+                                        const unsigned int z_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 5, cell->active_fe_index());
+
+                                        boundary_values[x_displacement] = 0;
+                                        boundary_values[y_displacement] = 0;
+                                        boundary_values[z_displacement] = 0;
+                                        boundary_values[x_displacement_multiplier] = 0;
+                                        boundary_values[y_displacement_multiplier] = 0;
+                                        boundary_values[z_displacement_multiplier] = 0;
+                                    }
+                                    /*Find bottom right corner*/
+                                    if (std::fabs(vert(0) - 6) < 1e-12 && std::fabs(
+                                            vert(1) - 0) < 1e-12 && ((std::fabs(
+                                            vert(2) - 0) < 1e-12) || (std::fabs(
+                                            vert(2) - 1) < 1e-12))) {
+//                              const unsigned int x_displacement =
+//                                    cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
+                                        const unsigned int y_displacement =
+                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
+                                        const unsigned int z_displacement =
+                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
+//                              const unsigned int x_displacement_multiplier =
+//                                    cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
+                                        const unsigned int y_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 4, cell->active_fe_index());
+                                        const unsigned int z_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 5, cell->active_fe_index());
+//                              boundary_values[x_displacement] = 0;
+                                        boundary_values[y_displacement] = 0;
+                                        boundary_values[z_displacement] = 0;
+//                              boundary_values[x_displacement_multiplier] = 0;
+                                        boundary_values[y_displacement_multiplier] = 0;
+                                        boundary_values[z_displacement_multiplier] = 0;
+                                    }
+                                }
+                            }
+                        }
+                    }
+                }
+            } else {
+                throw;
+            }
+        } else if (Input::geometry_base == GeometryOptions::l_shape) {
+            if (dim == 2) {
+                for (const auto &cell: dof_handler.active_cell_iterators()) {
+                    if(cell->is_locally_owned())
+                    {
+                        for (unsigned int face_number = 0;
+                             face_number < GeometryInfo<dim>::faces_per_cell;
+                             ++face_number) {
+                            if (cell->face(face_number)->at_boundary()) {
+                                for (unsigned int vertex_number = 0;
+                                     vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                                     ++vertex_number) {
+                                    const auto vert = cell->vertex(vertex_number);
+                                    /*Find top left corner*/
+                                    if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
+                                            vert(1) - 2) < 1e-12) {
+
+                                        const unsigned int x_displacement =
+                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
+                                        const unsigned int y_displacement =
+                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
+                                        const unsigned int x_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
+                                        const unsigned int y_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
+                                        /*set bottom left BC*/
+                                        boundary_values[x_displacement] = 0;
+                                        boundary_values[y_displacement] = 0;
+                                        boundary_values[x_displacement_multiplier] = 0;
+                                        boundary_values[y_displacement_multiplier] = 0;
+                                    }
+                                    /*Find top right corner*/
+                                    if (std::fabs(vert(0) - 1) < 1e-12 && std::fabs(
+                                            vert(1) - 2) < 1e-12) {
+                                        const unsigned int x_displacement =
+                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
+                                        const unsigned int y_displacement =
+                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
+                                        const unsigned int x_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
+                                        const unsigned int y_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
+                                        boundary_values[x_displacement] = 0;
+                                        boundary_values[y_displacement] = 0;
+                                        boundary_values[x_displacement_multiplier] = 0;
+                                        boundary_values[y_displacement_multiplier] = 0;
+                                    }
+                                }
+                            }
+                        }
+                    }
+                }
+            } else if (dim == 3) {
+                for (const auto &cell: dof_handler.active_cell_iterators()) {
+                    if(cell->is_locally_owned())
+                    {
+                        for (unsigned int face_number = 0;
+                             face_number < GeometryInfo<dim>::faces_per_cell;
+                             ++face_number) {
+                            if (cell->face(face_number)->at_boundary()) {
+                                for (unsigned int vertex_number = 0;
+                                     vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                                     ++vertex_number) {
+                                    const auto vert = cell->vertex(vertex_number);
+                                    /*Find bottom left corner*/
+                                    if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
+                                            vert(1) - 2) < 1e-12 && ((std::fabs(
+                                            vert(2) - 0) < 1e-12) || (std::fabs(
+                                            vert(2) - 1) < 1e-12))) {
+
+
+                                        const unsigned int x_displacement =
+                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
+                                        const unsigned int y_displacement =
+                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
+                                        const unsigned int z_displacement =
+                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
+                                        const unsigned int x_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
+                                        const unsigned int y_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 4, cell->active_fe_index());
+                                        const unsigned int z_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 5, cell->active_fe_index());
+
+                                        boundary_values[x_displacement] = 0;
+                                        boundary_values[y_displacement] = 0;
+                                        boundary_values[z_displacement] = 0;
+                                        boundary_values[x_displacement_multiplier] = 0;
+                                        boundary_values[y_displacement_multiplier] = 0;
+                                        boundary_values[z_displacement_multiplier] = 0;
+                                    }
+                                    /*Find bottom right corner*/
+                                    if (std::fabs(vert(0) - 1) < 1e-12 && std::fabs(
+                                            vert(1) - 2) < 1e-12 && ((std::fabs(
+                                            vert(2) - 0) < 1e-12) || (std::fabs(
+                                            vert(2) - 1) < 1e-12))) {
+                                        const unsigned int x_displacement =
+                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
+                                        const unsigned int y_displacement =
+                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
+                                        const unsigned int z_displacement =
+                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
+                                        const unsigned int x_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
+                                        const unsigned int y_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 4, cell->active_fe_index());
+                                        const unsigned int z_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 5, cell->active_fe_index());
+                                        boundary_values[x_displacement] = 0;
+                                        boundary_values[y_displacement] = 0;
+                                        boundary_values[z_displacement] = 0;
+                                        boundary_values[x_displacement_multiplier] = 0;
+                                        boundary_values[y_displacement_multiplier] = 0;
+                                        boundary_values[z_displacement_multiplier] = 0;
+                                    }
+                                }
+                            }
+                        }
+                    }
+                }
+            } else {
+                throw;
+            }
+        }
+
+
+    }
+
+
+    //This makes a giant 10-by-10 block matrix, and also sets up the necessary block vectors.  The
+    // sparsity pattern for this matrix includes the sparsity pattern for the filter matrix. It also initializes
+    // any block vectors we will use.
+    template<int dim>
+    void
+    KktSystem<dim>::setup_block_system() {
+
+        //MAKE n_u and n_P
+
+        /*Setup 10 by 10 block matrix*/
+        std::vector<unsigned int> block_component(10, 2);
+
+        block_component[0] = 0;
+        block_component[5] = 1;
+
+        const std::vector<types::global_dof_index> dofs_per_block =
+                DoFTools::count_dofs_per_fe_block(dof_handler, block_component);
+        const unsigned int n_p = dofs_per_block[0];
+        const unsigned int n_u = dofs_per_block[1];
+
+        std::cout << "n_p:  " << n_p << "   n_u:  " << n_u << std::endl;
+
+        IndexSet locally_owned_dofs = dof_handler.locally_owned_dofs();
+        IndexSet locally_relevant_dofs;
+        DoFTools::extract_locally_relevant_dofs(dof_handler, locally_relevant_dofs);
+
+        dsp.reinit(10, 10);
+        owned_partitioning.resize(10);
+        owned_partitioning[0] = dof_handler.locally_owned_dofs().get_view(0, n_p);
+        owned_partitioning[1] = dof_handler.locally_owned_dofs().get_view(n_p, 2 * n_p);
+        owned_partitioning[2] = dof_handler.locally_owned_dofs().get_view(2 * n_p, 3 * n_p);
+        owned_partitioning[3] = dof_handler.locally_owned_dofs().get_view(3 * n_p, 4 * n_p);
+        owned_partitioning[4] = dof_handler.locally_owned_dofs().get_view(4 * n_p, 5 * n_p);
+        owned_partitioning[5] = dof_handler.locally_owned_dofs().get_view(5 * n_p, 5 * n_p + n_u);
+        owned_partitioning[6] = dof_handler.locally_owned_dofs().get_view(5 * n_p + n_u, 5 * n_p + 2 * n_u);
+        owned_partitioning[7] = dof_handler.locally_owned_dofs().get_view(5 * n_p + 2 * n_u, 6 * n_p + 2 * n_u);
+        owned_partitioning[8] = dof_handler.locally_owned_dofs().get_view(6 * n_p + 2 * n_u, 7 * n_p + 2 * n_u);
+        owned_partitioning[9] = dof_handler.locally_owned_dofs().get_view(7 * n_p + 2 * n_u, 7 * n_p + 2 * n_u + 1);
+        relevant_partitioning.resize(10);
+        relevant_partitioning[0] = locally_relevant_dofs.get_view(0, n_p);
+        relevant_partitioning[1] = locally_relevant_dofs.get_view(n_p, 2 * n_p);
+        relevant_partitioning[2] = locally_relevant_dofs.get_view(2 * n_p, 3 * n_p);
+        relevant_partitioning[3] = locally_relevant_dofs.get_view(3 * n_p, 4 * n_p);
+        relevant_partitioning[4] = locally_relevant_dofs.get_view(4 * n_p, 5 * n_p);
+        relevant_partitioning[5] = locally_relevant_dofs.get_view(5 * n_p, 5 * n_p + n_u);
+        relevant_partitioning[6] = locally_relevant_dofs.get_view(5 * n_p + n_u, 5 * n_p + 2 * n_u);
+        relevant_partitioning[7] = locally_relevant_dofs.get_view(5 * n_p + 2 * n_u, 6 * n_p + 2 * n_u);
+        relevant_partitioning[8] = locally_relevant_dofs.get_view(6 * n_p + 2 * n_u, 7 * n_p + 2 * n_u);
+        relevant_partitioning[9] = locally_relevant_dofs.get_view(7 * n_p + 2 * n_u, 7 * n_p + 2 * n_u + 1);
+
+        const std::vector<unsigned int> block_sizes = {n_p, n_p, n_p, n_p, n_p, n_u, n_u, n_p, n_p, 1};
+
+        for (unsigned int k = 0; k < 10; k++) {
+            for (unsigned int j = 0; j < 10; j++) {
+                dsp.block(j, k).reinit(block_sizes[j], block_sizes[k]);
+            }
+        }
+        dsp.collect_sizes();
+        Table<2, DoFTools::Coupling> coupling(2 * dim + 8, 2 * dim + 8);
+//Coupling for density
+            coupling[SolutionComponents::density<dim>][SolutionComponents::density<dim>] = DoFTools::always;
+
+            for (unsigned int i = 0; i < dim; i++) {
+                coupling[SolutionComponents::density<dim>][SolutionComponents::displacement<dim> +
+                                                           i] = DoFTools::always;
+                coupling[SolutionComponents::displacement<dim> +
+                         i][SolutionComponents::density<dim>] = DoFTools::always;
+            }
+
+            coupling[SolutionComponents::density<dim>][SolutionComponents::unfiltered_density_multiplier<dim>] = DoFTools::always;
+            coupling[SolutionComponents::unfiltered_density_multiplier<dim>][SolutionComponents::density<dim>] = DoFTools::always;
+
+            for (unsigned int i = 0; i < dim; i++) {
+                coupling[SolutionComponents::density<dim>][SolutionComponents::displacement_multiplier<dim> +
+                                                           i] = DoFTools::always;
+                coupling[SolutionComponents::displacement_multiplier<dim> +
+                         i][SolutionComponents::density<dim>] = DoFTools::always;
+            }
+
+//Coupling for displacement
+            for (unsigned int i = 0; i < dim; i++) {
+
+                for (unsigned int k = 0; k < dim; k++) {
+                    coupling[SolutionComponents::displacement<dim> + i][
+                            SolutionComponents::displacement_multiplier<dim> +
+                            k] = DoFTools::always;
+                    coupling[SolutionComponents::displacement_multiplier<dim> + k][
+                            SolutionComponents::displacement<dim> +
+                            i] = DoFTools::always;
+                }
+            }
+
+// coupling for unfiltered density
+            coupling[SolutionComponents::unfiltered_density<dim>][SolutionComponents::density_lower_slack_multiplier<dim>] = DoFTools::always;
+            coupling[SolutionComponents::density_lower_slack_multiplier<dim>][SolutionComponents::unfiltered_density<dim>] = DoFTools::always;
+
+            coupling[SolutionComponents::unfiltered_density<dim>][SolutionComponents::density_upper_slack_multiplier<dim>] = DoFTools::always;
+            coupling[SolutionComponents::density_upper_slack_multiplier<dim>][SolutionComponents::unfiltered_density<dim>] = DoFTools::always;
+
+            coupling[SolutionComponents::unfiltered_density<dim>][SolutionComponents::unfiltered_density_multiplier<dim>] = DoFTools::always;
+            coupling[SolutionComponents::unfiltered_density_multiplier<dim>][SolutionComponents::unfiltered_density<dim>] = DoFTools::always;
+
+
+
+
+//        Coupling for lower slack
+            coupling[SolutionComponents::density_lower_slack<dim>][SolutionComponents::density_lower_slack<dim>] = DoFTools::always;
+
+            coupling[SolutionComponents::density_lower_slack<dim>][SolutionComponents::density_lower_slack_multiplier<dim>] = DoFTools::always;
+            coupling[SolutionComponents::density_lower_slack_multiplier<dim>][SolutionComponents::density_lower_slack<dim>] = DoFTools::always;
+
+//
+            coupling[SolutionComponents::density_upper_slack<dim>][SolutionComponents::density_upper_slack<dim>] = DoFTools::always;
+            coupling[SolutionComponents::density_upper_slack<dim>][SolutionComponents::density_upper_slack_multiplier<dim>] = DoFTools::always;
+            coupling[SolutionComponents::density_upper_slack_multiplier<dim>][SolutionComponents::density_upper_slack<dim>] = DoFTools::always;
+
+            coupling[SolutionComponents::density_upper_slack_multiplier<dim>][SolutionComponents::density_upper_slack_multiplier<dim>] = DoFTools::always;
+            constraints.reinit(locally_relevant_dofs);
+            constraints.clear();
+            DoFTools::make_hanging_node_constraints(dof_handler,constraints);
+            constraints.close();
+
+            system_matrix.clear();
+
+//            DoFTools::make_sparsity_pattern(dof_handler, coupling, dsp, constraints, false);
+            DoFTools::make_sparsity_pattern(dof_handler, dsp, constraints, false);
+            SparsityTools::distribute_sparsity_pattern(dsp, dof_handler.locally_owned_dofs(), mpi_communicator,
+                                                       locally_relevant_dofs);
+            //adds the row into the sparsity pattern for the total volume constraint
+            for (const auto &cell: dof_handler.active_cell_iterators())
+            {
+                if (cell->is_locally_owned())
+                {
+                    std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
+                    cell->get_dof_indices(i);
+                    dsp.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier).add(i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)], 0);
+                    dsp.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).add(0, i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)]);
+                }
+
+            }
+
+            /*This finds neighbors whose values would be relevant, and adds them to the sparsity pattern of the matrix*/
+            setup_filter_matrix();
+            for (const auto &cell : dof_handler.active_cell_iterators()) {
+                if (cell->is_locally_owned())
+                {
+                    std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
+                    cell->get_dof_indices(i);
+                    for (const auto &neighbor_cell : density_filter.find_relevant_neighbors(cell)) {
+                        std::vector<types::global_dof_index> j(neighbor_cell->get_fe().n_dofs_per_cell());
+                        neighbor_cell->get_dof_indices(j);
+
+                        dsp.block(SolutionBlocks::unfiltered_density_multiplier,
+                                  SolutionBlocks::unfiltered_density).add(i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)], j[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)]);
+                        dsp.block(SolutionBlocks::unfiltered_density,
+                                  SolutionBlocks::unfiltered_density_multiplier).add(j[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)], i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)]);
+                    }
+                }
+            }
+
+
+            SparsityTools::distribute_sparsity_pattern(
+                    dsp,
+                    Utilities::MPI::all_gather(mpi_communicator,
+                                               dof_handler.locally_owned_dofs()),
+                    mpi_communicator,
+                    locally_relevant_dofs);
+            DoFTools::extract_locally_relevant_dofs(dof_handler, locally_relevant_dofs);
+            system_matrix.reinit(owned_partitioning, dsp, mpi_communicator);
+
+            locally_relevant_solution.reinit(owned_partitioning, relevant_partitioning, mpi_communicator);
+            distributed_solution.reinit(owned_partitioning, mpi_communicator);
+            system_rhs.reinit(owned_partitioning, mpi_communicator);
+
+            locally_relevant_solution.collect_sizes();
+            distributed_solution.collect_sizes();
+            system_rhs.collect_sizes();
+            system_matrix.collect_sizes();
+    }
+
+    ///This  is  where  the  magic  happens.   The  equations  describing  the newtons method for finding 0s in the KKT conditions are implemented here.
+
+
+    template<int dim>
+    void
+    KktSystem<dim>::assemble_block_system(const LA::MPI::BlockVector &distributed_state, const double barrier_size) {
+        /*Remove any values from old iterations*/
+
+        LA::MPI::BlockVector relevant_state(owned_partitioning, relevant_partitioning, mpi_communicator);
+        relevant_state = distributed_state;
+
+        system_matrix.reinit(owned_partitioning, dsp, mpi_communicator);
+        locally_relevant_solution = 0;
+        system_rhs = 0;
+
+        QGauss<dim> nine_quadrature(fe_nine.degree + 1);
+        QGauss<dim> ten_quadrature(fe_ten.degree + 1);
+
+        hp::QCollection<dim> q_collection;
+        q_collection.push_back(nine_quadrature);
+        q_collection.push_back(ten_quadrature);
+
+        hp::FEValues<dim> hp_fe_values(fe_collection,
+                                       q_collection,
+                                       update_values | update_quadrature_points |
+                                       update_JxW_values | update_gradients);
+
+        QGauss<dim - 1> common_face_quadrature(fe_ten.degree + 1);
+
+        FEFaceValues<dim> fe_nine_face_values(fe_nine,
+                                              common_face_quadrature,
+                                              update_JxW_values |
+                                              update_gradients | update_values);
+        FEFaceValues<dim> fe_ten_face_values(fe_ten,
+                                             common_face_quadrature,
+                                             update_normal_vectors |
+                                             update_values);
+
+        FullMatrix<double> cell_matrix;
+        Vector<double> cell_rhs;
+        std::vector<types::global_dof_index> local_dof_indices;
+
+        const FEValuesExtractors::Scalar densities(SolutionComponents::density<dim>);
+        const FEValuesExtractors::Vector displacements(SolutionComponents::displacement<dim>);
+        const FEValuesExtractors::Scalar unfiltered_densities(SolutionComponents::unfiltered_density<dim>);
+        const FEValuesExtractors::Vector displacement_multipliers(SolutionComponents::displacement_multiplier<dim>);
+        const FEValuesExtractors::Scalar unfiltered_density_multipliers(
+                SolutionComponents::unfiltered_density_multiplier<dim>);
+        const FEValuesExtractors::Scalar density_lower_slacks(SolutionComponents::density_lower_slack<dim>);
+        const FEValuesExtractors::Scalar density_lower_slack_multipliers(
+                SolutionComponents::density_lower_slack_multiplier<dim>);
+        const FEValuesExtractors::Scalar density_upper_slacks(SolutionComponents::density_upper_slack<dim>);
+        const FEValuesExtractors::Scalar density_upper_slack_multipliers(
+                SolutionComponents::density_upper_slack_multiplier<dim>);
+        const FEValuesExtractors::Scalar total_volume_multiplier(
+                SolutionComponents::total_volume_multiplier<dim>);
+
+        const Functions::ConstantFunction<dim> lambda(1.), mu(1.);
+
+        distributed_solution = distributed_state;
+        LA::MPI::BlockVector filtered_unfiltered_density_solution = distributed_solution;
+        LA::MPI::BlockVector filter_adjoint_unfiltered_density_multiplier_solution = distributed_solution;
+        filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = 0;
+        filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier) = 0;
+        auto op_f = linear_operator<LA::MPI::Vector>(density_filter.filter_matrix);
+        filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = op_f * distributed_solution.block(SolutionBlocks::unfiltered_density);
+        filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier) = transpose_operator(op_f) *  distributed_solution.block(SolutionBlocks::unfiltered_density_multiplier);
+
+        LA::MPI::BlockVector relevant_filtered_unfiltered_density_solution = locally_relevant_solution;
+        LA::MPI::BlockVector relevant_filter_adjoint_unfiltered_density_multiplier_solution = locally_relevant_solution;
+        relevant_filtered_unfiltered_density_solution =filtered_unfiltered_density_solution;
+        relevant_filter_adjoint_unfiltered_density_multiplier_solution = filter_adjoint_unfiltered_density_multiplier_solution;
+        for (const auto &cell: dof_handler.active_cell_iterators()) {
+            if(cell->is_locally_owned())
+            {
+                hp_fe_values.reinit(cell);
+                const FEValues<dim> &fe_values = hp_fe_values.get_present_fe_values();
+                cell_matrix.reinit(cell->get_fe().n_dofs_per_cell(),
+                                   cell->get_fe().n_dofs_per_cell());
+                cell_rhs.reinit(cell->get_fe().n_dofs_per_cell());
+
+                const unsigned int n_q_points = fe_values.n_quadrature_points;
+
+                std::vector<double> old_density_values(n_q_points);
+                std::vector<Tensor<1, dim>> old_displacement_values(n_q_points);
+                std::vector<double> old_displacement_divs(n_q_points);
+                std::vector<SymmetricTensor<2, dim>> old_displacement_symmgrads(
+                        n_q_points);
+                std::vector<Tensor<1, dim>> old_displacement_multiplier_values(
+                        n_q_points);
+                std::vector<double> old_displacement_multiplier_divs(n_q_points);
+                std::vector<SymmetricTensor<2, dim>> old_displacement_multiplier_symmgrads(
+                        n_q_points);
+                std::vector<double> old_lower_slack_multiplier_values(n_q_points);
+                std::vector<double> old_upper_slack_multiplier_values(n_q_points);
+                std::vector<double> old_lower_slack_values(n_q_points);
+                std::vector<double> old_upper_slack_values(n_q_points);
+                std::vector<double> old_unfiltered_density_values(n_q_points);
+                std::vector<double> old_unfiltered_density_multiplier_values(n_q_points);
+                std::vector<double> filtered_unfiltered_density_values(n_q_points);
+                std::vector<double> filter_adjoint_unfiltered_density_multiplier_values(n_q_points);
+                std::vector<double> lambda_values(n_q_points);
+                std::vector<double> mu_values(n_q_points);
+
+                const unsigned int dofs_per_cell = cell->get_fe().n_dofs_per_cell();
+
+                cell_matrix = 0;
+                cell_rhs = 0;
+                local_dof_indices.resize(cell->get_fe().n_dofs_per_cell());
+                cell->get_dof_indices(local_dof_indices);
+
+                lambda.value_list(fe_values.get_quadrature_points(), lambda_values);
+                mu.value_list(fe_values.get_quadrature_points(), mu_values);
+
+                fe_values[densities].get_function_values(relevant_state,
+                                                         old_density_values);
+                fe_values[displacements].get_function_values(relevant_state,
+                                                             old_displacement_values);
+                fe_values[displacements].get_function_divergences(relevant_state,
+                                                                  old_displacement_divs);
+                fe_values[displacements].get_function_symmetric_gradients(
+                        relevant_state, old_displacement_symmgrads);
+                fe_values[displacement_multipliers].get_function_values(
+                        relevant_state, old_displacement_multiplier_values);
+                fe_values[displacement_multipliers].get_function_divergences(
+                        relevant_state, old_displacement_multiplier_divs);
+                fe_values[displacement_multipliers].get_function_symmetric_gradients(
+                        relevant_state, old_displacement_multiplier_symmgrads);
+                fe_values[density_lower_slacks].get_function_values(
+                        relevant_state, old_lower_slack_values);
+                fe_values[density_lower_slack_multipliers].get_function_values(
+                        relevant_state, old_lower_slack_multiplier_values);
+                fe_values[density_upper_slacks].get_function_values(
+                        relevant_state, old_upper_slack_values);
+                fe_values[density_upper_slack_multipliers].get_function_values(
+                        relevant_state, old_upper_slack_multiplier_values);
+                fe_values[unfiltered_densities].get_function_values(
+                        relevant_state, old_unfiltered_density_values);
+                fe_values[unfiltered_density_multipliers].get_function_values(
+                        relevant_state, old_unfiltered_density_multiplier_values);
+                fe_values[unfiltered_densities].get_function_values(
+                        relevant_filtered_unfiltered_density_solution, filtered_unfiltered_density_values);
+                fe_values[unfiltered_density_multipliers].get_function_values(
+                        relevant_filter_adjoint_unfiltered_density_multiplier_solution,
+                        filter_adjoint_unfiltered_density_multiplier_values);
+
+                Tensor<1, dim> traction;
+                traction[1] = -1;
+
+                for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
+
+                    for (unsigned int i = 0; i < dofs_per_cell; ++i) {
+                        const SymmetricTensor<2, dim> displacement_phi_i_symmgrad =
+                                fe_values[displacements].symmetric_gradient(i, q_point);
+                        const double displacement_phi_i_div =
+                                fe_values[displacements].divergence(i, q_point);
+
+                        const SymmetricTensor<2, dim> displacement_multiplier_phi_i_symmgrad =
+                                fe_values[displacement_multipliers].symmetric_gradient(i,
+                                                                                       q_point);
+                        const double displacement_multiplier_phi_i_div =
+                                fe_values[displacement_multipliers].divergence(i,
+                                                                               q_point);
+
+
+                        const double density_phi_i = fe_values[densities].value(i,
+                                                                                q_point);
+                        const double unfiltered_density_phi_i = fe_values[unfiltered_densities].value(i,
+                                                                                                      q_point);
+                        const double unfiltered_density_multiplier_phi_i = fe_values[unfiltered_density_multipliers].value(
+                                i, q_point);
+
+                        const double lower_slack_multiplier_phi_i =
+                                fe_values[density_lower_slack_multipliers].value(i,
+                                                                                 q_point);
+
+                        const double lower_slack_phi_i =
+                                fe_values[density_lower_slacks].value(i, q_point);
+
+                        const double upper_slack_phi_i =
+                                fe_values[density_upper_slacks].value(i, q_point);
+
+                        const double upper_slack_multiplier_phi_i =
+                                fe_values[density_upper_slack_multipliers].value(i,
+                                                                                 q_point);
+
+
+                        for (unsigned int j = 0; j < dofs_per_cell; ++j) {
+                            const SymmetricTensor<2, dim> displacement_phi_j_symmgrad =
+                                    fe_values[displacements].symmetric_gradient(j,
+                                                                                q_point);
+                            const double displacement_phi_j_div =
+                                    fe_values[displacements].divergence(j, q_point);
+
+                            const SymmetricTensor<2, dim> displacement_multiplier_phi_j_symmgrad =
+                                    fe_values[displacement_multipliers].symmetric_gradient(
+                                            j, q_point);
+                            const double displacement_multiplier_phi_j_div =
+                                    fe_values[displacement_multipliers].divergence(j,
+                                                                                   q_point);
+
+                            const double density_phi_j = fe_values[densities].value(
+                                    j, q_point);
+
+                            const double unfiltered_density_phi_j = fe_values[unfiltered_densities].value(j,
+                                                                                                          q_point);
+                            const double unfiltered_density_multiplier_phi_j = fe_values[unfiltered_density_multipliers].value(
+                                    j, q_point);
+
+
+                            const double lower_slack_phi_j =
+                                    fe_values[density_lower_slacks].value(j, q_point);
+
+                            const double upper_slack_phi_j =
+                                    fe_values[density_upper_slacks].value(j, q_point);
+
+                            const double lower_slack_multiplier_phi_j =
+                                    fe_values[density_lower_slack_multipliers].value(j,
+                                                                                     q_point);
+
+                            const double upper_slack_multiplier_phi_j =
+                                    fe_values[density_upper_slack_multipliers].value(j,
+                                                                                     q_point);
+
+                            //Equation 0
+                            cell_matrix(i, j) +=
+                                    fe_values.JxW(q_point) *
+                                    (
+                                            -density_phi_i * unfiltered_density_multiplier_phi_j
+
+                                            - density_penalty_exponent * (density_penalty_exponent - 1)
+                                              * std::pow(
+                                                    old_density_values[q_point],
+                                                    density_penalty_exponent - 2)
+                                              * density_phi_i
+                                              * density_phi_j
+                                              * (old_displacement_multiplier_divs[q_point] * old_displacement_divs[q_point]
+                                                 * lambda_values[q_point]
+                                                 + 2 * mu_values[q_point]
+                                                   * (old_displacement_symmgrads[q_point] *
+                                                      old_displacement_multiplier_symmgrads[q_point]))
+
+                                            - density_penalty_exponent * std::pow(
+                                                    old_density_values[q_point],
+                                                    density_penalty_exponent - 1)
+                                              * density_phi_i
+                                              * (displacement_multiplier_phi_j_div * old_displacement_divs[q_point]
+                                                 * lambda_values[q_point]
+                                                 + 2 * mu_values[q_point]
+                                                   *
+                                                   (old_displacement_symmgrads[q_point] *
+                                                    displacement_multiplier_phi_j_symmgrad))
+
+                                            - density_penalty_exponent * std::pow(
+                                                    old_density_values[q_point],
+                                                    density_penalty_exponent - 1)
+                                              * density_phi_i
+                                              * (displacement_phi_j_div * old_displacement_multiplier_divs[q_point]
+                                                 * lambda_values[q_point]
+                                                 + 2 * mu_values[q_point]
+                                                   * (old_displacement_multiplier_symmgrads[q_point] *
+                                                      displacement_phi_j_symmgrad)));
+                            //Equation 1
+
+                            cell_matrix(i, j) +=
+                                    fe_values.JxW(q_point) * (
+                                            -density_penalty_exponent * std::pow(
+                                                    old_density_values[q_point],
+                                                    density_penalty_exponent - 1)
+                                            * density_phi_j
+                                            * (old_displacement_multiplier_divs[q_point] * displacement_phi_i_div
+                                               * lambda_values[q_point]
+                                               + 2 * mu_values[q_point]
+                                                 * (old_displacement_multiplier_symmgrads[q_point] *
+                                                    displacement_phi_i_symmgrad))
+
+                                            - std::pow(old_density_values[q_point],
+                                                       density_penalty_exponent)
+                                              * (displacement_multiplier_phi_j_div * displacement_phi_i_div
+                                                 * lambda_values[q_point]
+                                                 + 2 * mu_values[q_point]
+                                                   * (displacement_multiplier_phi_j_symmgrad * displacement_phi_i_symmgrad))
+
+                                    );
+
+                            //Equation 2 has to do with the filter, which is calculated elsewhere.
+                            cell_matrix(i, j) +=
+                                    fe_values.JxW(q_point) * (
+                                            -1 * unfiltered_density_phi_i * lower_slack_multiplier_phi_j
+                                            + unfiltered_density_phi_i * upper_slack_multiplier_phi_j);
+
+                            //Equation 3 - Primal Feasibility
+
+                            cell_matrix(i, j) +=
+                                    fe_values.JxW(q_point) * (
+
+                                            -1 * density_penalty_exponent * std::pow(
+                                                    old_density_values[q_point],
+                                                    density_penalty_exponent - 1)
+                                            * density_phi_j
+                                            * (old_displacement_divs[q_point] * displacement_multiplier_phi_i_div
+                                               * lambda_values[q_point]
+                                               + 2 * mu_values[q_point]
+                                                 * (old_displacement_symmgrads[q_point] *
+                                                    displacement_multiplier_phi_i_symmgrad))
+
+                                            + -1 * std::pow(old_density_values[q_point],
+                                                            density_penalty_exponent)
+                                              * (displacement_phi_j_div * displacement_multiplier_phi_i_div
+                                                 * lambda_values[q_point]
+                                                 + 2 * mu_values[q_point]
+                                                   *
+                                                   (displacement_phi_j_symmgrad * displacement_multiplier_phi_i_symmgrad)));
+
+                            //Equation 4 - more primal feasibility
+                            cell_matrix(i, j) +=
+                                    -1 * fe_values.JxW(q_point) * lower_slack_multiplier_phi_i *
+                                    (unfiltered_density_phi_j - lower_slack_phi_j);
+
+                            //Equation 5 - more primal feasibility
+                            cell_matrix(i, j) +=
+                                    -1 * fe_values.JxW(q_point) * upper_slack_multiplier_phi_i * (
+                                            -1 * unfiltered_density_phi_j - upper_slack_phi_j);
+
+                            //Equation 6 - more primal feasibility - part with filter added later
+                            cell_matrix(i, j) +=
+                                    -1 * fe_values.JxW(q_point) * unfiltered_density_multiplier_phi_i * (
+                                            density_phi_j);
+
+                            //Equation 7 - complementary slackness
+                            cell_matrix(i, j) += fe_values.JxW(q_point) *
+                                                 (lower_slack_phi_i * lower_slack_multiplier_phi_j
+                                                  + lower_slack_phi_i * lower_slack_phi_j *
+                                                    old_lower_slack_multiplier_values[q_point] /
+                                                    old_lower_slack_values[q_point]);
+                            //Equation 8 - complementary slackness
+                            cell_matrix(i, j) += fe_values.JxW(q_point) *
+                                                 (upper_slack_phi_i * upper_slack_multiplier_phi_j
+                                                  + upper_slack_phi_i * upper_slack_phi_j *
+                                                    old_upper_slack_multiplier_values[q_point] /
+                                                    old_upper_slack_values[q_point]);
+                        }
+
+                    }
+                }
+
+
+                MatrixTools::local_apply_boundary_values(boundary_values, local_dof_indices,
+                                                         cell_matrix, cell_rhs, true);
+
+
+                constraints.distribute_local_to_global(
+                        cell_matrix, cell_rhs, local_dof_indices, system_matrix, system_rhs);
+
+            }
+
+        }
+        std::cout << "compress 1" << std::endl;
+        system_matrix.compress(VectorOperation::add);
+        system_rhs = calculate_rhs(distributed_state, barrier_size);
+
+        for (const auto &cell: dof_handler.active_cell_iterators()) {
+            if(cell->is_locally_owned())
+            {
+                std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
+                cell->get_dof_indices(i);
+
+                const types::global_dof_index dof_index_lower_slack =
+                        i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)];
+                typename LA::MPI::SparseMatrix::iterator iter = density_filter.filter_matrix.begin(
+                        i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)]);
+                for (; iter != density_filter.filter_matrix.end(i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)]); iter++) {
+                    unsigned int j = iter->column();
+                    double value = iter->value() * cell->measure();
+
+                    system_matrix.block(SolutionBlocks::unfiltered_density_multiplier,
+                                        SolutionBlocks::unfiltered_density).add(i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)], j, value);
+                    system_matrix.block(SolutionBlocks::unfiltered_density,
+                                        SolutionBlocks::unfiltered_density_multiplier).add(j, i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)], value);
+                }
+
+                system_matrix.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).add(0, i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)],
+                                                                                                          cell->measure());
+
+                system_matrix.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier).add(i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)], 0,
+                                                                                                          cell->measure());
+            }
+        }
+        std::cout << "compress 2" << std::endl;
+        system_matrix.compress(VectorOperation::add);
+
+
+        std::cout << "assembled" << std::endl;
+
+    }
+
+    template<int dim>
+    double
+    KktSystem<dim>::calculate_objective_value(const LA::MPI::BlockVector &distributed_state) const {
+        /*Remove any values from old iterations*/
+
+        locally_relevant_solution = distributed_state;
+
+
+        QGauss<dim> nine_quadrature(fe_nine.degree + 1);
+        QGauss<dim> ten_quadrature(fe_ten.degree + 1);
+
+        hp::QCollection<dim> q_collection;
+        q_collection.push_back(nine_quadrature);
+        q_collection.push_back(ten_quadrature);
+
+        hp::FEValues<dim> hp_fe_values(fe_collection,
+                                       q_collection,
+                                       update_values | update_quadrature_points |
+                                       update_JxW_values | update_gradients);
+
+        QGauss<dim - 1> common_face_quadrature(fe_ten.degree + 1);
+
+        FEFaceValues<dim> fe_nine_face_values(fe_nine,
+                                              common_face_quadrature,
+                                              update_JxW_values |
+                                              update_gradients | update_values);
+        FEFaceValues<dim> fe_ten_face_values(fe_ten,
+                                             common_face_quadrature,
+                                             update_normal_vectors |
+                                             update_values);
+
+        FullMatrix<double> cell_matrix;
+        Vector<double> cell_rhs;
+
+        const FEValuesExtractors::Vector displacements(SolutionComponents::displacement<dim>);
+
+        Tensor<1, dim> traction;
+        traction[1] = -1;
+        distributed_solution = distributed_state;
+        double objective_value = 0;
+        for (const auto &cell: dof_handler.active_cell_iterators())
+        {
+            if(cell->is_locally_owned())
+            {
+                hp_fe_values.reinit(cell);
+                const FEValues<dim> &fe_values = hp_fe_values.get_present_fe_values();
+                const unsigned int dofs_per_cell = cell->get_fe().n_dofs_per_cell();
+                const unsigned int n_q_points = fe_values.n_quadrature_points;
+                const unsigned int n_face_q_points = common_face_quadrature.size();
+
+                std::vector<Tensor<1, dim>> old_displacement_values(n_q_points);
+                fe_values[displacements].get_function_values(
+                        locally_relevant_solution, old_displacement_values);
+
+                for (unsigned int face_number = 0;
+                     face_number < GeometryInfo<dim>::faces_per_cell;
+                     ++face_number)
+                {
+                    if (cell->face(face_number)->at_boundary() && cell->face(face_number)->boundary_id()
+                                                                  == BoundaryIds::down_force)
+                    {
+                        for (unsigned int face_q_point = 0;
+                             face_q_point < n_face_q_points; ++face_q_point) {
+                            for (unsigned int i = 0; i < dofs_per_cell; ++i) {
+                                if (cell->material_id() == MaterialIds::without_multiplier) {
+                                    fe_nine_face_values.reinit(cell, face_number);
+                                    objective_value += traction
+                                                       * fe_nine_face_values[displacements].value(i,
+                                                                                                  face_q_point)
+                                                       * fe_nine_face_values.JxW(face_q_point);
+                                } else {
+                                    fe_ten_face_values.reinit(cell, face_number);
+                                    objective_value += traction
+                                                       * fe_ten_face_values[displacements].value(i,
+                                                                                                 face_q_point)
+                                                       * fe_ten_face_values.JxW(face_q_point);
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+        }
+        double objective_value_out;
+        MPI_Allreduce(&objective_value, &objective_value_out, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+
+        std::cout << "objective value: " << objective_value_out << std::endl;
+        return objective_value;
+    }
+
+
+    //As the KKT System know which vectors correspond to the slack variables, the sum of the logs of the slacks is computed here for use in the filter.
+    template<int dim>
+    double
+    KktSystem<dim>::calculate_barrier_distance(const LA::MPI::BlockVector &state) const {
+        double barrier_distance_log_sum = 0;
+        unsigned int vect_size = state.block(SolutionBlocks::density_lower_slack).size();
+        distributed_solution = state;
+        for (unsigned int k = 0; k < vect_size; k++) {
+            if (distributed_solution.block(SolutionBlocks::density_lower_slack).in_local_range(k))
+            barrier_distance_log_sum += std::log(state.block(SolutionBlocks::density_lower_slack)[k]);
+        }
+        for (unsigned int k = 0; k < vect_size; k++) {
+            if (distributed_solution.block(SolutionBlocks::density_upper_slack).in_local_range(k))
+            barrier_distance_log_sum += std::log(state.block(SolutionBlocks::density_upper_slack)[k]);
+        }
+        double out_barrier_distance_log_sum;
+        MPI_Allreduce(&barrier_distance_log_sum, &out_barrier_distance_log_sum, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+
+        std::cout << "Barrier distance log sum: " << out_barrier_distance_log_sum << std::endl;
+
+        return out_barrier_distance_log_sum;
+    }
+
+    template<int dim>
+    double
+    KktSystem<dim>::calculate_rhs_norm(const LA::MPI::BlockVector &state, const double barrier_size) const {
+        return calculate_rhs(state, barrier_size).l2_norm();
+    }
+
+
+    //Feasibility conditions appear on the RHS of the linear system, so I compute the RHS to find it. Could probably be combined with the objective value finding part to make it faster.
+    template<int dim>
+    double
+    KktSystem<dim>::calculate_feasibility(const LA::MPI::BlockVector &state, const double barrier_size) const {
+       LA::MPI::BlockVector test_rhs = calculate_rhs(state, barrier_size);
+
+       double norm = 0;
+
+       distributed_solution = state;
+       double full_prod1 =1;
+       double full_prod2 = 1;
+
+       for (unsigned int k = 0; k < state.block(SolutionBlocks::density_upper_slack).size(); k++) {
+           double prod1 = 1;
+           double prod2 = 1;
+           if(state.block(SolutionBlocks::density_upper_slack).in_local_range(k))
+           {
+               prod1 = prod1 * state.block(SolutionBlocks::density_upper_slack)[k]
+                       * state.block(SolutionBlocks::density_upper_slack)[k];
+           }
+           if(state.block(SolutionBlocks::density_lower_slack).in_local_range(k))
+           {
+              prod2 = prod2 *  state.block(SolutionBlocks::density_lower_slack)[k]
+                      * state.block(SolutionBlocks::density_lower_slack)[k];
+           }
+           if(state.block(SolutionBlocks::density_upper_slack_multiplier).in_local_range(k))
+           {
+               prod1 = prod1 * state.block(SolutionBlocks::density_upper_slack_multiplier)[k]
+                       * state.block(SolutionBlocks::density_upper_slack_multiplier)[k];
+           }
+           if(state.block(SolutionBlocks::density_lower_slack_multiplier).in_local_range(k))
+           {
+               prod2 = prod2 *  state.block(SolutionBlocks::density_lower_slack_multiplier)[k]
+                       * state.block(SolutionBlocks::density_lower_slack_multiplier)[k];
+           }
+           MPI_Allreduce(&prod1, &full_prod1, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
+           MPI_Allreduce(&prod2, &full_prod2, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
+           norm = norm + full_prod1 + full_prod2;
+       }
+       std::cout << "pre-norm: " << norm << std::endl;
+
+        norm += std::pow(test_rhs.block(SolutionBlocks::displacement).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::density).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::unfiltered_density).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::displacement_multiplier).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::total_volume_multiplier).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::density_upper_slack_multiplier).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::density_lower_slack_multiplier).l2_norm(), 2);
+
+        std::cout << "norm: " << norm << std::endl;
+
+        return norm;
+    }
+
+    template<int dim>
+    double
+    KktSystem<dim>::calculate_convergence(const LA::MPI::BlockVector &state) const {
+        LA::MPI::BlockVector test_rhs = calculate_rhs(state, Input::min_barrier_size);
+        double norm = 0;
+
+        distributed_solution = state;
+        double full_prod1 =1;
+        double full_prod2 = 1;
+
+        for (unsigned int k = 0; k < state.block(SolutionBlocks::density_upper_slack).size(); k++) {
+            double prod1 = 1;
+            double prod2 = 1;
+            if(state.block(SolutionBlocks::density_upper_slack).in_local_range(k))
+            {
+                prod1 = prod1 * state.block(SolutionBlocks::density_upper_slack)[k]
+                        * state.block(SolutionBlocks::density_upper_slack)[k];
+            }
+            if(state.block(SolutionBlocks::density_lower_slack).in_local_range(k))
+            {
+               prod2 = prod2 *  state.block(SolutionBlocks::density_lower_slack)[k]
+                       * state.block(SolutionBlocks::density_lower_slack)[k];
+            }
+            if(state.block(SolutionBlocks::density_upper_slack_multiplier).in_local_range(k))
+            {
+                prod1 = prod1 * state.block(SolutionBlocks::density_upper_slack_multiplier)[k]
+                        * state.block(SolutionBlocks::density_upper_slack_multiplier)[k];
+            }
+            if(state.block(SolutionBlocks::density_lower_slack_multiplier).in_local_range(k))
+            {
+                prod2 = prod2 *  state.block(SolutionBlocks::density_lower_slack_multiplier)[k]
+                        * state.block(SolutionBlocks::density_lower_slack_multiplier)[k];
+            }
+            MPI_Allreduce(&prod1, &full_prod1, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
+            MPI_Allreduce(&prod2, &full_prod2, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
+            norm = norm + full_prod1 + full_prod2;
+        }
+        std::cout << "pre-norm: " << norm << std::endl;
+
+        norm += std::pow(test_rhs.block(SolutionBlocks::displacement).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::density).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::unfiltered_density).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::displacement_multiplier).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::total_volume_multiplier).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::density_upper_slack_multiplier).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::density_lower_slack_multiplier).l2_norm(), 2);
+
+        norm = std::pow(norm, .5);
+
+        std::cout << "l2 norm: " << system_rhs.l2_norm() << std::endl;
+        std::cout << "KKT norm: " << norm << std::endl;
+        return norm;
+    }
+
+    template<int dim>
+    LA::MPI::BlockVector
+    KktSystem<dim>::calculate_rhs(const LA::MPI::BlockVector &distributed_state, const double barrier_size) const {
+        LA::MPI::BlockVector test_rhs (system_rhs);
+        LA::MPI::BlockVector state (locally_relevant_solution);
+        state = distributed_state;
+        test_rhs = 0;
+
+
+        QGauss<dim> nine_quadrature(fe_nine.degree + 1);
+        QGauss<dim> ten_quadrature(fe_ten.degree + 1);
+
+        hp::QCollection<dim> q_collection;
+        q_collection.push_back(nine_quadrature);
+        q_collection.push_back(ten_quadrature);
+
+        hp::FEValues<dim> hp_fe_values(fe_collection,
+                                       q_collection,
+                                       update_values | update_quadrature_points |
+                                       update_JxW_values | update_gradients);
+
+        QGauss<dim - 1> common_face_quadrature(fe_ten.degree + 1);
+
+        FEFaceValues<dim> fe_nine_face_values(fe_nine,
+                                              common_face_quadrature,
+                                              update_JxW_values |
+                                              update_gradients | update_values);
+        FEFaceValues<dim> fe_ten_face_values(fe_ten,
+                                             common_face_quadrature,
+                                             update_normal_vectors |
+                                             update_values);
+
+        FullMatrix<double> cell_matrix;
+        Vector<double> cell_rhs;
+        std::vector<types::global_dof_index> local_dof_indices;
+
+        const FEValuesExtractors::Scalar densities(SolutionComponents::density<dim>);
+        const FEValuesExtractors::Vector displacements(SolutionComponents::displacement<dim>);
+        const FEValuesExtractors::Scalar unfiltered_densities(SolutionComponents::unfiltered_density<dim>);
+        const FEValuesExtractors::Vector displacement_multipliers(SolutionComponents::displacement_multiplier<dim>);
+        const FEValuesExtractors::Scalar unfiltered_density_multipliers(
+                SolutionComponents::unfiltered_density_multiplier<dim>);
+        const FEValuesExtractors::Scalar density_lower_slacks(SolutionComponents::density_lower_slack<dim>);
+        const FEValuesExtractors::Scalar density_lower_slack_multipliers(
+                SolutionComponents::density_lower_slack_multiplier<dim>);
+        const FEValuesExtractors::Scalar density_upper_slacks(SolutionComponents::density_upper_slack<dim>);
+        const FEValuesExtractors::Scalar density_upper_slack_multipliers(
+                SolutionComponents::density_upper_slack_multiplier<dim>);
+        const FEValuesExtractors::Scalar total_volume_multiplier(
+                SolutionComponents::total_volume_multiplier<dim>);
+
+
+        const unsigned int n_face_q_points = common_face_quadrature.size();
+
+        const Functions::ConstantFunction<dim> lambda(1.), mu(1.);
+
+        locally_relevant_solution = state;
+        distributed_solution = state;
+        LA::MPI::BlockVector filtered_unfiltered_density_solution (distributed_solution);
+        LA::MPI::BlockVector filter_adjoint_unfiltered_density_multiplier_solution (distributed_solution);
+        filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = 0;
+        filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier) = 0;
+
+        auto op_f = linear_operator<LA::MPI::Vector>(density_filter.filter_matrix);
+        filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = op_f * distributed_solution.block(SolutionBlocks::unfiltered_density);
+        filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier)
+                                                    = transpose_operator(op_f) * distributed_solution.block(SolutionBlocks::unfiltered_density_multiplier);
+        LA::MPI::BlockVector relevant_filtered_unfiltered_density_solution (locally_relevant_solution);
+        LA::MPI::BlockVector relevant_filter_adjoint_unfiltered_density_multiplier_solution (locally_relevant_solution);
+        relevant_filtered_unfiltered_density_solution = filtered_unfiltered_density_solution;
+        relevant_filter_adjoint_unfiltered_density_multiplier_solution = filter_adjoint_unfiltered_density_multiplier_solution;
+
+        double old_volume_multiplier_temp = 0;
+        double old_volume_multiplier;
+        if(distributed_state.block(SolutionBlocks::total_volume_multiplier).in_local_range(0))
+        {
+            old_volume_multiplier_temp = state.block(SolutionBlocks::total_volume_multiplier)[0];
+        }
+        MPI_Allreduce(&old_volume_multiplier_temp, &old_volume_multiplier, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+
+        for (const auto &cell: dof_handler.active_cell_iterators()) {
+            if(cell->is_locally_owned())
+            {
+                hp_fe_values.reinit(cell);
+                const FEValues<dim> &fe_values = hp_fe_values.get_present_fe_values();
+                cell_matrix.reinit(cell->get_fe().n_dofs_per_cell(),
+                                   cell->get_fe().n_dofs_per_cell());
+                cell_rhs.reinit(cell->get_fe().n_dofs_per_cell());
+
+                const unsigned int n_q_points = fe_values.n_quadrature_points;
+
+                std::vector<double> old_density_values(n_q_points);
+                std::vector<Tensor<1, dim>> old_displacement_values(n_q_points);
+                std::vector<double> old_displacement_divs(n_q_points);
+                std::vector<SymmetricTensor<2, dim>> old_displacement_symmgrads(
+                        n_q_points);
+                std::vector<Tensor<1, dim>> old_displacement_multiplier_values(
+                        n_q_points);
+                std::vector<double> old_displacement_multiplier_divs(n_q_points);
+                std::vector<SymmetricTensor<2, dim>> old_displacement_multiplier_symmgrads(
+                        n_q_points);
+                std::vector<double> old_lower_slack_multiplier_values(n_q_points);
+                std::vector<double> old_upper_slack_multiplier_values(n_q_points);
+                std::vector<double> old_lower_slack_values(n_q_points);
+                std::vector<double> old_upper_slack_values(n_q_points);
+                std::vector<double> old_unfiltered_density_values(n_q_points);
+                std::vector<double> old_unfiltered_density_multiplier_values(n_q_points);
+                std::vector<double> filtered_unfiltered_density_values(n_q_points);
+                std::vector<double> filter_adjoint_unfiltered_density_multiplier_values(n_q_points);
+                std::vector<double> lambda_values(n_q_points);
+                std::vector<double> mu_values(n_q_points);
+
+                const unsigned int dofs_per_cell = cell->get_fe().n_dofs_per_cell();
+
+                cell_matrix = 0;
+                cell_rhs = 0;
+                local_dof_indices.resize(cell->get_fe().n_dofs_per_cell());
+
+                cell->get_dof_indices(local_dof_indices);
+
+                lambda.value_list(fe_values.get_quadrature_points(), lambda_values);
+                mu.value_list(fe_values.get_quadrature_points(), mu_values);
+
+                fe_values[densities].get_function_values(state,
+                                                         old_density_values);
+                fe_values[displacements].get_function_values(state,
+                                                             old_displacement_values);
+                fe_values[displacements].get_function_divergences(state,
+                                                                  old_displacement_divs);
+                fe_values[displacements].get_function_symmetric_gradients(
+                        state, old_displacement_symmgrads);
+                fe_values[displacement_multipliers].get_function_values(
+                        state, old_displacement_multiplier_values);
+                fe_values[displacement_multipliers].get_function_divergences(
+                        state, old_displacement_multiplier_divs);
+                fe_values[displacement_multipliers].get_function_symmetric_gradients(
+                        state, old_displacement_multiplier_symmgrads);
+                fe_values[density_lower_slacks].get_function_values(
+                        state, old_lower_slack_values);
+                fe_values[density_lower_slack_multipliers].get_function_values(
+                        state, old_lower_slack_multiplier_values);
+                fe_values[density_upper_slacks].get_function_values(
+                        state, old_upper_slack_values);
+                fe_values[density_upper_slack_multipliers].get_function_values(
+                        state, old_upper_slack_multiplier_values);
+                fe_values[unfiltered_densities].get_function_values(
+                        state, old_unfiltered_density_values);
+                fe_values[unfiltered_density_multipliers].get_function_values(
+                        state, old_unfiltered_density_multiplier_values);
+                fe_values[unfiltered_densities].get_function_values(
+                        relevant_filtered_unfiltered_density_solution, filtered_unfiltered_density_values);
+                fe_values[unfiltered_density_multipliers].get_function_values(
+                        relevant_filter_adjoint_unfiltered_density_multiplier_solution,
+                        filter_adjoint_unfiltered_density_multiplier_values);
+
+
+                Tensor<1, dim> traction;
+                traction[1] = -1;
+
+                for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
+
+                    for (unsigned int i = 0; i < dofs_per_cell; ++i) {
+                        const SymmetricTensor<2, dim> displacement_phi_i_symmgrad =
+                                fe_values[displacements].symmetric_gradient(i, q_point);
+                        const double displacement_phi_i_div =
+                                fe_values[displacements].divergence(i, q_point);
+
+                        const SymmetricTensor<2, dim> displacement_multiplier_phi_i_symmgrad =
+                                fe_values[displacement_multipliers].symmetric_gradient(i,
+                                                                                       q_point);
+                        const double displacement_multiplier_phi_i_div =
+                                fe_values[displacement_multipliers].divergence(i,
+                                                                               q_point);
+
+
+                        const double density_phi_i = fe_values[densities].value(i,
+                                                                                q_point);
+                        const double unfiltered_density_phi_i = fe_values[unfiltered_densities].value(i,
+                                                                                                      q_point);
+                        const double unfiltered_density_multiplier_phi_i = fe_values[unfiltered_density_multipliers].value(
+                                i, q_point);
+
+                        const double lower_slack_multiplier_phi_i =
+                                fe_values[density_lower_slack_multipliers].value(i,
+                                                                                 q_point);
+
+                        const double lower_slack_phi_i =
+                                fe_values[density_lower_slacks].value(i, q_point);
+
+                        const double upper_slack_phi_i =
+                                fe_values[density_upper_slacks].value(i, q_point);
+
+                        const double upper_slack_multiplier_phi_i =
+                                fe_values[density_upper_slack_multipliers].value(i,
+                                                                                 q_point);
+
+
+                        //rhs eqn 0
+                        cell_rhs(i) +=
+                                -1 * fe_values.JxW(q_point) * (
+                                        -1 * density_penalty_exponent *
+                                        std::pow(old_density_values[q_point], density_penalty_exponent - 1) * density_phi_i
+                                        * (old_displacement_multiplier_divs[q_point] * old_displacement_divs[q_point]
+                                           * lambda_values[q_point]
+                                           + 2 * mu_values[q_point] * (old_displacement_symmgrads[q_point]
+                                                                       * old_displacement_multiplier_symmgrads[q_point]))
+                                        - density_phi_i * old_unfiltered_density_multiplier_values[q_point]
+                                        + old_volume_multiplier * density_phi_i
+                                );
+
+                        //rhs eqn 1 - boundary terms counted later
+                        cell_rhs(i) +=
+                                -1 * fe_values.JxW(q_point) * (
+                                        -1 * std::pow(old_density_values[q_point], density_penalty_exponent)
+                                        * (old_displacement_multiplier_divs[q_point] * displacement_phi_i_div
+                                           * lambda_values[q_point]
+                                           + 2 * mu_values[q_point] * (old_displacement_multiplier_symmgrads[q_point]
+                                                                       * displacement_phi_i_symmgrad))
+                                );
+
+                        //rhs eqn 2
+                        cell_rhs(i) +=
+                                -1 * fe_values.JxW(q_point) * (
+                                        unfiltered_density_phi_i *
+                                        filter_adjoint_unfiltered_density_multiplier_values[q_point]
+                                        + unfiltered_density_phi_i * old_upper_slack_multiplier_values[q_point]
+                                        + -1 * unfiltered_density_phi_i * old_lower_slack_multiplier_values[q_point]
+                                );
+
+
+
+
+                        //rhs eqn 3 - boundary terms counted later
+                        cell_rhs(i) +=
+                                -1 * fe_values.JxW(q_point) * (
+                                        -1 * std::pow(old_density_values[q_point], density_penalty_exponent)
+                                        * (old_displacement_divs[q_point] * displacement_multiplier_phi_i_div
+                                           * lambda_values[q_point]
+                                           + 2 * mu_values[q_point] * (displacement_multiplier_phi_i_symmgrad
+                                                                       * old_displacement_symmgrads[q_point]))
+                                );
+
+                        //rhs eqn 4
+                        cell_rhs(i) +=
+                                -1 * fe_values.JxW(q_point) *
+                                (-1 * lower_slack_multiplier_phi_i
+                                 * (old_unfiltered_density_values[q_point] - old_lower_slack_values[q_point])
+                                );
+
+                        //rhs eqn 5
+                        cell_rhs(i) +=
+                                -1 * fe_values.JxW(q_point) * (
+                                        -1 * upper_slack_multiplier_phi_i
+                                        * (1 - old_unfiltered_density_values[q_point]
+                                           - old_upper_slack_values[q_point]));
+
+                        //rhs eqn 6
+                        cell_rhs(i) +=
+                                -1 * fe_values.JxW(q_point) * (
+                                        -1 * unfiltered_density_multiplier_phi_i
+                                        * (old_density_values[q_point] - filtered_unfiltered_density_values[q_point])
+                                );
+
+                        //rhs eqn 7
+                        cell_rhs(i) +=
+                                -1 * fe_values.JxW(q_point) *
+                                (lower_slack_phi_i *
+                                 (old_lower_slack_multiplier_values[q_point] -
+                                  barrier_size / old_lower_slack_values[q_point]));
+
+                        //rhs eqn 8
+                        cell_rhs(i) +=
+                                -1 * fe_values.JxW(q_point) *
+                                (upper_slack_phi_i *
+                                 (old_upper_slack_multiplier_values[q_point] -
+                                  barrier_size / old_upper_slack_values[q_point]));
+
+                    }
+
+                }
+
+                for (unsigned int face_number = 0;
+                     face_number < GeometryInfo<dim>::faces_per_cell;
+                     ++face_number) {
+                    if (cell->face(face_number)->at_boundary() && cell->face(
+                            face_number)->boundary_id() == BoundaryIds::down_force) {
+                        for (unsigned int face_q_point = 0;
+                             face_q_point < n_face_q_points; ++face_q_point) {
+                            for (unsigned int i = 0; i < dofs_per_cell; ++i) {
+                                if (cell->material_id() == MaterialIds::without_multiplier) {
+                                    fe_nine_face_values.reinit(cell, face_number);
+                                    cell_rhs(i) += -1
+                                                   * traction
+                                                   * fe_nine_face_values[displacements].value(i,
+                                                                                              face_q_point)
+                                                   * fe_nine_face_values.JxW(face_q_point);
+
+                                    cell_rhs(i) += -1 * traction
+                                                   * fe_nine_face_values[displacement_multipliers].value(
+                                            i, face_q_point)
+                                                   * fe_nine_face_values.JxW(face_q_point);
+                                } else {
+                                    fe_ten_face_values.reinit(cell, face_number);
+                                    cell_rhs(i) += -1
+                                                   * traction
+                                                   * fe_ten_face_values[displacements].value(i,
+                                                                                             face_q_point)
+                                                   * fe_ten_face_values.JxW(face_q_point);
+
+                                    cell_rhs(i) += -1 * traction
+                                                   * fe_ten_face_values[displacement_multipliers].value(
+                                            i, face_q_point)
+                                                   * fe_ten_face_values.JxW(face_q_point);
+                                }
+                            }
+                        }
+                    }
+                }
+
+
+                MatrixTools::local_apply_boundary_values(boundary_values, local_dof_indices,
+                                                         cell_matrix, cell_rhs, true);
+                constraints.distribute_local_to_global(
+                        cell_rhs, local_dof_indices, test_rhs);
+            }
+        }
+
+        test_rhs.compress(VectorOperation::add);
+        double total_volume_temp = 0;
+        double goal_volume_temp = 0;
+        double total_volume, goal_volume;
+
+        distributed_solution = state;
+        for (const auto &cell: dof_handler.active_cell_iterators()) {
+            if(cell->is_locally_owned())
+            {
+                if (distributed_solution.block(SolutionBlocks::density).in_local_range(cell->active_cell_index()))
+                {
+                    total_volume_temp += cell->measure() * state.block(SolutionBlocks::density)[cell->active_cell_index()];
+                    goal_volume_temp += cell->measure() * Input::volume_percentage;
+                }
+            }
+        }
+
+        MPI_Allreduce(&total_volume_temp, &total_volume, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+        MPI_Allreduce(&goal_volume_temp, &goal_volume, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+
+        if (test_rhs.block(SolutionBlocks::total_volume_multiplier).in_local_range(0))
+        {
+            test_rhs.block(SolutionBlocks::total_volume_multiplier)[0] = goal_volume - total_volume;
+        }
+        test_rhs.compress(VectorOperation::insert);
+
+        return test_rhs;
+
+    }
+
+
+    template<int dim>
+    LA::MPI::BlockVector
+    KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
+        double gmres_tolerance;
+        if (Input::use_eisenstat_walker) {
+            gmres_tolerance = std::max(
+                    std::min(
+                            .1 * system_rhs.l2_norm() / (initial_rhs_error),
+                            .001
+                    ),
+                    Input::default_gmres_tolerance);
+        }
+        else {
+            gmres_tolerance = Input::default_gmres_tolerance;
+        }
+        locally_relevant_solution=state;
+        distributed_solution = state;
+        SolverControl solver_control(10000, gmres_tolerance * system_rhs.l2_norm());
+        TopOptSchurPreconditioner<dim> preconditioner(system_matrix);
+
+//        std::cout << " rhs " << std::endl;
+//        system_rhs.print(std::cout);
+//        std::cout << std::endl;
+
+//        preconditioner.initialize(system_matrix, boundary_values, dof_handler, locally_relevant_solution, distributed_solution);
+//        preconditioner.vmult(distributed_solution,system_rhs);
+
+//        std::cout << "solution" <<  std::endl;
+//        distributed_solution.print(std::cout);
+//        std::cout << std::endl;
+
+
+//        for (unsigned int k=0; k<10; k++)
+//        {
+//            for (unsigned int j=0; j<10; j++)
+//            {
+//                std::cout << k << ", " << j << std::endl;
+//                std::cout << system_matrix.block(k,j).frobenius_norm() << std::endl;
+//            }
+//        }
+
+        switch (Input::solver_choice) {
+            case SolverOptions::direct_solve: {
+//                SolverControl cn;
+//                std::string solver_type;
+//                TrilinosWrappers::SolverDirect::AdditionalData additional_data(false, "Amesos_Lapack");
+//                TrilinosWrappers::SolverDirect solver(cn, additional_data);
+//                solver.initialize(system_matrix);
+//                solver.solve(distributed_solution, system_rhs);
+                break;
+            }
+            case SolverOptions::exact_preconditioner_with_gmres: {
+                preconditioner.initialize(system_matrix, boundary_values, dof_handler, locally_relevant_solution, distributed_solution);
+                SolverFGMRES<LA::MPI::BlockVector> A_fgmres(solver_control);
+                A_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
+                std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
+                break;
+            }
+            case SolverOptions::inexact_K_with_exact_A_gmres: {
+                preconditioner.initialize(system_matrix, boundary_values, dof_handler,  locally_relevant_solution, distributed_solution);
+                SolverFGMRES<LA::MPI::BlockVector> B_fgmres(solver_control);
+                B_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
+                std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
+                break;
+            }
+            case SolverOptions::inexact_K_with_inexact_A_gmres: {
+                preconditioner.initialize(system_matrix, boundary_values, dof_handler,  locally_relevant_solution, distributed_solution);
+                SolverFGMRES<LA::MPI::BlockVector> C_fgmres(solver_control);
+                C_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
+                std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
+                break;
+            }
+            default:
+                throw;
+        }
+
+
+        constraints.distribute(distributed_solution);
+
+        if (Input::output_parts_of_matrix) {
+            preconditioner.print_stuff();
+        }
+
+        if (Input::output_full_preconditioned_matrix) {
+//            FullMatrix<double> preconditioned_full_mat(system_matrix.n(), system_matrix.n());
+//            const auto op_preconditioned_full_mat = linear_operator(preconditioner) * linear_operator(system_matrix);
+//            build_matrix_element_by_element(op_preconditioned_full_mat, preconditioned_full_mat);
+//            print_matrix("preconditioned_full_block_matrix.csv",preconditioned_full_mat);
+        }
+
+        if (Input::output_full_matrix) {
+//            const unsigned int vec_size = system_matrix.n();
+//            FullMatrix<double> full_mat(vec_size, vec_size);
+//            build_matrix_element_by_element(system_matrix,full_mat);
+//            std::ofstream Mat("full_block_matrix.csv");
+//            for (unsigned int i = 0; i < vec_size; i++) {
+//                Mat << full_mat(i, 0);
+//                for (unsigned int j = 1; j < vec_size; j++) {
+//                    Mat << "," << full_mat(i, j);
+//                }
+//                Mat << "\n";
+//            }
+//            Mat.close();
+        }
+
+//        std::cout << "solution" << std::endl;
+//        distributed_solution.print(std::cout);
+//        std::cout << std::endl;
+
+        return distributed_solution;
+    }
+
+    template<int dim>
+    void
+    KktSystem<dim>::calculate_initial_rhs_error() {
+        initial_rhs_error = system_rhs.l2_norm();
+    }
+
+    template<int dim>
+    LA::MPI::BlockVector
+    KktSystem<dim>::get_initial_state() {
+
+        std::vector<unsigned int> block_component(10, 2);
+        block_component[SolutionBlocks::density] = 0;
+        block_component[SolutionBlocks::displacement] = 1;
+        const std::vector<types::global_dof_index> dofs_per_block =
+                DoFTools::count_dofs_per_fe_block(dof_handler, block_component);
+        const unsigned int n_p = dofs_per_block[0];
+        const unsigned int n_u = dofs_per_block[1];
+        const std::vector<unsigned int> block_sizes = {n_p, n_p, n_p, n_p, n_p, n_u, n_u, n_p, n_p, 1};
+
+        LA::MPI::BlockVector state(owned_partitioning, mpi_communicator);
+        {
+            using namespace SolutionBlocks;
+            state.block(density).add(density_ratio);
+            state.block(unfiltered_density).add(density_ratio);
+            state.block(unfiltered_density_multiplier)
+                    .add(density_ratio);
+            state.block(density_lower_slack).add(density_ratio);
+            state.block(density_lower_slack_multiplier).add(50);
+            state.block(density_upper_slack).add(1 - density_ratio);
+            state.block(density_upper_slack_multiplier).add(50);
+            state.block(total_volume_multiplier).add(1);
+            state.block(displacement).add(0);
+            state.block(displacement_multiplier).add(0);
+        }
+        state.compress(VectorOperation::add);
+        return state;
+    }
+
+    template<int dim>
+    void
+    KktSystem<dim>::output(const LA::MPI::BlockVector &state, const unsigned int j) const {
+        locally_relevant_solution = state;
+        std::vector<std::string> solution_names(1, "low_slack_multiplier");
+        std::vector<DataComponentInterpretation::DataComponentInterpretation> data_component_interpretation(
+                1, DataComponentInterpretation::component_is_scalar);
+        solution_names.emplace_back("upper_slack_multiplier");
+        data_component_interpretation.push_back(
+                DataComponentInterpretation::component_is_scalar);
+        solution_names.emplace_back("low_slack");
+        data_component_interpretation.push_back(
+                DataComponentInterpretation::component_is_scalar);
+        solution_names.emplace_back("upper_slack");
+        data_component_interpretation.push_back(
+                DataComponentInterpretation::component_is_scalar);
+        solution_names.emplace_back("unfiltered_density");
+        data_component_interpretation.push_back(
+                DataComponentInterpretation::component_is_scalar);
+        for (unsigned int i = 0; i < dim; i++) {
+            solution_names.emplace_back("displacement");
+            data_component_interpretation.push_back(
+                    DataComponentInterpretation::component_is_part_of_vector);
+        }
+        for (unsigned int i = 0; i < dim; i++) {
+            solution_names.emplace_back("displacement_multiplier");
+            data_component_interpretation.push_back(
+                    DataComponentInterpretation::component_is_part_of_vector);
+        }
+        solution_names.emplace_back("density_multiplier");
+        data_component_interpretation.push_back(
+                DataComponentInterpretation::component_is_scalar);
+        solution_names.emplace_back("density");
+        data_component_interpretation.push_back(
+                DataComponentInterpretation::component_is_scalar);
+        solution_names.emplace_back("volume_multiplier");
+        data_component_interpretation.push_back(
+                DataComponentInterpretation::component_is_scalar);
+        DataOut<dim> data_out;
+        data_out.attach_dof_handler(dof_handler);
+        data_out.add_data_vector(locally_relevant_solution,
+                                 solution_names,
+                                 DataOut<dim>::type_dof_data,
+                                 data_component_interpretation);
+        data_out.build_patches();
+        std::string output("solution" + std::to_string(j) + ".vtu");
+        data_out.write_vtu_in_parallel(output, MPI_COMM_WORLD);
+
+    }
+
+    template<>
+    void
+    KktSystem<2>::output_stl(const LA::MPI::BlockVector &state) {
+        double height = .25;
+        const int dim = 2;
+        std::ofstream stlfile;
+        stlfile.open("bridge.stl");
+        stlfile << "solid bridge\n" << std::scientific;
+
+        for (const auto &cell: dof_handler.active_cell_iterators()) {
+            if (state.block(
+                    SolutionBlocks::density)[cell->active_cell_index()] > 0.5) {
+                const Tensor<1, dim> edge_directions[2] = {cell->vertex(1) -
+                                                           cell->vertex(0),
+                                                           cell->vertex(2) -
+                                                           cell->vertex(0)};
+                const Tensor<2, dim> edge_tensor(
+                        {{edge_directions[0][0], edge_directions[0][1]},
+                         {edge_directions[1][0], edge_directions[1][1]}});
+                const bool is_right_handed_cell = (determinant(edge_tensor) > 0);
+                if (is_right_handed_cell) {
+                    /* Write one side at z = 0. */
+                    stlfile << "   facet normal " << 0.000000e+00 << " "
+                            << 0.000000e+00 << " " << -1.000000e+00 << "\n";
+                    stlfile << "      outer loop\n";
+                    stlfile << "         vertex " << cell->vertex(0)[0] << " "
+                            << cell->vertex(0)[1] << " " << 0.000000e+00 << "\n";
+                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
+                            << cell->vertex(2)[1] << " " << 0.000000e+00 << "\n";
+                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
+                            << cell->vertex(1)[1] << " " << 0.000000e+00 << "\n";
+                    stlfile << "      endloop\n";
+                    stlfile << "   endfacet\n";
+                    stlfile << "   facet normal " << 0.000000e+00 << " "
+                            << 0.000000e+00 << " " << -1.000000e+00 << "\n";
+                    stlfile << "      outer loop\n";
+                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
+                            << cell->vertex(1)[1] << " " << 0.000000e+00 << "\n";
+                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
+                            << cell->vertex(2)[1] << " " << 0.000000e+00 << "\n";
+                    stlfile << "         vertex " << cell->vertex(3)[0] << " "
+                            << cell->vertex(3)[1] << " " << 0.000000e+00 << "\n";
+                    stlfile << "      endloop\n";
+                    stlfile << "   endfacet\n";
+                    /* Write one side at z = height. */
+                    stlfile << "   facet normal " << 0.000000e+00 << " "
+                            << 0.000000e+00 << " " << 1.000000e+00 << "\n";
+                    stlfile << "      outer loop\n";
+                    stlfile << "         vertex " << cell->vertex(0)[0] << " "
+                            << cell->vertex(0)[1] << " " << height << "\n";
+                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
+                            << cell->vertex(1)[1] << " " << height << "\n";
+                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
+                            << cell->vertex(2)[1] << " " << height << "\n";
+                    stlfile << "      endloop\n";
+                    stlfile << "   endfacet\n";
+                    stlfile << "   facet normal " << 0.000000e+00 << " "
+                            << 0.000000e+00 << " " << 1.000000e+00 << "\n";
+                    stlfile << "      outer loop\n";
+                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
+                            << cell->vertex(1)[1] << " " << height << "\n";
+                    stlfile << "         vertex " << cell->vertex(3)[0] << " "
+                            << cell->vertex(3)[1] << " " << height << "\n";
+                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
+                            << cell->vertex(2)[1] << " " << height << "\n";
+                    stlfile << "      endloop\n";
+                    stlfile << "   endfacet\n";
+                } else /* The cell has a left-handed set up */
+                {
+                    /* Write one side at z = 0. */
+                    stlfile << "   facet normal " << 0.000000e+00 << " "
+                            << 0.000000e+00 << " " << -1.000000e+00 << "\n";
+                    stlfile << "      outer loop\n";
+                    stlfile << "         vertex " << cell->vertex(0)[0] << " "
+                            << cell->vertex(0)[1] << " " << 0.000000e+00 << "\n";
+                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
+                            << cell->vertex(1)[1] << " " << 0.000000e+00 << "\n";
+                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
+                            << cell->vertex(2)[1] << " " << 0.000000e+00 << "\n";
+                    stlfile << "      endloop\n";
+                    stlfile << "   endfacet\n";
+                    stlfile << "   facet normal " << 0.000000e+00 << " "
+                            << 0.000000e+00 << " " << -1.000000e+00 << "\n";
+                    stlfile << "      outer loop\n";
+                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
+                            << cell->vertex(1)[1] << " " << 0.000000e+00 << "\n";
+                    stlfile << "         vertex " << cell->vertex(3)[0] << " "
+                            << cell->vertex(3)[1] << " " << 0.000000e+00 << "\n";
+                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
+                            << cell->vertex(2)[1] << " " << 0.000000e+00 << "\n";
+                    stlfile << "      endloop\n";
+                    stlfile << "   endfacet\n";
+                    /* Write one side at z = height. */
+                    stlfile << "   facet normal " << 0.000000e+00 << " "
+                            << 0.000000e+00 << " " << 1.000000e+00 << "\n";
+                    stlfile << "      outer loop\n";
+                    stlfile << "         vertex " << cell->vertex(0)[0] << " "
+                            << cell->vertex(0)[1] << " " << height << "\n";
+                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
+                            << cell->vertex(2)[1] << " " << height << "\n";
+                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
+                            << cell->vertex(1)[1] << " " << height << "\n";
+                    stlfile << "      endloop\n";
+                    stlfile << "   endfacet\n";
+                    stlfile << "   facet normal " << 0.000000e+00 << " "
+                            << 0.000000e+00 << " " << 1.000000e+00 << "\n";
+                    stlfile << "      outer loop\n";
+                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
+                            << cell->vertex(1)[1] << " " << height << "\n";
+                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
+                            << cell->vertex(2)[1] << " " << height << "\n";
+                    stlfile << "         vertex " << cell->vertex(3)[0] << " "
+                            << cell->vertex(3)[1] << " " << height << "\n";
+                    stlfile << "      endloop\n";
+                    stlfile << "   endfacet\n";
+                }
+                for (unsigned int face_number = 0;
+                     face_number < GeometryInfo<dim>::faces_per_cell;
+                     ++face_number) {
+                    const typename DoFHandler<dim>::face_iterator face =
+                            cell->face(face_number);
+                    if ((face->at_boundary()) ||
+                        (!face->at_boundary() &&
+                         (state.block(
+                                 SolutionBlocks::density)[cell->neighbor(face_number)->active_cell_index()] <
+                          0.5))) {
+                        const Tensor<1, dim> normal_vector =
+                                (face->center() - cell->center());
+                        const double normal_norm = normal_vector.norm();
+                        if ((face->vertex(0)[0] - face->vertex(0)[0]) *
+                            (face->vertex(1)[1] - face->vertex(0)[1]) *
+                            0.000000e+00 +
+                            (face->vertex(0)[1] - face->vertex(0)[1]) * (0 - 0) *
+                            normal_vector[0] +
+                            (height - 0) *
+                            (face->vertex(1)[0] - face->vertex(0)[0]) *
+                            normal_vector[1] -
+                            (face->vertex(0)[0] - face->vertex(0)[0]) * (0 - 0) *
+                            normal_vector[1] -
+                            (face->vertex(0)[1] - face->vertex(0)[1]) *
+                            (face->vertex(1)[0] - face->vertex(0)[0]) *
+                            normal_vector[0] -
+                            (height - 0) *
+                            (face->vertex(1)[1] - face->vertex(0)[1]) * 0 >
+                            0) {
+                            stlfile << "   facet normal "
+                                    << normal_vector[0] / normal_norm << " "
+                                    << normal_vector[1] / normal_norm << " "
+                                    << 0.000000e+00 << "\n";
+                            stlfile << "      outer loop\n";
+                            stlfile << "         vertex " << face->vertex(0)[0]
+                                    << " " << face->vertex(0)[1] << " "
+                                    << 0.000000e+00 << "\n";
+                            stlfile << "         vertex " << face->vertex(0)[0]
+                                    << " " << face->vertex(0)[1] << " " << height
+                                    << "\n";
+                            stlfile << "         vertex " << face->vertex(1)[0]
+                                    << " " << face->vertex(1)[1] << " "
+                                    << 0.000000e+00 << "\n";
+                            stlfile << "      endloop\n";
+                            stlfile << "   endfacet\n";
+                            stlfile << "   facet normal "
+                                    << normal_vector[0] / normal_norm << " "
+                                    << normal_vector[1] / normal_norm << " "
+                                    << 0.000000e+00 << "\n";
+                            stlfile << "      outer loop\n";
+                            stlfile << "         vertex " << face->vertex(0)[0]
+                                    << " " << face->vertex(0)[1] << " " << height
+                                    << "\n";
+                            stlfile << "         vertex " << face->vertex(1)[0]
+                                    << " " << face->vertex(1)[1] << " " << height
+                                    << "\n";
+                            stlfile << "         vertex " << face->vertex(1)[0]
+                                    << " " << face->vertex(1)[1] << " "
+                                    << 0.000000e+00 << "\n";
+                            stlfile << "      endloop\n";
+                            stlfile << "   endfacet\n";
+                        } else {
+                            stlfile << "   facet normal "
+                                    << normal_vector[0] / normal_norm << " "
+                                    << normal_vector[1] / normal_norm << " "
+                                    << 0.000000e+00 << "\n";
+                            stlfile << "      outer loop\n";
+                            stlfile << "         vertex " << face->vertex(0)[0]
+                                    << " " << face->vertex(0)[1] << " "
+                                    << 0.000000e+00 << "\n";
+                            stlfile << "         vertex " << face->vertex(1)[0]
+                                    << " " << face->vertex(1)[1] << " "
+                                    << 0.000000e+00 << "\n";
+                            stlfile << "         vertex " << face->vertex(0)[0]
+                                    << " " << face->vertex(0)[1] << " " << height
+                                    << "\n";
+                            stlfile << "      endloop\n";
+                            stlfile << "   endfacet\n";
+                            stlfile << "   facet normal "
+                                    << normal_vector[0] / normal_norm << " "
+                                    << normal_vector[1] / normal_norm << " "
+                                    << 0.000000e+00 << "\n";
+                            stlfile << "      outer loop\n";
+                            stlfile << "         vertex " << face->vertex(0)[0]
+                                    << " " << face->vertex(0)[1] << " " << height
+                                    << "\n";
+                            stlfile << "         vertex " << face->vertex(1)[0]
+                                    << " " << face->vertex(1)[1] << " "
+                                    << 0.000000e+00 << "\n";
+                            stlfile << "         vertex " << face->vertex(1)[0]
+                                    << " " << face->vertex(1)[1] << " " << height
+                                    << "\n";
+                            stlfile << "      endloop\n";
+                            stlfile << "   endfacet\n";
+                        }
+                    }
+                }
+            }
+        }
+        stlfile << "endsolid bridge";
+    }
+
+
+    template<>
+    void
+    KktSystem<3>::output_stl(const LA::MPI::BlockVector &state)
+    {
+        std::ofstream stlfile;
+        stlfile.open("bridge.stl");
+        stlfile << "solid bridge\n" << std::scientific;
+        const int dim = 3;
+        for (const auto &cell : dof_handler.active_cell_iterators())
+        {
+            if (state.block(
+                    SolutionBlocks::unfiltered_density)[cell->active_cell_index()] > 0.5)
+            {
+                for (const auto n : cell->face_indices())
+                {
+                    bool create_boundary = false;
+                    if (cell->at_boundary(n))
+                    {
+                        create_boundary = true;
+                    }
+                    else if (state.block(
+                            SolutionBlocks::unfiltered_density)[cell->neighbor(n)->active_cell_index()] <= 0.5)
+                    {
+                        create_boundary = true;
+                    }
+
+                    if (create_boundary)
+                    {
+                        const auto face = cell->face(n);
+                        const Tensor<1,dim> normal_vector = face->center() -
+                                                          cell->center();
+                        double normal_norm = normal_vector.norm();
+                        const Tensor<1,dim> edge_vectors_1 = face->vertex(1) - face->vertex(0);
+                        const Tensor<1,dim> edge_vectors_2 = face->vertex(2) - face->vertex(0);
+
+                        const Tensor<2, dim> edge_tensor (
+                                 {{edge_vectors_1[0], edge_vectors_1[1],edge_vectors_1[2]},
+                                 {edge_vectors_2[0], edge_vectors_2[1],edge_vectors_2[2]},
+                                 {normal_vector[0], normal_vector[1], normal_vector[2]}});
+                        const bool is_right_handed_cell = (determinant(edge_tensor) > 0);
+
+                        if (is_right_handed_cell)
+                        {
+                            stlfile << "   facet normal "
+                                    << normal_vector[0] / normal_norm << " "
+                                    << normal_vector[1] / normal_norm << " "
+                                    << normal_vector[2] / normal_norm << "\n";
+                            stlfile << "      outer loop\n";
+                            stlfile << "         vertex " << face->vertex(0)[0]
+                                    << " " << face->vertex(0)[1] << " "
+                                    << face->vertex(0)[2] << "\n";
+                            stlfile << "         vertex " << face->vertex(1)[0]
+                                    << " " << face->vertex(1)[1] << " "
+                                    << face->vertex(1)[2] << "\n";
+                            stlfile << "         vertex " << face->vertex(2)[0]
+                                    << " " << face->vertex(2)[1] << " "
+                                    << face->vertex(2)[2] << "\n";
+                            stlfile << "      endloop\n";
+                            stlfile << "   endfacet\n";
+                            stlfile << "   facet normal "
+                                    << normal_vector[0] / normal_norm << " "
+                                    << normal_vector[1] / normal_norm << " "
+                                    << normal_vector[2] / normal_norm << "\n";
+                            stlfile << "      outer loop\n";
+                            stlfile << "         vertex " << face->vertex(1)[0]
+                                    << " " << face->vertex(1)[1] << " " << face->vertex(1)[2]
+                                    << "\n";
+                            stlfile << "         vertex " << face->vertex(3)[0]
+                                    << " " << face->vertex(3)[1] << " " << face->vertex(3)[2]
+                                    << "\n";
+                            stlfile << "         vertex " << face->vertex(2)[0]
+                                    << " " << face->vertex(2)[1] << " "
+                                    << face->vertex(2)[2] << "\n";
+                            stlfile << "      endloop\n";
+                            stlfile << "   endfacet\n";
+                        }
+                        else
+                        {
+                            stlfile << "   facet normal "
+                                    << normal_vector[0] / normal_norm << " "
+                                    << normal_vector[1] / normal_norm << " "
+                                    << normal_vector[2] / normal_norm << "\n";
+                            stlfile << "      outer loop\n";
+                            stlfile << "         vertex " << face->vertex(0)[0]
+                                    << " " << face->vertex(0)[1] << " "
+                                    << face->vertex(0)[2] << "\n";
+                            stlfile << "         vertex " << face->vertex(2)[0]
+                                    << " " << face->vertex(2)[1] << " "
+                                    << face->vertex(2)[2] << "\n";
+                            stlfile << "         vertex " << face->vertex(1)[0]
+                                    << " " << face->vertex(1)[1] << " "
+                                    << face->vertex(1)[2] << "\n";
+                            stlfile << "      endloop\n";
+                            stlfile << "   endfacet\n";
+                            stlfile << "   facet normal "
+                                    << normal_vector[0] / normal_norm << " "
+                                    << normal_vector[1] / normal_norm << " "
+                                    << normal_vector[2] / normal_norm << "\n";
+                            stlfile << "      outer loop\n";
+                            stlfile << "         vertex " << face->vertex(1)[0]
+                                    << " " << face->vertex(1)[1] << " " << face->vertex(1)[2]
+                                    << "\n";
+                            stlfile << "         vertex " << face->vertex(2)[0]
+                                    << " " << face->vertex(2)[1] << " " << face->vertex(2)[2]
+                                    << "\n";
+                            stlfile << "         vertex " << face->vertex(3)[0]
+                                    << " " << face->vertex(3)[1] << " "
+                                    << face->vertex(3)[2] << "\n";
+                            stlfile << "      endloop\n";
+                            stlfile << "   endfacet\n";
+                        }
+
+                    }
+
+                }
+            }
+        }
+    }
+}
+
+template class SAND::KktSystem<2>;
+template class SAND::KktSystem<3>;

From 83270d31061867f4301a82ad10c3d8233877fc85 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Fri, 10 Dec 2021 13:42:12 -0700
Subject: [PATCH 45/95] running!

---
 include/density_filter.h                |    1 -
 include/input_information.h             |    4 +-
 include/kkt_system.h                    |    4 +-
 include/schur_preconditioner.h          |    4 +
 include/schur_preconditioner.h.autosave |  178 ++
 source/density_filter.cc                |   12 +-
 source/density_filter.cc.autosave       |  128 --
 source/kkt_system.cc                    |  114 +-
 source/kkt_system.cc.autosave           | 2300 -----------------------
 source/schur_preconditioner.cc          |   54 +-
 source/watchdog_main.cc                 |   32 +-
 11 files changed, 268 insertions(+), 2563 deletions(-)
 create mode 100644 include/schur_preconditioner.h.autosave
 delete mode 100644 source/density_filter.cc.autosave
 delete mode 100644 source/kkt_system.cc.autosave

diff --git a/include/density_filter.h b/include/density_filter.h
index 3fdde45..9656fad 100644
--- a/include/density_filter.h
+++ b/include/density_filter.h
@@ -66,7 +66,6 @@ namespace SAND {
 
     private:
 
-
     };
 }
 #endif //SAND_DENSITY_FILTER_H
diff --git a/include/input_information.h b/include/input_information.h
index 6ceda9c..5fc4649 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -16,8 +16,8 @@ namespace SAND {
 
         //geometry options
         constexpr unsigned int geometry_base = GeometryOptions::mbb;
-        constexpr unsigned int dim = 2;
-        constexpr unsigned int refinements =  3;
+        constexpr unsigned int dim = 3;
+        constexpr unsigned int refinements =  4;
 
         //nonlinear algorithm options
         constexpr double initial_barrier_size = 25;
diff --git a/include/kkt_system.h b/include/kkt_system.h
index 9db4a82..95907eb 100644
--- a/include/kkt_system.h
+++ b/include/kkt_system.h
@@ -56,6 +56,8 @@
 #include "../include/schur_preconditioner.h"
 #include "../include/density_filter.h"
 
+#include <deal.II/base/conditional_ostream.h>
+
 #include <iostream>
 #include <fstream>
 #include <algorithm>
@@ -144,7 +146,7 @@ namespace SAND {
         mutable DensityFilter<dim> density_filter;
 
         std::map<types::global_dof_index, double> boundary_values;
-
+        ConditionalOStream pcout;
 
         double initial_rhs_error;
 
diff --git a/include/schur_preconditioner.h b/include/schur_preconditioner.h
index 11b8255..724f7f1 100644
--- a/include/schur_preconditioner.h
+++ b/include/schur_preconditioner.h
@@ -61,6 +61,8 @@
 
 #include "../include/parameters_and_components.h"
 
+#include <deal.II/base/conditional_ostream.h>
+
 #include <iostream>
 #include <algorithm>
 
@@ -115,6 +117,7 @@ namespace SAND
         LA::MPI::BlockSparseMatrix &system_matrix;
 
     private:
+        MPI_Comm  mpi_communicator;
         unsigned int n_rows;
         unsigned int n_columns;
         unsigned int n_block_rows;
@@ -167,6 +170,7 @@ namespace SAND
         mutable VmultTrilinosSolverDirect a_inv_direct;
 //        TrilinosWrappers::SolverDirect a_inv_direct;
         mutable TimerOutput timer;
+        ConditionalOStream pcout;
 
     };
 
diff --git a/include/schur_preconditioner.h.autosave b/include/schur_preconditioner.h.autosave
new file mode 100644
index 0000000..1cca598
--- /dev/null
+++ b/include/schur_preconditioner.h.autosave
@@ -0,0 +1,178 @@
+//
+// Created by justin on 3/2/21.
+//
+
+#ifndef SAND_SCHUR_PRECONDITIONER_H
+#define SAND_SCHUR_PRECONDITIONER_H
+#include <deal.II/base/quadrature_lib.h>
+#include <deal.II/base/function.h>
+#include <deal.II/base/tensor.h>
+#include <deal.II/base/timer.h>
+
+#include <deal.II/lac/block_vector.h>
+#include <deal.II/lac/full_matrix.h>
+#include <deal.II/lac/block_sparse_matrix.h>
+#include <deal.II/lac/precondition.h>
+#include <deal.II/lac/linear_operator.h>
+#include <deal.II/lac/linear_operator_tools.h>
+#include <deal.II/lac/packaged_operation.h>
+#include <deal.II/lac/sparse_direct.h>
+#include <deal.II/lac/solver_gmres.h>
+#include <deal.II/lac/solver_cg.h>
+#include <deal.II/lac/solver_bicgstab.h>
+#include <deal.II/lac/generic_linear_algebra.h>
+#include <deal.II/lac/matrix_out.h>
+#include <deal.II/lac/trilinos_sparse_matrix.h>
+#include <deal.II/lac/trilinos_block_sparse_matrix.h>
+
+#include <deal.II/grid/tria.h>
+#include <deal.II/grid/grid_generator.h>
+#include <deal.II/grid/grid_refinement.h>
+
+#include <deal.II/dofs/dof_handler.h>
+#include <deal.II/dofs/dof_renumbering.h>
+#include <deal.II/dofs/dof_tools.h>
+
+#include <deal.II/fe/fe_values.h>
+#include <deal.II/fe/fe_dgq.h>
+#include <deal.II/fe/fe_system.h>
+#include <deal.II/fe/fe_q.h>
+
+#include <deal.II/numerics/vector_tools.h>
+#include <deal.II/numerics/matrix_tools.h>
+#include <deal.II/numerics/data_out.h>
+#include <deal.II/numerics/error_estimator.h>
+#include <deal.II/base/config.h>
+#include <deal.II/base/numbers.h>
+#include <deal.II/base/table.h>
+#include <deal.II/base/tensor.h>
+#include <deal.II/base/timer.h>
+
+#include <deal.II/differentiation/ad/ad_number_traits.h>
+
+#include <deal.II/lac/exceptions.h>
+#include <deal.II/lac/identity_matrix.h>
+
+
+#include <cstring>
+#include <iomanip>
+#include <vector>
+
+
+#include "../include/parameters_and_components.h"
+
+#include <deal.II/base/conditional_ostream.h>
+
+#include <iostream>
+#include <algorithm>
+
+
+namespace SAND
+{
+    namespace LA
+    {
+        using namespace dealii::LinearAlgebraTrilinos;
+    }
+    using namespace dealii;
+
+    class VmultTrilinosSolverDirect : public TrilinosWrappers::SparseMatrix {
+        public:
+            VmultTrilinosSolverDirect(SolverControl &cn,
+                                      const TrilinosWrappers::SolverDirect::AdditionalData &data
+                                      );
+            void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
+            void vmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const;
+            void Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
+            void Tvmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const;
+            void initialize(LA::MPI::SparseMatrix &a_mat);
+            unsigned int m() const;
+            unsigned int n() const;
+            int get_size()
+            {
+                return size;
+            }
+        private:
+            mutable TrilinosWrappers::SolverDirect solver_direct;
+            int size;
+    };
+
+    template<int dim>
+    class TopOptSchurPreconditioner: public Subscriptor {
+    public:
+        TopOptSchurPreconditioner(LA::MPI::BlockSparseMatrix &matrix_in);
+        void initialize (LA::MPI::BlockSparseMatrix &matrix, const std::map<types::global_dof_index, double> &boundary_values, const DoFHandler<dim> &dof_handler, const LA::MPI::BlockVector &state, const LA::MPI::BlockVector &distributed_state);
+        void vmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
+        void Tvmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
+        void vmult_add(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
+        void Tvmult_add(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
+        void clear();
+        unsigned int m() const;
+        unsigned int n() const;
+        void get_sparsity_pattern(BlockDynamicSparsityPattern &bdsp);
+
+        void assemble_mass_matrix(const LA::MPI::BlockVector &state, const hp::FECollection<dim> &fe_system, const DoFHandler<dim> &dof_handler, const AffineConstraints<double> &constraints,   const BlockSparsityPattern &bsp);
+
+        void print_stuff();
+
+        LA::MPI::BlockSparseMatrix &system_matrix;
+
+    private:
+        MPI_Comm  mpi_communicator;
+        unsigned int n_rows;
+        unsigned int n_columns;
+        unsigned int n_block_rows;
+        unsigned int n_block_columns;
+        void vmult_step_1(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
+        void vmult_step_2(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
+        void vmult_step_3(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
+        void vmult_step_4(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
+        void vmult_step_5(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
+
+        BlockSparsityPattern mass_sparsity;
+        LA::MPI::BlockSparseMatrix approx_h_mat;
+
+        SolverControl other_solver_control;
+        mutable SolverBicgstab<Vector<double>> other_bicgstab;
+        mutable SolverGMRES<Vector<double>> other_gmres;
+        mutable SolverCG<Vector<double>> other_cg;
+
+        LA::MPI::SparseMatrix &a_mat;
+        const LA::MPI::SparseMatrix &b_mat;
+        const LA::MPI::SparseMatrix &c_mat;
+        const LA::MPI::SparseMatrix &e_mat;
+        const LA::MPI::SparseMatrix &f_mat;
+        const LA::MPI::SparseMatrix &d_m_mat;
+        const LA::MPI::SparseMatrix &d_1_mat;
+        const LA::MPI::SparseMatrix &d_2_mat;
+        const LA::MPI::SparseMatrix &m_vect;
+
+        LA::MPI::SparseMatrix d_3_mat;
+        LA::MPI::SparseMatrix d_4_mat;
+        LA::MPI::SparseMatrix d_5_mat;
+        LA::MPI::SparseMatrix d_6_mat;
+        LA::MPI::SparseMatrix d_7_mat;
+        LA::MPI::SparseMatrix d_8_mat;
+        LA::MPI::SparseMatrix d_m_inv_mat;
+
+        FullMatrix<double> g_mat;
+        FullMatrix<double> h_mat;
+        FullMatrix<double> k_inv_mat;
+        LAPACKFullMatrix<double> k_mat;
+
+        mutable LA::MPI::Vector pre_j;
+        mutable LA::MPI::Vector pre_k;
+        mutable LA::MPI::Vector g_d_m_inv_density;
+        mutable LA::MPI::Vector k_g_d_m_inv_density;
+
+        std::string solver_type;
+        TrilinosWrappers::SolverDirect::AdditionalData additional_data;
+        SolverControl direct_solver_control;
+        mutable VmultTrilinosSolverDirect a_inv_direct;
+//        TrilinosWrappers::SolverDirect a_inv_direct;
+        mutable TimerOutput timer;
+//        ConditionalOStream pcout;
+
+    };
+
+}
+#endif //SAND_SCHUR_PRECONDITIONER_H
diff --git a/source/density_filter.cc b/source/density_filter.cc
index 78bddc9..2740900 100644
--- a/source/density_filter.cc
+++ b/source/density_filter.cc
@@ -36,7 +36,7 @@ namespace SAND {
                 {
                     std::vector<types::global_dof_index> j(neighbor_cell->get_fe().n_dofs_per_cell());
                     neighbor_cell->get_dof_indices(j);
-                    filter_dsp.add(i[16], j[16]);
+                    filter_dsp.add(i[cell->get_fe().component_to_system_index(0, 0)], j[cell->get_fe().component_to_system_index(0, 0)]);
                 }
             }
 
@@ -61,7 +61,7 @@ namespace SAND {
                             cell->center().distance(neighbor_cell->center());
                     /*value should be (max radius - distance between cells)*cell measure */
                     double value = (Input::filter_r - d)*neighbor_cell->measure();
-                    filter_matrix.add(i[16], j[16], value);
+                    filter_matrix.add(i[cell->get_fe().component_to_system_index(0, 0)], j[cell->get_fe().component_to_system_index(0, 0)], value);
                 }
             }
         }
@@ -75,13 +75,13 @@ namespace SAND {
                 cell->get_dof_indices(i);
                 double denominator = 0;
                 typename LA::MPI::SparseMatrix::iterator iter = filter_matrix.begin(
-                        i[16]);
-                for (; iter != filter_matrix.end(i[16]); iter++)
+                        i[cell->get_fe().component_to_system_index(0, 0)]);
+                for (; iter != filter_matrix.end(i[cell->get_fe().component_to_system_index(0, 0)]); iter++)
                 {
                     denominator = denominator + iter->value();
                 }
-                iter = filter_matrix.begin(i[16]);
-                for (; iter != filter_matrix.end(i[16]); iter++)
+                iter = filter_matrix.begin(i[cell->get_fe().component_to_system_index(0, 0)]);
+                for (; iter != filter_matrix.end(i[cell->get_fe().component_to_system_index(0, 0)]); iter++)
                 {
                     iter->value() = iter->value() / denominator;
                 }
diff --git a/source/density_filter.cc.autosave b/source/density_filter.cc.autosave
deleted file mode 100644
index 706552e..0000000
--- a/source/density_filter.cc.autosave
+++ /dev/null
@@ -1,128 +0,0 @@
-//
-// Created by justin on 5/13/21.
-//
-#include "../include/density_filter.h"
-#include "../include/input_information.h"
-#include <deal.II/base/tensor.h>
-#include <deal.II/lac/block_sparse_matrix.h>
-#include <deal.II/lac/precondition.h>
-#include <deal.II/grid/tria.h>
-#include <deal.II/numerics/vector_tools.h>
-#include <deal.II/numerics/matrix_tools.h>
-
-namespace SAND {
-    using namespace dealii;
-
-    /* When initialized, this function takes the current triangulation and creates a matrix corresponding to a
-     * convolution being applied to a piecewise constant function on that triangulation  */
-    template<int dim>
-    void
-    DensityFilter<dim>::initialize(DoFHandler<dim> &dof_handler) {
-        const std::vector<types::global_dof_index> dofs_per_block = DoFTools::count_dofs_per_fe_block(dof_handler);
-        filter_dsp.reinit(dofs_per_block[0],
-                          dofs_per_block[0]);
-        std::cout << "initialized with " << dofs_per_block[0] << std::endl;
-        std::set<unsigned int> neighbor_ids;
-        std::set<typename DoFHandler<dim>::cell_iterator> cells_to_check;
-        std::set<typename DoFHandler<dim>::cell_iterator> cells_to_check_temp;
-        /*finds neighbors whose values would be relevant, and adds them to the sparsity pattern of the matrix*/
-        for (const auto &cell : dof_handler.active_cell_iterators()) {
-            if(cell->is_locally_owned())
-            {
-
-                std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
-                cell->get_dof_indices(i);
-                for (const auto &neighbor_cell : find_relevant_neighbors(cell))
-                {
-                    std::vector<types::global_dof_index> j(neighbor_cell->get_fe().n_dofs_per_cell());
-                    neighbor_cell->get_dof_indices(j);
-                    filter_dsp.add(i[cell->get_fe().component_to_system_index(0,0)], j[cell->get_fe().component_to_system_index(0,0)]);
-                }
-            }
-
-        }
-        filter_sparsity_pattern.copy_from(filter_dsp);
-
-        const auto owned_dofs = dof_handler.locally_owned_dofs().get_view(0, dofs_per_block[0]);
-
-        filter_matrix.reinit(owned_dofs, filter_sparsity_pattern, MPI_COMM_WORLD);
-
-        /*adds values to the matrix corresponding to the max radius - */
-        for (const auto &cell : dof_handler.active_cell_iterators())
-        {
-            if(cell->is_locally_owned())
-            {
-                std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
-                cell->get_dof_indices(i);
-                for (const auto &neighbor_cell : find_relevant_neighbors(cell)) {
-                    std::vector<types::global_dof_index> j(neighbor_cell->get_fe().n_dofs_per_cell());
-                    neighbor_cell->get_dof_indices(j);
-                    const double d =
-                            cell->center().distance(neighbor_cell->center());
-                    /*value should be (max radius - distance between cells)*cell measure */
-                    double value = (Input::filter_r - d)*neighbor_cell->measure();
-                    filter_matrix.add(i[cell->get_fe().component_to_system_index(0,0)], j[cell->get_fe().component_to_system_index(0,0)], value);
-                }
-            }
-        }
-
-        //here we normalize the filter so it computes an average. Sum of values in a row should be 1
-        for (const auto &cell : dof_handler.active_cell_iterators())
-        {
-            if(cell->is_locally_owned())
-            {
-                std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
-                cell->get_dof_indices(i);
-                double denominator = 0;
-                typename LA::MPI::SparseMatrix::iterator iter = filter_matrix.begin(
-                        i[cell->get_fe().component_to_system_index(0,0)]);
-                for (; iter != filter_matrix.end(i[cell->get_fe().component_to_system_index(0,0)]); iter++)
-                {
-                    denominator = denominator + iter->value();
-                }
-                iter = filter_matrix.begin(i[cell->get_fe().component_to_system_index(0,0)]);
-                for (; iter != filter_matrix.end(i[cell->get_fe().component_to_system_index(0,0)]); iter++)
-                {
-                    iter->value() = iter->value() / denominator;
-                }
-            }
-        }
-        std::cout << "density nonzero entries" << filter_matrix.n_nonzero_elements() << std::endl;
-    }
-
-    /*This function finds which neighbors are within a certain radius of the initial cell.*/
-    template<int dim>
-    std::set<typename DoFHandler<dim>::cell_iterator>
-    DensityFilter<dim>::find_relevant_neighbors(typename DoFHandler<dim>::cell_iterator cell) const {
-        std::set<unsigned int> neighbor_ids;
-        std::set<typename DoFHandler<dim>::cell_iterator> cells_to_check;
-        neighbor_ids.insert(cell->active_cell_index());
-        cells_to_check.insert(cell);
-        bool new_neighbors_found;
-        do {
-            new_neighbors_found = false;
-            for (const auto &check_cell :
-                    std::vector<typename DoFHandler<dim>::cell_iterator>(
-                            cells_to_check.begin(), cells_to_check.end())) {
-                for (const auto n : check_cell->face_indices()) {
-                    if (!(check_cell->face(n)->at_boundary())) {
-                        const auto &neighbor = check_cell->neighbor(n);
-                        const double distance =
-                                cell->center().distance(neighbor->center());
-                        if ((distance < Input::filter_r) &&
-                            !(neighbor_ids.count(neighbor->active_cell_index())))
-                        {
-                            cells_to_check.insert(neighbor);
-                            neighbor_ids.insert(neighbor->active_cell_index());
-                            new_neighbors_found = true;
-                        }
-                    }
-                }
-            }
-        } while (new_neighbors_found);
-        return cells_to_check;
-    }
-
-}//SAND namespace
-    template class SAND::DensityFilter<2>;
-    template class SAND::DensityFilter<3>;
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index 6b22358..788ed43 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -35,6 +35,9 @@
 #include <deal.II/numerics/data_out.h>
 #include <deal.II/numerics/error_estimator.h>
 
+
+#include <deal.II/base/conditional_ostream.h>
+
 #include "../include/input_information.h"
 
 #include <iostream>
@@ -65,7 +68,8 @@ namespace SAND {
                    FE_DGQ<dim>(0) ^ 1),
             density_ratio(Input::volume_percentage),
             density_penalty_exponent(Input::density_penalty_exponent),
-            density_filter()
+            density_filter(),
+            pcout(std::cout,(Utilities::MPI::this_mpi_process(mpi_communicator) == 0))
     {
         fe_collection.push_back(fe_nine);
         fe_collection.push_back(fe_ten);
@@ -612,7 +616,7 @@ namespace SAND {
         const unsigned int n_p = dofs_per_block[0];
         const unsigned int n_u = dofs_per_block[1];
 
-        std::cout << "n_p:  " << n_p << "   n_u:  " << n_u << std::endl;
+        pcout << "n_p:  " << n_p << "   n_u:  " << n_u << std::endl;
 
         IndexSet locally_owned_dofs = dof_handler.locally_owned_dofs();
         IndexSet locally_relevant_dofs;
@@ -1129,7 +1133,7 @@ namespace SAND {
             }
 
         }
-        std::cout << "compress 1" << std::endl;
+        pcout << "compress 1" << std::endl;
         system_matrix.compress(VectorOperation::add);
         system_rhs = calculate_rhs(distributed_state, barrier_size);
 
@@ -1140,32 +1144,31 @@ namespace SAND {
                 cell->get_dof_indices(i);
 
                 const types::global_dof_index dof_index_lower_slack =
-                        i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)];
-                Assert (cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>,0)==16, ExcInternalError());
+                        i[cell->get_fe().component_to_system_index(0, 0)];
                 typename LA::MPI::SparseMatrix::iterator iter = density_filter.filter_matrix.begin(
-                        i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)]);
-                for (; iter != density_filter.filter_matrix.end(i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)]); iter++) {
+                        i[cell->get_fe().component_to_system_index(0, 0)]);
+                for (; iter != density_filter.filter_matrix.end(i[cell->get_fe().component_to_system_index(0, 0)]); iter++) {
                     unsigned int j = iter->column();
                     double value = iter->value() * cell->measure();
 
                     system_matrix.block(SolutionBlocks::unfiltered_density_multiplier,
-                                        SolutionBlocks::unfiltered_density).add(i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)], j, value);
+                                        SolutionBlocks::unfiltered_density).add(i[cell->get_fe().component_to_system_index(0, 0)], j, value);
                     system_matrix.block(SolutionBlocks::unfiltered_density,
-                                        SolutionBlocks::unfiltered_density_multiplier).add(j, i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)], value);
+                                        SolutionBlocks::unfiltered_density_multiplier).add(j, i[cell->get_fe().component_to_system_index(0, 0)], value);
                 }
 
-                system_matrix.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).add(0, i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)],
+                system_matrix.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).add(0, i[cell->get_fe().component_to_system_index(0, 0)],
                                                                                                           cell->measure());
 
-                system_matrix.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier).add(i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)], 0,
+                system_matrix.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier).add(i[cell->get_fe().component_to_system_index(0, 0)], 0,
                                                                                                           cell->measure());
             }
         }
-        std::cout << "compress 2" << std::endl;
+        pcout << "compress 2" << std::endl;
         system_matrix.compress(VectorOperation::add);
 
 
-        std::cout << "assembled" << std::endl;
+        pcout << "assembled" << std::endl;
 
     }
 
@@ -1255,7 +1258,7 @@ namespace SAND {
         double objective_value_out;
         MPI_Allreduce(&objective_value, &objective_value_out, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
 
-        std::cout << "objective value: " << objective_value_out << std::endl;
+        pcout << "objective value: " << objective_value_out << std::endl;
         return objective_value;
     }
 
@@ -1278,7 +1281,7 @@ namespace SAND {
         double out_barrier_distance_log_sum;
         MPI_Allreduce(&barrier_distance_log_sum, &out_barrier_distance_log_sum, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
 
-        std::cout << "Barrier distance log sum: " << out_barrier_distance_log_sum << std::endl;
+        pcout << "Barrier distance log sum: " << out_barrier_distance_log_sum << std::endl;
 
         return out_barrier_distance_log_sum;
     }
@@ -1329,7 +1332,7 @@ namespace SAND {
            MPI_Allreduce(&prod2, &full_prod2, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
            norm = norm + full_prod1 + full_prod2;
        }
-       std::cout << "pre-norm: " << norm << std::endl;
+       pcout << "pre-norm: " << norm << std::endl;
 
         norm += std::pow(test_rhs.block(SolutionBlocks::displacement).l2_norm(), 2);
         norm += std::pow(test_rhs.block(SolutionBlocks::density).l2_norm(), 2);
@@ -1340,7 +1343,7 @@ namespace SAND {
         norm += std::pow(test_rhs.block(SolutionBlocks::density_upper_slack_multiplier).l2_norm(), 2);
         norm += std::pow(test_rhs.block(SolutionBlocks::density_lower_slack_multiplier).l2_norm(), 2);
 
-        std::cout << "norm: " << norm << std::endl;
+        pcout << "norm: " << norm << std::endl;
 
         return norm;
     }
@@ -1382,7 +1385,7 @@ namespace SAND {
             MPI_Allreduce(&prod2, &full_prod2, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
             norm = norm + full_prod1 + full_prod2;
         }
-        std::cout << "pre-norm: " << norm << std::endl;
+        pcout << "pre-norm: " << norm << std::endl;
 
         norm += std::pow(test_rhs.block(SolutionBlocks::displacement).l2_norm(), 2);
         norm += std::pow(test_rhs.block(SolutionBlocks::density).l2_norm(), 2);
@@ -1395,8 +1398,8 @@ namespace SAND {
 
         norm = std::pow(norm, .5);
 
-        std::cout << "l2 norm: " << system_rhs.l2_norm() << std::endl;
-        std::cout << "KKT norm: " << norm << std::endl;
+        pcout << "l2 norm: " << system_rhs.l2_norm() << std::endl;
+        pcout << "KKT norm: " << norm << std::endl;
         return norm;
     }
 
@@ -1778,56 +1781,21 @@ namespace SAND {
         SolverControl solver_control(10000, gmres_tolerance * system_rhs.l2_norm());
         TopOptSchurPreconditioner<dim> preconditioner(system_matrix);
 
-//        std::cout << " rhs " << std::endl;
-//        system_rhs.print(std::cout);
-//        std::cout << std::endl;
-
-//        preconditioner.initialize(system_matrix, boundary_values, dof_handler, locally_relevant_solution, distributed_solution);
-//        preconditioner.vmult(distributed_solution,system_rhs);
-
-//        std::cout << "solution" <<  std::endl;
-//        distributed_solution.print(std::cout);
-//        std::cout << std::endl;
-
-
-//        for (unsigned int k=0; k<10; k++)
-//        {
-//            for (unsigned int j=0; j<10; j++)
-//            {
-//                std::cout << k << ", " << j << std::endl;
-//                std::cout << system_matrix.block(k,j).frobenius_norm() << std::endl;
-//            }
-//        }
 
         switch (Input::solver_choice) {
-            case SolverOptions::direct_solve: {
-//                SolverControl cn;
-//                std::string solver_type;
-//                TrilinosWrappers::SolverDirect::AdditionalData additional_data(false, "Amesos_Lapack");
-//                TrilinosWrappers::SolverDirect solver(cn, additional_data);
-//                solver.initialize(system_matrix);
-//                solver.solve(distributed_solution, system_rhs);
-                break;
-            }
-            case SolverOptions::exact_preconditioner_with_gmres: {
-                preconditioner.initialize(system_matrix, boundary_values, dof_handler, locally_relevant_solution, distributed_solution);
-                SolverFGMRES<LA::MPI::BlockVector> A_fgmres(solver_control);
-                A_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
-                std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
-                break;
-            }
+
             case SolverOptions::inexact_K_with_exact_A_gmres: {
                 preconditioner.initialize(system_matrix, boundary_values, dof_handler,  locally_relevant_solution, distributed_solution);
                 SolverFGMRES<LA::MPI::BlockVector> B_fgmres(solver_control);
                 B_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
-                std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
+                pcout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
                 break;
             }
             case SolverOptions::inexact_K_with_inexact_A_gmres: {
                 preconditioner.initialize(system_matrix, boundary_values, dof_handler,  locally_relevant_solution, distributed_solution);
                 SolverFGMRES<LA::MPI::BlockVector> C_fgmres(solver_control);
                 C_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
-                std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
+                pcout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
                 break;
             }
             default:
@@ -1837,35 +1805,7 @@ namespace SAND {
 
         constraints.distribute(distributed_solution);
 
-        if (Input::output_parts_of_matrix) {
-            preconditioner.print_stuff();
-        }
-
-        if (Input::output_full_preconditioned_matrix) {
-//            FullMatrix<double> preconditioned_full_mat(system_matrix.n(), system_matrix.n());
-//            const auto op_preconditioned_full_mat = linear_operator(preconditioner) * linear_operator(system_matrix);
-//            build_matrix_element_by_element(op_preconditioned_full_mat, preconditioned_full_mat);
-//            print_matrix("preconditioned_full_block_matrix.csv",preconditioned_full_mat);
-        }
-
-        if (Input::output_full_matrix) {
-//            const unsigned int vec_size = system_matrix.n();
-//            FullMatrix<double> full_mat(vec_size, vec_size);
-//            build_matrix_element_by_element(system_matrix,full_mat);
-//            std::ofstream Mat("full_block_matrix.csv");
-//            for (unsigned int i = 0; i < vec_size; i++) {
-//                Mat << full_mat(i, 0);
-//                for (unsigned int j = 1; j < vec_size; j++) {
-//                    Mat << "," << full_mat(i, j);
-//                }
-//                Mat << "\n";
-//            }
-//            Mat.close();
-        }
-
-//        std::cout << "solution" << std::endl;
-//        distributed_solution.print(std::cout);
-//        std::cout << std::endl;
+        pcout << "here" << std::endl;
 
         return distributed_solution;
     }
diff --git a/source/kkt_system.cc.autosave b/source/kkt_system.cc.autosave
deleted file mode 100644
index 1b5a8b4..0000000
--- a/source/kkt_system.cc.autosave
+++ /dev/null
@@ -1,2300 +0,0 @@
-//
-// Created by justin on 2/17/21.
-//
-#include "../include/kkt_system.h"
-#include <deal.II/base/quadrature_lib.h>
-#include <deal.II/base/function.h>
-#include <deal.II/base/tensor.h>
-#include <deal.II/base/timer.h>
-
-#include <deal.II/lac/full_matrix.h>
-#include <deal.II/lac/linear_operator.h>
-#include <deal.II/lac/solver_gmres.h>
-#include <deal.II/lac/generic_linear_algebra.h>
-#include <deal.II/lac/trilinos_sparse_matrix.h>
-
-
-#include <deal.II/lac/matrix_out.h>
-
-#include <deal.II/grid/grid_generator.h>
-
-#include <deal.II/dofs/dof_renumbering.h>
-#include <deal.II/dofs/dof_tools.h>
-
-#include <deal.II/fe/fe_values.h>
-#include <deal.II/fe/fe_dgq.h>
-#include <deal.II/fe/fe_system.h>
-#include <deal.II/fe/fe_q.h>
-#include <deal.II/fe/fe_nothing.h>
-
-#include <deal.II/hp/fe_collection.h>
-#include <deal.II/hp/fe_values.h>
-
-#include <deal.II/numerics/vector_tools.h>
-#include <deal.II/numerics/matrix_tools.h>
-#include <deal.II/numerics/data_out.h>
-#include <deal.II/numerics/error_estimator.h>
-
-#include "../include/input_information.h"
-
-#include <iostream>
-#include <algorithm>
-
-// This problem initializes with a FESystem composed of 2×dim FE_Q(1) elements, and 8 FE_DGQ(0)  elements.
-// The  piecewise  constant  functions  are  for  density-related  variables,and displacement-related variables are assigned to the FE_Q(1) elements.
-namespace SAND {
-    template<int dim>
-    KktSystem<dim>::KktSystem()
-            :
-            mpi_communicator(MPI_COMM_WORLD),
-            triangulation(mpi_communicator,
-                          typename Triangulation<dim>::MeshSmoothing(
-                                  Triangulation<dim>::smoothing_on_refinement |
-                                  Triangulation<dim>::smoothing_on_coarsening)),
-            dof_handler(triangulation),
-            /*fe should have 1 FE_DGQ<dim>(0) element for density, dim FE_Q finite elements for displacement,
-             * another dim FE_Q elements for the lagrange multiplier on the FE constraint, and 2 more FE_DGQ<dim>(0)
-             * elements for the upper and lower bound constraints */
-            fe_nine(FE_DGQ<dim>(0) ^ 5,
-                    (FESystem<dim>(FE_Q<dim>(1) ^ dim)) ^ 2,
-                    FE_DGQ<dim>(0) ^ 2,
-                    FE_Nothing<dim>() ^ 1),
-            fe_ten(FE_DGQ<dim>(0) ^ 5,
-                   (FESystem<dim>(FE_Q<dim>(1) ^ dim)) ^ 2,
-                   FE_DGQ<dim>(0) ^ 2,
-                   FE_DGQ<dim>(0) ^ 1),
-            density_ratio(Input::volume_percentage),
-            density_penalty_exponent(Input::density_penalty_exponent),
-            density_filter()
-    {
-        fe_collection.push_back(fe_nine);
-        fe_collection.push_back(fe_ten);
-
-    }
-
-
-//A  function  used  once  at  the  beginning  of  the  program,  this  creates  a  matrix  H  so  that H* unfiltered density = filtered density
-
-    template<int dim>
-    void
-    KktSystem<dim>::setup_filter_matrix() {
-
-        density_filter.initialize(dof_handler);
-    }
-
-    //This triangulation matches the problem description -
-    // a 6-by-1 rectangle where a force will be applied in the top center.
-
-    template<int dim>
-    void
-    KktSystem<dim>::create_triangulation() {
-
-        std::vector<unsigned int> sub_blocks(2*dim+8, 0);
-
-        sub_blocks[0]=0;
-        sub_blocks[1]=1;
-        sub_blocks[2]=2;
-        sub_blocks[3]=3;
-        sub_blocks[4]=4;
-        for(int i=0; i<dim; i++)
-        {
-            sub_blocks[5+i]=5;
-        }
-        for(int i=0; i<dim; i++)
-        {
-            sub_blocks[5+dim+i]=6;
-        }
-        sub_blocks[5+2*dim]=7;
-        sub_blocks[6+2*dim]=8;
-        sub_blocks[7+2*dim]=9;
-
-
-        if (Input::geometry_base == GeometryOptions::mbb) {
-            const double width = 6;
-            const unsigned int width_refine = 6;
-            const double height = 1;
-            const unsigned int height_refine = 1;
-            const double depth = 1;
-            const unsigned int depth_refine = 1;
-            const double downforce_y = 1;
-            const double downforce_x = 3;
-            const double downforce_size = .3;
-
-            if (dim == 2)
-            {
-                GridGenerator::subdivided_hyper_rectangle(triangulation,
-                                                          {width_refine, height_refine},
-                                                          Point<dim>(0, 0),
-                                                          Point<dim>(width, height));
-
-                triangulation.refine_global(Input::refinements);
-
-                /*Set BCIDs   */
-                for (const auto &cell: dof_handler.active_cell_iterators()) {
-                    if(cell->is_locally_owned())
-                    {
-                        cell->set_active_fe_index(0);
-                        cell->set_material_id(MaterialIds::without_multiplier);
-                        for (unsigned int face_number = 0;
-                             face_number < GeometryInfo<dim>::faces_per_cell;
-                             ++face_number) {
-                            if (cell->face(face_number)->at_boundary()) {
-                                const auto center = cell->face(face_number)->center();
-
-                                if (std::fabs(center(1) - downforce_y) < 1e-12) {
-                                    if (std::fabs(center(0) - downforce_x) < downforce_size) {
-                                        cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
-                                    } else {
-                                        cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
-                                    }
-                                }
-                            }
-                        }
-                        for (unsigned int vertex_number = 0;
-                             vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                             ++vertex_number) {
-                            if (std::abs(cell->vertex(vertex_number)(0)) + std::abs(cell->vertex(vertex_number)(1)) <
-                                1e-10) {
-                                cell->set_active_fe_index(1);
-                                cell->set_material_id(MaterialIds::with_multiplier);
-                            }
-                        }
-                    }
-                }
-
-
-
-                dof_handler.distribute_dofs(fe_collection);
-
-                DoFRenumbering::component_wise(dof_handler, sub_blocks);
-            }
-            else if (dim == 3)
-            {
-                GridGenerator::subdivided_hyper_rectangle(triangulation,
-                                                          {width_refine, height_refine, depth_refine},
-                                                          Point<dim>(0, 0, 0),
-                                                          Point<dim>(width, height, depth));
-
-                triangulation.refine_global(Input::refinements);
-
-                for (const auto &cell: dof_handler.active_cell_iterators()) {
-                    if (cell->is_locally_owned())
-                    {
-                        cell->set_active_fe_index(0);
-                        cell->set_material_id(MaterialIds::without_multiplier);
-                        for (unsigned int face_number = 0;
-                             face_number < GeometryInfo<dim>::faces_per_cell;
-                             ++face_number) {
-                            if (cell->face(face_number)->at_boundary()) {
-                                const auto center = cell->face(face_number)->center();
-
-                                if (std::fabs(center(1) - downforce_y) < 1e-12) {
-                                    if (std::fabs(center(0) - downforce_x) < downforce_size) {
-                                        cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
-                                    } else {
-                                        cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
-                                    }
-                                }
-                            }
-                        }
-                        for (unsigned int vertex_number = 0;
-                             vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                             ++vertex_number) {
-                            if (std::abs(cell->vertex(vertex_number)(0)) + std::abs(cell->vertex(vertex_number)(1))
-                                + std::abs(cell->vertex(vertex_number)(2)) < 1e-10) {
-                                cell->set_active_fe_index(1);
-                                cell->set_material_id(MaterialIds::with_multiplier);
-                            }
-                        }
-                    }
-                }
-
-                dof_handler.distribute_dofs(fe_collection);
-
-                DoFRenumbering::component_wise(dof_handler, sub_blocks);
-
-            } else {
-                throw;
-            }
-        } else if (Input::geometry_base == GeometryOptions::l_shape) {
-            const double width = 2;
-            const unsigned int width_refine = 2;
-            const double height = 2;
-            const unsigned int height_refine = 2;
-            const double depth = 1;
-            const unsigned int depth_refine = 1;
-            const double downforce_x = 2;
-            const double downforce_y = 1;
-            const double downforce_z = .5;
-            const double downforce_size = .3;
-
-            if (dim == 2) {
-                GridGenerator::subdivided_hyper_L(triangulation,
-                                                  {width_refine, height_refine},
-                                                  Point<dim>(0, 0),
-                                                  Point<dim>(width, height),
-                                                  {-1, -1});
-
-                triangulation.refine_global(Input::refinements);
-
-                /*Set BCIDs   */
-                for (const auto &cell: dof_handler.active_cell_iterators()) {
-                    if (cell->is_locally_owned())
-                    {
-                        cell->set_active_fe_index(0);
-                        cell->set_material_id(MaterialIds::without_multiplier);
-                        for (unsigned int face_number = 0;
-                             face_number < GeometryInfo<dim>::faces_per_cell;
-                             ++face_number) {
-                            if (cell->face(face_number)->at_boundary()) {
-                                const auto center = cell->face(face_number)->center();
-
-                                if (std::fabs(center(0) - downforce_x) < 1e-12) {
-                                    if (std::fabs(center(1) - downforce_y) < downforce_size) {
-                                        cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
-                                    } else {
-                                        cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
-                                    }
-                                }
-                            }
-                        }
-                        for (unsigned int vertex_number = 0;
-                             vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                             ++vertex_number) {
-                            if (std::abs(cell->vertex(vertex_number)(0)) + std::abs(cell->vertex(vertex_number)(1)) <
-                                1e-10) {
-                                cell->set_active_fe_index(1);
-                                cell->set_material_id(MaterialIds::with_multiplier);
-                            }
-                        }
-                    }
-                }
-
-                dof_handler.distribute_dofs(fe_collection);
-
-                DoFRenumbering::component_wise(dof_handler, sub_blocks);
-
-            } else if (dim == 3) {
-                GridGenerator::subdivided_hyper_L(triangulation,
-                                                  {width_refine, height_refine, depth_refine},
-                                                  Point<dim>(0, 0, 0),
-                                                  Point<dim>(width, height, depth),
-                                                  {-1, -1, depth_refine});
-
-                triangulation.refine_global(Input::refinements);
-
-                /*Set BCIDs   */
-                for (const auto &cell: dof_handler.active_cell_iterators()) {
-                    if(cell->is_locally_owned())
-                    {
-                        cell->set_active_fe_index(0);
-                        cell->set_material_id(MaterialIds::without_multiplier);
-                        for (unsigned int face_number = 0;
-                             face_number < GeometryInfo<dim>::faces_per_cell;
-                             ++face_number) {
-                            if (cell->face(face_number)->at_boundary()) {
-                                const auto center = cell->face(face_number)->center();
-
-                                if (std::fabs(center(0) - downforce_x) < 1e-12) {
-                                    if (std::fabs(center(1) - downforce_y) < downforce_size) {
-                                        cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
-                                        if (std::fabs(center(2) - downforce_z) < downforce_size) {
-                                            cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
-                                        } else {
-                                            cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
-                                        }
-                                    } else {
-                                        cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
-                                    }
-                                }
-                            }
-                        }
-                        for (unsigned int vertex_number = 0;
-                             vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                             ++vertex_number) {
-                            if (std::abs(cell->vertex(vertex_number)(0)) + std::abs(cell->vertex(vertex_number)(1)) <
-                                1e-10) {
-                                cell->set_active_fe_index(1);
-                                cell->set_material_id(MaterialIds::with_multiplier);
-                            }
-                        }
-                    }
-                }
-
-                dof_handler.distribute_dofs(fe_collection);
-
-                DoFRenumbering::component_wise(dof_handler, sub_blocks);
-            } else {
-                throw;
-            }
-        } else {
-            throw;
-        }
-
-    }
-
-// The  bottom  corners  are  kept  in  place  in  the  y  direction  -  the  bottom  left  also  in  the  x direction.
-// Because deal.ii is formulated to enforce boundary conditions along regions of the boundary,
-// we do this to ensure these BCs are only enforced at points.
-    template<int dim>
-    void
-    KktSystem<dim>::setup_boundary_values() {
-        if (Input::geometry_base == GeometryOptions::mbb) {
-            if (dim == 2) {
-                for (const auto &cell: dof_handler.active_cell_iterators()) {
-                    if(cell->is_locally_owned())
-                    {
-                        for (unsigned int face_number = 0;
-                             face_number < GeometryInfo<dim>::faces_per_cell;
-                             ++face_number) {
-                            if (cell->face(face_number)->at_boundary()) {
-                                for (unsigned int vertex_number = 0;
-                                     vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                                     ++vertex_number) {
-                                    const auto vert = cell->vertex(vertex_number);
-                                    /*Find bottom left corner*/
-                                    if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
-                                            vert(1) - 0) < 1e-12) {
-
-                                        const unsigned int x_displacement =
-                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
-                                        const unsigned int y_displacement =
-                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
-                                        const unsigned int x_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
-                                        const unsigned int y_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
-                                        /*set bottom left BC*/
-                                        boundary_values[x_displacement] = 0;
-                                        boundary_values[y_displacement] = 0;
-                                        boundary_values[x_displacement_multiplier] = 0;
-                                        boundary_values[y_displacement_multiplier] = 0;
-                                    }
-                                    /*Find bottom right corner*/
-                                    if (std::fabs(vert(0) - 6) < 1e-12 && std::fabs(
-                                            vert(1) - 0) < 1e-12) {
-//                            const unsigned int x_displacement =
-//                                    cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
-                                        const unsigned int y_displacement =
-                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
-//                            const unsigned int x_displacement_multiplier =
-//                                    cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
-                                        const unsigned int y_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
-//                            boundary_values[x_displacement] = 0;
-                                        boundary_values[y_displacement] = 0;
-//                            boundary_values[x_displacement_multiplier] = 0;
-                                        boundary_values[y_displacement_multiplier] = 0;
-                                    }
-                                }
-                            }
-                        }
-                    }
-
-                }
-            } else if (dim == 3) {
-                for (const auto &cell: dof_handler.active_cell_iterators()) {
-                    if(cell->is_locally_owned())
-                    {
-                        for (unsigned int face_number = 0;
-                             face_number < GeometryInfo<dim>::faces_per_cell;
-                             ++face_number) {
-                            if (cell->face(face_number)->at_boundary()) {
-                                for (unsigned int vertex_number = 0;
-                                     vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                                     ++vertex_number) {
-                                    const auto vert = cell->vertex(vertex_number);
-                                    /*Find bottom left corner*/
-                                    if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
-                                            vert(1) - 0) < 1e-12 && ((std::fabs(
-                                            vert(2) - 0) < 1e-12) || (std::fabs(
-                                            vert(2) - 1) < 1e-12))) {
-
-
-                                        const unsigned int x_displacement =
-                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
-                                        const unsigned int y_displacement =
-                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
-                                        const unsigned int z_displacement =
-                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
-                                        const unsigned int x_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
-                                        const unsigned int y_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 4, cell->active_fe_index());
-                                        const unsigned int z_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 5, cell->active_fe_index());
-
-                                        boundary_values[x_displacement] = 0;
-                                        boundary_values[y_displacement] = 0;
-                                        boundary_values[z_displacement] = 0;
-                                        boundary_values[x_displacement_multiplier] = 0;
-                                        boundary_values[y_displacement_multiplier] = 0;
-                                        boundary_values[z_displacement_multiplier] = 0;
-                                    }
-                                    /*Find bottom right corner*/
-                                    if (std::fabs(vert(0) - 6) < 1e-12 && std::fabs(
-                                            vert(1) - 0) < 1e-12 && ((std::fabs(
-                                            vert(2) - 0) < 1e-12) || (std::fabs(
-                                            vert(2) - 1) < 1e-12))) {
-//                              const unsigned int x_displacement =
-//                                    cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
-                                        const unsigned int y_displacement =
-                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
-                                        const unsigned int z_displacement =
-                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
-//                              const unsigned int x_displacement_multiplier =
-//                                    cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
-                                        const unsigned int y_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 4, cell->active_fe_index());
-                                        const unsigned int z_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 5, cell->active_fe_index());
-//                              boundary_values[x_displacement] = 0;
-                                        boundary_values[y_displacement] = 0;
-                                        boundary_values[z_displacement] = 0;
-//                              boundary_values[x_displacement_multiplier] = 0;
-                                        boundary_values[y_displacement_multiplier] = 0;
-                                        boundary_values[z_displacement_multiplier] = 0;
-                                    }
-                                }
-                            }
-                        }
-                    }
-                }
-            } else {
-                throw;
-            }
-        } else if (Input::geometry_base == GeometryOptions::l_shape) {
-            if (dim == 2) {
-                for (const auto &cell: dof_handler.active_cell_iterators()) {
-                    if(cell->is_locally_owned())
-                    {
-                        for (unsigned int face_number = 0;
-                             face_number < GeometryInfo<dim>::faces_per_cell;
-                             ++face_number) {
-                            if (cell->face(face_number)->at_boundary()) {
-                                for (unsigned int vertex_number = 0;
-                                     vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                                     ++vertex_number) {
-                                    const auto vert = cell->vertex(vertex_number);
-                                    /*Find top left corner*/
-                                    if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
-                                            vert(1) - 2) < 1e-12) {
-
-                                        const unsigned int x_displacement =
-                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
-                                        const unsigned int y_displacement =
-                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
-                                        const unsigned int x_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
-                                        const unsigned int y_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
-                                        /*set bottom left BC*/
-                                        boundary_values[x_displacement] = 0;
-                                        boundary_values[y_displacement] = 0;
-                                        boundary_values[x_displacement_multiplier] = 0;
-                                        boundary_values[y_displacement_multiplier] = 0;
-                                    }
-                                    /*Find top right corner*/
-                                    if (std::fabs(vert(0) - 1) < 1e-12 && std::fabs(
-                                            vert(1) - 2) < 1e-12) {
-                                        const unsigned int x_displacement =
-                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
-                                        const unsigned int y_displacement =
-                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
-                                        const unsigned int x_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
-                                        const unsigned int y_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
-                                        boundary_values[x_displacement] = 0;
-                                        boundary_values[y_displacement] = 0;
-                                        boundary_values[x_displacement_multiplier] = 0;
-                                        boundary_values[y_displacement_multiplier] = 0;
-                                    }
-                                }
-                            }
-                        }
-                    }
-                }
-            } else if (dim == 3) {
-                for (const auto &cell: dof_handler.active_cell_iterators()) {
-                    if(cell->is_locally_owned())
-                    {
-                        for (unsigned int face_number = 0;
-                             face_number < GeometryInfo<dim>::faces_per_cell;
-                             ++face_number) {
-                            if (cell->face(face_number)->at_boundary()) {
-                                for (unsigned int vertex_number = 0;
-                                     vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                                     ++vertex_number) {
-                                    const auto vert = cell->vertex(vertex_number);
-                                    /*Find bottom left corner*/
-                                    if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
-                                            vert(1) - 2) < 1e-12 && ((std::fabs(
-                                            vert(2) - 0) < 1e-12) || (std::fabs(
-                                            vert(2) - 1) < 1e-12))) {
-
-
-                                        const unsigned int x_displacement =
-                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
-                                        const unsigned int y_displacement =
-                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
-                                        const unsigned int z_displacement =
-                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
-                                        const unsigned int x_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
-                                        const unsigned int y_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 4, cell->active_fe_index());
-                                        const unsigned int z_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 5, cell->active_fe_index());
-
-                                        boundary_values[x_displacement] = 0;
-                                        boundary_values[y_displacement] = 0;
-                                        boundary_values[z_displacement] = 0;
-                                        boundary_values[x_displacement_multiplier] = 0;
-                                        boundary_values[y_displacement_multiplier] = 0;
-                                        boundary_values[z_displacement_multiplier] = 0;
-                                    }
-                                    /*Find bottom right corner*/
-                                    if (std::fabs(vert(0) - 1) < 1e-12 && std::fabs(
-                                            vert(1) - 2) < 1e-12 && ((std::fabs(
-                                            vert(2) - 0) < 1e-12) || (std::fabs(
-                                            vert(2) - 1) < 1e-12))) {
-                                        const unsigned int x_displacement =
-                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
-                                        const unsigned int y_displacement =
-                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
-                                        const unsigned int z_displacement =
-                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
-                                        const unsigned int x_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
-                                        const unsigned int y_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 4, cell->active_fe_index());
-                                        const unsigned int z_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 5, cell->active_fe_index());
-                                        boundary_values[x_displacement] = 0;
-                                        boundary_values[y_displacement] = 0;
-                                        boundary_values[z_displacement] = 0;
-                                        boundary_values[x_displacement_multiplier] = 0;
-                                        boundary_values[y_displacement_multiplier] = 0;
-                                        boundary_values[z_displacement_multiplier] = 0;
-                                    }
-                                }
-                            }
-                        }
-                    }
-                }
-            } else {
-                throw;
-            }
-        }
-
-
-    }
-
-
-    //This makes a giant 10-by-10 block matrix, and also sets up the necessary block vectors.  The
-    // sparsity pattern for this matrix includes the sparsity pattern for the filter matrix. It also initializes
-    // any block vectors we will use.
-    template<int dim>
-    void
-    KktSystem<dim>::setup_block_system() {
-
-        //MAKE n_u and n_P
-
-        /*Setup 10 by 10 block matrix*/
-        std::vector<unsigned int> block_component(10, 2);
-
-        block_component[0] = 0;
-        block_component[5] = 1;
-
-        const std::vector<types::global_dof_index> dofs_per_block =
-                DoFTools::count_dofs_per_fe_block(dof_handler, block_component);
-        const unsigned int n_p = dofs_per_block[0];
-        const unsigned int n_u = dofs_per_block[1];
-
-        std::cout << "n_p:  " << n_p << "   n_u:  " << n_u << std::endl;
-
-        IndexSet locally_owned_dofs = dof_handler.locally_owned_dofs();
-        IndexSet locally_relevant_dofs;
-        DoFTools::extract_locally_relevant_dofs(dof_handler, locally_relevant_dofs);
-
-        dsp.reinit(10, 10);
-        owned_partitioning.resize(10);
-        owned_partitioning[0] = dof_handler.locally_owned_dofs().get_view(0, n_p);
-        owned_partitioning[1] = dof_handler.locally_owned_dofs().get_view(n_p, 2 * n_p);
-        owned_partitioning[2] = dof_handler.locally_owned_dofs().get_view(2 * n_p, 3 * n_p);
-        owned_partitioning[3] = dof_handler.locally_owned_dofs().get_view(3 * n_p, 4 * n_p);
-        owned_partitioning[4] = dof_handler.locally_owned_dofs().get_view(4 * n_p, 5 * n_p);
-        owned_partitioning[5] = dof_handler.locally_owned_dofs().get_view(5 * n_p, 5 * n_p + n_u);
-        owned_partitioning[6] = dof_handler.locally_owned_dofs().get_view(5 * n_p + n_u, 5 * n_p + 2 * n_u);
-        owned_partitioning[7] = dof_handler.locally_owned_dofs().get_view(5 * n_p + 2 * n_u, 6 * n_p + 2 * n_u);
-        owned_partitioning[8] = dof_handler.locally_owned_dofs().get_view(6 * n_p + 2 * n_u, 7 * n_p + 2 * n_u);
-        owned_partitioning[9] = dof_handler.locally_owned_dofs().get_view(7 * n_p + 2 * n_u, 7 * n_p + 2 * n_u + 1);
-        relevant_partitioning.resize(10);
-        relevant_partitioning[0] = locally_relevant_dofs.get_view(0, n_p);
-        relevant_partitioning[1] = locally_relevant_dofs.get_view(n_p, 2 * n_p);
-        relevant_partitioning[2] = locally_relevant_dofs.get_view(2 * n_p, 3 * n_p);
-        relevant_partitioning[3] = locally_relevant_dofs.get_view(3 * n_p, 4 * n_p);
-        relevant_partitioning[4] = locally_relevant_dofs.get_view(4 * n_p, 5 * n_p);
-        relevant_partitioning[5] = locally_relevant_dofs.get_view(5 * n_p, 5 * n_p + n_u);
-        relevant_partitioning[6] = locally_relevant_dofs.get_view(5 * n_p + n_u, 5 * n_p + 2 * n_u);
-        relevant_partitioning[7] = locally_relevant_dofs.get_view(5 * n_p + 2 * n_u, 6 * n_p + 2 * n_u);
-        relevant_partitioning[8] = locally_relevant_dofs.get_view(6 * n_p + 2 * n_u, 7 * n_p + 2 * n_u);
-        relevant_partitioning[9] = locally_relevant_dofs.get_view(7 * n_p + 2 * n_u, 7 * n_p + 2 * n_u + 1);
-
-        const std::vector<unsigned int> block_sizes = {n_p, n_p, n_p, n_p, n_p, n_u, n_u, n_p, n_p, 1};
-
-        for (unsigned int k = 0; k < 10; k++) {
-            for (unsigned int j = 0; j < 10; j++) {
-                dsp.block(j, k).reinit(block_sizes[j], block_sizes[k]);
-            }
-        }
-        dsp.collect_sizes();
-        Table<2, DoFTools::Coupling> coupling(2 * dim + 8, 2 * dim + 8);
-//Coupling for density
-            coupling[SolutionComponents::density<dim>][SolutionComponents::density<dim>] = DoFTools::always;
-
-            for (unsigned int i = 0; i < dim; i++) {
-                coupling[SolutionComponents::density<dim>][SolutionComponents::displacement<dim> +
-                                                           i] = DoFTools::always;
-                coupling[SolutionComponents::displacement<dim> +
-                         i][SolutionComponents::density<dim>] = DoFTools::always;
-            }
-
-            coupling[SolutionComponents::density<dim>][SolutionComponents::unfiltered_density_multiplier<dim>] = DoFTools::always;
-            coupling[SolutionComponents::unfiltered_density_multiplier<dim>][SolutionComponents::density<dim>] = DoFTools::always;
-
-            for (unsigned int i = 0; i < dim; i++) {
-                coupling[SolutionComponents::density<dim>][SolutionComponents::displacement_multiplier<dim> +
-                                                           i] = DoFTools::always;
-                coupling[SolutionComponents::displacement_multiplier<dim> +
-                         i][SolutionComponents::density<dim>] = DoFTools::always;
-            }
-
-//Coupling for displacement
-            for (unsigned int i = 0; i < dim; i++) {
-
-                for (unsigned int k = 0; k < dim; k++) {
-                    coupling[SolutionComponents::displacement<dim> + i][
-                            SolutionComponents::displacement_multiplier<dim> +
-                            k] = DoFTools::always;
-                    coupling[SolutionComponents::displacement_multiplier<dim> + k][
-                            SolutionComponents::displacement<dim> +
-                            i] = DoFTools::always;
-                }
-            }
-
-// coupling for unfiltered density
-            coupling[SolutionComponents::unfiltered_density<dim>][SolutionComponents::density_lower_slack_multiplier<dim>] = DoFTools::always;
-            coupling[SolutionComponents::density_lower_slack_multiplier<dim>][SolutionComponents::unfiltered_density<dim>] = DoFTools::always;
-
-            coupling[SolutionComponents::unfiltered_density<dim>][SolutionComponents::density_upper_slack_multiplier<dim>] = DoFTools::always;
-            coupling[SolutionComponents::density_upper_slack_multiplier<dim>][SolutionComponents::unfiltered_density<dim>] = DoFTools::always;
-
-            coupling[SolutionComponents::unfiltered_density<dim>][SolutionComponents::unfiltered_density_multiplier<dim>] = DoFTools::always;
-            coupling[SolutionComponents::unfiltered_density_multiplier<dim>][SolutionComponents::unfiltered_density<dim>] = DoFTools::always;
-
-
-
-
-//        Coupling for lower slack
-            coupling[SolutionComponents::density_lower_slack<dim>][SolutionComponents::density_lower_slack<dim>] = DoFTools::always;
-
-            coupling[SolutionComponents::density_lower_slack<dim>][SolutionComponents::density_lower_slack_multiplier<dim>] = DoFTools::always;
-            coupling[SolutionComponents::density_lower_slack_multiplier<dim>][SolutionComponents::density_lower_slack<dim>] = DoFTools::always;
-
-//
-            coupling[SolutionComponents::density_upper_slack<dim>][SolutionComponents::density_upper_slack<dim>] = DoFTools::always;
-            coupling[SolutionComponents::density_upper_slack<dim>][SolutionComponents::density_upper_slack_multiplier<dim>] = DoFTools::always;
-            coupling[SolutionComponents::density_upper_slack_multiplier<dim>][SolutionComponents::density_upper_slack<dim>] = DoFTools::always;
-
-            coupling[SolutionComponents::density_upper_slack_multiplier<dim>][SolutionComponents::density_upper_slack_multiplier<dim>] = DoFTools::always;
-            constraints.reinit(locally_relevant_dofs);
-            constraints.clear();
-            DoFTools::make_hanging_node_constraints(dof_handler,constraints);
-            constraints.close();
-
-            system_matrix.clear();
-
-//            DoFTools::make_sparsity_pattern(dof_handler, coupling, dsp, constraints, false);
-            DoFTools::make_sparsity_pattern(dof_handler, dsp, constraints, false);
-            SparsityTools::distribute_sparsity_pattern(dsp, dof_handler.locally_owned_dofs(), mpi_communicator,
-                                                       locally_relevant_dofs);
-            //adds the row into the sparsity pattern for the total volume constraint
-            for (const auto &cell: dof_handler.active_cell_iterators())
-            {
-                if (cell->is_locally_owned())
-                {
-                    std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
-                    cell->get_dof_indices(i);
-                    dsp.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier).add(i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)], 0);
-                    dsp.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).add(0, i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)]);
-                }
-
-            }
-
-            /*This finds neighbors whose values would be relevant, and adds them to the sparsity pattern of the matrix*/
-            setup_filter_matrix();
-            for (const auto &cell : dof_handler.active_cell_iterators()) {
-                if (cell->is_locally_owned())
-                {
-                    std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
-                    cell->get_dof_indices(i);
-                    for (const auto &neighbor_cell : density_filter.find_relevant_neighbors(cell)) {
-                        std::vector<types::global_dof_index> j(neighbor_cell->get_fe().n_dofs_per_cell());
-                        neighbor_cell->get_dof_indices(j);
-
-                        dsp.block(SolutionBlocks::unfiltered_density_multiplier,
-                                  SolutionBlocks::unfiltered_density).add(i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)], j[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)]);
-                        dsp.block(SolutionBlocks::unfiltered_density,
-                                  SolutionBlocks::unfiltered_density_multiplier).add(j[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)], i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)]);
-                    }
-                }
-            }
-
-
-            SparsityTools::distribute_sparsity_pattern(
-                    dsp,
-                    Utilities::MPI::all_gather(mpi_communicator,
-                                               dof_handler.locally_owned_dofs()),
-                    mpi_communicator,
-                    locally_relevant_dofs);
-            DoFTools::extract_locally_relevant_dofs(dof_handler, locally_relevant_dofs);
-            system_matrix.reinit(owned_partitioning, dsp, mpi_communicator);
-
-            locally_relevant_solution.reinit(owned_partitioning, relevant_partitioning, mpi_communicator);
-            distributed_solution.reinit(owned_partitioning, mpi_communicator);
-            system_rhs.reinit(owned_partitioning, mpi_communicator);
-
-            locally_relevant_solution.collect_sizes();
-            distributed_solution.collect_sizes();
-            system_rhs.collect_sizes();
-            system_matrix.collect_sizes();
-    }
-
-    ///This  is  where  the  magic  happens.   The  equations  describing  the newtons method for finding 0s in the KKT conditions are implemented here.
-
-
-    template<int dim>
-    void
-    KktSystem<dim>::assemble_block_system(const LA::MPI::BlockVector &distributed_state, const double barrier_size) {
-        /*Remove any values from old iterations*/
-
-        LA::MPI::BlockVector relevant_state(owned_partitioning, relevant_partitioning, mpi_communicator);
-        relevant_state = distributed_state;
-
-        system_matrix.reinit(owned_partitioning, dsp, mpi_communicator);
-        locally_relevant_solution = 0;
-        system_rhs = 0;
-
-        QGauss<dim> nine_quadrature(fe_nine.degree + 1);
-        QGauss<dim> ten_quadrature(fe_ten.degree + 1);
-
-        hp::QCollection<dim> q_collection;
-        q_collection.push_back(nine_quadrature);
-        q_collection.push_back(ten_quadrature);
-
-        hp::FEValues<dim> hp_fe_values(fe_collection,
-                                       q_collection,
-                                       update_values | update_quadrature_points |
-                                       update_JxW_values | update_gradients);
-
-        QGauss<dim - 1> common_face_quadrature(fe_ten.degree + 1);
-
-        FEFaceValues<dim> fe_nine_face_values(fe_nine,
-                                              common_face_quadrature,
-                                              update_JxW_values |
-                                              update_gradients | update_values);
-        FEFaceValues<dim> fe_ten_face_values(fe_ten,
-                                             common_face_quadrature,
-                                             update_normal_vectors |
-                                             update_values);
-
-        FullMatrix<double> cell_matrix;
-        Vector<double> cell_rhs;
-        std::vector<types::global_dof_index> local_dof_indices;
-
-        const FEValuesExtractors::Scalar densities(SolutionComponents::density<dim>);
-        const FEValuesExtractors::Vector displacements(SolutionComponents::displacement<dim>);
-        const FEValuesExtractors::Scalar unfiltered_densities(SolutionComponents::unfiltered_density<dim>);
-        const FEValuesExtractors::Vector displacement_multipliers(SolutionComponents::displacement_multiplier<dim>);
-        const FEValuesExtractors::Scalar unfiltered_density_multipliers(
-                SolutionComponents::unfiltered_density_multiplier<dim>);
-        const FEValuesExtractors::Scalar density_lower_slacks(SolutionComponents::density_lower_slack<dim>);
-        const FEValuesExtractors::Scalar density_lower_slack_multipliers(
-                SolutionComponents::density_lower_slack_multiplier<dim>);
-        const FEValuesExtractors::Scalar density_upper_slacks(SolutionComponents::density_upper_slack<dim>);
-        const FEValuesExtractors::Scalar density_upper_slack_multipliers(
-                SolutionComponents::density_upper_slack_multiplier<dim>);
-        const FEValuesExtractors::Scalar total_volume_multiplier(
-                SolutionComponents::total_volume_multiplier<dim>);
-
-        const Functions::ConstantFunction<dim> lambda(1.), mu(1.);
-
-        distributed_solution = distributed_state;
-        LA::MPI::BlockVector filtered_unfiltered_density_solution = distributed_solution;
-        LA::MPI::BlockVector filter_adjoint_unfiltered_density_multiplier_solution = distributed_solution;
-        filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = 0;
-        filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier) = 0;
-        auto op_f = linear_operator<LA::MPI::Vector>(density_filter.filter_matrix);
-        filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = op_f * distributed_solution.block(SolutionBlocks::unfiltered_density);
-        filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier) = transpose_operator(op_f) *  distributed_solution.block(SolutionBlocks::unfiltered_density_multiplier);
-
-        LA::MPI::BlockVector relevant_filtered_unfiltered_density_solution = locally_relevant_solution;
-        LA::MPI::BlockVector relevant_filter_adjoint_unfiltered_density_multiplier_solution = locally_relevant_solution;
-        relevant_filtered_unfiltered_density_solution =filtered_unfiltered_density_solution;
-        relevant_filter_adjoint_unfiltered_density_multiplier_solution = filter_adjoint_unfiltered_density_multiplier_solution;
-        for (const auto &cell: dof_handler.active_cell_iterators()) {
-            if(cell->is_locally_owned())
-            {
-                hp_fe_values.reinit(cell);
-                const FEValues<dim> &fe_values = hp_fe_values.get_present_fe_values();
-                cell_matrix.reinit(cell->get_fe().n_dofs_per_cell(),
-                                   cell->get_fe().n_dofs_per_cell());
-                cell_rhs.reinit(cell->get_fe().n_dofs_per_cell());
-
-                const unsigned int n_q_points = fe_values.n_quadrature_points;
-
-                std::vector<double> old_density_values(n_q_points);
-                std::vector<Tensor<1, dim>> old_displacement_values(n_q_points);
-                std::vector<double> old_displacement_divs(n_q_points);
-                std::vector<SymmetricTensor<2, dim>> old_displacement_symmgrads(
-                        n_q_points);
-                std::vector<Tensor<1, dim>> old_displacement_multiplier_values(
-                        n_q_points);
-                std::vector<double> old_displacement_multiplier_divs(n_q_points);
-                std::vector<SymmetricTensor<2, dim>> old_displacement_multiplier_symmgrads(
-                        n_q_points);
-                std::vector<double> old_lower_slack_multiplier_values(n_q_points);
-                std::vector<double> old_upper_slack_multiplier_values(n_q_points);
-                std::vector<double> old_lower_slack_values(n_q_points);
-                std::vector<double> old_upper_slack_values(n_q_points);
-                std::vector<double> old_unfiltered_density_values(n_q_points);
-                std::vector<double> old_unfiltered_density_multiplier_values(n_q_points);
-                std::vector<double> filtered_unfiltered_density_values(n_q_points);
-                std::vector<double> filter_adjoint_unfiltered_density_multiplier_values(n_q_points);
-                std::vector<double> lambda_values(n_q_points);
-                std::vector<double> mu_values(n_q_points);
-
-                const unsigned int dofs_per_cell = cell->get_fe().n_dofs_per_cell();
-
-                cell_matrix = 0;
-                cell_rhs = 0;
-                local_dof_indices.resize(cell->get_fe().n_dofs_per_cell());
-                cell->get_dof_indices(local_dof_indices);
-
-                lambda.value_list(fe_values.get_quadrature_points(), lambda_values);
-                mu.value_list(fe_values.get_quadrature_points(), mu_values);
-
-                fe_values[densities].get_function_values(relevant_state,
-                                                         old_density_values);
-                fe_values[displacements].get_function_values(relevant_state,
-                                                             old_displacement_values);
-                fe_values[displacements].get_function_divergences(relevant_state,
-                                                                  old_displacement_divs);
-                fe_values[displacements].get_function_symmetric_gradients(
-                        relevant_state, old_displacement_symmgrads);
-                fe_values[displacement_multipliers].get_function_values(
-                        relevant_state, old_displacement_multiplier_values);
-                fe_values[displacement_multipliers].get_function_divergences(
-                        relevant_state, old_displacement_multiplier_divs);
-                fe_values[displacement_multipliers].get_function_symmetric_gradients(
-                        relevant_state, old_displacement_multiplier_symmgrads);
-                fe_values[density_lower_slacks].get_function_values(
-                        relevant_state, old_lower_slack_values);
-                fe_values[density_lower_slack_multipliers].get_function_values(
-                        relevant_state, old_lower_slack_multiplier_values);
-                fe_values[density_upper_slacks].get_function_values(
-                        relevant_state, old_upper_slack_values);
-                fe_values[density_upper_slack_multipliers].get_function_values(
-                        relevant_state, old_upper_slack_multiplier_values);
-                fe_values[unfiltered_densities].get_function_values(
-                        relevant_state, old_unfiltered_density_values);
-                fe_values[unfiltered_density_multipliers].get_function_values(
-                        relevant_state, old_unfiltered_density_multiplier_values);
-                fe_values[unfiltered_densities].get_function_values(
-                        relevant_filtered_unfiltered_density_solution, filtered_unfiltered_density_values);
-                fe_values[unfiltered_density_multipliers].get_function_values(
-                        relevant_filter_adjoint_unfiltered_density_multiplier_solution,
-                        filter_adjoint_unfiltered_density_multiplier_values);
-
-                Tensor<1, dim> traction;
-                traction[1] = -1;
-
-                for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
-
-                    for (unsigned int i = 0; i < dofs_per_cell; ++i) {
-                        const SymmetricTensor<2, dim> displacement_phi_i_symmgrad =
-                                fe_values[displacements].symmetric_gradient(i, q_point);
-                        const double displacement_phi_i_div =
-                                fe_values[displacements].divergence(i, q_point);
-
-                        const SymmetricTensor<2, dim> displacement_multiplier_phi_i_symmgrad =
-                                fe_values[displacement_multipliers].symmetric_gradient(i,
-                                                                                       q_point);
-                        const double displacement_multiplier_phi_i_div =
-                                fe_values[displacement_multipliers].divergence(i,
-                                                                               q_point);
-
-
-                        const double density_phi_i = fe_values[densities].value(i,
-                                                                                q_point);
-                        const double unfiltered_density_phi_i = fe_values[unfiltered_densities].value(i,
-                                                                                                      q_point);
-                        const double unfiltered_density_multiplier_phi_i = fe_values[unfiltered_density_multipliers].value(
-                                i, q_point);
-
-                        const double lower_slack_multiplier_phi_i =
-                                fe_values[density_lower_slack_multipliers].value(i,
-                                                                                 q_point);
-
-                        const double lower_slack_phi_i =
-                                fe_values[density_lower_slacks].value(i, q_point);
-
-                        const double upper_slack_phi_i =
-                                fe_values[density_upper_slacks].value(i, q_point);
-
-                        const double upper_slack_multiplier_phi_i =
-                                fe_values[density_upper_slack_multipliers].value(i,
-                                                                                 q_point);
-
-
-                        for (unsigned int j = 0; j < dofs_per_cell; ++j) {
-                            const SymmetricTensor<2, dim> displacement_phi_j_symmgrad =
-                                    fe_values[displacements].symmetric_gradient(j,
-                                                                                q_point);
-                            const double displacement_phi_j_div =
-                                    fe_values[displacements].divergence(j, q_point);
-
-                            const SymmetricTensor<2, dim> displacement_multiplier_phi_j_symmgrad =
-                                    fe_values[displacement_multipliers].symmetric_gradient(
-                                            j, q_point);
-                            const double displacement_multiplier_phi_j_div =
-                                    fe_values[displacement_multipliers].divergence(j,
-                                                                                   q_point);
-
-                            const double density_phi_j = fe_values[densities].value(
-                                    j, q_point);
-
-                            const double unfiltered_density_phi_j = fe_values[unfiltered_densities].value(j,
-                                                                                                          q_point);
-                            const double unfiltered_density_multiplier_phi_j = fe_values[unfiltered_density_multipliers].value(
-                                    j, q_point);
-
-
-                            const double lower_slack_phi_j =
-                                    fe_values[density_lower_slacks].value(j, q_point);
-
-                            const double upper_slack_phi_j =
-                                    fe_values[density_upper_slacks].value(j, q_point);
-
-                            const double lower_slack_multiplier_phi_j =
-                                    fe_values[density_lower_slack_multipliers].value(j,
-                                                                                     q_point);
-
-                            const double upper_slack_multiplier_phi_j =
-                                    fe_values[density_upper_slack_multipliers].value(j,
-                                                                                     q_point);
-
-                            //Equation 0
-                            cell_matrix(i, j) +=
-                                    fe_values.JxW(q_point) *
-                                    (
-                                            -density_phi_i * unfiltered_density_multiplier_phi_j
-
-                                            - density_penalty_exponent * (density_penalty_exponent - 1)
-                                              * std::pow(
-                                                    old_density_values[q_point],
-                                                    density_penalty_exponent - 2)
-                                              * density_phi_i
-                                              * density_phi_j
-                                              * (old_displacement_multiplier_divs[q_point] * old_displacement_divs[q_point]
-                                                 * lambda_values[q_point]
-                                                 + 2 * mu_values[q_point]
-                                                   * (old_displacement_symmgrads[q_point] *
-                                                      old_displacement_multiplier_symmgrads[q_point]))
-
-                                            - density_penalty_exponent * std::pow(
-                                                    old_density_values[q_point],
-                                                    density_penalty_exponent - 1)
-                                              * density_phi_i
-                                              * (displacement_multiplier_phi_j_div * old_displacement_divs[q_point]
-                                                 * lambda_values[q_point]
-                                                 + 2 * mu_values[q_point]
-                                                   *
-                                                   (old_displacement_symmgrads[q_point] *
-                                                    displacement_multiplier_phi_j_symmgrad))
-
-                                            - density_penalty_exponent * std::pow(
-                                                    old_density_values[q_point],
-                                                    density_penalty_exponent - 1)
-                                              * density_phi_i
-                                              * (displacement_phi_j_div * old_displacement_multiplier_divs[q_point]
-                                                 * lambda_values[q_point]
-                                                 + 2 * mu_values[q_point]
-                                                   * (old_displacement_multiplier_symmgrads[q_point] *
-                                                      displacement_phi_j_symmgrad)));
-                            //Equation 1
-
-                            cell_matrix(i, j) +=
-                                    fe_values.JxW(q_point) * (
-                                            -density_penalty_exponent * std::pow(
-                                                    old_density_values[q_point],
-                                                    density_penalty_exponent - 1)
-                                            * density_phi_j
-                                            * (old_displacement_multiplier_divs[q_point] * displacement_phi_i_div
-                                               * lambda_values[q_point]
-                                               + 2 * mu_values[q_point]
-                                                 * (old_displacement_multiplier_symmgrads[q_point] *
-                                                    displacement_phi_i_symmgrad))
-
-                                            - std::pow(old_density_values[q_point],
-                                                       density_penalty_exponent)
-                                              * (displacement_multiplier_phi_j_div * displacement_phi_i_div
-                                                 * lambda_values[q_point]
-                                                 + 2 * mu_values[q_point]
-                                                   * (displacement_multiplier_phi_j_symmgrad * displacement_phi_i_symmgrad))
-
-                                    );
-
-                            //Equation 2 has to do with the filter, which is calculated elsewhere.
-                            cell_matrix(i, j) +=
-                                    fe_values.JxW(q_point) * (
-                                            -1 * unfiltered_density_phi_i * lower_slack_multiplier_phi_j
-                                            + unfiltered_density_phi_i * upper_slack_multiplier_phi_j);
-
-                            //Equation 3 - Primal Feasibility
-
-                            cell_matrix(i, j) +=
-                                    fe_values.JxW(q_point) * (
-
-                                            -1 * density_penalty_exponent * std::pow(
-                                                    old_density_values[q_point],
-                                                    density_penalty_exponent - 1)
-                                            * density_phi_j
-                                            * (old_displacement_divs[q_point] * displacement_multiplier_phi_i_div
-                                               * lambda_values[q_point]
-                                               + 2 * mu_values[q_point]
-                                                 * (old_displacement_symmgrads[q_point] *
-                                                    displacement_multiplier_phi_i_symmgrad))
-
-                                            + -1 * std::pow(old_density_values[q_point],
-                                                            density_penalty_exponent)
-                                              * (displacement_phi_j_div * displacement_multiplier_phi_i_div
-                                                 * lambda_values[q_point]
-                                                 + 2 * mu_values[q_point]
-                                                   *
-                                                   (displacement_phi_j_symmgrad * displacement_multiplier_phi_i_symmgrad)));
-
-                            //Equation 4 - more primal feasibility
-                            cell_matrix(i, j) +=
-                                    -1 * fe_values.JxW(q_point) * lower_slack_multiplier_phi_i *
-                                    (unfiltered_density_phi_j - lower_slack_phi_j);
-
-                            //Equation 5 - more primal feasibility
-                            cell_matrix(i, j) +=
-                                    -1 * fe_values.JxW(q_point) * upper_slack_multiplier_phi_i * (
-                                            -1 * unfiltered_density_phi_j - upper_slack_phi_j);
-
-                            //Equation 6 - more primal feasibility - part with filter added later
-                            cell_matrix(i, j) +=
-                                    -1 * fe_values.JxW(q_point) * unfiltered_density_multiplier_phi_i * (
-                                            density_phi_j);
-
-                            //Equation 7 - complementary slackness
-                            cell_matrix(i, j) += fe_values.JxW(q_point) *
-                                                 (lower_slack_phi_i * lower_slack_multiplier_phi_j
-                                                  + lower_slack_phi_i * lower_slack_phi_j *
-                                                    old_lower_slack_multiplier_values[q_point] /
-                                                    old_lower_slack_values[q_point]);
-                            //Equation 8 - complementary slackness
-                            cell_matrix(i, j) += fe_values.JxW(q_point) *
-                                                 (upper_slack_phi_i * upper_slack_multiplier_phi_j
-                                                  + upper_slack_phi_i * upper_slack_phi_j *
-                                                    old_upper_slack_multiplier_values[q_point] /
-                                                    old_upper_slack_values[q_point]);
-                        }
-
-                    }
-                }
-
-
-                MatrixTools::local_apply_boundary_values(boundary_values, local_dof_indices,
-                                                         cell_matrix, cell_rhs, true);
-
-
-                constraints.distribute_local_to_global(
-                        cell_matrix, cell_rhs, local_dof_indices, system_matrix, system_rhs);
-
-            }
-
-        }
-        std::cout << "compress 1" << std::endl;
-        system_matrix.compress(VectorOperation::add);
-        system_rhs = calculate_rhs(distributed_state, barrier_size);
-
-        for (const auto &cell: dof_handler.active_cell_iterators()) {
-            if(cell->is_locally_owned())
-            {
-                std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
-                cell->get_dof_indices(i);
-
-                const types::global_dof_index dof_index_lower_slack =
-                        i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)];
-                typename LA::MPI::SparseMatrix::iterator iter = density_filter.filter_matrix.begin(
-                        i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)]);
-                for (; iter != density_filter.filter_matrix.end(i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)]); iter++) {
-                    unsigned int j = iter->column();
-                    double value = iter->value() * cell->measure();
-
-                    system_matrix.block(SolutionBlocks::unfiltered_density_multiplier,
-                                        SolutionBlocks::unfiltered_density).add(i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)], j, value);
-                    system_matrix.block(SolutionBlocks::unfiltered_density,
-                                        SolutionBlocks::unfiltered_density_multiplier).add(j, i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)], value);
-                }
-
-                system_matrix.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).add(0, i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)],
-                                                                                                          cell->measure());
-
-                system_matrix.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier).add(i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)], 0,
-                                                                                                          cell->measure());
-            }
-        }
-        std::cout << "compress 2" << std::endl;
-        system_matrix.compress(VectorOperation::add);
-
-
-        std::cout << "assembled" << std::endl;
-
-    }
-
-    template<int dim>
-    double
-    KktSystem<dim>::calculate_objective_value(const LA::MPI::BlockVector &distributed_state) const {
-        /*Remove any values from old iterations*/
-
-        locally_relevant_solution = distributed_state;
-
-
-        QGauss<dim> nine_quadrature(fe_nine.degree + 1);
-        QGauss<dim> ten_quadrature(fe_ten.degree + 1);
-
-        hp::QCollection<dim> q_collection;
-        q_collection.push_back(nine_quadrature);
-        q_collection.push_back(ten_quadrature);
-
-        hp::FEValues<dim> hp_fe_values(fe_collection,
-                                       q_collection,
-                                       update_values | update_quadrature_points |
-                                       update_JxW_values | update_gradients);
-
-        QGauss<dim - 1> common_face_quadrature(fe_ten.degree + 1);
-
-        FEFaceValues<dim> fe_nine_face_values(fe_nine,
-                                              common_face_quadrature,
-                                              update_JxW_values |
-                                              update_gradients | update_values);
-        FEFaceValues<dim> fe_ten_face_values(fe_ten,
-                                             common_face_quadrature,
-                                             update_normal_vectors |
-                                             update_values);
-
-        FullMatrix<double> cell_matrix;
-        Vector<double> cell_rhs;
-
-        const FEValuesExtractors::Vector displacements(SolutionComponents::displacement<dim>);
-
-        Tensor<1, dim> traction;
-        traction[1] = -1;
-        distributed_solution = distributed_state;
-        double objective_value = 0;
-        for (const auto &cell: dof_handler.active_cell_iterators())
-        {
-            if(cell->is_locally_owned())
-            {
-                hp_fe_values.reinit(cell);
-                const FEValues<dim> &fe_values = hp_fe_values.get_present_fe_values();
-                const unsigned int dofs_per_cell = cell->get_fe().n_dofs_per_cell();
-                const unsigned int n_q_points = fe_values.n_quadrature_points;
-                const unsigned int n_face_q_points = common_face_quadrature.size();
-
-                std::vector<Tensor<1, dim>> old_displacement_values(n_q_points);
-                fe_values[displacements].get_function_values(
-                        locally_relevant_solution, old_displacement_values);
-
-                for (unsigned int face_number = 0;
-                     face_number < GeometryInfo<dim>::faces_per_cell;
-                     ++face_number)
-                {
-                    if (cell->face(face_number)->at_boundary() && cell->face(face_number)->boundary_id()
-                                                                  == BoundaryIds::down_force)
-                    {
-                        for (unsigned int face_q_point = 0;
-                             face_q_point < n_face_q_points; ++face_q_point) {
-                            for (unsigned int i = 0; i < dofs_per_cell; ++i) {
-                                if (cell->material_id() == MaterialIds::without_multiplier) {
-                                    fe_nine_face_values.reinit(cell, face_number);
-                                    objective_value += traction
-                                                       * fe_nine_face_values[displacements].value(i,
-                                                                                                  face_q_point)
-                                                       * fe_nine_face_values.JxW(face_q_point);
-                                } else {
-                                    fe_ten_face_values.reinit(cell, face_number);
-                                    objective_value += traction
-                                                       * fe_ten_face_values[displacements].value(i,
-                                                                                                 face_q_point)
-                                                       * fe_ten_face_values.JxW(face_q_point);
-                                }
-                            }
-                        }
-                    }
-                }
-            }
-        }
-        double objective_value_out;
-        MPI_Allreduce(&objective_value, &objective_value_out, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-
-        std::cout << "objective value: " << objective_value_out << std::endl;
-        return objective_value;
-    }
-
-
-    //As the KKT System know which vectors correspond to the slack variables, the sum of the logs of the slacks is computed here for use in the filter.
-    template<int dim>
-    double
-    KktSystem<dim>::calculate_barrier_distance(const LA::MPI::BlockVector &state) const {
-        double barrier_distance_log_sum = 0;
-        unsigned int vect_size = state.block(SolutionBlocks::density_lower_slack).size();
-        distributed_solution = state;
-        for (unsigned int k = 0; k < vect_size; k++) {
-            if (distributed_solution.block(SolutionBlocks::density_lower_slack).in_local_range(k))
-            barrier_distance_log_sum += std::log(state.block(SolutionBlocks::density_lower_slack)[k]);
-        }
-        for (unsigned int k = 0; k < vect_size; k++) {
-            if (distributed_solution.block(SolutionBlocks::density_upper_slack).in_local_range(k))
-            barrier_distance_log_sum += std::log(state.block(SolutionBlocks::density_upper_slack)[k]);
-        }
-        double out_barrier_distance_log_sum;
-        MPI_Allreduce(&barrier_distance_log_sum, &out_barrier_distance_log_sum, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-
-        std::cout << "Barrier distance log sum: " << out_barrier_distance_log_sum << std::endl;
-
-        return out_barrier_distance_log_sum;
-    }
-
-    template<int dim>
-    double
-    KktSystem<dim>::calculate_rhs_norm(const LA::MPI::BlockVector &state, const double barrier_size) const {
-        return calculate_rhs(state, barrier_size).l2_norm();
-    }
-
-
-    //Feasibility conditions appear on the RHS of the linear system, so I compute the RHS to find it. Could probably be combined with the objective value finding part to make it faster.
-    template<int dim>
-    double
-    KktSystem<dim>::calculate_feasibility(const LA::MPI::BlockVector &state, const double barrier_size) const {
-       LA::MPI::BlockVector test_rhs = calculate_rhs(state, barrier_size);
-
-       double norm = 0;
-
-       distributed_solution = state;
-       double full_prod1 =1;
-       double full_prod2 = 1;
-
-       for (unsigned int k = 0; k < state.block(SolutionBlocks::density_upper_slack).size(); k++) {
-           double prod1 = 1;
-           double prod2 = 1;
-           if(state.block(SolutionBlocks::density_upper_slack).in_local_range(k))
-           {
-               prod1 = prod1 * state.block(SolutionBlocks::density_upper_slack)[k]
-                       * state.block(SolutionBlocks::density_upper_slack)[k];
-           }
-           if(state.block(SolutionBlocks::density_lower_slack).in_local_range(k))
-           {
-              prod2 = prod2 *  state.block(SolutionBlocks::density_lower_slack)[k]
-                      * state.block(SolutionBlocks::density_lower_slack)[k];
-           }
-           if(state.block(SolutionBlocks::density_upper_slack_multiplier).in_local_range(k))
-           {
-               prod1 = prod1 * state.block(SolutionBlocks::density_upper_slack_multiplier)[k]
-                       * state.block(SolutionBlocks::density_upper_slack_multiplier)[k];
-           }
-           if(state.block(SolutionBlocks::density_lower_slack_multiplier).in_local_range(k))
-           {
-               prod2 = prod2 *  state.block(SolutionBlocks::density_lower_slack_multiplier)[k]
-                       * state.block(SolutionBlocks::density_lower_slack_multiplier)[k];
-           }
-           MPI_Allreduce(&prod1, &full_prod1, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
-           MPI_Allreduce(&prod2, &full_prod2, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
-           norm = norm + full_prod1 + full_prod2;
-       }
-       std::cout << "pre-norm: " << norm << std::endl;
-
-        norm += std::pow(test_rhs.block(SolutionBlocks::displacement).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::density).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::unfiltered_density).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::displacement_multiplier).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::total_volume_multiplier).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::density_upper_slack_multiplier).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::density_lower_slack_multiplier).l2_norm(), 2);
-
-        std::cout << "norm: " << norm << std::endl;
-
-        return norm;
-    }
-
-    template<int dim>
-    double
-    KktSystem<dim>::calculate_convergence(const LA::MPI::BlockVector &state) const {
-        LA::MPI::BlockVector test_rhs = calculate_rhs(state, Input::min_barrier_size);
-        double norm = 0;
-
-        distributed_solution = state;
-        double full_prod1 =1;
-        double full_prod2 = 1;
-
-        for (unsigned int k = 0; k < state.block(SolutionBlocks::density_upper_slack).size(); k++) {
-            double prod1 = 1;
-            double prod2 = 1;
-            if(state.block(SolutionBlocks::density_upper_slack).in_local_range(k))
-            {
-                prod1 = prod1 * state.block(SolutionBlocks::density_upper_slack)[k]
-                        * state.block(SolutionBlocks::density_upper_slack)[k];
-            }
-            if(state.block(SolutionBlocks::density_lower_slack).in_local_range(k))
-            {
-               prod2 = prod2 *  state.block(SolutionBlocks::density_lower_slack)[k]
-                       * state.block(SolutionBlocks::density_lower_slack)[k];
-            }
-            if(state.block(SolutionBlocks::density_upper_slack_multiplier).in_local_range(k))
-            {
-                prod1 = prod1 * state.block(SolutionBlocks::density_upper_slack_multiplier)[k]
-                        * state.block(SolutionBlocks::density_upper_slack_multiplier)[k];
-            }
-            if(state.block(SolutionBlocks::density_lower_slack_multiplier).in_local_range(k))
-            {
-                prod2 = prod2 *  state.block(SolutionBlocks::density_lower_slack_multiplier)[k]
-                        * state.block(SolutionBlocks::density_lower_slack_multiplier)[k];
-            }
-            MPI_Allreduce(&prod1, &full_prod1, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
-            MPI_Allreduce(&prod2, &full_prod2, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
-            norm = norm + full_prod1 + full_prod2;
-        }
-        std::cout << "pre-norm: " << norm << std::endl;
-
-        norm += std::pow(test_rhs.block(SolutionBlocks::displacement).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::density).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::unfiltered_density).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::displacement_multiplier).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::total_volume_multiplier).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::density_upper_slack_multiplier).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::density_lower_slack_multiplier).l2_norm(), 2);
-
-        norm = std::pow(norm, .5);
-
-        std::cout << "l2 norm: " << system_rhs.l2_norm() << std::endl;
-        std::cout << "KKT norm: " << norm << std::endl;
-        return norm;
-    }
-
-    template<int dim>
-    LA::MPI::BlockVector
-    KktSystem<dim>::calculate_rhs(const LA::MPI::BlockVector &distributed_state, const double barrier_size) const {
-        LA::MPI::BlockVector test_rhs (system_rhs);
-        LA::MPI::BlockVector state (locally_relevant_solution);
-        state = distributed_state;
-        test_rhs = 0;
-
-
-        QGauss<dim> nine_quadrature(fe_nine.degree + 1);
-        QGauss<dim> ten_quadrature(fe_ten.degree + 1);
-
-        hp::QCollection<dim> q_collection;
-        q_collection.push_back(nine_quadrature);
-        q_collection.push_back(ten_quadrature);
-
-        hp::FEValues<dim> hp_fe_values(fe_collection,
-                                       q_collection,
-                                       update_values | update_quadrature_points |
-                                       update_JxW_values | update_gradients);
-
-        QGauss<dim - 1> common_face_quadrature(fe_ten.degree + 1);
-
-        FEFaceValues<dim> fe_nine_face_values(fe_nine,
-                                              common_face_quadrature,
-                                              update_JxW_values |
-                                              update_gradients | update_values);
-        FEFaceValues<dim> fe_ten_face_values(fe_ten,
-                                             common_face_quadrature,
-                                             update_normal_vectors |
-                                             update_values);
-
-        FullMatrix<double> cell_matrix;
-        Vector<double> cell_rhs;
-        std::vector<types::global_dof_index> local_dof_indices;
-
-        const FEValuesExtractors::Scalar densities(SolutionComponents::density<dim>);
-        const FEValuesExtractors::Vector displacements(SolutionComponents::displacement<dim>);
-        const FEValuesExtractors::Scalar unfiltered_densities(SolutionComponents::unfiltered_density<dim>);
-        const FEValuesExtractors::Vector displacement_multipliers(SolutionComponents::displacement_multiplier<dim>);
-        const FEValuesExtractors::Scalar unfiltered_density_multipliers(
-                SolutionComponents::unfiltered_density_multiplier<dim>);
-        const FEValuesExtractors::Scalar density_lower_slacks(SolutionComponents::density_lower_slack<dim>);
-        const FEValuesExtractors::Scalar density_lower_slack_multipliers(
-                SolutionComponents::density_lower_slack_multiplier<dim>);
-        const FEValuesExtractors::Scalar density_upper_slacks(SolutionComponents::density_upper_slack<dim>);
-        const FEValuesExtractors::Scalar density_upper_slack_multipliers(
-                SolutionComponents::density_upper_slack_multiplier<dim>);
-        const FEValuesExtractors::Scalar total_volume_multiplier(
-                SolutionComponents::total_volume_multiplier<dim>);
-
-
-        const unsigned int n_face_q_points = common_face_quadrature.size();
-
-        const Functions::ConstantFunction<dim> lambda(1.), mu(1.);
-
-        locally_relevant_solution = state;
-        distributed_solution = state;
-        LA::MPI::BlockVector filtered_unfiltered_density_solution (distributed_solution);
-        LA::MPI::BlockVector filter_adjoint_unfiltered_density_multiplier_solution (distributed_solution);
-        filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = 0;
-        filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier) = 0;
-
-        auto op_f = linear_operator<LA::MPI::Vector>(density_filter.filter_matrix);
-        filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = op_f * distributed_solution.block(SolutionBlocks::unfiltered_density);
-        filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier)
-                                                    = transpose_operator(op_f) * distributed_solution.block(SolutionBlocks::unfiltered_density_multiplier);
-        LA::MPI::BlockVector relevant_filtered_unfiltered_density_solution (locally_relevant_solution);
-        LA::MPI::BlockVector relevant_filter_adjoint_unfiltered_density_multiplier_solution (locally_relevant_solution);
-        relevant_filtered_unfiltered_density_solution = filtered_unfiltered_density_solution;
-        relevant_filter_adjoint_unfiltered_density_multiplier_solution = filter_adjoint_unfiltered_density_multiplier_solution;
-
-        double old_volume_multiplier_temp = 0;
-        double old_volume_multiplier;
-        if(distributed_state.block(SolutionBlocks::total_volume_multiplier).in_local_range(0))
-        {
-            old_volume_multiplier_temp = state.block(SolutionBlocks::total_volume_multiplier)[0];
-        }
-        MPI_Allreduce(&old_volume_multiplier_temp, &old_volume_multiplier, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-
-        for (const auto &cell: dof_handler.active_cell_iterators()) {
-            if(cell->is_locally_owned())
-            {
-                hp_fe_values.reinit(cell);
-                const FEValues<dim> &fe_values = hp_fe_values.get_present_fe_values();
-                cell_matrix.reinit(cell->get_fe().n_dofs_per_cell(),
-                                   cell->get_fe().n_dofs_per_cell());
-                cell_rhs.reinit(cell->get_fe().n_dofs_per_cell());
-
-                const unsigned int n_q_points = fe_values.n_quadrature_points;
-
-                std::vector<double> old_density_values(n_q_points);
-                std::vector<Tensor<1, dim>> old_displacement_values(n_q_points);
-                std::vector<double> old_displacement_divs(n_q_points);
-                std::vector<SymmetricTensor<2, dim>> old_displacement_symmgrads(
-                        n_q_points);
-                std::vector<Tensor<1, dim>> old_displacement_multiplier_values(
-                        n_q_points);
-                std::vector<double> old_displacement_multiplier_divs(n_q_points);
-                std::vector<SymmetricTensor<2, dim>> old_displacement_multiplier_symmgrads(
-                        n_q_points);
-                std::vector<double> old_lower_slack_multiplier_values(n_q_points);
-                std::vector<double> old_upper_slack_multiplier_values(n_q_points);
-                std::vector<double> old_lower_slack_values(n_q_points);
-                std::vector<double> old_upper_slack_values(n_q_points);
-                std::vector<double> old_unfiltered_density_values(n_q_points);
-                std::vector<double> old_unfiltered_density_multiplier_values(n_q_points);
-                std::vector<double> filtered_unfiltered_density_values(n_q_points);
-                std::vector<double> filter_adjoint_unfiltered_density_multiplier_values(n_q_points);
-                std::vector<double> lambda_values(n_q_points);
-                std::vector<double> mu_values(n_q_points);
-
-                const unsigned int dofs_per_cell = cell->get_fe().n_dofs_per_cell();
-
-                cell_matrix = 0;
-                cell_rhs = 0;
-                local_dof_indices.resize(cell->get_fe().n_dofs_per_cell());
-
-                cell->get_dof_indices(local_dof_indices);
-
-                lambda.value_list(fe_values.get_quadrature_points(), lambda_values);
-                mu.value_list(fe_values.get_quadrature_points(), mu_values);
-
-                fe_values[densities].get_function_values(state,
-                                                         old_density_values);
-                fe_values[displacements].get_function_values(state,
-                                                             old_displacement_values);
-                fe_values[displacements].get_function_divergences(state,
-                                                                  old_displacement_divs);
-                fe_values[displacements].get_function_symmetric_gradients(
-                        state, old_displacement_symmgrads);
-                fe_values[displacement_multipliers].get_function_values(
-                        state, old_displacement_multiplier_values);
-                fe_values[displacement_multipliers].get_function_divergences(
-                        state, old_displacement_multiplier_divs);
-                fe_values[displacement_multipliers].get_function_symmetric_gradients(
-                        state, old_displacement_multiplier_symmgrads);
-                fe_values[density_lower_slacks].get_function_values(
-                        state, old_lower_slack_values);
-                fe_values[density_lower_slack_multipliers].get_function_values(
-                        state, old_lower_slack_multiplier_values);
-                fe_values[density_upper_slacks].get_function_values(
-                        state, old_upper_slack_values);
-                fe_values[density_upper_slack_multipliers].get_function_values(
-                        state, old_upper_slack_multiplier_values);
-                fe_values[unfiltered_densities].get_function_values(
-                        state, old_unfiltered_density_values);
-                fe_values[unfiltered_density_multipliers].get_function_values(
-                        state, old_unfiltered_density_multiplier_values);
-                fe_values[unfiltered_densities].get_function_values(
-                        relevant_filtered_unfiltered_density_solution, filtered_unfiltered_density_values);
-                fe_values[unfiltered_density_multipliers].get_function_values(
-                        relevant_filter_adjoint_unfiltered_density_multiplier_solution,
-                        filter_adjoint_unfiltered_density_multiplier_values);
-
-
-                Tensor<1, dim> traction;
-                traction[1] = -1;
-
-                for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
-
-                    for (unsigned int i = 0; i < dofs_per_cell; ++i) {
-                        const SymmetricTensor<2, dim> displacement_phi_i_symmgrad =
-                                fe_values[displacements].symmetric_gradient(i, q_point);
-                        const double displacement_phi_i_div =
-                                fe_values[displacements].divergence(i, q_point);
-
-                        const SymmetricTensor<2, dim> displacement_multiplier_phi_i_symmgrad =
-                                fe_values[displacement_multipliers].symmetric_gradient(i,
-                                                                                       q_point);
-                        const double displacement_multiplier_phi_i_div =
-                                fe_values[displacement_multipliers].divergence(i,
-                                                                               q_point);
-
-
-                        const double density_phi_i = fe_values[densities].value(i,
-                                                                                q_point);
-                        const double unfiltered_density_phi_i = fe_values[unfiltered_densities].value(i,
-                                                                                                      q_point);
-                        const double unfiltered_density_multiplier_phi_i = fe_values[unfiltered_density_multipliers].value(
-                                i, q_point);
-
-                        const double lower_slack_multiplier_phi_i =
-                                fe_values[density_lower_slack_multipliers].value(i,
-                                                                                 q_point);
-
-                        const double lower_slack_phi_i =
-                                fe_values[density_lower_slacks].value(i, q_point);
-
-                        const double upper_slack_phi_i =
-                                fe_values[density_upper_slacks].value(i, q_point);
-
-                        const double upper_slack_multiplier_phi_i =
-                                fe_values[density_upper_slack_multipliers].value(i,
-                                                                                 q_point);
-
-
-                        //rhs eqn 0
-                        cell_rhs(i) +=
-                                -1 * fe_values.JxW(q_point) * (
-                                        -1 * density_penalty_exponent *
-                                        std::pow(old_density_values[q_point], density_penalty_exponent - 1) * density_phi_i
-                                        * (old_displacement_multiplier_divs[q_point] * old_displacement_divs[q_point]
-                                           * lambda_values[q_point]
-                                           + 2 * mu_values[q_point] * (old_displacement_symmgrads[q_point]
-                                                                       * old_displacement_multiplier_symmgrads[q_point]))
-                                        - density_phi_i * old_unfiltered_density_multiplier_values[q_point]
-                                        + old_volume_multiplier * density_phi_i
-                                );
-
-                        //rhs eqn 1 - boundary terms counted later
-                        cell_rhs(i) +=
-                                -1 * fe_values.JxW(q_point) * (
-                                        -1 * std::pow(old_density_values[q_point], density_penalty_exponent)
-                                        * (old_displacement_multiplier_divs[q_point] * displacement_phi_i_div
-                                           * lambda_values[q_point]
-                                           + 2 * mu_values[q_point] * (old_displacement_multiplier_symmgrads[q_point]
-                                                                       * displacement_phi_i_symmgrad))
-                                );
-
-                        //rhs eqn 2
-                        cell_rhs(i) +=
-                                -1 * fe_values.JxW(q_point) * (
-                                        unfiltered_density_phi_i *
-                                        filter_adjoint_unfiltered_density_multiplier_values[q_point]
-                                        + unfiltered_density_phi_i * old_upper_slack_multiplier_values[q_point]
-                                        + -1 * unfiltered_density_phi_i * old_lower_slack_multiplier_values[q_point]
-                                );
-
-
-
-
-                        //rhs eqn 3 - boundary terms counted later
-                        cell_rhs(i) +=
-                                -1 * fe_values.JxW(q_point) * (
-                                        -1 * std::pow(old_density_values[q_point], density_penalty_exponent)
-                                        * (old_displacement_divs[q_point] * displacement_multiplier_phi_i_div
-                                           * lambda_values[q_point]
-                                           + 2 * mu_values[q_point] * (displacement_multiplier_phi_i_symmgrad
-                                                                       * old_displacement_symmgrads[q_point]))
-                                );
-
-                        //rhs eqn 4
-                        cell_rhs(i) +=
-                                -1 * fe_values.JxW(q_point) *
-                                (-1 * lower_slack_multiplier_phi_i
-                                 * (old_unfiltered_density_values[q_point] - old_lower_slack_values[q_point])
-                                );
-
-                        //rhs eqn 5
-                        cell_rhs(i) +=
-                                -1 * fe_values.JxW(q_point) * (
-                                        -1 * upper_slack_multiplier_phi_i
-                                        * (1 - old_unfiltered_density_values[q_point]
-                                           - old_upper_slack_values[q_point]));
-
-                        //rhs eqn 6
-                        cell_rhs(i) +=
-                                -1 * fe_values.JxW(q_point) * (
-                                        -1 * unfiltered_density_multiplier_phi_i
-                                        * (old_density_values[q_point] - filtered_unfiltered_density_values[q_point])
-                                );
-
-                        //rhs eqn 7
-                        cell_rhs(i) +=
-                                -1 * fe_values.JxW(q_point) *
-                                (lower_slack_phi_i *
-                                 (old_lower_slack_multiplier_values[q_point] -
-                                  barrier_size / old_lower_slack_values[q_point]));
-
-                        //rhs eqn 8
-                        cell_rhs(i) +=
-                                -1 * fe_values.JxW(q_point) *
-                                (upper_slack_phi_i *
-                                 (old_upper_slack_multiplier_values[q_point] -
-                                  barrier_size / old_upper_slack_values[q_point]));
-
-                    }
-
-                }
-
-                for (unsigned int face_number = 0;
-                     face_number < GeometryInfo<dim>::faces_per_cell;
-                     ++face_number) {
-                    if (cell->face(face_number)->at_boundary() && cell->face(
-                            face_number)->boundary_id() == BoundaryIds::down_force) {
-                        for (unsigned int face_q_point = 0;
-                             face_q_point < n_face_q_points; ++face_q_point) {
-                            for (unsigned int i = 0; i < dofs_per_cell; ++i) {
-                                if (cell->material_id() == MaterialIds::without_multiplier) {
-                                    fe_nine_face_values.reinit(cell, face_number);
-                                    cell_rhs(i) += -1
-                                                   * traction
-                                                   * fe_nine_face_values[displacements].value(i,
-                                                                                              face_q_point)
-                                                   * fe_nine_face_values.JxW(face_q_point);
-
-                                    cell_rhs(i) += -1 * traction
-                                                   * fe_nine_face_values[displacement_multipliers].value(
-                                            i, face_q_point)
-                                                   * fe_nine_face_values.JxW(face_q_point);
-                                } else {
-                                    fe_ten_face_values.reinit(cell, face_number);
-                                    cell_rhs(i) += -1
-                                                   * traction
-                                                   * fe_ten_face_values[displacements].value(i,
-                                                                                             face_q_point)
-                                                   * fe_ten_face_values.JxW(face_q_point);
-
-                                    cell_rhs(i) += -1 * traction
-                                                   * fe_ten_face_values[displacement_multipliers].value(
-                                            i, face_q_point)
-                                                   * fe_ten_face_values.JxW(face_q_point);
-                                }
-                            }
-                        }
-                    }
-                }
-
-
-                MatrixTools::local_apply_boundary_values(boundary_values, local_dof_indices,
-                                                         cell_matrix, cell_rhs, true);
-                constraints.distribute_local_to_global(
-                        cell_rhs, local_dof_indices, test_rhs);
-            }
-        }
-
-        test_rhs.compress(VectorOperation::add);
-        double total_volume_temp = 0;
-        double goal_volume_temp = 0;
-        double total_volume, goal_volume;
-
-        distributed_solution = state;
-        for (const auto &cell: dof_handler.active_cell_iterators()) {
-            if(cell->is_locally_owned())
-            {
-                if (distributed_solution.block(SolutionBlocks::density).in_local_range(cell->active_cell_index()))
-                {
-                    total_volume_temp += cell->measure() * state.block(SolutionBlocks::density)[cell->active_cell_index()];
-                    goal_volume_temp += cell->measure() * Input::volume_percentage;
-                }
-            }
-        }
-
-        MPI_Allreduce(&total_volume_temp, &total_volume, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-        MPI_Allreduce(&goal_volume_temp, &goal_volume, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-
-        if (test_rhs.block(SolutionBlocks::total_volume_multiplier).in_local_range(0))
-        {
-            test_rhs.block(SolutionBlocks::total_volume_multiplier)[0] = goal_volume - total_volume;
-        }
-        test_rhs.compress(VectorOperation::insert);
-
-        return test_rhs;
-
-    }
-
-
-    template<int dim>
-    LA::MPI::BlockVector
-    KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
-        double gmres_tolerance;
-        if (Input::use_eisenstat_walker) {
-            gmres_tolerance = std::max(
-                    std::min(
-                            .1 * system_rhs.l2_norm() / (initial_rhs_error),
-                            .001
-                    ),
-                    Input::default_gmres_tolerance);
-        }
-        else {
-            gmres_tolerance = Input::default_gmres_tolerance;
-        }
-        locally_relevant_solution=state;
-        distributed_solution = state;
-        SolverControl solver_control(10000, gmres_tolerance * system_rhs.l2_norm());
-        TopOptSchurPreconditioner<dim> preconditioner(system_matrix);
-
-//        std::cout << " rhs " << std::endl;
-//        system_rhs.print(std::cout);
-//        std::cout << std::endl;
-
-//        preconditioner.initialize(system_matrix, boundary_values, dof_handler, locally_relevant_solution, distributed_solution);
-//        preconditioner.vmult(distributed_solution,system_rhs);
-
-//        std::cout << "solution" <<  std::endl;
-//        distributed_solution.print(std::cout);
-//        std::cout << std::endl;
-
-
-//        for (unsigned int k=0; k<10; k++)
-//        {
-//            for (unsigned int j=0; j<10; j++)
-//            {
-//                std::cout << k << ", " << j << std::endl;
-//                std::cout << system_matrix.block(k,j).frobenius_norm() << std::endl;
-//            }
-//        }
-
-        switch (Input::solver_choice) {
-            case SolverOptions::direct_solve: {
-//                SolverControl cn;
-//                std::string solver_type;
-//                TrilinosWrappers::SolverDirect::AdditionalData additional_data(false, "Amesos_Lapack");
-//                TrilinosWrappers::SolverDirect solver(cn, additional_data);
-//                solver.initialize(system_matrix);
-//                solver.solve(distributed_solution, system_rhs);
-                break;
-            }
-            case SolverOptions::exact_preconditioner_with_gmres: {
-                preconditioner.initialize(system_matrix, boundary_values, dof_handler, locally_relevant_solution, distributed_solution);
-                SolverFGMRES<LA::MPI::BlockVector> A_fgmres(solver_control);
-                A_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
-                std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
-                break;
-            }
-            case SolverOptions::inexact_K_with_exact_A_gmres: {
-                preconditioner.initialize(system_matrix, boundary_values, dof_handler,  locally_relevant_solution, distributed_solution);
-                SolverFGMRES<LA::MPI::BlockVector> B_fgmres(solver_control);
-                B_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
-                std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
-                break;
-            }
-            case SolverOptions::inexact_K_with_inexact_A_gmres: {
-                preconditioner.initialize(system_matrix, boundary_values, dof_handler,  locally_relevant_solution, distributed_solution);
-                SolverFGMRES<LA::MPI::BlockVector> C_fgmres(solver_control);
-                C_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
-                std::cout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
-                break;
-            }
-            default:
-                throw;
-        }
-
-
-        constraints.distribute(distributed_solution);
-
-        if (Input::output_parts_of_matrix) {
-            preconditioner.print_stuff();
-        }
-
-        if (Input::output_full_preconditioned_matrix) {
-//            FullMatrix<double> preconditioned_full_mat(system_matrix.n(), system_matrix.n());
-//            const auto op_preconditioned_full_mat = linear_operator(preconditioner) * linear_operator(system_matrix);
-//            build_matrix_element_by_element(op_preconditioned_full_mat, preconditioned_full_mat);
-//            print_matrix("preconditioned_full_block_matrix.csv",preconditioned_full_mat);
-        }
-
-        if (Input::output_full_matrix) {
-//            const unsigned int vec_size = system_matrix.n();
-//            FullMatrix<double> full_mat(vec_size, vec_size);
-//            build_matrix_element_by_element(system_matrix,full_mat);
-//            std::ofstream Mat("full_block_matrix.csv");
-//            for (unsigned int i = 0; i < vec_size; i++) {
-//                Mat << full_mat(i, 0);
-//                for (unsigned int j = 1; j < vec_size; j++) {
-//                    Mat << "," << full_mat(i, j);
-//                }
-//                Mat << "\n";
-//            }
-//            Mat.close();
-        }
-
-//        std::cout << "solution" << std::endl;
-//        distributed_solution.print(std::cout);
-//        std::cout << std::endl;
-
-        return distributed_solution;
-    }
-
-    template<int dim>
-    void
-    KktSystem<dim>::calculate_initial_rhs_error() {
-        initial_rhs_error = system_rhs.l2_norm();
-    }
-
-    template<int dim>
-    LA::MPI::BlockVector
-    KktSystem<dim>::get_initial_state() {
-
-        std::vector<unsigned int> block_component(10, 2);
-        block_component[SolutionBlocks::density] = 0;
-        block_component[SolutionBlocks::displacement] = 1;
-        const std::vector<types::global_dof_index> dofs_per_block =
-                DoFTools::count_dofs_per_fe_block(dof_handler, block_component);
-        const unsigned int n_p = dofs_per_block[0];
-        const unsigned int n_u = dofs_per_block[1];
-        const std::vector<unsigned int> block_sizes = {n_p, n_p, n_p, n_p, n_p, n_u, n_u, n_p, n_p, 1};
-
-        LA::MPI::BlockVector state(owned_partitioning, mpi_communicator);
-        {
-            using namespace SolutionBlocks;
-            state.block(density).add(density_ratio);
-            state.block(unfiltered_density).add(density_ratio);
-            state.block(unfiltered_density_multiplier)
-                    .add(density_ratio);
-            state.block(density_lower_slack).add(density_ratio);
-            state.block(density_lower_slack_multiplier).add(50);
-            state.block(density_upper_slack).add(1 - density_ratio);
-            state.block(density_upper_slack_multiplier).add(50);
-            state.block(total_volume_multiplier).add(1);
-            state.block(displacement).add(0);
-            state.block(displacement_multiplier).add(0);
-        }
-        state.compress(VectorOperation::add);
-        return state;
-    }
-
-    template<int dim>
-    void
-    KktSystem<dim>::output(const LA::MPI::BlockVector &state, const unsigned int j) const {
-        locally_relevant_solution = state;
-        std::vector<std::string> solution_names(1, "low_slack_multiplier");
-        std::vector<DataComponentInterpretation::DataComponentInterpretation> data_component_interpretation(
-                1, DataComponentInterpretation::component_is_scalar);
-        solution_names.emplace_back("upper_slack_multiplier");
-        data_component_interpretation.push_back(
-                DataComponentInterpretation::component_is_scalar);
-        solution_names.emplace_back("low_slack");
-        data_component_interpretation.push_back(
-                DataComponentInterpretation::component_is_scalar);
-        solution_names.emplace_back("upper_slack");
-        data_component_interpretation.push_back(
-                DataComponentInterpretation::component_is_scalar);
-        solution_names.emplace_back("unfiltered_density");
-        data_component_interpretation.push_back(
-                DataComponentInterpretation::component_is_scalar);
-        for (unsigned int i = 0; i < dim; i++) {
-            solution_names.emplace_back("displacement");
-            data_component_interpretation.push_back(
-                    DataComponentInterpretation::component_is_part_of_vector);
-        }
-        for (unsigned int i = 0; i < dim; i++) {
-            solution_names.emplace_back("displacement_multiplier");
-            data_component_interpretation.push_back(
-                    DataComponentInterpretation::component_is_part_of_vector);
-        }
-        solution_names.emplace_back("density_multiplier");
-        data_component_interpretation.push_back(
-                DataComponentInterpretation::component_is_scalar);
-        solution_names.emplace_back("density");
-        data_component_interpretation.push_back(
-                DataComponentInterpretation::component_is_scalar);
-        solution_names.emplace_back("volume_multiplier");
-        data_component_interpretation.push_back(
-                DataComponentInterpretation::component_is_scalar);
-        DataOut<dim> data_out;
-        data_out.attach_dof_handler(dof_handler);
-        data_out.add_data_vector(locally_relevant_solution,
-                                 solution_names,
-                                 DataOut<dim>::type_dof_data,
-                                 data_component_interpretation);
-        data_out.build_patches();
-        std::string output("solution" + std::to_string(j) + ".vtu");
-        data_out.write_vtu_in_parallel(output, MPI_COMM_WORLD);
-
-    }
-
-    template<>
-    void
-    KktSystem<2>::output_stl(const LA::MPI::BlockVector &state) {
-        double height = .25;
-        const int dim = 2;
-        std::ofstream stlfile;
-        stlfile.open("bridge.stl");
-        stlfile << "solid bridge\n" << std::scientific;
-
-        for (const auto &cell: dof_handler.active_cell_iterators()) {
-            if (state.block(
-                    SolutionBlocks::density)[cell->active_cell_index()] > 0.5) {
-                const Tensor<1, dim> edge_directions[2] = {cell->vertex(1) -
-                                                           cell->vertex(0),
-                                                           cell->vertex(2) -
-                                                           cell->vertex(0)};
-                const Tensor<2, dim> edge_tensor(
-                        {{edge_directions[0][0], edge_directions[0][1]},
-                         {edge_directions[1][0], edge_directions[1][1]}});
-                const bool is_right_handed_cell = (determinant(edge_tensor) > 0);
-                if (is_right_handed_cell) {
-                    /* Write one side at z = 0. */
-                    stlfile << "   facet normal " << 0.000000e+00 << " "
-                            << 0.000000e+00 << " " << -1.000000e+00 << "\n";
-                    stlfile << "      outer loop\n";
-                    stlfile << "         vertex " << cell->vertex(0)[0] << " "
-                            << cell->vertex(0)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
-                            << cell->vertex(2)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
-                            << cell->vertex(1)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "      endloop\n";
-                    stlfile << "   endfacet\n";
-                    stlfile << "   facet normal " << 0.000000e+00 << " "
-                            << 0.000000e+00 << " " << -1.000000e+00 << "\n";
-                    stlfile << "      outer loop\n";
-                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
-                            << cell->vertex(1)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
-                            << cell->vertex(2)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "         vertex " << cell->vertex(3)[0] << " "
-                            << cell->vertex(3)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "      endloop\n";
-                    stlfile << "   endfacet\n";
-                    /* Write one side at z = height. */
-                    stlfile << "   facet normal " << 0.000000e+00 << " "
-                            << 0.000000e+00 << " " << 1.000000e+00 << "\n";
-                    stlfile << "      outer loop\n";
-                    stlfile << "         vertex " << cell->vertex(0)[0] << " "
-                            << cell->vertex(0)[1] << " " << height << "\n";
-                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
-                            << cell->vertex(1)[1] << " " << height << "\n";
-                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
-                            << cell->vertex(2)[1] << " " << height << "\n";
-                    stlfile << "      endloop\n";
-                    stlfile << "   endfacet\n";
-                    stlfile << "   facet normal " << 0.000000e+00 << " "
-                            << 0.000000e+00 << " " << 1.000000e+00 << "\n";
-                    stlfile << "      outer loop\n";
-                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
-                            << cell->vertex(1)[1] << " " << height << "\n";
-                    stlfile << "         vertex " << cell->vertex(3)[0] << " "
-                            << cell->vertex(3)[1] << " " << height << "\n";
-                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
-                            << cell->vertex(2)[1] << " " << height << "\n";
-                    stlfile << "      endloop\n";
-                    stlfile << "   endfacet\n";
-                } else /* The cell has a left-handed set up */
-                {
-                    /* Write one side at z = 0. */
-                    stlfile << "   facet normal " << 0.000000e+00 << " "
-                            << 0.000000e+00 << " " << -1.000000e+00 << "\n";
-                    stlfile << "      outer loop\n";
-                    stlfile << "         vertex " << cell->vertex(0)[0] << " "
-                            << cell->vertex(0)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
-                            << cell->vertex(1)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
-                            << cell->vertex(2)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "      endloop\n";
-                    stlfile << "   endfacet\n";
-                    stlfile << "   facet normal " << 0.000000e+00 << " "
-                            << 0.000000e+00 << " " << -1.000000e+00 << "\n";
-                    stlfile << "      outer loop\n";
-                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
-                            << cell->vertex(1)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "         vertex " << cell->vertex(3)[0] << " "
-                            << cell->vertex(3)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
-                            << cell->vertex(2)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "      endloop\n";
-                    stlfile << "   endfacet\n";
-                    /* Write one side at z = height. */
-                    stlfile << "   facet normal " << 0.000000e+00 << " "
-                            << 0.000000e+00 << " " << 1.000000e+00 << "\n";
-                    stlfile << "      outer loop\n";
-                    stlfile << "         vertex " << cell->vertex(0)[0] << " "
-                            << cell->vertex(0)[1] << " " << height << "\n";
-                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
-                            << cell->vertex(2)[1] << " " << height << "\n";
-                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
-                            << cell->vertex(1)[1] << " " << height << "\n";
-                    stlfile << "      endloop\n";
-                    stlfile << "   endfacet\n";
-                    stlfile << "   facet normal " << 0.000000e+00 << " "
-                            << 0.000000e+00 << " " << 1.000000e+00 << "\n";
-                    stlfile << "      outer loop\n";
-                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
-                            << cell->vertex(1)[1] << " " << height << "\n";
-                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
-                            << cell->vertex(2)[1] << " " << height << "\n";
-                    stlfile << "         vertex " << cell->vertex(3)[0] << " "
-                            << cell->vertex(3)[1] << " " << height << "\n";
-                    stlfile << "      endloop\n";
-                    stlfile << "   endfacet\n";
-                }
-                for (unsigned int face_number = 0;
-                     face_number < GeometryInfo<dim>::faces_per_cell;
-                     ++face_number) {
-                    const typename DoFHandler<dim>::face_iterator face =
-                            cell->face(face_number);
-                    if ((face->at_boundary()) ||
-                        (!face->at_boundary() &&
-                         (state.block(
-                                 SolutionBlocks::density)[cell->neighbor(face_number)->active_cell_index()] <
-                          0.5))) {
-                        const Tensor<1, dim> normal_vector =
-                                (face->center() - cell->center());
-                        const double normal_norm = normal_vector.norm();
-                        if ((face->vertex(0)[0] - face->vertex(0)[0]) *
-                            (face->vertex(1)[1] - face->vertex(0)[1]) *
-                            0.000000e+00 +
-                            (face->vertex(0)[1] - face->vertex(0)[1]) * (0 - 0) *
-                            normal_vector[0] +
-                            (height - 0) *
-                            (face->vertex(1)[0] - face->vertex(0)[0]) *
-                            normal_vector[1] -
-                            (face->vertex(0)[0] - face->vertex(0)[0]) * (0 - 0) *
-                            normal_vector[1] -
-                            (face->vertex(0)[1] - face->vertex(0)[1]) *
-                            (face->vertex(1)[0] - face->vertex(0)[0]) *
-                            normal_vector[0] -
-                            (height - 0) *
-                            (face->vertex(1)[1] - face->vertex(0)[1]) * 0 >
-                            0) {
-                            stlfile << "   facet normal "
-                                    << normal_vector[0] / normal_norm << " "
-                                    << normal_vector[1] / normal_norm << " "
-                                    << 0.000000e+00 << "\n";
-                            stlfile << "      outer loop\n";
-                            stlfile << "         vertex " << face->vertex(0)[0]
-                                    << " " << face->vertex(0)[1] << " "
-                                    << 0.000000e+00 << "\n";
-                            stlfile << "         vertex " << face->vertex(0)[0]
-                                    << " " << face->vertex(0)[1] << " " << height
-                                    << "\n";
-                            stlfile << "         vertex " << face->vertex(1)[0]
-                                    << " " << face->vertex(1)[1] << " "
-                                    << 0.000000e+00 << "\n";
-                            stlfile << "      endloop\n";
-                            stlfile << "   endfacet\n";
-                            stlfile << "   facet normal "
-                                    << normal_vector[0] / normal_norm << " "
-                                    << normal_vector[1] / normal_norm << " "
-                                    << 0.000000e+00 << "\n";
-                            stlfile << "      outer loop\n";
-                            stlfile << "         vertex " << face->vertex(0)[0]
-                                    << " " << face->vertex(0)[1] << " " << height
-                                    << "\n";
-                            stlfile << "         vertex " << face->vertex(1)[0]
-                                    << " " << face->vertex(1)[1] << " " << height
-                                    << "\n";
-                            stlfile << "         vertex " << face->vertex(1)[0]
-                                    << " " << face->vertex(1)[1] << " "
-                                    << 0.000000e+00 << "\n";
-                            stlfile << "      endloop\n";
-                            stlfile << "   endfacet\n";
-                        } else {
-                            stlfile << "   facet normal "
-                                    << normal_vector[0] / normal_norm << " "
-                                    << normal_vector[1] / normal_norm << " "
-                                    << 0.000000e+00 << "\n";
-                            stlfile << "      outer loop\n";
-                            stlfile << "         vertex " << face->vertex(0)[0]
-                                    << " " << face->vertex(0)[1] << " "
-                                    << 0.000000e+00 << "\n";
-                            stlfile << "         vertex " << face->vertex(1)[0]
-                                    << " " << face->vertex(1)[1] << " "
-                                    << 0.000000e+00 << "\n";
-                            stlfile << "         vertex " << face->vertex(0)[0]
-                                    << " " << face->vertex(0)[1] << " " << height
-                                    << "\n";
-                            stlfile << "      endloop\n";
-                            stlfile << "   endfacet\n";
-                            stlfile << "   facet normal "
-                                    << normal_vector[0] / normal_norm << " "
-                                    << normal_vector[1] / normal_norm << " "
-                                    << 0.000000e+00 << "\n";
-                            stlfile << "      outer loop\n";
-                            stlfile << "         vertex " << face->vertex(0)[0]
-                                    << " " << face->vertex(0)[1] << " " << height
-                                    << "\n";
-                            stlfile << "         vertex " << face->vertex(1)[0]
-                                    << " " << face->vertex(1)[1] << " "
-                                    << 0.000000e+00 << "\n";
-                            stlfile << "         vertex " << face->vertex(1)[0]
-                                    << " " << face->vertex(1)[1] << " " << height
-                                    << "\n";
-                            stlfile << "      endloop\n";
-                            stlfile << "   endfacet\n";
-                        }
-                    }
-                }
-            }
-        }
-        stlfile << "endsolid bridge";
-    }
-
-
-    template<>
-    void
-    KktSystem<3>::output_stl(const LA::MPI::BlockVector &state)
-    {
-        std::ofstream stlfile;
-        stlfile.open("bridge.stl");
-        stlfile << "solid bridge\n" << std::scientific;
-        const int dim = 3;
-        for (const auto &cell : dof_handler.active_cell_iterators())
-        {
-            if (state.block(
-                    SolutionBlocks::unfiltered_density)[cell->active_cell_index()] > 0.5)
-            {
-                for (const auto n : cell->face_indices())
-                {
-                    bool create_boundary = false;
-                    if (cell->at_boundary(n))
-                    {
-                        create_boundary = true;
-                    }
-                    else if (state.block(
-                            SolutionBlocks::unfiltered_density)[cell->neighbor(n)->active_cell_index()] <= 0.5)
-                    {
-                        create_boundary = true;
-                    }
-
-                    if (create_boundary)
-                    {
-                        const auto face = cell->face(n);
-                        const Tensor<1,dim> normal_vector = face->center() -
-                                                          cell->center();
-                        double normal_norm = normal_vector.norm();
-                        const Tensor<1,dim> edge_vectors_1 = face->vertex(1) - face->vertex(0);
-                        const Tensor<1,dim> edge_vectors_2 = face->vertex(2) - face->vertex(0);
-
-                        const Tensor<2, dim> edge_tensor (
-                                 {{edge_vectors_1[0], edge_vectors_1[1],edge_vectors_1[2]},
-                                 {edge_vectors_2[0], edge_vectors_2[1],edge_vectors_2[2]},
-                                 {normal_vector[0], normal_vector[1], normal_vector[2]}});
-                        const bool is_right_handed_cell = (determinant(edge_tensor) > 0);
-
-                        if (is_right_handed_cell)
-                        {
-                            stlfile << "   facet normal "
-                                    << normal_vector[0] / normal_norm << " "
-                                    << normal_vector[1] / normal_norm << " "
-                                    << normal_vector[2] / normal_norm << "\n";
-                            stlfile << "      outer loop\n";
-                            stlfile << "         vertex " << face->vertex(0)[0]
-                                    << " " << face->vertex(0)[1] << " "
-                                    << face->vertex(0)[2] << "\n";
-                            stlfile << "         vertex " << face->vertex(1)[0]
-                                    << " " << face->vertex(1)[1] << " "
-                                    << face->vertex(1)[2] << "\n";
-                            stlfile << "         vertex " << face->vertex(2)[0]
-                                    << " " << face->vertex(2)[1] << " "
-                                    << face->vertex(2)[2] << "\n";
-                            stlfile << "      endloop\n";
-                            stlfile << "   endfacet\n";
-                            stlfile << "   facet normal "
-                                    << normal_vector[0] / normal_norm << " "
-                                    << normal_vector[1] / normal_norm << " "
-                                    << normal_vector[2] / normal_norm << "\n";
-                            stlfile << "      outer loop\n";
-                            stlfile << "         vertex " << face->vertex(1)[0]
-                                    << " " << face->vertex(1)[1] << " " << face->vertex(1)[2]
-                                    << "\n";
-                            stlfile << "         vertex " << face->vertex(3)[0]
-                                    << " " << face->vertex(3)[1] << " " << face->vertex(3)[2]
-                                    << "\n";
-                            stlfile << "         vertex " << face->vertex(2)[0]
-                                    << " " << face->vertex(2)[1] << " "
-                                    << face->vertex(2)[2] << "\n";
-                            stlfile << "      endloop\n";
-                            stlfile << "   endfacet\n";
-                        }
-                        else
-                        {
-                            stlfile << "   facet normal "
-                                    << normal_vector[0] / normal_norm << " "
-                                    << normal_vector[1] / normal_norm << " "
-                                    << normal_vector[2] / normal_norm << "\n";
-                            stlfile << "      outer loop\n";
-                            stlfile << "         vertex " << face->vertex(0)[0]
-                                    << " " << face->vertex(0)[1] << " "
-                                    << face->vertex(0)[2] << "\n";
-                            stlfile << "         vertex " << face->vertex(2)[0]
-                                    << " " << face->vertex(2)[1] << " "
-                                    << face->vertex(2)[2] << "\n";
-                            stlfile << "         vertex " << face->vertex(1)[0]
-                                    << " " << face->vertex(1)[1] << " "
-                                    << face->vertex(1)[2] << "\n";
-                            stlfile << "      endloop\n";
-                            stlfile << "   endfacet\n";
-                            stlfile << "   facet normal "
-                                    << normal_vector[0] / normal_norm << " "
-                                    << normal_vector[1] / normal_norm << " "
-                                    << normal_vector[2] / normal_norm << "\n";
-                            stlfile << "      outer loop\n";
-                            stlfile << "         vertex " << face->vertex(1)[0]
-                                    << " " << face->vertex(1)[1] << " " << face->vertex(1)[2]
-                                    << "\n";
-                            stlfile << "         vertex " << face->vertex(2)[0]
-                                    << " " << face->vertex(2)[1] << " " << face->vertex(2)[2]
-                                    << "\n";
-                            stlfile << "         vertex " << face->vertex(3)[0]
-                                    << " " << face->vertex(3)[1] << " "
-                                    << face->vertex(3)[2] << "\n";
-                            stlfile << "      endloop\n";
-                            stlfile << "   endfacet\n";
-                        }
-
-                    }
-
-                }
-            }
-        }
-    }
-}
-
-template class SAND::KktSystem<2>;
-template class SAND::KktSystem<3>;
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index a936565..4afe7a3 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -23,6 +23,7 @@ namespace SAND {
     template<int dim>
     TopOptSchurPreconditioner<dim>::TopOptSchurPreconditioner(LA::MPI::BlockSparseMatrix &matrix_in)
             :
+            mpi_communicator(MPI_COMM_WORLD),
             system_matrix(matrix_in),
             n_rows(0),
             n_columns(0),
@@ -45,6 +46,7 @@ namespace SAND {
             additional_data(false, solver_type),
             direct_solver_control(1, 0),
             a_inv_direct(direct_solver_control, additional_data),
+            pcout(std::cout,(Utilities::MPI::this_mpi_process(mpi_communicator) == 0)),
             timer(std::cout, TimerOutput::summary, TimerOutput::wall_times)
     {
 
@@ -140,13 +142,13 @@ namespace SAND {
             d_7_mat=0;
             d_8_mat=0;
             d_m_inv_mat=0;
-            std::cout << "diag reinit" << std::endl;
+            pcout << "diag reinit" << std::endl;
         }
         {
             const types::global_dof_index n_p = system_matrix.block(SolutionBlocks::density,
                                                                     SolutionBlocks::density).m();
 
-            std::cout << "np: " << n_p << std::endl;
+            pcout << "np: " << n_p << std::endl;
 
 
 
@@ -226,7 +228,7 @@ namespace SAND {
         d_8_mat.compress(VectorOperation::insert);
         d_m_inv_mat.compress(VectorOperation::insert);
 
-        std::cout << "compressed" << std::endl;
+        pcout << "compressed" << std::endl;
 
         pre_j=distributed_state.block(SolutionBlocks::density);
         pre_k=distributed_state.block(SolutionBlocks::density);
@@ -240,7 +242,7 @@ namespace SAND {
         op_d_8 = linear_operator<VectorType,VectorType,PayloadType>(d_8_mat);
         op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) * transpose_operator<VectorType, VectorType, PayloadType>(linear_operator<VectorType,VectorType,PayloadType>(f_mat));
 
-        std::cout << "ops made" << std::endl;
+        pcout << "ops made" << std::endl;
 
         if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
         {
@@ -300,7 +302,7 @@ namespace SAND {
     template<int dim>
     void TopOptSchurPreconditioner<dim>::vmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
         LA::MPI::BlockVector temp_src;
-        std::cout << "vmult" << std::endl;
+        pcout << "vmult" << std::endl;
         {
             TimerOutput::Scope t(timer, "part 1");
             vmult_step_1(dst, src);
@@ -360,9 +362,9 @@ namespace SAND {
         auto dst_temp = dst;
 //        auto temp = src.block(SolutionBlocks::unfiltered_density);
 //        temp = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) * src.block(SolutionBlocks::unfiltered_density);
-//        std::cout <<std::endl <<std::endl<< "temp output:" << std::endl;
-//        temp.print(std::cout);
-//        std::cout << std::endl;
+//        pcout <<std::endl <<std::endl<< "temp output:" << std::endl;
+//        temp.print(pcout);
+//        pcout << std::endl;
 
 
         dst.block(SolutionBlocks::unfiltered_density_multiplier) = dst_temp.block(SolutionBlocks::unfiltered_density_multiplier)
@@ -446,8 +448,8 @@ namespace SAND {
             catch (std::exception &exc)
             {
                 std::cerr << "Failure of linear solver step_4_gmres_1" << std::endl;
-                std::cout << "first residual: " << step_4_gmres_control_1.initial_value() << std::endl;
-                std::cout << "last residual: " << step_4_gmres_control_1.last_value() << std::endl;
+                pcout << "first residual: " << step_4_gmres_control_1.initial_value() << std::endl;
+                pcout << "last residual: " << step_4_gmres_control_1.last_value() << std::endl;
                 throw;
             }
             SolverControl step_4_gmres_control_2 (100000, src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6);
@@ -458,8 +460,8 @@ namespace SAND {
             } catch (std::exception &exc)
             {
                 std::cerr << "Failure of linear solver step_4_gmres_2" << std::endl;
-                std::cout << "first residual: " << step_4_gmres_control_2.initial_value() << std::endl;
-                std::cout << "last residual: " << step_4_gmres_control_2.last_value() << std::endl;
+                pcout << "first residual: " << step_4_gmres_control_2.initial_value() << std::endl;
+                pcout << "last residual: " << step_4_gmres_control_2.last_value() << std::endl;
                 throw;
             }
         }
@@ -491,8 +493,8 @@ namespace SAND {
             } catch (std::exception &exc)
             {
                 std::cerr << "Failure of linear solver step_4_gmres_1" << std::endl;
-                std::cout << "first residual: " << step_4_gmres_control_1.initial_value() << std::endl;
-                std::cout << "last residual: " << step_4_gmres_control_1.last_value() << std::endl;
+                pcout << "first residual: " << step_4_gmres_control_1.initial_value() << std::endl;
+                pcout << "last residual: " << step_4_gmres_control_1.last_value() << std::endl;
                 throw;
             }
 
@@ -505,14 +507,14 @@ namespace SAND {
             } catch (std::exception &exc)
             {
                 std::cerr << "Failure of linear solver step_4_gmres_2" << std::endl;
-                std::cout << "first residual: " << step_4_gmres_control_2.initial_value() << std::endl;
-                std::cout << "last residual: " << step_4_gmres_control_2.last_value() << std::endl;
+                pcout << "first residual: " << step_4_gmres_control_2.initial_value() << std::endl;
+                pcout << "last residual: " << step_4_gmres_control_2.last_value() << std::endl;
                 throw;
             }
         }
         else
         {
-            std::cout << "shouldn't get here";
+            pcout << "shouldn't get here";
             throw;
         }
 
@@ -607,8 +609,8 @@ namespace SAND {
                 } catch (std::exception &exc)
                 {
                     std::cerr << "Failure of linear solver step_5_gmres_1" << std::endl;
-                    std::cout << "first residual: " << step_5_gmres_control_1.initial_value() << std::endl;
-                    std::cout << "last residual: " << step_5_gmres_control_1.last_value() << std::endl;
+                    pcout << "first residual: " << step_5_gmres_control_1.initial_value() << std::endl;
+                    pcout << "last residual: " << step_5_gmres_control_1.last_value() << std::endl;
                     throw;
                 }
 
@@ -620,8 +622,8 @@ namespace SAND {
                 } catch (std::exception &exc)
                 {
                     std::cerr << "Failure of linear solver step_5_gmres_2" << std::endl;
-                    std::cout << "first residual: " << step_5_gmres_control_2.initial_value() << std::endl;
-                    std::cout << "last residual: " << step_5_gmres_control_2.last_value() << std::endl;
+                    pcout << "first residual: " << step_5_gmres_control_2.initial_value() << std::endl;
+                    pcout << "last residual: " << step_5_gmres_control_2.last_value() << std::endl;
                     throw;
                 }
             }
@@ -652,8 +654,8 @@ namespace SAND {
                 } catch (std::exception &exc)
                 {
                     std::cerr << "Failure of linear solver step_5_gmres_1" << std::endl;
-                    std::cout << "first residual: " << step_5_gmres_control_1.initial_value() << std::endl;
-                    std::cout << "last residual: " << step_5_gmres_control_1.last_value() << std::endl;
+                    pcout << "first residual: " << step_5_gmres_control_1.initial_value() << std::endl;
+                    pcout << "last residual: " << step_5_gmres_control_1.last_value() << std::endl;
                     throw;
                 }
 
@@ -665,14 +667,14 @@ namespace SAND {
                 } catch (std::exception &exc)
                 {
                     std::cerr << "Failure of linear solver step_5_gmres_2" << std::endl;
-                    std::cout << "first residual: " << step_5_gmres_control_2.initial_value() << std::endl;
-                    std::cout << "last residual: " << step_5_gmres_control_2.last_value() << std::endl;
+                    pcout << "first residual: " << step_5_gmres_control_2.initial_value() << std::endl;
+                    pcout << "last residual: " << step_5_gmres_control_2.last_value() << std::endl;
                     throw;
                 }
             }
             else
             {
-                std::cout << "shouldn't get here";
+                pcout << "shouldn't get here";
                 throw;
             }
 
diff --git a/source/watchdog_main.cc b/source/watchdog_main.cc
index 880aeb0..8e50439 100644
--- a/source/watchdog_main.cc
+++ b/source/watchdog_main.cc
@@ -9,6 +9,8 @@
 #include <deal.II/lac/generic_linear_algebra.h>
 #include <deal.II/lac/trilinos_parallel_block_vector.h>
 #include <deal.II/lac/generic_linear_algebra.h>
+#include <deal.II/base/conditional_ostream.h>
+#include <deal.II/base/utilities.h>
 
 ///Above are fairly normal files to include.  I also use the sparse direct package, which requiresBLAS/LAPACK
 /// to  perform  a  direct  solve  while  I  work  on  a  fast  iterative  solver  for  this problem.
@@ -32,7 +34,7 @@ namespace SAND {
         run();
 
     private:
-
+        MPI_Comm  mpi_communicator;
         std::pair<double,double>
         calculate_max_step_size(const LA::MPI::BlockVector &state, const LA::MPI::BlockVector &step) const;
 
@@ -51,13 +53,17 @@ namespace SAND {
         KktSystem<dim> kkt_system;
         MarkovFilter markov_filter;
         double barrier_size;
-        TimerOutput overall_timer;
         bool mixed_barrier_monotone_mode;
+        ConditionalOStream pcout;
+        TimerOutput overall_timer;
     };
 
     template<int dim>
     SANDTopOpt<dim>::SANDTopOpt()
-            :overall_timer(std::cout, TimerOutput::never, TimerOutput::wall_times)
+            :
+              mpi_communicator(MPI_COMM_WORLD),
+              pcout(std::cout,(Utilities::MPI::this_mpi_process(mpi_communicator) == 0)),
+              overall_timer(pcout, TimerOutput::never, TimerOutput::wall_times)
     {
     }
 
@@ -103,7 +109,7 @@ namespace SAND {
                 step_size_z_high = step_size_z;
             }
         }
-        std::cout << "s step : " << step_size_s_low << " z size : " << step_size_z_low << std::endl;
+        pcout << "s step : " << step_size_s_low << " z size : " << step_size_z_low << std::endl;
         return {step_size_s_low, step_size_z_low};
     }
 
@@ -114,8 +120,9 @@ namespace SAND {
     SANDTopOpt<dim>::find_max_step(const LA::MPI::BlockVector &state)
     {
         kkt_system.assemble_block_system(state, barrier_size);
+        pcout << "pre" << std::endl;
         const LA::MPI::BlockVector step = kkt_system.solve(state);
-
+        pcout << "post" << std::endl;
         const auto max_step_sizes= calculate_max_step_size(state,step);
         const double step_size_s = max_step_sizes.first;
         const double step_size_z = max_step_sizes.second;
@@ -132,6 +139,7 @@ namespace SAND {
         max_step.block(SolutionBlocks::displacement_multiplier) = step_size_z * step.block(SolutionBlocks::displacement_multiplier);
         max_step.block(SolutionBlocks::total_volume_multiplier) = step_size_z * step.block(SolutionBlocks::total_volume_multiplier);
 
+        pcout << "here" << std::endl;
         return max_step;
     }
 
@@ -216,7 +224,7 @@ namespace SAND {
                             + current_state.block(SolutionBlocks::density_upper_slack)*current_state.block(SolutionBlocks::density_upper_slack_multiplier)
                            )/(2*vect_size);
             double loqo_complimentarity_deviation = loqo_min/loqo_average;
-            std::cout << "loqo cd: " << loqo_complimentarity_deviation << std::endl;
+            pcout << "loqo cd: " << loqo_complimentarity_deviation << std::endl;
             double loqo_multiplier;
             if((.05 * (1-loqo_complimentarity_deviation)/loqo_complimentarity_deviation)<2)
             {
@@ -226,7 +234,7 @@ namespace SAND {
             {
                 loqo_multiplier = .8;
             }
-            std::cout << "loqo mult: " << loqo_multiplier << std::endl;
+            pcout << "loqo mult: " << loqo_multiplier << std::endl;
             if (loqo_multiplier< 0)
             {
                 barrier_size = std::abs(loqo_multiplier) * loqo_average;
@@ -261,7 +269,7 @@ namespace SAND {
                 {
                     barrier_size = barrier_size * .8;
                     mixed_barrier_monotone_mode=false;
-                    std::cout << "monotone mode turned off" << std::endl;
+                    pcout << "monotone mode turned off" << std::endl;
                 }
             }
             else
@@ -309,7 +317,7 @@ namespace SAND {
                 {
                     loqo_multiplier = 1/.8;
                     mixed_barrier_monotone_mode = true;
-                    std::cout << "monotone mode turned on" << std::endl;
+                    pcout << "monotone mode turned on" << std::endl;
                 }
                 if (loqo_multiplier<.01)
                 {
@@ -336,7 +344,7 @@ namespace SAND {
         barrier_size = Input::initial_barrier_size;
         kkt_system.create_triangulation();
         kkt_system.setup_boundary_values();
-        std::cout << "setup kkt system" << std::endl;
+        pcout << "setup kkt system" << std::endl;
         kkt_system.setup_block_system();
 
         if (Input::barrier_reduction==BarrierOptions::mixed)
@@ -389,7 +397,7 @@ namespace SAND {
                         //iterate number of steps by number of steps taken in this process
                         iteration_number = iteration_number + k + 1;
                         found_step = true;
-                        std::cout << "found workable step after " << k+1 << " iterations"<<std::endl;
+                        pcout << "found workable step after " << k+1 << " iterations"<<std::endl;
                         //break for loop
                         markov_filter.add_point(kkt_system.calculate_objective_value(current_state), kkt_system.calculate_barrier_distance(current_state), kkt_system.calculate_feasibility(current_state,barrier_size));
                         break;
@@ -442,7 +450,7 @@ namespace SAND {
                 converged = check_convergence(current_state);
                 update_barrier(current_state);
                 markov_filter.update_barrier_value(barrier_size);
-                std::cout << "barrier size is now " << barrier_size << " on iteration number " << iteration_number << std::endl;
+                pcout << "barrier size is now " << barrier_size << " on iteration number " << iteration_number << std::endl;
 
                 overall_timer.leave_subsection();
                 overall_timer.print_summary();

From b05e85f3dc5166d660cb900e5596105f12705142 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Fri, 10 Dec 2021 14:34:43 -0700
Subject: [PATCH 46/95] removed some operators

---
 include/schur_preconditioner.h.autosave | 178 ------------------------
 source/schur_preconditioner.cc          |  67 ---------
 2 files changed, 245 deletions(-)
 delete mode 100644 include/schur_preconditioner.h.autosave

diff --git a/include/schur_preconditioner.h.autosave b/include/schur_preconditioner.h.autosave
deleted file mode 100644
index 1cca598..0000000
--- a/include/schur_preconditioner.h.autosave
+++ /dev/null
@@ -1,178 +0,0 @@
-//
-// Created by justin on 3/2/21.
-//
-
-#ifndef SAND_SCHUR_PRECONDITIONER_H
-#define SAND_SCHUR_PRECONDITIONER_H
-#include <deal.II/base/quadrature_lib.h>
-#include <deal.II/base/function.h>
-#include <deal.II/base/tensor.h>
-#include <deal.II/base/timer.h>
-
-#include <deal.II/lac/block_vector.h>
-#include <deal.II/lac/full_matrix.h>
-#include <deal.II/lac/block_sparse_matrix.h>
-#include <deal.II/lac/precondition.h>
-#include <deal.II/lac/linear_operator.h>
-#include <deal.II/lac/linear_operator_tools.h>
-#include <deal.II/lac/packaged_operation.h>
-#include <deal.II/lac/sparse_direct.h>
-#include <deal.II/lac/solver_gmres.h>
-#include <deal.II/lac/solver_cg.h>
-#include <deal.II/lac/solver_bicgstab.h>
-#include <deal.II/lac/generic_linear_algebra.h>
-#include <deal.II/lac/matrix_out.h>
-#include <deal.II/lac/trilinos_sparse_matrix.h>
-#include <deal.II/lac/trilinos_block_sparse_matrix.h>
-
-#include <deal.II/grid/tria.h>
-#include <deal.II/grid/grid_generator.h>
-#include <deal.II/grid/grid_refinement.h>
-
-#include <deal.II/dofs/dof_handler.h>
-#include <deal.II/dofs/dof_renumbering.h>
-#include <deal.II/dofs/dof_tools.h>
-
-#include <deal.II/fe/fe_values.h>
-#include <deal.II/fe/fe_dgq.h>
-#include <deal.II/fe/fe_system.h>
-#include <deal.II/fe/fe_q.h>
-
-#include <deal.II/numerics/vector_tools.h>
-#include <deal.II/numerics/matrix_tools.h>
-#include <deal.II/numerics/data_out.h>
-#include <deal.II/numerics/error_estimator.h>
-#include <deal.II/base/config.h>
-#include <deal.II/base/numbers.h>
-#include <deal.II/base/table.h>
-#include <deal.II/base/tensor.h>
-#include <deal.II/base/timer.h>
-
-#include <deal.II/differentiation/ad/ad_number_traits.h>
-
-#include <deal.II/lac/exceptions.h>
-#include <deal.II/lac/identity_matrix.h>
-
-
-#include <cstring>
-#include <iomanip>
-#include <vector>
-
-
-#include "../include/parameters_and_components.h"
-
-#include <deal.II/base/conditional_ostream.h>
-
-#include <iostream>
-#include <algorithm>
-
-
-namespace SAND
-{
-    namespace LA
-    {
-        using namespace dealii::LinearAlgebraTrilinos;
-    }
-    using namespace dealii;
-
-    class VmultTrilinosSolverDirect : public TrilinosWrappers::SparseMatrix {
-        public:
-            VmultTrilinosSolverDirect(SolverControl &cn,
-                                      const TrilinosWrappers::SolverDirect::AdditionalData &data
-                                      );
-            void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
-            void vmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const;
-            void Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
-            void Tvmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const;
-            void initialize(LA::MPI::SparseMatrix &a_mat);
-            unsigned int m() const;
-            unsigned int n() const;
-            int get_size()
-            {
-                return size;
-            }
-        private:
-            mutable TrilinosWrappers::SolverDirect solver_direct;
-            int size;
-    };
-
-    template<int dim>
-    class TopOptSchurPreconditioner: public Subscriptor {
-    public:
-        TopOptSchurPreconditioner(LA::MPI::BlockSparseMatrix &matrix_in);
-        void initialize (LA::MPI::BlockSparseMatrix &matrix, const std::map<types::global_dof_index, double> &boundary_values, const DoFHandler<dim> &dof_handler, const LA::MPI::BlockVector &state, const LA::MPI::BlockVector &distributed_state);
-        void vmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
-        void Tvmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
-        void vmult_add(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
-        void Tvmult_add(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
-        void clear();
-        unsigned int m() const;
-        unsigned int n() const;
-        void get_sparsity_pattern(BlockDynamicSparsityPattern &bdsp);
-
-        void assemble_mass_matrix(const LA::MPI::BlockVector &state, const hp::FECollection<dim> &fe_system, const DoFHandler<dim> &dof_handler, const AffineConstraints<double> &constraints,   const BlockSparsityPattern &bsp);
-
-        void print_stuff();
-
-        LA::MPI::BlockSparseMatrix &system_matrix;
-
-    private:
-        MPI_Comm  mpi_communicator;
-        unsigned int n_rows;
-        unsigned int n_columns;
-        unsigned int n_block_rows;
-        unsigned int n_block_columns;
-        void vmult_step_1(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
-        void vmult_step_2(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
-        void vmult_step_3(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
-        void vmult_step_4(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
-        void vmult_step_5(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
-
-        BlockSparsityPattern mass_sparsity;
-        LA::MPI::BlockSparseMatrix approx_h_mat;
-
-        SolverControl other_solver_control;
-        mutable SolverBicgstab<Vector<double>> other_bicgstab;
-        mutable SolverGMRES<Vector<double>> other_gmres;
-        mutable SolverCG<Vector<double>> other_cg;
-
-        LA::MPI::SparseMatrix &a_mat;
-        const LA::MPI::SparseMatrix &b_mat;
-        const LA::MPI::SparseMatrix &c_mat;
-        const LA::MPI::SparseMatrix &e_mat;
-        const LA::MPI::SparseMatrix &f_mat;
-        const LA::MPI::SparseMatrix &d_m_mat;
-        const LA::MPI::SparseMatrix &d_1_mat;
-        const LA::MPI::SparseMatrix &d_2_mat;
-        const LA::MPI::SparseMatrix &m_vect;
-
-        LA::MPI::SparseMatrix d_3_mat;
-        LA::MPI::SparseMatrix d_4_mat;
-        LA::MPI::SparseMatrix d_5_mat;
-        LA::MPI::SparseMatrix d_6_mat;
-        LA::MPI::SparseMatrix d_7_mat;
-        LA::MPI::SparseMatrix d_8_mat;
-        LA::MPI::SparseMatrix d_m_inv_mat;
-
-        FullMatrix<double> g_mat;
-        FullMatrix<double> h_mat;
-        FullMatrix<double> k_inv_mat;
-        LAPACKFullMatrix<double> k_mat;
-
-        mutable LA::MPI::Vector pre_j;
-        mutable LA::MPI::Vector pre_k;
-        mutable LA::MPI::Vector g_d_m_inv_density;
-        mutable LA::MPI::Vector k_g_d_m_inv_density;
-
-        std::string solver_type;
-        TrilinosWrappers::SolverDirect::AdditionalData additional_data;
-        SolverControl direct_solver_control;
-        mutable VmultTrilinosSolverDirect a_inv_direct;
-//        TrilinosWrappers::SolverDirect a_inv_direct;
-        mutable TimerOutput timer;
-//        ConditionalOStream pcout;
-
-    };
-
-}
-#endif //SAND_SCHUR_PRECONDITIONER_H
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 4afe7a3..b9d96bd 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -234,68 +234,7 @@ namespace SAND {
         pre_k=distributed_state.block(SolutionBlocks::density);
         g_d_m_inv_density=distributed_state.block(SolutionBlocks::density);
         k_g_d_m_inv_density=distributed_state.block(SolutionBlocks::density);
-        LinearOperator<VectorType,VectorType,PayloadType> op_g;
-        LinearOperator<VectorType,VectorType,PayloadType> op_h;
-        LinearOperator<VectorType,VectorType,PayloadType> op_f;
-        LinearOperator<VectorType,VectorType,PayloadType> op_d_8;
-        op_f = linear_operator<VectorType,VectorType,PayloadType>(f_mat);
-        op_d_8 = linear_operator<VectorType,VectorType,PayloadType>(d_8_mat);
-        op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) * transpose_operator<VectorType, VectorType, PayloadType>(linear_operator<VectorType,VectorType,PayloadType>(f_mat));
 
-        pcout << "ops made" << std::endl;
-
-        if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
-        {
-            SolverControl            solver_control(100000, 1e-6);
-            SolverCG<LA::MPI::Vector> a_solver_cg(solver_control);
-            auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg,PreconditionIdentity());
-            op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
-                   - transpose_operator<VectorType, VectorType, PayloadType>(linear_operator<VectorType, VectorType, PayloadType>(c_mat)) * a_inv_op * linear_operator<VectorType,VectorType,PayloadType>(e_mat)
-                   - transpose_operator<VectorType, VectorType, PayloadType>(linear_operator<VectorType, VectorType, PayloadType>(e_mat)) * a_inv_op * linear_operator<VectorType,VectorType,PayloadType>(c_mat);
-        }
-        else
-        {
-            auto a_inv_op =linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct);
-            op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
-                   - transpose_operator<VectorType, VectorType, PayloadType>(c_mat) * a_inv_op * linear_operator<VectorType, VectorType, PayloadType>(e_mat)
-                   - transpose_operator<VectorType, VectorType, PayloadType>(e_mat) * a_inv_op * linear_operator<VectorType, VectorType, PayloadType>(c_mat);
-        }
-
-
-        if(Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres || Input::solver_choice == SolverOptions::inexact_K_with_exact_A_gmres)
-        {
-
-        }
-        else
-        {
-//            {
-//                TimerOutput::Scope t(timer, "build g_mat");
-//                g_mat.reinit(b_mat.n(), b_mat.n());
-//                build_matrix_element_by_element(op_g, g_mat);
-//            }
-//
-//
-//            {
-//                TimerOutput::Scope t(timer, "build h_mat");
-//                h_mat.reinit(b_mat.n(), b_mat.n());
-//                build_matrix_element_by_element(op_h, h_mat);
-//            }
-//
-//            {
-//                TimerOutput::Scope t(timer, "build k_inv_mat");
-//                auto op_k_inv = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h -
-//                                linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
-//                k_inv_mat.reinit(b_mat.n(), b_mat.n());
-//                build_matrix_element_by_element(op_k_inv, k_inv_mat);
-//            }
-        }
-
-        if (Input::solver_choice == SolverOptions::exact_preconditioner_with_gmres)
-        {
-            TimerOutput::Scope t(timer, "invert k_mat");
-            k_mat.copy_from(k_inv_mat);
-            k_mat.invert();
-        }
     }
 
 
@@ -360,12 +299,6 @@ namespace SAND {
     void TopOptSchurPreconditioner<dim>::vmult_step_2(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
         dst = src;
         auto dst_temp = dst;
-//        auto temp = src.block(SolutionBlocks::unfiltered_density);
-//        temp = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) * src.block(SolutionBlocks::unfiltered_density);
-//        pcout <<std::endl <<std::endl<< "temp output:" << std::endl;
-//        temp.print(pcout);
-//        pcout << std::endl;
-
 
         dst.block(SolutionBlocks::unfiltered_density_multiplier) = dst_temp.block(SolutionBlocks::unfiltered_density_multiplier)
                 - linear_operator<VectorType,VectorType,PayloadType>(f_mat)*linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) * src.block(SolutionBlocks::unfiltered_density);

From e7d9007ba7d6e0d53f1ff4caa1a4f05f215f4218 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <oconnor@up.math.colostate.edu>
Date: Fri, 17 Dec 2021 15:08:19 -0700
Subject: [PATCH 47/95] mod

---
 include/input_information.h    |  2 +-
 include/schur_preconditioner.h | 17 ++++++++++
 source/schur_preconditioner.cc | 58 ++++++++++++++++------------------
 3 files changed, 45 insertions(+), 32 deletions(-)

diff --git a/include/input_information.h b/include/input_information.h
index 5fc4649..13ccac3 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -17,7 +17,7 @@ namespace SAND {
         //geometry options
         constexpr unsigned int geometry_base = GeometryOptions::mbb;
         constexpr unsigned int dim = 3;
-        constexpr unsigned int refinements =  4;
+        constexpr unsigned int refinements =  3;
 
         //nonlinear algorithm options
         constexpr double initial_barrier_size = 25;
diff --git a/include/schur_preconditioner.h b/include/schur_preconditioner.h
index 724f7f1..be1f39e 100644
--- a/include/schur_preconditioner.h
+++ b/include/schur_preconditioner.h
@@ -69,6 +69,10 @@
 
 namespace SAND
 {
+    using MatrixType  = dealii::TrilinosWrappers::SparseMatrix;
+    using VectorType  = dealii::TrilinosWrappers::MPI::Vector;
+    using PayloadType = dealii::TrilinosWrappers::internal::LinearOperatorImplementation::TrilinosPayload;
+    using PayloadVectorType = typename PayloadType::VectorType;
     namespace LA
     {
         using namespace dealii::LinearAlgebraTrilinos;
@@ -172,6 +176,19 @@ namespace SAND
         mutable TimerOutput timer;
         ConditionalOStream pcout;
 
+        LinearOperator<VectorType,VectorType,PayloadType> op_d_8;
+        LinearOperator<VectorType,VectorType,PayloadType> op_f;
+        LinearOperator<VectorType,VectorType,PayloadType> op_b;
+        LinearOperator<VectorType,VectorType,PayloadType> op_c;
+        LinearOperator<VectorType,VectorType,PayloadType> op_a_inv;
+        LinearOperator<VectorType,VectorType,PayloadType> op_e;
+        LinearOperator<VectorType,VectorType,PayloadType> op_d_m;
+        LinearOperator<VectorType,VectorType,PayloadType> op_d_m_inv;
+        LinearOperator<VectorType,VectorType,PayloadType> op_g;
+        LinearOperator<VectorType,VectorType,PayloadType> op_h;
+        LinearOperator<VectorType,VectorType,PayloadType> op_k_inv;
+
+
     };
 
 }
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index b9d96bd..0e774c9 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -235,6 +235,31 @@ namespace SAND {
         g_d_m_inv_density=distributed_state.block(SolutionBlocks::density);
         k_g_d_m_inv_density=distributed_state.block(SolutionBlocks::density);
 
+
+        op_d_8 = linear_operator<VectorType,VectorType,PayloadType>(d_8_mat);
+        op_f = linear_operator<VectorType,VectorType,PayloadType>(f_mat);
+        op_b = linear_operator<VectorType,VectorType,PayloadType>(b_mat);
+        op_c = linear_operator<VectorType,VectorType,PayloadType>(c_mat);
+        op_a_inv = linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct);
+        op_e = linear_operator<VectorType,VectorType,PayloadType>(e_mat);
+        op_d_m= linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
+        op_d_m_inv= linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat);
+
+        op_g = op_f * op_d_8 *
+                transpose_operator(op_f);
+
+
+        op_h = op_b
+               - transpose_operator(op_c) * op_a_inv * op_e
+               - transpose_operator(op_e) * op_a_inv * op_c;
+
+//        TrilinosWrappers::internal::LinearOperatorImplementation::TrilinosPayload inv_payload = inverse_payload(TrilinosWrappers::SolverGMRES(),TrilinosWrappers::PreconditionIdentity());
+
+        op_k_inv = (-1 * op_g *op_d_m_inv * op_h - op_d_m);
+
+
+
+
     }
 
 
@@ -352,24 +377,6 @@ namespace SAND {
 
         else if (Input::solver_choice == SolverOptions::inexact_K_with_exact_A_gmres)
         {
-            auto op_d_8 = linear_operator<VectorType,VectorType,PayloadType>(d_8_mat);
-            auto op_f = linear_operator<VectorType,VectorType,PayloadType>(f_mat);
-            auto op_b = linear_operator<VectorType,VectorType,PayloadType>(b_mat);
-            auto op_c = linear_operator<VectorType,VectorType,PayloadType>(c_mat);
-            auto op_a_inv = linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct);
-            auto op_e = linear_operator<VectorType,VectorType,PayloadType>(e_mat);
-            auto op_d_m= linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
-            auto op_d_m_inv= linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat);
-
-            auto op_g = op_f * op_d_8 *
-                    transpose_operator(op_f);
-
-
-            auto op_h = op_b
-                   - transpose_operator(op_c) * op_a_inv * op_e
-                   - transpose_operator(op_e) * op_a_inv * op_c;
-
-            auto op_k_inv = -1 * op_g *op_d_m_inv * op_h - op_d_m;
 
             g_d_m_inv_density = op_g * op_d_m_inv * src.block(SolutionBlocks::density);
             SolverControl step_4_gmres_control_1 (100000, g_d_m_inv_density.l2_norm()*1e-6);
@@ -518,22 +525,11 @@ namespace SAND {
 
             else if (Input::solver_choice == SolverOptions::inexact_K_with_exact_A_gmres)
             {
-                auto op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) *
-                            transpose_operator(linear_operator<VectorType,VectorType,PayloadType>(f_mat));
-
-                auto op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
-                            - transpose_operator<VectorType, VectorType, PayloadType>(c_mat) *
-                                    linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct) * linear_operator<VectorType,VectorType,PayloadType>(e_mat)
-                            - transpose_operator<VectorType, VectorType, PayloadType>(e_mat) * linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct) * linear_operator<VectorType,VectorType,PayloadType>(c_mat);
-
-                auto op_k_inv = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
-
                 pre_j = src.block(SolutionBlocks::density) + op_h * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
                 auto pre_pre_k = pre_k;
                 pre_pre_k = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
                 pre_k =  pre_pre_k + src.block(SolutionBlocks::unfiltered_density_multiplier);
 
-
                 SolverControl step_5_gmres_control_1 (100000, pre_j.l2_norm()*1e-6);
                 SolverGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
                 try {
@@ -580,7 +576,7 @@ namespace SAND {
                 pre_k = -1* op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density) + src.block(SolutionBlocks::unfiltered_density_multiplier);
 
                 SolverControl step_5_gmres_control_1 (10000, std::max(pre_j.l2_norm()*1e-6,1e-6));
-                SolverGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
+                TrilinosWrappers::SolverGMRES step_5_gmres_1 (step_5_gmres_control_1);
                 try {
                     dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType,VectorType,PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
                                                                                pre_j;
@@ -593,7 +589,7 @@ namespace SAND {
                 }
 
                 SolverControl step_5_gmres_control_2 (10000, std::max(pre_k.l2_norm()*1e-6,1e-6));
-                SolverGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
+                TrilinosWrappers::SolverGMRES step_5_gmres_2 (step_5_gmres_control_2);
                 try {
                     dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) *
                                                          pre_k;

From 41de392620c09421279456ae4bb28ce970702746 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Mon, 20 Dec 2021 09:39:12 -0700
Subject: [PATCH 48/95] stuff

---
 include/input_information.h             |    2 +-
 source/kkt_system.cc.autosave           | 2241 +++++++++++++++++++++++
 source/schur_preconditioner.cc          |    2 +-
 source/schur_preconditioner.cc.autosave |  693 +++++++
 4 files changed, 2936 insertions(+), 2 deletions(-)
 create mode 100644 source/kkt_system.cc.autosave
 create mode 100644 source/schur_preconditioner.cc.autosave

diff --git a/include/input_information.h b/include/input_information.h
index 5fc4649..e268732 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -17,7 +17,7 @@ namespace SAND {
         //geometry options
         constexpr unsigned int geometry_base = GeometryOptions::mbb;
         constexpr unsigned int dim = 3;
-        constexpr unsigned int refinements =  4;
+        constexpr unsigned int refinements =  5;
 
         //nonlinear algorithm options
         constexpr double initial_barrier_size = 25;
diff --git a/source/kkt_system.cc.autosave b/source/kkt_system.cc.autosave
new file mode 100644
index 0000000..788ed43
--- /dev/null
+++ b/source/kkt_system.cc.autosave
@@ -0,0 +1,2241 @@
+//
+// Created by justin on 2/17/21.
+//
+#include "../include/kkt_system.h"
+#include <deal.II/base/quadrature_lib.h>
+#include <deal.II/base/function.h>
+#include <deal.II/base/tensor.h>
+#include <deal.II/base/timer.h>
+
+#include <deal.II/lac/full_matrix.h>
+#include <deal.II/lac/linear_operator.h>
+#include <deal.II/lac/solver_gmres.h>
+#include <deal.II/lac/generic_linear_algebra.h>
+#include <deal.II/lac/trilinos_sparse_matrix.h>
+
+
+#include <deal.II/lac/matrix_out.h>
+
+#include <deal.II/grid/grid_generator.h>
+
+#include <deal.II/dofs/dof_renumbering.h>
+#include <deal.II/dofs/dof_tools.h>
+
+#include <deal.II/fe/fe_values.h>
+#include <deal.II/fe/fe_dgq.h>
+#include <deal.II/fe/fe_system.h>
+#include <deal.II/fe/fe_q.h>
+#include <deal.II/fe/fe_nothing.h>
+
+#include <deal.II/hp/fe_collection.h>
+#include <deal.II/hp/fe_values.h>
+
+#include <deal.II/numerics/vector_tools.h>
+#include <deal.II/numerics/matrix_tools.h>
+#include <deal.II/numerics/data_out.h>
+#include <deal.II/numerics/error_estimator.h>
+
+
+#include <deal.II/base/conditional_ostream.h>
+
+#include "../include/input_information.h"
+
+#include <iostream>
+#include <algorithm>
+
+// This problem initializes with a FESystem composed of 2×dim FE_Q(1) elements, and 8 FE_DGQ(0)  elements.
+// The  piecewise  constant  functions  are  for  density-related  variables,and displacement-related variables are assigned to the FE_Q(1) elements.
+namespace SAND {
+    template<int dim>
+    KktSystem<dim>::KktSystem()
+            :
+            mpi_communicator(MPI_COMM_WORLD),
+            triangulation(mpi_communicator,
+                          typename Triangulation<dim>::MeshSmoothing(
+                                  Triangulation<dim>::smoothing_on_refinement |
+                                  Triangulation<dim>::smoothing_on_coarsening)),
+            dof_handler(triangulation),
+            /*fe should have 1 FE_DGQ<dim>(0) element for density, dim FE_Q finite elements for displacement,
+             * another dim FE_Q elements for the lagrange multiplier on the FE constraint, and 2 more FE_DGQ<dim>(0)
+             * elements for the upper and lower bound constraints */
+            fe_nine(FE_DGQ<dim>(0) ^ 5,
+                    (FESystem<dim>(FE_Q<dim>(1) ^ dim)) ^ 2,
+                    FE_DGQ<dim>(0) ^ 2,
+                    FE_Nothing<dim>() ^ 1),
+            fe_ten(FE_DGQ<dim>(0) ^ 5,
+                   (FESystem<dim>(FE_Q<dim>(1) ^ dim)) ^ 2,
+                   FE_DGQ<dim>(0) ^ 2,
+                   FE_DGQ<dim>(0) ^ 1),
+            density_ratio(Input::volume_percentage),
+            density_penalty_exponent(Input::density_penalty_exponent),
+            density_filter(),
+            pcout(std::cout,(Utilities::MPI::this_mpi_process(mpi_communicator) == 0))
+    {
+        fe_collection.push_back(fe_nine);
+        fe_collection.push_back(fe_ten);
+
+    }
+
+
+//A  function  used  once  at  the  beginning  of  the  program,  this  creates  a  matrix  H  so  that H* unfiltered density = filtered density
+
+    template<int dim>
+    void
+    KktSystem<dim>::setup_filter_matrix() {
+
+        density_filter.initialize(dof_handler);
+    }
+
+    //This triangulation matches the problem description -
+    // a 6-by-1 rectangle where a force will be applied in the top center.
+
+    template<int dim>
+    void
+    KktSystem<dim>::create_triangulation() {
+
+        std::vector<unsigned int> sub_blocks(2*dim+8, 0);
+
+        sub_blocks[0]=0;
+        sub_blocks[1]=1;
+        sub_blocks[2]=2;
+        sub_blocks[3]=3;
+        sub_blocks[4]=4;
+        for(int i=0; i<dim; i++)
+        {
+            sub_blocks[5+i]=5;
+        }
+        for(int i=0; i<dim; i++)
+        {
+            sub_blocks[5+dim+i]=6;
+        }
+        sub_blocks[5+2*dim]=7;
+        sub_blocks[6+2*dim]=8;
+        sub_blocks[7+2*dim]=9;
+
+
+        if (Input::geometry_base == GeometryOptions::mbb) {
+            const double width = 6;
+            const unsigned int width_refine = 6;
+            const double height = 1;
+            const unsigned int height_refine = 1;
+            const double depth = 1;
+            const unsigned int depth_refine = 1;
+            const double downforce_y = 1;
+            const double downforce_x = 3;
+            const double downforce_size = .3;
+
+            if (dim == 2)
+            {
+                GridGenerator::subdivided_hyper_rectangle(triangulation,
+                                                          {width_refine, height_refine},
+                                                          Point<dim>(0, 0),
+                                                          Point<dim>(width, height));
+
+                triangulation.refine_global(Input::refinements);
+
+                /*Set BCIDs   */
+                for (const auto &cell: dof_handler.active_cell_iterators()) {
+                    if(cell->is_locally_owned())
+                    {
+                        cell->set_active_fe_index(0);
+                        cell->set_material_id(MaterialIds::without_multiplier);
+                        for (unsigned int face_number = 0;
+                             face_number < GeometryInfo<dim>::faces_per_cell;
+                             ++face_number) {
+                            if (cell->face(face_number)->at_boundary()) {
+                                const auto center = cell->face(face_number)->center();
+
+                                if (std::fabs(center(1) - downforce_y) < 1e-12) {
+                                    if (std::fabs(center(0) - downforce_x) < downforce_size) {
+                                        cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
+                                    } else {
+                                        cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
+                                    }
+                                }
+                            }
+                        }
+                        for (unsigned int vertex_number = 0;
+                             vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                             ++vertex_number) {
+                            if (std::abs(cell->vertex(vertex_number)(0)) + std::abs(cell->vertex(vertex_number)(1)) <
+                                1e-10) {
+                                cell->set_active_fe_index(1);
+                                cell->set_material_id(MaterialIds::with_multiplier);
+                            }
+                        }
+                    }
+                }
+
+
+
+                dof_handler.distribute_dofs(fe_collection);
+
+                DoFRenumbering::component_wise(dof_handler, sub_blocks);
+            }
+            else if (dim == 3)
+            {
+                GridGenerator::subdivided_hyper_rectangle(triangulation,
+                                                          {width_refine, height_refine, depth_refine},
+                                                          Point<dim>(0, 0, 0),
+                                                          Point<dim>(width, height, depth));
+
+                triangulation.refine_global(Input::refinements);
+
+                for (const auto &cell: dof_handler.active_cell_iterators()) {
+                    if (cell->is_locally_owned())
+                    {
+                        cell->set_active_fe_index(0);
+                        cell->set_material_id(MaterialIds::without_multiplier);
+                        for (unsigned int face_number = 0;
+                             face_number < GeometryInfo<dim>::faces_per_cell;
+                             ++face_number) {
+                            if (cell->face(face_number)->at_boundary()) {
+                                const auto center = cell->face(face_number)->center();
+
+                                if (std::fabs(center(1) - downforce_y) < 1e-12) {
+                                    if (std::fabs(center(0) - downforce_x) < downforce_size) {
+                                        cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
+                                    } else {
+                                        cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
+                                    }
+                                }
+                            }
+                        }
+                        for (unsigned int vertex_number = 0;
+                             vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                             ++vertex_number) {
+                            if (std::abs(cell->vertex(vertex_number)(0)) + std::abs(cell->vertex(vertex_number)(1))
+                                + std::abs(cell->vertex(vertex_number)(2)) < 1e-10) {
+                                cell->set_active_fe_index(1);
+                                cell->set_material_id(MaterialIds::with_multiplier);
+                            }
+                        }
+                    }
+                }
+
+                dof_handler.distribute_dofs(fe_collection);
+
+                DoFRenumbering::component_wise(dof_handler, sub_blocks);
+
+            } else {
+                throw;
+            }
+        } else if (Input::geometry_base == GeometryOptions::l_shape) {
+            const double width = 2;
+            const unsigned int width_refine = 2;
+            const double height = 2;
+            const unsigned int height_refine = 2;
+            const double depth = 1;
+            const unsigned int depth_refine = 1;
+            const double downforce_x = 2;
+            const double downforce_y = 1;
+            const double downforce_z = .5;
+            const double downforce_size = .3;
+
+            if (dim == 2) {
+                GridGenerator::subdivided_hyper_L(triangulation,
+                                                  {width_refine, height_refine},
+                                                  Point<dim>(0, 0),
+                                                  Point<dim>(width, height),
+                                                  {-1, -1});
+
+                triangulation.refine_global(Input::refinements);
+
+                /*Set BCIDs   */
+                for (const auto &cell: dof_handler.active_cell_iterators()) {
+                    if (cell->is_locally_owned())
+                    {
+                        cell->set_active_fe_index(0);
+                        cell->set_material_id(MaterialIds::without_multiplier);
+                        for (unsigned int face_number = 0;
+                             face_number < GeometryInfo<dim>::faces_per_cell;
+                             ++face_number) {
+                            if (cell->face(face_number)->at_boundary()) {
+                                const auto center = cell->face(face_number)->center();
+
+                                if (std::fabs(center(0) - downforce_x) < 1e-12) {
+                                    if (std::fabs(center(1) - downforce_y) < downforce_size) {
+                                        cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
+                                    } else {
+                                        cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
+                                    }
+                                }
+                            }
+                        }
+                        for (unsigned int vertex_number = 0;
+                             vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                             ++vertex_number) {
+                            if (std::abs(cell->vertex(vertex_number)(0)) + std::abs(cell->vertex(vertex_number)(1)) <
+                                1e-10) {
+                                cell->set_active_fe_index(1);
+                                cell->set_material_id(MaterialIds::with_multiplier);
+                            }
+                        }
+                    }
+                }
+
+                dof_handler.distribute_dofs(fe_collection);
+
+                DoFRenumbering::component_wise(dof_handler, sub_blocks);
+
+            } else if (dim == 3) {
+                GridGenerator::subdivided_hyper_L(triangulation,
+                                                  {width_refine, height_refine, depth_refine},
+                                                  Point<dim>(0, 0, 0),
+                                                  Point<dim>(width, height, depth),
+                                                  {-1, -1, depth_refine});
+
+                triangulation.refine_global(Input::refinements);
+
+                /*Set BCIDs   */
+                for (const auto &cell: dof_handler.active_cell_iterators()) {
+                    if(cell->is_locally_owned())
+                    {
+                        cell->set_active_fe_index(0);
+                        cell->set_material_id(MaterialIds::without_multiplier);
+                        for (unsigned int face_number = 0;
+                             face_number < GeometryInfo<dim>::faces_per_cell;
+                             ++face_number) {
+                            if (cell->face(face_number)->at_boundary()) {
+                                const auto center = cell->face(face_number)->center();
+
+                                if (std::fabs(center(0) - downforce_x) < 1e-12) {
+                                    if (std::fabs(center(1) - downforce_y) < downforce_size) {
+                                        cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
+                                        if (std::fabs(center(2) - downforce_z) < downforce_size) {
+                                            cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
+                                        } else {
+                                            cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
+                                        }
+                                    } else {
+                                        cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
+                                    }
+                                }
+                            }
+                        }
+                        for (unsigned int vertex_number = 0;
+                             vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                             ++vertex_number) {
+                            if (std::abs(cell->vertex(vertex_number)(0)) + std::abs(cell->vertex(vertex_number)(1)) <
+                                1e-10) {
+                                cell->set_active_fe_index(1);
+                                cell->set_material_id(MaterialIds::with_multiplier);
+                            }
+                        }
+                    }
+                }
+
+                dof_handler.distribute_dofs(fe_collection);
+
+                DoFRenumbering::component_wise(dof_handler, sub_blocks);
+            } else {
+                throw;
+            }
+        } else {
+            throw;
+        }
+
+    }
+
+// The  bottom  corners  are  kept  in  place  in  the  y  direction  -  the  bottom  left  also  in  the  x direction.
+// Because deal.ii is formulated to enforce boundary conditions along regions of the boundary,
+// we do this to ensure these BCs are only enforced at points.
+    template<int dim>
+    void
+    KktSystem<dim>::setup_boundary_values() {
+        if (Input::geometry_base == GeometryOptions::mbb) {
+            if (dim == 2) {
+                for (const auto &cell: dof_handler.active_cell_iterators()) {
+                    if(cell->is_locally_owned())
+                    {
+                        for (unsigned int face_number = 0;
+                             face_number < GeometryInfo<dim>::faces_per_cell;
+                             ++face_number) {
+                            if (cell->face(face_number)->at_boundary()) {
+                                for (unsigned int vertex_number = 0;
+                                     vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                                     ++vertex_number) {
+                                    const auto vert = cell->vertex(vertex_number);
+                                    /*Find bottom left corner*/
+                                    if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
+                                            vert(1) - 0) < 1e-12) {
+
+                                        const unsigned int x_displacement =
+                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
+                                        const unsigned int y_displacement =
+                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
+                                        const unsigned int x_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
+                                        const unsigned int y_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
+                                        /*set bottom left BC*/
+                                        boundary_values[x_displacement] = 0;
+                                        boundary_values[y_displacement] = 0;
+                                        boundary_values[x_displacement_multiplier] = 0;
+                                        boundary_values[y_displacement_multiplier] = 0;
+                                    }
+                                    /*Find bottom right corner*/
+                                    if (std::fabs(vert(0) - 6) < 1e-12 && std::fabs(
+                                            vert(1) - 0) < 1e-12) {
+//                            const unsigned int x_displacement =
+//                                    cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
+                                        const unsigned int y_displacement =
+                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
+//                            const unsigned int x_displacement_multiplier =
+//                                    cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
+                                        const unsigned int y_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
+//                            boundary_values[x_displacement] = 0;
+                                        boundary_values[y_displacement] = 0;
+//                            boundary_values[x_displacement_multiplier] = 0;
+                                        boundary_values[y_displacement_multiplier] = 0;
+                                    }
+                                }
+                            }
+                        }
+                    }
+
+                }
+            } else if (dim == 3) {
+                for (const auto &cell: dof_handler.active_cell_iterators()) {
+                    if(cell->is_locally_owned())
+                    {
+                        for (unsigned int face_number = 0;
+                             face_number < GeometryInfo<dim>::faces_per_cell;
+                             ++face_number) {
+                            if (cell->face(face_number)->at_boundary()) {
+                                for (unsigned int vertex_number = 0;
+                                     vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                                     ++vertex_number) {
+                                    const auto vert = cell->vertex(vertex_number);
+                                    /*Find bottom left corner*/
+                                    if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
+                                            vert(1) - 0) < 1e-12 && ((std::fabs(
+                                            vert(2) - 0) < 1e-12) || (std::fabs(
+                                            vert(2) - 1) < 1e-12))) {
+
+
+                                        const unsigned int x_displacement =
+                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
+                                        const unsigned int y_displacement =
+                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
+                                        const unsigned int z_displacement =
+                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
+                                        const unsigned int x_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
+                                        const unsigned int y_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 4, cell->active_fe_index());
+                                        const unsigned int z_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 5, cell->active_fe_index());
+
+                                        boundary_values[x_displacement] = 0;
+                                        boundary_values[y_displacement] = 0;
+                                        boundary_values[z_displacement] = 0;
+                                        boundary_values[x_displacement_multiplier] = 0;
+                                        boundary_values[y_displacement_multiplier] = 0;
+                                        boundary_values[z_displacement_multiplier] = 0;
+                                    }
+                                    /*Find bottom right corner*/
+                                    if (std::fabs(vert(0) - 6) < 1e-12 && std::fabs(
+                                            vert(1) - 0) < 1e-12 && ((std::fabs(
+                                            vert(2) - 0) < 1e-12) || (std::fabs(
+                                            vert(2) - 1) < 1e-12))) {
+//                              const unsigned int x_displacement =
+//                                    cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
+                                        const unsigned int y_displacement =
+                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
+                                        const unsigned int z_displacement =
+                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
+//                              const unsigned int x_displacement_multiplier =
+//                                    cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
+                                        const unsigned int y_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 4, cell->active_fe_index());
+                                        const unsigned int z_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 5, cell->active_fe_index());
+//                              boundary_values[x_displacement] = 0;
+                                        boundary_values[y_displacement] = 0;
+                                        boundary_values[z_displacement] = 0;
+//                              boundary_values[x_displacement_multiplier] = 0;
+                                        boundary_values[y_displacement_multiplier] = 0;
+                                        boundary_values[z_displacement_multiplier] = 0;
+                                    }
+                                }
+                            }
+                        }
+                    }
+                }
+            } else {
+                throw;
+            }
+        } else if (Input::geometry_base == GeometryOptions::l_shape) {
+            if (dim == 2) {
+                for (const auto &cell: dof_handler.active_cell_iterators()) {
+                    if(cell->is_locally_owned())
+                    {
+                        for (unsigned int face_number = 0;
+                             face_number < GeometryInfo<dim>::faces_per_cell;
+                             ++face_number) {
+                            if (cell->face(face_number)->at_boundary()) {
+                                for (unsigned int vertex_number = 0;
+                                     vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                                     ++vertex_number) {
+                                    const auto vert = cell->vertex(vertex_number);
+                                    /*Find top left corner*/
+                                    if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
+                                            vert(1) - 2) < 1e-12) {
+
+                                        const unsigned int x_displacement =
+                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
+                                        const unsigned int y_displacement =
+                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
+                                        const unsigned int x_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
+                                        const unsigned int y_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
+                                        /*set bottom left BC*/
+                                        boundary_values[x_displacement] = 0;
+                                        boundary_values[y_displacement] = 0;
+                                        boundary_values[x_displacement_multiplier] = 0;
+                                        boundary_values[y_displacement_multiplier] = 0;
+                                    }
+                                    /*Find top right corner*/
+                                    if (std::fabs(vert(0) - 1) < 1e-12 && std::fabs(
+                                            vert(1) - 2) < 1e-12) {
+                                        const unsigned int x_displacement =
+                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
+                                        const unsigned int y_displacement =
+                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
+                                        const unsigned int x_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
+                                        const unsigned int y_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
+                                        boundary_values[x_displacement] = 0;
+                                        boundary_values[y_displacement] = 0;
+                                        boundary_values[x_displacement_multiplier] = 0;
+                                        boundary_values[y_displacement_multiplier] = 0;
+                                    }
+                                }
+                            }
+                        }
+                    }
+                }
+            } else if (dim == 3) {
+                for (const auto &cell: dof_handler.active_cell_iterators()) {
+                    if(cell->is_locally_owned())
+                    {
+                        for (unsigned int face_number = 0;
+                             face_number < GeometryInfo<dim>::faces_per_cell;
+                             ++face_number) {
+                            if (cell->face(face_number)->at_boundary()) {
+                                for (unsigned int vertex_number = 0;
+                                     vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                                     ++vertex_number) {
+                                    const auto vert = cell->vertex(vertex_number);
+                                    /*Find bottom left corner*/
+                                    if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
+                                            vert(1) - 2) < 1e-12 && ((std::fabs(
+                                            vert(2) - 0) < 1e-12) || (std::fabs(
+                                            vert(2) - 1) < 1e-12))) {
+
+
+                                        const unsigned int x_displacement =
+                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
+                                        const unsigned int y_displacement =
+                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
+                                        const unsigned int z_displacement =
+                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
+                                        const unsigned int x_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
+                                        const unsigned int y_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 4, cell->active_fe_index());
+                                        const unsigned int z_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 5, cell->active_fe_index());
+
+                                        boundary_values[x_displacement] = 0;
+                                        boundary_values[y_displacement] = 0;
+                                        boundary_values[z_displacement] = 0;
+                                        boundary_values[x_displacement_multiplier] = 0;
+                                        boundary_values[y_displacement_multiplier] = 0;
+                                        boundary_values[z_displacement_multiplier] = 0;
+                                    }
+                                    /*Find bottom right corner*/
+                                    if (std::fabs(vert(0) - 1) < 1e-12 && std::fabs(
+                                            vert(1) - 2) < 1e-12 && ((std::fabs(
+                                            vert(2) - 0) < 1e-12) || (std::fabs(
+                                            vert(2) - 1) < 1e-12))) {
+                                        const unsigned int x_displacement =
+                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
+                                        const unsigned int y_displacement =
+                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
+                                        const unsigned int z_displacement =
+                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
+                                        const unsigned int x_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
+                                        const unsigned int y_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 4, cell->active_fe_index());
+                                        const unsigned int z_displacement_multiplier =
+                                                cell->vertex_dof_index(vertex_number, 5, cell->active_fe_index());
+                                        boundary_values[x_displacement] = 0;
+                                        boundary_values[y_displacement] = 0;
+                                        boundary_values[z_displacement] = 0;
+                                        boundary_values[x_displacement_multiplier] = 0;
+                                        boundary_values[y_displacement_multiplier] = 0;
+                                        boundary_values[z_displacement_multiplier] = 0;
+                                    }
+                                }
+                            }
+                        }
+                    }
+                }
+            } else {
+                throw;
+            }
+        }
+
+
+    }
+
+
+    //This makes a giant 10-by-10 block matrix, and also sets up the necessary block vectors.  The
+    // sparsity pattern for this matrix includes the sparsity pattern for the filter matrix. It also initializes
+    // any block vectors we will use.
+    template<int dim>
+    void
+    KktSystem<dim>::setup_block_system() {
+
+        //MAKE n_u and n_P
+
+        /*Setup 10 by 10 block matrix*/
+        std::vector<unsigned int> block_component(10, 2);
+
+        block_component[0] = 0;
+        block_component[5] = 1;
+
+        const std::vector<types::global_dof_index> dofs_per_block =
+                DoFTools::count_dofs_per_fe_block(dof_handler, block_component);
+        const unsigned int n_p = dofs_per_block[0];
+        const unsigned int n_u = dofs_per_block[1];
+
+        pcout << "n_p:  " << n_p << "   n_u:  " << n_u << std::endl;
+
+        IndexSet locally_owned_dofs = dof_handler.locally_owned_dofs();
+        IndexSet locally_relevant_dofs;
+        DoFTools::extract_locally_relevant_dofs(dof_handler, locally_relevant_dofs);
+
+        dsp.reinit(10, 10);
+        owned_partitioning.resize(10);
+        owned_partitioning[0] = dof_handler.locally_owned_dofs().get_view(0, n_p);
+        owned_partitioning[1] = dof_handler.locally_owned_dofs().get_view(n_p, 2 * n_p);
+        owned_partitioning[2] = dof_handler.locally_owned_dofs().get_view(2 * n_p, 3 * n_p);
+        owned_partitioning[3] = dof_handler.locally_owned_dofs().get_view(3 * n_p, 4 * n_p);
+        owned_partitioning[4] = dof_handler.locally_owned_dofs().get_view(4 * n_p, 5 * n_p);
+        owned_partitioning[5] = dof_handler.locally_owned_dofs().get_view(5 * n_p, 5 * n_p + n_u);
+        owned_partitioning[6] = dof_handler.locally_owned_dofs().get_view(5 * n_p + n_u, 5 * n_p + 2 * n_u);
+        owned_partitioning[7] = dof_handler.locally_owned_dofs().get_view(5 * n_p + 2 * n_u, 6 * n_p + 2 * n_u);
+        owned_partitioning[8] = dof_handler.locally_owned_dofs().get_view(6 * n_p + 2 * n_u, 7 * n_p + 2 * n_u);
+        owned_partitioning[9] = dof_handler.locally_owned_dofs().get_view(7 * n_p + 2 * n_u, 7 * n_p + 2 * n_u + 1);
+        relevant_partitioning.resize(10);
+        relevant_partitioning[0] = locally_relevant_dofs.get_view(0, n_p);
+        relevant_partitioning[1] = locally_relevant_dofs.get_view(n_p, 2 * n_p);
+        relevant_partitioning[2] = locally_relevant_dofs.get_view(2 * n_p, 3 * n_p);
+        relevant_partitioning[3] = locally_relevant_dofs.get_view(3 * n_p, 4 * n_p);
+        relevant_partitioning[4] = locally_relevant_dofs.get_view(4 * n_p, 5 * n_p);
+        relevant_partitioning[5] = locally_relevant_dofs.get_view(5 * n_p, 5 * n_p + n_u);
+        relevant_partitioning[6] = locally_relevant_dofs.get_view(5 * n_p + n_u, 5 * n_p + 2 * n_u);
+        relevant_partitioning[7] = locally_relevant_dofs.get_view(5 * n_p + 2 * n_u, 6 * n_p + 2 * n_u);
+        relevant_partitioning[8] = locally_relevant_dofs.get_view(6 * n_p + 2 * n_u, 7 * n_p + 2 * n_u);
+        relevant_partitioning[9] = locally_relevant_dofs.get_view(7 * n_p + 2 * n_u, 7 * n_p + 2 * n_u + 1);
+
+        const std::vector<unsigned int> block_sizes = {n_p, n_p, n_p, n_p, n_p, n_u, n_u, n_p, n_p, 1};
+
+        for (unsigned int k = 0; k < 10; k++) {
+            for (unsigned int j = 0; j < 10; j++) {
+                dsp.block(j, k).reinit(block_sizes[j], block_sizes[k]);
+            }
+        }
+        dsp.collect_sizes();
+        Table<2, DoFTools::Coupling> coupling(2 * dim + 8, 2 * dim + 8);
+//Coupling for density
+            coupling[SolutionComponents::density<dim>][SolutionComponents::density<dim>] = DoFTools::always;
+
+            for (unsigned int i = 0; i < dim; i++) {
+                coupling[SolutionComponents::density<dim>][SolutionComponents::displacement<dim> +
+                                                           i] = DoFTools::always;
+                coupling[SolutionComponents::displacement<dim> +
+                         i][SolutionComponents::density<dim>] = DoFTools::always;
+            }
+
+            coupling[SolutionComponents::density<dim>][SolutionComponents::unfiltered_density_multiplier<dim>] = DoFTools::always;
+            coupling[SolutionComponents::unfiltered_density_multiplier<dim>][SolutionComponents::density<dim>] = DoFTools::always;
+
+            for (unsigned int i = 0; i < dim; i++) {
+                coupling[SolutionComponents::density<dim>][SolutionComponents::displacement_multiplier<dim> +
+                                                           i] = DoFTools::always;
+                coupling[SolutionComponents::displacement_multiplier<dim> +
+                         i][SolutionComponents::density<dim>] = DoFTools::always;
+            }
+
+//Coupling for displacement
+            for (unsigned int i = 0; i < dim; i++) {
+
+                for (unsigned int k = 0; k < dim; k++) {
+                    coupling[SolutionComponents::displacement<dim> + i][
+                            SolutionComponents::displacement_multiplier<dim> +
+                            k] = DoFTools::always;
+                    coupling[SolutionComponents::displacement_multiplier<dim> + k][
+                            SolutionComponents::displacement<dim> +
+                            i] = DoFTools::always;
+                }
+            }
+
+// coupling for unfiltered density
+            coupling[SolutionComponents::unfiltered_density<dim>][SolutionComponents::density_lower_slack_multiplier<dim>] = DoFTools::always;
+            coupling[SolutionComponents::density_lower_slack_multiplier<dim>][SolutionComponents::unfiltered_density<dim>] = DoFTools::always;
+
+            coupling[SolutionComponents::unfiltered_density<dim>][SolutionComponents::density_upper_slack_multiplier<dim>] = DoFTools::always;
+            coupling[SolutionComponents::density_upper_slack_multiplier<dim>][SolutionComponents::unfiltered_density<dim>] = DoFTools::always;
+
+            coupling[SolutionComponents::unfiltered_density<dim>][SolutionComponents::unfiltered_density_multiplier<dim>] = DoFTools::always;
+            coupling[SolutionComponents::unfiltered_density_multiplier<dim>][SolutionComponents::unfiltered_density<dim>] = DoFTools::always;
+
+
+
+
+//        Coupling for lower slack
+            coupling[SolutionComponents::density_lower_slack<dim>][SolutionComponents::density_lower_slack<dim>] = DoFTools::always;
+
+            coupling[SolutionComponents::density_lower_slack<dim>][SolutionComponents::density_lower_slack_multiplier<dim>] = DoFTools::always;
+            coupling[SolutionComponents::density_lower_slack_multiplier<dim>][SolutionComponents::density_lower_slack<dim>] = DoFTools::always;
+
+//
+            coupling[SolutionComponents::density_upper_slack<dim>][SolutionComponents::density_upper_slack<dim>] = DoFTools::always;
+            coupling[SolutionComponents::density_upper_slack<dim>][SolutionComponents::density_upper_slack_multiplier<dim>] = DoFTools::always;
+            coupling[SolutionComponents::density_upper_slack_multiplier<dim>][SolutionComponents::density_upper_slack<dim>] = DoFTools::always;
+
+            coupling[SolutionComponents::density_upper_slack_multiplier<dim>][SolutionComponents::density_upper_slack_multiplier<dim>] = DoFTools::always;
+            constraints.reinit(locally_relevant_dofs);
+            constraints.clear();
+            DoFTools::make_hanging_node_constraints(dof_handler,constraints);
+            constraints.close();
+
+            system_matrix.clear();
+
+//            DoFTools::make_sparsity_pattern(dof_handler, coupling, dsp, constraints, false);
+            DoFTools::make_sparsity_pattern(dof_handler, dsp, constraints, false);
+            SparsityTools::distribute_sparsity_pattern(dsp, dof_handler.locally_owned_dofs(), mpi_communicator,
+                                                       locally_relevant_dofs);
+            //adds the row into the sparsity pattern for the total volume constraint
+            for (const auto &cell: dof_handler.active_cell_iterators())
+            {
+                if (cell->is_locally_owned())
+                {
+                    std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
+                    cell->get_dof_indices(i);
+                    dsp.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier).add(i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)], 0);
+                    dsp.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).add(0, i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)]);
+                }
+
+            }
+
+            /*This finds neighbors whose values would be relevant, and adds them to the sparsity pattern of the matrix*/
+            setup_filter_matrix();
+            for (const auto &cell : dof_handler.active_cell_iterators()) {
+                if (cell->is_locally_owned())
+                {
+                    std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
+                    cell->get_dof_indices(i);
+                    for (const auto &neighbor_cell : density_filter.find_relevant_neighbors(cell)) {
+                        std::vector<types::global_dof_index> j(neighbor_cell->get_fe().n_dofs_per_cell());
+                        neighbor_cell->get_dof_indices(j);
+
+                        dsp.block(SolutionBlocks::unfiltered_density_multiplier,
+                                  SolutionBlocks::unfiltered_density).add(i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)], j[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)]);
+                        dsp.block(SolutionBlocks::unfiltered_density,
+                                  SolutionBlocks::unfiltered_density_multiplier).add(j[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)], i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)]);
+                    }
+                }
+            }
+
+
+            SparsityTools::distribute_sparsity_pattern(
+                    dsp,
+                    Utilities::MPI::all_gather(mpi_communicator,
+                                               dof_handler.locally_owned_dofs()),
+                    mpi_communicator,
+                    locally_relevant_dofs);
+            DoFTools::extract_locally_relevant_dofs(dof_handler, locally_relevant_dofs);
+            system_matrix.reinit(owned_partitioning, dsp, mpi_communicator);
+
+            locally_relevant_solution.reinit(owned_partitioning, relevant_partitioning, mpi_communicator);
+            distributed_solution.reinit(owned_partitioning, mpi_communicator);
+            system_rhs.reinit(owned_partitioning, mpi_communicator);
+
+            locally_relevant_solution.collect_sizes();
+            distributed_solution.collect_sizes();
+            system_rhs.collect_sizes();
+            system_matrix.collect_sizes();
+    }
+
+    ///This  is  where  the  magic  happens.   The  equations  describing  the newtons method for finding 0s in the KKT conditions are implemented here.
+
+
+    template<int dim>
+    void
+    KktSystem<dim>::assemble_block_system(const LA::MPI::BlockVector &distributed_state, const double barrier_size) {
+        /*Remove any values from old iterations*/
+
+        LA::MPI::BlockVector relevant_state(owned_partitioning, relevant_partitioning, mpi_communicator);
+        relevant_state = distributed_state;
+
+        system_matrix.reinit(owned_partitioning, dsp, mpi_communicator);
+        locally_relevant_solution = 0;
+        system_rhs = 0;
+
+        QGauss<dim> nine_quadrature(fe_nine.degree + 1);
+        QGauss<dim> ten_quadrature(fe_ten.degree + 1);
+
+        hp::QCollection<dim> q_collection;
+        q_collection.push_back(nine_quadrature);
+        q_collection.push_back(ten_quadrature);
+
+        hp::FEValues<dim> hp_fe_values(fe_collection,
+                                       q_collection,
+                                       update_values | update_quadrature_points |
+                                       update_JxW_values | update_gradients);
+
+        QGauss<dim - 1> common_face_quadrature(fe_ten.degree + 1);
+
+        FEFaceValues<dim> fe_nine_face_values(fe_nine,
+                                              common_face_quadrature,
+                                              update_JxW_values |
+                                              update_gradients | update_values);
+        FEFaceValues<dim> fe_ten_face_values(fe_ten,
+                                             common_face_quadrature,
+                                             update_normal_vectors |
+                                             update_values);
+
+        FullMatrix<double> cell_matrix;
+        Vector<double> cell_rhs;
+        std::vector<types::global_dof_index> local_dof_indices;
+
+        const FEValuesExtractors::Scalar densities(SolutionComponents::density<dim>);
+        const FEValuesExtractors::Vector displacements(SolutionComponents::displacement<dim>);
+        const FEValuesExtractors::Scalar unfiltered_densities(SolutionComponents::unfiltered_density<dim>);
+        const FEValuesExtractors::Vector displacement_multipliers(SolutionComponents::displacement_multiplier<dim>);
+        const FEValuesExtractors::Scalar unfiltered_density_multipliers(
+                SolutionComponents::unfiltered_density_multiplier<dim>);
+        const FEValuesExtractors::Scalar density_lower_slacks(SolutionComponents::density_lower_slack<dim>);
+        const FEValuesExtractors::Scalar density_lower_slack_multipliers(
+                SolutionComponents::density_lower_slack_multiplier<dim>);
+        const FEValuesExtractors::Scalar density_upper_slacks(SolutionComponents::density_upper_slack<dim>);
+        const FEValuesExtractors::Scalar density_upper_slack_multipliers(
+                SolutionComponents::density_upper_slack_multiplier<dim>);
+        const FEValuesExtractors::Scalar total_volume_multiplier(
+                SolutionComponents::total_volume_multiplier<dim>);
+
+        const Functions::ConstantFunction<dim> lambda(1.), mu(1.);
+
+        distributed_solution = distributed_state;
+        LA::MPI::BlockVector filtered_unfiltered_density_solution = distributed_solution;
+        LA::MPI::BlockVector filter_adjoint_unfiltered_density_multiplier_solution = distributed_solution;
+        filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = 0;
+        filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier) = 0;
+        auto op_f = linear_operator<LA::MPI::Vector>(density_filter.filter_matrix);
+        filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = op_f * distributed_solution.block(SolutionBlocks::unfiltered_density);
+        filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier) = transpose_operator(op_f) *  distributed_solution.block(SolutionBlocks::unfiltered_density_multiplier);
+
+        LA::MPI::BlockVector relevant_filtered_unfiltered_density_solution = locally_relevant_solution;
+        LA::MPI::BlockVector relevant_filter_adjoint_unfiltered_density_multiplier_solution = locally_relevant_solution;
+        relevant_filtered_unfiltered_density_solution =filtered_unfiltered_density_solution;
+        relevant_filter_adjoint_unfiltered_density_multiplier_solution = filter_adjoint_unfiltered_density_multiplier_solution;
+        for (const auto &cell: dof_handler.active_cell_iterators()) {
+            if(cell->is_locally_owned())
+            {
+                hp_fe_values.reinit(cell);
+                const FEValues<dim> &fe_values = hp_fe_values.get_present_fe_values();
+                cell_matrix.reinit(cell->get_fe().n_dofs_per_cell(),
+                                   cell->get_fe().n_dofs_per_cell());
+                cell_rhs.reinit(cell->get_fe().n_dofs_per_cell());
+
+                const unsigned int n_q_points = fe_values.n_quadrature_points;
+
+                std::vector<double> old_density_values(n_q_points);
+                std::vector<Tensor<1, dim>> old_displacement_values(n_q_points);
+                std::vector<double> old_displacement_divs(n_q_points);
+                std::vector<SymmetricTensor<2, dim>> old_displacement_symmgrads(
+                        n_q_points);
+                std::vector<Tensor<1, dim>> old_displacement_multiplier_values(
+                        n_q_points);
+                std::vector<double> old_displacement_multiplier_divs(n_q_points);
+                std::vector<SymmetricTensor<2, dim>> old_displacement_multiplier_symmgrads(
+                        n_q_points);
+                std::vector<double> old_lower_slack_multiplier_values(n_q_points);
+                std::vector<double> old_upper_slack_multiplier_values(n_q_points);
+                std::vector<double> old_lower_slack_values(n_q_points);
+                std::vector<double> old_upper_slack_values(n_q_points);
+                std::vector<double> old_unfiltered_density_values(n_q_points);
+                std::vector<double> old_unfiltered_density_multiplier_values(n_q_points);
+                std::vector<double> filtered_unfiltered_density_values(n_q_points);
+                std::vector<double> filter_adjoint_unfiltered_density_multiplier_values(n_q_points);
+                std::vector<double> lambda_values(n_q_points);
+                std::vector<double> mu_values(n_q_points);
+
+                const unsigned int dofs_per_cell = cell->get_fe().n_dofs_per_cell();
+
+                cell_matrix = 0;
+                cell_rhs = 0;
+                local_dof_indices.resize(cell->get_fe().n_dofs_per_cell());
+                cell->get_dof_indices(local_dof_indices);
+
+                lambda.value_list(fe_values.get_quadrature_points(), lambda_values);
+                mu.value_list(fe_values.get_quadrature_points(), mu_values);
+
+                fe_values[densities].get_function_values(relevant_state,
+                                                         old_density_values);
+                fe_values[displacements].get_function_values(relevant_state,
+                                                             old_displacement_values);
+                fe_values[displacements].get_function_divergences(relevant_state,
+                                                                  old_displacement_divs);
+                fe_values[displacements].get_function_symmetric_gradients(
+                        relevant_state, old_displacement_symmgrads);
+                fe_values[displacement_multipliers].get_function_values(
+                        relevant_state, old_displacement_multiplier_values);
+                fe_values[displacement_multipliers].get_function_divergences(
+                        relevant_state, old_displacement_multiplier_divs);
+                fe_values[displacement_multipliers].get_function_symmetric_gradients(
+                        relevant_state, old_displacement_multiplier_symmgrads);
+                fe_values[density_lower_slacks].get_function_values(
+                        relevant_state, old_lower_slack_values);
+                fe_values[density_lower_slack_multipliers].get_function_values(
+                        relevant_state, old_lower_slack_multiplier_values);
+                fe_values[density_upper_slacks].get_function_values(
+                        relevant_state, old_upper_slack_values);
+                fe_values[density_upper_slack_multipliers].get_function_values(
+                        relevant_state, old_upper_slack_multiplier_values);
+                fe_values[unfiltered_densities].get_function_values(
+                        relevant_state, old_unfiltered_density_values);
+                fe_values[unfiltered_density_multipliers].get_function_values(
+                        relevant_state, old_unfiltered_density_multiplier_values);
+                fe_values[unfiltered_densities].get_function_values(
+                        relevant_filtered_unfiltered_density_solution, filtered_unfiltered_density_values);
+                fe_values[unfiltered_density_multipliers].get_function_values(
+                        relevant_filter_adjoint_unfiltered_density_multiplier_solution,
+                        filter_adjoint_unfiltered_density_multiplier_values);
+
+                Tensor<1, dim> traction;
+                traction[1] = -1;
+
+                for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
+
+                    for (unsigned int i = 0; i < dofs_per_cell; ++i) {
+                        const SymmetricTensor<2, dim> displacement_phi_i_symmgrad =
+                                fe_values[displacements].symmetric_gradient(i, q_point);
+                        const double displacement_phi_i_div =
+                                fe_values[displacements].divergence(i, q_point);
+
+                        const SymmetricTensor<2, dim> displacement_multiplier_phi_i_symmgrad =
+                                fe_values[displacement_multipliers].symmetric_gradient(i,
+                                                                                       q_point);
+                        const double displacement_multiplier_phi_i_div =
+                                fe_values[displacement_multipliers].divergence(i,
+                                                                               q_point);
+
+
+                        const double density_phi_i = fe_values[densities].value(i,
+                                                                                q_point);
+                        const double unfiltered_density_phi_i = fe_values[unfiltered_densities].value(i,
+                                                                                                      q_point);
+                        const double unfiltered_density_multiplier_phi_i = fe_values[unfiltered_density_multipliers].value(
+                                i, q_point);
+
+                        const double lower_slack_multiplier_phi_i =
+                                fe_values[density_lower_slack_multipliers].value(i,
+                                                                                 q_point);
+
+                        const double lower_slack_phi_i =
+                                fe_values[density_lower_slacks].value(i, q_point);
+
+                        const double upper_slack_phi_i =
+                                fe_values[density_upper_slacks].value(i, q_point);
+
+                        const double upper_slack_multiplier_phi_i =
+                                fe_values[density_upper_slack_multipliers].value(i,
+                                                                                 q_point);
+
+
+                        for (unsigned int j = 0; j < dofs_per_cell; ++j) {
+                            const SymmetricTensor<2, dim> displacement_phi_j_symmgrad =
+                                    fe_values[displacements].symmetric_gradient(j,
+                                                                                q_point);
+                            const double displacement_phi_j_div =
+                                    fe_values[displacements].divergence(j, q_point);
+
+                            const SymmetricTensor<2, dim> displacement_multiplier_phi_j_symmgrad =
+                                    fe_values[displacement_multipliers].symmetric_gradient(
+                                            j, q_point);
+                            const double displacement_multiplier_phi_j_div =
+                                    fe_values[displacement_multipliers].divergence(j,
+                                                                                   q_point);
+
+                            const double density_phi_j = fe_values[densities].value(
+                                    j, q_point);
+
+                            const double unfiltered_density_phi_j = fe_values[unfiltered_densities].value(j,
+                                                                                                          q_point);
+                            const double unfiltered_density_multiplier_phi_j = fe_values[unfiltered_density_multipliers].value(
+                                    j, q_point);
+
+
+                            const double lower_slack_phi_j =
+                                    fe_values[density_lower_slacks].value(j, q_point);
+
+                            const double upper_slack_phi_j =
+                                    fe_values[density_upper_slacks].value(j, q_point);
+
+                            const double lower_slack_multiplier_phi_j =
+                                    fe_values[density_lower_slack_multipliers].value(j,
+                                                                                     q_point);
+
+                            const double upper_slack_multiplier_phi_j =
+                                    fe_values[density_upper_slack_multipliers].value(j,
+                                                                                     q_point);
+
+                            //Equation 0
+                            cell_matrix(i, j) +=
+                                    fe_values.JxW(q_point) *
+                                    (
+                                            -density_phi_i * unfiltered_density_multiplier_phi_j
+
+                                            - density_penalty_exponent * (density_penalty_exponent - 1)
+                                              * std::pow(
+                                                    old_density_values[q_point],
+                                                    density_penalty_exponent - 2)
+                                              * density_phi_i
+                                              * density_phi_j
+                                              * (old_displacement_multiplier_divs[q_point] * old_displacement_divs[q_point]
+                                                 * lambda_values[q_point]
+                                                 + 2 * mu_values[q_point]
+                                                   * (old_displacement_symmgrads[q_point] *
+                                                      old_displacement_multiplier_symmgrads[q_point]))
+
+                                            - density_penalty_exponent * std::pow(
+                                                    old_density_values[q_point],
+                                                    density_penalty_exponent - 1)
+                                              * density_phi_i
+                                              * (displacement_multiplier_phi_j_div * old_displacement_divs[q_point]
+                                                 * lambda_values[q_point]
+                                                 + 2 * mu_values[q_point]
+                                                   *
+                                                   (old_displacement_symmgrads[q_point] *
+                                                    displacement_multiplier_phi_j_symmgrad))
+
+                                            - density_penalty_exponent * std::pow(
+                                                    old_density_values[q_point],
+                                                    density_penalty_exponent - 1)
+                                              * density_phi_i
+                                              * (displacement_phi_j_div * old_displacement_multiplier_divs[q_point]
+                                                 * lambda_values[q_point]
+                                                 + 2 * mu_values[q_point]
+                                                   * (old_displacement_multiplier_symmgrads[q_point] *
+                                                      displacement_phi_j_symmgrad)));
+                            //Equation 1
+
+                            cell_matrix(i, j) +=
+                                    fe_values.JxW(q_point) * (
+                                            -density_penalty_exponent * std::pow(
+                                                    old_density_values[q_point],
+                                                    density_penalty_exponent - 1)
+                                            * density_phi_j
+                                            * (old_displacement_multiplier_divs[q_point] * displacement_phi_i_div
+                                               * lambda_values[q_point]
+                                               + 2 * mu_values[q_point]
+                                                 * (old_displacement_multiplier_symmgrads[q_point] *
+                                                    displacement_phi_i_symmgrad))
+
+                                            - std::pow(old_density_values[q_point],
+                                                       density_penalty_exponent)
+                                              * (displacement_multiplier_phi_j_div * displacement_phi_i_div
+                                                 * lambda_values[q_point]
+                                                 + 2 * mu_values[q_point]
+                                                   * (displacement_multiplier_phi_j_symmgrad * displacement_phi_i_symmgrad))
+
+                                    );
+
+                            //Equation 2 has to do with the filter, which is calculated elsewhere.
+                            cell_matrix(i, j) +=
+                                    fe_values.JxW(q_point) * (
+                                            -1 * unfiltered_density_phi_i * lower_slack_multiplier_phi_j
+                                            + unfiltered_density_phi_i * upper_slack_multiplier_phi_j);
+
+                            //Equation 3 - Primal Feasibility
+
+                            cell_matrix(i, j) +=
+                                    fe_values.JxW(q_point) * (
+
+                                            -1 * density_penalty_exponent * std::pow(
+                                                    old_density_values[q_point],
+                                                    density_penalty_exponent - 1)
+                                            * density_phi_j
+                                            * (old_displacement_divs[q_point] * displacement_multiplier_phi_i_div
+                                               * lambda_values[q_point]
+                                               + 2 * mu_values[q_point]
+                                                 * (old_displacement_symmgrads[q_point] *
+                                                    displacement_multiplier_phi_i_symmgrad))
+
+                                            + -1 * std::pow(old_density_values[q_point],
+                                                            density_penalty_exponent)
+                                              * (displacement_phi_j_div * displacement_multiplier_phi_i_div
+                                                 * lambda_values[q_point]
+                                                 + 2 * mu_values[q_point]
+                                                   *
+                                                   (displacement_phi_j_symmgrad * displacement_multiplier_phi_i_symmgrad)));
+
+                            //Equation 4 - more primal feasibility
+                            cell_matrix(i, j) +=
+                                    -1 * fe_values.JxW(q_point) * lower_slack_multiplier_phi_i *
+                                    (unfiltered_density_phi_j - lower_slack_phi_j);
+
+                            //Equation 5 - more primal feasibility
+                            cell_matrix(i, j) +=
+                                    -1 * fe_values.JxW(q_point) * upper_slack_multiplier_phi_i * (
+                                            -1 * unfiltered_density_phi_j - upper_slack_phi_j);
+
+                            //Equation 6 - more primal feasibility - part with filter added later
+                            cell_matrix(i, j) +=
+                                    -1 * fe_values.JxW(q_point) * unfiltered_density_multiplier_phi_i * (
+                                            density_phi_j);
+
+                            //Equation 7 - complementary slackness
+                            cell_matrix(i, j) += fe_values.JxW(q_point) *
+                                                 (lower_slack_phi_i * lower_slack_multiplier_phi_j
+                                                  + lower_slack_phi_i * lower_slack_phi_j *
+                                                    old_lower_slack_multiplier_values[q_point] /
+                                                    old_lower_slack_values[q_point]);
+                            //Equation 8 - complementary slackness
+                            cell_matrix(i, j) += fe_values.JxW(q_point) *
+                                                 (upper_slack_phi_i * upper_slack_multiplier_phi_j
+                                                  + upper_slack_phi_i * upper_slack_phi_j *
+                                                    old_upper_slack_multiplier_values[q_point] /
+                                                    old_upper_slack_values[q_point]);
+                        }
+
+                    }
+                }
+
+
+                MatrixTools::local_apply_boundary_values(boundary_values, local_dof_indices,
+                                                         cell_matrix, cell_rhs, true);
+
+
+                constraints.distribute_local_to_global(
+                        cell_matrix, cell_rhs, local_dof_indices, system_matrix, system_rhs);
+
+            }
+
+        }
+        pcout << "compress 1" << std::endl;
+        system_matrix.compress(VectorOperation::add);
+        system_rhs = calculate_rhs(distributed_state, barrier_size);
+
+        for (const auto &cell: dof_handler.active_cell_iterators()) {
+            if(cell->is_locally_owned())
+            {
+                std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
+                cell->get_dof_indices(i);
+
+                const types::global_dof_index dof_index_lower_slack =
+                        i[cell->get_fe().component_to_system_index(0, 0)];
+                typename LA::MPI::SparseMatrix::iterator iter = density_filter.filter_matrix.begin(
+                        i[cell->get_fe().component_to_system_index(0, 0)]);
+                for (; iter != density_filter.filter_matrix.end(i[cell->get_fe().component_to_system_index(0, 0)]); iter++) {
+                    unsigned int j = iter->column();
+                    double value = iter->value() * cell->measure();
+
+                    system_matrix.block(SolutionBlocks::unfiltered_density_multiplier,
+                                        SolutionBlocks::unfiltered_density).add(i[cell->get_fe().component_to_system_index(0, 0)], j, value);
+                    system_matrix.block(SolutionBlocks::unfiltered_density,
+                                        SolutionBlocks::unfiltered_density_multiplier).add(j, i[cell->get_fe().component_to_system_index(0, 0)], value);
+                }
+
+                system_matrix.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).add(0, i[cell->get_fe().component_to_system_index(0, 0)],
+                                                                                                          cell->measure());
+
+                system_matrix.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier).add(i[cell->get_fe().component_to_system_index(0, 0)], 0,
+                                                                                                          cell->measure());
+            }
+        }
+        pcout << "compress 2" << std::endl;
+        system_matrix.compress(VectorOperation::add);
+
+
+        pcout << "assembled" << std::endl;
+
+    }
+
+    template<int dim>
+    double
+    KktSystem<dim>::calculate_objective_value(const LA::MPI::BlockVector &distributed_state) const {
+        /*Remove any values from old iterations*/
+
+        locally_relevant_solution = distributed_state;
+
+
+        QGauss<dim> nine_quadrature(fe_nine.degree + 1);
+        QGauss<dim> ten_quadrature(fe_ten.degree + 1);
+
+        hp::QCollection<dim> q_collection;
+        q_collection.push_back(nine_quadrature);
+        q_collection.push_back(ten_quadrature);
+
+        hp::FEValues<dim> hp_fe_values(fe_collection,
+                                       q_collection,
+                                       update_values | update_quadrature_points |
+                                       update_JxW_values | update_gradients);
+
+        QGauss<dim - 1> common_face_quadrature(fe_ten.degree + 1);
+
+        FEFaceValues<dim> fe_nine_face_values(fe_nine,
+                                              common_face_quadrature,
+                                              update_JxW_values |
+                                              update_gradients | update_values);
+        FEFaceValues<dim> fe_ten_face_values(fe_ten,
+                                             common_face_quadrature,
+                                             update_normal_vectors |
+                                             update_values);
+
+        FullMatrix<double> cell_matrix;
+        Vector<double> cell_rhs;
+
+        const FEValuesExtractors::Vector displacements(SolutionComponents::displacement<dim>);
+
+        Tensor<1, dim> traction;
+        traction[1] = -1;
+        distributed_solution = distributed_state;
+        double objective_value = 0;
+        for (const auto &cell: dof_handler.active_cell_iterators())
+        {
+            if(cell->is_locally_owned())
+            {
+                hp_fe_values.reinit(cell);
+                const FEValues<dim> &fe_values = hp_fe_values.get_present_fe_values();
+                const unsigned int dofs_per_cell = cell->get_fe().n_dofs_per_cell();
+                const unsigned int n_q_points = fe_values.n_quadrature_points;
+                const unsigned int n_face_q_points = common_face_quadrature.size();
+
+                std::vector<Tensor<1, dim>> old_displacement_values(n_q_points);
+                fe_values[displacements].get_function_values(
+                        locally_relevant_solution, old_displacement_values);
+
+                for (unsigned int face_number = 0;
+                     face_number < GeometryInfo<dim>::faces_per_cell;
+                     ++face_number)
+                {
+                    if (cell->face(face_number)->at_boundary() && cell->face(face_number)->boundary_id()
+                                                                  == BoundaryIds::down_force)
+                    {
+                        for (unsigned int face_q_point = 0;
+                             face_q_point < n_face_q_points; ++face_q_point) {
+                            for (unsigned int i = 0; i < dofs_per_cell; ++i) {
+                                if (cell->material_id() == MaterialIds::without_multiplier) {
+                                    fe_nine_face_values.reinit(cell, face_number);
+                                    objective_value += traction
+                                                       * fe_nine_face_values[displacements].value(i,
+                                                                                                  face_q_point)
+                                                       * fe_nine_face_values.JxW(face_q_point);
+                                } else {
+                                    fe_ten_face_values.reinit(cell, face_number);
+                                    objective_value += traction
+                                                       * fe_ten_face_values[displacements].value(i,
+                                                                                                 face_q_point)
+                                                       * fe_ten_face_values.JxW(face_q_point);
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+        }
+        double objective_value_out;
+        MPI_Allreduce(&objective_value, &objective_value_out, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+
+        pcout << "objective value: " << objective_value_out << std::endl;
+        return objective_value;
+    }
+
+
+    //As the KKT System know which vectors correspond to the slack variables, the sum of the logs of the slacks is computed here for use in the filter.
+    template<int dim>
+    double
+    KktSystem<dim>::calculate_barrier_distance(const LA::MPI::BlockVector &state) const {
+        double barrier_distance_log_sum = 0;
+        unsigned int vect_size = state.block(SolutionBlocks::density_lower_slack).size();
+        distributed_solution = state;
+        for (unsigned int k = 0; k < vect_size; k++) {
+            if (distributed_solution.block(SolutionBlocks::density_lower_slack).in_local_range(k))
+            barrier_distance_log_sum += std::log(state.block(SolutionBlocks::density_lower_slack)[k]);
+        }
+        for (unsigned int k = 0; k < vect_size; k++) {
+            if (distributed_solution.block(SolutionBlocks::density_upper_slack).in_local_range(k))
+            barrier_distance_log_sum += std::log(state.block(SolutionBlocks::density_upper_slack)[k]);
+        }
+        double out_barrier_distance_log_sum;
+        MPI_Allreduce(&barrier_distance_log_sum, &out_barrier_distance_log_sum, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+
+        pcout << "Barrier distance log sum: " << out_barrier_distance_log_sum << std::endl;
+
+        return out_barrier_distance_log_sum;
+    }
+
+    template<int dim>
+    double
+    KktSystem<dim>::calculate_rhs_norm(const LA::MPI::BlockVector &state, const double barrier_size) const {
+        return calculate_rhs(state, barrier_size).l2_norm();
+    }
+
+
+    //Feasibility conditions appear on the RHS of the linear system, so I compute the RHS to find it. Could probably be combined with the objective value finding part to make it faster.
+    template<int dim>
+    double
+    KktSystem<dim>::calculate_feasibility(const LA::MPI::BlockVector &state, const double barrier_size) const {
+       LA::MPI::BlockVector test_rhs = calculate_rhs(state, barrier_size);
+
+       double norm = 0;
+
+       distributed_solution = state;
+       double full_prod1 =1;
+       double full_prod2 = 1;
+
+       for (unsigned int k = 0; k < state.block(SolutionBlocks::density_upper_slack).size(); k++) {
+           double prod1 = 1;
+           double prod2 = 1;
+           if(state.block(SolutionBlocks::density_upper_slack).in_local_range(k))
+           {
+               prod1 = prod1 * state.block(SolutionBlocks::density_upper_slack)[k]
+                       * state.block(SolutionBlocks::density_upper_slack)[k];
+           }
+           if(state.block(SolutionBlocks::density_lower_slack).in_local_range(k))
+           {
+              prod2 = prod2 *  state.block(SolutionBlocks::density_lower_slack)[k]
+                      * state.block(SolutionBlocks::density_lower_slack)[k];
+           }
+           if(state.block(SolutionBlocks::density_upper_slack_multiplier).in_local_range(k))
+           {
+               prod1 = prod1 * state.block(SolutionBlocks::density_upper_slack_multiplier)[k]
+                       * state.block(SolutionBlocks::density_upper_slack_multiplier)[k];
+           }
+           if(state.block(SolutionBlocks::density_lower_slack_multiplier).in_local_range(k))
+           {
+               prod2 = prod2 *  state.block(SolutionBlocks::density_lower_slack_multiplier)[k]
+                       * state.block(SolutionBlocks::density_lower_slack_multiplier)[k];
+           }
+           MPI_Allreduce(&prod1, &full_prod1, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
+           MPI_Allreduce(&prod2, &full_prod2, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
+           norm = norm + full_prod1 + full_prod2;
+       }
+       pcout << "pre-norm: " << norm << std::endl;
+
+        norm += std::pow(test_rhs.block(SolutionBlocks::displacement).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::density).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::unfiltered_density).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::displacement_multiplier).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::total_volume_multiplier).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::density_upper_slack_multiplier).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::density_lower_slack_multiplier).l2_norm(), 2);
+
+        pcout << "norm: " << norm << std::endl;
+
+        return norm;
+    }
+
+    template<int dim>
+    double
+    KktSystem<dim>::calculate_convergence(const LA::MPI::BlockVector &state) const {
+        LA::MPI::BlockVector test_rhs = calculate_rhs(state, Input::min_barrier_size);
+        double norm = 0;
+
+        distributed_solution = state;
+        double full_prod1 =1;
+        double full_prod2 = 1;
+
+        for (unsigned int k = 0; k < state.block(SolutionBlocks::density_upper_slack).size(); k++) {
+            double prod1 = 1;
+            double prod2 = 1;
+            if(state.block(SolutionBlocks::density_upper_slack).in_local_range(k))
+            {
+                prod1 = prod1 * state.block(SolutionBlocks::density_upper_slack)[k]
+                        * state.block(SolutionBlocks::density_upper_slack)[k];
+            }
+            if(state.block(SolutionBlocks::density_lower_slack).in_local_range(k))
+            {
+               prod2 = prod2 *  state.block(SolutionBlocks::density_lower_slack)[k]
+                       * state.block(SolutionBlocks::density_lower_slack)[k];
+            }
+            if(state.block(SolutionBlocks::density_upper_slack_multiplier).in_local_range(k))
+            {
+                prod1 = prod1 * state.block(SolutionBlocks::density_upper_slack_multiplier)[k]
+                        * state.block(SolutionBlocks::density_upper_slack_multiplier)[k];
+            }
+            if(state.block(SolutionBlocks::density_lower_slack_multiplier).in_local_range(k))
+            {
+                prod2 = prod2 *  state.block(SolutionBlocks::density_lower_slack_multiplier)[k]
+                        * state.block(SolutionBlocks::density_lower_slack_multiplier)[k];
+            }
+            MPI_Allreduce(&prod1, &full_prod1, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
+            MPI_Allreduce(&prod2, &full_prod2, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
+            norm = norm + full_prod1 + full_prod2;
+        }
+        pcout << "pre-norm: " << norm << std::endl;
+
+        norm += std::pow(test_rhs.block(SolutionBlocks::displacement).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::density).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::unfiltered_density).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::displacement_multiplier).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::total_volume_multiplier).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::density_upper_slack_multiplier).l2_norm(), 2);
+        norm += std::pow(test_rhs.block(SolutionBlocks::density_lower_slack_multiplier).l2_norm(), 2);
+
+        norm = std::pow(norm, .5);
+
+        pcout << "l2 norm: " << system_rhs.l2_norm() << std::endl;
+        pcout << "KKT norm: " << norm << std::endl;
+        return norm;
+    }
+
+    template<int dim>
+    LA::MPI::BlockVector
+    KktSystem<dim>::calculate_rhs(const LA::MPI::BlockVector &distributed_state, const double barrier_size) const {
+        LA::MPI::BlockVector test_rhs (system_rhs);
+        LA::MPI::BlockVector state (locally_relevant_solution);
+        state = distributed_state;
+        test_rhs = 0;
+
+
+        QGauss<dim> nine_quadrature(fe_nine.degree + 1);
+        QGauss<dim> ten_quadrature(fe_ten.degree + 1);
+
+        hp::QCollection<dim> q_collection;
+        q_collection.push_back(nine_quadrature);
+        q_collection.push_back(ten_quadrature);
+
+        hp::FEValues<dim> hp_fe_values(fe_collection,
+                                       q_collection,
+                                       update_values | update_quadrature_points |
+                                       update_JxW_values | update_gradients);
+
+        QGauss<dim - 1> common_face_quadrature(fe_ten.degree + 1);
+
+        FEFaceValues<dim> fe_nine_face_values(fe_nine,
+                                              common_face_quadrature,
+                                              update_JxW_values |
+                                              update_gradients | update_values);
+        FEFaceValues<dim> fe_ten_face_values(fe_ten,
+                                             common_face_quadrature,
+                                             update_normal_vectors |
+                                             update_values);
+
+        FullMatrix<double> cell_matrix;
+        Vector<double> cell_rhs;
+        std::vector<types::global_dof_index> local_dof_indices;
+
+        const FEValuesExtractors::Scalar densities(SolutionComponents::density<dim>);
+        const FEValuesExtractors::Vector displacements(SolutionComponents::displacement<dim>);
+        const FEValuesExtractors::Scalar unfiltered_densities(SolutionComponents::unfiltered_density<dim>);
+        const FEValuesExtractors::Vector displacement_multipliers(SolutionComponents::displacement_multiplier<dim>);
+        const FEValuesExtractors::Scalar unfiltered_density_multipliers(
+                SolutionComponents::unfiltered_density_multiplier<dim>);
+        const FEValuesExtractors::Scalar density_lower_slacks(SolutionComponents::density_lower_slack<dim>);
+        const FEValuesExtractors::Scalar density_lower_slack_multipliers(
+                SolutionComponents::density_lower_slack_multiplier<dim>);
+        const FEValuesExtractors::Scalar density_upper_slacks(SolutionComponents::density_upper_slack<dim>);
+        const FEValuesExtractors::Scalar density_upper_slack_multipliers(
+                SolutionComponents::density_upper_slack_multiplier<dim>);
+        const FEValuesExtractors::Scalar total_volume_multiplier(
+                SolutionComponents::total_volume_multiplier<dim>);
+
+
+        const unsigned int n_face_q_points = common_face_quadrature.size();
+
+        const Functions::ConstantFunction<dim> lambda(1.), mu(1.);
+
+        locally_relevant_solution = state;
+        distributed_solution = state;
+        LA::MPI::BlockVector filtered_unfiltered_density_solution (distributed_solution);
+        LA::MPI::BlockVector filter_adjoint_unfiltered_density_multiplier_solution (distributed_solution);
+        filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = 0;
+        filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier) = 0;
+
+        auto op_f = linear_operator<LA::MPI::Vector>(density_filter.filter_matrix);
+        filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = op_f * distributed_solution.block(SolutionBlocks::unfiltered_density);
+        filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier)
+                                                    = transpose_operator(op_f) * distributed_solution.block(SolutionBlocks::unfiltered_density_multiplier);
+        LA::MPI::BlockVector relevant_filtered_unfiltered_density_solution (locally_relevant_solution);
+        LA::MPI::BlockVector relevant_filter_adjoint_unfiltered_density_multiplier_solution (locally_relevant_solution);
+        relevant_filtered_unfiltered_density_solution = filtered_unfiltered_density_solution;
+        relevant_filter_adjoint_unfiltered_density_multiplier_solution = filter_adjoint_unfiltered_density_multiplier_solution;
+
+        double old_volume_multiplier_temp = 0;
+        double old_volume_multiplier;
+        if(distributed_state.block(SolutionBlocks::total_volume_multiplier).in_local_range(0))
+        {
+            old_volume_multiplier_temp = state.block(SolutionBlocks::total_volume_multiplier)[0];
+        }
+        MPI_Allreduce(&old_volume_multiplier_temp, &old_volume_multiplier, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+
+        for (const auto &cell: dof_handler.active_cell_iterators()) {
+            if(cell->is_locally_owned())
+            {
+                hp_fe_values.reinit(cell);
+                const FEValues<dim> &fe_values = hp_fe_values.get_present_fe_values();
+                cell_matrix.reinit(cell->get_fe().n_dofs_per_cell(),
+                                   cell->get_fe().n_dofs_per_cell());
+                cell_rhs.reinit(cell->get_fe().n_dofs_per_cell());
+
+                const unsigned int n_q_points = fe_values.n_quadrature_points;
+
+                std::vector<double> old_density_values(n_q_points);
+                std::vector<Tensor<1, dim>> old_displacement_values(n_q_points);
+                std::vector<double> old_displacement_divs(n_q_points);
+                std::vector<SymmetricTensor<2, dim>> old_displacement_symmgrads(
+                        n_q_points);
+                std::vector<Tensor<1, dim>> old_displacement_multiplier_values(
+                        n_q_points);
+                std::vector<double> old_displacement_multiplier_divs(n_q_points);
+                std::vector<SymmetricTensor<2, dim>> old_displacement_multiplier_symmgrads(
+                        n_q_points);
+                std::vector<double> old_lower_slack_multiplier_values(n_q_points);
+                std::vector<double> old_upper_slack_multiplier_values(n_q_points);
+                std::vector<double> old_lower_slack_values(n_q_points);
+                std::vector<double> old_upper_slack_values(n_q_points);
+                std::vector<double> old_unfiltered_density_values(n_q_points);
+                std::vector<double> old_unfiltered_density_multiplier_values(n_q_points);
+                std::vector<double> filtered_unfiltered_density_values(n_q_points);
+                std::vector<double> filter_adjoint_unfiltered_density_multiplier_values(n_q_points);
+                std::vector<double> lambda_values(n_q_points);
+                std::vector<double> mu_values(n_q_points);
+
+                const unsigned int dofs_per_cell = cell->get_fe().n_dofs_per_cell();
+
+                cell_matrix = 0;
+                cell_rhs = 0;
+                local_dof_indices.resize(cell->get_fe().n_dofs_per_cell());
+
+                cell->get_dof_indices(local_dof_indices);
+
+                lambda.value_list(fe_values.get_quadrature_points(), lambda_values);
+                mu.value_list(fe_values.get_quadrature_points(), mu_values);
+
+                fe_values[densities].get_function_values(state,
+                                                         old_density_values);
+                fe_values[displacements].get_function_values(state,
+                                                             old_displacement_values);
+                fe_values[displacements].get_function_divergences(state,
+                                                                  old_displacement_divs);
+                fe_values[displacements].get_function_symmetric_gradients(
+                        state, old_displacement_symmgrads);
+                fe_values[displacement_multipliers].get_function_values(
+                        state, old_displacement_multiplier_values);
+                fe_values[displacement_multipliers].get_function_divergences(
+                        state, old_displacement_multiplier_divs);
+                fe_values[displacement_multipliers].get_function_symmetric_gradients(
+                        state, old_displacement_multiplier_symmgrads);
+                fe_values[density_lower_slacks].get_function_values(
+                        state, old_lower_slack_values);
+                fe_values[density_lower_slack_multipliers].get_function_values(
+                        state, old_lower_slack_multiplier_values);
+                fe_values[density_upper_slacks].get_function_values(
+                        state, old_upper_slack_values);
+                fe_values[density_upper_slack_multipliers].get_function_values(
+                        state, old_upper_slack_multiplier_values);
+                fe_values[unfiltered_densities].get_function_values(
+                        state, old_unfiltered_density_values);
+                fe_values[unfiltered_density_multipliers].get_function_values(
+                        state, old_unfiltered_density_multiplier_values);
+                fe_values[unfiltered_densities].get_function_values(
+                        relevant_filtered_unfiltered_density_solution, filtered_unfiltered_density_values);
+                fe_values[unfiltered_density_multipliers].get_function_values(
+                        relevant_filter_adjoint_unfiltered_density_multiplier_solution,
+                        filter_adjoint_unfiltered_density_multiplier_values);
+
+
+                Tensor<1, dim> traction;
+                traction[1] = -1;
+
+                for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
+
+                    for (unsigned int i = 0; i < dofs_per_cell; ++i) {
+                        const SymmetricTensor<2, dim> displacement_phi_i_symmgrad =
+                                fe_values[displacements].symmetric_gradient(i, q_point);
+                        const double displacement_phi_i_div =
+                                fe_values[displacements].divergence(i, q_point);
+
+                        const SymmetricTensor<2, dim> displacement_multiplier_phi_i_symmgrad =
+                                fe_values[displacement_multipliers].symmetric_gradient(i,
+                                                                                       q_point);
+                        const double displacement_multiplier_phi_i_div =
+                                fe_values[displacement_multipliers].divergence(i,
+                                                                               q_point);
+
+
+                        const double density_phi_i = fe_values[densities].value(i,
+                                                                                q_point);
+                        const double unfiltered_density_phi_i = fe_values[unfiltered_densities].value(i,
+                                                                                                      q_point);
+                        const double unfiltered_density_multiplier_phi_i = fe_values[unfiltered_density_multipliers].value(
+                                i, q_point);
+
+                        const double lower_slack_multiplier_phi_i =
+                                fe_values[density_lower_slack_multipliers].value(i,
+                                                                                 q_point);
+
+                        const double lower_slack_phi_i =
+                                fe_values[density_lower_slacks].value(i, q_point);
+
+                        const double upper_slack_phi_i =
+                                fe_values[density_upper_slacks].value(i, q_point);
+
+                        const double upper_slack_multiplier_phi_i =
+                                fe_values[density_upper_slack_multipliers].value(i,
+                                                                                 q_point);
+
+
+                        //rhs eqn 0
+                        cell_rhs(i) +=
+                                -1 * fe_values.JxW(q_point) * (
+                                        -1 * density_penalty_exponent *
+                                        std::pow(old_density_values[q_point], density_penalty_exponent - 1) * density_phi_i
+                                        * (old_displacement_multiplier_divs[q_point] * old_displacement_divs[q_point]
+                                           * lambda_values[q_point]
+                                           + 2 * mu_values[q_point] * (old_displacement_symmgrads[q_point]
+                                                                       * old_displacement_multiplier_symmgrads[q_point]))
+                                        - density_phi_i * old_unfiltered_density_multiplier_values[q_point]
+                                        + old_volume_multiplier * density_phi_i
+                                );
+
+                        //rhs eqn 1 - boundary terms counted later
+                        cell_rhs(i) +=
+                                -1 * fe_values.JxW(q_point) * (
+                                        -1 * std::pow(old_density_values[q_point], density_penalty_exponent)
+                                        * (old_displacement_multiplier_divs[q_point] * displacement_phi_i_div
+                                           * lambda_values[q_point]
+                                           + 2 * mu_values[q_point] * (old_displacement_multiplier_symmgrads[q_point]
+                                                                       * displacement_phi_i_symmgrad))
+                                );
+
+                        //rhs eqn 2
+                        cell_rhs(i) +=
+                                -1 * fe_values.JxW(q_point) * (
+                                        unfiltered_density_phi_i *
+                                        filter_adjoint_unfiltered_density_multiplier_values[q_point]
+                                        + unfiltered_density_phi_i * old_upper_slack_multiplier_values[q_point]
+                                        + -1 * unfiltered_density_phi_i * old_lower_slack_multiplier_values[q_point]
+                                );
+
+
+
+
+                        //rhs eqn 3 - boundary terms counted later
+                        cell_rhs(i) +=
+                                -1 * fe_values.JxW(q_point) * (
+                                        -1 * std::pow(old_density_values[q_point], density_penalty_exponent)
+                                        * (old_displacement_divs[q_point] * displacement_multiplier_phi_i_div
+                                           * lambda_values[q_point]
+                                           + 2 * mu_values[q_point] * (displacement_multiplier_phi_i_symmgrad
+                                                                       * old_displacement_symmgrads[q_point]))
+                                );
+
+                        //rhs eqn 4
+                        cell_rhs(i) +=
+                                -1 * fe_values.JxW(q_point) *
+                                (-1 * lower_slack_multiplier_phi_i
+                                 * (old_unfiltered_density_values[q_point] - old_lower_slack_values[q_point])
+                                );
+
+                        //rhs eqn 5
+                        cell_rhs(i) +=
+                                -1 * fe_values.JxW(q_point) * (
+                                        -1 * upper_slack_multiplier_phi_i
+                                        * (1 - old_unfiltered_density_values[q_point]
+                                           - old_upper_slack_values[q_point]));
+
+                        //rhs eqn 6
+                        cell_rhs(i) +=
+                                -1 * fe_values.JxW(q_point) * (
+                                        -1 * unfiltered_density_multiplier_phi_i
+                                        * (old_density_values[q_point] - filtered_unfiltered_density_values[q_point])
+                                );
+
+                        //rhs eqn 7
+                        cell_rhs(i) +=
+                                -1 * fe_values.JxW(q_point) *
+                                (lower_slack_phi_i *
+                                 (old_lower_slack_multiplier_values[q_point] -
+                                  barrier_size / old_lower_slack_values[q_point]));
+
+                        //rhs eqn 8
+                        cell_rhs(i) +=
+                                -1 * fe_values.JxW(q_point) *
+                                (upper_slack_phi_i *
+                                 (old_upper_slack_multiplier_values[q_point] -
+                                  barrier_size / old_upper_slack_values[q_point]));
+
+                    }
+
+                }
+
+                for (unsigned int face_number = 0;
+                     face_number < GeometryInfo<dim>::faces_per_cell;
+                     ++face_number) {
+                    if (cell->face(face_number)->at_boundary() && cell->face(
+                            face_number)->boundary_id() == BoundaryIds::down_force) {
+                        for (unsigned int face_q_point = 0;
+                             face_q_point < n_face_q_points; ++face_q_point) {
+                            for (unsigned int i = 0; i < dofs_per_cell; ++i) {
+                                if (cell->material_id() == MaterialIds::without_multiplier) {
+                                    fe_nine_face_values.reinit(cell, face_number);
+                                    cell_rhs(i) += -1
+                                                   * traction
+                                                   * fe_nine_face_values[displacements].value(i,
+                                                                                              face_q_point)
+                                                   * fe_nine_face_values.JxW(face_q_point);
+
+                                    cell_rhs(i) += -1 * traction
+                                                   * fe_nine_face_values[displacement_multipliers].value(
+                                            i, face_q_point)
+                                                   * fe_nine_face_values.JxW(face_q_point);
+                                } else {
+                                    fe_ten_face_values.reinit(cell, face_number);
+                                    cell_rhs(i) += -1
+                                                   * traction
+                                                   * fe_ten_face_values[displacements].value(i,
+                                                                                             face_q_point)
+                                                   * fe_ten_face_values.JxW(face_q_point);
+
+                                    cell_rhs(i) += -1 * traction
+                                                   * fe_ten_face_values[displacement_multipliers].value(
+                                            i, face_q_point)
+                                                   * fe_ten_face_values.JxW(face_q_point);
+                                }
+                            }
+                        }
+                    }
+                }
+
+
+                MatrixTools::local_apply_boundary_values(boundary_values, local_dof_indices,
+                                                         cell_matrix, cell_rhs, true);
+                constraints.distribute_local_to_global(
+                        cell_rhs, local_dof_indices, test_rhs);
+            }
+        }
+
+        test_rhs.compress(VectorOperation::add);
+        double total_volume_temp = 0;
+        double goal_volume_temp = 0;
+        double total_volume, goal_volume;
+
+        distributed_solution = state;
+        for (const auto &cell: dof_handler.active_cell_iterators()) {
+            if(cell->is_locally_owned())
+            {
+                if (distributed_solution.block(SolutionBlocks::density).in_local_range(cell->active_cell_index()))
+                {
+                    total_volume_temp += cell->measure() * state.block(SolutionBlocks::density)[cell->active_cell_index()];
+                    goal_volume_temp += cell->measure() * Input::volume_percentage;
+                }
+            }
+        }
+
+        MPI_Allreduce(&total_volume_temp, &total_volume, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+        MPI_Allreduce(&goal_volume_temp, &goal_volume, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+
+        if (test_rhs.block(SolutionBlocks::total_volume_multiplier).in_local_range(0))
+        {
+            test_rhs.block(SolutionBlocks::total_volume_multiplier)[0] = goal_volume - total_volume;
+        }
+        test_rhs.compress(VectorOperation::insert);
+
+        return test_rhs;
+
+    }
+
+
+    template<int dim>
+    LA::MPI::BlockVector
+    KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
+        double gmres_tolerance;
+        if (Input::use_eisenstat_walker) {
+            gmres_tolerance = std::max(
+                    std::min(
+                            .1 * system_rhs.l2_norm() / (initial_rhs_error),
+                            .001
+                    ),
+                    Input::default_gmres_tolerance);
+        }
+        else {
+            gmres_tolerance = Input::default_gmres_tolerance;
+        }
+        locally_relevant_solution=state;
+        distributed_solution = state;
+        SolverControl solver_control(10000, gmres_tolerance * system_rhs.l2_norm());
+        TopOptSchurPreconditioner<dim> preconditioner(system_matrix);
+
+
+        switch (Input::solver_choice) {
+
+            case SolverOptions::inexact_K_with_exact_A_gmres: {
+                preconditioner.initialize(system_matrix, boundary_values, dof_handler,  locally_relevant_solution, distributed_solution);
+                SolverFGMRES<LA::MPI::BlockVector> B_fgmres(solver_control);
+                B_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
+                pcout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
+                break;
+            }
+            case SolverOptions::inexact_K_with_inexact_A_gmres: {
+                preconditioner.initialize(system_matrix, boundary_values, dof_handler,  locally_relevant_solution, distributed_solution);
+                SolverFGMRES<LA::MPI::BlockVector> C_fgmres(solver_control);
+                C_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
+                pcout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
+                break;
+            }
+            default:
+                throw;
+        }
+
+
+        constraints.distribute(distributed_solution);
+
+        pcout << "here" << std::endl;
+
+        return distributed_solution;
+    }
+
+    template<int dim>
+    void
+    KktSystem<dim>::calculate_initial_rhs_error() {
+        initial_rhs_error = system_rhs.l2_norm();
+    }
+
+    template<int dim>
+    LA::MPI::BlockVector
+    KktSystem<dim>::get_initial_state() {
+
+        std::vector<unsigned int> block_component(10, 2);
+        block_component[SolutionBlocks::density] = 0;
+        block_component[SolutionBlocks::displacement] = 1;
+        const std::vector<types::global_dof_index> dofs_per_block =
+                DoFTools::count_dofs_per_fe_block(dof_handler, block_component);
+        const unsigned int n_p = dofs_per_block[0];
+        const unsigned int n_u = dofs_per_block[1];
+        const std::vector<unsigned int> block_sizes = {n_p, n_p, n_p, n_p, n_p, n_u, n_u, n_p, n_p, 1};
+
+        LA::MPI::BlockVector state(owned_partitioning, mpi_communicator);
+        {
+            using namespace SolutionBlocks;
+            state.block(density).add(density_ratio);
+            state.block(unfiltered_density).add(density_ratio);
+            state.block(unfiltered_density_multiplier)
+                    .add(density_ratio);
+            state.block(density_lower_slack).add(density_ratio);
+            state.block(density_lower_slack_multiplier).add(50);
+            state.block(density_upper_slack).add(1 - density_ratio);
+            state.block(density_upper_slack_multiplier).add(50);
+            state.block(total_volume_multiplier).add(1);
+            state.block(displacement).add(0);
+            state.block(displacement_multiplier).add(0);
+        }
+        state.compress(VectorOperation::add);
+        return state;
+    }
+
+    template<int dim>
+    void
+    KktSystem<dim>::output(const LA::MPI::BlockVector &state, const unsigned int j) const {
+        locally_relevant_solution = state;
+        std::vector<std::string> solution_names(1, "low_slack_multiplier");
+        std::vector<DataComponentInterpretation::DataComponentInterpretation> data_component_interpretation(
+                1, DataComponentInterpretation::component_is_scalar);
+        solution_names.emplace_back("upper_slack_multiplier");
+        data_component_interpretation.push_back(
+                DataComponentInterpretation::component_is_scalar);
+        solution_names.emplace_back("low_slack");
+        data_component_interpretation.push_back(
+                DataComponentInterpretation::component_is_scalar);
+        solution_names.emplace_back("upper_slack");
+        data_component_interpretation.push_back(
+                DataComponentInterpretation::component_is_scalar);
+        solution_names.emplace_back("unfiltered_density");
+        data_component_interpretation.push_back(
+                DataComponentInterpretation::component_is_scalar);
+        for (unsigned int i = 0; i < dim; i++) {
+            solution_names.emplace_back("displacement");
+            data_component_interpretation.push_back(
+                    DataComponentInterpretation::component_is_part_of_vector);
+        }
+        for (unsigned int i = 0; i < dim; i++) {
+            solution_names.emplace_back("displacement_multiplier");
+            data_component_interpretation.push_back(
+                    DataComponentInterpretation::component_is_part_of_vector);
+        }
+        solution_names.emplace_back("density_multiplier");
+        data_component_interpretation.push_back(
+                DataComponentInterpretation::component_is_scalar);
+        solution_names.emplace_back("density");
+        data_component_interpretation.push_back(
+                DataComponentInterpretation::component_is_scalar);
+        solution_names.emplace_back("volume_multiplier");
+        data_component_interpretation.push_back(
+                DataComponentInterpretation::component_is_scalar);
+        DataOut<dim> data_out;
+        data_out.attach_dof_handler(dof_handler);
+        data_out.add_data_vector(locally_relevant_solution,
+                                 solution_names,
+                                 DataOut<dim>::type_dof_data,
+                                 data_component_interpretation);
+        data_out.build_patches();
+        std::string output("solution" + std::to_string(j) + ".vtu");
+        data_out.write_vtu_in_parallel(output, MPI_COMM_WORLD);
+
+    }
+
+    template<>
+    void
+    KktSystem<2>::output_stl(const LA::MPI::BlockVector &state) {
+        double height = .25;
+        const int dim = 2;
+        std::ofstream stlfile;
+        stlfile.open("bridge.stl");
+        stlfile << "solid bridge\n" << std::scientific;
+
+        for (const auto &cell: dof_handler.active_cell_iterators()) {
+            if (state.block(
+                    SolutionBlocks::density)[cell->active_cell_index()] > 0.5) {
+                const Tensor<1, dim> edge_directions[2] = {cell->vertex(1) -
+                                                           cell->vertex(0),
+                                                           cell->vertex(2) -
+                                                           cell->vertex(0)};
+                const Tensor<2, dim> edge_tensor(
+                        {{edge_directions[0][0], edge_directions[0][1]},
+                         {edge_directions[1][0], edge_directions[1][1]}});
+                const bool is_right_handed_cell = (determinant(edge_tensor) > 0);
+                if (is_right_handed_cell) {
+                    /* Write one side at z = 0. */
+                    stlfile << "   facet normal " << 0.000000e+00 << " "
+                            << 0.000000e+00 << " " << -1.000000e+00 << "\n";
+                    stlfile << "      outer loop\n";
+                    stlfile << "         vertex " << cell->vertex(0)[0] << " "
+                            << cell->vertex(0)[1] << " " << 0.000000e+00 << "\n";
+                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
+                            << cell->vertex(2)[1] << " " << 0.000000e+00 << "\n";
+                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
+                            << cell->vertex(1)[1] << " " << 0.000000e+00 << "\n";
+                    stlfile << "      endloop\n";
+                    stlfile << "   endfacet\n";
+                    stlfile << "   facet normal " << 0.000000e+00 << " "
+                            << 0.000000e+00 << " " << -1.000000e+00 << "\n";
+                    stlfile << "      outer loop\n";
+                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
+                            << cell->vertex(1)[1] << " " << 0.000000e+00 << "\n";
+                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
+                            << cell->vertex(2)[1] << " " << 0.000000e+00 << "\n";
+                    stlfile << "         vertex " << cell->vertex(3)[0] << " "
+                            << cell->vertex(3)[1] << " " << 0.000000e+00 << "\n";
+                    stlfile << "      endloop\n";
+                    stlfile << "   endfacet\n";
+                    /* Write one side at z = height. */
+                    stlfile << "   facet normal " << 0.000000e+00 << " "
+                            << 0.000000e+00 << " " << 1.000000e+00 << "\n";
+                    stlfile << "      outer loop\n";
+                    stlfile << "         vertex " << cell->vertex(0)[0] << " "
+                            << cell->vertex(0)[1] << " " << height << "\n";
+                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
+                            << cell->vertex(1)[1] << " " << height << "\n";
+                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
+                            << cell->vertex(2)[1] << " " << height << "\n";
+                    stlfile << "      endloop\n";
+                    stlfile << "   endfacet\n";
+                    stlfile << "   facet normal " << 0.000000e+00 << " "
+                            << 0.000000e+00 << " " << 1.000000e+00 << "\n";
+                    stlfile << "      outer loop\n";
+                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
+                            << cell->vertex(1)[1] << " " << height << "\n";
+                    stlfile << "         vertex " << cell->vertex(3)[0] << " "
+                            << cell->vertex(3)[1] << " " << height << "\n";
+                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
+                            << cell->vertex(2)[1] << " " << height << "\n";
+                    stlfile << "      endloop\n";
+                    stlfile << "   endfacet\n";
+                } else /* The cell has a left-handed set up */
+                {
+                    /* Write one side at z = 0. */
+                    stlfile << "   facet normal " << 0.000000e+00 << " "
+                            << 0.000000e+00 << " " << -1.000000e+00 << "\n";
+                    stlfile << "      outer loop\n";
+                    stlfile << "         vertex " << cell->vertex(0)[0] << " "
+                            << cell->vertex(0)[1] << " " << 0.000000e+00 << "\n";
+                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
+                            << cell->vertex(1)[1] << " " << 0.000000e+00 << "\n";
+                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
+                            << cell->vertex(2)[1] << " " << 0.000000e+00 << "\n";
+                    stlfile << "      endloop\n";
+                    stlfile << "   endfacet\n";
+                    stlfile << "   facet normal " << 0.000000e+00 << " "
+                            << 0.000000e+00 << " " << -1.000000e+00 << "\n";
+                    stlfile << "      outer loop\n";
+                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
+                            << cell->vertex(1)[1] << " " << 0.000000e+00 << "\n";
+                    stlfile << "         vertex " << cell->vertex(3)[0] << " "
+                            << cell->vertex(3)[1] << " " << 0.000000e+00 << "\n";
+                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
+                            << cell->vertex(2)[1] << " " << 0.000000e+00 << "\n";
+                    stlfile << "      endloop\n";
+                    stlfile << "   endfacet\n";
+                    /* Write one side at z = height. */
+                    stlfile << "   facet normal " << 0.000000e+00 << " "
+                            << 0.000000e+00 << " " << 1.000000e+00 << "\n";
+                    stlfile << "      outer loop\n";
+                    stlfile << "         vertex " << cell->vertex(0)[0] << " "
+                            << cell->vertex(0)[1] << " " << height << "\n";
+                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
+                            << cell->vertex(2)[1] << " " << height << "\n";
+                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
+                            << cell->vertex(1)[1] << " " << height << "\n";
+                    stlfile << "      endloop\n";
+                    stlfile << "   endfacet\n";
+                    stlfile << "   facet normal " << 0.000000e+00 << " "
+                            << 0.000000e+00 << " " << 1.000000e+00 << "\n";
+                    stlfile << "      outer loop\n";
+                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
+                            << cell->vertex(1)[1] << " " << height << "\n";
+                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
+                            << cell->vertex(2)[1] << " " << height << "\n";
+                    stlfile << "         vertex " << cell->vertex(3)[0] << " "
+                            << cell->vertex(3)[1] << " " << height << "\n";
+                    stlfile << "      endloop\n";
+                    stlfile << "   endfacet\n";
+                }
+                for (unsigned int face_number = 0;
+                     face_number < GeometryInfo<dim>::faces_per_cell;
+                     ++face_number) {
+                    const typename DoFHandler<dim>::face_iterator face =
+                            cell->face(face_number);
+                    if ((face->at_boundary()) ||
+                        (!face->at_boundary() &&
+                         (state.block(
+                                 SolutionBlocks::density)[cell->neighbor(face_number)->active_cell_index()] <
+                          0.5))) {
+                        const Tensor<1, dim> normal_vector =
+                                (face->center() - cell->center());
+                        const double normal_norm = normal_vector.norm();
+                        if ((face->vertex(0)[0] - face->vertex(0)[0]) *
+                            (face->vertex(1)[1] - face->vertex(0)[1]) *
+                            0.000000e+00 +
+                            (face->vertex(0)[1] - face->vertex(0)[1]) * (0 - 0) *
+                            normal_vector[0] +
+                            (height - 0) *
+                            (face->vertex(1)[0] - face->vertex(0)[0]) *
+                            normal_vector[1] -
+                            (face->vertex(0)[0] - face->vertex(0)[0]) * (0 - 0) *
+                            normal_vector[1] -
+                            (face->vertex(0)[1] - face->vertex(0)[1]) *
+                            (face->vertex(1)[0] - face->vertex(0)[0]) *
+                            normal_vector[0] -
+                            (height - 0) *
+                            (face->vertex(1)[1] - face->vertex(0)[1]) * 0 >
+                            0) {
+                            stlfile << "   facet normal "
+                                    << normal_vector[0] / normal_norm << " "
+                                    << normal_vector[1] / normal_norm << " "
+                                    << 0.000000e+00 << "\n";
+                            stlfile << "      outer loop\n";
+                            stlfile << "         vertex " << face->vertex(0)[0]
+                                    << " " << face->vertex(0)[1] << " "
+                                    << 0.000000e+00 << "\n";
+                            stlfile << "         vertex " << face->vertex(0)[0]
+                                    << " " << face->vertex(0)[1] << " " << height
+                                    << "\n";
+                            stlfile << "         vertex " << face->vertex(1)[0]
+                                    << " " << face->vertex(1)[1] << " "
+                                    << 0.000000e+00 << "\n";
+                            stlfile << "      endloop\n";
+                            stlfile << "   endfacet\n";
+                            stlfile << "   facet normal "
+                                    << normal_vector[0] / normal_norm << " "
+                                    << normal_vector[1] / normal_norm << " "
+                                    << 0.000000e+00 << "\n";
+                            stlfile << "      outer loop\n";
+                            stlfile << "         vertex " << face->vertex(0)[0]
+                                    << " " << face->vertex(0)[1] << " " << height
+                                    << "\n";
+                            stlfile << "         vertex " << face->vertex(1)[0]
+                                    << " " << face->vertex(1)[1] << " " << height
+                                    << "\n";
+                            stlfile << "         vertex " << face->vertex(1)[0]
+                                    << " " << face->vertex(1)[1] << " "
+                                    << 0.000000e+00 << "\n";
+                            stlfile << "      endloop\n";
+                            stlfile << "   endfacet\n";
+                        } else {
+                            stlfile << "   facet normal "
+                                    << normal_vector[0] / normal_norm << " "
+                                    << normal_vector[1] / normal_norm << " "
+                                    << 0.000000e+00 << "\n";
+                            stlfile << "      outer loop\n";
+                            stlfile << "         vertex " << face->vertex(0)[0]
+                                    << " " << face->vertex(0)[1] << " "
+                                    << 0.000000e+00 << "\n";
+                            stlfile << "         vertex " << face->vertex(1)[0]
+                                    << " " << face->vertex(1)[1] << " "
+                                    << 0.000000e+00 << "\n";
+                            stlfile << "         vertex " << face->vertex(0)[0]
+                                    << " " << face->vertex(0)[1] << " " << height
+                                    << "\n";
+                            stlfile << "      endloop\n";
+                            stlfile << "   endfacet\n";
+                            stlfile << "   facet normal "
+                                    << normal_vector[0] / normal_norm << " "
+                                    << normal_vector[1] / normal_norm << " "
+                                    << 0.000000e+00 << "\n";
+                            stlfile << "      outer loop\n";
+                            stlfile << "         vertex " << face->vertex(0)[0]
+                                    << " " << face->vertex(0)[1] << " " << height
+                                    << "\n";
+                            stlfile << "         vertex " << face->vertex(1)[0]
+                                    << " " << face->vertex(1)[1] << " "
+                                    << 0.000000e+00 << "\n";
+                            stlfile << "         vertex " << face->vertex(1)[0]
+                                    << " " << face->vertex(1)[1] << " " << height
+                                    << "\n";
+                            stlfile << "      endloop\n";
+                            stlfile << "   endfacet\n";
+                        }
+                    }
+                }
+            }
+        }
+        stlfile << "endsolid bridge";
+    }
+
+
+    template<>
+    void
+    KktSystem<3>::output_stl(const LA::MPI::BlockVector &state)
+    {
+        std::ofstream stlfile;
+        stlfile.open("bridge.stl");
+        stlfile << "solid bridge\n" << std::scientific;
+        const int dim = 3;
+        for (const auto &cell : dof_handler.active_cell_iterators())
+        {
+            if (state.block(
+                    SolutionBlocks::unfiltered_density)[cell->active_cell_index()] > 0.5)
+            {
+                for (const auto n : cell->face_indices())
+                {
+                    bool create_boundary = false;
+                    if (cell->at_boundary(n))
+                    {
+                        create_boundary = true;
+                    }
+                    else if (state.block(
+                            SolutionBlocks::unfiltered_density)[cell->neighbor(n)->active_cell_index()] <= 0.5)
+                    {
+                        create_boundary = true;
+                    }
+
+                    if (create_boundary)
+                    {
+                        const auto face = cell->face(n);
+                        const Tensor<1,dim> normal_vector = face->center() -
+                                                          cell->center();
+                        double normal_norm = normal_vector.norm();
+                        const Tensor<1,dim> edge_vectors_1 = face->vertex(1) - face->vertex(0);
+                        const Tensor<1,dim> edge_vectors_2 = face->vertex(2) - face->vertex(0);
+
+                        const Tensor<2, dim> edge_tensor (
+                                 {{edge_vectors_1[0], edge_vectors_1[1],edge_vectors_1[2]},
+                                 {edge_vectors_2[0], edge_vectors_2[1],edge_vectors_2[2]},
+                                 {normal_vector[0], normal_vector[1], normal_vector[2]}});
+                        const bool is_right_handed_cell = (determinant(edge_tensor) > 0);
+
+                        if (is_right_handed_cell)
+                        {
+                            stlfile << "   facet normal "
+                                    << normal_vector[0] / normal_norm << " "
+                                    << normal_vector[1] / normal_norm << " "
+                                    << normal_vector[2] / normal_norm << "\n";
+                            stlfile << "      outer loop\n";
+                            stlfile << "         vertex " << face->vertex(0)[0]
+                                    << " " << face->vertex(0)[1] << " "
+                                    << face->vertex(0)[2] << "\n";
+                            stlfile << "         vertex " << face->vertex(1)[0]
+                                    << " " << face->vertex(1)[1] << " "
+                                    << face->vertex(1)[2] << "\n";
+                            stlfile << "         vertex " << face->vertex(2)[0]
+                                    << " " << face->vertex(2)[1] << " "
+                                    << face->vertex(2)[2] << "\n";
+                            stlfile << "      endloop\n";
+                            stlfile << "   endfacet\n";
+                            stlfile << "   facet normal "
+                                    << normal_vector[0] / normal_norm << " "
+                                    << normal_vector[1] / normal_norm << " "
+                                    << normal_vector[2] / normal_norm << "\n";
+                            stlfile << "      outer loop\n";
+                            stlfile << "         vertex " << face->vertex(1)[0]
+                                    << " " << face->vertex(1)[1] << " " << face->vertex(1)[2]
+                                    << "\n";
+                            stlfile << "         vertex " << face->vertex(3)[0]
+                                    << " " << face->vertex(3)[1] << " " << face->vertex(3)[2]
+                                    << "\n";
+                            stlfile << "         vertex " << face->vertex(2)[0]
+                                    << " " << face->vertex(2)[1] << " "
+                                    << face->vertex(2)[2] << "\n";
+                            stlfile << "      endloop\n";
+                            stlfile << "   endfacet\n";
+                        }
+                        else
+                        {
+                            stlfile << "   facet normal "
+                                    << normal_vector[0] / normal_norm << " "
+                                    << normal_vector[1] / normal_norm << " "
+                                    << normal_vector[2] / normal_norm << "\n";
+                            stlfile << "      outer loop\n";
+                            stlfile << "         vertex " << face->vertex(0)[0]
+                                    << " " << face->vertex(0)[1] << " "
+                                    << face->vertex(0)[2] << "\n";
+                            stlfile << "         vertex " << face->vertex(2)[0]
+                                    << " " << face->vertex(2)[1] << " "
+                                    << face->vertex(2)[2] << "\n";
+                            stlfile << "         vertex " << face->vertex(1)[0]
+                                    << " " << face->vertex(1)[1] << " "
+                                    << face->vertex(1)[2] << "\n";
+                            stlfile << "      endloop\n";
+                            stlfile << "   endfacet\n";
+                            stlfile << "   facet normal "
+                                    << normal_vector[0] / normal_norm << " "
+                                    << normal_vector[1] / normal_norm << " "
+                                    << normal_vector[2] / normal_norm << "\n";
+                            stlfile << "      outer loop\n";
+                            stlfile << "         vertex " << face->vertex(1)[0]
+                                    << " " << face->vertex(1)[1] << " " << face->vertex(1)[2]
+                                    << "\n";
+                            stlfile << "         vertex " << face->vertex(2)[0]
+                                    << " " << face->vertex(2)[1] << " " << face->vertex(2)[2]
+                                    << "\n";
+                            stlfile << "         vertex " << face->vertex(3)[0]
+                                    << " " << face->vertex(3)[1] << " "
+                                    << face->vertex(3)[2] << "\n";
+                            stlfile << "      endloop\n";
+                            stlfile << "   endfacet\n";
+                        }
+
+                    }
+
+                }
+            }
+        }
+    }
+}
+
+template class SAND::KktSystem<2>;
+template class SAND::KktSystem<3>;
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index b9d96bd..20b1517 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -47,7 +47,7 @@ namespace SAND {
             direct_solver_control(1, 0),
             a_inv_direct(direct_solver_control, additional_data),
             pcout(std::cout,(Utilities::MPI::this_mpi_process(mpi_communicator) == 0)),
-            timer(std::cout, TimerOutput::summary, TimerOutput::wall_times)
+            timer(pcout, TimerOutput::summary, TimerOutput::wall_times)
     {
 
     }
diff --git a/source/schur_preconditioner.cc.autosave b/source/schur_preconditioner.cc.autosave
new file mode 100644
index 0000000..a1bebac
--- /dev/null
+++ b/source/schur_preconditioner.cc.autosave
@@ -0,0 +1,693 @@
+//
+// Created by justin on 2/17/21.
+//
+#include <deal.II/lac/linear_operator.h>
+#include <deal.II/lac/sparse_direct.h>
+#include <deal.II/lac/sparse_matrix.h>
+#include <deal.II/base/timer.h>
+#include <deal.II/lac/generic_linear_algebra.h>
+#include <deal.II/lac/trilinos_solver.h>
+#include <deal.II/lac/trilinos_precondition.h>
+#include "../include/schur_preconditioner.h"
+#include "../include/input_information.h"
+#include "../include/sand_tools.h"
+#include <fstream>
+
+namespace SAND {
+    using MatrixType  = dealii::TrilinosWrappers::SparseMatrix;
+    using VectorType  = dealii::TrilinosWrappers::MPI::Vector;
+    using PayloadType = dealii::TrilinosWrappers::internal::LinearOperatorImplementation::TrilinosPayload;
+    using PayloadVectorType = typename PayloadType::VectorType;
+    using size_type         = dealii::types::global_dof_index;
+    using namespace dealii;
+    template<int dim>
+    TopOptSchurPreconditioner<dim>::TopOptSchurPreconditioner(LA::MPI::BlockSparseMatrix &matrix_in)
+            :
+            mpi_communicator(MPI_COMM_WORLD),
+            system_matrix(matrix_in),
+            n_rows(0),
+            n_columns(0),
+            n_block_rows(0),
+            n_block_columns(0),
+            other_solver_control(100000, 1e-6),
+            other_bicgstab(other_solver_control),
+            other_gmres(other_solver_control),
+            other_cg(other_solver_control),
+            a_mat(matrix_in.block(SolutionBlocks::displacement, SolutionBlocks::displacement_multiplier)),
+            b_mat(matrix_in.block(SolutionBlocks::density, SolutionBlocks::density)),
+            c_mat(matrix_in.block(SolutionBlocks::displacement,SolutionBlocks::density)),
+            e_mat(matrix_in.block(SolutionBlocks::displacement_multiplier,SolutionBlocks::density)),
+            f_mat(matrix_in.block(SolutionBlocks::unfiltered_density_multiplier,SolutionBlocks::unfiltered_density)),
+            d_m_mat(matrix_in.block(SolutionBlocks::density_upper_slack_multiplier, SolutionBlocks::density_upper_slack)),
+            d_1_mat(matrix_in.block(SolutionBlocks::density_lower_slack, SolutionBlocks::density_lower_slack)),
+            d_2_mat(matrix_in.block(SolutionBlocks::density_upper_slack, SolutionBlocks::density_upper_slack)),
+            m_vect(matrix_in.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier)),
+            solver_type("Amesos_Klu"),
+            additional_data(false, solver_type),
+            direct_solver_control(1, 0),
+            a_inv_direct(direct_solver_control, additional_data),
+            pcout(std::cout,(Utilities::MPI::this_mpi_process(mpi_communicator) == 0)),
+            timer(pcout, TimerOutput::summary, TimerOutput::wall_times)
+    {
+
+    }
+
+    template<int dim>
+    void TopOptSchurPreconditioner<dim>::initialize(LA::MPI::BlockSparseMatrix &matrix, const std::map<types::global_dof_index, double> &boundary_values,const DoFHandler<dim> &dof_handler, const LA::MPI::BlockVector &locally_relevant_state, const LA::MPI::BlockVector &distributed_state)
+    {
+        TimerOutput::Scope t(timer, "initialize");
+        {
+
+            TimerOutput::Scope t(timer, "diag stuff");
+            const types::global_dof_index disp_start_index = system_matrix.get_row_indices().block_start(
+                    SolutionBlocks::displacement);
+            const types::global_dof_index disp_mult_start_index = system_matrix.get_row_indices().block_start(
+                    SolutionBlocks::displacement_multiplier);
+            for (auto&[dof_index, boundary_value]: boundary_values) {
+
+                const types::global_dof_index n_u = system_matrix.block(SolutionBlocks::displacement,
+                                                                        SolutionBlocks::displacement).m();
+                if ((dof_index >= disp_start_index) && (dof_index < disp_start_index + n_u)) {
+                    double diag_val = system_matrix.block(SolutionBlocks::displacement,
+                                                          SolutionBlocks::displacement).el(
+                            dof_index - disp_start_index, dof_index - disp_start_index);
+                    system_matrix.block(SolutionBlocks::displacement, SolutionBlocks::displacement_multiplier).set(
+                            dof_index - disp_start_index, dof_index - disp_start_index, diag_val);
+                } else if ((dof_index >= disp_mult_start_index) && (dof_index < disp_mult_start_index + n_u)) {
+                    double diag_val = system_matrix.block(SolutionBlocks::displacement_multiplier,
+                                                          SolutionBlocks::displacement_multiplier).el(
+                            dof_index - disp_mult_start_index, dof_index - disp_mult_start_index);
+                    system_matrix.block(SolutionBlocks::displacement_multiplier, SolutionBlocks::displacement).set(
+                            dof_index - disp_mult_start_index, dof_index - disp_mult_start_index, diag_val);
+                }
+            }
+
+            //set diagonal to 0?
+            for (auto&[dof_index, boundary_value]: boundary_values) {
+                const types::global_dof_index disp_start_index = system_matrix.get_row_indices().block_start(
+                        SolutionBlocks::displacement);
+                const types::global_dof_index disp_mult_start_index = system_matrix.get_row_indices().block_start(
+                        SolutionBlocks::displacement_multiplier);
+                const types::global_dof_index n_u = system_matrix.block(SolutionBlocks::displacement,
+                                                                        SolutionBlocks::displacement).m();
+                if ((dof_index >= disp_start_index) && (dof_index < disp_start_index + n_u)) {
+                    system_matrix.block(SolutionBlocks::displacement, SolutionBlocks::displacement).set(
+                            dof_index - disp_start_index, dof_index - disp_start_index, 0);
+                } else if ((dof_index >= disp_mult_start_index) && (dof_index < disp_mult_start_index + n_u)) {
+                    system_matrix.block(SolutionBlocks::displacement_multiplier,
+                                        SolutionBlocks::displacement_multiplier).set(
+                            dof_index - disp_mult_start_index, dof_index - disp_mult_start_index, 0);
+                }
+            }
+
+//            system_matrix.compress(VectorOperation::insert);
+
+//            const unsigned int m = a_mat.m();
+//            const unsigned int n = a_mat.n();
+//            std::ofstream Xmat("a_mat_par.csv");
+//            for (unsigned int i = 0; i < m; i++)
+//            {
+//                Xmat << a_mat.el(i, 0);
+//                for (unsigned int j = 1; j < n; j++)
+//                {
+//                    Xmat << "," << a_mat.el(i, j);
+//                }
+//                Xmat << "\n";
+//            }
+//            Xmat.close();
+
+        }
+        if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
+        {
+
+        }
+        else
+        {
+            TimerOutput::Scope t(timer, "build A inv");
+            a_inv_direct.initialize(a_mat);
+        }
+        {
+            TimerOutput::Scope t(timer, "reinit diag matrices");
+            d_3_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
+            d_4_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
+            d_5_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
+            d_6_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
+            d_7_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
+            d_8_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
+            d_m_inv_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
+            d_3_mat=0;
+            d_4_mat=0;
+            d_5_mat=0;
+            d_6_mat=0;
+            d_7_mat=0;
+            d_8_mat=0;
+            d_m_inv_mat=0;
+            pcout << "diag reinit" << std::endl;
+        }
+        {
+            const types::global_dof_index n_p = system_matrix.block(SolutionBlocks::density,
+                                                                    SolutionBlocks::density).m();
+
+            pcout << "np: " << n_p << std::endl;
+
+
+
+
+            double l_global[n_p]= {0};
+            double lm_global[n_p]= {0};
+            double u_global[n_p]= {0};
+            double um_global[n_p]= {0};
+            double m_global[n_p]= {0};
+
+            double l[n_p]= {0};
+            double lm[n_p]= {0};
+            double u[n_p]= {0};
+            double um[n_p]= {0};
+            double m[n_p]= {0};
+
+            TimerOutput::Scope t(timer, "build diag matrices");
+            for (const auto cell: dof_handler.active_cell_iterators())
+            {
+                if(cell->is_locally_owned())
+                {
+                    std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
+                    cell->get_dof_indices(i);
+
+
+
+                    const int i_ind = cell->get_fe().component_to_system_index(0,0);
+
+                    if(distributed_state.block(SolutionBlocks::density_lower_slack_multiplier).in_local_range(i[i_ind]))
+                    {
+                        lm[i[i_ind]] = distributed_state.block(SolutionBlocks::density_lower_slack_multiplier)[i[i_ind]];
+                        m[i[i_ind]] = cell->measure();
+                    }
+
+                    if(distributed_state.block(SolutionBlocks::density_lower_slack).in_local_range(i[i_ind]))
+                    {
+                        l[i[i_ind]] =  distributed_state.block(SolutionBlocks::density_lower_slack)[i[i_ind]];
+                    }
+
+                    if(distributed_state.block(SolutionBlocks::density_upper_slack_multiplier).in_local_range(i[i_ind]))
+                    {
+                        um[i[i_ind]]= distributed_state.block(SolutionBlocks::density_upper_slack_multiplier)[i[i_ind]];
+                    }
+
+                    if(distributed_state.block(SolutionBlocks::density_upper_slack).in_local_range(i[i_ind]))
+                    {
+                        u[i[i_ind]] = distributed_state.block(SolutionBlocks::density_upper_slack)[i[i_ind]];
+                    }
+                }
+            }
+
+            MPI_Allreduce(lm.data(), &lm_global, n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+            MPI_Allreduce(&l, &l_global, n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+            MPI_Allreduce(&um, &um_global, n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+            MPI_Allreduce(&u, &u_global, n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+            MPI_Allreduce(&m, &m_global, n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+
+            for (unsigned int k=0; k< n_p; k++)
+            {
+                    if(distributed_state.block(0).in_local_range(k))
+                    {
+                        d_3_mat.set(k, k, -1 * lm_global[k]/(m_global[k]*l_global[k]));
+                        d_4_mat.set(k, k, -1 * um_global[k]/(m_global[k]*u_global[k]));
+                        d_5_mat.set(k, k, lm_global[k]/l_global[k]);
+                        d_6_mat.set(k, k, um_global[k]/u_global[k]);
+                        d_7_mat.set(k, k, m_global[k]*(lm_global[k]*u_global[k] + um_global[k]*l_global[k])/(l_global[k]*u_global[k]));
+                        d_8_mat.set(k, k, l_global[k]*u_global[k]/(m_global[k]*(lm_global[k]*u_global[k] + um_global[k]*l_global[k])));
+                        d_m_inv_mat.set(k, k, 1 / m_global[k]);
+                    }
+            }
+        }
+        d_3_mat.compress(VectorOperation::insert);
+        d_4_mat.compress(VectorOperation::insert);
+        d_5_mat.compress(VectorOperation::insert);
+        d_6_mat.compress(VectorOperation::insert);
+        d_7_mat.compress(VectorOperation::insert);
+        d_8_mat.compress(VectorOperation::insert);
+        d_m_inv_mat.compress(VectorOperation::insert);
+
+        pcout << "compressed" << std::endl;
+
+        pre_j=distributed_state.block(SolutionBlocks::density);
+        pre_k=distributed_state.block(SolutionBlocks::density);
+        g_d_m_inv_density=distributed_state.block(SolutionBlocks::density);
+        k_g_d_m_inv_density=distributed_state.block(SolutionBlocks::density);
+
+    }
+
+
+    template<int dim>
+    void TopOptSchurPreconditioner<dim>::vmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
+        LA::MPI::BlockVector temp_src;
+        pcout << "vmult" << std::endl;
+        {
+            TimerOutput::Scope t(timer, "part 1");
+            vmult_step_1(dst, src);
+            temp_src = dst;
+        }
+
+        {
+            TimerOutput::Scope t(timer, "part 2");
+            vmult_step_2(dst, temp_src);
+            temp_src = dst;
+        }
+
+        {
+            TimerOutput::Scope t(timer, "part 3");
+            vmult_step_3(dst, temp_src);
+            temp_src = dst;
+        }
+        {
+            TimerOutput::Scope t(timer, "part 4");
+            vmult_step_4(dst, temp_src);
+            temp_src = dst;
+        }
+        vmult_step_5(dst, temp_src);        
+    }
+
+    template<int dim>
+    void TopOptSchurPreconditioner<dim>::Tvmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
+        dst = src;
+    }
+
+    template<int dim>
+    void TopOptSchurPreconditioner<dim>::vmult_add(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
+        LA::MPI::BlockVector dst_temp = dst;
+        vmult(dst_temp, src);
+        dst += dst_temp;
+    }
+
+    template<int dim>
+    void TopOptSchurPreconditioner<dim>::Tvmult_add(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
+        dst = src;
+    }
+
+    template<int dim>
+    void TopOptSchurPreconditioner<dim>::vmult_step_1(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
+        dst = src;
+        auto dst_temp = dst;
+        dst.block(SolutionBlocks::unfiltered_density) = dst_temp.block(SolutionBlocks::unfiltered_density)
+                - linear_operator<VectorType,VectorType,PayloadType>(d_5_mat) * src.block(SolutionBlocks::density_lower_slack_multiplier)
+                + linear_operator<VectorType,VectorType,PayloadType>(d_6_mat) * src.block(SolutionBlocks::density_upper_slack_multiplier)
+                + src.block(SolutionBlocks::density_lower_slack)
+                - src.block(SolutionBlocks::density_upper_slack);
+    }
+
+    template<int dim>
+    void TopOptSchurPreconditioner<dim>::vmult_step_2(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
+        dst = src;
+        auto dst_temp = dst;
+
+        dst.block(SolutionBlocks::unfiltered_density_multiplier) = dst_temp.block(SolutionBlocks::unfiltered_density_multiplier)
+                - linear_operator<VectorType,VectorType,PayloadType>(f_mat)*linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) * src.block(SolutionBlocks::unfiltered_density);
+
+    }
+
+    template<int dim>
+    void TopOptSchurPreconditioner<dim>::vmult_step_3(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
+        dst = src;
+
+        if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
+        {
+            SolverControl            solver_control_1(100000, 1e-6);
+            SolverControl            solver_control_2(100000, 1e-6);
+            SolverCG<LA::MPI::Vector> a_solver_cg_1(solver_control_1);
+            SolverCG<LA::MPI::Vector> a_solver_cg_2(solver_control_2);
+            auto dst_temp = dst;
+            a_solver_cg_1.solve(a_mat,dst_temp.block(SolutionBlocks::displacement_multiplier),src.block(SolutionBlocks::displacement_multiplier),PreconditionIdentity());
+            a_solver_cg_2.solve(a_mat,dst_temp.block(SolutionBlocks::displacement),src.block(SolutionBlocks::displacement),PreconditionIdentity());
+            c_mat.Tvmult(dst_temp.block(SolutionBlocks::density_upper_slack),dst_temp.block(SolutionBlocks::displacement_multiplier));
+            e_mat.Tvmult(dst_temp.block(SolutionBlocks::density_lower_slack),dst_temp.block(SolutionBlocks::displacement));
+
+            dst.block(SolutionBlocks::density) = dst_temp.block(SolutionBlocks::density) - dst_temp.block(SolutionBlocks::density_upper_slack) - dst_temp.block(SolutionBlocks::density_lower_slack);
+
+
+        }
+        else
+        {
+            auto dst_temp = dst;
+            dst.block(SolutionBlocks::density) = dst_temp.block(SolutionBlocks::density)
+                    - transpose_operator<VectorType,VectorType,PayloadType>(e_mat) * linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct)* src.block(SolutionBlocks::displacement)
+                    - transpose_operator<VectorType,VectorType,PayloadType>(c_mat) * linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct)* src.block(SolutionBlocks::displacement_multiplier);
+        }
+
+    }
+
+    template<int dim>
+    void TopOptSchurPreconditioner<dim>::vmult_step_4(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
+        dst = src;
+        auto dst_temp = dst;
+        auto k_density_mult =  src.block(SolutionBlocks::density);
+
+
+
+        if (Input::solver_choice == SolverOptions::exact_preconditioner_with_gmres)
+        {
+//            g_d_m_inv_density = linear_operator<VectorType,VectorType,PayloadType>(g_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
+//            k_g_d_m_inv_density = linear_operator<VectorType,VectorType,PayloadType>(k_mat) * g_d_m_inv_density;
+//            k_density_mult = linear_operator<VectorType,VectorType,PayloadType>(k_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
+        }
+
+        else if (Input::solver_choice == SolverOptions::inexact_K_with_exact_A_gmres)
+        {
+            auto op_d_8 = linear_operator<VectorType,VectorType,PayloadType>(d_8_mat);
+            auto op_f = linear_operator<VectorType,VectorType,PayloadType>(f_mat);
+            auto op_b = linear_operator<VectorType,VectorType,PayloadType>(b_mat);
+            auto op_c = linear_operator<VectorType,VectorType,PayloadType>(c_mat);
+            auto op_a_inv = linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct);
+            auto op_e = linear_operator<VectorType,VectorType,PayloadType>(e_mat);
+            auto op_d_m= linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
+            auto op_d_m_inv= linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat);
+
+            auto op_g = op_f * op_d_8 *
+                    transpose_operator(op_f);
+
+
+            auto op_h = op_b
+                   - transpose_operator(op_c) * op_a_inv * op_e
+                   - transpose_operator(op_e) * op_a_inv * op_c;
+
+            auto op_k_inv = -1 * op_g *op_d_m_inv * op_h - op_d_m;
+
+            g_d_m_inv_density = op_g * op_d_m_inv * src.block(SolutionBlocks::density);
+            SolverControl step_4_gmres_control_1 (100000, g_d_m_inv_density.l2_norm()*1e-6);
+            SolverGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
+            try {
+                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
+                                      g_d_m_inv_density;
+            }
+            catch (std::exception &exc)
+            {
+                std::cerr << "Failure of linear solver step_4_gmres_1" << std::endl;
+                pcout << "first residual: " << step_4_gmres_control_1.initial_value() << std::endl;
+                pcout << "last residual: " << step_4_gmres_control_1.last_value() << std::endl;
+                throw;
+            }
+            SolverControl step_4_gmres_control_2 (100000, src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6);
+            SolverGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
+            try {
+                k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, PreconditionIdentity()) *
+                                 src.block(SolutionBlocks::unfiltered_density_multiplier);
+            } catch (std::exception &exc)
+            {
+                std::cerr << "Failure of linear solver step_4_gmres_2" << std::endl;
+                pcout << "first residual: " << step_4_gmres_control_2.initial_value() << std::endl;
+                pcout << "last residual: " << step_4_gmres_control_2.last_value() << std::endl;
+                throw;
+            }
+        }
+        else if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
+        {
+
+            SolverControl            solver_control(100000, 1e-6);
+            SolverCG<LA::MPI::Vector> a_solver_cg(solver_control);
+
+            auto preconditioner = PreconditionIdentity();
+
+            auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg, preconditioner);
+
+
+            auto op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) *
+                        transpose_operator<VectorType, VectorType, PayloadType>(f_mat);
+
+            auto op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
+                        - transpose_operator<VectorType, VectorType, PayloadType>(c_mat) * a_inv_op * linear_operator<VectorType,VectorType,PayloadType>(e_mat)
+                        - transpose_operator<VectorType, VectorType, PayloadType>(e_mat) * a_inv_op * linear_operator<VectorType,VectorType,PayloadType>(c_mat);
+
+            auto op_k_inv = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
+
+            SolverControl step_4_gmres_control_1 (10000, std::max(g_d_m_inv_density.l2_norm()*1e-6,1e-6));
+            SolverGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
+            try {
+                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
+                                      g_d_m_inv_density;
+            } catch (std::exception &exc)
+            {
+                std::cerr << "Failure of linear solver step_4_gmres_1" << std::endl;
+                pcout << "first residual: " << step_4_gmres_control_1.initial_value() << std::endl;
+                pcout << "last residual: " << step_4_gmres_control_1.last_value() << std::endl;
+                throw;
+            }
+
+            SolverControl step_4_gmres_control_2 (10000, std::max(src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6,1e-6));
+            SolverGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
+            try {
+
+                k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, PreconditionIdentity()) *
+                                 src.block(SolutionBlocks::unfiltered_density_multiplier);
+            } catch (std::exception &exc)
+            {
+                std::cerr << "Failure of linear solver step_4_gmres_2" << std::endl;
+                pcout << "first residual: " << step_4_gmres_control_2.initial_value() << std::endl;
+                pcout << "last residual: " << step_4_gmres_control_2.last_value() << std::endl;
+                throw;
+            }
+        }
+        else
+        {
+            pcout << "shouldn't get here";
+            throw;
+        }
+
+        dst.block(SolutionBlocks::total_volume_multiplier) = transpose_operator<VectorType, VectorType, PayloadType>(m_vect)*k_g_d_m_inv_density
+                                                        - transpose_operator<VectorType, VectorType, PayloadType>(m_vect)*k_density_mult
+                                                        +dst_temp.block(SolutionBlocks::total_volume_multiplier);
+    }
+
+
+
+
+    template<int dim>
+    void TopOptSchurPreconditioner<dim>::vmult_step_5(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
+        {
+            //First Block Inverse
+            TimerOutput::Scope t(timer, "inverse 1");
+            dst.block(SolutionBlocks::density_lower_slack_multiplier) = linear_operator<VectorType,VectorType,PayloadType>(d_3_mat) * src.block(SolutionBlocks::density_lower_slack_multiplier) +
+                    linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density_lower_slack);
+            dst.block(SolutionBlocks::density_upper_slack_multiplier) = linear_operator<VectorType,VectorType,PayloadType>(d_4_mat) * src.block(SolutionBlocks::density_upper_slack_multiplier) +
+                    linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density_upper_slack);
+            dst.block(SolutionBlocks::density_lower_slack) = linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density_lower_slack_multiplier);
+            dst.block(SolutionBlocks::density_upper_slack) = linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density_upper_slack_multiplier);
+        }
+
+
+        {
+            //Second Block Inverse
+            TimerOutput::Scope t(timer, "inverse 2");
+            dst.block(SolutionBlocks::unfiltered_density) =
+                    linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) * src.block(SolutionBlocks::unfiltered_density);
+        }
+
+
+        {
+            //Third Block Inverse
+            TimerOutput::Scope t(timer, "inverse 3");
+            if(Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
+            {
+                SolverControl            solver_control(100000, 1e-6);
+                SolverCG<LA::MPI::Vector> a_solver_cg(solver_control);
+
+                auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg, PreconditionIdentity());
+
+                dst.block(SolutionBlocks::displacement) = a_inv_op * src.block(SolutionBlocks::displacement_multiplier);
+                dst.block(SolutionBlocks::displacement_multiplier) = a_inv_op * src.block(SolutionBlocks::displacement);
+            } else
+            {
+                dst.block(SolutionBlocks::displacement) = linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct) * src.block(SolutionBlocks::displacement_multiplier);
+                dst.block(SolutionBlocks::displacement_multiplier) = linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct) * src.block(SolutionBlocks::displacement);
+            }
+
+        }
+
+
+        {
+            //Fourth (ugly) Block Inverse
+            TimerOutput::Scope t(timer, "inverse 4");
+
+
+
+            if (Input::solver_choice == SolverOptions::exact_preconditioner_with_gmres)
+            {
+//                pre_j = src.block(SolutionBlocks::density) + linear_operator<VectorType,VectorType,PayloadType>(h_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
+//                pre_k = -1* linear_operator<VectorType,VectorType,PayloadType>(g_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density) + src.block(SolutionBlocks::unfiltered_density_multiplier);
+//                dst.block(SolutionBlocks::unfiltered_density_multiplier) = transpose_operator<VectorType, VectorType, PayloadType>(k_mat) * pre_j;
+//                dst.block(SolutionBlocks::density) = linear_operator<VectorType,VectorType,PayloadType>(k_mat) * pre_k;
+            }
+
+            else if (Input::solver_choice == SolverOptions::inexact_K_with_exact_A_gmres)
+            {
+                auto op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) *
+                            transpose_operator(linear_operator<VectorType,VectorType,PayloadType>(f_mat));
+
+                auto op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
+                            - transpose_operator<VectorType, VectorType, PayloadType>(c_mat) *
+                                    linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct) * linear_operator<VectorType,VectorType,PayloadType>(e_mat)
+                            - transpose_operator<VectorType, VectorType, PayloadType>(e_mat) * linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct) * linear_operator<VectorType,VectorType,PayloadType>(c_mat);
+
+                auto op_k_inv = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
+
+                pre_j = src.block(SolutionBlocks::density) + op_h * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
+                auto pre_pre_k = pre_k;
+                pre_pre_k = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
+                pre_k =  pre_pre_k + src.block(SolutionBlocks::unfiltered_density_multiplier);
+
+
+                SolverControl step_5_gmres_control_1 (100000, pre_j.l2_norm()*1e-6);
+                SolverGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
+                try {
+                    dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
+                                                                               pre_j;
+                } catch (std::exception &exc)
+                {
+                    std::cerr << "Failure of linear solver step_5_gmres_1" << std::endl;
+                    pcout << "first residual: " << step_5_gmres_control_1.initial_value() << std::endl;
+                    pcout << "last residual: " << step_5_gmres_control_1.last_value() << std::endl;
+                    throw;
+                }
+
+
+                SolverControl step_5_gmres_control_2 (100000, pre_k.l2_norm()*1e-6);
+                SolverGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
+                try {
+                    dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) * pre_k;
+                } catch (std::exception &exc)
+                {
+                    std::cerr << "Failure of linear solver step_5_gmres_2" << std::endl;
+                    pcout << "first residual: " << step_5_gmres_control_2.initial_value() << std::endl;
+                    pcout << "last residual: " << step_5_gmres_control_2.last_value() << std::endl;
+                    throw;
+                }
+            }
+            else if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
+            {
+
+                SolverControl            solver_control(100000, 1e-6);
+                SolverCG<LA::MPI::Vector> a_solver_cg (solver_control);
+                auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg, PreconditionIdentity());
+
+                auto op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) *
+                            transpose_operator<VectorType, VectorType, PayloadType>(f_mat);
+
+                auto op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
+                            - transpose_operator<VectorType, VectorType, PayloadType>(c_mat) * a_inv_op * linear_operator<VectorType,VectorType,PayloadType>(e_mat)
+                            - transpose_operator<VectorType, VectorType, PayloadType>(e_mat) * a_inv_op * linear_operator<VectorType,VectorType,PayloadType>(c_mat);
+
+                auto op_k_inv = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
+
+                pre_j = src.block(SolutionBlocks::density) + op_h * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
+                pre_k = -1* op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density) + src.block(SolutionBlocks::unfiltered_density_multiplier);
+
+                SolverControl step_5_gmres_control_1 (10000, std::max(pre_j.l2_norm()*1e-6,1e-6));
+                SolverGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
+                try {
+                    dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType,VectorType,PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
+                                                                               pre_j;
+                } catch (std::exception &exc)
+                {
+                    std::cerr << "Failure of linear solver step_5_gmres_1" << std::endl;
+                    pcout << "first residual: " << step_5_gmres_control_1.initial_value() << std::endl;
+                    pcout << "last residual: " << step_5_gmres_control_1.last_value() << std::endl;
+                    throw;
+                }
+
+                SolverControl step_5_gmres_control_2 (10000, std::max(pre_k.l2_norm()*1e-6,1e-6));
+                SolverGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
+                try {
+                    dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) *
+                                                         pre_k;
+                } catch (std::exception &exc)
+                {
+                    std::cerr << "Failure of linear solver step_5_gmres_2" << std::endl;
+                    pcout << "first residual: " << step_5_gmres_control_2.initial_value() << std::endl;
+                    pcout << "last residual: " << step_5_gmres_control_2.last_value() << std::endl;
+                    throw;
+                }
+            }
+            else
+            {
+                pcout << "shouldn't get here";
+                throw;
+            }
+
+        }
+        {
+            dst.block(SolutionBlocks::total_volume_multiplier) = src.block(SolutionBlocks::total_volume_multiplier);
+        }
+    }
+
+
+    template<int dim>
+    void TopOptSchurPreconditioner<dim>::print_stuff()
+    {
+
+//        print_matrix(std::string("OAmat.csv"),a_mat);
+//        print_matrix(std::string("OBmat.csv"),b_mat);
+//        print_matrix(std::string("OCmat.csv"),c_mat);
+//        print_matrix(std::string("OEmat.csv"),e_mat);
+//        print_matrix(std::string("OFmat.csv"),f_mat);
+        FullMatrix<double> g_mat;
+        FullMatrix<double> h_mat;
+        FullMatrix<double> k_inv_mat;
+        g_mat.reinit(b_mat.m(),b_mat.n());
+        h_mat.reinit(b_mat.m(),b_mat.n());
+        k_inv_mat.reinit(b_mat.m(),b_mat.n());
+        auto op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) *
+                    transpose_operator(linear_operator<VectorType,VectorType,PayloadType>(f_mat));
+
+        auto op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
+                    - transpose_operator(linear_operator<VectorType,VectorType,PayloadType>(c_mat)) * linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct) * linear_operator<VectorType,VectorType,PayloadType>(e_mat)
+                    - transpose_operator(linear_operator<VectorType,VectorType,PayloadType>(e_mat)) * linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct) * linear_operator<VectorType,VectorType,PayloadType>(c_mat);
+
+        auto op_k_inv = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
+//        build_matrix_element_by_element(op_g,g_mat, src.block(SolutionBlocks::density));
+//        build_matrix_element_by_element(op_h,h_mat, src.block(SolutionBlocks::density));
+//        build_matrix_element_by_element(op_k_inv,k_inv_mat, src.block(SolutionBlocks::density));
+//        print_matrix(std::string("OGmat.csv"),g_mat);
+//        print_matrix(std::string("OHmat.csv"),h_mat);
+//        print_matrix(std::string("OKinvmat.csv"),k_inv_mat);
+
+
+    }
+
+    void VmultTrilinosSolverDirect::initialize(LA::MPI::SparseMatrix &a_mat)
+    {
+        reinit(a_mat);
+        solver_direct.initialize(a_mat);
+        size = a_mat.n();
+    }
+
+    void
+    VmultTrilinosSolverDirect::vmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
+    {
+        solver_direct.solve(dst, src);
+    }
+
+    void VmultTrilinosSolverDirect::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+    {
+        solver_direct.solve(dst, src);
+    }
+
+    void
+    VmultTrilinosSolverDirect::Tvmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
+    {
+        solver_direct.solve(dst, src);
+    }
+
+    void VmultTrilinosSolverDirect::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+    {
+        solver_direct.solve(dst, src);
+    }
+
+
+    VmultTrilinosSolverDirect::VmultTrilinosSolverDirect(SolverControl &cn,
+                     const TrilinosWrappers::SolverDirect::AdditionalData &data)
+    : solver_direct(cn, data)
+    {
+
+    }
+
+}
+template class SAND::TopOptSchurPreconditioner<2>;
+template class SAND::TopOptSchurPreconditioner<3>;

From 76ede5521c153d7084668e8ea48a9af998c5c7b9 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Mon, 27 Dec 2021 13:55:30 -0700
Subject: [PATCH 49/95] using solve instead of inverse_operator

---
 include/input_information.h    |  4 ++--
 source/schur_preconditioner.cc | 33 ++++++++++++++++++++++-----------
 2 files changed, 24 insertions(+), 13 deletions(-)

diff --git a/include/input_information.h b/include/input_information.h
index 13ccac3..2cd273a 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -16,14 +16,14 @@ namespace SAND {
 
         //geometry options
         constexpr unsigned int geometry_base = GeometryOptions::mbb;
-        constexpr unsigned int dim = 3;
+        constexpr unsigned int dim = 2;
         constexpr unsigned int refinements =  3;
 
         //nonlinear algorithm options
         constexpr double initial_barrier_size = 25;
         constexpr double min_barrier_size = 0;
         constexpr double fraction_to_boundary = .9;
-        constexpr unsigned int max_steps=25;
+        constexpr unsigned int max_steps=2;
         constexpr unsigned int barrier_reduction=BarrierOptions::loqo;
         constexpr double required_norm = .0001;
 
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 0e774c9..0de39b0 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -382,8 +382,10 @@ namespace SAND {
             SolverControl step_4_gmres_control_1 (100000, g_d_m_inv_density.l2_norm()*1e-6);
             SolverGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
             try {
-                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
-                                      g_d_m_inv_density;
+                TimerOutput::Scope u(timer, "actual inv");
+//                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
+//                                      g_d_m_inv_density;
+                step_4_gmres_1.solve(op_k_inv,k_g_d_m_inv_density,g_d_m_inv_density, PreconditionIdentity() );
             }
             catch (std::exception &exc)
             {
@@ -395,8 +397,10 @@ namespace SAND {
             SolverControl step_4_gmres_control_2 (100000, src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6);
             SolverGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
             try {
-                k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, PreconditionIdentity()) *
-                                 src.block(SolutionBlocks::unfiltered_density_multiplier);
+                TimerOutput::Scope u(timer, "actual inv");
+//                k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, PreconditionIdentity()) *
+//                                 src.block(SolutionBlocks::unfiltered_density_multiplier);
+                step_4_gmres_2.solve(op_k_inv,k_density_mult,src.block(SolutionBlocks::unfiltered_density_multiplier), PreconditionIdentity() );
             } catch (std::exception &exc)
             {
                 std::cerr << "Failure of linear solver step_4_gmres_2" << std::endl;
@@ -413,7 +417,9 @@ namespace SAND {
 
             auto preconditioner = PreconditionIdentity();
 
-            auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg, preconditioner);
+            auto a_inv_op = op_d_8;
+
+                    inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg, preconditioner);
 
 
             auto op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) *
@@ -423,13 +429,16 @@ namespace SAND {
                         - transpose_operator<VectorType, VectorType, PayloadType>(c_mat) * a_inv_op * linear_operator<VectorType,VectorType,PayloadType>(e_mat)
                         - transpose_operator<VectorType, VectorType, PayloadType>(e_mat) * a_inv_op * linear_operator<VectorType,VectorType,PayloadType>(c_mat);
 
-            auto op_k_inv = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
+            auto op_k_inv = op_d_8;
+
+                    -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
 
             SolverControl step_4_gmres_control_1 (10000, std::max(g_d_m_inv_density.l2_norm()*1e-6,1e-6));
             SolverGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
             try {
-                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
-                                      g_d_m_inv_density;
+//                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
+//                                      g_d_m_inv_density;
+//                step_4_gmres_1.solve(op_k_inv,k_g_d_m_inv_density,g_d_m_inv_density, PreconditionIdentity() );
             } catch (std::exception &exc)
             {
                 std::cerr << "Failure of linear solver step_4_gmres_1" << std::endl;
@@ -533,8 +542,9 @@ namespace SAND {
                 SolverControl step_5_gmres_control_1 (100000, pre_j.l2_norm()*1e-6);
                 SolverGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
                 try {
-                    dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
-                                                                               pre_j;
+//                    dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
+//                                                                               pre_j;
+                    step_5_gmres_1.solve(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv),dst.block(SolutionBlocks::unfiltered_density_multiplier), pre_j , PreconditionIdentity());
                 } catch (std::exception &exc)
                 {
                     std::cerr << "Failure of linear solver step_5_gmres_1" << std::endl;
@@ -547,7 +557,8 @@ namespace SAND {
                 SolverControl step_5_gmres_control_2 (100000, pre_k.l2_norm()*1e-6);
                 SolverGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
                 try {
-                    dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) * pre_k;
+//                    dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) * pre_k;
+                        step_5_gmres_1.solve(op_k_inv,dst.block(SolutionBlocks::density), pre_k , PreconditionIdentity());
                 } catch (std::exception &exc)
                 {
                     std::cerr << "Failure of linear solver step_5_gmres_2" << std::endl;

From 1fe58a7a8525766e7628a18dbcb4ab0306be0c97 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Mon, 27 Dec 2021 13:56:23 -0700
Subject: [PATCH 50/95] using solve instead of inverse_operator

---
 include/input_information.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/include/input_information.h b/include/input_information.h
index 2cd273a..aac4cee 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -16,8 +16,8 @@ namespace SAND {
 
         //geometry options
         constexpr unsigned int geometry_base = GeometryOptions::mbb;
-        constexpr unsigned int dim = 2;
-        constexpr unsigned int refinements =  3;
+        constexpr unsigned int dim = 3;
+        constexpr unsigned int refinements =  6;
 
         //nonlinear algorithm options
         constexpr double initial_barrier_size = 25;

From a395e70e73abc9dc8ad8bb137f2a59ea687b79ec Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Mon, 27 Dec 2021 15:02:44 -0700
Subject: [PATCH 51/95] mod

---
 include/input_information.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/include/input_information.h b/include/input_information.h
index aac4cee..5ac9c75 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -16,8 +16,8 @@ namespace SAND {
 
         //geometry options
         constexpr unsigned int geometry_base = GeometryOptions::mbb;
-        constexpr unsigned int dim = 3;
-        constexpr unsigned int refinements =  6;
+        constexpr unsigned int dim = 2;
+        constexpr unsigned int refinements =  4;
 
         //nonlinear algorithm options
         constexpr double initial_barrier_size = 25;

From 6acc1e3c5a384ebf00dd25ab49f87d1e2c2a3d58 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Mon, 27 Dec 2021 15:13:00 -0700
Subject: [PATCH 52/95] more refinements

---
 include/input_information.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/include/input_information.h b/include/input_information.h
index 5ac9c75..aac4cee 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -16,8 +16,8 @@ namespace SAND {
 
         //geometry options
         constexpr unsigned int geometry_base = GeometryOptions::mbb;
-        constexpr unsigned int dim = 2;
-        constexpr unsigned int refinements =  4;
+        constexpr unsigned int dim = 3;
+        constexpr unsigned int refinements =  6;
 
         //nonlinear algorithm options
         constexpr double initial_barrier_size = 25;

From b851836a55bc81b803d0bbc6f90c0b98688bd7bb Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Mon, 27 Dec 2021 16:02:33 -0700
Subject: [PATCH 53/95] 5 refines

---
 include/input_information.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/include/input_information.h b/include/input_information.h
index aac4cee..8f67ff6 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -17,7 +17,7 @@ namespace SAND {
         //geometry options
         constexpr unsigned int geometry_base = GeometryOptions::mbb;
         constexpr unsigned int dim = 3;
-        constexpr unsigned int refinements =  6;
+        constexpr unsigned int refinements =  5;
 
         //nonlinear algorithm options
         constexpr double initial_barrier_size = 25;

From eb1cf371f7995952ebd6c88eae27712ca159b072 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Mon, 3 Jan 2022 11:46:55 -0700
Subject: [PATCH 54/95] ready to try

---
 include/input_information.h    |  2 +-
 include/schur_preconditioner.h |  3 +--
 source/schur_preconditioner.cc | 41 +++++++++++++++++-----------------
 3 files changed, 23 insertions(+), 23 deletions(-)

diff --git a/include/input_information.h b/include/input_information.h
index 8f67ff6..e268732 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -23,7 +23,7 @@ namespace SAND {
         constexpr double initial_barrier_size = 25;
         constexpr double min_barrier_size = 0;
         constexpr double fraction_to_boundary = .9;
-        constexpr unsigned int max_steps=2;
+        constexpr unsigned int max_steps=25;
         constexpr unsigned int barrier_reduction=BarrierOptions::loqo;
         constexpr double required_norm = .0001;
 
diff --git a/include/schur_preconditioner.h b/include/schur_preconditioner.h
index be1f39e..d4e6ade 100644
--- a/include/schur_preconditioner.h
+++ b/include/schur_preconditioner.h
@@ -187,8 +187,7 @@ namespace SAND
         LinearOperator<VectorType,VectorType,PayloadType> op_g;
         LinearOperator<VectorType,VectorType,PayloadType> op_h;
         LinearOperator<VectorType,VectorType,PayloadType> op_k_inv;
-
-
+        LinearOperator<VectorType,VectorType,PayloadType> op_j_inv;
     };
 
 }
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 0de39b0..2e0f4f3 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -153,17 +153,17 @@ namespace SAND {
 
 
 
-            double l_global[n_p]= {0};
-            double lm_global[n_p]= {0};
-            double u_global[n_p]= {0};
-            double um_global[n_p]= {0};
-            double m_global[n_p]= {0};
-
-            double l[n_p]= {0};
-            double lm[n_p]= {0};
-            double u[n_p]= {0};
-            double um[n_p]= {0};
-            double m[n_p]= {0};
+            std::vector<double> l_global(n_p);
+            std::vector<double> lm_global(n_p);
+            std::vector<double> u_global(n_p);
+            std::vector<double> um_global(n_p);
+            std::vector<double> m_global(n_p);
+
+            std::vector<double> l(n_p);
+            std::vector<double> lm(n_p);
+            std::vector<double> u(n_p);
+            std::vector<double> um(n_p);
+            std::vector<double> m(n_p);
 
             TimerOutput::Scope t(timer, "build diag matrices");
             for (const auto cell: dof_handler.active_cell_iterators())
@@ -200,11 +200,11 @@ namespace SAND {
                 }
             }
 
-            MPI_Allreduce(&lm, &lm_global, n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-            MPI_Allreduce(&l, &l_global, n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-            MPI_Allreduce(&um, &um_global, n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-            MPI_Allreduce(&u, &u_global, n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-            MPI_Allreduce(&m, &m_global, n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+            MPI_Allreduce(lm.data(), lm_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+            MPI_Allreduce(l.data(), l_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+            MPI_Allreduce(um.data(), um_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+            MPI_Allreduce(u.data(), u_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+            MPI_Allreduce(m.data(), m_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
 
             for (unsigned int k=0; k< n_p; k++)
             {
@@ -257,7 +257,7 @@ namespace SAND {
 
         op_k_inv = (-1 * op_g *op_d_m_inv * op_h - op_d_m);
 
-
+        op_j_inv = transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv);
 
 
     }
@@ -538,13 +538,13 @@ namespace SAND {
                 auto pre_pre_k = pre_k;
                 pre_pre_k = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
                 pre_k =  pre_pre_k + src.block(SolutionBlocks::unfiltered_density_multiplier);
-
                 SolverControl step_5_gmres_control_1 (100000, pre_j.l2_norm()*1e-6);
                 SolverGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
                 try {
+                    TimerOutput::Scope t(timer, "actual inverse 4.1");
 //                    dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
 //                                                                               pre_j;
-                    step_5_gmres_1.solve(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv),dst.block(SolutionBlocks::unfiltered_density_multiplier), pre_j , PreconditionIdentity());
+                    step_5_gmres_1.solve(op_j_inv, dst.block(SolutionBlocks::unfiltered_density_multiplier), pre_j , PreconditionIdentity());
                 } catch (std::exception &exc)
                 {
                     std::cerr << "Failure of linear solver step_5_gmres_1" << std::endl;
@@ -557,8 +557,9 @@ namespace SAND {
                 SolverControl step_5_gmres_control_2 (100000, pre_k.l2_norm()*1e-6);
                 SolverGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
                 try {
+                    TimerOutput::Scope t(timer, "actual inverse 4.2");
 //                    dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) * pre_k;
-                        step_5_gmres_1.solve(op_k_inv,dst.block(SolutionBlocks::density), pre_k , PreconditionIdentity());
+                        step_5_gmres_2.solve(op_k_inv,dst.block(SolutionBlocks::density), pre_k , PreconditionIdentity());
                 } catch (std::exception &exc)
                 {
                     std::cerr << "Failure of linear solver step_5_gmres_2" << std::endl;

From 3683a6aeb8a9453363a16225f342d4a7d1a0d37b Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Tue, 4 Jan 2022 10:07:37 -0700
Subject: [PATCH 55/95] 4 refinements

---
 include/input_information.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/include/input_information.h b/include/input_information.h
index e268732..5fc4649 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -17,7 +17,7 @@ namespace SAND {
         //geometry options
         constexpr unsigned int geometry_base = GeometryOptions::mbb;
         constexpr unsigned int dim = 3;
-        constexpr unsigned int refinements =  5;
+        constexpr unsigned int refinements =  4;
 
         //nonlinear algorithm options
         constexpr double initial_barrier_size = 25;

From b4614fbb83102ea4e76054db052b2c3a9f81d383 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Sat, 8 Jan 2022 12:25:11 -0700
Subject: [PATCH 56/95] hi

---
 include/input_information.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/include/input_information.h b/include/input_information.h
index 5fc4649..e268732 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -17,7 +17,7 @@ namespace SAND {
         //geometry options
         constexpr unsigned int geometry_base = GeometryOptions::mbb;
         constexpr unsigned int dim = 3;
-        constexpr unsigned int refinements =  4;
+        constexpr unsigned int refinements =  5;
 
         //nonlinear algorithm options
         constexpr double initial_barrier_size = 25;

From e374410b5d1b31b53c6dacf1b6e60a1d39fbf2ab Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Mon, 10 Jan 2022 08:27:57 -0700
Subject: [PATCH 57/95] warning killing

---
 include/schur_preconditioner.h | 5 +++--
 source/kkt_system.cc           | 6 ++----
 source/markov_filter.cc        | 2 +-
 source/schur_preconditioner.cc | 4 ++--
 4 files changed, 8 insertions(+), 9 deletions(-)

diff --git a/include/schur_preconditioner.h b/include/schur_preconditioner.h
index d4e6ade..ddf9b09 100644
--- a/include/schur_preconditioner.h
+++ b/include/schur_preconditioner.h
@@ -104,7 +104,7 @@ namespace SAND
     class TopOptSchurPreconditioner: public Subscriptor {
     public:
         TopOptSchurPreconditioner(LA::MPI::BlockSparseMatrix &matrix_in);
-        void initialize (LA::MPI::BlockSparseMatrix &matrix, const std::map<types::global_dof_index, double> &boundary_values, const DoFHandler<dim> &dof_handler, const LA::MPI::BlockVector &state, const LA::MPI::BlockVector &distributed_state);
+        void initialize (LA::MPI::BlockSparseMatrix &matrix, const std::map<types::global_dof_index, double> &boundary_values, const DoFHandler<dim> &dof_handler, const LA::MPI::BlockVector &distributed_state);
         void vmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
         void Tvmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
         void vmult_add(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
@@ -173,8 +173,9 @@ namespace SAND
         SolverControl direct_solver_control;
         mutable VmultTrilinosSolverDirect a_inv_direct;
 //        TrilinosWrappers::SolverDirect a_inv_direct;
-        mutable TimerOutput timer;
         ConditionalOStream pcout;
+        mutable TimerOutput timer;
+
 
         LinearOperator<VectorType,VectorType,PayloadType> op_d_8;
         LinearOperator<VectorType,VectorType,PayloadType> op_f;
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index 788ed43..0497f42 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -1143,8 +1143,6 @@ namespace SAND {
                 std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
                 cell->get_dof_indices(i);
 
-                const types::global_dof_index dof_index_lower_slack =
-                        i[cell->get_fe().component_to_system_index(0, 0)];
                 typename LA::MPI::SparseMatrix::iterator iter = density_filter.filter_matrix.begin(
                         i[cell->get_fe().component_to_system_index(0, 0)]);
                 for (; iter != density_filter.filter_matrix.end(i[cell->get_fe().component_to_system_index(0, 0)]); iter++) {
@@ -1785,14 +1783,14 @@ namespace SAND {
         switch (Input::solver_choice) {
 
             case SolverOptions::inexact_K_with_exact_A_gmres: {
-                preconditioner.initialize(system_matrix, boundary_values, dof_handler,  locally_relevant_solution, distributed_solution);
+                preconditioner.initialize(system_matrix, boundary_values, dof_handler, distributed_solution);
                 SolverFGMRES<LA::MPI::BlockVector> B_fgmres(solver_control);
                 B_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
                 pcout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
                 break;
             }
             case SolverOptions::inexact_K_with_inexact_A_gmres: {
-                preconditioner.initialize(system_matrix, boundary_values, dof_handler,  locally_relevant_solution, distributed_solution);
+                preconditioner.initialize(system_matrix, boundary_values, dof_handler, distributed_solution);
                 SolverFGMRES<LA::MPI::BlockVector> C_fgmres(solver_control);
                 C_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
                 pcout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
diff --git a/source/markov_filter.cc b/source/markov_filter.cc
index f5d2c5c..b13609e 100644
--- a/source/markov_filter.cc
+++ b/source/markov_filter.cc
@@ -47,4 +47,4 @@ MarkovFilter::check_filter(const double objective_value_input, const  double bar
     {
         return false;
     }
-}
\ No newline at end of file
+}
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 2e0f4f3..9412af8 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -23,8 +23,8 @@ namespace SAND {
     template<int dim>
     TopOptSchurPreconditioner<dim>::TopOptSchurPreconditioner(LA::MPI::BlockSparseMatrix &matrix_in)
             :
-            mpi_communicator(MPI_COMM_WORLD),
             system_matrix(matrix_in),
+            mpi_communicator(MPI_COMM_WORLD),
             n_rows(0),
             n_columns(0),
             n_block_rows(0),
@@ -53,7 +53,7 @@ namespace SAND {
     }
 
     template<int dim>
-    void TopOptSchurPreconditioner<dim>::initialize(LA::MPI::BlockSparseMatrix &matrix, const std::map<types::global_dof_index, double> &boundary_values,const DoFHandler<dim> &dof_handler, const LA::MPI::BlockVector &locally_relevant_state, const LA::MPI::BlockVector &distributed_state)
+    void TopOptSchurPreconditioner<dim>::initialize(LA::MPI::BlockSparseMatrix &matrix, const std::map<types::global_dof_index, double> &boundary_values,const DoFHandler<dim> &dof_handler, const LA::MPI::BlockVector &distributed_state)
     {
         TimerOutput::Scope t(timer, "initialize");
         {

From b0b8ec46a5510718996a95a9f4a27ffd508cef93 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Tue, 18 Jan 2022 10:15:01 -0700
Subject: [PATCH 58/95] killed errors

---
 include/density_filter.h       |  1 +
 include/input_information.h    |  6 +++---
 include/kkt_system.h           |  2 +-
 source/density_filter.cc       |  9 +++++----
 source/schur_preconditioner.cc | 23 ++++++++++++-----------
 5 files changed, 22 insertions(+), 19 deletions(-)

diff --git a/include/density_filter.h b/include/density_filter.h
index 9656fad..5bee367 100644
--- a/include/density_filter.h
+++ b/include/density_filter.h
@@ -26,6 +26,7 @@
 #include <deal.II/grid/tria.h>
 #include <deal.II/grid/grid_generator.h>
 #include <deal.II/grid/grid_refinement.h>
+#include <deal.II/grid/cell_id.h>
 
 #include <deal.II/dofs/dof_handler.h>
 #include <deal.II/dofs/dof_renumbering.h>
diff --git a/include/input_information.h b/include/input_information.h
index e268732..995f9de 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -16,13 +16,13 @@ namespace SAND {
 
         //geometry options
         constexpr unsigned int geometry_base = GeometryOptions::mbb;
-        constexpr unsigned int dim = 3;
-        constexpr unsigned int refinements =  5;
+        constexpr unsigned int dim = 2;
+        constexpr unsigned int refinements =  4;
 
         //nonlinear algorithm options
         constexpr double initial_barrier_size = 25;
         constexpr double min_barrier_size = 0;
-        constexpr double fraction_to_boundary = .9;
+        constexpr double fraction_to_boundary = .84;
         constexpr unsigned int max_steps=25;
         constexpr unsigned int barrier_reduction=BarrierOptions::loqo;
         constexpr double required_norm = .0001;
diff --git a/include/kkt_system.h b/include/kkt_system.h
index 95907eb..5d79cf9 100644
--- a/include/kkt_system.h
+++ b/include/kkt_system.h
@@ -134,7 +134,7 @@ namespace SAND {
         mutable LA::MPI::BlockVector locally_relevant_solution;
         mutable LA::MPI::BlockVector distributed_solution;
         LA::MPI::BlockVector system_rhs;
-        parallel::shared::Triangulation<dim> triangulation;
+        parallel::distributed::Triangulation<dim> triangulation;
         DoFHandler<dim> dof_handler;
         AffineConstraints<double> constraints;
         FESystem<dim> fe_nine;
diff --git a/source/density_filter.cc b/source/density_filter.cc
index 2740900..359e4e0 100644
--- a/source/density_filter.cc
+++ b/source/density_filter.cc
@@ -7,6 +7,7 @@
 #include <deal.II/lac/block_sparse_matrix.h>
 #include <deal.II/lac/precondition.h>
 #include <deal.II/grid/tria.h>
+#include <deal.II/grid/cell_id.h>
 #include <deal.II/numerics/vector_tools.h>
 #include <deal.II/numerics/matrix_tools.h>
 
@@ -94,9 +95,9 @@ namespace SAND {
     template<int dim>
     std::set<typename DoFHandler<dim>::cell_iterator>
     DensityFilter<dim>::find_relevant_neighbors(typename DoFHandler<dim>::cell_iterator cell) const {
-        std::set<unsigned int> neighbor_ids;
+        std::set<CellId> neighbor_ids;
         std::set<typename DoFHandler<dim>::cell_iterator> cells_to_check;
-        neighbor_ids.insert(cell->active_cell_index());
+        neighbor_ids.insert(cell->id());
         cells_to_check.insert(cell);
         bool new_neighbors_found;
         do {
@@ -110,10 +111,10 @@ namespace SAND {
                         const double distance =
                                 cell->center().distance(neighbor->center());
                         if ((distance < Input::filter_r) &&
-                            !(neighbor_ids.count(neighbor->active_cell_index())))
+                            !(neighbor_ids.count(neighbor->id())))
                         {
                             cells_to_check.insert(neighbor);
-                            neighbor_ids.insert(neighbor->active_cell_index());
+                            neighbor_ids.insert(neighbor->id());
                             new_neighbors_found = true;
                         }
                     }
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 9412af8..84418db 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -257,7 +257,7 @@ namespace SAND {
 
         op_k_inv = (-1 * op_g *op_d_m_inv * op_h - op_d_m);
 
-        op_j_inv = transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv);
+
 
 
     }
@@ -366,8 +366,6 @@ namespace SAND {
         auto dst_temp = dst;
         auto k_density_mult =  src.block(SolutionBlocks::density);
 
-
-
         if (Input::solver_choice == SolverOptions::exact_preconditioner_with_gmres)
         {
 //            g_d_m_inv_density = linear_operator<VectorType,VectorType,PayloadType>(g_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
@@ -534,20 +532,23 @@ namespace SAND {
 
             else if (Input::solver_choice == SolverOptions::inexact_K_with_exact_A_gmres)
             {
-                pre_j = src.block(SolutionBlocks::density) + op_h * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
                 auto pre_pre_k = pre_k;
-                pre_pre_k = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
-                pre_k =  pre_pre_k + src.block(SolutionBlocks::unfiltered_density_multiplier);
+                {
+                    TimerOutput::Scope t(timer, "not inverse 5.1");
+                    pre_j = src.block(SolutionBlocks::density) + op_h * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
+                    pre_pre_k = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
+                    pre_k =  pre_pre_k + src.block(SolutionBlocks::unfiltered_density_multiplier);
+                }
                 SolverControl step_5_gmres_control_1 (100000, pre_j.l2_norm()*1e-6);
                 SolverGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
                 try {
-                    TimerOutput::Scope t(timer, "actual inverse 4.1");
+                    TimerOutput::Scope t(timer, "actual inverse 5.1");
 //                    dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
 //                                                                               pre_j;
-                    step_5_gmres_1.solve(op_j_inv, dst.block(SolutionBlocks::unfiltered_density_multiplier), pre_j , PreconditionIdentity());
+                    step_5_gmres_1.solve(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), dst.block(SolutionBlocks::unfiltered_density_multiplier), pre_j , PreconditionIdentity());
                 } catch (std::exception &exc)
                 {
-                    std::cerr << "Failure of linear solver step_5_gmres_1" << std::endl;
+                    std::cerr << "Failure of linear solver step_5_gmres_1 again" << std::endl;
                     pcout << "first residual: " << step_5_gmres_control_1.initial_value() << std::endl;
                     pcout << "last residual: " << step_5_gmres_control_1.last_value() << std::endl;
                     throw;
@@ -557,9 +558,9 @@ namespace SAND {
                 SolverControl step_5_gmres_control_2 (100000, pre_k.l2_norm()*1e-6);
                 SolverGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
                 try {
-                    TimerOutput::Scope t(timer, "actual inverse 4.2");
+                    TimerOutput::Scope t(timer, "actual inverse 5.2");
 //                    dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) * pre_k;
-                        step_5_gmres_2.solve(op_k_inv,dst.block(SolutionBlocks::density), pre_k , PreconditionIdentity());
+                    step_5_gmres_2.solve(op_k_inv,dst.block(SolutionBlocks::density), pre_k , PreconditionIdentity());
                 } catch (std::exception &exc)
                 {
                     std::cerr << "Failure of linear solver step_5_gmres_2" << std::endl;

From f5dc6962f071578e1e6c53dcfdbd23b8eefec180 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Tue, 18 Jan 2022 11:51:40 -0700
Subject: [PATCH 59/95] ready to go

---
 include/input_information.h    |  4 ++--
 source/kkt_system.cc           | 12 ++++++------
 source/schur_preconditioner.cc |  7 +++++++
 3 files changed, 15 insertions(+), 8 deletions(-)

diff --git a/include/input_information.h b/include/input_information.h
index 995f9de..a77071e 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -16,8 +16,8 @@ namespace SAND {
 
         //geometry options
         constexpr unsigned int geometry_base = GeometryOptions::mbb;
-        constexpr unsigned int dim = 2;
-        constexpr unsigned int refinements =  4;
+        constexpr unsigned int dim = 3;
+        constexpr unsigned int refinements =  5;
 
         //nonlinear algorithm options
         constexpr double initial_barrier_size = 25;
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index 0497f42..d17d446 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -1737,9 +1737,9 @@ namespace SAND {
         for (const auto &cell: dof_handler.active_cell_iterators()) {
             if(cell->is_locally_owned())
             {
-                if (distributed_solution.block(SolutionBlocks::density).in_local_range(cell->active_cell_index()))
+                if (distributed_solution.block(SolutionBlocks::density).in_local_range(cell->get_fe().component_to_system_index(0, 0)))
                 {
-                    total_volume_temp += cell->measure() * state.block(SolutionBlocks::density)[cell->active_cell_index()];
+                    total_volume_temp += cell->measure() * state.block(SolutionBlocks::density)[cell->get_fe().component_to_system_index(0, 0)];
                     goal_volume_temp += cell->measure() * Input::volume_percentage;
                 }
             }
@@ -1907,7 +1907,7 @@ namespace SAND {
 
         for (const auto &cell: dof_handler.active_cell_iterators()) {
             if (state.block(
-                    SolutionBlocks::density)[cell->active_cell_index()] > 0.5) {
+                    SolutionBlocks::density)[cell->get_fe().component_to_system_index(0, 0)] > 0.5) {
                 const Tensor<1, dim> edge_directions[2] = {cell->vertex(1) -
                                                            cell->vertex(0),
                                                            cell->vertex(2) -
@@ -2020,7 +2020,7 @@ namespace SAND {
                     if ((face->at_boundary()) ||
                         (!face->at_boundary() &&
                          (state.block(
-                                 SolutionBlocks::density)[cell->neighbor(face_number)->active_cell_index()] <
+                                 SolutionBlocks::density)[cell->neighbor(face_number)->get_fe().component_to_system_index(0, 0)] <
                           0.5))) {
                         const Tensor<1, dim> normal_vector =
                                 (face->center() - cell->center());
@@ -2126,7 +2126,7 @@ namespace SAND {
         for (const auto &cell : dof_handler.active_cell_iterators())
         {
             if (state.block(
-                    SolutionBlocks::unfiltered_density)[cell->active_cell_index()] > 0.5)
+                    SolutionBlocks::unfiltered_density)[cell->get_fe().component_to_system_index(0, 0)] > 0.5)
             {
                 for (const auto n : cell->face_indices())
                 {
@@ -2136,7 +2136,7 @@ namespace SAND {
                         create_boundary = true;
                     }
                     else if (state.block(
-                            SolutionBlocks::unfiltered_density)[cell->neighbor(n)->active_cell_index()] <= 0.5)
+                            SolutionBlocks::unfiltered_density)[cell->neighbor(n)->get_fe().component_to_system_index(0, 0)] <= 0.5)
                     {
                         create_boundary = true;
                     }
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 84418db..aa4429c 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -124,6 +124,13 @@ namespace SAND {
         else
         {
             TimerOutput::Scope t(timer, "build A inv");
+
+            if (Utilities::MPI::this_mpi_process(mpi_communicator) == 0)
+            {
+                std::string name = "a_mat";
+                print_matrix(name, a_mat);
+            }
+
             a_inv_direct.initialize(a_mat);
         }
         {

From ec2cb2075d2106bc76243df7c9ed589ca5a70a44 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Wed, 19 Jan 2022 09:47:54 -0700
Subject: [PATCH 60/95] no more matrix output

---
 source/kkt_system.cc.autosave           | 2241 -----------------------
 source/schur_preconditioner.cc.autosave |  148 +-
 2 files changed, 82 insertions(+), 2307 deletions(-)
 delete mode 100644 source/kkt_system.cc.autosave

diff --git a/source/kkt_system.cc.autosave b/source/kkt_system.cc.autosave
deleted file mode 100644
index 788ed43..0000000
--- a/source/kkt_system.cc.autosave
+++ /dev/null
@@ -1,2241 +0,0 @@
-//
-// Created by justin on 2/17/21.
-//
-#include "../include/kkt_system.h"
-#include <deal.II/base/quadrature_lib.h>
-#include <deal.II/base/function.h>
-#include <deal.II/base/tensor.h>
-#include <deal.II/base/timer.h>
-
-#include <deal.II/lac/full_matrix.h>
-#include <deal.II/lac/linear_operator.h>
-#include <deal.II/lac/solver_gmres.h>
-#include <deal.II/lac/generic_linear_algebra.h>
-#include <deal.II/lac/trilinos_sparse_matrix.h>
-
-
-#include <deal.II/lac/matrix_out.h>
-
-#include <deal.II/grid/grid_generator.h>
-
-#include <deal.II/dofs/dof_renumbering.h>
-#include <deal.II/dofs/dof_tools.h>
-
-#include <deal.II/fe/fe_values.h>
-#include <deal.II/fe/fe_dgq.h>
-#include <deal.II/fe/fe_system.h>
-#include <deal.II/fe/fe_q.h>
-#include <deal.II/fe/fe_nothing.h>
-
-#include <deal.II/hp/fe_collection.h>
-#include <deal.II/hp/fe_values.h>
-
-#include <deal.II/numerics/vector_tools.h>
-#include <deal.II/numerics/matrix_tools.h>
-#include <deal.II/numerics/data_out.h>
-#include <deal.II/numerics/error_estimator.h>
-
-
-#include <deal.II/base/conditional_ostream.h>
-
-#include "../include/input_information.h"
-
-#include <iostream>
-#include <algorithm>
-
-// This problem initializes with a FESystem composed of 2×dim FE_Q(1) elements, and 8 FE_DGQ(0)  elements.
-// The  piecewise  constant  functions  are  for  density-related  variables,and displacement-related variables are assigned to the FE_Q(1) elements.
-namespace SAND {
-    template<int dim>
-    KktSystem<dim>::KktSystem()
-            :
-            mpi_communicator(MPI_COMM_WORLD),
-            triangulation(mpi_communicator,
-                          typename Triangulation<dim>::MeshSmoothing(
-                                  Triangulation<dim>::smoothing_on_refinement |
-                                  Triangulation<dim>::smoothing_on_coarsening)),
-            dof_handler(triangulation),
-            /*fe should have 1 FE_DGQ<dim>(0) element for density, dim FE_Q finite elements for displacement,
-             * another dim FE_Q elements for the lagrange multiplier on the FE constraint, and 2 more FE_DGQ<dim>(0)
-             * elements for the upper and lower bound constraints */
-            fe_nine(FE_DGQ<dim>(0) ^ 5,
-                    (FESystem<dim>(FE_Q<dim>(1) ^ dim)) ^ 2,
-                    FE_DGQ<dim>(0) ^ 2,
-                    FE_Nothing<dim>() ^ 1),
-            fe_ten(FE_DGQ<dim>(0) ^ 5,
-                   (FESystem<dim>(FE_Q<dim>(1) ^ dim)) ^ 2,
-                   FE_DGQ<dim>(0) ^ 2,
-                   FE_DGQ<dim>(0) ^ 1),
-            density_ratio(Input::volume_percentage),
-            density_penalty_exponent(Input::density_penalty_exponent),
-            density_filter(),
-            pcout(std::cout,(Utilities::MPI::this_mpi_process(mpi_communicator) == 0))
-    {
-        fe_collection.push_back(fe_nine);
-        fe_collection.push_back(fe_ten);
-
-    }
-
-
-//A  function  used  once  at  the  beginning  of  the  program,  this  creates  a  matrix  H  so  that H* unfiltered density = filtered density
-
-    template<int dim>
-    void
-    KktSystem<dim>::setup_filter_matrix() {
-
-        density_filter.initialize(dof_handler);
-    }
-
-    //This triangulation matches the problem description -
-    // a 6-by-1 rectangle where a force will be applied in the top center.
-
-    template<int dim>
-    void
-    KktSystem<dim>::create_triangulation() {
-
-        std::vector<unsigned int> sub_blocks(2*dim+8, 0);
-
-        sub_blocks[0]=0;
-        sub_blocks[1]=1;
-        sub_blocks[2]=2;
-        sub_blocks[3]=3;
-        sub_blocks[4]=4;
-        for(int i=0; i<dim; i++)
-        {
-            sub_blocks[5+i]=5;
-        }
-        for(int i=0; i<dim; i++)
-        {
-            sub_blocks[5+dim+i]=6;
-        }
-        sub_blocks[5+2*dim]=7;
-        sub_blocks[6+2*dim]=8;
-        sub_blocks[7+2*dim]=9;
-
-
-        if (Input::geometry_base == GeometryOptions::mbb) {
-            const double width = 6;
-            const unsigned int width_refine = 6;
-            const double height = 1;
-            const unsigned int height_refine = 1;
-            const double depth = 1;
-            const unsigned int depth_refine = 1;
-            const double downforce_y = 1;
-            const double downforce_x = 3;
-            const double downforce_size = .3;
-
-            if (dim == 2)
-            {
-                GridGenerator::subdivided_hyper_rectangle(triangulation,
-                                                          {width_refine, height_refine},
-                                                          Point<dim>(0, 0),
-                                                          Point<dim>(width, height));
-
-                triangulation.refine_global(Input::refinements);
-
-                /*Set BCIDs   */
-                for (const auto &cell: dof_handler.active_cell_iterators()) {
-                    if(cell->is_locally_owned())
-                    {
-                        cell->set_active_fe_index(0);
-                        cell->set_material_id(MaterialIds::without_multiplier);
-                        for (unsigned int face_number = 0;
-                             face_number < GeometryInfo<dim>::faces_per_cell;
-                             ++face_number) {
-                            if (cell->face(face_number)->at_boundary()) {
-                                const auto center = cell->face(face_number)->center();
-
-                                if (std::fabs(center(1) - downforce_y) < 1e-12) {
-                                    if (std::fabs(center(0) - downforce_x) < downforce_size) {
-                                        cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
-                                    } else {
-                                        cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
-                                    }
-                                }
-                            }
-                        }
-                        for (unsigned int vertex_number = 0;
-                             vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                             ++vertex_number) {
-                            if (std::abs(cell->vertex(vertex_number)(0)) + std::abs(cell->vertex(vertex_number)(1)) <
-                                1e-10) {
-                                cell->set_active_fe_index(1);
-                                cell->set_material_id(MaterialIds::with_multiplier);
-                            }
-                        }
-                    }
-                }
-
-
-
-                dof_handler.distribute_dofs(fe_collection);
-
-                DoFRenumbering::component_wise(dof_handler, sub_blocks);
-            }
-            else if (dim == 3)
-            {
-                GridGenerator::subdivided_hyper_rectangle(triangulation,
-                                                          {width_refine, height_refine, depth_refine},
-                                                          Point<dim>(0, 0, 0),
-                                                          Point<dim>(width, height, depth));
-
-                triangulation.refine_global(Input::refinements);
-
-                for (const auto &cell: dof_handler.active_cell_iterators()) {
-                    if (cell->is_locally_owned())
-                    {
-                        cell->set_active_fe_index(0);
-                        cell->set_material_id(MaterialIds::without_multiplier);
-                        for (unsigned int face_number = 0;
-                             face_number < GeometryInfo<dim>::faces_per_cell;
-                             ++face_number) {
-                            if (cell->face(face_number)->at_boundary()) {
-                                const auto center = cell->face(face_number)->center();
-
-                                if (std::fabs(center(1) - downforce_y) < 1e-12) {
-                                    if (std::fabs(center(0) - downforce_x) < downforce_size) {
-                                        cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
-                                    } else {
-                                        cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
-                                    }
-                                }
-                            }
-                        }
-                        for (unsigned int vertex_number = 0;
-                             vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                             ++vertex_number) {
-                            if (std::abs(cell->vertex(vertex_number)(0)) + std::abs(cell->vertex(vertex_number)(1))
-                                + std::abs(cell->vertex(vertex_number)(2)) < 1e-10) {
-                                cell->set_active_fe_index(1);
-                                cell->set_material_id(MaterialIds::with_multiplier);
-                            }
-                        }
-                    }
-                }
-
-                dof_handler.distribute_dofs(fe_collection);
-
-                DoFRenumbering::component_wise(dof_handler, sub_blocks);
-
-            } else {
-                throw;
-            }
-        } else if (Input::geometry_base == GeometryOptions::l_shape) {
-            const double width = 2;
-            const unsigned int width_refine = 2;
-            const double height = 2;
-            const unsigned int height_refine = 2;
-            const double depth = 1;
-            const unsigned int depth_refine = 1;
-            const double downforce_x = 2;
-            const double downforce_y = 1;
-            const double downforce_z = .5;
-            const double downforce_size = .3;
-
-            if (dim == 2) {
-                GridGenerator::subdivided_hyper_L(triangulation,
-                                                  {width_refine, height_refine},
-                                                  Point<dim>(0, 0),
-                                                  Point<dim>(width, height),
-                                                  {-1, -1});
-
-                triangulation.refine_global(Input::refinements);
-
-                /*Set BCIDs   */
-                for (const auto &cell: dof_handler.active_cell_iterators()) {
-                    if (cell->is_locally_owned())
-                    {
-                        cell->set_active_fe_index(0);
-                        cell->set_material_id(MaterialIds::without_multiplier);
-                        for (unsigned int face_number = 0;
-                             face_number < GeometryInfo<dim>::faces_per_cell;
-                             ++face_number) {
-                            if (cell->face(face_number)->at_boundary()) {
-                                const auto center = cell->face(face_number)->center();
-
-                                if (std::fabs(center(0) - downforce_x) < 1e-12) {
-                                    if (std::fabs(center(1) - downforce_y) < downforce_size) {
-                                        cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
-                                    } else {
-                                        cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
-                                    }
-                                }
-                            }
-                        }
-                        for (unsigned int vertex_number = 0;
-                             vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                             ++vertex_number) {
-                            if (std::abs(cell->vertex(vertex_number)(0)) + std::abs(cell->vertex(vertex_number)(1)) <
-                                1e-10) {
-                                cell->set_active_fe_index(1);
-                                cell->set_material_id(MaterialIds::with_multiplier);
-                            }
-                        }
-                    }
-                }
-
-                dof_handler.distribute_dofs(fe_collection);
-
-                DoFRenumbering::component_wise(dof_handler, sub_blocks);
-
-            } else if (dim == 3) {
-                GridGenerator::subdivided_hyper_L(triangulation,
-                                                  {width_refine, height_refine, depth_refine},
-                                                  Point<dim>(0, 0, 0),
-                                                  Point<dim>(width, height, depth),
-                                                  {-1, -1, depth_refine});
-
-                triangulation.refine_global(Input::refinements);
-
-                /*Set BCIDs   */
-                for (const auto &cell: dof_handler.active_cell_iterators()) {
-                    if(cell->is_locally_owned())
-                    {
-                        cell->set_active_fe_index(0);
-                        cell->set_material_id(MaterialIds::without_multiplier);
-                        for (unsigned int face_number = 0;
-                             face_number < GeometryInfo<dim>::faces_per_cell;
-                             ++face_number) {
-                            if (cell->face(face_number)->at_boundary()) {
-                                const auto center = cell->face(face_number)->center();
-
-                                if (std::fabs(center(0) - downforce_x) < 1e-12) {
-                                    if (std::fabs(center(1) - downforce_y) < downforce_size) {
-                                        cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
-                                        if (std::fabs(center(2) - downforce_z) < downforce_size) {
-                                            cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
-                                        } else {
-                                            cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
-                                        }
-                                    } else {
-                                        cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
-                                    }
-                                }
-                            }
-                        }
-                        for (unsigned int vertex_number = 0;
-                             vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                             ++vertex_number) {
-                            if (std::abs(cell->vertex(vertex_number)(0)) + std::abs(cell->vertex(vertex_number)(1)) <
-                                1e-10) {
-                                cell->set_active_fe_index(1);
-                                cell->set_material_id(MaterialIds::with_multiplier);
-                            }
-                        }
-                    }
-                }
-
-                dof_handler.distribute_dofs(fe_collection);
-
-                DoFRenumbering::component_wise(dof_handler, sub_blocks);
-            } else {
-                throw;
-            }
-        } else {
-            throw;
-        }
-
-    }
-
-// The  bottom  corners  are  kept  in  place  in  the  y  direction  -  the  bottom  left  also  in  the  x direction.
-// Because deal.ii is formulated to enforce boundary conditions along regions of the boundary,
-// we do this to ensure these BCs are only enforced at points.
-    template<int dim>
-    void
-    KktSystem<dim>::setup_boundary_values() {
-        if (Input::geometry_base == GeometryOptions::mbb) {
-            if (dim == 2) {
-                for (const auto &cell: dof_handler.active_cell_iterators()) {
-                    if(cell->is_locally_owned())
-                    {
-                        for (unsigned int face_number = 0;
-                             face_number < GeometryInfo<dim>::faces_per_cell;
-                             ++face_number) {
-                            if (cell->face(face_number)->at_boundary()) {
-                                for (unsigned int vertex_number = 0;
-                                     vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                                     ++vertex_number) {
-                                    const auto vert = cell->vertex(vertex_number);
-                                    /*Find bottom left corner*/
-                                    if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
-                                            vert(1) - 0) < 1e-12) {
-
-                                        const unsigned int x_displacement =
-                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
-                                        const unsigned int y_displacement =
-                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
-                                        const unsigned int x_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
-                                        const unsigned int y_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
-                                        /*set bottom left BC*/
-                                        boundary_values[x_displacement] = 0;
-                                        boundary_values[y_displacement] = 0;
-                                        boundary_values[x_displacement_multiplier] = 0;
-                                        boundary_values[y_displacement_multiplier] = 0;
-                                    }
-                                    /*Find bottom right corner*/
-                                    if (std::fabs(vert(0) - 6) < 1e-12 && std::fabs(
-                                            vert(1) - 0) < 1e-12) {
-//                            const unsigned int x_displacement =
-//                                    cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
-                                        const unsigned int y_displacement =
-                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
-//                            const unsigned int x_displacement_multiplier =
-//                                    cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
-                                        const unsigned int y_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
-//                            boundary_values[x_displacement] = 0;
-                                        boundary_values[y_displacement] = 0;
-//                            boundary_values[x_displacement_multiplier] = 0;
-                                        boundary_values[y_displacement_multiplier] = 0;
-                                    }
-                                }
-                            }
-                        }
-                    }
-
-                }
-            } else if (dim == 3) {
-                for (const auto &cell: dof_handler.active_cell_iterators()) {
-                    if(cell->is_locally_owned())
-                    {
-                        for (unsigned int face_number = 0;
-                             face_number < GeometryInfo<dim>::faces_per_cell;
-                             ++face_number) {
-                            if (cell->face(face_number)->at_boundary()) {
-                                for (unsigned int vertex_number = 0;
-                                     vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                                     ++vertex_number) {
-                                    const auto vert = cell->vertex(vertex_number);
-                                    /*Find bottom left corner*/
-                                    if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
-                                            vert(1) - 0) < 1e-12 && ((std::fabs(
-                                            vert(2) - 0) < 1e-12) || (std::fabs(
-                                            vert(2) - 1) < 1e-12))) {
-
-
-                                        const unsigned int x_displacement =
-                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
-                                        const unsigned int y_displacement =
-                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
-                                        const unsigned int z_displacement =
-                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
-                                        const unsigned int x_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
-                                        const unsigned int y_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 4, cell->active_fe_index());
-                                        const unsigned int z_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 5, cell->active_fe_index());
-
-                                        boundary_values[x_displacement] = 0;
-                                        boundary_values[y_displacement] = 0;
-                                        boundary_values[z_displacement] = 0;
-                                        boundary_values[x_displacement_multiplier] = 0;
-                                        boundary_values[y_displacement_multiplier] = 0;
-                                        boundary_values[z_displacement_multiplier] = 0;
-                                    }
-                                    /*Find bottom right corner*/
-                                    if (std::fabs(vert(0) - 6) < 1e-12 && std::fabs(
-                                            vert(1) - 0) < 1e-12 && ((std::fabs(
-                                            vert(2) - 0) < 1e-12) || (std::fabs(
-                                            vert(2) - 1) < 1e-12))) {
-//                              const unsigned int x_displacement =
-//                                    cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
-                                        const unsigned int y_displacement =
-                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
-                                        const unsigned int z_displacement =
-                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
-//                              const unsigned int x_displacement_multiplier =
-//                                    cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
-                                        const unsigned int y_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 4, cell->active_fe_index());
-                                        const unsigned int z_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 5, cell->active_fe_index());
-//                              boundary_values[x_displacement] = 0;
-                                        boundary_values[y_displacement] = 0;
-                                        boundary_values[z_displacement] = 0;
-//                              boundary_values[x_displacement_multiplier] = 0;
-                                        boundary_values[y_displacement_multiplier] = 0;
-                                        boundary_values[z_displacement_multiplier] = 0;
-                                    }
-                                }
-                            }
-                        }
-                    }
-                }
-            } else {
-                throw;
-            }
-        } else if (Input::geometry_base == GeometryOptions::l_shape) {
-            if (dim == 2) {
-                for (const auto &cell: dof_handler.active_cell_iterators()) {
-                    if(cell->is_locally_owned())
-                    {
-                        for (unsigned int face_number = 0;
-                             face_number < GeometryInfo<dim>::faces_per_cell;
-                             ++face_number) {
-                            if (cell->face(face_number)->at_boundary()) {
-                                for (unsigned int vertex_number = 0;
-                                     vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                                     ++vertex_number) {
-                                    const auto vert = cell->vertex(vertex_number);
-                                    /*Find top left corner*/
-                                    if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
-                                            vert(1) - 2) < 1e-12) {
-
-                                        const unsigned int x_displacement =
-                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
-                                        const unsigned int y_displacement =
-                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
-                                        const unsigned int x_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
-                                        const unsigned int y_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
-                                        /*set bottom left BC*/
-                                        boundary_values[x_displacement] = 0;
-                                        boundary_values[y_displacement] = 0;
-                                        boundary_values[x_displacement_multiplier] = 0;
-                                        boundary_values[y_displacement_multiplier] = 0;
-                                    }
-                                    /*Find top right corner*/
-                                    if (std::fabs(vert(0) - 1) < 1e-12 && std::fabs(
-                                            vert(1) - 2) < 1e-12) {
-                                        const unsigned int x_displacement =
-                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
-                                        const unsigned int y_displacement =
-                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
-                                        const unsigned int x_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
-                                        const unsigned int y_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
-                                        boundary_values[x_displacement] = 0;
-                                        boundary_values[y_displacement] = 0;
-                                        boundary_values[x_displacement_multiplier] = 0;
-                                        boundary_values[y_displacement_multiplier] = 0;
-                                    }
-                                }
-                            }
-                        }
-                    }
-                }
-            } else if (dim == 3) {
-                for (const auto &cell: dof_handler.active_cell_iterators()) {
-                    if(cell->is_locally_owned())
-                    {
-                        for (unsigned int face_number = 0;
-                             face_number < GeometryInfo<dim>::faces_per_cell;
-                             ++face_number) {
-                            if (cell->face(face_number)->at_boundary()) {
-                                for (unsigned int vertex_number = 0;
-                                     vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                                     ++vertex_number) {
-                                    const auto vert = cell->vertex(vertex_number);
-                                    /*Find bottom left corner*/
-                                    if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
-                                            vert(1) - 2) < 1e-12 && ((std::fabs(
-                                            vert(2) - 0) < 1e-12) || (std::fabs(
-                                            vert(2) - 1) < 1e-12))) {
-
-
-                                        const unsigned int x_displacement =
-                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
-                                        const unsigned int y_displacement =
-                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
-                                        const unsigned int z_displacement =
-                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
-                                        const unsigned int x_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
-                                        const unsigned int y_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 4, cell->active_fe_index());
-                                        const unsigned int z_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 5, cell->active_fe_index());
-
-                                        boundary_values[x_displacement] = 0;
-                                        boundary_values[y_displacement] = 0;
-                                        boundary_values[z_displacement] = 0;
-                                        boundary_values[x_displacement_multiplier] = 0;
-                                        boundary_values[y_displacement_multiplier] = 0;
-                                        boundary_values[z_displacement_multiplier] = 0;
-                                    }
-                                    /*Find bottom right corner*/
-                                    if (std::fabs(vert(0) - 1) < 1e-12 && std::fabs(
-                                            vert(1) - 2) < 1e-12 && ((std::fabs(
-                                            vert(2) - 0) < 1e-12) || (std::fabs(
-                                            vert(2) - 1) < 1e-12))) {
-                                        const unsigned int x_displacement =
-                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
-                                        const unsigned int y_displacement =
-                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
-                                        const unsigned int z_displacement =
-                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
-                                        const unsigned int x_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
-                                        const unsigned int y_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 4, cell->active_fe_index());
-                                        const unsigned int z_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 5, cell->active_fe_index());
-                                        boundary_values[x_displacement] = 0;
-                                        boundary_values[y_displacement] = 0;
-                                        boundary_values[z_displacement] = 0;
-                                        boundary_values[x_displacement_multiplier] = 0;
-                                        boundary_values[y_displacement_multiplier] = 0;
-                                        boundary_values[z_displacement_multiplier] = 0;
-                                    }
-                                }
-                            }
-                        }
-                    }
-                }
-            } else {
-                throw;
-            }
-        }
-
-
-    }
-
-
-    //This makes a giant 10-by-10 block matrix, and also sets up the necessary block vectors.  The
-    // sparsity pattern for this matrix includes the sparsity pattern for the filter matrix. It also initializes
-    // any block vectors we will use.
-    template<int dim>
-    void
-    KktSystem<dim>::setup_block_system() {
-
-        //MAKE n_u and n_P
-
-        /*Setup 10 by 10 block matrix*/
-        std::vector<unsigned int> block_component(10, 2);
-
-        block_component[0] = 0;
-        block_component[5] = 1;
-
-        const std::vector<types::global_dof_index> dofs_per_block =
-                DoFTools::count_dofs_per_fe_block(dof_handler, block_component);
-        const unsigned int n_p = dofs_per_block[0];
-        const unsigned int n_u = dofs_per_block[1];
-
-        pcout << "n_p:  " << n_p << "   n_u:  " << n_u << std::endl;
-
-        IndexSet locally_owned_dofs = dof_handler.locally_owned_dofs();
-        IndexSet locally_relevant_dofs;
-        DoFTools::extract_locally_relevant_dofs(dof_handler, locally_relevant_dofs);
-
-        dsp.reinit(10, 10);
-        owned_partitioning.resize(10);
-        owned_partitioning[0] = dof_handler.locally_owned_dofs().get_view(0, n_p);
-        owned_partitioning[1] = dof_handler.locally_owned_dofs().get_view(n_p, 2 * n_p);
-        owned_partitioning[2] = dof_handler.locally_owned_dofs().get_view(2 * n_p, 3 * n_p);
-        owned_partitioning[3] = dof_handler.locally_owned_dofs().get_view(3 * n_p, 4 * n_p);
-        owned_partitioning[4] = dof_handler.locally_owned_dofs().get_view(4 * n_p, 5 * n_p);
-        owned_partitioning[5] = dof_handler.locally_owned_dofs().get_view(5 * n_p, 5 * n_p + n_u);
-        owned_partitioning[6] = dof_handler.locally_owned_dofs().get_view(5 * n_p + n_u, 5 * n_p + 2 * n_u);
-        owned_partitioning[7] = dof_handler.locally_owned_dofs().get_view(5 * n_p + 2 * n_u, 6 * n_p + 2 * n_u);
-        owned_partitioning[8] = dof_handler.locally_owned_dofs().get_view(6 * n_p + 2 * n_u, 7 * n_p + 2 * n_u);
-        owned_partitioning[9] = dof_handler.locally_owned_dofs().get_view(7 * n_p + 2 * n_u, 7 * n_p + 2 * n_u + 1);
-        relevant_partitioning.resize(10);
-        relevant_partitioning[0] = locally_relevant_dofs.get_view(0, n_p);
-        relevant_partitioning[1] = locally_relevant_dofs.get_view(n_p, 2 * n_p);
-        relevant_partitioning[2] = locally_relevant_dofs.get_view(2 * n_p, 3 * n_p);
-        relevant_partitioning[3] = locally_relevant_dofs.get_view(3 * n_p, 4 * n_p);
-        relevant_partitioning[4] = locally_relevant_dofs.get_view(4 * n_p, 5 * n_p);
-        relevant_partitioning[5] = locally_relevant_dofs.get_view(5 * n_p, 5 * n_p + n_u);
-        relevant_partitioning[6] = locally_relevant_dofs.get_view(5 * n_p + n_u, 5 * n_p + 2 * n_u);
-        relevant_partitioning[7] = locally_relevant_dofs.get_view(5 * n_p + 2 * n_u, 6 * n_p + 2 * n_u);
-        relevant_partitioning[8] = locally_relevant_dofs.get_view(6 * n_p + 2 * n_u, 7 * n_p + 2 * n_u);
-        relevant_partitioning[9] = locally_relevant_dofs.get_view(7 * n_p + 2 * n_u, 7 * n_p + 2 * n_u + 1);
-
-        const std::vector<unsigned int> block_sizes = {n_p, n_p, n_p, n_p, n_p, n_u, n_u, n_p, n_p, 1};
-
-        for (unsigned int k = 0; k < 10; k++) {
-            for (unsigned int j = 0; j < 10; j++) {
-                dsp.block(j, k).reinit(block_sizes[j], block_sizes[k]);
-            }
-        }
-        dsp.collect_sizes();
-        Table<2, DoFTools::Coupling> coupling(2 * dim + 8, 2 * dim + 8);
-//Coupling for density
-            coupling[SolutionComponents::density<dim>][SolutionComponents::density<dim>] = DoFTools::always;
-
-            for (unsigned int i = 0; i < dim; i++) {
-                coupling[SolutionComponents::density<dim>][SolutionComponents::displacement<dim> +
-                                                           i] = DoFTools::always;
-                coupling[SolutionComponents::displacement<dim> +
-                         i][SolutionComponents::density<dim>] = DoFTools::always;
-            }
-
-            coupling[SolutionComponents::density<dim>][SolutionComponents::unfiltered_density_multiplier<dim>] = DoFTools::always;
-            coupling[SolutionComponents::unfiltered_density_multiplier<dim>][SolutionComponents::density<dim>] = DoFTools::always;
-
-            for (unsigned int i = 0; i < dim; i++) {
-                coupling[SolutionComponents::density<dim>][SolutionComponents::displacement_multiplier<dim> +
-                                                           i] = DoFTools::always;
-                coupling[SolutionComponents::displacement_multiplier<dim> +
-                         i][SolutionComponents::density<dim>] = DoFTools::always;
-            }
-
-//Coupling for displacement
-            for (unsigned int i = 0; i < dim; i++) {
-
-                for (unsigned int k = 0; k < dim; k++) {
-                    coupling[SolutionComponents::displacement<dim> + i][
-                            SolutionComponents::displacement_multiplier<dim> +
-                            k] = DoFTools::always;
-                    coupling[SolutionComponents::displacement_multiplier<dim> + k][
-                            SolutionComponents::displacement<dim> +
-                            i] = DoFTools::always;
-                }
-            }
-
-// coupling for unfiltered density
-            coupling[SolutionComponents::unfiltered_density<dim>][SolutionComponents::density_lower_slack_multiplier<dim>] = DoFTools::always;
-            coupling[SolutionComponents::density_lower_slack_multiplier<dim>][SolutionComponents::unfiltered_density<dim>] = DoFTools::always;
-
-            coupling[SolutionComponents::unfiltered_density<dim>][SolutionComponents::density_upper_slack_multiplier<dim>] = DoFTools::always;
-            coupling[SolutionComponents::density_upper_slack_multiplier<dim>][SolutionComponents::unfiltered_density<dim>] = DoFTools::always;
-
-            coupling[SolutionComponents::unfiltered_density<dim>][SolutionComponents::unfiltered_density_multiplier<dim>] = DoFTools::always;
-            coupling[SolutionComponents::unfiltered_density_multiplier<dim>][SolutionComponents::unfiltered_density<dim>] = DoFTools::always;
-
-
-
-
-//        Coupling for lower slack
-            coupling[SolutionComponents::density_lower_slack<dim>][SolutionComponents::density_lower_slack<dim>] = DoFTools::always;
-
-            coupling[SolutionComponents::density_lower_slack<dim>][SolutionComponents::density_lower_slack_multiplier<dim>] = DoFTools::always;
-            coupling[SolutionComponents::density_lower_slack_multiplier<dim>][SolutionComponents::density_lower_slack<dim>] = DoFTools::always;
-
-//
-            coupling[SolutionComponents::density_upper_slack<dim>][SolutionComponents::density_upper_slack<dim>] = DoFTools::always;
-            coupling[SolutionComponents::density_upper_slack<dim>][SolutionComponents::density_upper_slack_multiplier<dim>] = DoFTools::always;
-            coupling[SolutionComponents::density_upper_slack_multiplier<dim>][SolutionComponents::density_upper_slack<dim>] = DoFTools::always;
-
-            coupling[SolutionComponents::density_upper_slack_multiplier<dim>][SolutionComponents::density_upper_slack_multiplier<dim>] = DoFTools::always;
-            constraints.reinit(locally_relevant_dofs);
-            constraints.clear();
-            DoFTools::make_hanging_node_constraints(dof_handler,constraints);
-            constraints.close();
-
-            system_matrix.clear();
-
-//            DoFTools::make_sparsity_pattern(dof_handler, coupling, dsp, constraints, false);
-            DoFTools::make_sparsity_pattern(dof_handler, dsp, constraints, false);
-            SparsityTools::distribute_sparsity_pattern(dsp, dof_handler.locally_owned_dofs(), mpi_communicator,
-                                                       locally_relevant_dofs);
-            //adds the row into the sparsity pattern for the total volume constraint
-            for (const auto &cell: dof_handler.active_cell_iterators())
-            {
-                if (cell->is_locally_owned())
-                {
-                    std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
-                    cell->get_dof_indices(i);
-                    dsp.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier).add(i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)], 0);
-                    dsp.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).add(0, i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)]);
-                }
-
-            }
-
-            /*This finds neighbors whose values would be relevant, and adds them to the sparsity pattern of the matrix*/
-            setup_filter_matrix();
-            for (const auto &cell : dof_handler.active_cell_iterators()) {
-                if (cell->is_locally_owned())
-                {
-                    std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
-                    cell->get_dof_indices(i);
-                    for (const auto &neighbor_cell : density_filter.find_relevant_neighbors(cell)) {
-                        std::vector<types::global_dof_index> j(neighbor_cell->get_fe().n_dofs_per_cell());
-                        neighbor_cell->get_dof_indices(j);
-
-                        dsp.block(SolutionBlocks::unfiltered_density_multiplier,
-                                  SolutionBlocks::unfiltered_density).add(i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)], j[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)]);
-                        dsp.block(SolutionBlocks::unfiltered_density,
-                                  SolutionBlocks::unfiltered_density_multiplier).add(j[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)], i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)]);
-                    }
-                }
-            }
-
-
-            SparsityTools::distribute_sparsity_pattern(
-                    dsp,
-                    Utilities::MPI::all_gather(mpi_communicator,
-                                               dof_handler.locally_owned_dofs()),
-                    mpi_communicator,
-                    locally_relevant_dofs);
-            DoFTools::extract_locally_relevant_dofs(dof_handler, locally_relevant_dofs);
-            system_matrix.reinit(owned_partitioning, dsp, mpi_communicator);
-
-            locally_relevant_solution.reinit(owned_partitioning, relevant_partitioning, mpi_communicator);
-            distributed_solution.reinit(owned_partitioning, mpi_communicator);
-            system_rhs.reinit(owned_partitioning, mpi_communicator);
-
-            locally_relevant_solution.collect_sizes();
-            distributed_solution.collect_sizes();
-            system_rhs.collect_sizes();
-            system_matrix.collect_sizes();
-    }
-
-    ///This  is  where  the  magic  happens.   The  equations  describing  the newtons method for finding 0s in the KKT conditions are implemented here.
-
-
-    template<int dim>
-    void
-    KktSystem<dim>::assemble_block_system(const LA::MPI::BlockVector &distributed_state, const double barrier_size) {
-        /*Remove any values from old iterations*/
-
-        LA::MPI::BlockVector relevant_state(owned_partitioning, relevant_partitioning, mpi_communicator);
-        relevant_state = distributed_state;
-
-        system_matrix.reinit(owned_partitioning, dsp, mpi_communicator);
-        locally_relevant_solution = 0;
-        system_rhs = 0;
-
-        QGauss<dim> nine_quadrature(fe_nine.degree + 1);
-        QGauss<dim> ten_quadrature(fe_ten.degree + 1);
-
-        hp::QCollection<dim> q_collection;
-        q_collection.push_back(nine_quadrature);
-        q_collection.push_back(ten_quadrature);
-
-        hp::FEValues<dim> hp_fe_values(fe_collection,
-                                       q_collection,
-                                       update_values | update_quadrature_points |
-                                       update_JxW_values | update_gradients);
-
-        QGauss<dim - 1> common_face_quadrature(fe_ten.degree + 1);
-
-        FEFaceValues<dim> fe_nine_face_values(fe_nine,
-                                              common_face_quadrature,
-                                              update_JxW_values |
-                                              update_gradients | update_values);
-        FEFaceValues<dim> fe_ten_face_values(fe_ten,
-                                             common_face_quadrature,
-                                             update_normal_vectors |
-                                             update_values);
-
-        FullMatrix<double> cell_matrix;
-        Vector<double> cell_rhs;
-        std::vector<types::global_dof_index> local_dof_indices;
-
-        const FEValuesExtractors::Scalar densities(SolutionComponents::density<dim>);
-        const FEValuesExtractors::Vector displacements(SolutionComponents::displacement<dim>);
-        const FEValuesExtractors::Scalar unfiltered_densities(SolutionComponents::unfiltered_density<dim>);
-        const FEValuesExtractors::Vector displacement_multipliers(SolutionComponents::displacement_multiplier<dim>);
-        const FEValuesExtractors::Scalar unfiltered_density_multipliers(
-                SolutionComponents::unfiltered_density_multiplier<dim>);
-        const FEValuesExtractors::Scalar density_lower_slacks(SolutionComponents::density_lower_slack<dim>);
-        const FEValuesExtractors::Scalar density_lower_slack_multipliers(
-                SolutionComponents::density_lower_slack_multiplier<dim>);
-        const FEValuesExtractors::Scalar density_upper_slacks(SolutionComponents::density_upper_slack<dim>);
-        const FEValuesExtractors::Scalar density_upper_slack_multipliers(
-                SolutionComponents::density_upper_slack_multiplier<dim>);
-        const FEValuesExtractors::Scalar total_volume_multiplier(
-                SolutionComponents::total_volume_multiplier<dim>);
-
-        const Functions::ConstantFunction<dim> lambda(1.), mu(1.);
-
-        distributed_solution = distributed_state;
-        LA::MPI::BlockVector filtered_unfiltered_density_solution = distributed_solution;
-        LA::MPI::BlockVector filter_adjoint_unfiltered_density_multiplier_solution = distributed_solution;
-        filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = 0;
-        filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier) = 0;
-        auto op_f = linear_operator<LA::MPI::Vector>(density_filter.filter_matrix);
-        filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = op_f * distributed_solution.block(SolutionBlocks::unfiltered_density);
-        filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier) = transpose_operator(op_f) *  distributed_solution.block(SolutionBlocks::unfiltered_density_multiplier);
-
-        LA::MPI::BlockVector relevant_filtered_unfiltered_density_solution = locally_relevant_solution;
-        LA::MPI::BlockVector relevant_filter_adjoint_unfiltered_density_multiplier_solution = locally_relevant_solution;
-        relevant_filtered_unfiltered_density_solution =filtered_unfiltered_density_solution;
-        relevant_filter_adjoint_unfiltered_density_multiplier_solution = filter_adjoint_unfiltered_density_multiplier_solution;
-        for (const auto &cell: dof_handler.active_cell_iterators()) {
-            if(cell->is_locally_owned())
-            {
-                hp_fe_values.reinit(cell);
-                const FEValues<dim> &fe_values = hp_fe_values.get_present_fe_values();
-                cell_matrix.reinit(cell->get_fe().n_dofs_per_cell(),
-                                   cell->get_fe().n_dofs_per_cell());
-                cell_rhs.reinit(cell->get_fe().n_dofs_per_cell());
-
-                const unsigned int n_q_points = fe_values.n_quadrature_points;
-
-                std::vector<double> old_density_values(n_q_points);
-                std::vector<Tensor<1, dim>> old_displacement_values(n_q_points);
-                std::vector<double> old_displacement_divs(n_q_points);
-                std::vector<SymmetricTensor<2, dim>> old_displacement_symmgrads(
-                        n_q_points);
-                std::vector<Tensor<1, dim>> old_displacement_multiplier_values(
-                        n_q_points);
-                std::vector<double> old_displacement_multiplier_divs(n_q_points);
-                std::vector<SymmetricTensor<2, dim>> old_displacement_multiplier_symmgrads(
-                        n_q_points);
-                std::vector<double> old_lower_slack_multiplier_values(n_q_points);
-                std::vector<double> old_upper_slack_multiplier_values(n_q_points);
-                std::vector<double> old_lower_slack_values(n_q_points);
-                std::vector<double> old_upper_slack_values(n_q_points);
-                std::vector<double> old_unfiltered_density_values(n_q_points);
-                std::vector<double> old_unfiltered_density_multiplier_values(n_q_points);
-                std::vector<double> filtered_unfiltered_density_values(n_q_points);
-                std::vector<double> filter_adjoint_unfiltered_density_multiplier_values(n_q_points);
-                std::vector<double> lambda_values(n_q_points);
-                std::vector<double> mu_values(n_q_points);
-
-                const unsigned int dofs_per_cell = cell->get_fe().n_dofs_per_cell();
-
-                cell_matrix = 0;
-                cell_rhs = 0;
-                local_dof_indices.resize(cell->get_fe().n_dofs_per_cell());
-                cell->get_dof_indices(local_dof_indices);
-
-                lambda.value_list(fe_values.get_quadrature_points(), lambda_values);
-                mu.value_list(fe_values.get_quadrature_points(), mu_values);
-
-                fe_values[densities].get_function_values(relevant_state,
-                                                         old_density_values);
-                fe_values[displacements].get_function_values(relevant_state,
-                                                             old_displacement_values);
-                fe_values[displacements].get_function_divergences(relevant_state,
-                                                                  old_displacement_divs);
-                fe_values[displacements].get_function_symmetric_gradients(
-                        relevant_state, old_displacement_symmgrads);
-                fe_values[displacement_multipliers].get_function_values(
-                        relevant_state, old_displacement_multiplier_values);
-                fe_values[displacement_multipliers].get_function_divergences(
-                        relevant_state, old_displacement_multiplier_divs);
-                fe_values[displacement_multipliers].get_function_symmetric_gradients(
-                        relevant_state, old_displacement_multiplier_symmgrads);
-                fe_values[density_lower_slacks].get_function_values(
-                        relevant_state, old_lower_slack_values);
-                fe_values[density_lower_slack_multipliers].get_function_values(
-                        relevant_state, old_lower_slack_multiplier_values);
-                fe_values[density_upper_slacks].get_function_values(
-                        relevant_state, old_upper_slack_values);
-                fe_values[density_upper_slack_multipliers].get_function_values(
-                        relevant_state, old_upper_slack_multiplier_values);
-                fe_values[unfiltered_densities].get_function_values(
-                        relevant_state, old_unfiltered_density_values);
-                fe_values[unfiltered_density_multipliers].get_function_values(
-                        relevant_state, old_unfiltered_density_multiplier_values);
-                fe_values[unfiltered_densities].get_function_values(
-                        relevant_filtered_unfiltered_density_solution, filtered_unfiltered_density_values);
-                fe_values[unfiltered_density_multipliers].get_function_values(
-                        relevant_filter_adjoint_unfiltered_density_multiplier_solution,
-                        filter_adjoint_unfiltered_density_multiplier_values);
-
-                Tensor<1, dim> traction;
-                traction[1] = -1;
-
-                for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
-
-                    for (unsigned int i = 0; i < dofs_per_cell; ++i) {
-                        const SymmetricTensor<2, dim> displacement_phi_i_symmgrad =
-                                fe_values[displacements].symmetric_gradient(i, q_point);
-                        const double displacement_phi_i_div =
-                                fe_values[displacements].divergence(i, q_point);
-
-                        const SymmetricTensor<2, dim> displacement_multiplier_phi_i_symmgrad =
-                                fe_values[displacement_multipliers].symmetric_gradient(i,
-                                                                                       q_point);
-                        const double displacement_multiplier_phi_i_div =
-                                fe_values[displacement_multipliers].divergence(i,
-                                                                               q_point);
-
-
-                        const double density_phi_i = fe_values[densities].value(i,
-                                                                                q_point);
-                        const double unfiltered_density_phi_i = fe_values[unfiltered_densities].value(i,
-                                                                                                      q_point);
-                        const double unfiltered_density_multiplier_phi_i = fe_values[unfiltered_density_multipliers].value(
-                                i, q_point);
-
-                        const double lower_slack_multiplier_phi_i =
-                                fe_values[density_lower_slack_multipliers].value(i,
-                                                                                 q_point);
-
-                        const double lower_slack_phi_i =
-                                fe_values[density_lower_slacks].value(i, q_point);
-
-                        const double upper_slack_phi_i =
-                                fe_values[density_upper_slacks].value(i, q_point);
-
-                        const double upper_slack_multiplier_phi_i =
-                                fe_values[density_upper_slack_multipliers].value(i,
-                                                                                 q_point);
-
-
-                        for (unsigned int j = 0; j < dofs_per_cell; ++j) {
-                            const SymmetricTensor<2, dim> displacement_phi_j_symmgrad =
-                                    fe_values[displacements].symmetric_gradient(j,
-                                                                                q_point);
-                            const double displacement_phi_j_div =
-                                    fe_values[displacements].divergence(j, q_point);
-
-                            const SymmetricTensor<2, dim> displacement_multiplier_phi_j_symmgrad =
-                                    fe_values[displacement_multipliers].symmetric_gradient(
-                                            j, q_point);
-                            const double displacement_multiplier_phi_j_div =
-                                    fe_values[displacement_multipliers].divergence(j,
-                                                                                   q_point);
-
-                            const double density_phi_j = fe_values[densities].value(
-                                    j, q_point);
-
-                            const double unfiltered_density_phi_j = fe_values[unfiltered_densities].value(j,
-                                                                                                          q_point);
-                            const double unfiltered_density_multiplier_phi_j = fe_values[unfiltered_density_multipliers].value(
-                                    j, q_point);
-
-
-                            const double lower_slack_phi_j =
-                                    fe_values[density_lower_slacks].value(j, q_point);
-
-                            const double upper_slack_phi_j =
-                                    fe_values[density_upper_slacks].value(j, q_point);
-
-                            const double lower_slack_multiplier_phi_j =
-                                    fe_values[density_lower_slack_multipliers].value(j,
-                                                                                     q_point);
-
-                            const double upper_slack_multiplier_phi_j =
-                                    fe_values[density_upper_slack_multipliers].value(j,
-                                                                                     q_point);
-
-                            //Equation 0
-                            cell_matrix(i, j) +=
-                                    fe_values.JxW(q_point) *
-                                    (
-                                            -density_phi_i * unfiltered_density_multiplier_phi_j
-
-                                            - density_penalty_exponent * (density_penalty_exponent - 1)
-                                              * std::pow(
-                                                    old_density_values[q_point],
-                                                    density_penalty_exponent - 2)
-                                              * density_phi_i
-                                              * density_phi_j
-                                              * (old_displacement_multiplier_divs[q_point] * old_displacement_divs[q_point]
-                                                 * lambda_values[q_point]
-                                                 + 2 * mu_values[q_point]
-                                                   * (old_displacement_symmgrads[q_point] *
-                                                      old_displacement_multiplier_symmgrads[q_point]))
-
-                                            - density_penalty_exponent * std::pow(
-                                                    old_density_values[q_point],
-                                                    density_penalty_exponent - 1)
-                                              * density_phi_i
-                                              * (displacement_multiplier_phi_j_div * old_displacement_divs[q_point]
-                                                 * lambda_values[q_point]
-                                                 + 2 * mu_values[q_point]
-                                                   *
-                                                   (old_displacement_symmgrads[q_point] *
-                                                    displacement_multiplier_phi_j_symmgrad))
-
-                                            - density_penalty_exponent * std::pow(
-                                                    old_density_values[q_point],
-                                                    density_penalty_exponent - 1)
-                                              * density_phi_i
-                                              * (displacement_phi_j_div * old_displacement_multiplier_divs[q_point]
-                                                 * lambda_values[q_point]
-                                                 + 2 * mu_values[q_point]
-                                                   * (old_displacement_multiplier_symmgrads[q_point] *
-                                                      displacement_phi_j_symmgrad)));
-                            //Equation 1
-
-                            cell_matrix(i, j) +=
-                                    fe_values.JxW(q_point) * (
-                                            -density_penalty_exponent * std::pow(
-                                                    old_density_values[q_point],
-                                                    density_penalty_exponent - 1)
-                                            * density_phi_j
-                                            * (old_displacement_multiplier_divs[q_point] * displacement_phi_i_div
-                                               * lambda_values[q_point]
-                                               + 2 * mu_values[q_point]
-                                                 * (old_displacement_multiplier_symmgrads[q_point] *
-                                                    displacement_phi_i_symmgrad))
-
-                                            - std::pow(old_density_values[q_point],
-                                                       density_penalty_exponent)
-                                              * (displacement_multiplier_phi_j_div * displacement_phi_i_div
-                                                 * lambda_values[q_point]
-                                                 + 2 * mu_values[q_point]
-                                                   * (displacement_multiplier_phi_j_symmgrad * displacement_phi_i_symmgrad))
-
-                                    );
-
-                            //Equation 2 has to do with the filter, which is calculated elsewhere.
-                            cell_matrix(i, j) +=
-                                    fe_values.JxW(q_point) * (
-                                            -1 * unfiltered_density_phi_i * lower_slack_multiplier_phi_j
-                                            + unfiltered_density_phi_i * upper_slack_multiplier_phi_j);
-
-                            //Equation 3 - Primal Feasibility
-
-                            cell_matrix(i, j) +=
-                                    fe_values.JxW(q_point) * (
-
-                                            -1 * density_penalty_exponent * std::pow(
-                                                    old_density_values[q_point],
-                                                    density_penalty_exponent - 1)
-                                            * density_phi_j
-                                            * (old_displacement_divs[q_point] * displacement_multiplier_phi_i_div
-                                               * lambda_values[q_point]
-                                               + 2 * mu_values[q_point]
-                                                 * (old_displacement_symmgrads[q_point] *
-                                                    displacement_multiplier_phi_i_symmgrad))
-
-                                            + -1 * std::pow(old_density_values[q_point],
-                                                            density_penalty_exponent)
-                                              * (displacement_phi_j_div * displacement_multiplier_phi_i_div
-                                                 * lambda_values[q_point]
-                                                 + 2 * mu_values[q_point]
-                                                   *
-                                                   (displacement_phi_j_symmgrad * displacement_multiplier_phi_i_symmgrad)));
-
-                            //Equation 4 - more primal feasibility
-                            cell_matrix(i, j) +=
-                                    -1 * fe_values.JxW(q_point) * lower_slack_multiplier_phi_i *
-                                    (unfiltered_density_phi_j - lower_slack_phi_j);
-
-                            //Equation 5 - more primal feasibility
-                            cell_matrix(i, j) +=
-                                    -1 * fe_values.JxW(q_point) * upper_slack_multiplier_phi_i * (
-                                            -1 * unfiltered_density_phi_j - upper_slack_phi_j);
-
-                            //Equation 6 - more primal feasibility - part with filter added later
-                            cell_matrix(i, j) +=
-                                    -1 * fe_values.JxW(q_point) * unfiltered_density_multiplier_phi_i * (
-                                            density_phi_j);
-
-                            //Equation 7 - complementary slackness
-                            cell_matrix(i, j) += fe_values.JxW(q_point) *
-                                                 (lower_slack_phi_i * lower_slack_multiplier_phi_j
-                                                  + lower_slack_phi_i * lower_slack_phi_j *
-                                                    old_lower_slack_multiplier_values[q_point] /
-                                                    old_lower_slack_values[q_point]);
-                            //Equation 8 - complementary slackness
-                            cell_matrix(i, j) += fe_values.JxW(q_point) *
-                                                 (upper_slack_phi_i * upper_slack_multiplier_phi_j
-                                                  + upper_slack_phi_i * upper_slack_phi_j *
-                                                    old_upper_slack_multiplier_values[q_point] /
-                                                    old_upper_slack_values[q_point]);
-                        }
-
-                    }
-                }
-
-
-                MatrixTools::local_apply_boundary_values(boundary_values, local_dof_indices,
-                                                         cell_matrix, cell_rhs, true);
-
-
-                constraints.distribute_local_to_global(
-                        cell_matrix, cell_rhs, local_dof_indices, system_matrix, system_rhs);
-
-            }
-
-        }
-        pcout << "compress 1" << std::endl;
-        system_matrix.compress(VectorOperation::add);
-        system_rhs = calculate_rhs(distributed_state, barrier_size);
-
-        for (const auto &cell: dof_handler.active_cell_iterators()) {
-            if(cell->is_locally_owned())
-            {
-                std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
-                cell->get_dof_indices(i);
-
-                const types::global_dof_index dof_index_lower_slack =
-                        i[cell->get_fe().component_to_system_index(0, 0)];
-                typename LA::MPI::SparseMatrix::iterator iter = density_filter.filter_matrix.begin(
-                        i[cell->get_fe().component_to_system_index(0, 0)]);
-                for (; iter != density_filter.filter_matrix.end(i[cell->get_fe().component_to_system_index(0, 0)]); iter++) {
-                    unsigned int j = iter->column();
-                    double value = iter->value() * cell->measure();
-
-                    system_matrix.block(SolutionBlocks::unfiltered_density_multiplier,
-                                        SolutionBlocks::unfiltered_density).add(i[cell->get_fe().component_to_system_index(0, 0)], j, value);
-                    system_matrix.block(SolutionBlocks::unfiltered_density,
-                                        SolutionBlocks::unfiltered_density_multiplier).add(j, i[cell->get_fe().component_to_system_index(0, 0)], value);
-                }
-
-                system_matrix.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).add(0, i[cell->get_fe().component_to_system_index(0, 0)],
-                                                                                                          cell->measure());
-
-                system_matrix.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier).add(i[cell->get_fe().component_to_system_index(0, 0)], 0,
-                                                                                                          cell->measure());
-            }
-        }
-        pcout << "compress 2" << std::endl;
-        system_matrix.compress(VectorOperation::add);
-
-
-        pcout << "assembled" << std::endl;
-
-    }
-
-    template<int dim>
-    double
-    KktSystem<dim>::calculate_objective_value(const LA::MPI::BlockVector &distributed_state) const {
-        /*Remove any values from old iterations*/
-
-        locally_relevant_solution = distributed_state;
-
-
-        QGauss<dim> nine_quadrature(fe_nine.degree + 1);
-        QGauss<dim> ten_quadrature(fe_ten.degree + 1);
-
-        hp::QCollection<dim> q_collection;
-        q_collection.push_back(nine_quadrature);
-        q_collection.push_back(ten_quadrature);
-
-        hp::FEValues<dim> hp_fe_values(fe_collection,
-                                       q_collection,
-                                       update_values | update_quadrature_points |
-                                       update_JxW_values | update_gradients);
-
-        QGauss<dim - 1> common_face_quadrature(fe_ten.degree + 1);
-
-        FEFaceValues<dim> fe_nine_face_values(fe_nine,
-                                              common_face_quadrature,
-                                              update_JxW_values |
-                                              update_gradients | update_values);
-        FEFaceValues<dim> fe_ten_face_values(fe_ten,
-                                             common_face_quadrature,
-                                             update_normal_vectors |
-                                             update_values);
-
-        FullMatrix<double> cell_matrix;
-        Vector<double> cell_rhs;
-
-        const FEValuesExtractors::Vector displacements(SolutionComponents::displacement<dim>);
-
-        Tensor<1, dim> traction;
-        traction[1] = -1;
-        distributed_solution = distributed_state;
-        double objective_value = 0;
-        for (const auto &cell: dof_handler.active_cell_iterators())
-        {
-            if(cell->is_locally_owned())
-            {
-                hp_fe_values.reinit(cell);
-                const FEValues<dim> &fe_values = hp_fe_values.get_present_fe_values();
-                const unsigned int dofs_per_cell = cell->get_fe().n_dofs_per_cell();
-                const unsigned int n_q_points = fe_values.n_quadrature_points;
-                const unsigned int n_face_q_points = common_face_quadrature.size();
-
-                std::vector<Tensor<1, dim>> old_displacement_values(n_q_points);
-                fe_values[displacements].get_function_values(
-                        locally_relevant_solution, old_displacement_values);
-
-                for (unsigned int face_number = 0;
-                     face_number < GeometryInfo<dim>::faces_per_cell;
-                     ++face_number)
-                {
-                    if (cell->face(face_number)->at_boundary() && cell->face(face_number)->boundary_id()
-                                                                  == BoundaryIds::down_force)
-                    {
-                        for (unsigned int face_q_point = 0;
-                             face_q_point < n_face_q_points; ++face_q_point) {
-                            for (unsigned int i = 0; i < dofs_per_cell; ++i) {
-                                if (cell->material_id() == MaterialIds::without_multiplier) {
-                                    fe_nine_face_values.reinit(cell, face_number);
-                                    objective_value += traction
-                                                       * fe_nine_face_values[displacements].value(i,
-                                                                                                  face_q_point)
-                                                       * fe_nine_face_values.JxW(face_q_point);
-                                } else {
-                                    fe_ten_face_values.reinit(cell, face_number);
-                                    objective_value += traction
-                                                       * fe_ten_face_values[displacements].value(i,
-                                                                                                 face_q_point)
-                                                       * fe_ten_face_values.JxW(face_q_point);
-                                }
-                            }
-                        }
-                    }
-                }
-            }
-        }
-        double objective_value_out;
-        MPI_Allreduce(&objective_value, &objective_value_out, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-
-        pcout << "objective value: " << objective_value_out << std::endl;
-        return objective_value;
-    }
-
-
-    //As the KKT System know which vectors correspond to the slack variables, the sum of the logs of the slacks is computed here for use in the filter.
-    template<int dim>
-    double
-    KktSystem<dim>::calculate_barrier_distance(const LA::MPI::BlockVector &state) const {
-        double barrier_distance_log_sum = 0;
-        unsigned int vect_size = state.block(SolutionBlocks::density_lower_slack).size();
-        distributed_solution = state;
-        for (unsigned int k = 0; k < vect_size; k++) {
-            if (distributed_solution.block(SolutionBlocks::density_lower_slack).in_local_range(k))
-            barrier_distance_log_sum += std::log(state.block(SolutionBlocks::density_lower_slack)[k]);
-        }
-        for (unsigned int k = 0; k < vect_size; k++) {
-            if (distributed_solution.block(SolutionBlocks::density_upper_slack).in_local_range(k))
-            barrier_distance_log_sum += std::log(state.block(SolutionBlocks::density_upper_slack)[k]);
-        }
-        double out_barrier_distance_log_sum;
-        MPI_Allreduce(&barrier_distance_log_sum, &out_barrier_distance_log_sum, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-
-        pcout << "Barrier distance log sum: " << out_barrier_distance_log_sum << std::endl;
-
-        return out_barrier_distance_log_sum;
-    }
-
-    template<int dim>
-    double
-    KktSystem<dim>::calculate_rhs_norm(const LA::MPI::BlockVector &state, const double barrier_size) const {
-        return calculate_rhs(state, barrier_size).l2_norm();
-    }
-
-
-    //Feasibility conditions appear on the RHS of the linear system, so I compute the RHS to find it. Could probably be combined with the objective value finding part to make it faster.
-    template<int dim>
-    double
-    KktSystem<dim>::calculate_feasibility(const LA::MPI::BlockVector &state, const double barrier_size) const {
-       LA::MPI::BlockVector test_rhs = calculate_rhs(state, barrier_size);
-
-       double norm = 0;
-
-       distributed_solution = state;
-       double full_prod1 =1;
-       double full_prod2 = 1;
-
-       for (unsigned int k = 0; k < state.block(SolutionBlocks::density_upper_slack).size(); k++) {
-           double prod1 = 1;
-           double prod2 = 1;
-           if(state.block(SolutionBlocks::density_upper_slack).in_local_range(k))
-           {
-               prod1 = prod1 * state.block(SolutionBlocks::density_upper_slack)[k]
-                       * state.block(SolutionBlocks::density_upper_slack)[k];
-           }
-           if(state.block(SolutionBlocks::density_lower_slack).in_local_range(k))
-           {
-              prod2 = prod2 *  state.block(SolutionBlocks::density_lower_slack)[k]
-                      * state.block(SolutionBlocks::density_lower_slack)[k];
-           }
-           if(state.block(SolutionBlocks::density_upper_slack_multiplier).in_local_range(k))
-           {
-               prod1 = prod1 * state.block(SolutionBlocks::density_upper_slack_multiplier)[k]
-                       * state.block(SolutionBlocks::density_upper_slack_multiplier)[k];
-           }
-           if(state.block(SolutionBlocks::density_lower_slack_multiplier).in_local_range(k))
-           {
-               prod2 = prod2 *  state.block(SolutionBlocks::density_lower_slack_multiplier)[k]
-                       * state.block(SolutionBlocks::density_lower_slack_multiplier)[k];
-           }
-           MPI_Allreduce(&prod1, &full_prod1, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
-           MPI_Allreduce(&prod2, &full_prod2, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
-           norm = norm + full_prod1 + full_prod2;
-       }
-       pcout << "pre-norm: " << norm << std::endl;
-
-        norm += std::pow(test_rhs.block(SolutionBlocks::displacement).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::density).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::unfiltered_density).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::displacement_multiplier).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::total_volume_multiplier).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::density_upper_slack_multiplier).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::density_lower_slack_multiplier).l2_norm(), 2);
-
-        pcout << "norm: " << norm << std::endl;
-
-        return norm;
-    }
-
-    template<int dim>
-    double
-    KktSystem<dim>::calculate_convergence(const LA::MPI::BlockVector &state) const {
-        LA::MPI::BlockVector test_rhs = calculate_rhs(state, Input::min_barrier_size);
-        double norm = 0;
-
-        distributed_solution = state;
-        double full_prod1 =1;
-        double full_prod2 = 1;
-
-        for (unsigned int k = 0; k < state.block(SolutionBlocks::density_upper_slack).size(); k++) {
-            double prod1 = 1;
-            double prod2 = 1;
-            if(state.block(SolutionBlocks::density_upper_slack).in_local_range(k))
-            {
-                prod1 = prod1 * state.block(SolutionBlocks::density_upper_slack)[k]
-                        * state.block(SolutionBlocks::density_upper_slack)[k];
-            }
-            if(state.block(SolutionBlocks::density_lower_slack).in_local_range(k))
-            {
-               prod2 = prod2 *  state.block(SolutionBlocks::density_lower_slack)[k]
-                       * state.block(SolutionBlocks::density_lower_slack)[k];
-            }
-            if(state.block(SolutionBlocks::density_upper_slack_multiplier).in_local_range(k))
-            {
-                prod1 = prod1 * state.block(SolutionBlocks::density_upper_slack_multiplier)[k]
-                        * state.block(SolutionBlocks::density_upper_slack_multiplier)[k];
-            }
-            if(state.block(SolutionBlocks::density_lower_slack_multiplier).in_local_range(k))
-            {
-                prod2 = prod2 *  state.block(SolutionBlocks::density_lower_slack_multiplier)[k]
-                        * state.block(SolutionBlocks::density_lower_slack_multiplier)[k];
-            }
-            MPI_Allreduce(&prod1, &full_prod1, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
-            MPI_Allreduce(&prod2, &full_prod2, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
-            norm = norm + full_prod1 + full_prod2;
-        }
-        pcout << "pre-norm: " << norm << std::endl;
-
-        norm += std::pow(test_rhs.block(SolutionBlocks::displacement).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::density).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::unfiltered_density).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::displacement_multiplier).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::total_volume_multiplier).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::density_upper_slack_multiplier).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::density_lower_slack_multiplier).l2_norm(), 2);
-
-        norm = std::pow(norm, .5);
-
-        pcout << "l2 norm: " << system_rhs.l2_norm() << std::endl;
-        pcout << "KKT norm: " << norm << std::endl;
-        return norm;
-    }
-
-    template<int dim>
-    LA::MPI::BlockVector
-    KktSystem<dim>::calculate_rhs(const LA::MPI::BlockVector &distributed_state, const double barrier_size) const {
-        LA::MPI::BlockVector test_rhs (system_rhs);
-        LA::MPI::BlockVector state (locally_relevant_solution);
-        state = distributed_state;
-        test_rhs = 0;
-
-
-        QGauss<dim> nine_quadrature(fe_nine.degree + 1);
-        QGauss<dim> ten_quadrature(fe_ten.degree + 1);
-
-        hp::QCollection<dim> q_collection;
-        q_collection.push_back(nine_quadrature);
-        q_collection.push_back(ten_quadrature);
-
-        hp::FEValues<dim> hp_fe_values(fe_collection,
-                                       q_collection,
-                                       update_values | update_quadrature_points |
-                                       update_JxW_values | update_gradients);
-
-        QGauss<dim - 1> common_face_quadrature(fe_ten.degree + 1);
-
-        FEFaceValues<dim> fe_nine_face_values(fe_nine,
-                                              common_face_quadrature,
-                                              update_JxW_values |
-                                              update_gradients | update_values);
-        FEFaceValues<dim> fe_ten_face_values(fe_ten,
-                                             common_face_quadrature,
-                                             update_normal_vectors |
-                                             update_values);
-
-        FullMatrix<double> cell_matrix;
-        Vector<double> cell_rhs;
-        std::vector<types::global_dof_index> local_dof_indices;
-
-        const FEValuesExtractors::Scalar densities(SolutionComponents::density<dim>);
-        const FEValuesExtractors::Vector displacements(SolutionComponents::displacement<dim>);
-        const FEValuesExtractors::Scalar unfiltered_densities(SolutionComponents::unfiltered_density<dim>);
-        const FEValuesExtractors::Vector displacement_multipliers(SolutionComponents::displacement_multiplier<dim>);
-        const FEValuesExtractors::Scalar unfiltered_density_multipliers(
-                SolutionComponents::unfiltered_density_multiplier<dim>);
-        const FEValuesExtractors::Scalar density_lower_slacks(SolutionComponents::density_lower_slack<dim>);
-        const FEValuesExtractors::Scalar density_lower_slack_multipliers(
-                SolutionComponents::density_lower_slack_multiplier<dim>);
-        const FEValuesExtractors::Scalar density_upper_slacks(SolutionComponents::density_upper_slack<dim>);
-        const FEValuesExtractors::Scalar density_upper_slack_multipliers(
-                SolutionComponents::density_upper_slack_multiplier<dim>);
-        const FEValuesExtractors::Scalar total_volume_multiplier(
-                SolutionComponents::total_volume_multiplier<dim>);
-
-
-        const unsigned int n_face_q_points = common_face_quadrature.size();
-
-        const Functions::ConstantFunction<dim> lambda(1.), mu(1.);
-
-        locally_relevant_solution = state;
-        distributed_solution = state;
-        LA::MPI::BlockVector filtered_unfiltered_density_solution (distributed_solution);
-        LA::MPI::BlockVector filter_adjoint_unfiltered_density_multiplier_solution (distributed_solution);
-        filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = 0;
-        filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier) = 0;
-
-        auto op_f = linear_operator<LA::MPI::Vector>(density_filter.filter_matrix);
-        filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = op_f * distributed_solution.block(SolutionBlocks::unfiltered_density);
-        filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier)
-                                                    = transpose_operator(op_f) * distributed_solution.block(SolutionBlocks::unfiltered_density_multiplier);
-        LA::MPI::BlockVector relevant_filtered_unfiltered_density_solution (locally_relevant_solution);
-        LA::MPI::BlockVector relevant_filter_adjoint_unfiltered_density_multiplier_solution (locally_relevant_solution);
-        relevant_filtered_unfiltered_density_solution = filtered_unfiltered_density_solution;
-        relevant_filter_adjoint_unfiltered_density_multiplier_solution = filter_adjoint_unfiltered_density_multiplier_solution;
-
-        double old_volume_multiplier_temp = 0;
-        double old_volume_multiplier;
-        if(distributed_state.block(SolutionBlocks::total_volume_multiplier).in_local_range(0))
-        {
-            old_volume_multiplier_temp = state.block(SolutionBlocks::total_volume_multiplier)[0];
-        }
-        MPI_Allreduce(&old_volume_multiplier_temp, &old_volume_multiplier, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-
-        for (const auto &cell: dof_handler.active_cell_iterators()) {
-            if(cell->is_locally_owned())
-            {
-                hp_fe_values.reinit(cell);
-                const FEValues<dim> &fe_values = hp_fe_values.get_present_fe_values();
-                cell_matrix.reinit(cell->get_fe().n_dofs_per_cell(),
-                                   cell->get_fe().n_dofs_per_cell());
-                cell_rhs.reinit(cell->get_fe().n_dofs_per_cell());
-
-                const unsigned int n_q_points = fe_values.n_quadrature_points;
-
-                std::vector<double> old_density_values(n_q_points);
-                std::vector<Tensor<1, dim>> old_displacement_values(n_q_points);
-                std::vector<double> old_displacement_divs(n_q_points);
-                std::vector<SymmetricTensor<2, dim>> old_displacement_symmgrads(
-                        n_q_points);
-                std::vector<Tensor<1, dim>> old_displacement_multiplier_values(
-                        n_q_points);
-                std::vector<double> old_displacement_multiplier_divs(n_q_points);
-                std::vector<SymmetricTensor<2, dim>> old_displacement_multiplier_symmgrads(
-                        n_q_points);
-                std::vector<double> old_lower_slack_multiplier_values(n_q_points);
-                std::vector<double> old_upper_slack_multiplier_values(n_q_points);
-                std::vector<double> old_lower_slack_values(n_q_points);
-                std::vector<double> old_upper_slack_values(n_q_points);
-                std::vector<double> old_unfiltered_density_values(n_q_points);
-                std::vector<double> old_unfiltered_density_multiplier_values(n_q_points);
-                std::vector<double> filtered_unfiltered_density_values(n_q_points);
-                std::vector<double> filter_adjoint_unfiltered_density_multiplier_values(n_q_points);
-                std::vector<double> lambda_values(n_q_points);
-                std::vector<double> mu_values(n_q_points);
-
-                const unsigned int dofs_per_cell = cell->get_fe().n_dofs_per_cell();
-
-                cell_matrix = 0;
-                cell_rhs = 0;
-                local_dof_indices.resize(cell->get_fe().n_dofs_per_cell());
-
-                cell->get_dof_indices(local_dof_indices);
-
-                lambda.value_list(fe_values.get_quadrature_points(), lambda_values);
-                mu.value_list(fe_values.get_quadrature_points(), mu_values);
-
-                fe_values[densities].get_function_values(state,
-                                                         old_density_values);
-                fe_values[displacements].get_function_values(state,
-                                                             old_displacement_values);
-                fe_values[displacements].get_function_divergences(state,
-                                                                  old_displacement_divs);
-                fe_values[displacements].get_function_symmetric_gradients(
-                        state, old_displacement_symmgrads);
-                fe_values[displacement_multipliers].get_function_values(
-                        state, old_displacement_multiplier_values);
-                fe_values[displacement_multipliers].get_function_divergences(
-                        state, old_displacement_multiplier_divs);
-                fe_values[displacement_multipliers].get_function_symmetric_gradients(
-                        state, old_displacement_multiplier_symmgrads);
-                fe_values[density_lower_slacks].get_function_values(
-                        state, old_lower_slack_values);
-                fe_values[density_lower_slack_multipliers].get_function_values(
-                        state, old_lower_slack_multiplier_values);
-                fe_values[density_upper_slacks].get_function_values(
-                        state, old_upper_slack_values);
-                fe_values[density_upper_slack_multipliers].get_function_values(
-                        state, old_upper_slack_multiplier_values);
-                fe_values[unfiltered_densities].get_function_values(
-                        state, old_unfiltered_density_values);
-                fe_values[unfiltered_density_multipliers].get_function_values(
-                        state, old_unfiltered_density_multiplier_values);
-                fe_values[unfiltered_densities].get_function_values(
-                        relevant_filtered_unfiltered_density_solution, filtered_unfiltered_density_values);
-                fe_values[unfiltered_density_multipliers].get_function_values(
-                        relevant_filter_adjoint_unfiltered_density_multiplier_solution,
-                        filter_adjoint_unfiltered_density_multiplier_values);
-
-
-                Tensor<1, dim> traction;
-                traction[1] = -1;
-
-                for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
-
-                    for (unsigned int i = 0; i < dofs_per_cell; ++i) {
-                        const SymmetricTensor<2, dim> displacement_phi_i_symmgrad =
-                                fe_values[displacements].symmetric_gradient(i, q_point);
-                        const double displacement_phi_i_div =
-                                fe_values[displacements].divergence(i, q_point);
-
-                        const SymmetricTensor<2, dim> displacement_multiplier_phi_i_symmgrad =
-                                fe_values[displacement_multipliers].symmetric_gradient(i,
-                                                                                       q_point);
-                        const double displacement_multiplier_phi_i_div =
-                                fe_values[displacement_multipliers].divergence(i,
-                                                                               q_point);
-
-
-                        const double density_phi_i = fe_values[densities].value(i,
-                                                                                q_point);
-                        const double unfiltered_density_phi_i = fe_values[unfiltered_densities].value(i,
-                                                                                                      q_point);
-                        const double unfiltered_density_multiplier_phi_i = fe_values[unfiltered_density_multipliers].value(
-                                i, q_point);
-
-                        const double lower_slack_multiplier_phi_i =
-                                fe_values[density_lower_slack_multipliers].value(i,
-                                                                                 q_point);
-
-                        const double lower_slack_phi_i =
-                                fe_values[density_lower_slacks].value(i, q_point);
-
-                        const double upper_slack_phi_i =
-                                fe_values[density_upper_slacks].value(i, q_point);
-
-                        const double upper_slack_multiplier_phi_i =
-                                fe_values[density_upper_slack_multipliers].value(i,
-                                                                                 q_point);
-
-
-                        //rhs eqn 0
-                        cell_rhs(i) +=
-                                -1 * fe_values.JxW(q_point) * (
-                                        -1 * density_penalty_exponent *
-                                        std::pow(old_density_values[q_point], density_penalty_exponent - 1) * density_phi_i
-                                        * (old_displacement_multiplier_divs[q_point] * old_displacement_divs[q_point]
-                                           * lambda_values[q_point]
-                                           + 2 * mu_values[q_point] * (old_displacement_symmgrads[q_point]
-                                                                       * old_displacement_multiplier_symmgrads[q_point]))
-                                        - density_phi_i * old_unfiltered_density_multiplier_values[q_point]
-                                        + old_volume_multiplier * density_phi_i
-                                );
-
-                        //rhs eqn 1 - boundary terms counted later
-                        cell_rhs(i) +=
-                                -1 * fe_values.JxW(q_point) * (
-                                        -1 * std::pow(old_density_values[q_point], density_penalty_exponent)
-                                        * (old_displacement_multiplier_divs[q_point] * displacement_phi_i_div
-                                           * lambda_values[q_point]
-                                           + 2 * mu_values[q_point] * (old_displacement_multiplier_symmgrads[q_point]
-                                                                       * displacement_phi_i_symmgrad))
-                                );
-
-                        //rhs eqn 2
-                        cell_rhs(i) +=
-                                -1 * fe_values.JxW(q_point) * (
-                                        unfiltered_density_phi_i *
-                                        filter_adjoint_unfiltered_density_multiplier_values[q_point]
-                                        + unfiltered_density_phi_i * old_upper_slack_multiplier_values[q_point]
-                                        + -1 * unfiltered_density_phi_i * old_lower_slack_multiplier_values[q_point]
-                                );
-
-
-
-
-                        //rhs eqn 3 - boundary terms counted later
-                        cell_rhs(i) +=
-                                -1 * fe_values.JxW(q_point) * (
-                                        -1 * std::pow(old_density_values[q_point], density_penalty_exponent)
-                                        * (old_displacement_divs[q_point] * displacement_multiplier_phi_i_div
-                                           * lambda_values[q_point]
-                                           + 2 * mu_values[q_point] * (displacement_multiplier_phi_i_symmgrad
-                                                                       * old_displacement_symmgrads[q_point]))
-                                );
-
-                        //rhs eqn 4
-                        cell_rhs(i) +=
-                                -1 * fe_values.JxW(q_point) *
-                                (-1 * lower_slack_multiplier_phi_i
-                                 * (old_unfiltered_density_values[q_point] - old_lower_slack_values[q_point])
-                                );
-
-                        //rhs eqn 5
-                        cell_rhs(i) +=
-                                -1 * fe_values.JxW(q_point) * (
-                                        -1 * upper_slack_multiplier_phi_i
-                                        * (1 - old_unfiltered_density_values[q_point]
-                                           - old_upper_slack_values[q_point]));
-
-                        //rhs eqn 6
-                        cell_rhs(i) +=
-                                -1 * fe_values.JxW(q_point) * (
-                                        -1 * unfiltered_density_multiplier_phi_i
-                                        * (old_density_values[q_point] - filtered_unfiltered_density_values[q_point])
-                                );
-
-                        //rhs eqn 7
-                        cell_rhs(i) +=
-                                -1 * fe_values.JxW(q_point) *
-                                (lower_slack_phi_i *
-                                 (old_lower_slack_multiplier_values[q_point] -
-                                  barrier_size / old_lower_slack_values[q_point]));
-
-                        //rhs eqn 8
-                        cell_rhs(i) +=
-                                -1 * fe_values.JxW(q_point) *
-                                (upper_slack_phi_i *
-                                 (old_upper_slack_multiplier_values[q_point] -
-                                  barrier_size / old_upper_slack_values[q_point]));
-
-                    }
-
-                }
-
-                for (unsigned int face_number = 0;
-                     face_number < GeometryInfo<dim>::faces_per_cell;
-                     ++face_number) {
-                    if (cell->face(face_number)->at_boundary() && cell->face(
-                            face_number)->boundary_id() == BoundaryIds::down_force) {
-                        for (unsigned int face_q_point = 0;
-                             face_q_point < n_face_q_points; ++face_q_point) {
-                            for (unsigned int i = 0; i < dofs_per_cell; ++i) {
-                                if (cell->material_id() == MaterialIds::without_multiplier) {
-                                    fe_nine_face_values.reinit(cell, face_number);
-                                    cell_rhs(i) += -1
-                                                   * traction
-                                                   * fe_nine_face_values[displacements].value(i,
-                                                                                              face_q_point)
-                                                   * fe_nine_face_values.JxW(face_q_point);
-
-                                    cell_rhs(i) += -1 * traction
-                                                   * fe_nine_face_values[displacement_multipliers].value(
-                                            i, face_q_point)
-                                                   * fe_nine_face_values.JxW(face_q_point);
-                                } else {
-                                    fe_ten_face_values.reinit(cell, face_number);
-                                    cell_rhs(i) += -1
-                                                   * traction
-                                                   * fe_ten_face_values[displacements].value(i,
-                                                                                             face_q_point)
-                                                   * fe_ten_face_values.JxW(face_q_point);
-
-                                    cell_rhs(i) += -1 * traction
-                                                   * fe_ten_face_values[displacement_multipliers].value(
-                                            i, face_q_point)
-                                                   * fe_ten_face_values.JxW(face_q_point);
-                                }
-                            }
-                        }
-                    }
-                }
-
-
-                MatrixTools::local_apply_boundary_values(boundary_values, local_dof_indices,
-                                                         cell_matrix, cell_rhs, true);
-                constraints.distribute_local_to_global(
-                        cell_rhs, local_dof_indices, test_rhs);
-            }
-        }
-
-        test_rhs.compress(VectorOperation::add);
-        double total_volume_temp = 0;
-        double goal_volume_temp = 0;
-        double total_volume, goal_volume;
-
-        distributed_solution = state;
-        for (const auto &cell: dof_handler.active_cell_iterators()) {
-            if(cell->is_locally_owned())
-            {
-                if (distributed_solution.block(SolutionBlocks::density).in_local_range(cell->active_cell_index()))
-                {
-                    total_volume_temp += cell->measure() * state.block(SolutionBlocks::density)[cell->active_cell_index()];
-                    goal_volume_temp += cell->measure() * Input::volume_percentage;
-                }
-            }
-        }
-
-        MPI_Allreduce(&total_volume_temp, &total_volume, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-        MPI_Allreduce(&goal_volume_temp, &goal_volume, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-
-        if (test_rhs.block(SolutionBlocks::total_volume_multiplier).in_local_range(0))
-        {
-            test_rhs.block(SolutionBlocks::total_volume_multiplier)[0] = goal_volume - total_volume;
-        }
-        test_rhs.compress(VectorOperation::insert);
-
-        return test_rhs;
-
-    }
-
-
-    template<int dim>
-    LA::MPI::BlockVector
-    KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
-        double gmres_tolerance;
-        if (Input::use_eisenstat_walker) {
-            gmres_tolerance = std::max(
-                    std::min(
-                            .1 * system_rhs.l2_norm() / (initial_rhs_error),
-                            .001
-                    ),
-                    Input::default_gmres_tolerance);
-        }
-        else {
-            gmres_tolerance = Input::default_gmres_tolerance;
-        }
-        locally_relevant_solution=state;
-        distributed_solution = state;
-        SolverControl solver_control(10000, gmres_tolerance * system_rhs.l2_norm());
-        TopOptSchurPreconditioner<dim> preconditioner(system_matrix);
-
-
-        switch (Input::solver_choice) {
-
-            case SolverOptions::inexact_K_with_exact_A_gmres: {
-                preconditioner.initialize(system_matrix, boundary_values, dof_handler,  locally_relevant_solution, distributed_solution);
-                SolverFGMRES<LA::MPI::BlockVector> B_fgmres(solver_control);
-                B_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
-                pcout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
-                break;
-            }
-            case SolverOptions::inexact_K_with_inexact_A_gmres: {
-                preconditioner.initialize(system_matrix, boundary_values, dof_handler,  locally_relevant_solution, distributed_solution);
-                SolverFGMRES<LA::MPI::BlockVector> C_fgmres(solver_control);
-                C_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
-                pcout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
-                break;
-            }
-            default:
-                throw;
-        }
-
-
-        constraints.distribute(distributed_solution);
-
-        pcout << "here" << std::endl;
-
-        return distributed_solution;
-    }
-
-    template<int dim>
-    void
-    KktSystem<dim>::calculate_initial_rhs_error() {
-        initial_rhs_error = system_rhs.l2_norm();
-    }
-
-    template<int dim>
-    LA::MPI::BlockVector
-    KktSystem<dim>::get_initial_state() {
-
-        std::vector<unsigned int> block_component(10, 2);
-        block_component[SolutionBlocks::density] = 0;
-        block_component[SolutionBlocks::displacement] = 1;
-        const std::vector<types::global_dof_index> dofs_per_block =
-                DoFTools::count_dofs_per_fe_block(dof_handler, block_component);
-        const unsigned int n_p = dofs_per_block[0];
-        const unsigned int n_u = dofs_per_block[1];
-        const std::vector<unsigned int> block_sizes = {n_p, n_p, n_p, n_p, n_p, n_u, n_u, n_p, n_p, 1};
-
-        LA::MPI::BlockVector state(owned_partitioning, mpi_communicator);
-        {
-            using namespace SolutionBlocks;
-            state.block(density).add(density_ratio);
-            state.block(unfiltered_density).add(density_ratio);
-            state.block(unfiltered_density_multiplier)
-                    .add(density_ratio);
-            state.block(density_lower_slack).add(density_ratio);
-            state.block(density_lower_slack_multiplier).add(50);
-            state.block(density_upper_slack).add(1 - density_ratio);
-            state.block(density_upper_slack_multiplier).add(50);
-            state.block(total_volume_multiplier).add(1);
-            state.block(displacement).add(0);
-            state.block(displacement_multiplier).add(0);
-        }
-        state.compress(VectorOperation::add);
-        return state;
-    }
-
-    template<int dim>
-    void
-    KktSystem<dim>::output(const LA::MPI::BlockVector &state, const unsigned int j) const {
-        locally_relevant_solution = state;
-        std::vector<std::string> solution_names(1, "low_slack_multiplier");
-        std::vector<DataComponentInterpretation::DataComponentInterpretation> data_component_interpretation(
-                1, DataComponentInterpretation::component_is_scalar);
-        solution_names.emplace_back("upper_slack_multiplier");
-        data_component_interpretation.push_back(
-                DataComponentInterpretation::component_is_scalar);
-        solution_names.emplace_back("low_slack");
-        data_component_interpretation.push_back(
-                DataComponentInterpretation::component_is_scalar);
-        solution_names.emplace_back("upper_slack");
-        data_component_interpretation.push_back(
-                DataComponentInterpretation::component_is_scalar);
-        solution_names.emplace_back("unfiltered_density");
-        data_component_interpretation.push_back(
-                DataComponentInterpretation::component_is_scalar);
-        for (unsigned int i = 0; i < dim; i++) {
-            solution_names.emplace_back("displacement");
-            data_component_interpretation.push_back(
-                    DataComponentInterpretation::component_is_part_of_vector);
-        }
-        for (unsigned int i = 0; i < dim; i++) {
-            solution_names.emplace_back("displacement_multiplier");
-            data_component_interpretation.push_back(
-                    DataComponentInterpretation::component_is_part_of_vector);
-        }
-        solution_names.emplace_back("density_multiplier");
-        data_component_interpretation.push_back(
-                DataComponentInterpretation::component_is_scalar);
-        solution_names.emplace_back("density");
-        data_component_interpretation.push_back(
-                DataComponentInterpretation::component_is_scalar);
-        solution_names.emplace_back("volume_multiplier");
-        data_component_interpretation.push_back(
-                DataComponentInterpretation::component_is_scalar);
-        DataOut<dim> data_out;
-        data_out.attach_dof_handler(dof_handler);
-        data_out.add_data_vector(locally_relevant_solution,
-                                 solution_names,
-                                 DataOut<dim>::type_dof_data,
-                                 data_component_interpretation);
-        data_out.build_patches();
-        std::string output("solution" + std::to_string(j) + ".vtu");
-        data_out.write_vtu_in_parallel(output, MPI_COMM_WORLD);
-
-    }
-
-    template<>
-    void
-    KktSystem<2>::output_stl(const LA::MPI::BlockVector &state) {
-        double height = .25;
-        const int dim = 2;
-        std::ofstream stlfile;
-        stlfile.open("bridge.stl");
-        stlfile << "solid bridge\n" << std::scientific;
-
-        for (const auto &cell: dof_handler.active_cell_iterators()) {
-            if (state.block(
-                    SolutionBlocks::density)[cell->active_cell_index()] > 0.5) {
-                const Tensor<1, dim> edge_directions[2] = {cell->vertex(1) -
-                                                           cell->vertex(0),
-                                                           cell->vertex(2) -
-                                                           cell->vertex(0)};
-                const Tensor<2, dim> edge_tensor(
-                        {{edge_directions[0][0], edge_directions[0][1]},
-                         {edge_directions[1][0], edge_directions[1][1]}});
-                const bool is_right_handed_cell = (determinant(edge_tensor) > 0);
-                if (is_right_handed_cell) {
-                    /* Write one side at z = 0. */
-                    stlfile << "   facet normal " << 0.000000e+00 << " "
-                            << 0.000000e+00 << " " << -1.000000e+00 << "\n";
-                    stlfile << "      outer loop\n";
-                    stlfile << "         vertex " << cell->vertex(0)[0] << " "
-                            << cell->vertex(0)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
-                            << cell->vertex(2)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
-                            << cell->vertex(1)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "      endloop\n";
-                    stlfile << "   endfacet\n";
-                    stlfile << "   facet normal " << 0.000000e+00 << " "
-                            << 0.000000e+00 << " " << -1.000000e+00 << "\n";
-                    stlfile << "      outer loop\n";
-                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
-                            << cell->vertex(1)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
-                            << cell->vertex(2)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "         vertex " << cell->vertex(3)[0] << " "
-                            << cell->vertex(3)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "      endloop\n";
-                    stlfile << "   endfacet\n";
-                    /* Write one side at z = height. */
-                    stlfile << "   facet normal " << 0.000000e+00 << " "
-                            << 0.000000e+00 << " " << 1.000000e+00 << "\n";
-                    stlfile << "      outer loop\n";
-                    stlfile << "         vertex " << cell->vertex(0)[0] << " "
-                            << cell->vertex(0)[1] << " " << height << "\n";
-                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
-                            << cell->vertex(1)[1] << " " << height << "\n";
-                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
-                            << cell->vertex(2)[1] << " " << height << "\n";
-                    stlfile << "      endloop\n";
-                    stlfile << "   endfacet\n";
-                    stlfile << "   facet normal " << 0.000000e+00 << " "
-                            << 0.000000e+00 << " " << 1.000000e+00 << "\n";
-                    stlfile << "      outer loop\n";
-                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
-                            << cell->vertex(1)[1] << " " << height << "\n";
-                    stlfile << "         vertex " << cell->vertex(3)[0] << " "
-                            << cell->vertex(3)[1] << " " << height << "\n";
-                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
-                            << cell->vertex(2)[1] << " " << height << "\n";
-                    stlfile << "      endloop\n";
-                    stlfile << "   endfacet\n";
-                } else /* The cell has a left-handed set up */
-                {
-                    /* Write one side at z = 0. */
-                    stlfile << "   facet normal " << 0.000000e+00 << " "
-                            << 0.000000e+00 << " " << -1.000000e+00 << "\n";
-                    stlfile << "      outer loop\n";
-                    stlfile << "         vertex " << cell->vertex(0)[0] << " "
-                            << cell->vertex(0)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
-                            << cell->vertex(1)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
-                            << cell->vertex(2)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "      endloop\n";
-                    stlfile << "   endfacet\n";
-                    stlfile << "   facet normal " << 0.000000e+00 << " "
-                            << 0.000000e+00 << " " << -1.000000e+00 << "\n";
-                    stlfile << "      outer loop\n";
-                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
-                            << cell->vertex(1)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "         vertex " << cell->vertex(3)[0] << " "
-                            << cell->vertex(3)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
-                            << cell->vertex(2)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "      endloop\n";
-                    stlfile << "   endfacet\n";
-                    /* Write one side at z = height. */
-                    stlfile << "   facet normal " << 0.000000e+00 << " "
-                            << 0.000000e+00 << " " << 1.000000e+00 << "\n";
-                    stlfile << "      outer loop\n";
-                    stlfile << "         vertex " << cell->vertex(0)[0] << " "
-                            << cell->vertex(0)[1] << " " << height << "\n";
-                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
-                            << cell->vertex(2)[1] << " " << height << "\n";
-                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
-                            << cell->vertex(1)[1] << " " << height << "\n";
-                    stlfile << "      endloop\n";
-                    stlfile << "   endfacet\n";
-                    stlfile << "   facet normal " << 0.000000e+00 << " "
-                            << 0.000000e+00 << " " << 1.000000e+00 << "\n";
-                    stlfile << "      outer loop\n";
-                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
-                            << cell->vertex(1)[1] << " " << height << "\n";
-                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
-                            << cell->vertex(2)[1] << " " << height << "\n";
-                    stlfile << "         vertex " << cell->vertex(3)[0] << " "
-                            << cell->vertex(3)[1] << " " << height << "\n";
-                    stlfile << "      endloop\n";
-                    stlfile << "   endfacet\n";
-                }
-                for (unsigned int face_number = 0;
-                     face_number < GeometryInfo<dim>::faces_per_cell;
-                     ++face_number) {
-                    const typename DoFHandler<dim>::face_iterator face =
-                            cell->face(face_number);
-                    if ((face->at_boundary()) ||
-                        (!face->at_boundary() &&
-                         (state.block(
-                                 SolutionBlocks::density)[cell->neighbor(face_number)->active_cell_index()] <
-                          0.5))) {
-                        const Tensor<1, dim> normal_vector =
-                                (face->center() - cell->center());
-                        const double normal_norm = normal_vector.norm();
-                        if ((face->vertex(0)[0] - face->vertex(0)[0]) *
-                            (face->vertex(1)[1] - face->vertex(0)[1]) *
-                            0.000000e+00 +
-                            (face->vertex(0)[1] - face->vertex(0)[1]) * (0 - 0) *
-                            normal_vector[0] +
-                            (height - 0) *
-                            (face->vertex(1)[0] - face->vertex(0)[0]) *
-                            normal_vector[1] -
-                            (face->vertex(0)[0] - face->vertex(0)[0]) * (0 - 0) *
-                            normal_vector[1] -
-                            (face->vertex(0)[1] - face->vertex(0)[1]) *
-                            (face->vertex(1)[0] - face->vertex(0)[0]) *
-                            normal_vector[0] -
-                            (height - 0) *
-                            (face->vertex(1)[1] - face->vertex(0)[1]) * 0 >
-                            0) {
-                            stlfile << "   facet normal "
-                                    << normal_vector[0] / normal_norm << " "
-                                    << normal_vector[1] / normal_norm << " "
-                                    << 0.000000e+00 << "\n";
-                            stlfile << "      outer loop\n";
-                            stlfile << "         vertex " << face->vertex(0)[0]
-                                    << " " << face->vertex(0)[1] << " "
-                                    << 0.000000e+00 << "\n";
-                            stlfile << "         vertex " << face->vertex(0)[0]
-                                    << " " << face->vertex(0)[1] << " " << height
-                                    << "\n";
-                            stlfile << "         vertex " << face->vertex(1)[0]
-                                    << " " << face->vertex(1)[1] << " "
-                                    << 0.000000e+00 << "\n";
-                            stlfile << "      endloop\n";
-                            stlfile << "   endfacet\n";
-                            stlfile << "   facet normal "
-                                    << normal_vector[0] / normal_norm << " "
-                                    << normal_vector[1] / normal_norm << " "
-                                    << 0.000000e+00 << "\n";
-                            stlfile << "      outer loop\n";
-                            stlfile << "         vertex " << face->vertex(0)[0]
-                                    << " " << face->vertex(0)[1] << " " << height
-                                    << "\n";
-                            stlfile << "         vertex " << face->vertex(1)[0]
-                                    << " " << face->vertex(1)[1] << " " << height
-                                    << "\n";
-                            stlfile << "         vertex " << face->vertex(1)[0]
-                                    << " " << face->vertex(1)[1] << " "
-                                    << 0.000000e+00 << "\n";
-                            stlfile << "      endloop\n";
-                            stlfile << "   endfacet\n";
-                        } else {
-                            stlfile << "   facet normal "
-                                    << normal_vector[0] / normal_norm << " "
-                                    << normal_vector[1] / normal_norm << " "
-                                    << 0.000000e+00 << "\n";
-                            stlfile << "      outer loop\n";
-                            stlfile << "         vertex " << face->vertex(0)[0]
-                                    << " " << face->vertex(0)[1] << " "
-                                    << 0.000000e+00 << "\n";
-                            stlfile << "         vertex " << face->vertex(1)[0]
-                                    << " " << face->vertex(1)[1] << " "
-                                    << 0.000000e+00 << "\n";
-                            stlfile << "         vertex " << face->vertex(0)[0]
-                                    << " " << face->vertex(0)[1] << " " << height
-                                    << "\n";
-                            stlfile << "      endloop\n";
-                            stlfile << "   endfacet\n";
-                            stlfile << "   facet normal "
-                                    << normal_vector[0] / normal_norm << " "
-                                    << normal_vector[1] / normal_norm << " "
-                                    << 0.000000e+00 << "\n";
-                            stlfile << "      outer loop\n";
-                            stlfile << "         vertex " << face->vertex(0)[0]
-                                    << " " << face->vertex(0)[1] << " " << height
-                                    << "\n";
-                            stlfile << "         vertex " << face->vertex(1)[0]
-                                    << " " << face->vertex(1)[1] << " "
-                                    << 0.000000e+00 << "\n";
-                            stlfile << "         vertex " << face->vertex(1)[0]
-                                    << " " << face->vertex(1)[1] << " " << height
-                                    << "\n";
-                            stlfile << "      endloop\n";
-                            stlfile << "   endfacet\n";
-                        }
-                    }
-                }
-            }
-        }
-        stlfile << "endsolid bridge";
-    }
-
-
-    template<>
-    void
-    KktSystem<3>::output_stl(const LA::MPI::BlockVector &state)
-    {
-        std::ofstream stlfile;
-        stlfile.open("bridge.stl");
-        stlfile << "solid bridge\n" << std::scientific;
-        const int dim = 3;
-        for (const auto &cell : dof_handler.active_cell_iterators())
-        {
-            if (state.block(
-                    SolutionBlocks::unfiltered_density)[cell->active_cell_index()] > 0.5)
-            {
-                for (const auto n : cell->face_indices())
-                {
-                    bool create_boundary = false;
-                    if (cell->at_boundary(n))
-                    {
-                        create_boundary = true;
-                    }
-                    else if (state.block(
-                            SolutionBlocks::unfiltered_density)[cell->neighbor(n)->active_cell_index()] <= 0.5)
-                    {
-                        create_boundary = true;
-                    }
-
-                    if (create_boundary)
-                    {
-                        const auto face = cell->face(n);
-                        const Tensor<1,dim> normal_vector = face->center() -
-                                                          cell->center();
-                        double normal_norm = normal_vector.norm();
-                        const Tensor<1,dim> edge_vectors_1 = face->vertex(1) - face->vertex(0);
-                        const Tensor<1,dim> edge_vectors_2 = face->vertex(2) - face->vertex(0);
-
-                        const Tensor<2, dim> edge_tensor (
-                                 {{edge_vectors_1[0], edge_vectors_1[1],edge_vectors_1[2]},
-                                 {edge_vectors_2[0], edge_vectors_2[1],edge_vectors_2[2]},
-                                 {normal_vector[0], normal_vector[1], normal_vector[2]}});
-                        const bool is_right_handed_cell = (determinant(edge_tensor) > 0);
-
-                        if (is_right_handed_cell)
-                        {
-                            stlfile << "   facet normal "
-                                    << normal_vector[0] / normal_norm << " "
-                                    << normal_vector[1] / normal_norm << " "
-                                    << normal_vector[2] / normal_norm << "\n";
-                            stlfile << "      outer loop\n";
-                            stlfile << "         vertex " << face->vertex(0)[0]
-                                    << " " << face->vertex(0)[1] << " "
-                                    << face->vertex(0)[2] << "\n";
-                            stlfile << "         vertex " << face->vertex(1)[0]
-                                    << " " << face->vertex(1)[1] << " "
-                                    << face->vertex(1)[2] << "\n";
-                            stlfile << "         vertex " << face->vertex(2)[0]
-                                    << " " << face->vertex(2)[1] << " "
-                                    << face->vertex(2)[2] << "\n";
-                            stlfile << "      endloop\n";
-                            stlfile << "   endfacet\n";
-                            stlfile << "   facet normal "
-                                    << normal_vector[0] / normal_norm << " "
-                                    << normal_vector[1] / normal_norm << " "
-                                    << normal_vector[2] / normal_norm << "\n";
-                            stlfile << "      outer loop\n";
-                            stlfile << "         vertex " << face->vertex(1)[0]
-                                    << " " << face->vertex(1)[1] << " " << face->vertex(1)[2]
-                                    << "\n";
-                            stlfile << "         vertex " << face->vertex(3)[0]
-                                    << " " << face->vertex(3)[1] << " " << face->vertex(3)[2]
-                                    << "\n";
-                            stlfile << "         vertex " << face->vertex(2)[0]
-                                    << " " << face->vertex(2)[1] << " "
-                                    << face->vertex(2)[2] << "\n";
-                            stlfile << "      endloop\n";
-                            stlfile << "   endfacet\n";
-                        }
-                        else
-                        {
-                            stlfile << "   facet normal "
-                                    << normal_vector[0] / normal_norm << " "
-                                    << normal_vector[1] / normal_norm << " "
-                                    << normal_vector[2] / normal_norm << "\n";
-                            stlfile << "      outer loop\n";
-                            stlfile << "         vertex " << face->vertex(0)[0]
-                                    << " " << face->vertex(0)[1] << " "
-                                    << face->vertex(0)[2] << "\n";
-                            stlfile << "         vertex " << face->vertex(2)[0]
-                                    << " " << face->vertex(2)[1] << " "
-                                    << face->vertex(2)[2] << "\n";
-                            stlfile << "         vertex " << face->vertex(1)[0]
-                                    << " " << face->vertex(1)[1] << " "
-                                    << face->vertex(1)[2] << "\n";
-                            stlfile << "      endloop\n";
-                            stlfile << "   endfacet\n";
-                            stlfile << "   facet normal "
-                                    << normal_vector[0] / normal_norm << " "
-                                    << normal_vector[1] / normal_norm << " "
-                                    << normal_vector[2] / normal_norm << "\n";
-                            stlfile << "      outer loop\n";
-                            stlfile << "         vertex " << face->vertex(1)[0]
-                                    << " " << face->vertex(1)[1] << " " << face->vertex(1)[2]
-                                    << "\n";
-                            stlfile << "         vertex " << face->vertex(2)[0]
-                                    << " " << face->vertex(2)[1] << " " << face->vertex(2)[2]
-                                    << "\n";
-                            stlfile << "         vertex " << face->vertex(3)[0]
-                                    << " " << face->vertex(3)[1] << " "
-                                    << face->vertex(3)[2] << "\n";
-                            stlfile << "      endloop\n";
-                            stlfile << "   endfacet\n";
-                        }
-
-                    }
-
-                }
-            }
-        }
-    }
-}
-
-template class SAND::KktSystem<2>;
-template class SAND::KktSystem<3>;
diff --git a/source/schur_preconditioner.cc.autosave b/source/schur_preconditioner.cc.autosave
index a1bebac..0b4878c 100644
--- a/source/schur_preconditioner.cc.autosave
+++ b/source/schur_preconditioner.cc.autosave
@@ -23,8 +23,8 @@ namespace SAND {
     template<int dim>
     TopOptSchurPreconditioner<dim>::TopOptSchurPreconditioner(LA::MPI::BlockSparseMatrix &matrix_in)
             :
-            mpi_communicator(MPI_COMM_WORLD),
             system_matrix(matrix_in),
+            mpi_communicator(MPI_COMM_WORLD),
             n_rows(0),
             n_columns(0),
             n_block_rows(0),
@@ -53,7 +53,7 @@ namespace SAND {
     }
 
     template<int dim>
-    void TopOptSchurPreconditioner<dim>::initialize(LA::MPI::BlockSparseMatrix &matrix, const std::map<types::global_dof_index, double> &boundary_values,const DoFHandler<dim> &dof_handler, const LA::MPI::BlockVector &locally_relevant_state, const LA::MPI::BlockVector &distributed_state)
+    void TopOptSchurPreconditioner<dim>::initialize(LA::MPI::BlockSparseMatrix &matrix, const std::map<types::global_dof_index, double> &boundary_values,const DoFHandler<dim> &dof_handler, const LA::MPI::BlockVector &distributed_state)
     {
         TimerOutput::Scope t(timer, "initialize");
         {
@@ -124,6 +124,13 @@ namespace SAND {
         else
         {
             TimerOutput::Scope t(timer, "build A inv");
+
+//            if (Utilities::MPI::this_mpi_process(mpi_communicator) == 0)
+//            {
+//                std::string name = "a_mat";
+//                print_matrix(name, a_mat);
+//            }
+
             a_inv_direct.initialize(a_mat);
         }
         {
@@ -153,17 +160,17 @@ namespace SAND {
 
 
 
-            double l_global[n_p]= {0};
-            double lm_global[n_p]= {0};
-            double u_global[n_p]= {0};
-            double um_global[n_p]= {0};
-            double m_global[n_p]= {0};
+            std::vector<double> l_global(n_p);
+            std::vector<double> lm_global(n_p);
+            std::vector<double> u_global(n_p);
+            std::vector<double> um_global(n_p);
+            std::vector<double> m_global(n_p);
 
-            double l[n_p]= {0};
-            double lm[n_p]= {0};
-            double u[n_p]= {0};
-            double um[n_p]= {0};
-            double m[n_p]= {0};
+            std::vector<double> l(n_p);
+            std::vector<double> lm(n_p);
+            std::vector<double> u(n_p);
+            std::vector<double> um(n_p);
+            std::vector<double> m(n_p);
 
             TimerOutput::Scope t(timer, "build diag matrices");
             for (const auto cell: dof_handler.active_cell_iterators())
@@ -200,11 +207,11 @@ namespace SAND {
                 }
             }
 
-            MPI_Allreduce(lm.data(), &lm_global, n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-            MPI_Allreduce(&l, &l_global, n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-            MPI_Allreduce(&um, &um_global, n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-            MPI_Allreduce(&u, &u_global, n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-            MPI_Allreduce(&m, &m_global, n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+            MPI_Allreduce(lm.data(), lm_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+            MPI_Allreduce(l.data(), l_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+            MPI_Allreduce(um.data(), um_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+            MPI_Allreduce(u.data(), u_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+            MPI_Allreduce(m.data(), m_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
 
             for (unsigned int k=0; k< n_p; k++)
             {
@@ -235,6 +242,31 @@ namespace SAND {
         g_d_m_inv_density=distributed_state.block(SolutionBlocks::density);
         k_g_d_m_inv_density=distributed_state.block(SolutionBlocks::density);
 
+
+        op_d_8 = linear_operator<VectorType,VectorType,PayloadType>(d_8_mat);
+        op_f = linear_operator<VectorType,VectorType,PayloadType>(f_mat);
+        op_b = linear_operator<VectorType,VectorType,PayloadType>(b_mat);
+        op_c = linear_operator<VectorType,VectorType,PayloadType>(c_mat);
+        op_a_inv = linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct);
+        op_e = linear_operator<VectorType,VectorType,PayloadType>(e_mat);
+        op_d_m= linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
+        op_d_m_inv= linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat);
+
+        op_g = op_f * op_d_8 *
+                transpose_operator(op_f);
+
+
+        op_h = op_b
+               - transpose_operator(op_c) * op_a_inv * op_e
+               - transpose_operator(op_e) * op_a_inv * op_c;
+
+//        TrilinosWrappers::internal::LinearOperatorImplementation::TrilinosPayload inv_payload = inverse_payload(TrilinosWrappers::SolverGMRES(),TrilinosWrappers::PreconditionIdentity());
+
+        op_k_inv = (-1 * op_g *op_d_m_inv * op_h - op_d_m);
+
+
+
+
     }
 
 
@@ -341,8 +373,6 @@ namespace SAND {
         auto dst_temp = dst;
         auto k_density_mult =  src.block(SolutionBlocks::density);
 
-
-
         if (Input::solver_choice == SolverOptions::exact_preconditioner_with_gmres)
         {
 //            g_d_m_inv_density = linear_operator<VectorType,VectorType,PayloadType>(g_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
@@ -352,31 +382,15 @@ namespace SAND {
 
         else if (Input::solver_choice == SolverOptions::inexact_K_with_exact_A_gmres)
         {
-            auto op_d_8 = linear_operator<VectorType,VectorType,PayloadType>(d_8_mat);
-            auto op_f = linear_operator<VectorType,VectorType,PayloadType>(f_mat);
-            auto op_b = linear_operator<VectorType,VectorType,PayloadType>(b_mat);
-            auto op_c = linear_operator<VectorType,VectorType,PayloadType>(c_mat);
-            auto op_a_inv = linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct);
-            auto op_e = linear_operator<VectorType,VectorType,PayloadType>(e_mat);
-            auto op_d_m= linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
-            auto op_d_m_inv= linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat);
-
-            auto op_g = op_f * op_d_8 *
-                    transpose_operator(op_f);
-
-
-            auto op_h = op_b
-                   - transpose_operator(op_c) * op_a_inv * op_e
-                   - transpose_operator(op_e) * op_a_inv * op_c;
-
-            auto op_k_inv = -1 * op_g *op_d_m_inv * op_h - op_d_m;
 
             g_d_m_inv_density = op_g * op_d_m_inv * src.block(SolutionBlocks::density);
             SolverControl step_4_gmres_control_1 (100000, g_d_m_inv_density.l2_norm()*1e-6);
             SolverGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
             try {
-                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
-                                      g_d_m_inv_density;
+                TimerOutput::Scope u(timer, "actual inv");
+//                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
+//                                      g_d_m_inv_density;
+                step_4_gmres_1.solve(op_k_inv,k_g_d_m_inv_density,g_d_m_inv_density, PreconditionIdentity() );
             }
             catch (std::exception &exc)
             {
@@ -388,8 +402,10 @@ namespace SAND {
             SolverControl step_4_gmres_control_2 (100000, src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6);
             SolverGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
             try {
-                k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, PreconditionIdentity()) *
-                                 src.block(SolutionBlocks::unfiltered_density_multiplier);
+                TimerOutput::Scope u(timer, "actual inv");
+//                k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, PreconditionIdentity()) *
+//                                 src.block(SolutionBlocks::unfiltered_density_multiplier);
+                step_4_gmres_2.solve(op_k_inv,k_density_mult,src.block(SolutionBlocks::unfiltered_density_multiplier), PreconditionIdentity() );
             } catch (std::exception &exc)
             {
                 std::cerr << "Failure of linear solver step_4_gmres_2" << std::endl;
@@ -406,7 +422,9 @@ namespace SAND {
 
             auto preconditioner = PreconditionIdentity();
 
-            auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg, preconditioner);
+            auto a_inv_op = op_d_8;
+
+                    inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg, preconditioner);
 
 
             auto op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) *
@@ -416,13 +434,16 @@ namespace SAND {
                         - transpose_operator<VectorType, VectorType, PayloadType>(c_mat) * a_inv_op * linear_operator<VectorType,VectorType,PayloadType>(e_mat)
                         - transpose_operator<VectorType, VectorType, PayloadType>(e_mat) * a_inv_op * linear_operator<VectorType,VectorType,PayloadType>(c_mat);
 
-            auto op_k_inv = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
+            auto op_k_inv = op_d_8;
+
+                    -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
 
             SolverControl step_4_gmres_control_1 (10000, std::max(g_d_m_inv_density.l2_norm()*1e-6,1e-6));
             SolverGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
             try {
-                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
-                                      g_d_m_inv_density;
+//                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
+//                                      g_d_m_inv_density;
+//                step_4_gmres_1.solve(op_k_inv,k_g_d_m_inv_density,g_d_m_inv_density, PreconditionIdentity() );
             } catch (std::exception &exc)
             {
                 std::cerr << "Failure of linear solver step_4_gmres_1" << std::endl;
@@ -518,30 +539,23 @@ namespace SAND {
 
             else if (Input::solver_choice == SolverOptions::inexact_K_with_exact_A_gmres)
             {
-                auto op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) *
-                            transpose_operator(linear_operator<VectorType,VectorType,PayloadType>(f_mat));
-
-                auto op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
-                            - transpose_operator<VectorType, VectorType, PayloadType>(c_mat) *
-                                    linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct) * linear_operator<VectorType,VectorType,PayloadType>(e_mat)
-                            - transpose_operator<VectorType, VectorType, PayloadType>(e_mat) * linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct) * linear_operator<VectorType,VectorType,PayloadType>(c_mat);
-
-                auto op_k_inv = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
-
-                pre_j = src.block(SolutionBlocks::density) + op_h * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
                 auto pre_pre_k = pre_k;
-                pre_pre_k = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
-                pre_k =  pre_pre_k + src.block(SolutionBlocks::unfiltered_density_multiplier);
-
-
+                {
+                    TimerOutput::Scope t(timer, "not inverse 5.1");
+                    pre_j = src.block(SolutionBlocks::density) + op_h * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
+                    pre_pre_k = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
+                    pre_k =  pre_pre_k + src.block(SolutionBlocks::unfiltered_density_multiplier);
+                }
                 SolverControl step_5_gmres_control_1 (100000, pre_j.l2_norm()*1e-6);
                 SolverGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
                 try {
-                    dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
-                                                                               pre_j;
+                    TimerOutput::Scope t(timer, "actual inverse 5.1");
+//                    dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
+//                                                                               pre_j;
+                    step_5_gmres_1.solve(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), dst.block(SolutionBlocks::unfiltered_density_multiplier), pre_j , PreconditionIdentity());
                 } catch (std::exception &exc)
                 {
-                    std::cerr << "Failure of linear solver step_5_gmres_1" << std::endl;
+                    std::cerr << "Failure of linear solver step_5_gmres_1 again" << std::endl;
                     pcout << "first residual: " << step_5_gmres_control_1.initial_value() << std::endl;
                     pcout << "last residual: " << step_5_gmres_control_1.last_value() << std::endl;
                     throw;
@@ -551,7 +565,9 @@ namespace SAND {
                 SolverControl step_5_gmres_control_2 (100000, pre_k.l2_norm()*1e-6);
                 SolverGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
                 try {
-                    dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) * pre_k;
+                    TimerOutput::Scope t(timer, "actual inverse 5.2");
+//                    dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) * pre_k;
+                    step_5_gmres_2.solve(op_k_inv,dst.block(SolutionBlocks::density), pre_k , PreconditionIdentity());
                 } catch (std::exception &exc)
                 {
                     std::cerr << "Failure of linear solver step_5_gmres_2" << std::endl;
@@ -580,7 +596,7 @@ namespace SAND {
                 pre_k = -1* op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density) + src.block(SolutionBlocks::unfiltered_density_multiplier);
 
                 SolverControl step_5_gmres_control_1 (10000, std::max(pre_j.l2_norm()*1e-6,1e-6));
-                SolverGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
+                TrilinosWrappers::SolverGMRES step_5_gmres_1 (step_5_gmres_control_1);
                 try {
                     dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType,VectorType,PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
                                                                                pre_j;
@@ -593,7 +609,7 @@ namespace SAND {
                 }
 
                 SolverControl step_5_gmres_control_2 (10000, std::max(pre_k.l2_norm()*1e-6,1e-6));
-                SolverGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
+                TrilinosWrappers::SolverGMRES step_5_gmres_2 (step_5_gmres_control_2);
                 try {
                     dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) *
                                                          pre_k;

From dc314f08869311a9dc2eb760332230c9bc3caa5d Mon Sep 17 00:00:00 2001
From: Justin O'Connor <oconnor@tempest.math.colostate.edu>
Date: Wed, 19 Jan 2022 14:36:41 -0700
Subject: [PATCH 61/95] fixing density filter

---
 include/density_filter.h                |  12 +-
 source/density_filter.cc                | 138 +++--
 source/schur_preconditioner.cc.autosave | 709 ------------------------
 3 files changed, 109 insertions(+), 750 deletions(-)
 delete mode 100644 source/schur_preconditioner.cc.autosave

diff --git a/include/density_filter.h b/include/density_filter.h
index 5bee367..44ed718 100644
--- a/include/density_filter.h
+++ b/include/density_filter.h
@@ -58,14 +58,22 @@ namespace SAND {
     class DensityFilter {
     public:
 
-        DensityFilter()=default;
+        MPI_Comm  mpi_communicator;
+        std::vector<IndexSet> owned_partitioning;
+        std::vector<IndexSet> relevant_partitioning;
+
+        DensityFilter();
         DynamicSparsityPattern filter_dsp;
         LA::MPI::SparseMatrix filter_matrix;
         SparsityPattern filter_sparsity_pattern;
         void initialize(DoFHandler<dim> &dof_handler);
-        std::set<typename DoFHandler<dim>::cell_iterator> find_relevant_neighbors(typename DoFHandler<dim>::cell_iterator cell) const;
+        std::set<unsigned int> find_relevant_neighbors(typename DoFHandler<dim>::cell_iterator cell) const;
 
     private:
+        LA::MPI::Vector cell_m;
+        LA::MPI::Vector x_coord;
+        LA::MPI::Vector y_coord;
+        LA::MPI::Vector z_coord;
 
     };
 }
diff --git a/source/density_filter.cc b/source/density_filter.cc
index 359e4e0..bc0bb20 100644
--- a/source/density_filter.cc
+++ b/source/density_filter.cc
@@ -16,10 +16,30 @@ namespace SAND {
 
     /* When initialized, this function takes the current triangulation and creates a matrix corresponding to a
      * convolution being applied to a piecewise constant function on that triangulation  */
+
+    template<int dim>
+    DensityFilter<dim>::DensityFilter() :
+        mpi_communicator(MPI_COMM_WORLD)
+    {
+
+    }
+
+
     template<int dim>
     void
     DensityFilter<dim>::initialize(DoFHandler<dim> &dof_handler) {
-        const std::vector<types::global_dof_index> dofs_per_block = DoFTools::count_dofs_per_fe_block(dof_handler);
+        std::vector<unsigned int> block_component(10, 2);
+        block_component[SolutionBlocks::density] = 0;
+        block_component[SolutionBlocks::displacement] = 1;
+        const std::vector<types::global_dof_index> dofs_per_block =
+                DoFTools::count_dofs_per_fe_block(dof_handler, block_component);
+        const unsigned int n_p = dofs_per_block[0];
+        IndexSet local_owned = dof_handler.locally_owned_dofs().get_view(0, n_p);
+        x_coord.reinit(local_owned,mpi_communicator);
+        y_coord.reinit(local_owned,mpi_communicator);
+        z_coord.reinit(local_owned,mpi_communicator);
+        cell_m.reinit(local_owned,mpi_communicator);
+
         filter_dsp.reinit(dofs_per_block[0],
                           dofs_per_block[0]);
         std::cout << "initialized with " << dofs_per_block[0] << std::endl;
@@ -27,42 +47,67 @@ namespace SAND {
         std::set<typename DoFHandler<dim>::cell_iterator> cells_to_check;
         std::set<typename DoFHandler<dim>::cell_iterator> cells_to_check_temp;
         /*finds neighbors whose values would be relevant, and adds them to the sparsity pattern of the matrix*/
+         for (const auto &cell : dof_handler.active_cell_iterators())
+         {
+             if(cell->is_locally_owned())
+             {
+                std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
+                cell->get_dof_indices(i);
+                x_coord.set(i[cell->get_fe().component_to_system_index(0, 0)],cell->center(0)) ;
+                y_coord.set(i[cell->get_fe().component_to_system_index(0, 0)],cell->center(1)) ;
+                cell_m.set(i[cell->get_fe().component_to_system_index(0, 0)],cell->measure());
+                if (dim==3)
+                {
+                    z_coord.set(i[cell->get_fe().component_to_system_index(0, 0)],cell->center(2)) ;
+                }
+             }
+         }
+         x_coord.compress(VectorOperation::add);
+         y_coord.compress(VectorOperation::add);
+         z_coord.compress(VectorOperation::add);
+         cell_m.compress(VectorOperation::add);
+
         for (const auto &cell : dof_handler.active_cell_iterators()) {
             if(cell->is_locally_owned())
             {
-
                 std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
                 cell->get_dof_indices(i);
-                for (const auto &neighbor_cell : find_relevant_neighbors(cell))
+                for (const auto &neighbor_cell_index : find_relevant_neighbors(cell))
                 {
-                    std::vector<types::global_dof_index> j(neighbor_cell->get_fe().n_dofs_per_cell());
-                    neighbor_cell->get_dof_indices(j);
-                    filter_dsp.add(i[cell->get_fe().component_to_system_index(0, 0)], j[cell->get_fe().component_to_system_index(0, 0)]);
+                    filter_dsp.set(i[cell->get_fe().component_to_system_index(0, 0)], neighbor_cell_index);
                 }
             }
-
         }
+
         filter_sparsity_pattern.copy_from(filter_dsp);
 
         const auto owned_dofs = dof_handler.locally_owned_dofs().get_view(0, dofs_per_block[0]);
 
         filter_matrix.reinit(owned_dofs, filter_sparsity_pattern, MPI_COMM_WORLD);
 
-        /*adds values to the matrix corresponding to the max radius - */
+        /*adds values to the matrix corresponding to the max radius - distance*/
         for (const auto &cell : dof_handler.active_cell_iterators())
         {
             if(cell->is_locally_owned())
             {
                 std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
                 cell->get_dof_indices(i);
-                for (const auto &neighbor_cell : find_relevant_neighbors(cell)) {
-                    std::vector<types::global_dof_index> j(neighbor_cell->get_fe().n_dofs_per_cell());
-                    neighbor_cell->get_dof_indices(j);
-                    const double d =
-                            cell->center().distance(neighbor_cell->center());
+                for (const auto &neighbor_cell_index : find_relevant_neighbors(cell)) {
+                    double d_x = std::abs(x_coord[i[cell->get_fe().component_to_system_index(0, 0)]]-x_coord[neighbor_cell_index]);
+                    double d_y = std::abs(y_coord[i[cell->get_fe().component_to_system_index(0, 0)]]-y_coord[neighbor_cell_index]);
+                    double d;
+                    if (dim==3)
+                    {
+                        double d_z = std::abs(z_coord[i[cell->get_fe().component_to_system_index(0, 0)]]-z_coord[neighbor_cell_index]);
+                        d = std::pow(d_x*d_x + d_y*d_y + d_z*d_z , .5);
+                    }
+                    else
+                    {
+                        d = std::pow(d_x*d_x + d_y*d_y , .5);
+                    }
                     /*value should be (max radius - distance between cells)*cell measure */
-                    double value = (Input::filter_r - d)*neighbor_cell->measure();
-                    filter_matrix.add(i[cell->get_fe().component_to_system_index(0, 0)], j[cell->get_fe().component_to_system_index(0, 0)], value);
+                    double value = (Input::filter_r - d)*cell_m[neighbor_cell_index];
+                    filter_matrix.add(i[cell->get_fe().component_to_system_index(0, 0)], neighbor_cell_index, value);
                 }
             }
         }
@@ -93,35 +138,50 @@ namespace SAND {
 
     /*This function finds which neighbors are within a certain radius of the initial cell.*/
     template<int dim>
-    std::set<typename DoFHandler<dim>::cell_iterator>
-    DensityFilter<dim>::find_relevant_neighbors(typename DoFHandler<dim>::cell_iterator cell) const {
-        std::set<CellId> neighbor_ids;
-        std::set<typename DoFHandler<dim>::cell_iterator> cells_to_check;
-        neighbor_ids.insert(cell->id());
-        cells_to_check.insert(cell);
-        bool new_neighbors_found;
-        do {
-            new_neighbors_found = false;
-            for (const auto &check_cell :
-                    std::vector<typename DoFHandler<dim>::cell_iterator>(
-                            cells_to_check.begin(), cells_to_check.end())) {
-                for (const auto n : check_cell->face_indices()) {
-                    if (!(check_cell->face(n)->at_boundary())) {
-                        const auto &neighbor = check_cell->neighbor(n);
-                        const double distance =
-                                cell->center().distance(neighbor->center());
-                        if ((distance < Input::filter_r) &&
-                            !(neighbor_ids.count(neighbor->id())))
+    std::set<unsigned int>
+    DensityFilter<dim>::find_relevant_neighbors(typename DoFHandler<dim>::cell_iterator cell) const
+    {
+        std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
+        cell->get_dof_indices(i);
+        double d_x,d_y,d_z;
+        std::set<unsigned int> relevant_cells;
+        auto cell_index = i[cell->get_fe().component_to_system_index(0, 0)];
+        if(cell->is_locally_owned())
+        {
+            for (unsigned int i=0; i < x_coord.size(); i++)
+            {
+                d_x = std::abs(x_coord[cell_index]-x_coord[i]);
+
+                if (d_x < Input::filter_r)
+                {
+                    d_y = std::abs(y_coord[cell_index]-y_coord[i]);
+
+                    if ((d_x*d_x + d_y*d_y) < (Input::filter_r*Input::filter_r))
+                    {
+
+                        if (dim == 3)
                         {
-                            cells_to_check.insert(neighbor);
-                            neighbor_ids.insert(neighbor->id());
-                            new_neighbors_found = true;
+                            d_z = std::abs(z_coord[cell_index]-z_coord[i]);
+
+                            if ((d_x*d_x + d_y*d_y + d_z*d_z) < (Input::filter_r*Input::filter_r))
+                            {
+                                relevant_cells.insert(i);
+                            }
+                        }
+                        else
+                        {
+                            relevant_cells.insert(i);
                         }
+
                     }
                 }
+
             }
-        } while (new_neighbors_found);
-        return cells_to_check;
+        }
+        else
+        {
+             throw;
+        }
     }
 
 }//SAND namespace
diff --git a/source/schur_preconditioner.cc.autosave b/source/schur_preconditioner.cc.autosave
deleted file mode 100644
index 0b4878c..0000000
--- a/source/schur_preconditioner.cc.autosave
+++ /dev/null
@@ -1,709 +0,0 @@
-//
-// Created by justin on 2/17/21.
-//
-#include <deal.II/lac/linear_operator.h>
-#include <deal.II/lac/sparse_direct.h>
-#include <deal.II/lac/sparse_matrix.h>
-#include <deal.II/base/timer.h>
-#include <deal.II/lac/generic_linear_algebra.h>
-#include <deal.II/lac/trilinos_solver.h>
-#include <deal.II/lac/trilinos_precondition.h>
-#include "../include/schur_preconditioner.h"
-#include "../include/input_information.h"
-#include "../include/sand_tools.h"
-#include <fstream>
-
-namespace SAND {
-    using MatrixType  = dealii::TrilinosWrappers::SparseMatrix;
-    using VectorType  = dealii::TrilinosWrappers::MPI::Vector;
-    using PayloadType = dealii::TrilinosWrappers::internal::LinearOperatorImplementation::TrilinosPayload;
-    using PayloadVectorType = typename PayloadType::VectorType;
-    using size_type         = dealii::types::global_dof_index;
-    using namespace dealii;
-    template<int dim>
-    TopOptSchurPreconditioner<dim>::TopOptSchurPreconditioner(LA::MPI::BlockSparseMatrix &matrix_in)
-            :
-            system_matrix(matrix_in),
-            mpi_communicator(MPI_COMM_WORLD),
-            n_rows(0),
-            n_columns(0),
-            n_block_rows(0),
-            n_block_columns(0),
-            other_solver_control(100000, 1e-6),
-            other_bicgstab(other_solver_control),
-            other_gmres(other_solver_control),
-            other_cg(other_solver_control),
-            a_mat(matrix_in.block(SolutionBlocks::displacement, SolutionBlocks::displacement_multiplier)),
-            b_mat(matrix_in.block(SolutionBlocks::density, SolutionBlocks::density)),
-            c_mat(matrix_in.block(SolutionBlocks::displacement,SolutionBlocks::density)),
-            e_mat(matrix_in.block(SolutionBlocks::displacement_multiplier,SolutionBlocks::density)),
-            f_mat(matrix_in.block(SolutionBlocks::unfiltered_density_multiplier,SolutionBlocks::unfiltered_density)),
-            d_m_mat(matrix_in.block(SolutionBlocks::density_upper_slack_multiplier, SolutionBlocks::density_upper_slack)),
-            d_1_mat(matrix_in.block(SolutionBlocks::density_lower_slack, SolutionBlocks::density_lower_slack)),
-            d_2_mat(matrix_in.block(SolutionBlocks::density_upper_slack, SolutionBlocks::density_upper_slack)),
-            m_vect(matrix_in.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier)),
-            solver_type("Amesos_Klu"),
-            additional_data(false, solver_type),
-            direct_solver_control(1, 0),
-            a_inv_direct(direct_solver_control, additional_data),
-            pcout(std::cout,(Utilities::MPI::this_mpi_process(mpi_communicator) == 0)),
-            timer(pcout, TimerOutput::summary, TimerOutput::wall_times)
-    {
-
-    }
-
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::initialize(LA::MPI::BlockSparseMatrix &matrix, const std::map<types::global_dof_index, double> &boundary_values,const DoFHandler<dim> &dof_handler, const LA::MPI::BlockVector &distributed_state)
-    {
-        TimerOutput::Scope t(timer, "initialize");
-        {
-
-            TimerOutput::Scope t(timer, "diag stuff");
-            const types::global_dof_index disp_start_index = system_matrix.get_row_indices().block_start(
-                    SolutionBlocks::displacement);
-            const types::global_dof_index disp_mult_start_index = system_matrix.get_row_indices().block_start(
-                    SolutionBlocks::displacement_multiplier);
-            for (auto&[dof_index, boundary_value]: boundary_values) {
-
-                const types::global_dof_index n_u = system_matrix.block(SolutionBlocks::displacement,
-                                                                        SolutionBlocks::displacement).m();
-                if ((dof_index >= disp_start_index) && (dof_index < disp_start_index + n_u)) {
-                    double diag_val = system_matrix.block(SolutionBlocks::displacement,
-                                                          SolutionBlocks::displacement).el(
-                            dof_index - disp_start_index, dof_index - disp_start_index);
-                    system_matrix.block(SolutionBlocks::displacement, SolutionBlocks::displacement_multiplier).set(
-                            dof_index - disp_start_index, dof_index - disp_start_index, diag_val);
-                } else if ((dof_index >= disp_mult_start_index) && (dof_index < disp_mult_start_index + n_u)) {
-                    double diag_val = system_matrix.block(SolutionBlocks::displacement_multiplier,
-                                                          SolutionBlocks::displacement_multiplier).el(
-                            dof_index - disp_mult_start_index, dof_index - disp_mult_start_index);
-                    system_matrix.block(SolutionBlocks::displacement_multiplier, SolutionBlocks::displacement).set(
-                            dof_index - disp_mult_start_index, dof_index - disp_mult_start_index, diag_val);
-                }
-            }
-
-            //set diagonal to 0?
-            for (auto&[dof_index, boundary_value]: boundary_values) {
-                const types::global_dof_index disp_start_index = system_matrix.get_row_indices().block_start(
-                        SolutionBlocks::displacement);
-                const types::global_dof_index disp_mult_start_index = system_matrix.get_row_indices().block_start(
-                        SolutionBlocks::displacement_multiplier);
-                const types::global_dof_index n_u = system_matrix.block(SolutionBlocks::displacement,
-                                                                        SolutionBlocks::displacement).m();
-                if ((dof_index >= disp_start_index) && (dof_index < disp_start_index + n_u)) {
-                    system_matrix.block(SolutionBlocks::displacement, SolutionBlocks::displacement).set(
-                            dof_index - disp_start_index, dof_index - disp_start_index, 0);
-                } else if ((dof_index >= disp_mult_start_index) && (dof_index < disp_mult_start_index + n_u)) {
-                    system_matrix.block(SolutionBlocks::displacement_multiplier,
-                                        SolutionBlocks::displacement_multiplier).set(
-                            dof_index - disp_mult_start_index, dof_index - disp_mult_start_index, 0);
-                }
-            }
-
-//            system_matrix.compress(VectorOperation::insert);
-
-//            const unsigned int m = a_mat.m();
-//            const unsigned int n = a_mat.n();
-//            std::ofstream Xmat("a_mat_par.csv");
-//            for (unsigned int i = 0; i < m; i++)
-//            {
-//                Xmat << a_mat.el(i, 0);
-//                for (unsigned int j = 1; j < n; j++)
-//                {
-//                    Xmat << "," << a_mat.el(i, j);
-//                }
-//                Xmat << "\n";
-//            }
-//            Xmat.close();
-
-        }
-        if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
-        {
-
-        }
-        else
-        {
-            TimerOutput::Scope t(timer, "build A inv");
-
-//            if (Utilities::MPI::this_mpi_process(mpi_communicator) == 0)
-//            {
-//                std::string name = "a_mat";
-//                print_matrix(name, a_mat);
-//            }
-
-            a_inv_direct.initialize(a_mat);
-        }
-        {
-            TimerOutput::Scope t(timer, "reinit diag matrices");
-            d_3_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
-            d_4_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
-            d_5_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
-            d_6_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
-            d_7_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
-            d_8_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
-            d_m_inv_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
-            d_3_mat=0;
-            d_4_mat=0;
-            d_5_mat=0;
-            d_6_mat=0;
-            d_7_mat=0;
-            d_8_mat=0;
-            d_m_inv_mat=0;
-            pcout << "diag reinit" << std::endl;
-        }
-        {
-            const types::global_dof_index n_p = system_matrix.block(SolutionBlocks::density,
-                                                                    SolutionBlocks::density).m();
-
-            pcout << "np: " << n_p << std::endl;
-
-
-
-
-            std::vector<double> l_global(n_p);
-            std::vector<double> lm_global(n_p);
-            std::vector<double> u_global(n_p);
-            std::vector<double> um_global(n_p);
-            std::vector<double> m_global(n_p);
-
-            std::vector<double> l(n_p);
-            std::vector<double> lm(n_p);
-            std::vector<double> u(n_p);
-            std::vector<double> um(n_p);
-            std::vector<double> m(n_p);
-
-            TimerOutput::Scope t(timer, "build diag matrices");
-            for (const auto cell: dof_handler.active_cell_iterators())
-            {
-                if(cell->is_locally_owned())
-                {
-                    std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
-                    cell->get_dof_indices(i);
-
-
-
-                    const int i_ind = cell->get_fe().component_to_system_index(0,0);
-
-                    if(distributed_state.block(SolutionBlocks::density_lower_slack_multiplier).in_local_range(i[i_ind]))
-                    {
-                        lm[i[i_ind]] = distributed_state.block(SolutionBlocks::density_lower_slack_multiplier)[i[i_ind]];
-                        m[i[i_ind]] = cell->measure();
-                    }
-
-                    if(distributed_state.block(SolutionBlocks::density_lower_slack).in_local_range(i[i_ind]))
-                    {
-                        l[i[i_ind]] =  distributed_state.block(SolutionBlocks::density_lower_slack)[i[i_ind]];
-                    }
-
-                    if(distributed_state.block(SolutionBlocks::density_upper_slack_multiplier).in_local_range(i[i_ind]))
-                    {
-                        um[i[i_ind]]= distributed_state.block(SolutionBlocks::density_upper_slack_multiplier)[i[i_ind]];
-                    }
-
-                    if(distributed_state.block(SolutionBlocks::density_upper_slack).in_local_range(i[i_ind]))
-                    {
-                        u[i[i_ind]] = distributed_state.block(SolutionBlocks::density_upper_slack)[i[i_ind]];
-                    }
-                }
-            }
-
-            MPI_Allreduce(lm.data(), lm_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-            MPI_Allreduce(l.data(), l_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-            MPI_Allreduce(um.data(), um_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-            MPI_Allreduce(u.data(), u_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-            MPI_Allreduce(m.data(), m_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-
-            for (unsigned int k=0; k< n_p; k++)
-            {
-                    if(distributed_state.block(0).in_local_range(k))
-                    {
-                        d_3_mat.set(k, k, -1 * lm_global[k]/(m_global[k]*l_global[k]));
-                        d_4_mat.set(k, k, -1 * um_global[k]/(m_global[k]*u_global[k]));
-                        d_5_mat.set(k, k, lm_global[k]/l_global[k]);
-                        d_6_mat.set(k, k, um_global[k]/u_global[k]);
-                        d_7_mat.set(k, k, m_global[k]*(lm_global[k]*u_global[k] + um_global[k]*l_global[k])/(l_global[k]*u_global[k]));
-                        d_8_mat.set(k, k, l_global[k]*u_global[k]/(m_global[k]*(lm_global[k]*u_global[k] + um_global[k]*l_global[k])));
-                        d_m_inv_mat.set(k, k, 1 / m_global[k]);
-                    }
-            }
-        }
-        d_3_mat.compress(VectorOperation::insert);
-        d_4_mat.compress(VectorOperation::insert);
-        d_5_mat.compress(VectorOperation::insert);
-        d_6_mat.compress(VectorOperation::insert);
-        d_7_mat.compress(VectorOperation::insert);
-        d_8_mat.compress(VectorOperation::insert);
-        d_m_inv_mat.compress(VectorOperation::insert);
-
-        pcout << "compressed" << std::endl;
-
-        pre_j=distributed_state.block(SolutionBlocks::density);
-        pre_k=distributed_state.block(SolutionBlocks::density);
-        g_d_m_inv_density=distributed_state.block(SolutionBlocks::density);
-        k_g_d_m_inv_density=distributed_state.block(SolutionBlocks::density);
-
-
-        op_d_8 = linear_operator<VectorType,VectorType,PayloadType>(d_8_mat);
-        op_f = linear_operator<VectorType,VectorType,PayloadType>(f_mat);
-        op_b = linear_operator<VectorType,VectorType,PayloadType>(b_mat);
-        op_c = linear_operator<VectorType,VectorType,PayloadType>(c_mat);
-        op_a_inv = linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct);
-        op_e = linear_operator<VectorType,VectorType,PayloadType>(e_mat);
-        op_d_m= linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
-        op_d_m_inv= linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat);
-
-        op_g = op_f * op_d_8 *
-                transpose_operator(op_f);
-
-
-        op_h = op_b
-               - transpose_operator(op_c) * op_a_inv * op_e
-               - transpose_operator(op_e) * op_a_inv * op_c;
-
-//        TrilinosWrappers::internal::LinearOperatorImplementation::TrilinosPayload inv_payload = inverse_payload(TrilinosWrappers::SolverGMRES(),TrilinosWrappers::PreconditionIdentity());
-
-        op_k_inv = (-1 * op_g *op_d_m_inv * op_h - op_d_m);
-
-
-
-
-    }
-
-
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::vmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
-        LA::MPI::BlockVector temp_src;
-        pcout << "vmult" << std::endl;
-        {
-            TimerOutput::Scope t(timer, "part 1");
-            vmult_step_1(dst, src);
-            temp_src = dst;
-        }
-
-        {
-            TimerOutput::Scope t(timer, "part 2");
-            vmult_step_2(dst, temp_src);
-            temp_src = dst;
-        }
-
-        {
-            TimerOutput::Scope t(timer, "part 3");
-            vmult_step_3(dst, temp_src);
-            temp_src = dst;
-        }
-        {
-            TimerOutput::Scope t(timer, "part 4");
-            vmult_step_4(dst, temp_src);
-            temp_src = dst;
-        }
-        vmult_step_5(dst, temp_src);        
-    }
-
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::Tvmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
-        dst = src;
-    }
-
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::vmult_add(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
-        LA::MPI::BlockVector dst_temp = dst;
-        vmult(dst_temp, src);
-        dst += dst_temp;
-    }
-
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::Tvmult_add(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
-        dst = src;
-    }
-
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::vmult_step_1(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
-        dst = src;
-        auto dst_temp = dst;
-        dst.block(SolutionBlocks::unfiltered_density) = dst_temp.block(SolutionBlocks::unfiltered_density)
-                - linear_operator<VectorType,VectorType,PayloadType>(d_5_mat) * src.block(SolutionBlocks::density_lower_slack_multiplier)
-                + linear_operator<VectorType,VectorType,PayloadType>(d_6_mat) * src.block(SolutionBlocks::density_upper_slack_multiplier)
-                + src.block(SolutionBlocks::density_lower_slack)
-                - src.block(SolutionBlocks::density_upper_slack);
-    }
-
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::vmult_step_2(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
-        dst = src;
-        auto dst_temp = dst;
-
-        dst.block(SolutionBlocks::unfiltered_density_multiplier) = dst_temp.block(SolutionBlocks::unfiltered_density_multiplier)
-                - linear_operator<VectorType,VectorType,PayloadType>(f_mat)*linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) * src.block(SolutionBlocks::unfiltered_density);
-
-    }
-
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::vmult_step_3(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
-        dst = src;
-
-        if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
-        {
-            SolverControl            solver_control_1(100000, 1e-6);
-            SolverControl            solver_control_2(100000, 1e-6);
-            SolverCG<LA::MPI::Vector> a_solver_cg_1(solver_control_1);
-            SolverCG<LA::MPI::Vector> a_solver_cg_2(solver_control_2);
-            auto dst_temp = dst;
-            a_solver_cg_1.solve(a_mat,dst_temp.block(SolutionBlocks::displacement_multiplier),src.block(SolutionBlocks::displacement_multiplier),PreconditionIdentity());
-            a_solver_cg_2.solve(a_mat,dst_temp.block(SolutionBlocks::displacement),src.block(SolutionBlocks::displacement),PreconditionIdentity());
-            c_mat.Tvmult(dst_temp.block(SolutionBlocks::density_upper_slack),dst_temp.block(SolutionBlocks::displacement_multiplier));
-            e_mat.Tvmult(dst_temp.block(SolutionBlocks::density_lower_slack),dst_temp.block(SolutionBlocks::displacement));
-
-            dst.block(SolutionBlocks::density) = dst_temp.block(SolutionBlocks::density) - dst_temp.block(SolutionBlocks::density_upper_slack) - dst_temp.block(SolutionBlocks::density_lower_slack);
-
-
-        }
-        else
-        {
-            auto dst_temp = dst;
-            dst.block(SolutionBlocks::density) = dst_temp.block(SolutionBlocks::density)
-                    - transpose_operator<VectorType,VectorType,PayloadType>(e_mat) * linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct)* src.block(SolutionBlocks::displacement)
-                    - transpose_operator<VectorType,VectorType,PayloadType>(c_mat) * linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct)* src.block(SolutionBlocks::displacement_multiplier);
-        }
-
-    }
-
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::vmult_step_4(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
-        dst = src;
-        auto dst_temp = dst;
-        auto k_density_mult =  src.block(SolutionBlocks::density);
-
-        if (Input::solver_choice == SolverOptions::exact_preconditioner_with_gmres)
-        {
-//            g_d_m_inv_density = linear_operator<VectorType,VectorType,PayloadType>(g_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
-//            k_g_d_m_inv_density = linear_operator<VectorType,VectorType,PayloadType>(k_mat) * g_d_m_inv_density;
-//            k_density_mult = linear_operator<VectorType,VectorType,PayloadType>(k_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
-        }
-
-        else if (Input::solver_choice == SolverOptions::inexact_K_with_exact_A_gmres)
-        {
-
-            g_d_m_inv_density = op_g * op_d_m_inv * src.block(SolutionBlocks::density);
-            SolverControl step_4_gmres_control_1 (100000, g_d_m_inv_density.l2_norm()*1e-6);
-            SolverGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
-            try {
-                TimerOutput::Scope u(timer, "actual inv");
-//                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
-//                                      g_d_m_inv_density;
-                step_4_gmres_1.solve(op_k_inv,k_g_d_m_inv_density,g_d_m_inv_density, PreconditionIdentity() );
-            }
-            catch (std::exception &exc)
-            {
-                std::cerr << "Failure of linear solver step_4_gmres_1" << std::endl;
-                pcout << "first residual: " << step_4_gmres_control_1.initial_value() << std::endl;
-                pcout << "last residual: " << step_4_gmres_control_1.last_value() << std::endl;
-                throw;
-            }
-            SolverControl step_4_gmres_control_2 (100000, src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6);
-            SolverGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
-            try {
-                TimerOutput::Scope u(timer, "actual inv");
-//                k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, PreconditionIdentity()) *
-//                                 src.block(SolutionBlocks::unfiltered_density_multiplier);
-                step_4_gmres_2.solve(op_k_inv,k_density_mult,src.block(SolutionBlocks::unfiltered_density_multiplier), PreconditionIdentity() );
-            } catch (std::exception &exc)
-            {
-                std::cerr << "Failure of linear solver step_4_gmres_2" << std::endl;
-                pcout << "first residual: " << step_4_gmres_control_2.initial_value() << std::endl;
-                pcout << "last residual: " << step_4_gmres_control_2.last_value() << std::endl;
-                throw;
-            }
-        }
-        else if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
-        {
-
-            SolverControl            solver_control(100000, 1e-6);
-            SolverCG<LA::MPI::Vector> a_solver_cg(solver_control);
-
-            auto preconditioner = PreconditionIdentity();
-
-            auto a_inv_op = op_d_8;
-
-                    inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg, preconditioner);
-
-
-            auto op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) *
-                        transpose_operator<VectorType, VectorType, PayloadType>(f_mat);
-
-            auto op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
-                        - transpose_operator<VectorType, VectorType, PayloadType>(c_mat) * a_inv_op * linear_operator<VectorType,VectorType,PayloadType>(e_mat)
-                        - transpose_operator<VectorType, VectorType, PayloadType>(e_mat) * a_inv_op * linear_operator<VectorType,VectorType,PayloadType>(c_mat);
-
-            auto op_k_inv = op_d_8;
-
-                    -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
-
-            SolverControl step_4_gmres_control_1 (10000, std::max(g_d_m_inv_density.l2_norm()*1e-6,1e-6));
-            SolverGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
-            try {
-//                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
-//                                      g_d_m_inv_density;
-//                step_4_gmres_1.solve(op_k_inv,k_g_d_m_inv_density,g_d_m_inv_density, PreconditionIdentity() );
-            } catch (std::exception &exc)
-            {
-                std::cerr << "Failure of linear solver step_4_gmres_1" << std::endl;
-                pcout << "first residual: " << step_4_gmres_control_1.initial_value() << std::endl;
-                pcout << "last residual: " << step_4_gmres_control_1.last_value() << std::endl;
-                throw;
-            }
-
-            SolverControl step_4_gmres_control_2 (10000, std::max(src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6,1e-6));
-            SolverGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
-            try {
-
-                k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, PreconditionIdentity()) *
-                                 src.block(SolutionBlocks::unfiltered_density_multiplier);
-            } catch (std::exception &exc)
-            {
-                std::cerr << "Failure of linear solver step_4_gmres_2" << std::endl;
-                pcout << "first residual: " << step_4_gmres_control_2.initial_value() << std::endl;
-                pcout << "last residual: " << step_4_gmres_control_2.last_value() << std::endl;
-                throw;
-            }
-        }
-        else
-        {
-            pcout << "shouldn't get here";
-            throw;
-        }
-
-        dst.block(SolutionBlocks::total_volume_multiplier) = transpose_operator<VectorType, VectorType, PayloadType>(m_vect)*k_g_d_m_inv_density
-                                                        - transpose_operator<VectorType, VectorType, PayloadType>(m_vect)*k_density_mult
-                                                        +dst_temp.block(SolutionBlocks::total_volume_multiplier);
-    }
-
-
-
-
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::vmult_step_5(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
-        {
-            //First Block Inverse
-            TimerOutput::Scope t(timer, "inverse 1");
-            dst.block(SolutionBlocks::density_lower_slack_multiplier) = linear_operator<VectorType,VectorType,PayloadType>(d_3_mat) * src.block(SolutionBlocks::density_lower_slack_multiplier) +
-                    linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density_lower_slack);
-            dst.block(SolutionBlocks::density_upper_slack_multiplier) = linear_operator<VectorType,VectorType,PayloadType>(d_4_mat) * src.block(SolutionBlocks::density_upper_slack_multiplier) +
-                    linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density_upper_slack);
-            dst.block(SolutionBlocks::density_lower_slack) = linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density_lower_slack_multiplier);
-            dst.block(SolutionBlocks::density_upper_slack) = linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density_upper_slack_multiplier);
-        }
-
-
-        {
-            //Second Block Inverse
-            TimerOutput::Scope t(timer, "inverse 2");
-            dst.block(SolutionBlocks::unfiltered_density) =
-                    linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) * src.block(SolutionBlocks::unfiltered_density);
-        }
-
-
-        {
-            //Third Block Inverse
-            TimerOutput::Scope t(timer, "inverse 3");
-            if(Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
-            {
-                SolverControl            solver_control(100000, 1e-6);
-                SolverCG<LA::MPI::Vector> a_solver_cg(solver_control);
-
-                auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg, PreconditionIdentity());
-
-                dst.block(SolutionBlocks::displacement) = a_inv_op * src.block(SolutionBlocks::displacement_multiplier);
-                dst.block(SolutionBlocks::displacement_multiplier) = a_inv_op * src.block(SolutionBlocks::displacement);
-            } else
-            {
-                dst.block(SolutionBlocks::displacement) = linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct) * src.block(SolutionBlocks::displacement_multiplier);
-                dst.block(SolutionBlocks::displacement_multiplier) = linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct) * src.block(SolutionBlocks::displacement);
-            }
-
-        }
-
-
-        {
-            //Fourth (ugly) Block Inverse
-            TimerOutput::Scope t(timer, "inverse 4");
-
-
-
-            if (Input::solver_choice == SolverOptions::exact_preconditioner_with_gmres)
-            {
-//                pre_j = src.block(SolutionBlocks::density) + linear_operator<VectorType,VectorType,PayloadType>(h_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
-//                pre_k = -1* linear_operator<VectorType,VectorType,PayloadType>(g_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density) + src.block(SolutionBlocks::unfiltered_density_multiplier);
-//                dst.block(SolutionBlocks::unfiltered_density_multiplier) = transpose_operator<VectorType, VectorType, PayloadType>(k_mat) * pre_j;
-//                dst.block(SolutionBlocks::density) = linear_operator<VectorType,VectorType,PayloadType>(k_mat) * pre_k;
-            }
-
-            else if (Input::solver_choice == SolverOptions::inexact_K_with_exact_A_gmres)
-            {
-                auto pre_pre_k = pre_k;
-                {
-                    TimerOutput::Scope t(timer, "not inverse 5.1");
-                    pre_j = src.block(SolutionBlocks::density) + op_h * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
-                    pre_pre_k = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
-                    pre_k =  pre_pre_k + src.block(SolutionBlocks::unfiltered_density_multiplier);
-                }
-                SolverControl step_5_gmres_control_1 (100000, pre_j.l2_norm()*1e-6);
-                SolverGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
-                try {
-                    TimerOutput::Scope t(timer, "actual inverse 5.1");
-//                    dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
-//                                                                               pre_j;
-                    step_5_gmres_1.solve(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), dst.block(SolutionBlocks::unfiltered_density_multiplier), pre_j , PreconditionIdentity());
-                } catch (std::exception &exc)
-                {
-                    std::cerr << "Failure of linear solver step_5_gmres_1 again" << std::endl;
-                    pcout << "first residual: " << step_5_gmres_control_1.initial_value() << std::endl;
-                    pcout << "last residual: " << step_5_gmres_control_1.last_value() << std::endl;
-                    throw;
-                }
-
-
-                SolverControl step_5_gmres_control_2 (100000, pre_k.l2_norm()*1e-6);
-                SolverGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
-                try {
-                    TimerOutput::Scope t(timer, "actual inverse 5.2");
-//                    dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) * pre_k;
-                    step_5_gmres_2.solve(op_k_inv,dst.block(SolutionBlocks::density), pre_k , PreconditionIdentity());
-                } catch (std::exception &exc)
-                {
-                    std::cerr << "Failure of linear solver step_5_gmres_2" << std::endl;
-                    pcout << "first residual: " << step_5_gmres_control_2.initial_value() << std::endl;
-                    pcout << "last residual: " << step_5_gmres_control_2.last_value() << std::endl;
-                    throw;
-                }
-            }
-            else if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
-            {
-
-                SolverControl            solver_control(100000, 1e-6);
-                SolverCG<LA::MPI::Vector> a_solver_cg (solver_control);
-                auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg, PreconditionIdentity());
-
-                auto op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) *
-                            transpose_operator<VectorType, VectorType, PayloadType>(f_mat);
-
-                auto op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
-                            - transpose_operator<VectorType, VectorType, PayloadType>(c_mat) * a_inv_op * linear_operator<VectorType,VectorType,PayloadType>(e_mat)
-                            - transpose_operator<VectorType, VectorType, PayloadType>(e_mat) * a_inv_op * linear_operator<VectorType,VectorType,PayloadType>(c_mat);
-
-                auto op_k_inv = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
-
-                pre_j = src.block(SolutionBlocks::density) + op_h * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
-                pre_k = -1* op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density) + src.block(SolutionBlocks::unfiltered_density_multiplier);
-
-                SolverControl step_5_gmres_control_1 (10000, std::max(pre_j.l2_norm()*1e-6,1e-6));
-                TrilinosWrappers::SolverGMRES step_5_gmres_1 (step_5_gmres_control_1);
-                try {
-                    dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType,VectorType,PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
-                                                                               pre_j;
-                } catch (std::exception &exc)
-                {
-                    std::cerr << "Failure of linear solver step_5_gmres_1" << std::endl;
-                    pcout << "first residual: " << step_5_gmres_control_1.initial_value() << std::endl;
-                    pcout << "last residual: " << step_5_gmres_control_1.last_value() << std::endl;
-                    throw;
-                }
-
-                SolverControl step_5_gmres_control_2 (10000, std::max(pre_k.l2_norm()*1e-6,1e-6));
-                TrilinosWrappers::SolverGMRES step_5_gmres_2 (step_5_gmres_control_2);
-                try {
-                    dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) *
-                                                         pre_k;
-                } catch (std::exception &exc)
-                {
-                    std::cerr << "Failure of linear solver step_5_gmres_2" << std::endl;
-                    pcout << "first residual: " << step_5_gmres_control_2.initial_value() << std::endl;
-                    pcout << "last residual: " << step_5_gmres_control_2.last_value() << std::endl;
-                    throw;
-                }
-            }
-            else
-            {
-                pcout << "shouldn't get here";
-                throw;
-            }
-
-        }
-        {
-            dst.block(SolutionBlocks::total_volume_multiplier) = src.block(SolutionBlocks::total_volume_multiplier);
-        }
-    }
-
-
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::print_stuff()
-    {
-
-//        print_matrix(std::string("OAmat.csv"),a_mat);
-//        print_matrix(std::string("OBmat.csv"),b_mat);
-//        print_matrix(std::string("OCmat.csv"),c_mat);
-//        print_matrix(std::string("OEmat.csv"),e_mat);
-//        print_matrix(std::string("OFmat.csv"),f_mat);
-        FullMatrix<double> g_mat;
-        FullMatrix<double> h_mat;
-        FullMatrix<double> k_inv_mat;
-        g_mat.reinit(b_mat.m(),b_mat.n());
-        h_mat.reinit(b_mat.m(),b_mat.n());
-        k_inv_mat.reinit(b_mat.m(),b_mat.n());
-        auto op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) *
-                    transpose_operator(linear_operator<VectorType,VectorType,PayloadType>(f_mat));
-
-        auto op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
-                    - transpose_operator(linear_operator<VectorType,VectorType,PayloadType>(c_mat)) * linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct) * linear_operator<VectorType,VectorType,PayloadType>(e_mat)
-                    - transpose_operator(linear_operator<VectorType,VectorType,PayloadType>(e_mat)) * linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct) * linear_operator<VectorType,VectorType,PayloadType>(c_mat);
-
-        auto op_k_inv = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
-//        build_matrix_element_by_element(op_g,g_mat, src.block(SolutionBlocks::density));
-//        build_matrix_element_by_element(op_h,h_mat, src.block(SolutionBlocks::density));
-//        build_matrix_element_by_element(op_k_inv,k_inv_mat, src.block(SolutionBlocks::density));
-//        print_matrix(std::string("OGmat.csv"),g_mat);
-//        print_matrix(std::string("OHmat.csv"),h_mat);
-//        print_matrix(std::string("OKinvmat.csv"),k_inv_mat);
-
-
-    }
-
-    void VmultTrilinosSolverDirect::initialize(LA::MPI::SparseMatrix &a_mat)
-    {
-        reinit(a_mat);
-        solver_direct.initialize(a_mat);
-        size = a_mat.n();
-    }
-
-    void
-    VmultTrilinosSolverDirect::vmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
-    {
-        solver_direct.solve(dst, src);
-    }
-
-    void VmultTrilinosSolverDirect::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
-    {
-        solver_direct.solve(dst, src);
-    }
-
-    void
-    VmultTrilinosSolverDirect::Tvmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
-    {
-        solver_direct.solve(dst, src);
-    }
-
-    void VmultTrilinosSolverDirect::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
-    {
-        solver_direct.solve(dst, src);
-    }
-
-
-    VmultTrilinosSolverDirect::VmultTrilinosSolverDirect(SolverControl &cn,
-                     const TrilinosWrappers::SolverDirect::AdditionalData &data)
-    : solver_direct(cn, data)
-    {
-
-    }
-
-}
-template class SAND::TopOptSchurPreconditioner<2>;
-template class SAND::TopOptSchurPreconditioner<3>;

From 7671d0c9de8173ae44828923c490276cea08adde Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Tue, 25 Jan 2022 14:55:53 -0700
Subject: [PATCH 62/95] here

---
 include/density_filter.h       |  2 +-
 source/density_filter.cc       | 49 ++++++++++++++++++----------------
 source/schur_preconditioner.cc | 11 ++++----
 3 files changed, 32 insertions(+), 30 deletions(-)

diff --git a/include/density_filter.h b/include/density_filter.h
index 44ed718..c98fe90 100644
--- a/include/density_filter.h
+++ b/include/density_filter.h
@@ -67,7 +67,7 @@ namespace SAND {
         LA::MPI::SparseMatrix filter_matrix;
         SparsityPattern filter_sparsity_pattern;
         void initialize(DoFHandler<dim> &dof_handler);
-        std::set<unsigned int> find_relevant_neighbors(typename DoFHandler<dim>::cell_iterator cell) const;
+        std::set<types::global_dof_index> find_relevant_neighbors(types::global_dof_index cell_index) const;
 
     private:
         LA::MPI::Vector cell_m;
diff --git a/source/density_filter.cc b/source/density_filter.cc
index bc0bb20..4cef2f9 100644
--- a/source/density_filter.cc
+++ b/source/density_filter.cc
@@ -49,16 +49,19 @@ namespace SAND {
         /*finds neighbors whose values would be relevant, and adds them to the sparsity pattern of the matrix*/
          for (const auto &cell : dof_handler.active_cell_iterators())
          {
+
+
              if(cell->is_locally_owned())
              {
                 std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
                 cell->get_dof_indices(i);
-                x_coord.set(i[cell->get_fe().component_to_system_index(0, 0)],cell->center(0)) ;
-                y_coord.set(i[cell->get_fe().component_to_system_index(0, 0)],cell->center(1)) ;
-                cell_m.set(i[cell->get_fe().component_to_system_index(0, 0)],cell->measure());
+                const int i_ind = cell->get_fe().component_to_system_index(0, 0);
+                x_coord.set(i[i_ind],cell->center()[0]) ;
+                y_coord.set(i[i_ind],cell->center()[1]) ;
+                cell_m.set(i[i_ind],cell->measure());
                 if (dim==3)
                 {
-                    z_coord.set(i[cell->get_fe().component_to_system_index(0, 0)],cell->center(2)) ;
+                    z_coord.set(i[i_ind],cell->center()[2]) ;
                 }
              }
          }
@@ -72,9 +75,9 @@ namespace SAND {
             {
                 std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
                 cell->get_dof_indices(i);
-                for (const auto &neighbor_cell_index : find_relevant_neighbors(cell))
+                for (const auto &neighbor_cell_index : find_relevant_neighbors(i[cell->get_fe().component_to_system_index(0, 0)]))
                 {
-                    filter_dsp.set(i[cell->get_fe().component_to_system_index(0, 0)], neighbor_cell_index);
+                    filter_dsp.add(i[cell->get_fe().component_to_system_index(0, 0)], neighbor_cell_index);
                 }
             }
         }
@@ -90,15 +93,17 @@ namespace SAND {
         {
             if(cell->is_locally_owned())
             {
-                std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
+                std::vector<unsigned int> i(cell->get_fe().n_dofs_per_cell());
                 cell->get_dof_indices(i);
-                for (const auto &neighbor_cell_index : find_relevant_neighbors(cell)) {
-                    double d_x = std::abs(x_coord[i[cell->get_fe().component_to_system_index(0, 0)]]-x_coord[neighbor_cell_index]);
-                    double d_y = std::abs(y_coord[i[cell->get_fe().component_to_system_index(0, 0)]]-y_coord[neighbor_cell_index]);
+                const int i_ind = &cell->get_fe().component_to_system_index(0, 0);
+                for (const auto &neighbor_cell_index : find_relevant_neighbors(i[i_ind]))
+                {
+                    double d_x = std::abs(x_coord[i[i_ind]]-x_coord[neighbor_cell_index]);
+                    double d_y = std::abs(y_coord[i[i_ind]]-y_coord[neighbor_cell_index]);
                     double d;
                     if (dim==3)
                     {
-                        double d_z = std::abs(z_coord[i[cell->get_fe().component_to_system_index(0, 0)]]-z_coord[neighbor_cell_index]);
+                        double d_z = std::abs(z_coord[i[i_ind]]-z_coord[neighbor_cell_index]);
                         d = std::pow(d_x*d_x + d_y*d_y + d_z*d_z , .5);
                     }
                     else
@@ -107,7 +112,7 @@ namespace SAND {
                     }
                     /*value should be (max radius - distance between cells)*cell measure */
                     double value = (Input::filter_r - d)*cell_m[neighbor_cell_index];
-                    filter_matrix.add(i[cell->get_fe().component_to_system_index(0, 0)], neighbor_cell_index, value);
+                    filter_matrix.add(i[i_ind], neighbor_cell_index, value);
                 }
             }
         }
@@ -119,15 +124,16 @@ namespace SAND {
             {
                 std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
                 cell->get_dof_indices(i);
+                const int i_ind = cell->get_fe().component_to_system_index(0, 0);
                 double denominator = 0;
                 typename LA::MPI::SparseMatrix::iterator iter = filter_matrix.begin(
-                        i[cell->get_fe().component_to_system_index(0, 0)]);
-                for (; iter != filter_matrix.end(i[cell->get_fe().component_to_system_index(0, 0)]); iter++)
+                        i[i_ind]);
+                for (; iter != filter_matrix.end(i[i_ind]); iter++)
                 {
                     denominator = denominator + iter->value();
                 }
-                iter = filter_matrix.begin(i[cell->get_fe().component_to_system_index(0, 0)]);
-                for (; iter != filter_matrix.end(i[cell->get_fe().component_to_system_index(0, 0)]); iter++)
+                iter = filter_matrix.begin(i[i_ind]);
+                for (; iter != filter_matrix.end(i[i_ind]); iter++)
                 {
                     iter->value() = iter->value() / denominator;
                 }
@@ -138,15 +144,12 @@ namespace SAND {
 
     /*This function finds which neighbors are within a certain radius of the initial cell.*/
     template<int dim>
-    std::set<unsigned int>
-    DensityFilter<dim>::find_relevant_neighbors(typename DoFHandler<dim>::cell_iterator cell) const
+    std::set<types::global_dof_index>
+    DensityFilter<dim>::find_relevant_neighbors(types::global_dof_index cell_index) const
     {
-        std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
-        cell->get_dof_indices(i);
         double d_x,d_y,d_z;
-        std::set<unsigned int> relevant_cells;
-        auto cell_index = i[cell->get_fe().component_to_system_index(0, 0)];
-        if(cell->is_locally_owned())
+        std::set<types::global_dof_index> relevant_cells;
+        if(x_coord.in_local_range(cell_index))
         {
             for (unsigned int i=0; i < x_coord.size(); i++)
             {
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 7da113a..737b441 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -125,12 +125,11 @@ namespace SAND {
         {
             TimerOutput::Scope t(timer, "build A inv");
 
-            if (Utilities::MPI::this_mpi_process(mpi_communicator) == 0)
-            {
-                std::string name = "a_mat";
-                print_matrix(name, a_mat);
-            }
-
+//            if (Utilities::MPI::this_mpi_process(mpi_communicator) == 0)
+//            {
+//                std::string name = "a_mat";
+//                print_matrix(name, a_mat);
+//            }
             a_inv_direct.initialize(a_mat);
         }
         {

From 4f89e7b1df47166766e9f0e410b7ff792d4c73c2 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Tue, 25 Jan 2022 15:19:12 -0700
Subject: [PATCH 63/95] working?

---
 source/density_filter.cc | 29 +++++++++++++++--------------
 source/kkt_system.cc     | 11 +++++------
 2 files changed, 20 insertions(+), 20 deletions(-)

diff --git a/source/density_filter.cc b/source/density_filter.cc
index 4cef2f9..9f4eb97 100644
--- a/source/density_filter.cc
+++ b/source/density_filter.cc
@@ -11,7 +11,8 @@
 #include <deal.II/numerics/vector_tools.h>
 #include <deal.II/numerics/matrix_tools.h>
 
-namespace SAND {
+namespace SAND
+{
     using namespace dealii;
 
     /* When initialized, this function takes the current triangulation and creates a matrix corresponding to a
@@ -49,19 +50,17 @@ namespace SAND {
         /*finds neighbors whose values would be relevant, and adds them to the sparsity pattern of the matrix*/
          for (const auto &cell : dof_handler.active_cell_iterators())
          {
-
-
              if(cell->is_locally_owned())
              {
                 std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
                 cell->get_dof_indices(i);
-                const int i_ind = cell->get_fe().component_to_system_index(0, 0);
-                x_coord.set(i[i_ind],cell->center()[0]) ;
-                y_coord.set(i[i_ind],cell->center()[1]) ;
-                cell_m.set(i[i_ind],cell->measure());
+                const unsigned int i_val = i[cell->get_fe().component_to_system_index(0, 0)];
+                x_coord[i_val] = cell->center()[0] ;
+                y_coord[i_val] = cell->center()[1] ;
+                cell_m[i_val] = cell->measure();
                 if (dim==3)
                 {
-                    z_coord.set(i[i_ind],cell->center()[2]) ;
+                    z_coord[i_val] = cell->center()[2] ;
                 }
              }
          }
@@ -95,15 +94,14 @@ namespace SAND {
             {
                 std::vector<unsigned int> i(cell->get_fe().n_dofs_per_cell());
                 cell->get_dof_indices(i);
-                const int i_ind = &cell->get_fe().component_to_system_index(0, 0);
-                for (const auto &neighbor_cell_index : find_relevant_neighbors(i[i_ind]))
+                for (const auto &neighbor_cell_index : find_relevant_neighbors(i[cell->get_fe().component_to_system_index(0, 0)]))
                 {
-                    double d_x = std::abs(x_coord[i[i_ind]]-x_coord[neighbor_cell_index]);
-                    double d_y = std::abs(y_coord[i[i_ind]]-y_coord[neighbor_cell_index]);
+                    double d_x = std::abs(x_coord[i[cell->get_fe().component_to_system_index(0, 0)]]-x_coord[neighbor_cell_index]);
+                    double d_y = std::abs(y_coord[i[cell->get_fe().component_to_system_index(0, 0)]]-y_coord[neighbor_cell_index]);
                     double d;
                     if (dim==3)
                     {
-                        double d_z = std::abs(z_coord[i[i_ind]]-z_coord[neighbor_cell_index]);
+                        double d_z = std::abs(z_coord[i[cell->get_fe().component_to_system_index(0, 0)]]-z_coord[neighbor_cell_index]);
                         d = std::pow(d_x*d_x + d_y*d_y + d_z*d_z , .5);
                     }
                     else
@@ -112,7 +110,7 @@ namespace SAND {
                     }
                     /*value should be (max radius - distance between cells)*cell measure */
                     double value = (Input::filter_r - d)*cell_m[neighbor_cell_index];
-                    filter_matrix.add(i[i_ind], neighbor_cell_index, value);
+                    filter_matrix.add(i[cell->get_fe().component_to_system_index(0, 0)], neighbor_cell_index, value);
                 }
             }
         }
@@ -185,6 +183,9 @@ namespace SAND {
         {
              throw;
         }
+
+        return relevant_cells;
+
     }
 
 }//SAND namespace
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index d17d446..60adfe0 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -744,14 +744,13 @@ namespace SAND {
                 {
                     std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
                     cell->get_dof_indices(i);
-                    for (const auto &neighbor_cell : density_filter.find_relevant_neighbors(cell)) {
-                        std::vector<types::global_dof_index> j(neighbor_cell->get_fe().n_dofs_per_cell());
-                        neighbor_cell->get_dof_indices(j);
-
+                    const unsigned int cell_index = i[cell->get_fe().component_to_system_index(0, 0)];
+                    for (const auto &neighbor_cell_index : density_filter.find_relevant_neighbors(cell_index))
+                    {
                         dsp.block(SolutionBlocks::unfiltered_density_multiplier,
-                                  SolutionBlocks::unfiltered_density).add(i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)], j[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)]);
+                                  SolutionBlocks::unfiltered_density).add(cell_index, neighbor_cell_index);
                         dsp.block(SolutionBlocks::unfiltered_density,
-                                  SolutionBlocks::unfiltered_density_multiplier).add(j[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)], i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)]);
+                                  SolutionBlocks::unfiltered_density_multiplier).add(neighbor_cell_index, cell_index);
                     }
                 }
             }

From 4512edf06d7298d3f1940f6c326928311d16b265 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Tue, 25 Jan 2022 15:28:49 -0700
Subject: [PATCH 64/95] working?

---
 include/input_information.h    | 4 ++--
 source/density_filter.cc       | 8 ++++----
 source/schur_preconditioner.cc | 6 ++++--
 3 files changed, 10 insertions(+), 8 deletions(-)

diff --git a/include/input_information.h b/include/input_information.h
index 13a9f36..ef3d88a 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -17,8 +17,8 @@ namespace SAND {
         //geometry options
         constexpr unsigned int geometry_base = GeometryOptions::mbb;
 
-        constexpr unsigned int dim = 3;
-        constexpr unsigned int refinements =  5;
+        constexpr unsigned int dim = 2;
+        constexpr unsigned int refinements =  4;
 
         //nonlinear algorithm options
         constexpr double initial_barrier_size = 25;
diff --git a/source/density_filter.cc b/source/density_filter.cc
index 9f4eb97..cc47706 100644
--- a/source/density_filter.cc
+++ b/source/density_filter.cc
@@ -64,10 +64,10 @@ namespace SAND
                 }
              }
          }
-         x_coord.compress(VectorOperation::add);
-         y_coord.compress(VectorOperation::add);
-         z_coord.compress(VectorOperation::add);
-         cell_m.compress(VectorOperation::add);
+         x_coord.compress(VectorOperation::insert);
+         y_coord.compress(VectorOperation::insert);
+         z_coord.compress(VectorOperation::insert);
+         cell_m.compress(VectorOperation::insert);
 
         for (const auto &cell : dof_handler.active_cell_iterators()) {
             if(cell->is_locally_owned())
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 737b441..cbdea6e 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -63,7 +63,8 @@ namespace SAND {
                     SolutionBlocks::displacement);
             const types::global_dof_index disp_mult_start_index = system_matrix.get_row_indices().block_start(
                     SolutionBlocks::displacement_multiplier);
-            for (auto&[dof_index, boundary_value]: boundary_values) {
+            for (auto &pair: boundary_values) {
+                const auto dof_index=pair.first;
 
                 const types::global_dof_index n_u = system_matrix.block(SolutionBlocks::displacement,
                                                                         SolutionBlocks::displacement).m();
@@ -83,7 +84,8 @@ namespace SAND {
             }
 
             //set diagonal to 0?
-            for (auto&[dof_index, boundary_value]: boundary_values) {
+            for (auto &pair: boundary_values) {
+                const auto dof_index=pair.first;
                 const types::global_dof_index disp_start_index = system_matrix.get_row_indices().block_start(
                         SolutionBlocks::displacement);
                 const types::global_dof_index disp_mult_start_index = system_matrix.get_row_indices().block_start(

From 854a473c660edb9672398880345e06b9a1dca8dd Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Tue, 25 Jan 2022 16:06:22 -0700
Subject: [PATCH 65/95] working!

---
 include/density_filter.h    | 12 ++++++++----
 include/input_information.h |  2 +-
 source/density_filter.cc    | 38 +++++++++++++++++--------------------
 3 files changed, 26 insertions(+), 26 deletions(-)

diff --git a/include/density_filter.h b/include/density_filter.h
index c98fe90..ea77fe8 100644
--- a/include/density_filter.h
+++ b/include/density_filter.h
@@ -70,10 +70,14 @@ namespace SAND {
         std::set<types::global_dof_index> find_relevant_neighbors(types::global_dof_index cell_index) const;
 
     private:
-        LA::MPI::Vector cell_m;
-        LA::MPI::Vector x_coord;
-        LA::MPI::Vector y_coord;
-        LA::MPI::Vector z_coord;
+        std::vector<double> cell_m;
+        std::vector<double> x_coord;
+        std::vector<double> y_coord;
+        std::vector<double> z_coord;
+        std::vector<double> cell_m_part;
+        std::vector<double> x_coord_part;
+        std::vector<double> y_coord_part;
+        std::vector<double> z_coord_part;
 
     };
 }
diff --git a/include/input_information.h b/include/input_information.h
index ef3d88a..ed211db 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -17,7 +17,7 @@ namespace SAND {
         //geometry options
         constexpr unsigned int geometry_base = GeometryOptions::mbb;
 
-        constexpr unsigned int dim = 2;
+        constexpr unsigned int dim = 3;
         constexpr unsigned int refinements =  4;
 
         //nonlinear algorithm options
diff --git a/source/density_filter.cc b/source/density_filter.cc
index cc47706..91cef21 100644
--- a/source/density_filter.cc
+++ b/source/density_filter.cc
@@ -22,7 +22,6 @@ namespace SAND
     DensityFilter<dim>::DensityFilter() :
         mpi_communicator(MPI_COMM_WORLD)
     {
-
     }
 
 
@@ -36,10 +35,14 @@ namespace SAND
                 DoFTools::count_dofs_per_fe_block(dof_handler, block_component);
         const unsigned int n_p = dofs_per_block[0];
         IndexSet local_owned = dof_handler.locally_owned_dofs().get_view(0, n_p);
-        x_coord.reinit(local_owned,mpi_communicator);
-        y_coord.reinit(local_owned,mpi_communicator);
-        z_coord.reinit(local_owned,mpi_communicator);
-        cell_m.reinit(local_owned,mpi_communicator);
+        x_coord.resize(n_p);
+        y_coord.resize(n_p);
+        z_coord.resize(n_p);
+        cell_m.resize(n_p);
+        x_coord_part.resize(n_p);
+        y_coord_part.resize(n_p);
+        z_coord_part.resize(n_p);
+        cell_m_part.resize(n_p);
 
         filter_dsp.reinit(dofs_per_block[0],
                           dofs_per_block[0]);
@@ -55,19 +58,20 @@ namespace SAND
                 std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
                 cell->get_dof_indices(i);
                 const unsigned int i_val = i[cell->get_fe().component_to_system_index(0, 0)];
-                x_coord[i_val] = cell->center()[0] ;
-                y_coord[i_val] = cell->center()[1] ;
-                cell_m[i_val] = cell->measure();
+                x_coord_part[i_val] = cell->center()[0] ;
+                y_coord_part[i_val] = cell->center()[1] ;
+                cell_m_part[i_val] = cell->measure();
                 if (dim==3)
                 {
-                    z_coord[i_val] = cell->center()[2] ;
+                    z_coord_part[i_val] = cell->center()[2] ;
                 }
              }
          }
-         x_coord.compress(VectorOperation::insert);
-         y_coord.compress(VectorOperation::insert);
-         z_coord.compress(VectorOperation::insert);
-         cell_m.compress(VectorOperation::insert);
+
+        MPI_Allreduce(x_coord_part.data(), x_coord.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+        MPI_Allreduce(y_coord_part.data(), y_coord.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+        MPI_Allreduce(z_coord_part.data(), z_coord.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+        MPI_Allreduce(cell_m_part.data(), cell_m.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
 
         for (const auto &cell : dof_handler.active_cell_iterators()) {
             if(cell->is_locally_owned())
@@ -147,8 +151,6 @@ namespace SAND
     {
         double d_x,d_y,d_z;
         std::set<types::global_dof_index> relevant_cells;
-        if(x_coord.in_local_range(cell_index))
-        {
             for (unsigned int i=0; i < x_coord.size(); i++)
             {
                 d_x = std::abs(x_coord[cell_index]-x_coord[i]);
@@ -176,12 +178,6 @@ namespace SAND
 
                     }
                 }
-
-            }
-        }
-        else
-        {
-             throw;
         }
 
         return relevant_cells;

From 1534e635e1bc313dfb46c6b7726530faf6c42ae1 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Thu, 27 Jan 2022 09:30:51 -0700
Subject: [PATCH 66/95] added stuff

---
 source/schur_preconditioner.cc.autosave | 710 ++++++++++++++++++++++++
 source/watchdog_main.cc                 |  14 +-
 2 files changed, 722 insertions(+), 2 deletions(-)
 create mode 100644 source/schur_preconditioner.cc.autosave

diff --git a/source/schur_preconditioner.cc.autosave b/source/schur_preconditioner.cc.autosave
new file mode 100644
index 0000000..cbdea6e
--- /dev/null
+++ b/source/schur_preconditioner.cc.autosave
@@ -0,0 +1,710 @@
+//
+// Created by justin on 2/17/21.
+//
+#include <deal.II/lac/linear_operator.h>
+#include <deal.II/lac/sparse_direct.h>
+#include <deal.II/lac/sparse_matrix.h>
+#include <deal.II/base/timer.h>
+#include <deal.II/lac/generic_linear_algebra.h>
+#include <deal.II/lac/trilinos_solver.h>
+#include <deal.II/lac/trilinos_precondition.h>
+#include "../include/schur_preconditioner.h"
+#include "../include/input_information.h"
+#include "../include/sand_tools.h"
+#include <fstream>
+
+namespace SAND {
+    using MatrixType  = dealii::TrilinosWrappers::SparseMatrix;
+    using VectorType  = dealii::TrilinosWrappers::MPI::Vector;
+    using PayloadType = dealii::TrilinosWrappers::internal::LinearOperatorImplementation::TrilinosPayload;
+    using PayloadVectorType = typename PayloadType::VectorType;
+    using size_type         = dealii::types::global_dof_index;
+    using namespace dealii;
+    template<int dim>
+    TopOptSchurPreconditioner<dim>::TopOptSchurPreconditioner(LA::MPI::BlockSparseMatrix &matrix_in)
+            :
+            system_matrix(matrix_in),
+            mpi_communicator(MPI_COMM_WORLD),
+            n_rows(0),
+            n_columns(0),
+            n_block_rows(0),
+            n_block_columns(0),
+            other_solver_control(100000, 1e-6),
+            other_bicgstab(other_solver_control),
+            other_gmres(other_solver_control),
+            other_cg(other_solver_control),
+            a_mat(matrix_in.block(SolutionBlocks::displacement, SolutionBlocks::displacement_multiplier)),
+            b_mat(matrix_in.block(SolutionBlocks::density, SolutionBlocks::density)),
+            c_mat(matrix_in.block(SolutionBlocks::displacement,SolutionBlocks::density)),
+            e_mat(matrix_in.block(SolutionBlocks::displacement_multiplier,SolutionBlocks::density)),
+            f_mat(matrix_in.block(SolutionBlocks::unfiltered_density_multiplier,SolutionBlocks::unfiltered_density)),
+            d_m_mat(matrix_in.block(SolutionBlocks::density_upper_slack_multiplier, SolutionBlocks::density_upper_slack)),
+            d_1_mat(matrix_in.block(SolutionBlocks::density_lower_slack, SolutionBlocks::density_lower_slack)),
+            d_2_mat(matrix_in.block(SolutionBlocks::density_upper_slack, SolutionBlocks::density_upper_slack)),
+            m_vect(matrix_in.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier)),
+            solver_type("Amesos_Klu"),
+            additional_data(false, solver_type),
+            direct_solver_control(1, 0),
+            a_inv_direct(direct_solver_control, additional_data),
+            pcout(std::cout,(Utilities::MPI::this_mpi_process(mpi_communicator) == 0)),
+            timer(pcout, TimerOutput::summary, TimerOutput::wall_times)
+    {
+
+    }
+
+    template<int dim>
+    void TopOptSchurPreconditioner<dim>::initialize(LA::MPI::BlockSparseMatrix &matrix, const std::map<types::global_dof_index, double> &boundary_values,const DoFHandler<dim> &dof_handler, const LA::MPI::BlockVector &distributed_state)
+    {
+        TimerOutput::Scope t(timer, "initialize");
+        {
+
+            TimerOutput::Scope t(timer, "diag stuff");
+            const types::global_dof_index disp_start_index = system_matrix.get_row_indices().block_start(
+                    SolutionBlocks::displacement);
+            const types::global_dof_index disp_mult_start_index = system_matrix.get_row_indices().block_start(
+                    SolutionBlocks::displacement_multiplier);
+            for (auto &pair: boundary_values) {
+                const auto dof_index=pair.first;
+
+                const types::global_dof_index n_u = system_matrix.block(SolutionBlocks::displacement,
+                                                                        SolutionBlocks::displacement).m();
+                if ((dof_index >= disp_start_index) && (dof_index < disp_start_index + n_u)) {
+                    double diag_val = system_matrix.block(SolutionBlocks::displacement,
+                                                          SolutionBlocks::displacement).el(
+                            dof_index - disp_start_index, dof_index - disp_start_index);
+                    system_matrix.block(SolutionBlocks::displacement, SolutionBlocks::displacement_multiplier).set(
+                            dof_index - disp_start_index, dof_index - disp_start_index, diag_val);
+                } else if ((dof_index >= disp_mult_start_index) && (dof_index < disp_mult_start_index + n_u)) {
+                    double diag_val = system_matrix.block(SolutionBlocks::displacement_multiplier,
+                                                          SolutionBlocks::displacement_multiplier).el(
+                            dof_index - disp_mult_start_index, dof_index - disp_mult_start_index);
+                    system_matrix.block(SolutionBlocks::displacement_multiplier, SolutionBlocks::displacement).set(
+                            dof_index - disp_mult_start_index, dof_index - disp_mult_start_index, diag_val);
+                }
+            }
+
+            //set diagonal to 0?
+            for (auto &pair: boundary_values) {
+                const auto dof_index=pair.first;
+                const types::global_dof_index disp_start_index = system_matrix.get_row_indices().block_start(
+                        SolutionBlocks::displacement);
+                const types::global_dof_index disp_mult_start_index = system_matrix.get_row_indices().block_start(
+                        SolutionBlocks::displacement_multiplier);
+                const types::global_dof_index n_u = system_matrix.block(SolutionBlocks::displacement,
+                                                                        SolutionBlocks::displacement).m();
+                if ((dof_index >= disp_start_index) && (dof_index < disp_start_index + n_u)) {
+                    system_matrix.block(SolutionBlocks::displacement, SolutionBlocks::displacement).set(
+                            dof_index - disp_start_index, dof_index - disp_start_index, 0);
+                } else if ((dof_index >= disp_mult_start_index) && (dof_index < disp_mult_start_index + n_u)) {
+                    system_matrix.block(SolutionBlocks::displacement_multiplier,
+                                        SolutionBlocks::displacement_multiplier).set(
+                            dof_index - disp_mult_start_index, dof_index - disp_mult_start_index, 0);
+                }
+            }
+
+//            system_matrix.compress(VectorOperation::insert);
+
+//            const unsigned int m = a_mat.m();
+//            const unsigned int n = a_mat.n();
+//            std::ofstream Xmat("a_mat_par.csv");
+//            for (unsigned int i = 0; i < m; i++)
+//            {
+//                Xmat << a_mat.el(i, 0);
+//                for (unsigned int j = 1; j < n; j++)
+//                {
+//                    Xmat << "," << a_mat.el(i, j);
+//                }
+//                Xmat << "\n";
+//            }
+//            Xmat.close();
+
+        }
+        if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
+        {
+
+        }
+        else
+        {
+            TimerOutput::Scope t(timer, "build A inv");
+
+//            if (Utilities::MPI::this_mpi_process(mpi_communicator) == 0)
+//            {
+//                std::string name = "a_mat";
+//                print_matrix(name, a_mat);
+//            }
+            a_inv_direct.initialize(a_mat);
+        }
+        {
+            TimerOutput::Scope t(timer, "reinit diag matrices");
+            d_3_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
+            d_4_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
+            d_5_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
+            d_6_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
+            d_7_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
+            d_8_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
+            d_m_inv_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
+            d_3_mat=0;
+            d_4_mat=0;
+            d_5_mat=0;
+            d_6_mat=0;
+            d_7_mat=0;
+            d_8_mat=0;
+            d_m_inv_mat=0;
+            pcout << "diag reinit" << std::endl;
+        }
+        {
+            const types::global_dof_index n_p = system_matrix.block(SolutionBlocks::density,
+                                                                    SolutionBlocks::density).m();
+
+            pcout << "np: " << n_p << std::endl;
+
+
+
+
+            std::vector<double> l_global(n_p);
+            std::vector<double> lm_global(n_p);
+            std::vector<double> u_global(n_p);
+            std::vector<double> um_global(n_p);
+            std::vector<double> m_global(n_p);
+
+            std::vector<double> l(n_p);
+            std::vector<double> lm(n_p);
+            std::vector<double> u(n_p);
+            std::vector<double> um(n_p);
+            std::vector<double> m(n_p);
+
+            TimerOutput::Scope t(timer, "build diag matrices");
+            for (const auto cell: dof_handler.active_cell_iterators())
+            {
+                if(cell->is_locally_owned())
+                {
+                    std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
+                    cell->get_dof_indices(i);
+
+
+
+                    const int i_ind = cell->get_fe().component_to_system_index(0,0);
+
+                    if(distributed_state.block(SolutionBlocks::density_lower_slack_multiplier).in_local_range(i[i_ind]))
+                    {
+                        lm[i[i_ind]] = distributed_state.block(SolutionBlocks::density_lower_slack_multiplier)[i[i_ind]];
+                        m[i[i_ind]] = cell->measure();
+                    }
+
+                    if(distributed_state.block(SolutionBlocks::density_lower_slack).in_local_range(i[i_ind]))
+                    {
+                        l[i[i_ind]] =  distributed_state.block(SolutionBlocks::density_lower_slack)[i[i_ind]];
+                    }
+
+                    if(distributed_state.block(SolutionBlocks::density_upper_slack_multiplier).in_local_range(i[i_ind]))
+                    {
+                        um[i[i_ind]]= distributed_state.block(SolutionBlocks::density_upper_slack_multiplier)[i[i_ind]];
+                    }
+
+                    if(distributed_state.block(SolutionBlocks::density_upper_slack).in_local_range(i[i_ind]))
+                    {
+                        u[i[i_ind]] = distributed_state.block(SolutionBlocks::density_upper_slack)[i[i_ind]];
+                    }
+                }
+            }
+
+            MPI_Allreduce(lm.data(), lm_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+            MPI_Allreduce(l.data(), l_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+            MPI_Allreduce(um.data(), um_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+            MPI_Allreduce(u.data(), u_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+            MPI_Allreduce(m.data(), m_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+
+            for (unsigned int k=0; k< n_p; k++)
+            {
+                    if(distributed_state.block(0).in_local_range(k))
+                    {
+                        d_3_mat.set(k, k, -1 * lm_global[k]/(m_global[k]*l_global[k]));
+                        d_4_mat.set(k, k, -1 * um_global[k]/(m_global[k]*u_global[k]));
+                        d_5_mat.set(k, k, lm_global[k]/l_global[k]);
+                        d_6_mat.set(k, k, um_global[k]/u_global[k]);
+                        d_7_mat.set(k, k, m_global[k]*(lm_global[k]*u_global[k] + um_global[k]*l_global[k])/(l_global[k]*u_global[k]));
+                        d_8_mat.set(k, k, l_global[k]*u_global[k]/(m_global[k]*(lm_global[k]*u_global[k] + um_global[k]*l_global[k])));
+                        d_m_inv_mat.set(k, k, 1 / m_global[k]);
+                    }
+            }
+        }
+        d_3_mat.compress(VectorOperation::insert);
+        d_4_mat.compress(VectorOperation::insert);
+        d_5_mat.compress(VectorOperation::insert);
+        d_6_mat.compress(VectorOperation::insert);
+        d_7_mat.compress(VectorOperation::insert);
+        d_8_mat.compress(VectorOperation::insert);
+        d_m_inv_mat.compress(VectorOperation::insert);
+
+        pcout << "compressed" << std::endl;
+
+        pre_j=distributed_state.block(SolutionBlocks::density);
+        pre_k=distributed_state.block(SolutionBlocks::density);
+        g_d_m_inv_density=distributed_state.block(SolutionBlocks::density);
+        k_g_d_m_inv_density=distributed_state.block(SolutionBlocks::density);
+
+
+        op_d_8 = linear_operator<VectorType,VectorType,PayloadType>(d_8_mat);
+        op_f = linear_operator<VectorType,VectorType,PayloadType>(f_mat);
+        op_b = linear_operator<VectorType,VectorType,PayloadType>(b_mat);
+        op_c = linear_operator<VectorType,VectorType,PayloadType>(c_mat);
+        op_a_inv = linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct);
+        op_e = linear_operator<VectorType,VectorType,PayloadType>(e_mat);
+        op_d_m= linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
+        op_d_m_inv= linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat);
+
+        op_g = op_f * op_d_8 *
+                transpose_operator(op_f);
+
+
+        op_h = op_b
+               - transpose_operator(op_c) * op_a_inv * op_e
+               - transpose_operator(op_e) * op_a_inv * op_c;
+
+//        TrilinosWrappers::internal::LinearOperatorImplementation::TrilinosPayload inv_payload = inverse_payload(TrilinosWrappers::SolverGMRES(),TrilinosWrappers::PreconditionIdentity());
+
+        op_k_inv = (-1 * op_g *op_d_m_inv * op_h - op_d_m);
+
+
+
+
+    }
+
+
+    template<int dim>
+    void TopOptSchurPreconditioner<dim>::vmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
+        LA::MPI::BlockVector temp_src;
+        pcout << "vmult" << std::endl;
+        {
+            TimerOutput::Scope t(timer, "part 1");
+            vmult_step_1(dst, src);
+            temp_src = dst;
+        }
+
+        {
+            TimerOutput::Scope t(timer, "part 2");
+            vmult_step_2(dst, temp_src);
+            temp_src = dst;
+        }
+
+        {
+            TimerOutput::Scope t(timer, "part 3");
+            vmult_step_3(dst, temp_src);
+            temp_src = dst;
+        }
+        {
+            TimerOutput::Scope t(timer, "part 4");
+            vmult_step_4(dst, temp_src);
+            temp_src = dst;
+        }
+        vmult_step_5(dst, temp_src);        
+    }
+
+    template<int dim>
+    void TopOptSchurPreconditioner<dim>::Tvmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
+        dst = src;
+    }
+
+    template<int dim>
+    void TopOptSchurPreconditioner<dim>::vmult_add(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
+        LA::MPI::BlockVector dst_temp = dst;
+        vmult(dst_temp, src);
+        dst += dst_temp;
+    }
+
+    template<int dim>
+    void TopOptSchurPreconditioner<dim>::Tvmult_add(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
+        dst = src;
+    }
+
+    template<int dim>
+    void TopOptSchurPreconditioner<dim>::vmult_step_1(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
+        dst = src;
+        auto dst_temp = dst;
+        dst.block(SolutionBlocks::unfiltered_density) = dst_temp.block(SolutionBlocks::unfiltered_density)
+                - linear_operator<VectorType,VectorType,PayloadType>(d_5_mat) * src.block(SolutionBlocks::density_lower_slack_multiplier)
+                + linear_operator<VectorType,VectorType,PayloadType>(d_6_mat) * src.block(SolutionBlocks::density_upper_slack_multiplier)
+                + src.block(SolutionBlocks::density_lower_slack)
+                - src.block(SolutionBlocks::density_upper_slack);
+    }
+
+    template<int dim>
+    void TopOptSchurPreconditioner<dim>::vmult_step_2(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
+        dst = src;
+        auto dst_temp = dst;
+
+        dst.block(SolutionBlocks::unfiltered_density_multiplier) = dst_temp.block(SolutionBlocks::unfiltered_density_multiplier)
+                - linear_operator<VectorType,VectorType,PayloadType>(f_mat)*linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) * src.block(SolutionBlocks::unfiltered_density);
+
+    }
+
+    template<int dim>
+    void TopOptSchurPreconditioner<dim>::vmult_step_3(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
+        dst = src;
+
+        if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
+        {
+            SolverControl            solver_control_1(100000, 1e-6);
+            SolverControl            solver_control_2(100000, 1e-6);
+            SolverCG<LA::MPI::Vector> a_solver_cg_1(solver_control_1);
+            SolverCG<LA::MPI::Vector> a_solver_cg_2(solver_control_2);
+            auto dst_temp = dst;
+            a_solver_cg_1.solve(a_mat,dst_temp.block(SolutionBlocks::displacement_multiplier),src.block(SolutionBlocks::displacement_multiplier),PreconditionIdentity());
+            a_solver_cg_2.solve(a_mat,dst_temp.block(SolutionBlocks::displacement),src.block(SolutionBlocks::displacement),PreconditionIdentity());
+            c_mat.Tvmult(dst_temp.block(SolutionBlocks::density_upper_slack),dst_temp.block(SolutionBlocks::displacement_multiplier));
+            e_mat.Tvmult(dst_temp.block(SolutionBlocks::density_lower_slack),dst_temp.block(SolutionBlocks::displacement));
+
+            dst.block(SolutionBlocks::density) = dst_temp.block(SolutionBlocks::density) - dst_temp.block(SolutionBlocks::density_upper_slack) - dst_temp.block(SolutionBlocks::density_lower_slack);
+
+
+        }
+        else
+        {
+            auto dst_temp = dst;
+            dst.block(SolutionBlocks::density) = dst_temp.block(SolutionBlocks::density)
+                    - transpose_operator<VectorType,VectorType,PayloadType>(e_mat) * linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct)* src.block(SolutionBlocks::displacement)
+                    - transpose_operator<VectorType,VectorType,PayloadType>(c_mat) * linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct)* src.block(SolutionBlocks::displacement_multiplier);
+        }
+
+    }
+
+    template<int dim>
+    void TopOptSchurPreconditioner<dim>::vmult_step_4(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
+        dst = src;
+        auto dst_temp = dst;
+        auto k_density_mult =  src.block(SolutionBlocks::density);
+
+        if (Input::solver_choice == SolverOptions::exact_preconditioner_with_gmres)
+        {
+//            g_d_m_inv_density = linear_operator<VectorType,VectorType,PayloadType>(g_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
+//            k_g_d_m_inv_density = linear_operator<VectorType,VectorType,PayloadType>(k_mat) * g_d_m_inv_density;
+//            k_density_mult = linear_operator<VectorType,VectorType,PayloadType>(k_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
+        }
+
+        else if (Input::solver_choice == SolverOptions::inexact_K_with_exact_A_gmres)
+        {
+
+            g_d_m_inv_density = op_g * op_d_m_inv * src.block(SolutionBlocks::density);
+            SolverControl step_4_gmres_control_1 (100000, g_d_m_inv_density.l2_norm()*1e-6);
+            SolverGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
+            try {
+                TimerOutput::Scope u(timer, "actual inv");
+//                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
+//                                      g_d_m_inv_density;
+                step_4_gmres_1.solve(op_k_inv,k_g_d_m_inv_density,g_d_m_inv_density, PreconditionIdentity() );
+            }
+            catch (std::exception &exc)
+            {
+                std::cerr << "Failure of linear solver step_4_gmres_1" << std::endl;
+                pcout << "first residual: " << step_4_gmres_control_1.initial_value() << std::endl;
+                pcout << "last residual: " << step_4_gmres_control_1.last_value() << std::endl;
+                throw;
+            }
+            SolverControl step_4_gmres_control_2 (100000, src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6);
+            SolverGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
+            try {
+                TimerOutput::Scope u(timer, "actual inv");
+//                k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, PreconditionIdentity()) *
+//                                 src.block(SolutionBlocks::unfiltered_density_multiplier);
+                step_4_gmres_2.solve(op_k_inv,k_density_mult,src.block(SolutionBlocks::unfiltered_density_multiplier), PreconditionIdentity() );
+            } catch (std::exception &exc)
+            {
+                std::cerr << "Failure of linear solver step_4_gmres_2" << std::endl;
+                pcout << "first residual: " << step_4_gmres_control_2.initial_value() << std::endl;
+                pcout << "last residual: " << step_4_gmres_control_2.last_value() << std::endl;
+                throw;
+            }
+        }
+        else if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
+        {
+
+            SolverControl            solver_control(100000, 1e-6);
+            SolverCG<LA::MPI::Vector> a_solver_cg(solver_control);
+
+            auto preconditioner = PreconditionIdentity();
+
+            auto a_inv_op = op_d_8;
+
+                    inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg, preconditioner);
+
+
+            auto op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) *
+                        transpose_operator<VectorType, VectorType, PayloadType>(f_mat);
+
+            auto op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
+                        - transpose_operator<VectorType, VectorType, PayloadType>(c_mat) * a_inv_op * linear_operator<VectorType,VectorType,PayloadType>(e_mat)
+                        - transpose_operator<VectorType, VectorType, PayloadType>(e_mat) * a_inv_op * linear_operator<VectorType,VectorType,PayloadType>(c_mat);
+
+            auto op_k_inv = op_d_8;
+
+                    -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
+
+            SolverControl step_4_gmres_control_1 (10000, std::max(g_d_m_inv_density.l2_norm()*1e-6,1e-6));
+            SolverGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
+            try {
+//                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
+//                                      g_d_m_inv_density;
+//                step_4_gmres_1.solve(op_k_inv,k_g_d_m_inv_density,g_d_m_inv_density, PreconditionIdentity() );
+            } catch (std::exception &exc)
+            {
+                std::cerr << "Failure of linear solver step_4_gmres_1" << std::endl;
+                pcout << "first residual: " << step_4_gmres_control_1.initial_value() << std::endl;
+                pcout << "last residual: " << step_4_gmres_control_1.last_value() << std::endl;
+                throw;
+            }
+
+            SolverControl step_4_gmres_control_2 (10000, std::max(src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6,1e-6));
+            SolverGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
+            try {
+
+                k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, PreconditionIdentity()) *
+                                 src.block(SolutionBlocks::unfiltered_density_multiplier);
+            } catch (std::exception &exc)
+            {
+                std::cerr << "Failure of linear solver step_4_gmres_2" << std::endl;
+                pcout << "first residual: " << step_4_gmres_control_2.initial_value() << std::endl;
+                pcout << "last residual: " << step_4_gmres_control_2.last_value() << std::endl;
+                throw;
+            }
+        }
+        else
+        {
+            pcout << "shouldn't get here";
+            throw;
+        }
+
+        dst.block(SolutionBlocks::total_volume_multiplier) = transpose_operator<VectorType, VectorType, PayloadType>(m_vect)*k_g_d_m_inv_density
+                                                        - transpose_operator<VectorType, VectorType, PayloadType>(m_vect)*k_density_mult
+                                                        +dst_temp.block(SolutionBlocks::total_volume_multiplier);
+    }
+
+
+
+
+    template<int dim>
+    void TopOptSchurPreconditioner<dim>::vmult_step_5(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
+        {
+            //First Block Inverse
+            TimerOutput::Scope t(timer, "inverse 1");
+            dst.block(SolutionBlocks::density_lower_slack_multiplier) = linear_operator<VectorType,VectorType,PayloadType>(d_3_mat) * src.block(SolutionBlocks::density_lower_slack_multiplier) +
+                    linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density_lower_slack);
+            dst.block(SolutionBlocks::density_upper_slack_multiplier) = linear_operator<VectorType,VectorType,PayloadType>(d_4_mat) * src.block(SolutionBlocks::density_upper_slack_multiplier) +
+                    linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density_upper_slack);
+            dst.block(SolutionBlocks::density_lower_slack) = linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density_lower_slack_multiplier);
+            dst.block(SolutionBlocks::density_upper_slack) = linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density_upper_slack_multiplier);
+        }
+
+
+        {
+            //Second Block Inverse
+            TimerOutput::Scope t(timer, "inverse 2");
+            dst.block(SolutionBlocks::unfiltered_density) =
+                    linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) * src.block(SolutionBlocks::unfiltered_density);
+        }
+
+
+        {
+            //Third Block Inverse
+            TimerOutput::Scope t(timer, "inverse 3");
+            if(Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
+            {
+                SolverControl            solver_control(100000, 1e-6);
+                SolverCG<LA::MPI::Vector> a_solver_cg(solver_control);
+
+                auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg, PreconditionIdentity());
+
+                dst.block(SolutionBlocks::displacement) = a_inv_op * src.block(SolutionBlocks::displacement_multiplier);
+                dst.block(SolutionBlocks::displacement_multiplier) = a_inv_op * src.block(SolutionBlocks::displacement);
+            } else
+            {
+                dst.block(SolutionBlocks::displacement) = linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct) * src.block(SolutionBlocks::displacement_multiplier);
+                dst.block(SolutionBlocks::displacement_multiplier) = linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct) * src.block(SolutionBlocks::displacement);
+            }
+
+        }
+
+
+        {
+            //Fourth (ugly) Block Inverse
+            TimerOutput::Scope t(timer, "inverse 4");
+
+
+
+            if (Input::solver_choice == SolverOptions::exact_preconditioner_with_gmres)
+            {
+//                pre_j = src.block(SolutionBlocks::density) + linear_operator<VectorType,VectorType,PayloadType>(h_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
+//                pre_k = -1* linear_operator<VectorType,VectorType,PayloadType>(g_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density) + src.block(SolutionBlocks::unfiltered_density_multiplier);
+//                dst.block(SolutionBlocks::unfiltered_density_multiplier) = transpose_operator<VectorType, VectorType, PayloadType>(k_mat) * pre_j;
+//                dst.block(SolutionBlocks::density) = linear_operator<VectorType,VectorType,PayloadType>(k_mat) * pre_k;
+            }
+
+            else if (Input::solver_choice == SolverOptions::inexact_K_with_exact_A_gmres)
+            {
+                auto pre_pre_k = pre_k;
+                {
+                    TimerOutput::Scope t(timer, "not inverse 5.1");
+                    pre_j = src.block(SolutionBlocks::density) + op_h * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
+                    pre_pre_k = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
+                    pre_k =  pre_pre_k + src.block(SolutionBlocks::unfiltered_density_multiplier);
+                }
+                SolverControl step_5_gmres_control_1 (100000, pre_j.l2_norm()*1e-6);
+                SolverGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
+                try {
+                    TimerOutput::Scope t(timer, "actual inverse 5.1");
+//                    dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
+//                                                                               pre_j;
+                    step_5_gmres_1.solve(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), dst.block(SolutionBlocks::unfiltered_density_multiplier), pre_j , PreconditionIdentity());
+                } catch (std::exception &exc)
+                {
+                    std::cerr << "Failure of linear solver step_5_gmres_1 again" << std::endl;
+                    pcout << "first residual: " << step_5_gmres_control_1.initial_value() << std::endl;
+                    pcout << "last residual: " << step_5_gmres_control_1.last_value() << std::endl;
+                    throw;
+                }
+
+
+                SolverControl step_5_gmres_control_2 (100000, pre_k.l2_norm()*1e-6);
+                SolverGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
+                try {
+                    TimerOutput::Scope t(timer, "actual inverse 5.2");
+//                    dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) * pre_k;
+                    step_5_gmres_2.solve(op_k_inv,dst.block(SolutionBlocks::density), pre_k , PreconditionIdentity());
+                } catch (std::exception &exc)
+                {
+                    std::cerr << "Failure of linear solver step_5_gmres_2" << std::endl;
+                    pcout << "first residual: " << step_5_gmres_control_2.initial_value() << std::endl;
+                    pcout << "last residual: " << step_5_gmres_control_2.last_value() << std::endl;
+                    throw;
+                }
+            }
+            else if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
+            {
+
+                SolverControl            solver_control(100000, 1e-6);
+                SolverCG<LA::MPI::Vector> a_solver_cg (solver_control);
+                auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg, PreconditionIdentity());
+
+                auto op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) *
+                            transpose_operator<VectorType, VectorType, PayloadType>(f_mat);
+
+                auto op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
+                            - transpose_operator<VectorType, VectorType, PayloadType>(c_mat) * a_inv_op * linear_operator<VectorType,VectorType,PayloadType>(e_mat)
+                            - transpose_operator<VectorType, VectorType, PayloadType>(e_mat) * a_inv_op * linear_operator<VectorType,VectorType,PayloadType>(c_mat);
+
+                auto op_k_inv = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
+
+                pre_j = src.block(SolutionBlocks::density) + op_h * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
+                pre_k = -1* op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density) + src.block(SolutionBlocks::unfiltered_density_multiplier);
+
+                SolverControl step_5_gmres_control_1 (10000, std::max(pre_j.l2_norm()*1e-6,1e-6));
+                TrilinosWrappers::SolverGMRES step_5_gmres_1 (step_5_gmres_control_1);
+                try {
+                    dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType,VectorType,PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
+                                                                               pre_j;
+                } catch (std::exception &exc)
+                {
+                    std::cerr << "Failure of linear solver step_5_gmres_1" << std::endl;
+                    pcout << "first residual: " << step_5_gmres_control_1.initial_value() << std::endl;
+                    pcout << "last residual: " << step_5_gmres_control_1.last_value() << std::endl;
+                    throw;
+                }
+
+                SolverControl step_5_gmres_control_2 (10000, std::max(pre_k.l2_norm()*1e-6,1e-6));
+                TrilinosWrappers::SolverGMRES step_5_gmres_2 (step_5_gmres_control_2);
+                try {
+                    dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) *
+                                                         pre_k;
+                } catch (std::exception &exc)
+                {
+                    std::cerr << "Failure of linear solver step_5_gmres_2" << std::endl;
+                    pcout << "first residual: " << step_5_gmres_control_2.initial_value() << std::endl;
+                    pcout << "last residual: " << step_5_gmres_control_2.last_value() << std::endl;
+                    throw;
+                }
+            }
+            else
+            {
+                pcout << "shouldn't get here";
+                throw;
+            }
+
+        }
+        {
+            dst.block(SolutionBlocks::total_volume_multiplier) = src.block(SolutionBlocks::total_volume_multiplier);
+        }
+    }
+
+
+    template<int dim>
+    void TopOptSchurPreconditioner<dim>::print_stuff()
+    {
+
+//        print_matrix(std::string("OAmat.csv"),a_mat);
+//        print_matrix(std::string("OBmat.csv"),b_mat);
+//        print_matrix(std::string("OCmat.csv"),c_mat);
+//        print_matrix(std::string("OEmat.csv"),e_mat);
+//        print_matrix(std::string("OFmat.csv"),f_mat);
+        FullMatrix<double> g_mat;
+        FullMatrix<double> h_mat;
+        FullMatrix<double> k_inv_mat;
+        g_mat.reinit(b_mat.m(),b_mat.n());
+        h_mat.reinit(b_mat.m(),b_mat.n());
+        k_inv_mat.reinit(b_mat.m(),b_mat.n());
+        auto op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) *
+                    transpose_operator(linear_operator<VectorType,VectorType,PayloadType>(f_mat));
+
+        auto op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
+                    - transpose_operator(linear_operator<VectorType,VectorType,PayloadType>(c_mat)) * linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct) * linear_operator<VectorType,VectorType,PayloadType>(e_mat)
+                    - transpose_operator(linear_operator<VectorType,VectorType,PayloadType>(e_mat)) * linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct) * linear_operator<VectorType,VectorType,PayloadType>(c_mat);
+
+        auto op_k_inv = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
+//        build_matrix_element_by_element(op_g,g_mat, src.block(SolutionBlocks::density));
+//        build_matrix_element_by_element(op_h,h_mat, src.block(SolutionBlocks::density));
+//        build_matrix_element_by_element(op_k_inv,k_inv_mat, src.block(SolutionBlocks::density));
+//        print_matrix(std::string("OGmat.csv"),g_mat);
+//        print_matrix(std::string("OHmat.csv"),h_mat);
+//        print_matrix(std::string("OKinvmat.csv"),k_inv_mat);
+
+
+    }
+
+    void VmultTrilinosSolverDirect::initialize(LA::MPI::SparseMatrix &a_mat)
+    {
+        reinit(a_mat);
+        solver_direct.initialize(a_mat);
+        size = a_mat.n();
+    }
+
+    void
+    VmultTrilinosSolverDirect::vmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
+    {
+        solver_direct.solve(dst, src);
+    }
+
+    void VmultTrilinosSolverDirect::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+    {
+        solver_direct.solve(dst, src);
+    }
+
+    void
+    VmultTrilinosSolverDirect::Tvmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
+    {
+        solver_direct.solve(dst, src);
+    }
+
+    void VmultTrilinosSolverDirect::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+    {
+        solver_direct.solve(dst, src);
+    }
+
+
+    VmultTrilinosSolverDirect::VmultTrilinosSolverDirect(SolverControl &cn,
+                     const TrilinosWrappers::SolverDirect::AdditionalData &data)
+    : solver_direct(cn, data)
+    {
+
+    }
+
+}
+template class SAND::TopOptSchurPreconditioner<2>;
+template class SAND::TopOptSchurPreconditioner<3>;
diff --git a/source/watchdog_main.cc b/source/watchdog_main.cc
index 8e50439..893c41b 100644
--- a/source/watchdog_main.cc
+++ b/source/watchdog_main.cc
@@ -119,9 +119,19 @@ namespace SAND {
     const LA::MPI::BlockVector
     SANDTopOpt<dim>::find_max_step(const LA::MPI::BlockVector &state)
     {
-        kkt_system.assemble_block_system(state, barrier_size);
+        TimerOutput::Scope t(overall_timer, "find step");
+        {
+            TimerOutput::Scope t(overall_timer, "assemble");
+            kkt_system.assemble_block_system(state, barrier_size);
+        }
+
         pcout << "pre" << std::endl;
-        const LA::MPI::BlockVector step = kkt_system.solve(state);
+        LA::MPI::BlockVector step;
+        {
+            TimerOutput::Scope t(overall_timer, "solve");
+            step = kkt_system.solve(state);
+        }
+
         pcout << "post" << std::endl;
         const auto max_step_sizes= calculate_max_step_size(state,step);
         const double step_size_s = max_step_sizes.first;

From 426629bed433b69332360c7ca02606ad1fc3d803 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <oconnor@tempest.math.colostate.edu>
Date: Thu, 27 Jan 2022 14:37:03 -0700
Subject: [PATCH 67/95] fixed volume bug

---
 include/density_filter.h       |   2 +
 include/input_information.h    |   8 +--
 source/density_filter.cc       |   3 +-
 source/kkt_system.cc           |   8 ++-
 source/schur_preconditioner.cc | 103 ++++++++++++++-------------------
 5 files changed, 58 insertions(+), 66 deletions(-)

diff --git a/include/density_filter.h b/include/density_filter.h
index ea77fe8..cad75bf 100644
--- a/include/density_filter.h
+++ b/include/density_filter.h
@@ -79,6 +79,8 @@ namespace SAND {
         std::vector<double> y_coord_part;
         std::vector<double> z_coord_part;
 
+        ConditionalOStream pcout;
+
     };
 }
 #endif //SAND_DENSITY_FILTER_H
diff --git a/include/input_information.h b/include/input_information.h
index ed211db..d23785b 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -17,15 +17,15 @@ namespace SAND {
         //geometry options
         constexpr unsigned int geometry_base = GeometryOptions::mbb;
 
-        constexpr unsigned int dim = 3;
-        constexpr unsigned int refinements =  4;
+        constexpr unsigned int dim = 2;
+        constexpr unsigned int refinements =  5;
 
         //nonlinear algorithm options
         constexpr double initial_barrier_size = 25;
         constexpr double min_barrier_size = 0;
 
-        constexpr double fraction_to_boundary = .9;
-        constexpr unsigned int max_steps=2;
+        constexpr double fraction_to_boundary = .7;
+        constexpr unsigned int max_steps=30;
 
         constexpr unsigned int barrier_reduction=BarrierOptions::loqo;
         constexpr double required_norm = .0001;
diff --git a/source/density_filter.cc b/source/density_filter.cc
index 91cef21..8fc5562 100644
--- a/source/density_filter.cc
+++ b/source/density_filter.cc
@@ -20,7 +20,8 @@ namespace SAND
 
     template<int dim>
     DensityFilter<dim>::DensityFilter() :
-        mpi_communicator(MPI_COMM_WORLD)
+        mpi_communicator(MPI_COMM_WORLD),
+        pcout(std::cout,(Utilities::MPI::this_mpi_process(mpi_communicator) == 0))
     {
     }
 
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index 60adfe0..abf1384 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -1736,9 +1736,11 @@ namespace SAND {
         for (const auto &cell: dof_handler.active_cell_iterators()) {
             if(cell->is_locally_owned())
             {
-                if (distributed_solution.block(SolutionBlocks::density).in_local_range(cell->get_fe().component_to_system_index(0, 0)))
+                std::vector<unsigned int> i(cell->get_fe().n_dofs_per_cell());
+                cell->get_dof_indices(i);
+                if (distributed_solution.block(SolutionBlocks::density).in_local_range(i[cell->get_fe().component_to_system_index(0, 0)]))
                 {
-                    total_volume_temp += cell->measure() * state.block(SolutionBlocks::density)[cell->get_fe().component_to_system_index(0, 0)];
+                    total_volume_temp += cell->measure() * state.block(SolutionBlocks::density)[i[cell->get_fe().component_to_system_index(0, 0)]];
                     goal_volume_temp += cell->measure() * Input::volume_percentage;
                 }
             }
@@ -1747,6 +1749,8 @@ namespace SAND {
         MPI_Allreduce(&total_volume_temp, &total_volume, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
         MPI_Allreduce(&goal_volume_temp, &goal_volume, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
 
+        pcout << "total_volume: " << total_volume  << " and goal_volume: " << goal_volume << std::endl;
+
         if (test_rhs.block(SolutionBlocks::total_volume_multiplier).in_local_range(0))
         {
             test_rhs.block(SolutionBlocks::total_volume_multiplier)[0] = goal_volume - total_volume;
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index cbdea6e..f1f39a3 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -58,67 +58,58 @@ namespace SAND {
         TimerOutput::Scope t(timer, "initialize");
         {
 
-            TimerOutput::Scope t(timer, "diag stuff");
-            const types::global_dof_index disp_start_index = system_matrix.get_row_indices().block_start(
-                    SolutionBlocks::displacement);
-            const types::global_dof_index disp_mult_start_index = system_matrix.get_row_indices().block_start(
-                    SolutionBlocks::displacement_multiplier);
-            for (auto &pair: boundary_values) {
-                const auto dof_index=pair.first;
-
-                const types::global_dof_index n_u = system_matrix.block(SolutionBlocks::displacement,
-                                                                        SolutionBlocks::displacement).m();
-                if ((dof_index >= disp_start_index) && (dof_index < disp_start_index + n_u)) {
-                    double diag_val = system_matrix.block(SolutionBlocks::displacement,
-                                                          SolutionBlocks::displacement).el(
-                            dof_index - disp_start_index, dof_index - disp_start_index);
-                    system_matrix.block(SolutionBlocks::displacement, SolutionBlocks::displacement_multiplier).set(
-                            dof_index - disp_start_index, dof_index - disp_start_index, diag_val);
-                } else if ((dof_index >= disp_mult_start_index) && (dof_index < disp_mult_start_index + n_u)) {
-                    double diag_val = system_matrix.block(SolutionBlocks::displacement_multiplier,
-                                                          SolutionBlocks::displacement_multiplier).el(
-                            dof_index - disp_mult_start_index, dof_index - disp_mult_start_index);
-                    system_matrix.block(SolutionBlocks::displacement_multiplier, SolutionBlocks::displacement).set(
-                            dof_index - disp_mult_start_index, dof_index - disp_mult_start_index, diag_val);
-                }
-            }
-
-            //set diagonal to 0?
-            for (auto &pair: boundary_values) {
-                const auto dof_index=pair.first;
+            if(Utilities::MPI::this_mpi_process(mpi_communicator) == 0)
+            {
+                TimerOutput::Scope t(timer, "diag stuff");
                 const types::global_dof_index disp_start_index = system_matrix.get_row_indices().block_start(
                         SolutionBlocks::displacement);
                 const types::global_dof_index disp_mult_start_index = system_matrix.get_row_indices().block_start(
                         SolutionBlocks::displacement_multiplier);
-                const types::global_dof_index n_u = system_matrix.block(SolutionBlocks::displacement,
-                                                                        SolutionBlocks::displacement).m();
-                if ((dof_index >= disp_start_index) && (dof_index < disp_start_index + n_u)) {
-                    system_matrix.block(SolutionBlocks::displacement, SolutionBlocks::displacement).set(
-                            dof_index - disp_start_index, dof_index - disp_start_index, 0);
-                } else if ((dof_index >= disp_mult_start_index) && (dof_index < disp_mult_start_index + n_u)) {
-                    system_matrix.block(SolutionBlocks::displacement_multiplier,
-                                        SolutionBlocks::displacement_multiplier).set(
-                            dof_index - disp_mult_start_index, dof_index - disp_mult_start_index, 0);
+                for (auto &pair: boundary_values) {
+                    const auto dof_index=pair.first;
+
+                    const types::global_dof_index n_u = system_matrix.block(SolutionBlocks::displacement,
+                                                                            SolutionBlocks::displacement).m();
+                    if ((dof_index >= disp_start_index) && (dof_index < disp_start_index + n_u)) {
+                        double diag_val = system_matrix.block(SolutionBlocks::displacement,
+                                                              SolutionBlocks::displacement).el(
+                                dof_index - disp_start_index, dof_index - disp_start_index);
+                        system_matrix.block(SolutionBlocks::displacement, SolutionBlocks::displacement_multiplier).set(
+                                dof_index - disp_start_index, dof_index - disp_start_index, diag_val);
+                    } else if ((dof_index >= disp_mult_start_index) && (dof_index < disp_mult_start_index + n_u)) {
+                        double diag_val = system_matrix.block(SolutionBlocks::displacement_multiplier,
+                                                              SolutionBlocks::displacement_multiplier).el(
+                                dof_index - disp_mult_start_index, dof_index - disp_mult_start_index);
+                        system_matrix.block(SolutionBlocks::displacement_multiplier, SolutionBlocks::displacement).set(
+                                dof_index - disp_mult_start_index, dof_index - disp_mult_start_index, diag_val);
+                    }
                 }
-            }
 
-//            system_matrix.compress(VectorOperation::insert);
-
-//            const unsigned int m = a_mat.m();
-//            const unsigned int n = a_mat.n();
-//            std::ofstream Xmat("a_mat_par.csv");
-//            for (unsigned int i = 0; i < m; i++)
-//            {
-//                Xmat << a_mat.el(i, 0);
-//                for (unsigned int j = 1; j < n; j++)
-//                {
-//                    Xmat << "," << a_mat.el(i, j);
-//                }
-//                Xmat << "\n";
-//            }
-//            Xmat.close();
+                //set diagonal to 0?
+                for (auto &pair: boundary_values) {
+                    const auto dof_index=pair.first;
+                    const types::global_dof_index disp_start_index = system_matrix.get_row_indices().block_start(
+                            SolutionBlocks::displacement);
+                    const types::global_dof_index disp_mult_start_index = system_matrix.get_row_indices().block_start(
+                            SolutionBlocks::displacement_multiplier);
+                    const types::global_dof_index n_u = system_matrix.block(SolutionBlocks::displacement,
+                                                                            SolutionBlocks::displacement).m();
+                    if ((dof_index >= disp_start_index) && (dof_index < disp_start_index + n_u)) {
+                        system_matrix.block(SolutionBlocks::displacement, SolutionBlocks::displacement).set(
+                                dof_index - disp_start_index, dof_index - disp_start_index, 0);
+                    } else if ((dof_index >= disp_mult_start_index) && (dof_index < disp_mult_start_index + n_u)) {
+                        system_matrix.block(SolutionBlocks::displacement_multiplier,
+                                            SolutionBlocks::displacement_multiplier).set(
+                                dof_index - disp_mult_start_index, dof_index - disp_mult_start_index, 0);
+                    }
+                }
+            }
 
+            system_matrix.compress(VectorOperation::insert);
         }
+
+
+
         if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
         {
 
@@ -126,12 +117,6 @@ namespace SAND {
         else
         {
             TimerOutput::Scope t(timer, "build A inv");
-
-//            if (Utilities::MPI::this_mpi_process(mpi_communicator) == 0)
-//            {
-//                std::string name = "a_mat";
-//                print_matrix(name, a_mat);
-//            }
             a_inv_direct.initialize(a_mat);
         }
         {

From dacbc27e33748b7b65df77a3f0e33dd14abd22c9 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Thu, 27 Jan 2022 15:23:39 -0700
Subject: [PATCH 68/95] Bug found!

---
 source/density_filter.cc                |   2 -
 source/schur_preconditioner.cc          |  15 +-
 source/schur_preconditioner.cc.autosave | 710 ------------------------
 3 files changed, 9 insertions(+), 718 deletions(-)
 delete mode 100644 source/schur_preconditioner.cc.autosave

diff --git a/source/density_filter.cc b/source/density_filter.cc
index 8fc5562..486c3db 100644
--- a/source/density_filter.cc
+++ b/source/density_filter.cc
@@ -47,7 +47,6 @@ namespace SAND
 
         filter_dsp.reinit(dofs_per_block[0],
                           dofs_per_block[0]);
-        std::cout << "initialized with " << dofs_per_block[0] << std::endl;
         std::set<unsigned int> neighbor_ids;
         std::set<typename DoFHandler<dim>::cell_iterator> cells_to_check;
         std::set<typename DoFHandler<dim>::cell_iterator> cells_to_check_temp;
@@ -142,7 +141,6 @@ namespace SAND
                 }
             }
         }
-        std::cout << "density nonzero entries" << filter_matrix.n_nonzero_elements() << std::endl;
     }
 
     /*This function finds which neighbors are within a certain radius of the initial cell.*/
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index f1f39a3..0bbbf7a 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -57,26 +57,30 @@ namespace SAND {
     {
         TimerOutput::Scope t(timer, "initialize");
         {
-
-            if(Utilities::MPI::this_mpi_process(mpi_communicator) == 0)
-            {
                 TimerOutput::Scope t(timer, "diag stuff");
                 const types::global_dof_index disp_start_index = system_matrix.get_row_indices().block_start(
                         SolutionBlocks::displacement);
                 const types::global_dof_index disp_mult_start_index = system_matrix.get_row_indices().block_start(
                         SolutionBlocks::displacement_multiplier);
+                std::cout << "displacement start: " << disp_start_index << std::endl;
+                std::cout << "displacement multiplier start: " << disp_mult_start_index << std::endl;
+
                 for (auto &pair: boundary_values) {
                     const auto dof_index=pair.first;
-
+                    std::cout << "constraint index: " << dof_index << " on process: " << Utilities::MPI::this_mpi_process(mpi_communicator) << std::endl;
                     const types::global_dof_index n_u = system_matrix.block(SolutionBlocks::displacement,
                                                                             SolutionBlocks::displacement).m();
                     if ((dof_index >= disp_start_index) && (dof_index < disp_start_index + n_u)) {
                         double diag_val = system_matrix.block(SolutionBlocks::displacement,
                                                               SolutionBlocks::displacement).el(
                                 dof_index - disp_start_index, dof_index - disp_start_index);
+
                         system_matrix.block(SolutionBlocks::displacement, SolutionBlocks::displacement_multiplier).set(
                                 dof_index - disp_start_index, dof_index - disp_start_index, diag_val);
-                    } else if ((dof_index >= disp_mult_start_index) && (dof_index < disp_mult_start_index + n_u)) {
+
+                    }
+                    else if ((dof_index >= disp_mult_start_index) && (dof_index < disp_mult_start_index + n_u))
+                    {
                         double diag_val = system_matrix.block(SolutionBlocks::displacement_multiplier,
                                                               SolutionBlocks::displacement_multiplier).el(
                                 dof_index - disp_mult_start_index, dof_index - disp_mult_start_index);
@@ -103,7 +107,6 @@ namespace SAND {
                                 dof_index - disp_mult_start_index, dof_index - disp_mult_start_index, 0);
                     }
                 }
-            }
 
             system_matrix.compress(VectorOperation::insert);
         }
diff --git a/source/schur_preconditioner.cc.autosave b/source/schur_preconditioner.cc.autosave
deleted file mode 100644
index cbdea6e..0000000
--- a/source/schur_preconditioner.cc.autosave
+++ /dev/null
@@ -1,710 +0,0 @@
-//
-// Created by justin on 2/17/21.
-//
-#include <deal.II/lac/linear_operator.h>
-#include <deal.II/lac/sparse_direct.h>
-#include <deal.II/lac/sparse_matrix.h>
-#include <deal.II/base/timer.h>
-#include <deal.II/lac/generic_linear_algebra.h>
-#include <deal.II/lac/trilinos_solver.h>
-#include <deal.II/lac/trilinos_precondition.h>
-#include "../include/schur_preconditioner.h"
-#include "../include/input_information.h"
-#include "../include/sand_tools.h"
-#include <fstream>
-
-namespace SAND {
-    using MatrixType  = dealii::TrilinosWrappers::SparseMatrix;
-    using VectorType  = dealii::TrilinosWrappers::MPI::Vector;
-    using PayloadType = dealii::TrilinosWrappers::internal::LinearOperatorImplementation::TrilinosPayload;
-    using PayloadVectorType = typename PayloadType::VectorType;
-    using size_type         = dealii::types::global_dof_index;
-    using namespace dealii;
-    template<int dim>
-    TopOptSchurPreconditioner<dim>::TopOptSchurPreconditioner(LA::MPI::BlockSparseMatrix &matrix_in)
-            :
-            system_matrix(matrix_in),
-            mpi_communicator(MPI_COMM_WORLD),
-            n_rows(0),
-            n_columns(0),
-            n_block_rows(0),
-            n_block_columns(0),
-            other_solver_control(100000, 1e-6),
-            other_bicgstab(other_solver_control),
-            other_gmres(other_solver_control),
-            other_cg(other_solver_control),
-            a_mat(matrix_in.block(SolutionBlocks::displacement, SolutionBlocks::displacement_multiplier)),
-            b_mat(matrix_in.block(SolutionBlocks::density, SolutionBlocks::density)),
-            c_mat(matrix_in.block(SolutionBlocks::displacement,SolutionBlocks::density)),
-            e_mat(matrix_in.block(SolutionBlocks::displacement_multiplier,SolutionBlocks::density)),
-            f_mat(matrix_in.block(SolutionBlocks::unfiltered_density_multiplier,SolutionBlocks::unfiltered_density)),
-            d_m_mat(matrix_in.block(SolutionBlocks::density_upper_slack_multiplier, SolutionBlocks::density_upper_slack)),
-            d_1_mat(matrix_in.block(SolutionBlocks::density_lower_slack, SolutionBlocks::density_lower_slack)),
-            d_2_mat(matrix_in.block(SolutionBlocks::density_upper_slack, SolutionBlocks::density_upper_slack)),
-            m_vect(matrix_in.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier)),
-            solver_type("Amesos_Klu"),
-            additional_data(false, solver_type),
-            direct_solver_control(1, 0),
-            a_inv_direct(direct_solver_control, additional_data),
-            pcout(std::cout,(Utilities::MPI::this_mpi_process(mpi_communicator) == 0)),
-            timer(pcout, TimerOutput::summary, TimerOutput::wall_times)
-    {
-
-    }
-
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::initialize(LA::MPI::BlockSparseMatrix &matrix, const std::map<types::global_dof_index, double> &boundary_values,const DoFHandler<dim> &dof_handler, const LA::MPI::BlockVector &distributed_state)
-    {
-        TimerOutput::Scope t(timer, "initialize");
-        {
-
-            TimerOutput::Scope t(timer, "diag stuff");
-            const types::global_dof_index disp_start_index = system_matrix.get_row_indices().block_start(
-                    SolutionBlocks::displacement);
-            const types::global_dof_index disp_mult_start_index = system_matrix.get_row_indices().block_start(
-                    SolutionBlocks::displacement_multiplier);
-            for (auto &pair: boundary_values) {
-                const auto dof_index=pair.first;
-
-                const types::global_dof_index n_u = system_matrix.block(SolutionBlocks::displacement,
-                                                                        SolutionBlocks::displacement).m();
-                if ((dof_index >= disp_start_index) && (dof_index < disp_start_index + n_u)) {
-                    double diag_val = system_matrix.block(SolutionBlocks::displacement,
-                                                          SolutionBlocks::displacement).el(
-                            dof_index - disp_start_index, dof_index - disp_start_index);
-                    system_matrix.block(SolutionBlocks::displacement, SolutionBlocks::displacement_multiplier).set(
-                            dof_index - disp_start_index, dof_index - disp_start_index, diag_val);
-                } else if ((dof_index >= disp_mult_start_index) && (dof_index < disp_mult_start_index + n_u)) {
-                    double diag_val = system_matrix.block(SolutionBlocks::displacement_multiplier,
-                                                          SolutionBlocks::displacement_multiplier).el(
-                            dof_index - disp_mult_start_index, dof_index - disp_mult_start_index);
-                    system_matrix.block(SolutionBlocks::displacement_multiplier, SolutionBlocks::displacement).set(
-                            dof_index - disp_mult_start_index, dof_index - disp_mult_start_index, diag_val);
-                }
-            }
-
-            //set diagonal to 0?
-            for (auto &pair: boundary_values) {
-                const auto dof_index=pair.first;
-                const types::global_dof_index disp_start_index = system_matrix.get_row_indices().block_start(
-                        SolutionBlocks::displacement);
-                const types::global_dof_index disp_mult_start_index = system_matrix.get_row_indices().block_start(
-                        SolutionBlocks::displacement_multiplier);
-                const types::global_dof_index n_u = system_matrix.block(SolutionBlocks::displacement,
-                                                                        SolutionBlocks::displacement).m();
-                if ((dof_index >= disp_start_index) && (dof_index < disp_start_index + n_u)) {
-                    system_matrix.block(SolutionBlocks::displacement, SolutionBlocks::displacement).set(
-                            dof_index - disp_start_index, dof_index - disp_start_index, 0);
-                } else if ((dof_index >= disp_mult_start_index) && (dof_index < disp_mult_start_index + n_u)) {
-                    system_matrix.block(SolutionBlocks::displacement_multiplier,
-                                        SolutionBlocks::displacement_multiplier).set(
-                            dof_index - disp_mult_start_index, dof_index - disp_mult_start_index, 0);
-                }
-            }
-
-//            system_matrix.compress(VectorOperation::insert);
-
-//            const unsigned int m = a_mat.m();
-//            const unsigned int n = a_mat.n();
-//            std::ofstream Xmat("a_mat_par.csv");
-//            for (unsigned int i = 0; i < m; i++)
-//            {
-//                Xmat << a_mat.el(i, 0);
-//                for (unsigned int j = 1; j < n; j++)
-//                {
-//                    Xmat << "," << a_mat.el(i, j);
-//                }
-//                Xmat << "\n";
-//            }
-//            Xmat.close();
-
-        }
-        if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
-        {
-
-        }
-        else
-        {
-            TimerOutput::Scope t(timer, "build A inv");
-
-//            if (Utilities::MPI::this_mpi_process(mpi_communicator) == 0)
-//            {
-//                std::string name = "a_mat";
-//                print_matrix(name, a_mat);
-//            }
-            a_inv_direct.initialize(a_mat);
-        }
-        {
-            TimerOutput::Scope t(timer, "reinit diag matrices");
-            d_3_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
-            d_4_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
-            d_5_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
-            d_6_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
-            d_7_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
-            d_8_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
-            d_m_inv_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
-            d_3_mat=0;
-            d_4_mat=0;
-            d_5_mat=0;
-            d_6_mat=0;
-            d_7_mat=0;
-            d_8_mat=0;
-            d_m_inv_mat=0;
-            pcout << "diag reinit" << std::endl;
-        }
-        {
-            const types::global_dof_index n_p = system_matrix.block(SolutionBlocks::density,
-                                                                    SolutionBlocks::density).m();
-
-            pcout << "np: " << n_p << std::endl;
-
-
-
-
-            std::vector<double> l_global(n_p);
-            std::vector<double> lm_global(n_p);
-            std::vector<double> u_global(n_p);
-            std::vector<double> um_global(n_p);
-            std::vector<double> m_global(n_p);
-
-            std::vector<double> l(n_p);
-            std::vector<double> lm(n_p);
-            std::vector<double> u(n_p);
-            std::vector<double> um(n_p);
-            std::vector<double> m(n_p);
-
-            TimerOutput::Scope t(timer, "build diag matrices");
-            for (const auto cell: dof_handler.active_cell_iterators())
-            {
-                if(cell->is_locally_owned())
-                {
-                    std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
-                    cell->get_dof_indices(i);
-
-
-
-                    const int i_ind = cell->get_fe().component_to_system_index(0,0);
-
-                    if(distributed_state.block(SolutionBlocks::density_lower_slack_multiplier).in_local_range(i[i_ind]))
-                    {
-                        lm[i[i_ind]] = distributed_state.block(SolutionBlocks::density_lower_slack_multiplier)[i[i_ind]];
-                        m[i[i_ind]] = cell->measure();
-                    }
-
-                    if(distributed_state.block(SolutionBlocks::density_lower_slack).in_local_range(i[i_ind]))
-                    {
-                        l[i[i_ind]] =  distributed_state.block(SolutionBlocks::density_lower_slack)[i[i_ind]];
-                    }
-
-                    if(distributed_state.block(SolutionBlocks::density_upper_slack_multiplier).in_local_range(i[i_ind]))
-                    {
-                        um[i[i_ind]]= distributed_state.block(SolutionBlocks::density_upper_slack_multiplier)[i[i_ind]];
-                    }
-
-                    if(distributed_state.block(SolutionBlocks::density_upper_slack).in_local_range(i[i_ind]))
-                    {
-                        u[i[i_ind]] = distributed_state.block(SolutionBlocks::density_upper_slack)[i[i_ind]];
-                    }
-                }
-            }
-
-            MPI_Allreduce(lm.data(), lm_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-            MPI_Allreduce(l.data(), l_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-            MPI_Allreduce(um.data(), um_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-            MPI_Allreduce(u.data(), u_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-            MPI_Allreduce(m.data(), m_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-
-            for (unsigned int k=0; k< n_p; k++)
-            {
-                    if(distributed_state.block(0).in_local_range(k))
-                    {
-                        d_3_mat.set(k, k, -1 * lm_global[k]/(m_global[k]*l_global[k]));
-                        d_4_mat.set(k, k, -1 * um_global[k]/(m_global[k]*u_global[k]));
-                        d_5_mat.set(k, k, lm_global[k]/l_global[k]);
-                        d_6_mat.set(k, k, um_global[k]/u_global[k]);
-                        d_7_mat.set(k, k, m_global[k]*(lm_global[k]*u_global[k] + um_global[k]*l_global[k])/(l_global[k]*u_global[k]));
-                        d_8_mat.set(k, k, l_global[k]*u_global[k]/(m_global[k]*(lm_global[k]*u_global[k] + um_global[k]*l_global[k])));
-                        d_m_inv_mat.set(k, k, 1 / m_global[k]);
-                    }
-            }
-        }
-        d_3_mat.compress(VectorOperation::insert);
-        d_4_mat.compress(VectorOperation::insert);
-        d_5_mat.compress(VectorOperation::insert);
-        d_6_mat.compress(VectorOperation::insert);
-        d_7_mat.compress(VectorOperation::insert);
-        d_8_mat.compress(VectorOperation::insert);
-        d_m_inv_mat.compress(VectorOperation::insert);
-
-        pcout << "compressed" << std::endl;
-
-        pre_j=distributed_state.block(SolutionBlocks::density);
-        pre_k=distributed_state.block(SolutionBlocks::density);
-        g_d_m_inv_density=distributed_state.block(SolutionBlocks::density);
-        k_g_d_m_inv_density=distributed_state.block(SolutionBlocks::density);
-
-
-        op_d_8 = linear_operator<VectorType,VectorType,PayloadType>(d_8_mat);
-        op_f = linear_operator<VectorType,VectorType,PayloadType>(f_mat);
-        op_b = linear_operator<VectorType,VectorType,PayloadType>(b_mat);
-        op_c = linear_operator<VectorType,VectorType,PayloadType>(c_mat);
-        op_a_inv = linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct);
-        op_e = linear_operator<VectorType,VectorType,PayloadType>(e_mat);
-        op_d_m= linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
-        op_d_m_inv= linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat);
-
-        op_g = op_f * op_d_8 *
-                transpose_operator(op_f);
-
-
-        op_h = op_b
-               - transpose_operator(op_c) * op_a_inv * op_e
-               - transpose_operator(op_e) * op_a_inv * op_c;
-
-//        TrilinosWrappers::internal::LinearOperatorImplementation::TrilinosPayload inv_payload = inverse_payload(TrilinosWrappers::SolverGMRES(),TrilinosWrappers::PreconditionIdentity());
-
-        op_k_inv = (-1 * op_g *op_d_m_inv * op_h - op_d_m);
-
-
-
-
-    }
-
-
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::vmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
-        LA::MPI::BlockVector temp_src;
-        pcout << "vmult" << std::endl;
-        {
-            TimerOutput::Scope t(timer, "part 1");
-            vmult_step_1(dst, src);
-            temp_src = dst;
-        }
-
-        {
-            TimerOutput::Scope t(timer, "part 2");
-            vmult_step_2(dst, temp_src);
-            temp_src = dst;
-        }
-
-        {
-            TimerOutput::Scope t(timer, "part 3");
-            vmult_step_3(dst, temp_src);
-            temp_src = dst;
-        }
-        {
-            TimerOutput::Scope t(timer, "part 4");
-            vmult_step_4(dst, temp_src);
-            temp_src = dst;
-        }
-        vmult_step_5(dst, temp_src);        
-    }
-
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::Tvmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
-        dst = src;
-    }
-
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::vmult_add(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
-        LA::MPI::BlockVector dst_temp = dst;
-        vmult(dst_temp, src);
-        dst += dst_temp;
-    }
-
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::Tvmult_add(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
-        dst = src;
-    }
-
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::vmult_step_1(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
-        dst = src;
-        auto dst_temp = dst;
-        dst.block(SolutionBlocks::unfiltered_density) = dst_temp.block(SolutionBlocks::unfiltered_density)
-                - linear_operator<VectorType,VectorType,PayloadType>(d_5_mat) * src.block(SolutionBlocks::density_lower_slack_multiplier)
-                + linear_operator<VectorType,VectorType,PayloadType>(d_6_mat) * src.block(SolutionBlocks::density_upper_slack_multiplier)
-                + src.block(SolutionBlocks::density_lower_slack)
-                - src.block(SolutionBlocks::density_upper_slack);
-    }
-
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::vmult_step_2(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
-        dst = src;
-        auto dst_temp = dst;
-
-        dst.block(SolutionBlocks::unfiltered_density_multiplier) = dst_temp.block(SolutionBlocks::unfiltered_density_multiplier)
-                - linear_operator<VectorType,VectorType,PayloadType>(f_mat)*linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) * src.block(SolutionBlocks::unfiltered_density);
-
-    }
-
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::vmult_step_3(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
-        dst = src;
-
-        if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
-        {
-            SolverControl            solver_control_1(100000, 1e-6);
-            SolverControl            solver_control_2(100000, 1e-6);
-            SolverCG<LA::MPI::Vector> a_solver_cg_1(solver_control_1);
-            SolverCG<LA::MPI::Vector> a_solver_cg_2(solver_control_2);
-            auto dst_temp = dst;
-            a_solver_cg_1.solve(a_mat,dst_temp.block(SolutionBlocks::displacement_multiplier),src.block(SolutionBlocks::displacement_multiplier),PreconditionIdentity());
-            a_solver_cg_2.solve(a_mat,dst_temp.block(SolutionBlocks::displacement),src.block(SolutionBlocks::displacement),PreconditionIdentity());
-            c_mat.Tvmult(dst_temp.block(SolutionBlocks::density_upper_slack),dst_temp.block(SolutionBlocks::displacement_multiplier));
-            e_mat.Tvmult(dst_temp.block(SolutionBlocks::density_lower_slack),dst_temp.block(SolutionBlocks::displacement));
-
-            dst.block(SolutionBlocks::density) = dst_temp.block(SolutionBlocks::density) - dst_temp.block(SolutionBlocks::density_upper_slack) - dst_temp.block(SolutionBlocks::density_lower_slack);
-
-
-        }
-        else
-        {
-            auto dst_temp = dst;
-            dst.block(SolutionBlocks::density) = dst_temp.block(SolutionBlocks::density)
-                    - transpose_operator<VectorType,VectorType,PayloadType>(e_mat) * linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct)* src.block(SolutionBlocks::displacement)
-                    - transpose_operator<VectorType,VectorType,PayloadType>(c_mat) * linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct)* src.block(SolutionBlocks::displacement_multiplier);
-        }
-
-    }
-
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::vmult_step_4(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
-        dst = src;
-        auto dst_temp = dst;
-        auto k_density_mult =  src.block(SolutionBlocks::density);
-
-        if (Input::solver_choice == SolverOptions::exact_preconditioner_with_gmres)
-        {
-//            g_d_m_inv_density = linear_operator<VectorType,VectorType,PayloadType>(g_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
-//            k_g_d_m_inv_density = linear_operator<VectorType,VectorType,PayloadType>(k_mat) * g_d_m_inv_density;
-//            k_density_mult = linear_operator<VectorType,VectorType,PayloadType>(k_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
-        }
-
-        else if (Input::solver_choice == SolverOptions::inexact_K_with_exact_A_gmres)
-        {
-
-            g_d_m_inv_density = op_g * op_d_m_inv * src.block(SolutionBlocks::density);
-            SolverControl step_4_gmres_control_1 (100000, g_d_m_inv_density.l2_norm()*1e-6);
-            SolverGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
-            try {
-                TimerOutput::Scope u(timer, "actual inv");
-//                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
-//                                      g_d_m_inv_density;
-                step_4_gmres_1.solve(op_k_inv,k_g_d_m_inv_density,g_d_m_inv_density, PreconditionIdentity() );
-            }
-            catch (std::exception &exc)
-            {
-                std::cerr << "Failure of linear solver step_4_gmres_1" << std::endl;
-                pcout << "first residual: " << step_4_gmres_control_1.initial_value() << std::endl;
-                pcout << "last residual: " << step_4_gmres_control_1.last_value() << std::endl;
-                throw;
-            }
-            SolverControl step_4_gmres_control_2 (100000, src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6);
-            SolverGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
-            try {
-                TimerOutput::Scope u(timer, "actual inv");
-//                k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, PreconditionIdentity()) *
-//                                 src.block(SolutionBlocks::unfiltered_density_multiplier);
-                step_4_gmres_2.solve(op_k_inv,k_density_mult,src.block(SolutionBlocks::unfiltered_density_multiplier), PreconditionIdentity() );
-            } catch (std::exception &exc)
-            {
-                std::cerr << "Failure of linear solver step_4_gmres_2" << std::endl;
-                pcout << "first residual: " << step_4_gmres_control_2.initial_value() << std::endl;
-                pcout << "last residual: " << step_4_gmres_control_2.last_value() << std::endl;
-                throw;
-            }
-        }
-        else if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
-        {
-
-            SolverControl            solver_control(100000, 1e-6);
-            SolverCG<LA::MPI::Vector> a_solver_cg(solver_control);
-
-            auto preconditioner = PreconditionIdentity();
-
-            auto a_inv_op = op_d_8;
-
-                    inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg, preconditioner);
-
-
-            auto op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) *
-                        transpose_operator<VectorType, VectorType, PayloadType>(f_mat);
-
-            auto op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
-                        - transpose_operator<VectorType, VectorType, PayloadType>(c_mat) * a_inv_op * linear_operator<VectorType,VectorType,PayloadType>(e_mat)
-                        - transpose_operator<VectorType, VectorType, PayloadType>(e_mat) * a_inv_op * linear_operator<VectorType,VectorType,PayloadType>(c_mat);
-
-            auto op_k_inv = op_d_8;
-
-                    -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
-
-            SolverControl step_4_gmres_control_1 (10000, std::max(g_d_m_inv_density.l2_norm()*1e-6,1e-6));
-            SolverGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
-            try {
-//                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
-//                                      g_d_m_inv_density;
-//                step_4_gmres_1.solve(op_k_inv,k_g_d_m_inv_density,g_d_m_inv_density, PreconditionIdentity() );
-            } catch (std::exception &exc)
-            {
-                std::cerr << "Failure of linear solver step_4_gmres_1" << std::endl;
-                pcout << "first residual: " << step_4_gmres_control_1.initial_value() << std::endl;
-                pcout << "last residual: " << step_4_gmres_control_1.last_value() << std::endl;
-                throw;
-            }
-
-            SolverControl step_4_gmres_control_2 (10000, std::max(src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6,1e-6));
-            SolverGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
-            try {
-
-                k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, PreconditionIdentity()) *
-                                 src.block(SolutionBlocks::unfiltered_density_multiplier);
-            } catch (std::exception &exc)
-            {
-                std::cerr << "Failure of linear solver step_4_gmres_2" << std::endl;
-                pcout << "first residual: " << step_4_gmres_control_2.initial_value() << std::endl;
-                pcout << "last residual: " << step_4_gmres_control_2.last_value() << std::endl;
-                throw;
-            }
-        }
-        else
-        {
-            pcout << "shouldn't get here";
-            throw;
-        }
-
-        dst.block(SolutionBlocks::total_volume_multiplier) = transpose_operator<VectorType, VectorType, PayloadType>(m_vect)*k_g_d_m_inv_density
-                                                        - transpose_operator<VectorType, VectorType, PayloadType>(m_vect)*k_density_mult
-                                                        +dst_temp.block(SolutionBlocks::total_volume_multiplier);
-    }
-
-
-
-
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::vmult_step_5(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
-        {
-            //First Block Inverse
-            TimerOutput::Scope t(timer, "inverse 1");
-            dst.block(SolutionBlocks::density_lower_slack_multiplier) = linear_operator<VectorType,VectorType,PayloadType>(d_3_mat) * src.block(SolutionBlocks::density_lower_slack_multiplier) +
-                    linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density_lower_slack);
-            dst.block(SolutionBlocks::density_upper_slack_multiplier) = linear_operator<VectorType,VectorType,PayloadType>(d_4_mat) * src.block(SolutionBlocks::density_upper_slack_multiplier) +
-                    linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density_upper_slack);
-            dst.block(SolutionBlocks::density_lower_slack) = linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density_lower_slack_multiplier);
-            dst.block(SolutionBlocks::density_upper_slack) = linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density_upper_slack_multiplier);
-        }
-
-
-        {
-            //Second Block Inverse
-            TimerOutput::Scope t(timer, "inverse 2");
-            dst.block(SolutionBlocks::unfiltered_density) =
-                    linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) * src.block(SolutionBlocks::unfiltered_density);
-        }
-
-
-        {
-            //Third Block Inverse
-            TimerOutput::Scope t(timer, "inverse 3");
-            if(Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
-            {
-                SolverControl            solver_control(100000, 1e-6);
-                SolverCG<LA::MPI::Vector> a_solver_cg(solver_control);
-
-                auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg, PreconditionIdentity());
-
-                dst.block(SolutionBlocks::displacement) = a_inv_op * src.block(SolutionBlocks::displacement_multiplier);
-                dst.block(SolutionBlocks::displacement_multiplier) = a_inv_op * src.block(SolutionBlocks::displacement);
-            } else
-            {
-                dst.block(SolutionBlocks::displacement) = linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct) * src.block(SolutionBlocks::displacement_multiplier);
-                dst.block(SolutionBlocks::displacement_multiplier) = linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct) * src.block(SolutionBlocks::displacement);
-            }
-
-        }
-
-
-        {
-            //Fourth (ugly) Block Inverse
-            TimerOutput::Scope t(timer, "inverse 4");
-
-
-
-            if (Input::solver_choice == SolverOptions::exact_preconditioner_with_gmres)
-            {
-//                pre_j = src.block(SolutionBlocks::density) + linear_operator<VectorType,VectorType,PayloadType>(h_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
-//                pre_k = -1* linear_operator<VectorType,VectorType,PayloadType>(g_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density) + src.block(SolutionBlocks::unfiltered_density_multiplier);
-//                dst.block(SolutionBlocks::unfiltered_density_multiplier) = transpose_operator<VectorType, VectorType, PayloadType>(k_mat) * pre_j;
-//                dst.block(SolutionBlocks::density) = linear_operator<VectorType,VectorType,PayloadType>(k_mat) * pre_k;
-            }
-
-            else if (Input::solver_choice == SolverOptions::inexact_K_with_exact_A_gmres)
-            {
-                auto pre_pre_k = pre_k;
-                {
-                    TimerOutput::Scope t(timer, "not inverse 5.1");
-                    pre_j = src.block(SolutionBlocks::density) + op_h * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
-                    pre_pre_k = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
-                    pre_k =  pre_pre_k + src.block(SolutionBlocks::unfiltered_density_multiplier);
-                }
-                SolverControl step_5_gmres_control_1 (100000, pre_j.l2_norm()*1e-6);
-                SolverGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
-                try {
-                    TimerOutput::Scope t(timer, "actual inverse 5.1");
-//                    dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
-//                                                                               pre_j;
-                    step_5_gmres_1.solve(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), dst.block(SolutionBlocks::unfiltered_density_multiplier), pre_j , PreconditionIdentity());
-                } catch (std::exception &exc)
-                {
-                    std::cerr << "Failure of linear solver step_5_gmres_1 again" << std::endl;
-                    pcout << "first residual: " << step_5_gmres_control_1.initial_value() << std::endl;
-                    pcout << "last residual: " << step_5_gmres_control_1.last_value() << std::endl;
-                    throw;
-                }
-
-
-                SolverControl step_5_gmres_control_2 (100000, pre_k.l2_norm()*1e-6);
-                SolverGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
-                try {
-                    TimerOutput::Scope t(timer, "actual inverse 5.2");
-//                    dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) * pre_k;
-                    step_5_gmres_2.solve(op_k_inv,dst.block(SolutionBlocks::density), pre_k , PreconditionIdentity());
-                } catch (std::exception &exc)
-                {
-                    std::cerr << "Failure of linear solver step_5_gmres_2" << std::endl;
-                    pcout << "first residual: " << step_5_gmres_control_2.initial_value() << std::endl;
-                    pcout << "last residual: " << step_5_gmres_control_2.last_value() << std::endl;
-                    throw;
-                }
-            }
-            else if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
-            {
-
-                SolverControl            solver_control(100000, 1e-6);
-                SolverCG<LA::MPI::Vector> a_solver_cg (solver_control);
-                auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg, PreconditionIdentity());
-
-                auto op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) *
-                            transpose_operator<VectorType, VectorType, PayloadType>(f_mat);
-
-                auto op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
-                            - transpose_operator<VectorType, VectorType, PayloadType>(c_mat) * a_inv_op * linear_operator<VectorType,VectorType,PayloadType>(e_mat)
-                            - transpose_operator<VectorType, VectorType, PayloadType>(e_mat) * a_inv_op * linear_operator<VectorType,VectorType,PayloadType>(c_mat);
-
-                auto op_k_inv = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
-
-                pre_j = src.block(SolutionBlocks::density) + op_h * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
-                pre_k = -1* op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density) + src.block(SolutionBlocks::unfiltered_density_multiplier);
-
-                SolverControl step_5_gmres_control_1 (10000, std::max(pre_j.l2_norm()*1e-6,1e-6));
-                TrilinosWrappers::SolverGMRES step_5_gmres_1 (step_5_gmres_control_1);
-                try {
-                    dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType,VectorType,PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
-                                                                               pre_j;
-                } catch (std::exception &exc)
-                {
-                    std::cerr << "Failure of linear solver step_5_gmres_1" << std::endl;
-                    pcout << "first residual: " << step_5_gmres_control_1.initial_value() << std::endl;
-                    pcout << "last residual: " << step_5_gmres_control_1.last_value() << std::endl;
-                    throw;
-                }
-
-                SolverControl step_5_gmres_control_2 (10000, std::max(pre_k.l2_norm()*1e-6,1e-6));
-                TrilinosWrappers::SolverGMRES step_5_gmres_2 (step_5_gmres_control_2);
-                try {
-                    dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) *
-                                                         pre_k;
-                } catch (std::exception &exc)
-                {
-                    std::cerr << "Failure of linear solver step_5_gmres_2" << std::endl;
-                    pcout << "first residual: " << step_5_gmres_control_2.initial_value() << std::endl;
-                    pcout << "last residual: " << step_5_gmres_control_2.last_value() << std::endl;
-                    throw;
-                }
-            }
-            else
-            {
-                pcout << "shouldn't get here";
-                throw;
-            }
-
-        }
-        {
-            dst.block(SolutionBlocks::total_volume_multiplier) = src.block(SolutionBlocks::total_volume_multiplier);
-        }
-    }
-
-
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::print_stuff()
-    {
-
-//        print_matrix(std::string("OAmat.csv"),a_mat);
-//        print_matrix(std::string("OBmat.csv"),b_mat);
-//        print_matrix(std::string("OCmat.csv"),c_mat);
-//        print_matrix(std::string("OEmat.csv"),e_mat);
-//        print_matrix(std::string("OFmat.csv"),f_mat);
-        FullMatrix<double> g_mat;
-        FullMatrix<double> h_mat;
-        FullMatrix<double> k_inv_mat;
-        g_mat.reinit(b_mat.m(),b_mat.n());
-        h_mat.reinit(b_mat.m(),b_mat.n());
-        k_inv_mat.reinit(b_mat.m(),b_mat.n());
-        auto op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) *
-                    transpose_operator(linear_operator<VectorType,VectorType,PayloadType>(f_mat));
-
-        auto op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
-                    - transpose_operator(linear_operator<VectorType,VectorType,PayloadType>(c_mat)) * linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct) * linear_operator<VectorType,VectorType,PayloadType>(e_mat)
-                    - transpose_operator(linear_operator<VectorType,VectorType,PayloadType>(e_mat)) * linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct) * linear_operator<VectorType,VectorType,PayloadType>(c_mat);
-
-        auto op_k_inv = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
-//        build_matrix_element_by_element(op_g,g_mat, src.block(SolutionBlocks::density));
-//        build_matrix_element_by_element(op_h,h_mat, src.block(SolutionBlocks::density));
-//        build_matrix_element_by_element(op_k_inv,k_inv_mat, src.block(SolutionBlocks::density));
-//        print_matrix(std::string("OGmat.csv"),g_mat);
-//        print_matrix(std::string("OHmat.csv"),h_mat);
-//        print_matrix(std::string("OKinvmat.csv"),k_inv_mat);
-
-
-    }
-
-    void VmultTrilinosSolverDirect::initialize(LA::MPI::SparseMatrix &a_mat)
-    {
-        reinit(a_mat);
-        solver_direct.initialize(a_mat);
-        size = a_mat.n();
-    }
-
-    void
-    VmultTrilinosSolverDirect::vmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
-    {
-        solver_direct.solve(dst, src);
-    }
-
-    void VmultTrilinosSolverDirect::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
-    {
-        solver_direct.solve(dst, src);
-    }
-
-    void
-    VmultTrilinosSolverDirect::Tvmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
-    {
-        solver_direct.solve(dst, src);
-    }
-
-    void VmultTrilinosSolverDirect::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
-    {
-        solver_direct.solve(dst, src);
-    }
-
-
-    VmultTrilinosSolverDirect::VmultTrilinosSolverDirect(SolverControl &cn,
-                     const TrilinosWrappers::SolverDirect::AdditionalData &data)
-    : solver_direct(cn, data)
-    {
-
-    }
-
-}
-template class SAND::TopOptSchurPreconditioner<2>;
-template class SAND::TopOptSchurPreconditioner<3>;

From eb19e4f73d2983242f303ff64459b165ecb8dcd6 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Thu, 27 Jan 2022 15:26:34 -0700
Subject: [PATCH 69/95] ready to run on up

---
 include/input_information.h    | 2 +-
 source/schur_preconditioner.cc | 3 ---
 2 files changed, 1 insertion(+), 4 deletions(-)

diff --git a/include/input_information.h b/include/input_information.h
index d23785b..0a1fb61 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -17,7 +17,7 @@ namespace SAND {
         //geometry options
         constexpr unsigned int geometry_base = GeometryOptions::mbb;
 
-        constexpr unsigned int dim = 2;
+        constexpr unsigned int dim = 3;
         constexpr unsigned int refinements =  5;
 
         //nonlinear algorithm options
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 0bbbf7a..b1deb80 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -62,12 +62,9 @@ namespace SAND {
                         SolutionBlocks::displacement);
                 const types::global_dof_index disp_mult_start_index = system_matrix.get_row_indices().block_start(
                         SolutionBlocks::displacement_multiplier);
-                std::cout << "displacement start: " << disp_start_index << std::endl;
-                std::cout << "displacement multiplier start: " << disp_mult_start_index << std::endl;
 
                 for (auto &pair: boundary_values) {
                     const auto dof_index=pair.first;
-                    std::cout << "constraint index: " << dof_index << " on process: " << Utilities::MPI::this_mpi_process(mpi_communicator) << std::endl;
                     const types::global_dof_index n_u = system_matrix.block(SolutionBlocks::displacement,
                                                                             SolutionBlocks::displacement).m();
                     if ((dof_index >= disp_start_index) && (dof_index < disp_start_index + n_u)) {

From bdbb45542eaf1ffc3baa15a327600dde120dbf05 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Fri, 28 Jan 2022 10:26:39 -0700
Subject: [PATCH 70/95] making new branch testing trilinos solvers

---
 include/input_information.h | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/include/input_information.h b/include/input_information.h
index 0a1fb61..6ec8e3a 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -17,8 +17,8 @@ namespace SAND {
         //geometry options
         constexpr unsigned int geometry_base = GeometryOptions::mbb;
 
-        constexpr unsigned int dim = 3;
-        constexpr unsigned int refinements =  5;
+        constexpr unsigned int dim = 2;
+        constexpr unsigned int refinements =  4;
 
         //nonlinear algorithm options
         constexpr double initial_barrier_size = 25;
@@ -31,7 +31,7 @@ namespace SAND {
         constexpr double required_norm = .0001;
 
         //density filter options
-        constexpr double filter_r = .251;
+        constexpr double filter_r = .1;
 
         //other options
         constexpr double density_penalty_exponent = 3;

From 3cd48c974be293623e7e6f05e5a8af6bbee7b17e Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Wed, 2 Feb 2022 10:35:36 -0700
Subject: [PATCH 71/95] looks good

---
 include/input_information.h    |  6 +++---
 source/schur_preconditioner.cc | 35 +++++++++++++++++++---------------
 2 files changed, 23 insertions(+), 18 deletions(-)

diff --git a/include/input_information.h b/include/input_information.h
index 6ec8e3a..972ee6a 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -17,8 +17,8 @@ namespace SAND {
         //geometry options
         constexpr unsigned int geometry_base = GeometryOptions::mbb;
 
-        constexpr unsigned int dim = 2;
-        constexpr unsigned int refinements =  4;
+        constexpr unsigned int dim = 3;
+        constexpr unsigned int refinements =  3;
 
         //nonlinear algorithm options
         constexpr double initial_barrier_size = 25;
@@ -31,7 +31,7 @@ namespace SAND {
         constexpr double required_norm = .0001;
 
         //density filter options
-        constexpr double filter_r = .1;
+        constexpr double filter_r = .25;
 
         //other options
         constexpr double density_penalty_exponent = 3;
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index b1deb80..fb61b56 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -359,6 +359,8 @@ namespace SAND {
         auto dst_temp = dst;
         auto k_density_mult =  src.block(SolutionBlocks::density);
 
+        TrilinosWrappers::PreconditionIdentity preconditioner;
+        preconditioner.initialize(b_mat);
         if (Input::solver_choice == SolverOptions::exact_preconditioner_with_gmres)
         {
 //            g_d_m_inv_density = linear_operator<VectorType,VectorType,PayloadType>(g_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
@@ -371,12 +373,12 @@ namespace SAND {
 
             g_d_m_inv_density = op_g * op_d_m_inv * src.block(SolutionBlocks::density);
             SolverControl step_4_gmres_control_1 (100000, g_d_m_inv_density.l2_norm()*1e-6);
-            SolverGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
+            TrilinosWrappers::SolverGMRES step_4_gmres_1 (step_4_gmres_control_1);
             try {
                 TimerOutput::Scope u(timer, "actual inv");
-//                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
-//                                      g_d_m_inv_density;
-                step_4_gmres_1.solve(op_k_inv,k_g_d_m_inv_density,g_d_m_inv_density, PreconditionIdentity() );
+                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
+                                      g_d_m_inv_density;
+//                step_4_gmres_1.solve(op_k_inv,k_g_d_m_inv_density,g_d_m_inv_density, preconditioner );
             }
             catch (std::exception &exc)
             {
@@ -386,12 +388,12 @@ namespace SAND {
                 throw;
             }
             SolverControl step_4_gmres_control_2 (100000, src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6);
-            SolverGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
+            TrilinosWrappers::SolverGMRES step_4_gmres_2 (step_4_gmres_control_2);
             try {
                 TimerOutput::Scope u(timer, "actual inv");
-//                k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, PreconditionIdentity()) *
-//                                 src.block(SolutionBlocks::unfiltered_density_multiplier);
-                step_4_gmres_2.solve(op_k_inv,k_density_mult,src.block(SolutionBlocks::unfiltered_density_multiplier), PreconditionIdentity() );
+                k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, PreconditionIdentity()) *
+                                 src.block(SolutionBlocks::unfiltered_density_multiplier);
+//                step_4_gmres_2.solve(op_k_inv,k_density_mult,src.block(SolutionBlocks::unfiltered_density_multiplier), PreconditionIdentity());
             } catch (std::exception &exc)
             {
                 std::cerr << "Failure of linear solver step_4_gmres_2" << std::endl;
@@ -525,6 +527,9 @@ namespace SAND {
 
             else if (Input::solver_choice == SolverOptions::inexact_K_with_exact_A_gmres)
             {
+
+                TrilinosWrappers::PreconditionIdentity preconditioner;
+                preconditioner.initialize(b_mat);
                 auto pre_pre_k = pre_k;
                 {
                     TimerOutput::Scope t(timer, "not inverse 5.1");
@@ -533,12 +538,12 @@ namespace SAND {
                     pre_k =  pre_pre_k + src.block(SolutionBlocks::unfiltered_density_multiplier);
                 }
                 SolverControl step_5_gmres_control_1 (100000, pre_j.l2_norm()*1e-6);
-                SolverGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
+                TrilinosWrappers::SolverGMRES step_5_gmres_1 (step_5_gmres_control_1);
                 try {
                     TimerOutput::Scope t(timer, "actual inverse 5.1");
-//                    dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
-//                                                                               pre_j;
-                    step_5_gmres_1.solve(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), dst.block(SolutionBlocks::unfiltered_density_multiplier), pre_j , PreconditionIdentity());
+                    dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
+                                                                               pre_j;
+//                    step_5_gmres_1.solve(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), dst.block(SolutionBlocks::unfiltered_density_multiplier), pre_j , preconditioner);
                 } catch (std::exception &exc)
                 {
                     std::cerr << "Failure of linear solver step_5_gmres_1 again" << std::endl;
@@ -549,11 +554,11 @@ namespace SAND {
 
 
                 SolverControl step_5_gmres_control_2 (100000, pre_k.l2_norm()*1e-6);
-                SolverGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
+                TrilinosWrappers::SolverGMRES step_5_gmres_2 (step_5_gmres_control_2);
                 try {
                     TimerOutput::Scope t(timer, "actual inverse 5.2");
-//                    dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) * pre_k;
-                    step_5_gmres_2.solve(op_k_inv,dst.block(SolutionBlocks::density), pre_k , PreconditionIdentity());
+                    dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) * pre_k;
+//                    step_5_gmres_2.solve(op_k_inv,dst.block(SolutionBlocks::density), pre_k , preconditioner);
                 } catch (std::exception &exc)
                 {
                     std::cerr << "Failure of linear solver step_5_gmres_2" << std::endl;

From bcd575327f1989694f0fb8b9a930257a2909f54e Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Wed, 2 Feb 2022 11:10:33 -0700
Subject: [PATCH 72/95] away from trilinos solver

---
 include/input_information.h    |  2 +-
 source/schur_preconditioner.cc | 30 +++++++++++++++---------------
 2 files changed, 16 insertions(+), 16 deletions(-)

diff --git a/include/input_information.h b/include/input_information.h
index 972ee6a..bf1fae4 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -18,7 +18,7 @@ namespace SAND {
         constexpr unsigned int geometry_base = GeometryOptions::mbb;
 
         constexpr unsigned int dim = 3;
-        constexpr unsigned int refinements =  3;
+        constexpr unsigned int refinements = 3;
 
         //nonlinear algorithm options
         constexpr double initial_barrier_size = 25;
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index fb61b56..156ee49 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -373,12 +373,12 @@ namespace SAND {
 
             g_d_m_inv_density = op_g * op_d_m_inv * src.block(SolutionBlocks::density);
             SolverControl step_4_gmres_control_1 (100000, g_d_m_inv_density.l2_norm()*1e-6);
-            TrilinosWrappers::SolverGMRES step_4_gmres_1 (step_4_gmres_control_1);
+            SolverGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
             try {
                 TimerOutput::Scope u(timer, "actual inv");
-                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
-                                      g_d_m_inv_density;
-//                step_4_gmres_1.solve(op_k_inv,k_g_d_m_inv_density,g_d_m_inv_density, preconditioner );
+//                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
+//                                      g_d_m_inv_density;
+                step_4_gmres_1.solve(op_k_inv,k_g_d_m_inv_density,g_d_m_inv_density, preconditioner );
             }
             catch (std::exception &exc)
             {
@@ -388,12 +388,12 @@ namespace SAND {
                 throw;
             }
             SolverControl step_4_gmres_control_2 (100000, src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6);
-            TrilinosWrappers::SolverGMRES step_4_gmres_2 (step_4_gmres_control_2);
+            SolverGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
             try {
                 TimerOutput::Scope u(timer, "actual inv");
-                k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, PreconditionIdentity()) *
-                                 src.block(SolutionBlocks::unfiltered_density_multiplier);
-//                step_4_gmres_2.solve(op_k_inv,k_density_mult,src.block(SolutionBlocks::unfiltered_density_multiplier), PreconditionIdentity());
+//                k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, PreconditionIdentity()) *
+//                                 src.block(SolutionBlocks::unfiltered_density_multiplier);
+                step_4_gmres_2.solve(op_k_inv,k_density_mult,src.block(SolutionBlocks::unfiltered_density_multiplier), PreconditionIdentity());
             } catch (std::exception &exc)
             {
                 std::cerr << "Failure of linear solver step_4_gmres_2" << std::endl;
@@ -538,12 +538,12 @@ namespace SAND {
                     pre_k =  pre_pre_k + src.block(SolutionBlocks::unfiltered_density_multiplier);
                 }
                 SolverControl step_5_gmres_control_1 (100000, pre_j.l2_norm()*1e-6);
-                TrilinosWrappers::SolverGMRES step_5_gmres_1 (step_5_gmres_control_1);
+                SolverGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
                 try {
                     TimerOutput::Scope t(timer, "actual inverse 5.1");
-                    dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
-                                                                               pre_j;
-//                    step_5_gmres_1.solve(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), dst.block(SolutionBlocks::unfiltered_density_multiplier), pre_j , preconditioner);
+//                    dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
+//                                                                               pre_j;
+                    step_5_gmres_1.solve(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), dst.block(SolutionBlocks::unfiltered_density_multiplier), pre_j , preconditioner);
                 } catch (std::exception &exc)
                 {
                     std::cerr << "Failure of linear solver step_5_gmres_1 again" << std::endl;
@@ -554,11 +554,11 @@ namespace SAND {
 
 
                 SolverControl step_5_gmres_control_2 (100000, pre_k.l2_norm()*1e-6);
-                TrilinosWrappers::SolverGMRES step_5_gmres_2 (step_5_gmres_control_2);
+                SolverGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
                 try {
                     TimerOutput::Scope t(timer, "actual inverse 5.2");
-                    dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) * pre_k;
-//                    step_5_gmres_2.solve(op_k_inv,dst.block(SolutionBlocks::density), pre_k , preconditioner);
+//                    dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) * pre_k;
+                    step_5_gmres_2.solve(op_k_inv,dst.block(SolutionBlocks::density), pre_k , preconditioner);
                 } catch (std::exception &exc)
                 {
                     std::cerr << "Failure of linear solver step_5_gmres_2" << std::endl;

From a970e303c62edc7c24ee9f9ee73d1ab8c55d1213 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <oconnor@venture.math.colostate.edu>
Date: Wed, 9 Feb 2022 13:24:39 -0700
Subject: [PATCH 73/95] AMG implemented

---
 include/schur_preconditioner.h |   8 +-
 source/density_filter.cc       |   1 -
 source/schur_preconditioner.cc | 131 ++++++++-------------------------
 3 files changed, 36 insertions(+), 104 deletions(-)

diff --git a/include/schur_preconditioner.h b/include/schur_preconditioner.h
index ddf9b09..32508be 100644
--- a/include/schur_preconditioner.h
+++ b/include/schur_preconditioner.h
@@ -136,9 +136,9 @@ namespace SAND
         LA::MPI::BlockSparseMatrix approx_h_mat;
 
         SolverControl other_solver_control;
-        mutable SolverBicgstab<Vector<double>> other_bicgstab;
-        mutable SolverGMRES<Vector<double>> other_gmres;
-        mutable SolverCG<Vector<double>> other_cg;
+        mutable SolverBicgstab<LA::MPI::Vector> other_bicgstab;
+        mutable SolverGMRES<LA::MPI::Vector> other_gmres;
+        mutable SolverCG<LA::MPI::Vector> other_cg;
 
         LA::MPI::SparseMatrix &a_mat;
         const LA::MPI::SparseMatrix &b_mat;
@@ -176,12 +176,14 @@ namespace SAND
         ConditionalOStream pcout;
         mutable TimerOutput timer;
 
+        mutable TrilinosWrappers::PreconditionAMG pre_amg;
 
         LinearOperator<VectorType,VectorType,PayloadType> op_d_8;
         LinearOperator<VectorType,VectorType,PayloadType> op_f;
         LinearOperator<VectorType,VectorType,PayloadType> op_b;
         LinearOperator<VectorType,VectorType,PayloadType> op_c;
         LinearOperator<VectorType,VectorType,PayloadType> op_a_inv;
+        LinearOperator<VectorType,VectorType,PayloadType> op_a_inv_ind;
         LinearOperator<VectorType,VectorType,PayloadType> op_e;
         LinearOperator<VectorType,VectorType,PayloadType> op_d_m;
         LinearOperator<VectorType,VectorType,PayloadType> op_d_m_inv;
diff --git a/source/density_filter.cc b/source/density_filter.cc
index 486c3db..d2be6f6 100644
--- a/source/density_filter.cc
+++ b/source/density_filter.cc
@@ -178,7 +178,6 @@ namespace SAND
                     }
                 }
         }
-
         return relevant_cells;
 
     }
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 156ee49..42fe9c0 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -117,7 +117,22 @@ namespace SAND {
         else
         {
             TimerOutput::Scope t(timer, "build A inv");
-            a_inv_direct.initialize(a_mat);
+
+             std::vector<std::vector<bool>> constant_modes;
+             FEValuesExtractors::Vector     displacement_components(SolutionComponents::displacement<dim>);
+             DoFTools::extract_constant_modes(dof_handler,
+                                              dof_handler.get_fe_collection()
+                                              .component_mask(displacement_components),
+                                              constant_modes);
+             TrilinosWrappers::PreconditionAMG::AdditionalData amg_data;
+             amg_data.constant_modes = constant_modes;
+             amg_data.n_cycles = 1;
+             amg_data.w_cycle = false;
+
+             pre_amg.initialize(a_mat,amg_data);
+             op_a_inv = linear_operator<VectorType,VectorType,PayloadType>(pre_amg);
+             op_a_inv_ind = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),other_cg ,pre_amg);
+
         }
         {
             TimerOutput::Scope t(timer, "reinit diag matrices");
@@ -233,7 +248,6 @@ namespace SAND {
         op_f = linear_operator<VectorType,VectorType,PayloadType>(f_mat);
         op_b = linear_operator<VectorType,VectorType,PayloadType>(b_mat);
         op_c = linear_operator<VectorType,VectorType,PayloadType>(c_mat);
-        op_a_inv = linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct);
         op_e = linear_operator<VectorType,VectorType,PayloadType>(e_mat);
         op_d_m= linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
         op_d_m_inv= linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat);
@@ -327,29 +341,17 @@ namespace SAND {
     void TopOptSchurPreconditioner<dim>::vmult_step_3(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
         dst = src;
 
-        if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
-        {
-            SolverControl            solver_control_1(100000, 1e-6);
-            SolverControl            solver_control_2(100000, 1e-6);
-            SolverCG<LA::MPI::Vector> a_solver_cg_1(solver_control_1);
-            SolverCG<LA::MPI::Vector> a_solver_cg_2(solver_control_2);
-            auto dst_temp = dst;
-            a_solver_cg_1.solve(a_mat,dst_temp.block(SolutionBlocks::displacement_multiplier),src.block(SolutionBlocks::displacement_multiplier),PreconditionIdentity());
-            a_solver_cg_2.solve(a_mat,dst_temp.block(SolutionBlocks::displacement),src.block(SolutionBlocks::displacement),PreconditionIdentity());
-            c_mat.Tvmult(dst_temp.block(SolutionBlocks::density_upper_slack),dst_temp.block(SolutionBlocks::displacement_multiplier));
-            e_mat.Tvmult(dst_temp.block(SolutionBlocks::density_lower_slack),dst_temp.block(SolutionBlocks::displacement));
-
-            dst.block(SolutionBlocks::density) = dst_temp.block(SolutionBlocks::density) - dst_temp.block(SolutionBlocks::density_upper_slack) - dst_temp.block(SolutionBlocks::density_lower_slack);
+        SolverControl            solver_control_1(100000, 1e-6);
+        SolverControl            solver_control_2(100000, 1e-6);
+        SolverCG<LA::MPI::Vector> a_solver_cg_1(solver_control_1);
+        SolverCG<LA::MPI::Vector> a_solver_cg_2(solver_control_2);
+        auto dst_temp = dst;
+        a_solver_cg_1.solve(a_mat,dst_temp.block(SolutionBlocks::displacement_multiplier),src.block(SolutionBlocks::displacement_multiplier),pre_amg);
+        a_solver_cg_2.solve(a_mat,dst_temp.block(SolutionBlocks::displacement),src.block(SolutionBlocks::displacement),pre_amg);
+        c_mat.Tvmult(dst_temp.block(SolutionBlocks::density_upper_slack),dst_temp.block(SolutionBlocks::displacement_multiplier));
+        e_mat.Tvmult(dst_temp.block(SolutionBlocks::density_lower_slack),dst_temp.block(SolutionBlocks::displacement));
 
-
-        }
-        else
-        {
-            auto dst_temp = dst;
-            dst.block(SolutionBlocks::density) = dst_temp.block(SolutionBlocks::density)
-                    - transpose_operator<VectorType,VectorType,PayloadType>(e_mat) * linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct)* src.block(SolutionBlocks::displacement)
-                    - transpose_operator<VectorType,VectorType,PayloadType>(c_mat) * linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct)* src.block(SolutionBlocks::displacement_multiplier);
-        }
+        dst.block(SolutionBlocks::density) = dst_temp.block(SolutionBlocks::density) - dst_temp.block(SolutionBlocks::density_upper_slack) - dst_temp.block(SolutionBlocks::density_lower_slack);
 
     }
 
@@ -361,15 +363,6 @@ namespace SAND {
 
         TrilinosWrappers::PreconditionIdentity preconditioner;
         preconditioner.initialize(b_mat);
-        if (Input::solver_choice == SolverOptions::exact_preconditioner_with_gmres)
-        {
-//            g_d_m_inv_density = linear_operator<VectorType,VectorType,PayloadType>(g_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
-//            k_g_d_m_inv_density = linear_operator<VectorType,VectorType,PayloadType>(k_mat) * g_d_m_inv_density;
-//            k_density_mult = linear_operator<VectorType,VectorType,PayloadType>(k_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
-        }
-
-        else if (Input::solver_choice == SolverOptions::inexact_K_with_exact_A_gmres)
-        {
 
             g_d_m_inv_density = op_g * op_d_m_inv * src.block(SolutionBlocks::density);
             SolverControl step_4_gmres_control_1 (100000, g_d_m_inv_density.l2_norm()*1e-6);
@@ -401,64 +394,7 @@ namespace SAND {
                 pcout << "last residual: " << step_4_gmres_control_2.last_value() << std::endl;
                 throw;
             }
-        }
-        else if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
-        {
-
-            SolverControl            solver_control(100000, 1e-6);
-            SolverCG<LA::MPI::Vector> a_solver_cg(solver_control);
-
-            auto preconditioner = PreconditionIdentity();
-
-            auto a_inv_op = op_d_8;
-
-                    inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg, preconditioner);
-
-
-            auto op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) *
-                        transpose_operator<VectorType, VectorType, PayloadType>(f_mat);
 
-            auto op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
-                        - transpose_operator<VectorType, VectorType, PayloadType>(c_mat) * a_inv_op * linear_operator<VectorType,VectorType,PayloadType>(e_mat)
-                        - transpose_operator<VectorType, VectorType, PayloadType>(e_mat) * a_inv_op * linear_operator<VectorType,VectorType,PayloadType>(c_mat);
-
-            auto op_k_inv = op_d_8;
-
-                    -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
-
-            SolverControl step_4_gmres_control_1 (10000, std::max(g_d_m_inv_density.l2_norm()*1e-6,1e-6));
-            SolverGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
-            try {
-//                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
-//                                      g_d_m_inv_density;
-//                step_4_gmres_1.solve(op_k_inv,k_g_d_m_inv_density,g_d_m_inv_density, PreconditionIdentity() );
-            } catch (std::exception &exc)
-            {
-                std::cerr << "Failure of linear solver step_4_gmres_1" << std::endl;
-                pcout << "first residual: " << step_4_gmres_control_1.initial_value() << std::endl;
-                pcout << "last residual: " << step_4_gmres_control_1.last_value() << std::endl;
-                throw;
-            }
-
-            SolverControl step_4_gmres_control_2 (10000, std::max(src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6,1e-6));
-            SolverGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
-            try {
-
-                k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, PreconditionIdentity()) *
-                                 src.block(SolutionBlocks::unfiltered_density_multiplier);
-            } catch (std::exception &exc)
-            {
-                std::cerr << "Failure of linear solver step_4_gmres_2" << std::endl;
-                pcout << "first residual: " << step_4_gmres_control_2.initial_value() << std::endl;
-                pcout << "last residual: " << step_4_gmres_control_2.last_value() << std::endl;
-                throw;
-            }
-        }
-        else
-        {
-            pcout << "shouldn't get here";
-            throw;
-        }
 
         dst.block(SolutionBlocks::total_volume_multiplier) = transpose_operator<VectorType, VectorType, PayloadType>(m_vect)*k_g_d_m_inv_density
                                                         - transpose_operator<VectorType, VectorType, PayloadType>(m_vect)*k_density_mult
@@ -493,20 +429,15 @@ namespace SAND {
         {
             //Third Block Inverse
             TimerOutput::Scope t(timer, "inverse 3");
-            if(Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
-            {
+
                 SolverControl            solver_control(100000, 1e-6);
                 SolverCG<LA::MPI::Vector> a_solver_cg(solver_control);
 
-                auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg, PreconditionIdentity());
+                auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg, pre_amg);
 
                 dst.block(SolutionBlocks::displacement) = a_inv_op * src.block(SolutionBlocks::displacement_multiplier);
                 dst.block(SolutionBlocks::displacement_multiplier) = a_inv_op * src.block(SolutionBlocks::displacement);
-            } else
-            {
-                dst.block(SolutionBlocks::displacement) = linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct) * src.block(SolutionBlocks::displacement_multiplier);
-                dst.block(SolutionBlocks::displacement_multiplier) = linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct) * src.block(SolutionBlocks::displacement);
-            }
+
 
         }
 
@@ -644,8 +575,8 @@ namespace SAND {
                     transpose_operator(linear_operator<VectorType,VectorType,PayloadType>(f_mat));
 
         auto op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
-                    - transpose_operator(linear_operator<VectorType,VectorType,PayloadType>(c_mat)) * linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct) * linear_operator<VectorType,VectorType,PayloadType>(e_mat)
-                    - transpose_operator(linear_operator<VectorType,VectorType,PayloadType>(e_mat)) * linear_operator<VectorType,VectorType,PayloadType>(a_inv_direct) * linear_operator<VectorType,VectorType,PayloadType>(c_mat);
+                    - transpose_operator(linear_operator<VectorType,VectorType,PayloadType>(c_mat)) * linear_operator<VectorType,VectorType,PayloadType>(op_a_inv) * linear_operator<VectorType,VectorType,PayloadType>(e_mat)
+                    - transpose_operator(linear_operator<VectorType,VectorType,PayloadType>(e_mat)) * linear_operator<VectorType,VectorType,PayloadType>(op_a_inv) * linear_operator<VectorType,VectorType,PayloadType>(c_mat);
 
         auto op_k_inv = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
 //        build_matrix_element_by_element(op_g,g_mat, src.block(SolutionBlocks::density));

From 4b3674b1adb877f358ec27f8f6560ecdabbd3e8b Mon Sep 17 00:00:00 2001
From: Justin O'Connor <oconnor@venture.math.colostate.edu>
Date: Thu, 17 Feb 2022 10:24:23 -0700
Subject: [PATCH 74/95] getting there

---
 include/input_information.h    |   4 +-
 include/schur_preconditioner.h | 103 +++++++++-
 source/kkt_system.cc           |   7 +-
 source/schur_preconditioner.cc | 339 ++++++++++++++++++++++++++++++---
 4 files changed, 420 insertions(+), 33 deletions(-)

diff --git a/include/input_information.h b/include/input_information.h
index bf1fae4..91bca81 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -17,8 +17,8 @@ namespace SAND {
         //geometry options
         constexpr unsigned int geometry_base = GeometryOptions::mbb;
 
-        constexpr unsigned int dim = 3;
-        constexpr unsigned int refinements = 3;
+        constexpr unsigned int dim = 2;
+        constexpr unsigned int refinements = 5;
 
         //nonlinear algorithm options
         constexpr double initial_barrier_size = 25;
diff --git a/include/schur_preconditioner.h b/include/schur_preconditioner.h
index 32508be..ef6cff3 100644
--- a/include/schur_preconditioner.h
+++ b/include/schur_preconditioner.h
@@ -100,6 +100,94 @@ namespace SAND
             int size;
     };
 
+
+    class GMatrix : public TrilinosWrappers::SparseMatrix {
+        public:
+            GMatrix(const LA::MPI::SparseMatrix &f_mat_in, LA::MPI::SparseMatrix &d_8_mat_in);
+            void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
+            void vmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const;
+            void Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
+            void Tvmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const;
+            void initialize(LA::MPI::Vector &exemplar_density_vector);
+            unsigned int m() const;
+            unsigned int n() const;
+        private:
+            const LA::MPI::SparseMatrix &f_mat;
+            LA::MPI::SparseMatrix &d_8_mat;
+            mutable LA::MPI::Vector temp_vect_1;
+            mutable LA::MPI::Vector temp_vect_2;
+
+
+    };
+
+    class HMatrix : public TrilinosWrappers::SparseMatrix {
+        public:
+            HMatrix(LA::MPI::SparseMatrix &a_mat_in, const LA::MPI::SparseMatrix &b_mat_in, const LA::MPI::SparseMatrix &c_mat_in, const LA::MPI::SparseMatrix &e_mat_in,TrilinosWrappers::PreconditionAMG &pre_amg_in);
+            void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
+            void vmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const;
+            void Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
+            void Tvmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const;
+            void initialize(LA::MPI::Vector &exemplar_density_vector,  LA::MPI::Vector &exemplar_displacement_vector);
+            unsigned int m() const;
+            unsigned int n() const;
+        private:
+            LA::MPI::SparseMatrix &a_mat;
+            const LA::MPI::SparseMatrix &b_mat;
+            const LA::MPI::SparseMatrix &c_mat;
+            const LA::MPI::SparseMatrix &e_mat;
+            TrilinosWrappers::PreconditionAMG &pre_amg;
+            mutable LA::MPI::Vector temp_vect_1;
+            mutable LA::MPI::Vector temp_vect_2;
+            mutable LA::MPI::Vector temp_vect_3;
+            mutable LA::MPI::Vector temp_vect_4;
+            mutable LA::MPI::Vector temp_vect_5;
+            mutable LA::MPI::Vector temp_vect_6;
+            mutable LA::MPI::Vector temp_vect_7;
+
+    };
+
+    class KinvMatrix : public TrilinosWrappers::SparseMatrix {
+        public:
+            KinvMatrix(HMatrix &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in);
+            void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
+            void vmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const;
+            void Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
+            void Tvmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const;
+            void initialize(LA::MPI::Vector &exemplar_density_vector);
+            unsigned int m() const;
+            unsigned int n() const;
+        private:
+            HMatrix &h_mat;
+            GMatrix &g_mat;
+            const LA::MPI::SparseMatrix &d_m_mat;
+            LA::MPI::SparseMatrix &d_m_inv_mat;
+            mutable LA::MPI::Vector temp_vect_1;
+            mutable LA::MPI::Vector temp_vect_2;
+            mutable LA::MPI::Vector temp_vect_3;
+            mutable LA::MPI::Vector temp_vect_4;
+    };
+
+    class JinvMatrix : public TrilinosWrappers::SparseMatrix {
+        public:
+            JinvMatrix(HMatrix &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in);
+            void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
+            void vmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const;
+            void Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
+            void Tvmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const;
+            void initialize(LA::MPI::Vector &exemplar_density_vector);
+            unsigned int m() const;
+            unsigned int n() const;
+        private:
+            HMatrix &h_mat;
+            GMatrix &g_mat;
+            const LA::MPI::SparseMatrix &d_m_mat;
+            LA::MPI::SparseMatrix &d_m_inv_mat;
+            mutable LA::MPI::Vector temp_vect_1;
+            mutable LA::MPI::Vector temp_vect_2;
+            mutable LA::MPI::Vector temp_vect_3;
+            mutable LA::MPI::Vector temp_vect_4;
+    };
+
     template<int dim>
     class TopOptSchurPreconditioner: public Subscriptor {
     public:
@@ -158,11 +246,6 @@ namespace SAND
         LA::MPI::SparseMatrix d_8_mat;
         LA::MPI::SparseMatrix d_m_inv_mat;
 
-        FullMatrix<double> g_mat;
-        FullMatrix<double> h_mat;
-        FullMatrix<double> k_inv_mat;
-        LAPACKFullMatrix<double> k_mat;
-
         mutable LA::MPI::Vector pre_j;
         mutable LA::MPI::Vector pre_k;
         mutable LA::MPI::Vector g_d_m_inv_density;
@@ -172,12 +255,19 @@ namespace SAND
         TrilinosWrappers::SolverDirect::AdditionalData additional_data;
         SolverControl direct_solver_control;
         mutable VmultTrilinosSolverDirect a_inv_direct;
-//        TrilinosWrappers::SolverDirect a_inv_direct;
         ConditionalOStream pcout;
         mutable TimerOutput timer;
 
         mutable TrilinosWrappers::PreconditionAMG pre_amg;
 
+        GMatrix g_mat;
+        HMatrix h_mat;
+
+        JinvMatrix j_inv_mat;
+        KinvMatrix k_inv_mat;
+
+
+
         LinearOperator<VectorType,VectorType,PayloadType> op_d_8;
         LinearOperator<VectorType,VectorType,PayloadType> op_f;
         LinearOperator<VectorType,VectorType,PayloadType> op_b;
@@ -193,5 +283,6 @@ namespace SAND
         LinearOperator<VectorType,VectorType,PayloadType> op_j_inv;
     };
 
+
 }
 #endif //SAND_SCHUR_PRECONDITIONER_H
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index abf1384..6b24b68 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -1779,13 +1779,15 @@ namespace SAND {
         }
         locally_relevant_solution=state;
         distributed_solution = state;
-        SolverControl solver_control(10000, gmres_tolerance * system_rhs.l2_norm());
+        SolverControl solver_control(1000000, gmres_tolerance * system_rhs.l2_norm());
         TopOptSchurPreconditioner<dim> preconditioner(system_matrix);
-
+        pcout << "about to solve" << std::endl;
 
         switch (Input::solver_choice) {
 
             case SolverOptions::inexact_K_with_exact_A_gmres: {
+
+
                 preconditioner.initialize(system_matrix, boundary_values, dof_handler, distributed_solution);
                 SolverFGMRES<LA::MPI::BlockVector> B_fgmres(solver_control);
                 B_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
@@ -1803,7 +1805,6 @@ namespace SAND {
                 throw;
         }
 
-
         constraints.distribute(distributed_solution);
 
         pcout << "here" << std::endl;
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 42fe9c0..4d409ad 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -47,7 +47,11 @@ namespace SAND {
             direct_solver_control(1, 0),
             a_inv_direct(direct_solver_control, additional_data),
             pcout(std::cout,(Utilities::MPI::this_mpi_process(mpi_communicator) == 0)),
-            timer(pcout, TimerOutput::summary, TimerOutput::wall_times)
+            timer(pcout, TimerOutput::summary, TimerOutput::wall_times),
+            g_mat(f_mat, d_8_mat),
+            h_mat(a_mat, b_mat, c_mat, e_mat, pre_amg),
+            j_inv_mat(h_mat, g_mat, d_m_mat, d_m_inv_mat),
+            k_inv_mat(h_mat, g_mat, d_m_mat, d_m_inv_mat)
     {
 
     }
@@ -158,9 +162,6 @@ namespace SAND {
 
             pcout << "np: " << n_p << std::endl;
 
-
-
-
             std::vector<double> l_global(n_p);
             std::vector<double> lm_global(n_p);
             std::vector<double> u_global(n_p);
@@ -181,8 +182,6 @@ namespace SAND {
                     std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
                     cell->get_dof_indices(i);
 
-
-
                     const int i_ind = cell->get_fe().component_to_system_index(0,0);
 
                     if(distributed_state.block(SolutionBlocks::density_lower_slack_multiplier).in_local_range(i[i_ind]))
@@ -264,7 +263,13 @@ namespace SAND {
 
         op_k_inv = (-1 * op_g *op_d_m_inv * op_h - op_d_m);
 
+        LA::MPI::Vector density_exemplar = distributed_state.block(SolutionBlocks::density);
+        LA::MPI::Vector displacement_exemplar = distributed_state.block(SolutionBlocks::displacement);
 
+        g_mat.initialize(density_exemplar);
+        h_mat.initialize(density_exemplar, displacement_exemplar);
+        j_inv_mat.initialize(density_exemplar);
+        k_inv_mat.initialize(density_exemplar);
 
 
     }
@@ -291,12 +296,12 @@ namespace SAND {
             vmult_step_3(dst, temp_src);
             temp_src = dst;
         }
-        {
-            TimerOutput::Scope t(timer, "part 4");
-            vmult_step_4(dst, temp_src);
-            temp_src = dst;
-        }
-        vmult_step_5(dst, temp_src);        
+//        {
+//            TimerOutput::Scope t(timer, "part 4");
+//            vmult_step_4(dst, temp_src);
+//            temp_src = dst;
+//        }
+//        vmult_step_5(dst, temp_src);
     }
 
     template<int dim>
@@ -366,12 +371,12 @@ namespace SAND {
 
             g_d_m_inv_density = op_g * op_d_m_inv * src.block(SolutionBlocks::density);
             SolverControl step_4_gmres_control_1 (100000, g_d_m_inv_density.l2_norm()*1e-6);
-            SolverGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
+            SolverFGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
             try {
                 TimerOutput::Scope u(timer, "actual inv");
 //                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
 //                                      g_d_m_inv_density;
-                step_4_gmres_1.solve(op_k_inv,k_g_d_m_inv_density,g_d_m_inv_density, preconditioner );
+                step_4_gmres_1.solve(k_inv_mat,k_g_d_m_inv_density,g_d_m_inv_density, preconditioner );
             }
             catch (std::exception &exc)
             {
@@ -381,12 +386,12 @@ namespace SAND {
                 throw;
             }
             SolverControl step_4_gmres_control_2 (100000, src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6);
-            SolverGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
+            SolverFGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
             try {
                 TimerOutput::Scope u(timer, "actual inv");
 //                k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, PreconditionIdentity()) *
 //                                 src.block(SolutionBlocks::unfiltered_density_multiplier);
-                step_4_gmres_2.solve(op_k_inv,k_density_mult,src.block(SolutionBlocks::unfiltered_density_multiplier), PreconditionIdentity());
+                step_4_gmres_2.solve(k_inv_mat,k_density_mult,src.block(SolutionBlocks::unfiltered_density_multiplier), PreconditionIdentity());
             } catch (std::exception &exc)
             {
                 std::cerr << "Failure of linear solver step_4_gmres_2" << std::endl;
@@ -469,12 +474,12 @@ namespace SAND {
                     pre_k =  pre_pre_k + src.block(SolutionBlocks::unfiltered_density_multiplier);
                 }
                 SolverControl step_5_gmres_control_1 (100000, pre_j.l2_norm()*1e-6);
-                SolverGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
+                SolverFGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
                 try {
                     TimerOutput::Scope t(timer, "actual inverse 5.1");
 //                    dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
 //                                                                               pre_j;
-                    step_5_gmres_1.solve(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), dst.block(SolutionBlocks::unfiltered_density_multiplier), pre_j , preconditioner);
+                    step_5_gmres_1.solve(j_inv_mat, dst.block(SolutionBlocks::unfiltered_density_multiplier), pre_j , preconditioner);
                 } catch (std::exception &exc)
                 {
                     std::cerr << "Failure of linear solver step_5_gmres_1 again" << std::endl;
@@ -485,11 +490,11 @@ namespace SAND {
 
 
                 SolverControl step_5_gmres_control_2 (100000, pre_k.l2_norm()*1e-6);
-                SolverGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
+                SolverFGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
                 try {
                     TimerOutput::Scope t(timer, "actual inverse 5.2");
 //                    dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) * pre_k;
-                    step_5_gmres_2.solve(op_k_inv,dst.block(SolutionBlocks::density), pre_k , preconditioner);
+                    step_5_gmres_2.solve(k_inv_mat,dst.block(SolutionBlocks::density), pre_k , preconditioner);
                 } catch (std::exception &exc)
                 {
                     std::cerr << "Failure of linear solver step_5_gmres_2" << std::endl;
@@ -517,7 +522,7 @@ namespace SAND {
                 pre_j = src.block(SolutionBlocks::density) + op_h * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
                 pre_k = -1* op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density) + src.block(SolutionBlocks::unfiltered_density_multiplier);
 
-                SolverControl step_5_gmres_control_1 (10000, std::max(pre_j.l2_norm()*1e-6,1e-6));
+                SolverControl step_5_gmres_control_1 (10000, std::max(pre_j.l2_norm()*1e-4,1e-4));
                 TrilinosWrappers::SolverGMRES step_5_gmres_1 (step_5_gmres_control_1);
                 try {
                     dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType,VectorType,PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
@@ -530,7 +535,7 @@ namespace SAND {
                     throw;
                 }
 
-                SolverControl step_5_gmres_control_2 (10000, std::max(pre_k.l2_norm()*1e-6,1e-6));
+                SolverControl step_5_gmres_control_2 (10000, std::max(pre_k.l2_norm()*1e-4,1e-4));
                 TrilinosWrappers::SolverGMRES step_5_gmres_2 (step_5_gmres_control_2);
                 try {
                     dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) *
@@ -589,6 +594,8 @@ namespace SAND {
 
     }
 
+
+
     void VmultTrilinosSolverDirect::initialize(LA::MPI::SparseMatrix &a_mat)
     {
         reinit(a_mat);
@@ -626,6 +633,294 @@ namespace SAND {
 
     }
 
+    // ******************     GMATRIX     ***********************
+
+    GMatrix::GMatrix(const LA::MPI::SparseMatrix &f_mat_in, LA::MPI::SparseMatrix &d_8_mat_in)
+            :
+            f_mat(f_mat_in),
+            d_8_mat(d_8_mat_in)
+    {
+
+    }
+
+    void
+    GMatrix::initialize(LA::MPI::Vector &exemplar_density_vector)
+    {
+        temp_vect_1 = exemplar_density_vector;
+        temp_vect_2 = exemplar_density_vector;
+    }
+
+    void
+    GMatrix::vmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
+    {
+//        f_mat.Tvmult(temp_vect_1,src);
+//        d_8_mat.vmult(temp_vect_2, temp_vect_1);
+//        f_mat.vmult(dst,temp_vect_2);
+    }
+
+    void GMatrix::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+    {
+        f_mat.Tvmult(temp_vect_1,src);
+        d_8_mat.vmult(temp_vect_2, temp_vect_1);
+        f_mat.vmult(dst,temp_vect_2);
+    }
+
+    void
+    GMatrix::Tvmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
+    {
+//        f_mat.Tvmult(temp_vect_1,src);
+//        d_8_mat.vmult(temp_vect_2, temp_vect_1);
+//        f_mat.vmult(dst,temp_vect_2);
+    }
+
+    void GMatrix::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+    {
+        f_mat.Tvmult(temp_vect_1,src);
+        d_8_mat.vmult(temp_vect_2, temp_vect_1);
+        f_mat.vmult(dst,temp_vect_2);
+    }
+
+
+    // ******************     HMatrix     ***********************
+
+    HMatrix::HMatrix(LA::MPI::SparseMatrix &a_mat_in, const LA::MPI::SparseMatrix &b_mat_in, const LA::MPI::SparseMatrix &c_mat_in, const LA::MPI::SparseMatrix &e_mat_in,TrilinosWrappers::PreconditionAMG &pre_amg_in)
+            :
+            a_mat(a_mat_in),
+            b_mat(b_mat_in),
+            c_mat(c_mat_in),
+            e_mat(e_mat_in),
+            pre_amg(pre_amg_in)
+    {
+
+    }
+
+    void
+    HMatrix::initialize(LA::MPI::Vector &exemplar_density_vector,  LA::MPI::Vector &exemplar_displacement_vector)
+    {
+        temp_vect_1 = exemplar_displacement_vector;
+        temp_vect_2 = exemplar_displacement_vector;
+        temp_vect_3 = exemplar_displacement_vector;
+        temp_vect_4 = exemplar_displacement_vector;
+        temp_vect_5 = exemplar_density_vector;
+        temp_vect_6 = exemplar_density_vector;
+        temp_vect_7 = exemplar_density_vector;
+
+
+    }
+
+    void
+    HMatrix::vmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
+    {
+
+//        c_mat.vmult(temp_vect_1,src);
+//        e_mat.vmult(temp_vect_2,src);
+
+//        SolverControl            solver_control(1000, 1e-2*temp_vect_2.l2_norm());
+//        SolverCG<LA::MPI::Vector> a_solver_cg(solver_control);
+
+//        a_solver_cg.solve(a_mat,temp_vect_3,temp_vect_1,pre_amg);
+//        a_solver_cg.solve(a_mat,temp_vect_4,temp_vect_2,pre_amg);
+
+//        c_mat.Tvmult(temp_vect_6,temp_vect_4);
+//        e_mat.Tvmult(temp_vect_5,temp_vect_3);
+
+//        b_mat.vmult(temp_vect_7,src);
+//        dst =  temp_vect_7 - temp_vect_6 - temp_vect_5;
+
+    }
+
+    void HMatrix::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+    {
+        c_mat.vmult(temp_vect_1,src);
+        e_mat.vmult(temp_vect_2,src);
+
+//        SolverControl            solver_control(1000, 1e-4*temp_vect_2.l2_norm());
+//        SolverCG<LA::MPI::Vector> a_solver_cg(solver_control);
+
+//        a_solver_cg.solve(a_mat,temp_vect_3,temp_vect_1,pre_amg);
+//        a_solver_cg.solve(a_mat,temp_vect_4,temp_vect_2,pre_amg);
+
+        pre_amg.vmult(temp_vect_3,temp_vect_1);
+        pre_amg.vmult(temp_vect_4,temp_vect_2);
+
+        c_mat.Tvmult(temp_vect_6,temp_vect_4);
+        e_mat.Tvmult(temp_vect_5,temp_vect_3);
+
+        b_mat.vmult(temp_vect_7,src);
+        dst =  temp_vect_7 - temp_vect_6 - temp_vect_5;
+    }
+
+    void
+    HMatrix::Tvmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
+    {
+//        c_mat.vmult(temp_vect_1,src);
+//        e_mat.vmult(temp_vect_2,src);
+
+//        SolverControl            solver_control(1000, 1e-2*temp_vect_2.l2_norm());
+//        SolverCG<LA::MPI::Vector> a_solver_cg(solver_control);
+
+//        a_solver_cg.solve(a_mat,temp_vect_3,temp_vect_1,pre_amg);
+//        a_solver_cg.solve(a_mat,temp_vect_4,temp_vect_2,pre_amg);
+
+//        c_mat.Tvmult(temp_vect_6,temp_vect_4);
+//        e_mat.Tvmult(temp_vect_5,temp_vect_3);
+
+//        b_mat.vmult(temp_vect_7,src);
+//        dst =  temp_vect_7 - temp_vect_6 - temp_vect_5;
+    }
+
+    void HMatrix::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+    {
+        c_mat.vmult(temp_vect_1,src);
+        e_mat.vmult(temp_vect_2,src);
+
+//        SolverControl            solver_control(100, 1e-3*temp_vect_2.l2_norm());
+//        SolverCG<LA::MPI::Vector> a_solver_cg(solver_control);
+
+//        a_solver_cg.solve(a_mat,temp_vect_3,temp_vect_1,pre_amg);
+//        a_solver_cg.solve(a_mat,temp_vect_4,temp_vect_2,pre_amg);
+
+        pre_amg.vmult(temp_vect_3,temp_vect_1);
+        pre_amg.vmult(temp_vect_4,temp_vect_2);
+
+        c_mat.Tvmult(temp_vect_6,temp_vect_4);
+        e_mat.Tvmult(temp_vect_5,temp_vect_3);
+
+        b_mat.vmult(temp_vect_7,src);
+        dst =  temp_vect_7 - temp_vect_6 - temp_vect_5;
+    }
+
+
+    // ******************     JMatrix     ***********************
+    JinvMatrix::JinvMatrix(HMatrix &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in)
+            :
+            h_mat(h_mat_in),
+            g_mat(g_mat_in),
+            d_m_mat(d_m_mat_in),
+            d_m_inv_mat(d_m_inv_mat_in)
+    {
+
+    }
+
+    void
+    JinvMatrix::initialize( LA::MPI::Vector &exemplar_density_vector)
+    {
+        temp_vect_1 = exemplar_density_vector;
+        temp_vect_2 = exemplar_density_vector;
+        temp_vect_3 = exemplar_density_vector;
+        temp_vect_4 = exemplar_density_vector;
+    }
+
+    void
+    JinvMatrix::vmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
+    {
+//        g_mat.vmult(temp_vect_1,src);
+//        d_m_inv_mat.vmult(temp_vect_2,temp_vect_1);
+//        h_mat.vmult(temp_vect_3,temp_vect_2);
+//        d_m_mat.vmult(temp_vect_4,src);
+
+//        dst = -1*temp_vect_4 - temp_vect_3;
+    }
+
+    void JinvMatrix::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+    {
+        g_mat.vmult(temp_vect_1,src);
+        d_m_inv_mat.vmult(temp_vect_2,temp_vect_1);
+        h_mat.vmult(temp_vect_3,temp_vect_2);
+        d_m_mat.vmult(temp_vect_4,src);
+
+        dst = -1*temp_vect_4 - temp_vect_3;
+    }
+
+    void
+    JinvMatrix::Tvmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
+    {
+//        h_mat.vmult(temp_vect_1,src);
+//        d_m_inv_mat.vmult(temp_vect_2,temp_vect_1);
+//        g_mat.vmult(temp_vect_3,temp_vect_2);
+//        d_m_mat.vmult(temp_vect_4,src);
+
+//        dst = -1*temp_vect_4 - temp_vect_3;
+    }
+
+    void JinvMatrix::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+    {
+        h_mat.vmult(temp_vect_1,src);
+        d_m_inv_mat.vmult(temp_vect_2,temp_vect_1);
+        g_mat.vmult(temp_vect_3,temp_vect_2);
+        d_m_mat.vmult(temp_vect_4,src);
+
+        dst = -1*temp_vect_4 - temp_vect_3;
+    }
+
+
+    // ******************     KinvMatrix     ***********************
+    KinvMatrix::KinvMatrix(HMatrix &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in)
+            :
+            h_mat(h_mat_in),
+            g_mat(g_mat_in),
+            d_m_mat(d_m_mat_in),
+            d_m_inv_mat(d_m_inv_mat_in)
+    {
+
+    }
+
+    void
+    KinvMatrix::initialize(LA::MPI::Vector &exemplar_density_vector)
+    {
+        temp_vect_1 = exemplar_density_vector;
+        temp_vect_2 = exemplar_density_vector;
+        temp_vect_3 = exemplar_density_vector;
+        temp_vect_4 = exemplar_density_vector;
+
+    }
+
+    void
+    KinvMatrix::vmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
+    {
+//        h_mat.vmult(temp_vect_1,src);
+//        d_m_inv_mat.vmult(temp_vect_2,temp_vect_1);
+//        g_mat.vmult(temp_vect_3,temp_vect_2);
+//        d_m_mat.vmult(temp_vect_4,src);
+
+//        dst = -1*temp_vect_4 - temp_vect_3;
+    }
+
+    void KinvMatrix::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+    {
+        h_mat.vmult(temp_vect_1,src);
+        d_m_inv_mat.vmult(temp_vect_2,temp_vect_1);
+        g_mat.vmult(temp_vect_3,temp_vect_2);
+        d_m_mat.vmult(temp_vect_4,src);
+
+        dst = -1*temp_vect_4 - temp_vect_3;
+    }
+
+    void
+    KinvMatrix::Tvmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
+    {
+
+//        g_mat.vmult(temp_vect_1,src);
+//        d_m_inv_mat.vmult(temp_vect_2,temp_vect_1);
+//        h_mat.vmult(temp_vect_3,temp_vect_2);
+//        d_m_mat.vmult(temp_vect_4,src);
+
+//        dst = -1*temp_vect_4 - temp_vect_3;
+    }
+
+    void KinvMatrix::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+    {
+        g_mat.vmult(temp_vect_1,src);
+        d_m_inv_mat.vmult(temp_vect_2,temp_vect_1);
+        h_mat.vmult(temp_vect_3,temp_vect_2);
+        d_m_mat.vmult(temp_vect_4,src);
+
+        dst = -1*temp_vect_4 - temp_vect_3;
+    }
+
 }
+
+
+
 template class SAND::TopOptSchurPreconditioner<2>;
 template class SAND::TopOptSchurPreconditioner<3>;

From 2b55a618daefcb1253e045ecb42ee00c1df90e64 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <oconnor@venture.math.colostate.edu>
Date: Mon, 21 Mar 2022 08:50:34 -0600
Subject: [PATCH 75/95] commit

---
 include/input_information.h    |   9 +-
 include/schur_preconditioner.h |   8 -
 source/schur_preconditioner.cc | 260 +++++++++------------------------
 3 files changed, 75 insertions(+), 202 deletions(-)

diff --git a/include/input_information.h b/include/input_information.h
index 91bca81..20b70c0 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -17,15 +17,15 @@ namespace SAND {
         //geometry options
         constexpr unsigned int geometry_base = GeometryOptions::mbb;
 
-        constexpr unsigned int dim = 2;
-        constexpr unsigned int refinements = 5;
+        constexpr unsigned int dim = 3;
+        constexpr unsigned int refinements = 4;
 
         //nonlinear algorithm options
         constexpr double initial_barrier_size = 25;
         constexpr double min_barrier_size = 0;
 
         constexpr double fraction_to_boundary = .7;
-        constexpr unsigned int max_steps=30;
+        constexpr unsigned int max_steps=2;
 
         constexpr unsigned int barrier_reduction=BarrierOptions::loqo;
         constexpr double required_norm = .0001;
@@ -46,6 +46,9 @@ namespace SAND {
         constexpr bool use_eisenstat_walker = false;
         constexpr double default_gmres_tolerance = 1e-9;
 
+        constexpr unsigned int a_inv_iterations = 25;
+        constexpr unsigned int k_inv_iterations = 25;
+
 
     }
 }
diff --git a/include/schur_preconditioner.h b/include/schur_preconditioner.h
index ef6cff3..63ed7da 100644
--- a/include/schur_preconditioner.h
+++ b/include/schur_preconditioner.h
@@ -105,9 +105,7 @@ namespace SAND
         public:
             GMatrix(const LA::MPI::SparseMatrix &f_mat_in, LA::MPI::SparseMatrix &d_8_mat_in);
             void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
-            void vmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const;
             void Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
-            void Tvmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const;
             void initialize(LA::MPI::Vector &exemplar_density_vector);
             unsigned int m() const;
             unsigned int n() const;
@@ -124,9 +122,7 @@ namespace SAND
         public:
             HMatrix(LA::MPI::SparseMatrix &a_mat_in, const LA::MPI::SparseMatrix &b_mat_in, const LA::MPI::SparseMatrix &c_mat_in, const LA::MPI::SparseMatrix &e_mat_in,TrilinosWrappers::PreconditionAMG &pre_amg_in);
             void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
-            void vmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const;
             void Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
-            void Tvmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const;
             void initialize(LA::MPI::Vector &exemplar_density_vector,  LA::MPI::Vector &exemplar_displacement_vector);
             unsigned int m() const;
             unsigned int n() const;
@@ -150,9 +146,7 @@ namespace SAND
         public:
             KinvMatrix(HMatrix &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in);
             void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
-            void vmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const;
             void Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
-            void Tvmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const;
             void initialize(LA::MPI::Vector &exemplar_density_vector);
             unsigned int m() const;
             unsigned int n() const;
@@ -171,9 +165,7 @@ namespace SAND
         public:
             JinvMatrix(HMatrix &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in);
             void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
-            void vmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const;
             void Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
-            void Tvmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const;
             void initialize(LA::MPI::Vector &exemplar_density_vector);
             unsigned int m() const;
             unsigned int n() const;
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 4d409ad..d79ecb6 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -29,7 +29,7 @@ namespace SAND {
             n_columns(0),
             n_block_rows(0),
             n_block_columns(0),
-            other_solver_control(100000, 1e-6),
+            other_solver_control(1000000, 1e-6),
             other_bicgstab(other_solver_control),
             other_gmres(other_solver_control),
             other_cg(other_solver_control),
@@ -130,8 +130,8 @@ namespace SAND {
                                               constant_modes);
              TrilinosWrappers::PreconditionAMG::AdditionalData amg_data;
              amg_data.constant_modes = constant_modes;
-             amg_data.n_cycles = 1;
-             amg_data.w_cycle = false;
+//             amg_data.n_cycles = 1;
+//             amg_data.w_cycle = true;
 
              pre_amg.initialize(a_mat,amg_data);
              op_a_inv = linear_operator<VectorType,VectorType,PayloadType>(pre_amg);
@@ -296,12 +296,12 @@ namespace SAND {
             vmult_step_3(dst, temp_src);
             temp_src = dst;
         }
-//        {
-//            TimerOutput::Scope t(timer, "part 4");
-//            vmult_step_4(dst, temp_src);
-//            temp_src = dst;
-//        }
-//        vmult_step_5(dst, temp_src);
+        {
+            TimerOutput::Scope t(timer, "part 4");
+            vmult_step_4(dst, temp_src);
+            temp_src = dst;
+        }
+        vmult_step_5(dst, temp_src);
     }
 
     template<int dim>
@@ -370,34 +370,28 @@ namespace SAND {
         preconditioner.initialize(b_mat);
 
             g_d_m_inv_density = op_g * op_d_m_inv * src.block(SolutionBlocks::density);
-            SolverControl step_4_gmres_control_1 (100000, g_d_m_inv_density.l2_norm()*1e-6);
+            SolverControl step_4_gmres_control_1 (Input::k_inv_iterations, g_d_m_inv_density.l2_norm()*1e-6);
             SolverFGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
             try {
-                TimerOutput::Scope u(timer, "actual inv");
-//                k_g_d_m_inv_density = inverse_operator(op_k_inv, step_4_gmres_1, PreconditionIdentity()) *
-//                                      g_d_m_inv_density;
                 step_4_gmres_1.solve(k_inv_mat,k_g_d_m_inv_density,g_d_m_inv_density, preconditioner );
             }
             catch (std::exception &exc)
             {
-                std::cerr << "Failure of linear solver step_4_gmres_1" << std::endl;
-                pcout << "first residual: " << step_4_gmres_control_1.initial_value() << std::endl;
-                pcout << "last residual: " << step_4_gmres_control_1.last_value() << std::endl;
-                throw;
+//                pcout << "Failure of linear solver step_4_gmres_1" << std::endl;
+//                pcout << "first residual: " << step_4_gmres_control_1.initial_value() << std::endl;
+//                pcout << "last residual: " << step_4_gmres_control_1.last_value() << std::endl;
+//                throw;
             }
-            SolverControl step_4_gmres_control_2 (100000, src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*1e-6);
+            SolverControl step_4_gmres_control_2 (Input::k_inv_iterations, 1e-6 * src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm() );
             SolverFGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
             try {
-                TimerOutput::Scope u(timer, "actual inv");
-//                k_density_mult = inverse_operator(op_k_inv,step_4_gmres_2, PreconditionIdentity()) *
-//                                 src.block(SolutionBlocks::unfiltered_density_multiplier);
                 step_4_gmres_2.solve(k_inv_mat,k_density_mult,src.block(SolutionBlocks::unfiltered_density_multiplier), PreconditionIdentity());
             } catch (std::exception &exc)
             {
-                std::cerr << "Failure of linear solver step_4_gmres_2" << std::endl;
-                pcout << "first residual: " << step_4_gmres_control_2.initial_value() << std::endl;
-                pcout << "last residual: " << step_4_gmres_control_2.last_value() << std::endl;
-                throw;
+//                pcout << "Failure of linear solver step_4_gmres_2" << std::endl;
+//                pcout << "first residual: " << step_4_gmres_control_2.initial_value() << std::endl;
+//                pcout << "last residual: " << step_4_gmres_control_2.last_value() << std::endl;
+//                throw;
             }
 
 
@@ -473,79 +467,31 @@ namespace SAND {
                     pre_pre_k = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
                     pre_k =  pre_pre_k + src.block(SolutionBlocks::unfiltered_density_multiplier);
                 }
-                SolverControl step_5_gmres_control_1 (100000, pre_j.l2_norm()*1e-6);
+                SolverControl step_5_gmres_control_1 (Input::k_inv_iterations, 1e-6*pre_j.l2_norm());
                 SolverFGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
                 try {
                     TimerOutput::Scope t(timer, "actual inverse 5.1");
-//                    dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType, VectorType, PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
-//                                                                               pre_j;
                     step_5_gmres_1.solve(j_inv_mat, dst.block(SolutionBlocks::unfiltered_density_multiplier), pre_j , preconditioner);
                 } catch (std::exception &exc)
                 {
-                    std::cerr << "Failure of linear solver step_5_gmres_1 again" << std::endl;
-                    pcout << "first residual: " << step_5_gmres_control_1.initial_value() << std::endl;
-                    pcout << "last residual: " << step_5_gmres_control_1.last_value() << std::endl;
-                    throw;
+//                    pcout << "Failure of linear solver step_5_gmres_1" << std::endl;
+//                    pcout << "first residual: " << step_5_gmres_control_1.initial_value() << std::endl;
+//                    pcout << "last residual: " << step_5_gmres_control_1.last_value() << std::endl;
+//                    throw;
                 }
 
 
-                SolverControl step_5_gmres_control_2 (100000, pre_k.l2_norm()*1e-6);
+                SolverControl step_5_gmres_control_2 (Input::k_inv_iterations, 1e-6*pre_k.l2_norm());
                 SolverFGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
                 try {
                     TimerOutput::Scope t(timer, "actual inverse 5.2");
-//                    dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) * pre_k;
                     step_5_gmres_2.solve(k_inv_mat,dst.block(SolutionBlocks::density), pre_k , preconditioner);
                 } catch (std::exception &exc)
                 {
-                    std::cerr << "Failure of linear solver step_5_gmres_2" << std::endl;
-                    pcout << "first residual: " << step_5_gmres_control_2.initial_value() << std::endl;
-                    pcout << "last residual: " << step_5_gmres_control_2.last_value() << std::endl;
-                    throw;
-                }
-            }
-            else if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
-            {
-
-                SolverControl            solver_control(100000, 1e-6);
-                SolverCG<LA::MPI::Vector> a_solver_cg (solver_control);
-                auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg, PreconditionIdentity());
-
-                auto op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) *
-                            transpose_operator<VectorType, VectorType, PayloadType>(f_mat);
-
-                auto op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
-                            - transpose_operator<VectorType, VectorType, PayloadType>(c_mat) * a_inv_op * linear_operator<VectorType,VectorType,PayloadType>(e_mat)
-                            - transpose_operator<VectorType, VectorType, PayloadType>(e_mat) * a_inv_op * linear_operator<VectorType,VectorType,PayloadType>(c_mat);
-
-                auto op_k_inv = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
-
-                pre_j = src.block(SolutionBlocks::density) + op_h * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
-                pre_k = -1* op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density) + src.block(SolutionBlocks::unfiltered_density_multiplier);
-
-                SolverControl step_5_gmres_control_1 (10000, std::max(pre_j.l2_norm()*1e-4,1e-4));
-                TrilinosWrappers::SolverGMRES step_5_gmres_1 (step_5_gmres_control_1);
-                try {
-                    dst.block(SolutionBlocks::unfiltered_density_multiplier) = inverse_operator(transpose_operator<VectorType,VectorType,PayloadType>(op_k_inv), step_5_gmres_1, PreconditionIdentity()) *
-                                                                               pre_j;
-                } catch (std::exception &exc)
-                {
-                    std::cerr << "Failure of linear solver step_5_gmres_1" << std::endl;
-                    pcout << "first residual: " << step_5_gmres_control_1.initial_value() << std::endl;
-                    pcout << "last residual: " << step_5_gmres_control_1.last_value() << std::endl;
-                    throw;
-                }
-
-                SolverControl step_5_gmres_control_2 (10000, std::max(pre_k.l2_norm()*1e-4,1e-4));
-                TrilinosWrappers::SolverGMRES step_5_gmres_2 (step_5_gmres_control_2);
-                try {
-                    dst.block(SolutionBlocks::density) = inverse_operator(op_k_inv, step_5_gmres_2, PreconditionIdentity()) *
-                                                         pre_k;
-                } catch (std::exception &exc)
-                {
-                    std::cerr << "Failure of linear solver step_5_gmres_2" << std::endl;
-                    pcout << "first residual: " << step_5_gmres_control_2.initial_value() << std::endl;
-                    pcout << "last residual: " << step_5_gmres_control_2.last_value() << std::endl;
-                    throw;
+//                    pcout << "Failure of linear solver step_5_gmres_2" << std::endl;
+//                    pcout << "first residual: " << step_5_gmres_control_2.initial_value() << std::endl;
+//                    pcout << "last residual: " << step_5_gmres_control_2.last_value() << std::endl;
+//                    throw;
                 }
             }
             else
@@ -650,13 +596,6 @@ namespace SAND {
         temp_vect_2 = exemplar_density_vector;
     }
 
-    void
-    GMatrix::vmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
-    {
-//        f_mat.Tvmult(temp_vect_1,src);
-//        d_8_mat.vmult(temp_vect_2, temp_vect_1);
-//        f_mat.vmult(dst,temp_vect_2);
-    }
 
     void GMatrix::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
     {
@@ -665,13 +604,7 @@ namespace SAND {
         f_mat.vmult(dst,temp_vect_2);
     }
 
-    void
-    GMatrix::Tvmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
-    {
-//        f_mat.Tvmult(temp_vect_1,src);
-//        d_8_mat.vmult(temp_vect_2, temp_vect_1);
-//        f_mat.vmult(dst,temp_vect_2);
-    }
+
 
     void GMatrix::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
     {
@@ -708,80 +641,70 @@ namespace SAND {
 
     }
 
-    void
-    HMatrix::vmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
-    {
-
-//        c_mat.vmult(temp_vect_1,src);
-//        e_mat.vmult(temp_vect_2,src);
-
-//        SolverControl            solver_control(1000, 1e-2*temp_vect_2.l2_norm());
-//        SolverCG<LA::MPI::Vector> a_solver_cg(solver_control);
-
-//        a_solver_cg.solve(a_mat,temp_vect_3,temp_vect_1,pre_amg);
-//        a_solver_cg.solve(a_mat,temp_vect_4,temp_vect_2,pre_amg);
-
-//        c_mat.Tvmult(temp_vect_6,temp_vect_4);
-//        e_mat.Tvmult(temp_vect_5,temp_vect_3);
-
-//        b_mat.vmult(temp_vect_7,src);
-//        dst =  temp_vect_7 - temp_vect_6 - temp_vect_5;
-
-    }
 
     void HMatrix::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
     {
         c_mat.vmult(temp_vect_1,src);
         e_mat.vmult(temp_vect_2,src);
 
-//        SolverControl            solver_control(1000, 1e-4*temp_vect_2.l2_norm());
-//        SolverCG<LA::MPI::Vector> a_solver_cg(solver_control);
+        SolverControl            solver_control_1(Input::a_inv_iterations, temp_vect_1.l2_norm()*1e-6);
+        SolverCG<LA::MPI::Vector> a_solver_cg_1(solver_control_1);
 
-//        a_solver_cg.solve(a_mat,temp_vect_3,temp_vect_1,pre_amg);
-//        a_solver_cg.solve(a_mat,temp_vect_4,temp_vect_2,pre_amg);
+        try
+        {
+            a_solver_cg_1.solve(a_mat,temp_vect_3,temp_vect_1,pre_amg);
+//            std::cout << "   " << solver_control_1.last_step()
+//                        << " CG iterations needed to obtain convergence." << std::endl;
+        } catch (std::exception &exc)
+        {
 
-        pre_amg.vmult(temp_vect_3,temp_vect_1);
-        pre_amg.vmult(temp_vect_4,temp_vect_2);
+        }
+        SolverControl            solver_control_2(Input::a_inv_iterations, temp_vect_2.l2_norm()*1e-6);
+        SolverCG<LA::MPI::Vector> a_solver_cg_2(solver_control_2);
+        try
+        {
+            a_solver_cg_2.solve(a_mat,temp_vect_4,temp_vect_2,pre_amg);
+//            std::cout << "   " << solver_control_2.last_step()
+//                        << " CG iterations needed to obtain convergence." << std::endl;
+        } catch (std::exception &exc)
+        {
+
+        }
 
         c_mat.Tvmult(temp_vect_6,temp_vect_4);
         e_mat.Tvmult(temp_vect_5,temp_vect_3);
 
         b_mat.vmult(temp_vect_7,src);
         dst =  temp_vect_7 - temp_vect_6 - temp_vect_5;
-    }
 
-    void
-    HMatrix::Tvmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
-    {
-//        c_mat.vmult(temp_vect_1,src);
-//        e_mat.vmult(temp_vect_2,src);
-
-//        SolverControl            solver_control(1000, 1e-2*temp_vect_2.l2_norm());
-//        SolverCG<LA::MPI::Vector> a_solver_cg(solver_control);
+    }
 
-//        a_solver_cg.solve(a_mat,temp_vect_3,temp_vect_1,pre_amg);
-//        a_solver_cg.solve(a_mat,temp_vect_4,temp_vect_2,pre_amg);
 
-//        c_mat.Tvmult(temp_vect_6,temp_vect_4);
-//        e_mat.Tvmult(temp_vect_5,temp_vect_3);
-
-//        b_mat.vmult(temp_vect_7,src);
-//        dst =  temp_vect_7 - temp_vect_6 - temp_vect_5;
-    }
 
     void HMatrix::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
     {
         c_mat.vmult(temp_vect_1,src);
         e_mat.vmult(temp_vect_2,src);
 
-//        SolverControl            solver_control(100, 1e-3*temp_vect_2.l2_norm());
-//        SolverCG<LA::MPI::Vector> a_solver_cg(solver_control);
+        SolverControl            solver_control_1(Input::a_inv_iterations, temp_vect_1.l2_norm()*1e-6);
+        SolverCG<LA::MPI::Vector> a_solver_cg_1(solver_control_1);
+
+        try
+        {
+            a_solver_cg_1.solve(a_mat,temp_vect_3,temp_vect_1,pre_amg);
+        } catch (std::exception &exc)
+        {
 
-//        a_solver_cg.solve(a_mat,temp_vect_3,temp_vect_1,pre_amg);
-//        a_solver_cg.solve(a_mat,temp_vect_4,temp_vect_2,pre_amg);
+        }
+        SolverControl            solver_control_2(Input::a_inv_iterations, temp_vect_2.l2_norm()*1e-6);
+        SolverCG<LA::MPI::Vector> a_solver_cg_2(solver_control_2);
+        try
+        {
+            a_solver_cg_2.solve(a_mat,temp_vect_4,temp_vect_2,pre_amg);
+        } catch (std::exception &exc)
+        {
 
-        pre_amg.vmult(temp_vect_3,temp_vect_1);
-        pre_amg.vmult(temp_vect_4,temp_vect_2);
+        }
 
         c_mat.Tvmult(temp_vect_6,temp_vect_4);
         e_mat.Tvmult(temp_vect_5,temp_vect_3);
@@ -811,17 +734,6 @@ namespace SAND {
         temp_vect_4 = exemplar_density_vector;
     }
 
-    void
-    JinvMatrix::vmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
-    {
-//        g_mat.vmult(temp_vect_1,src);
-//        d_m_inv_mat.vmult(temp_vect_2,temp_vect_1);
-//        h_mat.vmult(temp_vect_3,temp_vect_2);
-//        d_m_mat.vmult(temp_vect_4,src);
-
-//        dst = -1*temp_vect_4 - temp_vect_3;
-    }
-
     void JinvMatrix::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
     {
         g_mat.vmult(temp_vect_1,src);
@@ -832,17 +744,6 @@ namespace SAND {
         dst = -1*temp_vect_4 - temp_vect_3;
     }
 
-    void
-    JinvMatrix::Tvmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
-    {
-//        h_mat.vmult(temp_vect_1,src);
-//        d_m_inv_mat.vmult(temp_vect_2,temp_vect_1);
-//        g_mat.vmult(temp_vect_3,temp_vect_2);
-//        d_m_mat.vmult(temp_vect_4,src);
-
-//        dst = -1*temp_vect_4 - temp_vect_3;
-    }
-
     void JinvMatrix::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
     {
         h_mat.vmult(temp_vect_1,src);
@@ -875,17 +776,6 @@ namespace SAND {
 
     }
 
-    void
-    KinvMatrix::vmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
-    {
-//        h_mat.vmult(temp_vect_1,src);
-//        d_m_inv_mat.vmult(temp_vect_2,temp_vect_1);
-//        g_mat.vmult(temp_vect_3,temp_vect_2);
-//        d_m_mat.vmult(temp_vect_4,src);
-
-//        dst = -1*temp_vect_4 - temp_vect_3;
-    }
-
     void KinvMatrix::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
     {
         h_mat.vmult(temp_vect_1,src);
@@ -896,18 +786,6 @@ namespace SAND {
         dst = -1*temp_vect_4 - temp_vect_3;
     }
 
-    void
-    KinvMatrix::Tvmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
-    {
-
-//        g_mat.vmult(temp_vect_1,src);
-//        d_m_inv_mat.vmult(temp_vect_2,temp_vect_1);
-//        h_mat.vmult(temp_vect_3,temp_vect_2);
-//        d_m_mat.vmult(temp_vect_4,src);
-
-//        dst = -1*temp_vect_4 - temp_vect_3;
-    }
-
     void KinvMatrix::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
     {
         g_mat.vmult(temp_vect_1,src);

From d2b70f1a0d3904f250f696fd8f51c6b31725109d Mon Sep 17 00:00:00 2001
From: Justin O'Connor <oconnor@up.math.colostate.edu>
Date: Mon, 21 Mar 2022 10:20:54 -0600
Subject: [PATCH 76/95] prettier

---
 include/input_information.h    |   2 +-
 include/schur_preconditioner.h |   3 +-
 source/schur_preconditioner.cc | 313 ++++++++++++++++++++-------------
 3 files changed, 196 insertions(+), 122 deletions(-)

diff --git a/include/input_information.h b/include/input_information.h
index 20b70c0..9859154 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -17,7 +17,7 @@ namespace SAND {
         //geometry options
         constexpr unsigned int geometry_base = GeometryOptions::mbb;
 
-        constexpr unsigned int dim = 3;
+        constexpr unsigned int dim = 2;
         constexpr unsigned int refinements = 4;
 
         //nonlinear algorithm options
diff --git a/include/schur_preconditioner.h b/include/schur_preconditioner.h
index 63ed7da..d0a1e35 100644
--- a/include/schur_preconditioner.h
+++ b/include/schur_preconditioner.h
@@ -120,7 +120,7 @@ namespace SAND
 
     class HMatrix : public TrilinosWrappers::SparseMatrix {
         public:
-            HMatrix(LA::MPI::SparseMatrix &a_mat_in, const LA::MPI::SparseMatrix &b_mat_in, const LA::MPI::SparseMatrix &c_mat_in, const LA::MPI::SparseMatrix &e_mat_in,TrilinosWrappers::PreconditionAMG &pre_amg_in);
+            HMatrix(LA::MPI::SparseMatrix &a_mat_in, const LA::MPI::SparseMatrix &b_mat_in, const LA::MPI::SparseMatrix &c_mat_in, const LA::MPI::SparseMatrix &e_mat_in,TrilinosWrappers::PreconditionAMG &pre_amg_in, VmultTrilinosSolverDirect &a_inv_direct_in);
             void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
             void Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
             void initialize(LA::MPI::Vector &exemplar_density_vector,  LA::MPI::Vector &exemplar_displacement_vector);
@@ -132,6 +132,7 @@ namespace SAND
             const LA::MPI::SparseMatrix &c_mat;
             const LA::MPI::SparseMatrix &e_mat;
             TrilinosWrappers::PreconditionAMG &pre_amg;
+            VmultTrilinosSolverDirect &a_inv_direct;
             mutable LA::MPI::Vector temp_vect_1;
             mutable LA::MPI::Vector temp_vect_2;
             mutable LA::MPI::Vector temp_vect_3;
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index d79ecb6..9fe276a 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -49,7 +49,7 @@ namespace SAND {
             pcout(std::cout,(Utilities::MPI::this_mpi_process(mpi_communicator) == 0)),
             timer(pcout, TimerOutput::summary, TimerOutput::wall_times),
             g_mat(f_mat, d_8_mat),
-            h_mat(a_mat, b_mat, c_mat, e_mat, pre_amg),
+            h_mat(a_mat, b_mat, c_mat, e_mat, pre_amg, a_inv_direct),
             j_inv_mat(h_mat, g_mat, d_m_mat, d_m_inv_mat),
             k_inv_mat(h_mat, g_mat, d_m_mat, d_m_inv_mat)
     {
@@ -115,10 +115,6 @@ namespace SAND {
 
 
         if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
-        {
-
-        }
-        else
         {
             TimerOutput::Scope t(timer, "build A inv");
 
@@ -134,10 +130,12 @@ namespace SAND {
 //             amg_data.w_cycle = true;
 
              pre_amg.initialize(a_mat,amg_data);
-             op_a_inv = linear_operator<VectorType,VectorType,PayloadType>(pre_amg);
-             op_a_inv_ind = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),other_cg ,pre_amg);
 
         }
+        else
+        {
+             a_inv_direct.initialize(a_mat);
+        }
         {
             TimerOutput::Scope t(timer, "reinit diag matrices");
             d_3_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
@@ -242,27 +240,6 @@ namespace SAND {
         g_d_m_inv_density=distributed_state.block(SolutionBlocks::density);
         k_g_d_m_inv_density=distributed_state.block(SolutionBlocks::density);
 
-
-        op_d_8 = linear_operator<VectorType,VectorType,PayloadType>(d_8_mat);
-        op_f = linear_operator<VectorType,VectorType,PayloadType>(f_mat);
-        op_b = linear_operator<VectorType,VectorType,PayloadType>(b_mat);
-        op_c = linear_operator<VectorType,VectorType,PayloadType>(c_mat);
-        op_e = linear_operator<VectorType,VectorType,PayloadType>(e_mat);
-        op_d_m= linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
-        op_d_m_inv= linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat);
-
-        op_g = op_f * op_d_8 *
-                transpose_operator(op_f);
-
-
-        op_h = op_b
-               - transpose_operator(op_c) * op_a_inv * op_e
-               - transpose_operator(op_e) * op_a_inv * op_c;
-
-//        TrilinosWrappers::internal::LinearOperatorImplementation::TrilinosPayload inv_payload = inverse_payload(TrilinosWrappers::SolverGMRES(),TrilinosWrappers::PreconditionIdentity());
-
-        op_k_inv = (-1 * op_g *op_d_m_inv * op_h - op_d_m);
-
         LA::MPI::Vector density_exemplar = distributed_state.block(SolutionBlocks::density);
         LA::MPI::Vector displacement_exemplar = distributed_state.block(SolutionBlocks::displacement);
 
@@ -282,26 +259,31 @@ namespace SAND {
         {
             TimerOutput::Scope t(timer, "part 1");
             vmult_step_1(dst, src);
+            pcout << "step 1 done" << std::endl;
             temp_src = dst;
         }
 
         {
             TimerOutput::Scope t(timer, "part 2");
             vmult_step_2(dst, temp_src);
+            pcout << "step 2 done" << std::endl;
             temp_src = dst;
         }
 
         {
             TimerOutput::Scope t(timer, "part 3");
             vmult_step_3(dst, temp_src);
+            pcout << "step 3 done" << std::endl;
             temp_src = dst;
         }
         {
             TimerOutput::Scope t(timer, "part 4");
             vmult_step_4(dst, temp_src);
+            pcout << "step 4 done" << std::endl;
             temp_src = dst;
         }
         vmult_step_5(dst, temp_src);
+        pcout << "step 5 done" << std::endl;
     }
 
     template<int dim>
@@ -345,18 +327,32 @@ namespace SAND {
     template<int dim>
     void TopOptSchurPreconditioner<dim>::vmult_step_3(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
         dst = src;
-
-        SolverControl            solver_control_1(100000, 1e-6);
-        SolverControl            solver_control_2(100000, 1e-6);
-        SolverCG<LA::MPI::Vector> a_solver_cg_1(solver_control_1);
-        SolverCG<LA::MPI::Vector> a_solver_cg_2(solver_control_2);
         auto dst_temp = dst;
-        a_solver_cg_1.solve(a_mat,dst_temp.block(SolutionBlocks::displacement_multiplier),src.block(SolutionBlocks::displacement_multiplier),pre_amg);
-        a_solver_cg_2.solve(a_mat,dst_temp.block(SolutionBlocks::displacement),src.block(SolutionBlocks::displacement),pre_amg);
-        c_mat.Tvmult(dst_temp.block(SolutionBlocks::density_upper_slack),dst_temp.block(SolutionBlocks::displacement_multiplier));
-        e_mat.Tvmult(dst_temp.block(SolutionBlocks::density_lower_slack),dst_temp.block(SolutionBlocks::displacement));
 
-        dst.block(SolutionBlocks::density) = dst_temp.block(SolutionBlocks::density) - dst_temp.block(SolutionBlocks::density_upper_slack) - dst_temp.block(SolutionBlocks::density_lower_slack);
+        if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
+        {
+            SolverControl            solver_control_1(100000, 1e-6);
+            SolverControl            solver_control_2(100000, 1e-6);
+            SolverCG<LA::MPI::Vector> a_solver_cg_1(solver_control_1);
+            SolverCG<LA::MPI::Vector> a_solver_cg_2(solver_control_2);
+            a_solver_cg_1.solve(a_mat,dst_temp.block(SolutionBlocks::displacement_multiplier),src.block(SolutionBlocks::displacement_multiplier),pre_amg);
+            a_solver_cg_2.solve(a_mat,dst_temp.block(SolutionBlocks::displacement),src.block(SolutionBlocks::displacement),pre_amg);
+            c_mat.Tvmult(dst_temp.block(SolutionBlocks::density_upper_slack),dst_temp.block(SolutionBlocks::displacement_multiplier));
+            e_mat.Tvmult(dst_temp.block(SolutionBlocks::density_lower_slack),dst_temp.block(SolutionBlocks::displacement));
+
+            dst.block(SolutionBlocks::density) = dst_temp.block(SolutionBlocks::density) - dst_temp.block(SolutionBlocks::density_upper_slack) - dst_temp.block(SolutionBlocks::density_lower_slack);
+
+        }
+        else
+        {
+             a_inv_direct.vmult(dst_temp.block(SolutionBlocks::displacement_multiplier),src.block(SolutionBlocks::displacement_multiplier));
+             a_inv_direct.vmult(dst_temp.block(SolutionBlocks::displacement),src.block(SolutionBlocks::displacement));
+             c_mat.Tvmult(dst_temp.block(SolutionBlocks::density_upper_slack),dst_temp.block(SolutionBlocks::displacement_multiplier));
+             e_mat.Tvmult(dst_temp.block(SolutionBlocks::density_lower_slack),dst_temp.block(SolutionBlocks::displacement));
+             dst.block(SolutionBlocks::density) = dst_temp.block(SolutionBlocks::density) - dst_temp.block(SolutionBlocks::density_upper_slack) - dst_temp.block(SolutionBlocks::density_lower_slack);
+        }
+
+
 
     }
 
@@ -369,7 +365,9 @@ namespace SAND {
         TrilinosWrappers::PreconditionIdentity preconditioner;
         preconditioner.initialize(b_mat);
 
-            g_d_m_inv_density = op_g * op_d_m_inv * src.block(SolutionBlocks::density);
+            auto d_m_inv_density = g_d_m_inv_density;
+            d_m_inv_mat.vmult(d_m_inv_density,src.block(SolutionBlocks::density));
+            g_mat.vmult(g_d_m_inv_density,d_m_inv_density);
             SolverControl step_4_gmres_control_1 (Input::k_inv_iterations, g_d_m_inv_density.l2_norm()*1e-6);
             SolverFGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
             try {
@@ -415,7 +413,7 @@ namespace SAND {
             dst.block(SolutionBlocks::density_lower_slack) = linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density_lower_slack_multiplier);
             dst.block(SolutionBlocks::density_upper_slack) = linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density_upper_slack_multiplier);
         }
-
+            pcout << "inv1 done" << std::endl;
 
         {
             //Second Block Inverse
@@ -423,12 +421,14 @@ namespace SAND {
             dst.block(SolutionBlocks::unfiltered_density) =
                     linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) * src.block(SolutionBlocks::unfiltered_density);
         }
+            pcout << "inv2 done" << std::endl;
 
 
         {
             //Third Block Inverse
             TimerOutput::Scope t(timer, "inverse 3");
-
+            if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
+            {
                 SolverControl            solver_control(100000, 1e-6);
                 SolverCG<LA::MPI::Vector> a_solver_cg(solver_control);
 
@@ -436,6 +436,12 @@ namespace SAND {
 
                 dst.block(SolutionBlocks::displacement) = a_inv_op * src.block(SolutionBlocks::displacement_multiplier);
                 dst.block(SolutionBlocks::displacement_multiplier) = a_inv_op * src.block(SolutionBlocks::displacement);
+            }
+            else
+            {
+                a_inv_direct.vmult( dst.block(SolutionBlocks::displacement), src.block(SolutionBlocks::displacement_multiplier));
+                a_inv_direct.vmult( dst.block(SolutionBlocks::displacement_multiplier), src.block(SolutionBlocks::displacement));
+            }
 
 
         }
@@ -455,17 +461,70 @@ namespace SAND {
 //                dst.block(SolutionBlocks::density) = linear_operator<VectorType,VectorType,PayloadType>(k_mat) * pre_k;
             }
 
+            else if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
+            {
+
+                TrilinosWrappers::PreconditionIdentity preconditioner;
+                preconditioner.initialize(b_mat);
+                auto pre_pre_k = pre_k;
+                auto pre_pre_pre_k = pre_k;
+                auto d_m_inv_unfil_density_mult = pre_k;
+                auto h_d_m_inv_unfil_density_mult = pre_k;
+
+                {
+                    TimerOutput::Scope t(timer, "not inverse 5.1");
+                    d_m_inv_mat.vmult(d_m_inv_unfil_density_mult, src.block(SolutionBlocks::unfiltered_density_multiplier));
+                    h_mat.vmult(h_d_m_inv_unfil_density_mult,d_m_inv_unfil_density_mult);
+                    pre_j = src.block(SolutionBlocks::density) + h_d_m_inv_unfil_density_mult;
+                    d_m_inv_mat.vmult(pre_pre_pre_k,src.block(SolutionBlocks::density));
+                    g_mat.vmult(pre_pre_k,pre_pre_pre_k);
+                    pre_k =  -1 * pre_pre_k + src.block(SolutionBlocks::unfiltered_density_multiplier);
+                }
+                SolverControl step_5_gmres_control_1 (Input::k_inv_iterations, 1e-6*pre_j.l2_norm());
+                SolverFGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
+                try {
+                    TimerOutput::Scope t(timer, "actual inverse 5.1");
+                    step_5_gmres_1.solve(j_inv_mat, dst.block(SolutionBlocks::unfiltered_density_multiplier), pre_j , preconditioner);
+                } catch (std::exception &exc)
+                {
+//                    pcout << "Failure of linear solver step_5_gmres_1" << std::endl;
+//                    pcout << "first residual: " << step_5_gmres_control_1.initial_value() << std::endl;
+//                    pcout << "last residual: " << step_5_gmres_control_1.last_value() << std::endl;
+//                    throw;
+                }
+
+
+                SolverControl step_5_gmres_control_2 (Input::k_inv_iterations, 1e-6*pre_k.l2_norm());
+                SolverFGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
+                try {
+                    TimerOutput::Scope t(timer, "actual inverse 5.2");
+                    step_5_gmres_2.solve(k_inv_mat,dst.block(SolutionBlocks::density), pre_k , preconditioner);
+                } catch (std::exception &exc)
+                {
+//                    pcout << "Failure of linear solver step_5_gmres_2" << std::endl;
+//                    pcout << "first residual: " << step_5_gmres_control_2.initial_value() << std::endl;
+//                    pcout << "last residual: " << step_5_gmres_control_2.last_value() << std::endl;
+//                    throw;
+                }
+            }
             else if (Input::solver_choice == SolverOptions::inexact_K_with_exact_A_gmres)
             {
 
                 TrilinosWrappers::PreconditionIdentity preconditioner;
                 preconditioner.initialize(b_mat);
                 auto pre_pre_k = pre_k;
+                auto pre_pre_pre_k = pre_k;
+                auto d_m_inv_unfil_density_mult = pre_k;
+                auto h_d_m_inv_unfil_density_mult = pre_k;
+
                 {
                     TimerOutput::Scope t(timer, "not inverse 5.1");
-                    pre_j = src.block(SolutionBlocks::density) + op_h * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
-                    pre_pre_k = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density);
-                    pre_k =  pre_pre_k + src.block(SolutionBlocks::unfiltered_density_multiplier);
+                    d_m_inv_mat.vmult(d_m_inv_unfil_density_mult, src.block(SolutionBlocks::unfiltered_density_multiplier));
+                    h_mat.vmult(h_d_m_inv_unfil_density_mult,d_m_inv_unfil_density_mult);
+                    pre_j = src.block(SolutionBlocks::density) + h_d_m_inv_unfil_density_mult;
+                    d_m_inv_mat.vmult(pre_pre_pre_k,src.block(SolutionBlocks::density));
+                    g_mat.vmult(pre_pre_k,pre_pre_pre_k);
+                    pre_k =  -1 * pre_pre_k + src.block(SolutionBlocks::unfiltered_density_multiplier);
                 }
                 SolverControl step_5_gmres_control_1 (Input::k_inv_iterations, 1e-6*pre_j.l2_norm());
                 SolverFGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
@@ -511,32 +570,6 @@ namespace SAND {
     void TopOptSchurPreconditioner<dim>::print_stuff()
     {
 
-//        print_matrix(std::string("OAmat.csv"),a_mat);
-//        print_matrix(std::string("OBmat.csv"),b_mat);
-//        print_matrix(std::string("OCmat.csv"),c_mat);
-//        print_matrix(std::string("OEmat.csv"),e_mat);
-//        print_matrix(std::string("OFmat.csv"),f_mat);
-        FullMatrix<double> g_mat;
-        FullMatrix<double> h_mat;
-        FullMatrix<double> k_inv_mat;
-        g_mat.reinit(b_mat.m(),b_mat.n());
-        h_mat.reinit(b_mat.m(),b_mat.n());
-        k_inv_mat.reinit(b_mat.m(),b_mat.n());
-        auto op_g = linear_operator<VectorType,VectorType,PayloadType>(f_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) *
-                    transpose_operator(linear_operator<VectorType,VectorType,PayloadType>(f_mat));
-
-        auto op_h = linear_operator<VectorType,VectorType,PayloadType>(b_mat)
-                    - transpose_operator(linear_operator<VectorType,VectorType,PayloadType>(c_mat)) * linear_operator<VectorType,VectorType,PayloadType>(op_a_inv) * linear_operator<VectorType,VectorType,PayloadType>(e_mat)
-                    - transpose_operator(linear_operator<VectorType,VectorType,PayloadType>(e_mat)) * linear_operator<VectorType,VectorType,PayloadType>(op_a_inv) * linear_operator<VectorType,VectorType,PayloadType>(c_mat);
-
-        auto op_k_inv = -1 * op_g * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * op_h - linear_operator<VectorType,VectorType,PayloadType>(d_m_mat);
-//        build_matrix_element_by_element(op_g,g_mat, src.block(SolutionBlocks::density));
-//        build_matrix_element_by_element(op_h,h_mat, src.block(SolutionBlocks::density));
-//        build_matrix_element_by_element(op_k_inv,k_inv_mat, src.block(SolutionBlocks::density));
-//        print_matrix(std::string("OGmat.csv"),g_mat);
-//        print_matrix(std::string("OHmat.csv"),h_mat);
-//        print_matrix(std::string("OKinvmat.csv"),k_inv_mat);
-
 
     }
 
@@ -616,13 +649,14 @@ namespace SAND {
 
     // ******************     HMatrix     ***********************
 
-    HMatrix::HMatrix(LA::MPI::SparseMatrix &a_mat_in, const LA::MPI::SparseMatrix &b_mat_in, const LA::MPI::SparseMatrix &c_mat_in, const LA::MPI::SparseMatrix &e_mat_in,TrilinosWrappers::PreconditionAMG &pre_amg_in)
+    HMatrix::HMatrix(LA::MPI::SparseMatrix &a_mat_in, const LA::MPI::SparseMatrix &b_mat_in, const LA::MPI::SparseMatrix &c_mat_in, const LA::MPI::SparseMatrix &e_mat_in,TrilinosWrappers::PreconditionAMG &pre_amg_in,VmultTrilinosSolverDirect &a_inv_direct_in)
             :
             a_mat(a_mat_in),
             b_mat(b_mat_in),
             c_mat(c_mat_in),
             e_mat(e_mat_in),
-            pre_amg(pre_amg_in)
+            pre_amg(pre_amg_in),
+            a_inv_direct(a_inv_direct_in)
     {
 
     }
@@ -644,38 +678,57 @@ namespace SAND {
 
     void HMatrix::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
     {
-        c_mat.vmult(temp_vect_1,src);
-        e_mat.vmult(temp_vect_2,src);
+        if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
+        {
+            c_mat.vmult(temp_vect_1,src);
+            e_mat.vmult(temp_vect_2,src);
 
-        SolverControl            solver_control_1(Input::a_inv_iterations, temp_vect_1.l2_norm()*1e-6);
-        SolverCG<LA::MPI::Vector> a_solver_cg_1(solver_control_1);
+            SolverControl            solver_control_1(Input::a_inv_iterations, temp_vect_1.l2_norm()*1e-6);
+            SolverCG<LA::MPI::Vector> a_solver_cg_1(solver_control_1);
 
-        try
-        {
-            a_solver_cg_1.solve(a_mat,temp_vect_3,temp_vect_1,pre_amg);
-//            std::cout << "   " << solver_control_1.last_step()
-//                        << " CG iterations needed to obtain convergence." << std::endl;
-        } catch (std::exception &exc)
-        {
+            try
+            {
+                a_solver_cg_1.solve(a_mat,temp_vect_3,temp_vect_1,pre_amg);
+    //            std::cout << "   " << solver_control_1.last_step()
+    //                        << " CG iterations needed to obtain convergence." << std::endl;
+            } catch (std::exception &exc)
+            {
+
+            }
+            SolverControl            solver_control_2(Input::a_inv_iterations, temp_vect_2.l2_norm()*1e-6);
+            SolverCG<LA::MPI::Vector> a_solver_cg_2(solver_control_2);
+            try
+            {
+                a_solver_cg_2.solve(a_mat,temp_vect_4,temp_vect_2,pre_amg);
+    //            std::cout << "   " << solver_control_2.last_step()
+    //                        << " CG iterations needed to obtain convergence." << std::endl;
+            } catch (std::exception &exc)
+            {
+
+            }
+
+            c_mat.Tvmult(temp_vect_6,temp_vect_4);
+            e_mat.Tvmult(temp_vect_5,temp_vect_3);
 
+            b_mat.vmult(temp_vect_7,src);
+            dst =  temp_vect_7 - temp_vect_6 - temp_vect_5;
         }
-        SolverControl            solver_control_2(Input::a_inv_iterations, temp_vect_2.l2_norm()*1e-6);
-        SolverCG<LA::MPI::Vector> a_solver_cg_2(solver_control_2);
-        try
-        {
-            a_solver_cg_2.solve(a_mat,temp_vect_4,temp_vect_2,pre_amg);
-//            std::cout << "   " << solver_control_2.last_step()
-//                        << " CG iterations needed to obtain convergence." << std::endl;
-        } catch (std::exception &exc)
+        else
         {
+            c_mat.vmult(temp_vect_1,src);
+            e_mat.vmult(temp_vect_2,src);
+
+            a_inv_direct.vmult(temp_vect_3,temp_vect_1);
+            a_inv_direct.vmult(temp_vect_4,temp_vect_2);
+
+            c_mat.Tvmult(temp_vect_6,temp_vect_4);
+            e_mat.Tvmult(temp_vect_5,temp_vect_3);
 
+            b_mat.vmult(temp_vect_7,src);
+            dst =  temp_vect_7 - temp_vect_6 - temp_vect_5;
         }
 
-        c_mat.Tvmult(temp_vect_6,temp_vect_4);
-        e_mat.Tvmult(temp_vect_5,temp_vect_3);
 
-        b_mat.vmult(temp_vect_7,src);
-        dst =  temp_vect_7 - temp_vect_6 - temp_vect_5;
 
     }
 
@@ -683,38 +736,58 @@ namespace SAND {
 
     void HMatrix::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
     {
-        c_mat.vmult(temp_vect_1,src);
-        e_mat.vmult(temp_vect_2,src);
+        if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
+        {
+            c_mat.vmult(temp_vect_1,src);
+            e_mat.vmult(temp_vect_2,src);
 
-        SolverControl            solver_control_1(Input::a_inv_iterations, temp_vect_1.l2_norm()*1e-6);
-        SolverCG<LA::MPI::Vector> a_solver_cg_1(solver_control_1);
+            SolverControl            solver_control_1(Input::a_inv_iterations, temp_vect_1.l2_norm()*1e-6);
+            SolverCG<LA::MPI::Vector> a_solver_cg_1(solver_control_1);
 
-        try
-        {
-            a_solver_cg_1.solve(a_mat,temp_vect_3,temp_vect_1,pre_amg);
-        } catch (std::exception &exc)
-        {
+            try
+            {
+                a_solver_cg_1.solve(a_mat,temp_vect_3,temp_vect_1,pre_amg);
+    //            std::cout << "   " << solver_control_1.last_step()
+    //                        << " CG iterations needed to obtain convergence." << std::endl;
+            } catch (std::exception &exc)
+            {
 
-        }
-        SolverControl            solver_control_2(Input::a_inv_iterations, temp_vect_2.l2_norm()*1e-6);
-        SolverCG<LA::MPI::Vector> a_solver_cg_2(solver_control_2);
-        try
-        {
-            a_solver_cg_2.solve(a_mat,temp_vect_4,temp_vect_2,pre_amg);
-        } catch (std::exception &exc)
-        {
+            }
+            SolverControl            solver_control_2(Input::a_inv_iterations, temp_vect_2.l2_norm()*1e-6);
+            SolverCG<LA::MPI::Vector> a_solver_cg_2(solver_control_2);
+            try
+            {
+                a_solver_cg_2.solve(a_mat,temp_vect_4,temp_vect_2,pre_amg);
+    //            std::cout << "   " << solver_control_2.last_step()
+    //                        << " CG iterations needed to obtain convergence." << std::endl;
+            } catch (std::exception &exc)
+            {
+
+            }
+
+            c_mat.Tvmult(temp_vect_6,temp_vect_4);
+            e_mat.Tvmult(temp_vect_5,temp_vect_3);
 
+            b_mat.vmult(temp_vect_7,src);
+            dst =  temp_vect_7 - temp_vect_6 - temp_vect_5;
         }
+        else
+        {
+            c_mat.vmult(temp_vect_1,src);
+            e_mat.vmult(temp_vect_2,src);
 
-        c_mat.Tvmult(temp_vect_6,temp_vect_4);
-        e_mat.Tvmult(temp_vect_5,temp_vect_3);
+            a_inv_direct.vmult(temp_vect_3,temp_vect_1);
+            a_inv_direct.vmult(temp_vect_4,temp_vect_2);
 
-        b_mat.vmult(temp_vect_7,src);
-        dst =  temp_vect_7 - temp_vect_6 - temp_vect_5;
-    }
+            c_mat.Tvmult(temp_vect_6,temp_vect_4);
+            e_mat.Tvmult(temp_vect_5,temp_vect_3);
 
+            b_mat.vmult(temp_vect_7,src);
+            dst =  temp_vect_7 - temp_vect_6 - temp_vect_5;
+        }
 
-    // ******************     JMatrix     ***********************
+    }
+    // ******************     JinvMatrix     ***********************
     JinvMatrix::JinvMatrix(HMatrix &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in)
             :
             h_mat(h_mat_in),
@@ -726,12 +799,13 @@ namespace SAND {
     }
 
     void
-    JinvMatrix::initialize( LA::MPI::Vector &exemplar_density_vector)
+    JinvMatrix::initialize(LA::MPI::Vector &exemplar_density_vector)
     {
         temp_vect_1 = exemplar_density_vector;
         temp_vect_2 = exemplar_density_vector;
         temp_vect_3 = exemplar_density_vector;
         temp_vect_4 = exemplar_density_vector;
+
     }
 
     void JinvMatrix::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
@@ -754,7 +828,6 @@ namespace SAND {
         dst = -1*temp_vect_4 - temp_vect_3;
     }
 
-
     // ******************     KinvMatrix     ***********************
     KinvMatrix::KinvMatrix(HMatrix &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in)
             :

From 038e058467452c1139f4d36af332d2f919dab770 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <oconnor@up.math.colostate.edu>
Date: Mon, 21 Mar 2022 19:08:05 -0600
Subject: [PATCH 77/95] started mfgmg

---
 include/matrix_free_gmg.h | 49 +++++++++++++++++++++++++++++++++++++
 source/matrix_free_gmg.cc | 51 +++++++++++++++++++++++++++++++++++++++
 2 files changed, 100 insertions(+)
 create mode 100644 include/matrix_free_gmg.h
 create mode 100644 source/matrix_free_gmg.cc

diff --git a/include/matrix_free_gmg.h b/include/matrix_free_gmg.h
new file mode 100644
index 0000000..68462ea
--- /dev/null
+++ b/include/matrix_free_gmg.h
@@ -0,0 +1,49 @@
+#ifndef MATRIX_FREE_GMG_H
+#define MATRIX_FREE_GMG_H
+
+#include <deal.II/lac/linear_operator.h>
+#include <deal.II/lac/sparse_direct.h>
+
+using namespace dealii;
+
+template <int dim, int fe_degree, typename number>
+class ElasticityOperator
+  : public MatrixFreeOperators::
+      Base<dim, LinearAlgebra::distributed::Vector<number>>
+{
+public:
+
+  using value_type = number;
+
+  ElasticityOperator();
+
+  void clear() override;
+
+  void evaluate_coefficient(const Coefficient<dim> &coefficient_function);
+
+  virtual void compute_diagonal() override;
+
+private:
+
+  virtual void apply_add(
+    LinearAlgebra::distributed::Vector<number> &      dst,
+    const LinearAlgebra::distributed::Vector<number> &src) const override;
+
+  void
+  local_apply(const MatrixFree<dim, number> &                   data,
+              LinearAlgebra::distributed::Vector<number> &      dst,
+              const LinearAlgebra::distributed::Vector<number> &src,
+              const std::pair<unsigned int, unsigned int> &cell_range) const;
+
+  void local_compute_diagonal(
+    const MatrixFree<dim, number> &              data,
+    LinearAlgebra::distributed::Vector<number> & dst,
+    const unsigned int &                         dummy,
+    const std::pair<unsigned int, unsigned int> &cell_range) const;
+
+  Table<2, VectorizedArray<number>> coefficient;
+};
+
+
+
+#endif // MATRIX_FREE_GMG_H
diff --git a/source/matrix_free_gmg.cc b/source/matrix_free_gmg.cc
new file mode 100644
index 0000000..8a0b712
--- /dev/null
+++ b/source/matrix_free_gmg.cc
@@ -0,0 +1,51 @@
+//
+// Created by justin on 2/17/21.
+//
+#include "../include/matrix_free_gmg.h"
+namespace SAND {
+    using namespace dealii;
+
+
+    template <int dim, int fe_degree, typename number>
+    ElasticityOperator<dim, fe_degree, number>::ElasticityOperator()
+      : MatrixFreeOperators::Base<dim,
+                                  LinearAlgebra::distributed::Vector<number>>()
+    {}
+
+    template <int dim, int fe_degree, typename number>
+    void ElasticityOperator<dim, fe_degree, number>::clear()
+    {
+      MatrixFreeOperators::Base<dim, LinearAlgebra::distributed::Vector<number>>::
+        clear();
+    }
+
+    template <int dim, int fe_degree, typename number>
+    void ElasticityOperator<dim, fe_degree, number>::local_apply(
+      const MatrixFree<dim, number> &                   data,
+      LinearAlgebra::distributed::Vector<number> &      dst,
+      const LinearAlgebra::distributed::Vector<number> &src,
+      const std::pair<unsigned int, unsigned int> &     cell_range) const
+    {
+      FEEvaluation<dim, fe_degree, fe_degree + 1, 1, number> phi(data);
+      for (unsigned int cell = cell_range.first; cell < cell_range.second; ++cell)
+        {
+          phi.reinit(cell);
+          phi.read_dof_values(src);
+
+          phi.evaluate(EvaluationFlags::gradients);
+
+          for (unsigned int q = 0; q < phi.n_q_points; ++q)
+          {
+
+            phi.submit_divergence(coefficient_rho_p_lambda(cell, q) * phi.get_gradient(q), q);
+            phi.submit_symmetric_gradient(coefficient_2_rho_p_mu(cell, q) * phi.get_gradient(q), q);
+
+          }
+          phi.integrate(EvaluationFlags::gradients);
+          phi.distribute_local_to_global(dst);
+        }
+    }
+
+}
+template class SAND::ElasticityOperator<2>;
+template class SAND::ElasticityOperator<3>;

From f7777500c8c178453f826439000cd0ce275246ed Mon Sep 17 00:00:00 2001
From: Justin O'Connor <oconnor@up.math.colostate.edu>
Date: Mon, 28 Mar 2022 11:56:50 -0600
Subject: [PATCH 78/95] getting to a working mfgmg

---
 include/input_information.h    |  3 ++
 include/matrix_free_gmg.h      | 93 +++++++++++++++++++++++-----------
 include/schur_preconditioner.h | 29 ++++++-----
 source/kkt_system.cc           |  2 +-
 source/matrix_free_gmg.cc      | 62 +++++++++++++++++------
 source/schur_preconditioner.cc |  5 +-
 6 files changed, 131 insertions(+), 63 deletions(-)

diff --git a/include/input_information.h b/include/input_information.h
index 9859154..342f920 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -49,6 +49,9 @@ namespace SAND {
         constexpr unsigned int a_inv_iterations = 25;
         constexpr unsigned int k_inv_iterations = 25;
 
+        //Material Options
+        constexpr double material_lambda = 1;
+        constexpr double material_mu = 1;
 
     }
 }
diff --git a/include/matrix_free_gmg.h b/include/matrix_free_gmg.h
index 68462ea..f6d84d1 100644
--- a/include/matrix_free_gmg.h
+++ b/include/matrix_free_gmg.h
@@ -1,48 +1,81 @@
 #ifndef MATRIX_FREE_GMG_H
 #define MATRIX_FREE_GMG_H
 
-#include <deal.II/lac/linear_operator.h>
-#include <deal.II/lac/sparse_direct.h>
+#include <deal.II/base/quadrature_lib.h>
+#include <deal.II/base/function.h>
+#include <deal.II/base/timer.h>
+#include <deal.II/lac/affine_constraints.h>
+#include <deal.II/lac/solver_cg.h>
+#include <deal.II/lac/la_parallel_vector.h>
+#include <deal.II/lac/precondition.h>
+#include <deal.II/fe/fe_q.h>
+#include <deal.II/fe/fe_system.h>
+#include <deal.II/grid/tria.h>
+#include <deal.II/grid/grid_generator.h>
+#include <deal.II/multigrid/multigrid.h>
+#include <deal.II/multigrid/mg_transfer_matrix_free.h>
+#include <deal.II/multigrid/mg_tools.h>
+#include <deal.II/multigrid/mg_coarse.h>
+#include <deal.II/multigrid/mg_smoother.h>
+#include <deal.II/multigrid/mg_matrix.h>
+#include <deal.II/numerics/data_out.h>
+#include <deal.II/numerics/vector_tools.h>
+#include <deal.II/matrix_free/matrix_free.h>
+#include <deal.II/matrix_free/operators.h>
+#include <deal.II/matrix_free/fe_evaluation.h>
+
+
+
+namespace SAND
+{
+    using namespace dealii;
 
-using namespace dealii;
+    template <int dim, typename number>
+    struct OperatorCellData
+    {
+       Table<2, VectorizedArray<number>> viscosity;
 
-template <int dim, int fe_degree, typename number>
-class ElasticityOperator
-  : public MatrixFreeOperators::
-      Base<dim, LinearAlgebra::distributed::Vector<number>>
-{
-public:
+       std::size_t
+       memory_consumption() const;
+    };
+
+    template <int dim, int fe_degree, typename number>
+    class ElasticityOperator
+      : public MatrixFreeOperators::
+          Base<dim, LinearAlgebra::distributed::Vector<number>>
+    {
+    public:
+
+      using value_type = number;
 
-  using value_type = number;
+      ElasticityOperator(DoFHandler<dim> &big_dof_handler_in, BlockVector<double> &state_in);
 
-  ElasticityOperator();
+      void clear() override;
 
-  void clear() override;
+    private:
 
-  void evaluate_coefficient(const Coefficient<dim> &coefficient_function);
+      double coefficient_rho_p_lambda(const auto &cell_iter, unsigned int q);
+      double coefficient_2_rho_p_mu(const auto &cell_iter, unsigned int q);
+      const BlockVector<double> &state;
 
-  virtual void compute_diagonal() override;
 
-private:
+      const DoFHandler<dim> &big_dof_handler;
+      DoFHandler<dim> displacement_dof_handler;
+      FESystem<dim> fe_displacement;
+//      virtual void apply_add(
+//        LinearAlgebra::distributed::Vector<number> &      dst,
+//        const LinearAlgebra::distributed::Vector<number> &src) const override;
 
-  virtual void apply_add(
-    LinearAlgebra::distributed::Vector<number> &      dst,
-    const LinearAlgebra::distributed::Vector<number> &src) const override;
+      void
+      local_apply(const MatrixFree<dim, number> &                   data,
+                  LinearAlgebra::distributed::Vector<number> &      dst,
+                  const LinearAlgebra::distributed::Vector<number> &src,
+                  const std::pair<unsigned int, unsigned int> &cell_range);
 
-  void
-  local_apply(const MatrixFree<dim, number> &                   data,
-              LinearAlgebra::distributed::Vector<number> &      dst,
-              const LinearAlgebra::distributed::Vector<number> &src,
-              const std::pair<unsigned int, unsigned int> &cell_range) const;
+    };
 
-  void local_compute_diagonal(
-    const MatrixFree<dim, number> &              data,
-    LinearAlgebra::distributed::Vector<number> & dst,
-    const unsigned int &                         dummy,
-    const std::pair<unsigned int, unsigned int> &cell_range) const;
+}
 
-  Table<2, VectorizedArray<number>> coefficient;
-};
 
 
 
diff --git a/include/schur_preconditioner.h b/include/schur_preconditioner.h
index d0a1e35..73125eb 100644
--- a/include/schur_preconditioner.h
+++ b/include/schur_preconditioner.h
@@ -184,7 +184,7 @@ namespace SAND
     template<int dim>
     class TopOptSchurPreconditioner: public Subscriptor {
     public:
-        TopOptSchurPreconditioner(LA::MPI::BlockSparseMatrix &matrix_in);
+        TopOptSchurPreconditioner(LA::MPI::BlockSparseMatrix &matrix_in, DoFHandler<dim> &big_dof_handler_in);
         void initialize (LA::MPI::BlockSparseMatrix &matrix, const std::map<types::global_dof_index, double> &boundary_values, const DoFHandler<dim> &dof_handler, const LA::MPI::BlockVector &distributed_state);
         void vmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
         void Tvmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
@@ -230,6 +230,7 @@ namespace SAND
         const LA::MPI::SparseMatrix &d_1_mat;
         const LA::MPI::SparseMatrix &d_2_mat;
         const LA::MPI::SparseMatrix &m_vect;
+        const DoFHandler<dim> &big_dof_handler;
 
         LA::MPI::SparseMatrix d_3_mat;
         LA::MPI::SparseMatrix d_4_mat;
@@ -261,19 +262,19 @@ namespace SAND
 
 
 
-        LinearOperator<VectorType,VectorType,PayloadType> op_d_8;
-        LinearOperator<VectorType,VectorType,PayloadType> op_f;
-        LinearOperator<VectorType,VectorType,PayloadType> op_b;
-        LinearOperator<VectorType,VectorType,PayloadType> op_c;
-        LinearOperator<VectorType,VectorType,PayloadType> op_a_inv;
-        LinearOperator<VectorType,VectorType,PayloadType> op_a_inv_ind;
-        LinearOperator<VectorType,VectorType,PayloadType> op_e;
-        LinearOperator<VectorType,VectorType,PayloadType> op_d_m;
-        LinearOperator<VectorType,VectorType,PayloadType> op_d_m_inv;
-        LinearOperator<VectorType,VectorType,PayloadType> op_g;
-        LinearOperator<VectorType,VectorType,PayloadType> op_h;
-        LinearOperator<VectorType,VectorType,PayloadType> op_k_inv;
-        LinearOperator<VectorType,VectorType,PayloadType> op_j_inv;
+//        LinearOperator<VectorType,VectorType,PayloadType> op_d_8;
+//        LinearOperator<VectorType,VectorType,PayloadType> op_f;
+//        LinearOperator<VectorType,VectorType,PayloadType> op_b;
+//        LinearOperator<VectorType,VectorType,PayloadType> op_c;
+//        LinearOperator<VectorType,VectorType,PayloadType> op_a_inv;
+//        LinearOperator<VectorType,VectorType,PayloadType> op_a_inv_ind;
+//        LinearOperator<VectorType,VectorType,PayloadType> op_e;
+//        LinearOperator<VectorType,VectorType,PayloadType> op_d_m;
+//        LinearOperator<VectorType,VectorType,PayloadType> op_d_m_inv;
+//        LinearOperator<VectorType,VectorType,PayloadType> op_g;
+//        LinearOperator<VectorType,VectorType,PayloadType> op_h;
+//        LinearOperator<VectorType,VectorType,PayloadType> op_k_inv;
+//        LinearOperator<VectorType,VectorType,PayloadType> op_j_inv;
     };
 
 
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index 6b24b68..7e1883b 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -1780,7 +1780,7 @@ namespace SAND {
         locally_relevant_solution=state;
         distributed_solution = state;
         SolverControl solver_control(1000000, gmres_tolerance * system_rhs.l2_norm());
-        TopOptSchurPreconditioner<dim> preconditioner(system_matrix);
+        TopOptSchurPreconditioner<dim> preconditioner(system_matrix, dof_handler);
         pcout << "about to solve" << std::endl;
 
         switch (Input::solver_choice) {
diff --git a/source/matrix_free_gmg.cc b/source/matrix_free_gmg.cc
index 8a0b712..d008a91 100644
--- a/source/matrix_free_gmg.cc
+++ b/source/matrix_free_gmg.cc
@@ -2,34 +2,64 @@
 // Created by justin on 2/17/21.
 //
 #include "../include/matrix_free_gmg.h"
+#include "../include/input_information.h"
+#include "../include/parameters_and_components.h"
 namespace SAND {
     using namespace dealii;
 
 
     template <int dim, int fe_degree, typename number>
-    ElasticityOperator<dim, fe_degree, number>::ElasticityOperator()
+    ElasticityOperator<dim, fe_degree, number>::ElasticityOperator(DoFHandler<dim> &big_dof_handler_in, BlockVector<double> &state_in)
       : MatrixFreeOperators::Base<dim,
-                                  LinearAlgebra::distributed::Vector<number>>()
-    {}
+                                  LinearAlgebra::distributed::Vector<number>>(),
+        big_dof_handler(big_dof_handler_in),
+        displacement_dof_handler(big_dof_handler.get_triangulation()),
+        fe_displacement(FE_Q<dim>(1) ^ dim),
+        state(state_in)
+    {
+        displacement_dof_handler.distribute_dofs(fe_displacement);
+
+    }
 
     template <int dim, int fe_degree, typename number>
     void ElasticityOperator<dim, fe_degree, number>::clear()
     {
-      MatrixFreeOperators::Base<dim, LinearAlgebra::distributed::Vector<number>>::
-        clear();
+      MatrixFreeOperators::Base<dim, LinearAlgebra::distributed::Vector<number>>::clear();
+    }
+
+    template <int dim, int fe_degree, typename number>
+    double ElasticityOperator<dim, fe_degree, number>::coefficient_rho_p_lambda(const  auto &cell_iter)
+    {
+        std::vector<unsigned int> i(cell_iter->get_fe().n_dofs_per_cell());
+        cell_iter->get_dof_indices(i);
+        return std::pow(state.block(SolutionBlocks::density)[i[cell_iter->get_fe().component_to_system_index(0, 0)]],Input::density_penalty_exponent)*Input::material_lambda;
     }
 
+    template <int dim, int fe_degree, typename number>
+    double ElasticityOperator<dim, fe_degree, number>::coefficient_2_rho_p_mu(const auto &cell_iter)
+    {
+        std::vector<unsigned int> i(cell_iter->get_fe().n_dofs_per_cell());
+        cell_iter->get_dof_indices(i);
+        return std::pow(state.block(SolutionBlocks::density)[i[cell_iter->get_fe().component_to_system_index(0, 0)]],Input::density_penalty_exponent)*2*Input::material_mu;
+
+    }
+
+
     template <int dim, int fe_degree, typename number>
     void ElasticityOperator<dim, fe_degree, number>::local_apply(
       const MatrixFree<dim, number> &                   data,
       LinearAlgebra::distributed::Vector<number> &      dst,
       const LinearAlgebra::distributed::Vector<number> &src,
-      const std::pair<unsigned int, unsigned int> &     cell_range) const
+      const std::pair<unsigned int, unsigned int> &     cell_range)
     {
-      FEEvaluation<dim, fe_degree, fe_degree + 1, 1, number> phi(data);
-      for (unsigned int cell = cell_range.first; cell < cell_range.second; ++cell)
-        {
-          phi.reinit(cell);
+      FEEvaluation<dim, 1, 2, dim, double> phi(data);
+      auto big_dof_iter = big_dof_handler.begin_active();
+      auto displacement_dof_iter = displacement_dof_handler.begin_active();
+      for (const auto &cell_iter: triangulation.active_cell_iterators())
+      {
+          big_dof_iter.copy_from(cell_iter);
+          displacement_dof_iter.copy_from(cell_iter);
+          phi.reinit(displacement_dof_iter);
           phi.read_dof_values(src);
 
           phi.evaluate(EvaluationFlags::gradients);
@@ -37,15 +67,15 @@ namespace SAND {
           for (unsigned int q = 0; q < phi.n_q_points; ++q)
           {
 
-            phi.submit_divergence(coefficient_rho_p_lambda(cell, q) * phi.get_gradient(q), q);
-            phi.submit_symmetric_gradient(coefficient_2_rho_p_mu(cell, q) * phi.get_gradient(q), q);
+            phi.submit_divergence(coefficient_rho_p_lambda(big_dof_iter) * phi.get_divergence(q), q);
+            phi.submit_symmetric_gradient(coefficient_2_rho_p_mu(big_dof_iter) * phi.get_symmetric_gradient(q), q);
 
           }
+
           phi.integrate(EvaluationFlags::gradients);
           phi.distribute_local_to_global(dst);
-        }
+      }
     }
-
 }
-template class SAND::ElasticityOperator<2>;
-template class SAND::ElasticityOperator<3>;
+template class SAND::ElasticityOperator<2,1,double>;
+template class SAND::ElasticityOperator<3,1,double>;
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 9fe276a..56e2d36 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -21,7 +21,7 @@ namespace SAND {
     using size_type         = dealii::types::global_dof_index;
     using namespace dealii;
     template<int dim>
-    TopOptSchurPreconditioner<dim>::TopOptSchurPreconditioner(LA::MPI::BlockSparseMatrix &matrix_in)
+    TopOptSchurPreconditioner<dim>::TopOptSchurPreconditioner(LA::MPI::BlockSparseMatrix &matrix_in, DoFHandler<dim> &big_dof_handler_in)
             :
             system_matrix(matrix_in),
             mpi_communicator(MPI_COMM_WORLD),
@@ -51,7 +51,8 @@ namespace SAND {
             g_mat(f_mat, d_8_mat),
             h_mat(a_mat, b_mat, c_mat, e_mat, pre_amg, a_inv_direct),
             j_inv_mat(h_mat, g_mat, d_m_mat, d_m_inv_mat),
-            k_inv_mat(h_mat, g_mat, d_m_mat, d_m_inv_mat)
+            k_inv_mat(h_mat, g_mat, d_m_mat, d_m_inv_mat),
+            big_dof_handler(big_dof_handler_in)
     {
 
     }

From 632ce1509108a7cee63a30bf58d85c55af27bfc9 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <oconnor@up.math.colostate.edu>
Date: Mon, 11 Apr 2022 12:29:21 -0600
Subject: [PATCH 79/95] MFGMG running, but solve is incorrect. Very pretty
 though?

---
 include/kkt_system.h             |   50 +
 include/matrix_free_elasticity.h |  103 +
 include/matrix_free_gmg.h        |   82 -
 include/schur_preconditioner.h   |   37 +-
 source/kkt_system.cc             | 4131 +++++++++++++++++-------------
 source/matrix_free_elasticity.cc |  179 ++
 source/matrix_free_gmg.cc        |   81 -
 source/schur_preconditioner.cc   |    7 +-
 8 files changed, 2735 insertions(+), 1935 deletions(-)
 create mode 100644 include/matrix_free_elasticity.h
 delete mode 100644 include/matrix_free_gmg.h
 create mode 100644 source/matrix_free_elasticity.cc
 delete mode 100644 source/matrix_free_gmg.cc

diff --git a/include/kkt_system.h b/include/kkt_system.h
index 5d79cf9..7aa94a8 100644
--- a/include/kkt_system.h
+++ b/include/kkt_system.h
@@ -53,8 +53,21 @@
 #include <deal.II/distributed/tria.h>
 #include <deal.II/distributed/grid_refinement.h>
 
+#include <deal.II/multigrid/multigrid.h>
+#include <deal.II/multigrid/mg_transfer_matrix_free.h>
+#include <deal.II/multigrid/mg_tools.h>
+#include <deal.II/multigrid/mg_coarse.h>
+#include <deal.II/multigrid/mg_smoother.h>
+#include <deal.II/multigrid/mg_matrix.h>
+
+#include <deal.II/matrix_free/matrix_free.h>
+#include <deal.II/matrix_free/operators.h>
+#include <deal.II/matrix_free/fe_evaluation.h>
+
+
 #include "../include/schur_preconditioner.h"
 #include "../include/density_filter.h"
+#include "matrix_free_elasticity.h"
 
 #include <deal.II/base/conditional_ostream.h>
 
@@ -62,6 +75,9 @@
 #include <fstream>
 #include <algorithm>
 namespace SAND {
+
+
+
     namespace LA
     {
         using namespace dealii::LinearAlgebraTrilinos;
@@ -71,6 +87,8 @@ namespace SAND {
     template<int dim>
     class KktSystem {
 
+    using SystemMFMatrixType = MF_Elasticity_Operator<dim, 1, double>;
+    using LevelMFMatrixType = MF_Elasticity_Operator<dim, 1, double>;
     public:
         MPI_Comm  mpi_communicator;
         std::vector<IndexSet> owned_partitioning;
@@ -128,6 +146,9 @@ namespace SAND {
         LA::MPI::BlockVector
         calculate_rhs(const LA::MPI::BlockVector &test_solution, const double barrier_size) const;
 
+       PreconditionMG<dim,LinearAlgebra::distributed::Vector<double>,MGTransferMatrixFree<dim, double>>
+            build_mf_gmg_preconditioner();
+
         BlockDynamicSparsityPattern dsp;
         BlockSparsityPattern sparsity_pattern;
         mutable LA::MPI::BlockSparseMatrix system_matrix;
@@ -136,20 +157,49 @@ namespace SAND {
         LA::MPI::BlockVector system_rhs;
         parallel::distributed::Triangulation<dim> triangulation;
         DoFHandler<dim> dof_handler;
+        DoFHandler<dim> dof_handler_displacement;
+        DoFHandler<dim> dof_handler_density;
+
         AffineConstraints<double> constraints;
         FESystem<dim> fe_nine;
         FESystem<dim> fe_ten;
         hp::FECollection<dim> fe_collection;
+        FESystem<dim> fe_displacement;
+        FE_DGQ<dim> fe_density;
         const double density_ratio;
         const double density_penalty_exponent;
 
         mutable DensityFilter<dim> density_filter;
 
         std::map<types::global_dof_index, double> boundary_values;
+        MGLevelObject<std::map<types::global_dof_index, double>> level_boundary_values;
         ConditionalOStream pcout;
 
         double initial_rhs_error;
 
+        MGConstrainedDoFs mg_constrained_dofs;
+        SystemMFMatrixType elasticity_system_mf;
+        MGLevelObject<LevelMFMatrixType> mg_matrices;
+
+
+        OperatorCellData<dim, GMGNumberType> active_cell_data;
+        MGLevelObject<OperatorCellData<dim, GMGNumberType>> level_cell_data;
+        dealii::LinearAlgebra::distributed::Vector<double> active_density_vector;
+        MGLevelObject<dealii::LinearAlgebra::distributed::Vector<double>> level_density_vector;
+        MGTransferMatrixFree<dim,GMGNumberType> transfer;
+        MGTransferMatrixFree<dim, double> mg_transfer;
+
+        using SmootherType = PreconditionChebyshev<LevelMFMatrixType, LinearAlgebra::distributed::Vector<double>>;
+        mg::SmootherRelaxation<SmootherType,LinearAlgebra::distributed::Vector<double>> mg_smoother;
+        MGLevelObject<typename SmootherType::AdditionalData> smoother_data;
+        MGCoarseGridApplySmoother<LinearAlgebra::distributed::Vector<double>> mg_coarse;
+        MGLevelObject<MatrixFreeOperators::MGInterfaceOperator<LevelMFMatrixType>> mg_interface_matrices;
+
+        std::set<types::boundary_id> dirichlet_boundary;
+
+        LinearAlgebra::distributed::Vector<double> distributed_displacement_sol;
+        LinearAlgebra::distributed::Vector<double> distributed_displacement_rhs;
+
     };
 }
 
diff --git a/include/matrix_free_elasticity.h b/include/matrix_free_elasticity.h
new file mode 100644
index 0000000..6f5ea7b
--- /dev/null
+++ b/include/matrix_free_elasticity.h
@@ -0,0 +1,103 @@
+#ifndef MATRIX_FREE_GMG_H
+#define MATRIX_FREE_GMG_H
+
+#include <deal.II/base/quadrature_lib.h>
+#include <deal.II/base/function.h>
+#include <deal.II/base/timer.h>
+#include <deal.II/lac/affine_constraints.h>
+#include <deal.II/lac/solver_cg.h>
+#include <deal.II/lac/la_parallel_vector.h>
+#include <deal.II/lac/precondition.h>
+#include <deal.II/fe/fe_q.h>
+#include <deal.II/fe/fe_system.h>
+#include <deal.II/grid/tria.h>
+#include <deal.II/grid/grid_generator.h>
+#include <deal.II/dofs/dof_handler.h>
+#include <deal.II/dofs/dof_renumbering.h>
+#include <deal.II/dofs/dof_tools.h>
+#include <deal.II/multigrid/multigrid.h>
+#include <deal.II/multigrid/mg_transfer_matrix_free.h>
+#include <deal.II/multigrid/mg_tools.h>
+#include <deal.II/multigrid/mg_coarse.h>
+#include <deal.II/multigrid/mg_smoother.h>
+#include <deal.II/multigrid/mg_matrix.h>
+#include <deal.II/numerics/data_out.h>
+#include <deal.II/numerics/vector_tools.h>
+#include <deal.II/matrix_free/matrix_free.h>
+#include <deal.II/matrix_free/operators.h>
+#include <deal.II/matrix_free/fe_evaluation.h>
+
+#include "../include/parameters_and_components.h"
+#include "../include/input_information.h"
+
+
+
+namespace SAND
+{
+using GMGNumberType = double;
+using namespace dealii;
+
+template <int dim, typename number>
+struct OperatorCellData
+{
+    Table<2, VectorizedArray<number>> density;
+    std::size_t
+    memory_consumption() const;
+};
+
+template <int dim, int fe_degree, typename number>
+class MF_Elasticity_Operator
+        : public MatrixFreeOperators::
+        Base<dim, LinearAlgebra::distributed::Vector<number>>
+{
+public:
+
+    using value_type = number;
+
+    MF_Elasticity_Operator();
+
+    void set_cell_data (const OperatorCellData<dim,number> &data);
+
+    void compute_diagonal() override;
+
+    void clear() override;
+
+private:
+
+    void apply_add (dealii::LinearAlgebra::distributed::Vector<number> &dst,
+                    const dealii::LinearAlgebra::distributed::Vector<number> &src) const override;
+
+
+    void local_apply (const dealii::MatrixFree<dim, number> &data,
+                      dealii::LinearAlgebra::distributed::Vector<number> &dst,
+                      const dealii::LinearAlgebra::distributed::Vector<number> &src,
+                      const std::pair<unsigned int, unsigned int> &cell_range) const;
+
+    void local_apply_face (const dealii::MatrixFree<dim, number> &data,
+                           dealii::LinearAlgebra::distributed::Vector<number> &dst,
+                           const dealii::LinearAlgebra::distributed::Vector<number> &src,
+                           const std::pair<unsigned int, unsigned int> &face_range) const;
+
+
+    void local_apply_boundary_face (const dealii::MatrixFree<dim, number> &data,
+                                    dealii::LinearAlgebra::distributed::Vector<number> &dst,
+                                    const dealii::LinearAlgebra::distributed::Vector<number> &src,
+                                    const std::pair<unsigned int, unsigned int> &face_range) const;
+
+    void local_compute_diagonal (const MatrixFree<dim,number>                     &data,
+                              dealii::LinearAlgebra::distributed::Vector<number>  &dst,
+                              const unsigned int &,
+                              const std::pair<unsigned int,unsigned int>       &cell_range) const;
+
+
+    const OperatorCellData<dim,number> *cell_data;
+
+};
+
+
+}
+
+
+
+
+#endif // MATRIX_FREE_GMG_H
diff --git a/include/matrix_free_gmg.h b/include/matrix_free_gmg.h
deleted file mode 100644
index f6d84d1..0000000
--- a/include/matrix_free_gmg.h
+++ /dev/null
@@ -1,82 +0,0 @@
-#ifndef MATRIX_FREE_GMG_H
-#define MATRIX_FREE_GMG_H
-
-#include <deal.II/base/quadrature_lib.h>
-#include <deal.II/base/function.h>
-#include <deal.II/base/timer.h>
-#include <deal.II/lac/affine_constraints.h>
-#include <deal.II/lac/solver_cg.h>
-#include <deal.II/lac/la_parallel_vector.h>
-#include <deal.II/lac/precondition.h>
-#include <deal.II/fe/fe_q.h>
-#include <deal.II/fe/fe_system.h>
-#include <deal.II/grid/tria.h>
-#include <deal.II/grid/grid_generator.h>
-#include <deal.II/multigrid/multigrid.h>
-#include <deal.II/multigrid/mg_transfer_matrix_free.h>
-#include <deal.II/multigrid/mg_tools.h>
-#include <deal.II/multigrid/mg_coarse.h>
-#include <deal.II/multigrid/mg_smoother.h>
-#include <deal.II/multigrid/mg_matrix.h>
-#include <deal.II/numerics/data_out.h>
-#include <deal.II/numerics/vector_tools.h>
-#include <deal.II/matrix_free/matrix_free.h>
-#include <deal.II/matrix_free/operators.h>
-#include <deal.II/matrix_free/fe_evaluation.h>
-
-
-
-namespace SAND
-{
-    using namespace dealii;
-
-    template <int dim, typename number>
-    struct OperatorCellData
-    {
-       Table<2, VectorizedArray<number>> viscosity;
-
-       std::size_t
-       memory_consumption() const;
-    };
-
-    template <int dim, int fe_degree, typename number>
-    class ElasticityOperator
-      : public MatrixFreeOperators::
-          Base<dim, LinearAlgebra::distributed::Vector<number>>
-    {
-    public:
-
-      using value_type = number;
-
-      ElasticityOperator(DoFHandler<dim> &big_dof_handler_in, BlockVector<double> &state_in);
-
-      void clear() override;
-
-    private:
-
-      double coefficient_rho_p_lambda(const auto &cell_iter, unsigned int q);
-      double coefficient_2_rho_p_mu(const auto &cell_iter, unsigned int q);
-      const BlockVector<double> &state;
-
-
-      const DoFHandler<dim> &big_dof_handler;
-      DoFHandler<dim> displacement_dof_handler;
-      FESystem<dim> fe_displacement;
-//      virtual void apply_add(
-//        LinearAlgebra::distributed::Vector<number> &      dst,
-//        const LinearAlgebra::distributed::Vector<number> &src) const override;
-
-      void
-      local_apply(const MatrixFree<dim, number> &                   data,
-                  LinearAlgebra::distributed::Vector<number> &      dst,
-                  const LinearAlgebra::distributed::Vector<number> &src,
-                  const std::pair<unsigned int, unsigned int> &cell_range);
-
-    };
-
-}
-
-
-
-
-#endif // MATRIX_FREE_GMG_H
diff --git a/include/schur_preconditioner.h b/include/schur_preconditioner.h
index 73125eb..e2c010f 100644
--- a/include/schur_preconditioner.h
+++ b/include/schur_preconditioner.h
@@ -60,6 +60,7 @@
 
 
 #include "../include/parameters_and_components.h"
+#include "matrix_free_elasticity.h"
 
 #include <deal.II/base/conditional_ostream.h>
 
@@ -100,6 +101,22 @@ namespace SAND
             int size;
     };
 
+    class AInvMatGMGMF : public TrilinosWrappers::SparseMatrix {
+        public:
+            AInvMatGMGMF();
+            void vmult() const;
+            void Tvmult() const;
+            void initialize();
+            unsigned int m() const;
+            unsigned int n() const;
+        private:
+            const LA::MPI::SparseMatrix &f_mat;
+            LA::MPI::SparseMatrix &d_8_mat;
+            mutable LA::MPI::Vector temp_vect_1;
+            mutable LA::MPI::Vector temp_vect_2;
+
+
+    };
 
     class GMatrix : public TrilinosWrappers::SparseMatrix {
         public:
@@ -184,7 +201,8 @@ namespace SAND
     template<int dim>
     class TopOptSchurPreconditioner: public Subscriptor {
     public:
-        TopOptSchurPreconditioner(LA::MPI::BlockSparseMatrix &matrix_in, DoFHandler<dim> &big_dof_handler_in);
+        TopOptSchurPreconditioner(LA::MPI::BlockSparseMatrix &matrix_in, DoFHandler<dim> &big_dof_handler_in, MF_Elasticity_Operator<dim,1,double> &mf_elasticity_operator_in , PreconditionMG<dim,LinearAlgebra::distributed::Vector<double>,MGTransferMatrixFree<dim, double>>
+                                  &mf_gmg_preconditioner_in);
         void initialize (LA::MPI::BlockSparseMatrix &matrix, const std::map<types::global_dof_index, double> &boundary_values, const DoFHandler<dim> &dof_handler, const LA::MPI::BlockVector &distributed_state);
         void vmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
         void Tvmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
@@ -260,21 +278,8 @@ namespace SAND
         JinvMatrix j_inv_mat;
         KinvMatrix k_inv_mat;
 
-
-
-//        LinearOperator<VectorType,VectorType,PayloadType> op_d_8;
-//        LinearOperator<VectorType,VectorType,PayloadType> op_f;
-//        LinearOperator<VectorType,VectorType,PayloadType> op_b;
-//        LinearOperator<VectorType,VectorType,PayloadType> op_c;
-//        LinearOperator<VectorType,VectorType,PayloadType> op_a_inv;
-//        LinearOperator<VectorType,VectorType,PayloadType> op_a_inv_ind;
-//        LinearOperator<VectorType,VectorType,PayloadType> op_e;
-//        LinearOperator<VectorType,VectorType,PayloadType> op_d_m;
-//        LinearOperator<VectorType,VectorType,PayloadType> op_d_m_inv;
-//        LinearOperator<VectorType,VectorType,PayloadType> op_g;
-//        LinearOperator<VectorType,VectorType,PayloadType> op_h;
-//        LinearOperator<VectorType,VectorType,PayloadType> op_k_inv;
-//        LinearOperator<VectorType,VectorType,PayloadType> op_j_inv;
+        MF_Elasticity_Operator<dim,1,double> &mf_elasticity_operator;
+        PreconditionMG<dim,LinearAlgebra::distributed::Vector<double>,MGTransferMatrixFree<dim, double>>  &mf_gmg_preconditioner;
     };
 
 
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index 7e1883b..e02da9a 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -39,6 +39,7 @@
 #include <deal.II/base/conditional_ostream.h>
 
 #include "../include/input_information.h"
+#include "../include/matrix_free_elasticity.h"
 
 #include <iostream>
 #include <algorithm>
@@ -46,1139 +47,2139 @@
 // This problem initializes with a FESystem composed of 2×dim FE_Q(1) elements, and 8 FE_DGQ(0)  elements.
 // The  piecewise  constant  functions  are  for  density-related  variables,and displacement-related variables are assigned to the FE_Q(1) elements.
 namespace SAND {
-    template<int dim>
-    KktSystem<dim>::KktSystem()
-            :
-            mpi_communicator(MPI_COMM_WORLD),
-            triangulation(mpi_communicator,
-                          typename Triangulation<dim>::MeshSmoothing(
-                                  Triangulation<dim>::smoothing_on_refinement |
-                                  Triangulation<dim>::smoothing_on_coarsening)),
-            dof_handler(triangulation),
-            /*fe should have 1 FE_DGQ<dim>(0) element for density, dim FE_Q finite elements for displacement,
-             * another dim FE_Q elements for the lagrange multiplier on the FE constraint, and 2 more FE_DGQ<dim>(0)
-             * elements for the upper and lower bound constraints */
-            fe_nine(FE_DGQ<dim>(0) ^ 5,
-                    (FESystem<dim>(FE_Q<dim>(1) ^ dim)) ^ 2,
-                    FE_DGQ<dim>(0) ^ 2,
-                    FE_Nothing<dim>() ^ 1),
-            fe_ten(FE_DGQ<dim>(0) ^ 5,
-                   (FESystem<dim>(FE_Q<dim>(1) ^ dim)) ^ 2,
-                   FE_DGQ<dim>(0) ^ 2,
-                   FE_DGQ<dim>(0) ^ 1),
-            density_ratio(Input::volume_percentage),
-            density_penalty_exponent(Input::density_penalty_exponent),
-            density_filter(),
-            pcout(std::cout,(Utilities::MPI::this_mpi_process(mpi_communicator) == 0))
-    {
-        fe_collection.push_back(fe_nine);
-        fe_collection.push_back(fe_ten);
 
-    }
+namespace ChangeVectorTypes
+{
+template <typename number>
+void copy(LA::MPI::Vector &                                         out,
+          const dealii::LinearAlgebra::distributed::Vector<number> &in)
+{
+    dealii::LinearAlgebra::ReadWriteVector<double> rwv(
+                out.locally_owned_elements());
+    rwv.import(in, VectorOperation::insert);
+    out.import(rwv, VectorOperation::insert);
+}
+template <typename number>
+void copy(dealii::LinearAlgebra::distributed::Vector<number> &out,
+          const LA::MPI::Vector &                             in)
+{
+    dealii::LinearAlgebra::ReadWriteVector<double> rwv;
+    rwv.reinit(in);
+    out.import(rwv, VectorOperation::insert);
+}
+} // namespace ChangeVectorTypes
+
+
+template<int dim>
+KktSystem<dim>::KktSystem()
+    :
+      mpi_communicator(MPI_COMM_WORLD),
+      triangulation(mpi_communicator,
+                    Triangulation<dim>::limit_level_difference_at_vertices,
+                    parallel::distributed::Triangulation<dim>::construct_multigrid_hierarchy),
+      dof_handler(triangulation),
+      dof_handler_displacement(triangulation),
+      dof_handler_density(triangulation),
+      /*fe should have 1 FE_DGQ<dim>(0) element for density, dim FE_Q finite elements for displacement,
+                   * another dim FE_Q elements for the lagrange multiplier on the FE constraint, and 2 more FE_DGQ<dim>(0)
+                   * elements for the upper and lower bound constraints */
+      fe_nine(FE_DGQ<dim>(0) ^ 5,
+              (FESystem<dim>(FE_Q<dim>(1) ^ dim)) ^ 2,
+              FE_DGQ<dim>(0) ^ 2,
+              FE_Nothing<dim>() ^ 1),
+      fe_ten(FE_DGQ<dim>(0) ^ 5,
+             (FESystem<dim>(FE_Q<dim>(1) ^ dim)) ^ 2,
+             FE_DGQ<dim>(0) ^ 2,
+             FE_DGQ<dim>(0) ^ 1),
+      fe_displacement(FE_Q<dim>(1) ^ dim),
+      fe_density(0),
+      density_ratio(Input::volume_percentage),
+      density_penalty_exponent(Input::density_penalty_exponent),
+      density_filter(),
+      pcout(std::cout,(Utilities::MPI::this_mpi_process(mpi_communicator) == 0))
+{
+    fe_collection.push_back(fe_nine);
+    fe_collection.push_back(fe_ten);
+
+}
 
 
 //A  function  used  once  at  the  beginning  of  the  program,  this  creates  a  matrix  H  so  that H* unfiltered density = filtered density
 
-    template<int dim>
-    void
-    KktSystem<dim>::setup_filter_matrix() {
+template<int dim>
+void
+KktSystem<dim>::setup_filter_matrix() {
 
-        density_filter.initialize(dof_handler);
-    }
+    density_filter.initialize(dof_handler);
+}
 
-    //This triangulation matches the problem description -
-    // a 6-by-1 rectangle where a force will be applied in the top center.
+//This triangulation matches the problem description -
+// a 6-by-1 rectangle where a force will be applied in the top center.
 
-    template<int dim>
-    void
-    KktSystem<dim>::create_triangulation() {
+template<int dim>
+void
+KktSystem<dim>::create_triangulation() {
 
-        std::vector<unsigned int> sub_blocks(2*dim+8, 0);
+    std::vector<unsigned int> sub_blocks(2*dim+8, 0);
 
-        sub_blocks[0]=0;
-        sub_blocks[1]=1;
-        sub_blocks[2]=2;
-        sub_blocks[3]=3;
-        sub_blocks[4]=4;
-        for(int i=0; i<dim; i++)
-        {
-            sub_blocks[5+i]=5;
-        }
-        for(int i=0; i<dim; i++)
+    sub_blocks[0]=0;
+    sub_blocks[1]=1;
+    sub_blocks[2]=2;
+    sub_blocks[3]=3;
+    sub_blocks[4]=4;
+    for(int i=0; i<dim; i++)
+    {
+        sub_blocks[5+i]=5;
+    }
+    for(int i=0; i<dim; i++)
+    {
+        sub_blocks[5+dim+i]=6;
+    }
+    sub_blocks[5+2*dim]=7;
+    sub_blocks[6+2*dim]=8;
+    sub_blocks[7+2*dim]=9;
+
+
+    if (Input::geometry_base == GeometryOptions::mbb) {
+        const double width = 6;
+        const unsigned int width_refine = 6;
+        const double height = 1;
+        const unsigned int height_refine = 1;
+        const double depth = 1;
+        const unsigned int depth_refine = 1;
+        const double downforce_y = 1;
+        const double downforce_x = 3;
+        const double downforce_size = .3;
+
+        if (dim == 2)
         {
-            sub_blocks[5+dim+i]=6;
-        }
-        sub_blocks[5+2*dim]=7;
-        sub_blocks[6+2*dim]=8;
-        sub_blocks[7+2*dim]=9;
-
-
-        if (Input::geometry_base == GeometryOptions::mbb) {
-            const double width = 6;
-            const unsigned int width_refine = 6;
-            const double height = 1;
-            const unsigned int height_refine = 1;
-            const double depth = 1;
-            const unsigned int depth_refine = 1;
-            const double downforce_y = 1;
-            const double downforce_x = 3;
-            const double downforce_size = .3;
-
-            if (dim == 2)
-            {
-                GridGenerator::subdivided_hyper_rectangle(triangulation,
-                                                          {width_refine, height_refine},
-                                                          Point<dim>(0, 0),
-                                                          Point<dim>(width, height));
+            GridGenerator::subdivided_hyper_rectangle(triangulation,
+                                                      {width_refine, height_refine},
+                                                      Point<dim>(0, 0),
+                                                      Point<dim>(width, height));
 
-                triangulation.refine_global(Input::refinements);
+            triangulation.refine_global(Input::refinements);
 
-                /*Set BCIDs   */
-                for (const auto &cell: dof_handler.active_cell_iterators()) {
-                    if(cell->is_locally_owned())
+            /*Set BCIDs   */
+            for (const auto &cell: dof_handler.active_cell_iterators())
+            {
+                if(cell->is_locally_owned())
+                {
+                    cell->set_active_fe_index(0);
+                    cell->set_material_id(MaterialIds::without_multiplier);
+                    for (unsigned int face_number = 0;
+                         face_number < GeometryInfo<dim>::faces_per_cell;
+                         ++face_number)
                     {
-                        cell->set_active_fe_index(0);
-                        cell->set_material_id(MaterialIds::without_multiplier);
-                        for (unsigned int face_number = 0;
-                             face_number < GeometryInfo<dim>::faces_per_cell;
-                             ++face_number) {
-                            if (cell->face(face_number)->at_boundary()) {
-                                const auto center = cell->face(face_number)->center();
+                        if (cell->face(face_number)->at_boundary())
+                        {
+                            const auto center = cell->face(face_number)->center();
 
-                                if (std::fabs(center(1) - downforce_y) < 1e-12) {
-                                    if (std::fabs(center(0) - downforce_x) < downforce_size) {
-                                        cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
-                                    } else {
-                                        cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
-                                    }
+                            if (std::fabs(center(1) - downforce_y) < 1e-12)
+                            {
+                                if (std::fabs(center(0) - downforce_x) < downforce_size)
+                                {
+                                    cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
+                                }
+                                else
+                                {
+                                    cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
                                 }
                             }
                         }
-                        for (unsigned int vertex_number = 0;
-                             vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                             ++vertex_number) {
-                            if (std::abs(cell->vertex(vertex_number)(0)) + std::abs(cell->vertex(vertex_number)(1)) <
+                    }
+                    for (unsigned int vertex_number = 0;
+                         vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                         ++vertex_number) {
+                        if (std::abs(cell->vertex(vertex_number)(0)) + std::abs(cell->vertex(vertex_number)(1)) <
                                 1e-10) {
-                                cell->set_active_fe_index(1);
-                                cell->set_material_id(MaterialIds::with_multiplier);
-                            }
+                            cell->set_active_fe_index(1);
+                            cell->set_material_id(MaterialIds::with_multiplier);
                         }
                     }
                 }
+            }
 
 
 
-                dof_handler.distribute_dofs(fe_collection);
+            dof_handler.distribute_dofs(fe_collection);
+            dof_handler_displacement.distribute_dofs(fe_displacement);
 
-                DoFRenumbering::component_wise(dof_handler, sub_blocks);
-            }
-            else if (dim == 3)
-            {
-                GridGenerator::subdivided_hyper_rectangle(triangulation,
-                                                          {width_refine, height_refine, depth_refine},
-                                                          Point<dim>(0, 0, 0),
-                                                          Point<dim>(width, height, depth));
+            DoFRenumbering::component_wise(dof_handler, sub_blocks);
+            DoFRenumbering::component_wise(dof_handler_displacement);
 
-                triangulation.refine_global(Input::refinements);
+//            DoFRenumbering::hierarchical(dof_handler);
+            DoFRenumbering::hierarchical(dof_handler_displacement);
+        }
+        else if (dim == 3)
+        {
+            GridGenerator::subdivided_hyper_rectangle(triangulation,
+                                                      {width_refine, height_refine, depth_refine},
+                                                      Point<dim>(0, 0, 0),
+                                                      Point<dim>(width, height, depth));
 
-                for (const auto &cell: dof_handler.active_cell_iterators()) {
-                    if (cell->is_locally_owned())
-                    {
-                        cell->set_active_fe_index(0);
-                        cell->set_material_id(MaterialIds::without_multiplier);
-                        for (unsigned int face_number = 0;
-                             face_number < GeometryInfo<dim>::faces_per_cell;
-                             ++face_number) {
-                            if (cell->face(face_number)->at_boundary()) {
-                                const auto center = cell->face(face_number)->center();
+            triangulation.refine_global(Input::refinements);
 
-                                if (std::fabs(center(1) - downforce_y) < 1e-12) {
-                                    if (std::fabs(center(0) - downforce_x) < downforce_size) {
-                                        cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
-                                    } else {
-                                        cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
-                                    }
+            for (const auto &cell: dof_handler.active_cell_iterators()) {
+                if (cell->is_locally_owned())
+                {
+                    cell->set_active_fe_index(0);
+                    cell->set_material_id(MaterialIds::without_multiplier);
+                    for (unsigned int face_number = 0;
+                         face_number < GeometryInfo<dim>::faces_per_cell;
+                         ++face_number) {
+                        if (cell->face(face_number)->at_boundary()) {
+                            const auto center = cell->face(face_number)->center();
+
+                            if (std::fabs(center(1) - downforce_y) < 1e-12) {
+                                if (std::fabs(center(0) - downforce_x) < downforce_size) {
+                                    cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
+                                } else {
+                                    cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
                                 }
                             }
                         }
-                        for (unsigned int vertex_number = 0;
-                             vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                             ++vertex_number) {
-                            if (std::abs(cell->vertex(vertex_number)(0)) + std::abs(cell->vertex(vertex_number)(1))
+                    }
+                    for (unsigned int vertex_number = 0;
+                         vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                         ++vertex_number) {
+                        if (std::abs(cell->vertex(vertex_number)(0)) + std::abs(cell->vertex(vertex_number)(1))
                                 + std::abs(cell->vertex(vertex_number)(2)) < 1e-10) {
-                                cell->set_active_fe_index(1);
-                                cell->set_material_id(MaterialIds::with_multiplier);
-                            }
+                            cell->set_active_fe_index(1);
+                            cell->set_material_id(MaterialIds::with_multiplier);
                         }
                     }
                 }
+            }
 
-                dof_handler.distribute_dofs(fe_collection);
+            dof_handler.distribute_dofs(fe_collection);
+            dof_handler_displacement.distribute_dofs(fe_displacement);
 
-                DoFRenumbering::component_wise(dof_handler, sub_blocks);
+            DoFRenumbering::component_wise(dof_handler, sub_blocks);
+            DoFRenumbering::component_wise(dof_handler_displacement);
 
-            } else {
-                throw;
-            }
-        } else if (Input::geometry_base == GeometryOptions::l_shape) {
-            const double width = 2;
-            const unsigned int width_refine = 2;
-            const double height = 2;
-            const unsigned int height_refine = 2;
-            const double depth = 1;
-            const unsigned int depth_refine = 1;
-            const double downforce_x = 2;
-            const double downforce_y = 1;
-            const double downforce_z = .5;
-            const double downforce_size = .3;
-
-            if (dim == 2) {
-                GridGenerator::subdivided_hyper_L(triangulation,
-                                                  {width_refine, height_refine},
-                                                  Point<dim>(0, 0),
-                                                  Point<dim>(width, height),
-                                                  {-1, -1});
-
-                triangulation.refine_global(Input::refinements);
-
-                /*Set BCIDs   */
-                for (const auto &cell: dof_handler.active_cell_iterators()) {
-                    if (cell->is_locally_owned())
-                    {
-                        cell->set_active_fe_index(0);
-                        cell->set_material_id(MaterialIds::without_multiplier);
-                        for (unsigned int face_number = 0;
-                             face_number < GeometryInfo<dim>::faces_per_cell;
-                             ++face_number) {
-                            if (cell->face(face_number)->at_boundary()) {
-                                const auto center = cell->face(face_number)->center();
+            DoFRenumbering::hierarchical(dof_handler);
+            DoFRenumbering::hierarchical(dof_handler_displacement);
 
-                                if (std::fabs(center(0) - downforce_x) < 1e-12) {
-                                    if (std::fabs(center(1) - downforce_y) < downforce_size) {
-                                        cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
-                                    } else {
-                                        cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
-                                    }
+        } else {
+            throw;
+        }
+    } else if (Input::geometry_base == GeometryOptions::l_shape) {
+        const double width = 2;
+        const unsigned int width_refine = 2;
+        const double height = 2;
+        const unsigned int height_refine = 2;
+        const double depth = 1;
+        const unsigned int depth_refine = 1;
+        const double downforce_x = 2;
+        const double downforce_y = 1;
+        const double downforce_z = .5;
+        const double downforce_size = .3;
+
+        if (dim == 2) {
+            GridGenerator::subdivided_hyper_L(triangulation,
+                                              {width_refine, height_refine},
+                                              Point<dim>(0, 0),
+                                              Point<dim>(width, height),
+                                              {-1, -1});
+
+            triangulation.refine_global(Input::refinements);
+
+            /*Set BCIDs   */
+            for (const auto &cell: dof_handler.active_cell_iterators()) {
+                if (cell->is_locally_owned())
+                {
+                    cell->set_active_fe_index(0);
+                    cell->set_material_id(MaterialIds::without_multiplier);
+                    for (unsigned int face_number = 0;
+                         face_number < GeometryInfo<dim>::faces_per_cell;
+                         ++face_number) {
+                        if (cell->face(face_number)->at_boundary()) {
+                            const auto center = cell->face(face_number)->center();
+
+                            if (std::fabs(center(0) - downforce_x) < 1e-12) {
+                                if (std::fabs(center(1) - downforce_y) < downforce_size) {
+                                    cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
+                                } else {
+                                    cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
                                 }
                             }
                         }
-                        for (unsigned int vertex_number = 0;
-                             vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                             ++vertex_number) {
-                            if (std::abs(cell->vertex(vertex_number)(0)) + std::abs(cell->vertex(vertex_number)(1)) <
+                    }
+                    for (unsigned int vertex_number = 0;
+                         vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                         ++vertex_number) {
+                        if (std::abs(cell->vertex(vertex_number)(0)) + std::abs(cell->vertex(vertex_number)(1)) <
                                 1e-10) {
-                                cell->set_active_fe_index(1);
-                                cell->set_material_id(MaterialIds::with_multiplier);
-                            }
+                            cell->set_active_fe_index(1);
+                            cell->set_material_id(MaterialIds::with_multiplier);
                         }
                     }
                 }
+            }
 
-                dof_handler.distribute_dofs(fe_collection);
+            dof_handler.distribute_dofs(fe_collection);
+            dof_handler_displacement.distribute_dofs(fe_displacement);
 
-                DoFRenumbering::component_wise(dof_handler, sub_blocks);
+            DoFRenumbering::component_wise(dof_handler, sub_blocks);
+            DoFRenumbering::component_wise(dof_handler_displacement);
 
-            } else if (dim == 3) {
-                GridGenerator::subdivided_hyper_L(triangulation,
-                                                  {width_refine, height_refine, depth_refine},
-                                                  Point<dim>(0, 0, 0),
-                                                  Point<dim>(width, height, depth),
-                                                  {-1, -1, depth_refine});
+            DoFRenumbering::hierarchical(dof_handler);
+            DoFRenumbering::hierarchical(dof_handler_displacement);
 
-                triangulation.refine_global(Input::refinements);
+        } else if (dim == 3) {
+            GridGenerator::subdivided_hyper_L(triangulation,
+                                              {width_refine, height_refine, depth_refine},
+                                              Point<dim>(0, 0, 0),
+                                              Point<dim>(width, height, depth),
+                                              {-1, -1, depth_refine});
 
-                /*Set BCIDs   */
-                for (const auto &cell: dof_handler.active_cell_iterators()) {
-                    if(cell->is_locally_owned())
-                    {
-                        cell->set_active_fe_index(0);
-                        cell->set_material_id(MaterialIds::without_multiplier);
-                        for (unsigned int face_number = 0;
-                             face_number < GeometryInfo<dim>::faces_per_cell;
-                             ++face_number) {
-                            if (cell->face(face_number)->at_boundary()) {
-                                const auto center = cell->face(face_number)->center();
+            triangulation.refine_global(Input::refinements);
 
-                                if (std::fabs(center(0) - downforce_x) < 1e-12) {
-                                    if (std::fabs(center(1) - downforce_y) < downforce_size) {
+            /*Set BCIDs   */
+            for (const auto &cell: dof_handler.active_cell_iterators()) {
+                if(cell->is_locally_owned())
+                {
+                    cell->set_active_fe_index(0);
+                    cell->set_material_id(MaterialIds::without_multiplier);
+                    for (unsigned int face_number = 0;
+                         face_number < GeometryInfo<dim>::faces_per_cell;
+                         ++face_number) {
+                        if (cell->face(face_number)->at_boundary()) {
+                            const auto center = cell->face(face_number)->center();
+
+                            if (std::fabs(center(0) - downforce_x) < 1e-12) {
+                                if (std::fabs(center(1) - downforce_y) < downforce_size) {
+                                    cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
+                                    if (std::fabs(center(2) - downforce_z) < downforce_size) {
                                         cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
-                                        if (std::fabs(center(2) - downforce_z) < downforce_size) {
-                                            cell->face(face_number)->set_boundary_id(BoundaryIds::down_force);
-                                        } else {
-                                            cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
-                                        }
                                     } else {
                                         cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
                                     }
+                                } else {
+                                    cell->face(face_number)->set_boundary_id(BoundaryIds::no_force);
                                 }
                             }
                         }
-                        for (unsigned int vertex_number = 0;
-                             vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                             ++vertex_number) {
-                            if (std::abs(cell->vertex(vertex_number)(0)) + std::abs(cell->vertex(vertex_number)(1)) <
+                    }
+                    for (unsigned int vertex_number = 0;
+                         vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                         ++vertex_number) {
+                        if (std::abs(cell->vertex(vertex_number)(0)) + std::abs(cell->vertex(vertex_number)(1)) <
                                 1e-10) {
-                                cell->set_active_fe_index(1);
-                                cell->set_material_id(MaterialIds::with_multiplier);
-                            }
+                            cell->set_active_fe_index(1);
+                            cell->set_material_id(MaterialIds::with_multiplier);
                         }
                     }
                 }
+            }
 
-                dof_handler.distribute_dofs(fe_collection);
+            dof_handler.distribute_dofs(fe_collection);
+            dof_handler_displacement.distribute_dofs(fe_displacement);
 
-                DoFRenumbering::component_wise(dof_handler, sub_blocks);
-            } else {
-                throw;
-            }
+            DoFRenumbering::component_wise(dof_handler, sub_blocks);
+            DoFRenumbering::component_wise(dof_handler_displacement);
+
+            DoFRenumbering::hierarchical(dof_handler);
+            DoFRenumbering::hierarchical(dof_handler_displacement);
         } else {
             throw;
         }
-
+    } else {
+        throw;
     }
 
+}
+
 // The  bottom  corners  are  kept  in  place  in  the  y  direction  -  the  bottom  left  also  in  the  x direction.
 // Because deal.ii is formulated to enforce boundary conditions along regions of the boundary,
 // we do this to ensure these BCs are only enforced at points.
-    template<int dim>
-    void
-    KktSystem<dim>::setup_boundary_values() {
-        if (Input::geometry_base == GeometryOptions::mbb) {
-            if (dim == 2) {
-                for (const auto &cell: dof_handler.active_cell_iterators()) {
+template<int dim>
+void
+KktSystem<dim>::setup_boundary_values()
+{
+    std::cout << "here" << std::endl;
+    if (Input::geometry_base == GeometryOptions::mbb)
+    {
+        if (dim == 2)
+        {
+            for (const auto &cell: dof_handler.active_cell_iterators())
+            {
+                if(cell->is_locally_owned())
+                {
+                    for (unsigned int face_number = 0;
+                         face_number < GeometryInfo<dim>::faces_per_cell;
+                         ++face_number)
+                    {
+                        if (cell->face(face_number)->at_boundary())
+                        {
+                            for (unsigned int vertex_number = 0;
+                                 vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                                 ++vertex_number)
+                            {
+                                const auto vert = cell->vertex(vertex_number);
+                                /*Find bottom left corner*/
+                                if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
+                                            vert(1) - 0) < 1e-12)
+                                {
+
+                                    const unsigned int x_displacement =
+                                            cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
+                                    const unsigned int y_displacement =
+                                            cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
+                                    const unsigned int x_displacement_multiplier =
+                                            cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
+                                    const unsigned int y_displacement_multiplier =
+                                            cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
+                                    /*set bottom left BC*/
+                                    boundary_values[x_displacement] = 0;
+                                    boundary_values[y_displacement] = 0;
+                                    boundary_values[x_displacement_multiplier] = 0;
+                                    boundary_values[y_displacement_multiplier] = 0;
+                                }
+                                /*Find bottom right corner*/
+                                if (std::fabs(vert(0) - 6) < 1e-12 && std::fabs(
+                                            vert(1) - 0) < 1e-12)
+                                {
+                                    //                            const unsigned int x_displacement =
+                                    //                                    cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
+                                    const unsigned int y_displacement =
+                                            cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
+                                    //                            const unsigned int x_displacement_multiplier =
+                                    //                                    cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
+                                    const unsigned int y_displacement_multiplier =
+                                            cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
+                                    //                            boundary_values[x_displacement] = 0;
+                                    boundary_values[y_displacement] = 0;
+                                    //                            boundary_values[x_displacement_multiplier] = 0;
+                                    boundary_values[y_displacement_multiplier] = 0;
+                                }
+                            }
+                        }
+                    }
+                }
+
+            }
+            const unsigned int n_levels = triangulation.n_levels();
+            level_boundary_values.resize(0,n_levels-1);
+            for (unsigned int level = 0; level < n_levels; ++level)
+            {
+                for (auto cell=dof_handler_displacement.begin_active(level); cell!=dof_handler_displacement.end_active(level); ++cell)
+                {
                     if(cell->is_locally_owned())
                     {
                         for (unsigned int face_number = 0;
                              face_number < GeometryInfo<dim>::faces_per_cell;
-                             ++face_number) {
-                            if (cell->face(face_number)->at_boundary()) {
+                             ++face_number)
+                        {
+                            if (cell->face(face_number)->at_boundary())
+                            {
                                 for (unsigned int vertex_number = 0;
                                      vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                                     ++vertex_number) {
+                                     ++vertex_number)
+                                {
                                     const auto vert = cell->vertex(vertex_number);
                                     /*Find bottom left corner*/
                                     if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
-                                            vert(1) - 0) < 1e-12) {
+                                            vert(1) - 0) < 1e-12)
+                                    {
 
                                         const unsigned int x_displacement =
-                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
+                                                cell->vertex_dof_index(vertex_number, 0, 0);
+
                                         const unsigned int y_displacement =
-                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
-                                        const unsigned int x_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
-                                        const unsigned int y_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
+                                                cell->vertex_dof_index(vertex_number, 1, 0);
                                         /*set bottom left BC*/
-                                        boundary_values[x_displacement] = 0;
-                                        boundary_values[y_displacement] = 0;
-                                        boundary_values[x_displacement_multiplier] = 0;
-                                        boundary_values[y_displacement_multiplier] = 0;
+                                        level_boundary_values[level][x_displacement] = 0;
+                                        level_boundary_values[level][y_displacement] = 0;
+
+                                        dirichlet_boundary.insert(x_displacement);
+                                        dirichlet_boundary.insert(y_displacement);
                                     }
                                     /*Find bottom right corner*/
                                     if (std::fabs(vert(0) - 6) < 1e-12 && std::fabs(
-                                            vert(1) - 0) < 1e-12) {
-//                            const unsigned int x_displacement =
-//                                    cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
+                                            vert(1) - 0) < 1e-12)
+                                    {
                                         const unsigned int y_displacement =
-                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
-//                            const unsigned int x_displacement_multiplier =
-//                                    cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
-                                        const unsigned int y_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
-//                            boundary_values[x_displacement] = 0;
-                                        boundary_values[y_displacement] = 0;
-//                            boundary_values[x_displacement_multiplier] = 0;
-                                        boundary_values[y_displacement_multiplier] = 0;
+                                                cell->vertex_dof_index(vertex_number, 1, 0);
+                                        level_boundary_values[level][y_displacement] = 0;
                                     }
                                 }
                             }
                         }
                     }
-
                 }
-            } else if (dim == 3) {
-                for (const auto &cell: dof_handler.active_cell_iterators()) {
-                    if(cell->is_locally_owned())
-                    {
-                        for (unsigned int face_number = 0;
-                             face_number < GeometryInfo<dim>::faces_per_cell;
-                             ++face_number) {
-                            if (cell->face(face_number)->at_boundary()) {
-                                for (unsigned int vertex_number = 0;
-                                     vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                                     ++vertex_number) {
-                                    const auto vert = cell->vertex(vertex_number);
-                                    /*Find bottom left corner*/
-                                    if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
-                                            vert(1) - 0) < 1e-12 && ((std::fabs(
-                                            vert(2) - 0) < 1e-12) || (std::fabs(
-                                            vert(2) - 1) < 1e-12))) {
 
 
-                                        const unsigned int x_displacement =
-                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
-                                        const unsigned int y_displacement =
-                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
-                                        const unsigned int z_displacement =
-                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
-                                        const unsigned int x_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
-                                        const unsigned int y_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 4, cell->active_fe_index());
-                                        const unsigned int z_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 5, cell->active_fe_index());
-
-                                        boundary_values[x_displacement] = 0;
-                                        boundary_values[y_displacement] = 0;
-                                        boundary_values[z_displacement] = 0;
-                                        boundary_values[x_displacement_multiplier] = 0;
-                                        boundary_values[y_displacement_multiplier] = 0;
-                                        boundary_values[z_displacement_multiplier] = 0;
-                                    }
-                                    /*Find bottom right corner*/
-                                    if (std::fabs(vert(0) - 6) < 1e-12 && std::fabs(
+            }
+        } else if (dim == 3)
+        {
+            for (const auto &cell: dof_handler.active_cell_iterators()) {
+                if(cell->is_locally_owned())
+                {
+                    for (unsigned int face_number = 0;
+                         face_number < GeometryInfo<dim>::faces_per_cell;
+                         ++face_number) {
+                        if (cell->face(face_number)->at_boundary()) {
+                            for (unsigned int vertex_number = 0;
+                                 vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                                 ++vertex_number) {
+                                const auto vert = cell->vertex(vertex_number);
+                                /*Find bottom left corner*/
+                                if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
                                             vert(1) - 0) < 1e-12 && ((std::fabs(
-                                            vert(2) - 0) < 1e-12) || (std::fabs(
-                                            vert(2) - 1) < 1e-12))) {
-//                              const unsigned int x_displacement =
-//                                    cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
-                                        const unsigned int y_displacement =
-                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
-                                        const unsigned int z_displacement =
-                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
-//                              const unsigned int x_displacement_multiplier =
-//                                    cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
-                                        const unsigned int y_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 4, cell->active_fe_index());
-                                        const unsigned int z_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 5, cell->active_fe_index());
-//                              boundary_values[x_displacement] = 0;
-                                        boundary_values[y_displacement] = 0;
-                                        boundary_values[z_displacement] = 0;
-//                              boundary_values[x_displacement_multiplier] = 0;
-                                        boundary_values[y_displacement_multiplier] = 0;
-                                        boundary_values[z_displacement_multiplier] = 0;
-                                    }
+                                                                          vert(2) - 0) < 1e-12) || (std::fabs(
+                                                                                                        vert(2) - 1) < 1e-12))) {
+
+
+                                    const unsigned int x_displacement =
+                                            cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
+                                    const unsigned int y_displacement =
+                                            cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
+                                    const unsigned int z_displacement =
+                                            cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
+                                    const unsigned int x_displacement_multiplier =
+                                            cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
+                                    const unsigned int y_displacement_multiplier =
+                                            cell->vertex_dof_index(vertex_number, 4, cell->active_fe_index());
+                                    const unsigned int z_displacement_multiplier =
+                                            cell->vertex_dof_index(vertex_number, 5, cell->active_fe_index());
+
+                                    boundary_values[x_displacement] = 0;
+                                    boundary_values[y_displacement] = 0;
+                                    boundary_values[z_displacement] = 0;
+                                    boundary_values[x_displacement_multiplier] = 0;
+                                    boundary_values[y_displacement_multiplier] = 0;
+                                    boundary_values[z_displacement_multiplier] = 0;
+                                }
+                                /*Find bottom right corner*/
+                                if (std::fabs(vert(0) - 6) < 1e-12 && std::fabs(
+                                            vert(1) - 0) < 1e-12 && ((std::fabs(
+                                                                          vert(2) - 0) < 1e-12) || (std::fabs(
+                                                                                                        vert(2) - 1) < 1e-12))) {
+                                    //                              const unsigned int x_displacement =
+                                    //                                    cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
+                                    const unsigned int y_displacement =
+                                            cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
+                                    const unsigned int z_displacement =
+                                            cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
+                                    //                              const unsigned int x_displacement_multiplier =
+                                    //                                    cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
+                                    const unsigned int y_displacement_multiplier =
+                                            cell->vertex_dof_index(vertex_number, 4, cell->active_fe_index());
+                                    const unsigned int z_displacement_multiplier =
+                                            cell->vertex_dof_index(vertex_number, 5, cell->active_fe_index());
+                                    //                              boundary_values[x_displacement] = 0;
+                                    boundary_values[y_displacement] = 0;
+                                    boundary_values[z_displacement] = 0;
+                                    //                              boundary_values[x_displacement_multiplier] = 0;
+                                    boundary_values[y_displacement_multiplier] = 0;
+                                    boundary_values[z_displacement_multiplier] = 0;
                                 }
                             }
                         }
                     }
                 }
-            } else {
-                throw;
+
             }
-        } else if (Input::geometry_base == GeometryOptions::l_shape) {
-            if (dim == 2) {
-                for (const auto &cell: dof_handler.active_cell_iterators()) {
-                    if(cell->is_locally_owned())
-                    {
-                        for (unsigned int face_number = 0;
-                             face_number < GeometryInfo<dim>::faces_per_cell;
-                             ++face_number) {
-                            if (cell->face(face_number)->at_boundary()) {
-                                for (unsigned int vertex_number = 0;
-                                     vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                                     ++vertex_number) {
-                                    const auto vert = cell->vertex(vertex_number);
-                                    /*Find top left corner*/
-                                    if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
+            //                const unsigned int n_levels = triangulation.n_levels();
+            //                for (unsigned int level = 0; level < n_levels; ++level)
+            //                {
+            //                    for (auto cell=dof_handler_displacement.begin_active(level);
+            //                         cell!=dof_handler.end_active(level);
+            //                         ++cell)
+            //                    {
+            //                        if(cell->is_locally_owned())
+            //                        {
+            //                            for (unsigned int face_number = 0;
+            //                                 face_number < GeometryInfo<dim>::faces_per_cell;
+            //                                 ++face_number)
+            //                            {
+            //                                if (cell->face(face_number)->at_boundary())
+            //                                {
+            //                                    for (unsigned int vertex_number = 0;
+            //                                         vertex_number < GeometryInfo<dim>::vertices_per_cell;
+            //                                         ++vertex_number)
+            //                                    {
+            //                                        const auto vert = cell->vertex(vertex_number);
+            //                                        /*Find bottom left corner*/
+            //                                        if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
+            //                                                    vert(1) - 0) < 1e-12 && ((std::fabs(
+            //                                                    vert(2) - 0) < 1e-12) || (std::fabs(
+            //                                                    vert(2) - 1) < 1e-12)))
+            //                                        {
+
+            //                                            const unsigned int x_displacement =
+            //                                                    cell->vertex_dof_index(vertex_number, 0, 0);
+            //                                            const unsigned int y_displacement =
+            //                                                    cell->vertex_dof_index(vertex_number, 1, 0);
+            //                                            const unsigned int z_displacement =
+            //                                                    cell->vertex_dof_index(vertex_number, 2, 0);
+            //                                            /*set bottom left BC*/
+            //                                            level_boundary_values[level][x_displacement] = 0;
+            //                                            level_boundary_values[level][y_displacement] = 0;
+            //                                            level_boundary_values[level][z_displacement] = 0;
+            //                                        }
+            //                                        /*Find bottom right corner*/
+            //                                        if (std::fabs(vert(0) - 6) < 1e-12 && std::fabs(
+            //                                                    vert(1) - 0) < 1e-12 && ((std::fabs(
+            //                                                    vert(2) - 0) < 1e-12) || (std::fabs(
+            //                                                    vert(2) - 1) < 1e-12)))
+            //                                        {
+            //                                            const unsigned int y_displacement =
+            //                                                    cell->vertex_dof_index(vertex_number, 1, 0);
+            //                                            const unsigned int z_displacement =
+            //                                                    cell->vertex_dof_index(vertex_number, 2, 0);
+            //                                            level_boundary_values[level][y_displacement] = 0;
+            //                                            level_boundary_values[level][z_displacement] = 0;
+            //                                        }
+            //                                    }
+            //                                }
+            //                            }
+            //                        }
+            //                    }
+            //                }
+
+
+        }
+        else
+        {
+            throw;
+        }
+    } else if (Input::geometry_base == GeometryOptions::l_shape) {
+        if (dim == 2)
+        {
+            for (const auto &cell: dof_handler.active_cell_iterators())
+            {
+                if(cell->is_locally_owned())
+                {
+                    for (unsigned int face_number = 0;
+                         face_number < GeometryInfo<dim>::faces_per_cell;
+                         ++face_number) {
+                        if (cell->face(face_number)->at_boundary()) {
+                            for (unsigned int vertex_number = 0;
+                                 vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                                 ++vertex_number) {
+                                const auto vert = cell->vertex(vertex_number);
+                                /*Find top left corner*/
+                                if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
                                             vert(1) - 2) < 1e-12) {
 
-                                        const unsigned int x_displacement =
-                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
-                                        const unsigned int y_displacement =
-                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
-                                        const unsigned int x_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
-                                        const unsigned int y_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
-                                        /*set bottom left BC*/
-                                        boundary_values[x_displacement] = 0;
-                                        boundary_values[y_displacement] = 0;
-                                        boundary_values[x_displacement_multiplier] = 0;
-                                        boundary_values[y_displacement_multiplier] = 0;
-                                    }
-                                    /*Find top right corner*/
-                                    if (std::fabs(vert(0) - 1) < 1e-12 && std::fabs(
+                                    const unsigned int x_displacement =
+                                            cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
+                                    const unsigned int y_displacement =
+                                            cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
+                                    const unsigned int x_displacement_multiplier =
+                                            cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
+                                    const unsigned int y_displacement_multiplier =
+                                            cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
+                                    /*set bottom left BC*/
+                                    boundary_values[x_displacement] = 0;
+                                    boundary_values[y_displacement] = 0;
+                                    boundary_values[x_displacement_multiplier] = 0;
+                                    boundary_values[y_displacement_multiplier] = 0;
+                                }
+                                /*Find top right corner*/
+                                if (std::fabs(vert(0) - 1) < 1e-12 && std::fabs(
                                             vert(1) - 2) < 1e-12) {
-                                        const unsigned int x_displacement =
-                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
-                                        const unsigned int y_displacement =
-                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
-                                        const unsigned int x_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
-                                        const unsigned int y_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
-                                        boundary_values[x_displacement] = 0;
-                                        boundary_values[y_displacement] = 0;
-                                        boundary_values[x_displacement_multiplier] = 0;
-                                        boundary_values[y_displacement_multiplier] = 0;
-                                    }
+                                    const unsigned int x_displacement =
+                                            cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
+                                    const unsigned int y_displacement =
+                                            cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
+                                    const unsigned int x_displacement_multiplier =
+                                            cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
+                                    const unsigned int y_displacement_multiplier =
+                                            cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
+                                    boundary_values[x_displacement] = 0;
+                                    boundary_values[y_displacement] = 0;
+                                    boundary_values[x_displacement_multiplier] = 0;
+                                    boundary_values[y_displacement_multiplier] = 0;
                                 }
                             }
                         }
                     }
                 }
-            } else if (dim == 3) {
-                for (const auto &cell: dof_handler.active_cell_iterators()) {
-                    if(cell->is_locally_owned())
-                    {
-                        for (unsigned int face_number = 0;
-                             face_number < GeometryInfo<dim>::faces_per_cell;
-                             ++face_number) {
-                            if (cell->face(face_number)->at_boundary()) {
-                                for (unsigned int vertex_number = 0;
-                                     vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                                     ++vertex_number) {
-                                    const auto vert = cell->vertex(vertex_number);
-                                    /*Find bottom left corner*/
-                                    if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
+            }
+            //                const unsigned int n_levels = triangulation.n_levels();
+            //                for (unsigned int level = 0; level < n_levels; ++level)
+            //                {
+            //                    for (auto cell=dof_handler_displacement.begin_active(level);
+            //                         cell!=dof_handler.end_active(level);
+            //                         ++cell)
+            //                    {
+            //                        if(cell->is_locally_owned())
+            //                        {
+            //                            for (unsigned int face_number = 0;
+            //                                 face_number < GeometryInfo<dim>::faces_per_cell;
+            //                                 ++face_number)
+            //                            {
+            //                                if (cell->face(face_number)->at_boundary())
+            //                                {
+            //                                    for (unsigned int vertex_number = 0;
+            //                                         vertex_number < GeometryInfo<dim>::vertices_per_cell;
+            //                                         ++vertex_number)
+            //                                    {
+            //                                        const auto vert = cell->vertex(vertex_number);
+            //                                        /*Find bottom left corner*/
+            //                                        if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
+            //                                                    vert(1) - 2) < 1e-12)
+            //                                        {
+
+            //                                            const unsigned int x_displacement =
+            //                                                    cell->vertex_dof_index(vertex_number, 0, 0);
+            //                                            const unsigned int y_displacement =
+            //                                                    cell->vertex_dof_index(vertex_number, 1, 0);
+            //                                            /*set bottom left BC*/
+            //                                            level_boundary_values[level][x_displacement] = 0;
+            //                                            level_boundary_values[level][y_displacement] = 0;
+            //                                        }
+            //                                        /*Find bottom right corner*/
+            //                                        if (std::fabs(vert(0) - 1) < 1e-12 && std::fabs(
+            //                                                    vert(1) - 2) < 1e-12)
+            //                                        {
+            //                                            const unsigned int x_displacement =
+            //                                                    cell->vertex_dof_index(vertex_number, 0, 0);
+            //                                            const unsigned int y_displacement =
+            //                                                    cell->vertex_dof_index(vertex_number, 1, 0);
+            //                                            level_boundary_values[level][x_displacement] = 0;
+            //                                            level_boundary_values[level][y_displacement] = 0;
+            //                                        }
+            //                                    }
+            //                                }
+            //                            }
+            //                        }
+            //                    }
+            //                }
+        }
+        else if (dim == 3)
+        {
+            for (const auto &cell: dof_handler.active_cell_iterators()) {
+                if(cell->is_locally_owned())
+                {
+                    for (unsigned int face_number = 0;
+                         face_number < GeometryInfo<dim>::faces_per_cell;
+                         ++face_number) {
+                        if (cell->face(face_number)->at_boundary()) {
+                            for (unsigned int vertex_number = 0;
+                                 vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                                 ++vertex_number) {
+                                const auto vert = cell->vertex(vertex_number);
+                                /*Find bottom left corner*/
+                                if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
                                             vert(1) - 2) < 1e-12 && ((std::fabs(
-                                            vert(2) - 0) < 1e-12) || (std::fabs(
-                                            vert(2) - 1) < 1e-12))) {
-
-
-                                        const unsigned int x_displacement =
-                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
-                                        const unsigned int y_displacement =
-                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
-                                        const unsigned int z_displacement =
-                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
-                                        const unsigned int x_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
-                                        const unsigned int y_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 4, cell->active_fe_index());
-                                        const unsigned int z_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 5, cell->active_fe_index());
-
-                                        boundary_values[x_displacement] = 0;
-                                        boundary_values[y_displacement] = 0;
-                                        boundary_values[z_displacement] = 0;
-                                        boundary_values[x_displacement_multiplier] = 0;
-                                        boundary_values[y_displacement_multiplier] = 0;
-                                        boundary_values[z_displacement_multiplier] = 0;
-                                    }
-                                    /*Find bottom right corner*/
-                                    if (std::fabs(vert(0) - 1) < 1e-12 && std::fabs(
+                                                                          vert(2) - 0) < 1e-12) || (std::fabs(
+                                                                                                        vert(2) - 1) < 1e-12))) {
+
+
+                                    const unsigned int x_displacement =
+                                            cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
+                                    const unsigned int y_displacement =
+                                            cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
+                                    const unsigned int z_displacement =
+                                            cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
+                                    const unsigned int x_displacement_multiplier =
+                                            cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
+                                    const unsigned int y_displacement_multiplier =
+                                            cell->vertex_dof_index(vertex_number, 4, cell->active_fe_index());
+                                    const unsigned int z_displacement_multiplier =
+                                            cell->vertex_dof_index(vertex_number, 5, cell->active_fe_index());
+
+                                    boundary_values[x_displacement] = 0;
+                                    boundary_values[y_displacement] = 0;
+                                    boundary_values[z_displacement] = 0;
+                                    boundary_values[x_displacement_multiplier] = 0;
+                                    boundary_values[y_displacement_multiplier] = 0;
+                                    boundary_values[z_displacement_multiplier] = 0;
+                                }
+                                /*Find bottom right corner*/
+                                if (std::fabs(vert(0) - 1) < 1e-12 && std::fabs(
                                             vert(1) - 2) < 1e-12 && ((std::fabs(
-                                            vert(2) - 0) < 1e-12) || (std::fabs(
-                                            vert(2) - 1) < 1e-12))) {
-                                        const unsigned int x_displacement =
-                                                cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
-                                        const unsigned int y_displacement =
-                                                cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
-                                        const unsigned int z_displacement =
-                                                cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
-                                        const unsigned int x_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
-                                        const unsigned int y_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 4, cell->active_fe_index());
-                                        const unsigned int z_displacement_multiplier =
-                                                cell->vertex_dof_index(vertex_number, 5, cell->active_fe_index());
-                                        boundary_values[x_displacement] = 0;
-                                        boundary_values[y_displacement] = 0;
-                                        boundary_values[z_displacement] = 0;
-                                        boundary_values[x_displacement_multiplier] = 0;
-                                        boundary_values[y_displacement_multiplier] = 0;
-                                        boundary_values[z_displacement_multiplier] = 0;
-                                    }
+                                                                          vert(2) - 0) < 1e-12) || (std::fabs(
+                                                                                                        vert(2) - 1) < 1e-12))) {
+                                    const unsigned int x_displacement =
+                                            cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
+                                    const unsigned int y_displacement =
+                                            cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
+                                    const unsigned int z_displacement =
+                                            cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
+                                    const unsigned int x_displacement_multiplier =
+                                            cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
+                                    const unsigned int y_displacement_multiplier =
+                                            cell->vertex_dof_index(vertex_number, 4, cell->active_fe_index());
+                                    const unsigned int z_displacement_multiplier =
+                                            cell->vertex_dof_index(vertex_number, 5, cell->active_fe_index());
+                                    boundary_values[x_displacement] = 0;
+                                    boundary_values[y_displacement] = 0;
+                                    boundary_values[z_displacement] = 0;
+                                    boundary_values[x_displacement_multiplier] = 0;
+                                    boundary_values[y_displacement_multiplier] = 0;
+                                    boundary_values[z_displacement_multiplier] = 0;
                                 }
                             }
                         }
                     }
                 }
-            } else {
-                throw;
             }
+            //                const unsigned int n_levels = triangulation.n_levels();
+            //                for (unsigned int level = 0; level < n_levels; ++level)
+            //                {
+            //                    for (auto cell=dof_handler_displacement.begin_active(level);
+            //                         cell!=dof_handler.end_active(level);
+            //                         ++cell)
+            //                    {
+            //                        if(cell->is_locally_owned())
+            //                        {
+            //                            for (unsigned int face_number = 0;
+            //                                 face_number < GeometryInfo<dim>::faces_per_cell;
+            //                                 ++face_number)
+            //                            {
+            //                                if (cell->face(face_number)->at_boundary())
+            //                                {
+            //                                    for (unsigned int vertex_number = 0;
+            //                                         vertex_number < GeometryInfo<dim>::vertices_per_cell;
+            //                                         ++vertex_number)
+            //                                    {
+            //                                        const auto vert = cell->vertex(vertex_number);
+            //                                        /*Find bottom left corner*/
+            //                                        if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
+            //                                                    vert(1) - 2) < 1e-12 && ((std::fabs(
+            //                                                    vert(2) - 0) < 1e-12) || (std::fabs(
+            //                                                    vert(2) - 1) < 1e-12)))
+            //                                        {
+
+            //                                            const unsigned int x_displacement =
+            //                                                    cell->vertex_dof_index(vertex_number, 0, 0);
+            //                                            const unsigned int y_displacement =
+            //                                                    cell->vertex_dof_index(vertex_number, 1, 0);
+            //                                            const unsigned int z_displacement =
+            //                                                    cell->vertex_dof_index(vertex_number, 2, 0);
+            //                                            /*set bottom left BC*/
+            //                                            level_boundary_values[level][x_displacement] = 0;
+            //                                            level_boundary_values[level][y_displacement] = 0;
+            //                                            level_boundary_values[level][z_displacement] = 0;
+            //                                        }
+            //                                        /*Find bottom right corner*/
+            //                                        if (std::fabs(vert(0) - 1) < 1e-12 && std::fabs(
+            //                                                    vert(1) - 2) < 1e-12 && ((std::fabs(
+            //                                                    vert(2) - 0) < 1e-12) || (std::fabs(
+            //                                                    vert(2) - 1) < 1e-12)))
+            //                                        {
+            //                                            const unsigned int x_displacement =
+            //                                                    cell->vertex_dof_index(vertex_number, 0, 0);
+            //                                            const unsigned int y_displacement =
+            //                                                    cell->vertex_dof_index(vertex_number, 1, 0);
+            //                                            const unsigned int z_displacement =
+            //                                                    cell->vertex_dof_index(vertex_number, 2, 0);
+            //                                            level_boundary_values[level][x_displacement] = 0;
+            //                                            level_boundary_values[level][y_displacement] = 0;
+            //                                            level_boundary_values[level][z_displacement] = 0;
+            //                                        }
+            //                                    }
+            //                                }
+            //                            }
+            //                        }
+            //                    }
+            //                }
+        } else {
+            throw;
+        }
+    }
+
+
+}
+
+
+//This makes a giant 10-by-10 block matrix, and also sets up the necessary block vectors.  The
+// sparsity pattern for this matrix includes the sparsity pattern for the filter matrix. It also initializes
+// any block vectors we will use.
+template<int dim>
+void
+KktSystem<dim>::setup_block_system() {
+
+    //MAKE n_u and n_P
+
+    /*Setup 10 by 10 block matrix*/
+    std::vector<unsigned int> block_component(10, 2);
+
+    block_component[0] = 0;
+    block_component[5] = 1;
+
+    const std::vector<types::global_dof_index> dofs_per_block =
+            DoFTools::count_dofs_per_fe_block(dof_handler, block_component);
+    const unsigned int n_p = dofs_per_block[0];
+    const unsigned int n_u = dofs_per_block[1];
+
+    pcout << "n_p:  " << n_p << "   n_u:  " << n_u << std::endl;
+
+    IndexSet locally_owned_dofs = dof_handler.locally_owned_dofs();
+    IndexSet locally_relevant_dofs;
+    DoFTools::extract_locally_relevant_dofs(dof_handler, locally_relevant_dofs);
+
+    dsp.reinit(10, 10);
+    owned_partitioning.resize(10);
+    owned_partitioning[0] = dof_handler.locally_owned_dofs().get_view(0, n_p);
+    owned_partitioning[1] = dof_handler.locally_owned_dofs().get_view(n_p, 2 * n_p);
+    owned_partitioning[2] = dof_handler.locally_owned_dofs().get_view(2 * n_p, 3 * n_p);
+    owned_partitioning[3] = dof_handler.locally_owned_dofs().get_view(3 * n_p, 4 * n_p);
+    owned_partitioning[4] = dof_handler.locally_owned_dofs().get_view(4 * n_p, 5 * n_p);
+    owned_partitioning[5] = dof_handler.locally_owned_dofs().get_view(5 * n_p, 5 * n_p + n_u);
+    owned_partitioning[6] = dof_handler.locally_owned_dofs().get_view(5 * n_p + n_u, 5 * n_p + 2 * n_u);
+    owned_partitioning[7] = dof_handler.locally_owned_dofs().get_view(5 * n_p + 2 * n_u, 6 * n_p + 2 * n_u);
+    owned_partitioning[8] = dof_handler.locally_owned_dofs().get_view(6 * n_p + 2 * n_u, 7 * n_p + 2 * n_u);
+    owned_partitioning[9] = dof_handler.locally_owned_dofs().get_view(7 * n_p + 2 * n_u, 7 * n_p + 2 * n_u + 1);
+    relevant_partitioning.resize(10);
+    relevant_partitioning[0] = locally_relevant_dofs.get_view(0, n_p);
+    relevant_partitioning[1] = locally_relevant_dofs.get_view(n_p, 2 * n_p);
+    relevant_partitioning[2] = locally_relevant_dofs.get_view(2 * n_p, 3 * n_p);
+    relevant_partitioning[3] = locally_relevant_dofs.get_view(3 * n_p, 4 * n_p);
+    relevant_partitioning[4] = locally_relevant_dofs.get_view(4 * n_p, 5 * n_p);
+    relevant_partitioning[5] = locally_relevant_dofs.get_view(5 * n_p, 5 * n_p + n_u);
+    relevant_partitioning[6] = locally_relevant_dofs.get_view(5 * n_p + n_u, 5 * n_p + 2 * n_u);
+    relevant_partitioning[7] = locally_relevant_dofs.get_view(5 * n_p + 2 * n_u, 6 * n_p + 2 * n_u);
+    relevant_partitioning[8] = locally_relevant_dofs.get_view(6 * n_p + 2 * n_u, 7 * n_p + 2 * n_u);
+    relevant_partitioning[9] = locally_relevant_dofs.get_view(7 * n_p + 2 * n_u, 7 * n_p + 2 * n_u + 1);
+
+    const std::vector<unsigned int> block_sizes = {n_p, n_p, n_p, n_p, n_p, n_u, n_u, n_p, n_p, 1};
+
+    for (unsigned int k = 0; k < 10; k++) {
+        for (unsigned int j = 0; j < 10; j++) {
+            dsp.block(j, k).reinit(block_sizes[j], block_sizes[k]);
         }
+    }
+    dsp.collect_sizes();
+    Table<2, DoFTools::Coupling> coupling(2 * dim + 8, 2 * dim + 8);
+    //Coupling for density
+    coupling[SolutionComponents::density<dim>][SolutionComponents::density<dim>] = DoFTools::always;
+
+    for (unsigned int i = 0; i < dim; i++) {
+        coupling[SolutionComponents::density<dim>][SolutionComponents::displacement<dim> +
+                i] = DoFTools::always;
+        coupling[SolutionComponents::displacement<dim> +
+                i][SolutionComponents::density<dim>] = DoFTools::always;
+    }
 
+    coupling[SolutionComponents::density<dim>][SolutionComponents::unfiltered_density_multiplier<dim>] = DoFTools::always;
+    coupling[SolutionComponents::unfiltered_density_multiplier<dim>][SolutionComponents::density<dim>] = DoFTools::always;
 
+    for (unsigned int i = 0; i < dim; i++) {
+        coupling[SolutionComponents::density<dim>][SolutionComponents::displacement_multiplier<dim> +
+                i] = DoFTools::always;
+        coupling[SolutionComponents::displacement_multiplier<dim> +
+                i][SolutionComponents::density<dim>] = DoFTools::always;
     }
 
+    //Coupling for displacement
+    for (unsigned int i = 0; i < dim; i++) {
 
-    //This makes a giant 10-by-10 block matrix, and also sets up the necessary block vectors.  The
-    // sparsity pattern for this matrix includes the sparsity pattern for the filter matrix. It also initializes
-    // any block vectors we will use.
-    template<int dim>
-    void
-    KktSystem<dim>::setup_block_system() {
-
-        //MAKE n_u and n_P
-
-        /*Setup 10 by 10 block matrix*/
-        std::vector<unsigned int> block_component(10, 2);
-
-        block_component[0] = 0;
-        block_component[5] = 1;
-
-        const std::vector<types::global_dof_index> dofs_per_block =
-                DoFTools::count_dofs_per_fe_block(dof_handler, block_component);
-        const unsigned int n_p = dofs_per_block[0];
-        const unsigned int n_u = dofs_per_block[1];
-
-        pcout << "n_p:  " << n_p << "   n_u:  " << n_u << std::endl;
-
-        IndexSet locally_owned_dofs = dof_handler.locally_owned_dofs();
-        IndexSet locally_relevant_dofs;
-        DoFTools::extract_locally_relevant_dofs(dof_handler, locally_relevant_dofs);
-
-        dsp.reinit(10, 10);
-        owned_partitioning.resize(10);
-        owned_partitioning[0] = dof_handler.locally_owned_dofs().get_view(0, n_p);
-        owned_partitioning[1] = dof_handler.locally_owned_dofs().get_view(n_p, 2 * n_p);
-        owned_partitioning[2] = dof_handler.locally_owned_dofs().get_view(2 * n_p, 3 * n_p);
-        owned_partitioning[3] = dof_handler.locally_owned_dofs().get_view(3 * n_p, 4 * n_p);
-        owned_partitioning[4] = dof_handler.locally_owned_dofs().get_view(4 * n_p, 5 * n_p);
-        owned_partitioning[5] = dof_handler.locally_owned_dofs().get_view(5 * n_p, 5 * n_p + n_u);
-        owned_partitioning[6] = dof_handler.locally_owned_dofs().get_view(5 * n_p + n_u, 5 * n_p + 2 * n_u);
-        owned_partitioning[7] = dof_handler.locally_owned_dofs().get_view(5 * n_p + 2 * n_u, 6 * n_p + 2 * n_u);
-        owned_partitioning[8] = dof_handler.locally_owned_dofs().get_view(6 * n_p + 2 * n_u, 7 * n_p + 2 * n_u);
-        owned_partitioning[9] = dof_handler.locally_owned_dofs().get_view(7 * n_p + 2 * n_u, 7 * n_p + 2 * n_u + 1);
-        relevant_partitioning.resize(10);
-        relevant_partitioning[0] = locally_relevant_dofs.get_view(0, n_p);
-        relevant_partitioning[1] = locally_relevant_dofs.get_view(n_p, 2 * n_p);
-        relevant_partitioning[2] = locally_relevant_dofs.get_view(2 * n_p, 3 * n_p);
-        relevant_partitioning[3] = locally_relevant_dofs.get_view(3 * n_p, 4 * n_p);
-        relevant_partitioning[4] = locally_relevant_dofs.get_view(4 * n_p, 5 * n_p);
-        relevant_partitioning[5] = locally_relevant_dofs.get_view(5 * n_p, 5 * n_p + n_u);
-        relevant_partitioning[6] = locally_relevant_dofs.get_view(5 * n_p + n_u, 5 * n_p + 2 * n_u);
-        relevant_partitioning[7] = locally_relevant_dofs.get_view(5 * n_p + 2 * n_u, 6 * n_p + 2 * n_u);
-        relevant_partitioning[8] = locally_relevant_dofs.get_view(6 * n_p + 2 * n_u, 7 * n_p + 2 * n_u);
-        relevant_partitioning[9] = locally_relevant_dofs.get_view(7 * n_p + 2 * n_u, 7 * n_p + 2 * n_u + 1);
-
-        const std::vector<unsigned int> block_sizes = {n_p, n_p, n_p, n_p, n_p, n_u, n_u, n_p, n_p, 1};
-
-        for (unsigned int k = 0; k < 10; k++) {
-            for (unsigned int j = 0; j < 10; j++) {
-                dsp.block(j, k).reinit(block_sizes[j], block_sizes[k]);
-            }
+        for (unsigned int k = 0; k < dim; k++) {
+            coupling[SolutionComponents::displacement<dim> + i][
+                    SolutionComponents::displacement_multiplier<dim> +
+                    k] = DoFTools::always;
+            coupling[SolutionComponents::displacement_multiplier<dim> + k][
+                    SolutionComponents::displacement<dim> +
+                    i] = DoFTools::always;
         }
-        dsp.collect_sizes();
-        Table<2, DoFTools::Coupling> coupling(2 * dim + 8, 2 * dim + 8);
-//Coupling for density
-            coupling[SolutionComponents::density<dim>][SolutionComponents::density<dim>] = DoFTools::always;
-
-            for (unsigned int i = 0; i < dim; i++) {
-                coupling[SolutionComponents::density<dim>][SolutionComponents::displacement<dim> +
-                                                           i] = DoFTools::always;
-                coupling[SolutionComponents::displacement<dim> +
-                         i][SolutionComponents::density<dim>] = DoFTools::always;
-            }
+    }
 
-            coupling[SolutionComponents::density<dim>][SolutionComponents::unfiltered_density_multiplier<dim>] = DoFTools::always;
-            coupling[SolutionComponents::unfiltered_density_multiplier<dim>][SolutionComponents::density<dim>] = DoFTools::always;
+    // coupling for unfiltered density
+    coupling[SolutionComponents::unfiltered_density<dim>][SolutionComponents::density_lower_slack_multiplier<dim>] = DoFTools::always;
+    coupling[SolutionComponents::density_lower_slack_multiplier<dim>][SolutionComponents::unfiltered_density<dim>] = DoFTools::always;
 
-            for (unsigned int i = 0; i < dim; i++) {
-                coupling[SolutionComponents::density<dim>][SolutionComponents::displacement_multiplier<dim> +
-                                                           i] = DoFTools::always;
-                coupling[SolutionComponents::displacement_multiplier<dim> +
-                         i][SolutionComponents::density<dim>] = DoFTools::always;
-            }
+    coupling[SolutionComponents::unfiltered_density<dim>][SolutionComponents::density_upper_slack_multiplier<dim>] = DoFTools::always;
+    coupling[SolutionComponents::density_upper_slack_multiplier<dim>][SolutionComponents::unfiltered_density<dim>] = DoFTools::always;
 
-//Coupling for displacement
-            for (unsigned int i = 0; i < dim; i++) {
+    coupling[SolutionComponents::unfiltered_density<dim>][SolutionComponents::unfiltered_density_multiplier<dim>] = DoFTools::always;
+    coupling[SolutionComponents::unfiltered_density_multiplier<dim>][SolutionComponents::unfiltered_density<dim>] = DoFTools::always;
 
-                for (unsigned int k = 0; k < dim; k++) {
-                    coupling[SolutionComponents::displacement<dim> + i][
-                            SolutionComponents::displacement_multiplier<dim> +
-                            k] = DoFTools::always;
-                    coupling[SolutionComponents::displacement_multiplier<dim> + k][
-                            SolutionComponents::displacement<dim> +
-                            i] = DoFTools::always;
-                }
-            }
 
-// coupling for unfiltered density
-            coupling[SolutionComponents::unfiltered_density<dim>][SolutionComponents::density_lower_slack_multiplier<dim>] = DoFTools::always;
-            coupling[SolutionComponents::density_lower_slack_multiplier<dim>][SolutionComponents::unfiltered_density<dim>] = DoFTools::always;
 
-            coupling[SolutionComponents::unfiltered_density<dim>][SolutionComponents::density_upper_slack_multiplier<dim>] = DoFTools::always;
-            coupling[SolutionComponents::density_upper_slack_multiplier<dim>][SolutionComponents::unfiltered_density<dim>] = DoFTools::always;
 
-            coupling[SolutionComponents::unfiltered_density<dim>][SolutionComponents::unfiltered_density_multiplier<dim>] = DoFTools::always;
-            coupling[SolutionComponents::unfiltered_density_multiplier<dim>][SolutionComponents::unfiltered_density<dim>] = DoFTools::always;
+    //        Coupling for lower slack
+    coupling[SolutionComponents::density_lower_slack<dim>][SolutionComponents::density_lower_slack<dim>] = DoFTools::always;
 
+    coupling[SolutionComponents::density_lower_slack<dim>][SolutionComponents::density_lower_slack_multiplier<dim>] = DoFTools::always;
+    coupling[SolutionComponents::density_lower_slack_multiplier<dim>][SolutionComponents::density_lower_slack<dim>] = DoFTools::always;
 
+    //
+    coupling[SolutionComponents::density_upper_slack<dim>][SolutionComponents::density_upper_slack<dim>] = DoFTools::always;
+    coupling[SolutionComponents::density_upper_slack<dim>][SolutionComponents::density_upper_slack_multiplier<dim>] = DoFTools::always;
+    coupling[SolutionComponents::density_upper_slack_multiplier<dim>][SolutionComponents::density_upper_slack<dim>] = DoFTools::always;
 
+    coupling[SolutionComponents::density_upper_slack_multiplier<dim>][SolutionComponents::density_upper_slack_multiplier<dim>] = DoFTools::always;
+    constraints.reinit(locally_relevant_dofs);
+    constraints.clear();
+    DoFTools::make_hanging_node_constraints(dof_handler,constraints);
+    constraints.close();
 
-//        Coupling for lower slack
-            coupling[SolutionComponents::density_lower_slack<dim>][SolutionComponents::density_lower_slack<dim>] = DoFTools::always;
+    system_matrix.clear();
 
-            coupling[SolutionComponents::density_lower_slack<dim>][SolutionComponents::density_lower_slack_multiplier<dim>] = DoFTools::always;
-            coupling[SolutionComponents::density_lower_slack_multiplier<dim>][SolutionComponents::density_lower_slack<dim>] = DoFTools::always;
+    //            DoFTools::make_sparsity_pattern(dof_handler, coupling, dsp, constraints, false);
+    DoFTools::make_sparsity_pattern(dof_handler, dsp, constraints, false);
+    SparsityTools::distribute_sparsity_pattern(dsp, dof_handler.locally_owned_dofs(), mpi_communicator,
+                                               locally_relevant_dofs);
+    //adds the row into the sparsity pattern for the total volume constraint
+    for (const auto &cell: dof_handler.active_cell_iterators())
+    {
+        if (cell->is_locally_owned())
+        {
+            std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
+            cell->get_dof_indices(i);
+            dsp.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier).add(i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)], 0);
+            dsp.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).add(0, i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)]);
+        }
 
-//
-            coupling[SolutionComponents::density_upper_slack<dim>][SolutionComponents::density_upper_slack<dim>] = DoFTools::always;
-            coupling[SolutionComponents::density_upper_slack<dim>][SolutionComponents::density_upper_slack_multiplier<dim>] = DoFTools::always;
-            coupling[SolutionComponents::density_upper_slack_multiplier<dim>][SolutionComponents::density_upper_slack<dim>] = DoFTools::always;
-
-            coupling[SolutionComponents::density_upper_slack_multiplier<dim>][SolutionComponents::density_upper_slack_multiplier<dim>] = DoFTools::always;
-            constraints.reinit(locally_relevant_dofs);
-            constraints.clear();
-            DoFTools::make_hanging_node_constraints(dof_handler,constraints);
-            constraints.close();
-
-            system_matrix.clear();
-
-//            DoFTools::make_sparsity_pattern(dof_handler, coupling, dsp, constraints, false);
-            DoFTools::make_sparsity_pattern(dof_handler, dsp, constraints, false);
-            SparsityTools::distribute_sparsity_pattern(dsp, dof_handler.locally_owned_dofs(), mpi_communicator,
-                                                       locally_relevant_dofs);
-            //adds the row into the sparsity pattern for the total volume constraint
-            for (const auto &cell: dof_handler.active_cell_iterators())
-            {
-                if (cell->is_locally_owned())
-                {
-                    std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
-                    cell->get_dof_indices(i);
-                    dsp.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier).add(i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)], 0);
-                    dsp.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).add(0, i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)]);
-                }
+    }
 
+    /*This finds neighbors whose values would be relevant, and adds them to the sparsity pattern of the matrix*/
+    setup_filter_matrix();
+    for (const auto &cell : dof_handler.active_cell_iterators()) {
+        if (cell->is_locally_owned())
+        {
+            std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
+            cell->get_dof_indices(i);
+            const unsigned int cell_index = i[cell->get_fe().component_to_system_index(0, 0)];
+            for (const auto &neighbor_cell_index : density_filter.find_relevant_neighbors(cell_index))
+            {
+                dsp.block(SolutionBlocks::unfiltered_density_multiplier,
+                          SolutionBlocks::unfiltered_density).add(cell_index, neighbor_cell_index);
+                dsp.block(SolutionBlocks::unfiltered_density,
+                          SolutionBlocks::unfiltered_density_multiplier).add(neighbor_cell_index, cell_index);
             }
+        }
+    }
 
-            /*This finds neighbors whose values would be relevant, and adds them to the sparsity pattern of the matrix*/
-            setup_filter_matrix();
-            for (const auto &cell : dof_handler.active_cell_iterators()) {
-                if (cell->is_locally_owned())
-                {
-                    std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
-                    cell->get_dof_indices(i);
-                    const unsigned int cell_index = i[cell->get_fe().component_to_system_index(0, 0)];
-                    for (const auto &neighbor_cell_index : density_filter.find_relevant_neighbors(cell_index))
-                    {
-                        dsp.block(SolutionBlocks::unfiltered_density_multiplier,
-                                  SolutionBlocks::unfiltered_density).add(cell_index, neighbor_cell_index);
-                        dsp.block(SolutionBlocks::unfiltered_density,
-                                  SolutionBlocks::unfiltered_density_multiplier).add(neighbor_cell_index, cell_index);
-                    }
-                }
-            }
 
+    SparsityTools::distribute_sparsity_pattern(
+                dsp,
+                Utilities::MPI::all_gather(mpi_communicator,
+                                           dof_handler.locally_owned_dofs()),
+                mpi_communicator,
+                locally_relevant_dofs);
+    DoFTools::extract_locally_relevant_dofs(dof_handler, locally_relevant_dofs);
+    system_matrix.reinit(owned_partitioning, dsp, mpi_communicator);
 
-            SparsityTools::distribute_sparsity_pattern(
-                    dsp,
-                    Utilities::MPI::all_gather(mpi_communicator,
-                                               dof_handler.locally_owned_dofs()),
-                    mpi_communicator,
-                    locally_relevant_dofs);
-            DoFTools::extract_locally_relevant_dofs(dof_handler, locally_relevant_dofs);
-            system_matrix.reinit(owned_partitioning, dsp, mpi_communicator);
+    locally_relevant_solution.reinit(owned_partitioning, relevant_partitioning, mpi_communicator);
+    distributed_solution.reinit(owned_partitioning, mpi_communicator);
+    system_rhs.reinit(owned_partitioning, mpi_communicator);
 
-            locally_relevant_solution.reinit(owned_partitioning, relevant_partitioning, mpi_communicator);
-            distributed_solution.reinit(owned_partitioning, mpi_communicator);
-            system_rhs.reinit(owned_partitioning, mpi_communicator);
+    locally_relevant_solution.collect_sizes();
+    distributed_solution.collect_sizes();
+    system_rhs.collect_sizes();
+    system_matrix.collect_sizes();
+}
 
-            locally_relevant_solution.collect_sizes();
-            distributed_solution.collect_sizes();
-            system_rhs.collect_sizes();
-            system_matrix.collect_sizes();
-    }
+///This  is  where  the  magic  happens.   The  equations  describing  the newtons method for finding 0s in the KKT conditions are implemented here.
 
-    ///This  is  where  the  magic  happens.   The  equations  describing  the newtons method for finding 0s in the KKT conditions are implemented here.
 
+template<int dim>
+void
+KktSystem<dim>::assemble_block_system(const LA::MPI::BlockVector &distributed_state, const double barrier_size) {
+    /*Remove any values from old iterations*/
 
-    template<int dim>
-    void
-    KktSystem<dim>::assemble_block_system(const LA::MPI::BlockVector &distributed_state, const double barrier_size) {
-        /*Remove any values from old iterations*/
+    LA::MPI::BlockVector relevant_state(owned_partitioning, relevant_partitioning, mpi_communicator);
+    relevant_state = distributed_state;
 
-        LA::MPI::BlockVector relevant_state(owned_partitioning, relevant_partitioning, mpi_communicator);
-        relevant_state = distributed_state;
+    system_matrix.reinit(owned_partitioning, dsp, mpi_communicator);
+    locally_relevant_solution = 0;
+    system_rhs = 0;
 
-        system_matrix.reinit(owned_partitioning, dsp, mpi_communicator);
-        locally_relevant_solution = 0;
-        system_rhs = 0;
+    QGauss<dim> nine_quadrature(fe_nine.degree + 1);
+    QGauss<dim> ten_quadrature(fe_ten.degree + 1);
 
-        QGauss<dim> nine_quadrature(fe_nine.degree + 1);
-        QGauss<dim> ten_quadrature(fe_ten.degree + 1);
+    hp::QCollection<dim> q_collection;
+    q_collection.push_back(nine_quadrature);
+    q_collection.push_back(ten_quadrature);
 
-        hp::QCollection<dim> q_collection;
-        q_collection.push_back(nine_quadrature);
-        q_collection.push_back(ten_quadrature);
+    hp::FEValues<dim> hp_fe_values(fe_collection,
+                                   q_collection,
+                                   update_values | update_quadrature_points |
+                                   update_JxW_values | update_gradients);
 
-        hp::FEValues<dim> hp_fe_values(fe_collection,
-                                       q_collection,
-                                       update_values | update_quadrature_points |
-                                       update_JxW_values | update_gradients);
+    QGauss<dim - 1> common_face_quadrature(fe_ten.degree + 1);
+
+    FEFaceValues<dim> fe_nine_face_values(fe_nine,
+                                          common_face_quadrature,
+                                          update_JxW_values |
+                                          update_gradients | update_values);
+    FEFaceValues<dim> fe_ten_face_values(fe_ten,
+                                         common_face_quadrature,
+                                         update_normal_vectors |
+                                         update_values);
 
-        QGauss<dim - 1> common_face_quadrature(fe_ten.degree + 1);
-
-        FEFaceValues<dim> fe_nine_face_values(fe_nine,
-                                              common_face_quadrature,
-                                              update_JxW_values |
-                                              update_gradients | update_values);
-        FEFaceValues<dim> fe_ten_face_values(fe_ten,
-                                             common_face_quadrature,
-                                             update_normal_vectors |
-                                             update_values);
-
-        FullMatrix<double> cell_matrix;
-        Vector<double> cell_rhs;
-        std::vector<types::global_dof_index> local_dof_indices;
-
-        const FEValuesExtractors::Scalar densities(SolutionComponents::density<dim>);
-        const FEValuesExtractors::Vector displacements(SolutionComponents::displacement<dim>);
-        const FEValuesExtractors::Scalar unfiltered_densities(SolutionComponents::unfiltered_density<dim>);
-        const FEValuesExtractors::Vector displacement_multipliers(SolutionComponents::displacement_multiplier<dim>);
-        const FEValuesExtractors::Scalar unfiltered_density_multipliers(
+    FullMatrix<double> cell_matrix;
+    Vector<double> cell_rhs;
+    std::vector<types::global_dof_index> local_dof_indices;
+
+    const FEValuesExtractors::Scalar densities(SolutionComponents::density<dim>);
+    const FEValuesExtractors::Vector displacements(SolutionComponents::displacement<dim>);
+    const FEValuesExtractors::Scalar unfiltered_densities(SolutionComponents::unfiltered_density<dim>);
+    const FEValuesExtractors::Vector displacement_multipliers(SolutionComponents::displacement_multiplier<dim>);
+    const FEValuesExtractors::Scalar unfiltered_density_multipliers(
                 SolutionComponents::unfiltered_density_multiplier<dim>);
-        const FEValuesExtractors::Scalar density_lower_slacks(SolutionComponents::density_lower_slack<dim>);
-        const FEValuesExtractors::Scalar density_lower_slack_multipliers(
+    const FEValuesExtractors::Scalar density_lower_slacks(SolutionComponents::density_lower_slack<dim>);
+    const FEValuesExtractors::Scalar density_lower_slack_multipliers(
                 SolutionComponents::density_lower_slack_multiplier<dim>);
-        const FEValuesExtractors::Scalar density_upper_slacks(SolutionComponents::density_upper_slack<dim>);
-        const FEValuesExtractors::Scalar density_upper_slack_multipliers(
+    const FEValuesExtractors::Scalar density_upper_slacks(SolutionComponents::density_upper_slack<dim>);
+    const FEValuesExtractors::Scalar density_upper_slack_multipliers(
                 SolutionComponents::density_upper_slack_multiplier<dim>);
-        const FEValuesExtractors::Scalar total_volume_multiplier(
+    const FEValuesExtractors::Scalar total_volume_multiplier(
                 SolutionComponents::total_volume_multiplier<dim>);
 
-        const Functions::ConstantFunction<dim> lambda(1.), mu(1.);
-
-        distributed_solution = distributed_state;
-        LA::MPI::BlockVector filtered_unfiltered_density_solution = distributed_solution;
-        LA::MPI::BlockVector filter_adjoint_unfiltered_density_multiplier_solution = distributed_solution;
-        filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = 0;
-        filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier) = 0;
-        auto op_f = linear_operator<LA::MPI::Vector>(density_filter.filter_matrix);
-        filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = op_f * distributed_solution.block(SolutionBlocks::unfiltered_density);
-        filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier) = transpose_operator(op_f) *  distributed_solution.block(SolutionBlocks::unfiltered_density_multiplier);
-
-        LA::MPI::BlockVector relevant_filtered_unfiltered_density_solution = locally_relevant_solution;
-        LA::MPI::BlockVector relevant_filter_adjoint_unfiltered_density_multiplier_solution = locally_relevant_solution;
-        relevant_filtered_unfiltered_density_solution =filtered_unfiltered_density_solution;
-        relevant_filter_adjoint_unfiltered_density_multiplier_solution = filter_adjoint_unfiltered_density_multiplier_solution;
-        for (const auto &cell: dof_handler.active_cell_iterators()) {
-            if(cell->is_locally_owned())
-            {
-                hp_fe_values.reinit(cell);
-                const FEValues<dim> &fe_values = hp_fe_values.get_present_fe_values();
-                cell_matrix.reinit(cell->get_fe().n_dofs_per_cell(),
-                                   cell->get_fe().n_dofs_per_cell());
-                cell_rhs.reinit(cell->get_fe().n_dofs_per_cell());
-
-                const unsigned int n_q_points = fe_values.n_quadrature_points;
-
-                std::vector<double> old_density_values(n_q_points);
-                std::vector<Tensor<1, dim>> old_displacement_values(n_q_points);
-                std::vector<double> old_displacement_divs(n_q_points);
-                std::vector<SymmetricTensor<2, dim>> old_displacement_symmgrads(
+    const Functions::ConstantFunction<dim> lambda(1.), mu(1.);
+
+    distributed_solution = distributed_state;
+    LA::MPI::BlockVector filtered_unfiltered_density_solution = distributed_solution;
+    LA::MPI::BlockVector filter_adjoint_unfiltered_density_multiplier_solution = distributed_solution;
+    filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = 0;
+    filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier) = 0;
+    auto op_f = linear_operator<LA::MPI::Vector>(density_filter.filter_matrix);
+    filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = op_f * distributed_solution.block(SolutionBlocks::unfiltered_density);
+    filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier) = transpose_operator(op_f) *  distributed_solution.block(SolutionBlocks::unfiltered_density_multiplier);
+
+    LA::MPI::BlockVector relevant_filtered_unfiltered_density_solution = locally_relevant_solution;
+    LA::MPI::BlockVector relevant_filter_adjoint_unfiltered_density_multiplier_solution = locally_relevant_solution;
+    relevant_filtered_unfiltered_density_solution =filtered_unfiltered_density_solution;
+    relevant_filter_adjoint_unfiltered_density_multiplier_solution = filter_adjoint_unfiltered_density_multiplier_solution;
+    for (const auto &cell: dof_handler.active_cell_iterators()) {
+        if(cell->is_locally_owned())
+        {
+            hp_fe_values.reinit(cell);
+            const FEValues<dim> &fe_values = hp_fe_values.get_present_fe_values();
+            cell_matrix.reinit(cell->get_fe().n_dofs_per_cell(),
+                               cell->get_fe().n_dofs_per_cell());
+            cell_rhs.reinit(cell->get_fe().n_dofs_per_cell());
+
+            const unsigned int n_q_points = fe_values.n_quadrature_points;
+
+            std::vector<double> old_density_values(n_q_points);
+            std::vector<Tensor<1, dim>> old_displacement_values(n_q_points);
+            std::vector<double> old_displacement_divs(n_q_points);
+            std::vector<SymmetricTensor<2, dim>> old_displacement_symmgrads(
                         n_q_points);
-                std::vector<Tensor<1, dim>> old_displacement_multiplier_values(
+            std::vector<Tensor<1, dim>> old_displacement_multiplier_values(
                         n_q_points);
-                std::vector<double> old_displacement_multiplier_divs(n_q_points);
-                std::vector<SymmetricTensor<2, dim>> old_displacement_multiplier_symmgrads(
+            std::vector<double> old_displacement_multiplier_divs(n_q_points);
+            std::vector<SymmetricTensor<2, dim>> old_displacement_multiplier_symmgrads(
                         n_q_points);
-                std::vector<double> old_lower_slack_multiplier_values(n_q_points);
-                std::vector<double> old_upper_slack_multiplier_values(n_q_points);
-                std::vector<double> old_lower_slack_values(n_q_points);
-                std::vector<double> old_upper_slack_values(n_q_points);
-                std::vector<double> old_unfiltered_density_values(n_q_points);
-                std::vector<double> old_unfiltered_density_multiplier_values(n_q_points);
-                std::vector<double> filtered_unfiltered_density_values(n_q_points);
-                std::vector<double> filter_adjoint_unfiltered_density_multiplier_values(n_q_points);
-                std::vector<double> lambda_values(n_q_points);
-                std::vector<double> mu_values(n_q_points);
-
-                const unsigned int dofs_per_cell = cell->get_fe().n_dofs_per_cell();
-
-                cell_matrix = 0;
-                cell_rhs = 0;
-                local_dof_indices.resize(cell->get_fe().n_dofs_per_cell());
-                cell->get_dof_indices(local_dof_indices);
-
-                lambda.value_list(fe_values.get_quadrature_points(), lambda_values);
-                mu.value_list(fe_values.get_quadrature_points(), mu_values);
-
-                fe_values[densities].get_function_values(relevant_state,
-                                                         old_density_values);
-                fe_values[displacements].get_function_values(relevant_state,
-                                                             old_displacement_values);
-                fe_values[displacements].get_function_divergences(relevant_state,
-                                                                  old_displacement_divs);
-                fe_values[displacements].get_function_symmetric_gradients(
+            std::vector<double> old_lower_slack_multiplier_values(n_q_points);
+            std::vector<double> old_upper_slack_multiplier_values(n_q_points);
+            std::vector<double> old_lower_slack_values(n_q_points);
+            std::vector<double> old_upper_slack_values(n_q_points);
+            std::vector<double> old_unfiltered_density_values(n_q_points);
+            std::vector<double> old_unfiltered_density_multiplier_values(n_q_points);
+            std::vector<double> filtered_unfiltered_density_values(n_q_points);
+            std::vector<double> filter_adjoint_unfiltered_density_multiplier_values(n_q_points);
+            std::vector<double> lambda_values(n_q_points);
+            std::vector<double> mu_values(n_q_points);
+
+            const unsigned int dofs_per_cell = cell->get_fe().n_dofs_per_cell();
+
+            cell_matrix = 0;
+            cell_rhs = 0;
+            local_dof_indices.resize(cell->get_fe().n_dofs_per_cell());
+            cell->get_dof_indices(local_dof_indices);
+
+            lambda.value_list(fe_values.get_quadrature_points(), lambda_values);
+            mu.value_list(fe_values.get_quadrature_points(), mu_values);
+
+            fe_values[densities].get_function_values(relevant_state,
+                                                     old_density_values);
+            fe_values[displacements].get_function_values(relevant_state,
+                                                         old_displacement_values);
+            fe_values[displacements].get_function_divergences(relevant_state,
+                                                              old_displacement_divs);
+            fe_values[displacements].get_function_symmetric_gradients(
                         relevant_state, old_displacement_symmgrads);
-                fe_values[displacement_multipliers].get_function_values(
+            fe_values[displacement_multipliers].get_function_values(
                         relevant_state, old_displacement_multiplier_values);
-                fe_values[displacement_multipliers].get_function_divergences(
+            fe_values[displacement_multipliers].get_function_divergences(
                         relevant_state, old_displacement_multiplier_divs);
-                fe_values[displacement_multipliers].get_function_symmetric_gradients(
+            fe_values[displacement_multipliers].get_function_symmetric_gradients(
                         relevant_state, old_displacement_multiplier_symmgrads);
-                fe_values[density_lower_slacks].get_function_values(
+            fe_values[density_lower_slacks].get_function_values(
                         relevant_state, old_lower_slack_values);
-                fe_values[density_lower_slack_multipliers].get_function_values(
+            fe_values[density_lower_slack_multipliers].get_function_values(
                         relevant_state, old_lower_slack_multiplier_values);
-                fe_values[density_upper_slacks].get_function_values(
+            fe_values[density_upper_slacks].get_function_values(
                         relevant_state, old_upper_slack_values);
-                fe_values[density_upper_slack_multipliers].get_function_values(
+            fe_values[density_upper_slack_multipliers].get_function_values(
                         relevant_state, old_upper_slack_multiplier_values);
-                fe_values[unfiltered_densities].get_function_values(
+            fe_values[unfiltered_densities].get_function_values(
                         relevant_state, old_unfiltered_density_values);
-                fe_values[unfiltered_density_multipliers].get_function_values(
+            fe_values[unfiltered_density_multipliers].get_function_values(
                         relevant_state, old_unfiltered_density_multiplier_values);
-                fe_values[unfiltered_densities].get_function_values(
+            fe_values[unfiltered_densities].get_function_values(
+                        relevant_filtered_unfiltered_density_solution, filtered_unfiltered_density_values);
+            fe_values[unfiltered_density_multipliers].get_function_values(
+                        relevant_filter_adjoint_unfiltered_density_multiplier_solution,
+                        filter_adjoint_unfiltered_density_multiplier_values);
+
+            Tensor<1, dim> traction;
+            traction[1] = -1;
+
+            for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
+
+                for (unsigned int i = 0; i < dofs_per_cell; ++i) {
+                    const SymmetricTensor<2, dim> displacement_phi_i_symmgrad =
+                            fe_values[displacements].symmetric_gradient(i, q_point);
+                    const double displacement_phi_i_div =
+                            fe_values[displacements].divergence(i, q_point);
+
+                    const SymmetricTensor<2, dim> displacement_multiplier_phi_i_symmgrad =
+                            fe_values[displacement_multipliers].symmetric_gradient(i,
+                                                                                   q_point);
+                    const double displacement_multiplier_phi_i_div =
+                            fe_values[displacement_multipliers].divergence(i,
+                                                                           q_point);
+
+
+                    const double density_phi_i = fe_values[densities].value(i,
+                                                                            q_point);
+                    const double unfiltered_density_phi_i = fe_values[unfiltered_densities].value(i,
+                                                                                                  q_point);
+                    const double unfiltered_density_multiplier_phi_i = fe_values[unfiltered_density_multipliers].value(
+                                i, q_point);
+
+                    const double lower_slack_multiplier_phi_i =
+                            fe_values[density_lower_slack_multipliers].value(i,
+                                                                             q_point);
+
+                    const double lower_slack_phi_i =
+                            fe_values[density_lower_slacks].value(i, q_point);
+
+                    const double upper_slack_phi_i =
+                            fe_values[density_upper_slacks].value(i, q_point);
+
+                    const double upper_slack_multiplier_phi_i =
+                            fe_values[density_upper_slack_multipliers].value(i,
+                                                                             q_point);
+
+
+                    for (unsigned int j = 0; j < dofs_per_cell; ++j) {
+                        const SymmetricTensor<2, dim> displacement_phi_j_symmgrad =
+                                fe_values[displacements].symmetric_gradient(j,
+                                                                            q_point);
+                        const double displacement_phi_j_div =
+                                fe_values[displacements].divergence(j, q_point);
+
+                        const SymmetricTensor<2, dim> displacement_multiplier_phi_j_symmgrad =
+                                fe_values[displacement_multipliers].symmetric_gradient(
+                                    j, q_point);
+                        const double displacement_multiplier_phi_j_div =
+                                fe_values[displacement_multipliers].divergence(j,
+                                                                               q_point);
+
+                        const double density_phi_j = fe_values[densities].value(
+                                    j, q_point);
+
+                        const double unfiltered_density_phi_j = fe_values[unfiltered_densities].value(j,
+                                                                                                      q_point);
+                        const double unfiltered_density_multiplier_phi_j = fe_values[unfiltered_density_multipliers].value(
+                                    j, q_point);
+
+
+                        const double lower_slack_phi_j =
+                                fe_values[density_lower_slacks].value(j, q_point);
+
+                        const double upper_slack_phi_j =
+                                fe_values[density_upper_slacks].value(j, q_point);
+
+                        const double lower_slack_multiplier_phi_j =
+                                fe_values[density_lower_slack_multipliers].value(j,
+                                                                                 q_point);
+
+                        const double upper_slack_multiplier_phi_j =
+                                fe_values[density_upper_slack_multipliers].value(j,
+                                                                                 q_point);
+
+                        //Equation 0
+                        cell_matrix(i, j) +=
+                                fe_values.JxW(q_point) *
+                                (
+                                    -density_phi_i * unfiltered_density_multiplier_phi_j
+
+                                    - density_penalty_exponent * (density_penalty_exponent - 1)
+                                    * std::pow(
+                                        old_density_values[q_point],
+                                        density_penalty_exponent - 2)
+                                    * density_phi_i
+                                    * density_phi_j
+                                    * (old_displacement_multiplier_divs[q_point] * old_displacement_divs[q_point]
+                                       * lambda_values[q_point]
+                                       + 2 * mu_values[q_point]
+                                       * (old_displacement_symmgrads[q_point] *
+                                          old_displacement_multiplier_symmgrads[q_point]))
+
+                                    - density_penalty_exponent * std::pow(
+                                        old_density_values[q_point],
+                                        density_penalty_exponent - 1)
+                                    * density_phi_i
+                                    * (displacement_multiplier_phi_j_div * old_displacement_divs[q_point]
+                                       * lambda_values[q_point]
+                                       + 2 * mu_values[q_point]
+                                       *
+                                       (old_displacement_symmgrads[q_point] *
+                                        displacement_multiplier_phi_j_symmgrad))
+
+                                    - density_penalty_exponent * std::pow(
+                                        old_density_values[q_point],
+                                        density_penalty_exponent - 1)
+                                    * density_phi_i
+                                    * (displacement_phi_j_div * old_displacement_multiplier_divs[q_point]
+                                       * lambda_values[q_point]
+                                       + 2 * mu_values[q_point]
+                                       * (old_displacement_multiplier_symmgrads[q_point] *
+                                          displacement_phi_j_symmgrad)));
+                        //Equation 1
+
+                        cell_matrix(i, j) +=
+                                fe_values.JxW(q_point) * (
+                                    -density_penalty_exponent * std::pow(
+                                        old_density_values[q_point],
+                                        density_penalty_exponent - 1)
+                                    * density_phi_j
+                                    * (old_displacement_multiplier_divs[q_point] * displacement_phi_i_div
+                                       * lambda_values[q_point]
+                                       + 2 * mu_values[q_point]
+                                       * (old_displacement_multiplier_symmgrads[q_point] *
+                                          displacement_phi_i_symmgrad))
+
+                                    - std::pow(old_density_values[q_point],
+                                               density_penalty_exponent)
+                                    * (displacement_multiplier_phi_j_div * displacement_phi_i_div
+                                       * lambda_values[q_point]
+                                       + 2 * mu_values[q_point]
+                                       * (displacement_multiplier_phi_j_symmgrad * displacement_phi_i_symmgrad))
+
+                                    );
+
+                        //Equation 2 has to do with the filter, which is calculated elsewhere.
+                        cell_matrix(i, j) +=
+                                fe_values.JxW(q_point) * (
+                                    -1 * unfiltered_density_phi_i * lower_slack_multiplier_phi_j
+                                    + unfiltered_density_phi_i * upper_slack_multiplier_phi_j);
+
+                        //Equation 3 - Primal Feasibility
+
+                        cell_matrix(i, j) +=
+                                fe_values.JxW(q_point) * (
+
+                                    -1 * density_penalty_exponent * std::pow(
+                                        old_density_values[q_point],
+                                        density_penalty_exponent - 1)
+                                    * density_phi_j
+                                    * (old_displacement_divs[q_point] * displacement_multiplier_phi_i_div
+                                       * lambda_values[q_point]
+                                       + 2 * mu_values[q_point]
+                                       * (old_displacement_symmgrads[q_point] *
+                                          displacement_multiplier_phi_i_symmgrad))
+
+                                    + -1 * std::pow(old_density_values[q_point],
+                                                    density_penalty_exponent)
+                                    * (displacement_phi_j_div * displacement_multiplier_phi_i_div
+                                       * lambda_values[q_point]
+                                       + 2 * mu_values[q_point]
+                                       *
+                                       (displacement_phi_j_symmgrad * displacement_multiplier_phi_i_symmgrad)));
+
+                        //Equation 4 - more primal feasibility
+                        cell_matrix(i, j) +=
+                                -1 * fe_values.JxW(q_point) * lower_slack_multiplier_phi_i *
+                                (unfiltered_density_phi_j - lower_slack_phi_j);
+
+                        //Equation 5 - more primal feasibility
+                        cell_matrix(i, j) +=
+                                -1 * fe_values.JxW(q_point) * upper_slack_multiplier_phi_i * (
+                                    -1 * unfiltered_density_phi_j - upper_slack_phi_j);
+
+                        //Equation 6 - more primal feasibility - part with filter added later
+                        cell_matrix(i, j) +=
+                                -1 * fe_values.JxW(q_point) * unfiltered_density_multiplier_phi_i * (
+                                    density_phi_j);
+
+                        //Equation 7 - complementary slackness
+                        cell_matrix(i, j) += fe_values.JxW(q_point) *
+                                (lower_slack_phi_i * lower_slack_multiplier_phi_j
+                                 + lower_slack_phi_i * lower_slack_phi_j *
+                                 old_lower_slack_multiplier_values[q_point] /
+                                 old_lower_slack_values[q_point]);
+                        //Equation 8 - complementary slackness
+                        cell_matrix(i, j) += fe_values.JxW(q_point) *
+                                (upper_slack_phi_i * upper_slack_multiplier_phi_j
+                                 + upper_slack_phi_i * upper_slack_phi_j *
+                                 old_upper_slack_multiplier_values[q_point] /
+                                 old_upper_slack_values[q_point]);
+                    }
+
+                }
+            }
+
+
+            MatrixTools::local_apply_boundary_values(boundary_values, local_dof_indices,
+                                                     cell_matrix, cell_rhs, true);
+
+
+            constraints.distribute_local_to_global(
+                        cell_matrix, cell_rhs, local_dof_indices, system_matrix, system_rhs);
+
+        }
+
+    }
+    pcout << "compress 1" << std::endl;
+    system_matrix.compress(VectorOperation::add);
+    system_rhs = calculate_rhs(distributed_state, barrier_size);
+
+    for (const auto &cell: dof_handler.active_cell_iterators()) {
+        if(cell->is_locally_owned())
+        {
+            std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
+            cell->get_dof_indices(i);
+
+            typename LA::MPI::SparseMatrix::iterator iter = density_filter.filter_matrix.begin(
+                        i[cell->get_fe().component_to_system_index(0, 0)]);
+            for (; iter != density_filter.filter_matrix.end(i[cell->get_fe().component_to_system_index(0, 0)]); iter++) {
+                unsigned int j = iter->column();
+                double value = iter->value() * cell->measure();
+
+                system_matrix.block(SolutionBlocks::unfiltered_density_multiplier,
+                                    SolutionBlocks::unfiltered_density).add(i[cell->get_fe().component_to_system_index(0, 0)], j, value);
+                system_matrix.block(SolutionBlocks::unfiltered_density,
+                                    SolutionBlocks::unfiltered_density_multiplier).add(j, i[cell->get_fe().component_to_system_index(0, 0)], value);
+            }
+
+            system_matrix.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).add(0, i[cell->get_fe().component_to_system_index(0, 0)],
+                    cell->measure());
+
+            system_matrix.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier).add(i[cell->get_fe().component_to_system_index(0, 0)], 0,
+                    cell->measure());
+        }
+    }
+    pcout << "compress 2" << std::endl;
+    system_matrix.compress(VectorOperation::add);
+
+
+    pcout << "assembled" << std::endl;
+
+}
+
+template<int dim>
+double
+KktSystem<dim>::calculate_objective_value(const LA::MPI::BlockVector &distributed_state) const {
+    /*Remove any values from old iterations*/
+
+    locally_relevant_solution = distributed_state;
+
+
+    QGauss<dim> nine_quadrature(fe_nine.degree + 1);
+    QGauss<dim> ten_quadrature(fe_ten.degree + 1);
+
+    hp::QCollection<dim> q_collection;
+    q_collection.push_back(nine_quadrature);
+    q_collection.push_back(ten_quadrature);
+
+    hp::FEValues<dim> hp_fe_values(fe_collection,
+                                   q_collection,
+                                   update_values | update_quadrature_points |
+                                   update_JxW_values | update_gradients);
+
+    QGauss<dim - 1> common_face_quadrature(fe_ten.degree + 1);
+
+    FEFaceValues<dim> fe_nine_face_values(fe_nine,
+                                          common_face_quadrature,
+                                          update_JxW_values |
+                                          update_gradients | update_values);
+    FEFaceValues<dim> fe_ten_face_values(fe_ten,
+                                         common_face_quadrature,
+                                         update_normal_vectors |
+                                         update_values);
+
+    FullMatrix<double> cell_matrix;
+    Vector<double> cell_rhs;
+
+    const FEValuesExtractors::Vector displacements(SolutionComponents::displacement<dim>);
+
+    Tensor<1, dim> traction;
+    traction[1] = -1;
+    distributed_solution = distributed_state;
+    double objective_value = 0;
+    for (const auto &cell: dof_handler.active_cell_iterators())
+    {
+        if(cell->is_locally_owned())
+        {
+            hp_fe_values.reinit(cell);
+            const FEValues<dim> &fe_values = hp_fe_values.get_present_fe_values();
+            const unsigned int dofs_per_cell = cell->get_fe().n_dofs_per_cell();
+            const unsigned int n_q_points = fe_values.n_quadrature_points;
+            const unsigned int n_face_q_points = common_face_quadrature.size();
+
+            std::vector<Tensor<1, dim>> old_displacement_values(n_q_points);
+            fe_values[displacements].get_function_values(
+                        locally_relevant_solution, old_displacement_values);
+
+            for (unsigned int face_number = 0;
+                 face_number < GeometryInfo<dim>::faces_per_cell;
+                 ++face_number)
+            {
+                if (cell->face(face_number)->at_boundary() && cell->face(face_number)->boundary_id()
+                        == BoundaryIds::down_force)
+                {
+                    for (unsigned int face_q_point = 0;
+                         face_q_point < n_face_q_points; ++face_q_point) {
+                        for (unsigned int i = 0; i < dofs_per_cell; ++i) {
+                            if (cell->material_id() == MaterialIds::without_multiplier) {
+                                fe_nine_face_values.reinit(cell, face_number);
+                                objective_value += traction
+                                        * fe_nine_face_values[displacements].value(i,
+                                                                                   face_q_point)
+                                        * fe_nine_face_values.JxW(face_q_point);
+                            } else {
+                                fe_ten_face_values.reinit(cell, face_number);
+                                objective_value += traction
+                                        * fe_ten_face_values[displacements].value(i,
+                                                                                  face_q_point)
+                                        * fe_ten_face_values.JxW(face_q_point);
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
+    double objective_value_out;
+    MPI_Allreduce(&objective_value, &objective_value_out, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+
+    pcout << "objective value: " << objective_value_out << std::endl;
+    return objective_value;
+}
+
+
+//As the KKT System know which vectors correspond to the slack variables, the sum of the logs of the slacks is computed here for use in the filter.
+template<int dim>
+double
+KktSystem<dim>::calculate_barrier_distance(const LA::MPI::BlockVector &state) const {
+    double barrier_distance_log_sum = 0;
+    unsigned int vect_size = state.block(SolutionBlocks::density_lower_slack).size();
+    distributed_solution = state;
+    for (unsigned int k = 0; k < vect_size; k++) {
+        if (distributed_solution.block(SolutionBlocks::density_lower_slack).in_local_range(k))
+            barrier_distance_log_sum += std::log(state.block(SolutionBlocks::density_lower_slack)[k]);
+    }
+    for (unsigned int k = 0; k < vect_size; k++) {
+        if (distributed_solution.block(SolutionBlocks::density_upper_slack).in_local_range(k))
+            barrier_distance_log_sum += std::log(state.block(SolutionBlocks::density_upper_slack)[k]);
+    }
+    double out_barrier_distance_log_sum;
+    MPI_Allreduce(&barrier_distance_log_sum, &out_barrier_distance_log_sum, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+
+    pcout << "Barrier distance log sum: " << out_barrier_distance_log_sum << std::endl;
+
+    return out_barrier_distance_log_sum;
+}
+
+template<int dim>
+double
+KktSystem<dim>::calculate_rhs_norm(const LA::MPI::BlockVector &state, const double barrier_size) const {
+    return calculate_rhs(state, barrier_size).l2_norm();
+}
+
+
+//Feasibility conditions appear on the RHS of the linear system, so I compute the RHS to find it. Could probably be combined with the objective value finding part to make it faster.
+template<int dim>
+double
+KktSystem<dim>::calculate_feasibility(const LA::MPI::BlockVector &state, const double barrier_size) const {
+    LA::MPI::BlockVector test_rhs = calculate_rhs(state, barrier_size);
+
+    double norm = 0;
+
+    distributed_solution = state;
+    double full_prod1 =1;
+    double full_prod2 = 1;
+
+    for (unsigned int k = 0; k < state.block(SolutionBlocks::density_upper_slack).size(); k++) {
+        double prod1 = 1;
+        double prod2 = 1;
+        if(state.block(SolutionBlocks::density_upper_slack).in_local_range(k))
+        {
+            prod1 = prod1 * state.block(SolutionBlocks::density_upper_slack)[k]
+                    * state.block(SolutionBlocks::density_upper_slack)[k];
+        }
+        if(state.block(SolutionBlocks::density_lower_slack).in_local_range(k))
+        {
+            prod2 = prod2 *  state.block(SolutionBlocks::density_lower_slack)[k]
+                    * state.block(SolutionBlocks::density_lower_slack)[k];
+        }
+        if(state.block(SolutionBlocks::density_upper_slack_multiplier).in_local_range(k))
+        {
+            prod1 = prod1 * state.block(SolutionBlocks::density_upper_slack_multiplier)[k]
+                    * state.block(SolutionBlocks::density_upper_slack_multiplier)[k];
+        }
+        if(state.block(SolutionBlocks::density_lower_slack_multiplier).in_local_range(k))
+        {
+            prod2 = prod2 *  state.block(SolutionBlocks::density_lower_slack_multiplier)[k]
+                    * state.block(SolutionBlocks::density_lower_slack_multiplier)[k];
+        }
+        MPI_Allreduce(&prod1, &full_prod1, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
+        MPI_Allreduce(&prod2, &full_prod2, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
+        norm = norm + full_prod1 + full_prod2;
+    }
+    pcout << "pre-norm: " << norm << std::endl;
+
+    norm += std::pow(test_rhs.block(SolutionBlocks::displacement).l2_norm(), 2);
+    norm += std::pow(test_rhs.block(SolutionBlocks::density).l2_norm(), 2);
+    norm += std::pow(test_rhs.block(SolutionBlocks::unfiltered_density).l2_norm(), 2);
+    norm += std::pow(test_rhs.block(SolutionBlocks::displacement_multiplier).l2_norm(), 2);
+    norm += std::pow(test_rhs.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm(), 2);
+    norm += std::pow(test_rhs.block(SolutionBlocks::total_volume_multiplier).l2_norm(), 2);
+    norm += std::pow(test_rhs.block(SolutionBlocks::density_upper_slack_multiplier).l2_norm(), 2);
+    norm += std::pow(test_rhs.block(SolutionBlocks::density_lower_slack_multiplier).l2_norm(), 2);
+
+    pcout << "norm: " << norm << std::endl;
+
+    return norm;
+}
+
+template<int dim>
+double
+KktSystem<dim>::calculate_convergence(const LA::MPI::BlockVector &state) const {
+    LA::MPI::BlockVector test_rhs = calculate_rhs(state, Input::min_barrier_size);
+    double norm = 0;
+
+    distributed_solution = state;
+    double full_prod1 =1;
+    double full_prod2 = 1;
+
+    for (unsigned int k = 0; k < state.block(SolutionBlocks::density_upper_slack).size(); k++) {
+        double prod1 = 1;
+        double prod2 = 1;
+        if(state.block(SolutionBlocks::density_upper_slack).in_local_range(k))
+        {
+            prod1 = prod1 * state.block(SolutionBlocks::density_upper_slack)[k]
+                    * state.block(SolutionBlocks::density_upper_slack)[k];
+        }
+        if(state.block(SolutionBlocks::density_lower_slack).in_local_range(k))
+        {
+            prod2 = prod2 *  state.block(SolutionBlocks::density_lower_slack)[k]
+                    * state.block(SolutionBlocks::density_lower_slack)[k];
+        }
+        if(state.block(SolutionBlocks::density_upper_slack_multiplier).in_local_range(k))
+        {
+            prod1 = prod1 * state.block(SolutionBlocks::density_upper_slack_multiplier)[k]
+                    * state.block(SolutionBlocks::density_upper_slack_multiplier)[k];
+        }
+        if(state.block(SolutionBlocks::density_lower_slack_multiplier).in_local_range(k))
+        {
+            prod2 = prod2 *  state.block(SolutionBlocks::density_lower_slack_multiplier)[k]
+                    * state.block(SolutionBlocks::density_lower_slack_multiplier)[k];
+        }
+        MPI_Allreduce(&prod1, &full_prod1, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
+        MPI_Allreduce(&prod2, &full_prod2, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
+        norm = norm + full_prod1 + full_prod2;
+    }
+    pcout << "pre-norm: " << norm << std::endl;
+
+    norm += std::pow(test_rhs.block(SolutionBlocks::displacement).l2_norm(), 2);
+    norm += std::pow(test_rhs.block(SolutionBlocks::density).l2_norm(), 2);
+    norm += std::pow(test_rhs.block(SolutionBlocks::unfiltered_density).l2_norm(), 2);
+    norm += std::pow(test_rhs.block(SolutionBlocks::displacement_multiplier).l2_norm(), 2);
+    norm += std::pow(test_rhs.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm(), 2);
+    norm += std::pow(test_rhs.block(SolutionBlocks::total_volume_multiplier).l2_norm(), 2);
+    norm += std::pow(test_rhs.block(SolutionBlocks::density_upper_slack_multiplier).l2_norm(), 2);
+    norm += std::pow(test_rhs.block(SolutionBlocks::density_lower_slack_multiplier).l2_norm(), 2);
+
+    norm = std::pow(norm, .5);
+
+    pcout << "l2 norm: " << system_rhs.l2_norm() << std::endl;
+    pcout << "KKT norm: " << norm << std::endl;
+    return norm;
+}
+
+template<int dim>
+LA::MPI::BlockVector
+KktSystem<dim>::calculate_rhs(const LA::MPI::BlockVector &distributed_state, const double barrier_size) const {
+    LA::MPI::BlockVector test_rhs (system_rhs);
+    LA::MPI::BlockVector state (locally_relevant_solution);
+    state = distributed_state;
+    test_rhs = 0;
+
+
+    QGauss<dim> nine_quadrature(fe_nine.degree + 1);
+    QGauss<dim> ten_quadrature(fe_ten.degree + 1);
+
+    hp::QCollection<dim> q_collection;
+    q_collection.push_back(nine_quadrature);
+    q_collection.push_back(ten_quadrature);
+
+    hp::FEValues<dim> hp_fe_values(fe_collection,
+                                   q_collection,
+                                   update_values | update_quadrature_points |
+                                   update_JxW_values | update_gradients);
+
+    QGauss<dim - 1> common_face_quadrature(fe_ten.degree + 1);
+
+    FEFaceValues<dim> fe_nine_face_values(fe_nine,
+                                          common_face_quadrature,
+                                          update_JxW_values |
+                                          update_gradients | update_values);
+    FEFaceValues<dim> fe_ten_face_values(fe_ten,
+                                         common_face_quadrature,
+                                         update_normal_vectors |
+                                         update_values);
+
+    FullMatrix<double> cell_matrix;
+    Vector<double> cell_rhs;
+    std::vector<types::global_dof_index> local_dof_indices;
+
+    const FEValuesExtractors::Scalar densities(SolutionComponents::density<dim>);
+    const FEValuesExtractors::Vector displacements(SolutionComponents::displacement<dim>);
+    const FEValuesExtractors::Scalar unfiltered_densities(SolutionComponents::unfiltered_density<dim>);
+    const FEValuesExtractors::Vector displacement_multipliers(SolutionComponents::displacement_multiplier<dim>);
+    const FEValuesExtractors::Scalar unfiltered_density_multipliers(
+                SolutionComponents::unfiltered_density_multiplier<dim>);
+    const FEValuesExtractors::Scalar density_lower_slacks(SolutionComponents::density_lower_slack<dim>);
+    const FEValuesExtractors::Scalar density_lower_slack_multipliers(
+                SolutionComponents::density_lower_slack_multiplier<dim>);
+    const FEValuesExtractors::Scalar density_upper_slacks(SolutionComponents::density_upper_slack<dim>);
+    const FEValuesExtractors::Scalar density_upper_slack_multipliers(
+                SolutionComponents::density_upper_slack_multiplier<dim>);
+    const FEValuesExtractors::Scalar total_volume_multiplier(
+                SolutionComponents::total_volume_multiplier<dim>);
+
+
+    const unsigned int n_face_q_points = common_face_quadrature.size();
+
+    const Functions::ConstantFunction<dim> lambda(1.), mu(1.);
+
+    locally_relevant_solution = state;
+    distributed_solution = state;
+    LA::MPI::BlockVector filtered_unfiltered_density_solution (distributed_solution);
+    LA::MPI::BlockVector filter_adjoint_unfiltered_density_multiplier_solution (distributed_solution);
+    filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = 0;
+    filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier) = 0;
+
+    auto op_f = linear_operator<LA::MPI::Vector>(density_filter.filter_matrix);
+    filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = op_f * distributed_solution.block(SolutionBlocks::unfiltered_density);
+    filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier)
+            = transpose_operator(op_f) * distributed_solution.block(SolutionBlocks::unfiltered_density_multiplier);
+    LA::MPI::BlockVector relevant_filtered_unfiltered_density_solution (locally_relevant_solution);
+    LA::MPI::BlockVector relevant_filter_adjoint_unfiltered_density_multiplier_solution (locally_relevant_solution);
+    relevant_filtered_unfiltered_density_solution = filtered_unfiltered_density_solution;
+    relevant_filter_adjoint_unfiltered_density_multiplier_solution = filter_adjoint_unfiltered_density_multiplier_solution;
+
+    double old_volume_multiplier_temp = 0;
+    double old_volume_multiplier;
+    if(distributed_state.block(SolutionBlocks::total_volume_multiplier).in_local_range(0))
+    {
+        old_volume_multiplier_temp = state.block(SolutionBlocks::total_volume_multiplier)[0];
+    }
+    MPI_Allreduce(&old_volume_multiplier_temp, &old_volume_multiplier, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+
+    for (const auto &cell: dof_handler.active_cell_iterators()) {
+        if(cell->is_locally_owned())
+        {
+            hp_fe_values.reinit(cell);
+            const FEValues<dim> &fe_values = hp_fe_values.get_present_fe_values();
+            cell_matrix.reinit(cell->get_fe().n_dofs_per_cell(),
+                               cell->get_fe().n_dofs_per_cell());
+            cell_rhs.reinit(cell->get_fe().n_dofs_per_cell());
+
+            const unsigned int n_q_points = fe_values.n_quadrature_points;
+
+            std::vector<double> old_density_values(n_q_points);
+            std::vector<Tensor<1, dim>> old_displacement_values(n_q_points);
+            std::vector<double> old_displacement_divs(n_q_points);
+            std::vector<SymmetricTensor<2, dim>> old_displacement_symmgrads(
+                        n_q_points);
+            std::vector<Tensor<1, dim>> old_displacement_multiplier_values(
+                        n_q_points);
+            std::vector<double> old_displacement_multiplier_divs(n_q_points);
+            std::vector<SymmetricTensor<2, dim>> old_displacement_multiplier_symmgrads(
+                        n_q_points);
+            std::vector<double> old_lower_slack_multiplier_values(n_q_points);
+            std::vector<double> old_upper_slack_multiplier_values(n_q_points);
+            std::vector<double> old_lower_slack_values(n_q_points);
+            std::vector<double> old_upper_slack_values(n_q_points);
+            std::vector<double> old_unfiltered_density_values(n_q_points);
+            std::vector<double> old_unfiltered_density_multiplier_values(n_q_points);
+            std::vector<double> filtered_unfiltered_density_values(n_q_points);
+            std::vector<double> filter_adjoint_unfiltered_density_multiplier_values(n_q_points);
+            std::vector<double> lambda_values(n_q_points);
+            std::vector<double> mu_values(n_q_points);
+
+            const unsigned int dofs_per_cell = cell->get_fe().n_dofs_per_cell();
+
+            cell_matrix = 0;
+            cell_rhs = 0;
+            local_dof_indices.resize(cell->get_fe().n_dofs_per_cell());
+
+            cell->get_dof_indices(local_dof_indices);
+
+            lambda.value_list(fe_values.get_quadrature_points(), lambda_values);
+            mu.value_list(fe_values.get_quadrature_points(), mu_values);
+
+            fe_values[densities].get_function_values(state,
+                                                     old_density_values);
+            fe_values[displacements].get_function_values(state,
+                                                         old_displacement_values);
+            fe_values[displacements].get_function_divergences(state,
+                                                              old_displacement_divs);
+            fe_values[displacements].get_function_symmetric_gradients(
+                        state, old_displacement_symmgrads);
+            fe_values[displacement_multipliers].get_function_values(
+                        state, old_displacement_multiplier_values);
+            fe_values[displacement_multipliers].get_function_divergences(
+                        state, old_displacement_multiplier_divs);
+            fe_values[displacement_multipliers].get_function_symmetric_gradients(
+                        state, old_displacement_multiplier_symmgrads);
+            fe_values[density_lower_slacks].get_function_values(
+                        state, old_lower_slack_values);
+            fe_values[density_lower_slack_multipliers].get_function_values(
+                        state, old_lower_slack_multiplier_values);
+            fe_values[density_upper_slacks].get_function_values(
+                        state, old_upper_slack_values);
+            fe_values[density_upper_slack_multipliers].get_function_values(
+                        state, old_upper_slack_multiplier_values);
+            fe_values[unfiltered_densities].get_function_values(
+                        state, old_unfiltered_density_values);
+            fe_values[unfiltered_density_multipliers].get_function_values(
+                        state, old_unfiltered_density_multiplier_values);
+            fe_values[unfiltered_densities].get_function_values(
                         relevant_filtered_unfiltered_density_solution, filtered_unfiltered_density_values);
-                fe_values[unfiltered_density_multipliers].get_function_values(
+            fe_values[unfiltered_density_multipliers].get_function_values(
                         relevant_filter_adjoint_unfiltered_density_multiplier_solution,
                         filter_adjoint_unfiltered_density_multiplier_values);
 
-                Tensor<1, dim> traction;
-                traction[1] = -1;
 
-                for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
+            Tensor<1, dim> traction;
+            traction[1] = -1;
+
+            for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
+
+                for (unsigned int i = 0; i < dofs_per_cell; ++i) {
+                    const SymmetricTensor<2, dim> displacement_phi_i_symmgrad =
+                            fe_values[displacements].symmetric_gradient(i, q_point);
+                    const double displacement_phi_i_div =
+                            fe_values[displacements].divergence(i, q_point);
+
+                    const SymmetricTensor<2, dim> displacement_multiplier_phi_i_symmgrad =
+                            fe_values[displacement_multipliers].symmetric_gradient(i,
+                                                                                   q_point);
+                    const double displacement_multiplier_phi_i_div =
+                            fe_values[displacement_multipliers].divergence(i,
+                                                                           q_point);
+
+
+                    const double density_phi_i = fe_values[densities].value(i,
+                                                                            q_point);
+                    const double unfiltered_density_phi_i = fe_values[unfiltered_densities].value(i,
+                                                                                                  q_point);
+                    const double unfiltered_density_multiplier_phi_i = fe_values[unfiltered_density_multipliers].value(
+                                i, q_point);
+
+                    const double lower_slack_multiplier_phi_i =
+                            fe_values[density_lower_slack_multipliers].value(i,
+                                                                             q_point);
+
+                    const double lower_slack_phi_i =
+                            fe_values[density_lower_slacks].value(i, q_point);
+
+                    const double upper_slack_phi_i =
+                            fe_values[density_upper_slacks].value(i, q_point);
+
+                    const double upper_slack_multiplier_phi_i =
+                            fe_values[density_upper_slack_multipliers].value(i,
+                                                                             q_point);
+
+
+                    //rhs eqn 0
+                    cell_rhs(i) +=
+                            -1 * fe_values.JxW(q_point) * (
+                                -1 * density_penalty_exponent *
+                                std::pow(old_density_values[q_point], density_penalty_exponent - 1) * density_phi_i
+                                * (old_displacement_multiplier_divs[q_point] * old_displacement_divs[q_point]
+                                   * lambda_values[q_point]
+                                   + 2 * mu_values[q_point] * (old_displacement_symmgrads[q_point]
+                                                               * old_displacement_multiplier_symmgrads[q_point]))
+                                - density_phi_i * old_unfiltered_density_multiplier_values[q_point]
+                                + old_volume_multiplier * density_phi_i
+                                );
+
+                    //rhs eqn 1 - boundary terms counted later
+                    cell_rhs(i) +=
+                            -1 * fe_values.JxW(q_point) * (
+                                -1 * std::pow(old_density_values[q_point], density_penalty_exponent)
+                                * (old_displacement_multiplier_divs[q_point] * displacement_phi_i_div
+                                   * lambda_values[q_point]
+                                   + 2 * mu_values[q_point] * (old_displacement_multiplier_symmgrads[q_point]
+                                                               * displacement_phi_i_symmgrad))
+                                );
+
+                    //rhs eqn 2
+                    cell_rhs(i) +=
+                            -1 * fe_values.JxW(q_point) * (
+                                unfiltered_density_phi_i *
+                                filter_adjoint_unfiltered_density_multiplier_values[q_point]
+                                + unfiltered_density_phi_i * old_upper_slack_multiplier_values[q_point]
+                                + -1 * unfiltered_density_phi_i * old_lower_slack_multiplier_values[q_point]
+                                );
+
 
-                    for (unsigned int i = 0; i < dofs_per_cell; ++i) {
-                        const SymmetricTensor<2, dim> displacement_phi_i_symmgrad =
-                                fe_values[displacements].symmetric_gradient(i, q_point);
-                        const double displacement_phi_i_div =
-                                fe_values[displacements].divergence(i, q_point);
 
-                        const SymmetricTensor<2, dim> displacement_multiplier_phi_i_symmgrad =
-                                fe_values[displacement_multipliers].symmetric_gradient(i,
-                                                                                       q_point);
-                        const double displacement_multiplier_phi_i_div =
-                                fe_values[displacement_multipliers].divergence(i,
-                                                                               q_point);
 
+                    //rhs eqn 3 - boundary terms counted later
+                    cell_rhs(i) +=
+                            -1 * fe_values.JxW(q_point) * (
+                                -1 * std::pow(old_density_values[q_point], density_penalty_exponent)
+                                * (old_displacement_divs[q_point] * displacement_multiplier_phi_i_div
+                                   * lambda_values[q_point]
+                                   + 2 * mu_values[q_point] * (displacement_multiplier_phi_i_symmgrad
+                                                               * old_displacement_symmgrads[q_point]))
+                                );
 
-                        const double density_phi_i = fe_values[densities].value(i,
-                                                                                q_point);
-                        const double unfiltered_density_phi_i = fe_values[unfiltered_densities].value(i,
-                                                                                                      q_point);
-                        const double unfiltered_density_multiplier_phi_i = fe_values[unfiltered_density_multipliers].value(
-                                i, q_point);
+                    //rhs eqn 4
+                    cell_rhs(i) +=
+                            -1 * fe_values.JxW(q_point) *
+                            (-1 * lower_slack_multiplier_phi_i
+                             * (old_unfiltered_density_values[q_point] - old_lower_slack_values[q_point])
+                             );
+
+                    //rhs eqn 5
+                    cell_rhs(i) +=
+                            -1 * fe_values.JxW(q_point) * (
+                                -1 * upper_slack_multiplier_phi_i
+                                * (1 - old_unfiltered_density_values[q_point]
+                                   - old_upper_slack_values[q_point]));
+
+                    //rhs eqn 6
+                    cell_rhs(i) +=
+                            -1 * fe_values.JxW(q_point) * (
+                                -1 * unfiltered_density_multiplier_phi_i
+                                * (old_density_values[q_point] - filtered_unfiltered_density_values[q_point])
+                                );
 
-                        const double lower_slack_multiplier_phi_i =
-                                fe_values[density_lower_slack_multipliers].value(i,
-                                                                                 q_point);
+                    //rhs eqn 7
+                    cell_rhs(i) +=
+                            -1 * fe_values.JxW(q_point) *
+                            (lower_slack_phi_i *
+                             (old_lower_slack_multiplier_values[q_point] -
+                              barrier_size / old_lower_slack_values[q_point]));
 
-                        const double lower_slack_phi_i =
-                                fe_values[density_lower_slacks].value(i, q_point);
+                    //rhs eqn 8
+                    cell_rhs(i) +=
+                            -1 * fe_values.JxW(q_point) *
+                            (upper_slack_phi_i *
+                             (old_upper_slack_multiplier_values[q_point] -
+                              barrier_size / old_upper_slack_values[q_point]));
 
-                        const double upper_slack_phi_i =
-                                fe_values[density_upper_slacks].value(i, q_point);
+                }
 
-                        const double upper_slack_multiplier_phi_i =
-                                fe_values[density_upper_slack_multipliers].value(i,
-                                                                                 q_point);
+            }
 
+            for (unsigned int face_number = 0;
+                 face_number < GeometryInfo<dim>::faces_per_cell;
+                 ++face_number) {
+                if (cell->face(face_number)->at_boundary() && cell->face(
+                            face_number)->boundary_id() == BoundaryIds::down_force) {
+                    for (unsigned int face_q_point = 0;
+                         face_q_point < n_face_q_points; ++face_q_point) {
+                        for (unsigned int i = 0; i < dofs_per_cell; ++i) {
+                            if (cell->material_id() == MaterialIds::without_multiplier) {
+                                fe_nine_face_values.reinit(cell, face_number);
+                                cell_rhs(i) += -1
+                                        * traction
+                                        * fe_nine_face_values[displacements].value(i,
+                                                                                   face_q_point)
+                                        * fe_nine_face_values.JxW(face_q_point);
+
+                                cell_rhs(i) += -1 * traction
+                                        * fe_nine_face_values[displacement_multipliers].value(
+                                            i, face_q_point)
+                                        * fe_nine_face_values.JxW(face_q_point);
+                            } else {
+                                fe_ten_face_values.reinit(cell, face_number);
+                                cell_rhs(i) += -1
+                                        * traction
+                                        * fe_ten_face_values[displacements].value(i,
+                                                                                  face_q_point)
+                                        * fe_ten_face_values.JxW(face_q_point);
+
+                                cell_rhs(i) += -1 * traction
+                                        * fe_ten_face_values[displacement_multipliers].value(
+                                            i, face_q_point)
+                                        * fe_ten_face_values.JxW(face_q_point);
+                            }
+                        }
+                    }
+                }
+            }
 
-                        for (unsigned int j = 0; j < dofs_per_cell; ++j) {
-                            const SymmetricTensor<2, dim> displacement_phi_j_symmgrad =
-                                    fe_values[displacements].symmetric_gradient(j,
-                                                                                q_point);
-                            const double displacement_phi_j_div =
-                                    fe_values[displacements].divergence(j, q_point);
 
-                            const SymmetricTensor<2, dim> displacement_multiplier_phi_j_symmgrad =
-                                    fe_values[displacement_multipliers].symmetric_gradient(
-                                            j, q_point);
-                            const double displacement_multiplier_phi_j_div =
-                                    fe_values[displacement_multipliers].divergence(j,
-                                                                                   q_point);
+            MatrixTools::local_apply_boundary_values(boundary_values, local_dof_indices,
+                                                     cell_matrix, cell_rhs, true);
+            constraints.distribute_local_to_global(
+                        cell_rhs, local_dof_indices, test_rhs);
+        }
+    }
 
-                            const double density_phi_j = fe_values[densities].value(
-                                    j, q_point);
+    test_rhs.compress(VectorOperation::add);
+    double total_volume_temp = 0;
+    double goal_volume_temp = 0;
+    double total_volume, goal_volume;
 
-                            const double unfiltered_density_phi_j = fe_values[unfiltered_densities].value(j,
-                                                                                                          q_point);
-                            const double unfiltered_density_multiplier_phi_j = fe_values[unfiltered_density_multipliers].value(
-                                    j, q_point);
+    distributed_solution = state;
+    for (const auto &cell: dof_handler.active_cell_iterators()) {
+        if(cell->is_locally_owned())
+        {
+            std::vector<unsigned int> i(cell->get_fe().n_dofs_per_cell());
+            cell->get_dof_indices(i);
+            if (distributed_solution.block(SolutionBlocks::density).in_local_range(i[cell->get_fe().component_to_system_index(0, 0)]))
+            {
+                total_volume_temp += cell->measure() * state.block(SolutionBlocks::density)[i[cell->get_fe().component_to_system_index(0, 0)]];
+                goal_volume_temp += cell->measure() * Input::volume_percentage;
+            }
+        }
+    }
 
+    MPI_Allreduce(&total_volume_temp, &total_volume, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+    MPI_Allreduce(&goal_volume_temp, &goal_volume, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
 
-                            const double lower_slack_phi_j =
-                                    fe_values[density_lower_slacks].value(j, q_point);
-
-                            const double upper_slack_phi_j =
-                                    fe_values[density_upper_slacks].value(j, q_point);
-
-                            const double lower_slack_multiplier_phi_j =
-                                    fe_values[density_lower_slack_multipliers].value(j,
-                                                                                     q_point);
-
-                            const double upper_slack_multiplier_phi_j =
-                                    fe_values[density_upper_slack_multipliers].value(j,
-                                                                                     q_point);
-
-                            //Equation 0
-                            cell_matrix(i, j) +=
-                                    fe_values.JxW(q_point) *
-                                    (
-                                            -density_phi_i * unfiltered_density_multiplier_phi_j
-
-                                            - density_penalty_exponent * (density_penalty_exponent - 1)
-                                              * std::pow(
-                                                    old_density_values[q_point],
-                                                    density_penalty_exponent - 2)
-                                              * density_phi_i
-                                              * density_phi_j
-                                              * (old_displacement_multiplier_divs[q_point] * old_displacement_divs[q_point]
-                                                 * lambda_values[q_point]
-                                                 + 2 * mu_values[q_point]
-                                                   * (old_displacement_symmgrads[q_point] *
-                                                      old_displacement_multiplier_symmgrads[q_point]))
-
-                                            - density_penalty_exponent * std::pow(
-                                                    old_density_values[q_point],
-                                                    density_penalty_exponent - 1)
-                                              * density_phi_i
-                                              * (displacement_multiplier_phi_j_div * old_displacement_divs[q_point]
-                                                 * lambda_values[q_point]
-                                                 + 2 * mu_values[q_point]
-                                                   *
-                                                   (old_displacement_symmgrads[q_point] *
-                                                    displacement_multiplier_phi_j_symmgrad))
-
-                                            - density_penalty_exponent * std::pow(
-                                                    old_density_values[q_point],
-                                                    density_penalty_exponent - 1)
-                                              * density_phi_i
-                                              * (displacement_phi_j_div * old_displacement_multiplier_divs[q_point]
-                                                 * lambda_values[q_point]
-                                                 + 2 * mu_values[q_point]
-                                                   * (old_displacement_multiplier_symmgrads[q_point] *
-                                                      displacement_phi_j_symmgrad)));
-                            //Equation 1
-
-                            cell_matrix(i, j) +=
-                                    fe_values.JxW(q_point) * (
-                                            -density_penalty_exponent * std::pow(
-                                                    old_density_values[q_point],
-                                                    density_penalty_exponent - 1)
-                                            * density_phi_j
-                                            * (old_displacement_multiplier_divs[q_point] * displacement_phi_i_div
-                                               * lambda_values[q_point]
-                                               + 2 * mu_values[q_point]
-                                                 * (old_displacement_multiplier_symmgrads[q_point] *
-                                                    displacement_phi_i_symmgrad))
-
-                                            - std::pow(old_density_values[q_point],
-                                                       density_penalty_exponent)
-                                              * (displacement_multiplier_phi_j_div * displacement_phi_i_div
-                                                 * lambda_values[q_point]
-                                                 + 2 * mu_values[q_point]
-                                                   * (displacement_multiplier_phi_j_symmgrad * displacement_phi_i_symmgrad))
+    pcout << "total_volume: " << total_volume  << " and goal_volume: " << goal_volume << std::endl;
 
-                                    );
+    if (test_rhs.block(SolutionBlocks::total_volume_multiplier).in_local_range(0))
+    {
+        test_rhs.block(SolutionBlocks::total_volume_multiplier)[0] = goal_volume - total_volume;
+    }
+    test_rhs.compress(VectorOperation::insert);
 
-                            //Equation 2 has to do with the filter, which is calculated elsewhere.
-                            cell_matrix(i, j) +=
-                                    fe_values.JxW(q_point) * (
-                                            -1 * unfiltered_density_phi_i * lower_slack_multiplier_phi_j
-                                            + unfiltered_density_phi_i * upper_slack_multiplier_phi_j);
-
-                            //Equation 3 - Primal Feasibility
-
-                            cell_matrix(i, j) +=
-                                    fe_values.JxW(q_point) * (
-
-                                            -1 * density_penalty_exponent * std::pow(
-                                                    old_density_values[q_point],
-                                                    density_penalty_exponent - 1)
-                                            * density_phi_j
-                                            * (old_displacement_divs[q_point] * displacement_multiplier_phi_i_div
-                                               * lambda_values[q_point]
-                                               + 2 * mu_values[q_point]
-                                                 * (old_displacement_symmgrads[q_point] *
-                                                    displacement_multiplier_phi_i_symmgrad))
-
-                                            + -1 * std::pow(old_density_values[q_point],
-                                                            density_penalty_exponent)
-                                              * (displacement_phi_j_div * displacement_multiplier_phi_i_div
-                                                 * lambda_values[q_point]
-                                                 + 2 * mu_values[q_point]
-                                                   *
-                                                   (displacement_phi_j_symmgrad * displacement_multiplier_phi_i_symmgrad)));
-
-                            //Equation 4 - more primal feasibility
-                            cell_matrix(i, j) +=
-                                    -1 * fe_values.JxW(q_point) * lower_slack_multiplier_phi_i *
-                                    (unfiltered_density_phi_j - lower_slack_phi_j);
-
-                            //Equation 5 - more primal feasibility
-                            cell_matrix(i, j) +=
-                                    -1 * fe_values.JxW(q_point) * upper_slack_multiplier_phi_i * (
-                                            -1 * unfiltered_density_phi_j - upper_slack_phi_j);
-
-                            //Equation 6 - more primal feasibility - part with filter added later
-                            cell_matrix(i, j) +=
-                                    -1 * fe_values.JxW(q_point) * unfiltered_density_multiplier_phi_i * (
-                                            density_phi_j);
-
-                            //Equation 7 - complementary slackness
-                            cell_matrix(i, j) += fe_values.JxW(q_point) *
-                                                 (lower_slack_phi_i * lower_slack_multiplier_phi_j
-                                                  + lower_slack_phi_i * lower_slack_phi_j *
-                                                    old_lower_slack_multiplier_values[q_point] /
-                                                    old_lower_slack_values[q_point]);
-                            //Equation 8 - complementary slackness
-                            cell_matrix(i, j) += fe_values.JxW(q_point) *
-                                                 (upper_slack_phi_i * upper_slack_multiplier_phi_j
-                                                  + upper_slack_phi_i * upper_slack_phi_j *
-                                                    old_upper_slack_multiplier_values[q_point] /
-                                                    old_upper_slack_values[q_point]);
-                        }
+    return test_rhs;
 
-                    }
-                }
+}
 
 
-                MatrixTools::local_apply_boundary_values(boundary_values, local_dof_indices,
-                                                         cell_matrix, cell_rhs, true);
+template<int dim>
+LA::MPI::BlockVector
+KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
+    double gmres_tolerance;
+    if (Input::use_eisenstat_walker) {
+        gmres_tolerance = std::max(
+                    std::min(
+                        .1 * system_rhs.l2_norm() / (initial_rhs_error),
+                        .001
+                        ),
+                    Input::default_gmres_tolerance);
+    }
+    else {
+        gmres_tolerance = Input::default_gmres_tolerance;
+    }
+    locally_relevant_solution=state;
+    distributed_solution = state;
+    SolverControl solver_control(10, 1e-4);
 
+    using SystemMFMatrixType = MF_Elasticity_Operator<dim, 1, double>;
+    using LevelMFMatrixType = MF_Elasticity_Operator<dim, 1, double>;
 
-                constraints.distribute_local_to_global(
-                        cell_matrix, cell_rhs, local_dof_indices, system_matrix, system_rhs);
 
-            }
+    elasticity_system_mf.clear();
+    mg_matrices.clear_elements();
+    dof_handler_displacement.distribute_mg_dofs();
+    pcout << "Number of degrees of freedom: " << dof_handler_displacement.n_dofs()
+          << std::endl;
+    IndexSet locally_relevant_dofs;
+    DoFTools::extract_locally_relevant_dofs(dof_handler_displacement, locally_relevant_dofs);
 
-        }
-        pcout << "compress 1" << std::endl;
-        system_matrix.compress(VectorOperation::add);
-        system_rhs = calculate_rhs(distributed_state, barrier_size);
+    //        constraints.clear();
+    //        constraints.reinit(locally_relevant_dofs);
+    //        DoFTools::make_hanging_node_constraints(dof_handler_displacement, constraints);
+    //        VectorTools::interpolate_boundary_values(dof_handler_displacement, 0, Functions::ZeroFunction<dim>(), constraints);
+    //        constraints.close();
 
-        for (const auto &cell: dof_handler.active_cell_iterators()) {
-            if(cell->is_locally_owned())
-            {
-                std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
-                cell->get_dof_indices(i);
 
-                typename LA::MPI::SparseMatrix::iterator iter = density_filter.filter_matrix.begin(
-                        i[cell->get_fe().component_to_system_index(0, 0)]);
-                for (; iter != density_filter.filter_matrix.end(i[cell->get_fe().component_to_system_index(0, 0)]); iter++) {
-                    unsigned int j = iter->column();
-                    double value = iter->value() * cell->measure();
-
-                    system_matrix.block(SolutionBlocks::unfiltered_density_multiplier,
-                                        SolutionBlocks::unfiltered_density).add(i[cell->get_fe().component_to_system_index(0, 0)], j, value);
-                    system_matrix.block(SolutionBlocks::unfiltered_density,
-                                        SolutionBlocks::unfiltered_density_multiplier).add(j, i[cell->get_fe().component_to_system_index(0, 0)], value);
-                }
+    {
+        typename MatrixFree<dim, double>::AdditionalData additional_data;
+        additional_data.tasks_parallel_scheme =
+                MatrixFree<dim, double>::AdditionalData::none;
+        additional_data.mapping_update_flags =
+                (update_gradients | update_JxW_values | update_quadrature_points);
+        std::shared_ptr<MatrixFree<dim, double>> system_mf_storage(
+                    new MatrixFree<dim, double>());
+        system_mf_storage->reinit(dof_handler_displacement,
+                                  constraints,
+                                  QGauss<1>(fe_displacement.degree + 1),
+                                  additional_data);
+        elasticity_system_mf.initialize(system_mf_storage);
+    }
 
-                system_matrix.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).add(0, i[cell->get_fe().component_to_system_index(0, 0)],
-                                                                                                          cell->measure());
 
-                system_matrix.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier).add(i[cell->get_fe().component_to_system_index(0, 0)], 0,
-                                                                                                          cell->measure());
-            }
-        }
-        pcout << "compress 2" << std::endl;
-        system_matrix.compress(VectorOperation::add);
+    LinearAlgebra::distributed::Vector<double> distributed_displacement_sol;
+    LinearAlgebra::distributed::Vector<double> distributed_displacement_rhs;
 
+    elasticity_system_mf.initialize_dof_vector(distributed_displacement_sol);
+    elasticity_system_mf.initialize_dof_vector(distributed_displacement_rhs);
 
-        pcout << "assembled" << std::endl;
+    ChangeVectorTypes::copy(distributed_displacement_sol,distributed_solution.block(SolutionBlocks::displacement));
+    ChangeVectorTypes::copy(distributed_displacement_rhs,system_rhs.block(SolutionBlocks::displacement));
 
+    const unsigned int n_levels = triangulation.n_global_levels();
+    mg_matrices.resize(0, n_levels - 1);
+    mg_constrained_dofs.initialize(dof_handler_displacement);
+    mg_constrained_dofs.make_zero_boundary_constraints(dof_handler_displacement,
+                                                           dirichlet_boundary);
+
+    for (unsigned int level = 0; level < n_levels; ++level)
+    {
+        IndexSet relevant_dofs;
+        DoFTools::extract_locally_relevant_level_dofs(dof_handler_displacement,
+                                                      level,
+                                                      relevant_dofs);
+        AffineConstraints<double> level_constraints;
+            level_constraints.reinit(relevant_dofs);
+            level_constraints.add_lines( mg_constrained_dofs.get_boundary_indices(level));
+        level_constraints.close();
+        typename MatrixFree<dim, double>::AdditionalData additional_data;
+        additional_data.tasks_parallel_scheme =
+                MatrixFree<dim, double>::AdditionalData::none;
+        additional_data.mapping_update_flags =
+                (update_gradients | update_JxW_values | update_quadrature_points);
+        additional_data.mg_level = level;
+        std::shared_ptr<MatrixFree<dim, double>> mg_mf_storage_level(
+                    new MatrixFree<dim, double>());
+        mg_mf_storage_level->reinit(dof_handler_displacement,
+                                    level_constraints,
+                                    QGauss<1>(fe_displacement.degree + 1),
+                                    additional_data);
+        mg_matrices[level].clear();
+        mg_matrices[level].initialize(mg_mf_storage_level,
+                                      mg_constrained_dofs,
+                                      level);
     }
 
-    template<int dim>
-    double
-    KktSystem<dim>::calculate_objective_value(const LA::MPI::BlockVector &distributed_state) const {
-        /*Remove any values from old iterations*/
 
-        locally_relevant_solution = distributed_state;
+    //+++++++++++++++++++++++++EVALUATE MATRIX LEVEL DENSITIES HERE +++++++++++++++++++++++++++++++++++++
+
+
+    dof_handler_density.distribute_dofs(fe_density);
 
+    DoFRenumbering::component_wise(dof_handler_density);
+
+    DoFRenumbering::hierarchical(dof_handler_density);
+
+    dof_handler_density.distribute_mg_dofs();
+
+    /*OperatorCellData<dim, GMGNumberType> active_cell_data;
+    MGLevelObject<OperatorCellData<dim, GMGNumberType>> level_cell_data;
+    dealii::LinearAlgebra::distributed::Vector<double> */
+    active_density_vector.reinit(dof_handler_density.locally_owned_dofs(),triangulation.get_communicator());
+
+
+//    elasticity_system_mf.initialize_dof_vector(active_density_vector);
+
+    ChangeVectorTypes::copy(active_density_vector,distributed_solution.block(SolutionBlocks::density));
+
+    const unsigned int n_cells = elasticity_system_mf.get_matrix_free()->n_cell_batches();
+    {
+        active_cell_data.density.reinit(TableIndices<2>(n_cells, 1));
 
-        QGauss<dim> nine_quadrature(fe_nine.degree + 1);
-        QGauss<dim> ten_quadrature(fe_ten.degree + 1);
+        QGauss<dim> nine_quadrature(2);
+        QGauss<dim> ten_quadrature(2);
 
         hp::QCollection<dim> q_collection;
         q_collection.push_back(nine_quadrature);
@@ -1189,847 +2190,469 @@ namespace SAND {
                                        update_values | update_quadrature_points |
                                        update_JxW_values | update_gradients);
 
-        QGauss<dim - 1> common_face_quadrature(fe_ten.degree + 1);
 
-        FEFaceValues<dim> fe_nine_face_values(fe_nine,
-                                              common_face_quadrature,
-                                              update_JxW_values |
-                                              update_gradients | update_values);
-        FEFaceValues<dim> fe_ten_face_values(fe_ten,
-                                             common_face_quadrature,
-                                             update_normal_vectors |
-                                             update_values);
 
-        FullMatrix<double> cell_matrix;
-        Vector<double> cell_rhs;
-
-        const FEValuesExtractors::Vector displacements(SolutionComponents::displacement<dim>);
-
-        Tensor<1, dim> traction;
-        traction[1] = -1;
-        distributed_solution = distributed_state;
-        double objective_value = 0;
-        for (const auto &cell: dof_handler.active_cell_iterators())
-        {
-            if(cell->is_locally_owned())
+        for (const auto &cell : dof_handler.active_cell_iterators())
+            if (cell->is_locally_owned())
             {
+                // For each cell, create a local mass matrix and rhs.
+
                 hp_fe_values.reinit(cell);
                 const FEValues<dim> &fe_values = hp_fe_values.get_present_fe_values();
-                const unsigned int dofs_per_cell = cell->get_fe().n_dofs_per_cell();
+
+                const unsigned int dofs_per_cell = fe_values.dofs_per_cell;
                 const unsigned int n_q_points = fe_values.n_quadrature_points;
-                const unsigned int n_face_q_points = common_face_quadrature.size();
 
-                std::vector<Tensor<1, dim>> old_displacement_values(n_q_points);
-                fe_values[displacements].get_function_values(
-                        locally_relevant_solution, old_displacement_values);
+                std::vector<types::global_dof_index> local_dof_indices (dofs_per_cell);
+                cell->get_dof_indices (local_dof_indices);
+                Vector<double> cell_vector (dofs_per_cell);
+                Vector<double> local_projection (dofs_per_cell);
+                FullMatrix<double> local_mass_matrix (dofs_per_cell, dofs_per_cell);
+
+                std::vector<double> rhs_values(n_q_points);
+                std::vector<double> old_density_values(n_q_points);
+
+                const FEValuesExtractors::Scalar densities(SolutionComponents::density<dim>);
 
-                for (unsigned int face_number = 0;
-                     face_number < GeometryInfo<dim>::faces_per_cell;
-                     ++face_number)
+                fe_values[densities].get_function_values(distributed_solution, old_density_values);
+                double cell_density = old_density_values[0];
+
+                for (unsigned int i=0; i<rhs_values.size(); ++i)
                 {
-                    if (cell->face(face_number)->at_boundary() && cell->face(face_number)->boundary_id()
-                                                                  == BoundaryIds::down_force)
-                    {
-                        for (unsigned int face_q_point = 0;
-                             face_q_point < n_face_q_points; ++face_q_point) {
-                            for (unsigned int i = 0; i < dofs_per_cell; ++i) {
-                                if (cell->material_id() == MaterialIds::without_multiplier) {
-                                    fe_nine_face_values.reinit(cell, face_number);
-                                    objective_value += traction
-                                                       * fe_nine_face_values[displacements].value(i,
-                                                                                                  face_q_point)
-                                                       * fe_nine_face_values.JxW(face_q_point);
-                                } else {
-                                    fe_ten_face_values.reinit(cell, face_number);
-                                    objective_value += traction
-                                                       * fe_ten_face_values[displacements].value(i,
-                                                                                                 face_q_point)
-                                                       * fe_ten_face_values.JxW(face_q_point);
-                                }
-                            }
-                        }
-                    }
+                    rhs_values[i] = cell_density;
                 }
-            }
-        }
-        double objective_value_out;
-        MPI_Allreduce(&objective_value, &objective_value_out, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
 
-        pcout << "objective value: " << objective_value_out << std::endl;
-        return objective_value;
-    }
+                local_projection = cell_density;
 
+                std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
+                cell->get_dof_indices(i);
+                const unsigned int i_val = i[cell->get_fe().component_to_system_index(0, 0)];
+                active_density_vector[i_val] = cell_density;
 
-    //As the KKT System know which vectors correspond to the slack variables, the sum of the logs of the slacks is computed here for use in the filter.
-    template<int dim>
-    double
-    KktSystem<dim>::calculate_barrier_distance(const LA::MPI::BlockVector &state) const {
-        double barrier_distance_log_sum = 0;
-        unsigned int vect_size = state.block(SolutionBlocks::density_lower_slack).size();
-        distributed_solution = state;
-        for (unsigned int k = 0; k < vect_size; k++) {
-            if (distributed_solution.block(SolutionBlocks::density_lower_slack).in_local_range(k))
-            barrier_distance_log_sum += std::log(state.block(SolutionBlocks::density_lower_slack)[k]);
-        }
-        for (unsigned int k = 0; k < vect_size; k++) {
-            if (distributed_solution.block(SolutionBlocks::density_upper_slack).in_local_range(k))
-            barrier_distance_log_sum += std::log(state.block(SolutionBlocks::density_upper_slack)[k]);
-        }
-        double out_barrier_distance_log_sum;
-        MPI_Allreduce(&barrier_distance_log_sum, &out_barrier_distance_log_sum, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
 
-        pcout << "Barrier distance log sum: " << out_barrier_distance_log_sum << std::endl;
+            }
 
-        return out_barrier_distance_log_sum;
+        active_density_vector.compress(VectorOperation::insert);
     }
 
-    template<int dim>
-    double
-    KktSystem<dim>::calculate_rhs_norm(const LA::MPI::BlockVector &state, const double barrier_size) const {
-        return calculate_rhs(state, barrier_size).l2_norm();
-    }
 
 
-    //Feasibility conditions appear on the RHS of the linear system, so I compute the RHS to find it. Could probably be combined with the objective value finding part to make it faster.
-    template<int dim>
-    double
-    KktSystem<dim>::calculate_feasibility(const LA::MPI::BlockVector &state, const double barrier_size) const {
-       LA::MPI::BlockVector test_rhs = calculate_rhs(state, barrier_size);
-
-       double norm = 0;
-
-       distributed_solution = state;
-       double full_prod1 =1;
-       double full_prod2 = 1;
-
-       for (unsigned int k = 0; k < state.block(SolutionBlocks::density_upper_slack).size(); k++) {
-           double prod1 = 1;
-           double prod2 = 1;
-           if(state.block(SolutionBlocks::density_upper_slack).in_local_range(k))
-           {
-               prod1 = prod1 * state.block(SolutionBlocks::density_upper_slack)[k]
-                       * state.block(SolutionBlocks::density_upper_slack)[k];
-           }
-           if(state.block(SolutionBlocks::density_lower_slack).in_local_range(k))
-           {
-              prod2 = prod2 *  state.block(SolutionBlocks::density_lower_slack)[k]
-                      * state.block(SolutionBlocks::density_lower_slack)[k];
-           }
-           if(state.block(SolutionBlocks::density_upper_slack_multiplier).in_local_range(k))
-           {
-               prod1 = prod1 * state.block(SolutionBlocks::density_upper_slack_multiplier)[k]
-                       * state.block(SolutionBlocks::density_upper_slack_multiplier)[k];
-           }
-           if(state.block(SolutionBlocks::density_lower_slack_multiplier).in_local_range(k))
-           {
-               prod2 = prod2 *  state.block(SolutionBlocks::density_lower_slack_multiplier)[k]
-                       * state.block(SolutionBlocks::density_lower_slack_multiplier)[k];
-           }
-           MPI_Allreduce(&prod1, &full_prod1, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
-           MPI_Allreduce(&prod2, &full_prod2, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
-           norm = norm + full_prod1 + full_prod2;
-       }
-       pcout << "pre-norm: " << norm << std::endl;
-
-        norm += std::pow(test_rhs.block(SolutionBlocks::displacement).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::density).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::unfiltered_density).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::displacement_multiplier).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::total_volume_multiplier).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::density_upper_slack_multiplier).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::density_lower_slack_multiplier).l2_norm(), 2);
-
-        pcout << "norm: " << norm << std::endl;
-
-        return norm;
-    }
 
-    template<int dim>
-    double
-    KktSystem<dim>::calculate_convergence(const LA::MPI::BlockVector &state) const {
-        LA::MPI::BlockVector test_rhs = calculate_rhs(state, Input::min_barrier_size);
-        double norm = 0;
+    // MAKE ACTIVE_CELL_DATA
+    std::vector<types::global_dof_index> local_dof_indices(fe_density.dofs_per_cell);
+    for (unsigned int cell=0; cell<n_cells; ++cell)
+    {
+        const unsigned int n_components_filled = elasticity_system_mf.get_matrix_free()->n_active_entries_per_cell_batch(cell);
 
-        distributed_solution = state;
-        double full_prod1 =1;
-        double full_prod2 = 1;
+        for (unsigned int i=0; i<n_components_filled; ++i)
+        {
+            typename DoFHandler<dim>::active_cell_iterator FEQ_cell =elasticity_system_mf.get_matrix_free()->get_cell_iterator(cell,i);
+            typename DoFHandler<dim>::active_cell_iterator DG_cell(&(triangulation),
+                                                                   FEQ_cell->level(),
+                                                                   FEQ_cell->index(),
+                                                                   &dof_handler_density);
+
+            DG_cell->get_active_or_mg_dof_indices(local_dof_indices);
+
+            active_cell_data.density(cell, 0)[i] = active_density_vector(local_dof_indices[0]);
 
-        for (unsigned int k = 0; k < state.block(SolutionBlocks::density_upper_slack).size(); k++) {
-            double prod1 = 1;
-            double prod2 = 1;
-            if(state.block(SolutionBlocks::density_upper_slack).in_local_range(k))
-            {
-                prod1 = prod1 * state.block(SolutionBlocks::density_upper_slack)[k]
-                        * state.block(SolutionBlocks::density_upper_slack)[k];
-            }
-            if(state.block(SolutionBlocks::density_lower_slack).in_local_range(k))
-            {
-               prod2 = prod2 *  state.block(SolutionBlocks::density_lower_slack)[k]
-                       * state.block(SolutionBlocks::density_lower_slack)[k];
-            }
-            if(state.block(SolutionBlocks::density_upper_slack_multiplier).in_local_range(k))
-            {
-                prod1 = prod1 * state.block(SolutionBlocks::density_upper_slack_multiplier)[k]
-                        * state.block(SolutionBlocks::density_upper_slack_multiplier)[k];
-            }
-            if(state.block(SolutionBlocks::density_lower_slack_multiplier).in_local_range(k))
-            {
-                prod2 = prod2 *  state.block(SolutionBlocks::density_lower_slack_multiplier)[k]
-                        * state.block(SolutionBlocks::density_lower_slack_multiplier)[k];
-            }
-            MPI_Allreduce(&prod1, &full_prod1, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
-            MPI_Allreduce(&prod2, &full_prod2, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
-            norm = norm + full_prod1 + full_prod2;
         }
-        pcout << "pre-norm: " << norm << std::endl;
-
-        norm += std::pow(test_rhs.block(SolutionBlocks::displacement).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::density).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::unfiltered_density).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::displacement_multiplier).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::total_volume_multiplier).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::density_upper_slack_multiplier).l2_norm(), 2);
-        norm += std::pow(test_rhs.block(SolutionBlocks::density_lower_slack_multiplier).l2_norm(), 2);
-
-        norm = std::pow(norm, .5);
-
-        pcout << "l2 norm: " << system_rhs.l2_norm() << std::endl;
-        pcout << "KKT norm: " << norm << std::endl;
-        return norm;
     }
 
-    template<int dim>
-    LA::MPI::BlockVector
-    KktSystem<dim>::calculate_rhs(const LA::MPI::BlockVector &distributed_state, const double barrier_size) const {
-        LA::MPI::BlockVector test_rhs (system_rhs);
-        LA::MPI::BlockVector state (locally_relevant_solution);
-        state = distributed_state;
-        test_rhs = 0;
+    //MAKE LEVEL DENSITY VECTOR
 
+    level_cell_data.resize(0,n_levels-1);
+    level_density_vector = 0.;
+    level_density_vector.resize(0,n_levels-1);
 
-        QGauss<dim> nine_quadrature(fe_nine.degree + 1);
-        QGauss<dim> ten_quadrature(fe_ten.degree + 1);
+    transfer.build(dof_handler_density);
 
-        hp::QCollection<dim> q_collection;
-        q_collection.push_back(nine_quadrature);
-        q_collection.push_back(ten_quadrature);
+    std::cout << "level_density_size: " << level_density_vector[0].locally_owned_size() << std::endl;
+    std::cout << "active_density_size: " << active_density_vector.locally_owned_size() << std::endl;
 
-        hp::FEValues<dim> hp_fe_values(fe_collection,
-                                       q_collection,
-                                       update_values | update_quadrature_points |
-                                       update_JxW_values | update_gradients);
 
-        QGauss<dim - 1> common_face_quadrature(fe_ten.degree + 1);
-
-        FEFaceValues<dim> fe_nine_face_values(fe_nine,
-                                              common_face_quadrature,
-                                              update_JxW_values |
-                                              update_gradients | update_values);
-        FEFaceValues<dim> fe_ten_face_values(fe_ten,
-                                             common_face_quadrature,
-                                             update_normal_vectors |
-                                             update_values);
-
-        FullMatrix<double> cell_matrix;
-        Vector<double> cell_rhs;
-        std::vector<types::global_dof_index> local_dof_indices;
-
-        const FEValuesExtractors::Scalar densities(SolutionComponents::density<dim>);
-        const FEValuesExtractors::Vector displacements(SolutionComponents::displacement<dim>);
-        const FEValuesExtractors::Scalar unfiltered_densities(SolutionComponents::unfiltered_density<dim>);
-        const FEValuesExtractors::Vector displacement_multipliers(SolutionComponents::displacement_multiplier<dim>);
-        const FEValuesExtractors::Scalar unfiltered_density_multipliers(
-                SolutionComponents::unfiltered_density_multiplier<dim>);
-        const FEValuesExtractors::Scalar density_lower_slacks(SolutionComponents::density_lower_slack<dim>);
-        const FEValuesExtractors::Scalar density_lower_slack_multipliers(
-                SolutionComponents::density_lower_slack_multiplier<dim>);
-        const FEValuesExtractors::Scalar density_upper_slacks(SolutionComponents::density_upper_slack<dim>);
-        const FEValuesExtractors::Scalar density_upper_slack_multipliers(
-                SolutionComponents::density_upper_slack_multiplier<dim>);
-        const FEValuesExtractors::Scalar total_volume_multiplier(
-                SolutionComponents::total_volume_multiplier<dim>);
+    transfer.template interpolate_to_mg<double>(dof_handler_density,
+                                                level_density_vector,
+                                                active_density_vector);
 
 
-        const unsigned int n_face_q_points = common_face_quadrature.size();
 
-        const Functions::ConstantFunction<dim> lambda(1.), mu(1.);
 
-        locally_relevant_solution = state;
-        distributed_solution = state;
-        LA::MPI::BlockVector filtered_unfiltered_density_solution (distributed_solution);
-        LA::MPI::BlockVector filter_adjoint_unfiltered_density_multiplier_solution (distributed_solution);
-        filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = 0;
-        filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier) = 0;
 
-        auto op_f = linear_operator<LA::MPI::Vector>(density_filter.filter_matrix);
-        filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = op_f * distributed_solution.block(SolutionBlocks::unfiltered_density);
-        filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier)
-                                                    = transpose_operator(op_f) * distributed_solution.block(SolutionBlocks::unfiltered_density_multiplier);
-        LA::MPI::BlockVector relevant_filtered_unfiltered_density_solution (locally_relevant_solution);
-        LA::MPI::BlockVector relevant_filter_adjoint_unfiltered_density_multiplier_solution (locally_relevant_solution);
-        relevant_filtered_unfiltered_density_solution = filtered_unfiltered_density_solution;
-        relevant_filter_adjoint_unfiltered_density_multiplier_solution = filter_adjoint_unfiltered_density_multiplier_solution;
 
-        double old_volume_multiplier_temp = 0;
-        double old_volume_multiplier;
-        if(distributed_state.block(SolutionBlocks::total_volume_multiplier).in_local_range(0))
-        {
-            old_volume_multiplier_temp = state.block(SolutionBlocks::total_volume_multiplier)[0];
-        }
-        MPI_Allreduce(&old_volume_multiplier_temp, &old_volume_multiplier, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+    // MAKE LEVEL_CELL_DATA
+    for (unsigned int level=0; level<n_levels; ++level)
+    {
+        const unsigned int n_cells = mg_matrices[level].get_matrix_free()->n_cell_batches();
 
-        for (const auto &cell: dof_handler.active_cell_iterators()) {
-            if(cell->is_locally_owned())
+        level_cell_data[level].density.reinit(TableIndices<2>(n_cells, 1));
+        for (unsigned int cell=0; cell<n_cells; ++cell)
+        {
+            const unsigned int n_components_filled = mg_matrices[level].get_matrix_free()->n_active_entries_per_cell_batch(cell);
+            for (unsigned int i=0; i<n_components_filled; ++i)
             {
-                hp_fe_values.reinit(cell);
-                const FEValues<dim> &fe_values = hp_fe_values.get_present_fe_values();
-                cell_matrix.reinit(cell->get_fe().n_dofs_per_cell(),
-                                   cell->get_fe().n_dofs_per_cell());
-                cell_rhs.reinit(cell->get_fe().n_dofs_per_cell());
-
-                const unsigned int n_q_points = fe_values.n_quadrature_points;
-
-                std::vector<double> old_density_values(n_q_points);
-                std::vector<Tensor<1, dim>> old_displacement_values(n_q_points);
-                std::vector<double> old_displacement_divs(n_q_points);
-                std::vector<SymmetricTensor<2, dim>> old_displacement_symmgrads(
-                        n_q_points);
-                std::vector<Tensor<1, dim>> old_displacement_multiplier_values(
-                        n_q_points);
-                std::vector<double> old_displacement_multiplier_divs(n_q_points);
-                std::vector<SymmetricTensor<2, dim>> old_displacement_multiplier_symmgrads(
-                        n_q_points);
-                std::vector<double> old_lower_slack_multiplier_values(n_q_points);
-                std::vector<double> old_upper_slack_multiplier_values(n_q_points);
-                std::vector<double> old_lower_slack_values(n_q_points);
-                std::vector<double> old_upper_slack_values(n_q_points);
-                std::vector<double> old_unfiltered_density_values(n_q_points);
-                std::vector<double> old_unfiltered_density_multiplier_values(n_q_points);
-                std::vector<double> filtered_unfiltered_density_values(n_q_points);
-                std::vector<double> filter_adjoint_unfiltered_density_multiplier_values(n_q_points);
-                std::vector<double> lambda_values(n_q_points);
-                std::vector<double> mu_values(n_q_points);
-
-                const unsigned int dofs_per_cell = cell->get_fe().n_dofs_per_cell();
-
-                cell_matrix = 0;
-                cell_rhs = 0;
-                local_dof_indices.resize(cell->get_fe().n_dofs_per_cell());
-
-                cell->get_dof_indices(local_dof_indices);
-
-                lambda.value_list(fe_values.get_quadrature_points(), lambda_values);
-                mu.value_list(fe_values.get_quadrature_points(), mu_values);
-
-                fe_values[densities].get_function_values(state,
-                                                         old_density_values);
-                fe_values[displacements].get_function_values(state,
-                                                             old_displacement_values);
-                fe_values[displacements].get_function_divergences(state,
-                                                                  old_displacement_divs);
-                fe_values[displacements].get_function_symmetric_gradients(
-                        state, old_displacement_symmgrads);
-                fe_values[displacement_multipliers].get_function_values(
-                        state, old_displacement_multiplier_values);
-                fe_values[displacement_multipliers].get_function_divergences(
-                        state, old_displacement_multiplier_divs);
-                fe_values[displacement_multipliers].get_function_symmetric_gradients(
-                        state, old_displacement_multiplier_symmgrads);
-                fe_values[density_lower_slacks].get_function_values(
-                        state, old_lower_slack_values);
-                fe_values[density_lower_slack_multipliers].get_function_values(
-                        state, old_lower_slack_multiplier_values);
-                fe_values[density_upper_slacks].get_function_values(
-                        state, old_upper_slack_values);
-                fe_values[density_upper_slack_multipliers].get_function_values(
-                        state, old_upper_slack_multiplier_values);
-                fe_values[unfiltered_densities].get_function_values(
-                        state, old_unfiltered_density_values);
-                fe_values[unfiltered_density_multipliers].get_function_values(
-                        state, old_unfiltered_density_multiplier_values);
-                fe_values[unfiltered_densities].get_function_values(
-                        relevant_filtered_unfiltered_density_solution, filtered_unfiltered_density_values);
-                fe_values[unfiltered_density_multipliers].get_function_values(
-                        relevant_filter_adjoint_unfiltered_density_multiplier_solution,
-                        filter_adjoint_unfiltered_density_multiplier_values);
-
-
-                Tensor<1, dim> traction;
-                traction[1] = -1;
 
-                for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
+                typename DoFHandler<dim>::level_cell_iterator FEQ_cell = mg_matrices[level].get_matrix_free()->get_cell_iterator(cell,i);
+                typename DoFHandler<dim>::level_cell_iterator DG_cell(&(triangulation),
+                                                                      FEQ_cell->level(),
+                                                                      FEQ_cell->index(),
+                                                                      &dof_handler_density);
+                DG_cell->get_active_or_mg_dof_indices(local_dof_indices);
 
-                    for (unsigned int i = 0; i < dofs_per_cell; ++i) {
-                        const SymmetricTensor<2, dim> displacement_phi_i_symmgrad =
-                                fe_values[displacements].symmetric_gradient(i, q_point);
-                        const double displacement_phi_i_div =
-                                fe_values[displacements].divergence(i, q_point);
 
-                        const SymmetricTensor<2, dim> displacement_multiplier_phi_i_symmgrad =
-                                fe_values[displacement_multipliers].symmetric_gradient(i,
-                                                                                       q_point);
-                        const double displacement_multiplier_phi_i_div =
-                                fe_values[displacement_multipliers].divergence(i,
-                                                                               q_point);
+                level_cell_data[level].density(cell, 0)[i] = level_density_vector[level](local_dof_indices[0]);
 
+            }
 
-                        const double density_phi_i = fe_values[densities].value(i,
-                                                                                q_point);
-                        const double unfiltered_density_phi_i = fe_values[unfiltered_densities].value(i,
-                                                                                                      q_point);
-                        const double unfiltered_density_multiplier_phi_i = fe_values[unfiltered_density_multipliers].value(
-                                i, q_point);
+        }
 
-                        const double lower_slack_multiplier_phi_i =
-                                fe_values[density_lower_slack_multipliers].value(i,
-                                                                                 q_point);
+        // Store density tables and other data into the multigrid level matrix-free objects.
 
-                        const double lower_slack_phi_i =
-                                fe_values[density_lower_slacks].value(i, q_point);
+        mg_matrices[level].set_cell_data (level_cell_data[level]);
 
-                        const double upper_slack_phi_i =
-                                fe_values[density_upper_slacks].value(i, q_point);
 
-                        const double upper_slack_multiplier_phi_i =
-                                fe_values[density_upper_slack_multipliers].value(i,
-                                                                                 q_point);
+    }
 
+    elasticity_system_mf.set_cell_data(active_cell_data);
 
-                        //rhs eqn 0
-                        cell_rhs(i) +=
-                                -1 * fe_values.JxW(q_point) * (
-                                        -1 * density_penalty_exponent *
-                                        std::pow(old_density_values[q_point], density_penalty_exponent - 1) * density_phi_i
-                                        * (old_displacement_multiplier_divs[q_point] * old_displacement_divs[q_point]
-                                           * lambda_values[q_point]
-                                           + 2 * mu_values[q_point] * (old_displacement_symmgrads[q_point]
-                                                                       * old_displacement_multiplier_symmgrads[q_point]))
-                                        - density_phi_i * old_unfiltered_density_multiplier_values[q_point]
-                                        + old_volume_multiplier * density_phi_i
-                                );
 
-                        //rhs eqn 1 - boundary terms counted later
-                        cell_rhs(i) +=
-                                -1 * fe_values.JxW(q_point) * (
-                                        -1 * std::pow(old_density_values[q_point], density_penalty_exponent)
-                                        * (old_displacement_multiplier_divs[q_point] * displacement_phi_i_div
-                                           * lambda_values[q_point]
-                                           + 2 * mu_values[q_point] * (old_displacement_multiplier_symmgrads[q_point]
-                                                                       * displacement_phi_i_symmgrad))
-                                );
 
-                        //rhs eqn 2
-                        cell_rhs(i) +=
-                                -1 * fe_values.JxW(q_point) * (
-                                        unfiltered_density_phi_i *
-                                        filter_adjoint_unfiltered_density_multiplier_values[q_point]
-                                        + unfiltered_density_phi_i * old_upper_slack_multiplier_values[q_point]
-                                        + -1 * unfiltered_density_phi_i * old_lower_slack_multiplier_values[q_point]
-                                );
 
 
+    //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
 
-                        //rhs eqn 3 - boundary terms counted later
-                        cell_rhs(i) +=
-                                -1 * fe_values.JxW(q_point) * (
-                                        -1 * std::pow(old_density_values[q_point], density_penalty_exponent)
-                                        * (old_displacement_divs[q_point] * displacement_multiplier_phi_i_div
-                                           * lambda_values[q_point]
-                                           + 2 * mu_values[q_point] * (displacement_multiplier_phi_i_symmgrad
-                                                                       * old_displacement_symmgrads[q_point]))
-                                );
+    mg_transfer.initialize_constraints(mg_constrained_dofs);
+    mg_transfer.build(dof_handler_displacement);
 
-                        //rhs eqn 4
-                        cell_rhs(i) +=
-                                -1 * fe_values.JxW(q_point) *
-                                (-1 * lower_slack_multiplier_phi_i
-                                 * (old_unfiltered_density_values[q_point] - old_lower_slack_values[q_point])
-                                );
+    smoother_data.resize(0, triangulation.n_global_levels() - 1);
 
-                        //rhs eqn 5
-                        cell_rhs(i) +=
-                                -1 * fe_values.JxW(q_point) * (
-                                        -1 * upper_slack_multiplier_phi_i
-                                        * (1 - old_unfiltered_density_values[q_point]
-                                           - old_upper_slack_values[q_point]));
-
-                        //rhs eqn 6
-                        cell_rhs(i) +=
-                                -1 * fe_values.JxW(q_point) * (
-                                        -1 * unfiltered_density_multiplier_phi_i
-                                        * (old_density_values[q_point] - filtered_unfiltered_density_values[q_point])
-                                );
+    for (unsigned int level = 0; level < triangulation.n_global_levels();
+         ++level)
+    {
+        if (level > 0)
+        {
+            smoother_data[level].smoothing_range     = 15.;
+            smoother_data[level].degree              = 5;
+            smoother_data[level].eig_cg_n_iterations = 10;
+        }
+        else
+        {
+            smoother_data[0].smoothing_range = 1e-3;
+            smoother_data[0].degree          = numbers::invalid_unsigned_int;
+            smoother_data[0].eig_cg_n_iterations = mg_matrices[0].m();
+        }
+        mg_matrices[level].compute_diagonal();
+        smoother_data[level].preconditioner =
+                mg_matrices[level].get_matrix_diagonal_inverse();
+    }
 
-                        //rhs eqn 7
-                        cell_rhs(i) +=
-                                -1 * fe_values.JxW(q_point) *
-                                (lower_slack_phi_i *
-                                 (old_lower_slack_multiplier_values[q_point] -
-                                  barrier_size / old_lower_slack_values[q_point]));
+    mg_smoother.initialize(mg_matrices, smoother_data);
 
-                        //rhs eqn 8
-                        cell_rhs(i) +=
-                                -1 * fe_values.JxW(q_point) *
-                                (upper_slack_phi_i *
-                                 (old_upper_slack_multiplier_values[q_point] -
-                                  barrier_size / old_upper_slack_values[q_point]));
+    mg_coarse.initialize(mg_smoother);
+    mg::Matrix<LinearAlgebra::distributed::Vector<double>> mg_matrix(mg_matrices);
 
-                    }
+    mg_interface_matrices.resize(0, triangulation.n_global_levels() - 1);
+    for (unsigned int level = 0; level < triangulation.n_global_levels();
+         ++level)
+    {
+        mg_interface_matrices[level].initialize(mg_matrices[level]);
+    }
+    mg::Matrix<LinearAlgebra::distributed::Vector<double>> mg_interface(mg_interface_matrices);
 
-                }
+    Multigrid<LinearAlgebra::distributed::Vector<double>> mg(
+                mg_matrix, mg_coarse, mg_transfer, mg_smoother, mg_smoother);
+    mg.set_edge_matrices(mg_interface, mg_interface);
 
-                for (unsigned int face_number = 0;
-                     face_number < GeometryInfo<dim>::faces_per_cell;
-                     ++face_number) {
-                    if (cell->face(face_number)->at_boundary() && cell->face(
-                            face_number)->boundary_id() == BoundaryIds::down_force) {
-                        for (unsigned int face_q_point = 0;
-                             face_q_point < n_face_q_points; ++face_q_point) {
-                            for (unsigned int i = 0; i < dofs_per_cell; ++i) {
-                                if (cell->material_id() == MaterialIds::without_multiplier) {
-                                    fe_nine_face_values.reinit(cell, face_number);
-                                    cell_rhs(i) += -1
-                                                   * traction
-                                                   * fe_nine_face_values[displacements].value(i,
-                                                                                              face_q_point)
-                                                   * fe_nine_face_values.JxW(face_q_point);
-
-                                    cell_rhs(i) += -1 * traction
-                                                   * fe_nine_face_values[displacement_multipliers].value(
-                                            i, face_q_point)
-                                                   * fe_nine_face_values.JxW(face_q_point);
-                                } else {
-                                    fe_ten_face_values.reinit(cell, face_number);
-                                    cell_rhs(i) += -1
-                                                   * traction
-                                                   * fe_ten_face_values[displacements].value(i,
-                                                                                             face_q_point)
-                                                   * fe_ten_face_values.JxW(face_q_point);
-
-                                    cell_rhs(i) += -1 * traction
-                                                   * fe_ten_face_values[displacement_multipliers].value(
-                                            i, face_q_point)
-                                                   * fe_ten_face_values.JxW(face_q_point);
-                                }
-                            }
-                        }
-                    }
-                }
+    PreconditionMG<dim,LinearAlgebra::distributed::Vector<double>,MGTransferMatrixFree<dim, double>>
+            mf_gmg_preconditioner(dof_handler_displacement, mg, mg_transfer);
 
+    elasticity_system_mf.initialize_dof_vector(distributed_displacement_sol);
+    elasticity_system_mf.initialize_dof_vector(distributed_displacement_rhs);
 
-                MatrixTools::local_apply_boundary_values(boundary_values, local_dof_indices,
-                                                         cell_matrix, cell_rhs, true);
-                constraints.distribute_local_to_global(
-                        cell_rhs, local_dof_indices, test_rhs);
-            }
-        }
 
-        test_rhs.compress(VectorOperation::add);
-        double total_volume_temp = 0;
-        double goal_volume_temp = 0;
-        double total_volume, goal_volume;
 
-        distributed_solution = state;
-        for (const auto &cell: dof_handler.active_cell_iterators()) {
-            if(cell->is_locally_owned())
-            {
-                std::vector<unsigned int> i(cell->get_fe().n_dofs_per_cell());
-                cell->get_dof_indices(i);
-                if (distributed_solution.block(SolutionBlocks::density).in_local_range(i[cell->get_fe().component_to_system_index(0, 0)]))
-                {
-                    total_volume_temp += cell->measure() * state.block(SolutionBlocks::density)[i[cell->get_fe().component_to_system_index(0, 0)]];
-                    goal_volume_temp += cell->measure() * Input::volume_percentage;
-                }
-            }
-        }
+    ChangeVectorTypes::copy(distributed_displacement_sol,distributed_solution.block(SolutionBlocks::displacement));
+    ChangeVectorTypes::copy(distributed_displacement_rhs,system_rhs.block(SolutionBlocks::displacement));
+    SolverCG<LinearAlgebra::distributed::Vector<double>> CG_Solve(solver_control);
 
-        MPI_Allreduce(&total_volume_temp, &total_volume, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-        MPI_Allreduce(&goal_volume_temp, &goal_volume, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+    std::cout << "from here" << std::endl;
 
-        pcout << "total_volume: " << total_volume  << " and goal_volume: " << goal_volume << std::endl;
+    std::cout << "pre norm" << distributed_displacement_rhs.l2_norm() << std::endl;
 
-        if (test_rhs.block(SolutionBlocks::total_volume_multiplier).in_local_range(0))
-        {
-            test_rhs.block(SolutionBlocks::total_volume_multiplier)[0] = goal_volume - total_volume;
-        }
-        test_rhs.compress(VectorOperation::insert);
+    try
+    {
+        CG_Solve.solve(elasticity_system_mf,distributed_displacement_sol, distributed_displacement_rhs, mf_gmg_preconditioner);
+    }
+    catch(std::exception &exc)
+    {
+        ChangeVectorTypes::copy(distributed_solution.block(SolutionBlocks::displacement), distributed_displacement_sol);
+        output(distributed_solution, 0);
+        std::abort();
+    }
 
-        return test_rhs;
 
-    }
+    std::cout << "to here" << std::endl;
 
 
-    template<int dim>
-    LA::MPI::BlockVector
-    KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
-        double gmres_tolerance;
-        if (Input::use_eisenstat_walker) {
-            gmres_tolerance = std::max(
-                    std::min(
-                            .1 * system_rhs.l2_norm() / (initial_rhs_error),
-                            .001
-                    ),
-                    Input::default_gmres_tolerance);
-        }
-        else {
-            gmres_tolerance = Input::default_gmres_tolerance;
-        }
-        locally_relevant_solution=state;
-        distributed_solution = state;
-        SolverControl solver_control(1000000, gmres_tolerance * system_rhs.l2_norm());
-        TopOptSchurPreconditioner<dim> preconditioner(system_matrix, dof_handler);
-        pcout << "about to solve" << std::endl;
+    //        TopOptSchurPreconditioner<dim> preconditioner(system_matrix, dof_handler, elasticity_system_mf, mf_gmg_preconditioner);
+    //        pcout << "about to solve" << std::endl;
 
-        switch (Input::solver_choice) {
+    //        switch (Input::solver_choice) {
 
-            case SolverOptions::inexact_K_with_exact_A_gmres: {
+    //            case SolverOptions::inexact_K_with_exact_A_gmres: {
 
 
-                preconditioner.initialize(system_matrix, boundary_values, dof_handler, distributed_solution);
-                SolverFGMRES<LA::MPI::BlockVector> B_fgmres(solver_control);
-                B_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
-                pcout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
-                break;
-            }
-            case SolverOptions::inexact_K_with_inexact_A_gmres: {
-                preconditioner.initialize(system_matrix, boundary_values, dof_handler, distributed_solution);
-                SolverFGMRES<LA::MPI::BlockVector> C_fgmres(solver_control);
-                C_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
-                pcout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
-                break;
-            }
-            default:
-                throw;
-        }
+    //                preconditioner.initialize(system_matrix, boundary_values, dof_handler, distributed_solution);
+    //                SolverFGMRES<LA::MPI::BlockVector> B_fgmres(solver_control);
+    //                B_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
+    //                pcout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
+    //                break;
+    //            }
+    //            case SolverOptions::inexact_K_with_inexact_A_gmres: {
+    //                preconditioner.initialize(system_matrix, boundary_values, dof_handler, distributed_solution);
+    //                SolverFGMRES<LA::MPI::BlockVector> C_fgmres(solver_control);
+    //                C_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
+    //                pcout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
+    //                break;
+    //            }
+    //            default:
+    //                throw;
+    //        }
 
-        constraints.distribute(distributed_solution);
+    constraints.distribute(distributed_solution);
 
-        pcout << "here" << std::endl;
+    pcout << "here" << std::endl;
 
-        return distributed_solution;
-    }
+    return distributed_solution;
+}
 
-    template<int dim>
-    void
-    KktSystem<dim>::calculate_initial_rhs_error() {
-        initial_rhs_error = system_rhs.l2_norm();
-    }
+template<int dim>
+void
+KktSystem<dim>::calculate_initial_rhs_error() {
+    initial_rhs_error = system_rhs.l2_norm();
+}
 
-    template<int dim>
-    LA::MPI::BlockVector
-    KktSystem<dim>::get_initial_state() {
+template<int dim>
+LA::MPI::BlockVector
+KktSystem<dim>::get_initial_state() {
 
-        std::vector<unsigned int> block_component(10, 2);
-        block_component[SolutionBlocks::density] = 0;
-        block_component[SolutionBlocks::displacement] = 1;
-        const std::vector<types::global_dof_index> dofs_per_block =
-                DoFTools::count_dofs_per_fe_block(dof_handler, block_component);
-        const unsigned int n_p = dofs_per_block[0];
-        const unsigned int n_u = dofs_per_block[1];
-        const std::vector<unsigned int> block_sizes = {n_p, n_p, n_p, n_p, n_p, n_u, n_u, n_p, n_p, 1};
+    std::vector<unsigned int> block_component(10, 2);
+    block_component[SolutionBlocks::density] = 0;
+    block_component[SolutionBlocks::displacement] = 1;
+    const std::vector<types::global_dof_index> dofs_per_block =
+            DoFTools::count_dofs_per_fe_block(dof_handler, block_component);
+    const unsigned int n_p = dofs_per_block[0];
+    const unsigned int n_u = dofs_per_block[1];
+    const std::vector<unsigned int> block_sizes = {n_p, n_p, n_p, n_p, n_p, n_u, n_u, n_p, n_p, 1};
 
-        LA::MPI::BlockVector state(owned_partitioning, mpi_communicator);
-        {
-            using namespace SolutionBlocks;
-            state.block(density).add(density_ratio);
-            state.block(unfiltered_density).add(density_ratio);
-            state.block(unfiltered_density_multiplier)
-                    .add(density_ratio);
-            state.block(density_lower_slack).add(density_ratio);
-            state.block(density_lower_slack_multiplier).add(50);
-            state.block(density_upper_slack).add(1 - density_ratio);
-            state.block(density_upper_slack_multiplier).add(50);
-            state.block(total_volume_multiplier).add(1);
-            state.block(displacement).add(0);
-            state.block(displacement_multiplier).add(0);
-        }
-        state.compress(VectorOperation::add);
-        return state;
+    LA::MPI::BlockVector state(owned_partitioning, mpi_communicator);
+    {
+        using namespace SolutionBlocks;
+        state.block(density).add(density_ratio);
+        state.block(unfiltered_density).add(density_ratio);
+        state.block(unfiltered_density_multiplier)
+                .add(density_ratio);
+        state.block(density_lower_slack).add(density_ratio);
+        state.block(density_lower_slack_multiplier).add(50);
+        state.block(density_upper_slack).add(1 - density_ratio);
+        state.block(density_upper_slack_multiplier).add(50);
+        state.block(total_volume_multiplier).add(1);
+        state.block(displacement).add(0);
+        state.block(displacement_multiplier).add(0);
     }
+    state.compress(VectorOperation::add);
+    return state;
+}
 
-    template<int dim>
-    void
-    KktSystem<dim>::output(const LA::MPI::BlockVector &state, const unsigned int j) const {
-        locally_relevant_solution = state;
-        std::vector<std::string> solution_names(1, "low_slack_multiplier");
-        std::vector<DataComponentInterpretation::DataComponentInterpretation> data_component_interpretation(
+template<int dim>
+void
+KktSystem<dim>::output(const LA::MPI::BlockVector &state, const unsigned int j) const {
+    locally_relevant_solution = state;
+    std::vector<std::string> solution_names(1, "low_slack_multiplier");
+    std::vector<DataComponentInterpretation::DataComponentInterpretation> data_component_interpretation(
                 1, DataComponentInterpretation::component_is_scalar);
-        solution_names.emplace_back("upper_slack_multiplier");
-        data_component_interpretation.push_back(
+    solution_names.emplace_back("upper_slack_multiplier");
+    data_component_interpretation.push_back(
                 DataComponentInterpretation::component_is_scalar);
-        solution_names.emplace_back("low_slack");
-        data_component_interpretation.push_back(
+    solution_names.emplace_back("low_slack");
+    data_component_interpretation.push_back(
                 DataComponentInterpretation::component_is_scalar);
-        solution_names.emplace_back("upper_slack");
-        data_component_interpretation.push_back(
+    solution_names.emplace_back("upper_slack");
+    data_component_interpretation.push_back(
                 DataComponentInterpretation::component_is_scalar);
-        solution_names.emplace_back("unfiltered_density");
-        data_component_interpretation.push_back(
+    solution_names.emplace_back("unfiltered_density");
+    data_component_interpretation.push_back(
                 DataComponentInterpretation::component_is_scalar);
-        for (unsigned int i = 0; i < dim; i++) {
-            solution_names.emplace_back("displacement");
-            data_component_interpretation.push_back(
-                    DataComponentInterpretation::component_is_part_of_vector);
-        }
-        for (unsigned int i = 0; i < dim; i++) {
-            solution_names.emplace_back("displacement_multiplier");
-            data_component_interpretation.push_back(
+    for (unsigned int i = 0; i < dim; i++) {
+        solution_names.emplace_back("displacement");
+        data_component_interpretation.push_back(
                     DataComponentInterpretation::component_is_part_of_vector);
-        }
-        solution_names.emplace_back("density_multiplier");
+    }
+    for (unsigned int i = 0; i < dim; i++) {
+        solution_names.emplace_back("displacement_multiplier");
         data_component_interpretation.push_back(
+                    DataComponentInterpretation::component_is_part_of_vector);
+    }
+    solution_names.emplace_back("density_multiplier");
+    data_component_interpretation.push_back(
                 DataComponentInterpretation::component_is_scalar);
-        solution_names.emplace_back("density");
-        data_component_interpretation.push_back(
+    solution_names.emplace_back("density");
+    data_component_interpretation.push_back(
                 DataComponentInterpretation::component_is_scalar);
-        solution_names.emplace_back("volume_multiplier");
-        data_component_interpretation.push_back(
+    solution_names.emplace_back("volume_multiplier");
+    data_component_interpretation.push_back(
                 DataComponentInterpretation::component_is_scalar);
-        DataOut<dim> data_out;
-        data_out.attach_dof_handler(dof_handler);
-        data_out.add_data_vector(locally_relevant_solution,
-                                 solution_names,
-                                 DataOut<dim>::type_dof_data,
-                                 data_component_interpretation);
-        data_out.build_patches();
-        std::string output("solution" + std::to_string(j) + ".vtu");
-        data_out.write_vtu_in_parallel(output, MPI_COMM_WORLD);
+    DataOut<dim> data_out;
+    data_out.attach_dof_handler(dof_handler);
+    data_out.add_data_vector(locally_relevant_solution,
+                             solution_names,
+                             DataOut<dim>::type_dof_data,
+                             data_component_interpretation);
+    data_out.build_patches();
+    std::string output("solution" + std::to_string(j) + ".vtu");
+    data_out.write_vtu_in_parallel(output, MPI_COMM_WORLD);
 
-    }
+}
+
+template<int dim>
+PreconditionMG<dim,LinearAlgebra::distributed::Vector<double>,MGTransferMatrixFree<dim, double>>
+KktSystem<dim>::build_mf_gmg_preconditioner()
+{
 
-    template<>
-    void
-    KktSystem<2>::output_stl(const LA::MPI::BlockVector &state) {
-        double height = .25;
-        const int dim = 2;
-        std::ofstream stlfile;
-        stlfile.open("bridge.stl");
-        stlfile << "solid bridge\n" << std::scientific;
-
-        for (const auto &cell: dof_handler.active_cell_iterators()) {
-            if (state.block(
+}
+
+
+template<>
+void
+KktSystem<2>::output_stl(const LA::MPI::BlockVector &state) {
+    double height = .25;
+    const int dim = 2;
+    std::ofstream stlfile;
+    stlfile.open("bridge.stl");
+    stlfile << "solid bridge\n" << std::scientific;
+
+    for (const auto &cell: dof_handler.active_cell_iterators()) {
+        if (state.block(
                     SolutionBlocks::density)[cell->get_fe().component_to_system_index(0, 0)] > 0.5) {
-                const Tensor<1, dim> edge_directions[2] = {cell->vertex(1) -
-                                                           cell->vertex(0),
-                                                           cell->vertex(2) -
-                                                           cell->vertex(0)};
-                const Tensor<2, dim> edge_tensor(
+            const Tensor<1, dim> edge_directions[2] = {cell->vertex(1) -
+                                                       cell->vertex(0),
+                                                       cell->vertex(2) -
+                                                       cell->vertex(0)};
+            const Tensor<2, dim> edge_tensor(
                         {{edge_directions[0][0], edge_directions[0][1]},
                          {edge_directions[1][0], edge_directions[1][1]}});
-                const bool is_right_handed_cell = (determinant(edge_tensor) > 0);
-                if (is_right_handed_cell) {
-                    /* Write one side at z = 0. */
-                    stlfile << "   facet normal " << 0.000000e+00 << " "
+            const bool is_right_handed_cell = (determinant(edge_tensor) > 0);
+            if (is_right_handed_cell) {
+                /* Write one side at z = 0. */
+                stlfile << "   facet normal " << 0.000000e+00 << " "
                             << 0.000000e+00 << " " << -1.000000e+00 << "\n";
-                    stlfile << "      outer loop\n";
-                    stlfile << "         vertex " << cell->vertex(0)[0] << " "
+                stlfile << "      outer loop\n";
+                stlfile << "         vertex " << cell->vertex(0)[0] << " "
                             << cell->vertex(0)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
+                stlfile << "         vertex " << cell->vertex(2)[0] << " "
                             << cell->vertex(2)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
+                stlfile << "         vertex " << cell->vertex(1)[0] << " "
                             << cell->vertex(1)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "      endloop\n";
-                    stlfile << "   endfacet\n";
-                    stlfile << "   facet normal " << 0.000000e+00 << " "
+                stlfile << "      endloop\n";
+                stlfile << "   endfacet\n";
+                stlfile << "   facet normal " << 0.000000e+00 << " "
                             << 0.000000e+00 << " " << -1.000000e+00 << "\n";
-                    stlfile << "      outer loop\n";
-                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
+                stlfile << "      outer loop\n";
+                stlfile << "         vertex " << cell->vertex(1)[0] << " "
                             << cell->vertex(1)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
+                stlfile << "         vertex " << cell->vertex(2)[0] << " "
                             << cell->vertex(2)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "         vertex " << cell->vertex(3)[0] << " "
+                stlfile << "         vertex " << cell->vertex(3)[0] << " "
                             << cell->vertex(3)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "      endloop\n";
-                    stlfile << "   endfacet\n";
-                    /* Write one side at z = height. */
-                    stlfile << "   facet normal " << 0.000000e+00 << " "
+                stlfile << "      endloop\n";
+                stlfile << "   endfacet\n";
+                /* Write one side at z = height. */
+                stlfile << "   facet normal " << 0.000000e+00 << " "
                             << 0.000000e+00 << " " << 1.000000e+00 << "\n";
-                    stlfile << "      outer loop\n";
-                    stlfile << "         vertex " << cell->vertex(0)[0] << " "
+                stlfile << "      outer loop\n";
+                stlfile << "         vertex " << cell->vertex(0)[0] << " "
                             << cell->vertex(0)[1] << " " << height << "\n";
-                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
+                stlfile << "         vertex " << cell->vertex(1)[0] << " "
                             << cell->vertex(1)[1] << " " << height << "\n";
-                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
+                stlfile << "         vertex " << cell->vertex(2)[0] << " "
                             << cell->vertex(2)[1] << " " << height << "\n";
-                    stlfile << "      endloop\n";
-                    stlfile << "   endfacet\n";
-                    stlfile << "   facet normal " << 0.000000e+00 << " "
+                stlfile << "      endloop\n";
+                stlfile << "   endfacet\n";
+                stlfile << "   facet normal " << 0.000000e+00 << " "
                             << 0.000000e+00 << " " << 1.000000e+00 << "\n";
-                    stlfile << "      outer loop\n";
-                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
+                stlfile << "      outer loop\n";
+                stlfile << "         vertex " << cell->vertex(1)[0] << " "
                             << cell->vertex(1)[1] << " " << height << "\n";
-                    stlfile << "         vertex " << cell->vertex(3)[0] << " "
+                stlfile << "         vertex " << cell->vertex(3)[0] << " "
                             << cell->vertex(3)[1] << " " << height << "\n";
-                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
+                stlfile << "         vertex " << cell->vertex(2)[0] << " "
                             << cell->vertex(2)[1] << " " << height << "\n";
-                    stlfile << "      endloop\n";
-                    stlfile << "   endfacet\n";
-                } else /* The cell has a left-handed set up */
-                {
-                    /* Write one side at z = 0. */
-                    stlfile << "   facet normal " << 0.000000e+00 << " "
+                stlfile << "      endloop\n";
+                stlfile << "   endfacet\n";
+            } else /* The cell has a left-handed set up */
+            {
+                /* Write one side at z = 0. */
+                stlfile << "   facet normal " << 0.000000e+00 << " "
                             << 0.000000e+00 << " " << -1.000000e+00 << "\n";
-                    stlfile << "      outer loop\n";
-                    stlfile << "         vertex " << cell->vertex(0)[0] << " "
+                stlfile << "      outer loop\n";
+                stlfile << "         vertex " << cell->vertex(0)[0] << " "
                             << cell->vertex(0)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
+                stlfile << "         vertex " << cell->vertex(1)[0] << " "
                             << cell->vertex(1)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
+                stlfile << "         vertex " << cell->vertex(2)[0] << " "
                             << cell->vertex(2)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "      endloop\n";
-                    stlfile << "   endfacet\n";
-                    stlfile << "   facet normal " << 0.000000e+00 << " "
+                stlfile << "      endloop\n";
+                stlfile << "   endfacet\n";
+                stlfile << "   facet normal " << 0.000000e+00 << " "
                             << 0.000000e+00 << " " << -1.000000e+00 << "\n";
-                    stlfile << "      outer loop\n";
-                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
+                stlfile << "      outer loop\n";
+                stlfile << "         vertex " << cell->vertex(1)[0] << " "
                             << cell->vertex(1)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "         vertex " << cell->vertex(3)[0] << " "
+                stlfile << "         vertex " << cell->vertex(3)[0] << " "
                             << cell->vertex(3)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
+                stlfile << "         vertex " << cell->vertex(2)[0] << " "
                             << cell->vertex(2)[1] << " " << 0.000000e+00 << "\n";
-                    stlfile << "      endloop\n";
-                    stlfile << "   endfacet\n";
-                    /* Write one side at z = height. */
-                    stlfile << "   facet normal " << 0.000000e+00 << " "
+                stlfile << "      endloop\n";
+                stlfile << "   endfacet\n";
+                /* Write one side at z = height. */
+                stlfile << "   facet normal " << 0.000000e+00 << " "
                             << 0.000000e+00 << " " << 1.000000e+00 << "\n";
-                    stlfile << "      outer loop\n";
-                    stlfile << "         vertex " << cell->vertex(0)[0] << " "
+                stlfile << "      outer loop\n";
+                stlfile << "         vertex " << cell->vertex(0)[0] << " "
                             << cell->vertex(0)[1] << " " << height << "\n";
-                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
+                stlfile << "         vertex " << cell->vertex(2)[0] << " "
                             << cell->vertex(2)[1] << " " << height << "\n";
-                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
+                stlfile << "         vertex " << cell->vertex(1)[0] << " "
                             << cell->vertex(1)[1] << " " << height << "\n";
-                    stlfile << "      endloop\n";
-                    stlfile << "   endfacet\n";
-                    stlfile << "   facet normal " << 0.000000e+00 << " "
+                stlfile << "      endloop\n";
+                stlfile << "   endfacet\n";
+                stlfile << "   facet normal " << 0.000000e+00 << " "
                             << 0.000000e+00 << " " << 1.000000e+00 << "\n";
-                    stlfile << "      outer loop\n";
-                    stlfile << "         vertex " << cell->vertex(1)[0] << " "
+                stlfile << "      outer loop\n";
+                stlfile << "         vertex " << cell->vertex(1)[0] << " "
                             << cell->vertex(1)[1] << " " << height << "\n";
-                    stlfile << "         vertex " << cell->vertex(2)[0] << " "
+                stlfile << "         vertex " << cell->vertex(2)[0] << " "
                             << cell->vertex(2)[1] << " " << height << "\n";
-                    stlfile << "         vertex " << cell->vertex(3)[0] << " "
+                stlfile << "         vertex " << cell->vertex(3)[0] << " "
                             << cell->vertex(3)[1] << " " << height << "\n";
-                    stlfile << "      endloop\n";
-                    stlfile << "   endfacet\n";
-                }
-                for (unsigned int face_number = 0;
-                     face_number < GeometryInfo<dim>::faces_per_cell;
-                     ++face_number) {
-                    const typename DoFHandler<dim>::face_iterator face =
-                            cell->face(face_number);
-                    if ((face->at_boundary()) ||
+                stlfile << "      endloop\n";
+                stlfile << "   endfacet\n";
+            }
+            for (unsigned int face_number = 0;
+                 face_number < GeometryInfo<dim>::faces_per_cell;
+                 ++face_number) {
+                const typename DoFHandler<dim>::face_iterator face =
+                        cell->face(face_number);
+                if ((face->at_boundary()) ||
                         (!face->at_boundary() &&
                          (state.block(
-                                 SolutionBlocks::density)[cell->neighbor(face_number)->get_fe().component_to_system_index(0, 0)] <
+                              SolutionBlocks::density)[cell->neighbor(face_number)->get_fe().component_to_system_index(0, 0)] <
                           0.5))) {
-                        const Tensor<1, dim> normal_vector =
-                                (face->center() - cell->center());
-                        const double normal_norm = normal_vector.norm();
-                        if ((face->vertex(0)[0] - face->vertex(0)[0]) *
+                    const Tensor<1, dim> normal_vector =
+                            (face->center() - cell->center());
+                    const double normal_norm = normal_vector.norm();
+                    if ((face->vertex(0)[0] - face->vertex(0)[0]) *
                             (face->vertex(1)[1] - face->vertex(0)[1]) *
                             0.000000e+00 +
                             (face->vertex(0)[1] - face->vertex(0)[1]) * (0 - 0) *
@@ -2045,199 +2668,199 @@ namespace SAND {
                             (height - 0) *
                             (face->vertex(1)[1] - face->vertex(0)[1]) * 0 >
                             0) {
-                            stlfile << "   facet normal "
+                        stlfile << "   facet normal "
                                     << normal_vector[0] / normal_norm << " "
                                     << normal_vector[1] / normal_norm << " "
                                     << 0.000000e+00 << "\n";
-                            stlfile << "      outer loop\n";
-                            stlfile << "         vertex " << face->vertex(0)[0]
-                                    << " " << face->vertex(0)[1] << " "
+                        stlfile << "      outer loop\n";
+                        stlfile << "         vertex " << face->vertex(0)[0]
+                                << " " << face->vertex(0)[1] << " "
                                     << 0.000000e+00 << "\n";
-                            stlfile << "         vertex " << face->vertex(0)[0]
-                                    << " " << face->vertex(0)[1] << " " << height
-                                    << "\n";
-                            stlfile << "         vertex " << face->vertex(1)[0]
-                                    << " " << face->vertex(1)[1] << " "
+                        stlfile << "         vertex " << face->vertex(0)[0]
+                                << " " << face->vertex(0)[1] << " " << height
+                                << "\n";
+                        stlfile << "         vertex " << face->vertex(1)[0]
+                                << " " << face->vertex(1)[1] << " "
                                     << 0.000000e+00 << "\n";
-                            stlfile << "      endloop\n";
-                            stlfile << "   endfacet\n";
-                            stlfile << "   facet normal "
+                        stlfile << "      endloop\n";
+                        stlfile << "   endfacet\n";
+                        stlfile << "   facet normal "
                                     << normal_vector[0] / normal_norm << " "
                                     << normal_vector[1] / normal_norm << " "
                                     << 0.000000e+00 << "\n";
-                            stlfile << "      outer loop\n";
-                            stlfile << "         vertex " << face->vertex(0)[0]
-                                    << " " << face->vertex(0)[1] << " " << height
-                                    << "\n";
-                            stlfile << "         vertex " << face->vertex(1)[0]
-                                    << " " << face->vertex(1)[1] << " " << height
-                                    << "\n";
-                            stlfile << "         vertex " << face->vertex(1)[0]
-                                    << " " << face->vertex(1)[1] << " "
+                        stlfile << "      outer loop\n";
+                        stlfile << "         vertex " << face->vertex(0)[0]
+                                << " " << face->vertex(0)[1] << " " << height
+                                << "\n";
+                        stlfile << "         vertex " << face->vertex(1)[0]
+                                << " " << face->vertex(1)[1] << " " << height
+                                << "\n";
+                        stlfile << "         vertex " << face->vertex(1)[0]
+                                << " " << face->vertex(1)[1] << " "
                                     << 0.000000e+00 << "\n";
-                            stlfile << "      endloop\n";
-                            stlfile << "   endfacet\n";
-                        } else {
-                            stlfile << "   facet normal "
+                        stlfile << "      endloop\n";
+                        stlfile << "   endfacet\n";
+                    } else {
+                        stlfile << "   facet normal "
                                     << normal_vector[0] / normal_norm << " "
                                     << normal_vector[1] / normal_norm << " "
                                     << 0.000000e+00 << "\n";
-                            stlfile << "      outer loop\n";
-                            stlfile << "         vertex " << face->vertex(0)[0]
-                                    << " " << face->vertex(0)[1] << " "
+                        stlfile << "      outer loop\n";
+                        stlfile << "         vertex " << face->vertex(0)[0]
+                                << " " << face->vertex(0)[1] << " "
                                     << 0.000000e+00 << "\n";
-                            stlfile << "         vertex " << face->vertex(1)[0]
-                                    << " " << face->vertex(1)[1] << " "
+                        stlfile << "         vertex " << face->vertex(1)[0]
+                                << " " << face->vertex(1)[1] << " "
                                     << 0.000000e+00 << "\n";
-                            stlfile << "         vertex " << face->vertex(0)[0]
-                                    << " " << face->vertex(0)[1] << " " << height
-                                    << "\n";
-                            stlfile << "      endloop\n";
-                            stlfile << "   endfacet\n";
-                            stlfile << "   facet normal "
+                        stlfile << "         vertex " << face->vertex(0)[0]
+                                << " " << face->vertex(0)[1] << " " << height
+                                << "\n";
+                        stlfile << "      endloop\n";
+                        stlfile << "   endfacet\n";
+                        stlfile << "   facet normal "
                                     << normal_vector[0] / normal_norm << " "
                                     << normal_vector[1] / normal_norm << " "
                                     << 0.000000e+00 << "\n";
-                            stlfile << "      outer loop\n";
-                            stlfile << "         vertex " << face->vertex(0)[0]
-                                    << " " << face->vertex(0)[1] << " " << height
-                                    << "\n";
-                            stlfile << "         vertex " << face->vertex(1)[0]
-                                    << " " << face->vertex(1)[1] << " "
+                        stlfile << "      outer loop\n";
+                        stlfile << "         vertex " << face->vertex(0)[0]
+                                << " " << face->vertex(0)[1] << " " << height
+                                << "\n";
+                        stlfile << "         vertex " << face->vertex(1)[0]
+                                << " " << face->vertex(1)[1] << " "
                                     << 0.000000e+00 << "\n";
-                            stlfile << "         vertex " << face->vertex(1)[0]
-                                    << " " << face->vertex(1)[1] << " " << height
-                                    << "\n";
-                            stlfile << "      endloop\n";
-                            stlfile << "   endfacet\n";
-                        }
+                        stlfile << "         vertex " << face->vertex(1)[0]
+                                << " " << face->vertex(1)[1] << " " << height
+                                << "\n";
+                        stlfile << "      endloop\n";
+                        stlfile << "   endfacet\n";
                     }
                 }
             }
         }
-        stlfile << "endsolid bridge";
     }
+    stlfile << "endsolid bridge";
+}
 
 
-    template<>
-    void
-    KktSystem<3>::output_stl(const LA::MPI::BlockVector &state)
+template<>
+void
+KktSystem<3>::output_stl(const LA::MPI::BlockVector &state)
+{
+    std::ofstream stlfile;
+    stlfile.open("bridge.stl");
+    stlfile << "solid bridge\n" << std::scientific;
+    const int dim = 3;
+    for (const auto &cell : dof_handler.active_cell_iterators())
     {
-        std::ofstream stlfile;
-        stlfile.open("bridge.stl");
-        stlfile << "solid bridge\n" << std::scientific;
-        const int dim = 3;
-        for (const auto &cell : dof_handler.active_cell_iterators())
-        {
-            if (state.block(
+        if (state.block(
                     SolutionBlocks::unfiltered_density)[cell->get_fe().component_to_system_index(0, 0)] > 0.5)
+        {
+            for (const auto n : cell->face_indices())
             {
-                for (const auto n : cell->face_indices())
+                bool create_boundary = false;
+                if (cell->at_boundary(n))
                 {
-                    bool create_boundary = false;
-                    if (cell->at_boundary(n))
-                    {
-                        create_boundary = true;
-                    }
-                    else if (state.block(
-                            SolutionBlocks::unfiltered_density)[cell->neighbor(n)->get_fe().component_to_system_index(0, 0)] <= 0.5)
-                    {
-                        create_boundary = true;
-                    }
+                    create_boundary = true;
+                }
+                else if (state.block(
+                             SolutionBlocks::unfiltered_density)[cell->neighbor(n)->get_fe().component_to_system_index(0, 0)] <= 0.5)
+                {
+                    create_boundary = true;
+                }
 
-                    if (create_boundary)
-                    {
-                        const auto face = cell->face(n);
-                        const Tensor<1,dim> normal_vector = face->center() -
-                                                          cell->center();
-                        double normal_norm = normal_vector.norm();
-                        const Tensor<1,dim> edge_vectors_1 = face->vertex(1) - face->vertex(0);
-                        const Tensor<1,dim> edge_vectors_2 = face->vertex(2) - face->vertex(0);
-
-                        const Tensor<2, dim> edge_tensor (
-                                 {{edge_vectors_1[0], edge_vectors_1[1],edge_vectors_1[2]},
+                if (create_boundary)
+                {
+                    const auto face = cell->face(n);
+                    const Tensor<1,dim> normal_vector = face->center() -
+                            cell->center();
+                    double normal_norm = normal_vector.norm();
+                    const Tensor<1,dim> edge_vectors_1 = face->vertex(1) - face->vertex(0);
+                    const Tensor<1,dim> edge_vectors_2 = face->vertex(2) - face->vertex(0);
+
+                    const Tensor<2, dim> edge_tensor (
+                                {{edge_vectors_1[0], edge_vectors_1[1],edge_vectors_1[2]},
                                  {edge_vectors_2[0], edge_vectors_2[1],edge_vectors_2[2]},
                                  {normal_vector[0], normal_vector[1], normal_vector[2]}});
-                        const bool is_right_handed_cell = (determinant(edge_tensor) > 0);
+                    const bool is_right_handed_cell = (determinant(edge_tensor) > 0);
 
-                        if (is_right_handed_cell)
-                        {
-                            stlfile << "   facet normal "
+                    if (is_right_handed_cell)
+                    {
+                        stlfile << "   facet normal "
                                     << normal_vector[0] / normal_norm << " "
                                     << normal_vector[1] / normal_norm << " "
                                     << normal_vector[2] / normal_norm << "\n";
-                            stlfile << "      outer loop\n";
-                            stlfile << "         vertex " << face->vertex(0)[0]
-                                    << " " << face->vertex(0)[1] << " "
+                        stlfile << "      outer loop\n";
+                        stlfile << "         vertex " << face->vertex(0)[0]
+                                << " " << face->vertex(0)[1] << " "
                                     << face->vertex(0)[2] << "\n";
-                            stlfile << "         vertex " << face->vertex(1)[0]
-                                    << " " << face->vertex(1)[1] << " "
+                        stlfile << "         vertex " << face->vertex(1)[0]
+                                << " " << face->vertex(1)[1] << " "
                                     << face->vertex(1)[2] << "\n";
-                            stlfile << "         vertex " << face->vertex(2)[0]
-                                    << " " << face->vertex(2)[1] << " "
+                        stlfile << "         vertex " << face->vertex(2)[0]
+                                << " " << face->vertex(2)[1] << " "
                                     << face->vertex(2)[2] << "\n";
-                            stlfile << "      endloop\n";
-                            stlfile << "   endfacet\n";
-                            stlfile << "   facet normal "
+                        stlfile << "      endloop\n";
+                        stlfile << "   endfacet\n";
+                        stlfile << "   facet normal "
                                     << normal_vector[0] / normal_norm << " "
                                     << normal_vector[1] / normal_norm << " "
                                     << normal_vector[2] / normal_norm << "\n";
-                            stlfile << "      outer loop\n";
-                            stlfile << "         vertex " << face->vertex(1)[0]
-                                    << " " << face->vertex(1)[1] << " " << face->vertex(1)[2]
-                                    << "\n";
-                            stlfile << "         vertex " << face->vertex(3)[0]
-                                    << " " << face->vertex(3)[1] << " " << face->vertex(3)[2]
-                                    << "\n";
-                            stlfile << "         vertex " << face->vertex(2)[0]
-                                    << " " << face->vertex(2)[1] << " "
+                        stlfile << "      outer loop\n";
+                        stlfile << "         vertex " << face->vertex(1)[0]
+                                << " " << face->vertex(1)[1] << " " << face->vertex(1)[2]
+                                << "\n";
+                        stlfile << "         vertex " << face->vertex(3)[0]
+                                << " " << face->vertex(3)[1] << " " << face->vertex(3)[2]
+                                << "\n";
+                        stlfile << "         vertex " << face->vertex(2)[0]
+                                << " " << face->vertex(2)[1] << " "
                                     << face->vertex(2)[2] << "\n";
-                            stlfile << "      endloop\n";
-                            stlfile << "   endfacet\n";
-                        }
-                        else
-                        {
-                            stlfile << "   facet normal "
+                        stlfile << "      endloop\n";
+                        stlfile << "   endfacet\n";
+                    }
+                    else
+                    {
+                        stlfile << "   facet normal "
                                     << normal_vector[0] / normal_norm << " "
                                     << normal_vector[1] / normal_norm << " "
                                     << normal_vector[2] / normal_norm << "\n";
-                            stlfile << "      outer loop\n";
-                            stlfile << "         vertex " << face->vertex(0)[0]
-                                    << " " << face->vertex(0)[1] << " "
+                        stlfile << "      outer loop\n";
+                        stlfile << "         vertex " << face->vertex(0)[0]
+                                << " " << face->vertex(0)[1] << " "
                                     << face->vertex(0)[2] << "\n";
-                            stlfile << "         vertex " << face->vertex(2)[0]
-                                    << " " << face->vertex(2)[1] << " "
+                        stlfile << "         vertex " << face->vertex(2)[0]
+                                << " " << face->vertex(2)[1] << " "
                                     << face->vertex(2)[2] << "\n";
-                            stlfile << "         vertex " << face->vertex(1)[0]
-                                    << " " << face->vertex(1)[1] << " "
+                        stlfile << "         vertex " << face->vertex(1)[0]
+                                << " " << face->vertex(1)[1] << " "
                                     << face->vertex(1)[2] << "\n";
-                            stlfile << "      endloop\n";
-                            stlfile << "   endfacet\n";
-                            stlfile << "   facet normal "
+                        stlfile << "      endloop\n";
+                        stlfile << "   endfacet\n";
+                        stlfile << "   facet normal "
                                     << normal_vector[0] / normal_norm << " "
                                     << normal_vector[1] / normal_norm << " "
                                     << normal_vector[2] / normal_norm << "\n";
-                            stlfile << "      outer loop\n";
-                            stlfile << "         vertex " << face->vertex(1)[0]
-                                    << " " << face->vertex(1)[1] << " " << face->vertex(1)[2]
-                                    << "\n";
-                            stlfile << "         vertex " << face->vertex(2)[0]
-                                    << " " << face->vertex(2)[1] << " " << face->vertex(2)[2]
-                                    << "\n";
-                            stlfile << "         vertex " << face->vertex(3)[0]
-                                    << " " << face->vertex(3)[1] << " "
+                        stlfile << "      outer loop\n";
+                        stlfile << "         vertex " << face->vertex(1)[0]
+                                << " " << face->vertex(1)[1] << " " << face->vertex(1)[2]
+                                << "\n";
+                        stlfile << "         vertex " << face->vertex(2)[0]
+                                << " " << face->vertex(2)[1] << " " << face->vertex(2)[2]
+                                << "\n";
+                        stlfile << "         vertex " << face->vertex(3)[0]
+                                << " " << face->vertex(3)[1] << " "
                                     << face->vertex(3)[2] << "\n";
-                            stlfile << "      endloop\n";
-                            stlfile << "   endfacet\n";
-                        }
-
+                        stlfile << "      endloop\n";
+                        stlfile << "   endfacet\n";
                     }
 
                 }
+
             }
         }
     }
 }
+}
 
 template class SAND::KktSystem<2>;
 template class SAND::KktSystem<3>;
diff --git a/source/matrix_free_elasticity.cc b/source/matrix_free_elasticity.cc
new file mode 100644
index 0000000..6b3e044
--- /dev/null
+++ b/source/matrix_free_elasticity.cc
@@ -0,0 +1,179 @@
+//
+// Created by justin on 2/17/21.
+//
+#include "../include/matrix_free_elasticity.h"
+#include "../include/input_information.h"
+#include "../include/parameters_and_components.h"
+namespace SAND {
+using namespace dealii;
+
+
+template <int dim, int fe_degree, typename number>
+MF_Elasticity_Operator<dim, fe_degree, number>::MF_Elasticity_Operator()
+    : MatrixFreeOperators::Base<dim,LinearAlgebra::distributed::Vector<number>>()
+{
+
+}
+
+template <int dim, int fe_degree, typename number>
+void MF_Elasticity_Operator<dim, fe_degree, number>::clear()
+{
+    this->cell_data = nullptr;
+    MatrixFreeOperators::Base<dim, LinearAlgebra::distributed::Vector<number>>::clear();
+}
+
+
+template <int dim, int fe_degree, typename number>
+void
+MF_Elasticity_Operator<dim,fe_degree,number>::compute_diagonal ()
+{
+    this->inverse_diagonal_entries.
+    reset(new DiagonalMatrix<dealii::LinearAlgebra::distributed::Vector<number>>());
+    dealii::LinearAlgebra::distributed::Vector<number> &inverse_diagonal = this->inverse_diagonal_entries->get_vector();
+    this->data->initialize_dof_vector(inverse_diagonal);
+    unsigned int dummy = 0;
+    this->data->cell_loop (&MF_Elasticity_Operator::local_compute_diagonal, this,
+                           inverse_diagonal, dummy);
+
+    this->set_constrained_entries_to_one(inverse_diagonal);
+
+    for (auto &local_element : inverse_diagonal)
+      {
+        Assert(local_element > 0.,
+               ExcMessage("No diagonal entry in a positive definite operator "
+                          "should be zero or negative."));
+        local_element = 1./local_element;
+      }
+}
+
+
+
+template <int dim, int fe_degree, typename number>
+void
+MF_Elasticity_Operator<dim,fe_degree,number>
+::local_compute_diagonal (const MatrixFree<dim,number>                     &data,
+                          dealii::LinearAlgebra::distributed::Vector<number>  &dst,
+                          const unsigned int &,
+                          const std::pair<unsigned int,unsigned int>       &cell_range) const
+{
+  FEEvaluation<dim,fe_degree,fe_degree+1,dim,number> displacement (data, 0);
+
+  AlignedVector<VectorizedArray<number>> diagonal(displacement.dofs_per_cell);
+
+  for (unsigned int cell=cell_range.first; cell<cell_range.second; ++cell)
+    {
+
+      VectorizedArray<number> cell_density = cell_data->density(cell, 0);
+
+      displacement.reinit (cell);
+      for (unsigned int i=0; i<displacement.dofs_per_cell; ++i)
+        {
+          for (unsigned int j=0; j<displacement.dofs_per_cell; ++j)
+            displacement.begin_dof_values()[j] = VectorizedArray<number>();
+
+          displacement.begin_dof_values()[i] = make_vectorized_array<number> (1.);
+
+          displacement.evaluate (EvaluationFlags::gradients);
+
+          for (unsigned int q=0; q<displacement.n_q_points; ++q)
+            {
+
+              displacement.submit_divergence(std::pow(cell_density,Input::density_penalty_exponent)* Input::material_lambda * displacement.get_divergence(q), q);
+              displacement.submit_symmetric_gradient(std::pow(cell_density,Input::density_penalty_exponent)* 2.0 * Input::material_mu * displacement.get_symmetric_gradient(q), q);
+
+            }
+
+          displacement.integrate (EvaluationFlags::gradients);
+
+          diagonal[i] = displacement.begin_dof_values()[i];
+        }
+
+      for (unsigned int i=0; i<displacement.dofs_per_cell; ++i)
+        displacement.begin_dof_values()[i] = diagonal[i];
+      displacement.distribute_local_to_global (dst);
+    }
+}
+
+
+
+
+template <int dim, int fe_degree, typename number>
+void
+MF_Elasticity_Operator<dim, fe_degree, number>::local_apply(
+        const MatrixFree<dim, number> &                   data,
+        LinearAlgebra::distributed::Vector<number> &      dst,
+        const LinearAlgebra::distributed::Vector<number> &src,
+        const std::pair<unsigned int, unsigned int> &     cell_range) const
+{
+    FEEvaluation<dim, 1, 2, dim, double> displacement(data);
+
+    for (unsigned int cell=cell_range.first; cell<cell_range.second; ++cell)
+    {
+        VectorizedArray<number> cell_density = cell_data->density(cell, 0);
+
+        displacement.reinit(cell);
+        displacement.read_dof_values(src);
+
+        displacement.gather_evaluate(src, EvaluationFlags::gradients);
+
+        for (unsigned int q = 0; q < displacement.n_q_points; ++q)
+        {
+
+            displacement.submit_divergence(std::pow(cell_density,Input::density_penalty_exponent)* Input::material_lambda * displacement.get_divergence(q), q);
+            displacement.submit_symmetric_gradient(std::pow(cell_density,Input::density_penalty_exponent)* 2.0 * Input::material_mu * displacement.get_symmetric_gradient(q), q);
+
+        }
+
+        displacement.integrate_scatter(EvaluationFlags::gradients, dst);
+    }
+}
+
+template <int dim, int fe_degree, typename number>
+void
+MF_Elasticity_Operator<dim, fe_degree, number>
+::local_apply_face(const dealii::MatrixFree<dim, number> &,
+                   dealii::LinearAlgebra::distributed::Vector<number> &,
+                   const dealii::LinearAlgebra::distributed::Vector<number> &,
+                   const std::pair<unsigned int, unsigned int> &) const
+{
+}
+
+
+template <int dim, int fe_degree, typename number>
+void
+MF_Elasticity_Operator<dim, fe_degree, number>
+::local_apply_boundary_face(const dealii::MatrixFree<dim, number> &,
+                            dealii::LinearAlgebra::distributed::Vector<number> &,
+                            const dealii::LinearAlgebra::distributed::Vector<number> &,
+                            const std::pair<unsigned int, unsigned int> &) const
+{
+}
+
+
+template <int dim, int fe_degree, typename number>
+void
+MF_Elasticity_Operator<dim,fe_degree,number>
+::apply_add (dealii::LinearAlgebra::distributed::Vector<number> &dst,
+             const dealii::LinearAlgebra::distributed::Vector<number> &src) const
+{
+    MatrixFreeOperators::Base<dim, dealii::LinearAlgebra::distributed::Vector<number>>::
+            data->cell_loop(&MF_Elasticity_Operator::local_apply, this, dst, src);
+}
+
+template <int dim, int fe_degree, typename number>
+void
+MF_Elasticity_Operator<dim,fe_degree,number>::set_cell_data (const OperatorCellData<dim,number> &data)
+{
+    this->cell_data = &data;
+}
+
+
+
+}
+
+template class SAND::MF_Elasticity_Operator<2,1,double>;
+template class SAND::MF_Elasticity_Operator<3,1,double>;
+
+//template class SAND::MF_Elasticity_Operator<2,1,double>::MF_Elasticity_Operator();
+//template class SAND::MF_Elasticity_Operator<3,1,double>::MF_Elasticity_Operator();
+
diff --git a/source/matrix_free_gmg.cc b/source/matrix_free_gmg.cc
deleted file mode 100644
index d008a91..0000000
--- a/source/matrix_free_gmg.cc
+++ /dev/null
@@ -1,81 +0,0 @@
-//
-// Created by justin on 2/17/21.
-//
-#include "../include/matrix_free_gmg.h"
-#include "../include/input_information.h"
-#include "../include/parameters_and_components.h"
-namespace SAND {
-    using namespace dealii;
-
-
-    template <int dim, int fe_degree, typename number>
-    ElasticityOperator<dim, fe_degree, number>::ElasticityOperator(DoFHandler<dim> &big_dof_handler_in, BlockVector<double> &state_in)
-      : MatrixFreeOperators::Base<dim,
-                                  LinearAlgebra::distributed::Vector<number>>(),
-        big_dof_handler(big_dof_handler_in),
-        displacement_dof_handler(big_dof_handler.get_triangulation()),
-        fe_displacement(FE_Q<dim>(1) ^ dim),
-        state(state_in)
-    {
-        displacement_dof_handler.distribute_dofs(fe_displacement);
-
-    }
-
-    template <int dim, int fe_degree, typename number>
-    void ElasticityOperator<dim, fe_degree, number>::clear()
-    {
-      MatrixFreeOperators::Base<dim, LinearAlgebra::distributed::Vector<number>>::clear();
-    }
-
-    template <int dim, int fe_degree, typename number>
-    double ElasticityOperator<dim, fe_degree, number>::coefficient_rho_p_lambda(const  auto &cell_iter)
-    {
-        std::vector<unsigned int> i(cell_iter->get_fe().n_dofs_per_cell());
-        cell_iter->get_dof_indices(i);
-        return std::pow(state.block(SolutionBlocks::density)[i[cell_iter->get_fe().component_to_system_index(0, 0)]],Input::density_penalty_exponent)*Input::material_lambda;
-    }
-
-    template <int dim, int fe_degree, typename number>
-    double ElasticityOperator<dim, fe_degree, number>::coefficient_2_rho_p_mu(const auto &cell_iter)
-    {
-        std::vector<unsigned int> i(cell_iter->get_fe().n_dofs_per_cell());
-        cell_iter->get_dof_indices(i);
-        return std::pow(state.block(SolutionBlocks::density)[i[cell_iter->get_fe().component_to_system_index(0, 0)]],Input::density_penalty_exponent)*2*Input::material_mu;
-
-    }
-
-
-    template <int dim, int fe_degree, typename number>
-    void ElasticityOperator<dim, fe_degree, number>::local_apply(
-      const MatrixFree<dim, number> &                   data,
-      LinearAlgebra::distributed::Vector<number> &      dst,
-      const LinearAlgebra::distributed::Vector<number> &src,
-      const std::pair<unsigned int, unsigned int> &     cell_range)
-    {
-      FEEvaluation<dim, 1, 2, dim, double> phi(data);
-      auto big_dof_iter = big_dof_handler.begin_active();
-      auto displacement_dof_iter = displacement_dof_handler.begin_active();
-      for (const auto &cell_iter: triangulation.active_cell_iterators())
-      {
-          big_dof_iter.copy_from(cell_iter);
-          displacement_dof_iter.copy_from(cell_iter);
-          phi.reinit(displacement_dof_iter);
-          phi.read_dof_values(src);
-
-          phi.evaluate(EvaluationFlags::gradients);
-
-          for (unsigned int q = 0; q < phi.n_q_points; ++q)
-          {
-
-            phi.submit_divergence(coefficient_rho_p_lambda(big_dof_iter) * phi.get_divergence(q), q);
-            phi.submit_symmetric_gradient(coefficient_2_rho_p_mu(big_dof_iter) * phi.get_symmetric_gradient(q), q);
-
-          }
-
-          phi.integrate(EvaluationFlags::gradients);
-          phi.distribute_local_to_global(dst);
-      }
-    }
-}
-template class SAND::ElasticityOperator<2,1,double>;
-template class SAND::ElasticityOperator<3,1,double>;
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 56e2d36..c8c660e 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -21,7 +21,8 @@ namespace SAND {
     using size_type         = dealii::types::global_dof_index;
     using namespace dealii;
     template<int dim>
-    TopOptSchurPreconditioner<dim>::TopOptSchurPreconditioner(LA::MPI::BlockSparseMatrix &matrix_in, DoFHandler<dim> &big_dof_handler_in)
+    TopOptSchurPreconditioner<dim>::TopOptSchurPreconditioner(LA::MPI::BlockSparseMatrix &matrix_in, DoFHandler<dim> &big_dof_handler_in, MF_Elasticity_Operator<dim,1,double> &mf_elasticity_operator_in , PreconditionMG<dim,LinearAlgebra::distributed::Vector<double>,MGTransferMatrixFree<dim, double>>
+                                                              &mf_gmg_preconditioner_in)
             :
             system_matrix(matrix_in),
             mpi_communicator(MPI_COMM_WORLD),
@@ -52,7 +53,9 @@ namespace SAND {
             h_mat(a_mat, b_mat, c_mat, e_mat, pre_amg, a_inv_direct),
             j_inv_mat(h_mat, g_mat, d_m_mat, d_m_inv_mat),
             k_inv_mat(h_mat, g_mat, d_m_mat, d_m_inv_mat),
-            big_dof_handler(big_dof_handler_in)
+            big_dof_handler(big_dof_handler_in),
+            mf_elasticity_operator(mf_elasticity_operator_in),
+            mf_gmg_preconditioner(mf_gmg_preconditioner_in)
     {
 
     }

From a28634f9f6aa907aa666cb47a5a9ca9a5f5eed2f Mon Sep 17 00:00:00 2001
From: Justin O'Connor <oconnor@up.math.colostate.edu>
Date: Mon, 11 Apr 2022 20:02:17 -0600
Subject: [PATCH 80/95] GMG seems to work. About to implement in Schur
 Complement

---
 include/kkt_system.h             |   1 +
 source/kkt_system.cc             | 432 +++++++++++++++++--------------
 source/matrix_free_elasticity.cc |  14 +-
 3 files changed, 249 insertions(+), 198 deletions(-)

diff --git a/include/kkt_system.h b/include/kkt_system.h
index 7aa94a8..3b3d4bc 100644
--- a/include/kkt_system.h
+++ b/include/kkt_system.h
@@ -193,6 +193,7 @@ namespace SAND {
         mg::SmootherRelaxation<SmootherType,LinearAlgebra::distributed::Vector<double>> mg_smoother;
         MGLevelObject<typename SmootherType::AdditionalData> smoother_data;
         MGCoarseGridApplySmoother<LinearAlgebra::distributed::Vector<double>> mg_coarse;
+        MGLevelObject<std::set<types::boundary_id>> level_dirichlet_boundaries;
         MGLevelObject<MatrixFreeOperators::MGInterfaceOperator<LevelMFMatrixType>> mg_interface_matrices;
 
         std::set<types::boundary_id> dirichlet_boundary;
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index e02da9a..9649717 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -200,8 +200,6 @@ KktSystem<dim>::create_triangulation() {
                 }
             }
 
-
-
             dof_handler.distribute_dofs(fe_collection);
             dof_handler_displacement.distribute_dofs(fe_displacement);
 
@@ -461,6 +459,7 @@ KktSystem<dim>::setup_boundary_values()
 
             }
             const unsigned int n_levels = triangulation.n_levels();
+            level_dirichlet_boundaries.resize(0,n_levels-1);
             level_boundary_values.resize(0,n_levels-1);
             for (unsigned int level = 0; level < n_levels; ++level)
             {
@@ -490,11 +489,17 @@ KktSystem<dim>::setup_boundary_values()
                                         const unsigned int y_displacement =
                                                 cell->vertex_dof_index(vertex_number, 1, 0);
                                         /*set bottom left BC*/
+                                        level_dirichlet_boundaries[level].insert(x_displacement);
+                                        level_dirichlet_boundaries[level].insert(y_displacement);
+
+                                        if(level == n_levels-1)
+                                        {
+                                            dirichlet_boundary.insert(x_displacement);
+                                            dirichlet_boundary.insert(y_displacement);
+                                        }
+
                                         level_boundary_values[level][x_displacement] = 0;
                                         level_boundary_values[level][y_displacement] = 0;
-
-                                        dirichlet_boundary.insert(x_displacement);
-                                        dirichlet_boundary.insert(y_displacement);
                                     }
                                     /*Find bottom right corner*/
                                     if (std::fabs(vert(0) - 6) < 1e-12 && std::fabs(
@@ -503,6 +508,11 @@ KktSystem<dim>::setup_boundary_values()
                                         const unsigned int y_displacement =
                                                 cell->vertex_dof_index(vertex_number, 1, 0);
                                         level_boundary_values[level][y_displacement] = 0;
+                                        if(level == n_levels-1)
+                                        {
+                                            dirichlet_boundary.insert(y_displacement);
+                                        }
+                                        level_dirichlet_boundaries[level].insert(y_displacement);
                                     }
                                 }
                             }
@@ -582,63 +592,69 @@ KktSystem<dim>::setup_boundary_values()
                 }
 
             }
-            //                const unsigned int n_levels = triangulation.n_levels();
-            //                for (unsigned int level = 0; level < n_levels; ++level)
-            //                {
-            //                    for (auto cell=dof_handler_displacement.begin_active(level);
-            //                         cell!=dof_handler.end_active(level);
-            //                         ++cell)
-            //                    {
-            //                        if(cell->is_locally_owned())
-            //                        {
-            //                            for (unsigned int face_number = 0;
-            //                                 face_number < GeometryInfo<dim>::faces_per_cell;
-            //                                 ++face_number)
-            //                            {
-            //                                if (cell->face(face_number)->at_boundary())
-            //                                {
-            //                                    for (unsigned int vertex_number = 0;
-            //                                         vertex_number < GeometryInfo<dim>::vertices_per_cell;
-            //                                         ++vertex_number)
-            //                                    {
-            //                                        const auto vert = cell->vertex(vertex_number);
-            //                                        /*Find bottom left corner*/
-            //                                        if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
-            //                                                    vert(1) - 0) < 1e-12 && ((std::fabs(
-            //                                                    vert(2) - 0) < 1e-12) || (std::fabs(
-            //                                                    vert(2) - 1) < 1e-12)))
-            //                                        {
-
-            //                                            const unsigned int x_displacement =
-            //                                                    cell->vertex_dof_index(vertex_number, 0, 0);
-            //                                            const unsigned int y_displacement =
-            //                                                    cell->vertex_dof_index(vertex_number, 1, 0);
-            //                                            const unsigned int z_displacement =
-            //                                                    cell->vertex_dof_index(vertex_number, 2, 0);
-            //                                            /*set bottom left BC*/
-            //                                            level_boundary_values[level][x_displacement] = 0;
-            //                                            level_boundary_values[level][y_displacement] = 0;
-            //                                            level_boundary_values[level][z_displacement] = 0;
-            //                                        }
-            //                                        /*Find bottom right corner*/
-            //                                        if (std::fabs(vert(0) - 6) < 1e-12 && std::fabs(
-            //                                                    vert(1) - 0) < 1e-12 && ((std::fabs(
-            //                                                    vert(2) - 0) < 1e-12) || (std::fabs(
-            //                                                    vert(2) - 1) < 1e-12)))
-            //                                        {
-            //                                            const unsigned int y_displacement =
-            //                                                    cell->vertex_dof_index(vertex_number, 1, 0);
-            //                                            const unsigned int z_displacement =
-            //                                                    cell->vertex_dof_index(vertex_number, 2, 0);
-            //                                            level_boundary_values[level][y_displacement] = 0;
-            //                                            level_boundary_values[level][z_displacement] = 0;
-            //                                        }
-            //                                    }
-            //                                }
-            //                            }
-            //                        }
-            //                    }
-            //                }
+            const unsigned int n_levels = triangulation.n_levels();
+            for (unsigned int level = 0; level < n_levels; ++level)
+            {
+                for (auto cell=dof_handler_displacement.begin_active(level);
+                     cell!=dof_handler.end_active(level);
+                     ++cell)
+                {
+                    if(cell->is_locally_owned())
+                    {
+                        for (unsigned int face_number = 0;
+                             face_number < GeometryInfo<dim>::faces_per_cell;
+                             ++face_number)
+                        {
+                            if (cell->face(face_number)->at_boundary())
+                            {
+                                for (unsigned int vertex_number = 0;
+                                     vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                                     ++vertex_number)
+                                {
+                                    const auto vert = cell->vertex(vertex_number);
+                                    /*Find bottom left corner*/
+                                    if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
+                                                vert(1) - 0) < 1e-12 && ((std::fabs(
+                                                vert(2) - 0) < 1e-12) || (std::fabs(
+                                                vert(2) - 1) < 1e-12)))
+                                    {
+
+                                        const unsigned int x_displacement =
+                                                cell->vertex_dof_index(vertex_number, 0, 0);
+                                        const unsigned int y_displacement =
+                                                cell->vertex_dof_index(vertex_number, 1, 0);
+                                        const unsigned int z_displacement =
+                                                cell->vertex_dof_index(vertex_number, 2, 0);
+                                        /*set bottom left BC*/
+                                        level_boundary_values[level][x_displacement] = 0;
+                                        level_boundary_values[level][y_displacement] = 0;
+                                        level_boundary_values[level][z_displacement] = 0;
+                                        level_dirichlet_boundaries[level].insert(x_displacement);
+                                        level_dirichlet_boundaries[level].insert(y_displacement);
+                                        level_dirichlet_boundaries[level].insert(z_displacement);
+                                    }
+                                    /*Find bottom right corner*/
+                                    if (std::fabs(vert(0) - 6) < 1e-12 && std::fabs(
+                                                vert(1) - 0) < 1e-12 && ((std::fabs(
+                                                vert(2) - 0) < 1e-12) || (std::fabs(
+                                                vert(2) - 1) < 1e-12)))
+                                    {
+                                        const unsigned int y_displacement =
+                                                cell->vertex_dof_index(vertex_number, 1, 0);
+                                        const unsigned int z_displacement =
+                                                cell->vertex_dof_index(vertex_number, 2, 0);
+                                        level_boundary_values[level][y_displacement] = 0;
+                                        level_boundary_values[level][z_displacement] = 0;
+
+                                        level_dirichlet_boundaries[level].insert(y_displacement);
+                                        level_dirichlet_boundaries[level].insert(z_displacement);
+                                    }
+                                }
+                            }
+                        }
+                    }
+                }
+            }
 
 
         }
@@ -700,56 +716,63 @@ KktSystem<dim>::setup_boundary_values()
                     }
                 }
             }
-            //                const unsigned int n_levels = triangulation.n_levels();
-            //                for (unsigned int level = 0; level < n_levels; ++level)
-            //                {
-            //                    for (auto cell=dof_handler_displacement.begin_active(level);
-            //                         cell!=dof_handler.end_active(level);
-            //                         ++cell)
-            //                    {
-            //                        if(cell->is_locally_owned())
-            //                        {
-            //                            for (unsigned int face_number = 0;
-            //                                 face_number < GeometryInfo<dim>::faces_per_cell;
-            //                                 ++face_number)
-            //                            {
-            //                                if (cell->face(face_number)->at_boundary())
-            //                                {
-            //                                    for (unsigned int vertex_number = 0;
-            //                                         vertex_number < GeometryInfo<dim>::vertices_per_cell;
-            //                                         ++vertex_number)
-            //                                    {
-            //                                        const auto vert = cell->vertex(vertex_number);
-            //                                        /*Find bottom left corner*/
-            //                                        if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
-            //                                                    vert(1) - 2) < 1e-12)
-            //                                        {
-
-            //                                            const unsigned int x_displacement =
-            //                                                    cell->vertex_dof_index(vertex_number, 0, 0);
-            //                                            const unsigned int y_displacement =
-            //                                                    cell->vertex_dof_index(vertex_number, 1, 0);
-            //                                            /*set bottom left BC*/
-            //                                            level_boundary_values[level][x_displacement] = 0;
-            //                                            level_boundary_values[level][y_displacement] = 0;
-            //                                        }
-            //                                        /*Find bottom right corner*/
-            //                                        if (std::fabs(vert(0) - 1) < 1e-12 && std::fabs(
-            //                                                    vert(1) - 2) < 1e-12)
-            //                                        {
-            //                                            const unsigned int x_displacement =
-            //                                                    cell->vertex_dof_index(vertex_number, 0, 0);
-            //                                            const unsigned int y_displacement =
-            //                                                    cell->vertex_dof_index(vertex_number, 1, 0);
-            //                                            level_boundary_values[level][x_displacement] = 0;
-            //                                            level_boundary_values[level][y_displacement] = 0;
-            //                                        }
-            //                                    }
-            //                                }
-            //                            }
-            //                        }
-            //                    }
-            //                }
+            const unsigned int n_levels = triangulation.n_levels();
+            for (unsigned int level = 0; level < n_levels; ++level)
+            {
+                for (auto cell=dof_handler_displacement.begin_active(level);
+                     cell!=dof_handler.end_active(level);
+                     ++cell)
+                {
+                    if(cell->is_locally_owned())
+                    {
+                        for (unsigned int face_number = 0;
+                             face_number < GeometryInfo<dim>::faces_per_cell;
+                             ++face_number)
+                        {
+                            if (cell->face(face_number)->at_boundary())
+                            {
+                                for (unsigned int vertex_number = 0;
+                                     vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                                     ++vertex_number)
+                                {
+                                    const auto vert = cell->vertex(vertex_number);
+                                    /*Find bottom left corner*/
+                                    if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
+                                                vert(1) - 2) < 1e-12)
+                                    {
+
+                                        const unsigned int x_displacement =
+                                                cell->vertex_dof_index(vertex_number, 0, 0);
+                                        const unsigned int y_displacement =
+                                                cell->vertex_dof_index(vertex_number, 1, 0);
+                                        /*set bottom left BC*/
+                                        level_boundary_values[level][x_displacement] = 0;
+                                        level_boundary_values[level][y_displacement] = 0;
+
+                                        level_dirichlet_boundaries[level].insert(x_displacement);
+                                        level_dirichlet_boundaries[level].insert(y_displacement);
+
+                                    }
+                                    /*Find bottom right corner*/
+                                    if (std::fabs(vert(0) - 1) < 1e-12 && std::fabs(
+                                                vert(1) - 2) < 1e-12)
+                                    {
+                                        const unsigned int x_displacement =
+                                                cell->vertex_dof_index(vertex_number, 0, 0);
+                                        const unsigned int y_displacement =
+                                                cell->vertex_dof_index(vertex_number, 1, 0);
+                                        level_boundary_values[level][x_displacement] = 0;
+                                        level_boundary_values[level][y_displacement] = 0;
+
+                                        level_dirichlet_boundaries[level].insert(x_displacement);
+                                        level_dirichlet_boundaries[level].insert(y_displacement);
+                                    }
+                                }
+                            }
+                        }
+                    }
+                }
+            }
         }
         else if (dim == 3)
         {
@@ -820,66 +843,74 @@ KktSystem<dim>::setup_boundary_values()
                     }
                 }
             }
-            //                const unsigned int n_levels = triangulation.n_levels();
-            //                for (unsigned int level = 0; level < n_levels; ++level)
-            //                {
-            //                    for (auto cell=dof_handler_displacement.begin_active(level);
-            //                         cell!=dof_handler.end_active(level);
-            //                         ++cell)
-            //                    {
-            //                        if(cell->is_locally_owned())
-            //                        {
-            //                            for (unsigned int face_number = 0;
-            //                                 face_number < GeometryInfo<dim>::faces_per_cell;
-            //                                 ++face_number)
-            //                            {
-            //                                if (cell->face(face_number)->at_boundary())
-            //                                {
-            //                                    for (unsigned int vertex_number = 0;
-            //                                         vertex_number < GeometryInfo<dim>::vertices_per_cell;
-            //                                         ++vertex_number)
-            //                                    {
-            //                                        const auto vert = cell->vertex(vertex_number);
-            //                                        /*Find bottom left corner*/
-            //                                        if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
-            //                                                    vert(1) - 2) < 1e-12 && ((std::fabs(
-            //                                                    vert(2) - 0) < 1e-12) || (std::fabs(
-            //                                                    vert(2) - 1) < 1e-12)))
-            //                                        {
-
-            //                                            const unsigned int x_displacement =
-            //                                                    cell->vertex_dof_index(vertex_number, 0, 0);
-            //                                            const unsigned int y_displacement =
-            //                                                    cell->vertex_dof_index(vertex_number, 1, 0);
-            //                                            const unsigned int z_displacement =
-            //                                                    cell->vertex_dof_index(vertex_number, 2, 0);
-            //                                            /*set bottom left BC*/
-            //                                            level_boundary_values[level][x_displacement] = 0;
-            //                                            level_boundary_values[level][y_displacement] = 0;
-            //                                            level_boundary_values[level][z_displacement] = 0;
-            //                                        }
-            //                                        /*Find bottom right corner*/
-            //                                        if (std::fabs(vert(0) - 1) < 1e-12 && std::fabs(
-            //                                                    vert(1) - 2) < 1e-12 && ((std::fabs(
-            //                                                    vert(2) - 0) < 1e-12) || (std::fabs(
-            //                                                    vert(2) - 1) < 1e-12)))
-            //                                        {
-            //                                            const unsigned int x_displacement =
-            //                                                    cell->vertex_dof_index(vertex_number, 0, 0);
-            //                                            const unsigned int y_displacement =
-            //                                                    cell->vertex_dof_index(vertex_number, 1, 0);
-            //                                            const unsigned int z_displacement =
-            //                                                    cell->vertex_dof_index(vertex_number, 2, 0);
-            //                                            level_boundary_values[level][x_displacement] = 0;
-            //                                            level_boundary_values[level][y_displacement] = 0;
-            //                                            level_boundary_values[level][z_displacement] = 0;
-            //                                        }
-            //                                    }
-            //                                }
-            //                            }
-            //                        }
-            //                    }
-            //                }
+        const unsigned int n_levels = triangulation.n_levels();
+        for (unsigned int level = 0; level < n_levels; ++level)
+        {
+            for (auto cell=dof_handler_displacement.begin_active(level);
+                 cell!=dof_handler.end_active(level);
+                 ++cell)
+            {
+                if(cell->is_locally_owned())
+                {
+                    for (unsigned int face_number = 0;
+                         face_number < GeometryInfo<dim>::faces_per_cell;
+                         ++face_number)
+                    {
+                        if (cell->face(face_number)->at_boundary())
+                        {
+                            for (unsigned int vertex_number = 0;
+                                 vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                                 ++vertex_number)
+                            {
+                                const auto vert = cell->vertex(vertex_number);
+                                /*Find bottom left corner*/
+                                if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
+                                            vert(1) - 2) < 1e-12 && ((std::fabs(
+                                            vert(2) - 0) < 1e-12) || (std::fabs(
+                                            vert(2) - 1) < 1e-12)))
+                                {
+
+                                    const unsigned int x_displacement =
+                                            cell->vertex_dof_index(vertex_number, 0, 0);
+                                    const unsigned int y_displacement =
+                                            cell->vertex_dof_index(vertex_number, 1, 0);
+                                    const unsigned int z_displacement =
+                                            cell->vertex_dof_index(vertex_number, 2, 0);
+                                    /*set bottom left BC*/
+                                    level_boundary_values[level][x_displacement] = 0;
+                                    level_boundary_values[level][y_displacement] = 0;
+                                    level_boundary_values[level][z_displacement] = 0;
+
+                                    level_dirichlet_boundaries[level].insert(x_displacement);
+                                    level_dirichlet_boundaries[level].insert(y_displacement);
+                                    level_dirichlet_boundaries[level].insert(z_displacement);
+                                }
+                                /*Find bottom right corner*/
+                                if (std::fabs(vert(0) - 1) < 1e-12 && std::fabs(
+                                            vert(1) - 2) < 1e-12 && ((std::fabs(
+                                            vert(2) - 0) < 1e-12) || (std::fabs(
+                                            vert(2) - 1) < 1e-12)))
+                                {
+                                    const unsigned int x_displacement =
+                                            cell->vertex_dof_index(vertex_number, 0, 0);
+                                    const unsigned int y_displacement =
+                                            cell->vertex_dof_index(vertex_number, 1, 0);
+                                    const unsigned int z_displacement =
+                                            cell->vertex_dof_index(vertex_number, 2, 0);
+                                    level_boundary_values[level][x_displacement] = 0;
+                                    level_boundary_values[level][y_displacement] = 0;
+                                    level_boundary_values[level][z_displacement] = 0;
+
+                                    level_dirichlet_boundaries[level].insert(x_displacement);
+                                    level_dirichlet_boundaries[level].insert(y_displacement);
+                                    level_dirichlet_boundaries[level].insert(z_displacement);
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+        }
         } else {
             throw;
         }
@@ -1905,12 +1936,15 @@ KktSystem<dim>::calculate_rhs(const LA::MPI::BlockVector &distributed_state, con
 
                     //rhs eqn 1 - boundary terms counted later
                     cell_rhs(i) +=
-                            -1 * fe_values.JxW(q_point) * (
+                            -1 * fe_values.JxW(q_point)
+                            * (
                                 -1 * std::pow(old_density_values[q_point], density_penalty_exponent)
-                                * (old_displacement_multiplier_divs[q_point] * displacement_phi_i_div
-                                   * lambda_values[q_point]
-                                   + 2 * mu_values[q_point] * (old_displacement_multiplier_symmgrads[q_point]
-                                                               * displacement_phi_i_symmgrad))
+                                * (
+                                    old_displacement_multiplier_divs[q_point] * displacement_phi_i_div
+                                    * lambda_values[q_point]
+                                    + 2 * mu_values[q_point] * (old_displacement_multiplier_symmgrads[q_point]
+                                                               * displacement_phi_i_symmgrad)
+                                   )
                                 );
 
                     //rhs eqn 2
@@ -2072,12 +2106,12 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
     }
     locally_relevant_solution=state;
     distributed_solution = state;
-    SolverControl solver_control(10, 1e-4);
+
+    SolverControl solver_control(100, 1e-9);
 
     using SystemMFMatrixType = MF_Elasticity_Operator<dim, 1, double>;
     using LevelMFMatrixType = MF_Elasticity_Operator<dim, 1, double>;
 
-
     elasticity_system_mf.clear();
     mg_matrices.clear_elements();
     dof_handler_displacement.distribute_mg_dofs();
@@ -2086,11 +2120,11 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
     IndexSet locally_relevant_dofs;
     DoFTools::extract_locally_relevant_dofs(dof_handler_displacement, locally_relevant_dofs);
 
-    //        constraints.clear();
-    //        constraints.reinit(locally_relevant_dofs);
-    //        DoFTools::make_hanging_node_constraints(dof_handler_displacement, constraints);
-    //        VectorTools::interpolate_boundary_values(dof_handler_displacement, 0, Functions::ZeroFunction<dim>(), constraints);
-    //        constraints.close();
+    constraints.clear();
+        constraints.reinit(locally_relevant_dofs);
+        constraints.add_lines( dirichlet_boundary);
+    constraints.close();
+
 
 
     {
@@ -2120,10 +2154,16 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
 
     const unsigned int n_levels = triangulation.n_global_levels();
     mg_matrices.resize(0, n_levels - 1);
+
+    mg_constrained_dofs.clear();
+
     mg_constrained_dofs.initialize(dof_handler_displacement);
+
     mg_constrained_dofs.make_zero_boundary_constraints(dof_handler_displacement,
                                                            dirichlet_boundary);
 
+
+
     for (unsigned int level = 0; level < n_levels; ++level)
     {
         IndexSet relevant_dofs;
@@ -2132,20 +2172,23 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
                                                       relevant_dofs);
         AffineConstraints<double> level_constraints;
             level_constraints.reinit(relevant_dofs);
-            level_constraints.add_lines( mg_constrained_dofs.get_boundary_indices(level));
+            level_constraints.add_lines(mg_constrained_dofs.get_boundary_indices(level));
         level_constraints.close();
+
         typename MatrixFree<dim, double>::AdditionalData additional_data;
         additional_data.tasks_parallel_scheme =
                 MatrixFree<dim, double>::AdditionalData::none;
         additional_data.mapping_update_flags =
                 (update_gradients | update_JxW_values | update_quadrature_points);
         additional_data.mg_level = level;
+
         std::shared_ptr<MatrixFree<dim, double>> mg_mf_storage_level(
                     new MatrixFree<dim, double>());
         mg_mf_storage_level->reinit(dof_handler_displacement,
                                     level_constraints,
                                     QGauss<1>(fe_displacement.degree + 1),
                                     additional_data);
+
         mg_matrices[level].clear();
         mg_matrices[level].initialize(mg_mf_storage_level,
                                       mg_constrained_dofs,
@@ -2164,14 +2207,8 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
 
     dof_handler_density.distribute_mg_dofs();
 
-    /*OperatorCellData<dim, GMGNumberType> active_cell_data;
-    MGLevelObject<OperatorCellData<dim, GMGNumberType>> level_cell_data;
-    dealii::LinearAlgebra::distributed::Vector<double> */
     active_density_vector.reinit(dof_handler_density.locally_owned_dofs(),triangulation.get_communicator());
 
-
-//    elasticity_system_mf.initialize_dof_vector(active_density_vector);
-
     ChangeVectorTypes::copy(active_density_vector,distributed_solution.block(SolutionBlocks::density));
 
     const unsigned int n_cells = elasticity_system_mf.get_matrix_free()->n_cell_batches();
@@ -2367,6 +2404,8 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
     PreconditionMG<dim,LinearAlgebra::distributed::Vector<double>,MGTransferMatrixFree<dim, double>>
             mf_gmg_preconditioner(dof_handler_displacement, mg, mg_transfer);
 
+
+
     elasticity_system_mf.initialize_dof_vector(distributed_displacement_sol);
     elasticity_system_mf.initialize_dof_vector(distributed_displacement_rhs);
 
@@ -2374,6 +2413,7 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
 
     ChangeVectorTypes::copy(distributed_displacement_sol,distributed_solution.block(SolutionBlocks::displacement));
     ChangeVectorTypes::copy(distributed_displacement_rhs,system_rhs.block(SolutionBlocks::displacement));
+
     SolverCG<LinearAlgebra::distributed::Vector<double>> CG_Solve(solver_control);
 
     std::cout << "from here" << std::endl;
@@ -2382,16 +2422,18 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
 
     try
     {
-        CG_Solve.solve(elasticity_system_mf,distributed_displacement_sol, distributed_displacement_rhs, mf_gmg_preconditioner);
+        CG_Solve.solve(elasticity_system_mf, distributed_displacement_sol, -1* distributed_displacement_rhs, mf_gmg_preconditioner  /* PreconditionIdentity()*/);
     }
     catch(std::exception &exc)
     {
-        ChangeVectorTypes::copy(distributed_solution.block(SolutionBlocks::displacement), distributed_displacement_sol);
-        output(distributed_solution, 0);
-        std::abort();
+
     }
 
 
+    ChangeVectorTypes::copy(distributed_solution.block(SolutionBlocks::displacement), distributed_displacement_sol);
+    output(distributed_solution, 0);
+    std::abort();
+
     std::cout << "to here" << std::endl;
 
 
diff --git a/source/matrix_free_elasticity.cc b/source/matrix_free_elasticity.cc
index 6b3e044..5b7e638 100644
--- a/source/matrix_free_elasticity.cc
+++ b/source/matrix_free_elasticity.cc
@@ -110,17 +110,25 @@ MF_Elasticity_Operator<dim, fe_degree, number>::local_apply(
     for (unsigned int cell=cell_range.first; cell<cell_range.second; ++cell)
     {
         VectorizedArray<number> cell_density = cell_data->density(cell, 0);
+        double penalized_density = std::pow(cell_density[0],Input::density_penalty_exponent);
 
         displacement.reinit(cell);
-        displacement.read_dof_values(src);
+//        displacement.read_dof_values(src);
 
         displacement.gather_evaluate(src, EvaluationFlags::gradients);
 
         for (unsigned int q = 0; q < displacement.n_q_points; ++q)
         {
+            SymmetricTensor< 2, dim, VectorizedArray<double> > symgrad_term = penalized_density* 2.0 * Input::material_mu *displacement.get_symmetric_gradient(q);
+            VectorizedArray<number> div_term = trace(displacement.get_symmetric_gradient(q));
 
-            displacement.submit_divergence(std::pow(cell_density,Input::density_penalty_exponent)* Input::material_lambda * displacement.get_divergence(q), q);
-            displacement.submit_symmetric_gradient(std::pow(cell_density,Input::density_penalty_exponent)* 2.0 * Input::material_mu * displacement.get_symmetric_gradient(q), q);
+            for (unsigned int d = 0; d < dim; ++d)
+            {
+                symgrad_term[d][d] += penalized_density* Input::material_lambda * div_term;
+            }
+
+
+            displacement.submit_symmetric_gradient( displacement.get_symmetric_gradient(q), q);
 
         }
 

From 458e60abccbfcc8f23aef817d672ec8f73b6dbb2 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <oconnor@up.math.colostate.edu>
Date: Wed, 13 Apr 2022 13:40:06 -0600
Subject: [PATCH 81/95] working mf gmg

---
 include/input_information.h    |   2 +-
 include/kkt_system.h           |   7 +-
 include/schur_preconditioner.h |  48 ++--
 source/kkt_system.cc           | 417 ++++++++++++++++-----------------
 source/schur_preconditioner.cc | 281 +++++++++++++++-------
 5 files changed, 430 insertions(+), 325 deletions(-)

diff --git a/include/input_information.h b/include/input_information.h
index 342f920..3e072fb 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -42,7 +42,7 @@ namespace SAND {
         constexpr bool output_parts_of_matrix = false;
 
         //Linear solver options
-        constexpr unsigned int solver_choice = SolverOptions::inexact_K_with_exact_A_gmres;
+        constexpr unsigned int solver_choice = SolverOptions::inexact_K_with_inexact_A_gmres;
         constexpr bool use_eisenstat_walker = false;
         constexpr double default_gmres_tolerance = 1e-9;
 
diff --git a/include/kkt_system.h b/include/kkt_system.h
index 3b3d4bc..e4a5771 100644
--- a/include/kkt_system.h
+++ b/include/kkt_system.h
@@ -161,6 +161,7 @@ namespace SAND {
         DoFHandler<dim> dof_handler_density;
 
         AffineConstraints<double> constraints;
+        AffineConstraints<double> displacement_constraints;
         FESystem<dim> fe_nine;
         FESystem<dim> fe_ten;
         hp::FECollection<dim> fe_collection;
@@ -178,7 +179,7 @@ namespace SAND {
         double initial_rhs_error;
 
         MGConstrainedDoFs mg_constrained_dofs;
-        SystemMFMatrixType elasticity_system_mf;
+        SystemMFMatrixType elasticity_matrix_mf;
         MGLevelObject<LevelMFMatrixType> mg_matrices;
 
 
@@ -186,6 +187,7 @@ namespace SAND {
         MGLevelObject<OperatorCellData<dim, GMGNumberType>> level_cell_data;
         dealii::LinearAlgebra::distributed::Vector<double> active_density_vector;
         MGLevelObject<dealii::LinearAlgebra::distributed::Vector<double>> level_density_vector;
+
         MGTransferMatrixFree<dim,GMGNumberType> transfer;
         MGTransferMatrixFree<dim, double> mg_transfer;
 
@@ -193,7 +195,8 @@ namespace SAND {
         mg::SmootherRelaxation<SmootherType,LinearAlgebra::distributed::Vector<double>> mg_smoother;
         MGLevelObject<typename SmootherType::AdditionalData> smoother_data;
         MGCoarseGridApplySmoother<LinearAlgebra::distributed::Vector<double>> mg_coarse;
-        MGLevelObject<std::set<types::boundary_id>> level_dirichlet_boundaries;
+        MGLevelObject<std::set<types::boundary_id>> level_dirichlet_boundary_dofs;
+        MGLevelObject<AffineConstraints<double>> mg_level_constraints;
         MGLevelObject<MatrixFreeOperators::MGInterfaceOperator<LevelMFMatrixType>> mg_interface_matrices;
 
         std::set<types::boundary_id> dirichlet_boundary;
diff --git a/include/schur_preconditioner.h b/include/schur_preconditioner.h
index e2c010f..d5bdfa8 100644
--- a/include/schur_preconditioner.h
+++ b/include/schur_preconditioner.h
@@ -101,20 +101,24 @@ namespace SAND
             int size;
     };
 
-    class AInvMatGMGMF : public TrilinosWrappers::SparseMatrix {
+    template<int dim>
+    class AInvMatMFGMG : public TrilinosWrappers::SparseMatrix {
         public:
-            AInvMatGMGMF();
-            void vmult() const;
-            void Tvmult() const;
-            void initialize();
+            AInvMatMFGMG(MF_Elasticity_Operator<dim,1,double> &mf_elasticity_operator_in , PreconditionMG<dim,LinearAlgebra::distributed::Vector<double>,MGTransferMatrixFree<dim, double>>
+                         &mf_gmg_preconditioner_in);
+            void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
+            void Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
             unsigned int m() const;
             unsigned int n() const;
+            void set_tol(double tolerance_in);
+            void set_iter(unsigned int iterations_in);
         private:
-            const LA::MPI::SparseMatrix &f_mat;
-            LA::MPI::SparseMatrix &d_8_mat;
-            mutable LA::MPI::Vector temp_vect_1;
-            mutable LA::MPI::Vector temp_vect_2;
-
+            MF_Elasticity_Operator<dim,1,double> &mf_elasticity_operator;
+            PreconditionMG<dim,LinearAlgebra::distributed::Vector<double>,MGTransferMatrixFree<dim, double>> &mf_gmg_preconditioner;
+            double tolerance = 1e-6;
+            unsigned int iterations = 10;
+            mutable dealii::LinearAlgebra::distributed::Vector<double> temp_src;
+            mutable dealii::LinearAlgebra::distributed::Vector<double> temp_dst;
 
     };
 
@@ -135,9 +139,10 @@ namespace SAND
 
     };
 
+    template<int dim>
     class HMatrix : public TrilinosWrappers::SparseMatrix {
         public:
-            HMatrix(LA::MPI::SparseMatrix &a_mat_in, const LA::MPI::SparseMatrix &b_mat_in, const LA::MPI::SparseMatrix &c_mat_in, const LA::MPI::SparseMatrix &e_mat_in,TrilinosWrappers::PreconditionAMG &pre_amg_in, VmultTrilinosSolverDirect &a_inv_direct_in);
+            HMatrix(LA::MPI::SparseMatrix &a_mat_in, const LA::MPI::SparseMatrix &b_mat_in, const LA::MPI::SparseMatrix &c_mat_in, const LA::MPI::SparseMatrix &e_mat_in,TrilinosWrappers::PreconditionAMG &pre_amg_in, VmultTrilinosSolverDirect &a_inv_direct_in, AInvMatMFGMG<dim> &a_inv_mf_gmg_in);
             void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
             void Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
             void initialize(LA::MPI::Vector &exemplar_density_vector,  LA::MPI::Vector &exemplar_displacement_vector);
@@ -157,19 +162,21 @@ namespace SAND
             mutable LA::MPI::Vector temp_vect_5;
             mutable LA::MPI::Vector temp_vect_6;
             mutable LA::MPI::Vector temp_vect_7;
+            AInvMatMFGMG<dim> &a_inv_mf_gmg;
 
     };
 
+    template<int dim>
     class KinvMatrix : public TrilinosWrappers::SparseMatrix {
         public:
-            KinvMatrix(HMatrix &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in);
+            KinvMatrix(HMatrix<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in);
             void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
             void Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
             void initialize(LA::MPI::Vector &exemplar_density_vector);
             unsigned int m() const;
             unsigned int n() const;
         private:
-            HMatrix &h_mat;
+            HMatrix<dim> &h_mat;
             GMatrix &g_mat;
             const LA::MPI::SparseMatrix &d_m_mat;
             LA::MPI::SparseMatrix &d_m_inv_mat;
@@ -179,16 +186,17 @@ namespace SAND
             mutable LA::MPI::Vector temp_vect_4;
     };
 
+    template<int dim>
     class JinvMatrix : public TrilinosWrappers::SparseMatrix {
         public:
-            JinvMatrix(HMatrix &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in);
+            JinvMatrix(HMatrix<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in);
             void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
             void Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
             void initialize(LA::MPI::Vector &exemplar_density_vector);
             unsigned int m() const;
             unsigned int n() const;
         private:
-            HMatrix &h_mat;
+            HMatrix<dim> &h_mat;
             GMatrix &g_mat;
             const LA::MPI::SparseMatrix &d_m_mat;
             LA::MPI::SparseMatrix &d_m_inv_mat;
@@ -267,19 +275,19 @@ namespace SAND
         TrilinosWrappers::SolverDirect::AdditionalData additional_data;
         SolverControl direct_solver_control;
         mutable VmultTrilinosSolverDirect a_inv_direct;
+
+        AInvMatMFGMG<dim> a_inv_mf_gmg;
         ConditionalOStream pcout;
         mutable TimerOutput timer;
 
         mutable TrilinosWrappers::PreconditionAMG pre_amg;
 
         GMatrix g_mat;
-        HMatrix h_mat;
+        HMatrix<dim> h_mat;
 
-        JinvMatrix j_inv_mat;
-        KinvMatrix k_inv_mat;
+        JinvMatrix<dim> j_inv_mat;
+        KinvMatrix<dim> k_inv_mat;
 
-        MF_Elasticity_Operator<dim,1,double> &mf_elasticity_operator;
-        PreconditionMG<dim,LinearAlgebra::distributed::Vector<double>,MGTransferMatrixFree<dim, double>>  &mf_gmg_preconditioner;
     };
 
 
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index 9649717..c763167 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -202,6 +202,7 @@ KktSystem<dim>::create_triangulation() {
 
             dof_handler.distribute_dofs(fe_collection);
             dof_handler_displacement.distribute_dofs(fe_displacement);
+            dof_handler_displacement.distribute_mg_dofs();
 
             DoFRenumbering::component_wise(dof_handler, sub_blocks);
             DoFRenumbering::component_wise(dof_handler_displacement);
@@ -252,6 +253,7 @@ KktSystem<dim>::create_triangulation() {
 
             dof_handler.distribute_dofs(fe_collection);
             dof_handler_displacement.distribute_dofs(fe_displacement);
+            dof_handler_displacement.distribute_mg_dofs();
 
             DoFRenumbering::component_wise(dof_handler, sub_blocks);
             DoFRenumbering::component_wise(dof_handler_displacement);
@@ -318,6 +320,7 @@ KktSystem<dim>::create_triangulation() {
 
             dof_handler.distribute_dofs(fe_collection);
             dof_handler_displacement.distribute_dofs(fe_displacement);
+            dof_handler_displacement.distribute_mg_dofs();
 
             DoFRenumbering::component_wise(dof_handler, sub_blocks);
             DoFRenumbering::component_wise(dof_handler_displacement);
@@ -378,8 +381,9 @@ KktSystem<dim>::create_triangulation() {
             DoFRenumbering::component_wise(dof_handler, sub_blocks);
             DoFRenumbering::component_wise(dof_handler_displacement);
 
-            DoFRenumbering::hierarchical(dof_handler);
             DoFRenumbering::hierarchical(dof_handler_displacement);
+
+            dof_handler_displacement.distribute_mg_dofs();
         } else {
             throw;
         }
@@ -459,69 +463,77 @@ KktSystem<dim>::setup_boundary_values()
 
             }
             const unsigned int n_levels = triangulation.n_levels();
-            level_dirichlet_boundaries.resize(0,n_levels-1);
+            level_dirichlet_boundary_dofs.resize(0,n_levels-1);
             level_boundary_values.resize(0,n_levels-1);
-            for (unsigned int level = 0; level < n_levels; ++level)
+            mg_level_constraints.resize(0,n_levels-1);
+
+            for(unsigned int level = 0; level < n_levels; ++level)
             {
-                for (auto cell=dof_handler_displacement.begin_active(level); cell!=dof_handler_displacement.end_active(level); ++cell)
+                mg_level_constraints[level].reinit(dof_handler_displacement.locally_owned_mg_dofs (level));
+            }
+
+
+            for (auto cell=dof_handler_displacement.begin_active(n_levels-1); cell!=dof_handler_displacement.end_active(n_levels-1); ++cell)
+            {
+                if(cell->is_locally_owned())
                 {
-                    if(cell->is_locally_owned())
+                    for (unsigned int face_number = 0;
+                         face_number < GeometryInfo<dim>::faces_per_cell;
+                         ++face_number)
                     {
-                        for (unsigned int face_number = 0;
-                             face_number < GeometryInfo<dim>::faces_per_cell;
-                             ++face_number)
+                        if (cell->face(face_number)->at_boundary())
                         {
-                            if (cell->face(face_number)->at_boundary())
+                            for (unsigned int vertex_number = 0;
+                                 vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                                 ++vertex_number)
                             {
-                                for (unsigned int vertex_number = 0;
-                                     vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                                     ++vertex_number)
+                                const auto vert = cell->vertex(vertex_number);
+                                /*Find bottom left corner*/
+                                if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
+                                        vert(1) - 0) < 1e-12)
                                 {
-                                    const auto vert = cell->vertex(vertex_number);
-                                    /*Find bottom left corner*/
-                                    if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
-                                            vert(1) - 0) < 1e-12)
-                                    {
+                                     for (unsigned int level = 0; level < n_levels; ++level)
+                                     {
 
-                                        const unsigned int x_displacement =
-                                                cell->vertex_dof_index(vertex_number, 0, 0);
+                                         const unsigned int x_displacement =
+                                                 cell->mg_vertex_dof_index(level, vertex_number, 0,0);
 
-                                        const unsigned int y_displacement =
-                                                cell->vertex_dof_index(vertex_number, 1, 0);
-                                        /*set bottom left BC*/
-                                        level_dirichlet_boundaries[level].insert(x_displacement);
-                                        level_dirichlet_boundaries[level].insert(y_displacement);
+                                         const unsigned int y_displacement =
+                                                 cell->mg_vertex_dof_index(level, vertex_number, 1,0);
+                                         /*set bottom left BC*/
+                                         level_dirichlet_boundary_dofs[level].insert(x_displacement);
+                                         level_dirichlet_boundary_dofs[level].insert(y_displacement);
 
-                                        if(level == n_levels-1)
-                                        {
-                                            dirichlet_boundary.insert(x_displacement);
-                                            dirichlet_boundary.insert(y_displacement);
-                                        }
+                                         level_boundary_values[level][x_displacement] = 0;
+                                         level_boundary_values[level][y_displacement] = 0;
+                                     }
 
-                                        level_boundary_values[level][x_displacement] = 0;
-                                        level_boundary_values[level][y_displacement] = 0;
-                                    }
-                                    /*Find bottom right corner*/
-                                    if (std::fabs(vert(0) - 6) < 1e-12 && std::fabs(
-                                            vert(1) - 0) < 1e-12)
+                                }
+                                /*Find bottom right corner*/
+                                if (std::fabs(vert(0) - 6) < 1e-12 && std::fabs(
+                                        vert(1) - 0) < 1e-12)
+                                {
+                                    for (unsigned int level = 0; level < n_levels; ++level)
                                     {
+
                                         const unsigned int y_displacement =
-                                                cell->vertex_dof_index(vertex_number, 1, 0);
+                                                cell->mg_vertex_dof_index(level, vertex_number, 1, 0);
                                         level_boundary_values[level][y_displacement] = 0;
-                                        if(level == n_levels-1)
-                                        {
-                                            dirichlet_boundary.insert(y_displacement);
-                                        }
-                                        level_dirichlet_boundaries[level].insert(y_displacement);
+                                        level_dirichlet_boundary_dofs[level].insert(y_displacement);
                                     }
+
                                 }
                             }
                         }
                     }
                 }
-
-
             }
+            for (unsigned int level = 0; level < n_levels; ++level)
+            {
+                mg_level_constraints[level].add_lines(level_dirichlet_boundary_dofs[level]);
+                mg_level_constraints[level].close();
+            }
+
         } else if (dim == 3)
         {
             for (const auto &cell: dof_handler.active_cell_iterators()) {
@@ -620,18 +632,18 @@ KktSystem<dim>::setup_boundary_values()
                                     {
 
                                         const unsigned int x_displacement =
-                                                cell->vertex_dof_index(vertex_number, 0, 0);
+                                                cell->mg_vertex_dof_index(level, vertex_number, 0, 0);
                                         const unsigned int y_displacement =
-                                                cell->vertex_dof_index(vertex_number, 1, 0);
+                                                cell->mg_vertex_dof_index(level, vertex_number, 1, 0);
                                         const unsigned int z_displacement =
-                                                cell->vertex_dof_index(vertex_number, 2, 0);
+                                                cell->mg_vertex_dof_index(level, vertex_number, 2, 0);
                                         /*set bottom left BC*/
                                         level_boundary_values[level][x_displacement] = 0;
                                         level_boundary_values[level][y_displacement] = 0;
                                         level_boundary_values[level][z_displacement] = 0;
-                                        level_dirichlet_boundaries[level].insert(x_displacement);
-                                        level_dirichlet_boundaries[level].insert(y_displacement);
-                                        level_dirichlet_boundaries[level].insert(z_displacement);
+                                        level_dirichlet_boundary_dofs[level].insert(x_displacement);
+                                        level_dirichlet_boundary_dofs[level].insert(y_displacement);
+                                        level_dirichlet_boundary_dofs[level].insert(z_displacement);
                                     }
                                     /*Find bottom right corner*/
                                     if (std::fabs(vert(0) - 6) < 1e-12 && std::fabs(
@@ -640,23 +652,26 @@ KktSystem<dim>::setup_boundary_values()
                                                 vert(2) - 1) < 1e-12)))
                                     {
                                         const unsigned int y_displacement =
-                                                cell->vertex_dof_index(vertex_number, 1, 0);
+                                                cell->mg_vertex_dof_index(level, vertex_number, 1, 0);
                                         const unsigned int z_displacement =
-                                                cell->vertex_dof_index(vertex_number, 2, 0);
+                                                cell->mg_vertex_dof_index(level, vertex_number, 2, 0);
                                         level_boundary_values[level][y_displacement] = 0;
                                         level_boundary_values[level][z_displacement] = 0;
 
-                                        level_dirichlet_boundaries[level].insert(y_displacement);
-                                        level_dirichlet_boundaries[level].insert(z_displacement);
+                                        level_dirichlet_boundary_dofs[level].insert(y_displacement);
+                                        level_dirichlet_boundary_dofs[level].insert(z_displacement);
                                     }
                                 }
                             }
                         }
                     }
                 }
+                mg_level_constraints[level].add_lines(level_dirichlet_boundary_dofs[level]);
+                mg_level_constraints[level].close();
             }
 
 
+
         }
         else
         {
@@ -742,15 +757,15 @@ KktSystem<dim>::setup_boundary_values()
                                     {
 
                                         const unsigned int x_displacement =
-                                                cell->vertex_dof_index(vertex_number, 0, 0);
+                                                cell->mg_vertex_dof_index(level, vertex_number, 0, 0);
                                         const unsigned int y_displacement =
-                                                cell->vertex_dof_index(vertex_number, 1, 0);
+                                                cell->mg_vertex_dof_index(level, vertex_number, 1, 0);
                                         /*set bottom left BC*/
                                         level_boundary_values[level][x_displacement] = 0;
                                         level_boundary_values[level][y_displacement] = 0;
 
-                                        level_dirichlet_boundaries[level].insert(x_displacement);
-                                        level_dirichlet_boundaries[level].insert(y_displacement);
+                                        level_dirichlet_boundary_dofs[level].insert(x_displacement);
+                                        level_dirichlet_boundary_dofs[level].insert(y_displacement);
 
                                     }
                                     /*Find bottom right corner*/
@@ -758,21 +773,24 @@ KktSystem<dim>::setup_boundary_values()
                                                 vert(1) - 2) < 1e-12)
                                     {
                                         const unsigned int x_displacement =
-                                                cell->vertex_dof_index(vertex_number, 0, 0);
+                                                cell->mg_vertex_dof_index(level, vertex_number, 0, 0);
                                         const unsigned int y_displacement =
-                                                cell->vertex_dof_index(vertex_number, 1, 0);
+                                                cell->mg_vertex_dof_index(level, vertex_number, 1, 0);
                                         level_boundary_values[level][x_displacement] = 0;
                                         level_boundary_values[level][y_displacement] = 0;
 
-                                        level_dirichlet_boundaries[level].insert(x_displacement);
-                                        level_dirichlet_boundaries[level].insert(y_displacement);
+                                        level_dirichlet_boundary_dofs[level].insert(x_displacement);
+                                        level_dirichlet_boundary_dofs[level].insert(y_displacement);
                                     }
                                 }
                             }
                         }
                     }
                 }
+                mg_level_constraints[level].add_lines(level_dirichlet_boundary_dofs[level]);
+                mg_level_constraints[level].close();
             }
+
         }
         else if (dim == 3)
         {
@@ -843,74 +861,77 @@ KktSystem<dim>::setup_boundary_values()
                     }
                 }
             }
-        const unsigned int n_levels = triangulation.n_levels();
-        for (unsigned int level = 0; level < n_levels; ++level)
-        {
-            for (auto cell=dof_handler_displacement.begin_active(level);
-                 cell!=dof_handler.end_active(level);
-                 ++cell)
+            const unsigned int n_levels = triangulation.n_levels();
+            for (unsigned int level = 0; level < n_levels; ++level)
             {
-                if(cell->is_locally_owned())
+                for (auto cell=dof_handler_displacement.begin_active(level);
+                     cell!=dof_handler.end_active(level);
+                     ++cell)
                 {
-                    for (unsigned int face_number = 0;
-                         face_number < GeometryInfo<dim>::faces_per_cell;
-                         ++face_number)
+                    if(cell->is_locally_owned())
                     {
-                        if (cell->face(face_number)->at_boundary())
+                        for (unsigned int face_number = 0;
+                             face_number < GeometryInfo<dim>::faces_per_cell;
+                             ++face_number)
                         {
-                            for (unsigned int vertex_number = 0;
-                                 vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                                 ++vertex_number)
+                            if (cell->face(face_number)->at_boundary())
                             {
-                                const auto vert = cell->vertex(vertex_number);
-                                /*Find bottom left corner*/
-                                if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
-                                            vert(1) - 2) < 1e-12 && ((std::fabs(
-                                            vert(2) - 0) < 1e-12) || (std::fabs(
-                                            vert(2) - 1) < 1e-12)))
+                                for (unsigned int vertex_number = 0;
+                                     vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                                     ++vertex_number)
                                 {
+                                    const auto vert = cell->vertex(vertex_number);
+                                    /*Find bottom left corner*/
+                                    if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
+                                                vert(1) - 2) < 1e-12 && ((std::fabs(
+                                                vert(2) - 0) < 1e-12) || (std::fabs(
+                                                vert(2) - 1) < 1e-12)))
+                                    {
 
-                                    const unsigned int x_displacement =
-                                            cell->vertex_dof_index(vertex_number, 0, 0);
-                                    const unsigned int y_displacement =
-                                            cell->vertex_dof_index(vertex_number, 1, 0);
-                                    const unsigned int z_displacement =
-                                            cell->vertex_dof_index(vertex_number, 2, 0);
-                                    /*set bottom left BC*/
-                                    level_boundary_values[level][x_displacement] = 0;
-                                    level_boundary_values[level][y_displacement] = 0;
-                                    level_boundary_values[level][z_displacement] = 0;
+                                        const unsigned int x_displacement =
+                                                cell->mg_vertex_dof_index(level, vertex_number, 0, 0);
+                                        const unsigned int y_displacement =
+                                                cell->mg_vertex_dof_index(level, vertex_number, 1, 0);
+                                        const unsigned int z_displacement =
+                                                cell->mg_vertex_dof_index(level, vertex_number, 2, 0);
+                                        /*set bottom left BC*/
+                                        level_boundary_values[level][x_displacement] = 0;
+                                        level_boundary_values[level][y_displacement] = 0;
+                                        level_boundary_values[level][z_displacement] = 0;
 
-                                    level_dirichlet_boundaries[level].insert(x_displacement);
-                                    level_dirichlet_boundaries[level].insert(y_displacement);
-                                    level_dirichlet_boundaries[level].insert(z_displacement);
-                                }
-                                /*Find bottom right corner*/
-                                if (std::fabs(vert(0) - 1) < 1e-12 && std::fabs(
-                                            vert(1) - 2) < 1e-12 && ((std::fabs(
-                                            vert(2) - 0) < 1e-12) || (std::fabs(
-                                            vert(2) - 1) < 1e-12)))
-                                {
-                                    const unsigned int x_displacement =
-                                            cell->vertex_dof_index(vertex_number, 0, 0);
-                                    const unsigned int y_displacement =
-                                            cell->vertex_dof_index(vertex_number, 1, 0);
-                                    const unsigned int z_displacement =
-                                            cell->vertex_dof_index(vertex_number, 2, 0);
-                                    level_boundary_values[level][x_displacement] = 0;
-                                    level_boundary_values[level][y_displacement] = 0;
-                                    level_boundary_values[level][z_displacement] = 0;
-
-                                    level_dirichlet_boundaries[level].insert(x_displacement);
-                                    level_dirichlet_boundaries[level].insert(y_displacement);
-                                    level_dirichlet_boundaries[level].insert(z_displacement);
+                                        level_dirichlet_boundary_dofs[level].insert(x_displacement);
+                                        level_dirichlet_boundary_dofs[level].insert(y_displacement);
+                                        level_dirichlet_boundary_dofs[level].insert(z_displacement);
+                                    }
+                                    /*Find bottom right corner*/
+                                    if (std::fabs(vert(0) - 1) < 1e-12 && std::fabs(
+                                                vert(1) - 2) < 1e-12 && ((std::fabs(
+                                                vert(2) - 0) < 1e-12) || (std::fabs(
+                                                vert(2) - 1) < 1e-12)))
+                                    {
+                                        const unsigned int x_displacement =
+                                                cell->mg_vertex_dof_index(level, vertex_number, 0, 0);
+                                        const unsigned int y_displacement =
+                                                cell->mg_vertex_dof_index(level, vertex_number, 1, 0);
+                                        const unsigned int z_displacement =
+                                                cell->mg_vertex_dof_index(level, vertex_number, 2, 0);
+                                        level_boundary_values[level][x_displacement] = 0;
+                                        level_boundary_values[level][y_displacement] = 0;
+                                        level_boundary_values[level][z_displacement] = 0;
+
+                                        level_dirichlet_boundary_dofs[level].insert(x_displacement);
+                                        level_dirichlet_boundary_dofs[level].insert(y_displacement);
+                                        level_dirichlet_boundary_dofs[level].insert(z_displacement);
+                                    }
                                 }
                             }
                         }
                     }
                 }
+                mg_level_constraints[level].add_lines(level_dirichlet_boundary_dofs[level]);
+                mg_level_constraints[level].close();
             }
-        }
+
         } else {
             throw;
         }
@@ -1456,7 +1477,7 @@ KktSystem<dim>::assemble_block_system(const LA::MPI::BlockVector &distributed_st
         }
 
     }
-    pcout << "compress 1" << std::endl;
+
     system_matrix.compress(VectorOperation::add);
     system_rhs = calculate_rhs(distributed_state, barrier_size);
 
@@ -1485,7 +1506,6 @@ KktSystem<dim>::assemble_block_system(const LA::MPI::BlockVector &distributed_st
                     cell->measure());
         }
     }
-    pcout << "compress 2" << std::endl;
     system_matrix.compress(VectorOperation::add);
 
 
@@ -1579,7 +1599,6 @@ KktSystem<dim>::calculate_objective_value(const LA::MPI::BlockVector &distribute
     double objective_value_out;
     MPI_Allreduce(&objective_value, &objective_value_out, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
 
-    pcout << "objective value: " << objective_value_out << std::endl;
     return objective_value;
 }
 
@@ -1602,8 +1621,6 @@ KktSystem<dim>::calculate_barrier_distance(const LA::MPI::BlockVector &state) co
     double out_barrier_distance_log_sum;
     MPI_Allreduce(&barrier_distance_log_sum, &out_barrier_distance_log_sum, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
 
-    pcout << "Barrier distance log sum: " << out_barrier_distance_log_sum << std::endl;
-
     return out_barrier_distance_log_sum;
 }
 
@@ -1653,7 +1670,6 @@ KktSystem<dim>::calculate_feasibility(const LA::MPI::BlockVector &state, const d
         MPI_Allreduce(&prod2, &full_prod2, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
         norm = norm + full_prod1 + full_prod2;
     }
-    pcout << "pre-norm: " << norm << std::endl;
 
     norm += std::pow(test_rhs.block(SolutionBlocks::displacement).l2_norm(), 2);
     norm += std::pow(test_rhs.block(SolutionBlocks::density).l2_norm(), 2);
@@ -1664,8 +1680,6 @@ KktSystem<dim>::calculate_feasibility(const LA::MPI::BlockVector &state, const d
     norm += std::pow(test_rhs.block(SolutionBlocks::density_upper_slack_multiplier).l2_norm(), 2);
     norm += std::pow(test_rhs.block(SolutionBlocks::density_lower_slack_multiplier).l2_norm(), 2);
 
-    pcout << "norm: " << norm << std::endl;
-
     return norm;
 }
 
@@ -1706,7 +1720,7 @@ KktSystem<dim>::calculate_convergence(const LA::MPI::BlockVector &state) const {
         MPI_Allreduce(&prod2, &full_prod2, 1, MPI_DOUBLE, MPI_PROD, MPI_COMM_WORLD);
         norm = norm + full_prod1 + full_prod2;
     }
-    pcout << "pre-norm: " << norm << std::endl;
+
 
     norm += std::pow(test_rhs.block(SolutionBlocks::displacement).l2_norm(), 2);
     norm += std::pow(test_rhs.block(SolutionBlocks::density).l2_norm(), 2);
@@ -1719,7 +1733,6 @@ KktSystem<dim>::calculate_convergence(const LA::MPI::BlockVector &state) const {
 
     norm = std::pow(norm, .5);
 
-    pcout << "l2 norm: " << system_rhs.l2_norm() << std::endl;
     pcout << "KKT norm: " << norm << std::endl;
     return norm;
 }
@@ -2076,7 +2089,6 @@ KktSystem<dim>::calculate_rhs(const LA::MPI::BlockVector &distributed_state, con
     MPI_Allreduce(&total_volume_temp, &total_volume, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
     MPI_Allreduce(&goal_volume_temp, &goal_volume, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
 
-    pcout << "total_volume: " << total_volume  << " and goal_volume: " << goal_volume << std::endl;
 
     if (test_rhs.block(SolutionBlocks::total_volume_multiplier).in_local_range(0))
     {
@@ -2107,25 +2119,24 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
     locally_relevant_solution=state;
     distributed_solution = state;
 
-    SolverControl solver_control(100, 1e-9);
+    SolverControl solver_control(100, gmres_tolerance);
 
+
+    // ************ BEGIN MAKING MF GMG ELASTICITY PRECONDITIONER ***************************
     using SystemMFMatrixType = MF_Elasticity_Operator<dim, 1, double>;
     using LevelMFMatrixType = MF_Elasticity_Operator<dim, 1, double>;
 
-    elasticity_system_mf.clear();
+    elasticity_matrix_mf.clear();
     mg_matrices.clear_elements();
-    dof_handler_displacement.distribute_mg_dofs();
+
+
     pcout << "Number of degrees of freedom: " << dof_handler_displacement.n_dofs()
           << std::endl;
-    IndexSet locally_relevant_dofs;
-    DoFTools::extract_locally_relevant_dofs(dof_handler_displacement, locally_relevant_dofs);
-
-    constraints.clear();
-        constraints.reinit(locally_relevant_dofs);
-        constraints.add_lines( dirichlet_boundary);
-    constraints.close();
 
 
+    IndexSet locally_relevant_dofs;
+    DoFTools::extract_locally_relevant_dofs(dof_handler_displacement, locally_relevant_dofs);
+    displacement_constraints.copy_from(mg_level_constraints[triangulation.n_global_levels()-1]);
 
     {
         typename MatrixFree<dim, double>::AdditionalData additional_data;
@@ -2136,18 +2147,18 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
         std::shared_ptr<MatrixFree<dim, double>> system_mf_storage(
                     new MatrixFree<dim, double>());
         system_mf_storage->reinit(dof_handler_displacement,
-                                  constraints,
+                                  displacement_constraints,
                                   QGauss<1>(fe_displacement.degree + 1),
                                   additional_data);
-        elasticity_system_mf.initialize(system_mf_storage);
+        elasticity_matrix_mf.initialize(system_mf_storage);
     }
 
 
     LinearAlgebra::distributed::Vector<double> distributed_displacement_sol;
     LinearAlgebra::distributed::Vector<double> distributed_displacement_rhs;
 
-    elasticity_system_mf.initialize_dof_vector(distributed_displacement_sol);
-    elasticity_system_mf.initialize_dof_vector(distributed_displacement_rhs);
+    elasticity_matrix_mf.initialize_dof_vector(distributed_displacement_sol);
+    elasticity_matrix_mf.initialize_dof_vector(distributed_displacement_rhs);
 
     ChangeVectorTypes::copy(distributed_displacement_sol,distributed_solution.block(SolutionBlocks::displacement));
     ChangeVectorTypes::copy(distributed_displacement_rhs,system_rhs.block(SolutionBlocks::displacement));
@@ -2156,11 +2167,12 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
     mg_matrices.resize(0, n_levels - 1);
 
     mg_constrained_dofs.clear();
-
     mg_constrained_dofs.initialize(dof_handler_displacement);
 
-    mg_constrained_dofs.make_zero_boundary_constraints(dof_handler_displacement,
-                                                           dirichlet_boundary);
+    for (unsigned int level = 0; level < n_levels; ++level)
+    {
+        mg_constrained_dofs.add_user_constraints(level, mg_level_constraints[level]);
+    }
 
 
 
@@ -2170,10 +2182,6 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
         DoFTools::extract_locally_relevant_level_dofs(dof_handler_displacement,
                                                       level,
                                                       relevant_dofs);
-        AffineConstraints<double> level_constraints;
-            level_constraints.reinit(relevant_dofs);
-            level_constraints.add_lines(mg_constrained_dofs.get_boundary_indices(level));
-        level_constraints.close();
 
         typename MatrixFree<dim, double>::AdditionalData additional_data;
         additional_data.tasks_parallel_scheme =
@@ -2185,7 +2193,7 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
         std::shared_ptr<MatrixFree<dim, double>> mg_mf_storage_level(
                     new MatrixFree<dim, double>());
         mg_mf_storage_level->reinit(dof_handler_displacement,
-                                    level_constraints,
+                                    mg_level_constraints[level],
                                     QGauss<1>(fe_displacement.degree + 1),
                                     additional_data);
 
@@ -2202,7 +2210,6 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
     dof_handler_density.distribute_dofs(fe_density);
 
     DoFRenumbering::component_wise(dof_handler_density);
-
     DoFRenumbering::hierarchical(dof_handler_density);
 
     dof_handler_density.distribute_mg_dofs();
@@ -2211,7 +2218,7 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
 
     ChangeVectorTypes::copy(active_density_vector,distributed_solution.block(SolutionBlocks::density));
 
-    const unsigned int n_cells = elasticity_system_mf.get_matrix_free()->n_cell_batches();
+    const unsigned int n_cells = elasticity_matrix_mf.get_matrix_free()->n_cell_batches();
     {
         active_cell_data.density.reinit(TableIndices<2>(n_cells, 1));
 
@@ -2232,7 +2239,6 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
         for (const auto &cell : dof_handler.active_cell_iterators())
             if (cell->is_locally_owned())
             {
-                // For each cell, create a local mass matrix and rhs.
 
                 hp_fe_values.reinit(cell);
                 const FEValues<dim> &fe_values = hp_fe_values.get_present_fe_values();
@@ -2279,11 +2285,11 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
     std::vector<types::global_dof_index> local_dof_indices(fe_density.dofs_per_cell);
     for (unsigned int cell=0; cell<n_cells; ++cell)
     {
-        const unsigned int n_components_filled = elasticity_system_mf.get_matrix_free()->n_active_entries_per_cell_batch(cell);
+        const unsigned int n_components_filled = elasticity_matrix_mf.get_matrix_free()->n_active_entries_per_cell_batch(cell);
 
         for (unsigned int i=0; i<n_components_filled; ++i)
         {
-            typename DoFHandler<dim>::active_cell_iterator FEQ_cell =elasticity_system_mf.get_matrix_free()->get_cell_iterator(cell,i);
+            typename DoFHandler<dim>::active_cell_iterator FEQ_cell =elasticity_matrix_mf.get_matrix_free()->get_cell_iterator(cell,i);
             typename DoFHandler<dim>::active_cell_iterator DG_cell(&(triangulation),
                                                                    FEQ_cell->level(),
                                                                    FEQ_cell->index(),
@@ -2304,19 +2310,10 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
 
     transfer.build(dof_handler_density);
 
-    std::cout << "level_density_size: " << level_density_vector[0].locally_owned_size() << std::endl;
-    std::cout << "active_density_size: " << active_density_vector.locally_owned_size() << std::endl;
-
-
     transfer.template interpolate_to_mg<double>(dof_handler_density,
                                                 level_density_vector,
                                                 active_density_vector);
 
-
-
-
-
-
     // MAKE LEVEL_CELL_DATA
     for (unsigned int level=0; level<n_levels; ++level)
     {
@@ -2338,7 +2335,6 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
 
 
                 level_cell_data[level].density(cell, 0)[i] = level_density_vector[level](local_dof_indices[0]);
-
             }
 
         }
@@ -2346,15 +2342,9 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
         // Store density tables and other data into the multigrid level matrix-free objects.
 
         mg_matrices[level].set_cell_data (level_cell_data[level]);
-
-
     }
 
-    elasticity_system_mf.set_cell_data(active_cell_data);
-
-
-
-
+    elasticity_matrix_mf.set_cell_data(active_cell_data);
 
     //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
@@ -2405,62 +2395,65 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
             mf_gmg_preconditioner(dof_handler_displacement, mg, mg_transfer);
 
 
+//*************TEST SOLVE*************************
+//    elasticity_matrix_mf.initialize_dof_vector(distributed_displacement_sol);
+//    elasticity_matrix_mf.initialize_dof_vector(distributed_displacement_rhs);
 
-    elasticity_system_mf.initialize_dof_vector(distributed_displacement_sol);
-    elasticity_system_mf.initialize_dof_vector(distributed_displacement_rhs);
+//    ChangeVectorTypes::copy(distributed_displacement_sol,distributed_solution.block(SolutionBlocks::displacement));
+//    ChangeVectorTypes::copy(distributed_displacement_rhs,system_rhs.block(SolutionBlocks::displacement));
 
+//    SolverControl test_solver_control(500, 1e-6);
+//    SolverCG<LinearAlgebra::distributed::Vector<double>> CG_Solve(test_solver_control);
 
+//    std::cout << "pre norm: " << distributed_displacement_rhs.l2_norm() << std::endl;
 
-    ChangeVectorTypes::copy(distributed_displacement_sol,distributed_solution.block(SolutionBlocks::displacement));
-    ChangeVectorTypes::copy(distributed_displacement_rhs,system_rhs.block(SolutionBlocks::displacement));
+//    try
+//    {
+////        CG_Solve.solve(elasticity_matrix_mf, distributed_displacement_sol, -1* distributed_displacement_rhs,    PreconditionIdentity());
+//        CG_Solve.solve(elasticity_matrix_mf, distributed_displacement_sol, -1* distributed_displacement_rhs, mf_gmg_preconditioner  );
+//    }
+//    catch(std::exception &exc)
+//    {
+//        std::cout << "solve failed in " << test_solver_control.last_step() <<  " steps" << std::endl;
+//        throw;
+//    }
 
-    SolverCG<LinearAlgebra::distributed::Vector<double>> CG_Solve(solver_control);
+//    std::cout << "solved in " << test_solver_control.last_step() <<  " steps" << std::endl;
 
-    std::cout << "from here" << std::endl;
+//    ChangeVectorTypes::copy(distributed_solution.block(SolutionBlocks::displacement), distributed_displacement_sol);
+//    displacement_constraints.distribute(distributed_solution);
+//    output(distributed_solution, 0);
+//    std::abort();
 
-    std::cout << "pre norm" << distributed_displacement_rhs.l2_norm() << std::endl;
+    //***************END TEST SOLVE*************************
 
-    try
-    {
-        CG_Solve.solve(elasticity_system_mf, distributed_displacement_sol, -1* distributed_displacement_rhs, mf_gmg_preconditioner  /* PreconditionIdentity()*/);
-    }
-    catch(std::exception &exc)
-    {
-
-    }
-
-
-    ChangeVectorTypes::copy(distributed_solution.block(SolutionBlocks::displacement), distributed_displacement_sol);
-    output(distributed_solution, 0);
-    std::abort();
-
-    std::cout << "to here" << std::endl;
 
 
-    //        TopOptSchurPreconditioner<dim> preconditioner(system_matrix, dof_handler, elasticity_system_mf, mf_gmg_preconditioner);
-    //        pcout << "about to solve" << std::endl;
+    TopOptSchurPreconditioner<dim> preconditioner(system_matrix, dof_handler, elasticity_matrix_mf, mf_gmg_preconditioner);
+    pcout << "about to solve" << std::endl;
 
-    //        switch (Input::solver_choice) {
+    switch (Input::solver_choice)
+    {
 
-    //            case SolverOptions::inexact_K_with_exact_A_gmres: {
+        case SolverOptions::inexact_K_with_exact_A_gmres: {
 
 
-    //                preconditioner.initialize(system_matrix, boundary_values, dof_handler, distributed_solution);
-    //                SolverFGMRES<LA::MPI::BlockVector> B_fgmres(solver_control);
-    //                B_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
-    //                pcout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
-    //                break;
-    //            }
-    //            case SolverOptions::inexact_K_with_inexact_A_gmres: {
-    //                preconditioner.initialize(system_matrix, boundary_values, dof_handler, distributed_solution);
-    //                SolverFGMRES<LA::MPI::BlockVector> C_fgmres(solver_control);
-    //                C_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
-    //                pcout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
-    //                break;
-    //            }
-    //            default:
-    //                throw;
-    //        }
+            preconditioner.initialize(system_matrix, boundary_values, dof_handler, distributed_solution);
+            SolverFGMRES<LA::MPI::BlockVector> B_fgmres(solver_control);
+            B_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
+            pcout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
+            break;
+        }
+        case SolverOptions::inexact_K_with_inexact_A_gmres: {
+            preconditioner.initialize(system_matrix, boundary_values, dof_handler, distributed_solution);
+            SolverFGMRES<LA::MPI::BlockVector> C_fgmres(solver_control);
+            C_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
+            pcout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
+            break;
+        }
+        default:
+            throw;
+    }
 
     constraints.distribute(distributed_solution);
 
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index c8c660e..c6abc7e 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -19,9 +19,33 @@ namespace SAND {
     using PayloadType = dealii::TrilinosWrappers::internal::LinearOperatorImplementation::TrilinosPayload;
     using PayloadVectorType = typename PayloadType::VectorType;
     using size_type         = dealii::types::global_dof_index;
+
+    namespace ChangeVectorTypes
+    {
+    template <typename number>
+    void copy(LA::MPI::Vector &                                         out,
+              const dealii::LinearAlgebra::distributed::Vector<number> &in)
+    {
+        dealii::LinearAlgebra::ReadWriteVector<double> rwv(
+                    out.locally_owned_elements());
+        rwv.import(in, VectorOperation::insert);
+        out.import(rwv, VectorOperation::insert);
+    }
+    template <typename number>
+    void copy(dealii::LinearAlgebra::distributed::Vector<number> &out,
+              const LA::MPI::Vector &                             in)
+    {
+        dealii::LinearAlgebra::ReadWriteVector<double> rwv;
+        rwv.reinit(in);
+        out.import(rwv, VectorOperation::insert);
+    }
+    } // namespace ChangeVectorTypes
+
+
+
     using namespace dealii;
     template<int dim>
-    TopOptSchurPreconditioner<dim>::TopOptSchurPreconditioner(LA::MPI::BlockSparseMatrix &matrix_in, DoFHandler<dim> &big_dof_handler_in, MF_Elasticity_Operator<dim,1,double> &mf_elasticity_operator_in , PreconditionMG<dim,LinearAlgebra::distributed::Vector<double>,MGTransferMatrixFree<dim, double>>
+    TopOptSchurPreconditioner<dim>::TopOptSchurPreconditioner(LA::MPI::BlockSparseMatrix &matrix_in, DoFHandler<dim> &big_dof_handler_in, MF_Elasticity_Operator<dim,1,double> &mf_elasticity_operator_in , PreconditionMG<dim,LinearAlgebra::distributed::Vector<double> ,MGTransferMatrixFree<dim, double>>
                                                               &mf_gmg_preconditioner_in)
             :
             system_matrix(matrix_in),
@@ -47,15 +71,14 @@ namespace SAND {
             additional_data(false, solver_type),
             direct_solver_control(1, 0),
             a_inv_direct(direct_solver_control, additional_data),
+            a_inv_mf_gmg(mf_elasticity_operator_in, mf_gmg_preconditioner_in),
             pcout(std::cout,(Utilities::MPI::this_mpi_process(mpi_communicator) == 0)),
             timer(pcout, TimerOutput::summary, TimerOutput::wall_times),
             g_mat(f_mat, d_8_mat),
-            h_mat(a_mat, b_mat, c_mat, e_mat, pre_amg, a_inv_direct),
+            h_mat(a_mat, b_mat, c_mat, e_mat, pre_amg, a_inv_direct, a_inv_mf_gmg),
             j_inv_mat(h_mat, g_mat, d_m_mat, d_m_inv_mat),
             k_inv_mat(h_mat, g_mat, d_m_mat, d_m_inv_mat),
-            big_dof_handler(big_dof_handler_in),
-            mf_elasticity_operator(mf_elasticity_operator_in),
-            mf_gmg_preconditioner(mf_gmg_preconditioner_in)
+            big_dof_handler(big_dof_handler_in)
     {
 
     }
@@ -118,28 +141,28 @@ namespace SAND {
 
 
 
-        if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
-        {
-            TimerOutput::Scope t(timer, "build A inv");
+//        if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
+//        {
+//            TimerOutput::Scope t(timer, "build A inv");
 
-             std::vector<std::vector<bool>> constant_modes;
-             FEValuesExtractors::Vector     displacement_components(SolutionComponents::displacement<dim>);
-             DoFTools::extract_constant_modes(dof_handler,
-                                              dof_handler.get_fe_collection()
-                                              .component_mask(displacement_components),
-                                              constant_modes);
-             TrilinosWrappers::PreconditionAMG::AdditionalData amg_data;
-             amg_data.constant_modes = constant_modes;
-//             amg_data.n_cycles = 1;
-//             amg_data.w_cycle = true;
+//             std::vector<std::vector<bool>> constant_modes;
+//             FEValuesExtractors::Vector     displacement_components(SolutionComponents::displacement<dim>);
+//             DoFTools::extract_constant_modes(dof_handler,
+//                                              dof_handler.get_fe_collection()
+//                                              .component_mask(displacement_components),
+//                                              constant_modes);
+//             TrilinosWrappers::PreconditionAMG::AdditionalData amg_data;
+//             amg_data.constant_modes = constant_modes;
+////             amg_data.n_cycles = 1;
+////             amg_data.w_cycle = true;
 
-             pre_amg.initialize(a_mat,amg_data);
+//             pre_amg.initialize(a_mat,amg_data);
 
-        }
-        else
-        {
-             a_inv_direct.initialize(a_mat);
-        }
+//        }
+//        else
+//        {
+//             a_inv_direct.initialize(a_mat);
+//        }
         {
             TimerOutput::Scope t(timer, "reinit diag matrices");
             d_3_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
@@ -237,8 +260,6 @@ namespace SAND {
         d_8_mat.compress(VectorOperation::insert);
         d_m_inv_mat.compress(VectorOperation::insert);
 
-        pcout << "compressed" << std::endl;
-
         pre_j=distributed_state.block(SolutionBlocks::density);
         pre_k=distributed_state.block(SolutionBlocks::density);
         g_d_m_inv_density=distributed_state.block(SolutionBlocks::density);
@@ -259,35 +280,30 @@ namespace SAND {
     template<int dim>
     void TopOptSchurPreconditioner<dim>::vmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
         LA::MPI::BlockVector temp_src;
-        pcout << "vmult" << std::endl;
         {
             TimerOutput::Scope t(timer, "part 1");
             vmult_step_1(dst, src);
-            pcout << "step 1 done" << std::endl;
             temp_src = dst;
         }
 
         {
             TimerOutput::Scope t(timer, "part 2");
             vmult_step_2(dst, temp_src);
-            pcout << "step 2 done" << std::endl;
             temp_src = dst;
+
         }
 
         {
             TimerOutput::Scope t(timer, "part 3");
             vmult_step_3(dst, temp_src);
-            pcout << "step 3 done" << std::endl;
             temp_src = dst;
         }
         {
             TimerOutput::Scope t(timer, "part 4");
             vmult_step_4(dst, temp_src);
-            pcout << "step 4 done" << std::endl;
             temp_src = dst;
         }
         vmult_step_5(dst, temp_src);
-        pcout << "step 5 done" << std::endl;
     }
 
     template<int dim>
@@ -330,17 +346,22 @@ namespace SAND {
 
     template<int dim>
     void TopOptSchurPreconditioner<dim>::vmult_step_3(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
+
         dst = src;
         auto dst_temp = dst;
 
         if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
         {
-            SolverControl            solver_control_1(100000, 1e-6);
-            SolverControl            solver_control_2(100000, 1e-6);
-            SolverCG<LA::MPI::Vector> a_solver_cg_1(solver_control_1);
-            SolverCG<LA::MPI::Vector> a_solver_cg_2(solver_control_2);
-            a_solver_cg_1.solve(a_mat,dst_temp.block(SolutionBlocks::displacement_multiplier),src.block(SolutionBlocks::displacement_multiplier),pre_amg);
-            a_solver_cg_2.solve(a_mat,dst_temp.block(SolutionBlocks::displacement),src.block(SolutionBlocks::displacement),pre_amg);
+//            SolverControl            solver_control_1(100000, 1e-6);
+//            SolverControl            solver_control_2(100000, 1e-6);
+//            SolverCG<LA::MPI::Vector> a_solver_cg_1(solver_control_1);
+//            SolverCG<LA::MPI::Vector> a_solver_cg_2(solver_control_2);
+//            a_solver_cg_1.solve(a_mat,dst_temp.block(SolutionBlocks::displacement_multiplier),src.block(SolutionBlocks::displacement_multiplier),pre_amg);
+//            a_solver_cg_2.solve(a_mat,dst_temp.block(SolutionBlocks::displacement),src.block(SolutionBlocks::displacement),pre_amg);
+
+            a_inv_mf_gmg.vmult(dst_temp.block(SolutionBlocks::displacement_multiplier),src.block(SolutionBlocks::displacement_multiplier));
+
+            a_inv_mf_gmg.vmult(dst_temp.block(SolutionBlocks::displacement),src.block(SolutionBlocks::displacement));
             c_mat.Tvmult(dst_temp.block(SolutionBlocks::density_upper_slack),dst_temp.block(SolutionBlocks::displacement_multiplier));
             e_mat.Tvmult(dst_temp.block(SolutionBlocks::density_lower_slack),dst_temp.block(SolutionBlocks::displacement));
 
@@ -379,9 +400,9 @@ namespace SAND {
             }
             catch (std::exception &exc)
             {
-//                pcout << "Failure of linear solver step_4_gmres_1" << std::endl;
-//                pcout << "first residual: " << step_4_gmres_control_1.initial_value() << std::endl;
-//                pcout << "last residual: " << step_4_gmres_control_1.last_value() << std::endl;
+                pcout << "Failure of linear solver step_4_gmres_1" << std::endl;
+                pcout << "first residual: " << step_4_gmres_control_1.initial_value() << std::endl;
+                pcout << "last residual: " << step_4_gmres_control_1.last_value() << std::endl;
 //                throw;
             }
             SolverControl step_4_gmres_control_2 (Input::k_inv_iterations, 1e-6 * src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm() );
@@ -390,9 +411,9 @@ namespace SAND {
                 step_4_gmres_2.solve(k_inv_mat,k_density_mult,src.block(SolutionBlocks::unfiltered_density_multiplier), PreconditionIdentity());
             } catch (std::exception &exc)
             {
-//                pcout << "Failure of linear solver step_4_gmres_2" << std::endl;
-//                pcout << "first residual: " << step_4_gmres_control_2.initial_value() << std::endl;
-//                pcout << "last residual: " << step_4_gmres_control_2.last_value() << std::endl;
+                pcout << "Failure of linear solver step_4_gmres_2" << std::endl;
+                pcout << "first residual: " << step_4_gmres_control_2.initial_value() << std::endl;
+                pcout << "last residual: " << step_4_gmres_control_2.last_value() << std::endl;
 //                throw;
             }
 
@@ -417,7 +438,6 @@ namespace SAND {
             dst.block(SolutionBlocks::density_lower_slack) = linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density_lower_slack_multiplier);
             dst.block(SolutionBlocks::density_upper_slack) = linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density_upper_slack_multiplier);
         }
-            pcout << "inv1 done" << std::endl;
 
         {
             //Second Block Inverse
@@ -425,7 +445,6 @@ namespace SAND {
             dst.block(SolutionBlocks::unfiltered_density) =
                     linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) * src.block(SolutionBlocks::unfiltered_density);
         }
-            pcout << "inv2 done" << std::endl;
 
 
         {
@@ -433,13 +452,18 @@ namespace SAND {
             TimerOutput::Scope t(timer, "inverse 3");
             if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
             {
-                SolverControl            solver_control(100000, 1e-6);
-                SolverCG<LA::MPI::Vector> a_solver_cg(solver_control);
+//                SolverControl            solver_control(100000, 1e-6);
+//                SolverCG<LA::MPI::Vector> a_solver_cg(solver_control);
+
+//                auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg, pre_amg);
+
+//                dst.block(SolutionBlocks::displacement) = a_inv_op * src.block(SolutionBlocks::displacement_multiplier);
+//                dst.block(SolutionBlocks::displacement_multiplier) = a_inv_op * src.block(SolutionBlocks::displacement);
+
+                  a_inv_mf_gmg.vmult( dst.block(SolutionBlocks::displacement), src.block(SolutionBlocks::displacement_multiplier));
+                  a_inv_mf_gmg.vmult( dst.block(SolutionBlocks::displacement_multiplier), src.block(SolutionBlocks::displacement));
 
-                auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg, pre_amg);
 
-                dst.block(SolutionBlocks::displacement) = a_inv_op * src.block(SolutionBlocks::displacement_multiplier);
-                dst.block(SolutionBlocks::displacement_multiplier) = a_inv_op * src.block(SolutionBlocks::displacement);
             }
             else
             {
@@ -491,9 +515,9 @@ namespace SAND {
                     step_5_gmres_1.solve(j_inv_mat, dst.block(SolutionBlocks::unfiltered_density_multiplier), pre_j , preconditioner);
                 } catch (std::exception &exc)
                 {
-//                    pcout << "Failure of linear solver step_5_gmres_1" << std::endl;
-//                    pcout << "first residual: " << step_5_gmres_control_1.initial_value() << std::endl;
-//                    pcout << "last residual: " << step_5_gmres_control_1.last_value() << std::endl;
+                    pcout << "Failure of linear solver step_5_gmres_1" << std::endl;
+                    pcout << "first residual: " << step_5_gmres_control_1.initial_value() << std::endl;
+                    pcout << "last residual: " << step_5_gmres_control_1.last_value() << std::endl;
 //                    throw;
                 }
 
@@ -505,9 +529,9 @@ namespace SAND {
                     step_5_gmres_2.solve(k_inv_mat,dst.block(SolutionBlocks::density), pre_k , preconditioner);
                 } catch (std::exception &exc)
                 {
-//                    pcout << "Failure of linear solver step_5_gmres_2" << std::endl;
-//                    pcout << "first residual: " << step_5_gmres_control_2.initial_value() << std::endl;
-//                    pcout << "last residual: " << step_5_gmres_control_2.last_value() << std::endl;
+                    pcout << "Failure of linear solver step_5_gmres_2" << std::endl;
+                    pcout << "first residual: " << step_5_gmres_control_2.initial_value() << std::endl;
+                    pcout << "last residual: " << step_5_gmres_control_2.last_value() << std::endl;
 //                    throw;
                 }
             }
@@ -537,9 +561,9 @@ namespace SAND {
                     step_5_gmres_1.solve(j_inv_mat, dst.block(SolutionBlocks::unfiltered_density_multiplier), pre_j , preconditioner);
                 } catch (std::exception &exc)
                 {
-//                    pcout << "Failure of linear solver step_5_gmres_1" << std::endl;
-//                    pcout << "first residual: " << step_5_gmres_control_1.initial_value() << std::endl;
-//                    pcout << "last residual: " << step_5_gmres_control_1.last_value() << std::endl;
+                    pcout << "Failure of linear solver step_5_gmres_1" << std::endl;
+                    pcout << "first residual: " << step_5_gmres_control_1.initial_value() << std::endl;
+                    pcout << "last residual: " << step_5_gmres_control_1.last_value() << std::endl;
 //                    throw;
                 }
 
@@ -551,9 +575,9 @@ namespace SAND {
                     step_5_gmres_2.solve(k_inv_mat,dst.block(SolutionBlocks::density), pre_k , preconditioner);
                 } catch (std::exception &exc)
                 {
-//                    pcout << "Failure of linear solver step_5_gmres_2" << std::endl;
-//                    pcout << "first residual: " << step_5_gmres_control_2.initial_value() << std::endl;
-//                    pcout << "last residual: " << step_5_gmres_control_2.last_value() << std::endl;
+                    pcout << "Failure of linear solver step_5_gmres_2" << std::endl;
+                    pcout << "first residual: " << step_5_gmres_control_2.initial_value() << std::endl;
+                    pcout << "last residual: " << step_5_gmres_control_2.last_value() << std::endl;
 //                    throw;
                 }
             }
@@ -616,6 +640,61 @@ namespace SAND {
 
     }
 
+    // **************** A inv MF GMG **********************
+
+    template<int dim>
+    AInvMatMFGMG<dim>::AInvMatMFGMG(MF_Elasticity_Operator<dim,1,double> &mf_elasticity_operator_in , PreconditionMG<dim, LinearAlgebra::distributed::Vector<double>, MGTransferMatrixFree<dim, double> > &mf_gmg_preconditioner_in)
+        : mf_elasticity_operator(mf_elasticity_operator_in),
+          mf_gmg_preconditioner(mf_gmg_preconditioner_in)
+    {
+        mf_elasticity_operator.initialize_dof_vector(temp_dst);
+        mf_elasticity_operator.initialize_dof_vector(temp_src);
+    }
+
+    template<int dim>
+    void AInvMatMFGMG<dim>::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+    {
+
+        SolverControl            solver_control(iterations, 1e-6);
+        SolverCG<LinearAlgebra::distributed::Vector<double>> a_solver_cg(solver_control);
+
+        ChangeVectorTypes::copy(temp_src, src);
+        try{
+            a_solver_cg.solve(mf_elasticity_operator,temp_dst,temp_src, mf_gmg_preconditioner);
+        } catch (std::exception &exc)
+        {
+            std::cout << "failed with a reduction of: " << solver_control.initial_value()/solver_control.last_value() << std::endl;
+        }
+
+        ChangeVectorTypes::copy(dst,temp_dst);
+
+    }
+
+    template<int dim>
+    void AInvMatMFGMG<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const {
+        SolverControl            solver_control(iterations, src.l2_norm()*tolerance);
+        SolverCG<LinearAlgebra::distributed::Vector<double>> a_solver_cg(solver_control);
+
+        ChangeVectorTypes::copy(temp_src, src);
+        temp_dst = 0.;
+
+        a_solver_cg.solve(mf_elasticity_operator,temp_dst,temp_src, mf_gmg_preconditioner);
+
+        ChangeVectorTypes::copy(dst,temp_dst);
+    }
+
+    template<int dim>
+    void AInvMatMFGMG<dim>::set_tol(double tol_in)
+    {
+        tolerance = tol_in;
+    }
+    template<int dim>
+    void AInvMatMFGMG<dim>::set_iter(unsigned int iterations_in)
+    {
+        iterations = iterations_in;
+    }
+
+
     // ******************     GMATRIX     ***********************
 
     GMatrix::GMatrix(const LA::MPI::SparseMatrix &f_mat_in, LA::MPI::SparseMatrix &d_8_mat_in)
@@ -653,20 +732,23 @@ namespace SAND {
 
     // ******************     HMatrix     ***********************
 
-    HMatrix::HMatrix(LA::MPI::SparseMatrix &a_mat_in, const LA::MPI::SparseMatrix &b_mat_in, const LA::MPI::SparseMatrix &c_mat_in, const LA::MPI::SparseMatrix &e_mat_in,TrilinosWrappers::PreconditionAMG &pre_amg_in,VmultTrilinosSolverDirect &a_inv_direct_in)
+    template<int dim>
+    HMatrix<dim>::HMatrix(LA::MPI::SparseMatrix &a_mat_in, const LA::MPI::SparseMatrix &b_mat_in, const LA::MPI::SparseMatrix &c_mat_in, const LA::MPI::SparseMatrix &e_mat_in,TrilinosWrappers::PreconditionAMG &pre_amg_in,VmultTrilinosSolverDirect &a_inv_direct_in, AInvMatMFGMG<dim> &a_inv_mf_gmg_in)
             :
             a_mat(a_mat_in),
             b_mat(b_mat_in),
             c_mat(c_mat_in),
             e_mat(e_mat_in),
             pre_amg(pre_amg_in),
-            a_inv_direct(a_inv_direct_in)
+            a_inv_direct(a_inv_direct_in),
+            a_inv_mf_gmg(a_inv_mf_gmg_in)
     {
 
     }
 
+    template<int dim>
     void
-    HMatrix::initialize(LA::MPI::Vector &exemplar_density_vector,  LA::MPI::Vector &exemplar_displacement_vector)
+    HMatrix<dim>::initialize(LA::MPI::Vector &exemplar_density_vector,  LA::MPI::Vector &exemplar_displacement_vector)
     {
         temp_vect_1 = exemplar_displacement_vector;
         temp_vect_2 = exemplar_displacement_vector;
@@ -679,8 +761,8 @@ namespace SAND {
 
     }
 
-
-    void HMatrix::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+    template<int dim>
+    void HMatrix<dim>::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
     {
         if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
         {
@@ -692,9 +774,8 @@ namespace SAND {
 
             try
             {
-                a_solver_cg_1.solve(a_mat,temp_vect_3,temp_vect_1,pre_amg);
-    //            std::cout << "   " << solver_control_1.last_step()
-    //                        << " CG iterations needed to obtain convergence." << std::endl;
+//                a_solver_cg_1.solve(a_mat,temp_vect_3,temp_vect_1,pre_amg);
+                a_inv_mf_gmg.vmult(temp_vect_3,temp_vect_1);
             } catch (std::exception &exc)
             {
 
@@ -703,9 +784,8 @@ namespace SAND {
             SolverCG<LA::MPI::Vector> a_solver_cg_2(solver_control_2);
             try
             {
-                a_solver_cg_2.solve(a_mat,temp_vect_4,temp_vect_2,pre_amg);
-    //            std::cout << "   " << solver_control_2.last_step()
-    //                        << " CG iterations needed to obtain convergence." << std::endl;
+//                a_solver_cg_2.solve(a_mat,temp_vect_4,temp_vect_2,pre_amg);
+                a_inv_mf_gmg.vmult(temp_vect_4,temp_vect_2);
             } catch (std::exception &exc)
             {
 
@@ -737,8 +817,8 @@ namespace SAND {
     }
 
 
-
-    void HMatrix::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+    template<int dim>
+    void HMatrix<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
     {
         if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
         {
@@ -750,9 +830,9 @@ namespace SAND {
 
             try
             {
-                a_solver_cg_1.solve(a_mat,temp_vect_3,temp_vect_1,pre_amg);
-    //            std::cout << "   " << solver_control_1.last_step()
-    //                        << " CG iterations needed to obtain convergence." << std::endl;
+//                a_solver_cg_1.solve(a_mat,temp_vect_3,temp_vect_1,pre_amg);
+                a_inv_mf_gmg.vmult(temp_vect_3,temp_vect_1);
+
             } catch (std::exception &exc)
             {
 
@@ -761,9 +841,9 @@ namespace SAND {
             SolverCG<LA::MPI::Vector> a_solver_cg_2(solver_control_2);
             try
             {
-                a_solver_cg_2.solve(a_mat,temp_vect_4,temp_vect_2,pre_amg);
-    //            std::cout << "   " << solver_control_2.last_step()
-    //                        << " CG iterations needed to obtain convergence." << std::endl;
+//                a_solver_cg_2.solve(a_mat,temp_vect_4,temp_vect_2,pre_amg);
+                a_inv_mf_gmg.vmult(temp_vect_4,temp_vect_2);
+
             } catch (std::exception &exc)
             {
 
@@ -791,8 +871,10 @@ namespace SAND {
         }
 
     }
+
     // ******************     JinvMatrix     ***********************
-    JinvMatrix::JinvMatrix(HMatrix &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in)
+    template<int dim>
+    JinvMatrix<dim>::JinvMatrix(HMatrix<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in)
             :
             h_mat(h_mat_in),
             g_mat(g_mat_in),
@@ -802,8 +884,9 @@ namespace SAND {
 
     }
 
+    template<int dim>
     void
-    JinvMatrix::initialize(LA::MPI::Vector &exemplar_density_vector)
+    JinvMatrix<dim>::initialize(LA::MPI::Vector &exemplar_density_vector)
     {
         temp_vect_1 = exemplar_density_vector;
         temp_vect_2 = exemplar_density_vector;
@@ -812,7 +895,8 @@ namespace SAND {
 
     }
 
-    void JinvMatrix::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+    template<int dim>
+    void JinvMatrix<dim>::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
     {
         g_mat.vmult(temp_vect_1,src);
         d_m_inv_mat.vmult(temp_vect_2,temp_vect_1);
@@ -822,7 +906,8 @@ namespace SAND {
         dst = -1*temp_vect_4 - temp_vect_3;
     }
 
-    void JinvMatrix::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+    template<int dim>
+    void JinvMatrix<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
     {
         h_mat.vmult(temp_vect_1,src);
         d_m_inv_mat.vmult(temp_vect_2,temp_vect_1);
@@ -833,7 +918,8 @@ namespace SAND {
     }
 
     // ******************     KinvMatrix     ***********************
-    KinvMatrix::KinvMatrix(HMatrix &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in)
+    template<int dim>
+    KinvMatrix<dim>::KinvMatrix(HMatrix<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in)
             :
             h_mat(h_mat_in),
             g_mat(g_mat_in),
@@ -843,8 +929,9 @@ namespace SAND {
 
     }
 
+    template<int dim>
     void
-    KinvMatrix::initialize(LA::MPI::Vector &exemplar_density_vector)
+    KinvMatrix<dim>::initialize(LA::MPI::Vector &exemplar_density_vector)
     {
         temp_vect_1 = exemplar_density_vector;
         temp_vect_2 = exemplar_density_vector;
@@ -853,7 +940,8 @@ namespace SAND {
 
     }
 
-    void KinvMatrix::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+    template<int dim>
+    void KinvMatrix<dim>::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
     {
         h_mat.vmult(temp_vect_1,src);
         d_m_inv_mat.vmult(temp_vect_2,temp_vect_1);
@@ -863,7 +951,8 @@ namespace SAND {
         dst = -1*temp_vect_4 - temp_vect_3;
     }
 
-    void KinvMatrix::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+    template<int dim>
+    void KinvMatrix<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
     {
         g_mat.vmult(temp_vect_1,src);
         d_m_inv_mat.vmult(temp_vect_2,temp_vect_1);
@@ -879,3 +968,15 @@ namespace SAND {
 
 template class SAND::TopOptSchurPreconditioner<2>;
 template class SAND::TopOptSchurPreconditioner<3>;
+
+template class SAND::AInvMatMFGMG<2>;
+template class SAND::AInvMatMFGMG<3>;
+
+template class SAND::JinvMatrix<2>;
+template class SAND::JinvMatrix<3>;
+
+template class SAND::KinvMatrix<2>;
+template class SAND::KinvMatrix<3>;
+
+template class SAND::HMatrix<2>;
+template class SAND::HMatrix<3>;

From c6437d7eff92c5b161cb631dcf6034c40dac9b23 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Tue, 19 Apr 2022 20:20:49 -0600
Subject: [PATCH 82/95] updated with wrapped a_mat

---
 include/schur_preconditioner.h | 28 +++++++++++++++++++++-
 source/schur_preconditioner.cc | 43 +++++++++++++++++++++++++++-------
 2 files changed, 62 insertions(+), 9 deletions(-)

diff --git a/include/schur_preconditioner.h b/include/schur_preconditioner.h
index d5bdfa8..4e54550 100644
--- a/include/schur_preconditioner.h
+++ b/include/schur_preconditioner.h
@@ -101,18 +101,43 @@ namespace SAND
             int size;
     };
 
+    class AMatWrapped : public TrilinosWrappers::SparseMatrix {
+        public:
+            AMatWrapped(LA::MPI::SparseMatrix &a_mat);
+            void vmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const;
+            void Tvmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const;
+            unsigned int m() const;
+            unsigned int n() const;
+            void set_exemplar_vector (const LA::MPI::Vector &exemplar_vector)
+            {
+                temp_dst = exemplar_vector;
+                temp_src = exemplar_vector;
+            }
+
+        private:
+            const LA::MPI::SparseMatrix &a_mat;
+            mutable LA::MPI::Vector temp_src;
+            mutable LA::MPI::Vector temp_dst;
+
+    };
+
     template<int dim>
     class AInvMatMFGMG : public TrilinosWrappers::SparseMatrix {
         public:
             AInvMatMFGMG(MF_Elasticity_Operator<dim,1,double> &mf_elasticity_operator_in , PreconditionMG<dim,LinearAlgebra::distributed::Vector<double>,MGTransferMatrixFree<dim, double>>
-                         &mf_gmg_preconditioner_in);
+                         &mf_gmg_preconditioner_in,LA::MPI::SparseMatrix &a_mat);
             void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
             void Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
             unsigned int m() const;
             unsigned int n() const;
             void set_tol(double tolerance_in);
             void set_iter(unsigned int iterations_in);
+            void set_exemplar_vector (const LA::MPI::Vector &exemplar_vector)
+            {
+                a_mat_wrapped.set_exemplar_vector(exemplar_vector);
+            }
         private:
+            AMatWrapped a_mat_wrapped;
             MF_Elasticity_Operator<dim,1,double> &mf_elasticity_operator;
             PreconditionMG<dim,LinearAlgebra::distributed::Vector<double>,MGTransferMatrixFree<dim, double>> &mf_gmg_preconditioner;
             double tolerance = 1e-6;
@@ -122,6 +147,7 @@ namespace SAND
 
     };
 
+
     class GMatrix : public TrilinosWrappers::SparseMatrix {
         public:
             GMatrix(const LA::MPI::SparseMatrix &f_mat_in, LA::MPI::SparseMatrix &d_8_mat_in);
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index c6abc7e..fe48bd9 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -44,6 +44,7 @@ namespace SAND {
 
 
     using namespace dealii;
+
     template<int dim>
     TopOptSchurPreconditioner<dim>::TopOptSchurPreconditioner(LA::MPI::BlockSparseMatrix &matrix_in, DoFHandler<dim> &big_dof_handler_in, MF_Elasticity_Operator<dim,1,double> &mf_elasticity_operator_in , PreconditionMG<dim,LinearAlgebra::distributed::Vector<double> ,MGTransferMatrixFree<dim, double>>
                                                               &mf_gmg_preconditioner_in)
@@ -71,7 +72,7 @@ namespace SAND {
             additional_data(false, solver_type),
             direct_solver_control(1, 0),
             a_inv_direct(direct_solver_control, additional_data),
-            a_inv_mf_gmg(mf_elasticity_operator_in, mf_gmg_preconditioner_in),
+            a_inv_mf_gmg(mf_elasticity_operator_in, mf_gmg_preconditioner_in, a_mat),
             pcout(std::cout,(Utilities::MPI::this_mpi_process(mpi_communicator) == 0)),
             timer(pcout, TimerOutput::summary, TimerOutput::wall_times),
             g_mat(f_mat, d_8_mat),
@@ -86,6 +87,9 @@ namespace SAND {
     template<int dim>
     void TopOptSchurPreconditioner<dim>::initialize(LA::MPI::BlockSparseMatrix &matrix, const std::map<types::global_dof_index, double> &boundary_values,const DoFHandler<dim> &dof_handler, const LA::MPI::BlockVector &distributed_state)
     {
+
+        a_inv_mf_gmg.set_exemplar_vector(distributed_state.block(SolutionBlocks::displacement));
+
         TimerOutput::Scope t(timer, "initialize");
         {
                 TimerOutput::Scope t(timer, "diag stuff");
@@ -643,9 +647,10 @@ namespace SAND {
     // **************** A inv MF GMG **********************
 
     template<int dim>
-    AInvMatMFGMG<dim>::AInvMatMFGMG(MF_Elasticity_Operator<dim,1,double> &mf_elasticity_operator_in , PreconditionMG<dim, LinearAlgebra::distributed::Vector<double>, MGTransferMatrixFree<dim, double> > &mf_gmg_preconditioner_in)
+    AInvMatMFGMG<dim>::AInvMatMFGMG(MF_Elasticity_Operator<dim,1,double> &mf_elasticity_operator_in , PreconditionMG<dim, LinearAlgebra::distributed::Vector<double>, MGTransferMatrixFree<dim, double> > &mf_gmg_preconditioner_in, LA::MPI::SparseMatrix &a_mat)
         : mf_elasticity_operator(mf_elasticity_operator_in),
-          mf_gmg_preconditioner(mf_gmg_preconditioner_in)
+          mf_gmg_preconditioner(mf_gmg_preconditioner_in),
+          a_mat_wrapped(a_mat)
     {
         mf_elasticity_operator.initialize_dof_vector(temp_dst);
         mf_elasticity_operator.initialize_dof_vector(temp_src);
@@ -655,30 +660,33 @@ namespace SAND {
     void AInvMatMFGMG<dim>::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
     {
 
-        SolverControl            solver_control(iterations, 1e-6);
+        SolverControl            solver_control(100, src.l2_norm()*1e-10);
         SolverCG<LinearAlgebra::distributed::Vector<double>> a_solver_cg(solver_control);
 
         ChangeVectorTypes::copy(temp_src, src);
         try{
-            a_solver_cg.solve(mf_elasticity_operator,temp_dst,temp_src, mf_gmg_preconditioner);
+            a_solver_cg.solve(a_mat_wrapped,temp_dst,temp_src, mf_gmg_preconditioner);
         } catch (std::exception &exc)
         {
             std::cout << "failed with a reduction of: " << solver_control.initial_value()/solver_control.last_value() << std::endl;
         }
 
+        std::cout << "mult done" << std::endl;
+
         ChangeVectorTypes::copy(dst,temp_dst);
 
     }
 
     template<int dim>
-    void AInvMatMFGMG<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const {
-        SolverControl            solver_control(iterations, src.l2_norm()*tolerance);
+    void AInvMatMFGMG<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+    {
+        SolverControl            solver_control(1000, src.l2_norm()*1e-10);
         SolverCG<LinearAlgebra::distributed::Vector<double>> a_solver_cg(solver_control);
 
         ChangeVectorTypes::copy(temp_src, src);
         temp_dst = 0.;
 
-        a_solver_cg.solve(mf_elasticity_operator,temp_dst,temp_src, mf_gmg_preconditioner);
+        a_solver_cg.solve(a_mat_wrapped,temp_dst,temp_src, mf_gmg_preconditioner);
 
         ChangeVectorTypes::copy(dst,temp_dst);
     }
@@ -962,6 +970,24 @@ namespace SAND {
         dst = -1*temp_vect_4 - temp_vect_3;
     }
 
+
+    AMatWrapped::AMatWrapped(LA::MPI::SparseMatrix &a_mat_in)
+            :
+            a_mat(a_mat_in)
+    {
+    }
+    void AMatWrapped::vmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
+    {
+        ChangeVectorTypes::copy(temp_src, src);
+        a_mat.vmult(temp_dst,temp_src);
+        ChangeVectorTypes::copy(dst, temp_dst);
+    }
+    void AMatWrapped::Tvmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
+    {
+        ChangeVectorTypes::copy(temp_src, src);
+        a_mat.Tvmult(temp_dst,temp_src);
+        ChangeVectorTypes::copy(dst, temp_dst);
+    }
 }
 
 
@@ -980,3 +1006,4 @@ template class SAND::KinvMatrix<3>;
 
 template class SAND::HMatrix<2>;
 template class SAND::HMatrix<3>;
+

From 87eabf9af3769dd66e6601497551a478e55ff92a Mon Sep 17 00:00:00 2001
From: Justin O'Connor <oconnor@venture.math.colostate.edu>
Date: Wed, 20 Apr 2022 08:13:28 -0600
Subject: [PATCH 83/95] temp commit

---
 include/input_information.h      |  2 +-
 include/schur_preconditioner.h   |  2 +-
 source/kkt_system.cc             | 20 +++++++++++++-------
 source/matrix_free_elasticity.cc |  3 ---
 source/schur_preconditioner.cc   | 14 +++++++-------
 5 files changed, 22 insertions(+), 19 deletions(-)

diff --git a/include/input_information.h b/include/input_information.h
index 3e072fb..4568e01 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -47,7 +47,7 @@ namespace SAND {
         constexpr double default_gmres_tolerance = 1e-9;
 
         constexpr unsigned int a_inv_iterations = 25;
-        constexpr unsigned int k_inv_iterations = 25;
+        constexpr unsigned int k_inv_iterations = 1000;
 
         //Material Options
         constexpr double material_lambda = 1;
diff --git a/include/schur_preconditioner.h b/include/schur_preconditioner.h
index d5bdfa8..e30ac17 100644
--- a/include/schur_preconditioner.h
+++ b/include/schur_preconditioner.h
@@ -115,7 +115,7 @@ namespace SAND
         private:
             MF_Elasticity_Operator<dim,1,double> &mf_elasticity_operator;
             PreconditionMG<dim,LinearAlgebra::distributed::Vector<double>,MGTransferMatrixFree<dim, double>> &mf_gmg_preconditioner;
-            double tolerance = 1e-6;
+            double tolerance = 1e-9;
             unsigned int iterations = 10;
             mutable dealii::LinearAlgebra::distributed::Vector<double> temp_src;
             mutable dealii::LinearAlgebra::distributed::Vector<double> temp_dst;
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index c763167..f914647 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -443,17 +443,17 @@ KktSystem<dim>::setup_boundary_values()
                                 if (std::fabs(vert(0) - 6) < 1e-12 && std::fabs(
                                             vert(1) - 0) < 1e-12)
                                 {
-                                    //                            const unsigned int x_displacement =
-                                    //                                    cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
+//                                                                const unsigned int x_displacement =
+//                                                                        cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
                                     const unsigned int y_displacement =
                                             cell->vertex_dof_index(vertex_number, 1, cell->active_fe_index());
-                                    //                            const unsigned int x_displacement_multiplier =
-                                    //                                    cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
+//                                                                const unsigned int x_displacement_multiplier =
+//                                                                        cell->vertex_dof_index(vertex_number, 2, cell->active_fe_index());
                                     const unsigned int y_displacement_multiplier =
                                             cell->vertex_dof_index(vertex_number, 3, cell->active_fe_index());
-                                    //                            boundary_values[x_displacement] = 0;
+//                                    boundary_values[x_displacement] = 0;
                                     boundary_values[y_displacement] = 0;
-                                    //                            boundary_values[x_displacement_multiplier] = 0;
+//                                    boundary_values[x_displacement_multiplier] = 0;
                                     boundary_values[y_displacement_multiplier] = 0;
                                 }
                             }
@@ -516,10 +516,16 @@ KktSystem<dim>::setup_boundary_values()
                                     for (unsigned int level = 0; level < n_levels; ++level)
                                     {
 
+                                        const unsigned int x_displacement =
+                                                cell->mg_vertex_dof_index(level, vertex_number, 0,0);
                                         const unsigned int y_displacement =
                                                 cell->mg_vertex_dof_index(level, vertex_number, 1, 0);
-                                        level_boundary_values[level][y_displacement] = 0;
+
+//                                        level_dirichlet_boundary_dofs[level].insert(x_displacement);
                                         level_dirichlet_boundary_dofs[level].insert(y_displacement);
+
+//                                        level_boundary_values[level][x_displacement] = 0;
+                                        level_boundary_values[level][y_displacement] = 0;
                                     }
 
                                 }
diff --git a/source/matrix_free_elasticity.cc b/source/matrix_free_elasticity.cc
index 5b7e638..c4cc560 100644
--- a/source/matrix_free_elasticity.cc
+++ b/source/matrix_free_elasticity.cc
@@ -182,6 +182,3 @@ MF_Elasticity_Operator<dim,fe_degree,number>::set_cell_data (const OperatorCellD
 template class SAND::MF_Elasticity_Operator<2,1,double>;
 template class SAND::MF_Elasticity_Operator<3,1,double>;
 
-//template class SAND::MF_Elasticity_Operator<2,1,double>::MF_Elasticity_Operator();
-//template class SAND::MF_Elasticity_Operator<3,1,double>::MF_Elasticity_Operator();
-
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index c6abc7e..6e2a0f6 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -393,7 +393,7 @@ namespace SAND {
             auto d_m_inv_density = g_d_m_inv_density;
             d_m_inv_mat.vmult(d_m_inv_density,src.block(SolutionBlocks::density));
             g_mat.vmult(g_d_m_inv_density,d_m_inv_density);
-            SolverControl step_4_gmres_control_1 (Input::k_inv_iterations, g_d_m_inv_density.l2_norm()*1e-6);
+            SolverControl step_4_gmres_control_1 (Input::k_inv_iterations, g_d_m_inv_density.l2_norm()*1e-12);
             SolverFGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
             try {
                 step_4_gmres_1.solve(k_inv_mat,k_g_d_m_inv_density,g_d_m_inv_density, preconditioner );
@@ -405,7 +405,7 @@ namespace SAND {
                 pcout << "last residual: " << step_4_gmres_control_1.last_value() << std::endl;
 //                throw;
             }
-            SolverControl step_4_gmres_control_2 (Input::k_inv_iterations, 1e-6 * src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm() );
+            SolverControl step_4_gmres_control_2 (Input::k_inv_iterations, 1e-12 * src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm() );
             SolverFGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
             try {
                 step_4_gmres_2.solve(k_inv_mat,k_density_mult,src.block(SolutionBlocks::unfiltered_density_multiplier), PreconditionIdentity());
@@ -508,7 +508,7 @@ namespace SAND {
                     g_mat.vmult(pre_pre_k,pre_pre_pre_k);
                     pre_k =  -1 * pre_pre_k + src.block(SolutionBlocks::unfiltered_density_multiplier);
                 }
-                SolverControl step_5_gmres_control_1 (Input::k_inv_iterations, 1e-6*pre_j.l2_norm());
+                SolverControl step_5_gmres_control_1 (Input::k_inv_iterations, 1e-12*pre_j.l2_norm());
                 SolverFGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
                 try {
                     TimerOutput::Scope t(timer, "actual inverse 5.1");
@@ -522,7 +522,7 @@ namespace SAND {
                 }
 
 
-                SolverControl step_5_gmres_control_2 (Input::k_inv_iterations, 1e-6*pre_k.l2_norm());
+                SolverControl step_5_gmres_control_2 (Input::k_inv_iterations, 1e-12*pre_k.l2_norm());
                 SolverFGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
                 try {
                     TimerOutput::Scope t(timer, "actual inverse 5.2");
@@ -554,7 +554,7 @@ namespace SAND {
                     g_mat.vmult(pre_pre_k,pre_pre_pre_k);
                     pre_k =  -1 * pre_pre_k + src.block(SolutionBlocks::unfiltered_density_multiplier);
                 }
-                SolverControl step_5_gmres_control_1 (Input::k_inv_iterations, 1e-6*pre_j.l2_norm());
+                SolverControl step_5_gmres_control_1 (Input::k_inv_iterations, 1e-12*pre_j.l2_norm());
                 SolverFGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
                 try {
                     TimerOutput::Scope t(timer, "actual inverse 5.1");
@@ -568,7 +568,7 @@ namespace SAND {
                 }
 
 
-                SolverControl step_5_gmres_control_2 (Input::k_inv_iterations, 1e-6*pre_k.l2_norm());
+                SolverControl step_5_gmres_control_2 (Input::k_inv_iterations, 1e-12*pre_k.l2_norm());
                 SolverFGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
                 try {
                     TimerOutput::Scope t(timer, "actual inverse 5.2");
@@ -655,7 +655,7 @@ namespace SAND {
     void AInvMatMFGMG<dim>::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
     {
 
-        SolverControl            solver_control(iterations, 1e-6);
+        SolverControl            solver_control(25, 1e-12);
         SolverCG<LinearAlgebra::distributed::Vector<double>> a_solver_cg(solver_control);
 
         ChangeVectorTypes::copy(temp_src, src);

From 4f87351c8ae3446194035939637f344d5587a26f Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Wed, 27 Apr 2022 12:47:48 -0600
Subject: [PATCH 84/95] home stuff

---
 include/kkt_system.h             |  1 -
 source/kkt_system.cc             | 93 +++++++++++++++++++-------------
 source/matrix_free_elasticity.cc |  6 ++-
 3 files changed, 60 insertions(+), 40 deletions(-)

diff --git a/include/kkt_system.h b/include/kkt_system.h
index 4b4af9a..eee0542 100644
--- a/include/kkt_system.h
+++ b/include/kkt_system.h
@@ -179,7 +179,6 @@ namespace SAND {
         SystemMFMatrixType elasticity_matrix_mf;
         MGLevelObject<LevelMFMatrixType> mg_matrices;
 
-
         OperatorCellData<dim, GMGNumberType> active_cell_data;
         MGLevelObject<OperatorCellData<dim, GMGNumberType>> level_cell_data;
         dealii::LinearAlgebra::distributed::Vector<double> active_density_vector;
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index 24c740a..514eb1c 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -496,11 +496,11 @@ KktSystem<dim>::setup_boundary_values()
                                      {
 
                                          const unsigned int x_displacement =
-                                                 cell->mg_vertex_dof_index(level, vertex_number, 0,0);
-
+                                                 cell->mg_vertex_dof_index(level, vertex_number, 0, cell->active_fe_index());
                                          const unsigned int y_displacement =
-                                                 cell->mg_vertex_dof_index(level, vertex_number, 1,0);
+                                                 cell->mg_vertex_dof_index(level, vertex_number, 1, cell->active_fe_index());
                                          /*set bottom left BC*/
+
                                          level_dirichlet_boundary_dofs[level].insert(x_displacement);
                                          level_dirichlet_boundary_dofs[level].insert(y_displacement);
 
@@ -516,10 +516,10 @@ KktSystem<dim>::setup_boundary_values()
                                     for (unsigned int level = 0; level < n_levels; ++level)
                                     {
 
-                                        const unsigned int x_displacement =
-                                                cell->mg_vertex_dof_index(level, vertex_number, 0,0);
+//                                        const unsigned int x_displacement =
+//                                                cell->mg_vertex_dof_index(level, vertex_number, 0,cell->active_fe_index());
                                         const unsigned int y_displacement =
-                                                cell->mg_vertex_dof_index(level, vertex_number, 1, 0);
+                                                cell->mg_vertex_dof_index(level, vertex_number, 1, cell->active_fe_index());
 
 //                                        level_dirichlet_boundary_dofs[level].insert(x_displacement);
                                         level_dirichlet_boundary_dofs[level].insert(y_displacement);
@@ -527,7 +527,6 @@ KktSystem<dim>::setup_boundary_values()
 //                                        level_boundary_values[level][x_displacement] = 0;
                                         level_boundary_values[level][y_displacement] = 0;
                                     }
-
                                 }
                             }
                         }
@@ -1183,7 +1182,7 @@ KktSystem<dim>::assemble_block_system(const LA::MPI::BlockVector &distributed_st
     const FEValuesExtractors::Scalar total_volume_multiplier(
                 SolutionComponents::total_volume_multiplier<dim>);
 
-    const Functions::ConstantFunction<dim> lambda(1.), mu(1.);
+    const Functions::ConstantFunction<dim> lambda(Input::material_lambda), mu(Input::material_mu);
 
     distributed_solution = distributed_state;
     LA::MPI::BlockVector filtered_unfiltered_density_solution = distributed_solution;
@@ -2174,6 +2173,8 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
 
     mg_constrained_dofs.clear();
     mg_constrained_dofs.initialize(dof_handler_displacement);
+    const std::set<types::boundary_id> empty_boundary_set;
+    mg_constrained_dofs.make_zero_boundary_constraints(dof_handler_displacement,empty_boundary_set);
 
     for (unsigned int level = 0; level < n_levels; ++level)
     {
@@ -2307,6 +2308,7 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
 
         }
     }
+    elasticity_matrix_mf.set_cell_data(active_cell_data);
 
     //MAKE LEVEL DENSITY VECTOR
 
@@ -2350,7 +2352,7 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
         mg_matrices[level].set_cell_data (level_cell_data[level]);
     }
 
-    elasticity_matrix_mf.set_cell_data(active_cell_data);
+
 
     //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
@@ -2402,34 +2404,51 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
 
 
 //*************TEST SOLVE*************************
-//    elasticity_matrix_mf.initialize_dof_vector(distributed_displacement_sol);
-//    elasticity_matrix_mf.initialize_dof_vector(distributed_displacement_rhs);
-
-//    ChangeVectorTypes::copy(distributed_displacement_sol,distributed_solution.block(SolutionBlocks::displacement));
-//    ChangeVectorTypes::copy(distributed_displacement_rhs,system_rhs.block(SolutionBlocks::displacement));
-
-//    SolverControl test_solver_control(500, 1e-6);
-//    SolverCG<LinearAlgebra::distributed::Vector<double>> CG_Solve(test_solver_control);
-
-//    std::cout << "pre norm: " << distributed_displacement_rhs.l2_norm() << std::endl;
-
-//    try
-//    {
-////        CG_Solve.solve(elasticity_matrix_mf, distributed_displacement_sol, -1* distributed_displacement_rhs,    PreconditionIdentity());
-//        CG_Solve.solve(elasticity_matrix_mf, distributed_displacement_sol, -1* distributed_displacement_rhs, mf_gmg_preconditioner  );
-//    }
-//    catch(std::exception &exc)
-//    {
-//        std::cout << "solve failed in " << test_solver_control.last_step() <<  " steps" << std::endl;
-//        throw;
-//    }
-
-//    std::cout << "solved in " << test_solver_control.last_step() <<  " steps" << std::endl;
-
-//    ChangeVectorTypes::copy(distributed_solution.block(SolutionBlocks::displacement), distributed_displacement_sol);
-//    displacement_constraints.distribute(distributed_solution);
-//    output(distributed_solution, 0);
-//    std::abort();
+    elasticity_matrix_mf.initialize_dof_vector(distributed_displacement_sol);
+    elasticity_matrix_mf.initialize_dof_vector(distributed_displacement_rhs);
+
+    ChangeVectorTypes::copy(distributed_displacement_sol,distributed_solution.block(SolutionBlocks::displacement));
+    ChangeVectorTypes::copy(distributed_displacement_rhs,system_rhs.block(SolutionBlocks::displacement));
+
+    SolverControl test_solver_control_1(500, 1e-6);
+    SolverControl test_solver_control_2(500, 1e-6);
+    SolverCG<LinearAlgebra::distributed::Vector<double>> CG_Solve_1(test_solver_control_1);
+    SolverCG<LA::MPI::Vector> CG_Solve_2(test_solver_control_2);
+
+    std::cout << "pre norm: " << distributed_displacement_rhs.l2_norm() << std::endl;
+
+    try
+    {
+//        CG_Solve.solve(elasticity_matrix_mf, distributed_displacement_sol, -1* distributed_displacement_rhs,    );
+        CG_Solve_1.solve(elasticity_matrix_mf, distributed_displacement_sol, -1* distributed_displacement_rhs, PreconditionIdentity() );
+    }
+    catch(std::exception &exc)
+    {
+        std::cout << "solve failed in " << test_solver_control_1.last_step() <<  " steps" << std::endl;
+        throw;
+    }
+
+    std::cout << "solved in " << test_solver_control_1.last_step() <<  " steps" << std::endl;
+
+    try
+    {
+//        CG_Solve.solve(elasticity_matrix_mf, distributed_displacement_sol, -1* distributed_displacement_rhs,    PreconditionIdentity());
+        CG_Solve_2.solve(system_matrix.block(SolutionBlocks::displacement,SolutionBlocks::displacement_multiplier), distributed_solution.block(SolutionBlocks::displacement_multiplier), system_rhs.block(SolutionBlocks::displacement), PreconditionIdentity()  );
+    }
+    catch(std::exception &exc)
+    {
+        std::cout << "solve failed in " << test_solver_control_2.last_step() <<  " steps" << std::endl;
+        throw;
+    }
+
+    std::cout << "solved in " << test_solver_control_2.last_step() <<  " steps" << std::endl;
+
+    ChangeVectorTypes::copy(distributed_solution.block(SolutionBlocks::displacement), distributed_displacement_sol);
+    displacement_constraints.distribute(distributed_solution.block(SolutionBlocks::displacement));
+
+
+    output(distributed_solution, 0);
+    std::abort();
 
     //***************END TEST SOLVE*************************
 
diff --git a/source/matrix_free_elasticity.cc b/source/matrix_free_elasticity.cc
index eddca21..9934d3c 100644
--- a/source/matrix_free_elasticity.cc
+++ b/source/matrix_free_elasticity.cc
@@ -128,11 +128,11 @@ MF_Elasticity_Operator<dim, fe_degree, number>::local_apply(
         for (unsigned int q = 0; q < displacement.n_q_points; ++q)
         {
             SymmetricTensor< 2, dim, VectorizedArray<double> > symgrad_term = penalized_density* 2.0 * Input::material_mu *displacement.get_symmetric_gradient(q);
-            VectorizedArray<number> div_term = trace(displacement.get_symmetric_gradient(q));
+            VectorizedArray<number> div_term = penalized_density * Input::material_lambda * trace(displacement.get_symmetric_gradient(q));
 
             for (unsigned int d = 0; d < dim; ++d)
             {
-                symgrad_term[d][d] += penalized_density* Input::material_lambda * div_term;
+                symgrad_term[d][d] +=  div_term;
             }
 
 
@@ -152,6 +152,7 @@ ::local_apply_face(const dealii::MatrixFree<dim, number> &,
                    const dealii::LinearAlgebra::distributed::Vector<number> &,
                    const std::pair<unsigned int, unsigned int> &) const
 {
+
 }
 
 
@@ -163,6 +164,7 @@ ::local_apply_boundary_face(const dealii::MatrixFree<dim, number> &,
                             const dealii::LinearAlgebra::distributed::Vector<number> &,
                             const std::pair<unsigned int, unsigned int> &) const
 {
+
 }
 
 

From f54895350af0bdaad79d6ed023f2458bcbadf51f Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Thu, 28 Apr 2022 08:17:39 -0600
Subject: [PATCH 85/95] added some documentation

---
 source/density_filter.cc         |  19 +++---
 source/kkt_system.cc             | 113 +++++++++++--------------------
 source/markov_filter.cc          |   5 ++
 source/matrix_free_elasticity.cc |  18 ++---
 source/schur_preconditioner.cc   |  41 +++++++----
 source/watchdog_main.cc          |  17 ++++-
 6 files changed, 106 insertions(+), 107 deletions(-)

diff --git a/source/density_filter.cc b/source/density_filter.cc
index d2be6f6..781a9d5 100644
--- a/source/density_filter.cc
+++ b/source/density_filter.cc
@@ -15,9 +15,6 @@ namespace SAND
 {
     using namespace dealii;
 
-    /* When initialized, this function takes the current triangulation and creates a matrix corresponding to a
-     * convolution being applied to a piecewise constant function on that triangulation  */
-
     template<int dim>
     DensityFilter<dim>::DensityFilter() :
         mpi_communicator(MPI_COMM_WORLD),
@@ -25,10 +22,14 @@ namespace SAND
     {
     }
 
-
+    ///When initialized, this function takes the current triangulation and creates a matrix corresponding to a
+    /// convolution being applied to a piecewise constant function on that triangulation
+    ///
     template<int dim>
     void
     DensityFilter<dim>::initialize(DoFHandler<dim> &dof_handler) {
+        ///Start by making vectors to fill with information about the x,y,z coords of centers of cells
+        ///
         std::vector<unsigned int> block_component(10, 2);
         block_component[SolutionBlocks::density] = 0;
         block_component[SolutionBlocks::displacement] = 1;
@@ -50,7 +51,7 @@ namespace SAND
         std::set<unsigned int> neighbor_ids;
         std::set<typename DoFHandler<dim>::cell_iterator> cells_to_check;
         std::set<typename DoFHandler<dim>::cell_iterator> cells_to_check_temp;
-        /*finds neighbors whose values would be relevant, and adds them to the sparsity pattern of the matrix*/
+        ///finds neighbors whose values would be relevant, and adds them to the sparsity pattern of the matrix
          for (const auto &cell : dof_handler.active_cell_iterators())
          {
              if(cell->is_locally_owned())
@@ -91,7 +92,7 @@ namespace SAND
 
         filter_matrix.reinit(owned_dofs, filter_sparsity_pattern, MPI_COMM_WORLD);
 
-        /*adds values to the matrix corresponding to the max radius - distance*/
+        /// adds values to the matrix corresponding to the max radius - distance
         for (const auto &cell : dof_handler.active_cell_iterators())
         {
             if(cell->is_locally_owned())
@@ -112,14 +113,14 @@ namespace SAND
                     {
                         d = std::pow(d_x*d_x + d_y*d_y , .5);
                     }
-                    /*value should be (max radius - distance between cells)*cell measure */
+                    ///value should be (max radius - distance between cells)*cell measure
                     double value = (Input::filter_r - d)*cell_m[neighbor_cell_index];
                     filter_matrix.add(i[cell->get_fe().component_to_system_index(0, 0)], neighbor_cell_index, value);
                 }
             }
         }
 
-        //here we normalize the filter so it computes an average. Sum of values in a row should be 1
+        ///here we normalize the filter so it computes an average. Sum of values in a row should be 1
         for (const auto &cell : dof_handler.active_cell_iterators())
         {
             if(cell->is_locally_owned())
@@ -143,7 +144,7 @@ namespace SAND
         }
     }
 
-    /*This function finds which neighbors are within a certain radius of the initial cell.*/
+    ///This function finds which neighbors are within a certain radius of the initial cell.
     template<int dim>
     std::set<types::global_dof_index>
     DensityFilter<dim>::find_relevant_neighbors(types::global_dof_index cell_index) const
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index 514eb1c..d56472f 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -44,10 +44,11 @@
 #include <iostream>
 #include <algorithm>
 
-// This problem initializes with a FESystem composed of 2×dim FE_Q(1) elements, and 8 FE_DGQ(0)  elements.
-// The  piecewise  constant  functions  are  for  density-related  variables,and displacement-related variables are assigned to the FE_Q(1) elements.
+/// This problem initializes with a FESystem composed of 2×dim FE_Q(1) elements, and 8 FE_DGQ(0)  elements.
+/// The  piecewise  constant  functions  are  for  density-related  variables,and displacement-related variables are assigned to the FE_Q(1) elements.
 namespace SAND {
 
+///Necessary functions for going between Trilinos vectors and multigrid-compatible distributed vectors.
 namespace ChangeVectorTypes
 {
 template <typename number>
@@ -59,6 +60,8 @@ void copy(LA::MPI::Vector &                                         out,
     rwv.import(in, VectorOperation::insert);
     out.import(rwv, VectorOperation::insert);
 }
+
+
 template <typename number>
 void copy(dealii::LinearAlgebra::distributed::Vector<number> &out,
           const LA::MPI::Vector &                             in)
@@ -69,7 +72,8 @@ void copy(dealii::LinearAlgebra::distributed::Vector<number> &out,
 }
 } // namespace ChangeVectorTypes
 
-
+///The KKTSystem class calculates the Hessian and Gradient of the Lagrangian of the system, and solves the resulting system to be used
+/// as a step direction for the overarching solver.
 template<int dim>
 KktSystem<dim>::KktSystem()
     :
@@ -104,7 +108,7 @@ KktSystem<dim>::KktSystem()
 }
 
 
-//A  function  used  once  at  the  beginning  of  the  program,  this  creates  a  matrix  H  so  that H* unfiltered density = filtered density
+///A  function  used  once  at  the  beginning  of  the  program,  this  creates  a  matrix  H  so  that H* unfiltered density = filtered density
 
 template<int dim>
 void
@@ -113,13 +117,14 @@ KktSystem<dim>::setup_filter_matrix() {
     density_filter.initialize(dof_handler);
 }
 
-//This triangulation matches the problem description -
-// a 6-by-1 rectangle where a force will be applied in the top center.
+///This triangulation matches the problem description
 
 template<int dim>
 void
 KktSystem<dim>::create_triangulation() {
 
+    ///Start by defining the sub-blocks of the DoFHandler
+
     std::vector<unsigned int> sub_blocks(2*dim+8, 0);
 
     sub_blocks[0]=0;
@@ -139,7 +144,7 @@ KktSystem<dim>::create_triangulation() {
     sub_blocks[6+2*dim]=8;
     sub_blocks[7+2*dim]=9;
 
-
+    ///MBB Beam defined here
     if (Input::geometry_base == GeometryOptions::mbb) {
         const double width = 6;
         const unsigned int width_refine = 6;
@@ -200,15 +205,6 @@ KktSystem<dim>::create_triangulation() {
                 }
             }
 
-            dof_handler.distribute_dofs(fe_collection);
-            dof_handler_displacement.distribute_dofs(fe_displacement);
-            dof_handler_displacement.distribute_mg_dofs();
-
-            DoFRenumbering::component_wise(dof_handler, sub_blocks);
-            DoFRenumbering::component_wise(dof_handler_displacement);
-
-//            DoFRenumbering::hierarchical(dof_handler);
-            DoFRenumbering::hierarchical(dof_handler_displacement);
         }
         else if (dim == 3)
         {
@@ -251,19 +247,10 @@ KktSystem<dim>::create_triangulation() {
                 }
             }
 
-            dof_handler.distribute_dofs(fe_collection);
-            dof_handler_displacement.distribute_dofs(fe_displacement);
-            dof_handler_displacement.distribute_mg_dofs();
-
-            DoFRenumbering::component_wise(dof_handler, sub_blocks);
-            DoFRenumbering::component_wise(dof_handler_displacement);
-
-            DoFRenumbering::hierarchical(dof_handler);
-            DoFRenumbering::hierarchical(dof_handler_displacement);
-
         } else {
             throw;
         }
+    ///L-shaped cantilever with re-entrant corner
     } else if (Input::geometry_base == GeometryOptions::l_shape) {
         const double width = 2;
         const unsigned int width_refine = 2;
@@ -318,16 +305,6 @@ KktSystem<dim>::create_triangulation() {
                 }
             }
 
-            dof_handler.distribute_dofs(fe_collection);
-            dof_handler_displacement.distribute_dofs(fe_displacement);
-            dof_handler_displacement.distribute_mg_dofs();
-
-            DoFRenumbering::component_wise(dof_handler, sub_blocks);
-            DoFRenumbering::component_wise(dof_handler_displacement);
-
-            DoFRenumbering::hierarchical(dof_handler);
-            DoFRenumbering::hierarchical(dof_handler_displacement);
-
         } else if (dim == 3) {
             GridGenerator::subdivided_hyper_L(triangulation,
                                               {width_refine, height_refine, depth_refine},
@@ -374,16 +351,6 @@ KktSystem<dim>::create_triangulation() {
                     }
                 }
             }
-
-            dof_handler.distribute_dofs(fe_collection);
-            dof_handler_displacement.distribute_dofs(fe_displacement);
-
-            DoFRenumbering::component_wise(dof_handler, sub_blocks);
-            DoFRenumbering::component_wise(dof_handler_displacement);
-
-            DoFRenumbering::hierarchical(dof_handler_displacement);
-
-            dof_handler_displacement.distribute_mg_dofs();
         } else {
             throw;
         }
@@ -391,16 +358,23 @@ KktSystem<dim>::create_triangulation() {
         throw;
     }
 
+    dof_handler.distribute_dofs(fe_collection);
+    dof_handler_displacement.distribute_dofs(fe_displacement);
+    dof_handler_displacement.distribute_mg_dofs();
+
+    DoFRenumbering::component_wise(dof_handler, sub_blocks);
+
 }
 
-// The  bottom  corners  are  kept  in  place  in  the  y  direction  -  the  bottom  left  also  in  the  x direction.
-// Because deal.ii is formulated to enforce boundary conditions along regions of the boundary,
-// we do this to ensure these BCs are only enforced at points.
+/// Only individual points are given Dirichlet Boundary Conditions.
+/// For example, in the MBB caes, The  bottom  corners  are  kept  in  place  in  the  y  direction
+/// and the  bottom  left  also  in  the  x direction.
+/// Because deal.ii is formulated to enforce boundary conditions along regions of the boundary,
+/// we do this to ensure these BCs are only enforced at points.
 template<int dim>
 void
 KktSystem<dim>::setup_boundary_values()
 {
-    std::cout << "here" << std::endl;
     if (Input::geometry_base == GeometryOptions::mbb)
     {
         if (dim == 2)
@@ -675,8 +649,6 @@ KktSystem<dim>::setup_boundary_values()
                 mg_level_constraints[level].close();
             }
 
-
-
         }
         else
         {
@@ -946,9 +918,10 @@ KktSystem<dim>::setup_boundary_values()
 }
 
 
-//This makes a giant 10-by-10 block matrix, and also sets up the necessary block vectors.  The
-// sparsity pattern for this matrix includes the sparsity pattern for the filter matrix. It also initializes
-// any block vectors we will use.
+///This makes a giant 10-by-10 block matrix that when assembled will represents the 10 KKT equations that
+/// come from this problem, and also sets up the necessary block vectors.  The
+/// sparsity pattern for this matrix includes the sparsity pattern for the filter matrix. It also initializes
+/// any block vectors we will use.
 template<int dim>
 void
 KktSystem<dim>::setup_block_system() {
@@ -1048,9 +1021,6 @@ KktSystem<dim>::setup_block_system() {
     coupling[SolutionComponents::unfiltered_density<dim>][SolutionComponents::unfiltered_density_multiplier<dim>] = DoFTools::always;
     coupling[SolutionComponents::unfiltered_density_multiplier<dim>][SolutionComponents::unfiltered_density<dim>] = DoFTools::always;
 
-
-
-
     //        Coupling for lower slack
     coupling[SolutionComponents::density_lower_slack<dim>][SolutionComponents::density_lower_slack<dim>] = DoFTools::always;
 
@@ -1105,7 +1075,6 @@ KktSystem<dim>::setup_block_system() {
         }
     }
 
-
     SparsityTools::distribute_sparsity_pattern(
                 dsp,
                 Utilities::MPI::all_gather(mpi_communicator,
@@ -1125,9 +1094,7 @@ KktSystem<dim>::setup_block_system() {
     system_matrix.collect_sizes();
 }
 
-///This  is  where  the  magic  happens.   The  equations  describing  the newtons method for finding 0s in the KKT conditions are implemented here.
-
-
+///The  equations  describing  the newtons method for finding 0s in the KKT conditions are implemented here.
 template<int dim>
 void
 KktSystem<dim>::assemble_block_system(const LA::MPI::BlockVector &distributed_state, const double barrier_size) {
@@ -1518,6 +1485,7 @@ KktSystem<dim>::assemble_block_system(const LA::MPI::BlockVector &distributed_st
 
 }
 
+///For use in the filter, this calculates the objective value we are working to minimize.
 template<int dim>
 double
 KktSystem<dim>::calculate_objective_value(const LA::MPI::BlockVector &distributed_state) const {
@@ -1608,7 +1576,7 @@ KktSystem<dim>::calculate_objective_value(const LA::MPI::BlockVector &distribute
 }
 
 
-//As the KKT System know which vectors correspond to the slack variables, the sum of the logs of the slacks is computed here for use in the filter.
+///As the KKT System knows which vectors correspond to the slack variables, the sum of the logs of the slacks is computed here for use in the filter.
 template<int dim>
 double
 KktSystem<dim>::calculate_barrier_distance(const LA::MPI::BlockVector &state) const {
@@ -1629,6 +1597,7 @@ KktSystem<dim>::calculate_barrier_distance(const LA::MPI::BlockVector &state) co
     return out_barrier_distance_log_sum;
 }
 
+///Calculates the norm of the RHS. While not the KKT norm, we also expect this to be 0 at a minimum.
 template<int dim>
 double
 KktSystem<dim>::calculate_rhs_norm(const LA::MPI::BlockVector &state, const double barrier_size) const {
@@ -1636,7 +1605,7 @@ KktSystem<dim>::calculate_rhs_norm(const LA::MPI::BlockVector &state, const doub
 }
 
 
-//Feasibility conditions appear on the RHS of the linear system, so I compute the RHS to find it. Could probably be combined with the objective value finding part to make it faster.
+///Feasibility conditions appear on the RHS of the linear system, so I compute the RHS to find it. Could probably be combined with the objective value finding part to make it faster.
 template<int dim>
 double
 KktSystem<dim>::calculate_feasibility(const LA::MPI::BlockVector &state, const double barrier_size) const {
@@ -1688,6 +1657,7 @@ KktSystem<dim>::calculate_feasibility(const LA::MPI::BlockVector &state, const d
     return norm;
 }
 
+///calculates the KKT norm of the system, representing how close the program is to convergence.
 template<int dim>
 double
 KktSystem<dim>::calculate_convergence(const LA::MPI::BlockVector &state) const {
@@ -1742,6 +1712,7 @@ KktSystem<dim>::calculate_convergence(const LA::MPI::BlockVector &state) const {
     return norm;
 }
 
+/// Makes the RHS of the KKT equations
 template<int dim>
 LA::MPI::BlockVector
 KktSystem<dim>::calculate_rhs(const LA::MPI::BlockVector &distributed_state, const double barrier_size) const {
@@ -1750,7 +1721,6 @@ KktSystem<dim>::calculate_rhs(const LA::MPI::BlockVector &distributed_state, con
     state = distributed_state;
     test_rhs = 0;
 
-
     QGauss<dim> nine_quadrature(fe_nine.degree + 1);
     QGauss<dim> ten_quadrature(fe_ten.degree + 1);
 
@@ -1938,7 +1908,6 @@ KktSystem<dim>::calculate_rhs(const LA::MPI::BlockVector &distributed_state, con
                             fe_values[density_upper_slack_multipliers].value(i,
                                                                              q_point);
 
-
                     //rhs eqn 0
                     cell_rhs(i) +=
                             -1 * fe_values.JxW(q_point) * (
@@ -1974,9 +1943,6 @@ KktSystem<dim>::calculate_rhs(const LA::MPI::BlockVector &distributed_state, con
                                 + -1 * unfiltered_density_phi_i * old_lower_slack_multiplier_values[q_point]
                                 );
 
-
-
-
                     //rhs eqn 3 - boundary terms counted later
                     cell_rhs(i) +=
                             -1 * fe_values.JxW(q_point) * (
@@ -2105,7 +2071,7 @@ KktSystem<dim>::calculate_rhs(const LA::MPI::BlockVector &distributed_state, con
 
 }
 
-
+///Solves the big system to get the newton step
 template<int dim>
 LA::MPI::BlockVector
 KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
@@ -2487,12 +2453,14 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
     return distributed_solution;
 }
 
+///Calculates and stores the first RHS norm for comparison with future RHS norm values
 template<int dim>
 void
 KktSystem<dim>::calculate_initial_rhs_error() {
     initial_rhs_error = system_rhs.l2_norm();
 }
 
+///Creates an initial state vector used as an initial guess for the nonlinear solver.
 template<int dim>
 LA::MPI::BlockVector
 KktSystem<dim>::get_initial_state() {
@@ -2525,6 +2493,7 @@ KktSystem<dim>::get_initial_state() {
     return state;
 }
 
+///Outputs the current state to a vtk file
 template<int dim>
 void
 KktSystem<dim>::output(const LA::MPI::BlockVector &state, const unsigned int j) const {
@@ -2577,7 +2546,7 @@ KktSystem<dim>::output(const LA::MPI::BlockVector &state, const unsigned int j)
 
 
 
-
+///Outputs to a 3d-printable file. Not yet usable in parallel.
 template<>
 void
 KktSystem<2>::output_stl(const LA::MPI::BlockVector &state) {
@@ -2796,7 +2765,7 @@ KktSystem<2>::output_stl(const LA::MPI::BlockVector &state) {
     stlfile << "endsolid bridge";
 }
 
-
+///Outputs to a 3d-printable file. Not yet usable in parallel.
 template<>
 void
 KktSystem<3>::output_stl(const LA::MPI::BlockVector &state)
diff --git a/source/markov_filter.cc b/source/markov_filter.cc
index b13609e..b367f20 100644
--- a/source/markov_filter.cc
+++ b/source/markov_filter.cc
@@ -5,6 +5,7 @@
 
 using namespace dealii;
 
+///Initialized the markov filter with the initial values
 void
 MarkovFilter::setup(const double objective_value_input, const double barrier_distance_input,
                                         const double feasibility_input, const double barrier_value_input) {
@@ -16,6 +17,7 @@ MarkovFilter::setup(const double objective_value_input, const double barrier_dis
     filter_barrier_function_value = feasibility + barrier_value * barrier_distance;
 }
 
+///Adds new information to the markov filter
 void
 MarkovFilter::add_point(const double objective_value_input, const  double barrier_distance_input,
                                     const double feasibility_input)
@@ -27,6 +29,8 @@ MarkovFilter::add_point(const double objective_value_input, const  double barrie
     filter_barrier_function_value = objective_value + barrier_value * barrier_distance;
 }
 
+///As the barrier always changes, this needs to be taken into account when accepting/rejecting a step.
+/// This allows each point to be viewed in comparison to the current barrier value.
 void
 MarkovFilter::update_barrier_value(const double barrier_value_input)
 {
@@ -34,6 +38,7 @@ MarkovFilter::update_barrier_value(const double barrier_value_input)
     filter_barrier_function_value = objective_value + barrier_value * barrier_distance;
 }
 
+///Checks if a new point passes the filter.
 bool
 MarkovFilter::check_filter(const double objective_value_input, const  double barrier_distance_input,
                                        const double feasibility_input) const
diff --git a/source/matrix_free_elasticity.cc b/source/matrix_free_elasticity.cc
index 9934d3c..62af044 100644
--- a/source/matrix_free_elasticity.cc
+++ b/source/matrix_free_elasticity.cc
@@ -7,7 +7,7 @@
 namespace SAND {
 using namespace dealii;
 
-
+///Constructor
 template <int dim, int fe_degree, typename number>
 MF_Elasticity_Operator<dim, fe_degree, number>::MF_Elasticity_Operator()
     : MatrixFreeOperators::Base<dim,LinearAlgebra::distributed::Vector<number>>()
@@ -15,6 +15,7 @@ MF_Elasticity_Operator<dim, fe_degree, number>::MF_Elasticity_Operator()
 
 }
 
+///Clears the objects, removes density information
 template <int dim, int fe_degree, typename number>
 void MF_Elasticity_Operator<dim, fe_degree, number>::clear()
 {
@@ -22,7 +23,7 @@ void MF_Elasticity_Operator<dim, fe_degree, number>::clear()
     MatrixFreeOperators::Base<dim, LinearAlgebra::distributed::Vector<number>>::clear();
 }
 
-
+///Computes the diagonal for a preconditioner on the coarsest level.
 template <int dim, int fe_degree, typename number>
 void
 MF_Elasticity_Operator<dim,fe_degree,number>::compute_diagonal ()
@@ -46,8 +47,7 @@ MF_Elasticity_Operator<dim,fe_degree,number>::compute_diagonal ()
       }
 }
 
-
-
+///Computes the diagonal value locally for a cell
 template <int dim, int fe_degree, typename number>
 void
 MF_Elasticity_Operator<dim,fe_degree,number>
@@ -102,9 +102,7 @@ ::local_compute_diagonal (const MatrixFree<dim,number>                     &data
     }
 }
 
-
-
-
+///Applies the elasticity operator locally. Matches what happens in KKT System
 template <int dim, int fe_degree, typename number>
 void
 MF_Elasticity_Operator<dim, fe_degree, number>::local_apply(
@@ -144,6 +142,7 @@ MF_Elasticity_Operator<dim, fe_degree, number>::local_apply(
     }
 }
 
+///Nothing is applied on a face on the LHS, so left blank.
 template <int dim, int fe_degree, typename number>
 void
 MF_Elasticity_Operator<dim, fe_degree, number>
@@ -155,7 +154,7 @@ ::local_apply_face(const dealii::MatrixFree<dim, number> &,
 
 }
 
-
+///Nothing is applied on a face on the LHS, so left blank.
 template <int dim, int fe_degree, typename number>
 void
 MF_Elasticity_Operator<dim, fe_degree, number>
@@ -167,7 +166,7 @@ ::local_apply_boundary_face(const dealii::MatrixFree<dim, number> &,
 
 }
 
-
+///Loops over all cells to apply the elasticity operatorto the entire LHS vector
 template <int dim, int fe_degree, typename number>
 void
 MF_Elasticity_Operator<dim,fe_degree,number>
@@ -178,6 +177,7 @@ ::apply_add (dealii::LinearAlgebra::distributed::Vector<number> &dst,
             data->cell_loop(&MF_Elasticity_Operator::local_apply, this, dst, src);
 }
 
+///Sets cell data (density) to be input given.
 template <int dim, int fe_degree, typename number>
 void
 MF_Elasticity_Operator<dim,fe_degree,number>::set_cell_data (const OperatorCellData<dim,number> &data)
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 65e9590..903281c 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -45,6 +45,7 @@ namespace SAND {
 
     using namespace dealii;
 
+    ///Constructor... kinda big due to many references to matrices
     template<int dim>
     TopOptSchurPreconditioner<dim>::TopOptSchurPreconditioner(LA::MPI::BlockSparseMatrix &matrix_in, DoFHandler<dim> &big_dof_handler_in, MF_Elasticity_Operator<dim,1,double> &mf_elasticity_operator_in , PreconditionMG<dim,LinearAlgebra::distributed::Vector<double> ,MGTransferMatrixFree<dim, double>>
                                                               &mf_gmg_preconditioner_in)
@@ -84,6 +85,7 @@ namespace SAND {
 
     }
 
+    ///Initializes the preconditioner with information about the boundary values and DoF Handler.
     template<int dim>
     void TopOptSchurPreconditioner<dim>::initialize(LA::MPI::BlockSparseMatrix &matrix, const std::map<types::global_dof_index, double> &boundary_values,const DoFHandler<dim> &dof_handler, const LA::MPI::BlockVector &distributed_state)
     {
@@ -280,7 +282,7 @@ namespace SAND {
 
     }
 
-
+    ///Application of the preconditioner, broken up into 5 parts.
     template<int dim>
     void TopOptSchurPreconditioner<dim>::vmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
         LA::MPI::BlockVector temp_src;
@@ -310,6 +312,7 @@ namespace SAND {
         vmult_step_5(dst, temp_src);
     }
 
+    ///Not implemented
     template<int dim>
     void TopOptSchurPreconditioner<dim>::Tvmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
         dst = src;
@@ -322,6 +325,7 @@ namespace SAND {
         dst += dst_temp;
     }
 
+    ///Not implemented
     template<int dim>
     void TopOptSchurPreconditioner<dim>::Tvmult_add(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
         dst = src;
@@ -429,7 +433,7 @@ namespace SAND {
 
 
 
-
+    ///The only non-triangular vmult step, applies inverses down the block diagonal.
     template<int dim>
     void TopOptSchurPreconditioner<dim>::vmult_step_5(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
         {
@@ -466,8 +470,6 @@ namespace SAND {
 
                   a_inv_mf_gmg.vmult( dst.block(SolutionBlocks::displacement), src.block(SolutionBlocks::displacement_multiplier));
                   a_inv_mf_gmg.vmult( dst.block(SolutionBlocks::displacement_multiplier), src.block(SolutionBlocks::displacement));
-
-
             }
             else
             {
@@ -477,8 +479,6 @@ namespace SAND {
 
 
         }
-
-
         {
             //Fourth (ugly) Block Inverse
             TimerOutput::Scope t(timer, "inverse 4");
@@ -597,7 +597,7 @@ namespace SAND {
         }
     }
 
-
+    ///I used to use this function to output parts of the preconditioner for debugging.
     template<int dim>
     void TopOptSchurPreconditioner<dim>::print_stuff()
     {
@@ -605,8 +605,17 @@ namespace SAND {
 
     }
 
+    //******************************* Direct Trilinos Solver ***********************************
 
+    ///Constructor
+    VmultTrilinosSolverDirect::VmultTrilinosSolverDirect(SolverControl &cn,
+                     const TrilinosWrappers::SolverDirect::AdditionalData &data)
+    : solver_direct(cn, data)
+    {
 
+    }
+
+    ///Initialize a direct solver - works well up to a point of refinement, and then refuses to solve.
     void VmultTrilinosSolverDirect::initialize(LA::MPI::SparseMatrix &a_mat)
     {
         reinit(a_mat);
@@ -614,38 +623,37 @@ namespace SAND {
         size = a_mat.n();
     }
 
+    ///rephrases the solve as a vmult for easier use.
     void
     VmultTrilinosSolverDirect::vmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
     {
         solver_direct.solve(dst, src);
     }
 
+    ///rephrases the solve as a vmult for easier use.
     void VmultTrilinosSolverDirect::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
     {
         solver_direct.solve(dst, src);
     }
 
+    ///rephrases the solve as a vmult for easier use - note this is a symmetric matrix
     void
     VmultTrilinosSolverDirect::Tvmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
     {
         solver_direct.solve(dst, src);
     }
 
+    ///rephrases the solve as a vmult for easier use - note this is a symmetric matrix.
     void VmultTrilinosSolverDirect::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
     {
         solver_direct.solve(dst, src);
     }
 
 
-    VmultTrilinosSolverDirect::VmultTrilinosSolverDirect(SolverControl &cn,
-                     const TrilinosWrappers::SolverDirect::AdditionalData &data)
-    : solver_direct(cn, data)
-    {
-
-    }
 
     // **************** A inv MF GMG **********************
 
+    ///Initializes an object that performs the A inverse operation using matrix free GMG
     template<int dim>
     AInvMatMFGMG<dim>::AInvMatMFGMG(MF_Elasticity_Operator<dim,1,double> &mf_elasticity_operator_in , PreconditionMG<dim, LinearAlgebra::distributed::Vector<double>, MGTransferMatrixFree<dim, double> > &mf_gmg_preconditioner_in, LA::MPI::SparseMatrix &a_mat)
         : mf_elasticity_operator(mf_elasticity_operator_in),
@@ -656,6 +664,7 @@ namespace SAND {
         mf_elasticity_operator.initialize_dof_vector(temp_src);
     }
 
+    ///Performs the A inverse operation
     template<int dim>
     void AInvMatMFGMG<dim>::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
     {
@@ -675,6 +684,7 @@ namespace SAND {
 
     }
 
+    ///Performs the A inverse operation - note A is symmetric
     template<int dim>
     void AInvMatMFGMG<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
     {
@@ -686,12 +696,15 @@ namespace SAND {
         a_solver_cg.solve(a_mat_wrapped,temp_dst,temp_src, mf_gmg_preconditioner);
         ChangeVectorTypes::copy(dst,temp_dst);
     }
-
+\
+    ///Change the solver tolerance if needed
     template<int dim>
     void AInvMatMFGMG<dim>::set_tol(double tol_in)
     {
         tolerance = tol_in;
     }
+
+    ///Change the maximum number of iterations if needed.
     template<int dim>
     void AInvMatMFGMG<dim>::set_iter(unsigned int iterations_in)
     {
diff --git a/source/watchdog_main.cc b/source/watchdog_main.cc
index 893c41b..37efc40 100644
--- a/source/watchdog_main.cc
+++ b/source/watchdog_main.cc
@@ -14,7 +14,6 @@
 
 ///Above are fairly normal files to include.  I also use the sparse direct package, which requiresBLAS/LAPACK
 /// to  perform  a  direct  solve  while  I  work  on  a  fast  iterative  solver  for  this problem.
-/// Below is the main class for this problem.
 
 namespace SAND {
     namespace LA
@@ -25,6 +24,9 @@ namespace SAND {
 
     using namespace dealii;
 
+    /// Below is the main class for solving this problem. It handles the nonlinear solver portion of the problem,
+    /// taking information from the KKTSystem class for step directions, and calculating step lengths. This class
+    /// not only takes those steps, but handles the barrier parameter for the log barrier used.
     template<int dim>
     class SANDTopOpt {
     public:
@@ -58,6 +60,7 @@ namespace SAND {
         TimerOutput overall_timer;
     };
 
+    ///Constructor
     template<int dim>
     SANDTopOpt<dim>::SANDTopOpt()
             :
@@ -113,7 +116,7 @@ namespace SAND {
         return {step_size_s_low, step_size_z_low};
     }
 
-///Creates a rhs vector that we can use to look at the magnitude of the KKT conditions.  This is then used for testing the convergence before shrinking barrier size, as well as in the calculation of the l1 merit.
+    ///Creates a rhs vector that we can use to look at the magnitude of the KKT conditions.  This is then used for testing the convergence before shrinking barrier size, as well as in the calculation of the l1 merit.
 
     template<int dim>
     const LA::MPI::BlockVector
@@ -153,7 +156,7 @@ namespace SAND {
         return max_step;
     }
 
-    ///This is my back-stepping algorithm for a line search - keeps shrinking step size until it finds a step where the merit is decreased.
+    ///This is a simple back-stepping algorithm for a line search - keeps shrinking step size until it finds a step where the markov filter requirement is met.
 
     template<int dim>
     LA::MPI::BlockVector
@@ -192,10 +195,14 @@ namespace SAND {
               }
     }
 
+    ///This updates the barrier value using the selected barrier scheme - more work could be done to optimize
+    /// the performance of the mixed method
     template<int dim>
     void
     SANDTopOpt<dim>::update_barrier(LA::MPI::BlockVector &current_state)
     {
+        ///The LOQO scheme uses information about the similarity of the slack/slack multiplier product as a
+        /// heuristic for decreasing barrier value
         if (Input::barrier_reduction == BarrierOptions::loqo)
         {
             double loqo_min = 1000;
@@ -259,6 +266,8 @@ namespace SAND {
             }
         }
 
+        ///The monotome scheme fully solves the problem with one barrier size before decreasing the
+        /// barrier and starting again
         if (Input::barrier_reduction == BarrierOptions::monotone)
         {
             if (kkt_system.calculate_rhs_norm(current_state,barrier_size) < barrier_size * 1e-3)
@@ -271,6 +280,8 @@ namespace SAND {
             }
         }
 
+        ///The mixed method uses LOQO unless it gets stuck, at which point it switches to monotone, allowing for an adaptive method
+        /// that still globally converges the barrier value to 0.
         if (Input::barrier_reduction == BarrierOptions::mixed)
         {
             if (mixed_barrier_monotone_mode)

From a1090ff300c0ecd6b603a0e2eda0be9ce534ebb0 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Sat, 7 May 2022 11:44:58 -0600
Subject: [PATCH 86/95] MFGMG WORKS

---
 include/kkt_system.h           |    4 +-
 include/schur_preconditioner.h |    7 +-
 source/kkt_system.cc           |  195 +++-
 source/schur_preconditioner.cc | 1621 ++++++++++++++++----------------
 4 files changed, 986 insertions(+), 841 deletions(-)

diff --git a/include/kkt_system.h b/include/kkt_system.h
index eee0542..7632fdd 100644
--- a/include/kkt_system.h
+++ b/include/kkt_system.h
@@ -152,11 +152,13 @@ namespace SAND {
         mutable LA::MPI::BlockVector locally_relevant_solution;
         mutable LA::MPI::BlockVector distributed_solution;
         LA::MPI::BlockVector system_rhs;
-        parallel::distributed::Triangulation<dim> triangulation;
+        /*parallel::distributed::*/Triangulation<dim> triangulation;
         DoFHandler<dim> dof_handler;
         DoFHandler<dim> dof_handler_displacement;
         DoFHandler<dim> dof_handler_density;
 
+        std::map<types::global_dof_index,types::global_dof_index> displacement_to_system_dof_index_map;
+
         AffineConstraints<double> constraints;
         AffineConstraints<double> displacement_constraints;
         FESystem<dim> fe_nine;
diff --git a/include/schur_preconditioner.h b/include/schur_preconditioner.h
index 4e5f270..c61b67a 100644
--- a/include/schur_preconditioner.h
+++ b/include/schur_preconditioner.h
@@ -125,7 +125,7 @@ namespace SAND
     class AInvMatMFGMG : public TrilinosWrappers::SparseMatrix {
         public:
             AInvMatMFGMG(MF_Elasticity_Operator<dim,1,double> &mf_elasticity_operator_in , PreconditionMG<dim,LinearAlgebra::distributed::Vector<double>,MGTransferMatrixFree<dim, double>>
-                         &mf_gmg_preconditioner_in,LA::MPI::SparseMatrix &a_mat);
+                         &mf_gmg_preconditioner_in,LA::MPI::SparseMatrix &a_mat, std::map<types::global_dof_index,types::global_dof_index> &displacement_to_system_dof_index_map);
             void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
             void Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
             unsigned int m() const;
@@ -140,6 +140,7 @@ namespace SAND
             AMatWrapped a_mat_wrapped;
             MF_Elasticity_Operator<dim,1,double> &mf_elasticity_operator;
             PreconditionMG<dim,LinearAlgebra::distributed::Vector<double>,MGTransferMatrixFree<dim, double>> &mf_gmg_preconditioner;
+            const std::map<types::global_dof_index,types::global_dof_index> displacement_to_system_dof_index_map;
             double tolerance = 1e-9;
             unsigned int iterations = 10;
             mutable dealii::LinearAlgebra::distributed::Vector<double> temp_src;
@@ -236,7 +237,7 @@ namespace SAND
     class TopOptSchurPreconditioner: public Subscriptor {
     public:
         TopOptSchurPreconditioner(LA::MPI::BlockSparseMatrix &matrix_in, DoFHandler<dim> &big_dof_handler_in, MF_Elasticity_Operator<dim,1,double> &mf_elasticity_operator_in , PreconditionMG<dim,LinearAlgebra::distributed::Vector<double>,MGTransferMatrixFree<dim, double>>
-                                  &mf_gmg_preconditioner_in);
+                                  &mf_gmg_preconditioner_in, std::map<types::global_dof_index,types::global_dof_index> &displacement_to_system_dof_index_map);
         void initialize (LA::MPI::BlockSparseMatrix &matrix, const std::map<types::global_dof_index, double> &boundary_values, const DoFHandler<dim> &dof_handler, const LA::MPI::BlockVector &distributed_state);
         void vmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
         void Tvmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const;
@@ -314,6 +315,8 @@ namespace SAND
         JinvMatrix<dim> j_inv_mat;
         KinvMatrix<dim> k_inv_mat;
 
+        mutable int num_mults;
+
     };
 
 
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index d56472f..2eb6cc3 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -51,6 +51,36 @@ namespace SAND {
 ///Necessary functions for going between Trilinos vectors and multigrid-compatible distributed vectors.
 namespace ChangeVectorTypes
 {
+template <typename number>
+void copy_from_displacement_to_system_vector(LA::MPI::Vector                                           &out,
+                                             const dealii::LinearAlgebra::distributed::Vector<number>  &in,
+                                             std::map<types::global_dof_index,types::global_dof_index>  & displacement_to_system_dof_index_map)
+{
+//    dealii::LinearAlgebra::ReadWriteVector<double> rwv(
+//                out.locally_owned_elements());
+//    rwv.import(in, VectorOperation::insert);
+    for (const auto &index_pair : displacement_to_system_dof_index_map)
+    {
+        out[index_pair.second] = in[index_pair.first];
+    }
+//    out.import(rwv, VectorOperation::insert);
+}
+
+template <typename number>
+void copy_from_system_to_displacement_vector(dealii::LinearAlgebra::distributed::Vector<number>  &out,
+                                             const LA::MPI::Vector                                           &in,
+                                             std::map<types::global_dof_index,types::global_dof_index>  & displacement_to_system_dof_index_map)
+{
+//    dealii::LinearAlgebra::ReadWriteVector<double> rwv(
+//                out.locally_owned_elements());
+//    rwv.import(in, VectorOperation::insert);
+    for (const auto &index_pair : displacement_to_system_dof_index_map)
+    {
+        out[index_pair.first] = in[index_pair.second];
+    }
+//    out.import(rwv, VectorOperation::insert);
+}
+
 template <typename number>
 void copy(LA::MPI::Vector &                                         out,
           const dealii::LinearAlgebra::distributed::Vector<number> &in)
@@ -60,8 +90,6 @@ void copy(LA::MPI::Vector &                                         out,
     rwv.import(in, VectorOperation::insert);
     out.import(rwv, VectorOperation::insert);
 }
-
-
 template <typename number>
 void copy(dealii::LinearAlgebra::distributed::Vector<number> &out,
           const LA::MPI::Vector &                             in)
@@ -70,6 +98,8 @@ void copy(dealii::LinearAlgebra::distributed::Vector<number> &out,
     rwv.reinit(in);
     out.import(rwv, VectorOperation::insert);
 }
+
+
 } // namespace ChangeVectorTypes
 
 ///The KKTSystem class calculates the Hessian and Gradient of the Lagrangian of the system, and solves the resulting system to be used
@@ -78,9 +108,9 @@ template<int dim>
 KktSystem<dim>::KktSystem()
     :
       mpi_communicator(MPI_COMM_WORLD),
-      triangulation(mpi_communicator,
-                    Triangulation<dim>::limit_level_difference_at_vertices,
-                    parallel::distributed::Triangulation<dim>::construct_multigrid_hierarchy),
+      triangulation(/*mpi_communicator,*/
+                    Triangulation<dim>::limit_level_difference_at_vertices/*,
+                    parallel::distributed::Triangulation<dim>::construct_multigrid_hierarchy*/),
       dof_handler(triangulation),
       dof_handler_displacement(triangulation),
       dof_handler_density(triangulation),
@@ -359,10 +389,12 @@ KktSystem<dim>::create_triangulation() {
     }
 
     dof_handler.distribute_dofs(fe_collection);
+    DoFRenumbering::component_wise(dof_handler, sub_blocks);
+
     dof_handler_displacement.distribute_dofs(fe_displacement);
     dof_handler_displacement.distribute_mg_dofs();
 
-    DoFRenumbering::component_wise(dof_handler, sub_blocks);
+    displacement_to_system_dof_index_map.clear();
 
 }
 
@@ -1092,6 +1124,46 @@ KktSystem<dim>::setup_block_system() {
     distributed_solution.collect_sizes();
     system_rhs.collect_sizes();
     system_matrix.collect_sizes();
+
+    std::vector<types::global_dof_index> displacement_dof_indices;
+    std::vector<types::global_dof_index> system_dof_indices;
+    for (const auto &displacement_cell : dof_handler_displacement.active_cell_iterators())
+        if (displacement_cell->is_locally_owned())
+        {
+            typename DoFHandler<dim>::active_cell_iterator system_cell (&displacement_cell->get_triangulation(),
+                                                                        displacement_cell->level(),
+                                                                        displacement_cell->index(),
+                                                                        &dof_handler);
+
+            displacement_dof_indices.resize (displacement_cell->get_fe().dofs_per_cell);
+            system_dof_indices.resize (system_cell->get_fe().dofs_per_cell);
+
+            displacement_cell->get_dof_indices (displacement_dof_indices);
+            system_cell->get_dof_indices (system_dof_indices);
+
+            for (unsigned int i=0; i<displacement_dof_indices.size(); ++i)
+            {
+                displacement_to_system_dof_index_map[displacement_dof_indices[i]]
+                        = system_dof_indices[system_cell->get_fe().component_to_system_index(
+                            displacement_cell->get_fe().system_to_component_index(i).first+SolutionComponents::displacement<dim>,
+                            displacement_cell->get_fe().system_to_component_index(i).second
+                            )];
+            }
+        }
+    const types::global_dof_index disp_start_index = system_matrix.get_row_indices().block_start(
+            SolutionBlocks::displacement);
+    for (auto &index_pair : displacement_to_system_dof_index_map)
+        index_pair.second -=disp_start_index;
+    for (auto &index_pair : displacement_to_system_dof_index_map)
+    {
+        if(index_pair.first != index_pair.second)
+        {
+            std::cout << "inexact matching for index: " << index_pair.first << " and " << index_pair.second << std::endl;
+        }
+    }
+
+
+
 }
 
 ///The  equations  describing  the newtons method for finding 0s in the KKT conditions are implemented here.
@@ -2100,6 +2172,32 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
     elasticity_matrix_mf.clear();
     mg_matrices.clear_elements();
 
+    unsigned int n_dofs = dof_handler.n_dofs();
+    unsigned int n_dofs_displacement = dof_handler_displacement.n_dofs();
+
+    std::vector< Point< dim >> support_points (n_dofs);
+    std::vector< Point< dim >> support_points_displacement (n_dofs_displacement);
+
+    MappingQGeneric<dim,dim> generic_map_displacement(1);
+    MappingQGeneric<dim,dim> generic_map_1(1);
+    MappingQGeneric<dim,dim> generic_map_2(1);
+
+    hp::MappingCollection< dim, dim > hp_generic_map;
+
+    hp_generic_map.push_back(generic_map_1);
+    hp_generic_map.push_back(generic_map_2);
+
+    DoFTools::map_dofs_to_support_points(generic_map_displacement, dof_handler_displacement, support_points_displacement);
+    DoFTools::map_dofs_to_support_points(hp_generic_map, dof_handler, support_points);
+
+
+    const types::global_dof_index disp_mult_start_index = system_matrix.get_row_indices().block_start(SolutionBlocks::displacement_multiplier);
+
+    for (unsigned int d=0; d<n_dofs_displacement; d++)
+    {
+        if (support_points_displacement[d] != support_points[disp_mult_start_index+d])
+        std::cout << "d = " << d << ", points are " << support_points_displacement[d] << " and " << support_points[disp_mult_start_index+d] << std::endl;
+    }
 
     pcout << "Number of degrees of freedom: " << dof_handler_displacement.n_dofs()
           << std::endl;
@@ -2130,9 +2228,14 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
 
     elasticity_matrix_mf.initialize_dof_vector(distributed_displacement_sol);
     elasticity_matrix_mf.initialize_dof_vector(distributed_displacement_rhs);
+    std::cout << "displacement_vec_size = " << distributed_displacement_sol.size() << std::endl;
+    std::cout << "systdm_vec_size = " << distributed_solution.block(SolutionBlocks::displacement).size() << std::endl;
+
+    ChangeVectorTypes::copy_from_system_to_displacement_vector<double>(distributed_displacement_sol,distributed_solution.block(SolutionBlocks::displacement),displacement_to_system_dof_index_map);
+
+    ChangeVectorTypes::copy_from_system_to_displacement_vector<double>(distributed_displacement_rhs,system_rhs.block(SolutionBlocks::displacement),displacement_to_system_dof_index_map);
+
 
-    ChangeVectorTypes::copy(distributed_displacement_sol,distributed_solution.block(SolutionBlocks::displacement));
-    ChangeVectorTypes::copy(distributed_displacement_rhs,system_rhs.block(SolutionBlocks::displacement));
 
     const unsigned int n_levels = triangulation.n_global_levels();
     mg_matrices.resize(0, n_levels - 1);
@@ -2369,58 +2472,58 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
             mf_gmg_preconditioner(dof_handler_displacement, mg, mg_transfer);
 
 
-//*************TEST SOLVE*************************
-    elasticity_matrix_mf.initialize_dof_vector(distributed_displacement_sol);
-    elasticity_matrix_mf.initialize_dof_vector(distributed_displacement_rhs);
+////*************TEST SOLVE*************************
+//    elasticity_matrix_mf.initialize_dof_vector(distributed_displacement_sol);
+//    elasticity_matrix_mf.initialize_dof_vector(distributed_displacement_rhs);
 
-    ChangeVectorTypes::copy(distributed_displacement_sol,distributed_solution.block(SolutionBlocks::displacement));
-    ChangeVectorTypes::copy(distributed_displacement_rhs,system_rhs.block(SolutionBlocks::displacement));
+//    ChangeVectorTypes::copy_from_system_to_displacement_vector<double>(distributed_displacement_sol,distributed_solution.block(SolutionBlocks::displacement),displacement_to_system_dof_index_map);
+//    ChangeVectorTypes::copy_from_system_to_displacement_vector<double>(distributed_displacement_rhs,system_rhs.block(SolutionBlocks::displacement),displacement_to_system_dof_index_map);
 
-    SolverControl test_solver_control_1(500, 1e-6);
-    SolverControl test_solver_control_2(500, 1e-6);
-    SolverCG<LinearAlgebra::distributed::Vector<double>> CG_Solve_1(test_solver_control_1);
-    SolverCG<LA::MPI::Vector> CG_Solve_2(test_solver_control_2);
+//    SolverControl test_solver_control_1(500, 1e-6);
+//    SolverControl test_solver_control_2(500, 1e-6);
+//    SolverCG<LinearAlgebra::distributed::Vector<double>> CG_Solve_1(test_solver_control_1);
+//    SolverCG<LA::MPI::Vector> CG_Solve_2(test_solver_control_2);
 
-    std::cout << "pre norm: " << distributed_displacement_rhs.l2_norm() << std::endl;
+//    std::cout << "pre norm: " << distributed_displacement_rhs.l2_norm() << std::endl;
 
-    try
-    {
-//        CG_Solve.solve(elasticity_matrix_mf, distributed_displacement_sol, -1* distributed_displacement_rhs,    );
-        CG_Solve_1.solve(elasticity_matrix_mf, distributed_displacement_sol, -1* distributed_displacement_rhs, PreconditionIdentity() );
-    }
-    catch(std::exception &exc)
-    {
-        std::cout << "solve failed in " << test_solver_control_1.last_step() <<  " steps" << std::endl;
-        throw;
-    }
+//    try
+//    {
+////        CG_Solve.solve(elasticity_matrix_mf, distributed_displacement_sol, -1* distributed_displacement_rhs,    );
+//        CG_Solve_1.solve(elasticity_matrix_mf, distributed_displacement_sol, -1* distributed_displacement_rhs, mf_gmg_preconditioner);
+//    }
+//    catch(std::exception &exc)
+//    {
+//        std::cout << "solve failed in " << test_solver_control_1.last_step() <<  " steps" << std::endl;
+//        throw;
+//    }
 
-    std::cout << "solved in " << test_solver_control_1.last_step() <<  " steps" << std::endl;
+//    std::cout << "solved in " << test_solver_control_1.last_step() <<  " steps" << std::endl;
 
-    try
-    {
-//        CG_Solve.solve(elasticity_matrix_mf, distributed_displacement_sol, -1* distributed_displacement_rhs,    PreconditionIdentity());
-        CG_Solve_2.solve(system_matrix.block(SolutionBlocks::displacement,SolutionBlocks::displacement_multiplier), distributed_solution.block(SolutionBlocks::displacement_multiplier), system_rhs.block(SolutionBlocks::displacement), PreconditionIdentity()  );
-    }
-    catch(std::exception &exc)
-    {
-        std::cout << "solve failed in " << test_solver_control_2.last_step() <<  " steps" << std::endl;
-        throw;
-    }
+//    try
+//    {
+////        CG_Solve.solve(elasticity_matrix_mf, distributed_displacement_sol, -1* distributed_displacement_rhs,    PreconditionIdentity());
+//        CG_Solve_2.solve(system_matrix.block(SolutionBlocks::displacement,SolutionBlocks::displacement_multiplier), distributed_solution.block(SolutionBlocks::displacement_multiplier), system_rhs.block(SolutionBlocks::displacement), PreconditionIdentity()  );
+//    }
+//    catch(std::exception &exc)
+//    {
+//        std::cout << "solve failed in " << test_solver_control_2.last_step() <<  " steps" << std::endl;
+//        throw;
+//    }
 
-    std::cout << "solved in " << test_solver_control_2.last_step() <<  " steps" << std::endl;
+//    std::cout << "solved in " << test_solver_control_2.last_step() <<  " steps" << std::endl;
 
-    ChangeVectorTypes::copy(distributed_solution.block(SolutionBlocks::displacement), distributed_displacement_sol);
-    displacement_constraints.distribute(distributed_solution.block(SolutionBlocks::displacement));
+//    ChangeVectorTypes::copy_from_displacement_to_system_vector<double>(distributed_solution.block(SolutionBlocks::displacement), distributed_displacement_sol,displacement_to_system_dof_index_map);
+//    displacement_constraints.distribute(distributed_solution.block(SolutionBlocks::displacement));
 
 
-    output(distributed_solution, 0);
-    std::abort();
+//    output(distributed_solution, 0);
+//    std::abort();
 
-    //***************END TEST SOLVE*************************
+//    //***************END TEST SOLVE*************************
 
 
 
-    TopOptSchurPreconditioner<dim> preconditioner(system_matrix, dof_handler, elasticity_matrix_mf, mf_gmg_preconditioner);
+    TopOptSchurPreconditioner<dim> preconditioner(system_matrix, dof_handler, elasticity_matrix_mf, mf_gmg_preconditioner, displacement_to_system_dof_index_map);
     pcout << "about to solve" << std::endl;
 
     switch (Input::solver_choice)
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 903281c..ff90a76 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -14,989 +14,1026 @@
 #include <fstream>
 
 namespace SAND {
-    using MatrixType  = dealii::TrilinosWrappers::SparseMatrix;
-    using VectorType  = dealii::TrilinosWrappers::MPI::Vector;
-    using PayloadType = dealii::TrilinosWrappers::internal::LinearOperatorImplementation::TrilinosPayload;
-    using PayloadVectorType = typename PayloadType::VectorType;
-    using size_type         = dealii::types::global_dof_index;
-
-    namespace ChangeVectorTypes
-    {
-    template <typename number>
-    void copy(LA::MPI::Vector &                                         out,
-              const dealii::LinearAlgebra::distributed::Vector<number> &in)
+using MatrixType  = dealii::TrilinosWrappers::SparseMatrix;
+using VectorType  = dealii::TrilinosWrappers::MPI::Vector;
+using PayloadType = dealii::TrilinosWrappers::internal::LinearOperatorImplementation::TrilinosPayload;
+using PayloadVectorType = typename PayloadType::VectorType;
+using size_type         = dealii::types::global_dof_index;
+
+namespace ChangeVectorTypes
+{
+template <typename number>
+void copy_from_displacement_to_system_vector(LA::MPI::Vector                                           &out,
+                                             const dealii::LinearAlgebra::distributed::Vector<number>  &in,
+                                             const std::map<types::global_dof_index,types::global_dof_index>  & displacement_to_system_dof_index_map)
+{
+    //    dealii::LinearAlgebra::ReadWriteVector<double> rwv(
+    //                out.locally_owned_elements());
+    //    rwv.import(in, VectorOperation::insert);
+    for (const auto &index_pair : displacement_to_system_dof_index_map)
     {
-        dealii::LinearAlgebra::ReadWriteVector<double> rwv(
-                    out.locally_owned_elements());
-        rwv.import(in, VectorOperation::insert);
-        out.import(rwv, VectorOperation::insert);
+        out[index_pair.second] = in[index_pair.first];
     }
-    template <typename number>
-    void copy(dealii::LinearAlgebra::distributed::Vector<number> &out,
-              const LA::MPI::Vector &                             in)
+    //    out.import(rwv, VectorOperation::insert);
+}
+
+template <typename number>
+void copy_from_system_to_displacement_vector(dealii::LinearAlgebra::distributed::Vector<number>  &out,
+                                             const LA::MPI::Vector                                           &in,
+                                             const std::map<types::global_dof_index,types::global_dof_index>  & displacement_to_system_dof_index_map)
+{
+    //    dealii::LinearAlgebra::ReadWriteVector<double> rwv(
+    //                out.locally_owned_elements());
+    //    rwv.import(in, VectorOperation::insert);
+    for (const auto &index_pair : displacement_to_system_dof_index_map)
     {
-        dealii::LinearAlgebra::ReadWriteVector<double> rwv;
-        rwv.reinit(in);
-        out.import(rwv, VectorOperation::insert);
+        out[index_pair.first] = in[index_pair.second];
     }
-    } // namespace ChangeVectorTypes
-
-
-
-    using namespace dealii;
-
-    ///Constructor... kinda big due to many references to matrices
-    template<int dim>
-    TopOptSchurPreconditioner<dim>::TopOptSchurPreconditioner(LA::MPI::BlockSparseMatrix &matrix_in, DoFHandler<dim> &big_dof_handler_in, MF_Elasticity_Operator<dim,1,double> &mf_elasticity_operator_in , PreconditionMG<dim,LinearAlgebra::distributed::Vector<double> ,MGTransferMatrixFree<dim, double>>
-                                                              &mf_gmg_preconditioner_in)
-            :
-            system_matrix(matrix_in),
-            mpi_communicator(MPI_COMM_WORLD),
-            n_rows(0),
-            n_columns(0),
-            n_block_rows(0),
-            n_block_columns(0),
-            other_solver_control(1000000, 1e-6),
-            other_bicgstab(other_solver_control),
-            other_gmres(other_solver_control),
-            other_cg(other_solver_control),
-            a_mat(matrix_in.block(SolutionBlocks::displacement, SolutionBlocks::displacement_multiplier)),
-            b_mat(matrix_in.block(SolutionBlocks::density, SolutionBlocks::density)),
-            c_mat(matrix_in.block(SolutionBlocks::displacement,SolutionBlocks::density)),
-            e_mat(matrix_in.block(SolutionBlocks::displacement_multiplier,SolutionBlocks::density)),
-            f_mat(matrix_in.block(SolutionBlocks::unfiltered_density_multiplier,SolutionBlocks::unfiltered_density)),
-            d_m_mat(matrix_in.block(SolutionBlocks::density_upper_slack_multiplier, SolutionBlocks::density_upper_slack)),
-            d_1_mat(matrix_in.block(SolutionBlocks::density_lower_slack, SolutionBlocks::density_lower_slack)),
-            d_2_mat(matrix_in.block(SolutionBlocks::density_upper_slack, SolutionBlocks::density_upper_slack)),
-            m_vect(matrix_in.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier)),
-            solver_type("Amesos_Klu"),
-            additional_data(false, solver_type),
-            direct_solver_control(1, 0),
-            a_inv_direct(direct_solver_control, additional_data),
-            a_inv_mf_gmg(mf_elasticity_operator_in, mf_gmg_preconditioner_in, a_mat),
-            pcout(std::cout,(Utilities::MPI::this_mpi_process(mpi_communicator) == 0)),
-            timer(pcout, TimerOutput::summary, TimerOutput::wall_times),
-            g_mat(f_mat, d_8_mat),
-            h_mat(a_mat, b_mat, c_mat, e_mat, pre_amg, a_inv_direct, a_inv_mf_gmg),
-            j_inv_mat(h_mat, g_mat, d_m_mat, d_m_inv_mat),
-            k_inv_mat(h_mat, g_mat, d_m_mat, d_m_inv_mat),
-            big_dof_handler(big_dof_handler_in)
-    {
+    //    out.import(rwv, VectorOperation::insert);
+}
+
+template <typename number>
+void copy(LA::MPI::Vector &                                         out,
+          const dealii::LinearAlgebra::distributed::Vector<number> &in)
+{
+    dealii::LinearAlgebra::ReadWriteVector<double> rwv(
+                out.locally_owned_elements());
+    rwv.import(in, VectorOperation::insert);
+    out.import(rwv, VectorOperation::insert);
+}
+template <typename number>
+void copy(dealii::LinearAlgebra::distributed::Vector<number> &out,
+          const LA::MPI::Vector &                             in)
+{
+    dealii::LinearAlgebra::ReadWriteVector<double> rwv;
+    rwv.reinit(in);
+    out.import(rwv, VectorOperation::insert);
+}
 
-    }
 
-    ///Initializes the preconditioner with information about the boundary values and DoF Handler.
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::initialize(LA::MPI::BlockSparseMatrix &matrix, const std::map<types::global_dof_index, double> &boundary_values,const DoFHandler<dim> &dof_handler, const LA::MPI::BlockVector &distributed_state)
+} // namespace ChangeVectorTypes
+
+
+
+using namespace dealii;
+
+///Constructor... kinda big due to many references to matrices
+template<int dim>
+TopOptSchurPreconditioner<dim>::TopOptSchurPreconditioner(LA::MPI::BlockSparseMatrix &matrix_in, DoFHandler<dim> &big_dof_handler_in, MF_Elasticity_Operator<dim,1,double> &mf_elasticity_operator_in , PreconditionMG<dim,LinearAlgebra::distributed::Vector<double> ,MGTransferMatrixFree<dim, double>>
+                                                          &mf_gmg_preconditioner_in, std::map<types::global_dof_index,types::global_dof_index> &displacement_to_system_dof_index_map)
+    :
+      system_matrix(matrix_in),
+      mpi_communicator(MPI_COMM_WORLD),
+      n_rows(0),
+      n_columns(0),
+      n_block_rows(0),
+      n_block_columns(0),
+      other_solver_control(1000000, 1e-6),
+      other_bicgstab(other_solver_control),
+      other_gmres(other_solver_control),
+      other_cg(other_solver_control),
+      a_mat(matrix_in.block(SolutionBlocks::displacement, SolutionBlocks::displacement_multiplier)),
+      b_mat(matrix_in.block(SolutionBlocks::density, SolutionBlocks::density)),
+      c_mat(matrix_in.block(SolutionBlocks::displacement,SolutionBlocks::density)),
+      e_mat(matrix_in.block(SolutionBlocks::displacement_multiplier,SolutionBlocks::density)),
+      f_mat(matrix_in.block(SolutionBlocks::unfiltered_density_multiplier,SolutionBlocks::unfiltered_density)),
+      d_m_mat(matrix_in.block(SolutionBlocks::density_upper_slack_multiplier, SolutionBlocks::density_upper_slack)),
+      d_1_mat(matrix_in.block(SolutionBlocks::density_lower_slack, SolutionBlocks::density_lower_slack)),
+      d_2_mat(matrix_in.block(SolutionBlocks::density_upper_slack, SolutionBlocks::density_upper_slack)),
+      m_vect(matrix_in.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier)),
+      solver_type("Amesos_Klu"),
+      additional_data(false, solver_type),
+      direct_solver_control(1, 0),
+      a_inv_direct(direct_solver_control, additional_data),
+      a_inv_mf_gmg(mf_elasticity_operator_in, mf_gmg_preconditioner_in, a_mat, displacement_to_system_dof_index_map),
+      pcout(std::cout,(Utilities::MPI::this_mpi_process(mpi_communicator) == 0)),
+      timer(pcout, TimerOutput::summary, TimerOutput::wall_times),
+      g_mat(f_mat, d_8_mat),
+      h_mat(a_mat, b_mat, c_mat, e_mat, pre_amg, a_inv_direct, a_inv_mf_gmg),
+      j_inv_mat(h_mat, g_mat, d_m_mat, d_m_inv_mat),
+      k_inv_mat(h_mat, g_mat, d_m_mat, d_m_inv_mat),
+      big_dof_handler(big_dof_handler_in)
+{
+
+}
+
+///Initializes the preconditioner with information about the boundary values and DoF Handler.
+template<int dim>
+void TopOptSchurPreconditioner<dim>::initialize(LA::MPI::BlockSparseMatrix &matrix, const std::map<types::global_dof_index, double> &boundary_values,const DoFHandler<dim> &dof_handler, const LA::MPI::BlockVector &distributed_state)
+{
+
+    a_inv_mf_gmg.set_exemplar_vector(distributed_state.block(SolutionBlocks::displacement));
+
+    TimerOutput::Scope t(timer, "initialize");
     {
+        TimerOutput::Scope t(timer, "diag stuff");
+        const types::global_dof_index disp_start_index = system_matrix.get_row_indices().block_start(
+                    SolutionBlocks::displacement);
+        const types::global_dof_index disp_mult_start_index = system_matrix.get_row_indices().block_start(
+                    SolutionBlocks::displacement_multiplier);
+
+        for (auto &pair: boundary_values) {
+            const auto dof_index=pair.first;
+            const types::global_dof_index n_u = system_matrix.block(SolutionBlocks::displacement,
+                                                                    SolutionBlocks::displacement).m();
+            if ((dof_index >= disp_start_index) && (dof_index < disp_start_index + n_u)) {
+                double diag_val = system_matrix.block(SolutionBlocks::displacement,
+                                                      SolutionBlocks::displacement).el(
+                            dof_index - disp_start_index, dof_index - disp_start_index);
+
+                system_matrix.block(SolutionBlocks::displacement, SolutionBlocks::displacement_multiplier).set(
+                            dof_index - disp_start_index, dof_index - disp_start_index, diag_val);
 
-        a_inv_mf_gmg.set_exemplar_vector(distributed_state.block(SolutionBlocks::displacement));
+            }
+            else if ((dof_index >= disp_mult_start_index) && (dof_index < disp_mult_start_index + n_u))
+            {
+                double diag_val = system_matrix.block(SolutionBlocks::displacement_multiplier,
+                                                      SolutionBlocks::displacement_multiplier).el(
+                            dof_index - disp_mult_start_index, dof_index - disp_mult_start_index);
+                system_matrix.block(SolutionBlocks::displacement_multiplier, SolutionBlocks::displacement).set(
+                            dof_index - disp_mult_start_index, dof_index - disp_mult_start_index, diag_val);
+            }
+        }
 
-        TimerOutput::Scope t(timer, "initialize");
-        {
-                TimerOutput::Scope t(timer, "diag stuff");
-                const types::global_dof_index disp_start_index = system_matrix.get_row_indices().block_start(
+        //set diagonal to 0?
+        for (auto &pair: boundary_values) {
+            const auto dof_index=pair.first;
+            const types::global_dof_index disp_start_index = system_matrix.get_row_indices().block_start(
                         SolutionBlocks::displacement);
-                const types::global_dof_index disp_mult_start_index = system_matrix.get_row_indices().block_start(
+            const types::global_dof_index disp_mult_start_index = system_matrix.get_row_indices().block_start(
                         SolutionBlocks::displacement_multiplier);
+            const types::global_dof_index n_u = system_matrix.block(SolutionBlocks::displacement,
+                                                                    SolutionBlocks::displacement).m();
+            if ((dof_index >= disp_start_index) && (dof_index < disp_start_index + n_u)) {
+                system_matrix.block(SolutionBlocks::displacement, SolutionBlocks::displacement).set(
+                            dof_index - disp_start_index, dof_index - disp_start_index, 0);
+            } else if ((dof_index >= disp_mult_start_index) && (dof_index < disp_mult_start_index + n_u)) {
+                system_matrix.block(SolutionBlocks::displacement_multiplier,
+                                    SolutionBlocks::displacement_multiplier).set(
+                            dof_index - disp_mult_start_index, dof_index - disp_mult_start_index, 0);
+            }
+        }
 
-                for (auto &pair: boundary_values) {
-                    const auto dof_index=pair.first;
-                    const types::global_dof_index n_u = system_matrix.block(SolutionBlocks::displacement,
-                                                                            SolutionBlocks::displacement).m();
-                    if ((dof_index >= disp_start_index) && (dof_index < disp_start_index + n_u)) {
-                        double diag_val = system_matrix.block(SolutionBlocks::displacement,
-                                                              SolutionBlocks::displacement).el(
-                                dof_index - disp_start_index, dof_index - disp_start_index);
-
-                        system_matrix.block(SolutionBlocks::displacement, SolutionBlocks::displacement_multiplier).set(
-                                dof_index - disp_start_index, dof_index - disp_start_index, diag_val);
-
-                    }
-                    else if ((dof_index >= disp_mult_start_index) && (dof_index < disp_mult_start_index + n_u))
-                    {
-                        double diag_val = system_matrix.block(SolutionBlocks::displacement_multiplier,
-                                                              SolutionBlocks::displacement_multiplier).el(
-                                dof_index - disp_mult_start_index, dof_index - disp_mult_start_index);
-                        system_matrix.block(SolutionBlocks::displacement_multiplier, SolutionBlocks::displacement).set(
-                                dof_index - disp_mult_start_index, dof_index - disp_mult_start_index, diag_val);
-                    }
-                }
+        system_matrix.compress(VectorOperation::insert);
+    }
 
-                //set diagonal to 0?
-                for (auto &pair: boundary_values) {
-                    const auto dof_index=pair.first;
-                    const types::global_dof_index disp_start_index = system_matrix.get_row_indices().block_start(
-                            SolutionBlocks::displacement);
-                    const types::global_dof_index disp_mult_start_index = system_matrix.get_row_indices().block_start(
-                            SolutionBlocks::displacement_multiplier);
-                    const types::global_dof_index n_u = system_matrix.block(SolutionBlocks::displacement,
-                                                                            SolutionBlocks::displacement).m();
-                    if ((dof_index >= disp_start_index) && (dof_index < disp_start_index + n_u)) {
-                        system_matrix.block(SolutionBlocks::displacement, SolutionBlocks::displacement).set(
-                                dof_index - disp_start_index, dof_index - disp_start_index, 0);
-                    } else if ((dof_index >= disp_mult_start_index) && (dof_index < disp_mult_start_index + n_u)) {
-                        system_matrix.block(SolutionBlocks::displacement_multiplier,
-                                            SolutionBlocks::displacement_multiplier).set(
-                                dof_index - disp_mult_start_index, dof_index - disp_mult_start_index, 0);
-                    }
-                }
 
-            system_matrix.compress(VectorOperation::insert);
-        }
 
+    //        if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
+    //        {
+    //            TimerOutput::Scope t(timer, "build A inv");
+
+    //             std::vector<std::vector<bool>> constant_modes;
+    //             FEValuesExtractors::Vector     displacement_components(SolutionComponents::displacement<dim>);
+    //             DoFTools::extract_constant_modes(dof_handler,
+    //                                              dof_handler.get_fe_collection()
+    //                                              .component_mask(displacement_components),
+    //                                              constant_modes);
+    //             TrilinosWrappers::PreconditionAMG::AdditionalData amg_data;
+    //             amg_data.constant_modes = constant_modes;
+    ////             amg_data.n_cycles = 1;
+    ////             amg_data.w_cycle = true;
+
+    //             pre_amg.initialize(a_mat,amg_data);
 
+    //        }
+    //        else
+    //        {
+    //             a_inv_direct.initialize(a_mat);
+    //        }
+    {
+        TimerOutput::Scope t(timer, "reinit diag matrices");
+        d_3_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
+        d_4_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
+        d_5_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
+        d_6_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
+        d_7_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
+        d_8_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
+        d_m_inv_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
+        d_3_mat=0;
+        d_4_mat=0;
+        d_5_mat=0;
+        d_6_mat=0;
+        d_7_mat=0;
+        d_8_mat=0;
+        d_m_inv_mat=0;
+        pcout << "diag reinit" << std::endl;
+    }
+    {
+        const types::global_dof_index n_p = system_matrix.block(SolutionBlocks::density,
+                                                                SolutionBlocks::density).m();
 
-//        if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
-//        {
-//            TimerOutput::Scope t(timer, "build A inv");
+        pcout << "np: " << n_p << std::endl;
 
-//             std::vector<std::vector<bool>> constant_modes;
-//             FEValuesExtractors::Vector     displacement_components(SolutionComponents::displacement<dim>);
-//             DoFTools::extract_constant_modes(dof_handler,
-//                                              dof_handler.get_fe_collection()
-//                                              .component_mask(displacement_components),
-//                                              constant_modes);
-//             TrilinosWrappers::PreconditionAMG::AdditionalData amg_data;
-//             amg_data.constant_modes = constant_modes;
-////             amg_data.n_cycles = 1;
-////             amg_data.w_cycle = true;
+        std::vector<double> l_global(n_p);
+        std::vector<double> lm_global(n_p);
+        std::vector<double> u_global(n_p);
+        std::vector<double> um_global(n_p);
+        std::vector<double> m_global(n_p);
 
-//             pre_amg.initialize(a_mat,amg_data);
+        std::vector<double> l(n_p);
+        std::vector<double> lm(n_p);
+        std::vector<double> u(n_p);
+        std::vector<double> um(n_p);
+        std::vector<double> m(n_p);
 
-//        }
-//        else
-//        {
-//             a_inv_direct.initialize(a_mat);
-//        }
-        {
-            TimerOutput::Scope t(timer, "reinit diag matrices");
-            d_3_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
-            d_4_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
-            d_5_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
-            d_6_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
-            d_7_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
-            d_8_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
-            d_m_inv_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
-            d_3_mat=0;
-            d_4_mat=0;
-            d_5_mat=0;
-            d_6_mat=0;
-            d_7_mat=0;
-            d_8_mat=0;
-            d_m_inv_mat=0;
-            pcout << "diag reinit" << std::endl;
-        }
+        TimerOutput::Scope t(timer, "build diag matrices");
+        for (const auto cell: dof_handler.active_cell_iterators())
         {
-            const types::global_dof_index n_p = system_matrix.block(SolutionBlocks::density,
-                                                                    SolutionBlocks::density).m();
+            if(cell->is_locally_owned())
+            {
+                std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
+                cell->get_dof_indices(i);
+
+                const int i_ind = cell->get_fe().component_to_system_index(0,0);
 
-            pcout << "np: " << n_p << std::endl;
+                if(distributed_state.block(SolutionBlocks::density_lower_slack_multiplier).in_local_range(i[i_ind]))
+                {
+                    lm[i[i_ind]] = distributed_state.block(SolutionBlocks::density_lower_slack_multiplier)[i[i_ind]];
+                    m[i[i_ind]] = cell->measure();
+                }
 
-            std::vector<double> l_global(n_p);
-            std::vector<double> lm_global(n_p);
-            std::vector<double> u_global(n_p);
-            std::vector<double> um_global(n_p);
-            std::vector<double> m_global(n_p);
+                if(distributed_state.block(SolutionBlocks::density_lower_slack).in_local_range(i[i_ind]))
+                {
+                    l[i[i_ind]] =  distributed_state.block(SolutionBlocks::density_lower_slack)[i[i_ind]];
+                }
 
-            std::vector<double> l(n_p);
-            std::vector<double> lm(n_p);
-            std::vector<double> u(n_p);
-            std::vector<double> um(n_p);
-            std::vector<double> m(n_p);
+                if(distributed_state.block(SolutionBlocks::density_upper_slack_multiplier).in_local_range(i[i_ind]))
+                {
+                    um[i[i_ind]]= distributed_state.block(SolutionBlocks::density_upper_slack_multiplier)[i[i_ind]];
+                }
 
-            TimerOutput::Scope t(timer, "build diag matrices");
-            for (const auto cell: dof_handler.active_cell_iterators())
-            {
-                if(cell->is_locally_owned())
+                if(distributed_state.block(SolutionBlocks::density_upper_slack).in_local_range(i[i_ind]))
                 {
-                    std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
-                    cell->get_dof_indices(i);
-
-                    const int i_ind = cell->get_fe().component_to_system_index(0,0);
-
-                    if(distributed_state.block(SolutionBlocks::density_lower_slack_multiplier).in_local_range(i[i_ind]))
-                    {
-                        lm[i[i_ind]] = distributed_state.block(SolutionBlocks::density_lower_slack_multiplier)[i[i_ind]];
-                        m[i[i_ind]] = cell->measure();
-                    }
-
-                    if(distributed_state.block(SolutionBlocks::density_lower_slack).in_local_range(i[i_ind]))
-                    {
-                        l[i[i_ind]] =  distributed_state.block(SolutionBlocks::density_lower_slack)[i[i_ind]];
-                    }
-
-                    if(distributed_state.block(SolutionBlocks::density_upper_slack_multiplier).in_local_range(i[i_ind]))
-                    {
-                        um[i[i_ind]]= distributed_state.block(SolutionBlocks::density_upper_slack_multiplier)[i[i_ind]];
-                    }
-
-                    if(distributed_state.block(SolutionBlocks::density_upper_slack).in_local_range(i[i_ind]))
-                    {
-                        u[i[i_ind]] = distributed_state.block(SolutionBlocks::density_upper_slack)[i[i_ind]];
-                    }
+                    u[i[i_ind]] = distributed_state.block(SolutionBlocks::density_upper_slack)[i[i_ind]];
                 }
             }
+        }
 
-            MPI_Allreduce(lm.data(), lm_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-            MPI_Allreduce(l.data(), l_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-            MPI_Allreduce(um.data(), um_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-            MPI_Allreduce(u.data(), u_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
-            MPI_Allreduce(m.data(), m_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+        MPI_Allreduce(lm.data(), lm_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+        MPI_Allreduce(l.data(), l_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+        MPI_Allreduce(um.data(), um_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+        MPI_Allreduce(u.data(), u_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+        MPI_Allreduce(m.data(), m_global.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
 
-            for (unsigned int k=0; k< n_p; k++)
+        for (unsigned int k=0; k< n_p; k++)
+        {
+            if(distributed_state.block(0).in_local_range(k))
             {
-                    if(distributed_state.block(0).in_local_range(k))
-                    {
-                        d_3_mat.set(k, k, -1 * lm_global[k]/(m_global[k]*l_global[k]));
-                        d_4_mat.set(k, k, -1 * um_global[k]/(m_global[k]*u_global[k]));
-                        d_5_mat.set(k, k, lm_global[k]/l_global[k]);
-                        d_6_mat.set(k, k, um_global[k]/u_global[k]);
-                        d_7_mat.set(k, k, m_global[k]*(lm_global[k]*u_global[k] + um_global[k]*l_global[k])/(l_global[k]*u_global[k]));
-                        d_8_mat.set(k, k, l_global[k]*u_global[k]/(m_global[k]*(lm_global[k]*u_global[k] + um_global[k]*l_global[k])));
-                        d_m_inv_mat.set(k, k, 1 / m_global[k]);
-                    }
+                d_3_mat.set(k, k, -1 * lm_global[k]/(m_global[k]*l_global[k]));
+                d_4_mat.set(k, k, -1 * um_global[k]/(m_global[k]*u_global[k]));
+                d_5_mat.set(k, k, lm_global[k]/l_global[k]);
+                d_6_mat.set(k, k, um_global[k]/u_global[k]);
+                d_7_mat.set(k, k, m_global[k]*(lm_global[k]*u_global[k] + um_global[k]*l_global[k])/(l_global[k]*u_global[k]));
+                d_8_mat.set(k, k, l_global[k]*u_global[k]/(m_global[k]*(lm_global[k]*u_global[k] + um_global[k]*l_global[k])));
+                d_m_inv_mat.set(k, k, 1 / m_global[k]);
             }
         }
-        d_3_mat.compress(VectorOperation::insert);
-        d_4_mat.compress(VectorOperation::insert);
-        d_5_mat.compress(VectorOperation::insert);
-        d_6_mat.compress(VectorOperation::insert);
-        d_7_mat.compress(VectorOperation::insert);
-        d_8_mat.compress(VectorOperation::insert);
-        d_m_inv_mat.compress(VectorOperation::insert);
-
-        pre_j=distributed_state.block(SolutionBlocks::density);
-        pre_k=distributed_state.block(SolutionBlocks::density);
-        g_d_m_inv_density=distributed_state.block(SolutionBlocks::density);
-        k_g_d_m_inv_density=distributed_state.block(SolutionBlocks::density);
+    }
+    d_3_mat.compress(VectorOperation::insert);
+    d_4_mat.compress(VectorOperation::insert);
+    d_5_mat.compress(VectorOperation::insert);
+    d_6_mat.compress(VectorOperation::insert);
+    d_7_mat.compress(VectorOperation::insert);
+    d_8_mat.compress(VectorOperation::insert);
+    d_m_inv_mat.compress(VectorOperation::insert);
+
+    pre_j=distributed_state.block(SolutionBlocks::density);
+    pre_k=distributed_state.block(SolutionBlocks::density);
+    g_d_m_inv_density=distributed_state.block(SolutionBlocks::density);
+    k_g_d_m_inv_density=distributed_state.block(SolutionBlocks::density);
+
+    LA::MPI::Vector density_exemplar = distributed_state.block(SolutionBlocks::density);
+    LA::MPI::Vector displacement_exemplar = distributed_state.block(SolutionBlocks::displacement);
+
+    g_mat.initialize(density_exemplar);
+    h_mat.initialize(density_exemplar, displacement_exemplar);
+    j_inv_mat.initialize(density_exemplar);
+    k_inv_mat.initialize(density_exemplar);
+
+    num_mults = 0;
+}
 
-        LA::MPI::Vector density_exemplar = distributed_state.block(SolutionBlocks::density);
-        LA::MPI::Vector displacement_exemplar = distributed_state.block(SolutionBlocks::displacement);
+///Application of the preconditioner, broken up into 5 parts.
+template<int dim>
+void TopOptSchurPreconditioner<dim>::vmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
+    pcout << " starting vmult " << num_mults << std::endl;
+    LA::MPI::BlockVector temp_src;
+    {
+        TimerOutput::Scope t(timer, "part 1");
+        vmult_step_1(dst, src);
+        temp_src = dst;
+    }
 
-        g_mat.initialize(density_exemplar);
-        h_mat.initialize(density_exemplar, displacement_exemplar);
-        j_inv_mat.initialize(density_exemplar);
-        k_inv_mat.initialize(density_exemplar);
+    {
+        TimerOutput::Scope t(timer, "part 2");
+        vmult_step_2(dst, temp_src);
+        temp_src = dst;
 
+    }
 
+    {
+        TimerOutput::Scope t(timer, "part 3");
+        vmult_step_3(dst, temp_src);
+        temp_src = dst;
     }
+    {
+        TimerOutput::Scope t(timer, "part 4");
+        vmult_step_4(dst, temp_src);
+        temp_src = dst;
+    }
+    vmult_step_5(dst, temp_src);
+    num_mults++;
+}
 
-    ///Application of the preconditioner, broken up into 5 parts.
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::vmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
-        LA::MPI::BlockVector temp_src;
-        {
-            TimerOutput::Scope t(timer, "part 1");
-            vmult_step_1(dst, src);
-            temp_src = dst;
-        }
+///Not implemented
+template<int dim>
+void TopOptSchurPreconditioner<dim>::Tvmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
+    dst = src;
+}
 
-        {
-            TimerOutput::Scope t(timer, "part 2");
-            vmult_step_2(dst, temp_src);
-            temp_src = dst;
+template<int dim>
+void TopOptSchurPreconditioner<dim>::vmult_add(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
+    LA::MPI::BlockVector dst_temp = dst;
+    vmult(dst_temp, src);
+    dst += dst_temp;
+}
 
-        }
+///Not implemented
+template<int dim>
+void TopOptSchurPreconditioner<dim>::Tvmult_add(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
+    dst = src;
+}
 
-        {
-            TimerOutput::Scope t(timer, "part 3");
-            vmult_step_3(dst, temp_src);
-            temp_src = dst;
-        }
-        {
-            TimerOutput::Scope t(timer, "part 4");
-            vmult_step_4(dst, temp_src);
-            temp_src = dst;
-        }
-        vmult_step_5(dst, temp_src);
-    }
+template<int dim>
+void TopOptSchurPreconditioner<dim>::vmult_step_1(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
+    dst = src;
+    auto dst_temp = dst;
+    dst.block(SolutionBlocks::unfiltered_density) = dst_temp.block(SolutionBlocks::unfiltered_density)
+            - linear_operator<VectorType,VectorType,PayloadType>(d_5_mat) * src.block(SolutionBlocks::density_lower_slack_multiplier)
+            + linear_operator<VectorType,VectorType,PayloadType>(d_6_mat) * src.block(SolutionBlocks::density_upper_slack_multiplier)
+            + src.block(SolutionBlocks::density_lower_slack)
+            - src.block(SolutionBlocks::density_upper_slack);
+}
 
-    ///Not implemented
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::Tvmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
-        dst = src;
-    }
+template<int dim>
+void TopOptSchurPreconditioner<dim>::vmult_step_2(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
+    dst = src;
+    auto dst_temp = dst;
 
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::vmult_add(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
-        LA::MPI::BlockVector dst_temp = dst;
-        vmult(dst_temp, src);
-        dst += dst_temp;
-    }
+    dst.block(SolutionBlocks::unfiltered_density_multiplier) = dst_temp.block(SolutionBlocks::unfiltered_density_multiplier)
+            - linear_operator<VectorType,VectorType,PayloadType>(f_mat)*linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) * src.block(SolutionBlocks::unfiltered_density);
 
-    ///Not implemented
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::Tvmult_add(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
-        dst = src;
-    }
+}
 
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::vmult_step_1(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
-        dst = src;
-        auto dst_temp = dst;
-        dst.block(SolutionBlocks::unfiltered_density) = dst_temp.block(SolutionBlocks::unfiltered_density)
-                - linear_operator<VectorType,VectorType,PayloadType>(d_5_mat) * src.block(SolutionBlocks::density_lower_slack_multiplier)
-                + linear_operator<VectorType,VectorType,PayloadType>(d_6_mat) * src.block(SolutionBlocks::density_upper_slack_multiplier)
-                + src.block(SolutionBlocks::density_lower_slack)
-                - src.block(SolutionBlocks::density_upper_slack);
-    }
+template<int dim>
+void TopOptSchurPreconditioner<dim>::vmult_step_3(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
+
+    dst = src;
+    auto dst_temp = dst;
 
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::vmult_step_2(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
-        dst = src;
-        auto dst_temp = dst;
+    if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
+    {
+        //            SolverControl            solver_control_1(100000, 1e-6);
+        //            SolverControl            solver_control_2(100000, 1e-6);
+        //            SolverCG<LA::MPI::Vector> a_solver_cg_1(solver_control_1);
+        //            SolverCG<LA::MPI::Vector> a_solver_cg_2(solver_control_2);
+        //            a_solver_cg_1.solve(a_mat,dst_temp.block(SolutionBlocks::displacement_multiplier),src.block(SolutionBlocks::displacement_multiplier),pre_amg);
+        //            a_solver_cg_2.solve(a_mat,dst_temp.block(SolutionBlocks::displacement),src.block(SolutionBlocks::displacement),pre_amg);
+
+        a_inv_mf_gmg.vmult(dst_temp.block(SolutionBlocks::displacement_multiplier),src.block(SolutionBlocks::displacement_multiplier));
+
+        a_inv_mf_gmg.vmult(dst_temp.block(SolutionBlocks::displacement),src.block(SolutionBlocks::displacement));
+        c_mat.Tvmult(dst_temp.block(SolutionBlocks::density_upper_slack),dst_temp.block(SolutionBlocks::displacement_multiplier));
+        e_mat.Tvmult(dst_temp.block(SolutionBlocks::density_lower_slack),dst_temp.block(SolutionBlocks::displacement));
 
-        dst.block(SolutionBlocks::unfiltered_density_multiplier) = dst_temp.block(SolutionBlocks::unfiltered_density_multiplier)
-                - linear_operator<VectorType,VectorType,PayloadType>(f_mat)*linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) * src.block(SolutionBlocks::unfiltered_density);
+        dst.block(SolutionBlocks::density) = dst_temp.block(SolutionBlocks::density) - dst_temp.block(SolutionBlocks::density_upper_slack) - dst_temp.block(SolutionBlocks::density_lower_slack);
 
     }
+    else
+    {
+        a_inv_direct.vmult(dst_temp.block(SolutionBlocks::displacement_multiplier),src.block(SolutionBlocks::displacement_multiplier));
+        a_inv_direct.vmult(dst_temp.block(SolutionBlocks::displacement),src.block(SolutionBlocks::displacement));
+        c_mat.Tvmult(dst_temp.block(SolutionBlocks::density_upper_slack),dst_temp.block(SolutionBlocks::displacement_multiplier));
+        e_mat.Tvmult(dst_temp.block(SolutionBlocks::density_lower_slack),dst_temp.block(SolutionBlocks::displacement));
+        dst.block(SolutionBlocks::density) = dst_temp.block(SolutionBlocks::density) - dst_temp.block(SolutionBlocks::density_upper_slack) - dst_temp.block(SolutionBlocks::density_lower_slack);
+    }
 
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::vmult_step_3(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
 
-        dst = src;
-        auto dst_temp = dst;
 
-        if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
-        {
-//            SolverControl            solver_control_1(100000, 1e-6);
-//            SolverControl            solver_control_2(100000, 1e-6);
-//            SolverCG<LA::MPI::Vector> a_solver_cg_1(solver_control_1);
-//            SolverCG<LA::MPI::Vector> a_solver_cg_2(solver_control_2);
-//            a_solver_cg_1.solve(a_mat,dst_temp.block(SolutionBlocks::displacement_multiplier),src.block(SolutionBlocks::displacement_multiplier),pre_amg);
-//            a_solver_cg_2.solve(a_mat,dst_temp.block(SolutionBlocks::displacement),src.block(SolutionBlocks::displacement),pre_amg);
+}
 
-            a_inv_mf_gmg.vmult(dst_temp.block(SolutionBlocks::displacement_multiplier),src.block(SolutionBlocks::displacement_multiplier));
+template<int dim>
+void TopOptSchurPreconditioner<dim>::vmult_step_4(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
+    dst = src;
+    auto dst_temp = dst;
+    auto k_density_mult =  src.block(SolutionBlocks::density);
+
+    TrilinosWrappers::PreconditionIdentity preconditioner;
+    preconditioner.initialize(b_mat);
+
+    auto d_m_inv_density = g_d_m_inv_density;
+    d_m_inv_mat.vmult(d_m_inv_density,src.block(SolutionBlocks::density));
+    g_mat.vmult(g_d_m_inv_density,d_m_inv_density);
+    SolverControl step_4_gmres_control_1 (Input::k_inv_iterations, g_d_m_inv_density.l2_norm()*1e-12);
+    SolverFGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
+    try {
+        step_4_gmres_1.solve(k_inv_mat,k_g_d_m_inv_density,g_d_m_inv_density, preconditioner );
+    }
+    catch (std::exception &exc)
+    {
+        pcout << "Failure of linear solver step_4_gmres_1" << std::endl;
+        pcout << "first residual: " << step_4_gmres_control_1.initial_value() << std::endl;
+        pcout << "last residual: " << step_4_gmres_control_1.last_value() << std::endl;
+        //                throw;
+    }
+    SolverControl step_4_gmres_control_2 (Input::k_inv_iterations, 1e-12 * src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm() );
+    SolverFGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
+    try {
+        step_4_gmres_2.solve(k_inv_mat,k_density_mult,src.block(SolutionBlocks::unfiltered_density_multiplier), PreconditionIdentity());
+    } catch (std::exception &exc)
+    {
+        pcout << "Failure of linear solver step_4_gmres_2" << std::endl;
+        pcout << "first residual: " << step_4_gmres_control_2.initial_value() << std::endl;
+        pcout << "last residual: " << step_4_gmres_control_2.last_value() << std::endl;
+        //                throw;
+    }
 
-            a_inv_mf_gmg.vmult(dst_temp.block(SolutionBlocks::displacement),src.block(SolutionBlocks::displacement));
-            c_mat.Tvmult(dst_temp.block(SolutionBlocks::density_upper_slack),dst_temp.block(SolutionBlocks::displacement_multiplier));
-            e_mat.Tvmult(dst_temp.block(SolutionBlocks::density_lower_slack),dst_temp.block(SolutionBlocks::displacement));
 
-            dst.block(SolutionBlocks::density) = dst_temp.block(SolutionBlocks::density) - dst_temp.block(SolutionBlocks::density_upper_slack) - dst_temp.block(SolutionBlocks::density_lower_slack);
+    dst.block(SolutionBlocks::total_volume_multiplier) = transpose_operator<VectorType, VectorType, PayloadType>(m_vect)*k_g_d_m_inv_density
+            - transpose_operator<VectorType, VectorType, PayloadType>(m_vect)*k_density_mult
+            +dst_temp.block(SolutionBlocks::total_volume_multiplier);
+}
 
-        }
-        else
-        {
-             a_inv_direct.vmult(dst_temp.block(SolutionBlocks::displacement_multiplier),src.block(SolutionBlocks::displacement_multiplier));
-             a_inv_direct.vmult(dst_temp.block(SolutionBlocks::displacement),src.block(SolutionBlocks::displacement));
-             c_mat.Tvmult(dst_temp.block(SolutionBlocks::density_upper_slack),dst_temp.block(SolutionBlocks::displacement_multiplier));
-             e_mat.Tvmult(dst_temp.block(SolutionBlocks::density_lower_slack),dst_temp.block(SolutionBlocks::displacement));
-             dst.block(SolutionBlocks::density) = dst_temp.block(SolutionBlocks::density) - dst_temp.block(SolutionBlocks::density_upper_slack) - dst_temp.block(SolutionBlocks::density_lower_slack);
-        }
 
 
+///The only non-triangular vmult step, applies inverses down the block diagonal.
+template<int dim>
+void TopOptSchurPreconditioner<dim>::vmult_step_5(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
+    {
+        //First Block Inverse
+        TimerOutput::Scope t(timer, "inverse 1");
+        dst.block(SolutionBlocks::density_lower_slack_multiplier) = linear_operator<VectorType,VectorType,PayloadType>(d_3_mat) * src.block(SolutionBlocks::density_lower_slack_multiplier) +
+                linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density_lower_slack);
+        dst.block(SolutionBlocks::density_upper_slack_multiplier) = linear_operator<VectorType,VectorType,PayloadType>(d_4_mat) * src.block(SolutionBlocks::density_upper_slack_multiplier) +
+                linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density_upper_slack);
+        dst.block(SolutionBlocks::density_lower_slack) = linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density_lower_slack_multiplier);
+        dst.block(SolutionBlocks::density_upper_slack) = linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density_upper_slack_multiplier);
+    }
 
+    {
+        //Second Block Inverse
+        TimerOutput::Scope t(timer, "inverse 2");
+        dst.block(SolutionBlocks::unfiltered_density) =
+                linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) * src.block(SolutionBlocks::unfiltered_density);
     }
 
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::vmult_step_4(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
-        dst = src;
-        auto dst_temp = dst;
-        auto k_density_mult =  src.block(SolutionBlocks::density);
 
-        TrilinosWrappers::PreconditionIdentity preconditioner;
-        preconditioner.initialize(b_mat);
+    {
+        //Third Block Inverse
+        TimerOutput::Scope t(timer, "inverse 3");
+        if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
+        {
+            //                SolverControl            solver_control(100000, 1e-6);
+            //                SolverCG<LA::MPI::Vector> a_solver_cg(solver_control);
 
-            auto d_m_inv_density = g_d_m_inv_density;
-            d_m_inv_mat.vmult(d_m_inv_density,src.block(SolutionBlocks::density));
-            g_mat.vmult(g_d_m_inv_density,d_m_inv_density);
-            SolverControl step_4_gmres_control_1 (Input::k_inv_iterations, g_d_m_inv_density.l2_norm()*1e-12);
-            SolverFGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
-            try {
-                step_4_gmres_1.solve(k_inv_mat,k_g_d_m_inv_density,g_d_m_inv_density, preconditioner );
-            }
-            catch (std::exception &exc)
-            {
-                pcout << "Failure of linear solver step_4_gmres_1" << std::endl;
-                pcout << "first residual: " << step_4_gmres_control_1.initial_value() << std::endl;
-                pcout << "last residual: " << step_4_gmres_control_1.last_value() << std::endl;
-//                throw;
-            }
-            SolverControl step_4_gmres_control_2 (Input::k_inv_iterations, 1e-12 * src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm() );
-            SolverFGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
-            try {
-                step_4_gmres_2.solve(k_inv_mat,k_density_mult,src.block(SolutionBlocks::unfiltered_density_multiplier), PreconditionIdentity());
-            } catch (std::exception &exc)
-            {
-                pcout << "Failure of linear solver step_4_gmres_2" << std::endl;
-                pcout << "first residual: " << step_4_gmres_control_2.initial_value() << std::endl;
-                pcout << "last residual: " << step_4_gmres_control_2.last_value() << std::endl;
-//                throw;
-            }
+            //                auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg, pre_amg);
+
+            //                dst.block(SolutionBlocks::displacement) = a_inv_op * src.block(SolutionBlocks::displacement_multiplier);
+            //                dst.block(SolutionBlocks::displacement_multiplier) = a_inv_op * src.block(SolutionBlocks::displacement);
+
+            a_inv_mf_gmg.vmult( dst.block(SolutionBlocks::displacement), src.block(SolutionBlocks::displacement_multiplier));
+            a_inv_mf_gmg.vmult( dst.block(SolutionBlocks::displacement_multiplier), src.block(SolutionBlocks::displacement));
+        }
+        else
+        {
+            a_inv_direct.vmult( dst.block(SolutionBlocks::displacement), src.block(SolutionBlocks::displacement_multiplier));
+            a_inv_direct.vmult( dst.block(SolutionBlocks::displacement_multiplier), src.block(SolutionBlocks::displacement));
+        }
 
 
-        dst.block(SolutionBlocks::total_volume_multiplier) = transpose_operator<VectorType, VectorType, PayloadType>(m_vect)*k_g_d_m_inv_density
-                                                        - transpose_operator<VectorType, VectorType, PayloadType>(m_vect)*k_density_mult
-                                                        +dst_temp.block(SolutionBlocks::total_volume_multiplier);
     }
+    {
+        //Fourth (ugly) Block Inverse
+        TimerOutput::Scope t(timer, "inverse 4");
 
 
 
-    ///The only non-triangular vmult step, applies inverses down the block diagonal.
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::vmult_step_5(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
+        if (Input::solver_choice == SolverOptions::exact_preconditioner_with_gmres)
         {
-            //First Block Inverse
-            TimerOutput::Scope t(timer, "inverse 1");
-            dst.block(SolutionBlocks::density_lower_slack_multiplier) = linear_operator<VectorType,VectorType,PayloadType>(d_3_mat) * src.block(SolutionBlocks::density_lower_slack_multiplier) +
-                    linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density_lower_slack);
-            dst.block(SolutionBlocks::density_upper_slack_multiplier) = linear_operator<VectorType,VectorType,PayloadType>(d_4_mat) * src.block(SolutionBlocks::density_upper_slack_multiplier) +
-                    linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density_upper_slack);
-            dst.block(SolutionBlocks::density_lower_slack) = linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density_lower_slack_multiplier);
-            dst.block(SolutionBlocks::density_upper_slack) = linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density_upper_slack_multiplier);
+            //                pre_j = src.block(SolutionBlocks::density) + linear_operator<VectorType,VectorType,PayloadType>(h_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
+            //                pre_k = -1* linear_operator<VectorType,VectorType,PayloadType>(g_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density) + src.block(SolutionBlocks::unfiltered_density_multiplier);
+            //                dst.block(SolutionBlocks::unfiltered_density_multiplier) = transpose_operator<VectorType, VectorType, PayloadType>(k_mat) * pre_j;
+            //                dst.block(SolutionBlocks::density) = linear_operator<VectorType,VectorType,PayloadType>(k_mat) * pre_k;
         }
 
+        else if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
         {
-            //Second Block Inverse
-            TimerOutput::Scope t(timer, "inverse 2");
-            dst.block(SolutionBlocks::unfiltered_density) =
-                    linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) * src.block(SolutionBlocks::unfiltered_density);
-        }
 
+            TrilinosWrappers::PreconditionIdentity preconditioner;
+            preconditioner.initialize(b_mat);
+            auto pre_pre_k = pre_k;
+            auto pre_pre_pre_k = pre_k;
+            auto d_m_inv_unfil_density_mult = pre_k;
+            auto h_d_m_inv_unfil_density_mult = pre_k;
 
-        {
-            //Third Block Inverse
-            TimerOutput::Scope t(timer, "inverse 3");
-            if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
             {
-//                SolverControl            solver_control(100000, 1e-6);
-//                SolverCG<LA::MPI::Vector> a_solver_cg(solver_control);
-
-//                auto a_inv_op = inverse_operator(linear_operator<VectorType,VectorType,PayloadType>(a_mat),a_solver_cg, pre_amg);
-
-//                dst.block(SolutionBlocks::displacement) = a_inv_op * src.block(SolutionBlocks::displacement_multiplier);
-//                dst.block(SolutionBlocks::displacement_multiplier) = a_inv_op * src.block(SolutionBlocks::displacement);
-
-                  a_inv_mf_gmg.vmult( dst.block(SolutionBlocks::displacement), src.block(SolutionBlocks::displacement_multiplier));
-                  a_inv_mf_gmg.vmult( dst.block(SolutionBlocks::displacement_multiplier), src.block(SolutionBlocks::displacement));
+                TimerOutput::Scope t(timer, "not inverse 5.1");
+                d_m_inv_mat.vmult(d_m_inv_unfil_density_mult, src.block(SolutionBlocks::unfiltered_density_multiplier));
+                h_mat.vmult(h_d_m_inv_unfil_density_mult,d_m_inv_unfil_density_mult);
+                pre_j = src.block(SolutionBlocks::density) + h_d_m_inv_unfil_density_mult;
+                d_m_inv_mat.vmult(pre_pre_pre_k,src.block(SolutionBlocks::density));
+                g_mat.vmult(pre_pre_k,pre_pre_pre_k);
+                pre_k =  -1 * pre_pre_k + src.block(SolutionBlocks::unfiltered_density_multiplier);
             }
-            else
+            SolverControl step_5_gmres_control_1 (Input::k_inv_iterations, 1e-12*pre_j.l2_norm());
+            SolverFGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
+            try {
+                TimerOutput::Scope t(timer, "actual inverse 5.1");
+                step_5_gmres_1.solve(j_inv_mat, dst.block(SolutionBlocks::unfiltered_density_multiplier), pre_j , preconditioner);
+            } catch (std::exception &exc)
             {
-                a_inv_direct.vmult( dst.block(SolutionBlocks::displacement), src.block(SolutionBlocks::displacement_multiplier));
-                a_inv_direct.vmult( dst.block(SolutionBlocks::displacement_multiplier), src.block(SolutionBlocks::displacement));
+                pcout << "Failure of linear solver step_5_gmres_1" << std::endl;
+                pcout << "first residual: " << step_5_gmres_control_1.initial_value() << std::endl;
+                pcout << "last residual: " << step_5_gmres_control_1.last_value() << std::endl;
+                //                    throw;
             }
 
 
+            SolverControl step_5_gmres_control_2 (Input::k_inv_iterations, 1e-12*pre_k.l2_norm());
+            SolverFGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
+            try {
+                TimerOutput::Scope t(timer, "actual inverse 5.2");
+                step_5_gmres_2.solve(k_inv_mat,dst.block(SolutionBlocks::density), pre_k , preconditioner);
+            } catch (std::exception &exc)
+            {
+                pcout << "Failure of linear solver step_5_gmres_2" << std::endl;
+                pcout << "first residual: " << step_5_gmres_control_2.initial_value() << std::endl;
+                pcout << "last residual: " << step_5_gmres_control_2.last_value() << std::endl;
+                //                    throw;
+            }
         }
+        else if (Input::solver_choice == SolverOptions::inexact_K_with_exact_A_gmres)
         {
-            //Fourth (ugly) Block Inverse
-            TimerOutput::Scope t(timer, "inverse 4");
 
+            TrilinosWrappers::PreconditionIdentity preconditioner;
+            preconditioner.initialize(b_mat);
+            auto pre_pre_k = pre_k;
+            auto pre_pre_pre_k = pre_k;
+            auto d_m_inv_unfil_density_mult = pre_k;
+            auto h_d_m_inv_unfil_density_mult = pre_k;
 
-
-            if (Input::solver_choice == SolverOptions::exact_preconditioner_with_gmres)
             {
-//                pre_j = src.block(SolutionBlocks::density) + linear_operator<VectorType,VectorType,PayloadType>(h_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::unfiltered_density_multiplier);
-//                pre_k = -1* linear_operator<VectorType,VectorType,PayloadType>(g_mat) * linear_operator<VectorType,VectorType,PayloadType>(d_m_inv_mat) * src.block(SolutionBlocks::density) + src.block(SolutionBlocks::unfiltered_density_multiplier);
-//                dst.block(SolutionBlocks::unfiltered_density_multiplier) = transpose_operator<VectorType, VectorType, PayloadType>(k_mat) * pre_j;
-//                dst.block(SolutionBlocks::density) = linear_operator<VectorType,VectorType,PayloadType>(k_mat) * pre_k;
+                TimerOutput::Scope t(timer, "not inverse 5.1");
+                d_m_inv_mat.vmult(d_m_inv_unfil_density_mult, src.block(SolutionBlocks::unfiltered_density_multiplier));
+                h_mat.vmult(h_d_m_inv_unfil_density_mult,d_m_inv_unfil_density_mult);
+                pre_j = src.block(SolutionBlocks::density) + h_d_m_inv_unfil_density_mult;
+                d_m_inv_mat.vmult(pre_pre_pre_k,src.block(SolutionBlocks::density));
+                g_mat.vmult(pre_pre_k,pre_pre_pre_k);
+                pre_k =  -1 * pre_pre_k + src.block(SolutionBlocks::unfiltered_density_multiplier);
             }
-
-            else if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
+            SolverControl step_5_gmres_control_1 (Input::k_inv_iterations, 1e-12*pre_j.l2_norm());
+            SolverFGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
+            try {
+                TimerOutput::Scope t(timer, "actual inverse 5.1");
+                step_5_gmres_1.solve(j_inv_mat, dst.block(SolutionBlocks::unfiltered_density_multiplier), pre_j , preconditioner);
+            } catch (std::exception &exc)
             {
-
-                TrilinosWrappers::PreconditionIdentity preconditioner;
-                preconditioner.initialize(b_mat);
-                auto pre_pre_k = pre_k;
-                auto pre_pre_pre_k = pre_k;
-                auto d_m_inv_unfil_density_mult = pre_k;
-                auto h_d_m_inv_unfil_density_mult = pre_k;
-
-                {
-                    TimerOutput::Scope t(timer, "not inverse 5.1");
-                    d_m_inv_mat.vmult(d_m_inv_unfil_density_mult, src.block(SolutionBlocks::unfiltered_density_multiplier));
-                    h_mat.vmult(h_d_m_inv_unfil_density_mult,d_m_inv_unfil_density_mult);
-                    pre_j = src.block(SolutionBlocks::density) + h_d_m_inv_unfil_density_mult;
-                    d_m_inv_mat.vmult(pre_pre_pre_k,src.block(SolutionBlocks::density));
-                    g_mat.vmult(pre_pre_k,pre_pre_pre_k);
-                    pre_k =  -1 * pre_pre_k + src.block(SolutionBlocks::unfiltered_density_multiplier);
-                }
-                SolverControl step_5_gmres_control_1 (Input::k_inv_iterations, 1e-12*pre_j.l2_norm());
-                SolverFGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
-                try {
-                    TimerOutput::Scope t(timer, "actual inverse 5.1");
-                    step_5_gmres_1.solve(j_inv_mat, dst.block(SolutionBlocks::unfiltered_density_multiplier), pre_j , preconditioner);
-                } catch (std::exception &exc)
-                {
-                    pcout << "Failure of linear solver step_5_gmres_1" << std::endl;
-                    pcout << "first residual: " << step_5_gmres_control_1.initial_value() << std::endl;
-                    pcout << "last residual: " << step_5_gmres_control_1.last_value() << std::endl;
-//                    throw;
-                }
-
-
-                SolverControl step_5_gmres_control_2 (Input::k_inv_iterations, 1e-12*pre_k.l2_norm());
-                SolverFGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
-                try {
-                    TimerOutput::Scope t(timer, "actual inverse 5.2");
-                    step_5_gmres_2.solve(k_inv_mat,dst.block(SolutionBlocks::density), pre_k , preconditioner);
-                } catch (std::exception &exc)
-                {
-                    pcout << "Failure of linear solver step_5_gmres_2" << std::endl;
-                    pcout << "first residual: " << step_5_gmres_control_2.initial_value() << std::endl;
-                    pcout << "last residual: " << step_5_gmres_control_2.last_value() << std::endl;
-//                    throw;
-                }
+                pcout << "Failure of linear solver step_5_gmres_1" << std::endl;
+                pcout << "first residual: " << step_5_gmres_control_1.initial_value() << std::endl;
+                pcout << "last residual: " << step_5_gmres_control_1.last_value() << std::endl;
+                //                    throw;
             }
-            else if (Input::solver_choice == SolverOptions::inexact_K_with_exact_A_gmres)
-            {
 
-                TrilinosWrappers::PreconditionIdentity preconditioner;
-                preconditioner.initialize(b_mat);
-                auto pre_pre_k = pre_k;
-                auto pre_pre_pre_k = pre_k;
-                auto d_m_inv_unfil_density_mult = pre_k;
-                auto h_d_m_inv_unfil_density_mult = pre_k;
 
-                {
-                    TimerOutput::Scope t(timer, "not inverse 5.1");
-                    d_m_inv_mat.vmult(d_m_inv_unfil_density_mult, src.block(SolutionBlocks::unfiltered_density_multiplier));
-                    h_mat.vmult(h_d_m_inv_unfil_density_mult,d_m_inv_unfil_density_mult);
-                    pre_j = src.block(SolutionBlocks::density) + h_d_m_inv_unfil_density_mult;
-                    d_m_inv_mat.vmult(pre_pre_pre_k,src.block(SolutionBlocks::density));
-                    g_mat.vmult(pre_pre_k,pre_pre_pre_k);
-                    pre_k =  -1 * pre_pre_k + src.block(SolutionBlocks::unfiltered_density_multiplier);
-                }
-                SolverControl step_5_gmres_control_1 (Input::k_inv_iterations, 1e-12*pre_j.l2_norm());
-                SolverFGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
-                try {
-                    TimerOutput::Scope t(timer, "actual inverse 5.1");
-                    step_5_gmres_1.solve(j_inv_mat, dst.block(SolutionBlocks::unfiltered_density_multiplier), pre_j , preconditioner);
-                } catch (std::exception &exc)
-                {
-                    pcout << "Failure of linear solver step_5_gmres_1" << std::endl;
-                    pcout << "first residual: " << step_5_gmres_control_1.initial_value() << std::endl;
-                    pcout << "last residual: " << step_5_gmres_control_1.last_value() << std::endl;
-//                    throw;
-                }
-
-
-                SolverControl step_5_gmres_control_2 (Input::k_inv_iterations, 1e-12*pre_k.l2_norm());
-                SolverFGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
-                try {
-                    TimerOutput::Scope t(timer, "actual inverse 5.2");
-                    step_5_gmres_2.solve(k_inv_mat,dst.block(SolutionBlocks::density), pre_k , preconditioner);
-                } catch (std::exception &exc)
-                {
-                    pcout << "Failure of linear solver step_5_gmres_2" << std::endl;
-                    pcout << "first residual: " << step_5_gmres_control_2.initial_value() << std::endl;
-                    pcout << "last residual: " << step_5_gmres_control_2.last_value() << std::endl;
-//                    throw;
-                }
-            }
-            else
+            SolverControl step_5_gmres_control_2 (Input::k_inv_iterations, 1e-12*pre_k.l2_norm());
+            SolverFGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
+            try {
+                TimerOutput::Scope t(timer, "actual inverse 5.2");
+                step_5_gmres_2.solve(k_inv_mat,dst.block(SolutionBlocks::density), pre_k , preconditioner);
+            } catch (std::exception &exc)
             {
-                pcout << "shouldn't get here";
-                throw;
+                pcout << "Failure of linear solver step_5_gmres_2" << std::endl;
+                pcout << "first residual: " << step_5_gmres_control_2.initial_value() << std::endl;
+                pcout << "last residual: " << step_5_gmres_control_2.last_value() << std::endl;
+                //                    throw;
             }
-
         }
+        else
         {
-            dst.block(SolutionBlocks::total_volume_multiplier) = src.block(SolutionBlocks::total_volume_multiplier);
+            pcout << "shouldn't get here";
+            throw;
         }
-    }
 
-    ///I used to use this function to output parts of the preconditioner for debugging.
-    template<int dim>
-    void TopOptSchurPreconditioner<dim>::print_stuff()
+    }
     {
-
-
+        dst.block(SolutionBlocks::total_volume_multiplier) = src.block(SolutionBlocks::total_volume_multiplier);
     }
+}
 
-    //******************************* Direct Trilinos Solver ***********************************
+///I used to use this function to output parts of the preconditioner for debugging.
+template<int dim>
+void TopOptSchurPreconditioner<dim>::print_stuff()
+{
 
-    ///Constructor
-    VmultTrilinosSolverDirect::VmultTrilinosSolverDirect(SolverControl &cn,
-                     const TrilinosWrappers::SolverDirect::AdditionalData &data)
-    : solver_direct(cn, data)
-    {
 
-    }
+}
 
-    ///Initialize a direct solver - works well up to a point of refinement, and then refuses to solve.
-    void VmultTrilinosSolverDirect::initialize(LA::MPI::SparseMatrix &a_mat)
-    {
-        reinit(a_mat);
-        solver_direct.initialize(a_mat);
-        size = a_mat.n();
-    }
+//******************************* Direct Trilinos Solver ***********************************
 
-    ///rephrases the solve as a vmult for easier use.
-    void
-    VmultTrilinosSolverDirect::vmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
-    {
-        solver_direct.solve(dst, src);
-    }
+///Constructor
+VmultTrilinosSolverDirect::VmultTrilinosSolverDirect(SolverControl &cn,
+                                                     const TrilinosWrappers::SolverDirect::AdditionalData &data)
+    : solver_direct(cn, data)
+{
 
-    ///rephrases the solve as a vmult for easier use.
-    void VmultTrilinosSolverDirect::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
-    {
-        solver_direct.solve(dst, src);
-    }
+}
 
-    ///rephrases the solve as a vmult for easier use - note this is a symmetric matrix
-    void
-    VmultTrilinosSolverDirect::Tvmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
-    {
-        solver_direct.solve(dst, src);
-    }
+///Initialize a direct solver - works well up to a point of refinement, and then refuses to solve.
+void VmultTrilinosSolverDirect::initialize(LA::MPI::SparseMatrix &a_mat)
+{
+    reinit(a_mat);
+    solver_direct.initialize(a_mat);
+    size = a_mat.n();
+}
 
-    ///rephrases the solve as a vmult for easier use - note this is a symmetric matrix.
-    void VmultTrilinosSolverDirect::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
-    {
-        solver_direct.solve(dst, src);
-    }
+///rephrases the solve as a vmult for easier use.
+void
+VmultTrilinosSolverDirect::vmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
+{
+    solver_direct.solve(dst, src);
+}
 
+///rephrases the solve as a vmult for easier use.
+void VmultTrilinosSolverDirect::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+{
+    solver_direct.solve(dst, src);
+}
 
+///rephrases the solve as a vmult for easier use - note this is a symmetric matrix
+void
+VmultTrilinosSolverDirect::Tvmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
+{
+    solver_direct.solve(dst, src);
+}
 
-    // **************** A inv MF GMG **********************
+///rephrases the solve as a vmult for easier use - note this is a symmetric matrix.
+void VmultTrilinosSolverDirect::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+{
+    solver_direct.solve(dst, src);
+}
 
-    ///Initializes an object that performs the A inverse operation using matrix free GMG
-    template<int dim>
-    AInvMatMFGMG<dim>::AInvMatMFGMG(MF_Elasticity_Operator<dim,1,double> &mf_elasticity_operator_in , PreconditionMG<dim, LinearAlgebra::distributed::Vector<double>, MGTransferMatrixFree<dim, double> > &mf_gmg_preconditioner_in, LA::MPI::SparseMatrix &a_mat)
-        : mf_elasticity_operator(mf_elasticity_operator_in),
-          mf_gmg_preconditioner(mf_gmg_preconditioner_in),
-          a_mat_wrapped(a_mat)
-    {
-        mf_elasticity_operator.initialize_dof_vector(temp_dst);
-        mf_elasticity_operator.initialize_dof_vector(temp_src);
-    }
 
-    ///Performs the A inverse operation
-    template<int dim>
-    void AInvMatMFGMG<dim>::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
-    {
 
-        SolverControl            solver_control(50, 1e-6);
-        SolverCG<LinearAlgebra::distributed::Vector<double>> a_solver_cg(solver_control);
+// **************** A inv MF GMG **********************
 
-        ChangeVectorTypes::copy(temp_src, src);
-        try{
-            a_solver_cg.solve(a_mat_wrapped,temp_dst,temp_src, mf_gmg_preconditioner);
-        } catch (std::exception &exc)
-        {
-            std::cout << "failed with a reduction of: " << solver_control.initial_value()/solver_control.last_value() << std::endl;
-        }
+///Initializes an object that performs the A inverse operation using matrix free GMG
+template<int dim>
+AInvMatMFGMG<dim>::AInvMatMFGMG(MF_Elasticity_Operator<dim,1,double> &mf_elasticity_operator_in , PreconditionMG<dim, LinearAlgebra::distributed::Vector<double>, MGTransferMatrixFree<dim, double> > &mf_gmg_preconditioner_in, LA::MPI::SparseMatrix &a_mat, std::map<types::global_dof_index,types::global_dof_index> &displacement_to_system_dof_index_map_in)
+    : mf_elasticity_operator(mf_elasticity_operator_in),
+      mf_gmg_preconditioner(mf_gmg_preconditioner_in),
+      a_mat_wrapped(a_mat),
+      displacement_to_system_dof_index_map(displacement_to_system_dof_index_map_in)
 
-        ChangeVectorTypes::copy(dst,temp_dst);
+{
+    mf_elasticity_operator.initialize_dof_vector(temp_dst);
+    mf_elasticity_operator.initialize_dof_vector(temp_src);
+}
 
-    }
+///Performs the A inverse operation
+template<int dim>
+void AInvMatMFGMG<dim>::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+{
 
-    ///Performs the A inverse operation - note A is symmetric
-    template<int dim>
-    void AInvMatMFGMG<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
-    {
-        SolverControl            solver_control(50, src.l2_norm()*1e-6);
-        SolverCG<LinearAlgebra::distributed::Vector<double>> a_solver_cg(solver_control);
+    SolverControl            solver_control(50, std::max(src.l2_norm()*1e-20,1e-20));
+    SolverCG<LinearAlgebra::distributed::Vector<double>> a_solver_cg(solver_control);
 
-        ChangeVectorTypes::copy(temp_src, src);
-        temp_dst = 0.;
-        a_solver_cg.solve(a_mat_wrapped,temp_dst,temp_src, mf_gmg_preconditioner);
-        ChangeVectorTypes::copy(dst,temp_dst);
+    ChangeVectorTypes::copy_from_system_to_displacement_vector<double>(temp_src, src, displacement_to_system_dof_index_map);
+    try{
+        a_solver_cg.solve(mf_elasticity_operator,temp_dst,temp_src, mf_gmg_preconditioner);
     }
-\
-    ///Change the solver tolerance if needed
-    template<int dim>
-    void AInvMatMFGMG<dim>::set_tol(double tol_in)
+    catch (std::exception &exc)
     {
-        tolerance = tol_in;
+        std::cout << "failed with a reduction of: " << solver_control.initial_value()/solver_control.last_value() << std::endl;
     }
 
-    ///Change the maximum number of iterations if needed.
-    template<int dim>
-    void AInvMatMFGMG<dim>::set_iter(unsigned int iterations_in)
-    {
-        iterations = iterations_in;
-    }
+    ChangeVectorTypes::copy_from_displacement_to_system_vector<double>(dst,temp_dst, displacement_to_system_dof_index_map);
 
+}
 
-    // ******************     GMATRIX     ***********************
+///Performs the A inverse operation - note A is symmetric
+template<int dim>
+void AInvMatMFGMG<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+{
+    SolverControl            solver_control(50, std::max(src.l2_norm()*1e-20,1e-20));
+    SolverCG<LinearAlgebra::distributed::Vector<double>> a_solver_cg(solver_control);
+
+    ChangeVectorTypes::copy_from_system_to_displacement_vector<double>(temp_src, src, displacement_to_system_dof_index_map);
+    temp_dst = 0.;
+    a_solver_cg.solve(mf_elasticity_operator,temp_dst,temp_src, mf_gmg_preconditioner);
+    ChangeVectorTypes::copy_from_displacement_to_system_vector<double>(dst,temp_dst, displacement_to_system_dof_index_map);
+}
+\
+///Change the solver tolerance if needed
+template<int dim>
+void AInvMatMFGMG<dim>::set_tol(double tol_in)
+{
+    tolerance = tol_in;
+}
 
-    GMatrix::GMatrix(const LA::MPI::SparseMatrix &f_mat_in, LA::MPI::SparseMatrix &d_8_mat_in)
-            :
-            f_mat(f_mat_in),
-            d_8_mat(d_8_mat_in)
-    {
+///Change the maximum number of iterations if needed.
+template<int dim>
+void AInvMatMFGMG<dim>::set_iter(unsigned int iterations_in)
+{
+    iterations = iterations_in;
+}
 
-    }
 
-    void
-    GMatrix::initialize(LA::MPI::Vector &exemplar_density_vector)
-    {
-        temp_vect_1 = exemplar_density_vector;
-        temp_vect_2 = exemplar_density_vector;
-    }
+// ******************     GMATRIX     ***********************
 
+GMatrix::GMatrix(const LA::MPI::SparseMatrix &f_mat_in, LA::MPI::SparseMatrix &d_8_mat_in)
+    :
+      f_mat(f_mat_in),
+      d_8_mat(d_8_mat_in)
+{
 
-    void GMatrix::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
-    {
-        f_mat.Tvmult(temp_vect_1,src);
-        d_8_mat.vmult(temp_vect_2, temp_vect_1);
-        f_mat.vmult(dst,temp_vect_2);
-    }
+}
 
+void
+GMatrix::initialize(LA::MPI::Vector &exemplar_density_vector)
+{
+    temp_vect_1 = exemplar_density_vector;
+    temp_vect_2 = exemplar_density_vector;
+}
 
 
-    void GMatrix::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
-    {
-        f_mat.Tvmult(temp_vect_1,src);
-        d_8_mat.vmult(temp_vect_2, temp_vect_1);
-        f_mat.vmult(dst,temp_vect_2);
-    }
+void GMatrix::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+{
+    f_mat.Tvmult(temp_vect_1,src);
+    d_8_mat.vmult(temp_vect_2, temp_vect_1);
+    f_mat.vmult(dst,temp_vect_2);
+}
 
 
-    // ******************     HMatrix     ***********************
 
-    template<int dim>
-    HMatrix<dim>::HMatrix(LA::MPI::SparseMatrix &a_mat_in, const LA::MPI::SparseMatrix &b_mat_in, const LA::MPI::SparseMatrix &c_mat_in, const LA::MPI::SparseMatrix &e_mat_in,TrilinosWrappers::PreconditionAMG &pre_amg_in,VmultTrilinosSolverDirect &a_inv_direct_in, AInvMatMFGMG<dim> &a_inv_mf_gmg_in)
-            :
-            a_mat(a_mat_in),
-            b_mat(b_mat_in),
-            c_mat(c_mat_in),
-            e_mat(e_mat_in),
-            pre_amg(pre_amg_in),
-            a_inv_direct(a_inv_direct_in),
-            a_inv_mf_gmg(a_inv_mf_gmg_in)
-    {
+void GMatrix::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+{
+    f_mat.Tvmult(temp_vect_1,src);
+    d_8_mat.vmult(temp_vect_2, temp_vect_1);
+    f_mat.vmult(dst,temp_vect_2);
+}
 
-    }
 
-    template<int dim>
-    void
-    HMatrix<dim>::initialize(LA::MPI::Vector &exemplar_density_vector,  LA::MPI::Vector &exemplar_displacement_vector)
-    {
-        temp_vect_1 = exemplar_displacement_vector;
-        temp_vect_2 = exemplar_displacement_vector;
-        temp_vect_3 = exemplar_displacement_vector;
-        temp_vect_4 = exemplar_displacement_vector;
-        temp_vect_5 = exemplar_density_vector;
-        temp_vect_6 = exemplar_density_vector;
-        temp_vect_7 = exemplar_density_vector;
+// ******************     HMatrix     ***********************
 
+template<int dim>
+HMatrix<dim>::HMatrix(LA::MPI::SparseMatrix &a_mat_in, const LA::MPI::SparseMatrix &b_mat_in, const LA::MPI::SparseMatrix &c_mat_in, const LA::MPI::SparseMatrix &e_mat_in,TrilinosWrappers::PreconditionAMG &pre_amg_in,VmultTrilinosSolverDirect &a_inv_direct_in, AInvMatMFGMG<dim> &a_inv_mf_gmg_in)
+    :
+      a_mat(a_mat_in),
+      b_mat(b_mat_in),
+      c_mat(c_mat_in),
+      e_mat(e_mat_in),
+      pre_amg(pre_amg_in),
+      a_inv_direct(a_inv_direct_in),
+      a_inv_mf_gmg(a_inv_mf_gmg_in)
+{
 
-    }
+}
 
-    template<int dim>
-    void HMatrix<dim>::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
-    {
-        if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
-        {
-            c_mat.vmult(temp_vect_1,src);
-            e_mat.vmult(temp_vect_2,src);
+template<int dim>
+void
+HMatrix<dim>::initialize(LA::MPI::Vector &exemplar_density_vector,  LA::MPI::Vector &exemplar_displacement_vector)
+{
+    temp_vect_1 = exemplar_displacement_vector;
+    temp_vect_2 = exemplar_displacement_vector;
+    temp_vect_3 = exemplar_displacement_vector;
+    temp_vect_4 = exemplar_displacement_vector;
+    temp_vect_5 = exemplar_density_vector;
+    temp_vect_6 = exemplar_density_vector;
+    temp_vect_7 = exemplar_density_vector;
 
-            SolverControl            solver_control_1(Input::a_inv_iterations, temp_vect_1.l2_norm()*1e-6);
-            SolverCG<LA::MPI::Vector> a_solver_cg_1(solver_control_1);
 
-            try
-            {
-//                a_solver_cg_1.solve(a_mat,temp_vect_3,temp_vect_1,pre_amg);
-                a_inv_mf_gmg.vmult(temp_vect_3,temp_vect_1);
-            } catch (std::exception &exc)
-            {
+}
 
-            }
-            SolverControl            solver_control_2(Input::a_inv_iterations, temp_vect_2.l2_norm()*1e-6);
-            SolverCG<LA::MPI::Vector> a_solver_cg_2(solver_control_2);
-            try
-            {
-//                a_solver_cg_2.solve(a_mat,temp_vect_4,temp_vect_2,pre_amg);
-                a_inv_mf_gmg.vmult(temp_vect_4,temp_vect_2);
-            } catch (std::exception &exc)
-            {
+template<int dim>
+void HMatrix<dim>::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+{
+    if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
+    {
+        c_mat.vmult(temp_vect_1,src);
+        e_mat.vmult(temp_vect_2,src);
 
-            }
+        SolverControl            solver_control_1(Input::a_inv_iterations, temp_vect_1.l2_norm()*1e-6);
+        SolverCG<LA::MPI::Vector> a_solver_cg_1(solver_control_1);
 
-            c_mat.Tvmult(temp_vect_6,temp_vect_4);
-            e_mat.Tvmult(temp_vect_5,temp_vect_3);
+        try
+        {
+            //                a_solver_cg_1.solve(a_mat,temp_vect_3,temp_vect_1,pre_amg);
+            a_inv_mf_gmg.vmult(temp_vect_3,temp_vect_1);
+        } catch (std::exception &exc)
+        {
 
-            b_mat.vmult(temp_vect_7,src);
-            dst =  temp_vect_7 - temp_vect_6 - temp_vect_5;
         }
-        else
+        SolverControl            solver_control_2(Input::a_inv_iterations, temp_vect_2.l2_norm()*1e-6);
+        SolverCG<LA::MPI::Vector> a_solver_cg_2(solver_control_2);
+        try
+        {
+            //                a_solver_cg_2.solve(a_mat,temp_vect_4,temp_vect_2,pre_amg);
+            a_inv_mf_gmg.vmult(temp_vect_4,temp_vect_2);
+        } catch (std::exception &exc)
         {
-            c_mat.vmult(temp_vect_1,src);
-            e_mat.vmult(temp_vect_2,src);
 
-            a_inv_direct.vmult(temp_vect_3,temp_vect_1);
-            a_inv_direct.vmult(temp_vect_4,temp_vect_2);
+        }
 
-            c_mat.Tvmult(temp_vect_6,temp_vect_4);
-            e_mat.Tvmult(temp_vect_5,temp_vect_3);
+        c_mat.Tvmult(temp_vect_6,temp_vect_4);
+        e_mat.Tvmult(temp_vect_5,temp_vect_3);
 
-            b_mat.vmult(temp_vect_7,src);
-            dst =  temp_vect_7 - temp_vect_6 - temp_vect_5;
-        }
+        b_mat.vmult(temp_vect_7,src);
+        dst =  temp_vect_7 - temp_vect_6 - temp_vect_5;
+    }
+    else
+    {
+        c_mat.vmult(temp_vect_1,src);
+        e_mat.vmult(temp_vect_2,src);
 
+        a_inv_direct.vmult(temp_vect_3,temp_vect_1);
+        a_inv_direct.vmult(temp_vect_4,temp_vect_2);
 
+        c_mat.Tvmult(temp_vect_6,temp_vect_4);
+        e_mat.Tvmult(temp_vect_5,temp_vect_3);
 
+        b_mat.vmult(temp_vect_7,src);
+        dst =  temp_vect_7 - temp_vect_6 - temp_vect_5;
     }
 
 
-    template<int dim>
-    void HMatrix<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
-    {
-        if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
-        {
-            c_mat.vmult(temp_vect_1,src);
-            e_mat.vmult(temp_vect_2,src);
 
-            SolverControl            solver_control_1(Input::a_inv_iterations, temp_vect_1.l2_norm()*1e-6);
-            SolverCG<LA::MPI::Vector> a_solver_cg_1(solver_control_1);
+}
 
-            try
-            {
-//                a_solver_cg_1.solve(a_mat,temp_vect_3,temp_vect_1,pre_amg);
-                a_inv_mf_gmg.vmult(temp_vect_3,temp_vect_1);
 
-            } catch (std::exception &exc)
-            {
+template<int dim>
+void HMatrix<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+{
+    if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
+    {
+        c_mat.vmult(temp_vect_1,src);
+        e_mat.vmult(temp_vect_2,src);
 
-            }
-            SolverControl            solver_control_2(Input::a_inv_iterations, temp_vect_2.l2_norm()*1e-6);
-            SolverCG<LA::MPI::Vector> a_solver_cg_2(solver_control_2);
-            try
-            {
-//                a_solver_cg_2.solve(a_mat,temp_vect_4,temp_vect_2,pre_amg);
-                a_inv_mf_gmg.vmult(temp_vect_4,temp_vect_2);
+        SolverControl            solver_control_1(Input::a_inv_iterations, temp_vect_1.l2_norm()*1e-6);
+        SolverCG<LA::MPI::Vector> a_solver_cg_1(solver_control_1);
 
-            } catch (std::exception &exc)
-            {
-
-            }
+        try
+        {
+            //                a_solver_cg_1.solve(a_mat,temp_vect_3,temp_vect_1,pre_amg);
+            a_inv_mf_gmg.vmult(temp_vect_3,temp_vect_1);
 
-            c_mat.Tvmult(temp_vect_6,temp_vect_4);
-            e_mat.Tvmult(temp_vect_5,temp_vect_3);
+        } catch (std::exception &exc)
+        {
 
-            b_mat.vmult(temp_vect_7,src);
-            dst =  temp_vect_7 - temp_vect_6 - temp_vect_5;
         }
-        else
+        SolverControl            solver_control_2(Input::a_inv_iterations, temp_vect_2.l2_norm()*1e-6);
+        SolverCG<LA::MPI::Vector> a_solver_cg_2(solver_control_2);
+        try
         {
-            c_mat.vmult(temp_vect_1,src);
-            e_mat.vmult(temp_vect_2,src);
+            //                a_solver_cg_2.solve(a_mat,temp_vect_4,temp_vect_2,pre_amg);
+            a_inv_mf_gmg.vmult(temp_vect_4,temp_vect_2);
 
-            a_inv_direct.vmult(temp_vect_3,temp_vect_1);
-            a_inv_direct.vmult(temp_vect_4,temp_vect_2);
-
-            c_mat.Tvmult(temp_vect_6,temp_vect_4);
-            e_mat.Tvmult(temp_vect_5,temp_vect_3);
+        } catch (std::exception &exc)
+        {
 
-            b_mat.vmult(temp_vect_7,src);
-            dst =  temp_vect_7 - temp_vect_6 - temp_vect_5;
         }
 
-    }
+        c_mat.Tvmult(temp_vect_6,temp_vect_4);
+        e_mat.Tvmult(temp_vect_5,temp_vect_3);
 
-    // ******************     JinvMatrix     ***********************
-    template<int dim>
-    JinvMatrix<dim>::JinvMatrix(HMatrix<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in)
-            :
-            h_mat(h_mat_in),
-            g_mat(g_mat_in),
-            d_m_mat(d_m_mat_in),
-            d_m_inv_mat(d_m_inv_mat_in)
+        b_mat.vmult(temp_vect_7,src);
+        dst =  temp_vect_7 - temp_vect_6 - temp_vect_5;
+    }
+    else
     {
+        c_mat.vmult(temp_vect_1,src);
+        e_mat.vmult(temp_vect_2,src);
 
-    }
+        a_inv_direct.vmult(temp_vect_3,temp_vect_1);
+        a_inv_direct.vmult(temp_vect_4,temp_vect_2);
 
-    template<int dim>
-    void
-    JinvMatrix<dim>::initialize(LA::MPI::Vector &exemplar_density_vector)
-    {
-        temp_vect_1 = exemplar_density_vector;
-        temp_vect_2 = exemplar_density_vector;
-        temp_vect_3 = exemplar_density_vector;
-        temp_vect_4 = exemplar_density_vector;
+        c_mat.Tvmult(temp_vect_6,temp_vect_4);
+        e_mat.Tvmult(temp_vect_5,temp_vect_3);
 
+        b_mat.vmult(temp_vect_7,src);
+        dst =  temp_vect_7 - temp_vect_6 - temp_vect_5;
     }
 
-    template<int dim>
-    void JinvMatrix<dim>::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
-    {
-        g_mat.vmult(temp_vect_1,src);
-        d_m_inv_mat.vmult(temp_vect_2,temp_vect_1);
-        h_mat.vmult(temp_vect_3,temp_vect_2);
-        d_m_mat.vmult(temp_vect_4,src);
+}
 
-        dst = -1*temp_vect_4 - temp_vect_3;
-    }
+// ******************     JinvMatrix     ***********************
+template<int dim>
+JinvMatrix<dim>::JinvMatrix(HMatrix<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in)
+    :
+      h_mat(h_mat_in),
+      g_mat(g_mat_in),
+      d_m_mat(d_m_mat_in),
+      d_m_inv_mat(d_m_inv_mat_in)
+{
 
-    template<int dim>
-    void JinvMatrix<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
-    {
-        h_mat.vmult(temp_vect_1,src);
-        d_m_inv_mat.vmult(temp_vect_2,temp_vect_1);
-        g_mat.vmult(temp_vect_3,temp_vect_2);
-        d_m_mat.vmult(temp_vect_4,src);
+}
 
-        dst = -1*temp_vect_4 - temp_vect_3;
-    }
+template<int dim>
+void
+JinvMatrix<dim>::initialize(LA::MPI::Vector &exemplar_density_vector)
+{
+    temp_vect_1 = exemplar_density_vector;
+    temp_vect_2 = exemplar_density_vector;
+    temp_vect_3 = exemplar_density_vector;
+    temp_vect_4 = exemplar_density_vector;
 
-    // ******************     KinvMatrix     ***********************
-    template<int dim>
-    KinvMatrix<dim>::KinvMatrix(HMatrix<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in)
-            :
-            h_mat(h_mat_in),
-            g_mat(g_mat_in),
-            d_m_mat(d_m_mat_in),
-            d_m_inv_mat(d_m_inv_mat_in)
-    {
+}
 
-    }
+template<int dim>
+void JinvMatrix<dim>::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+{
+    g_mat.vmult(temp_vect_1,src);
+    d_m_inv_mat.vmult(temp_vect_2,temp_vect_1);
+    h_mat.vmult(temp_vect_3,temp_vect_2);
+    d_m_mat.vmult(temp_vect_4,src);
 
-    template<int dim>
-    void
-    KinvMatrix<dim>::initialize(LA::MPI::Vector &exemplar_density_vector)
-    {
-        temp_vect_1 = exemplar_density_vector;
-        temp_vect_2 = exemplar_density_vector;
-        temp_vect_3 = exemplar_density_vector;
-        temp_vect_4 = exemplar_density_vector;
+    dst = -1*temp_vect_4 - temp_vect_3;
+}
 
-    }
+template<int dim>
+void JinvMatrix<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+{
+    h_mat.vmult(temp_vect_1,src);
+    d_m_inv_mat.vmult(temp_vect_2,temp_vect_1);
+    g_mat.vmult(temp_vect_3,temp_vect_2);
+    d_m_mat.vmult(temp_vect_4,src);
 
-    template<int dim>
-    void KinvMatrix<dim>::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
-    {
-        h_mat.vmult(temp_vect_1,src);
-        d_m_inv_mat.vmult(temp_vect_2,temp_vect_1);
-        g_mat.vmult(temp_vect_3,temp_vect_2);
-        d_m_mat.vmult(temp_vect_4,src);
+    dst = -1*temp_vect_4 - temp_vect_3;
+}
 
-        dst = -1*temp_vect_4 - temp_vect_3;
-    }
+// ******************     KinvMatrix     ***********************
+template<int dim>
+KinvMatrix<dim>::KinvMatrix(HMatrix<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in)
+    :
+      h_mat(h_mat_in),
+      g_mat(g_mat_in),
+      d_m_mat(d_m_mat_in),
+      d_m_inv_mat(d_m_inv_mat_in)
+{
 
-    template<int dim>
-    void KinvMatrix<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
-    {
-        g_mat.vmult(temp_vect_1,src);
-        d_m_inv_mat.vmult(temp_vect_2,temp_vect_1);
-        h_mat.vmult(temp_vect_3,temp_vect_2);
-        d_m_mat.vmult(temp_vect_4,src);
+}
 
-        dst = -1*temp_vect_4 - temp_vect_3;
-    }
+template<int dim>
+void
+KinvMatrix<dim>::initialize(LA::MPI::Vector &exemplar_density_vector)
+{
+    temp_vect_1 = exemplar_density_vector;
+    temp_vect_2 = exemplar_density_vector;
+    temp_vect_3 = exemplar_density_vector;
+    temp_vect_4 = exemplar_density_vector;
 
+}
 
-    AMatWrapped::AMatWrapped(LA::MPI::SparseMatrix &a_mat_in)
-            :
-            a_mat(a_mat_in)
-    {
-    }
-    void AMatWrapped::vmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
-    {
-        ChangeVectorTypes::copy(temp_src, src);
-        a_mat.vmult(temp_dst,temp_src);
-        ChangeVectorTypes::copy(dst, temp_dst);
-    }
-    void AMatWrapped::Tvmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
-    {
-        ChangeVectorTypes::copy(temp_src, src);
-        a_mat.Tvmult(temp_dst,temp_src);
-        ChangeVectorTypes::copy(dst, temp_dst);
-    }
+template<int dim>
+void KinvMatrix<dim>::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+{
+    h_mat.vmult(temp_vect_1,src);
+    d_m_inv_mat.vmult(temp_vect_2,temp_vect_1);
+    g_mat.vmult(temp_vect_3,temp_vect_2);
+    d_m_mat.vmult(temp_vect_4,src);
+
+    dst = -1*temp_vect_4 - temp_vect_3;
+}
+
+template<int dim>
+void KinvMatrix<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+{
+    g_mat.vmult(temp_vect_1,src);
+    d_m_inv_mat.vmult(temp_vect_2,temp_vect_1);
+    h_mat.vmult(temp_vect_3,temp_vect_2);
+    d_m_mat.vmult(temp_vect_4,src);
+
+    dst = -1*temp_vect_4 - temp_vect_3;
+}
+
+
+AMatWrapped::AMatWrapped(LA::MPI::SparseMatrix &a_mat_in)
+    :
+      a_mat(a_mat_in)
+{
+}
+void AMatWrapped::vmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
+{
+    ChangeVectorTypes::copy(temp_src, src);
+    a_mat.vmult(temp_dst,temp_src);
+    ChangeVectorTypes::copy(dst, temp_dst);
+}
+void AMatWrapped::Tvmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const
+{
+    ChangeVectorTypes::copy(temp_src, src);
+    a_mat.Tvmult(temp_dst,temp_src);
+    ChangeVectorTypes::copy(dst, temp_dst);
+}
 }
 
 

From 2d80f8768f60b1abd5da074195399c4c1e85c45f Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Sat, 7 May 2022 11:55:21 -0600
Subject: [PATCH 87/95] refactoring

---
 include/watchdog.h                       | 63 ++++++++++++++++++++++++
 source/main.cc                           | 32 ++++++++++++
 source/{watchdog_main.cc => watchdog.cc} | 45 +++++------------
 3 files changed, 106 insertions(+), 34 deletions(-)
 create mode 100644 include/watchdog.h
 create mode 100644 source/main.cc
 rename source/{watchdog_main.cc => watchdog.cc} (93%)

diff --git a/include/watchdog.h b/include/watchdog.h
new file mode 100644
index 0000000..c086cee
--- /dev/null
+++ b/include/watchdog.h
@@ -0,0 +1,63 @@
+#include <deal.II/lac/block_vector.h>
+#include <deal.II/lac/packaged_operation.h>
+#include <deal.II/grid/tria.h>
+#include <deal.II/grid/grid_refinement.h>
+#include <iostream>
+#include "../include/markov_filter.h"
+#include "../include/kkt_system.h"
+#include "../include/input_information.h"
+#include <deal.II/lac/generic_linear_algebra.h>
+#include <deal.II/lac/trilinos_parallel_block_vector.h>
+#include <deal.II/lac/generic_linear_algebra.h>
+#include <deal.II/base/conditional_ostream.h>
+#include <deal.II/base/utilities.h>
+
+///Above are fairly normal files to include.  I also use the sparse direct package, which requiresBLAS/LAPACK
+/// to  perform  a  direct  solve  while  I  work  on  a  fast  iterative  solver  for  this problem.
+
+namespace SAND {
+    namespace LA
+    {
+        using namespace dealii::LinearAlgebraTrilinos;
+    }
+
+
+    using namespace dealii;
+
+    /// Below is the main class for solving this problem. It handles the nonlinear solver portion of the problem,
+    /// taking information from the KKTSystem class for step directions, and calculating step lengths. This class
+    /// not only takes those steps, but handles the barrier parameter for the log barrier used.
+    template<int dim>
+    class NonlinearWatchdog {
+    public:
+        NonlinearWatchdog();
+
+        void
+        run();
+
+    private:
+        MPI_Comm  mpi_communicator;
+        std::pair<double,double>
+        calculate_max_step_size(const LA::MPI::BlockVector &state, const LA::MPI::BlockVector &step) const;
+
+        const LA::MPI::BlockVector
+        find_max_step(const LA::MPI::BlockVector &state);
+
+        LA::MPI::BlockVector
+        take_scaled_step(const LA::MPI::BlockVector &state,const LA::MPI::BlockVector &max_step) const;
+
+        bool
+        check_convergence(const LA::MPI::BlockVector &state) const;
+
+        void
+        update_barrier(LA::MPI::BlockVector &current_state);
+
+        KktSystem<dim> kkt_system;
+        MarkovFilter markov_filter;
+        double barrier_size;
+        bool mixed_barrier_monotone_mode;
+        ConditionalOStream pcout;
+        TimerOutput overall_timer;
+    };
+
+} // namespace SAND
diff --git a/source/main.cc b/source/main.cc
new file mode 100644
index 0000000..1a55abc
--- /dev/null
+++ b/source/main.cc
@@ -0,0 +1,32 @@
+#include "../include/watchdog.h"
+
+///Above are fairly normal files to include.  I also use the sparse direct package, which requiresBLAS/LAPACK
+/// to  perform  a  direct  solve  while  I  work  on  a  fast  iterative  solver  for  this problem.
+
+int
+main(int argc, char *argv[]) {
+    try
+    {
+        Utilities::MPI::MPI_InitFinalize mpi_initialization(argc, argv, 1);
+        SAND::NonlinearWatchdog<SAND::Input::dim> elastic_problem;
+        elastic_problem.run();
+    }
+    catch (std::exception &exc) {
+        std::cerr << std::endl << std::endl
+                  << "----------------------------------------------------" << std::endl;
+        std::cerr << "Exception on processing: " << std::endl << exc.what()
+                  << std::endl << "Aborting!" << std::endl
+                  << "----------------------------------------------------" << std::endl;
+
+        return 1;
+    }
+    catch (...) {
+        std::cerr << std::endl << std::endl
+                  << "----------------------------------------------------" << std::endl;
+        std::cerr << "Unknown exception!" << std::endl << "Aborting!" << std::endl
+                  << "----------------------------------------------------" << std::endl;
+        return 1;
+    }
+
+    return 0;
+}
diff --git a/source/watchdog_main.cc b/source/watchdog.cc
similarity index 93%
rename from source/watchdog_main.cc
rename to source/watchdog.cc
index 37efc40..5fedf3b 100644
--- a/source/watchdog_main.cc
+++ b/source/watchdog.cc
@@ -28,9 +28,9 @@ namespace SAND {
     /// taking information from the KKTSystem class for step directions, and calculating step lengths. This class
     /// not only takes those steps, but handles the barrier parameter for the log barrier used.
     template<int dim>
-    class SANDTopOpt {
+    class NonlinearWatchdog {
     public:
-        SANDTopOpt();
+        NonlinearWatchdog();
 
         void
         run();
@@ -62,7 +62,7 @@ namespace SAND {
 
     ///Constructor
     template<int dim>
-    SANDTopOpt<dim>::SANDTopOpt()
+    NonlinearWatchdog<dim>::NonlinearWatchdog()
             :
               mpi_communicator(MPI_COMM_WORLD),
               pcout(std::cout,(Utilities::MPI::this_mpi_process(mpi_communicator) == 0)),
@@ -74,7 +74,7 @@ namespace SAND {
 
     template<int dim>
     std::pair<double,double>
-    SANDTopOpt<dim>::calculate_max_step_size(const LA::MPI::BlockVector &state, const LA::MPI::BlockVector &step) const {
+    NonlinearWatchdog<dim>::calculate_max_step_size(const LA::MPI::BlockVector &state, const LA::MPI::BlockVector &step) const {
 
         double step_size_s_low = 0;
         double step_size_z_low = 0;
@@ -120,7 +120,7 @@ namespace SAND {
 
     template<int dim>
     const LA::MPI::BlockVector
-    SANDTopOpt<dim>::find_max_step(const LA::MPI::BlockVector &state)
+    NonlinearWatchdog<dim>::find_max_step(const LA::MPI::BlockVector &state)
     {
         TimerOutput::Scope t(overall_timer, "find step");
         {
@@ -160,7 +160,7 @@ namespace SAND {
 
     template<int dim>
     LA::MPI::BlockVector
-    SANDTopOpt<dim>::take_scaled_step(const LA::MPI::BlockVector &state,const LA::MPI::BlockVector &max_step) const
+    NonlinearWatchdog<dim>::take_scaled_step(const LA::MPI::BlockVector &state,const LA::MPI::BlockVector &max_step) const
     {
         double step_size = 1;
             for(unsigned int k = 0; k<10; k++)
@@ -183,7 +183,7 @@ namespace SAND {
     ///Checks to see if the KKT conditions are sufficiently met to lower barrier size.
     template<int dim>
     bool
-    SANDTopOpt<dim>::check_convergence(const LA::MPI::BlockVector &state) const
+    NonlinearWatchdog<dim>::check_convergence(const LA::MPI::BlockVector &state) const
     {
               if (kkt_system.calculate_convergence(state) < Input::required_norm)
               {
@@ -199,7 +199,7 @@ namespace SAND {
     /// the performance of the mixed method
     template<int dim>
     void
-    SANDTopOpt<dim>::update_barrier(LA::MPI::BlockVector &current_state)
+    NonlinearWatchdog<dim>::update_barrier(LA::MPI::BlockVector &current_state)
     {
         ///The LOQO scheme uses information about the similarity of the slack/slack multiplier product as a
         /// heuristic for decreasing barrier value
@@ -360,7 +360,7 @@ namespace SAND {
     ///Contains watchdog algorithm
     template<int dim>
     void
-    SANDTopOpt<dim>::run() {
+    NonlinearWatchdog<dim>::run() {
         overall_timer.enter_subsection("Total Time");
         barrier_size = Input::initial_barrier_size;
         kkt_system.create_triangulation();
@@ -485,29 +485,6 @@ namespace SAND {
 
 } // namespace SAND
 
-int
-main(int argc, char *argv[]) {
-    try {
-        Utilities::MPI::MPI_InitFinalize mpi_initialization(argc, argv, 1);
-        SAND::SANDTopOpt<SAND::Input::dim> elastic_problem_2d;
-        elastic_problem_2d.run();
-    }
-    catch (std::exception &exc) {
-        std::cerr << std::endl << std::endl
-                  << "----------------------------------------------------" << std::endl;
-        std::cerr << "Exception on processing: " << std::endl << exc.what()
-                  << std::endl << "Aborting!" << std::endl
-                  << "----------------------------------------------------" << std::endl;
-
-        return 1;
-    }
-    catch (...) {
-        std::cerr << std::endl << std::endl
-                  << "----------------------------------------------------" << std::endl;
-        std::cerr << "Unknown exception!" << std::endl << "Aborting!" << std::endl
-                  << "----------------------------------------------------" << std::endl;
-        return 1;
-    }
 
-    return 0;
-}
+template class SAND::NonlinearWatchdog<2>;
+template class SAND::NonlinearWatchdog<3>;

From ff64a4643bf39a0574ccc9e47a3d32fe6bd5bcd7 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Sat, 7 May 2022 12:00:29 -0600
Subject: [PATCH 88/95] refactoring

---
 include/watchdog.h |  3 +++
 source/watchdog.cc | 52 ++++++++++++----------------------------------
 2 files changed, 16 insertions(+), 39 deletions(-)

diff --git a/include/watchdog.h b/include/watchdog.h
index c086cee..1a76a86 100644
--- a/include/watchdog.h
+++ b/include/watchdog.h
@@ -52,6 +52,9 @@ namespace SAND {
         void
         update_barrier(LA::MPI::BlockVector &current_state);
 
+        void
+        perform_initial_setup();
+
         KktSystem<dim> kkt_system;
         MarkovFilter markov_filter;
         double barrier_size;
diff --git a/source/watchdog.cc b/source/watchdog.cc
index 5fedf3b..8a45532 100644
--- a/source/watchdog.cc
+++ b/source/watchdog.cc
@@ -6,6 +6,7 @@
 #include "../include/markov_filter.h"
 #include "../include/kkt_system.h"
 #include "../include/input_information.h"
+#include "../include/watchdog.h"
 #include <deal.II/lac/generic_linear_algebra.h>
 #include <deal.II/lac/trilinos_parallel_block_vector.h>
 #include <deal.II/lac/generic_linear_algebra.h>
@@ -24,42 +25,6 @@ namespace SAND {
 
     using namespace dealii;
 
-    /// Below is the main class for solving this problem. It handles the nonlinear solver portion of the problem,
-    /// taking information from the KKTSystem class for step directions, and calculating step lengths. This class
-    /// not only takes those steps, but handles the barrier parameter for the log barrier used.
-    template<int dim>
-    class NonlinearWatchdog {
-    public:
-        NonlinearWatchdog();
-
-        void
-        run();
-
-    private:
-        MPI_Comm  mpi_communicator;
-        std::pair<double,double>
-        calculate_max_step_size(const LA::MPI::BlockVector &state, const LA::MPI::BlockVector &step) const;
-
-        const LA::MPI::BlockVector
-        find_max_step(const LA::MPI::BlockVector &state);
-
-        LA::MPI::BlockVector
-        take_scaled_step(const LA::MPI::BlockVector &state,const LA::MPI::BlockVector &max_step) const;
-
-        bool
-        check_convergence(const LA::MPI::BlockVector &state) const;
-
-        void
-        update_barrier(LA::MPI::BlockVector &current_state);
-
-        KktSystem<dim> kkt_system;
-        MarkovFilter markov_filter;
-        double barrier_size;
-        bool mixed_barrier_monotone_mode;
-        ConditionalOStream pcout;
-        TimerOutput overall_timer;
-    };
-
     ///Constructor
     template<int dim>
     NonlinearWatchdog<dim>::NonlinearWatchdog()
@@ -357,11 +322,10 @@ namespace SAND {
 
     }
 
-    ///Contains watchdog algorithm
     template<int dim>
     void
-    NonlinearWatchdog<dim>::run() {
-        overall_timer.enter_subsection("Total Time");
+    NonlinearWatchdog<dim>::perform_initial_setup()
+    {
         barrier_size = Input::initial_barrier_size;
         kkt_system.create_triangulation();
         kkt_system.setup_boundary_values();
@@ -372,6 +336,15 @@ namespace SAND {
         {
             mixed_barrier_monotone_mode = false;
         }
+    }
+
+    ///Contains watchdog algorithm
+    template<int dim>
+    void
+    NonlinearWatchdog<dim>::run() {
+        overall_timer.enter_subsection("Total Time");
+
+        perform_initial_setup();
 
         const unsigned int max_uphill_steps = 8;
         unsigned int iteration_number = 0;
@@ -473,6 +446,7 @@ namespace SAND {
                 markov_filter.update_barrier_value(barrier_size);
                 pcout << "barrier size is now " << barrier_size << " on iteration number " << iteration_number << std::endl;
 
+
                 overall_timer.leave_subsection();
                 overall_timer.print_summary();
                 overall_timer.enter_subsection("Total Time");

From de1ac6e0590d5cdf1da547c3e5f35f965e679f96 Mon Sep 17 00:00:00 2001
From: Justin O'Connor <juleoc@colostate.edu>
Date: Sat, 7 May 2022 12:10:22 -0600
Subject: [PATCH 89/95] refactoring

---
 include/watchdog.h |   3 +
 source/watchdog.cc | 193 ++++++++++++++++++++++++---------------------
 2 files changed, 105 insertions(+), 91 deletions(-)

diff --git a/include/watchdog.h b/include/watchdog.h
index 1a76a86..3ce4dce 100644
--- a/include/watchdog.h
+++ b/include/watchdog.h
@@ -55,6 +55,9 @@ namespace SAND {
         void
         perform_initial_setup();
 
+        void
+        nonlinear_step(LA::MPI::BlockVector &current_state, LA::MPI::BlockVector &current_step, const unsigned int max_uphill_steps, unsigned int &iteration_number);
+
         KktSystem<dim> kkt_system;
         MarkovFilter markov_filter;
         double barrier_size;
diff --git a/source/watchdog.cc b/source/watchdog.cc
index 8a45532..b71f7c2 100644
--- a/source/watchdog.cc
+++ b/source/watchdog.cc
@@ -338,121 +338,132 @@ namespace SAND {
         }
     }
 
-    ///Contains watchdog algorithm
+
     template<int dim>
     void
-    NonlinearWatchdog<dim>::run() {
-        overall_timer.enter_subsection("Total Time");
-
-        perform_initial_setup();
-
-        const unsigned int max_uphill_steps = 8;
-        unsigned int iteration_number = 0;
-        //while barrier value above minimal value and total iterations under some value
-        LA::MPI::BlockVector current_state = kkt_system.get_initial_state();
-        LA::MPI::BlockVector current_step;
-        markov_filter.setup(kkt_system.calculate_objective_value(current_state), kkt_system.calculate_barrier_distance(current_state), kkt_system.calculate_feasibility(current_state,barrier_size), barrier_size);
+    NonlinearWatchdog<dim>::nonlinear_step(LA::MPI::BlockVector &current_state, LA::MPI::BlockVector &current_step, const unsigned int max_uphill_steps, unsigned int &iteration_number)
+    {
 
-        while((barrier_size > Input::min_barrier_size || !check_convergence(current_state)) && iteration_number < Input::max_steps)
+        bool converged = false;
+        //while not converged
+        while(!converged && iteration_number < Input::max_steps)
         {
-            bool converged = false;
-            //while not converged
-            while(!converged && iteration_number < Input::max_steps)
-            {
-                bool found_step = false;
-                //save current state as watchdog state
+            bool found_step = false;
+            //save current state as watchdog state
 
-                const LA::MPI::BlockVector watchdog_state = current_state;
-                LA::MPI::BlockVector watchdog_step;
-                //for 1-8 steps - this is the number of steps away we will let it go uphill before demanding downhill
-                for(unsigned int k = 0; k<max_uphill_steps; k++)
-                {
+            const LA::MPI::BlockVector watchdog_state = current_state;
+            LA::MPI::BlockVector watchdog_step;
+            //for 1-8 steps - this is the number of steps away we will let it go uphill before demanding downhill
+            for(unsigned int k = 0; k<max_uphill_steps; k++)
+            {
 
-                    //compute step from current state  - function from kktSystem
-                    current_step = find_max_step(current_state);
+                //compute step from current state  - function from kktSystem
+                current_step = find_max_step(current_state);
 
-                    // save the first of these as the watchdog step
-                    if(k==0)
+                // save the first of these as the watchdog step
+                if(k==0)
+                {
+                    watchdog_step = current_step;
+                    if (iteration_number == 0)
                     {
-                        watchdog_step = current_step;
-                        if (iteration_number == 0)
-                        {
-                            kkt_system.calculate_initial_rhs_error();
-                        }
+                        kkt_system.calculate_initial_rhs_error();
                     }
-                    //apply full step to current state
-                    current_state=current_state+current_step;
+                }
 
+                //apply full step to current state
+                current_state=current_state+current_step;
 
-                    //if new state passes filter
-                    if(markov_filter.check_filter(kkt_system.calculate_objective_value(current_state), kkt_system.calculate_barrier_distance(current_state), kkt_system.calculate_feasibility(current_state,barrier_size)))
-                    {
-                        //Accept current state
-                        //iterate number of steps by number of steps taken in this process
-                        iteration_number = iteration_number + k + 1;
-                        found_step = true;
-                        pcout << "found workable step after " << k+1 << " iterations"<<std::endl;
-                        //break for loop
-                        markov_filter.add_point(kkt_system.calculate_objective_value(current_state), kkt_system.calculate_barrier_distance(current_state), kkt_system.calculate_feasibility(current_state,barrier_size));
-                        break;
-                        //end if
-                    }
-                    //end for
+
+                //if new state passes filter
+                if(markov_filter.check_filter(kkt_system.calculate_objective_value(current_state), kkt_system.calculate_barrier_distance(current_state), kkt_system.calculate_feasibility(current_state,barrier_size)))
+                {
+                    //Accept current state
+                    //iterate number of steps by number of steps taken in this process
+                    iteration_number = iteration_number + k + 1;
+                    found_step = true;
+                    pcout << "found workable step after " << k+1 << " iterations"<<std::endl;
+                    //break for loop
+                    markov_filter.add_point(kkt_system.calculate_objective_value(current_state), kkt_system.calculate_barrier_distance(current_state), kkt_system.calculate_feasibility(current_state,barrier_size));
+                    break;
+                    //end if
+                }
+                //end for
+            }
+            //if found step = false
+            if (!found_step)
+            {
+                //Compute step from current state
+                current_step = find_max_step(current_state);
+                //find step length so that merit of stretch state - sized step from current length - is less than merit of (current state + descent requirement * linear derivative of merit of current state in direction of current step)
+                //update stretch state with found step length
+                const LA::MPI::BlockVector stretch_state = take_scaled_step(current_state, current_step);
+                //if current merit is less than watchdog merit, or if stretch merit is less than earlier goal merit
+                if(markov_filter.check_filter(kkt_system.calculate_objective_value(current_state), kkt_system.calculate_barrier_distance(current_state), kkt_system.calculate_feasibility(current_state,barrier_size)))
+                {
+                    current_state = stretch_state;
+                    iteration_number = iteration_number + max_uphill_steps + 1;
+                    markov_filter.add_point(kkt_system.calculate_objective_value(current_state), kkt_system.calculate_barrier_distance(current_state), kkt_system.calculate_feasibility(current_state,barrier_size));
                 }
-                //if found step = false
-                if (!found_step)
+                else
                 {
-                    //Compute step from current state
-                    current_step = find_max_step(current_state);
-                    //find step length so that merit of stretch state - sized step from current length - is less than merit of (current state + descent requirement * linear derivative of merit of current state in direction of current step)
-                    //update stretch state with found step length
-                    const LA::MPI::BlockVector stretch_state = take_scaled_step(current_state, current_step);
-                    //if current merit is less than watchdog merit, or if stretch merit is less than earlier goal merit
-                    if(markov_filter.check_filter(kkt_system.calculate_objective_value(current_state), kkt_system.calculate_barrier_distance(current_state), kkt_system.calculate_feasibility(current_state,barrier_size)))
+                    //if merit of stretch state is bigger than watchdog merit
+                    if (markov_filter.check_filter(kkt_system.calculate_objective_value(current_state), kkt_system.calculate_barrier_distance(current_state), kkt_system.calculate_feasibility(current_state,barrier_size)))
                     {
-                        current_state = stretch_state;
-                        iteration_number = iteration_number + max_uphill_steps + 1;
+                        //find step length from watchdog state that meets descent requirement
+                        current_state = take_scaled_step(watchdog_state, watchdog_step);
+                        //update iteration count
+                        iteration_number = iteration_number +  max_uphill_steps + 1;
                         markov_filter.add_point(kkt_system.calculate_objective_value(current_state), kkt_system.calculate_barrier_distance(current_state), kkt_system.calculate_feasibility(current_state,barrier_size));
+
                     }
                     else
                     {
-                        //if merit of stretch state is bigger than watchdog merit
-                        if (markov_filter.check_filter(kkt_system.calculate_objective_value(current_state), kkt_system.calculate_barrier_distance(current_state), kkt_system.calculate_feasibility(current_state,barrier_size)))
-                        {
-                            //find step length from watchdog state that meets descent requirement
-                            current_state = take_scaled_step(watchdog_state, watchdog_step);
-                            //update iteration count
-                            iteration_number = iteration_number +  max_uphill_steps + 1;
-                            markov_filter.add_point(kkt_system.calculate_objective_value(current_state), kkt_system.calculate_barrier_distance(current_state), kkt_system.calculate_feasibility(current_state,barrier_size));
-
-                        }
-                        else
-                        {
-                            //calculate direction from stretch state
-                            const LA::MPI::BlockVector stretch_step = find_max_step(stretch_state);
-                            //find step length from stretch state that meets descent requirement
-                            current_state = take_scaled_step(stretch_state, stretch_step);
-                            //update iteration count
-                            iteration_number = iteration_number + max_uphill_steps + 2;
-                            markov_filter.add_point(kkt_system.calculate_objective_value(current_state), kkt_system.calculate_barrier_distance(current_state), kkt_system.calculate_feasibility(current_state,barrier_size));
-                        }
+                        //calculate direction from stretch state
+                        const LA::MPI::BlockVector stretch_step = find_max_step(stretch_state);
+                        //find step length from stretch state that meets descent requirement
+                        current_state = take_scaled_step(stretch_state, stretch_step);
+                        //update iteration count
+                        iteration_number = iteration_number + max_uphill_steps + 2;
+                        markov_filter.add_point(kkt_system.calculate_objective_value(current_state), kkt_system.calculate_barrier_distance(current_state), kkt_system.calculate_feasibility(current_state,barrier_size));
                     }
                 }
-                //output current state
-                kkt_system.output(current_state,iteration_number);
+            }
+            //output current state
+            kkt_system.output(current_state,iteration_number);
 
-                converged = check_convergence(current_state);
-                update_barrier(current_state);
-                markov_filter.update_barrier_value(barrier_size);
-                pcout << "barrier size is now " << barrier_size << " on iteration number " << iteration_number << std::endl;
+            converged = check_convergence(current_state);
+            update_barrier(current_state);
+            markov_filter.update_barrier_value(barrier_size);
+            pcout << "barrier size is now " << barrier_size << " on iteration number " << iteration_number << std::endl;
 
 
-                overall_timer.leave_subsection();
-                overall_timer.print_summary();
-                overall_timer.enter_subsection("Total Time");
-                //end while
-            }
+            overall_timer.leave_subsection();
+            overall_timer.print_summary();
+            overall_timer.enter_subsection("Total Time");
+
+        }//end while
+    }
+
+    ///Contains watchdog algorithm
+    template<int dim>
+    void
+    NonlinearWatchdog<dim>::run() {
+        overall_timer.enter_subsection("Total Time");
+
+        perform_initial_setup();
+
+        const unsigned int max_uphill_steps = 8;
+        unsigned int iteration_number = 0;
+
+        //while barrier value above minimal value and total iterations under some value
+        LA::MPI::BlockVector current_state = kkt_system.get_initial_state();
+        LA::MPI::BlockVector current_step;
 
+        markov_filter.setup(kkt_system.calculate_objective_value(current_state), kkt_system.calculate_barrier_distance(current_state), kkt_system.calculate_feasibility(current_state,barrier_size), barrier_size);
+
+        while((barrier_size > Input::min_barrier_size || !check_convergence(current_state)) && iteration_number < Input::max_steps)
+        {
+            nonlinear_step(current_state, current_step, max_uphill_steps, iteration_number);
         }
 //        kkt_system.output_stl(current_state);
     }

From e80537abfa10176cd1a211f000fa980dfc82a37b Mon Sep 17 00:00:00 2001
From: Justin O'Connor <oconnor@up.math.colostate.edu>
Date: Tue, 10 May 2022 21:20:44 -0600
Subject: [PATCH 90/95] trying to print

---
 include/input_information.h    |   4 +-
 include/schur_preconditioner.h |  53 +++++++-
 source/schur_preconditioner.cc | 222 +++++++++++++++++++++++++++++++++
 3 files changed, 274 insertions(+), 5 deletions(-)

diff --git a/include/input_information.h b/include/input_information.h
index 4568e01..8b4442d 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -18,7 +18,7 @@ namespace SAND {
         constexpr unsigned int geometry_base = GeometryOptions::mbb;
 
         constexpr unsigned int dim = 2;
-        constexpr unsigned int refinements = 4;
+        constexpr unsigned int refinements = 3;
 
         //nonlinear algorithm options
         constexpr double initial_barrier_size = 25;
@@ -47,7 +47,7 @@ namespace SAND {
         constexpr double default_gmres_tolerance = 1e-9;
 
         constexpr unsigned int a_inv_iterations = 25;
-        constexpr unsigned int k_inv_iterations = 1000;
+        constexpr unsigned int k_inv_iterations = 100;
 
         //Material Options
         constexpr double material_lambda = 1;
diff --git a/include/schur_preconditioner.h b/include/schur_preconditioner.h
index c61b67a..3634940 100644
--- a/include/schur_preconditioner.h
+++ b/include/schur_preconditioner.h
@@ -193,6 +193,33 @@ namespace SAND
 
     };
 
+    template<int dim>
+    class HMatrixDirect : public TrilinosWrappers::SparseMatrix {
+        public:
+            HMatrixDirect(LA::MPI::SparseMatrix &a_mat_in, const LA::MPI::SparseMatrix &b_mat_in, const LA::MPI::SparseMatrix &c_mat_in, const LA::MPI::SparseMatrix &e_mat_in,TrilinosWrappers::PreconditionAMG &pre_amg_in, VmultTrilinosSolverDirect &a_inv_direct_in, AInvMatMFGMG<dim> &a_inv_mf_gmg_in);
+            void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
+            void Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
+            void initialize(LA::MPI::Vector &exemplar_density_vector,  LA::MPI::Vector &exemplar_displacement_vector);
+            unsigned int m() const;
+            unsigned int n() const;
+        private:
+            LA::MPI::SparseMatrix &a_mat;
+            const LA::MPI::SparseMatrix &b_mat;
+            const LA::MPI::SparseMatrix &c_mat;
+            const LA::MPI::SparseMatrix &e_mat;
+            TrilinosWrappers::PreconditionAMG &pre_amg;
+            VmultTrilinosSolverDirect &a_inv_direct;
+            mutable LA::MPI::Vector temp_vect_1;
+            mutable LA::MPI::Vector temp_vect_2;
+            mutable LA::MPI::Vector temp_vect_3;
+            mutable LA::MPI::Vector temp_vect_4;
+            mutable LA::MPI::Vector temp_vect_5;
+            mutable LA::MPI::Vector temp_vect_6;
+            mutable LA::MPI::Vector temp_vect_7;
+            AInvMatMFGMG<dim> &a_inv_mf_gmg;
+
+    };
+
     template<int dim>
     class KinvMatrix : public TrilinosWrappers::SparseMatrix {
         public:
@@ -213,6 +240,26 @@ namespace SAND
             mutable LA::MPI::Vector temp_vect_4;
     };
 
+    template<int dim>
+    class KinvMatrixDirect : public TrilinosWrappers::SparseMatrix {
+        public:
+            KinvMatrixDirect(HMatrixDirect<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in);
+            void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
+            void Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
+            void initialize(LA::MPI::Vector &exemplar_density_vector);
+            unsigned int m() const;
+            unsigned int n() const;
+        private:
+            HMatrixDirect<dim> &h_mat;
+            GMatrix &g_mat;
+            const LA::MPI::SparseMatrix &d_m_mat;
+            LA::MPI::SparseMatrix &d_m_inv_mat;
+            mutable LA::MPI::Vector temp_vect_1;
+            mutable LA::MPI::Vector temp_vect_2;
+            mutable LA::MPI::Vector temp_vect_3;
+            mutable LA::MPI::Vector temp_vect_4;
+    };
+
     template<int dim>
     class JinvMatrix : public TrilinosWrappers::SparseMatrix {
         public:
@@ -291,7 +338,7 @@ namespace SAND
         LA::MPI::SparseMatrix d_6_mat;
         LA::MPI::SparseMatrix d_7_mat;
         LA::MPI::SparseMatrix d_8_mat;
-        LA::MPI::SparseMatrix d_m_inv_mat;
+        mutable LA::MPI::SparseMatrix d_m_inv_mat;
 
         mutable LA::MPI::Vector pre_j;
         mutable LA::MPI::Vector pre_k;
@@ -303,13 +350,13 @@ namespace SAND
         SolverControl direct_solver_control;
         mutable VmultTrilinosSolverDirect a_inv_direct;
 
-        AInvMatMFGMG<dim> a_inv_mf_gmg;
+        mutable AInvMatMFGMG<dim> a_inv_mf_gmg;
         ConditionalOStream pcout;
         mutable TimerOutput timer;
 
         mutable TrilinosWrappers::PreconditionAMG pre_amg;
 
-        GMatrix g_mat;
+        mutable GMatrix g_mat;
         HMatrix<dim> h_mat;
 
         JinvMatrix<dim> j_inv_mat;
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index ff90a76..7670449 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -8,6 +8,7 @@
 #include <deal.II/lac/generic_linear_algebra.h>
 #include <deal.II/lac/trilinos_solver.h>
 #include <deal.II/lac/trilinos_precondition.h>
+#include <deal.II/base/thread_management.h>
 #include "../include/schur_preconditioner.h"
 #include "../include/input_information.h"
 #include "../include/sand_tools.h"
@@ -344,6 +345,105 @@ void TopOptSchurPreconditioner<dim>::vmult(LA::MPI::BlockVector &dst, const LA::
     }
     vmult_step_5(dst, temp_src);
     num_mults++;
+
+
+
+
+
+
+
+
+    if (num_mults > 10)
+    {
+        FullMatrix<double> X_matrix;
+        FullMatrix<double> X_matrix_direct;
+        LA::MPI::Vector exemplar_vector;
+        const types::global_dof_index n_p = system_matrix.block(SolutionBlocks::density,
+                                                                SolutionBlocks::density).m();
+
+        X_matrix.reinit(n_p,n_p);
+        X_matrix_direct.reinit(n_p,n_p);
+        exemplar_vector.reinit(src.block(SolutionBlocks::density));
+
+        HMatrix<dim> h_mat_test (a_mat, b_mat, c_mat, e_mat, pre_amg, a_inv_direct, a_inv_mf_gmg);
+        HMatrixDirect<dim> h_mat_test_direct (a_mat, b_mat, c_mat, e_mat, pre_amg, a_inv_direct, a_inv_mf_gmg);
+
+        KinvMatrix<dim>         k_inv_test          (h_mat_test, g_mat, d_m_mat, d_m_inv_mat);
+
+        KinvMatrixDirect<dim>   k_inv_test_direct   (h_mat_test_direct, g_mat, d_m_mat, d_m_inv_mat);
+
+
+
+        Threads::TaskGroup<void> tasks;
+        for (unsigned int j=0; j<X_matrix.n(); ++j)
+            tasks += Threads::new_task ([&X_matrix, &exemplar_vector, &k_inv_test, j]()
+                                        {
+                                            LA::MPI::Vector e_j (exemplar_vector);
+                                            LA::MPI::Vector r_j (exemplar_vector);
+
+                                            e_j = 0;
+                                            e_j(j) = 1;
+                                            k_inv_test.vmult(r_j,e_j);
+
+                                            for (unsigned int i=0; i<X_matrix.m(); ++i)
+                                                X_matrix(i,j) = r_j(i);
+                                        });
+
+        tasks.join_all();
+
+        for (unsigned int j=0; j<X_matrix.n(); ++j)
+            tasks += Threads::new_task ([&X_matrix_direct, &exemplar_vector, &k_inv_test_direct, j]()
+                                        {
+                                            LA::MPI::Vector e_j (exemplar_vector);
+                                            LA::MPI::Vector r_j (exemplar_vector);
+
+                                            e_j = 0;
+                                            e_j(j) = 1;
+                                            k_inv_test_direct.vmult(r_j,e_j);
+
+                                            for (unsigned int i=0; i<X_matrix_direct.m(); ++i)
+                                                X_matrix_direct(i,j) = r_j(i);
+                                        });
+
+        tasks.join_all();
+
+
+
+
+
+
+        const unsigned int n = X_matrix.n();
+        const unsigned int m = X_matrix.m();
+
+
+        std::ofstream Xmat("Kmatrix_MFGMG");
+        for (unsigned int i = 0; i < m; i++)
+        {
+            Xmat << X_matrix(i, 0);
+            for (unsigned int j = 1; j < n; j++)
+            {
+                Xmat << "," << X_matrix(i, j);
+            }
+            Xmat << "\n";
+        }
+        Xmat.close();
+
+        std::ofstream Xmat_direct("Kmatrix_MFGMG_Direct");
+        for (unsigned int i = 0; i < m; i++)
+        {
+            Xmat_direct << X_matrix_direct(i, 0);
+            for (unsigned int j = 1; j < n; j++)
+            {
+                Xmat_direct << "," << X_matrix_direct(i, j);
+            }
+            Xmat_direct << "\n";
+        }
+        Xmat_direct.close();
+
+        std::abort();
+    }
+
+
 }
 
 ///Not implemented
@@ -926,6 +1026,78 @@ void HMatrix<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) cons
 
 }
 
+
+// ******************     HMatrixDirect     ***********************
+
+template<int dim>
+HMatrixDirect<dim>::HMatrixDirect(LA::MPI::SparseMatrix &a_mat_in, const LA::MPI::SparseMatrix &b_mat_in, const LA::MPI::SparseMatrix &c_mat_in, const LA::MPI::SparseMatrix &e_mat_in,TrilinosWrappers::PreconditionAMG &pre_amg_in,VmultTrilinosSolverDirect &a_inv_direct_in, AInvMatMFGMG<dim> &a_inv_mf_gmg_in)
+    :
+      a_mat(a_mat_in),
+      b_mat(b_mat_in),
+      c_mat(c_mat_in),
+      e_mat(e_mat_in),
+      pre_amg(pre_amg_in),
+      a_inv_direct(a_inv_direct_in),
+      a_inv_mf_gmg(a_inv_mf_gmg_in)
+{
+
+}
+
+template<int dim>
+void
+HMatrixDirect<dim>::initialize(LA::MPI::Vector &exemplar_density_vector,  LA::MPI::Vector &exemplar_displacement_vector)
+{
+    temp_vect_1 = exemplar_displacement_vector;
+    temp_vect_2 = exemplar_displacement_vector;
+    temp_vect_3 = exemplar_displacement_vector;
+    temp_vect_4 = exemplar_displacement_vector;
+    temp_vect_5 = exemplar_density_vector;
+    temp_vect_6 = exemplar_density_vector;
+    temp_vect_7 = exemplar_density_vector;
+
+
+}
+
+template<int dim>
+void HMatrixDirect<dim>::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+{
+
+        c_mat.vmult(temp_vect_1,src);
+        e_mat.vmult(temp_vect_2,src);
+
+        a_inv_direct.vmult(temp_vect_3,temp_vect_1);
+        a_inv_direct.vmult(temp_vect_4,temp_vect_2);
+
+        c_mat.Tvmult(temp_vect_6,temp_vect_4);
+        e_mat.Tvmult(temp_vect_5,temp_vect_3);
+
+        b_mat.vmult(temp_vect_7,src);
+        dst =  temp_vect_7 - temp_vect_6 - temp_vect_5;
+
+
+
+}
+
+
+template<int dim>
+void HMatrixDirect<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+{
+
+        c_mat.vmult(temp_vect_1,src);
+        e_mat.vmult(temp_vect_2,src);
+
+        a_inv_direct.vmult(temp_vect_3,temp_vect_1);
+        a_inv_direct.vmult(temp_vect_4,temp_vect_2);
+
+        c_mat.Tvmult(temp_vect_6,temp_vect_4);
+        e_mat.Tvmult(temp_vect_5,temp_vect_3);
+
+        b_mat.vmult(temp_vect_7,src);
+        dst =  temp_vect_7 - temp_vect_6 - temp_vect_5;
+
+}
+
+
 // ******************     JinvMatrix     ***********************
 template<int dim>
 JinvMatrix<dim>::JinvMatrix(HMatrix<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in)
@@ -1016,6 +1188,56 @@ void KinvMatrix<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) c
     dst = -1*temp_vect_4 - temp_vect_3;
 }
 
+// ******************     KinvMatrixDirect     ***********************
+template<int dim>
+KinvMatrixDirect<dim>::KinvMatrixDirect(HMatrixDirect<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in)
+    :
+      h_mat(h_mat_in),
+      g_mat(g_mat_in),
+      d_m_mat(d_m_mat_in),
+      d_m_inv_mat(d_m_inv_mat_in)
+{
+
+}
+
+template<int dim>
+void
+KinvMatrixDirect<dim>::initialize(LA::MPI::Vector &exemplar_density_vector)
+{
+    temp_vect_1 = exemplar_density_vector;
+    temp_vect_2 = exemplar_density_vector;
+    temp_vect_3 = exemplar_density_vector;
+    temp_vect_4 = exemplar_density_vector;
+
+}
+
+template<int dim>
+void KinvMatrixDirect<dim>::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+{
+    h_mat.vmult(temp_vect_1,src);
+    d_m_inv_mat.vmult(temp_vect_2,temp_vect_1);
+    g_mat.vmult(temp_vect_3,temp_vect_2);
+    d_m_mat.vmult(temp_vect_4,src);
+
+    dst = -1*temp_vect_4 - temp_vect_3;
+}
+
+template<int dim>
+void KinvMatrixDirect<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+{
+    g_mat.vmult(temp_vect_1,src);
+    d_m_inv_mat.vmult(temp_vect_2,temp_vect_1);
+    h_mat.vmult(temp_vect_3,temp_vect_2);
+    d_m_mat.vmult(temp_vect_4,src);
+
+    dst = -1*temp_vect_4 - temp_vect_3;
+}
+
+
+
+
+//**************************************************
+
 
 AMatWrapped::AMatWrapped(LA::MPI::SparseMatrix &a_mat_in)
     :

From 88291e89481a88c3614ff4f29a3bd39fc1f0758d Mon Sep 17 00:00:00 2001
From: Justin O'Connor <oconnor@up.math.colostate.edu>
Date: Wed, 11 May 2022 13:47:51 -0600
Subject: [PATCH 91/95] WORKING GMG PRECONDITIONER

---
 include/input_information.h      |   2 +-
 include/schur_preconditioner.h   |  20 ++++
 source/matrix_free_elasticity.cc |   8 +-
 source/schur_preconditioner.cc   | 200 ++++++++++++++++++++-----------
 4 files changed, 152 insertions(+), 78 deletions(-)

diff --git a/include/input_information.h b/include/input_information.h
index 8b4442d..f3c41a2 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -18,7 +18,7 @@ namespace SAND {
         constexpr unsigned int geometry_base = GeometryOptions::mbb;
 
         constexpr unsigned int dim = 2;
-        constexpr unsigned int refinements = 3;
+        constexpr unsigned int refinements = 4;
 
         //nonlinear algorithm options
         constexpr double initial_barrier_size = 25;
diff --git a/include/schur_preconditioner.h b/include/schur_preconditioner.h
index 3634940..9110d96 100644
--- a/include/schur_preconditioner.h
+++ b/include/schur_preconditioner.h
@@ -280,6 +280,26 @@ namespace SAND
             mutable LA::MPI::Vector temp_vect_4;
     };
 
+    template<int dim>
+    class JinvMatrixDirect : public TrilinosWrappers::SparseMatrix {
+        public:
+            JinvMatrixDirect(HMatrixDirect<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in);
+            void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
+            void Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
+            void initialize(LA::MPI::Vector &exemplar_density_vector);
+            unsigned int m() const;
+            unsigned int n() const;
+        private:
+            HMatrixDirect<dim> &h_mat;
+            GMatrix &g_mat;
+            const LA::MPI::SparseMatrix &d_m_mat;
+            LA::MPI::SparseMatrix &d_m_inv_mat;
+            mutable LA::MPI::Vector temp_vect_1;
+            mutable LA::MPI::Vector temp_vect_2;
+            mutable LA::MPI::Vector temp_vect_3;
+            mutable LA::MPI::Vector temp_vect_4;
+    };
+
     template<int dim>
     class TopOptSchurPreconditioner: public Subscriptor {
     public:
diff --git a/source/matrix_free_elasticity.cc b/source/matrix_free_elasticity.cc
index 62af044..2dc93fb 100644
--- a/source/matrix_free_elasticity.cc
+++ b/source/matrix_free_elasticity.cc
@@ -40,9 +40,9 @@ MF_Elasticity_Operator<dim,fe_degree,number>::compute_diagonal ()
 
     for (auto &local_element : inverse_diagonal)
       {
-        Assert(local_element > 0.,
-               ExcMessage("No diagonal entry in a positive definite operator "
-                          "should be zero or negative."));
+//        Assert(local_element > 0.,
+//               ExcMessage("No diagonal entry in a positive definite operator "
+//                          "should be zero or negative."));
         local_element = 1./local_element;
       }
 }
@@ -134,7 +134,7 @@ MF_Elasticity_Operator<dim, fe_degree, number>::local_apply(
             }
 
 
-            displacement.submit_symmetric_gradient( symgrad_term, q);
+            displacement.submit_symmetric_gradient(symgrad_term, q);
 
         }
 
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 7670449..6c4706c 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -202,6 +202,7 @@ void TopOptSchurPreconditioner<dim>::initialize(LA::MPI::BlockSparseMatrix &matr
     //        {
     //             a_inv_direct.initialize(a_mat);
     //        }
+    a_inv_direct.initialize(a_mat);
     {
         TimerOutput::Scope t(timer, "reinit diag matrices");
         d_3_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
@@ -347,101 +348,104 @@ void TopOptSchurPreconditioner<dim>::vmult(LA::MPI::BlockVector &dst, const LA::
     num_mults++;
 
 
+//    if (num_mults > 7)
+//    {
 
+//        std::cout << "entering problem area"  <<std::endl;
+//        FullMatrix<double> X_matrix;
+//        FullMatrix<double> X_matrix_direct;
+//        LA::MPI::Vector exemplar_vector;
+//        LA::MPI::Vector exemplar_vector_displacement;
+//        const types::global_dof_index n_p = system_matrix.block(SolutionBlocks::density,
+//                                                                SolutionBlocks::density).m();
 
+//        X_matrix.reinit(n_p,n_p);
+//        X_matrix_direct.reinit(n_p,n_p);
+//        exemplar_vector.reinit(src.block(SolutionBlocks::density));
+//        exemplar_vector_displacement.reinit(src.block(SolutionBlocks::displacement));
+//        exemplar_vector = 0;
 
+//        HMatrix<dim> h_mat_test (a_mat, b_mat, c_mat, e_mat, pre_amg, a_inv_direct, a_inv_mf_gmg);
+//        HMatrixDirect<dim> h_mat_test_direct (a_mat, b_mat, c_mat, e_mat, pre_amg, a_inv_direct, a_inv_mf_gmg);
 
+//        h_mat_test.initialize(exemplar_vector, exemplar_vector_displacement);
+//        h_mat_test_direct.initialize(exemplar_vector, exemplar_vector_displacement);
 
+//        KinvMatrix<dim>         k_inv_test          (h_mat_test, g_mat, d_m_mat, d_m_inv_mat);
 
-    if (num_mults > 10)
-    {
-        FullMatrix<double> X_matrix;
-        FullMatrix<double> X_matrix_direct;
-        LA::MPI::Vector exemplar_vector;
-        const types::global_dof_index n_p = system_matrix.block(SolutionBlocks::density,
-                                                                SolutionBlocks::density).m();
-
-        X_matrix.reinit(n_p,n_p);
-        X_matrix_direct.reinit(n_p,n_p);
-        exemplar_vector.reinit(src.block(SolutionBlocks::density));
+//        KinvMatrixDirect<dim>   k_inv_test_direct   (h_mat_test_direct, g_mat, d_m_mat, d_m_inv_mat);
 
-        HMatrix<dim> h_mat_test (a_mat, b_mat, c_mat, e_mat, pre_amg, a_inv_direct, a_inv_mf_gmg);
-        HMatrixDirect<dim> h_mat_test_direct (a_mat, b_mat, c_mat, e_mat, pre_amg, a_inv_direct, a_inv_mf_gmg);
 
-        KinvMatrix<dim>         k_inv_test          (h_mat_test, g_mat, d_m_mat, d_m_inv_mat);
 
-        KinvMatrixDirect<dim>   k_inv_test_direct   (h_mat_test_direct, g_mat, d_m_mat, d_m_inv_mat);
+//        k_inv_test.initialize(exemplar_vector);
+//        k_inv_test_direct.initialize(exemplar_vector);
 
+//        std::cout << "finished setup"  <<std::endl;
 
 
-        Threads::TaskGroup<void> tasks;
-        for (unsigned int j=0; j<X_matrix.n(); ++j)
-            tasks += Threads::new_task ([&X_matrix, &exemplar_vector, &k_inv_test, j]()
-                                        {
-                                            LA::MPI::Vector e_j (exemplar_vector);
-                                            LA::MPI::Vector r_j (exemplar_vector);
 
-                                            e_j = 0;
-                                            e_j(j) = 1;
-                                            k_inv_test.vmult(r_j,e_j);
 
-                                            for (unsigned int i=0; i<X_matrix.m(); ++i)
-                                                X_matrix(i,j) = r_j(i);
-                                        });
+//        for (unsigned int j=0; j<X_matrix.n(); ++j)
+//        {
+//            LA::MPI::Vector e_j (exemplar_vector);
+//            LA::MPI::Vector r_j (exemplar_vector);
 
-        tasks.join_all();
+//            e_j = 0;
+//            e_j(j) = 1;
 
-        for (unsigned int j=0; j<X_matrix.n(); ++j)
-            tasks += Threads::new_task ([&X_matrix_direct, &exemplar_vector, &k_inv_test_direct, j]()
-                                        {
-                                            LA::MPI::Vector e_j (exemplar_vector);
-                                            LA::MPI::Vector r_j (exemplar_vector);
+//            k_inv_test.vmult(r_j,e_j);
 
-                                            e_j = 0;
-                                            e_j(j) = 1;
-                                            k_inv_test_direct.vmult(r_j,e_j);
+//            for (unsigned int i=0; i<X_matrix.m(); ++i)
+//                X_matrix(i,j) = r_j(i);
+//        }
+//        std::cout << "+++++++++++++++++++++BUILT first BIG MATRIx++++++++++++++++++++++++++" << std::endl;
 
-                                            for (unsigned int i=0; i<X_matrix_direct.m(); ++i)
-                                                X_matrix_direct(i,j) = r_j(i);
-                                        });
 
-        tasks.join_all();
+//        for (unsigned int j=0; j<X_matrix.n(); ++j)
+//        {
+//            LA::MPI::Vector e_j (exemplar_vector);
+//            LA::MPI::Vector r_j (exemplar_vector);
 
+//            e_j = 0;
+//            e_j(j) = 1;
+//            k_inv_test_direct.vmult(r_j,e_j);
 
+//            for (unsigned int i=0; i<X_matrix_direct.m(); ++i)
+//                X_matrix_direct(i,j) = r_j(i);
+//        }
 
+//std::cout << "+++++++++++++++++++++BUILT BIG MATRICES++++++++++++++++++++++++++" << std::endl;
 
+//        const unsigned int n = X_matrix.n();
+//        const unsigned int m = X_matrix.m();
 
 
-        const unsigned int n = X_matrix.n();
-        const unsigned int m = X_matrix.m();
+//        std::ofstream Xmat("Kmatrix_MFGMG");
+//        for (unsigned int i = 0; i < m; i++)
+//        {
+//            Xmat << X_matrix(i, 0);
+//            for (unsigned int j = 1; j < n; j++)
+//            {
+//                Xmat << "," << X_matrix(i, j);
+//            }
+//            Xmat << "\n";
+//        }
+//        Xmat.close();
 
+//        std::ofstream Xmat_direct("Kmatrix_MFGMG_Direct");
+//        for (unsigned int i = 0; i < m; i++)
+//        {
+//            Xmat_direct << X_matrix_direct(i, 0);
+//            for (unsigned int j = 1; j < n; j++)
+//            {
+//                Xmat_direct << "," << X_matrix_direct(i, j);
+//            }
+//            Xmat_direct << "\n";
+//        }
+//        Xmat_direct.close();
 
-        std::ofstream Xmat("Kmatrix_MFGMG");
-        for (unsigned int i = 0; i < m; i++)
-        {
-            Xmat << X_matrix(i, 0);
-            for (unsigned int j = 1; j < n; j++)
-            {
-                Xmat << "," << X_matrix(i, j);
-            }
-            Xmat << "\n";
-        }
-        Xmat.close();
-
-        std::ofstream Xmat_direct("Kmatrix_MFGMG_Direct");
-        for (unsigned int i = 0; i < m; i++)
-        {
-            Xmat_direct << X_matrix_direct(i, 0);
-            for (unsigned int j = 1; j < n; j++)
-            {
-                Xmat_direct << "," << X_matrix_direct(i, j);
-            }
-            Xmat_direct << "\n";
-        }
-        Xmat_direct.close();
-
-        std::abort();
-    }
+//        std::abort();
+//    }
 
 
 }
@@ -507,7 +511,7 @@ void TopOptSchurPreconditioner<dim>::vmult_step_3(LA::MPI::BlockVector &dst, con
         c_mat.Tvmult(dst_temp.block(SolutionBlocks::density_upper_slack),dst_temp.block(SolutionBlocks::displacement_multiplier));
         e_mat.Tvmult(dst_temp.block(SolutionBlocks::density_lower_slack),dst_temp.block(SolutionBlocks::displacement));
 
-        dst.block(SolutionBlocks::density) = dst_temp.block(SolutionBlocks::density) - dst_temp.block(SolutionBlocks::density_upper_slack) - dst_temp.block(SolutionBlocks::density_lower_slack);
+        dst.block(SolutionBlocks::density) = dst_temp.block(SolutionBlocks::density) + dst_temp.block(SolutionBlocks::density_upper_slack) + dst_temp.block(SolutionBlocks::density_lower_slack);
 
     }
     else
@@ -594,6 +598,8 @@ void TopOptSchurPreconditioner<dim>::vmult_step_5(LA::MPI::BlockVector &dst, con
         TimerOutput::Scope t(timer, "inverse 3");
         if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
         {
+            LA::MPI::BlockVector dst_temp = dst;
+
             //                SolverControl            solver_control(100000, 1e-6);
             //                SolverCG<LA::MPI::Vector> a_solver_cg(solver_control);
 
@@ -602,8 +608,10 @@ void TopOptSchurPreconditioner<dim>::vmult_step_5(LA::MPI::BlockVector &dst, con
             //                dst.block(SolutionBlocks::displacement) = a_inv_op * src.block(SolutionBlocks::displacement_multiplier);
             //                dst.block(SolutionBlocks::displacement_multiplier) = a_inv_op * src.block(SolutionBlocks::displacement);
 
-            a_inv_mf_gmg.vmult( dst.block(SolutionBlocks::displacement), src.block(SolutionBlocks::displacement_multiplier));
-            a_inv_mf_gmg.vmult( dst.block(SolutionBlocks::displacement_multiplier), src.block(SolutionBlocks::displacement));
+            a_inv_mf_gmg.vmult( dst_temp.block(SolutionBlocks::displacement), src.block(SolutionBlocks::displacement_multiplier));
+            a_inv_mf_gmg.vmult( dst_temp.block(SolutionBlocks::displacement_multiplier), src.block(SolutionBlocks::displacement));
+            dst.block(SolutionBlocks::displacement) = -1 * dst_temp.block(SolutionBlocks::displacement);
+            dst.block(SolutionBlocks::displacement_multiplier) = -1 * dst_temp.block(SolutionBlocks::displacement_multiplier);
         }
         else
         {
@@ -949,7 +957,7 @@ void HMatrix<dim>::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
         e_mat.Tvmult(temp_vect_5,temp_vect_3);
 
         b_mat.vmult(temp_vect_7,src);
-        dst =  temp_vect_7 - temp_vect_6 - temp_vect_5;
+        dst =  temp_vect_7 + temp_vect_6 + temp_vect_5;
     }
     else
     {
@@ -1007,7 +1015,7 @@ void HMatrix<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) cons
         e_mat.Tvmult(temp_vect_5,temp_vect_3);
 
         b_mat.vmult(temp_vect_7,src);
-        dst =  temp_vect_7 - temp_vect_6 - temp_vect_5;
+        dst =  temp_vect_7 + temp_vect_6 + temp_vect_5;
     }
     else
     {
@@ -1143,6 +1151,52 @@ void JinvMatrix<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) c
     dst = -1*temp_vect_4 - temp_vect_3;
 }
 
+// ******************     JinvMatrixDirect     ***********************
+template<int dim>
+JinvMatrixDirect<dim>::JinvMatrixDirect(HMatrixDirect<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in)
+    :
+      h_mat(h_mat_in),
+      g_mat(g_mat_in),
+      d_m_mat(d_m_mat_in),
+      d_m_inv_mat(d_m_inv_mat_in)
+{
+
+}
+
+template<int dim>
+void
+JinvMatrixDirect<dim>::initialize(LA::MPI::Vector &exemplar_density_vector)
+{
+    temp_vect_1 = exemplar_density_vector;
+    temp_vect_2 = exemplar_density_vector;
+    temp_vect_3 = exemplar_density_vector;
+    temp_vect_4 = exemplar_density_vector;
+
+}
+
+template<int dim>
+void JinvMatrixDirect<dim>::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+{
+    g_mat.vmult(temp_vect_1,src);
+    d_m_inv_mat.vmult(temp_vect_2,temp_vect_1);
+    h_mat.vmult(temp_vect_3,temp_vect_2);
+    d_m_mat.vmult(temp_vect_4,src);
+
+    dst = -1*temp_vect_4 - temp_vect_3;
+}
+
+template<int dim>
+void JinvMatrixDirect<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+{
+    h_mat.vmult(temp_vect_1,src);
+    d_m_inv_mat.vmult(temp_vect_2,temp_vect_1);
+    g_mat.vmult(temp_vect_3,temp_vect_2);
+    d_m_mat.vmult(temp_vect_4,src);
+
+    dst = -1*temp_vect_4 - temp_vect_3;
+}
+
+
 // ******************     KinvMatrix     ***********************
 template<int dim>
 KinvMatrix<dim>::KinvMatrix(HMatrix<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in)

From ff05ed035f09c21e301ed4e835e8e442a3b00a95 Mon Sep 17 00:00:00 2001
From: juleoc02 <hOb02goblin>
Date: Fri, 27 May 2022 07:06:03 -0600
Subject: [PATCH 92/95] MPI MFGMG Runs, takes one extra GMRES iteration

---
 CMakeLists.txt       |  4 +--
 include/kkt_system.h |  2 +-
 source/kkt_system.cc | 68 +++++++++++++++++++++++++++-----------------
 source/watchdog.cc   |  3 ++
 4 files changed, 48 insertions(+), 29 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index b32c19c..c416efe 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,6 +1,7 @@
 # Set the name of the project and target:
 SET(TARGET "SAND")
 
+
 # Declare all source files the target consists of. Here, this is only
 # the one step-X.cc file, but as you expand your project you may wish
 # to add other source files as well. If your project becomes much larger,
@@ -22,8 +23,7 @@ SET(  TARGET_SRC ${TARGET_SRC} ${TARGET_INC}
 # Usually, you will not need to modify anything beyond this point...
 
 CMAKE_MINIMUM_REQUIRED(VERSION 3.1.0)
-
-FIND_PACKAGE(deal.II 10.0.0 QUIET
+FIND_PACKAGE(deal.II 9.4.0 QUIET
   HINTS ${deal.II_DIR} ${DEAL_II_DIR} ../ ../../ $ENV{DEAL_II_DIR}
   )
 IF(NOT ${deal.II_FOUND})
diff --git a/include/kkt_system.h b/include/kkt_system.h
index 7632fdd..d3078eb 100644
--- a/include/kkt_system.h
+++ b/include/kkt_system.h
@@ -152,7 +152,7 @@ namespace SAND {
         mutable LA::MPI::BlockVector locally_relevant_solution;
         mutable LA::MPI::BlockVector distributed_solution;
         LA::MPI::BlockVector system_rhs;
-        /*parallel::distributed::*/Triangulation<dim> triangulation;
+        parallel::distributed::Triangulation<dim> triangulation;
         DoFHandler<dim> dof_handler;
         DoFHandler<dim> dof_handler_displacement;
         DoFHandler<dim> dof_handler_density;
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index 2eb6cc3..a007484 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -74,6 +74,7 @@ void copy_from_system_to_displacement_vector(dealii::LinearAlgebra::distributed:
 //    dealii::LinearAlgebra::ReadWriteVector<double> rwv(
 //                out.locally_owned_elements());
 //    rwv.import(in, VectorOperation::insert);
+    ConditionalOStream pcout (std::cout,(Utilities::MPI::this_mpi_process(MPI_COMM_WORLD) == 1));
     for (const auto &index_pair : displacement_to_system_dof_index_map)
     {
         out[index_pair.first] = in[index_pair.second];
@@ -108,9 +109,9 @@ template<int dim>
 KktSystem<dim>::KktSystem()
     :
       mpi_communicator(MPI_COMM_WORLD),
-      triangulation(/*mpi_communicator,*/
-                    Triangulation<dim>::limit_level_difference_at_vertices/*,
-                    parallel::distributed::Triangulation<dim>::construct_multigrid_hierarchy*/),
+      triangulation(mpi_communicator,
+                    Triangulation<dim>::limit_level_difference_at_vertices,
+                    parallel::distributed::Triangulation<dim>::construct_multigrid_hierarchy),
       dof_handler(triangulation),
       dof_handler_displacement(triangulation),
       dof_handler_density(triangulation),
@@ -475,7 +476,11 @@ KktSystem<dim>::setup_boundary_values()
 
             for(unsigned int level = 0; level < n_levels; ++level)
             {
-                mg_level_constraints[level].reinit(dof_handler_displacement.locally_owned_mg_dofs (level));
+                IndexSet relevant_dofs;
+                DoFTools::extract_locally_relevant_level_dofs(dof_handler_displacement,
+                                                      level,
+                                                      relevant_dofs);
+                mg_level_constraints[level].reinit(relevant_dofs);
             }
 
 
@@ -500,12 +505,12 @@ KktSystem<dim>::setup_boundary_values()
                                 {
                                      for (unsigned int level = 0; level < n_levels; ++level)
                                      {
-
                                          const unsigned int x_displacement =
                                                  cell->mg_vertex_dof_index(level, vertex_number, 0, cell->active_fe_index());
                                          const unsigned int y_displacement =
                                                  cell->mg_vertex_dof_index(level, vertex_number, 1, cell->active_fe_index());
-                                         /*set bottom left BC*/
+                                                 
+                                        /*set bottom left BC*/
 
                                          level_dirichlet_boundary_dofs[level].insert(x_displacement);
                                          level_dirichlet_boundary_dofs[level].insert(y_displacement);
@@ -529,7 +534,8 @@ KktSystem<dim>::setup_boundary_values()
 
 //                                        level_dirichlet_boundary_dofs[level].insert(x_displacement);
                                         level_dirichlet_boundary_dofs[level].insert(y_displacement);
-
+                                        std::cout << "level is: " << level<< " and y = "  << y_displacement << std::endl;
+                                         
 //                                        level_boundary_values[level][x_displacement] = 0;
                                         level_boundary_values[level][y_displacement] = 0;
                                     }
@@ -1089,6 +1095,9 @@ KktSystem<dim>::setup_block_system() {
 
     }
 
+
+    pcout << "before filter setup" << std::endl;
+
     /*This finds neighbors whose values would be relevant, and adds them to the sparsity pattern of the matrix*/
     setup_filter_matrix();
     for (const auto &cell : dof_handler.active_cell_iterators()) {
@@ -1124,7 +1133,7 @@ KktSystem<dim>::setup_block_system() {
     distributed_solution.collect_sizes();
     system_rhs.collect_sizes();
     system_matrix.collect_sizes();
-
+    IndexSet locally_owned_displacement_dofs = dof_handler_displacement.locally_owned_dofs();
     std::vector<types::global_dof_index> displacement_dof_indices;
     std::vector<types::global_dof_index> system_dof_indices;
     for (const auto &displacement_cell : dof_handler_displacement.active_cell_iterators())
@@ -1143,11 +1152,15 @@ KktSystem<dim>::setup_block_system() {
 
             for (unsigned int i=0; i<displacement_dof_indices.size(); ++i)
             {
-                displacement_to_system_dof_index_map[displacement_dof_indices[i]]
-                        = system_dof_indices[system_cell->get_fe().component_to_system_index(
+                if(locally_owned_displacement_dofs.is_element(displacement_dof_indices[i]))
+                {
+                        displacement_to_system_dof_index_map[displacement_dof_indices[i]]
+                            = system_dof_indices[system_cell->get_fe().component_to_system_index(
                             displacement_cell->get_fe().system_to_component_index(i).first+SolutionComponents::displacement<dim>,
                             displacement_cell->get_fe().system_to_component_index(i).second
                             )];
+                }
+                
             }
         }
     const types::global_dof_index disp_start_index = system_matrix.get_row_indices().block_start(
@@ -2164,7 +2177,6 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
 
     SolverControl solver_control(100, gmres_tolerance);
 
-
     // ************ BEGIN MAKING MF GMG ELASTICITY PRECONDITIONER ***************************
     using SystemMFMatrixType = MF_Elasticity_Operator<dim, 1, double>;
     using LevelMFMatrixType = MF_Elasticity_Operator<dim, 1, double>;
@@ -2175,8 +2187,8 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
     unsigned int n_dofs = dof_handler.n_dofs();
     unsigned int n_dofs_displacement = dof_handler_displacement.n_dofs();
 
-    std::vector< Point< dim >> support_points (n_dofs);
-    std::vector< Point< dim >> support_points_displacement (n_dofs_displacement);
+    std::map< types::global_dof_index, Point< dim > > support_points;
+    std::map< types::global_dof_index, Point< dim > > support_points_displacement;
 
     MappingQGeneric<dim,dim> generic_map_displacement(1);
     MappingQGeneric<dim,dim> generic_map_1(1);
@@ -2193,20 +2205,30 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
 
     const types::global_dof_index disp_mult_start_index = system_matrix.get_row_indices().block_start(SolutionBlocks::displacement_multiplier);
 
-    for (unsigned int d=0; d<n_dofs_displacement; d++)
+    for (const auto &support_points_displacement_pair : support_points_displacement)
     {
-        if (support_points_displacement[d] != support_points[disp_mult_start_index+d])
-        std::cout << "d = " << d << ", points are " << support_points_displacement[d] << " and " << support_points[disp_mult_start_index+d] << std::endl;
+        if (support_points_displacement_pair.second != support_points[support_points_displacement_pair.first+disp_mult_start_index])
+            std::cout << "d = " << support_points_displacement_pair.first << ", points are " << support_points_displacement_pair.second << " and " << support_points[support_points_displacement_pair.first+disp_mult_start_index] << std::endl;
     }
 
     pcout << "Number of degrees of freedom: " << dof_handler_displacement.n_dofs()
           << std::endl;
 
 
+    std::vector<IndexSet> locally_owned_dofs = Utilities::MPI::all_gather(mpi_communicator, dof_handler_displacement.locally_owned_dofs());
+    IndexSet locally_active_dofs;
+    DoFTools::extract_locally_active_dofs(dof_handler_displacement, locally_active_dofs);
     IndexSet locally_relevant_dofs;
     DoFTools::extract_locally_relevant_dofs(dof_handler_displacement, locally_relevant_dofs);
+     
+    // AffineConstraints<double> temp_displacement_constraints;
+    displacement_constraints.clear();
+    displacement_constraints.reinit(locally_relevant_dofs);
     displacement_constraints.copy_from(mg_level_constraints[triangulation.n_global_levels()-1]);
-
+    pcout << "displacement constraint number: " << displacement_constraints.n_constraints() <<std::endl;
+    displacement_constraints.close();
+    // std::cout << displacement_constraints.is_consistent_in_parallel(locally_owned_dofs, locally_active_dofs,mpi_communicator) << "***********************************";
+    
     {
         typename MatrixFree<dim, double>::AdditionalData additional_data;
         additional_data.tasks_parallel_scheme =
@@ -2228,15 +2250,14 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
 
     elasticity_matrix_mf.initialize_dof_vector(distributed_displacement_sol);
     elasticity_matrix_mf.initialize_dof_vector(distributed_displacement_rhs);
-    std::cout << "displacement_vec_size = " << distributed_displacement_sol.size() << std::endl;
-    std::cout << "systdm_vec_size = " << distributed_solution.block(SolutionBlocks::displacement).size() << std::endl;
+    pcout << "displacement_vec_size = " << distributed_displacement_sol.size() << std::endl;
+    pcout << "systdm_vec_size = " << distributed_solution.block(SolutionBlocks::displacement).size() << std::endl;
 
     ChangeVectorTypes::copy_from_system_to_displacement_vector<double>(distributed_displacement_sol,distributed_solution.block(SolutionBlocks::displacement),displacement_to_system_dof_index_map);
 
     ChangeVectorTypes::copy_from_system_to_displacement_vector<double>(distributed_displacement_rhs,system_rhs.block(SolutionBlocks::displacement),displacement_to_system_dof_index_map);
 
 
-
     const unsigned int n_levels = triangulation.n_global_levels();
     mg_matrices.resize(0, n_levels - 1);
 
@@ -2332,8 +2353,7 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
                 std::vector<double> old_density_values(n_q_points);
 
                 const FEValuesExtractors::Scalar densities(SolutionComponents::density<dim>);
-
-                fe_values[densities].get_function_values(distributed_solution, old_density_values);
+                fe_values[densities].get_function_values(locally_relevant_solution, old_density_values);
                 double cell_density = old_density_values[0];
 
                 for (unsigned int i=0; i<rhs_values.size(); ++i)
@@ -2353,10 +2373,6 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
 
         active_density_vector.compress(VectorOperation::insert);
     }
-
-
-
-
     // MAKE ACTIVE_CELL_DATA
     std::vector<types::global_dof_index> local_dof_indices(fe_density.dofs_per_cell);
     for (unsigned int cell=0; cell<n_cells; ++cell)
diff --git a/source/watchdog.cc b/source/watchdog.cc
index b71f7c2..6a67634 100644
--- a/source/watchdog.cc
+++ b/source/watchdog.cc
@@ -331,6 +331,7 @@ namespace SAND {
         kkt_system.setup_boundary_values();
         pcout << "setup kkt system" << std::endl;
         kkt_system.setup_block_system();
+        pcout << "setup kkt system" << std::endl;
 
         if (Input::barrier_reduction==BarrierOptions::mixed)
         {
@@ -461,6 +462,8 @@ namespace SAND {
 
         markov_filter.setup(kkt_system.calculate_objective_value(current_state), kkt_system.calculate_barrier_distance(current_state), kkt_system.calculate_feasibility(current_state,barrier_size), barrier_size);
 
+        std::cout << "finished setup - beginning watchdog steps" << std::endl;
+
         while((barrier_size > Input::min_barrier_size || !check_convergence(current_state)) && iteration_number < Input::max_steps)
         {
             nonlinear_step(current_state, current_step, max_uphill_steps, iteration_number);

From 8998b5ea811638d72f8b987f029245d7ee28786a Mon Sep 17 00:00:00 2001
From: juleoc02 <hOb02goblin>
Date: Fri, 27 May 2022 21:12:40 -0600
Subject: [PATCH 93/95] update

---
 include/input_information.h |   4 +-
 source/kkt_system.cc        | 113 +++++++++++++++++++++++-------------
 2 files changed, 74 insertions(+), 43 deletions(-)

diff --git a/include/input_information.h b/include/input_information.h
index f3c41a2..6980c3a 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -17,7 +17,7 @@ namespace SAND {
         //geometry options
         constexpr unsigned int geometry_base = GeometryOptions::mbb;
 
-        constexpr unsigned int dim = 2;
+        constexpr unsigned int dim = 3;
         constexpr unsigned int refinements = 4;
 
         //nonlinear algorithm options
@@ -25,7 +25,7 @@ namespace SAND {
         constexpr double min_barrier_size = 0;
 
         constexpr double fraction_to_boundary = .7;
-        constexpr unsigned int max_steps=2;
+        constexpr unsigned int max_steps=30;
 
         constexpr unsigned int barrier_reduction=BarrierOptions::loqo;
         constexpr double required_norm = .0001;
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index a007484..41b0725 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -484,7 +484,9 @@ KktSystem<dim>::setup_boundary_values()
             }
 
 
-            for (auto cell=dof_handler_displacement.begin_active(n_levels-1); cell!=dof_handler_displacement.end_active(n_levels-1); ++cell)
+            for (auto cell=dof_handler_displacement.begin_active(n_levels-1); 
+                 cell!=dof_handler_displacement.end_active(n_levels-1); 
+                 ++cell)
             {
                 if(cell->is_locally_owned())
                 {
@@ -553,22 +555,26 @@ KktSystem<dim>::setup_boundary_values()
 
         } else if (dim == 3)
         {
-            for (const auto &cell: dof_handler.active_cell_iterators()) {
+            pcout << "setting up BVs" << std::endl;
+            for (const auto &cell: dof_handler.active_cell_iterators()) 
+            {
                 if(cell->is_locally_owned())
                 {
                     for (unsigned int face_number = 0;
                          face_number < GeometryInfo<dim>::faces_per_cell;
-                         ++face_number) {
-                        if (cell->face(face_number)->at_boundary()) {
+                         ++face_number) 
+                    {
+                        if (cell->face(face_number)->at_boundary()) 
+                        {
                             for (unsigned int vertex_number = 0;
                                  vertex_number < GeometryInfo<dim>::vertices_per_cell;
                                  ++vertex_number) {
                                 const auto vert = cell->vertex(vertex_number);
                                 /*Find bottom left corner*/
-                                if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
-                                            vert(1) - 0) < 1e-12 && ((std::fabs(
-                                                                          vert(2) - 0) < 1e-12) || (std::fabs(
-                                                                                                        vert(2) - 1) < 1e-12))) {
+                                if (std::fabs(vert(0) - 0) < 1e-12 && 
+                                    std::fabs(vert(1) - 0) < 1e-12 && 
+                                    ((std::fabs(vert(2) - 0) < 1e-12) || (std::fabs(vert(2) - 1) < 1e-12))) 
+                                {
 
 
                                     const unsigned int x_displacement =
@@ -592,10 +598,10 @@ KktSystem<dim>::setup_boundary_values()
                                     boundary_values[z_displacement_multiplier] = 0;
                                 }
                                 /*Find bottom right corner*/
-                                if (std::fabs(vert(0) - 6) < 1e-12 && std::fabs(
-                                            vert(1) - 0) < 1e-12 && ((std::fabs(
-                                                                          vert(2) - 0) < 1e-12) || (std::fabs(
-                                                                                                        vert(2) - 1) < 1e-12))) {
+                                if (std::fabs(vert(0) - 6) < 1e-12 && 
+                                    std::fabs(vert(1) - 0) < 1e-12 && 
+                                    ((std::fabs(vert(2) - 0) < 1e-12) || (std::fabs(vert(2) - 1) < 1e-12))) 
+                                {
                                     //                              const unsigned int x_displacement =
                                     //                                    cell->vertex_dof_index(vertex_number, 0, cell->active_fe_index());
                                     const unsigned int y_displacement =
@@ -621,39 +627,57 @@ KktSystem<dim>::setup_boundary_values()
                 }
 
             }
+
+            pcout << "setting up BVs for DOF_D" << std::endl;
+
             const unsigned int n_levels = triangulation.n_levels();
-            for (unsigned int level = 0; level < n_levels; ++level)
+            level_dirichlet_boundary_dofs.resize(0,n_levels-1);
+            level_boundary_values.resize(0,n_levels-1);
+            mg_level_constraints.resize(0,n_levels-1);
+
+            for(unsigned int level = 0; level < n_levels; ++level)
             {
-                for (auto cell=dof_handler_displacement.begin_active(level);
-                     cell!=dof_handler.end_active(level);
-                     ++cell)
+                IndexSet relevant_dofs;
+                DoFTools::extract_locally_relevant_level_dofs(dof_handler_displacement,
+                                                      level,
+                                                      relevant_dofs);
+                mg_level_constraints[level].reinit(relevant_dofs);
+            }
+
+            pcout << "loop 1" << std::endl;
+            for (auto cell=dof_handler_displacement.begin_active(n_levels-1);
+                    cell!=dof_handler_displacement.end_active(n_levels-1);
+                    ++cell)
+            {
+                pcout << "in loop 1" << std::endl;
+                if(cell->is_locally_owned())
                 {
-                    if(cell->is_locally_owned())
+                    for (unsigned int face_number = 0;
+                            face_number < GeometryInfo<dim>::faces_per_cell;
+                            ++face_number)
                     {
-                        for (unsigned int face_number = 0;
-                             face_number < GeometryInfo<dim>::faces_per_cell;
-                             ++face_number)
+                        if (cell->face(face_number)->at_boundary())
                         {
-                            if (cell->face(face_number)->at_boundary())
+                            for (unsigned int vertex_number = 0;
+                                    vertex_number < GeometryInfo<dim>::vertices_per_cell;
+                                    ++vertex_number)
                             {
-                                for (unsigned int vertex_number = 0;
-                                     vertex_number < GeometryInfo<dim>::vertices_per_cell;
-                                     ++vertex_number)
+                                const auto vert = cell->vertex(vertex_number);
+                                /*Find bottom left corner*/
+                                if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
+                                            vert(1) - 0) < 1e-12 && ((std::fabs(
+                                            vert(2) - 0) < 1e-12) || (std::fabs(
+                                            vert(2) - 1) < 1e-12)))
                                 {
-                                    const auto vert = cell->vertex(vertex_number);
-                                    /*Find bottom left corner*/
-                                    if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
-                                                vert(1) - 0) < 1e-12 && ((std::fabs(
-                                                vert(2) - 0) < 1e-12) || (std::fabs(
-                                                vert(2) - 1) < 1e-12)))
+                                    for (unsigned int level = 0; level < n_levels; ++level)
                                     {
-
+                                        pcout << "in loop 2" << std::endl;
                                         const unsigned int x_displacement =
-                                                cell->mg_vertex_dof_index(level, vertex_number, 0, 0);
+                                                 cell->mg_vertex_dof_index(level, vertex_number, 0, cell->active_fe_index());
                                         const unsigned int y_displacement =
-                                                cell->mg_vertex_dof_index(level, vertex_number, 1, 0);
+                                            cell->mg_vertex_dof_index(level, vertex_number, 1, cell->active_fe_index());
                                         const unsigned int z_displacement =
-                                                cell->mg_vertex_dof_index(level, vertex_number, 2, 0);
+                                            cell->mg_vertex_dof_index(level, vertex_number, 2, cell->active_fe_index());
                                         /*set bottom left BC*/
                                         level_boundary_values[level][x_displacement] = 0;
                                         level_boundary_values[level][y_displacement] = 0;
@@ -662,16 +686,20 @@ KktSystem<dim>::setup_boundary_values()
                                         level_dirichlet_boundary_dofs[level].insert(y_displacement);
                                         level_dirichlet_boundary_dofs[level].insert(z_displacement);
                                     }
-                                    /*Find bottom right corner*/
-                                    if (std::fabs(vert(0) - 6) < 1e-12 && std::fabs(
-                                                vert(1) - 0) < 1e-12 && ((std::fabs(
-                                                vert(2) - 0) < 1e-12) || (std::fabs(
-                                                vert(2) - 1) < 1e-12)))
+                                }
+                                /*Find bottom right corner*/
+                                if (std::fabs(vert(0) - 6) < 1e-12 && std::fabs(
+                                            vert(1) - 0) < 1e-12 && ((std::fabs(
+                                            vert(2) - 0) < 1e-12) || (std::fabs(
+                                            vert(2) - 1) < 1e-12)))
+                                {
+                                    for (unsigned int level = 0; level < n_levels; ++level)
                                     {
+                                        pcout << "in loop 3" << std::endl;
                                         const unsigned int y_displacement =
-                                                cell->mg_vertex_dof_index(level, vertex_number, 1, 0);
+                                                cell->mg_vertex_dof_index(level, vertex_number, 1, cell->active_fe_index());
                                         const unsigned int z_displacement =
-                                                cell->mg_vertex_dof_index(level, vertex_number, 2, 0);
+                                                cell->mg_vertex_dof_index(level, vertex_number, 2, cell->active_fe_index());
                                         level_boundary_values[level][y_displacement] = 0;
                                         level_boundary_values[level][z_displacement] = 0;
 
@@ -683,6 +711,9 @@ KktSystem<dim>::setup_boundary_values()
                         }
                     }
                 }
+            }
+            for (unsigned int level = 0; level < n_levels; ++level)
+            {
                 mg_level_constraints[level].add_lines(level_dirichlet_boundary_dofs[level]);
                 mg_level_constraints[level].close();
             }

From f864444007de64b4bc1e38b90a45cf077f5e16ab Mon Sep 17 00:00:00 2001
From: Justin O'Connor <oconnor@up.math.colostate.edu>
Date: Thu, 2 Feb 2023 09:09:30 -0700
Subject: [PATCH 94/95] Up to date

---
 .gitignore                       |   2 -
 include/input_information.h      |  22 +-
 include/kkt_system.h             |   2 +
 include/matrix_free_elasticity.h |   5 +
 include/schur_preconditioner.h   |  78 ++++++-
 source/density_filter.cc         |  91 ++++++---
 source/kkt_system.cc             | 339 +++++++++++++++++++++----------
 source/main.cc                   |  17 ++
 source/matrix_free_elasticity.cc |  46 +++--
 source/schur_preconditioner.cc   | 274 ++++++++++++++++++-------
 source/watchdog.cc               |   2 +-
 11 files changed, 650 insertions(+), 228 deletions(-)
 delete mode 100644 .gitignore

diff --git a/.gitignore b/.gitignore
deleted file mode 100644
index 25f9674..0000000
--- a/.gitignore
+++ /dev/null
@@ -1,2 +0,0 @@
-/Testing/
-/CMakeFiles/
diff --git a/include/input_information.h b/include/input_information.h
index 6980c3a..8d020cf 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -18,14 +18,15 @@ namespace SAND {
         constexpr unsigned int geometry_base = GeometryOptions::mbb;
 
         constexpr unsigned int dim = 3;
-        constexpr unsigned int refinements = 4;
+        // constexpr unsigned int refinements = 3;
+        extern unsigned int refinements;
 
         //nonlinear algorithm options
         constexpr double initial_barrier_size = 25;
         constexpr double min_barrier_size = 0;
 
-        constexpr double fraction_to_boundary = .7;
-        constexpr unsigned int max_steps=30;
+        constexpr double fraction_to_boundary = .9;
+        constexpr unsigned int max_steps=2;
 
         constexpr unsigned int barrier_reduction=BarrierOptions::loqo;
         constexpr double required_norm = .0001;
@@ -44,15 +45,20 @@ namespace SAND {
         //Linear solver options
         constexpr unsigned int solver_choice = SolverOptions::inexact_K_with_inexact_A_gmres;
         constexpr bool use_eisenstat_walker = false;
-        constexpr double default_gmres_tolerance = 1e-9;
+        constexpr double default_gmres_tolerance = 1e-6;
 
-        constexpr unsigned int a_inv_iterations = 25;
-        constexpr unsigned int k_inv_iterations = 100;
+        extern unsigned int a_inv_iterations;
+        extern unsigned int k_inv_iterations;
+
+        // constexpr double a_inv_iterations = 5;
+        // constexpr double k_inv_iterations = 30;
+
+        constexpr double a_rel_tol = 0.0;
+        constexpr double k_rel_tol = 0.0;
 
         //Material Options
         constexpr double material_lambda = 1;
         constexpr double material_mu = 1;
-
     }
 }
-#endif //SAND_INPUT_INFORMATION_H
+#endif //SAND_INPUT_INFORMATION_H
\ No newline at end of file
diff --git a/include/kkt_system.h b/include/kkt_system.h
index d3078eb..bb264a1 100644
--- a/include/kkt_system.h
+++ b/include/kkt_system.h
@@ -158,6 +158,7 @@ namespace SAND {
         DoFHandler<dim> dof_handler_density;
 
         std::map<types::global_dof_index,types::global_dof_index> displacement_to_system_dof_index_map;
+        MGLevelObject<std::map<types::global_dof_index,types::global_dof_index>> level_displacement_to_system_dof_index_map;
 
         AffineConstraints<double> constraints;
         AffineConstraints<double> displacement_constraints;
@@ -184,6 +185,7 @@ namespace SAND {
         OperatorCellData<dim, GMGNumberType> active_cell_data;
         MGLevelObject<OperatorCellData<dim, GMGNumberType>> level_cell_data;
         dealii::LinearAlgebra::distributed::Vector<double> active_density_vector;
+        dealii::LinearAlgebra::distributed::Vector<double> relevant_density_vector;
         MGLevelObject<dealii::LinearAlgebra::distributed::Vector<double>> level_density_vector;
 
         MGTransferMatrixFree<dim,GMGNumberType> transfer;
diff --git a/include/matrix_free_elasticity.h b/include/matrix_free_elasticity.h
index 6f5ea7b..525136f 100644
--- a/include/matrix_free_elasticity.h
+++ b/include/matrix_free_elasticity.h
@@ -26,6 +26,7 @@
 #include <deal.II/matrix_free/matrix_free.h>
 #include <deal.II/matrix_free/operators.h>
 #include <deal.II/matrix_free/fe_evaluation.h>
+#include <deal.II/base/utilities.h>
 
 #include "../include/parameters_and_components.h"
 #include "../include/input_information.h"
@@ -52,6 +53,8 @@ class MF_Elasticity_Operator
 {
 public:
 
+    MPI_Comm  mpi_communicator;
+
     using value_type = number;
 
     MF_Elasticity_Operator();
@@ -92,6 +95,8 @@ class MF_Elasticity_Operator
 
     const OperatorCellData<dim,number> *cell_data;
 
+    ConditionalOStream pcout;
+
 };
 
 
diff --git a/include/schur_preconditioner.h b/include/schur_preconditioner.h
index 9110d96..f3b8aed 100644
--- a/include/schur_preconditioner.h
+++ b/include/schur_preconditioner.h
@@ -60,6 +60,7 @@
 
 
 #include "../include/parameters_and_components.h"
+#include "../include/poly_pre.h"
 #include "matrix_free_elasticity.h"
 
 #include <deal.II/base/conditional_ostream.h>
@@ -141,8 +142,8 @@ namespace SAND
             MF_Elasticity_Operator<dim,1,double> &mf_elasticity_operator;
             PreconditionMG<dim,LinearAlgebra::distributed::Vector<double>,MGTransferMatrixFree<dim, double>> &mf_gmg_preconditioner;
             const std::map<types::global_dof_index,types::global_dof_index> displacement_to_system_dof_index_map;
-            double tolerance = 1e-9;
-            unsigned int iterations = 10;
+            double tolerance = Input::a_rel_tol;
+            unsigned int iterations = Input::a_inv_iterations;
             mutable dealii::LinearAlgebra::distributed::Vector<double> temp_src;
             mutable dealii::LinearAlgebra::distributed::Vector<double> temp_dst;
 
@@ -240,6 +241,26 @@ namespace SAND
             mutable LA::MPI::Vector temp_vect_4;
     };
 
+    template<int dim>
+    class KinvMatrixPart : public Subscriptor {
+        public:
+            KinvMatrixPart(HMatrix<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in);
+            void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
+            void Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
+            void initialize(LA::MPI::Vector &exemplar_density_vector);
+            unsigned int m() const;
+            unsigned int n() const;
+        private:
+            HMatrix<dim> &h_mat;
+            GMatrix &g_mat;
+            const LA::MPI::SparseMatrix &d_m_mat;
+            LA::MPI::SparseMatrix &d_m_inv_mat;
+            mutable LA::MPI::Vector temp_vect_1;
+            mutable LA::MPI::Vector temp_vect_2;
+            mutable LA::MPI::Vector temp_vect_3;
+            mutable LA::MPI::Vector temp_vect_4;
+    };
+
     template<int dim>
     class KinvMatrixDirect : public TrilinosWrappers::SparseMatrix {
         public:
@@ -280,6 +301,26 @@ namespace SAND
             mutable LA::MPI::Vector temp_vect_4;
     };
 
+    template<int dim>
+    class JinvMatrixPart : public Subscriptor {
+        public:
+            JinvMatrixPart(HMatrix<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in);
+            void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
+            void Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
+            void initialize(LA::MPI::Vector &exemplar_density_vector);
+            unsigned int m() const;
+            unsigned int n() const;
+        private:
+            HMatrix<dim> &h_mat;
+            GMatrix &g_mat;
+            const LA::MPI::SparseMatrix &d_m_mat;
+            LA::MPI::SparseMatrix &d_m_inv_mat;
+            mutable LA::MPI::Vector temp_vect_1;
+            mutable LA::MPI::Vector temp_vect_2;
+            mutable LA::MPI::Vector temp_vect_3;
+            mutable LA::MPI::Vector temp_vect_4;
+    };
+
     template<int dim>
     class JinvMatrixDirect : public TrilinosWrappers::SparseMatrix {
         public:
@@ -300,6 +341,8 @@ namespace SAND
             mutable LA::MPI::Vector temp_vect_4;
     };
 
+    
+
     template<int dim>
     class TopOptSchurPreconditioner: public Subscriptor {
     public:
@@ -382,10 +425,41 @@ namespace SAND
         JinvMatrix<dim> j_inv_mat;
         KinvMatrix<dim> k_inv_mat;
 
+        JinvMatrixPart<dim> j_inv_part;
+        KinvMatrixPart<dim> k_inv_part;
+
         mutable int num_mults;
 
     };
 
+        template<int dim>
+    class PolyPreJ {
+
+        public:
+            PolyPreJ(const JinvMatrixPart<dim> &inner_matrix_in, const int degree_in);
+            void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
+            // void vmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const;
+            
+
+        private:
+            const JinvMatrixPart<dim> &inner_matrix;
+            const int degree;
+    };
+
+    template<int dim>
+    class PolyPreK {
+
+        public:
+            PolyPreK(const KinvMatrixPart<dim> &inner_matrix_in, const int degree_in);
+            void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
+            // void vmult(LinearAlgebra::distributed::Vector<double> &dst, const LinearAlgebra::distributed::Vector<double> &src) const;
+            
+
+        private:
+            const KinvMatrixPart<dim> &inner_matrix;
+            const int degree;
+    };
+
 
 }
 #endif //SAND_SCHUR_PRECONDITIONER_H
diff --git a/source/density_filter.cc b/source/density_filter.cc
index 781a9d5..09ada1e 100644
--- a/source/density_filter.cc
+++ b/source/density_filter.cc
@@ -40,6 +40,7 @@ namespace SAND
         x_coord.resize(n_p);
         y_coord.resize(n_p);
         z_coord.resize(n_p);
+        auto row_sum = z_coord;
         cell_m.resize(n_p);
         x_coord_part.resize(n_p);
         y_coord_part.resize(n_p);
@@ -48,6 +49,26 @@ namespace SAND
 
         filter_dsp.reinit(dofs_per_block[0],
                           dofs_per_block[0]);
+        // filter_sparsity_pattern.copy_from(filter_dsp);
+
+        // const auto owned_dofs = dof_handler.locally_owned_dofs().get_view(0, dofs_per_block[0]);
+
+        // filter_matrix.reinit(owned_dofs, filter_sparsity_pattern, MPI_COMM_WORLD);
+
+
+        // //identity filter
+        // for (const auto &cell : dof_handler.active_cell_iterators())
+        // {
+        //     if(cell->is_locally_owned())
+        //     {
+        //         std::vector<unsigned int> i(cell->get_fe().n_dofs_per_cell());
+        //         cell->get_dof_indices(i);
+        //         filter_matrix.add(i[cell->get_fe().component_to_system_index(0, 0)], i[cell->get_fe().component_to_system_index(0, 0)], 1.0);
+        //     }
+        // }
+
+        pcout << "FILTER TO 1" << std::endl;
+       
         std::set<unsigned int> neighbor_ids;
         std::set<typename DoFHandler<dim>::cell_iterator> cells_to_check;
         std::set<typename DoFHandler<dim>::cell_iterator> cells_to_check_temp;
@@ -68,7 +89,7 @@ namespace SAND
                 }
              }
          }
-
+        pcout << "FILTER TO 2" << std::endl;
         MPI_Allreduce(x_coord_part.data(), x_coord.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
         MPI_Allreduce(y_coord_part.data(), y_coord.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
         MPI_Allreduce(z_coord_part.data(), z_coord.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
@@ -85,7 +106,7 @@ namespace SAND
                 }
             }
         }
-
+        pcout << "FILTER TO 3" << std::endl;
         filter_sparsity_pattern.copy_from(filter_dsp);
 
         const auto owned_dofs = dof_handler.locally_owned_dofs().get_view(0, dofs_per_block[0]);
@@ -99,6 +120,7 @@ namespace SAND
             {
                 std::vector<unsigned int> i(cell->get_fe().n_dofs_per_cell());
                 cell->get_dof_indices(i);
+                double value_total = 0;
                 for (const auto &neighbor_cell_index : find_relevant_neighbors(i[cell->get_fe().component_to_system_index(0, 0)]))
                 {
                     double d_x = std::abs(x_coord[i[cell->get_fe().component_to_system_index(0, 0)]]-x_coord[neighbor_cell_index]);
@@ -115,33 +137,54 @@ namespace SAND
                     }
                     ///value should be (max radius - distance between cells)*cell measure
                     double value = (Input::filter_r - d)*cell_m[neighbor_cell_index];
-                    filter_matrix.add(i[cell->get_fe().component_to_system_index(0, 0)], neighbor_cell_index, value);
+                    value_total += value;
                 }
-            }
-        }
-
-        ///here we normalize the filter so it computes an average. Sum of values in a row should be 1
-        for (const auto &cell : dof_handler.active_cell_iterators())
-        {
-            if(cell->is_locally_owned())
-            {
-                std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
-                cell->get_dof_indices(i);
-                const int i_ind = cell->get_fe().component_to_system_index(0, 0);
-                double denominator = 0;
-                typename LA::MPI::SparseMatrix::iterator iter = filter_matrix.begin(
-                        i[i_ind]);
-                for (; iter != filter_matrix.end(i[i_ind]); iter++)
-                {
-                    denominator = denominator + iter->value();
-                }
-                iter = filter_matrix.begin(i[i_ind]);
-                for (; iter != filter_matrix.end(i[i_ind]); iter++)
+            
+                for (const auto &neighbor_cell_index : find_relevant_neighbors(i[cell->get_fe().component_to_system_index(0, 0)]))
                 {
-                    iter->value() = iter->value() / denominator;
+                    double d_x = std::abs(x_coord[i[cell->get_fe().component_to_system_index(0, 0)]]-x_coord[neighbor_cell_index]);
+                    double d_y = std::abs(y_coord[i[cell->get_fe().component_to_system_index(0, 0)]]-y_coord[neighbor_cell_index]);
+                    double d;
+                    if (dim==3)
+                    {
+                        double d_z = std::abs(z_coord[i[cell->get_fe().component_to_system_index(0, 0)]]-z_coord[neighbor_cell_index]);
+                        d = std::pow(d_x*d_x + d_y*d_y + d_z*d_z , .5);
+                    }
+                    else
+                    {
+                        d = std::pow(d_x*d_x + d_y*d_y , .5);
+                    }
+                    ///value should be (max radius - distance between cells)*cell measure
+                    double value = (Input::filter_r - d)*cell_m[neighbor_cell_index];
+                    filter_matrix.add(i[cell->get_fe().component_to_system_index(0, 0)], neighbor_cell_index, value/value_total);
                 }
             }
         }
+        filter_matrix.compress(VectorOperation::add);
+        pcout << "FILTER TO 4" << std::endl;
+        ///here we normalize the filter so it computes an average. Sum of values in a row should be 1
+        // for (const auto &cell : dof_handler.active_cell_iterators())
+        // {
+        //     if(cell->is_locally_owned())
+        //     {
+        //         std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
+        //         cell->get_dof_indices(i);
+        //         const int i_ind = cell->get_fe().component_to_system_index(0, 0);
+        //         double denominator = 0;
+        //         typename LA::MPI::SparseMatrix::iterator iter = filter_matrix.begin(
+        //                 i[i_ind]);
+        //         for (; iter != filter_matrix.end(i[i_ind]); iter++)
+        //         {
+        //             denominator = denominator + iter->value();
+        //         }
+        //         iter = filter_matrix.begin(i[i_ind]);
+        //         for (; iter != filter_matrix.end(i[i_ind]); iter++)
+        //         {
+        //             iter->value() = iter->value() / denominator;
+        //         }
+        //     }
+        // }
+        // pcout << "FILTER TO 5" << std::endl;
     }
 
     ///This function finds which neighbors are within a certain radius of the initial cell.
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index 41b0725..7b6faec 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -40,6 +40,7 @@
 
 #include "../include/input_information.h"
 #include "../include/matrix_free_elasticity.h"
+#include "../include/poly_pre.h"
 
 #include <iostream>
 #include <algorithm>
@@ -144,7 +145,7 @@ KktSystem<dim>::KktSystem()
 template<int dim>
 void
 KktSystem<dim>::setup_filter_matrix() {
-
+    pcout << "IN KKT FILTER SETUP FUNCTION" << std::endl;
     density_filter.initialize(dof_handler);
 }
 
@@ -469,7 +470,7 @@ KktSystem<dim>::setup_boundary_values()
                 }
 
             }
-            const unsigned int n_levels = triangulation.n_levels();
+            const unsigned int n_levels = triangulation.n_global_levels();
             level_dirichlet_boundary_dofs.resize(0,n_levels-1);
             level_boundary_values.resize(0,n_levels-1);
             mg_level_constraints.resize(0,n_levels-1);
@@ -536,7 +537,6 @@ KktSystem<dim>::setup_boundary_values()
 
 //                                        level_dirichlet_boundary_dofs[level].insert(x_displacement);
                                         level_dirichlet_boundary_dofs[level].insert(y_displacement);
-                                        std::cout << "level is: " << level<< " and y = "  << y_displacement << std::endl;
                                          
 //                                        level_boundary_values[level][x_displacement] = 0;
                                         level_boundary_values[level][y_displacement] = 0;
@@ -549,7 +549,14 @@ KktSystem<dim>::setup_boundary_values()
             }
             for (unsigned int level = 0; level < n_levels; ++level)
             {
+                IndexSet relevant_dofs;
+                DoFTools::extract_locally_relevant_level_dofs(dof_handler_displacement,level,relevant_dofs);
                 mg_level_constraints[level].add_lines(level_dirichlet_boundary_dofs[level]);
+                mg_level_constraints[level].make_consistent_in_parallel(
+                    dof_handler_displacement.locally_owned_mg_dofs(level),
+                    relevant_dofs,
+                    mpi_communicator
+                );
                 mg_level_constraints[level].close();
             }
 
@@ -628,9 +635,7 @@ KktSystem<dim>::setup_boundary_values()
 
             }
 
-            pcout << "setting up BVs for DOF_D" << std::endl;
-
-            const unsigned int n_levels = triangulation.n_levels();
+            const unsigned int n_levels = triangulation.n_global_levels();
             level_dirichlet_boundary_dofs.resize(0,n_levels-1);
             level_boundary_values.resize(0,n_levels-1);
             mg_level_constraints.resize(0,n_levels-1);
@@ -644,12 +649,10 @@ KktSystem<dim>::setup_boundary_values()
                 mg_level_constraints[level].reinit(relevant_dofs);
             }
 
-            pcout << "loop 1" << std::endl;
             for (auto cell=dof_handler_displacement.begin_active(n_levels-1);
                     cell!=dof_handler_displacement.end_active(n_levels-1);
                     ++cell)
             {
-                pcout << "in loop 1" << std::endl;
                 if(cell->is_locally_owned())
                 {
                     for (unsigned int face_number = 0;
@@ -671,7 +674,6 @@ KktSystem<dim>::setup_boundary_values()
                                 {
                                     for (unsigned int level = 0; level < n_levels; ++level)
                                     {
-                                        pcout << "in loop 2" << std::endl;
                                         const unsigned int x_displacement =
                                                  cell->mg_vertex_dof_index(level, vertex_number, 0, cell->active_fe_index());
                                         const unsigned int y_displacement =
@@ -695,7 +697,6 @@ KktSystem<dim>::setup_boundary_values()
                                 {
                                     for (unsigned int level = 0; level < n_levels; ++level)
                                     {
-                                        pcout << "in loop 3" << std::endl;
                                         const unsigned int y_displacement =
                                                 cell->mg_vertex_dof_index(level, vertex_number, 1, cell->active_fe_index());
                                         const unsigned int z_displacement =
@@ -714,7 +715,14 @@ KktSystem<dim>::setup_boundary_values()
             }
             for (unsigned int level = 0; level < n_levels; ++level)
             {
+                IndexSet relevant_dofs;
+                DoFTools::extract_locally_relevant_level_dofs(dof_handler_displacement,level,relevant_dofs);
                 mg_level_constraints[level].add_lines(level_dirichlet_boundary_dofs[level]);
+                mg_level_constraints[level].make_consistent_in_parallel(
+                    dof_handler_displacement.locally_owned_mg_dofs(level),
+                    relevant_dofs,
+                    mpi_communicator
+                );
                 mg_level_constraints[level].close();
             }
 
@@ -777,7 +785,7 @@ KktSystem<dim>::setup_boundary_values()
                     }
                 }
             }
-            const unsigned int n_levels = triangulation.n_levels();
+            const unsigned int n_levels = triangulation.n_global_levels();
             for (unsigned int level = 0; level < n_levels; ++level)
             {
                 for (auto cell=dof_handler_displacement.begin_active(level);
@@ -833,7 +841,14 @@ KktSystem<dim>::setup_boundary_values()
                         }
                     }
                 }
+                IndexSet relevant_dofs;
+                DoFTools::extract_locally_relevant_level_dofs(dof_handler_displacement,level,relevant_dofs);
                 mg_level_constraints[level].add_lines(level_dirichlet_boundary_dofs[level]);
+                mg_level_constraints[level].make_consistent_in_parallel(
+                    dof_handler_displacement.locally_owned_mg_dofs(level),
+                    relevant_dofs,
+                    mpi_communicator
+                );
                 mg_level_constraints[level].close();
             }
 
@@ -907,7 +922,7 @@ KktSystem<dim>::setup_boundary_values()
                     }
                 }
             }
-            const unsigned int n_levels = triangulation.n_levels();
+            const unsigned int n_levels = triangulation.n_global_levels();
             for (unsigned int level = 0; level < n_levels; ++level)
             {
                 for (auto cell=dof_handler_displacement.begin_active(level);
@@ -974,7 +989,14 @@ KktSystem<dim>::setup_boundary_values()
                         }
                     }
                 }
+                IndexSet relevant_dofs;
+                DoFTools::extract_locally_relevant_level_dofs(dof_handler_displacement,level,relevant_dofs);
                 mg_level_constraints[level].add_lines(level_dirichlet_boundary_dofs[level]);
+                mg_level_constraints[level].make_consistent_in_parallel(
+                    dof_handler_displacement.locally_owned_mg_dofs(level),
+                    relevant_dofs,
+                    mpi_communicator
+                );
                 mg_level_constraints[level].close();
             }
 
@@ -1127,10 +1149,11 @@ KktSystem<dim>::setup_block_system() {
     }
 
 
-    pcout << "before filter setup" << std::endl;
+    pcout << "skipping filter setup" << std::endl;
 
     /*This finds neighbors whose values would be relevant, and adds them to the sparsity pattern of the matrix*/
     setup_filter_matrix();
+    pcout << "to here" << std::endl;
     for (const auto &cell : dof_handler.active_cell_iterators()) {
         if (cell->is_locally_owned())
         {
@@ -1146,7 +1169,7 @@ KktSystem<dim>::setup_block_system() {
             }
         }
     }
-
+    
     SparsityTools::distribute_sparsity_pattern(
                 dsp,
                 Utilities::MPI::all_gather(mpi_communicator,
@@ -1205,8 +1228,7 @@ KktSystem<dim>::setup_block_system() {
             std::cout << "inexact matching for index: " << index_pair.first << " and " << index_pair.second << std::endl;
         }
     }
-
-
+    
 
 }
 
@@ -2203,10 +2225,15 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
     else {
         gmres_tolerance = Input::default_gmres_tolerance;
     }
+
+
     locally_relevant_solution=state;
     distributed_solution = state;
 
-    SolverControl solver_control(100, gmres_tolerance);
+    density_filter.filter_matrix.vmult(distributed_solution.block(SolutionBlocks::density), distributed_solution.block(SolutionBlocks::unfiltered_density));
+    locally_relevant_solution = distributed_solution;
+
+    SolverControl solver_control(10000, gmres_tolerance);
 
     // ************ BEGIN MAKING MF GMG ELASTICITY PRECONDITIONER ***************************
     using SystemMFMatrixType = MF_Elasticity_Operator<dim, 1, double>;
@@ -2215,9 +2242,6 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
     elasticity_matrix_mf.clear();
     mg_matrices.clear_elements();
 
-    unsigned int n_dofs = dof_handler.n_dofs();
-    unsigned int n_dofs_displacement = dof_handler_displacement.n_dofs();
-
     std::map< types::global_dof_index, Point< dim > > support_points;
     std::map< types::global_dof_index, Point< dim > > support_points_displacement;
 
@@ -2239,27 +2263,30 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
     for (const auto &support_points_displacement_pair : support_points_displacement)
     {
         if (support_points_displacement_pair.second != support_points[support_points_displacement_pair.first+disp_mult_start_index])
-            std::cout << "d = " << support_points_displacement_pair.first << ", points are " << support_points_displacement_pair.second << " and " << support_points[support_points_displacement_pair.first+disp_mult_start_index] << std::endl;
+            pcout << "d = " << support_points_displacement_pair.first << ", points are " << support_points_displacement_pair.second << " and " << support_points[support_points_displacement_pair.first+disp_mult_start_index] << std::endl;
     }
 
     pcout << "Number of degrees of freedom: " << dof_handler_displacement.n_dofs()
           << std::endl;
 
+    MPI_Barrier(MPI_COMM_WORLD);
 
     std::vector<IndexSet> locally_owned_dofs = Utilities::MPI::all_gather(mpi_communicator, dof_handler_displacement.locally_owned_dofs());
     IndexSet locally_active_dofs;
     DoFTools::extract_locally_active_dofs(dof_handler_displacement, locally_active_dofs);
     IndexSet locally_relevant_dofs;
     DoFTools::extract_locally_relevant_dofs(dof_handler_displacement, locally_relevant_dofs);
-     
+    std::cout << Utilities::MPI::this_mpi_process(mpi_communicator) << " has relevant: " << locally_relevant_dofs.n_elements() << "and active: " << locally_relevant_dofs.n_elements() << std::endl;
     // AffineConstraints<double> temp_displacement_constraints;
+    
+    std::cout << Utilities::MPI::this_mpi_process(mpi_communicator) << " has levels: " << mg_level_constraints.max_level() << " and tria levels: " << triangulation.n_global_levels() << std::endl;
+
+
     displacement_constraints.clear();
-    displacement_constraints.reinit(locally_relevant_dofs);
+    displacement_constraints.reinit(locally_active_dofs); //FIXME SHOULD THIS BE RELEVANT???
     displacement_constraints.copy_from(mg_level_constraints[triangulation.n_global_levels()-1]);
     pcout << "displacement constraint number: " << displacement_constraints.n_constraints() <<std::endl;
     displacement_constraints.close();
-    // std::cout << displacement_constraints.is_consistent_in_parallel(locally_owned_dofs, locally_active_dofs,mpi_communicator) << "***********************************";
-    
     {
         typename MatrixFree<dim, double>::AdditionalData additional_data;
         additional_data.tasks_parallel_scheme =
@@ -2268,7 +2295,8 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
                 (update_gradients | update_JxW_values | update_quadrature_points);
         std::shared_ptr<MatrixFree<dim, double>> system_mf_storage(
                     new MatrixFree<dim, double>());
-        system_mf_storage->reinit(dof_handler_displacement,
+        system_mf_storage->reinit(generic_map_1,
+                                  dof_handler_displacement,
                                   displacement_constraints,
                                   QGauss<1>(fe_displacement.degree + 1),
                                   additional_data);
@@ -2281,13 +2309,21 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
 
     elasticity_matrix_mf.initialize_dof_vector(distributed_displacement_sol);
     elasticity_matrix_mf.initialize_dof_vector(distributed_displacement_rhs);
+
+    locally_relevant_solution = distributed_solution;
+
     pcout << "displacement_vec_size = " << distributed_displacement_sol.size() << std::endl;
     pcout << "systdm_vec_size = " << distributed_solution.block(SolutionBlocks::displacement).size() << std::endl;
 
-    ChangeVectorTypes::copy_from_system_to_displacement_vector<double>(distributed_displacement_sol,distributed_solution.block(SolutionBlocks::displacement),displacement_to_system_dof_index_map);
+    ChangeVectorTypes::copy_from_system_to_displacement_vector<double>(distributed_displacement_sol,locally_relevant_solution.block(SolutionBlocks::displacement),displacement_to_system_dof_index_map);
+
 
-    ChangeVectorTypes::copy_from_system_to_displacement_vector<double>(distributed_displacement_rhs,system_rhs.block(SolutionBlocks::displacement),displacement_to_system_dof_index_map);
+    locally_relevant_solution = system_rhs;
+    ChangeVectorTypes::copy_from_system_to_displacement_vector<double>(distributed_displacement_rhs,locally_relevant_solution.block(SolutionBlocks::displacement),displacement_to_system_dof_index_map);
 
+    locally_relevant_solution = distributed_solution;
+
+    pcout << "types changed" << std::endl;
 
     const unsigned int n_levels = triangulation.n_global_levels();
     mg_matrices.resize(0, n_levels - 1);
@@ -2295,14 +2331,18 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
     mg_constrained_dofs.clear();
     mg_constrained_dofs.initialize(dof_handler_displacement);
     const std::set<types::boundary_id> empty_boundary_set;
-    mg_constrained_dofs.make_zero_boundary_constraints(dof_handler_displacement,empty_boundary_set);
+    // mg_constrained_dofs.make_zero_boundary_constraints(dof_handler_displacement,empty_boundary_set);
+
 
     for (unsigned int level = 0; level < n_levels; ++level)
     {
         mg_constrained_dofs.add_user_constraints(level, mg_level_constraints[level]);
     }
 
-
+    // for (unsigned int level = 0; level < n_levels; ++level)
+    // {
+    //     mg_level_constraints[level].print(std::cout);
+    // }
 
     for (unsigned int level = 0; level < n_levels; ++level)
     {
@@ -2320,7 +2360,8 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
 
         std::shared_ptr<MatrixFree<dim, double>> mg_mf_storage_level(
                     new MatrixFree<dim, double>());
-        mg_mf_storage_level->reinit(dof_handler_displacement,
+        mg_mf_storage_level->reinit(generic_map_1,
+                                    dof_handler_displacement,
                                     mg_level_constraints[level],
                                     QGauss<1>(fe_displacement.degree + 1),
                                     additional_data);
@@ -2346,66 +2387,68 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
 
     ChangeVectorTypes::copy(active_density_vector,distributed_solution.block(SolutionBlocks::density));
 
+
     const unsigned int n_cells = elasticity_matrix_mf.get_matrix_free()->n_cell_batches();
-    {
-        active_cell_data.density.reinit(TableIndices<2>(n_cells, 1));
+    // {
+        
 
-        QGauss<dim> nine_quadrature(2);
-        QGauss<dim> ten_quadrature(2);
+    //     QGauss<dim> nine_quadrature(2);
+    //     QGauss<dim> ten_quadrature(2);
 
-        hp::QCollection<dim> q_collection;
-        q_collection.push_back(nine_quadrature);
-        q_collection.push_back(ten_quadrature);
+    //     hp::QCollection<dim> q_collection;
+    //     q_collection.push_back(nine_quadrature);
+    //     q_collection.push_back(ten_quadrature);
 
-        hp::FEValues<dim> hp_fe_values(fe_collection,
-                                       q_collection,
-                                       update_values | update_quadrature_points |
-                                       update_JxW_values | update_gradients);
+    //     hp::FEValues<dim> hp_fe_values(fe_collection,
+    //                                    q_collection,
+    //                                    update_values | update_quadrature_points |
+    //                                    update_JxW_values | update_gradients);
 
 
 
-        for (const auto &cell : dof_handler.active_cell_iterators())
-            if (cell->is_locally_owned())
-            {
+    //     for (const auto &cell : dof_handler.active_cell_iterators())
+    //         if (cell->is_locally_owned())
+    //         {
 
-                hp_fe_values.reinit(cell);
-                const FEValues<dim> &fe_values = hp_fe_values.get_present_fe_values();
+    //             hp_fe_values.reinit(cell);
+    //             const FEValues<dim> &fe_values = hp_fe_values.get_present_fe_values();
 
-                const unsigned int dofs_per_cell = fe_values.dofs_per_cell;
-                const unsigned int n_q_points = fe_values.n_quadrature_points;
+    //             const unsigned int dofs_per_cell = fe_values.dofs_per_cell;
+    //             const unsigned int n_q_points = fe_values.n_quadrature_points;
 
-                std::vector<types::global_dof_index> local_dof_indices (dofs_per_cell);
-                cell->get_dof_indices (local_dof_indices);
-                Vector<double> cell_vector (dofs_per_cell);
-                Vector<double> local_projection (dofs_per_cell);
-                FullMatrix<double> local_mass_matrix (dofs_per_cell, dofs_per_cell);
+    //             std::vector<types::global_dof_index> local_dof_indices (dofs_per_cell);
+    //             cell->get_dof_indices (local_dof_indices);
+    //             Vector<double> cell_vector (dofs_per_cell);
+    //             Vector<double> local_projection (dofs_per_cell);
+    //             FullMatrix<double> local_mass_matrix (dofs_per_cell, dofs_per_cell);
 
-                std::vector<double> rhs_values(n_q_points);
-                std::vector<double> old_density_values(n_q_points);
+    //             std::vector<double> rhs_values(n_q_points);
+    //             std::vector<double> old_density_values(n_q_points);
 
-                const FEValuesExtractors::Scalar densities(SolutionComponents::density<dim>);
-                fe_values[densities].get_function_values(locally_relevant_solution, old_density_values);
-                double cell_density = old_density_values[0];
+    //             const FEValuesExtractors::Scalar densities(SolutionComponents::density<dim>);
+    //             fe_values[densities].get_function_values(locally_relevant_solution, old_density_values);
+    //             double cell_density = old_density_values[0];
 
-                for (unsigned int i=0; i<rhs_values.size(); ++i)
-                {
-                    rhs_values[i] = cell_density;
-                }
+    //             for (unsigned int i=0; i<rhs_values.size(); ++i)
+    //             {
+    //                 rhs_values[i] = cell_density;
+    //             }
 
-                local_projection = cell_density;
+    //             local_projection = cell_density;
 
-                std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
-                cell->get_dof_indices(i);
-                const unsigned int i_val = i[cell->get_fe().component_to_system_index(0, 0)];
-                active_density_vector[i_val] = cell_density;
+    //             std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
+    //             cell->get_dof_indices(i);
+    //             const unsigned int i_val = i[cell->get_fe().component_to_system_index(0, 0)];
+    //             active_density_vector[i_val] = cell_density;
 
 
-            }
+    //         }
 
-        active_density_vector.compress(VectorOperation::insert);
-    }
+    //     // active_density_vector.compress(VectorOperation::insert);
+    // }
     // MAKE ACTIVE_CELL_DATA
     std::vector<types::global_dof_index> local_dof_indices(fe_density.dofs_per_cell);
+    active_cell_data.density.reinit(TableIndices<2>(n_cells, 1));
     for (unsigned int cell=0; cell<n_cells; ++cell)
     {
         const unsigned int n_components_filled = elasticity_matrix_mf.get_matrix_free()->n_active_entries_per_cell_batch(cell);
@@ -2421,9 +2464,9 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
             DG_cell->get_active_or_mg_dof_indices(local_dof_indices);
 
             active_cell_data.density(cell, 0)[i] = active_density_vector(local_dof_indices[0]);
-
         }
     }
+    
     elasticity_matrix_mf.set_cell_data(active_cell_data);
 
     //MAKE LEVEL DENSITY VECTOR
@@ -2434,9 +2477,9 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
 
     transfer.build(dof_handler_density);
 
-    transfer.template interpolate_to_mg<double>(dof_handler_density,
-                                                level_density_vector,
-                                                active_density_vector);
+    transfer.interpolate_to_mg(dof_handler_density,
+                               level_density_vector,
+                               active_density_vector);
 
     // MAKE LEVEL_CELL_DATA
     for (unsigned int level=0; level<n_levels; ++level)
@@ -2466,10 +2509,12 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
         // Store density tables and other data into the multigrid level matrix-free objects.
 
         mg_matrices[level].set_cell_data (level_cell_data[level]);
+
     }
 
 
 
+
     //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
 
@@ -2489,15 +2534,17 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
         }
         else
         {
-            smoother_data[0].smoothing_range = 1e-3;
-            smoother_data[0].degree          = numbers::invalid_unsigned_int;
-            smoother_data[0].eig_cg_n_iterations = mg_matrices[0].m();
+            smoother_data[0].smoothing_range = .001;
+            smoother_data[0].degree          = 4;
+            smoother_data[0].eig_cg_n_iterations = 100;
         }
         mg_matrices[level].compute_diagonal();
         smoother_data[level].preconditioner =
                 mg_matrices[level].get_matrix_diagonal_inverse();
     }
 
+    std::cout << Utilities::MPI::this_mpi_process(mpi_communicator) << " has matrix size " << mg_matrices[0].m() << " at level 0" << std::endl;
+
     mg_smoother.initialize(mg_matrices, smoother_data);
 
     mg_coarse.initialize(mg_smoother);
@@ -2514,42 +2561,87 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
     Multigrid<LinearAlgebra::distributed::Vector<double>> mg(
                 mg_matrix, mg_coarse, mg_transfer, mg_smoother, mg_smoother);
     mg.set_edge_matrices(mg_interface, mg_interface);
-
+    mg.set_cycle(Multigrid<LinearAlgebra::distributed::Vector<double>>::v_cycle);
+    mg.set_minlevel(0);
     PreconditionMG<dim,LinearAlgebra::distributed::Vector<double>,MGTransferMatrixFree<dim, double>>
             mf_gmg_preconditioner(dof_handler_displacement, mg, mg_transfer);
 
 
-////*************TEST SOLVE*************************
+// *************TEST SOLVE*************************
+// time the solve
+
+//    output(distributed_solution, 66);
+
+//    TimerOutput t(pcout, TimerOutput::never, TimerOutput::wall_times);
+//    {
+//    TimerOutput::Scope t_scope(t, "Solve_mfgmg");
 //    elasticity_matrix_mf.initialize_dof_vector(distributed_displacement_sol);
 //    elasticity_matrix_mf.initialize_dof_vector(distributed_displacement_rhs);
 
-//    ChangeVectorTypes::copy_from_system_to_displacement_vector<double>(distributed_displacement_sol,distributed_solution.block(SolutionBlocks::displacement),displacement_to_system_dof_index_map);
-//    ChangeVectorTypes::copy_from_system_to_displacement_vector<double>(distributed_displacement_rhs,system_rhs.block(SolutionBlocks::displacement),displacement_to_system_dof_index_map);
+//    locally_relevant_solution = system_rhs;
+//    ChangeVectorTypes::copy_from_system_to_displacement_vector<double>(distributed_displacement_rhs,locally_relevant_solution.block(SolutionBlocks::displacement),displacement_to_system_dof_index_map);
 
-//    SolverControl test_solver_control_1(500, 1e-6);
-//    SolverControl test_solver_control_2(500, 1e-6);
+//     pcout << "real rhs norm: " << system_rhs.block(SolutionBlocks::displacement).l2_norm() << std::endl;
+
+//    locally_relevant_solution = distributed_solution;
+//    ChangeVectorTypes::copy_from_system_to_displacement_vector<double>(distributed_displacement_sol,locally_relevant_solution.block(SolutionBlocks::displacement),displacement_to_system_dof_index_map);
+   
+//    SolverControl test_solver_control_1(20, 1e-6);
 //    SolverCG<LinearAlgebra::distributed::Vector<double>> CG_Solve_1(test_solver_control_1);
-//    SolverCG<LA::MPI::Vector> CG_Solve_2(test_solver_control_2);
 
-//    std::cout << "pre norm: " << distributed_displacement_rhs.l2_norm() << std::endl;
+//    pcout << "pre norm: " << distributed_displacement_rhs.l2_norm() << std::endl;
 
 //    try
 //    {
-////        CG_Solve.solve(elasticity_matrix_mf, distributed_displacement_sol, -1* distributed_displacement_rhs,    );
+//         // mf_gmg_preconditioner.vmult(distributed_displacement_sol, distributed_displacement_rhs);
+// //        CG_Solve.solve(elasticity_matrix_mf, distributed_displacement_sol, -1* distributed_displacement_rhs,    );
 //        CG_Solve_1.solve(elasticity_matrix_mf, distributed_displacement_sol, -1* distributed_displacement_rhs, mf_gmg_preconditioner);
 //    }
 //    catch(std::exception &exc)
 //    {
-//        std::cout << "solve failed in " << test_solver_control_1.last_step() <<  " steps" << std::endl;
-//        throw;
+//        pcout << "mfgmg diff: " << solver_control.initial_value()/solver_control.last_value() << std::endl;
 //    }
 
-//    std::cout << "solved in " << test_solver_control_1.last_step() <<  " steps" << std::endl;
+//    pcout << "mfgmg solved in " << test_solver_control_1.last_step() <<  " steps" << std::endl;
+// //    try
+// //    {
+// // //        CG_Solve.solve(elasticity_matrix_mf, distributed_displacement_sol, -1* distributed_displacement_rhs,    PreconditionIdentity());
+// //        CG_Solve_2.solve(system_matrix.block(SolutionBlocks::displacement,SolutionBlocks::displacement_multiplier), distributed_solution.block(SolutionBlocks::displacement_multiplier), system_rhs.block(SolutionBlocks::displacement), PreconditionIdentity()  );
+// //    }
+// //    catch(std::exception &exc)
+// //    {
+// //        std::cout << "solve failed in " << test_solver_control_2.last_step() <<  " steps" << std::endl;
+// //        throw;
+// //    }
 
-//    try
+// //    std::cout << "solved in " << test_solver_control_2.last_step() <<  " steps" << std::endl;
+
+
+//    }
+//    ChangeVectorTypes::copy_from_displacement_to_system_vector<double>(distributed_solution.block(SolutionBlocks::displacement), distributed_displacement_sol,displacement_to_system_dof_index_map);
+//    displacement_constraints.distribute(distributed_solution.block(SolutionBlocks::displacement));
+
+//     pcout << distributed_displacement_sol.linfty_norm() << "+++++++++++++" << std::endl;
+
+//     int a = Utilities::MPI::n_mpi_processes(mpi_communicator);
+
+//     ChangeVectorTypes::copy(distributed_solution.block(SolutionBlocks::density),active_density_vector);
+
+
+
+//     TrilinosWrappers::PreconditionAMG amg_pre;
+//     amg_pre.initialize(system_matrix.block(SolutionBlocks::displacement,SolutionBlocks::displacement_multiplier));
+//    {
+//     TimerOutput::Scope t_scope(t, "Solve_AMG");
+    
+//     SolverControl test_solver_control_2(50000, 1e-6);
+//     SolverCG<LA::MPI::Vector> CG_Solve_2(test_solver_control_2);
+
+//     distributed_solution.block(SolutionBlocks::displacement_multiplier) = 0.;
+
+//      try
 //    {
-////        CG_Solve.solve(elasticity_matrix_mf, distributed_displacement_sol, -1* distributed_displacement_rhs,    PreconditionIdentity());
-//        CG_Solve_2.solve(system_matrix.block(SolutionBlocks::displacement,SolutionBlocks::displacement_multiplier), distributed_solution.block(SolutionBlocks::displacement_multiplier), system_rhs.block(SolutionBlocks::displacement), PreconditionIdentity()  );
+//        CG_Solve_2.solve(system_matrix.block(SolutionBlocks::displacement,SolutionBlocks::displacement_multiplier), distributed_solution.block(SolutionBlocks::displacement_multiplier), system_rhs.block(SolutionBlocks::displacement), amg_pre);
 //    }
 //    catch(std::exception &exc)
 //    {
@@ -2557,16 +2649,33 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
 //        throw;
 //    }
 
-//    std::cout << "solved in " << test_solver_control_2.last_step() <<  " steps" << std::endl;
+//    std::cout << "amg solved in " << test_solver_control_2.last_step() <<  " steps" << std::endl;
 
-//    ChangeVectorTypes::copy_from_displacement_to_system_vector<double>(distributed_solution.block(SolutionBlocks::displacement), distributed_displacement_sol,displacement_to_system_dof_index_map);
-//    displacement_constraints.distribute(distributed_solution.block(SolutionBlocks::displacement));
+//    }
+//     distributed_solution.block(SolutionBlocks::displacement_multiplier)=0;
+//    {
+//     TimerOutput::Scope t_scope(t, "Solve_CG");
+//     SolverControl test_solver_control_3(50000, 1e-6);
+//     SolverCG<LA::MPI::Vector> CG_Solve_3(test_solver_control_3);
+//      try
+//    {
+//        CG_Solve_3.solve(system_matrix.block(SolutionBlocks::displacement,SolutionBlocks::displacement_multiplier), distributed_solution.block(SolutionBlocks::displacement), system_rhs.block(SolutionBlocks::displacement), PreconditionIdentity());
+//    }
+//    catch(std::exception &exc)
+//    {
+//        std::cout << "solve failed in " << test_solver_control_3.last_step() <<  " steps" << std::endl;
+//        throw;
+//    }
+
+//     std::cout << "CG solved in " << test_solver_control_3.last_step() <<  " steps" << std::endl;
+
+//    }
 
 
-//    output(distributed_solution, 0);
-//    std::abort();
+//    t.print_summary();
+//    MPI_Abort(mpi_communicator, 1);
 
-//    //***************END TEST SOLVE*************************
+//    ***************END TEST SOLVE*************************
 
 
 
@@ -2576,20 +2685,22 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
     switch (Input::solver_choice)
     {
 
-        case SolverOptions::inexact_K_with_exact_A_gmres: {
+        // case SolverOptions::inexact_K_with_exact_A_gmres: {
 
 
-            preconditioner.initialize(system_matrix, boundary_values, dof_handler, distributed_solution);
-            SolverFGMRES<LA::MPI::BlockVector> B_fgmres(solver_control);
-            B_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
-            pcout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
-            break;
-        }
+        //     preconditioner.initialize(system_matrix, boundary_values, dof_handler, distributed_solution);
+        //     pcout << "preconditioner initialized" << std::endl;
+        //     SolverFGMRES<LA::MPI::BlockVector> B_fgmres(solver_control);
+        //     B_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
+        //     pcout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
+        //     break;
+        // }
         case SolverOptions::inexact_K_with_inexact_A_gmres: {
             preconditioner.initialize(system_matrix, boundary_values, dof_handler, distributed_solution);
+            pcout << "preconditioner initialized" << std::endl;
             SolverFGMRES<LA::MPI::BlockVector> C_fgmres(solver_control);
             C_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
-            pcout << solver_control.last_step() << " steps to solve with GMRES" << std::endl;
+            pcout << solver_control.last_step() << " steps to solve with FGMRES" << std::endl;
             break;
         }
         default:
@@ -2638,8 +2749,20 @@ KktSystem<dim>::get_initial_state() {
         state.block(total_volume_multiplier).add(1);
         state.block(displacement).add(0);
         state.block(displacement_multiplier).add(0);
+        state.compress(VectorOperation::add);
+
+        // RANDOM PART HERE
+        // for(unsigned int k = 0; k<n_p; ++k)
+        // {
+        //     // std::rand(001);
+        //     // assign random values to the density
+        //     double r = std::rand()/double(RAND_MAX);
+        //     state.block(density)[k] = r;
+        //     state.block(unfiltered_density)[k] = r;
+        // }
+
     }
-    state.compress(VectorOperation::add);
+    state.compress(VectorOperation::insert);
     return state;
 }
 
diff --git a/source/main.cc b/source/main.cc
index 1a55abc..f73c0af 100644
--- a/source/main.cc
+++ b/source/main.cc
@@ -1,13 +1,30 @@
 #include "../include/watchdog.h"
+#include "../include/input_information.h"
+#include <cstdlib>
 
 ///Above are fairly normal files to include.  I also use the sparse direct package, which requiresBLAS/LAPACK
 /// to  perform  a  direct  solve  while  I  work  on  a  fast  iterative  solver  for  this problem.
 
+namespace SAND{
+    namespace Input{
+        unsigned int refinements;
+        unsigned int a_inv_iterations;
+        unsigned int k_inv_iterations;  
+    }
+}
+
+
 int
 main(int argc, char *argv[]) {
     try
     {
         Utilities::MPI::MPI_InitFinalize mpi_initialization(argc, argv, 1);
+        {
+            using namespace SAND::Input;
+            refinements = atoi(argv[1]);
+            a_inv_iterations = atoi(argv[2]);
+            k_inv_iterations = atoi(argv[3]);
+        }
         SAND::NonlinearWatchdog<SAND::Input::dim> elastic_problem;
         elastic_problem.run();
     }
diff --git a/source/matrix_free_elasticity.cc b/source/matrix_free_elasticity.cc
index 2dc93fb..0a491e7 100644
--- a/source/matrix_free_elasticity.cc
+++ b/source/matrix_free_elasticity.cc
@@ -10,9 +10,10 @@ using namespace dealii;
 ///Constructor
 template <int dim, int fe_degree, typename number>
 MF_Elasticity_Operator<dim, fe_degree, number>::MF_Elasticity_Operator()
-    : MatrixFreeOperators::Base<dim,LinearAlgebra::distributed::Vector<number>>()
+    : MatrixFreeOperators::Base<dim,LinearAlgebra::distributed::Vector<number>>(),
+    mpi_communicator(MPI_COMM_WORLD),
+    pcout(std::cout,(Utilities::MPI::this_mpi_process(mpi_communicator) == 0))
 {
-
 }
 
 ///Clears the objects, removes density information
@@ -30,14 +31,27 @@ MF_Elasticity_Operator<dim,fe_degree,number>::compute_diagonal ()
 {
     this->inverse_diagonal_entries.
     reset(new DiagonalMatrix<dealii::LinearAlgebra::distributed::Vector<number>>());
+    this->diagonal_entries.
+    reset(new DiagonalMatrix<dealii::LinearAlgebra::distributed::Vector<number>>());
+
     dealii::LinearAlgebra::distributed::Vector<number> &inverse_diagonal = this->inverse_diagonal_entries->get_vector();
+
+    dealii::LinearAlgebra::distributed::Vector<number> &diagonal = this->diagonal_entries->get_vector();
+
     this->data->initialize_dof_vector(inverse_diagonal);
-    unsigned int dummy = 0;
+    this->data->initialize_dof_vector(diagonal);
+    unsigned int dummy = 1;
+
+
     this->data->cell_loop (&MF_Elasticity_Operator::local_compute_diagonal, this,
-                           inverse_diagonal, dummy);
+                           diagonal, dummy);
+
+    this->set_constrained_entries_to_one(diagonal);
 
-    this->set_constrained_entries_to_one(inverse_diagonal);
+    // diagonal.compress(VectorOperation::add);
 
+    inverse_diagonal=diagonal;
+    
     for (auto &local_element : inverse_diagonal)
       {
 //        Assert(local_element > 0.,
@@ -45,6 +59,9 @@ MF_Elasticity_Operator<dim,fe_degree,number>::compute_diagonal ()
 //                          "should be zero or negative."));
         local_element = 1./local_element;
       }
+      // inverse_diagonal.compress(VectorOperation::insert);
+    //   diagona.print(lstd::cout);
+      pcout << "diag size: " << diagonal.size() << " with l2 norm " << diagonal.l2_norm() << std::endl;
 }
 
 ///Computes the diagonal value locally for a cell
@@ -56,10 +73,11 @@ ::local_compute_diagonal (const MatrixFree<dim,number>                     &data
                           const unsigned int &,
                           const std::pair<unsigned int,unsigned int>       &cell_range) const
 {
+  // pcout << "local_compute_diagonal" << std::endl;
   FEEvaluation<dim,fe_degree,fe_degree+1,dim,number> displacement (data, 0);
 
   AlignedVector<VectorizedArray<number>> diagonal(displacement.dofs_per_cell);
-
+  
   for (unsigned int cell=cell_range.first; cell<cell_range.second; ++cell)
     {
 
@@ -95,10 +113,11 @@ ::local_compute_diagonal (const MatrixFree<dim,number>                     &data
 
           diagonal[i] = displacement.begin_dof_values()[i];
         }
-
+      
       for (unsigned int i=0; i<displacement.dofs_per_cell; ++i)
         displacement.begin_dof_values()[i] = diagonal[i];
-      displacement.distribute_local_to_global (dst);
+      displacement.distribute_local_to_global (dst);     
+
     }
 }
 
@@ -111,7 +130,8 @@ MF_Elasticity_Operator<dim, fe_degree, number>::local_apply(
         const LinearAlgebra::distributed::Vector<number> &src,
         const std::pair<unsigned int, unsigned int> &     cell_range) const
 {
-    FEEvaluation<dim, 1, 2, dim, double> displacement(data);
+  // pcout << "local_apply" << std::endl;
+    FEEvaluation<dim, 1, 2, dim, double> displacement(data,0);
 
     for (unsigned int cell=cell_range.first; cell<cell_range.second; ++cell)
     {
@@ -133,13 +153,13 @@ MF_Elasticity_Operator<dim, fe_degree, number>::local_apply(
                 symgrad_term[d][d] +=  div_term;
             }
 
+            
 
             displacement.submit_symmetric_gradient(symgrad_term, q);
-
         }
-
         displacement.integrate_scatter(EvaluationFlags::gradients, dst);
     }
+
 }
 
 ///Nothing is applied on a face on the LHS, so left blank.
@@ -151,7 +171,6 @@ ::local_apply_face(const dealii::MatrixFree<dim, number> &,
                    const dealii::LinearAlgebra::distributed::Vector<number> &,
                    const std::pair<unsigned int, unsigned int> &) const
 {
-
 }
 
 ///Nothing is applied on a face on the LHS, so left blank.
@@ -163,7 +182,6 @@ ::local_apply_boundary_face(const dealii::MatrixFree<dim, number> &,
                             const dealii::LinearAlgebra::distributed::Vector<number> &,
                             const std::pair<unsigned int, unsigned int> &) const
 {
-
 }
 
 ///Loops over all cells to apply the elasticity operatorto the entire LHS vector
@@ -175,6 +193,7 @@ ::apply_add (dealii::LinearAlgebra::distributed::Vector<number> &dst,
 {
     MatrixFreeOperators::Base<dim, dealii::LinearAlgebra::distributed::Vector<number>>::
             data->cell_loop(&MF_Elasticity_Operator::local_apply, this, dst, src);
+    // dst.compress(VectorOperation::add);
 }
 
 ///Sets cell data (density) to be input given.
@@ -182,6 +201,7 @@ template <int dim, int fe_degree, typename number>
 void
 MF_Elasticity_Operator<dim,fe_degree,number>::set_cell_data (const OperatorCellData<dim,number> &data)
 {
+  pcout << "set_cell_data" << std::endl;
     this->cell_data = &data;
 }
 
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 6c4706c..893412b 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -12,6 +12,12 @@
 #include "../include/schur_preconditioner.h"
 #include "../include/input_information.h"
 #include "../include/sand_tools.h"
+#include <deal.II/base/config.h>
+#include <deal.II/base/array_view.h>
+#include <deal.II/base/index_set.h>
+#include <deal.II/base/mpi_tags.h>
+#include <deal.II/base/numbers.h>
+#include <deal.II/base/template_constraints.h>
 #include <fstream>
 
 namespace SAND {
@@ -78,6 +84,51 @@ void copy(dealii::LinearAlgebra::distributed::Vector<number> &out,
 
 using namespace dealii;
 
+
+    template<int dim>
+    PolyPreJ<dim>::PolyPreJ(const JinvMatrixPart<dim> &inner_matrix_in, const int degree_in):
+    degree(degree_in),
+    inner_matrix(inner_matrix_in)
+    {
+    }
+
+    template<int dim>
+    void
+    PolyPreJ<dim>::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+    {
+        LA::MPI::Vector temp_vec_1 = src;
+        LA::MPI::Vector temp_vec_2 = src;
+        for(int k=0; k<degree; k++)
+        {
+            temp_vec_2 = 0;
+            inner_matrix.vmult(temp_vec_2,temp_vec_1);
+            temp_vec_1 = temp_vec_2 + src;
+        }
+        dst = temp_vec_1;
+    }
+
+    template<int dim>
+    PolyPreK<dim>::PolyPreK(const KinvMatrixPart<dim> &inner_matrix_in, const int degree_in):
+    degree(degree_in),
+    inner_matrix(inner_matrix_in)
+    {
+    }
+
+    template<int dim>
+    void
+    PolyPreK<dim>::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+    {
+        LA::MPI::Vector temp_vec_1 = src;
+        LA::MPI::Vector temp_vec_2 = src;
+        for(int k=0; k<degree; k++)
+        {
+            temp_vec_2 = 0;
+            inner_matrix.vmult(temp_vec_2,temp_vec_1);
+            temp_vec_1 = temp_vec_2 + src;
+        }
+        dst = temp_vec_1;
+    }
+
 ///Constructor... kinda big due to many references to matrices
 template<int dim>
 TopOptSchurPreconditioner<dim>::TopOptSchurPreconditioner(LA::MPI::BlockSparseMatrix &matrix_in, DoFHandler<dim> &big_dof_handler_in, MF_Elasticity_Operator<dim,1,double> &mf_elasticity_operator_in , PreconditionMG<dim,LinearAlgebra::distributed::Vector<double> ,MGTransferMatrixFree<dim, double>>
@@ -113,8 +164,11 @@ TopOptSchurPreconditioner<dim>::TopOptSchurPreconditioner(LA::MPI::BlockSparseMa
       h_mat(a_mat, b_mat, c_mat, e_mat, pre_amg, a_inv_direct, a_inv_mf_gmg),
       j_inv_mat(h_mat, g_mat, d_m_mat, d_m_inv_mat),
       k_inv_mat(h_mat, g_mat, d_m_mat, d_m_inv_mat),
+      j_inv_part(h_mat, g_mat, d_m_mat, d_m_inv_mat),
+      k_inv_part(h_mat, g_mat, d_m_mat, d_m_inv_mat),
       big_dof_handler(big_dof_handler_in)
 {
+  
 
 }
 
@@ -156,6 +210,7 @@ void TopOptSchurPreconditioner<dim>::initialize(LA::MPI::BlockSparseMatrix &matr
             }
         }
 
+
         //set diagonal to 0?
         for (auto &pair: boundary_values) {
             const auto dof_index=pair.first;
@@ -175,34 +230,10 @@ void TopOptSchurPreconditioner<dim>::initialize(LA::MPI::BlockSparseMatrix &matr
             }
         }
 
+
         system_matrix.compress(VectorOperation::insert);
     }
 
-
-
-    //        if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
-    //        {
-    //            TimerOutput::Scope t(timer, "build A inv");
-
-    //             std::vector<std::vector<bool>> constant_modes;
-    //             FEValuesExtractors::Vector     displacement_components(SolutionComponents::displacement<dim>);
-    //             DoFTools::extract_constant_modes(dof_handler,
-    //                                              dof_handler.get_fe_collection()
-    //                                              .component_mask(displacement_components),
-    //                                              constant_modes);
-    //             TrilinosWrappers::PreconditionAMG::AdditionalData amg_data;
-    //             amg_data.constant_modes = constant_modes;
-    ////             amg_data.n_cycles = 1;
-    ////             amg_data.w_cycle = true;
-
-    //             pre_amg.initialize(a_mat,amg_data);
-
-    //        }
-    //        else
-    //        {
-    //             a_inv_direct.initialize(a_mat);
-    //        }
-    a_inv_direct.initialize(a_mat);
     {
         TimerOutput::Scope t(timer, "reinit diag matrices");
         d_3_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
@@ -212,6 +243,8 @@ void TopOptSchurPreconditioner<dim>::initialize(LA::MPI::BlockSparseMatrix &matr
         d_7_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
         d_8_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
         d_m_inv_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
+
+
         d_3_mat=0;
         d_4_mat=0;
         d_5_mat=0;
@@ -312,6 +345,8 @@ void TopOptSchurPreconditioner<dim>::initialize(LA::MPI::BlockSparseMatrix &matr
     h_mat.initialize(density_exemplar, displacement_exemplar);
     j_inv_mat.initialize(density_exemplar);
     k_inv_mat.initialize(density_exemplar);
+    // j_inv_part.initialize(density_exemplar);
+    // k_inv_part.initialize(density_exemplar);
 
     num_mults = 0;
 }
@@ -325,26 +360,30 @@ void TopOptSchurPreconditioner<dim>::vmult(LA::MPI::BlockVector &dst, const LA::
         TimerOutput::Scope t(timer, "part 1");
         vmult_step_1(dst, src);
         temp_src = dst;
+        pcout << "step 1 done" << std::endl;
     }
 
     {
         TimerOutput::Scope t(timer, "part 2");
         vmult_step_2(dst, temp_src);
         temp_src = dst;
-
+        pcout << "step 2 done" << std::endl;
     }
 
     {
         TimerOutput::Scope t(timer, "part 3");
         vmult_step_3(dst, temp_src);
         temp_src = dst;
+        pcout << "step 3 done" << std::endl;
     }
     {
         TimerOutput::Scope t(timer, "part 4");
         vmult_step_4(dst, temp_src);
         temp_src = dst;
+        pcout << "step 4 done" << std::endl;
     }
     vmult_step_5(dst, temp_src);
+    pcout << "step 5 done" << std::endl;
     num_mults++;
 
 
@@ -504,10 +543,15 @@ void TopOptSchurPreconditioner<dim>::vmult_step_3(LA::MPI::BlockVector &dst, con
         //            SolverCG<LA::MPI::Vector> a_solver_cg_2(solver_control_2);
         //            a_solver_cg_1.solve(a_mat,dst_temp.block(SolutionBlocks::displacement_multiplier),src.block(SolutionBlocks::displacement_multiplier),pre_amg);
         //            a_solver_cg_2.solve(a_mat,dst_temp.block(SolutionBlocks::displacement),src.block(SolutionBlocks::displacement),pre_amg);
+        a_inv_mf_gmg.set_tol(0);
+        a_inv_mf_gmg.set_iter(30);
 
         a_inv_mf_gmg.vmult(dst_temp.block(SolutionBlocks::displacement_multiplier),src.block(SolutionBlocks::displacement_multiplier));
-
         a_inv_mf_gmg.vmult(dst_temp.block(SolutionBlocks::displacement),src.block(SolutionBlocks::displacement));
+
+        a_inv_mf_gmg.set_tol(Input::a_rel_tol);
+        a_inv_mf_gmg.set_iter(Input::a_inv_iterations);
+
         c_mat.Tvmult(dst_temp.block(SolutionBlocks::density_upper_slack),dst_temp.block(SolutionBlocks::displacement_multiplier));
         e_mat.Tvmult(dst_temp.block(SolutionBlocks::density_lower_slack),dst_temp.block(SolutionBlocks::displacement));
 
@@ -536,30 +580,30 @@ void TopOptSchurPreconditioner<dim>::vmult_step_4(LA::MPI::BlockVector &dst, con
     TrilinosWrappers::PreconditionIdentity preconditioner;
     preconditioner.initialize(b_mat);
 
+    // PolyPreK<dim> poly_pre_k(k_inv_part,0);
+    // PolyPreJ<dim> poly_pre_j(j_inv_part,0);
+
     auto d_m_inv_density = g_d_m_inv_density;
     d_m_inv_mat.vmult(d_m_inv_density,src.block(SolutionBlocks::density));
     g_mat.vmult(g_d_m_inv_density,d_m_inv_density);
-    SolverControl step_4_gmres_control_1 (Input::k_inv_iterations, g_d_m_inv_density.l2_norm()*1e-12);
-    SolverFGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
+    SolverControl step_4_gmres_control_1 (Input::k_inv_iterations, g_d_m_inv_density.l2_norm()*Input::k_rel_tol);
+    SolverGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
     try {
-        step_4_gmres_1.solve(k_inv_mat,k_g_d_m_inv_density,g_d_m_inv_density, preconditioner );
+        step_4_gmres_1.solve(k_inv_mat,k_g_d_m_inv_density,g_d_m_inv_density, PreconditionIdentity() );
     }
     catch (std::exception &exc)
     {
-        pcout << "Failure of linear solver step_4_gmres_1" << std::endl;
-        pcout << "first residual: " << step_4_gmres_control_1.initial_value() << std::endl;
-        pcout << "last residual: " << step_4_gmres_control_1.last_value() << std::endl;
+        pcout << "Failure of linear solver 4-1 with convergence of " << step_4_gmres_control_1.last_value()/step_4_gmres_control_1.initial_value()<< std::endl;
         //                throw;
     }
-    SolverControl step_4_gmres_control_2 (Input::k_inv_iterations, 1e-12 * src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm() );
-    SolverFGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
+    SolverControl step_4_gmres_control_2 (Input::k_inv_iterations, src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*Input::k_rel_tol);
+    SolverGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
     try {
         step_4_gmres_2.solve(k_inv_mat,k_density_mult,src.block(SolutionBlocks::unfiltered_density_multiplier), PreconditionIdentity());
     } catch (std::exception &exc)
     {
-        pcout << "Failure of linear solver step_4_gmres_2" << std::endl;
-        pcout << "first residual: " << step_4_gmres_control_2.initial_value() << std::endl;
-        pcout << "last residual: " << step_4_gmres_control_2.last_value() << std::endl;
+
+        pcout << "Failure of linear solver 4-2 with convergence of " << step_4_gmres_control_2.last_value()/step_4_gmres_control_2.initial_value()<< std::endl;
         //                throw;
     }
 
@@ -569,8 +613,6 @@ void TopOptSchurPreconditioner<dim>::vmult_step_4(LA::MPI::BlockVector &dst, con
             +dst_temp.block(SolutionBlocks::total_volume_multiplier);
 }
 
-
-
 ///The only non-triangular vmult step, applies inverses down the block diagonal.
 template<int dim>
 void TopOptSchurPreconditioner<dim>::vmult_step_5(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
@@ -608,8 +650,13 @@ void TopOptSchurPreconditioner<dim>::vmult_step_5(LA::MPI::BlockVector &dst, con
             //                dst.block(SolutionBlocks::displacement) = a_inv_op * src.block(SolutionBlocks::displacement_multiplier);
             //                dst.block(SolutionBlocks::displacement_multiplier) = a_inv_op * src.block(SolutionBlocks::displacement);
 
+            a_inv_mf_gmg.set_tol(0);
+            a_inv_mf_gmg.set_iter(30);
             a_inv_mf_gmg.vmult( dst_temp.block(SolutionBlocks::displacement), src.block(SolutionBlocks::displacement_multiplier));
             a_inv_mf_gmg.vmult( dst_temp.block(SolutionBlocks::displacement_multiplier), src.block(SolutionBlocks::displacement));
+            a_inv_mf_gmg.set_tol(0);
+            a_inv_mf_gmg.set_iter(Input::a_inv_iterations);
+
             dst.block(SolutionBlocks::displacement) = -1 * dst_temp.block(SolutionBlocks::displacement);
             dst.block(SolutionBlocks::displacement_multiplier) = -1 * dst_temp.block(SolutionBlocks::displacement_multiplier);
         }
@@ -638,8 +685,8 @@ void TopOptSchurPreconditioner<dim>::vmult_step_5(LA::MPI::BlockVector &dst, con
         else if (Input::solver_choice == SolverOptions::inexact_K_with_inexact_A_gmres)
         {
 
-            TrilinosWrappers::PreconditionIdentity preconditioner;
-            preconditioner.initialize(b_mat);
+            // TrilinosWrappers::PreconditionIdentity preconditioner;
+            // preconditioner.initialize(b_mat);
             auto pre_pre_k = pre_k;
             auto pre_pre_pre_k = pre_k;
             auto d_m_inv_unfil_density_mult = pre_k;
@@ -654,38 +701,34 @@ void TopOptSchurPreconditioner<dim>::vmult_step_5(LA::MPI::BlockVector &dst, con
                 g_mat.vmult(pre_pre_k,pre_pre_pre_k);
                 pre_k =  -1 * pre_pre_k + src.block(SolutionBlocks::unfiltered_density_multiplier);
             }
-            SolverControl step_5_gmres_control_1 (Input::k_inv_iterations, 1e-12*pre_j.l2_norm());
-            SolverFGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
+            SolverControl step_5_gmres_control_1 (Input::k_inv_iterations, Input::k_rel_tol*pre_j.l2_norm());
+            SolverGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
             try {
                 TimerOutput::Scope t(timer, "actual inverse 5.1");
-                step_5_gmres_1.solve(j_inv_mat, dst.block(SolutionBlocks::unfiltered_density_multiplier), pre_j , preconditioner);
+                step_5_gmres_1.solve(j_inv_mat, dst.block(SolutionBlocks::unfiltered_density_multiplier), pre_j , PreconditionIdentity());
             } catch (std::exception &exc)
             {
-                pcout << "Failure of linear solver step_5_gmres_1" << std::endl;
-                pcout << "first residual: " << step_5_gmres_control_1.initial_value() << std::endl;
-                pcout << "last residual: " << step_5_gmres_control_1.last_value() << std::endl;
+                pcout << "Failure of linear solver 5-1 with convergence of " << step_5_gmres_control_1.last_value()/step_5_gmres_control_1.initial_value()<< std::endl;
                 //                    throw;
             }
 
 
-            SolverControl step_5_gmres_control_2 (Input::k_inv_iterations, 1e-12*pre_k.l2_norm());
-            SolverFGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
+            SolverControl step_5_gmres_control_2 (Input::k_inv_iterations, Input::k_rel_tol*pre_k.l2_norm());
+            SolverGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
             try {
                 TimerOutput::Scope t(timer, "actual inverse 5.2");
-                step_5_gmres_2.solve(k_inv_mat,dst.block(SolutionBlocks::density), pre_k , preconditioner);
+                step_5_gmres_2.solve(k_inv_mat,dst.block(SolutionBlocks::density), pre_k , PreconditionIdentity());
             } catch (std::exception &exc)
             {
-                pcout << "Failure of linear solver step_5_gmres_2" << std::endl;
-                pcout << "first residual: " << step_5_gmres_control_2.initial_value() << std::endl;
-                pcout << "last residual: " << step_5_gmres_control_2.last_value() << std::endl;
+                pcout << "Failure of linear solver 5-2 with convergence of " << step_5_gmres_control_2.last_value()/step_5_gmres_control_2.initial_value()<< std::endl;
                 //                    throw;
             }
         }
         else if (Input::solver_choice == SolverOptions::inexact_K_with_exact_A_gmres)
         {
 
-            TrilinosWrappers::PreconditionIdentity preconditioner;
-            preconditioner.initialize(b_mat);
+            // TrilinosWrappers::PreconditionIdentity preconditioner;
+            // preconditioner.initialize(b_mat);
             auto pre_pre_k = pre_k;
             auto pre_pre_pre_k = pre_k;
             auto d_m_inv_unfil_density_mult = pre_k;
@@ -701,10 +744,10 @@ void TopOptSchurPreconditioner<dim>::vmult_step_5(LA::MPI::BlockVector &dst, con
                 pre_k =  -1 * pre_pre_k + src.block(SolutionBlocks::unfiltered_density_multiplier);
             }
             SolverControl step_5_gmres_control_1 (Input::k_inv_iterations, 1e-12*pre_j.l2_norm());
-            SolverFGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
+            SolverGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
             try {
                 TimerOutput::Scope t(timer, "actual inverse 5.1");
-                step_5_gmres_1.solve(j_inv_mat, dst.block(SolutionBlocks::unfiltered_density_multiplier), pre_j , preconditioner);
+                step_5_gmres_1.solve(j_inv_mat, dst.block(SolutionBlocks::unfiltered_density_multiplier), pre_j , PreconditionIdentity());
             } catch (std::exception &exc)
             {
                 pcout << "Failure of linear solver step_5_gmres_1" << std::endl;
@@ -714,11 +757,11 @@ void TopOptSchurPreconditioner<dim>::vmult_step_5(LA::MPI::BlockVector &dst, con
             }
 
 
-            SolverControl step_5_gmres_control_2 (Input::k_inv_iterations, 1e-12*pre_k.l2_norm());
-            SolverFGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
+            SolverControl step_5_gmres_control_2 (Input::k_inv_iterations, Input::k_rel_tol*pre_k.l2_norm());
+            SolverGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
             try {
                 TimerOutput::Scope t(timer, "actual inverse 5.2");
-                step_5_gmres_2.solve(k_inv_mat,dst.block(SolutionBlocks::density), pre_k , preconditioner);
+                step_5_gmres_2.solve(k_inv_mat,dst.block(SolutionBlocks::density), pre_k , PreconditionIdentity());
             } catch (std::exception &exc)
             {
                 pcout << "Failure of linear solver step_5_gmres_2" << std::endl;
@@ -813,7 +856,7 @@ template<int dim>
 void AInvMatMFGMG<dim>::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
 {
 
-    SolverControl            solver_control(50, std::max(src.l2_norm()*1e-20,1e-20));
+    SolverControl            solver_control(iterations, src.l2_norm()*tolerance);
     SolverCG<LinearAlgebra::distributed::Vector<double>> a_solver_cg(solver_control);
 
     ChangeVectorTypes::copy_from_system_to_displacement_vector<double>(temp_src, src, displacement_to_system_dof_index_map);
@@ -822,7 +865,7 @@ void AInvMatMFGMG<dim>::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src)
     }
     catch (std::exception &exc)
     {
-        std::cout << "failed with a reduction of: " << solver_control.initial_value()/solver_control.last_value() << std::endl;
+        // std::cout << "failed with a reduction of: " << solver_control.initial_value()/solver_control.last_value() << std::endl;
     }
 
     ChangeVectorTypes::copy_from_displacement_to_system_vector<double>(dst,temp_dst, displacement_to_system_dof_index_map);
@@ -833,7 +876,7 @@ void AInvMatMFGMG<dim>::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src)
 template<int dim>
 void AInvMatMFGMG<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
 {
-    SolverControl            solver_control(50, std::max(src.l2_norm()*1e-20,1e-20));
+    SolverControl            solver_control(iterations, src.l2_norm()*tolerance);
     SolverCG<LinearAlgebra::distributed::Vector<double>> a_solver_cg(solver_control);
 
     ChangeVectorTypes::copy_from_system_to_displacement_vector<double>(temp_src, src, displacement_to_system_dof_index_map);
@@ -936,7 +979,6 @@ void HMatrix<dim>::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
 
         try
         {
-            //                a_solver_cg_1.solve(a_mat,temp_vect_3,temp_vect_1,pre_amg);
             a_inv_mf_gmg.vmult(temp_vect_3,temp_vect_1);
         } catch (std::exception &exc)
         {
@@ -946,7 +988,6 @@ void HMatrix<dim>::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
         SolverCG<LA::MPI::Vector> a_solver_cg_2(solver_control_2);
         try
         {
-            //                a_solver_cg_2.solve(a_mat,temp_vect_4,temp_vect_2,pre_amg);
             a_inv_mf_gmg.vmult(temp_vect_4,temp_vect_2);
         } catch (std::exception &exc)
         {
@@ -987,12 +1028,11 @@ void HMatrix<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) cons
         c_mat.vmult(temp_vect_1,src);
         e_mat.vmult(temp_vect_2,src);
 
-        SolverControl            solver_control_1(Input::a_inv_iterations, temp_vect_1.l2_norm()*1e-6);
-        SolverCG<LA::MPI::Vector> a_solver_cg_1(solver_control_1);
+        // SolverControl            solver_control_1(Input::a_inv_iterations, temp_vect_1.l2_norm()*1e-6);
+        // SolverCG<LA::MPI::Vector> a_solver_cg_1(solver_control_1);
 
         try
         {
-            //                a_solver_cg_1.solve(a_mat,temp_vect_3,temp_vect_1,pre_amg);
             a_inv_mf_gmg.vmult(temp_vect_3,temp_vect_1);
 
         } catch (std::exception &exc)
@@ -1003,7 +1043,6 @@ void HMatrix<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) cons
         SolverCG<LA::MPI::Vector> a_solver_cg_2(solver_control_2);
         try
         {
-            //                a_solver_cg_2.solve(a_mat,temp_vect_4,temp_vect_2,pre_amg);
             a_inv_mf_gmg.vmult(temp_vect_4,temp_vect_2);
 
         } catch (std::exception &exc)
@@ -1151,6 +1190,48 @@ void JinvMatrix<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) c
     dst = -1*temp_vect_4 - temp_vect_3;
 }
 
+// ******************     JinvMatrixPart     ***********************
+template<int dim>
+JinvMatrixPart<dim>::JinvMatrixPart(HMatrix<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in)
+    :
+      h_mat(h_mat_in),
+      g_mat(g_mat_in),
+      d_m_mat(d_m_mat_in),
+      d_m_inv_mat(d_m_inv_mat_in)
+{
+}
+
+template<int dim>
+void
+JinvMatrixPart<dim>::initialize(LA::MPI::Vector &exemplar_density_vector)
+{
+    temp_vect_1 = exemplar_density_vector;
+    temp_vect_2 = exemplar_density_vector;
+    temp_vect_3 = exemplar_density_vector;
+    temp_vect_4 = exemplar_density_vector;
+
+}
+
+template<int dim>
+void JinvMatrixPart<dim>::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+{
+    g_mat.vmult(temp_vect_1,src);
+    d_m_inv_mat.vmult(temp_vect_2,temp_vect_1);
+    h_mat.vmult(temp_vect_3,temp_vect_2);
+    d_m_inv_mat.vmult(temp_vect_4,temp_vect_3);
+    dst = -1 * temp_vect_4;
+}
+
+template<int dim>
+void JinvMatrixPart<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+{
+    h_mat.vmult(temp_vect_1,src);
+    d_m_inv_mat.vmult(temp_vect_2,temp_vect_1);
+    g_mat.vmult(temp_vect_3,temp_vect_2);
+    d_m_inv_mat.vmult(temp_vect_4,temp_vect_3);
+    dst = -1 * temp_vect_4;
+}
+
 // ******************     JinvMatrixDirect     ***********************
 template<int dim>
 JinvMatrixDirect<dim>::JinvMatrixDirect(HMatrixDirect<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in)
@@ -1242,6 +1323,59 @@ void KinvMatrix<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) c
     dst = -1*temp_vect_4 - temp_vect_3;
 }
 
+// ******************     KinvMatrixPart    ***********************
+template<int dim>
+KinvMatrixPart<dim>::KinvMatrixPart(HMatrix<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in)
+    :
+      h_mat(h_mat_in),
+      g_mat(g_mat_in),
+      d_m_mat(d_m_mat_in),
+      d_m_inv_mat(d_m_inv_mat_in)
+{
+
+}
+
+template<int dim>
+void
+KinvMatrixPart<dim>::initialize(LA::MPI::Vector &exemplar_density_vector)
+{
+    temp_vect_1 = exemplar_density_vector;
+    temp_vect_2 = exemplar_density_vector;
+    temp_vect_3 = exemplar_density_vector;
+    temp_vect_4 = exemplar_density_vector;
+
+}
+
+template<int dim>
+void KinvMatrixPart<dim>::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+{
+
+    h_mat.vmult(temp_vect_1,src);
+    d_m_inv_mat.vmult(temp_vect_2,temp_vect_1);
+    g_mat.vmult(temp_vect_3,temp_vect_2);
+    d_m_inv_mat.vmult(temp_vect_4,temp_vect_3);
+
+    dst = temp_vect_4;
+    //kinv is -Dm-GDmH
+    //kinv scaled would be I+DminvGDminvH
+    //kpart is -DminvGDmH
+
+}
+
+template<int dim>
+void KinvMatrixPart<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
+{
+    g_mat.vmult(temp_vect_1,src);
+    d_m_inv_mat.vmult(temp_vect_2,temp_vect_1);
+    h_mat.vmult(temp_vect_3,temp_vect_2);
+    d_m_inv_mat.vmult(temp_vect_4,temp_vect_3);
+
+    dst = temp_vect_4;
+    //kinv is -Dm-GDmH
+    //kinv scaled would be I+DminvGDminvH
+    //kpart is -DminvGDmH
+}
+
 // ******************     KinvMatrixDirect     ***********************
 template<int dim>
 KinvMatrixDirect<dim>::KinvMatrixDirect(HMatrixDirect<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in)
diff --git a/source/watchdog.cc b/source/watchdog.cc
index 6a67634..8ba8657 100644
--- a/source/watchdog.cc
+++ b/source/watchdog.cc
@@ -462,7 +462,7 @@ namespace SAND {
 
         markov_filter.setup(kkt_system.calculate_objective_value(current_state), kkt_system.calculate_barrier_distance(current_state), kkt_system.calculate_feasibility(current_state,barrier_size), barrier_size);
 
-        std::cout << "finished setup - beginning watchdog steps" << std::endl;
+        // std::cout << "finished setup - beginning watchdog steps" << std::endl;
 
         while((barrier_size > Input::min_barrier_size || !check_convergence(current_state)) && iteration_number < Input::max_steps)
         {

From 72541f3ee8ef52324c8ca41a79d4aff09096c7ed Mon Sep 17 00:00:00 2001
From: Justin O'Connor <oconnor@up.math.colostate.edu>
Date: Wed, 1 Mar 2023 13:45:20 -0700
Subject: [PATCH 95/95]  Ready for release

---
 include/density_filter.h         |   1 +
 include/input_information.h      |  14 +-
 include/schur_preconditioner.h   |  45 +++--
 source/density_filter.cc         |  75 +++++--
 source/kkt_system.cc             | 225 +++++++++++++--------
 source/matrix_free_elasticity.cc |   9 +-
 source/poly_pre.cc               |   7 +
 source/schur_preconditioner.cc   | 333 ++++++++++++++-----------------
 8 files changed, 392 insertions(+), 317 deletions(-)
 create mode 100644 source/poly_pre.cc

diff --git a/include/density_filter.h b/include/density_filter.h
index cad75bf..51daba2 100644
--- a/include/density_filter.h
+++ b/include/density_filter.h
@@ -65,6 +65,7 @@ namespace SAND {
         DensityFilter();
         DynamicSparsityPattern filter_dsp;
         LA::MPI::SparseMatrix filter_matrix;
+        LA::MPI::SparseMatrix filter_matrix_transpose;
         SparsityPattern filter_sparsity_pattern;
         void initialize(DoFHandler<dim> &dof_handler);
         std::set<types::global_dof_index> find_relevant_neighbors(types::global_dof_index cell_index) const;
diff --git a/include/input_information.h b/include/input_information.h
index 8d020cf..8e95b17 100644
--- a/include/input_information.h
+++ b/include/input_information.h
@@ -23,10 +23,10 @@ namespace SAND {
 
         //nonlinear algorithm options
         constexpr double initial_barrier_size = 25;
-        constexpr double min_barrier_size = 0;
+        constexpr double min_barrier_size = 1e-12;
 
-        constexpr double fraction_to_boundary = .9;
-        constexpr unsigned int max_steps=2;
+        constexpr double fraction_to_boundary = .7;
+        constexpr unsigned int max_steps=100;
 
         constexpr unsigned int barrier_reduction=BarrierOptions::loqo;
         constexpr double required_norm = .0001;
@@ -53,12 +53,12 @@ namespace SAND {
         // constexpr double a_inv_iterations = 5;
         // constexpr double k_inv_iterations = 30;
 
-        constexpr double a_rel_tol = 0.0;
-        constexpr double k_rel_tol = 0.0;
+        constexpr double a_rel_tol = 1e-12;
+        constexpr double k_rel_tol = 1e-12;
 
         //Material Options
-        constexpr double material_lambda = 1;
-        constexpr double material_mu = 1;
+        constexpr double material_lambda = 1.;
+        constexpr double material_mu = 1.;
     }
 }
 #endif //SAND_INPUT_INFORMATION_H
\ No newline at end of file
diff --git a/include/schur_preconditioner.h b/include/schur_preconditioner.h
index f3b8aed..8f06feb 100644
--- a/include/schur_preconditioner.h
+++ b/include/schur_preconditioner.h
@@ -137,13 +137,14 @@ namespace SAND
             {
                 a_mat_wrapped.set_exemplar_vector(exemplar_vector);
             }
+            double tolerance = Input::a_rel_tol;
+            unsigned int iterations = Input::a_inv_iterations;
         private:
             AMatWrapped a_mat_wrapped;
             MF_Elasticity_Operator<dim,1,double> &mf_elasticity_operator;
             PreconditionMG<dim,LinearAlgebra::distributed::Vector<double>,MGTransferMatrixFree<dim, double>> &mf_gmg_preconditioner;
             const std::map<types::global_dof_index,types::global_dof_index> displacement_to_system_dof_index_map;
-            double tolerance = Input::a_rel_tol;
-            unsigned int iterations = Input::a_inv_iterations;
+
             mutable dealii::LinearAlgebra::distributed::Vector<double> temp_src;
             mutable dealii::LinearAlgebra::distributed::Vector<double> temp_dst;
 
@@ -152,15 +153,17 @@ namespace SAND
 
     class GMatrix : public TrilinosWrappers::SparseMatrix {
         public:
-            GMatrix(const LA::MPI::SparseMatrix &f_mat_in, LA::MPI::SparseMatrix &d_8_mat_in);
+            GMatrix(const LA::MPI::SparseMatrix &f_mat_in,const LA::MPI::SparseMatrix &f_t_mat_in, LA::MPI::SparseMatrix &d_8_mat_in);
             void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
             void Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
-            void initialize(LA::MPI::Vector &exemplar_density_vector);
+            void initialize(LA::MPI::Vector &exemplar_density_vector, LA::MPI::SparseMatrix &d_m_inv_mat_in);
             unsigned int m() const;
             unsigned int n() const;
         private:
             const LA::MPI::SparseMatrix &f_mat;
+            const LA::MPI::SparseMatrix &f_t_mat;
             LA::MPI::SparseMatrix &d_8_mat;
+            LA::MPI::SparseMatrix d_m_inv_mat;
             mutable LA::MPI::Vector temp_vect_1;
             mutable LA::MPI::Vector temp_vect_2;
 
@@ -173,7 +176,7 @@ namespace SAND
             HMatrix(LA::MPI::SparseMatrix &a_mat_in, const LA::MPI::SparseMatrix &b_mat_in, const LA::MPI::SparseMatrix &c_mat_in, const LA::MPI::SparseMatrix &e_mat_in,TrilinosWrappers::PreconditionAMG &pre_amg_in, VmultTrilinosSolverDirect &a_inv_direct_in, AInvMatMFGMG<dim> &a_inv_mf_gmg_in);
             void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
             void Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
-            void initialize(LA::MPI::Vector &exemplar_density_vector,  LA::MPI::Vector &exemplar_displacement_vector);
+            void initialize(LA::MPI::Vector &exemplar_density_vector,  LA::MPI::Vector &exemplar_displacement_vector, LA::MPI::SparseMatrix &d_m_inv_mat_in);
             unsigned int m() const;
             unsigned int n() const;
         private:
@@ -181,6 +184,7 @@ namespace SAND
             const LA::MPI::SparseMatrix &b_mat;
             const LA::MPI::SparseMatrix &c_mat;
             const LA::MPI::SparseMatrix &e_mat;
+            LA::MPI::SparseMatrix d_m_inv_mat;
             TrilinosWrappers::PreconditionAMG &pre_amg;
             VmultTrilinosSolverDirect &a_inv_direct;
             mutable LA::MPI::Vector temp_vect_1;
@@ -224,17 +228,17 @@ namespace SAND
     template<int dim>
     class KinvMatrix : public TrilinosWrappers::SparseMatrix {
         public:
-            KinvMatrix(HMatrix<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in);
+            KinvMatrix(HMatrix<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in);
             void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
             void Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
-            void initialize(LA::MPI::Vector &exemplar_density_vector);
+            void initialize(LA::MPI::Vector &exemplar_density_vector, LA::MPI::SparseMatrix &d_m_inv_mat_in);
             unsigned int m() const;
             unsigned int n() const;
         private:
             HMatrix<dim> &h_mat;
             GMatrix &g_mat;
             const LA::MPI::SparseMatrix &d_m_mat;
-            LA::MPI::SparseMatrix &d_m_inv_mat;
+            LA::MPI::SparseMatrix d_m_inv_mat;
             mutable LA::MPI::Vector temp_vect_1;
             mutable LA::MPI::Vector temp_vect_2;
             mutable LA::MPI::Vector temp_vect_3;
@@ -244,7 +248,7 @@ namespace SAND
     template<int dim>
     class KinvMatrixPart : public Subscriptor {
         public:
-            KinvMatrixPart(HMatrix<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in);
+            KinvMatrixPart(HMatrix<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in);
             void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
             void Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
             void initialize(LA::MPI::Vector &exemplar_density_vector);
@@ -254,7 +258,7 @@ namespace SAND
             HMatrix<dim> &h_mat;
             GMatrix &g_mat;
             const LA::MPI::SparseMatrix &d_m_mat;
-            LA::MPI::SparseMatrix &d_m_inv_mat;
+            LA::MPI::SparseMatrix d_m_inv_mat;
             mutable LA::MPI::Vector temp_vect_1;
             mutable LA::MPI::Vector temp_vect_2;
             mutable LA::MPI::Vector temp_vect_3;
@@ -264,7 +268,7 @@ namespace SAND
     template<int dim>
     class KinvMatrixDirect : public TrilinosWrappers::SparseMatrix {
         public:
-            KinvMatrixDirect(HMatrixDirect<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in);
+            KinvMatrixDirect(HMatrixDirect<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in);
             void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
             void Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
             void initialize(LA::MPI::Vector &exemplar_density_vector);
@@ -274,7 +278,7 @@ namespace SAND
             HMatrixDirect<dim> &h_mat;
             GMatrix &g_mat;
             const LA::MPI::SparseMatrix &d_m_mat;
-            LA::MPI::SparseMatrix &d_m_inv_mat;
+            LA::MPI::SparseMatrix d_m_inv_mat;
             mutable LA::MPI::Vector temp_vect_1;
             mutable LA::MPI::Vector temp_vect_2;
             mutable LA::MPI::Vector temp_vect_3;
@@ -284,17 +288,17 @@ namespace SAND
     template<int dim>
     class JinvMatrix : public TrilinosWrappers::SparseMatrix {
         public:
-            JinvMatrix(HMatrix<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in);
+            JinvMatrix(HMatrix<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in);
             void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
             void Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
-            void initialize(LA::MPI::Vector &exemplar_density_vector);
+            void initialize(LA::MPI::Vector &exemplar_density_vector, LA::MPI::SparseMatrix &d_m_inv_mat_in);
             unsigned int m() const;
             unsigned int n() const;
         private:
             HMatrix<dim> &h_mat;
             GMatrix &g_mat;
             const LA::MPI::SparseMatrix &d_m_mat;
-            LA::MPI::SparseMatrix &d_m_inv_mat;
+            LA::MPI::SparseMatrix d_m_inv_mat;
             mutable LA::MPI::Vector temp_vect_1;
             mutable LA::MPI::Vector temp_vect_2;
             mutable LA::MPI::Vector temp_vect_3;
@@ -304,7 +308,7 @@ namespace SAND
     template<int dim>
     class JinvMatrixPart : public Subscriptor {
         public:
-            JinvMatrixPart(HMatrix<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in);
+            JinvMatrixPart(HMatrix<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in);
             void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
             void Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
             void initialize(LA::MPI::Vector &exemplar_density_vector);
@@ -314,7 +318,7 @@ namespace SAND
             HMatrix<dim> &h_mat;
             GMatrix &g_mat;
             const LA::MPI::SparseMatrix &d_m_mat;
-            LA::MPI::SparseMatrix &d_m_inv_mat;
+            LA::MPI::SparseMatrix d_m_inv_mat;
             mutable LA::MPI::Vector temp_vect_1;
             mutable LA::MPI::Vector temp_vect_2;
             mutable LA::MPI::Vector temp_vect_3;
@@ -324,7 +328,7 @@ namespace SAND
     template<int dim>
     class JinvMatrixDirect : public TrilinosWrappers::SparseMatrix {
         public:
-            JinvMatrixDirect(HMatrixDirect<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in);
+            JinvMatrixDirect(HMatrixDirect<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in);
             void vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
             void Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const;
             void initialize(LA::MPI::Vector &exemplar_density_vector);
@@ -334,7 +338,7 @@ namespace SAND
             HMatrixDirect<dim> &h_mat;
             GMatrix &g_mat;
             const LA::MPI::SparseMatrix &d_m_mat;
-            LA::MPI::SparseMatrix &d_m_inv_mat;
+            LA::MPI::SparseMatrix d_m_inv_mat;
             mutable LA::MPI::Vector temp_vect_1;
             mutable LA::MPI::Vector temp_vect_2;
             mutable LA::MPI::Vector temp_vect_3;
@@ -389,6 +393,7 @@ namespace SAND
         const LA::MPI::SparseMatrix &c_mat;
         const LA::MPI::SparseMatrix &e_mat;
         const LA::MPI::SparseMatrix &f_mat;
+        const LA::MPI::SparseMatrix &f_t_mat;
         const LA::MPI::SparseMatrix &d_m_mat;
         const LA::MPI::SparseMatrix &d_1_mat;
         const LA::MPI::SparseMatrix &d_2_mat;
@@ -401,7 +406,7 @@ namespace SAND
         LA::MPI::SparseMatrix d_6_mat;
         LA::MPI::SparseMatrix d_7_mat;
         LA::MPI::SparseMatrix d_8_mat;
-        mutable LA::MPI::SparseMatrix d_m_inv_mat;
+        LA::MPI::SparseMatrix d_m_inv_mat;
 
         mutable LA::MPI::Vector pre_j;
         mutable LA::MPI::Vector pre_k;
diff --git a/source/density_filter.cc b/source/density_filter.cc
index 09ada1e..4e64a56 100644
--- a/source/density_filter.cc
+++ b/source/density_filter.cc
@@ -41,6 +41,7 @@ namespace SAND
         y_coord.resize(n_p);
         z_coord.resize(n_p);
         auto row_sum = z_coord;
+        auto row_sum_full = z_coord;
         cell_m.resize(n_p);
         x_coord_part.resize(n_p);
         y_coord_part.resize(n_p);
@@ -49,7 +50,7 @@ namespace SAND
 
         filter_dsp.reinit(dofs_per_block[0],
                           dofs_per_block[0]);
-        // filter_sparsity_pattern.copy_from(filter_dsp);
+        filter_sparsity_pattern.copy_from(filter_dsp);
 
         // const auto owned_dofs = dof_handler.locally_owned_dofs().get_view(0, dofs_per_block[0]);
 
@@ -66,8 +67,6 @@ namespace SAND
         //         filter_matrix.add(i[cell->get_fe().component_to_system_index(0, 0)], i[cell->get_fe().component_to_system_index(0, 0)], 1.0);
         //     }
         // }
-
-        pcout << "FILTER TO 1" << std::endl;
        
         std::set<unsigned int> neighbor_ids;
         std::set<typename DoFHandler<dim>::cell_iterator> cells_to_check;
@@ -89,7 +88,6 @@ namespace SAND
                 }
              }
          }
-        pcout << "FILTER TO 2" << std::endl;
         MPI_Allreduce(x_coord_part.data(), x_coord.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
         MPI_Allreduce(y_coord_part.data(), y_coord.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
         MPI_Allreduce(z_coord_part.data(), z_coord.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
@@ -106,12 +104,11 @@ namespace SAND
                 }
             }
         }
-        pcout << "FILTER TO 3" << std::endl;
         filter_sparsity_pattern.copy_from(filter_dsp);
-
-        const auto owned_dofs = dof_handler.locally_owned_dofs().get_view(0, dofs_per_block[0]);
+        const auto owned_dofs = dof_handler.locally_owned_dofs().get_view(4 * n_p, 5 * n_p);
 
         filter_matrix.reinit(owned_dofs, filter_sparsity_pattern, MPI_COMM_WORLD);
+        filter_matrix_transpose.reinit(owned_dofs, filter_sparsity_pattern, MPI_COMM_WORLD);
 
         /// adds values to the matrix corresponding to the max radius - distance
         for (const auto &cell : dof_handler.active_cell_iterators())
@@ -120,15 +117,16 @@ namespace SAND
             {
                 std::vector<unsigned int> i(cell->get_fe().n_dofs_per_cell());
                 cell->get_dof_indices(i);
+                auto cell_index = i[cell->get_fe().component_to_system_index(0, 0)];
                 double value_total = 0;
-                for (const auto &neighbor_cell_index : find_relevant_neighbors(i[cell->get_fe().component_to_system_index(0, 0)]))
+                for (const auto &neighbor_cell_index : find_relevant_neighbors(cell_index))
                 {
-                    double d_x = std::abs(x_coord[i[cell->get_fe().component_to_system_index(0, 0)]]-x_coord[neighbor_cell_index]);
-                    double d_y = std::abs(y_coord[i[cell->get_fe().component_to_system_index(0, 0)]]-y_coord[neighbor_cell_index]);
+                    double d_x = std::abs(x_coord[cell_index]-x_coord[neighbor_cell_index]);
+                    double d_y = std::abs(y_coord[cell_index]-y_coord[neighbor_cell_index]);
                     double d;
                     if (dim==3)
                     {
-                        double d_z = std::abs(z_coord[i[cell->get_fe().component_to_system_index(0, 0)]]-z_coord[neighbor_cell_index]);
+                        double d_z = std::abs(z_coord[cell_index]-z_coord[neighbor_cell_index]);
                         d = std::pow(d_x*d_x + d_y*d_y + d_z*d_z , .5);
                     }
                     else
@@ -139,15 +137,26 @@ namespace SAND
                     double value = (Input::filter_r - d)*cell_m[neighbor_cell_index];
                     value_total += value;
                 }
-            
-                for (const auto &neighbor_cell_index : find_relevant_neighbors(i[cell->get_fe().component_to_system_index(0, 0)]))
+                row_sum[cell_index] = value_total;
+                
+            }
+        }
+        MPI_Allreduce(row_sum.data(), row_sum_full.data(), n_p, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
+        for (const auto &cell : dof_handler.active_cell_iterators())
+        {
+            if(cell->is_locally_owned())
+            {
+                std::vector<unsigned int> i(cell->get_fe().n_dofs_per_cell());
+                cell->get_dof_indices(i);
+                auto cell_index = i[cell->get_fe().component_to_system_index(0, 0)];
+                for (const auto &neighbor_cell_index : find_relevant_neighbors(cell_index))
                 {
-                    double d_x = std::abs(x_coord[i[cell->get_fe().component_to_system_index(0, 0)]]-x_coord[neighbor_cell_index]);
-                    double d_y = std::abs(y_coord[i[cell->get_fe().component_to_system_index(0, 0)]]-y_coord[neighbor_cell_index]);
+                    double d_x = std::abs(x_coord[cell_index]-x_coord[neighbor_cell_index]);
+                    double d_y = std::abs(y_coord[cell_index]-y_coord[neighbor_cell_index]);
                     double d;
                     if (dim==3)
                     {
-                        double d_z = std::abs(z_coord[i[cell->get_fe().component_to_system_index(0, 0)]]-z_coord[neighbor_cell_index]);
+                        double d_z = std::abs(z_coord[cell_index]-z_coord[neighbor_cell_index]);
                         d = std::pow(d_x*d_x + d_y*d_y + d_z*d_z , .5);
                     }
                     else
@@ -156,12 +165,25 @@ namespace SAND
                     }
                     ///value should be (max radius - distance between cells)*cell measure
                     double value = (Input::filter_r - d)*cell_m[neighbor_cell_index];
-                    filter_matrix.add(i[cell->get_fe().component_to_system_index(0, 0)], neighbor_cell_index, value/value_total);
+                    filter_matrix.set(cell_index, neighbor_cell_index, value/row_sum_full[cell_index]);
+                    filter_matrix_transpose.set(cell_index, neighbor_cell_index, value/row_sum_full[neighbor_cell_index]);
                 }
             }
         }
-        filter_matrix.compress(VectorOperation::add);
-        pcout << "FILTER TO 4" << std::endl;
+        double sum_sum = 0;
+        double x_sum = 0;
+        double y_sum = 0;
+        double z_sum = 0;
+        for (int i=0; i< row_sum_full.size(); i++)
+        {
+            sum_sum += std::abs(row_sum_full[i]);
+            x_sum += std::abs(x_coord[i]);
+            y_sum += std::abs(y_coord[i]);
+            z_sum += std::abs(z_coord[i]);
+        }
+        filter_matrix.compress(VectorOperation::insert);
+        filter_matrix_transpose.compress(VectorOperation::insert);
+
         ///here we normalize the filter so it computes an average. Sum of values in a row should be 1
         // for (const auto &cell : dof_handler.active_cell_iterators())
         // {
@@ -185,6 +207,21 @@ namespace SAND
         //     }
         // }
         // pcout << "FILTER TO 5" << std::endl;
+        // LA::MPI::Vector test_density_start;
+        // test_density_start.reinit(local_owned, mpi_communicator);
+        // LA::MPI::Vector test_density_end;
+        // test_density_end.reinit(local_owned, mpi_communicator);
+        // test_density_end = 0.;
+        // for (int i = 0; i<n_p; i++)
+        // {
+        //     if (test_density_start.in_local_range(i))
+        //     {
+        //         test_density_start[i] = i;
+        //     }
+        // }
+        // test_density_start.compress(VectorOperation::insert);
+        // filter_matrix.vmult(test_density_end,test_density_start);
+
     }
 
     ///This function finds which neighbors are within a certain radius of the initial cell.
diff --git a/source/kkt_system.cc b/source/kkt_system.cc
index 7b6faec..12b5c8b 100644
--- a/source/kkt_system.cc
+++ b/source/kkt_system.cc
@@ -667,10 +667,9 @@ KktSystem<dim>::setup_boundary_values()
                             {
                                 const auto vert = cell->vertex(vertex_number);
                                 /*Find bottom left corner*/
-                                if (std::fabs(vert(0) - 0) < 1e-12 && std::fabs(
-                                            vert(1) - 0) < 1e-12 && ((std::fabs(
-                                            vert(2) - 0) < 1e-12) || (std::fabs(
-                                            vert(2) - 1) < 1e-12)))
+                                if (std::fabs(vert(0) - 0) < 1e-12 && 
+                                    std::fabs(vert(1) - 0) < 1e-12 && 
+                                    ((std::fabs(vert(2) - 0) < 1e-12) || (std::fabs(vert(2) - 1) < 1e-12)))
                                 {
                                     for (unsigned int level = 0; level < n_levels; ++level)
                                     {
@@ -690,10 +689,10 @@ KktSystem<dim>::setup_boundary_values()
                                     }
                                 }
                                 /*Find bottom right corner*/
-                                if (std::fabs(vert(0) - 6) < 1e-12 && std::fabs(
-                                            vert(1) - 0) < 1e-12 && ((std::fabs(
-                                            vert(2) - 0) < 1e-12) || (std::fabs(
-                                            vert(2) - 1) < 1e-12)))
+                                if (std::fabs(vert(0) - 6) < 1e-12 && 
+                                    std::fabs(vert(1) - 0) < 1e-12 && 
+                                    ((std::fabs(vert(2) - 0) < 1e-12) || 
+                                     (std::fabs(vert(2) - 1) < 1e-12)))
                                 {
                                     for (unsigned int level = 0; level < n_levels; ++level)
                                     {
@@ -1136,24 +1135,25 @@ KktSystem<dim>::setup_block_system() {
     SparsityTools::distribute_sparsity_pattern(dsp, dof_handler.locally_owned_dofs(), mpi_communicator,
                                                locally_relevant_dofs);
     //adds the row into the sparsity pattern for the total volume constraint
-    for (const auto &cell: dof_handler.active_cell_iterators())
+    // for (const auto &cell: dof_handler.active_cell_iterators())
+    // {
+    //     if (cell->is_locally_owned())
+    //     {
+    //         std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
+    //         cell->get_dof_indices(i);
+    //         dsp.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier).add(i[cell->get_fe().component_to_system_index(0, 0)], 0);
+    //         dsp.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).add(0, i[cell->get_fe().component_to_system_index(0, 0)]);
+    //     }
+    // }
+    // Because of the single volume multiplier element only being on one processor, this works, and the above does not.
+    for (unsigned int i = 0; i<n_p; i++)
     {
-        if (cell->is_locally_owned())
-        {
-            std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
-            cell->get_dof_indices(i);
-            dsp.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier).add(i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)], 0);
-            dsp.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).add(0, i[cell->get_fe().component_to_system_index(SolutionComponents::density_lower_slack_multiplier<dim>, 0)]);
-        }
-
+        dsp.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier).add(i,0);
+        dsp.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).add(0,i);
     }
 
-
-    pcout << "skipping filter setup" << std::endl;
-
     /*This finds neighbors whose values would be relevant, and adds them to the sparsity pattern of the matrix*/
     setup_filter_matrix();
-    pcout << "to here" << std::endl;
     for (const auto &cell : dof_handler.active_cell_iterators()) {
         if (cell->is_locally_owned())
         {
@@ -1165,7 +1165,7 @@ KktSystem<dim>::setup_block_system() {
                 dsp.block(SolutionBlocks::unfiltered_density_multiplier,
                           SolutionBlocks::unfiltered_density).add(cell_index, neighbor_cell_index);
                 dsp.block(SolutionBlocks::unfiltered_density,
-                          SolutionBlocks::unfiltered_density_multiplier).add(neighbor_cell_index, cell_index);
+                          SolutionBlocks::unfiltered_density_multiplier).add(cell_index, neighbor_cell_index);
             }
         }
     }
@@ -1294,9 +1294,8 @@ KktSystem<dim>::assemble_block_system(const LA::MPI::BlockVector &distributed_st
     LA::MPI::BlockVector filter_adjoint_unfiltered_density_multiplier_solution = distributed_solution;
     filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = 0;
     filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier) = 0;
-    auto op_f = linear_operator<LA::MPI::Vector>(density_filter.filter_matrix);
-    filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = op_f * distributed_solution.block(SolutionBlocks::unfiltered_density);
-    filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier) = transpose_operator(op_f) *  distributed_solution.block(SolutionBlocks::unfiltered_density_multiplier);
+    density_filter.filter_matrix.vmult(filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density),distributed_solution.block(SolutionBlocks::unfiltered_density));
+    density_filter.filter_matrix_transpose.vmult(filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier),distributed_solution.block(SolutionBlocks::unfiltered_density_multiplier));
 
     LA::MPI::BlockVector relevant_filtered_unfiltered_density_solution = locally_relevant_solution;
     LA::MPI::BlockVector relevant_filter_adjoint_unfiltered_density_multiplier_solution = locally_relevant_solution;
@@ -1587,39 +1586,61 @@ KktSystem<dim>::assemble_block_system(const LA::MPI::BlockVector &distributed_st
         }
 
     }
-
+    // MPI_BARRIER(MPI_COMM_WORLD);
     system_matrix.compress(VectorOperation::add);
     system_rhs = calculate_rhs(distributed_state, barrier_size);
-
-    for (const auto &cell: dof_handler.active_cell_iterators()) {
+    double cell_measure;
+    for (const auto &cell: dof_handler.active_cell_iterators()) 
+    {
         if(cell->is_locally_owned())
         {
             std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
             cell->get_dof_indices(i);
+            const unsigned int cell_i = i[cell->get_fe().component_to_system_index(0, 0)];
 
-            typename LA::MPI::SparseMatrix::iterator iter = density_filter.filter_matrix.begin(
-                        i[cell->get_fe().component_to_system_index(0, 0)]);
-            for (; iter != density_filter.filter_matrix.end(i[cell->get_fe().component_to_system_index(0, 0)]); iter++) {
-                unsigned int j = iter->column();
-                double value = iter->value() * cell->measure();
+            // typename LA::MPI::SparseMatrix::iterator iter = density_filter.filter_matrix.begin(cell_i);
+            for (const unsigned int j : density_filter.find_relevant_neighbors(cell_i)) 
+            {
+                // unsigned int j = iter->column();
+                double value = density_filter.filter_matrix(cell_i,j) * cell->measure();
+                double value_transpose = density_filter.filter_matrix_transpose(cell_i,j) * cell->measure();
 
                 system_matrix.block(SolutionBlocks::unfiltered_density_multiplier,
-                                    SolutionBlocks::unfiltered_density).add(i[cell->get_fe().component_to_system_index(0, 0)], j, value);
+                                    SolutionBlocks::unfiltered_density).set(cell_i, j, value);
                 system_matrix.block(SolutionBlocks::unfiltered_density,
-                                    SolutionBlocks::unfiltered_density_multiplier).add(j, i[cell->get_fe().component_to_system_index(0, 0)], value);
+                                    SolutionBlocks::unfiltered_density_multiplier).set(cell_i, j, value_transpose);
             }
 
-            system_matrix.block(SolutionBlocks::total_volume_multiplier, SolutionBlocks::density).add(0, i[cell->get_fe().component_to_system_index(0, 0)],
-                    cell->measure());
+            cell_measure = cell->measure();
 
-            system_matrix.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier).add(i[cell->get_fe().component_to_system_index(0, 0)], 0,
+            system_matrix.block(SolutionBlocks::density, SolutionBlocks::total_volume_multiplier).set(cell_i, 0,
+                    cell->measure());
+            system_matrix.block(SolutionBlocks::total_volume_multiplier,SolutionBlocks::density).set(0,cell_i,
                     cell->measure());
         }
     }
-    system_matrix.compress(VectorOperation::add);
+    system_matrix.compress(VectorOperation::insert);
+    // for (const auto &cell: dof_handler.active_cell_iterators()) 
+    // {
+    //     if(cell->is_locally_owned())
+    //     {
+    //         std::vector<types::global_dof_index> i(cell->get_fe().n_dofs_per_cell());
+    //         cell->get_dof_indices(i);
+    //         const unsigned int cell_i = i[cell->get_fe().component_to_system_index(0, 0)];
+
+    //         for (const unsigned int j : density_filter.find_relevant_neighbors(cell_i)) {
+    //             double value = system_matrix.block(SolutionBlocks::unfiltered_density_multiplier,
+    //                                 SolutionBlocks::unfiltered_density).el(j,cell_i);
+
+    //             system_matrix.block(SolutionBlocks::unfiltered_density,
+    //                                 SolutionBlocks::unfiltered_density_multiplier).set(j, cell_i, value);
+    //         }
+    //     }
+    // }
+    // system_matrix.compress(VectorOperation::insert);
 
 
-    pcout << "assembled" << std::endl;
+    pcout << "assembled " << std::endl;
 
 }
 
@@ -1857,7 +1878,7 @@ KktSystem<dim>::calculate_rhs(const LA::MPI::BlockVector &distributed_state, con
     LA::MPI::BlockVector test_rhs (system_rhs);
     LA::MPI::BlockVector state (locally_relevant_solution);
     state = distributed_state;
-    test_rhs = 0;
+    test_rhs = 0.;
 
     QGauss<dim> nine_quadrature(fe_nine.degree + 1);
     QGauss<dim> ten_quadrature(fe_ten.degree + 1);
@@ -1913,10 +1934,9 @@ KktSystem<dim>::calculate_rhs(const LA::MPI::BlockVector &distributed_state, con
     filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = 0;
     filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier) = 0;
 
-    auto op_f = linear_operator<LA::MPI::Vector>(density_filter.filter_matrix);
-    filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density) = op_f * distributed_solution.block(SolutionBlocks::unfiltered_density);
-    filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier)
-            = transpose_operator(op_f) * distributed_solution.block(SolutionBlocks::unfiltered_density_multiplier);
+    density_filter.filter_matrix.vmult(filtered_unfiltered_density_solution.block(SolutionBlocks::unfiltered_density),distributed_solution.block(SolutionBlocks::unfiltered_density));
+    density_filter.filter_matrix_transpose.vmult(filter_adjoint_unfiltered_density_multiplier_solution.block(SolutionBlocks::unfiltered_density_multiplier),distributed_solution.block(SolutionBlocks::unfiltered_density_multiplier));
+
     LA::MPI::BlockVector relevant_filtered_unfiltered_density_solution (locally_relevant_solution);
     LA::MPI::BlockVector relevant_filter_adjoint_unfiltered_density_multiplier_solution (locally_relevant_solution);
     relevant_filtered_unfiltered_density_solution = filtered_unfiltered_density_solution;
@@ -2106,11 +2126,15 @@ KktSystem<dim>::calculate_rhs(const LA::MPI::BlockVector &distributed_state, con
                                    - old_upper_slack_values[q_point]));
 
                     //rhs eqn 6
-                    cell_rhs(i) +=
+                    if (std::abs(old_density_values[q_point] - filtered_unfiltered_density_values[q_point])>1e-12)
+                    {
+                        cell_rhs(i) +=
                             -1 * fe_values.JxW(q_point) * (
                                 -1 * unfiltered_density_multiplier_phi_i
                                 * (old_density_values[q_point] - filtered_unfiltered_density_values[q_point])
                                 );
+                    }
+
 
                     //rhs eqn 7
                     cell_rhs(i) +=
@@ -2223,16 +2247,13 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
                     Input::default_gmres_tolerance);
     }
     else {
-        gmres_tolerance = Input::default_gmres_tolerance;
+        gmres_tolerance = Input::default_gmres_tolerance*system_rhs.l2_norm();
     }
 
 
     locally_relevant_solution=state;
     distributed_solution = state;
 
-    density_filter.filter_matrix.vmult(distributed_solution.block(SolutionBlocks::density), distributed_solution.block(SolutionBlocks::unfiltered_density));
-    locally_relevant_solution = distributed_solution;
-
     SolverControl solver_control(10000, gmres_tolerance);
 
     // ************ BEGIN MAKING MF GMG ELASTICITY PRECONDITIONER ***************************
@@ -2266,9 +2287,6 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
             pcout << "d = " << support_points_displacement_pair.first << ", points are " << support_points_displacement_pair.second << " and " << support_points[support_points_displacement_pair.first+disp_mult_start_index] << std::endl;
     }
 
-    pcout << "Number of degrees of freedom: " << dof_handler_displacement.n_dofs()
-          << std::endl;
-
     MPI_Barrier(MPI_COMM_WORLD);
 
     std::vector<IndexSet> locally_owned_dofs = Utilities::MPI::all_gather(mpi_communicator, dof_handler_displacement.locally_owned_dofs());
@@ -2276,16 +2294,14 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
     DoFTools::extract_locally_active_dofs(dof_handler_displacement, locally_active_dofs);
     IndexSet locally_relevant_dofs;
     DoFTools::extract_locally_relevant_dofs(dof_handler_displacement, locally_relevant_dofs);
-    std::cout << Utilities::MPI::this_mpi_process(mpi_communicator) << " has relevant: " << locally_relevant_dofs.n_elements() << "and active: " << locally_relevant_dofs.n_elements() << std::endl;
-    // AffineConstraints<double> temp_displacement_constraints;
+   // AffineConstraints<double> temp_displacement_constraints;
+    
     
-    std::cout << Utilities::MPI::this_mpi_process(mpi_communicator) << " has levels: " << mg_level_constraints.max_level() << " and tria levels: " << triangulation.n_global_levels() << std::endl;
-
 
     displacement_constraints.clear();
-    displacement_constraints.reinit(locally_active_dofs); //FIXME SHOULD THIS BE RELEVANT???
+    displacement_constraints.reinit(locally_relevant_dofs); //FIXME SHOULD THIS BE RELEVANT???
     displacement_constraints.copy_from(mg_level_constraints[triangulation.n_global_levels()-1]);
-    pcout << "displacement constraint number: " << displacement_constraints.n_constraints() <<std::endl;
+    std::cout << "displacement constraint number: " << displacement_constraints.n_constraints() <<std::endl;
     displacement_constraints.close();
     {
         typename MatrixFree<dim, double>::AdditionalData additional_data;
@@ -2310,20 +2326,9 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
     elasticity_matrix_mf.initialize_dof_vector(distributed_displacement_sol);
     elasticity_matrix_mf.initialize_dof_vector(distributed_displacement_rhs);
 
-    locally_relevant_solution = distributed_solution;
-
-    pcout << "displacement_vec_size = " << distributed_displacement_sol.size() << std::endl;
-    pcout << "systdm_vec_size = " << distributed_solution.block(SolutionBlocks::displacement).size() << std::endl;
-
-    ChangeVectorTypes::copy_from_system_to_displacement_vector<double>(distributed_displacement_sol,locally_relevant_solution.block(SolutionBlocks::displacement),displacement_to_system_dof_index_map);
-
-
-    locally_relevant_solution = system_rhs;
-    ChangeVectorTypes::copy_from_system_to_displacement_vector<double>(distributed_displacement_rhs,locally_relevant_solution.block(SolutionBlocks::displacement),displacement_to_system_dof_index_map);
-
-    locally_relevant_solution = distributed_solution;
+    ChangeVectorTypes::copy_from_system_to_displacement_vector<double>(distributed_displacement_sol,distributed_solution.block(SolutionBlocks::displacement),displacement_to_system_dof_index_map);
 
-    pcout << "types changed" << std::endl;
+    ChangeVectorTypes::copy_from_system_to_displacement_vector<double>(distributed_displacement_rhs,system_rhs.block(SolutionBlocks::displacement),displacement_to_system_dof_index_map);
 
     const unsigned int n_levels = triangulation.n_global_levels();
     mg_matrices.resize(0, n_levels - 1);
@@ -2529,22 +2534,20 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
         if (level > 0)
         {
             smoother_data[level].smoothing_range     = 15.;
-            smoother_data[level].degree              = 5;
+            smoother_data[level].degree              = 10;
             smoother_data[level].eig_cg_n_iterations = 10;
         }
         else
         {
-            smoother_data[0].smoothing_range = .001;
-            smoother_data[0].degree          = 4;
-            smoother_data[0].eig_cg_n_iterations = 100;
+            smoother_data[0].smoothing_range = 1e-3;
+            smoother_data[0].degree          = numbers::invalid_unsigned_int;
+            smoother_data[0].eig_cg_n_iterations = mg_matrices[0].m();
         }
         mg_matrices[level].compute_diagonal();
         smoother_data[level].preconditioner =
                 mg_matrices[level].get_matrix_diagonal_inverse();
     }
 
-    std::cout << Utilities::MPI::this_mpi_process(mpi_communicator) << " has matrix size " << mg_matrices[0].m() << " at level 0" << std::endl;
-
     mg_smoother.initialize(mg_matrices, smoother_data);
 
     mg_coarse.initialize(mg_smoother);
@@ -2680,7 +2683,40 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
 
 
     TopOptSchurPreconditioner<dim> preconditioner(system_matrix, dof_handler, elasticity_matrix_mf, mf_gmg_preconditioner, displacement_to_system_dof_index_map);
-    pcout << "about to solve" << std::endl;
+    // pcout << "about to solve" << std::endl;
+    // preconditioner.initialize(system_matrix, boundary_values, dof_handler, distributed_solution);
+    // FullMatrix<double> out;
+    // out.reinit(system_matrix.m(),system_matrix.n());
+    // LA::MPI::BlockVector e_j (system_rhs);
+    // LA::MPI::BlockVector r_j (system_rhs);
+    // LA::MPI::BlockVector r2_j (system_rhs);
+    // for (unsigned int j=0; j<out.n(); ++j)
+    // {
+    //     e_j = 0.;
+    //     e_j(j) = 1;
+    //     system_matrix.vmult(system_rhs,e_j);
+    //     preconditioner.vmult(r2_j,system_rhs);
+
+    //     for (unsigned int i=0; i<out.m(); ++i)
+    //         out(i,j) = r2_j(i);
+    // }
+
+    // const unsigned int n = out.n();
+    // const unsigned int m = out.m();
+    // std::ofstream Xmat("preconditioned_mat.csv");
+    // for (unsigned int i = 0; i < m; i++)
+    // {
+    //     Xmat << out(i, 0);
+    //     for (unsigned int j = 1; j < n; j++)
+    //     {
+    //         Xmat << "," << out(i, j);
+    //     }
+    //     Xmat << "\n";
+    // }
+    // Xmat.close();
+    
+
+    
 
     switch (Input::solver_choice)
     {
@@ -2696,8 +2732,20 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
         //     break;
         // }
         case SolverOptions::inexact_K_with_inexact_A_gmres: {
+            pcout << "size of rhs block 0 :  " << system_rhs.block(0).l1_norm()<< std::endl;
+            pcout << "size of rhs block 1 :  " << system_rhs.block(1).l1_norm()<< std::endl;
+            pcout << "size of rhs block 2 :  " << system_rhs.block(2).l1_norm()<< std::endl;
+            pcout << "size of rhs block 3 :  " << system_rhs.block(3).l1_norm()<< std::endl;
+            pcout << "size of rhs block 4 :  " << system_rhs.block(4).l1_norm()<< std::endl;
+            pcout << "size of rhs block 5 :  " << system_rhs.block(5).l1_norm()<< std::endl;
+            pcout << "size of rhs block 6 :  " << system_rhs.block(6).l1_norm()<< std::endl;
+            pcout << "size of rhs block 7 :  " << system_rhs.block(7).l1_norm()<< std::endl;
+            pcout << "size of rhs block 8 :  " << system_rhs.block(8).l1_norm()<< std::endl;
+            pcout << "size of rhs block 9 :  " << system_rhs.block(9).l1_norm()<< std::endl;
+
             preconditioner.initialize(system_matrix, boundary_values, dof_handler, distributed_solution);
             pcout << "preconditioner initialized" << std::endl;
+            distributed_solution = 0.;
             SolverFGMRES<LA::MPI::BlockVector> C_fgmres(solver_control);
             C_fgmres.solve(system_matrix, distributed_solution, system_rhs, preconditioner);
             pcout << solver_control.last_step() << " steps to solve with FGMRES" << std::endl;
@@ -2705,12 +2753,21 @@ KktSystem<dim>::solve(const LA::MPI::BlockVector &state) {
         }
         default:
             throw;
+        
     }
 
     constraints.distribute(distributed_solution);
-
-    pcout << "here" << std::endl;
-
+    pcout << "size of distributed solution block 0 :  " << distributed_solution.block(0).l1_norm()<< std::endl;
+    pcout << "size of distributed solution block 1 :  " << distributed_solution.block(1).l1_norm()<< std::endl;
+    pcout << "size of distributed solution block 2 :  " << distributed_solution.block(2).l1_norm()<< std::endl;
+    pcout << "size of distributed solution block 3 :  " << distributed_solution.block(3).l1_norm()<< std::endl;
+    pcout << "size of distributed solution block 4 :  " << distributed_solution.block(4).l1_norm()<< std::endl;
+    pcout << "size of distributed solution block 5 :  " << distributed_solution.block(5).l1_norm()<< std::endl;
+    pcout << "size of distributed solution block 6 :  " << distributed_solution.block(6).l1_norm()<< std::endl;
+    pcout << "size of distributed solution block 7 :  " << distributed_solution.block(7).l1_norm()<< std::endl;
+    pcout << "size of distributed solution block 8 :  " << distributed_solution.block(8).l1_norm()<< std::endl;
+    pcout << "size of distributed solution block 9 :  " << distributed_solution.block(9).l1_norm()<< std::endl;
+    output(distributed_solution,100);
     return distributed_solution;
 }
 
@@ -2749,7 +2806,7 @@ KktSystem<dim>::get_initial_state() {
         state.block(total_volume_multiplier).add(1);
         state.block(displacement).add(0);
         state.block(displacement_multiplier).add(0);
-        state.compress(VectorOperation::add);
+        // state.compress(VectorOperation::add);
 
         // RANDOM PART HERE
         // for(unsigned int k = 0; k<n_p; ++k)
@@ -2762,7 +2819,7 @@ KktSystem<dim>::get_initial_state() {
         // }
 
     }
-    state.compress(VectorOperation::insert);
+    state.compress(VectorOperation::add);
     return state;
 }
 
diff --git a/source/matrix_free_elasticity.cc b/source/matrix_free_elasticity.cc
index 0a491e7..6da06dc 100644
--- a/source/matrix_free_elasticity.cc
+++ b/source/matrix_free_elasticity.cc
@@ -40,17 +40,18 @@ MF_Elasticity_Operator<dim,fe_degree,number>::compute_diagonal ()
 
     this->data->initialize_dof_vector(inverse_diagonal);
     this->data->initialize_dof_vector(diagonal);
-    unsigned int dummy = 1;
+    unsigned int dummy = 0;
 
 
     this->data->cell_loop (&MF_Elasticity_Operator::local_compute_diagonal, this,
                            diagonal, dummy);
+    this->data->cell_loop (&MF_Elasticity_Operator::local_compute_diagonal, this,
+                           inverse_diagonal, dummy);
 
     this->set_constrained_entries_to_one(diagonal);
+    this->set_constrained_entries_to_one(inverse_diagonal);
 
     // diagonal.compress(VectorOperation::add);
-
-    inverse_diagonal=diagonal;
     
     for (auto &local_element : inverse_diagonal)
       {
@@ -193,7 +194,6 @@ ::apply_add (dealii::LinearAlgebra::distributed::Vector<number> &dst,
 {
     MatrixFreeOperators::Base<dim, dealii::LinearAlgebra::distributed::Vector<number>>::
             data->cell_loop(&MF_Elasticity_Operator::local_apply, this, dst, src);
-    // dst.compress(VectorOperation::add);
 }
 
 ///Sets cell data (density) to be input given.
@@ -201,7 +201,6 @@ template <int dim, int fe_degree, typename number>
 void
 MF_Elasticity_Operator<dim,fe_degree,number>::set_cell_data (const OperatorCellData<dim,number> &data)
 {
-  pcout << "set_cell_data" << std::endl;
     this->cell_data = &data;
 }
 
diff --git a/source/poly_pre.cc b/source/poly_pre.cc
new file mode 100644
index 0000000..c5dad82
--- /dev/null
+++ b/source/poly_pre.cc
@@ -0,0 +1,7 @@
+#include "../include/poly_pre.h"
+#include "../include/schur_preconditioner.h"
+
+namespace SAND
+{
+
+}
diff --git a/source/schur_preconditioner.cc b/source/schur_preconditioner.cc
index 893412b..7582898 100644
--- a/source/schur_preconditioner.cc
+++ b/source/schur_preconditioner.cc
@@ -149,6 +149,7 @@ TopOptSchurPreconditioner<dim>::TopOptSchurPreconditioner(LA::MPI::BlockSparseMa
       c_mat(matrix_in.block(SolutionBlocks::displacement,SolutionBlocks::density)),
       e_mat(matrix_in.block(SolutionBlocks::displacement_multiplier,SolutionBlocks::density)),
       f_mat(matrix_in.block(SolutionBlocks::unfiltered_density_multiplier,SolutionBlocks::unfiltered_density)),
+      f_t_mat(matrix_in.block(SolutionBlocks::unfiltered_density,SolutionBlocks::unfiltered_density_multiplier)),
       d_m_mat(matrix_in.block(SolutionBlocks::density_upper_slack_multiplier, SolutionBlocks::density_upper_slack)),
       d_1_mat(matrix_in.block(SolutionBlocks::density_lower_slack, SolutionBlocks::density_lower_slack)),
       d_2_mat(matrix_in.block(SolutionBlocks::density_upper_slack, SolutionBlocks::density_upper_slack)),
@@ -160,12 +161,12 @@ TopOptSchurPreconditioner<dim>::TopOptSchurPreconditioner(LA::MPI::BlockSparseMa
       a_inv_mf_gmg(mf_elasticity_operator_in, mf_gmg_preconditioner_in, a_mat, displacement_to_system_dof_index_map),
       pcout(std::cout,(Utilities::MPI::this_mpi_process(mpi_communicator) == 0)),
       timer(pcout, TimerOutput::summary, TimerOutput::wall_times),
-      g_mat(f_mat, d_8_mat),
+      g_mat(f_mat, f_t_mat, d_8_mat),
       h_mat(a_mat, b_mat, c_mat, e_mat, pre_amg, a_inv_direct, a_inv_mf_gmg),
-      j_inv_mat(h_mat, g_mat, d_m_mat, d_m_inv_mat),
-      k_inv_mat(h_mat, g_mat, d_m_mat, d_m_inv_mat),
-      j_inv_part(h_mat, g_mat, d_m_mat, d_m_inv_mat),
-      k_inv_part(h_mat, g_mat, d_m_mat, d_m_inv_mat),
+      j_inv_mat(h_mat, g_mat, d_m_mat),
+      k_inv_mat(h_mat, g_mat, d_m_mat),
+      j_inv_part(h_mat, g_mat, d_m_mat),
+      k_inv_part(h_mat, g_mat, d_m_mat),
       big_dof_handler(big_dof_handler_in)
 {
   
@@ -238,7 +239,7 @@ void TopOptSchurPreconditioner<dim>::initialize(LA::MPI::BlockSparseMatrix &matr
         TimerOutput::Scope t(timer, "reinit diag matrices");
         d_3_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
         d_4_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
-        d_5_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
+        d_5_mat.reinit(matrix.block(SolutionBlocks::density_lower_slack_multiplier, SolutionBlocks::density_lower_slack_multiplier));
         d_6_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
         d_7_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
         d_8_mat.reinit(matrix.block(SolutionBlocks::density, SolutionBlocks::density));
@@ -252,14 +253,11 @@ void TopOptSchurPreconditioner<dim>::initialize(LA::MPI::BlockSparseMatrix &matr
         d_7_mat=0;
         d_8_mat=0;
         d_m_inv_mat=0;
-        pcout << "diag reinit" << std::endl;
     }
     {
         const types::global_dof_index n_p = system_matrix.block(SolutionBlocks::density,
                                                                 SolutionBlocks::density).m();
 
-        pcout << "np: " << n_p << std::endl;
-
         std::vector<double> l_global(n_p);
         std::vector<double> lm_global(n_p);
         std::vector<double> u_global(n_p);
@@ -341,10 +339,12 @@ void TopOptSchurPreconditioner<dim>::initialize(LA::MPI::BlockSparseMatrix &matr
     LA::MPI::Vector density_exemplar = distributed_state.block(SolutionBlocks::density);
     LA::MPI::Vector displacement_exemplar = distributed_state.block(SolutionBlocks::displacement);
 
-    g_mat.initialize(density_exemplar);
-    h_mat.initialize(density_exemplar, displacement_exemplar);
-    j_inv_mat.initialize(density_exemplar);
-    k_inv_mat.initialize(density_exemplar);
+    density_exemplar = 0.;
+    displacement_exemplar = 0.;
+    g_mat.initialize(density_exemplar, d_m_inv_mat);
+    h_mat.initialize(density_exemplar, displacement_exemplar,d_m_inv_mat);
+    j_inv_mat.initialize(density_exemplar,d_m_inv_mat);
+    k_inv_mat.initialize(density_exemplar,d_m_inv_mat);
     // j_inv_part.initialize(density_exemplar);
     // k_inv_part.initialize(density_exemplar);
 
@@ -354,139 +354,50 @@ void TopOptSchurPreconditioner<dim>::initialize(LA::MPI::BlockSparseMatrix &matr
 ///Application of the preconditioner, broken up into 5 parts.
 template<int dim>
 void TopOptSchurPreconditioner<dim>::vmult(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
-    pcout << " starting vmult " << num_mults << std::endl;
+
+    LA::MPI::BlockVector dst_tmp = dst;
+    dst_tmp = 0.;
+     LA::MPI::BlockVector src_temp = src;
+
+    // pcout << "size of matrix block:  " << system_matrix.block(9,8).linfty_norm() << "   " << system_matrix.block(5,6).linfty_norm() << std::endl;
     LA::MPI::BlockVector temp_src;
     {
+        dst = 0.;
         TimerOutput::Scope t(timer, "part 1");
         vmult_step_1(dst, src);
         temp_src = dst;
-        pcout << "step 1 done" << std::endl;
+        pcout << "step 1 done: " << dst.l2_norm() << std::endl;
     }
 
     {
+        dst = 0.;
         TimerOutput::Scope t(timer, "part 2");
         vmult_step_2(dst, temp_src);
         temp_src = dst;
-        pcout << "step 2 done" << std::endl;
+        pcout << "step 2 done: " << dst.l2_norm() << std::endl;
     }
 
     {
+        dst = 0.;
         TimerOutput::Scope t(timer, "part 3");
         vmult_step_3(dst, temp_src);
         temp_src = dst;
-        pcout << "step 3 done" << std::endl;
+        pcout << "step 3 done: " << dst.l2_norm() << std::endl;
     }
+
     {
+        dst = 0.;
         TimerOutput::Scope t(timer, "part 4");
         vmult_step_4(dst, temp_src);
         temp_src = dst;
-        pcout << "step 4 done" << std::endl;
+        pcout << "step 4 done: " << dst.l2_norm() << std::endl;
     }
+
+    dst = 0.;
     vmult_step_5(dst, temp_src);
-    pcout << "step 5 done" << std::endl;
+    pcout << "step 5 done: " << dst.l2_norm() << std::endl;
     num_mults++;
 
-
-//    if (num_mults > 7)
-//    {
-
-//        std::cout << "entering problem area"  <<std::endl;
-//        FullMatrix<double> X_matrix;
-//        FullMatrix<double> X_matrix_direct;
-//        LA::MPI::Vector exemplar_vector;
-//        LA::MPI::Vector exemplar_vector_displacement;
-//        const types::global_dof_index n_p = system_matrix.block(SolutionBlocks::density,
-//                                                                SolutionBlocks::density).m();
-
-//        X_matrix.reinit(n_p,n_p);
-//        X_matrix_direct.reinit(n_p,n_p);
-//        exemplar_vector.reinit(src.block(SolutionBlocks::density));
-//        exemplar_vector_displacement.reinit(src.block(SolutionBlocks::displacement));
-//        exemplar_vector = 0;
-
-//        HMatrix<dim> h_mat_test (a_mat, b_mat, c_mat, e_mat, pre_amg, a_inv_direct, a_inv_mf_gmg);
-//        HMatrixDirect<dim> h_mat_test_direct (a_mat, b_mat, c_mat, e_mat, pre_amg, a_inv_direct, a_inv_mf_gmg);
-
-//        h_mat_test.initialize(exemplar_vector, exemplar_vector_displacement);
-//        h_mat_test_direct.initialize(exemplar_vector, exemplar_vector_displacement);
-
-//        KinvMatrix<dim>         k_inv_test          (h_mat_test, g_mat, d_m_mat, d_m_inv_mat);
-
-//        KinvMatrixDirect<dim>   k_inv_test_direct   (h_mat_test_direct, g_mat, d_m_mat, d_m_inv_mat);
-
-
-
-//        k_inv_test.initialize(exemplar_vector);
-//        k_inv_test_direct.initialize(exemplar_vector);
-
-//        std::cout << "finished setup"  <<std::endl;
-
-
-
-
-//        for (unsigned int j=0; j<X_matrix.n(); ++j)
-//        {
-//            LA::MPI::Vector e_j (exemplar_vector);
-//            LA::MPI::Vector r_j (exemplar_vector);
-
-//            e_j = 0;
-//            e_j(j) = 1;
-
-//            k_inv_test.vmult(r_j,e_j);
-
-//            for (unsigned int i=0; i<X_matrix.m(); ++i)
-//                X_matrix(i,j) = r_j(i);
-//        }
-//        std::cout << "+++++++++++++++++++++BUILT first BIG MATRIx++++++++++++++++++++++++++" << std::endl;
-
-
-//        for (unsigned int j=0; j<X_matrix.n(); ++j)
-//        {
-//            LA::MPI::Vector e_j (exemplar_vector);
-//            LA::MPI::Vector r_j (exemplar_vector);
-
-//            e_j = 0;
-//            e_j(j) = 1;
-//            k_inv_test_direct.vmult(r_j,e_j);
-
-//            for (unsigned int i=0; i<X_matrix_direct.m(); ++i)
-//                X_matrix_direct(i,j) = r_j(i);
-//        }
-
-//std::cout << "+++++++++++++++++++++BUILT BIG MATRICES++++++++++++++++++++++++++" << std::endl;
-
-//        const unsigned int n = X_matrix.n();
-//        const unsigned int m = X_matrix.m();
-
-
-//        std::ofstream Xmat("Kmatrix_MFGMG");
-//        for (unsigned int i = 0; i < m; i++)
-//        {
-//            Xmat << X_matrix(i, 0);
-//            for (unsigned int j = 1; j < n; j++)
-//            {
-//                Xmat << "," << X_matrix(i, j);
-//            }
-//            Xmat << "\n";
-//        }
-//        Xmat.close();
-
-//        std::ofstream Xmat_direct("Kmatrix_MFGMG_Direct");
-//        for (unsigned int i = 0; i < m; i++)
-//        {
-//            Xmat_direct << X_matrix_direct(i, 0);
-//            for (unsigned int j = 1; j < n; j++)
-//            {
-//                Xmat_direct << "," << X_matrix_direct(i, j);
-//            }
-//            Xmat_direct << "\n";
-//        }
-//        Xmat_direct.close();
-
-//        std::abort();
-//    }
-
-
 }
 
 ///Not implemented
@@ -512,9 +423,15 @@ template<int dim>
 void TopOptSchurPreconditioner<dim>::vmult_step_1(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
     dst = src;
     auto dst_temp = dst;
+    auto dst_temp2 = dst;
+    auto dst_temp3 = dst;
+
+    d_5_mat.vmult(dst_temp2.block(SolutionBlocks::density_lower_slack_multiplier),src.block(SolutionBlocks::density_lower_slack_multiplier));
+    d_6_mat.vmult(dst_temp3.block(SolutionBlocks::density_upper_slack_multiplier),src.block(SolutionBlocks::density_upper_slack_multiplier));
+
     dst.block(SolutionBlocks::unfiltered_density) = dst_temp.block(SolutionBlocks::unfiltered_density)
-            - linear_operator<VectorType,VectorType,PayloadType>(d_5_mat) * src.block(SolutionBlocks::density_lower_slack_multiplier)
-            + linear_operator<VectorType,VectorType,PayloadType>(d_6_mat) * src.block(SolutionBlocks::density_upper_slack_multiplier)
+            - dst_temp2.block(SolutionBlocks::density_lower_slack_multiplier)
+            + dst_temp3.block(SolutionBlocks::density_upper_slack_multiplier)
             + src.block(SolutionBlocks::density_lower_slack)
             - src.block(SolutionBlocks::density_upper_slack);
 }
@@ -523,9 +440,11 @@ template<int dim>
 void TopOptSchurPreconditioner<dim>::vmult_step_2(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
     dst = src;
     auto dst_temp = dst;
-
-    dst.block(SolutionBlocks::unfiltered_density_multiplier) = dst_temp.block(SolutionBlocks::unfiltered_density_multiplier)
-            - linear_operator<VectorType,VectorType,PayloadType>(f_mat)*linear_operator<VectorType,VectorType,PayloadType>(d_8_mat) * src.block(SolutionBlocks::unfiltered_density);
+    auto dst_temp2 = dst;
+    auto dst_temp3 = dst;
+    d_8_mat.vmult(dst_temp2.block(SolutionBlocks::unfiltered_density), src.block(SolutionBlocks::unfiltered_density));
+    f_mat.vmult(dst_temp3.block(SolutionBlocks::unfiltered_density),dst_temp2.block(SolutionBlocks::unfiltered_density));
+    dst.block(SolutionBlocks::unfiltered_density_multiplier) = dst_temp.block(SolutionBlocks::unfiltered_density_multiplier) - dst_temp3.block(SolutionBlocks::unfiltered_density);
 
 }
 
@@ -537,14 +456,9 @@ void TopOptSchurPreconditioner<dim>::vmult_step_3(LA::MPI::BlockVector &dst, con
 
     if (Input::solver_choice==SolverOptions::inexact_K_with_inexact_A_gmres)
     {
-        //            SolverControl            solver_control_1(100000, 1e-6);
-        //            SolverControl            solver_control_2(100000, 1e-6);
-        //            SolverCG<LA::MPI::Vector> a_solver_cg_1(solver_control_1);
-        //            SolverCG<LA::MPI::Vector> a_solver_cg_2(solver_control_2);
-        //            a_solver_cg_1.solve(a_mat,dst_temp.block(SolutionBlocks::displacement_multiplier),src.block(SolutionBlocks::displacement_multiplier),pre_amg);
-        //            a_solver_cg_2.solve(a_mat,dst_temp.block(SolutionBlocks::displacement),src.block(SolutionBlocks::displacement),pre_amg);
+
         a_inv_mf_gmg.set_tol(0);
-        a_inv_mf_gmg.set_iter(30);
+        a_inv_mf_gmg.set_iter(100);
 
         a_inv_mf_gmg.vmult(dst_temp.block(SolutionBlocks::displacement_multiplier),src.block(SolutionBlocks::displacement_multiplier));
         a_inv_mf_gmg.vmult(dst_temp.block(SolutionBlocks::displacement),src.block(SolutionBlocks::displacement));
@@ -567,8 +481,6 @@ void TopOptSchurPreconditioner<dim>::vmult_step_3(LA::MPI::BlockVector &dst, con
         dst.block(SolutionBlocks::density) = dst_temp.block(SolutionBlocks::density) - dst_temp.block(SolutionBlocks::density_upper_slack) - dst_temp.block(SolutionBlocks::density_lower_slack);
     }
 
-
-
 }
 
 template<int dim>
@@ -577,6 +489,7 @@ void TopOptSchurPreconditioner<dim>::vmult_step_4(LA::MPI::BlockVector &dst, con
     auto dst_temp = dst;
     auto k_density_mult =  src.block(SolutionBlocks::density);
 
+
     TrilinosWrappers::PreconditionIdentity preconditioner;
     preconditioner.initialize(b_mat);
 
@@ -586,9 +499,12 @@ void TopOptSchurPreconditioner<dim>::vmult_step_4(LA::MPI::BlockVector &dst, con
     auto d_m_inv_density = g_d_m_inv_density;
     d_m_inv_mat.vmult(d_m_inv_density,src.block(SolutionBlocks::density));
     g_mat.vmult(g_d_m_inv_density,d_m_inv_density);
+    
     SolverControl step_4_gmres_control_1 (Input::k_inv_iterations, g_d_m_inv_density.l2_norm()*Input::k_rel_tol);
-    SolverGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
+    SolverFGMRES<LA::MPI::Vector> step_4_gmres_1 (step_4_gmres_control_1);
     try {
+        pcout << "SOLVE 1" << std::endl;
+        k_density_mult = 0.;
         step_4_gmres_1.solve(k_inv_mat,k_g_d_m_inv_density,g_d_m_inv_density, PreconditionIdentity() );
     }
     catch (std::exception &exc)
@@ -597,8 +513,10 @@ void TopOptSchurPreconditioner<dim>::vmult_step_4(LA::MPI::BlockVector &dst, con
         //                throw;
     }
     SolverControl step_4_gmres_control_2 (Input::k_inv_iterations, src.block(SolutionBlocks::unfiltered_density_multiplier).l2_norm()*Input::k_rel_tol);
-    SolverGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
+    SolverFGMRES<LA::MPI::Vector> step_4_gmres_2 (step_4_gmres_control_2);
     try {
+        pcout << "SOLVE 2" << std::endl;
+        k_density_mult = 0.;
         step_4_gmres_2.solve(k_inv_mat,k_density_mult,src.block(SolutionBlocks::unfiltered_density_multiplier), PreconditionIdentity());
     } catch (std::exception &exc)
     {
@@ -607,15 +525,42 @@ void TopOptSchurPreconditioner<dim>::vmult_step_4(LA::MPI::BlockVector &dst, con
         //                throw;
     }
 
+    if (num_mults == -10)
+    {
+        TimerOutput::Scope t(timer, "k_mult");
+        SolverControl step_5_gmres_control_4 (100000, 1e-12*g_d_m_inv_density.l2_norm());
+        step_5_gmres_control_4.enable_history_data();
+        SolverFGMRES<LA::MPI::Vector> step_5_gmres_4(step_5_gmres_control_4);
+        try {
+            TimerOutput::Scope t(timer, "actual inverse 5.3");
+            pcout << "SOLVE TEST" << std::endl;
+            dst_temp.block(SolutionBlocks::density) = 0.;
+            step_5_gmres_4.solve(k_inv_mat,dst_temp.block(SolutionBlocks::density), g_d_m_inv_density , PreconditionIdentity());
+        } catch (std::exception &exc)
+        {
+            pcout << "Failure of linear solver step_5_gmres_4" << std::endl;
+            pcout << "first residual: " << step_5_gmres_control_4.initial_value() << std::endl;
+            pcout << "last residual: " << step_5_gmres_control_4.last_value() << std::endl;
+            //                    throw;
+        }
+        auto history = step_5_gmres_control_4.get_history_data();
+        for (int i=0; i< step_5_gmres_control_4.last_step()+1; ++i)
+        {
+            pcout << i << "   " << history[i]/step_5_gmres_control_4.initial_value() << std::endl;
+        }
 
+        
+    }
     dst.block(SolutionBlocks::total_volume_multiplier) = transpose_operator<VectorType, VectorType, PayloadType>(m_vect)*k_g_d_m_inv_density
             - transpose_operator<VectorType, VectorType, PayloadType>(m_vect)*k_density_mult
             +dst_temp.block(SolutionBlocks::total_volume_multiplier);
+    
 }
 
 ///The only non-triangular vmult step, applies inverses down the block diagonal.
 template<int dim>
-void TopOptSchurPreconditioner<dim>::vmult_step_5(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const {
+void TopOptSchurPreconditioner<dim>::vmult_step_5(LA::MPI::BlockVector &dst, const LA::MPI::BlockVector &src) const 
+{
     {
         //First Block Inverse
         TimerOutput::Scope t(timer, "inverse 1");
@@ -651,10 +596,10 @@ void TopOptSchurPreconditioner<dim>::vmult_step_5(LA::MPI::BlockVector &dst, con
             //                dst.block(SolutionBlocks::displacement_multiplier) = a_inv_op * src.block(SolutionBlocks::displacement);
 
             a_inv_mf_gmg.set_tol(0);
-            a_inv_mf_gmg.set_iter(30);
+            a_inv_mf_gmg.set_iter(100);
             a_inv_mf_gmg.vmult( dst_temp.block(SolutionBlocks::displacement), src.block(SolutionBlocks::displacement_multiplier));
             a_inv_mf_gmg.vmult( dst_temp.block(SolutionBlocks::displacement_multiplier), src.block(SolutionBlocks::displacement));
-            a_inv_mf_gmg.set_tol(0);
+            a_inv_mf_gmg.set_tol(Input::a_rel_tol);
             a_inv_mf_gmg.set_iter(Input::a_inv_iterations);
 
             dst.block(SolutionBlocks::displacement) = -1 * dst_temp.block(SolutionBlocks::displacement);
@@ -702,9 +647,10 @@ void TopOptSchurPreconditioner<dim>::vmult_step_5(LA::MPI::BlockVector &dst, con
                 pre_k =  -1 * pre_pre_k + src.block(SolutionBlocks::unfiltered_density_multiplier);
             }
             SolverControl step_5_gmres_control_1 (Input::k_inv_iterations, Input::k_rel_tol*pre_j.l2_norm());
-            SolverGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
+            SolverFGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
             try {
                 TimerOutput::Scope t(timer, "actual inverse 5.1");
+                dst.block(SolutionBlocks::unfiltered_density_multiplier) = 0.;
                 step_5_gmres_1.solve(j_inv_mat, dst.block(SolutionBlocks::unfiltered_density_multiplier), pre_j , PreconditionIdentity());
             } catch (std::exception &exc)
             {
@@ -714,9 +660,10 @@ void TopOptSchurPreconditioner<dim>::vmult_step_5(LA::MPI::BlockVector &dst, con
 
 
             SolverControl step_5_gmres_control_2 (Input::k_inv_iterations, Input::k_rel_tol*pre_k.l2_norm());
-            SolverGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
+            SolverFGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
             try {
                 TimerOutput::Scope t(timer, "actual inverse 5.2");
+                dst.block(SolutionBlocks::density) = 0.;
                 step_5_gmres_2.solve(k_inv_mat,dst.block(SolutionBlocks::density), pre_k , PreconditionIdentity());
             } catch (std::exception &exc)
             {
@@ -743,10 +690,12 @@ void TopOptSchurPreconditioner<dim>::vmult_step_5(LA::MPI::BlockVector &dst, con
                 g_mat.vmult(pre_pre_k,pre_pre_pre_k);
                 pre_k =  -1 * pre_pre_k + src.block(SolutionBlocks::unfiltered_density_multiplier);
             }
-            SolverControl step_5_gmres_control_1 (Input::k_inv_iterations, 1e-12*pre_j.l2_norm());
-            SolverGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
+            SolverControl step_5_gmres_control_1 (Input::k_inv_iterations, Input::k_rel_tol*pre_j.l2_norm());
+            SolverFGMRES<LA::MPI::Vector> step_5_gmres_1 (step_5_gmres_control_1);
             try {
                 TimerOutput::Scope t(timer, "actual inverse 5.1");
+                dst.block(SolutionBlocks::unfiltered_density_multiplier) = 0.;
+                pcout << "SOLVE 3" << std::endl;
                 step_5_gmres_1.solve(j_inv_mat, dst.block(SolutionBlocks::unfiltered_density_multiplier), pre_j , PreconditionIdentity());
             } catch (std::exception &exc)
             {
@@ -758,9 +707,11 @@ void TopOptSchurPreconditioner<dim>::vmult_step_5(LA::MPI::BlockVector &dst, con
 
 
             SolverControl step_5_gmres_control_2 (Input::k_inv_iterations, Input::k_rel_tol*pre_k.l2_norm());
-            SolverGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
+            SolverFGMRES<LA::MPI::Vector> step_5_gmres_2 (step_5_gmres_control_2);
             try {
                 TimerOutput::Scope t(timer, "actual inverse 5.2");
+                dst.block(SolutionBlocks::density) = 0.;
+                pcout << "SOLVE 4" << std::endl;
                 step_5_gmres_2.solve(k_inv_mat,dst.block(SolutionBlocks::density), pre_k , PreconditionIdentity());
             } catch (std::exception &exc)
             {
@@ -778,8 +729,12 @@ void TopOptSchurPreconditioner<dim>::vmult_step_5(LA::MPI::BlockVector &dst, con
 
     }
     {
+        
         dst.block(SolutionBlocks::total_volume_multiplier) = src.block(SolutionBlocks::total_volume_multiplier);
+
+
     }
+    
 }
 
 ///I used to use this function to output parts of the preconditioner for debugging.
@@ -797,7 +752,7 @@ VmultTrilinosSolverDirect::VmultTrilinosSolverDirect(SolverControl &cn,
                                                      const TrilinosWrappers::SolverDirect::AdditionalData &data)
     : solver_direct(cn, data)
 {
-
+   
 }
 
 ///Initialize a direct solver - works well up to a point of refinement, and then refuses to solve.
@@ -861,6 +816,7 @@ void AInvMatMFGMG<dim>::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src)
 
     ChangeVectorTypes::copy_from_system_to_displacement_vector<double>(temp_src, src, displacement_to_system_dof_index_map);
     try{
+        temp_dst = 0.;
         a_solver_cg.solve(mf_elasticity_operator,temp_dst,temp_src, mf_gmg_preconditioner);
     }
     catch (std::exception &exc)
@@ -869,6 +825,7 @@ void AInvMatMFGMG<dim>::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src)
     }
 
     ChangeVectorTypes::copy_from_displacement_to_system_vector<double>(dst,temp_dst, displacement_to_system_dof_index_map);
+    dst.compress(VectorOperation::insert);
 
 }
 
@@ -884,7 +841,6 @@ void AInvMatMFGMG<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src)
     a_solver_cg.solve(mf_elasticity_operator,temp_dst,temp_src, mf_gmg_preconditioner);
     ChangeVectorTypes::copy_from_displacement_to_system_vector<double>(dst,temp_dst, displacement_to_system_dof_index_map);
 }
-\
 ///Change the solver tolerance if needed
 template<int dim>
 void AInvMatMFGMG<dim>::set_tol(double tol_in)
@@ -902,25 +858,33 @@ void AInvMatMFGMG<dim>::set_iter(unsigned int iterations_in)
 
 // ******************     GMATRIX     ***********************
 
-GMatrix::GMatrix(const LA::MPI::SparseMatrix &f_mat_in, LA::MPI::SparseMatrix &d_8_mat_in)
+GMatrix::GMatrix(const LA::MPI::SparseMatrix &f_mat_in, const LA::MPI::SparseMatrix &f_t_mat_in, LA::MPI::SparseMatrix &d_8_mat_in)
     :
       f_mat(f_mat_in),
+      f_t_mat(f_t_mat_in),
       d_8_mat(d_8_mat_in)
 {
 
 }
 
 void
-GMatrix::initialize(LA::MPI::Vector &exemplar_density_vector)
+GMatrix::initialize(LA::MPI::Vector &exemplar_density_vector, LA::MPI::SparseMatrix &d_m_inv_mat_in)
 {
     temp_vect_1 = exemplar_density_vector;
     temp_vect_2 = exemplar_density_vector;
+    temp_vect_1 = 0.;
+    temp_vect_2 = 0.;
+
+    d_m_inv_mat.copy_from(d_m_inv_mat_in);
 }
 
 
 void GMatrix::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
 {
-    f_mat.Tvmult(temp_vect_1,src);
+    temp_vect_1 = 0.;
+    temp_vect_2 = 0.;
+    dst = 0.;
+    f_t_mat.vmult(temp_vect_1,src);
     d_8_mat.vmult(temp_vect_2, temp_vect_1);
     f_mat.vmult(dst,temp_vect_2);
 }
@@ -929,7 +893,10 @@ void GMatrix::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
 
 void GMatrix::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
 {
-    f_mat.Tvmult(temp_vect_1,src);
+    temp_vect_1 = 0.;
+    temp_vect_2 = 0.;
+    dst = 0.;
+    f_t_mat.Tvmult(temp_vect_1,src);
     d_8_mat.vmult(temp_vect_2, temp_vect_1);
     f_mat.vmult(dst,temp_vect_2);
 }
@@ -953,7 +920,7 @@ HMatrix<dim>::HMatrix(LA::MPI::SparseMatrix &a_mat_in, const LA::MPI::SparseMatr
 
 template<int dim>
 void
-HMatrix<dim>::initialize(LA::MPI::Vector &exemplar_density_vector,  LA::MPI::Vector &exemplar_displacement_vector)
+HMatrix<dim>::initialize(LA::MPI::Vector &exemplar_density_vector,  LA::MPI::Vector &exemplar_displacement_vector, LA::MPI::SparseMatrix &d_m_inv_mat_in)
 {
     temp_vect_1 = exemplar_displacement_vector;
     temp_vect_2 = exemplar_displacement_vector;
@@ -963,6 +930,16 @@ HMatrix<dim>::initialize(LA::MPI::Vector &exemplar_density_vector,  LA::MPI::Vec
     temp_vect_6 = exemplar_density_vector;
     temp_vect_7 = exemplar_density_vector;
 
+    temp_vect_1 = 0.;
+    temp_vect_2 = 0.;
+    temp_vect_3 = 0.;
+    temp_vect_4 = 0.;
+    temp_vect_5 = 0.;
+    temp_vect_6 = 0.;
+    temp_vect_7 = 0.;
+
+    d_m_inv_mat.copy_from(d_m_inv_mat_in);
+
 
 }
 
@@ -974,9 +951,6 @@ void HMatrix<dim>::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
         c_mat.vmult(temp_vect_1,src);
         e_mat.vmult(temp_vect_2,src);
 
-        SolverControl            solver_control_1(Input::a_inv_iterations, temp_vect_1.l2_norm()*1e-6);
-        SolverCG<LA::MPI::Vector> a_solver_cg_1(solver_control_1);
-
         try
         {
             a_inv_mf_gmg.vmult(temp_vect_3,temp_vect_1);
@@ -984,8 +958,6 @@ void HMatrix<dim>::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
         {
 
         }
-        SolverControl            solver_control_2(Input::a_inv_iterations, temp_vect_2.l2_norm()*1e-6);
-        SolverCG<LA::MPI::Vector> a_solver_cg_2(solver_control_2);
         try
         {
             a_inv_mf_gmg.vmult(temp_vect_4,temp_vect_2);
@@ -1015,8 +987,6 @@ void HMatrix<dim>::vmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) const
         dst =  temp_vect_7 - temp_vect_6 - temp_vect_5;
     }
 
-
-
 }
 
 
@@ -1028,8 +998,6 @@ void HMatrix<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) cons
         c_mat.vmult(temp_vect_1,src);
         e_mat.vmult(temp_vect_2,src);
 
-        // SolverControl            solver_control_1(Input::a_inv_iterations, temp_vect_1.l2_norm()*1e-6);
-        // SolverCG<LA::MPI::Vector> a_solver_cg_1(solver_control_1);
 
         try
         {
@@ -1039,8 +1007,6 @@ void HMatrix<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) cons
         {
 
         }
-        SolverControl            solver_control_2(Input::a_inv_iterations, temp_vect_2.l2_norm()*1e-6);
-        SolverCG<LA::MPI::Vector> a_solver_cg_2(solver_control_2);
         try
         {
             a_inv_mf_gmg.vmult(temp_vect_4,temp_vect_2);
@@ -1147,24 +1113,24 @@ void HMatrixDirect<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src
 
 // ******************     JinvMatrix     ***********************
 template<int dim>
-JinvMatrix<dim>::JinvMatrix(HMatrix<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in)
+JinvMatrix<dim>::JinvMatrix(HMatrix<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in)
     :
       h_mat(h_mat_in),
       g_mat(g_mat_in),
-      d_m_mat(d_m_mat_in),
-      d_m_inv_mat(d_m_inv_mat_in)
+      d_m_mat(d_m_mat_in)
 {
 
 }
 
 template<int dim>
 void
-JinvMatrix<dim>::initialize(LA::MPI::Vector &exemplar_density_vector)
+JinvMatrix<dim>::initialize(LA::MPI::Vector &exemplar_density_vector, LA::MPI::SparseMatrix &d_m_inv_mat_in)
 {
     temp_vect_1 = exemplar_density_vector;
     temp_vect_2 = exemplar_density_vector;
     temp_vect_3 = exemplar_density_vector;
     temp_vect_4 = exemplar_density_vector;
+    d_m_inv_mat.copy_from(d_m_inv_mat_in);
 
 }
 
@@ -1192,12 +1158,11 @@ void JinvMatrix<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) c
 
 // ******************     JinvMatrixPart     ***********************
 template<int dim>
-JinvMatrixPart<dim>::JinvMatrixPart(HMatrix<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in)
+JinvMatrixPart<dim>::JinvMatrixPart(HMatrix<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in)
     :
       h_mat(h_mat_in),
       g_mat(g_mat_in),
-      d_m_mat(d_m_mat_in),
-      d_m_inv_mat(d_m_inv_mat_in)
+      d_m_mat(d_m_mat_in)
 {
 }
 
@@ -1234,12 +1199,11 @@ void JinvMatrixPart<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &sr
 
 // ******************     JinvMatrixDirect     ***********************
 template<int dim>
-JinvMatrixDirect<dim>::JinvMatrixDirect(HMatrixDirect<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in)
+JinvMatrixDirect<dim>::JinvMatrixDirect(HMatrixDirect<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in)
     :
       h_mat(h_mat_in),
       g_mat(g_mat_in),
-      d_m_mat(d_m_mat_in),
-      d_m_inv_mat(d_m_inv_mat_in)
+      d_m_mat(d_m_mat_in)
 {
 
 }
@@ -1280,25 +1244,32 @@ void JinvMatrixDirect<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &
 
 // ******************     KinvMatrix     ***********************
 template<int dim>
-KinvMatrix<dim>::KinvMatrix(HMatrix<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in)
+KinvMatrix<dim>::KinvMatrix(HMatrix<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in)
     :
       h_mat(h_mat_in),
       g_mat(g_mat_in),
-      d_m_mat(d_m_mat_in),
-      d_m_inv_mat(d_m_inv_mat_in)
+      d_m_mat(d_m_mat_in)
 {
 
 }
 
 template<int dim>
 void
-KinvMatrix<dim>::initialize(LA::MPI::Vector &exemplar_density_vector)
+KinvMatrix<dim>::initialize(LA::MPI::Vector &exemplar_density_vector, LA::MPI::SparseMatrix &d_m_inv_mat_in)
 {
+
     temp_vect_1 = exemplar_density_vector;
     temp_vect_2 = exemplar_density_vector;
     temp_vect_3 = exemplar_density_vector;
     temp_vect_4 = exemplar_density_vector;
 
+    temp_vect_1 = 0.;
+    temp_vect_2 = 0.;
+    temp_vect_3 = 0.;
+    temp_vect_4 = 0.;
+
+    d_m_inv_mat.copy_from(d_m_inv_mat_in);
+
 }
 
 template<int dim>
@@ -1325,12 +1296,11 @@ void KinvMatrix<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &src) c
 
 // ******************     KinvMatrixPart    ***********************
 template<int dim>
-KinvMatrixPart<dim>::KinvMatrixPart(HMatrix<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in)
+KinvMatrixPart<dim>::KinvMatrixPart(HMatrix<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in)
     :
       h_mat(h_mat_in),
       g_mat(g_mat_in),
-      d_m_mat(d_m_mat_in),
-      d_m_inv_mat(d_m_inv_mat_in)
+      d_m_mat(d_m_mat_in)
 {
 
 }
@@ -1378,12 +1348,11 @@ void KinvMatrixPart<dim>::Tvmult(LA::MPI::Vector &dst, const LA::MPI::Vector &sr
 
 // ******************     KinvMatrixDirect     ***********************
 template<int dim>
-KinvMatrixDirect<dim>::KinvMatrixDirect(HMatrixDirect<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in, LA::MPI::SparseMatrix &d_m_inv_mat_in)
+KinvMatrixDirect<dim>::KinvMatrixDirect(HMatrixDirect<dim> &h_mat_in, GMatrix &g_mat_in, const LA::MPI::SparseMatrix &d_m_mat_in)
     :
       h_mat(h_mat_in),
       g_mat(g_mat_in),
-      d_m_mat(d_m_mat_in),
-      d_m_inv_mat(d_m_inv_mat_in)
+      d_m_mat(d_m_mat_in)
 {
 
 }