diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml
new file mode 100644
index 00000000..05dbd71c
--- /dev/null
+++ b/.github/workflows/main.yml
@@ -0,0 +1,55 @@
+name: Build and Test Fatropy
+
+on:
+  push:
+    branches: [ main ]
+  pull_request:
+    branches: [ main ]
+
+jobs:
+  build:
+    runs-on: ubuntu-latest
+
+    steps:
+      - name: Checkout code
+        uses: actions/checkout@v2
+  
+      - name: Set up Python
+        uses: actions/setup-python@v2
+        with:
+          python-version: '3.x'
+  
+      - name: Cache pip dependencies
+        uses: actions/cache@v2
+        with:
+          path: ~/.cache/pip
+          key: ${{ runner.os }}-pip-${{ hashFiles('**/requirements.txt') }}
+          restore-keys: |
+            ${{ runner.os }}-pip-
+  
+      - name: Install virtualenv and activate
+        run: |
+          pip install virtualenv
+          virtualenv venv
+          source venv/bin/activate
+
+      - name: Build and install Fatropy
+        run: |
+          git submodule update --init --recursive
+          sudo apt-get update && sudo apt-get install -y cmake
+          export CMAKE_ARGS="-DBLASFEO_TARGET=X64_AUTOMATIC -DENABLE_MULTITHREADING=OFF"
+          cd fatropy
+          pip install .
+          
+      - name: Install Rockit and Rockit-Fatrop plugin
+        run: |
+          git clone https://gitlab.kuleuven.be/meco-software/rockit.git
+          git clone https://gitlab.kuleuven.be/u0110259/rockit_fatrop_plugin.git ./rockit/rockit/external/fatrop
+          cd rockit
+          pip install .
+
+      - name: Test Fatropy
+        run: |
+          git clone https://gitlab.kuleuven.be/robotgenskill/fatrop/fatrop_rockit_demo.git
+          python fatrop_rockit_demo/tutorial.py
+          python fatropy/tests/example.py
diff --git a/CMakeLists.txt b/CMakeLists.txt
index c89cb86a..3aac940c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,6 +1,6 @@
 #[[
 Fatrop - A fast trajectory optimization solver
-Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
 
 This file is part of Fatrop.
 
@@ -104,7 +104,7 @@ if(${WITH_SPECTOOL})
   # check if casadi/core/function_internal.hpp is available in the casadi path (CASADI_INCLUDE_DIR is set by findCasadi.cmake, and can contain multiple paths)
   message (STATUS "CASADI_INCLUDE_DIR is ${CASADI_INCLUDE_DIR}")
   if(NOT EXISTS "${CASADI_INCLUDE_DIR}/casadi/core/function_internal.hpp")
-    message(SEND_ERROR "${CASADI_INCLUDE_DIR}/casadi/core/function_internal.hpp not found, please install casadi with cmake option -DINSTALL_INTERNAL_HEADERS=ON") 
+    message(SEND_ERROR "${CASADI_INCLUDE_DIR}/casadi/core/function_internal.hpp not found, please install casadi with cmake option -DINSTALL_INTERNAL_HEADERS=ON")
   endif()
 
   set(INSTALL_TARGETS ${INSTALL_TARGETS} spectool)
@@ -121,7 +121,16 @@ endif()
 # endif()
 set(WITH_PYTHON OFF CACHE BOOL "Build python bindings (fatropy)")
 
-if(${WITH_PYTHON})
+if(WITH_PYTHON)
+  if(CMAKE_VERSION VERSION_LESS "3.18")
+    find_package(Python REQUIRED COMPONENTS Interpreter Development)
+  else()
+    find_package(Python REQUIRED COMPONENTS Interpreter Development.Module)
+  endif()
+  find_package(pybind11 CONFIG)
+  if(NOT pybind11_FOUND)
+    add_subdirectory(external/pybind11)
+  endif()
   add_subdirectory(fatropy)
 endif()
 
@@ -136,4 +145,4 @@ INSTALL(TARGETS ${INSTALL_TARGETS}
   INCLUDES DESTINATION ${INCLUDE_INSTALL_DIR}
   RUNTIME DESTINATION ${RUNTIME_INSTALL_DIR}
 )
-INSTALL(EXPORT fatropConfig NAMESPACE fatrop:: DESTINATION cmake)
\ No newline at end of file
+INSTALL(EXPORT fatropConfig NAMESPACE fatrop:: DESTINATION cmake)
diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md
new file mode 100644
index 00000000..47b6a0bb
--- /dev/null
+++ b/CONTRIBUTING.md
@@ -0,0 +1,27 @@
+# Contributing to fatrop
+
+Thank you for your interest in contributing to fatrop! We welcome contributions from the community and appreciate your efforts to make this project better. Before you start contributing, please take note of the following guidelines:
+
+## License
+This project is licensed under the LGPLv3 (Lesser General Public License version 3). By contributing to this project, you agree that your contributions will also be licensed under the LGPLv3.
+
+
+## Contributing
+To contribute, please follow these steps:
+1. Fork the repository.
+2. Create a new branch for your feature or bugfix.
+3. Make your changes in the new branch.
+4. Ensure your code adheres to the project's coding standards and passes all tests.
+5. Submit a pull request to the `main` branch with a clear description of your changes and the problem they solve.
+
+## License Modifications
+Please be aware that it is not possible to modify the license of this project. All contributions will be governed by the terms of the LGPLv3.
+
+## Copyright
+By submitting a pull request, you implicitly hand over the copyright of your contribution to the original copyright holders of this project. The copyright holders are mentioned in the copyright statements of each file.
+
+## Additional Note
+If you are introducing a significant feature or change, consider discussing it in an issue first to gather feedback and ensure alignment with the project's goals.
+
+## Feature Requests
+Feature requests can be made by emailing lander.vanroye@kuleuven.be. We appreciate your ideas and feedback to help improve fatrop.
\ No newline at end of file
diff --git a/README.md b/README.md
index f7588389..248b0a03 100644
--- a/README.md
+++ b/README.md
@@ -1,6 +1,6 @@
 <!--
 Fatrop - A fast trajectory optimization solver
-Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
 
 This file is part of Fatrop.
 
@@ -56,6 +56,7 @@ At this moment Fatrop is mainly tested on Ubuntu Linux machines. There are two i
   - [Build and install Fatrop only](#build-and-install-fatrop-only)
   - [Install rockit with Fatropy interface](#install-rockit-with-fatropy-interface)
   - [Examples](#examples)
+- [Citing](#citing)
 - [Developers](#developers)
 
 ## Build and install Fatrop and Fatropy
@@ -110,6 +111,22 @@ https://gitlab.kuleuven.be/robotgenskill/fatrop/fatrop_benchmarks
 
 Using Fatrop from C++: check file `fatrop/executables/RunFatrop.cpp`
 
+# Citing
+To cite Fatrop in your academic work, please use the following reference:
+
+```
+@inproceedings{vanroye2023fatrop,
+  title={Fatrop: A fast constrained optimal control problem solver for robot trajectory optimization and control},
+  author={Vanroye, Lander and Sathya, Ajay and De Schutter, Joris and Decr{\'e}, Wilm},
+  booktitle={2023 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
+  pages={10036--10043},
+  year={2023},
+  organization={IEEE}
+}
+```
+
+
+
 # Developers
 
 Fatrop is developed by [Lander Vanroye](https://www.kuleuven.be/wieiswie/en/person/00116913) at the [KU Leuven Robotics Research Group](https://www.mech.kuleuven.be/robotics) under supervision of [Wilm Decre](https://www.kuleuven.be/wieiswie/en/person/00052672).
diff --git a/compilation_instructions.md b/compilation_instructions.md
new file mode 100644
index 00000000..199e1e87
--- /dev/null
+++ b/compilation_instructions.md
@@ -0,0 +1,34 @@
+### Compilation instructions for fatrop python bindings with spectool
+
+Install dependencies:
+
+        sudo apt install git wget gfortran liblapack-dev pkg-config cmake gcc swig g++ pybind11-dev
+
+(recommended) create and activate a virtual environment:
+
+        python3 -m venv your_env_name
+        source your_env_name/bin/activate
+
+Update pip and setuptools:
+
+        pip install --upgrade pip setuptools
+
+find the python site packages directory:
+
+        export PYTHON_SITE_PACKAGES=$(python -c "from distutils.sysconfig import get_python_lib; print(get_python_lib())")
+
+Install casadi with python bindings from source (note that this will install casadi on your system, consider using a cmake prefix to install it in a specific directory):
+
+    git clone https://github.com/casadi/casadi.git 
+    mkdir casadi/build && cd casadi/build
+    cmake \
+        -DWITH_IPOPT=OFF -DWITH_BUILD_IPOPT=OFF\
+        -DWITH_BUILD_MUMPS=OFF -DWITH_BUILD_METIS=OFF\
+        -DWITH_OPENMP=OFF -DWITH_THREAD=OFF -DWITH_PYTHON=ON -DWITH_PYTHON3=ON -DPYTHON_PREFIX=$PYTHON_SITE_PACKAGES\
+        -DINSTALL_INTERNAL_HEADERS=ON ..
+    make -j && sudo make install
+
+Install fatropy with spectool from source:
+
+    git clone https://github.com/meco-group/fatrop.git --recursive
+    export CMAKE_ARGS="-DBLASFEO_TARGET=X64_AUTOMATIC -DENABLE_MULTITHREADING=OFF -DWITH_SPECTOOL=ON" && cd fatrop/fatropy && python -m pip install .
diff --git a/docs/CMakeLists.txt b/docs/CMakeLists.txt
index 4c1555e7..ba31c666 100644
--- a/docs/CMakeLists.txt
+++ b/docs/CMakeLists.txt
@@ -1,6 +1,6 @@
 #[[
 Fatrop - A fast trajectory optimization solver
-Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
 
 This file is part of Fatrop.
 
diff --git a/external/CMakeLists.txt b/external/CMakeLists.txt
index c82a8bde..c1d8e8cd 100644
--- a/external/CMakeLists.txt
+++ b/external/CMakeLists.txt
@@ -1,6 +1,6 @@
 #[[
 Fatrop - A fast trajectory optimization solver
-Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
 
 This file is part of Fatrop.
 
@@ -30,27 +30,10 @@ if (BUILD_WITH_BLASFEO)
     add_subdirectory(blasfeo)
 
 else()
-    set(BLASFEO_INSTALL_DIR "/opt/blasfeo" CACHE PATH "Path to blasfeo installation directory")
-    # find_library(blasfeo NAMES blasfeo PATHS ${BLASFEO_INSTALL_DIR}/lib)
+    find_package(blasfeo REQUIRED)
 
-    find_path(BLASFEO_INCLUDE_DIR NAMES "blasfeo.h" PATHS ${BLASFEO_INSTALL_DIR}/include)
-    find_library(BLASFEO_LIBRARY NAMES blasfeo HINTS ${BLASFEO_INSTALL_DIR}/lib)
-
-    # Create an imported static target for blasfeo
-    add_library(blasfeo STATIC IMPORTED GLOBAL)
-    # Set the path to the library file
-    set_target_properties(blasfeo PROPERTIES IMPORTED_LOCATION ${BLASFEO_LIBRARY})
-
-    # Set the path to the include directory
-    set_target_properties(blasfeo PROPERTIES INTERFACE_INCLUDE_DIRECTORIES ${BLASFEO_INCLUDE_DIR})
-    
-endif()
-
-if(WITH_PYTHON)
-    # add pybind11 for python bindings
-    add_subdirectory(pybind11)
 endif()
 
 if(WIN32)
     add_subdirectory(dlfcn-win32)
-endif(WIN32)
\ No newline at end of file
+endif(WIN32)
diff --git a/fatrop/CMakeLists.txt b/fatrop/CMakeLists.txt
index c354daa6..d46d3cd1 100644
--- a/fatrop/CMakeLists.txt
+++ b/fatrop/CMakeLists.txt
@@ -1,6 +1,6 @@
 #[[
 Fatrop - A fast trajectory optimization solver
-Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
 
 This file is part of Fatrop.
 
@@ -18,6 +18,7 @@ You should have received a copy of the GNU Lesser General Public License
 along with Fatrop.  If not, see <http://www.gnu.org/licenses/>.]]
 set(FATROP_INTERNAL
 blasfeo_wrapper/LinearAlgebraBlasfeo.hpp    blasfeo_wrapper/LinearAlgebraBlasfeo.cpp
+blasfeo_wrapper/blasfeo_ref.cpp
 function_evaluation/CasadiCodegen.hpp       function_evaluation/CasadiCodegen.cpp
 auxiliary/VectorUtils.hpp                   auxiliary/VectorUtils.cpp
 auxiliary/Common.hpp                        auxiliary/Common.cpp
@@ -41,6 +42,7 @@ ocp/OCPLinearSolver.hpp                     ocp/OCPLinearSolver.cpp
 ocp/OCPLSRiccati.hpp                        ocp/OCPLSRiccati.cpp
 ocp/OCPNoScaling.hpp                        ocp/OCPNoScaling.cpp
 ocp/OCPScalingMethod.hpp                    ocp/OCPScalingMethod.cpp
+ocp/FatropOCPResto.hpp
 solver/AlgBuilder.hpp              
 solver/AlgStrategy.hpp                      solver/AlgStrategy.cpp
 solver/FatropAlg.hpp                        solver/FatropAlg.cpp
@@ -82,6 +84,7 @@ ocp/OCPLinearSolver.hpp
 ocp/OCPLSRiccati.hpp
 ocp/OCPNoScaling.hpp
 ocp/OCPScalingMethod.hpp
+ocp/FatropOCPResto.hpp
 solver/AlgorithmQuantities.hpp
 solver/AlgBuilder.hpp
 solver/AlgStrategy.hpp
@@ -144,6 +147,8 @@ install(DIRECTORY "${CMAKE_SOURCE_DIR}/fatrop/${incl}" # source directory
         PATTERN "*.h" # select C header files
 )
 endforeach()
+install(FILES fatrop.hpp
+        DESTINATION "include/fatrop/")
 
 set(INCLUDE_INSTALL_DIR "include")
 set(LIBRARY_INSTALL_DIR "lib")
diff --git a/fatrop/auxiliary/Common.cpp b/fatrop/auxiliary/Common.cpp
index 6ac5acce..50d2b1c5 100644
--- a/fatrop/auxiliary/Common.cpp
+++ b/fatrop/auxiliary/Common.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
@@ -17,3 +17,21 @@
  * You should have received a copy of the GNU Lesser General Public License
  * along with Fatrop.  If not, see <http://www.gnu.org/licenses/>. */
 #include "fatrop/auxiliary/Common.hpp"
+#include <limits>
+#include <climits>
+#include <cstdlib>
+#include <algorithm>
+
+namespace fatrop
+{
+    bool CompareLessEqual(double lhs, double rhs)
+    {
+        double mach_eps = std::numeric_limits<double>::epsilon();
+        return (lhs - rhs <= 10. * mach_eps * std::max(std::abs(rhs), std::abs(lhs)));
+    }
+    bool CompareLessEqual(double lhs, double rhs, double ref)
+    {
+        double mach_eps = std::numeric_limits<double>::epsilon();
+        return (lhs - rhs <= 10. * mach_eps * std::abs(ref));
+    }
+}
\ No newline at end of file
diff --git a/fatrop/auxiliary/Common.hpp b/fatrop/auxiliary/Common.hpp
index 36731b25..637eb50e 100644
--- a/fatrop/auxiliary/Common.hpp
+++ b/fatrop/auxiliary/Common.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
@@ -28,6 +28,7 @@
 #define fatrop_int int
 namespace fatrop
 {
- 
+    bool CompareLessEqual(double lhs, double rhs);
+    bool CompareLessEqual(double lhs, double rhs, double ref);
 } // namespace fatrop
 #endif //  COMMONINCLUDED
\ No newline at end of file
diff --git a/fatrop/auxiliary/DynamicLib.cpp b/fatrop/auxiliary/DynamicLib.cpp
index fea7f230..2361a4e7 100644
--- a/fatrop/auxiliary/DynamicLib.cpp
+++ b/fatrop/auxiliary/DynamicLib.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/auxiliary/DynamicLib.hpp b/fatrop/auxiliary/DynamicLib.hpp
index 2554a43f..b7683f2e 100644
--- a/fatrop/auxiliary/DynamicLib.hpp
+++ b/fatrop/auxiliary/DynamicLib.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/auxiliary/FatropOptions.hpp b/fatrop/auxiliary/FatropOptions.hpp
index 4b53345d..1b1d6f61 100644
--- a/fatrop/auxiliary/FatropOptions.hpp
+++ b/fatrop/auxiliary/FatropOptions.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/auxiliary/FatropVector.cpp b/fatrop/auxiliary/FatropVector.cpp
index 1ebbf028..7b77004e 100644
--- a/fatrop/auxiliary/FatropVector.cpp
+++ b/fatrop/auxiliary/FatropVector.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/auxiliary/FatropVector.hpp b/fatrop/auxiliary/FatropVector.hpp
index bdf613e8..69acc43e 100644
--- a/fatrop/auxiliary/FatropVector.hpp
+++ b/fatrop/auxiliary/FatropVector.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/auxiliary/LinearAlgebra.cpp b/fatrop/auxiliary/LinearAlgebra.cpp
index 0e83133a..5df9cba4 100644
--- a/fatrop/auxiliary/LinearAlgebra.cpp
+++ b/fatrop/auxiliary/LinearAlgebra.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/auxiliary/LinearAlgebra.hpp b/fatrop/auxiliary/LinearAlgebra.hpp
index 52247543..858625c2 100644
--- a/fatrop/auxiliary/LinearAlgebra.hpp
+++ b/fatrop/auxiliary/LinearAlgebra.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/auxiliary/VectorUtils.cpp b/fatrop/auxiliary/VectorUtils.cpp
index 627036e7..9a0e1dee 100644
--- a/fatrop/auxiliary/VectorUtils.cpp
+++ b/fatrop/auxiliary/VectorUtils.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/auxiliary/VectorUtils.hpp b/fatrop/auxiliary/VectorUtils.hpp
index 310030e6..3a784698 100644
--- a/fatrop/auxiliary/VectorUtils.hpp
+++ b/fatrop/auxiliary/VectorUtils.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/blasfeo_wrapper/LinearAlgebraBlasfeo.cpp b/fatrop/blasfeo_wrapper/LinearAlgebraBlasfeo.cpp
index 9974c21b..c8fee0ca 100644
--- a/fatrop/blasfeo_wrapper/LinearAlgebraBlasfeo.cpp
+++ b/fatrop/blasfeo_wrapper/LinearAlgebraBlasfeo.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
@@ -36,118 +36,6 @@ namespace fatrop
         }
     }
 
-    // void fatrop_potrf_l_mn(fatrop_int m, fatrop_int n, struct blasfeo_dmat *sC, fatrop_int ci, fatrop_int cj, struct blasfeo_dmat *sD, fatrop_int di, fatrop_int dj)
-    // {
-    //     blasfeo_dpotrf_l_mn(m, n, sC, ci, cj, sD, di, dj);
-    //     fatrop_int minmn = (m < n) ? m : n;
-    //     for (fatrop_int i =0; i<minmn; i++){
-    //         assert(MATEL(sD, di+i,dj+i)>0);
-    //     }
-    // }
-    /** \brief D <= alpha * B * A^{-1} , with A lower triangular employing explicit inverse of diagonal, fatrop uses its own (naive) implementation since it  not implemented yet in blasfeo*/
-    void fatrop_dtrsm_rlnn(fatrop_int m, fatrop_int n, double alpha, MAT *sA, fatrop_int offs_ai, fatrop_int offs_aj, MAT *sB, fatrop_int offs_bi, fatrop_int offs_bj, MAT *sD, fatrop_int offs_di, fatrop_int offs_dj)
-    {
-        sD->use_dA = 0;
-        for (fatrop_int aj = n - 1; aj >= 0; aj--)
-        {
-            double ajj = MATEL(sA, offs_ai + aj, aj + offs_aj);
-            double inv_ajj = 1.0 / ajj;
-            double scjj = alpha * inv_ajj;
-            for (fatrop_int k = 0; k < m; k++)
-            {
-                MATEL(sD, offs_di + k, offs_dj + aj) = scjj * MATEL(sB, offs_bi + k, offs_bj + aj);
-            }
-            for (fatrop_int ai = aj + 1; ai < n; ai++)
-            {
-                double sc = -inv_ajj * MATEL(sA, offs_ai + ai, offs_aj + aj);
-                for (fatrop_int k = 0; k < m; k++)
-                {
-                    // this algorithm is "store bounded", the loops can be switched like in the alt version to make this more efficient
-                    MATEL(sD, offs_di + k, offs_dj + aj) += sc * MATEL(sD, offs_di + k, offs_dj + ai);
-                }
-            }
-        }
-    }
-    // /** \brief D <= alpha * B * A^{-1} , with A lower triangular employing explicit inverse of diagonal, fatrop uses its own (naive) implementation since it  not implemented yet in blasfeo*/
-    // // this is an experimental, more efficient, but the corner cases are not treated in unrolled loop!! it achieves a speed-up of about factor 3 w.r.t naive implementation
-    void fatrop_dtrsm_rlnn_alt(fatrop_int m, fatrop_int n, double alpha, MAT *sA, fatrop_int offs_ai, fatrop_int offs_aj, MAT *sB, fatrop_int offs_bi, fatrop_int offs_bj, MAT *sD, fatrop_int offs_di, fatrop_int offs_dj)
-    {
-        for (fatrop_int aj = n - 1; aj >= 0; aj--)
-        {
-            double ajj = MATEL(sA, offs_ai + aj, aj + offs_aj);
-            double inv_ajj = 1.0 / ajj;
-            double scjj = alpha * inv_ajj;
-            for (fatrop_int k = 0; k < m; k++)
-            {
-                // todo, check if possible to incude in main loop
-                MATEL(sD, offs_di + k, offs_dj + aj) = scjj * MATEL(sB, offs_bi + k, offs_bj + aj);
-            }
-            for (fatrop_int k = 0; k < m; k++)
-            {
-                double res = 0.0;
-                // double res4 = 0.0;
-                // double res5 = 0.0;
-                // double res6 = 0.0;
-                // double res7 = 0.0;
-                for (fatrop_int ai = aj + 1; ai < n; ai = ai + 1)
-                {
-                    // todo unroll loop -> more independent operations -> filled pipelines
-                    double sc = -inv_ajj * MATEL(sA, offs_ai + ai, offs_aj + aj);
-                    res += sc * MATEL(sD, offs_di + k, offs_dj + ai);
-                }
-                MATEL(sD, offs_di + k, offs_dj + aj) += res;
-            }
-        }
-    }
-    // /** \brief D <= alpha * B * A^{-1} , with A lower triangular employing explicit inverse of diagonal, fatrop uses its own (naive) implementation since it  not implemented yet in blasfeo*/
-    // // this is an experimental, more efficient, but the corner cases are not treated in unrolled loop!! it achieves a speed-up of about factor 3 w.r.t naive implementation
-    // void fatrop_dtrsm_rlnn_alt(fatrop_int m, fatrop_int n, double alpha, MAT *sA, fatrop_int offs_ai, fatrop_int offs_aj, MAT *sB, fatrop_int offs_bi, fatrop_int offs_bj, MAT *sD, fatrop_int offs_di, fatrop_int offs_dj)
-    // {
-    //     for (fatrop_int aj = n - 1; aj >= 0; aj--)
-    //     {
-    //         double ajj = MATEL(sA, offs_ai + aj, aj + offs_aj);
-    //         double inv_ajj = 1.0 / ajj;
-    //         double scjj = alpha * inv_ajj;
-    //         for (fatrop_int k = 0; k < m; k++)
-    //         {
-    //             // todo, check if possible to incude in main loop
-    //             MATEL(sD, offs_di + k, offs_dj + aj) = scjj * MATEL(sB, offs_bi + k, offs_bj + aj);
-    //         }
-    //         for (fatrop_int k = 0; k < m; k++)
-    //         {
-    //             double res = 0.0;
-    //             double res1 = 0.0;
-    //             double res2 = 0.0;
-    //             double res3 = 0.0;
-    //             // double res4 = 0.0;
-    //             // double res5 = 0.0;
-    //             // double res6 = 0.0;
-    //             // double res7 = 0.0;
-    //             for (fatrop_int ai = aj + 1; ai < n; ai = ai + 4)
-    //             {
-    //                 // todo unroll loop -> more independent operations -> filled pipelines
-    //                 double sc = -inv_ajj * MATEL(sA, offs_ai + ai, offs_aj + aj);
-    //                 res += sc * MATEL(sD, offs_di + k, offs_dj + ai);
-    //                 double sc1 = -inv_ajj * MATEL(sA, offs_ai + ai + 1, offs_aj + aj);
-    //                 res1 += sc1 * MATEL(sD, offs_di + k, offs_dj + ai + 1);
-    //                 double sc2 = -inv_ajj * MATEL(sA, offs_ai + ai + 2, offs_aj + aj);
-    //                 res2 += sc2 * MATEL(sD, offs_di + k, offs_dj + ai + 2);
-    //                 double sc3 = -inv_ajj * MATEL(sA, offs_ai + ai + 3, offs_aj + aj);
-    //                 res3 += sc3 * MATEL(sD, offs_di + k, offs_dj + ai + 3);
-    //                 // double sc4 = -inv_ajj * MATEL(sA, offs_ai + ai+4, offs_aj + aj);
-    //                 // res4 += sc * MATEL(sD, offs_di + k, offs_dj + ai+4);
-    //                 // double sc5 = -inv_ajj * MATEL(sA, offs_ai + ai+5, offs_aj + aj);
-    //                 // res5 += sc * MATEL(sD, offs_di + k, offs_dj + ai+5);
-    //                 // double sc6 = -inv_ajj * MATEL(sA, offs_ai + ai+6, offs_aj + aj);
-    //                 // res6 += sc * MATEL(sD, offs_di + k, offs_dj + ai+6);
-    //                 // double sc7 = -inv_ajj * MATEL(sA, offs_ai + ai+7, offs_aj + aj);
-    //                 // res7 += sc * MATEL(sD, offs_di + k, offs_dj + ai+7);
-    //             }
-    //             // MATEL(sD, offs_di + k, offs_dj + aj) += (res+res1+res2+res3+res4+res5+res6+res7);
-    //             MATEL(sD, offs_di + k, offs_dj + aj) += (res + res1) + (res2 + res3);
-    //         }
-    //     }
-    // }
     // B <= B + alpha*A^T (B is mxn)
     void fatrop_dgead_transposed(fatrop_int m, fatrop_int n, double alpha, struct blasfeo_dmat *sA, fatrop_int offs_ai, fatrop_int offs_aj, struct blasfeo_dmat *sB, fatrop_int offs_bi, fatrop_int offs_bj)
     {
@@ -374,6 +262,18 @@ namespace fatrop
         }
         return res;
     };
+    double sumsqr(const FatropVecBF &va)
+    {
+        VEC *va_p = (VEC *)va;
+        fatrop_int nels = va.nels();
+        fatrop_int offset = va.offset();
+        double res = 0.0;
+        for (fatrop_int i = offset; i < nels + offset; i++)
+        {
+            res += VECEL(va_p, i) * VECEL(va_p, i);
+        }
+        return res;
+    };
 } // namespace fatrop
 using namespace fatrop;
 FatropMatBF::FatropMatBF(const fatrop_int nrows, const fatrop_int ncols, const fatrop_int row_offset, const fatrop_int col_offset) : row_offset_(row_offset), col_offset_(col_offset), nrows_(nrows), ncols_(ncols)
@@ -539,7 +439,7 @@ void FatropVecBF::operator=(const FatropVec &fm)
         this->at(ai) = fm.get_el(ai);
     }
 }
-void FatropVecBF::copy(const FatropVecBF &fm)
+void FatropVecBF::copy(const FatropVecBF &fm) const
 {
     DBGASSERT(fm.nels() == nels());
     VEC *fm_p = (VEC *)fm;
diff --git a/fatrop/blasfeo_wrapper/LinearAlgebraBlasfeo.hpp b/fatrop/blasfeo_wrapper/LinearAlgebraBlasfeo.hpp
index 1cdb82a0..63265aa2 100644
--- a/fatrop/blasfeo_wrapper/LinearAlgebraBlasfeo.hpp
+++ b/fatrop/blasfeo_wrapper/LinearAlgebraBlasfeo.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
@@ -93,7 +93,8 @@ extern "C"
 #if defined(BLASFEO_REF_API)
 #define TRSM_RLNN blasfeo_dtrsm_rlnn
 #else
-#define TRSM_RLNN fatrop_dtrsm_rlnn_alt
+void blasfeo_ref_dtrsm_rlnn_copy(int m, int n, double alpha, struct MAT *sA, int ai, int aj, struct MAT *sB, int bi, int bj, struct MAT *sD, int di, int dj);
+#define TRSM_RLNN blasfeo_ref_dtrsm_rlnn_copy
 #endif
 
 #ifndef __GNUC__
@@ -325,7 +326,7 @@ namespace fatrop
             blasfeo_dvecse(nels(), val, vec_, offset());
         }
 
-        void copy(const FatropVecBF &fm);
+        void copy(const FatropVecBF &fm) const;
         void copyto(std::vector<double> &dest) const;
         void operator=(const std::vector<double> &fm);
         /** \brief set data pointer*/
@@ -380,6 +381,7 @@ namespace fatrop
     double LinfScaled(const FatropVecBF &va, const FatropVecBF &scales);
     double minabs(const FatropVecBF &va);
     double L1(const FatropVecBF &va);
+    double sumsqr(const FatropVecBF &va);
 
     /** \brief this class is used for the allocation of a blasfeo vector, the dimsensions are set from a vector */
     class FatropMemoryVecBF
diff --git a/fatrop/blasfeo_wrapper/blasfeo_ref.cpp b/fatrop/blasfeo_wrapper/blasfeo_ref.cpp
new file mode 100644
index 00000000..a79da738
--- /dev/null
+++ b/fatrop/blasfeo_wrapper/blasfeo_ref.cpp
@@ -0,0 +1,168 @@
+/**
+*  THIS FILE IS MODIFILED FROM blasfeo_ref/x_blas_ref3.c from the BLASFEO library.
+
+WITH THE FOLLOWING COPYRIGHT NOTICE:
+
+
+
+BLASFEO -- BLAS For Embedded Optimization.
+Copyright (C) 2019 by Gianluca Frison.
+Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.
+All rights reserved.
+
+The 2-Clause BSD License
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+**/
+
+extern "C"
+{
+#include <blasfeo.h>
+}
+#include <iostream>
+
+#if defined(BLASFEO_REF_API)
+#else
+#define MATEL BLASFEO_DMATEL
+#define MAT blasfeo_dmat
+#define VEC blasfeo_dvec
+void blasfeo_ref_dtrsm_rlnn_copy(int m, int n, double alpha, struct MAT *sA, int ai, int aj, struct MAT *sB, int bi, int bj, struct MAT *sD, int di, int dj)
+{
+    if (m <= 0 | n <= 0)
+        return;
+
+    // invalidate stored inverse diagonal of result matrix
+    sD->use_dA = 0;
+
+    char cl = 'l';
+    char cn = 'n';
+    char cr = 'r';
+    char ct = 't';
+    char cu = 'u';
+    int i1 = 1;
+    int ii, jj, kk, id;
+    double
+        d_00,
+        d_01,
+        d_10, d_11;
+    int aai = ai;
+    int aaj = aj;
+    int bbi = bi;
+    int bbj = bj;
+    int ddi = di;
+    int ddj = dj;
+    double *dA = sA->dA;
+    if (ai == 0 & aj == 0)
+    {
+        if (sA->use_dA < n)
+        {
+            // invert diagonal of pA
+            for (ii = 0; ii < n; ii++)
+                dA[ii] = 1.0 / MATEL(sA, aai + ii, aaj + ii);
+            // use only now
+            sA->use_dA = n;
+        }
+    }
+    else
+    {
+        for (ii = 0; ii < n; ii++)
+            dA[ii] = 1.0 / MATEL(sA, aai + ii, aaj + ii);
+        sA->use_dA = 0; // nonzero offset makes diagonal dirty
+    }
+    // solve
+    jj = 0;
+    for (; jj < n - 1; jj += 2)
+    {
+        ii = 0;
+        id = n - jj - 2;
+        for (; ii < m - 1; ii += 2)
+        {
+            d_00 = alpha * MATEL(sB, bbi + ii + 0, bbj + (id + 0));
+            d_10 = alpha * MATEL(sB, bbi + ii + 1, bbj + (id + 0));
+            d_01 = alpha * MATEL(sB, bbi + ii + 0, bbj + (id + 1));
+            d_11 = alpha * MATEL(sB, bbi + ii + 1, bbj + (id + 1));
+            kk = id + 2;
+            for (; kk < n; kk++)
+            {
+                d_00 -= MATEL(sA, aai + kk + 0, aaj + (id + 0)) * MATEL(sD, ddi + ii + 0, ddj + (kk + 0));
+                d_10 -= MATEL(sA, aai + kk + 0, aaj + (id + 0)) * MATEL(sD, ddi + ii + 1, ddj + (kk + 0));
+                d_01 -= MATEL(sA, aai + kk + 0, aaj + (id + 1)) * MATEL(sD, ddi + ii + 0, ddj + (kk + 0));
+                d_11 -= MATEL(sA, aai + kk + 0, aaj + (id + 1)) * MATEL(sD, ddi + ii + 1, ddj + (kk + 0));
+            }
+            d_01 *= dA[id + 1];
+            d_11 *= dA[id + 1];
+            d_00 -= MATEL(sA, aai + id + 1, aaj + (id + 0)) * d_01;
+            d_10 -= MATEL(sA, aai + id + 1, aaj + (id + 0)) * d_11;
+            d_00 *= dA[id + 0];
+            d_10 *= dA[id + 0];
+            MATEL(sD, ddi + ii + 0, ddj + (id + 0)) = d_00;
+            MATEL(sD, ddi + ii + 1, ddj + (id + 0)) = d_10;
+            MATEL(sD, ddi + ii + 0, ddj + (id + 1)) = d_01;
+            MATEL(sD, ddi + ii + 1, ddj + (id + 1)) = d_11;
+        }
+        for (; ii < m; ii++)
+        {
+            d_00 = alpha * MATEL(sB, bbi + ii + 0, bbj + (id + 0));
+            d_01 = alpha * MATEL(sB, bbi + ii + 0, bbj + (id + 1));
+            kk = id + 2;
+            for (; kk < n; kk++)
+            {
+                d_00 -= MATEL(sA, aai + kk + 0, aaj + (id + 0)) * MATEL(sD, ddi + ii + 0, ddj + (kk + 0));
+                d_01 -= MATEL(sA, aai + kk + 0, aaj + (id + 1)) * MATEL(sD, ddi + ii + 0, ddj + (kk + 0));
+            }
+            d_01 *= dA[id + 1];
+            d_00 -= MATEL(sA, aai + id + 1, aaj + (id + 0)) * d_01;
+            d_00 *= dA[id + 0];
+            MATEL(sD, ddi + ii + 0, ddj + (id + 0)) = d_00;
+            MATEL(sD, ddi + ii + 0, ddj + (id + 1)) = d_01;
+        }
+    }
+    for (; jj < n; jj++)
+    {
+        ii = 0;
+        id = n - jj - 1;
+        for (; ii < m - 1; ii += 2)
+        {
+            d_00 = alpha * MATEL(sB, bbi + ii + 0, bbj + (id + 0));
+            d_10 = alpha * MATEL(sB, bbi + ii + 1, bbj + (id + 0));
+            kk = id + 1;
+            for (; kk < n; kk++)
+            {
+                d_00 -= MATEL(sA, aai + kk + 0, aaj + (id + 0)) * MATEL(sD, ddi + ii + 0, ddj + (kk + 0));
+                d_10 -= MATEL(sA, aai + kk + 0, aaj + (id + 0)) * MATEL(sD, ddi + ii + 1, ddj + (kk + 0));
+            }
+            d_00 *= dA[id + 0];
+            d_10 *= dA[id + 0];
+            MATEL(sD, ddi + ii + 0, ddj + (id + 0)) = d_00;
+            MATEL(sD, ddi + ii + 1, ddj + (id + 0)) = d_10;
+        }
+        for (; ii < m; ii++)
+        {
+            d_00 = alpha * MATEL(sB, bbi + ii, bbj + (id));
+            kk = id + 1;
+            for (; kk < n; kk++)
+                d_00 -= MATEL(sA, aai + kk, aaj + (id)) * MATEL(sD, ddi + ii, ddj + (kk));
+            MATEL(sD, ddi + ii, ddj + (id)) = d_00 * dA[id];
+        }
+    }
+    return;
+}
+#endif
\ No newline at end of file
diff --git a/fatrop/executables/RunFatrop.cpp b/fatrop/executables/RunFatrop.cpp
index 4bf05957..c6ef4eb5 100644
--- a/fatrop/executables/RunFatrop.cpp
+++ b/fatrop/executables/RunFatrop.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/fatrop.hpp b/fatrop/fatrop.hpp
index bc10de05..1e75d9a9 100644
--- a/fatrop/fatrop.hpp
+++ b/fatrop/fatrop.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/function_evaluation/CasadiCodegen.cpp b/fatrop/function_evaluation/CasadiCodegen.cpp
index 826629e1..a26790b4 100644
--- a/fatrop/function_evaluation/CasadiCodegen.cpp
+++ b/fatrop/function_evaluation/CasadiCodegen.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/function_evaluation/CasadiCodegen.hpp b/fatrop/function_evaluation/CasadiCodegen.hpp
index 94102698..4a8ea7dd 100644
--- a/fatrop/function_evaluation/CasadiCodegen.hpp
+++ b/fatrop/function_evaluation/CasadiCodegen.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/function_evaluation/FunctionEvaluation.cpp b/fatrop/function_evaluation/FunctionEvaluation.cpp
index 69263906..9b1bb383 100644
--- a/fatrop/function_evaluation/FunctionEvaluation.cpp
+++ b/fatrop/function_evaluation/FunctionEvaluation.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/function_evaluation/FunctionEvaluation.hpp b/fatrop/function_evaluation/FunctionEvaluation.hpp
index 09dfd1c2..0abb4efc 100644
--- a/fatrop/function_evaluation/FunctionEvaluation.hpp
+++ b/fatrop/function_evaluation/FunctionEvaluation.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/json/json.cpp b/fatrop/json/json.cpp
index 2e4af8d5..00f7649e 100644
--- a/fatrop/json/json.cpp
+++ b/fatrop/json/json.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/json/json.h b/fatrop/json/json.h
index f869c31f..68ccf3ed 100644
--- a/fatrop/json/json.h
+++ b/fatrop/json/json.h
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/ocp/DuInfEvaluator.cpp b/fatrop/ocp/DuInfEvaluator.cpp
index de920583..9390cb5a 100644
--- a/fatrop/ocp/DuInfEvaluator.cpp
+++ b/fatrop/ocp/DuInfEvaluator.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/ocp/DuInfEvaluator.hpp b/fatrop/ocp/DuInfEvaluator.hpp
index b0fdf85d..96c112c5 100644
--- a/fatrop/ocp/DuInfEvaluator.hpp
+++ b/fatrop/ocp/DuInfEvaluator.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/ocp/FatropOCP.cpp b/fatrop/ocp/FatropOCP.cpp
index 06c71b25..34210311 100644
--- a/fatrop/ocp/FatropOCP.cpp
+++ b/fatrop/ocp/FatropOCP.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
@@ -232,7 +232,7 @@ int FatropOCP::solve_soc_rhs(
             // prepare next iteration
             error_prev = err_curr;
         }
-        printer_->level(1) << "WARNING: max number of refinement iterations reached, error: " << err_curr << endl;
+        // printer_->level(-1) << "WARNING: max number of refinement iterations reached, error: " << err_curr << endl;
     };
 
     if (ls_scaling)
diff --git a/fatrop/ocp/FatropOCP.hpp b/fatrop/ocp/FatropOCP.hpp
index a2d2bd38..280fea2d 100644
--- a/fatrop/ocp/FatropOCP.hpp
+++ b/fatrop/ocp/FatropOCP.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/ocp/FatropOCPBuilder.cpp b/fatrop/ocp/FatropOCPBuilder.cpp
index 8afeef2f..09dcfaac 100644
--- a/fatrop/ocp/FatropOCPBuilder.cpp
+++ b/fatrop/ocp/FatropOCPBuilder.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/ocp/FatropOCPBuilder.hpp b/fatrop/ocp/FatropOCPBuilder.hpp
index 359b0a16..bbd986b1 100644
--- a/fatrop/ocp/FatropOCPBuilder.hpp
+++ b/fatrop/ocp/FatropOCPBuilder.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/ocp/FatropOCPResto.hpp b/fatrop/ocp/FatropOCPResto.hpp
new file mode 100644
index 00000000..3386a0af
--- /dev/null
+++ b/fatrop/ocp/FatropOCPResto.hpp
@@ -0,0 +1,299 @@
+#pragma once
+#include "FatropOCP.hpp"
+#include "fatrop/solver/FatropOptions.hpp"
+#include <memory>
+namespace fatrop
+{
+    class FatropOCPResto : public FatropNLP
+    {
+    public:
+        FatropOCPResto(const std::shared_ptr<FatropOCP> &orig, const std::shared_ptr<FatropOptions> &opts) : orig_(orig), orig_dims_(orig->get_nlp_dims()), lower_(orig_dims_.nineqs), upper_(orig_dims_.nineqs), x_start_(orig_dims_.nvars), s_start_(orig_dims_.nineqs), x_tmp_(orig_dims_.nvars), s_tmp_(orig_dims_.nineqs), upper_bounded_(orig_dims_.nineqs), lower_bounded_(orig_dims_.nineqs), slack_dummy_(orig_dims_.nineqs), sigma_dummy_(orig_dims_.nineqs), gradb_dummy_(orig_dims_.nineqs), zl_dummy_(orig_dims_.nineqs), zu_dummy_(orig_dims_.nineqs), sigma_cache_(orig_dims_.nineqs * 3), gradb_cache_(orig_dims_.nineqs * 3)
+        {
+            opts->register_option(DoubleOption::lower_bounded("resto_rho", "Resto L1 penalty parameter", &rho, 1000., 0.0));
+            opts->register_option(DoubleOption::lower_bounded("resto_xi", "Resto xi parameter", &xi, 1., 0.0));
+            auto lower_v = lower_[0];
+            auto upper_v = upper_[0];
+            orig_->get_bounds(lower_v, upper_v);
+            n_p = orig_dims_.nineqs;
+            n_n = orig_dims_.nineqs;
+            for (fatrop_int i = 0; i < orig_dims_.nineqs; ++i)
+            {
+                bool lower_bounded = !std::isinf(lower_v.at(i));
+                bool upper_bounded = !std::isinf(upper_v.at(i));
+                upper_bounded_[i] = upper_bounded;
+                lower_bounded_[i] = lower_bounded;
+            }
+            this_dims_.nvars = orig_dims_.nvars;
+            this_dims_.nineqs = orig_dims_.nineqs + n_n + n_p;
+            this_dims_.neqs = orig_dims_.neqs;
+        }
+        virtual void pre_solve(const FatropVecBF &x_init, const FatropVecBF &s_init) override
+        {
+            x_start_[0].copy(x_init);
+            s_start_[0].copy(s_init.block(0, orig_dims_.nineqs));
+        };
+        virtual fatrop_int eval_lag_hess(
+            double obj_scale,
+            const FatropVecBF &primal_vars,
+            const FatropVecBF &slack_vars,
+            const FatropVecBF &lam) override
+        {
+            FatropVecBF slack_dummy_v = slack_dummy_[0];
+            update_slack_vars(slack_vars, slack_dummy_v);
+            orig_->eval_lag_hess(0.0, primal_vars, slack_dummy_v, lam);
+            // compute delta's
+            axpby(xi, primal_vars, -xi, x_start_[0], x_tmp_[0]);
+            axpby(xi, slack_vars.block(0, orig_dims_.nineqs), -xi, s_start_[0], s_tmp_[0]);
+            // iterate over all RSQrq's
+
+            {
+                OCPKKTMemory* OCP =&this->orig_->ocpkktmemory_;
+                // horizon length
+                fatrop_int K = OCP->K;
+                // offsets
+                const fatrop_int *offs_ux = (const fatrop_int *)OCP->aux.ux_offs.data();
+                OCPMACRO(MAT *, RSQrqt, _p);
+                VEC* x_tmp_p = (VEC*) x_tmp_[0];
+                OCPMACRO(fatrop_int *, nu, _p);
+                OCPMACRO(fatrop_int *, nx, _p);
+                for (fatrop_int k = 0; k < K; k++)
+                {
+                    fatrop_int nu_k = nu_p[k];
+                    fatrop_int nx_k = nx_p[k];
+                    fatrop_int offs_ux_k = offs_ux[k];
+                    // add xi to RSQrq diagonal
+                    DIARE(nx_k + nu_k, xi, RSQrqt_p + k, 0, 0);
+                    // add gradient to rq
+                    ROWAD(nu_k + nx_k, 1.0, x_tmp_p, offs_ux_k, RSQrqt_p+k, nu_k + nx_k, 0);
+                }
+            }
+            return 0;
+        };
+
+        virtual fatrop_int eval_constr_jac(
+            const FatropVecBF &primal_vars,
+            const FatropVecBF &slack_vars) override
+        {
+            FatropVecBF slack_dummy_v = slack_dummy_[0];
+            update_slack_vars(slack_vars, slack_dummy_v);
+            orig_->eval_constr_jac(primal_vars, slack_dummy_v);
+            return 0;
+        };
+        void update_slack_vars(const FatropVecBF &slack_vars, FatropVecBF &slack_dummy)
+        {
+            fatrop_int offs_n = orig_dims_.nineqs;
+            fatrop_int offs_p = orig_dims_.nineqs + n_n;
+            for (fatrop_int i = 0; i < orig_dims_.nineqs; ++i)
+            {
+                slack_dummy.at(i) = slack_vars.at(i) - slack_vars.at(i + offs_n) + slack_vars.at(i + offs_p);
+            }
+        }
+        virtual fatrop_int eval_constraint_viol(
+            const FatropVecBF &primal_vars,
+            const FatropVecBF &slack_vars,
+            FatropVecBF &constraint_violation) override
+        {
+            FatropVecBF slack_dummy_v = slack_dummy_[0];
+            update_slack_vars(slack_vars, slack_dummy_v);
+            orig_->eval_constraint_viol(primal_vars, slack_dummy_v, constraint_violation);
+            return 0;
+        };
+        virtual fatrop_int eval_obj_grad(
+            double obj_scale,
+            const FatropVecBF &primal_vars,
+            const FatropVecBF &slack_vars,
+            FatropVecBF &gradient_x,
+            FatropVecBF &gradient_s) override
+        {
+            // compute delta's
+            axpby(xi, primal_vars, -xi, x_start_[0], gradient_x);
+            axpby(xi, slack_vars.block(0, orig_dims_.nineqs), -xi, s_start_[0], gradient_s.block(0, orig_dims_.nineqs));
+            gradient_s.block(orig_dims_.nineqs, n_n + n_p) = rho;
+            return 0;
+        };
+        virtual fatrop_int eval_obj(
+            double obj_scale,
+            const FatropVecBF &primal_vars,
+            const FatropVecBF &slack_vars,
+            double &res) override
+        {
+            // compute delta's
+            axpby(1.0, primal_vars, -1.0, x_start_[0], x_tmp_[0]);
+            axpby(1.0, slack_vars.block(0, orig_dims_.nineqs), -1.0, s_start_[0], s_tmp_[0]);
+            res = 0.5*xi*(sumsqr(x_tmp_[0]) + sumsqr(s_tmp_[0]));
+            res += rho * sum(slack_vars.block(orig_dims_.nineqs, n_n + n_p));
+            return 0;
+        };
+        void get_initialization(const FatropVecBF &primal_vars_orig, FatropVecBF &intialization)
+        {
+            intialization.block(0, orig_dims_.nvars).copy(primal_vars_orig.block(0, orig_dims_.nvars));
+        }
+        virtual fatrop_int eval_dual_inf(
+            double obj_scale,
+            const FatropVecBF &lam,
+            const FatropVecBF &grad_x,
+            const FatropVecBF &grad_s,
+            FatropVecBF &du_inf_x, FatropVecBF &du_inf_s_wo_z) override
+        {
+            auto du_inf_s_wo_z_or = du_inf_s_wo_z.block(0, orig_dims_.nineqs);
+            orig_->eval_dual_inf(obj_scale, lam, grad_x, grad_s, du_inf_x, du_inf_s_wo_z_or);
+            auto lam_I = lam.block(orig_dims_.neqs - orig_dims_.nineqs, orig_dims_.nineqs);
+            axpby(1.0, lam_I, 1.0, grad_s.block(orig_dims_.nineqs, n_n), du_inf_s_wo_z.block(orig_dims_.nineqs, n_n));
+            axpby(-1.0, lam_I, 1.0, grad_s.block(orig_dims_.nineqs + n_n, n_p), du_inf_s_wo_z.block(orig_dims_.nineqs + n_n, n_p));
+            return 0;
+        }
+        void update_sigma_gradb(double inertia, const FatropVecBF &sigma_s, const FatropVecBF &sigma_n, const FatropVecBF &sigma_p, const FatropVecBF &gradb_s, const FatropVecBF &gradb_n, const FatropVecBF &gradb_p, const FatropVecBF &sigma_update, const FatropVecBF &gradb_update)
+        {
+            for (int i = 0; i < orig_dims_.nineqs; ++i)
+            {
+                double sigma_updt = 1.0 / (1.0 / (sigma_s.at(i) + inertia + xi) + 1.0 / (sigma_n.at(i) + inertia) + 1.0 / (sigma_p.at(i) + inertia));
+                sigma_update.at(i) = sigma_updt - inertia;
+                gradb_update.at(i) = ((gradb_s.at(i)) / (sigma_s.at(i) + inertia + xi) - (gradb_n.at(i)) / (sigma_n.at(i) + inertia) + (gradb_p.at(i)) / (sigma_p.at(i) + inertia)) * sigma_updt;
+            }
+        }
+        void update_delta_snp(double inertia, const FatropVecBF &sigma_s, const FatropVecBF &sigma_n, const FatropVecBF &sigma_p, const FatropVecBF &gradb_s, const FatropVecBF &gradb_n, const FatropVecBF &gradb_p, const FatropVecBF &lam_I, const FatropVecBF &delta_s, const FatropVecBF &delta_n, const FatropVecBF &delta_p)
+        {
+            for (int i = 0; i < orig_dims_.nineqs; ++i)
+            {
+                double lam_I_i = lam_I.at(i);
+                delta_s.at(i) = (-gradb_s.at(i) + lam_I_i) / (sigma_s.at(i) + inertia+xi);
+                delta_n.at(i) = (-gradb_n.at(i) - lam_I_i) / (sigma_n.at(i) + inertia);
+                delta_p.at(i) = (-gradb_p.at(i) + lam_I_i) / (sigma_p.at(i) + inertia);
+            }
+        }
+
+        virtual fatrop_int solve_pd_sys(
+            const double inertia_correction_w,
+            const double inertia_correction_c,
+            const FatropVecBF &ux,
+            const FatropVecBF &lam,
+            const FatropVecBF &delta_s,
+            const FatropVecBF &sigma_total,
+            const FatropVecBF &gradb_total) override
+        {
+            // (todo) check if quadratic penalty on delta s is treated right
+            inertia_correction_w_cache = inertia_correction_w;
+            sigma_cache_[0].copy(sigma_total);
+            gradb_cache_[0].copy(gradb_total);
+            auto lam_I = lam.block(orig_dims_.neqs - orig_dims_.nineqs, orig_dims_.nineqs);
+            auto sigma_s = sigma_total.block(0, orig_dims_.nineqs);
+            auto sigma_n = sigma_total.block(orig_dims_.nineqs, n_n);
+            auto sigma_p = sigma_total.block(orig_dims_.nineqs + n_n, n_p);
+            auto gradb_s = gradb_total.block(0, orig_dims_.nineqs);
+            auto gradb_n = gradb_total.block(orig_dims_.nineqs, n_n);
+            auto gradb_p = gradb_total.block(orig_dims_.nineqs + n_n, n_p);
+            auto delta_s_or = delta_s.block(0, orig_dims_.nineqs);
+            auto delta_n = delta_s.block(orig_dims_.nineqs, n_n);
+            auto delta_p = delta_s.block(orig_dims_.nineqs + n_n, n_p);
+            update_sigma_gradb(inertia_correction_w, sigma_s, sigma_n, sigma_p, gradb_s, gradb_n, gradb_p, sigma_dummy_[0], gradb_dummy_[0]);
+            int ret = orig_->solve_pd_sys(inertia_correction_w, inertia_correction_c, ux, lam, delta_s.block(0, orig_dims_.nineqs), sigma_dummy_[0], gradb_dummy_[0]);
+            update_delta_snp(inertia_correction_w, sigma_s, sigma_n, sigma_p, gradb_s, gradb_n, gradb_p, lam_I, delta_s_or, delta_n, delta_p);
+            return ret;
+        };
+        virtual fatrop_int solve_soc_rhs(
+            const FatropVecBF &ux,
+            const FatropVecBF &lam,
+            const FatropVecBF &delta_s,
+            const FatropVecBF &cosntraint_violation) override
+        {
+            auto lam_I = lam.block(orig_dims_.neqs - orig_dims_.nineqs, orig_dims_.nineqs);
+            auto sigma_s = sigma_cache_[0].block(0, orig_dims_.nineqs);
+            auto sigma_n = sigma_cache_[0].block(orig_dims_.nineqs, n_n);
+            auto sigma_p = sigma_cache_[0].block(orig_dims_.nineqs + n_n, n_p);
+            auto gradb_s = gradb_cache_[0].block(0, orig_dims_.nineqs);
+            auto gradb_n = gradb_cache_[0].block(orig_dims_.nineqs, n_n);
+            auto gradb_p = gradb_cache_[0].block(orig_dims_.nineqs + n_n, n_p);
+            auto delta_s_or = delta_s.block(0, orig_dims_.nineqs);
+            auto delta_n = delta_s.block(orig_dims_.nineqs, n_n);
+            auto delta_p = delta_s.block(orig_dims_.nineqs + n_n, n_p);
+            int ret = orig_->solve_soc_rhs(ux, lam, delta_s.block(0, orig_dims_.nineqs), cosntraint_violation);
+            update_delta_snp(inertia_correction_w_cache + xi, sigma_s, sigma_n, sigma_p, gradb_s, gradb_n, gradb_p, lam_I, delta_s_or, delta_n, delta_p);
+            return ret;
+        }
+        virtual NLPDims get_nlp_dims() const override { return this_dims_; };
+        virtual fatrop_int compute_scalings(
+            double &obj_scale,
+            FatropVecBF &x_scales,
+            FatropVecBF &lam_scales,
+            const FatropVecBF &grad_curr_x, const FatropVecBF &grad_curr_s) override
+        {
+            return orig_->compute_scalings(obj_scale, x_scales, lam_scales, grad_curr_x, grad_curr_s);
+        };
+        virtual fatrop_int initialize_slacks(double mu0,
+                                             FatropVecBF &s_curr) override
+        {
+            auto s_curr_or = s_curr.block(0, orig_dims_.nineqs);
+            auto n_curr = s_curr.block(orig_dims_.nineqs, n_n);
+            auto p_curr = s_curr.block(orig_dims_.nineqs + n_n, n_p);
+            FatropVecBF ineq_viol = s_tmp_[0];
+            int ret = orig_->initialize_slacks(mu0, ineq_viol);
+            for (int i = 0; i < orig_dims_.nineqs; i++)
+            {
+                double viol = 0.0;
+                viol = (ineq_viol.at(i));
+                double n_init = (mu0 - rho * viol) / (2 * rho) + std::sqrt(std::pow((mu0 - rho * viol) / (2 * rho), 2) + mu0 * viol / (2 * rho));
+                // if viol >>>> 0 -> n_init = 0 if viol <<< 0 n_init = viol
+                n_curr.at(i) = n_init;
+                p_curr.at(i) = viol + n_init;
+            }
+            return 0;
+        }
+        virtual fatrop_int initialize_dual(
+            const FatropVecBF &grad_x,
+            const FatropVecBF &grad_s,
+            FatropVecBF &dlam,
+            const FatropVecBF &zL,
+            const FatropVecBF &zU) override
+        {
+            // todo check if this is correct
+            // return orig_->initialize_dual(grad_x, grad_s, dlam, zL.block(0, orig_dims_.nineqs), zU.block(0, orig_dims_.nineqs));
+            return 0;
+        };
+        virtual fatrop_int get_bounds(
+            FatropVecBF &lower,
+            FatropVecBF &upper) const override
+        {
+            orig_->get_bounds(lower, upper);
+            lower.block(orig_dims_.nineqs, n_n) = 0.0;
+            lower.block(orig_dims_.nineqs + n_n, n_p) = 0.0;
+            upper.block(orig_dims_.nineqs, n_n) = std::numeric_limits<double>::infinity();
+            upper.block(orig_dims_.nineqs + n_n, n_p) = std::numeric_limits<double>::infinity();
+            return 0;
+        };
+        virtual fatrop_int get_initial_sol_guess(
+            FatropVecBF &initial) const override
+        {
+            orig_->get_initial_sol_guess(initial);
+            return 0;
+        }
+        virtual void update_mu(double mu) override
+        {
+            xi = std::sqrt(mu);
+            orig_->update_mu(mu);
+        };
+        virtual void reset() override
+        {
+            orig_->reset();
+        };
+        std::shared_ptr<FatropOCP> orig_;
+        NLPDims orig_dims_;
+        NLPDims this_dims_;
+        FatropMemoryVecBF lower_, upper_;
+        FatropMemoryVecBF x_start_, s_start_;
+        FatropMemoryVecBF x_tmp_, s_tmp_;
+        std::vector<bool> upper_bounded_;
+        std::vector<bool> lower_bounded_;
+        fatrop_int n_p = 0;
+        fatrop_int n_n = 0;
+        FatropMemoryVecBF slack_dummy_;
+        FatropMemoryVecBF sigma_dummy_;
+        FatropMemoryVecBF gradb_dummy_;
+        FatropMemoryVecBF zl_dummy_;
+        FatropMemoryVecBF zu_dummy_;
+        double inertia_correction_w_cache = 0.0;
+        FatropMemoryVecBF sigma_cache_;
+        FatropMemoryVecBF gradb_cache_;
+        double rho = 1e4;
+        double xi = 1.0;
+    };
+};
\ No newline at end of file
diff --git a/fatrop/ocp/OCP.cpp b/fatrop/ocp/OCP.cpp
index b3a6d759..48cecdc1 100644
--- a/fatrop/ocp/OCP.cpp
+++ b/fatrop/ocp/OCP.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/ocp/OCP.hpp b/fatrop/ocp/OCP.hpp
index b1f7f20a..5c824c2b 100644
--- a/fatrop/ocp/OCP.hpp
+++ b/fatrop/ocp/OCP.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/ocp/OCPAbstract.cpp b/fatrop/ocp/OCPAbstract.cpp
index 710e43d9..6a0b4383 100644
--- a/fatrop/ocp/OCPAbstract.cpp
+++ b/fatrop/ocp/OCPAbstract.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/ocp/OCPAbstract.hpp b/fatrop/ocp/OCPAbstract.hpp
index d49f5e5b..da4eed50 100644
--- a/fatrop/ocp/OCPAbstract.hpp
+++ b/fatrop/ocp/OCPAbstract.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/ocp/OCPAdapter.cpp b/fatrop/ocp/OCPAdapter.cpp
index 6583415a..32c4d3cb 100644
--- a/fatrop/ocp/OCPAdapter.cpp
+++ b/fatrop/ocp/OCPAdapter.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/ocp/OCPAdapter.hpp b/fatrop/ocp/OCPAdapter.hpp
index 45e14525..c54674f2 100644
--- a/fatrop/ocp/OCPAdapter.hpp
+++ b/fatrop/ocp/OCPAdapter.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/ocp/OCPCInterface.cpp b/fatrop/ocp/OCPCInterface.cpp
index c3e307f7..80338180 100644
--- a/fatrop/ocp/OCPCInterface.cpp
+++ b/fatrop/ocp/OCPCInterface.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
@@ -266,13 +266,72 @@ namespace fatrop
         }
     };
 
+
+    // Stream buffer for std::cout like printing
+    class FatropOcpCStreambuf : public std::streambuf
+    {
+    public:
+        FatropOcpCStreambuf(FatropOcpCWrite write_cb,
+                            FatropOcpCFlush flush_cb) : write(write_cb), flush(flush_cb)
+    {
+    }
+    protected:
+        int_type overflow(int_type ch) override
+        {
+            if (ch != traits_type::eof())
+            {
+                char c = static_cast<char>(ch);
+                write(&c, 1);
+            }
+            return ch;
+        }
+        std::streamsize xsputn(const char* s, std::streamsize num) override
+        {
+            // Delegate to write
+            // write uses 'int' instead of 'std::streamsize' so extra logic is needed
+            int max_chunk = std::numeric_limits<int>::max();
+            std::streamsize written = 0;
+            while (num>0) {
+                int chunk = static_cast<int>(std::min<std::streamsize>(num, max_chunk));
+                write(s + written, chunk);
+                written += chunk;
+                num -= chunk;
+            }
+            return written;
+        }
+        int sync() override {
+            if (flush) flush();
+            return 0;
+        }
+    private:
+        FatropOcpCWrite write;
+        FatropOcpCFlush flush;
+    };
+
+    class FatropOcpCStream : public std::ostream {
+        protected:
+            FatropOcpCStreambuf buf;
+        public:
+            FatropOcpCStream(FatropOcpCWrite write_cb,
+                        FatropOcpCFlush flush_cb) : std::ostream(&buf), buf(write_cb, flush_cb)
+        {
+        }
+    };
+
     class OcpSolverDriver
     {
     public:
-        OcpSolverDriver(FatropOcpCInterface* ocp_interface) :
+        OcpSolverDriver(FatropOcpCInterface* ocp_interface,
+                        FatropOcpCWrite write,
+                        FatropOcpCFlush flush) :
+                stream(write, flush),
                 m(std::make_shared<FatropOcpCDimsMapping>(ocp_interface)),
                 app(m)
         {
+            if (write!=0)
+            {
+                app.printer_ = std::make_shared<FatropPrinter>(0, stream);
+            }
             app.build();
             const OCPKKTMemory& kktmem = std::dynamic_pointer_cast<FatropOCP>(app.nlp_)->ocpkktmemory_;
             m->s.nx = kktmem.nx.data();
@@ -295,15 +354,19 @@ namespace fatrop
         {
             return app.optimize();
         }
+        std::shared_ptr<FatropPrinter> printer() {
+            return app.printer_;
+        }
+        FatropOcpCStream stream;
         std::shared_ptr<FatropOcpCDimsMapping> m;
         FatropOcpCStats stats;
         OCPAbstractApplication app;
     };
 
-    FatropOcpCSolver* fatrop_ocp_c_create(FatropOcpCInterface* ocp_interface)
+    FatropOcpCSolver* fatrop_ocp_c_create(FatropOcpCInterface* ocp_interface, FatropOcpCWrite write, FatropOcpCFlush flush)
     {
         FatropOcpCSolver* ret = new FatropOcpCSolver();
-        ret->driver = new OcpSolverDriver(ocp_interface);
+        ret->driver = new OcpSolverDriver(ocp_interface, write, flush);
         return ret;
     }
 
@@ -313,6 +376,8 @@ namespace fatrop
         {
             s->driver->app.set_option(name, val);
         } catch (std::exception& e) {
+            s->driver->printer()->level(1) << "Error setting double option " << name << " to " << val << "." << std::endl;
+            s->driver->printer()->level(1) << e.what() << std::endl;
             return 1;
         }
         return 0;
@@ -324,6 +389,8 @@ namespace fatrop
         {
             s->driver->app.set_option(name, bool(val));
         } catch (std::exception& e) {
+            s->driver->printer()->level(1) << "Error setting bool option " << name << " to " << val << "." << std::endl;
+            s->driver->printer()->level(1) << e.what() << std::endl; 
             return 1;
         }
         return 0;
@@ -335,6 +402,8 @@ namespace fatrop
         {
             s->driver->app.set_option(name, int(val));
         } catch (std::exception& e) {
+            s->driver->printer()->level(1) << "Error setting int option " << name << " to " << val << "." << std::endl;
+            s->driver->printer()->level(1) << e.what() << std::endl;
             return 1;
         }
         return 0;
@@ -346,6 +415,8 @@ namespace fatrop
         {
             s->driver->app.set_option(name, std::string(val));
         } catch (std::exception& e) {
+            s->driver->printer()->level(1) << "Error setting string option " << name << " to " << val << "." << std::endl;
+            s->driver->printer()->level(1) << e.what() << std::endl;
             return 1;
         }
         return 0;
@@ -363,8 +434,13 @@ namespace fatrop
 
     int fatrop_ocp_c_solve(FatropOcpCSolver* s)
     {
-        s->driver->solve();
-        return 0;
+        try
+        {
+            return s->driver->solve();
+        } catch (std::exception& e) {
+            s->driver->printer()->level(1) << "Uncaught Exception: " << e.what() << std::endl;
+            return -1;
+        }
     }
 
     void fatrop_ocp_c_destroy(FatropOcpCSolver* s)
diff --git a/fatrop/ocp/OCPCInterface.h b/fatrop/ocp/OCPCInterface.h
index cc09da7a..84de5c56 100644
--- a/fatrop/ocp/OCPCInterface.h
+++ b/fatrop/ocp/OCPCInterface.h
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
@@ -111,6 +111,9 @@ typedef fatrop_int (*FatropOcpCFullEvalContrViol)(const double*, const double*,
 typedef fatrop_int (*FatropOcpCFullEvalObjGrad)(double, const double*, const double*, const double*, double *, const struct FatropOcpCDims*, void*);
 typedef fatrop_int (*FatropOcpCFullEvalObj)(double, const double*, const double*, const double*, double *, const struct FatropOcpCDims*, void*);
 
+typedef void (*FatropOcpCWrite)(const char* msg, int num);
+typedef void (*FatropOcpCFlush)(void);
+
 
 struct FatropOcpCInterface
 {
@@ -255,7 +258,15 @@ struct FatropOcpCInterface
     void* user_data;
 };
 
-FATROP_SYMBOL_EXPORT struct FatropOcpCSolver* fatrop_ocp_c_create(struct FatropOcpCInterface* ocp_interface);
+/* 
+    * @brief Create a new OCP solver
+    * @param ocp_interface: pointer to the OCP interface
+    * @param write: function pointer to write function (can be zero to indicate stdout)
+    * @param flush: function pointer to flush function (can be zero)
+    * @return pointer to the OCP solver
+    
+*/
+FATROP_SYMBOL_EXPORT struct FatropOcpCSolver* fatrop_ocp_c_create(struct FatropOcpCInterface* ocp_interface, FatropOcpCWrite write, FatropOcpCFlush flush);
 
 FATROP_SYMBOL_EXPORT fatrop_int fatrop_ocp_c_solve(struct FatropOcpCSolver*);
 
@@ -275,4 +286,4 @@ FATROP_SYMBOL_EXPORT void fatrop_ocp_c_destroy(struct FatropOcpCSolver*);
 } // extern "C"
 #endif
 
-#endif // OCPCINTERFACEINCLUDED
\ No newline at end of file
+#endif // OCPCINTERFACEINCLUDED
diff --git a/fatrop/ocp/OCPDims.cpp b/fatrop/ocp/OCPDims.cpp
index b815af00..e3d3eb4c 100644
--- a/fatrop/ocp/OCPDims.cpp
+++ b/fatrop/ocp/OCPDims.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/ocp/OCPDims.hpp b/fatrop/ocp/OCPDims.hpp
index 91304c44..57dd7d47 100644
--- a/fatrop/ocp/OCPDims.hpp
+++ b/fatrop/ocp/OCPDims.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/ocp/OCPInitializer.cpp b/fatrop/ocp/OCPInitializer.cpp
index d7ecab62..9fb59d2c 100644
--- a/fatrop/ocp/OCPInitializer.cpp
+++ b/fatrop/ocp/OCPInitializer.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/ocp/OCPInitializer.hpp b/fatrop/ocp/OCPInitializer.hpp
index b959ad50..632bb45a 100644
--- a/fatrop/ocp/OCPInitializer.hpp
+++ b/fatrop/ocp/OCPInitializer.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/ocp/OCPKKT.cpp b/fatrop/ocp/OCPKKT.cpp
index 8ea1512b..bb281bfc 100644
--- a/fatrop/ocp/OCPKKT.cpp
+++ b/fatrop/ocp/OCPKKT.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/ocp/OCPKKT.hpp b/fatrop/ocp/OCPKKT.hpp
index 0b3d5f98..a13eb617 100644
--- a/fatrop/ocp/OCPKKT.hpp
+++ b/fatrop/ocp/OCPKKT.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/ocp/OCPLSRiccati.cpp b/fatrop/ocp/OCPLSRiccati.cpp
index 4c2f0b93..80d95ec6 100644
--- a/fatrop/ocp/OCPLSRiccati.cpp
+++ b/fatrop/ocp/OCPLSRiccati.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
@@ -85,7 +85,14 @@ OCPLSRiccati::OCPLSRiccati(const OCPDims &dims, const shared_ptr<FatropOptions>
                                                                                                                                       options_(options),
                                                                                                                                       printer_(printer)
 {
-    options_->register_option(BooleanOption("iterative_refinement", "iterative ref", &it_ref, true));
+    options_->register_option(BooleanOption("linsol_iterative_refinement", "iterative ref", &it_ref, true));
+    options_->register_option(BooleanOption("linsol_perturbed_mode", "linear solver perturbed mode", &perturbed_mode, false));
+    options_->register_option(BooleanOption("linsol_diagnostic", "linear solver diagnostic mode", &diagnostic, false));
+    options_->register_option(DoubleOption::lower_bounded("linsol_perturbed_mode_param", "linear solver perturbed mode param", &perturbed_mode_param, 1e-6, 0.));
+    options_->register_option(IntegerOption::lower_bounded("linsol_min_it_ref", "minimum number of iterative refinement steps", &min_it_ref, 0, 0));
+    options_->register_option(IntegerOption::lower_bounded("linsol_max_it_ref", "maximum number of iterative refinement steps", &max_it_ref, 5, 0));
+    options_->register_option(DoubleOption::lower_bounded("linsol_min_it_acc", "stopping criterion for iterative refinement procedure", &it_ref_acc, 1e-8, 0.));
+    options_->register_option(DoubleOption::lower_bounded("linsol_lu_fact_tol", "pivoting tolerance parameter for lu fact", &lu_fact_tol, 1e-5, 0.));
 };
 fatrop_int OCPLSRiccati::solve_pd_sys(
     OCPKKTMemory *OCP,
@@ -100,7 +107,9 @@ fatrop_int OCPLSRiccati::solve_pd_sys(
     // lastused_.inertia_correction_c = inertia_correction_c;
     // lastused_.inertia_correction_w = inertia_correction_w;
     int ret;
-    if (inertia_correction_c == 0.0)
+    if (perturbed_mode)
+        ret = solve_pd_sys_degenerate(OCP, inertia_correction_w, perturbed_mode_param, ux, lam, delta_s, sigma_total, gradb_total);
+    else if (inertia_correction_c == 0.0)
     {
         ret = solve_pd_sys_normal(OCP, inertia_correction_w, ux, lam, delta_s, sigma_total, gradb_total);
     }
@@ -112,7 +121,6 @@ fatrop_int OCPLSRiccati::solve_pd_sys(
         return ret;
     if (it_ref)
     {
-        const fatrop_int min_it_ref = 0;
         double err_curr = 0.0;
         get_rhs(
             OCP,
@@ -127,7 +135,7 @@ fatrop_int OCPLSRiccati::solve_pd_sys(
         double max_norm = max(Linf(rhs_gradb2[0]), max(Linf(rhs_g_ineq2[0]), max(Linf(rhs_g2[0]), max(Linf(rhs_rq2[0]), Linf(rhs_b2[0])))));
         max_norm = (max_norm == 0.0) ? 1.0 : max_norm;
         double error_prev = -1.0;
-        for (fatrop_int i = 0; i < 5; i++)
+        for (fatrop_int i = 0; i < max_it_ref; i++)
         {
             // blasfeo_tic(&timer);
             compute_pd_sys_times_vec(
@@ -157,10 +165,11 @@ fatrop_int OCPLSRiccati::solve_pd_sys(
             // cout << "residu g_ineq:  " << Linf(rhs_g_ineq[0]) / max_norm << "  ";
             // cout << "residu gradb:  " << Linf(rhs_gradb[0]) / max_norm  << "  "<<endl;
             err_curr = max(Linf(rhs_gradb[0]), max(Linf(rhs_g_ineq[0]), max(Linf(rhs_g[0]), max(Linf(rhs_rq[0]), Linf(rhs_b[0]))))) / max_norm;
-            // cout << "residu:  " << err_curr << endl;
+            if (diagnostic)
+                printer_->level(1) << "residu:  " << err_curr << endl;
             if (i >= min_it_ref)
             {
-                if (err_curr < 1e-6 || (error_prev > 0.0 && err_curr > 1.0 * error_prev))
+                if (err_curr < it_ref_acc || (error_prev > 0.0 && err_curr > 1 * error_prev))
                 {
                     if (err_curr > 1e-8)
                     {
@@ -172,7 +181,7 @@ fatrop_int OCPLSRiccati::solve_pd_sys(
             // blasfeo_tic(&timer);
             solve_rhs(
                 OCP,
-                inertia_correction_w, inertia_correction_c,
+                inertia_correction_w, perturbed_mode ? perturbed_mode_param : inertia_correction_c,
                 ux_test[0],
                 lam_test[0],
                 delta_s_test[0],
@@ -193,7 +202,7 @@ fatrop_int OCPLSRiccati::solve_pd_sys(
             // prepare next iteration
             error_prev = err_curr;
         }
-        printer_->level(1) << "WARNING: max number of refinement iterations reached, error: " << err_curr << endl;
+        // printer_->level(1) << "WARNING: max number of refinement iterations reached, error: " << err_curr << endl;
     }
     return 0;
 }
@@ -526,7 +535,7 @@ fatrop_int OCPLSRiccati::solve_pd_sys_normal(
             // symmetric transformation, done a little different than in paper, in order to fuse LA operations
             // LU_FACT_TRANSPOSE(Ggtstripe[:gamma_k, nu+nx+1], nu max)
             // if(k==K-2) blasfeo_print_dmat(1, gamma_k, Ggt_stripe_p, nu+nx, 0);
-            LU_FACT_transposed(gamma_k, nu + nx + 1, nu, rank_k, Ggt_stripe_p, Pl_p + k, Pr_p + k);
+            LU_FACT_transposed(gamma_k, nu + nx + 1, nu, rank_k, Ggt_stripe_p, Pl_p + k, Pr_p + k, lu_fact_tol);
 
             rho_p[k] = rank_k;
             if (gamma_k - rank_k > 0)
@@ -610,7 +619,7 @@ fatrop_int OCPLSRiccati::solve_pd_sys_normal(
         {
             GETR(gamma_I, nx + 1, Hh_p + 0, 0, 0, HhIt_p, 0, 0); // transposition may be avoided
             // HhIt[0].print();
-            LU_FACT_transposed(gamma_I, nx + 1, nx, rankI, HhIt_p, PlI_p, PrI_p);
+            LU_FACT_transposed(gamma_I, nx + 1, nx, rankI, HhIt_p, PlI_p, PrI_p, lu_fact_tol);
             if (rankI < gamma_I)
                 return -2;
             // PpIt_tilde <- Ggt[rankI:nx+1, :rankI] L-T (note that this is slightly different from the implementation)
diff --git a/fatrop/ocp/OCPLSRiccati.hpp b/fatrop/ocp/OCPLSRiccati.hpp
index 78cf7ddf..aeee3d73 100644
--- a/fatrop/ocp/OCPLSRiccati.hpp
+++ b/fatrop/ocp/OCPLSRiccati.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
@@ -186,6 +186,13 @@ namespace fatrop
         std::shared_ptr<FatropOptions> options_;
         std::shared_ptr<FatropPrinter> printer_;
         bool it_ref = true;
+        bool perturbed_mode = false;
+        double perturbed_mode_param = 1e-6;
+        int min_it_ref = 0;
+        int max_it_ref = 5;
+        double it_ref_acc = 1e-8;
+        double lu_fact_tol = 1e-5;
+        bool diagnostic = false;
     };
 };     // namespace
 #endif // OCPLSRICCATIINCLUDED
\ No newline at end of file
diff --git a/fatrop/ocp/OCPLinearSolver.cpp b/fatrop/ocp/OCPLinearSolver.cpp
index de2808de..e26f8ba2 100644
--- a/fatrop/ocp/OCPLinearSolver.cpp
+++ b/fatrop/ocp/OCPLinearSolver.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/ocp/OCPLinearSolver.hpp b/fatrop/ocp/OCPLinearSolver.hpp
index 51211c04..ff062814 100644
--- a/fatrop/ocp/OCPLinearSolver.hpp
+++ b/fatrop/ocp/OCPLinearSolver.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/ocp/OCPNoScaling.cpp b/fatrop/ocp/OCPNoScaling.cpp
index b5571884..b0b57412 100644
--- a/fatrop/ocp/OCPNoScaling.cpp
+++ b/fatrop/ocp/OCPNoScaling.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/ocp/OCPNoScaling.hpp b/fatrop/ocp/OCPNoScaling.hpp
index 4265845f..2f88d350 100644
--- a/fatrop/ocp/OCPNoScaling.hpp
+++ b/fatrop/ocp/OCPNoScaling.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/ocp/OCPScalingMethod.cpp b/fatrop/ocp/OCPScalingMethod.cpp
index 32a060fd..a6474e1b 100644
--- a/fatrop/ocp/OCPScalingMethod.cpp
+++ b/fatrop/ocp/OCPScalingMethod.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/ocp/OCPScalingMethod.hpp b/fatrop/ocp/OCPScalingMethod.hpp
index 1990ff6b..5ed458ac 100644
--- a/fatrop/ocp/OCPScalingMethod.hpp
+++ b/fatrop/ocp/OCPScalingMethod.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/ocp/StageOCP.cpp b/fatrop/ocp/StageOCP.cpp
index 7a65b898..139dfdea 100644
--- a/fatrop/ocp/StageOCP.cpp
+++ b/fatrop/ocp/StageOCP.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/ocp/StageOCP.hpp b/fatrop/ocp/StageOCP.hpp
index 04763246..9e2c6e96 100644
--- a/fatrop/ocp/StageOCP.hpp
+++ b/fatrop/ocp/StageOCP.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/ocp/StageOCPApplication.cpp b/fatrop/ocp/StageOCPApplication.cpp
index 57911cdf..e75ebf1a 100644
--- a/fatrop/ocp/StageOCPApplication.cpp
+++ b/fatrop/ocp/StageOCPApplication.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
@@ -20,6 +20,7 @@
 #include "fatrop/solver/AlgBuilder.hpp"
 #include "fatrop/ocp/OCPAdapter.hpp"
 #include "fatrop/ocp/FatropOCP.hpp"
+#include "fatrop/ocp/FatropOCPResto.hpp"
 #include "fatrop/ocp/FatropOCPBuilder.hpp"
 #include "fatrop/ocp/StageOCP.hpp"
 #include "fatrop/solver/FatropAlg.hpp"
@@ -37,7 +38,7 @@ NLPApplication::NLPApplication() : fatropoptions_(make_shared<FatropOptions>()),
     }
 }
 
-void NLPApplication::build(const shared_ptr<FatropNLP> &nlp)
+void NLPApplication::build(const shared_ptr<FatropNLP> &nlp, const shared_ptr<FatropNLP> &nlp_resto)
 {
     // keep nlp around for getting nlpdims
     nlp_ = nlp;
@@ -50,9 +51,10 @@ void NLPApplication::build(const shared_ptr<FatropNLP> &nlp)
     {
         nlp_ = std::make_shared<NLPL1>(nlp, fatropoptions_);
     }
+    std::shared_ptr<FatropData> fatropdata_resto;
     AlgBuilder algbuilder;
     algbuilder.set_printer(printer_);
-    algbuilder.build_fatrop_algorithm_objects(nlp_, fatropoptions_, fatropdata_, journaller_);
+    algbuilder.build_fatrop_algorithm_objects(nlp_, nlp_resto, fatropoptions_, fatropdata_, fatropdata_resto, journaller_);
     fatropoptions_->register_option(IntegerOption::un_bounded("print_level", "prfatrop_fatrop_int level", &printer_->print_level(), 10));
     fatropalg_ = algbuilder.build_algorithm();
     dirty = false;
@@ -147,8 +149,9 @@ OCPAbstractApplication::OCPAbstractApplication(const shared_ptr<OCPAbstract> &oc
 void OCPApplication::build()
 {
     // keep the adapter around for accessing the parameters for samplers and parameter setters
+    shared_ptr<FatropNLP> nlp_resto(make_shared<FatropOCPResto>(FatropOCPBuilder(ocp_, fatropoptions_, printer_).build(ocp_), fatropoptions_));
     shared_ptr<FatropNLP> nlp(FatropOCPBuilder(ocp_, fatropoptions_, printer_).build(ocp_));
-    NLPApplication::build(nlp);
+    NLPApplication::build(nlp, nlp_resto);
     dirty = false;
 }
 void OCPAbstractApplication::set_params(const std::vector<double> &global_params, const std::vector<double> &stage_params)
@@ -311,10 +314,7 @@ fatrop_int StageOCPApplication::optimize()
 {
     fatrop_int ret = NLPApplication::optimize();
     last_solution_.set_parameters(global_parameters(), stage_parameters());
-    if (ret == 0)
-    {
-        last_solution_.set_solution(last_solution_primal(), last_solution_dual(), last_solution_zL(), last_solution_zU());
-    }
+    last_solution_.set_solution(last_solution_primal(), last_solution_dual(), last_solution_zL(), last_solution_zU());
     return ret;
 }
 const StageOCPSolution &StageOCPApplication::last_solution() const
diff --git a/fatrop/ocp/StageOCPApplication.hpp b/fatrop/ocp/StageOCPApplication.hpp
index 4343bf06..3092d277 100644
--- a/fatrop/ocp/StageOCPApplication.hpp
+++ b/fatrop/ocp/StageOCPApplication.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
@@ -70,7 +70,7 @@ namespace fatrop
         NLPApplication();
 
     protected:
-        void build(const std::shared_ptr<FatropNLP> &nlp);
+        void build(const std::shared_ptr<FatropNLP> &nlp, const std::shared_ptr<FatropNLP> &nlp_resto);
 
     public:
         fatrop_int optimize() const;
diff --git a/fatrop/ocp/StageOCPExpressions.cpp b/fatrop/ocp/StageOCPExpressions.cpp
index 0033fc28..e8cf4090 100644
--- a/fatrop/ocp/StageOCPExpressions.cpp
+++ b/fatrop/ocp/StageOCPExpressions.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/ocp/StageOCPExpressions.hpp b/fatrop/ocp/StageOCPExpressions.hpp
index a7da83ec..21690ea8 100644
--- a/fatrop/ocp/StageOCPExpressions.hpp
+++ b/fatrop/ocp/StageOCPExpressions.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/solver/AlgBuilder.hpp b/fatrop/solver/AlgBuilder.hpp
index ff436c6b..d7a2f257 100644
--- a/fatrop/solver/AlgBuilder.hpp
+++ b/fatrop/solver/AlgBuilder.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
@@ -26,9 +26,9 @@ namespace fatrop
     class AlgBuilder
     {
     public:
-        void build_fatrop_algorithm_objects(const std::shared_ptr<FatropNLP> &nlp,
+        void build_fatrop_algorithm_objects(const std::shared_ptr<FatropNLP> &nlp,const std::shared_ptr<FatropNLP> &nlp_resto,
                                             const std::shared_ptr<FatropOptions> &fatropparams,
-                                            std::shared_ptr<FatropData> &fatropdata,
+                                            std::shared_ptr<FatropData> &fatropdata, std::shared_ptr<FatropData> &fatropdata_resto,
                                             std::shared_ptr<Journaller> &journaller)
         {
             if (fatropprinter_ == nullptr)
@@ -36,10 +36,13 @@ namespace fatrop
                 fatropprinter_ = std::make_shared<FatropPrinter>();
             }
             fatropdata = std::make_shared<FatropData>(nlp->get_nlp_dims(), fatropparams, fatropprinter_);
+            fatropdata_resto = std::make_shared<FatropData>(nlp_resto->get_nlp_dims(), fatropparams, fatropprinter_);
             journaller = std::make_shared<Journaller>(fatropparams->maxiter + 1, fatropprinter_);
             fatropdata_ = fatropdata; // keep this around for building the algorithm
+            fatropdata_resto_ = fatropdata_resto;
             journaller_ = journaller;
             nlp_ = nlp;
+            nlp_resto_ = nlp_resto;
             fatropoptions_ = fatropparams;
         }
         void set_printer(const std::shared_ptr<FatropPrinter> &printer)
@@ -51,13 +54,20 @@ namespace fatrop
             // TODO unsafe if maxiter is changed during application
             std::shared_ptr<Filter> filter = std::make_shared<Filter>(fatropoptions_->maxiter + 1);
             std::shared_ptr<LineSearch> linesearch = std::make_shared<BackTrackingLineSearch>(fatropoptions_, nlp_, fatropdata_, filter, journaller_, fatropprinter_);
-            return std::make_shared<FatropAlg>(nlp_, fatropdata_, fatropoptions_, filter, linesearch, journaller_, fatropprinter_);
+            std::shared_ptr<Filter> filter_resto = std::make_shared<Filter>(fatropoptions_->maxiter + 1);
+            std::shared_ptr<LineSearch> linesearch_resto = std::make_shared<BackTrackingLineSearch>(fatropoptions_, nlp_resto_, fatropdata_resto_, filter_resto, journaller_, fatropprinter_);
+            std::shared_ptr<FatropAlg> orig_alg = std::make_shared<FatropAlg>(nlp_, fatropdata_, fatropoptions_, filter, linesearch, journaller_, fatropprinter_, nullptr, nullptr, false);
+            std::shared_ptr<FatropAlg> resto_alg = std::make_shared<FatropAlg>(nlp_resto_, fatropdata_resto_, fatropoptions_, filter_resto, linesearch_resto, journaller_, fatropprinter_, orig_alg, nullptr, true);
+            orig_alg-> set_resto_alg(resto_alg);
+            return orig_alg;
         }
 
     private:
         std::shared_ptr<FatropNLP> nlp_;
+        std::shared_ptr<FatropNLP> nlp_resto_;
         std::shared_ptr<FatropOptions> fatropoptions_;
         std::shared_ptr<FatropData> fatropdata_;
+        std::shared_ptr<FatropData> fatropdata_resto_;
         std::shared_ptr<Journaller> journaller_;
         std::shared_ptr<FatropPrinter> fatropprinter_;
     };
diff --git a/fatrop/solver/AlgStrategy.cpp b/fatrop/solver/AlgStrategy.cpp
index 7a206a37..a437ef73 100644
--- a/fatrop/solver/AlgStrategy.cpp
+++ b/fatrop/solver/AlgStrategy.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/solver/AlgStrategy.hpp b/fatrop/solver/AlgStrategy.hpp
index 64d2085a..a825a0b0 100644
--- a/fatrop/solver/AlgStrategy.hpp
+++ b/fatrop/solver/AlgStrategy.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/solver/FatropAlg.cpp b/fatrop/solver/FatropAlg.cpp
index e387150f..7d9d53d8 100644
--- a/fatrop/solver/FatropAlg.cpp
+++ b/fatrop/solver/FatropAlg.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
@@ -27,14 +27,15 @@ FatropAlg::FatropAlg(
     const shared_ptr<Filter> &filter,
     const shared_ptr<LineSearch> &linesearch,
     const shared_ptr<Journaller> &journaller,
-    const shared_ptr<FatropPrinter> &printer)
+    const shared_ptr<FatropPrinter> &printer,
+    const shared_ptr<FatropAlg>&orig_, const shared_ptr<FatropAlg>&resto_alg_, bool resto_problem)
     : fatropnlp_(fatropnlp),
       fatropdata_(fatropdata),
       fatropoptions_(fatropparams),
       filter_(filter),
       linesearch_(linesearch),
       journaller_(journaller),
-      printer_(printer)
+      printer_(printer), orig_(orig_), resto_alg_(resto_alg_), resto_problem_(resto_problem)
 {
 
     fatropoptions_->register_option(DoubleOption::lower_bounded("tol", "tolerance", &tol, 1e-8, 0.0));
@@ -72,11 +73,13 @@ void FatropAlg::initialize()
 void FatropAlg::reset()
 {
     filter_->reset();
-    fatropdata_->reset();
+    if(!resto_problem_) fatropdata_->reset();
+    fatropdata_->reset_caches();
     journaller_->reset();
     fatropnlp_->reset();
     linesearch_->reset();
     stats = FatropStats();
+    if(!resto_problem_) start_iter_ = 0;
 }
 void FatropAlg::set_bounds(const vector<double> &lower, const vector<double> &upper)
 {
@@ -108,32 +111,25 @@ fatrop_int FatropAlg::optimize()
     reset();
     const double mu_min = tol / 10;
     double mu = mu0;
+    fatropnlp_->update_mu(mu);
     double delta_w_last = 0.0;
     LineSearchInfo lsinfo;
+    fatropnlp_->pre_solve(fatropdata_->x_curr, fatropdata_->s_curr);
     eval_constr_jac(); // todo twice evaluation
     eval_obj_grad_curr();
-    if (warm_start_init_point)
+    if(!resto_problem_) fatropnlp_->initialize_slacks(mu0,
+                                                   fatropdata_->s_curr);
+    if (!resto_problem_ && warm_start_init_point)
     {
-        fatropnlp_->initialize_slacks(mu,
-            fatropdata_->s_curr);
         fatropdata_->warmstart_dual();
         fatropdata_->bound_z();
     }
-    else
+    if (!resto_problem_ && !warm_start_init_point)
     {
-        fatrop_int initialization_res = perform_initializiation();
-        if (initialization_res == 0 && fatropdata_->delta_dual_max() < lammax)
-        {
-            printer_->level(1) << "accepted lam " << endl;
-            fatropdata_->accept_dual_initializiaton();
-        }
-        else
-        {
-            printer_->level(1) << "rejected lam " << endl;
-            fatropdata_->lam_curr.SetConstant(0.0);
-        }
+        perform_initializiation_dual();
     }
-    fatropdata_->bound_slacks();
+    if (!resto_problem_)
+        fatropdata_->bound_slacks();
     eval_constr_viol_curr();
     fatropdata_->theta_min = theta_min * MAX(1.0, fatropdata_->constr_viol_sum_curr());
     double theta_max = 1e4 * fatropdata_->constr_viol_sum_curr();
@@ -142,7 +138,7 @@ fatrop_int FatropAlg::optimize()
     double deltaw = 0;
     double deltac = 0.0;
     bool watch_dog_step = false;
-    for (fatrop_int i = 0; i < maxiter; i++)
+    for (iter_count_ = start_iter_; iter_count_ < maxiter; iter_count_++)
     {
         fatropdata_->obj_curr = eval_objective_curr();
         // if (fatropdata_->LamLinfCurr() > 1e12)
@@ -154,19 +150,20 @@ fatrop_int FatropAlg::optimize()
         eval_obj_grad_curr(); // needed for dual inf
         eval_dual_infeasiblity();
         IterationData &it_curr = journaller_->it_curr;
-        it_curr.iter = i;
+        it_curr.iter = iter_count_;
         it_curr.mu = mu;
         it_curr.objective = eval_objective_curr();
         it_curr.constraint_violation = fatropdata_->constr_viol_max_curr();
         it_curr.du_inf = fatropdata_->dual_inf_max_curr();
         it_curr.ls = ls;
         it_curr.reg = deltaw;
+        it_curr.resto = resto_problem_;
         if (no_no_full_steps_bc_filter >= 5)
         {
             bool reset_filter = (filter_reseted <= 5);
             if (reset_filter)
             {
-                printer_->level(1) << "resetted filter " << endl;
+                // printer_->level(1) << "resetted filter " << endl;
                 filter_reseted++;
                 filter_->reset();
                 no_no_full_steps_bc_filter = 0;
@@ -187,7 +184,7 @@ fatrop_int FatropAlg::optimize()
             }
         }
         journaller_->push();
-        journaller_->print_iterations();
+        journaller_->print_iterations(resto_problem_);
         double emu = fatropdata_->e_mu_curr(0.0);
         if (emu < acceptable_tol)
         {
@@ -197,12 +194,16 @@ fatrop_int FatropAlg::optimize()
         {
             no_acceptable_steps = 0;
         }
+        if(resto_problem_ && resto_stop_crit())
+        {
+            return 100;
+        }
 
         if (emu < tol || (no_acceptable_steps >= acceptable_iter) || ((no_conse_small_sd == 2) && (mu <= mu_min)))
         // if (emu < tol)
         {
             double total_time = blasfeo_toc(&timer);
-            journaller_->print_iterations();
+            journaller_->print_iterations(resto_problem_);
             if (no_conse_small_sd == 2)
             {
                 printer_->level(1) << "WARNING fatrop returned bc of very small search direction" << endl;
@@ -211,19 +212,19 @@ fatrop_int FatropAlg::optimize()
             {
                 printer_->level(1) << "WARNING fatrop returned acceptable tolerance" << endl;
             }
-            printer_->level(1) << "found solution" << endl;
             stats.eval_cv_count += linesearch_->eval_cv_count;
             stats.eval_obj_count += linesearch_->eval_obj_count;
             stats.eval_cv_time += linesearch_->eval_cv_time;
             stats.eval_obj_time += linesearch_->eval_obj_time;
             stats.time_total = total_time;
-            stats.iterations_count = i;
+            stats.iterations_count = iter_count_;
             stats.return_flag = 0;
             if (printer_->print_level() > 0)
             {
-                stats.print();
+                stats.print(printer_->level(1));
             }
-            fatropnlp_->finalize();
+            if(resto_problem_) return 2;
+            printer_->level(1) << "found solution" << endl;
             return 0;
         }
         // update mu
@@ -231,6 +232,9 @@ fatrop_int FatropAlg::optimize()
         while (!watch_dog_step && mu > mu_min && (fatropdata_->e_mu_curr(mu) <= kappa_eta * mu || (no_conse_small_sd == 2)))
         {
             mu = MAX(mu_min, MIN(kappa_mu * mu, pow(mu, theta_mu)));
+            fatropnlp_->update_mu(mu);
+            if(resto_problem_) fatropdata_->obj_curr = eval_objective_curr();
+            if(resto_problem_) eval_obj_grad_curr();
             filter_reseted = 0;
             filter_->reset();
             no_no_full_steps_bc_filter = 0;
@@ -286,8 +290,8 @@ fatrop_int FatropAlg::optimize()
         // cout << "norm delta_s " << Linf(fatropdata_->delta_s) << endl;
         // cout << "norm delta_x " << Linf(fatropdata_->delta_x) << endl;
         // cout << "norm delta_lam " << Linf(fatropdata_->lam_calc) << endl;
-        double stepsize = max(LinfScaled(fatropdata_->delta_x, fatropdata_->x_curr), LinfScaled(fatropdata_->delta_s, fatropdata_->s_curr));
-        bool small_search_direction_curr = stepsize < 1e-14;
+        // double stepsize = max(LinfScaled(fatropdata_->delta_x, fatropdata_->x_curr), LinfScaled(fatropdata_->delta_s, fatropdata_->s_curr));
+        bool small_search_direction_curr = fatropdata_->small_step_size();
         lsinfo = linesearch_->find_acceptable_trial_point(mu, small_search_direction_curr || watch_dog_step, watch_dog_step);
         if (recalc_y && (deltac == 0.0) && (fatropdata_->constr_viol_max_curr() < recalc_y_feas_tol))
         {
@@ -310,7 +314,7 @@ fatrop_int FatropAlg::optimize()
             if (ls == 1)
             {
                 // accept watchdog step -- continue
-                printer_->level(1) << "accepted watchdog step" << endl;
+                // printer_->level(1) << "accepted watchdog step" << endl;
                 watch_dog_step = false;
             }
             else
@@ -319,14 +323,14 @@ fatrop_int FatropAlg::optimize()
                 if (no_watch_dog_steps_taken >= max_watchdog_steps)
                 {
                     // reject watchdog step -- go back to x_k
-                    printer_->level(1) << "rejected watchdog step" << endl;
+                    // printer_->level(1) << "rejected watchdog step" << endl;
                     it_curr.type = 'x';
                     fatropdata_->restore_backup();
                     // delta_w_last = delta_w_last_backup;
                     watch_dog_step = false;
                     // todo make use of delta_x_backup and delta_s_backup
                     restore_watchdog_step = true;
-                    i--;
+                    iter_count_--;
                     continue;
                 };
                 it_curr.type = 'w';
@@ -335,9 +339,16 @@ fatrop_int FatropAlg::optimize()
 
         if (ls == 0)
         {
-            printer_->level(1) << "error: restoration phase not implemented yet" << endl;
-            stats.return_flag = 1;
-            return 1;
+            // if already in resto phase: solver failed
+            if(resto_problem_) return 1;
+            // prepare for restoration phase
+            int resto_res = start_resto_alg(mu, iter_count_);
+            if(resto_res != 100){stats.return_flag =1;return 1;}
+            else
+            {
+                return_from_resto_alg(mu);
+            }
+            iter_count_ = start_iter_ + 1;
         }
         if (watch_dog_step || ls == 1)
         {
@@ -373,7 +384,7 @@ fatrop_int FatropAlg::optimize()
         restore_watchdog_step = false;
         // if linesearch unsuccessful -> resto phase
     }
-    journaller_->print_iterations();
+    journaller_->print_iterations(resto_problem_);
     stats.return_flag = 1;
     return 1;
 }
@@ -457,19 +468,27 @@ fatrop_int FatropAlg::eval_dual_infeasiblity()
     stats.duinf_time += blasfeo_toc(&timer);
     return 0;
 }
-fatrop_int FatropAlg::perform_initializiation()
+fatrop_int FatropAlg::perform_initializiation_dual()
 {
     blasfeo_timer timer;
     blasfeo_tic(&timer);
-    fatrop_int res = fatropnlp_->initialize_slacks(mu0,
-        fatropdata_->s_curr);
-    res = fatropnlp_->initialize_dual(
+    fatrop_int res = fatropnlp_->initialize_dual(
         fatropdata_->grad_curr_x,
         fatropdata_->grad_curr_s,
         fatropdata_->lam_calc,
         // fatropdata_->s_curr,
         fatropdata_->zL_curr,
         fatropdata_->zU_curr);
+    if (res == 0 && fatropdata_->delta_dual_max() < lammax)
+    {
+        // printer_->level(1) << "accepted lam " << endl;
+        fatropdata_->accept_dual_initializiaton();
+    }
+    else
+    {
+        // printer_->level(1) << "rejected lam " << endl;
+        fatropdata_->lam_curr.SetConstant(0.0);
+    }
     stats.initialization_time += blasfeo_toc(&timer);
     return res;
 }
@@ -489,3 +508,79 @@ fatrop_int FatropAlg::solve_pd_sys(double inertia_correction_w, double inertia_c
     stats.compute_sd_time += el;
     return res;
 }
+
+fatrop_int FatropAlg::start_resto_alg(double mu, int iter)
+{
+    // augment filter with current iterate
+    filter_->augment(FilterData(0, fatropdata_->obj_curr + fatropdata_->eval_barrier_func_curr(mu), fatropdata_->constr_viol_sum_curr()));
+    fatrop_int n_ineqs = fatropdata_->n_ineqs;
+    // set mu_init of resto alg
+    mu = std::max(mu, fatropdata_->constr_viol_max_curr());
+    resto_alg_->mu0 = mu;
+    // set the starting iteration number
+    resto_alg_->start_iter_ = iter+1;
+    // initialize primal variables
+    resto_alg_->fatropdata_->x_curr.copy(fatropdata_->x_curr);
+    // initialize the first part of the slack variables
+    resto_alg_->fatropdata_->s_curr.block(0, fatropdata_->n_ineqs).copy(fatropdata_->s_curr);
+    // set n and p variables to zero
+    resto_alg_->fatropdata_->s_curr.block(fatropdata_-> n_ineqs, 2*fatropdata_->n_ineqs) = 0.;
+    // evaluate the constraint jacobian of resto_alg
+    // the constraint jacobian is required for the slack initialization (todo: make this more efficient)
+    resto_alg_->eval_constr_jac();
+    // call initialize slecks from the resto nlp
+    resto_alg_->fatropnlp_->initialize_slacks(mu, resto_alg_->fatropdata_->s_curr);
+    // initialize equality multipliers
+    resto_alg_->fatropdata_->lam_curr = 0.;
+    // initialize z0, zn and zp
+    for (fatrop_int i = 0; i < n_ineqs; i++)
+    {
+        resto_alg_->fatropdata_->zL_curr.at(i) = std::min(1000., fatropdata_->zL_curr.at(i));
+        resto_alg_->fatropdata_->zU_curr.at(i) = std::min(1000., fatropdata_->zU_curr.at(i));
+        resto_alg_->fatropdata_->zL_curr.at(n_ineqs + i) = mu / resto_alg_->fatropdata_->s_curr.at(n_ineqs + i);
+        resto_alg_->fatropdata_->zL_curr.at(2 * n_ineqs + i) = mu / resto_alg_->fatropdata_->s_curr.at(2 * n_ineqs + i);
+    }
+    // call resto alg
+    return resto_alg_->optimize();
+}
+
+fatrop_int FatropAlg::return_from_resto_alg(double mu)
+{
+    // compute delta x
+    axpby(1.0, resto_alg_->fatropdata_->x_curr, -1.0, fatropdata_->x_curr, fatropdata_->delta_x);
+    // compute delta s
+    axpby(1.0, resto_alg_->fatropdata_->s_curr.block(0, fatropdata_->n_ineqs), -1.0, fatropdata_->s_curr, fatropdata_->delta_s);
+    // compute delta z
+    fatropdata_->compute_delta_z();
+    // compute maximum step size
+    double alpha_primal = 1.0;
+    double alpha_dual = 1.0;
+    fatropdata_->maximum_step_size(alpha_primal, alpha_dual, std::max(1 - mu, 0.99));
+    // update trial step
+    fatropdata_->update_trial_step(alpha_primal, alpha_dual);
+    // augment filter
+    // filter_->augment(FilterData(0, linesearch_->eval_obj_trial() + fatropdata_->eval_barrier_func_trial(mu), fatropdata_->constr_viol_sum_curr()));
+    // evaluate some quantities before accepting the trial step
+    linesearch_->eval_constr_viol_trial();
+    // accept trial step
+    fatropdata_->accept_trial_step();
+    // initialize equality multipliers
+    eval_constr_jac(); // todo twice evaluation
+    eval_obj_grad_curr();
+    perform_initializiation_dual();
+    fatropdata_->modify_dual_bounds(mu);
+    // update iteration number
+    start_iter_ = resto_alg_->iter_count_ - 1;
+    return 0;
+}
+bool FatropAlg::resto_stop_crit()
+{
+    auto orig_p = orig_.lock();
+    // evaluate original's problem constraint violation of the current solution
+    orig_p->fatropnlp_->eval_constraint_viol(fatropdata_->x_curr, fatropdata_->s_curr, orig_p->fatropdata_->g_next);
+    double cv_i = orig_p->fatropdata_->constr_viol_sum_next();
+    double obj_i = 0.0;
+    orig_p->fatropnlp_->eval_obj(fatropdata_->obj_scale, fatropdata_->x_curr, fatropdata_->s_curr, obj_i);
+    // check if acceptable to original filter
+    return orig_p->filter_->is_acceptable(FilterData(0, obj_i, cv_i)) && cv_i < 0.9 * orig_p->fatropdata_->constr_viol_sum_curr();
+}
\ No newline at end of file
diff --git a/fatrop/solver/FatropAlg.hpp b/fatrop/solver/FatropAlg.hpp
index ea500f84..36384409 100644
--- a/fatrop/solver/FatropAlg.hpp
+++ b/fatrop/solver/FatropAlg.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
@@ -47,7 +47,8 @@ namespace fatrop
             const std::shared_ptr<Filter> &filter,
             const std::shared_ptr<LineSearch> &linesearch,
             const std::shared_ptr<Journaller> &journaller, 
-            const std::shared_ptr<FatropPrinter> &printer);
+            const std::shared_ptr<FatropPrinter> &printer, 
+            const std::shared_ptr<FatropAlg>&orig_, const std::shared_ptr<FatropAlg>&resto_alg_, bool resto_problem);
         void initialize() ;
         void reset() ;
         void set_bounds(const std::vector<double> &lower, const std::vector<double> &upper) ;
@@ -62,8 +63,11 @@ namespace fatrop
         double eval_objective_curr();
         double eval_objective_trial();
         fatrop_int eval_dual_infeasiblity();
-        fatrop_int perform_initializiation();
+        fatrop_int perform_initializiation_dual();
         fatrop_int solve_pd_sys(double inertia_correction_w, double inertia_correction_c, double mu);
+        fatrop_int start_resto_alg(double mu, int iter);
+        fatrop_int return_from_resto_alg(double mu);
+        bool resto_stop_crit();
         std::shared_ptr<FatropNLP> fatropnlp_;
         std::shared_ptr<FatropData> fatropdata_;
         std::shared_ptr<FatropOptions> fatropoptions_;
@@ -81,6 +85,10 @@ namespace fatrop
         double acceptable_tol;
         fatrop_int acceptable_iter;
         fatrop_int maxiter;
+        void set_resto_alg(const std::shared_ptr<FatropAlg> &resto_alg)
+        {
+            resto_alg_ = resto_alg;
+        };
 
     private:
         double lammax;
@@ -103,6 +111,11 @@ namespace fatrop
         double recalc_y_feas_tol;
         // bool first_try_watchdog;
         FatropStats stats;
+        std::weak_ptr<FatropAlg> orig_;
+        std::shared_ptr<FatropAlg> resto_alg_;
+        bool resto_problem_ = false;
+        fatrop_int start_iter_ = 0;
+        fatrop_int iter_count_ = 0;
     };
 } // namespace fatrop
 #endif // FATROPALGINCLUDED
\ No newline at end of file
diff --git a/fatrop/solver/FatropData.cpp b/fatrop/solver/FatropData.cpp
index b98fd523..8389196a 100644
--- a/fatrop/solver/FatropData.cpp
+++ b/fatrop/solver/FatropData.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
@@ -162,7 +162,6 @@ fatrop_int FatropData::reset()
     VECSE(lam_curr.nels(), 0.0, (VEC *)lam_curr, 0);
     VECSE(s_curr.nels(), 0.0, (VEC *)s_curr, 0);
     x_curr.copy(x_initial);
-    reset_caches();
     return 0;
 }
 fatrop_int FatropData::reset_caches()
@@ -493,7 +492,7 @@ double FatropData::constr_viol_max_curr()
 }
 double FatropData::constr_viol_max_next()
 {
-    return CACHEMACRO(cache_next.cv_linf, Linf(g_next));
+    return Linf(g_next);
 }
 double FatropData::constr_viol_sum_curr()
 {
@@ -505,7 +504,7 @@ double FatropData::constr_viol_sum_backup()
 }
 double FatropData::constr_viol_sum_next()
 {
-    return CACHEMACRO(cache_next.cv_l1, L1(g_next));
+    return L1(g_next);
 }
 double FatropData::dual_sum_curr()
 {
@@ -690,7 +689,7 @@ void FatropData::evaluate_barrier_quantities(double mu)
     VEC *gradb_U_p = (VEC *)gradb_U;
     VEC *gradb_plus_p = (VEC *)gradb_plus;
     // VEC *lam_curr_p = (VEC *)lam_curr;
-    VEC* du_inf_curr_s_wo_z_p = (VEC*)du_inf_curr_s_wo_z;
+    VEC *du_inf_curr_s_wo_z_p = (VEC *)du_inf_curr_s_wo_z;
     // fatrop_int eqs_offset = n_eqs - n_ineqs;
     // compute simga_LU gradb_LU and gradbplus
 
@@ -724,7 +723,7 @@ void FatropData::evaluate_barrier_quantities(double mu)
             VECEL(sigma_U_p, i) = 0.0;
             VECEL(gradb_U_p, i) = 0.0;
         }
-        double grad_barrier_plusi = VECEL(du_inf_curr_s_wo_z_p,  i);
+        double grad_barrier_plusi = VECEL(du_inf_curr_s_wo_z_p, i);
         if (!(lower_bounded && upper_bounded))
         {
             grad_barrier_plusi += lower_bounded ? kappa_d * mu : -kappa_d * mu;
@@ -762,4 +761,25 @@ void FatropData::compute_delta_z()
 void FatropData::compute_primal_dual_residu()
 {
 }
+
+bool FatropData::small_step_size()
+{
+    double *x_curr_dp = ((VEC *)x_curr)->pa;
+    double *s_curr_dp = ((VEC *)s_curr)->pa;
+    double *delta_x_dp = ((VEC *)delta_x)->pa;
+    double *delta_s_dp = ((VEC *)delta_s)->pa;
+
+    for (int i = 0; i < s_curr.nels(); i++)
+    {
+        if (std::abs(delta_s_dp[i]) > 1e-12 * std::max(1.0, std::abs(s_curr_dp[i])))
+            return false;
+    }
+    for (int i = 0; i < x_curr.nels(); i++)
+    {
+        if (std::abs(delta_x_dp[i]) > 1e-12 * std::max(1.0, std::abs(x_curr_dp[i])))
+            return false;
+    }
+    return true;
+}
+
 // void FatropData::B
\ No newline at end of file
diff --git a/fatrop/solver/FatropData.hpp b/fatrop/solver/FatropData.hpp
index 77bffcf1..98ce84f0 100644
--- a/fatrop/solver/FatropData.hpp
+++ b/fatrop/solver/FatropData.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
@@ -78,6 +78,7 @@ namespace fatrop
         void compute_delta_z();
         void compute_primal_dual_residu();
         void init_bounds();
+        bool small_step_size();
 
 
         const NLPDims nlpdims;
diff --git a/fatrop/solver/FatropOptions.cpp b/fatrop/solver/FatropOptions.cpp
index 4ef6afce..6bd24ec1 100644
--- a/fatrop/solver/FatropOptions.cpp
+++ b/fatrop/solver/FatropOptions.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
@@ -82,7 +82,7 @@ bool FatropOptions::has_option(const std::string &option_name) const
     {
         return true;
     }
-    if(integer_options.find(option_name) != integer_options.end())
+    if (integer_options.find(option_name) != integer_options.end())
     {
         return true;
     }
@@ -103,7 +103,8 @@ void FatropOptions::set(const string &option_name, T value) const
     {
         if constexpr (std::is_floating_point<T>::value)
         {
-            numeric_options.at(option_name).set(value);
+            for (auto &el : numeric_options.at(option_name))
+                el.set(value);
         }
         else
         {
@@ -114,13 +115,15 @@ void FatropOptions::set(const string &option_name, T value) const
     {
         if constexpr (std::is_integral<T>::value)
         {
-            integer_options.at(option_name).set(value);
+            for (auto &el : integer_options.at(option_name))
+                el.set(value);
         }
         else if constexpr (std::is_floating_point<T>::value)
         {
             if ((int)value == value)
             {
-                integer_options.at(option_name).set((int)value);
+                for (auto &el : integer_options.at(option_name))
+                    el.set(value);
             }
             else
             {
@@ -136,17 +139,20 @@ void FatropOptions::set(const string &option_name, T value) const
     {
         if constexpr (std::is_same<T, bool>::value)
         {
-            boolean_options.at(option_name).set(value);
+            for (auto &el : boolean_options.at(option_name))
+                el.set(value);
         }
         else if constexpr (std::is_integral<T>::value || std::is_floating_point<T>::value)
         {
             if (value == 0)
             {
-                boolean_options.at(option_name).set(false);
+                for (auto &el : boolean_options.at(option_name))
+                    el.set(value);
             }
             else if (value == 1)
             {
-                boolean_options.at(option_name).set(true);
+                for (auto &el : boolean_options.at(option_name))
+                    el.set(value);
             }
             else
             {
@@ -178,38 +184,50 @@ void FatropOptions::prebuilt_set(const string &option_name, T value)
 
 void FatropOptions::register_option(const DoubleOption &option)
 {
-    numeric_options[option.name_] = option;
+    numeric_options[option.name_].push_back(option);
     option.set_default();
     // check if available in prebuilt options
     if (prebuilt_double.find(option.name_) != prebuilt_double.end())
     {
         option.set(prebuilt_double[option.name_]);
     }
+    else
+    {
+        prebuilt_set(option.name_, option.default_value_);
+    }
 }
 void FatropOptions::register_option(const IntegerOption &option)
 {
-    integer_options[option.name_] = option;
+    integer_options[option.name_].push_back(option);
     option.set_default();
     // check if available in prebuilt options
     if (prebuilt_double.find(option.name_) != prebuilt_double.end())
     {
         option.set(prebuilt_double[option.name_]);
     }
+    else
+    {
+        prebuilt_set(option.name_, option.default_value_);
+    }
 }
 void FatropOptions::register_option(const BooleanOption &option)
 {
-    boolean_options[option.name_] = option;
+    boolean_options[option.name_].push_back(option);
     option.set_default();
     // check if available in prebuilt options
     if (prebuilt_double.find(option.name_) != prebuilt_double.end())
     {
         option.set(prebuilt_double[option.name_]);
     }
+    else
+    {
+        prebuilt_set(option.name_, option.default_value_);
+    }
 }
 
 void FatropOptions::register_option(const StringOption &option)
 {
-    string_options[option.name_] = option;
+    string_options[option.name_].push_back(option);
     option.set_default();
     // check if available in prebuilt options
     if (prebuilt_string.find(option.name_) != prebuilt_string.end())
@@ -224,21 +242,21 @@ auto operator<<(std::ostream &os, const FatropOptions &m) -> std::ostream &
     {
         for (auto const &x : m.numeric_options)
         {
-            os << "   " << x.first << " : " << *x.second.value << std::endl;
+                os << "   " << x.first << " : " << *x.second.at(0).value << std::endl;
         }
     }
     os << "Integer options :" << std::endl;
     {
         for (auto const &x : m.integer_options)
         {
-            os << "   " << x.first << " : " << *x.second.value << std::endl;
+                os << "   " << x.first << " : " << *x.second.at(0).value << std::endl;
         }
     }
     os << "Boolean options :" << std::endl;
     {
         for (auto const &x : m.boolean_options)
         {
-            os << "   " << x.first << " : " << *x.second.value << std::endl;
+                os << "   " << x.first << " : " << *x.second.at(0).value << std::endl;
         }
     }
     return os;
diff --git a/fatrop/solver/FatropOptions.hpp b/fatrop/solver/FatropOptions.hpp
index 57956a74..01ee9e3d 100644
--- a/fatrop/solver/FatropOptions.hpp
+++ b/fatrop/solver/FatropOptions.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
@@ -22,6 +22,7 @@
 #include <map>
 #include <type_traits>
 #include <iostream>
+#include <vector>
 #include "fatrop/auxiliary/Common.hpp"
 namespace fatrop
 {
@@ -82,10 +83,10 @@ namespace fatrop
         void register_option(const BooleanOption &option);
         void register_option(const StringOption &option);
         friend auto operator<<(std::ostream &os, const FatropOptions &m) -> std::ostream &;
-        std::map<std::string, DoubleOption> numeric_options;
-        std::map<std::string, IntegerOption> integer_options;
-        std::map<std::string, BooleanOption> boolean_options;
-        std::map<std::string, StringOption> string_options;
+        std::map<std::string, std::vector<DoubleOption>> numeric_options;
+        std::map<std::string, std::vector<IntegerOption>> integer_options;
+        std::map<std::string, std::vector<BooleanOption>> boolean_options;
+        std::map<std::string, std::vector<StringOption>> string_options;
         std::map<std::string, std::string> prebuilt_string;
         std::map<std::string, double> prebuilt_double;
     };
diff --git a/fatrop/solver/FatropPrinter.cpp b/fatrop/solver/FatropPrinter.cpp
index e673a5fe..68d0022b 100644
--- a/fatrop/solver/FatropPrinter.cpp
+++ b/fatrop/solver/FatropPrinter.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
@@ -17,3 +17,27 @@
  * You should have received a copy of the GNU Lesser General Public License
  * along with Fatrop.  If not, see <http://www.gnu.org/licenses/>. */
 #include "fatrop/solver/FatropPrinter.hpp"
+#include <cstdarg>
+
+
+namespace fatrop
+{
+    int FatropPrinter::printf(const char *fmt, ...)
+    {
+        va_list args;
+        va_start(args, fmt);
+        int n = vsnprintf(&printf_buffer_.front(), printf_buffer_.size(), fmt, args);
+        if (n>printf_buffer_.size())
+        {
+            printf_buffer_.resize(n+1);
+            n = vsnprintf(&printf_buffer_.front(), printf_buffer_.size(), fmt, args);
+        }
+        if (n>=0)
+        {
+            stream_ << &printf_buffer_.front() << std::flush;
+        }
+        va_end(args);
+        return n;
+    }
+
+} // namespace fatrop
\ No newline at end of file
diff --git a/fatrop/solver/FatropPrinter.hpp b/fatrop/solver/FatropPrinter.hpp
index f46aeab0..de97b02c 100644
--- a/fatrop/solver/FatropPrinter.hpp
+++ b/fatrop/solver/FatropPrinter.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
@@ -21,6 +21,7 @@
 #define FATROPPRINTERINCLUDED
 #include <iostream>
 #include <string>
+#include <vector>
 namespace fatrop
 {
     static std::ostream nullstream(nullptr);
@@ -40,7 +41,8 @@ namespace fatrop
     class FatropPrinter
     {
     public:
-        FatropPrinter(const int priority = 0, std::ostream &stream = std::cout) : priority_(priority), stream_(stream){};
+        FatropPrinter(const int priority = 0, std::ostream &stream = std::cout) : priority_(priority), stream_(stream),
+            printf_buffer_(256) {};
         std::ostream &
         level(const int p)
         {
@@ -50,9 +52,13 @@ namespace fatrop
         }
         int &print_level() { return priority_; }
 
+
+        int printf(const char *fmt, ...);
+
     private:
         int priority_;
         std::ostream& stream_;
+        std::vector<char> printf_buffer_;
     };
 
 } // namespace fatrop
diff --git a/fatrop/solver/FatropStats.hpp b/fatrop/solver/FatropStats.hpp
index 99a5a458..65e30c86 100644
--- a/fatrop/solver/FatropStats.hpp
+++ b/fatrop/solver/FatropStats.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
@@ -39,24 +39,24 @@ namespace fatrop
         int eval_obj_count = 0;
         int iterations_count = 0;
         int return_flag = 0;
-        void print()
+        void print(std::ostream& stream)
         {
-            std::cout << "---- stats ----" << std::endl;
-            std::cout << "compute_sd:     " << compute_sd_time << " s"<< std::endl;
-            std::cout << "duinf:          " << duinf_time << " s"<< std::endl;
-            std::cout << "initialization: " << initialization_time << " s  count: "<< iterations_count <<std::endl;
+            stream << "---- stats ----" << std::endl;
+            stream << "compute_sd:     " << compute_sd_time << " s"<< std::endl;
+            stream << "duinf:          " << duinf_time << " s"<< std::endl;
+            stream << "initialization: " << initialization_time << " s  count: "<< iterations_count <<std::endl;
             double time_FE = eval_hess_time + eval_jac_time + eval_cv_time + eval_grad_time + eval_obj_time ;
-            std::cout << "time_FE :       " << time_FE << " s"<< std::endl;
-            std::cout << "    eval hess:  " << eval_hess_time << " s  count: "<< eval_hess_count <<std::endl;
-            std::cout << "    eval jac:   " << eval_jac_time << " s  count: "<< eval_jac_count <<std::endl;
-            std::cout << "    eval cv:    " << eval_cv_time << " s  count: "<< eval_cv_count <<std::endl;
-            std::cout << "    eval grad:  " << eval_grad_time << " s  count: "<< eval_grad_count <<std::endl;
-            std::cout << "    eval obj:   " << eval_obj_time << " s  count: "<< eval_obj_count <<std::endl;
-            std::cout << "rest  :       " <<time_total -  time_FE - initialization_time - duinf_time - compute_sd_time<< " s"<< std::endl;
-            std::cout << "----- "<< std::endl;
-            std::cout << "time_w/o_FE : " << time_total - time_FE << " s" << std::endl;
-            std::cout << "time_FE :       " << time_FE << " s"<< std::endl;
-            std::cout << "time_total : " << time_total << " s  iterations: " << iterations_count << std::endl;
+            stream << "time_FE :       " << time_FE << " s"<< std::endl;
+            stream << "    eval hess:  " << eval_hess_time << " s  count: "<< eval_hess_count <<std::endl;
+            stream << "    eval jac:   " << eval_jac_time << " s  count: "<< eval_jac_count <<std::endl;
+            stream << "    eval cv:    " << eval_cv_time << " s  count: "<< eval_cv_count <<std::endl;
+            stream << "    eval grad:  " << eval_grad_time << " s  count: "<< eval_grad_count <<std::endl;
+            stream << "    eval obj:   " << eval_obj_time << " s  count: "<< eval_obj_count <<std::endl;
+            stream << "rest  :       " <<time_total -  time_FE - initialization_time - duinf_time - compute_sd_time<< " s"<< std::endl;
+            stream << "----- "<< std::endl;
+            stream << "time_w/o_FE : " << time_total - time_FE << " s" << std::endl;
+            stream << "time_FE :       " << time_FE << " s"<< std::endl;
+            stream << "time_total : " << time_total << " s  iterations: " << iterations_count << std::endl;
         }
     };
 } // namespace fatrop
diff --git a/fatrop/solver/Filter.cpp b/fatrop/solver/Filter.cpp
index cbe50583..27424ded 100644
--- a/fatrop/solver/Filter.cpp
+++ b/fatrop/solver/Filter.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/solver/Filter.hpp b/fatrop/solver/Filter.hpp
index 3a2b8d88..0b5b7472 100644
--- a/fatrop/solver/Filter.hpp
+++ b/fatrop/solver/Filter.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/solver/IterationData.cpp b/fatrop/solver/IterationData.cpp
index 491f6289..8a4f0008 100644
--- a/fatrop/solver/IterationData.cpp
+++ b/fatrop/solver/IterationData.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
@@ -22,13 +22,13 @@ Journaller::Journaller(const fatrop_int maxiter, const std::shared_ptr<FatropPri
 {
     iterationdata.reserve(maxiter + 1);
 }
-void Journaller::print_iterations()
+void Journaller::print_iterations(bool no_header)
 {
     if (printer_->print_level() < 1)
         return;
-    if (print_count == 0)
+    if (!no_header && print_count == 0)
     {
-        printf(" it  obj            cv        du        lg(mu) reg  alpha_du  alpha_pr  ls\n");
+        printer_->printf(" it   obj            cv        du        lg(mu) reg  alpha_du  alpha_pr  ls\n");
     }
     for (std::vector<double>::size_type i = print_count; i < iterationdata.size(); i++)
     {
@@ -43,11 +43,11 @@ void Journaller::print_iterations()
         // }
         if (iterationdata_i.reg == 0.0)
         {
-            printf("%3d, %.7e, %.2e, %.2e, %4.1f,  -.-, %.2e, %.2e, %d%c \n", iterationdata_i.iter, iterationdata_i.objective, iterationdata_i.constraint_violation, iterationdata_i.du_inf, log10(iterationdata_i.mu), iterationdata_i.alpha_du, iterationdata_i.alpha_pr, abs(iterationdata_i.ls), iterationdata_i.type);
+            printer_->printf("%3d %c %.7e  %.2e  %.2e  %4.1f   -.-  %.2e  %.2e  %d%c \n", iterationdata_i.iter, iterationdata_i.resto?'R':' ' , iterationdata_i.objective, iterationdata_i.constraint_violation, iterationdata_i.du_inf, log10(iterationdata_i.mu), iterationdata_i.alpha_du, iterationdata_i.alpha_pr, abs(iterationdata_i.ls), iterationdata_i.type);
         }
         else
         {
-            printf("%3d, %.7e, %.2e, %.2e, %4.1f, %4.1f, %.2e, %.2e, %d%c \n", iterationdata_i.iter, iterationdata_i.objective, iterationdata_i.constraint_violation, iterationdata_i.du_inf, log10(iterationdata_i.mu), log10(iterationdata_i.reg), iterationdata_i.alpha_du, iterationdata_i.alpha_pr, abs(iterationdata_i.ls), iterationdata_i.type);
+            printer_->printf("%3d %c %.7e  %.2e  %.2e  %4.1f  %4.1f  %.2e  %.2e  %d%c \n", iterationdata_i.iter, iterationdata_i.resto?'R':' ', iterationdata_i.objective, iterationdata_i.constraint_violation, iterationdata_i.du_inf, log10(iterationdata_i.mu), log10(iterationdata_i.reg), iterationdata_i.alpha_du, iterationdata_i.alpha_pr, abs(iterationdata_i.ls), iterationdata_i.type);
         }
     }
     print_count = iterationdata.size();
diff --git a/fatrop/solver/IterationData.hpp b/fatrop/solver/IterationData.hpp
index 60e40d5f..c2566907 100644
--- a/fatrop/solver/IterationData.hpp
+++ b/fatrop/solver/IterationData.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
@@ -38,12 +38,13 @@ namespace fatrop
         double alpha_pr = 0.0;
         double alpha_du = 0.0;
         char type = 'x';
+        bool resto = false;
     };
     class Journaller
     {
     public:
         Journaller(const fatrop_int maxiter, const std::shared_ptr<FatropPrinter> &printer);
-        void print_iterations();
+        void print_iterations(bool no_header = false);
         void push();
         void reset();
         fatrop_int print_count = 0;
diff --git a/fatrop/solver/LineSearch.cpp b/fatrop/solver/LineSearch.cpp
index e508ce69..d3a275ab 100644
--- a/fatrop/solver/LineSearch.cpp
+++ b/fatrop/solver/LineSearch.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
@@ -19,13 +19,16 @@
 #include "fatrop/solver/LineSearch.hpp"
 using namespace fatrop;
 using namespace std;
+
+
+
 LineSearch::LineSearch(
     const shared_ptr<FatropOptions> &fatropparams,
     const shared_ptr<FatropNLP> &nlp,
     const shared_ptr<FatropData> &fatropdata, const std::shared_ptr<FatropPrinter> &printer) : AlgStrategy(fatropparams),
                                                                                                fatropnlp_(nlp),
                                                                                                fatropdata_(fatropdata), printer_(printer){};
-inline fatrop_int LineSearch::eval_constr_viol_trial()
+fatrop_int LineSearch::eval_constr_viol_trial()
 {
     blasfeo_timer timer;
     blasfeo_tic(&timer);
@@ -83,10 +86,10 @@ fatrop_int LineSearch::compute_second_order_correction(double alpha) const
 {
     axpy(alpha, fatropdata_->g_soc, fatropdata_->g_next, fatropdata_->g_soc);
     fatrop_int res = fatropnlp_->solve_soc_rhs(fatropdata_->delta_x, fatropdata_->lam_calc, fatropdata_->delta_s, fatropdata_->g_soc);
-    if (res != 0)
-    {
-        printer_->level(1) << "SolveSOC failed" << endl;
-    }
+    // if (res != 0)
+    // {
+    //     // printer_->level(1) << "SolveSOC failed" << endl;
+    // }
     return res;
 };
 
@@ -104,7 +107,7 @@ BackTrackingLineSearch::BackTrackingLineSearch(
     fatrop_params_->register_option(DoubleOption::lower_bounded("s_phi", "s_phi", &s_phi, 2.3, 0.0));
     fatrop_params_->register_option(DoubleOption::lower_bounded("delta", "delta", &delta, 1.0, 0.0));
     fatrop_params_->register_option(DoubleOption::lower_bounded("s_theta", "s_theta", &s_theta, 1.1, 0.0));
-    fatrop_params_->register_option(DoubleOption::lower_bounded("gamma_theta", "gamma_theta", &gamma_theta, 1e-12, 0.0));
+    fatrop_params_->register_option(DoubleOption::lower_bounded("gamma_theta", "gamma_theta", &gamma_theta, 1e-5, 0.0));
     fatrop_params_->register_option(DoubleOption::lower_bounded("gamma_phi", "gamma_phi", &gamma_phi, 1e-8, 0.0));
     fatrop_params_->register_option(DoubleOption::lower_bounded("eta_phi", "eta_phi", &eta_phi, 1e-8, 0.0));
     fatrop_params_->register_option(DoubleOption::lower_bounded("gamma_alpha", "gamma_alpha", &gamma_alpha, 0.05, 0.0));
@@ -166,8 +169,8 @@ LineSearchInfo BackTrackingLineSearch::find_acceptable_trial_point(double mu, bo
         if (filter_->is_acceptable(FilterData(0, obj_next, cv_next)))
         {
             // cout << filter_->GetSize() << endl;
-            bool switch_cond = (lin_decr_curr < 0) && (alpha_primal_accent * pow(-lin_decr_curr, s_phi) > delta * pow(cv_curr, s_theta));
-            bool armijo = obj_next - obj_curr < eta_phi * alpha_primal_accent * lin_decr_curr;
+            bool switch_cond = (lin_decr_curr < 0) && (alpha_primal_accent * pow(-lin_decr_curr, s_phi) >= delta * pow(cv_curr, s_theta));
+            bool armijo = CompareLessEqual(obj_next - obj_curr, eta_phi * alpha_primal_accent * lin_decr_curr, obj_next);
             if (switch_cond && (cv_curr <= theta_min))
             {
                 // f-step
@@ -185,7 +188,7 @@ LineSearchInfo BackTrackingLineSearch::find_acceptable_trial_point(double mu, bo
             {
                 // h-step
                 // check sufficient decrease wrt current iterate
-                if ((cv_next < (1.0 - gamma_theta) * cv_curr) || (obj_next < obj_curr - gamma_phi * cv_curr))
+                if (CompareLessEqual(cv_next, (1.0 - gamma_theta) * cv_curr, cv_curr) || CompareLessEqual(obj_next, obj_curr - gamma_phi * cv_curr, obj_curr))
                 {
                     if (!switch_cond || !(armijo))
                     {
@@ -286,7 +289,6 @@ LineSearchInfo BackTrackingLineSearch::find_acceptable_trial_point(double mu, bo
             alpha_primal *= 0.50;
         }
     }
-    assert(false);
     res.ls = 0;
     return res;
 };
\ No newline at end of file
diff --git a/fatrop/solver/LineSearch.hpp b/fatrop/solver/LineSearch.hpp
index 61b3a530..39040dc9 100644
--- a/fatrop/solver/LineSearch.hpp
+++ b/fatrop/solver/LineSearch.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
@@ -43,7 +43,7 @@ namespace fatrop
             const std::shared_ptr<FatropNLP> &nlp,
             const std::shared_ptr<FatropData> &fatropdata, const std::shared_ptr<FatropPrinter> &printer);
         virtual LineSearchInfo find_acceptable_trial_point(double mu, bool small_sd, bool from_backup) = 0;
-        inline fatrop_int eval_constr_viol_trial();
+        fatrop_int eval_constr_viol_trial();
         double eval_obj_trial();
         void reset();
         virtual fatrop_int update_trial_step(double alpha_pr, double alpha_du) const;
diff --git a/fatrop/spectool/examples/example.cpp b/fatrop/spectool/examples/example.cpp
index dbfc5934..74b6bdf5 100644
--- a/fatrop/spectool/examples/example.cpp
+++ b/fatrop/spectool/examples/example.cpp
@@ -74,24 +74,24 @@ int main()
   //   std::cout << funcc(std::vector<cs::DM>{}) << std::endl;
   //   std::cout << funcc(std::vector<cs::DM>{}) << std::endl;
   // }
-  {
-    auto ss = Ocp();
-    auto x = ss.control(10);
-    auto ustage = uStage();
-    ustage.register_control({x});
-    ustage.add_objective(sum1(sin(x)));
-    ustage.subject_to(sumsqr(x) == 1.);
-    auto ustage_dup = ustage.clone();
-    ss.add_ustage(ustage);
-    ss.add_ustage(ustage_dup);
-    ss.solver("fatrop", {{"jit", true}});
-    std::cout <<cs::MX::evalf(cs::MX::substitute(sumsqr(x), x, cs::MX::ones(10))) << std::endl;
-    auto test_func = cs::Function("test", {x}, {x});
-    std::cout << test_func(cs::DM::ones(10)) << std::endl;
-    auto funcc = ss.to_function("example_dense", {}, {ustage.at_t0(x)});
-    std::cout << funcc(std::vector<cs::DM>{}) << std::endl;
-    // std::cout << funcc(std::vector<cs::DM>{}) << std::endl;
-  }
+  // {
+  //   auto ss = Ocp();
+  //   auto x = ss.control(10);
+  //   auto ustage = uStage();
+  //   ustage.register_control({x});
+  //   ustage.add_objective(sum1(sin(x)));
+  //   ustage.subject_to(sumsqr(x) == 1.);
+  //   auto ustage_dup = ustage.clone();
+  //   ss.add_ustage(ustage);
+  //   ss.add_ustage(ustage_dup);
+  //   ss.solver("fatrop", {{"jit", true}});
+  //   std::cout <<cs::MX::evalf(cs::MX::substitute(sumsqr(x), x, cs::MX::ones(10))) << std::endl;
+  //   auto test_func = cs::Function("test", {x}, {x});
+  //   std::cout << test_func(cs::DM::ones(10)) << std::endl;
+  //   auto funcc = ss.to_function("example_dense", {}, {ustage.at_t0(x)});
+  //   std::cout << funcc(std::vector<cs::DM>{}) << std::endl;
+  //   // std::cout << funcc(std::vector<cs::DM>{}) << std::endl;
+  // }
   // {
   //   auto func = fatropy::get_func_from_py("example", "get_func");
   //   func(std::vector<cs::DM>{2.5});
diff --git a/fatrop/spectool/examples/robot2d.py b/fatrop/spectool/examples/robot2d.py
index ff6ec684..0e043f2a 100644
--- a/fatrop/spectool/examples/robot2d.py
+++ b/fatrop/spectool/examples/robot2d.py
@@ -87,6 +87,43 @@ def get_dynamics(self, W, q, qd, U):
             T += 0.5 * link.mass * (dx**2 + dy**2) + 0.5 * link.inertia * dtheta**2
             V += link.mass * 9.81 * y
         return ld.get_ddq_jac(q, qd, U, T, V, W)
+
+    @property
+    def mass_total(self):
+        return sum([link.mass for link in self.links])
+
+    @property
+    def center_of_mass(self):
+        return sum([cs.vertcat(link.center.x, link.center.y)*link.mass for link in self.links]) / self.mass_total
+
+    @property
+    def kinetic_energy(self):
+        T = 0
+        for link in self.links:
+            mass_center_link = link.center
+            dx = mass_center_link.dx
+            dy = mass_center_link.dy
+            dtheta = mass_center_link.dtheta
+            T += 0.5 * link.mass * (dx*dx + dy*dy) + 0.5 * link.inertia * dtheta*dtheta
+        return T
+
+    @property
+    def potential_energy(self):
+        V = 0
+        for link in self.links:
+            mass_center_link = link.center
+            y = mass_center_link.y
+            V += link.mass * 9.81 * y
+        return V
+
+    @property
+    def energy(self):
+        return self.kinetic_energy + self.potential_energy
+    
+
+
+
+
     
 def animate(mechs_qs_vals):
     mechanisms, q_syms, q_valss = zip(*mechs_qs_vals)
diff --git a/fatrop/spectool/examples/simulator.py b/fatrop/spectool/examples/simulator.py
new file mode 100644
index 00000000..31af5e38
--- /dev/null
+++ b/fatrop/spectool/examples/simulator.py
@@ -0,0 +1,23 @@
+import casadi as cs
+import numpy as np
+from typing  import List
+
+class simulator:
+    def __init__(self, x: cs.MX, u :cs.MX, xp :cs.MX):
+        self.x = x
+        self.u = u
+        self.xp = xp
+        self.f = cs.Function('f', [x, u], [xp])
+
+    def step(self, x, u):
+        return self.f(x, u)
+
+    def simulate(self, x0 : cs.DM , u: cs.DM, out: List[cs.MX]):
+        N = u.shape[1]
+        x_res = [x0]
+        u = cs.horzsplit(u)
+        for i in range(N):
+            x_res.append(self.step(x_res[-1], u[i]))
+        out_map = cs.Function('out_map', [self.x, self.u], out)
+        u += [u[-1]] # to make the length of u equal to N
+        return [cs.horzcat(*outi)  for outi in zip(*[out_map(x_res[i], u[i]) for i in range(N+1)])]
\ No newline at end of file
diff --git a/fatrop/spectool/examples/walker2d_multi_steps_arms.py b/fatrop/spectool/examples/walker2d_multi_steps_arms.py
deleted file mode 100644
index 8886186f..00000000
--- a/fatrop/spectool/examples/walker2d_multi_steps_arms.py
+++ /dev/null
@@ -1,84 +0,0 @@
-import walker2d_dynamics
-import casadi as cs
-import numpy as np
-import fatropy.spectool as sp
-import robot2d as r2d
-
-N = 10
-N_steps = 4
-T = .5
-ocp = sp.Ocp()
-
-q = ocp.state(10)
-qd = ocp.state(10)
-tau = ocp.control(10)
-
-q0 = cs.DM([0, 0, 0, 0, 0 , 0., 0., 0., 0., 0.]) # foot_left, foot_right, knee_left, knee_right, hip_left, hip_right
-qd0 = cs.DM([0, 0, 0, 0, 0, 0., 0., 0., 0., 0.])
-qf = cs.DM([0, 0., 0, 0, 0, 0., 0., 0., 0., 0.])
-qdf = cs.DM([0, 0, 0, 0, 0, 0., 0., 0., 0., 0.])
-
-all_steps = []
-all_mechanisms = []
-
-for step in range(N_steps):
-    ddq_left, mechanism_left, links_left = walker2d_dynamics.ode_left_contact_arms(q, qd, tau, 2*step*0.25-1.)
-    ddq_right, mechanism_right, links_right = walker2d_dynamics.ode_right_contact_arms(q, qd, tau, (2*step+1)*0.25-1.)
-    all_mechanisms += (mechanism_left, mechanism_right)
-    stage_left = ocp.new_stage(N)
-    stage_right = ocp.new_stage(N)
-    all_steps.append((stage_left, stage_right))
-    # Define the dynamics
-
-    stage_left.set_next(q, q + qd * T / N)
-    stage_left.set_next(qd, qd + ddq_left * T / N)
-
-    stage_right.set_next(q, q + qd * T / N)
-    stage_right.set_next(qd, qd + ddq_right * T / N)
-
-    # transition dynamics
-    stage_left.at_tf().set_next(q, q)
-    stage_left.at_tf().set_next(qd, qd)
-    if step != N_steps - 1:
-        stage_right.at_tf().set_next(q, q)
-        stage_right.at_tf().set_next(qd, qd)
-
-    # transition constraints
-    foot_right = links_left[-1]
-    stage_left.at_tf().subject_to(foot_right.left.origin() == [(2*step+1)*0.25-1., 0.0])
-    stage_left.at_tf().subject_to(cs.sin(foot_right.left.theta/2) == 0.) # theta = 0. + k * pi
-    if step != N_steps - 1:
-        foot_left = links_right[-1]
-        stage_right.at_tf().subject_to(foot_left.left.origin() == [(2*step+2)*0.25-1., 0.0])
-        stage_right.at_tf().subject_to(cs.sin(foot_left.left.theta/2) == 0.) # theta = 0. + k * pi
-
-    # Define the cost function
-    for stage in [stage_left, stage_right]:
-        stage.add_objective(10*cs.sumsqr(tau), sp.t0, sp.mid, sp.tf)
-        stage.add_objective(cs.sumsqr(qd), sp.t0, sp.mid)
-
-    # no over-bending of knee
-    for stage in [stage_left, stage_right]:
-        stage.subject_to(0. <= q[2], sp.mid)
-        stage.subject_to(0. >= q[3], sp.mid)
-    # no torque in wrong direction from feet
-    stage_left.subject_to(tau[0]<0., sp.mid)
-    stage_right.subject_to(tau[1]>0., sp.mid)
-
-
-# Define the constraints
-ocp.at_t0().subject_to(q == q0)
-ocp.at_t0().subject_to(qd == qd0)
-ocp.at_tf().subject_to(q == qf)
-ocp.at_tf().subject_to(qd == qdf)
-
-ocp.set_initial(q, np.array([0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0.]))
-
-
-ocp.solver('fatrop', {"post_expand":True, "jit":False}, {})
-
-fun = ocp.to_function("fun", [], [ocp.sample(q)[1], stage_left.at_tf(foot_right.left.origin())])
-
-sol = fun()
-q_sol = sol['o0']
-r2d.animate([(all_mechanisms[i], q, q_sol[:, i*(N+1):(i+1)*(N+1)-1]) for i in range(2*N_steps)])
\ No newline at end of file
diff --git a/fatrop/spectool/spec/ocp.cpp b/fatrop/spectool/spec/ocp.cpp
index 15e8aa12..7efd51b5 100644
--- a/fatrop/spectool/spec/ocp.cpp
+++ b/fatrop/spectool/spec/ocp.cpp
@@ -58,7 +58,7 @@ namespace fatrop
         void Ocp::add_ustage(const uStage &ustage)
         {
             // check if ustage is already registered
-            if(std::find(ustages_.begin(), ustages_.end(), ustage) != ustages_.end()) 
+            if (std::find(ustages_.begin(), ustages_.end(), ustage) != ustages_.end())
                 throw std::runtime_error("ustage is already registered, consider using uStage::clone()");
             ustages_.push_back(ustage);
         }
@@ -139,20 +139,20 @@ namespace fatrop
         {
             return ustages_;
         }
-        cs::Function Ocp::to_function(const std::string &name, const std::vector<cs::MX> &in, const std::vector<cs::MX> &out) const 
+        cs::Function Ocp::to_function(const std::string &name, const std::vector<cs::MX> &in, const std::vector<cs::MX> &out) const
         {
-            if(get()->solver_name == "")
+            if (get()->solver_name == "")
                 throw std::runtime_error("solver not set, use ocp.solver(\"fatrop\") or ocp.solver(\"ipopt\")");
-            if(get()->solver_name == "fatrop")
-                get() -> solver_ptr = std::make_shared<SolverFatrop>();
-            else if(get()-> solver_name == "ipopt")
-                get() -> solver_ptr = std::make_shared<SolverOpti>();
+            if (get()->solver_name == "fatrop")
+                get()->solver_ptr = std::make_shared<SolverFatrop>();
+            else if (get()->solver_name == "ipopt")
+                get()->solver_ptr = std::make_shared<SolverOpti>();
             else
                 throw std::runtime_error("solver not supported");
-            get()->solver_ptr -> transcribe(*this, function_opts_);
+            get()->solver_ptr->transcribe(*this, function_opts_);
             std::vector<cs::MX> gist_solver_in;
             std::vector<cs::MX> gist_solver_out;
-            auto fatrop_func = get()-> solver_ptr -> to_function(name, *this, gist_solver_in, gist_solver_out, solver_opts_);
+            auto fatrop_func = get()->solver_ptr->to_function(name, *this, gist_solver_in, gist_solver_out, solver_opts_);
             cs::MX vars = gist_solver_in[0];
             cs::MX initial_guess = gist_solver_in[0];
             auto helper0 = cs::Function("helper0", in, {vars}, cs::Dict{{"allow_free", true}});
@@ -174,6 +174,7 @@ namespace fatrop
         }
         std::vector<cs::MX> Ocp::eval_at_initial(const std::vector<cs::MX> &expr) const
         {
+            auto all_vars_ocp = all_variables();
             auto all_vars = cs::MX::symvar(veccat(expr));
             std::vector<cs::MX> from;
             std::vector<cs::MX> to;
@@ -185,16 +186,15 @@ namespace fatrop
                     throw std::runtime_error("initial value has wrong size");
 
                 if (get()->is_state(var) || get()->is_control(var) || get()->is_hybrid(var) || get()->is_control_parameter(var))
-                    varr = sample(var).second;
+                    varr = sample(cs::MX::veccat({var})).second;
+                else if (get()->is_global_parameter(var))
+                    varr = var;
+                else if (!cs::Function("helper", {all_vars_ocp}, {var}, cs::Dict{{"allow_free", true}}).has_free())
+                    varr = var;
                 else
-                {
-                    if (get()->is_global_parameter(var))
-                        varr = var;
-                    else
-                        std::runtime_error("at eval_at_initial variable is not a state, control, hybrid or parameter");
-                }
-                if (value.size2() == 1)
-                    valuee = cs::MX::repmat(value, 1, varr.size2());
+                    throw std::runtime_error("unrecognized variable at eval_at_initial");
+                if (value.numel() != varr.numel())
+                    valuee = cs::MX::repmat(cs::MX::veccat({value}), 1, varr.size2());
                 else
                     valuee = value;
                 from.push_back(varr);
@@ -214,14 +214,14 @@ namespace fatrop
         {
             return Stage(*this, K);
         }
-        cs::MX Ocp::all_variables()
+        cs::MX Ocp::all_variables() const
         {
             std::vector<cs::MX> gist_in;
             std::vector<cs::MX> gist_out;
             SolverFatrop().gist(*this, gist_in, gist_out);
             return cs::MX::veccat(gist_out);
         }
-    
+
         void Ocp::subject_to(const cs::MX &expr)
         {
             auto vars = cs::MX::symvar(expr);
@@ -230,17 +230,17 @@ namespace fatrop
             for (const auto &ustage : ustages_)
             {
                 // auto ustage_vars = ustage.get_variables();
-                if(ustage.K() ==1 && ustage.get() -> is_evaluable(expr))
+                if (ustage.K() == 1 && ustage.get()->is_evaluable(expr))
                     dep_ustage_list.push_back(ustage);
             }
             // if only one ustage
             if (dep_ustage_list.size() == 1)
-                // add to this ustage
-                {
-                    auto to = cs::MX::veccat(dep_ustage_list[0].all_vars());
-                    auto from = dep_ustage_list[0].at_t0(to);
-                    dep_ustage_list[0].subject_to(cs::MX::substitute(expr, from, to));
-                }
+            // add to this ustage
+            {
+                auto to = cs::MX::veccat(dep_ustage_list[0].all_vars());
+                auto from = dep_ustage_list[0].at_t0(to);
+                dep_ustage_list[0].subject_to(cs::MX::substitute(expr, from, to));
+            }
             else
             {
                 // if multiple uStages add to multi_ustage_constraints
diff --git a/fatrop/spectool/spec/ocp.hpp b/fatrop/spectool/spec/ocp.hpp
index 39e146bd..e9bf289a 100644
--- a/fatrop/spectool/spec/ocp.hpp
+++ b/fatrop/spectool/spec/ocp.hpp
@@ -83,7 +83,7 @@ namespace fatrop
             uStage at_tf() const { return ustages_.back(); };
             void set_initial(const cs::MX &var, const cs::MX &value);
             std::vector<cs::MX> eval_at_initial(const std::vector<cs::MX> &expr) const;
-            cs::MX all_variables();
+            cs::MX all_variables() const;
             void solver(const std::string &name, const cs::Dict &function_opts = casadi::Dict(), const cs::Dict &solver_opts = casadi::Dict()){get()->solver_name = name; solver_opts_ = solver_opts; function_opts_ = function_opts;};
             std::shared_ptr<SolverInterface> get_solver() const {return get()->solver_ptr;};
             void subject_to(const cs::MX &expr);
diff --git a/fatrop/spectool/spec/ustage_quantities.cpp b/fatrop/spectool/spec/ustage_quantities.cpp
index 9f472986..64845216 100644
--- a/fatrop/spectool/spec/ustage_quantities.cpp
+++ b/fatrop/spectool/spec/ustage_quantities.cpp
@@ -17,14 +17,14 @@ namespace fatrop
             uo_map_mx<Hessian> hessians;
             uo_map_mx<Jacobian> jacobians;
             std::vector<cs::MX> x_next;
-            if (next || ustage -> K()>1)
+            if (next || ustage->K() > 1)
             {
-                auto next_states = (ustage->K()==1)? next->get_states(true, ustage): ustage->get_states(true, prev);
+                auto next_states = (ustage->K() == 1) ? next->get_states(true, ustage) : ustage->get_states(true, prev);
                 for (auto &state : next_states)
                 {
                     try
                     {
-                        x_next.push_back(ustage->get_next_states().at(state));
+                        x_next.push_back(cs::MX::veccat({ustage->get_next_states().at(state)}));
                     }
                     catch (const std::exception &e)
                     {
@@ -123,7 +123,7 @@ namespace fatrop
             ret.hess_g_eq = generate_hessian(ux, ret.lam_g_eq, cs::MX(0.0), equality_constraints, hessians);
             ret.hess_g_ineq = generate_hessian(ux, ret.lam_g_ineq, cs::MX(0.0), inequality_constraints, hessians);
             ret.lb = cs::DM::veccat(lb_vec);
-            ret.ub = cs::DM::veccat(ub_vec);  
+            ret.ub = cs::DM::veccat(ub_vec);
             return ret;
         }
 
@@ -138,20 +138,34 @@ namespace fatrop
             }
             else
             {
-                // check if Jac has the right dimensions
                 if (jac.Jx.size1() != g.size1() || jac.Jx.size2() != x.size1())
                 {
                     throw std::runtime_error("Jacobian has wrong dimensions");
                 }
+                auto P = evalf(jacobian(jac.x, x));
+                cs::MX x_r;
+                cs::MX jac_r;
+                if (P.is_eye())
+                {
+                    x_r = x;
+                    jac_r = jac.Jx;
+                }
+                else
+                {
+                    x_r = mtimes(transpose(P), jac.x);
+                    jac_r = mtimes(jac.Jx, P);
+                }
+                auto perm_vec = P.sparsity().permutation_vector(true);
+                // check if Jac has the right dimensions
                 // check if all elements of x are the same as jac.x
                 for (int i = 0; i < jac.x.size1(); i++)
                 {
-                    if (!is_equal(x(i), jac.x(i), 1))
+                    if (!is_equal(x(i), jac.x(perm_vec.at(i)), 2))
                     {
                         throw std::runtime_error("x must be the same as in jac");
                     }
                 }
-                ret_G = jac.Jx;
+                ret_G = jac_r;
             }
             return {ret_G, g};
         }
@@ -177,21 +191,39 @@ namespace fatrop
                 {
                     throw std::runtime_error("Hessian has wrong dimensions");
                 }
+                // x.is_symbolic()
+                auto P = evalf(jacobian(hess.x, x));
+                cs::MX x_r;
+                cs::MX Hx_r;
+                cs::MX Hxx_r;
+                if (P.is_eye())
+                {
+                    x_r = x;
+                    Hx_r = hess.Hx;
+                    Hxx_r = hess.Hxx;
+                }
+                else
+                {
+                    x_r = mtimes(transpose(P), hess.x);
+                    Hx_r = mtimes(transpose(P), hess.Hx);
+                    Hxx_r = mtimes(transpose(P), mtimes(hess.Hxx, P));
+                }
+                auto perm_vec = P.sparsity().permutation_vector(true);
                 // check if all elements of x are the same as hess.x
-                for (int i = 0; i < hess.x.size1(); i++)
+                for (int i = 0; i < x_r.size1(); i++)
                 {
-                    if (!is_equal(x(i), hess.x(i), 1))
+                    if (!is_equal(x(i), hess.x(perm_vec.at(i)), 2))
                     {
                         throw std::runtime_error("x must be the same as in hess");
                     }
                 }
                 // check if lam has the right dimensions
-                if(hess.lam.size()!= g.size())
+                if (hess.lam.size() != g.size())
                 {
                     throw std::runtime_error("lam has wrong dimensions");
                 }
-                ret_H = hess.Hxx;
-                ret_h = hess.Hx;
+                ret_H = Hxx_r;
+                ret_h = Hx_r;
                 lam = hess.lam;
             }
             return {ret_H, ret_h};
diff --git a/fatrop/templates/NLPAlg.cpp b/fatrop/templates/NLPAlg.cpp
index ae0a764d..2befb85e 100644
--- a/fatrop/templates/NLPAlg.cpp
+++ b/fatrop/templates/NLPAlg.cpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
diff --git a/fatrop/templates/NLPAlg.hpp b/fatrop/templates/NLPAlg.hpp
index c5efd1cf..d22e376b 100644
--- a/fatrop/templates/NLPAlg.hpp
+++ b/fatrop/templates/NLPAlg.hpp
@@ -1,6 +1,6 @@
 /*
  * Fatrop - A fast trajectory optimization solver
- * Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
  *
  * This file is part of Fatrop.
  *
@@ -92,11 +92,13 @@ namespace fatrop
             FatropVecBF &upper) const = 0;
         virtual fatrop_int get_initial_sol_guess(
             FatropVecBF &initial) const = 0;
+        virtual void pre_solve(const FatropVecBF& x_init,const FatropVecBF& s_init){};
         // virtual fatrop_int GetDefaultParams(
         //     FatropOptions &params) const = 0;
         virtual fatrop_int Callback(FatropVecBF& primal_vars){return 0;};
         virtual void finalize(){};
         virtual void reset(){};
+        virtual void update_mu(double mu){};
     };
 } // namespace fatrop
 #endif // NLPINCLUDED
\ No newline at end of file
diff --git a/fatropy/CMakeLists.txt b/fatropy/CMakeLists.txt
index 60a46738..ccfaee47 100644
--- a/fatropy/CMakeLists.txt
+++ b/fatropy/CMakeLists.txt
@@ -11,8 +11,12 @@ target_include_directories(_fatropy PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../fatro
 # define (VERSION_INFO) here.
 target_compile_definitions(_fatropy
                            PRIVATE VERSION_INFO=${EXAMPLE_VERSION_INFO})
-# if spectool 
-if(${WITH_SPECTOOL})
-add_library(python_func_import SHARED src/fatropy.cpp src/python_func_import.cpp)
-target_link_libraries(python_func_import PRIVATE fatrop pybind11::module pybind11::embed)
-endif()
\ No newline at end of file
+# # if spectool
+# if(${WITH_SPECTOOL})
+# add_library(python_func_import SHARED src/fatropy.cpp src/python_func_import.cpp)
+# target_link_libraries(python_func_import PRIVATE fatrop pybind11::module pybind11::embed)
+# endif()
+
+set(FATROPY_INSTALL_DIR "lib/python3.${Python_VERSION_MINOR}/site-packages")
+install(DIRECTORY fatropy DESTINATION "${FATROPY_INSTALL_DIR}")
+install(TARGETS _fatropy DESTINATION "${FATROPY_INSTALL_DIR}/fatropy")
diff --git a/fatropy/src/expose-spectool.hpp b/fatropy/src/expose-spectool.hpp
index c7ae534a..e74c3e17 100644
--- a/fatropy/src/expose-spectool.hpp
+++ b/fatropy/src/expose-spectool.hpp
@@ -136,7 +136,9 @@ namespace fatropy
             .def("at_tf", py::overload_cast<const casadi::MX &>(&fatrop::spectool::Stage::at_tf, py::const_))
             .def("at_t0", py::overload_cast<>(&fatrop::spectool::Stage::at_t0, py::const_))
             .def("at_tf", py::overload_cast<>(&fatrop::spectool::Stage::at_tf, py::const_))
-            .def("at_mid", &fatrop::spectool::Stage::at_mid);
+            .def("at_mid", &fatrop::spectool::Stage::at_mid)
+            .def("sample", &fatrop::spectool::Stage::sample);
+
 
             // def("add_objective", &fatrop::spectool::Stage::add_objective<fatrop::spectool::at>);
 
diff --git a/misc/license_header.txt b/misc/license_header.txt
index c27e5f1a..9fa4e181 100644
--- a/misc/license_header.txt
+++ b/misc/license_header.txt
@@ -1,5 +1,5 @@
 Fatrop - A fast trajectory optimization solver
-Copyright (C) 2022, 2023 Lander Vanroye, KU Leuven. All rights reserved.
+ Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
 
 This file is part of Fatrop.