From 9c47f7858f4521c8adbb74bf4ef2a09fddffd9a5 Mon Sep 17 00:00:00 2001
From: Andrey A Popov <apopov@vt.edu>
Date: Tue, 10 May 2022 11:55:38 -0400
Subject: [PATCH 01/32] Initial Commit of the constrained pendulum problem

For use in half-explicit methods and for data assimilation.
---
 .../+constrainedpendulum/+presets/Canonical.m | 22 ++++++++++++
 .../ConstrainedPendulumParameters.m           | 11 ++++++
 .../ConstrainedPendulumProblem.m              | 34 +++++++++++++++++++
 src/+otp/+constrainedpendulum/constraints.m   | 14 ++++++++
 .../constraintsjacobian.m                     | 27 +++++++++++++++
 src/+otp/+constrainedpendulum/f.m             | 17 ++++++++++
 6 files changed, 125 insertions(+)
 create mode 100644 src/+otp/+constrainedpendulum/+presets/Canonical.m
 create mode 100644 src/+otp/+constrainedpendulum/ConstrainedPendulumParameters.m
 create mode 100644 src/+otp/+constrainedpendulum/ConstrainedPendulumProblem.m
 create mode 100644 src/+otp/+constrainedpendulum/constraints.m
 create mode 100644 src/+otp/+constrainedpendulum/constraintsjacobian.m
 create mode 100644 src/+otp/+constrainedpendulum/f.m

diff --git a/src/+otp/+constrainedpendulum/+presets/Canonical.m b/src/+otp/+constrainedpendulum/+presets/Canonical.m
new file mode 100644
index 00000000..c4d70fc5
--- /dev/null
+++ b/src/+otp/+constrainedpendulum/+presets/Canonical.m
@@ -0,0 +1,22 @@
+classdef Canonical < otp.constrainedpendulum.ConstrainedPendulumProblem
+    %CANONICAL The constrained pendulum problem
+    %
+    % See
+    %    Ascher, Uri. "On symmetric schemes and differential-algebraic equations."
+    %    SIAM journal on scientific and statistical computing 10.5 (1989): 937-949.
+    
+    methods
+        function obj = Canonical
+            tspan = [0; 10];
+            
+            params = otp.constrainedpendulum.ConstrainedPendulumParameters;
+            params.Mass = 1;
+            params.Length = 1;
+            params.Gravity = otp.utils.PhysicalConstants.EarthGravity;
+            
+            y0 = [sqrt(2)/2; sqrt(2)/2; 0; 0];
+            
+            obj = obj@otp.constrainedpendulum.ConstrainedPendulumProblem(tspan, y0, params);
+        end
+    end
+end
diff --git a/src/+otp/+constrainedpendulum/ConstrainedPendulumParameters.m b/src/+otp/+constrainedpendulum/ConstrainedPendulumParameters.m
new file mode 100644
index 00000000..dd7817c2
--- /dev/null
+++ b/src/+otp/+constrainedpendulum/ConstrainedPendulumParameters.m
@@ -0,0 +1,11 @@
+classdef ConstrainedPendulumParameters
+    %CONSTRAINEDPENDULUMPARAMETERS
+    properties
+        %Gravity defines the magnitude of acceleration towards the ground
+        Gravity %MATLAB ONLY: (1,1) {mustBeNonnegative}
+        %Mass defines the mass of the pendulum
+        Mass %MATLAB ONLY: (1,1) {mustBeNonnegative}
+        %Length defines the length of the pendulum
+        Length %MATLAB ONLY: (1,1) {mustBeNonnegative}
+    end
+end
diff --git a/src/+otp/+constrainedpendulum/ConstrainedPendulumProblem.m b/src/+otp/+constrainedpendulum/ConstrainedPendulumProblem.m
new file mode 100644
index 00000000..c1b11f22
--- /dev/null
+++ b/src/+otp/+constrainedpendulum/ConstrainedPendulumProblem.m
@@ -0,0 +1,34 @@
+classdef ConstrainedPendulumProblem < otp.Problem
+    %CONSTRAINEDPENDULUM PROBLEM This is a Hessenberg Index-2 DAE problem posed in terms of three constraints
+    %
+    
+    properties
+        Constraints
+        ConstraintsJacobian
+    end
+
+    methods
+        function obj = ConstrainedPendulumProblem(timeSpan, y0, parameters)
+            obj@otp.Problem('Constrained Pendulum', 4, timeSpan, y0, parameters);
+        end
+    end
+    
+    methods (Access = protected)
+        function onSettingsChanged(obj)
+            m = obj.Parameters.Mass;
+            l = obj.Parameters.Length;
+            g = obj.Parameters.Gravity;
+
+            % get initial energy
+            initialconstraints = otp.constrainedpendulum.constraints([], obj.Y0, g, m, l, 0);
+            E0 = initialconstraints(3);
+            
+            obj.RHS = otp.RHS(@(t, y) otp.constrainedpendulum.f(t, y, g, m, l, E0));
+
+            obj.Constraints = @(t, y) otp.constrainedpendulum.constraints(t, y, g, m, l, E0);
+            obj.ConstraintsJacobian = @(t, y) otp.constrainedpendulum.constraintsjacobian(t, y, g, m, l, E0);
+
+        end
+    end
+end
+
diff --git a/src/+otp/+constrainedpendulum/constraints.m b/src/+otp/+constrainedpendulum/constraints.m
new file mode 100644
index 00000000..26a7b808
--- /dev/null
+++ b/src/+otp/+constrainedpendulum/constraints.m
@@ -0,0 +1,14 @@
+function c = constraints(~, state, g, m, l, E0)
+
+x = state(1, :);
+y = state(2, :);
+u = state(3, :);
+v = state(4, :);
+
+c1 = x.^2 + y.^2 - l^2;
+c2 = 2*(x.*u + y.*v);
+c3 = m*(g*(y + l) + 0.5*(u.^2 + v.^2)) - E0;
+
+c = [c1; c2; c3];
+
+end
\ No newline at end of file
diff --git a/src/+otp/+constrainedpendulum/constraintsjacobian.m b/src/+otp/+constrainedpendulum/constraintsjacobian.m
new file mode 100644
index 00000000..75f526f8
--- /dev/null
+++ b/src/+otp/+constrainedpendulum/constraintsjacobian.m
@@ -0,0 +1,27 @@
+function dc = constraintsjacobian(~, state, g, m, ~, ~)
+
+x = state(1);
+y = state(2);
+u = state(3);
+v = state(4);
+
+dc1dx = 2*x;
+dc1dy = 2*y;
+dc1du = 0;
+dc1dv = 0;
+
+dc2dx = 2*u;
+dc2dy = 2*v;
+dc2du = 2*x;
+dc2dv = 2*y;
+
+dc3dx = 0;
+dc3dy = m*g;
+dc3du = m*g*u;
+dc3dv = m*g*v;
+
+dc = [dc1dx, dc1dy, dc1du, dc1dv; ...
+   dc2dx, dc2dy, dc2du, dc2dv; ...
+   dc3dx, dc3dy, dc3du, dc3dv];
+
+end
diff --git a/src/+otp/+constrainedpendulum/f.m b/src/+otp/+constrainedpendulum/f.m
new file mode 100644
index 00000000..e4a668a6
--- /dev/null
+++ b/src/+otp/+constrainedpendulum/f.m
@@ -0,0 +1,17 @@
+function dstate = f(~, state, g, m, l, ~)
+
+x = state(1, :);
+y = state(2, :);
+u = state(3, :);
+v = state(4, :);
+
+lambda =  (m/(l^2))*(u.^2 + v.^2) - (g/(l^2))*y;
+
+dx = u;
+dy = v;
+du = -(lambda/m).*x;
+dv = -(lambda/m)*y - g/m;
+
+dstate = [dx; dy; du; dv];
+
+end

From 6d54390762b62d3c362efdd8a3ed2a7fd8809304 Mon Sep 17 00:00:00 2001
From: Andrey A Popov <apopov@vt.edu>
Date: Tue, 10 May 2022 14:44:43 -0400
Subject: [PATCH 02/32] very useful fix for lambda

---
 src/+otp/+constrainedpendulum/f.m | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/+otp/+constrainedpendulum/f.m b/src/+otp/+constrainedpendulum/f.m
index e4a668a6..fb065841 100644
--- a/src/+otp/+constrainedpendulum/f.m
+++ b/src/+otp/+constrainedpendulum/f.m
@@ -5,7 +5,9 @@
 u = state(3, :);
 v = state(4, :);
 
-lambda =  (m/(l^2))*(u.^2 + v.^2) - (g/(l^2))*y;
+lxy2 = x.^2 + y.^2;
+
+lambda =  (m/lxy2)*(u.^2 + v.^2) - (g/lxy2)*y;
 
 dx = u;
 dy = v;

From 0caf7ccc29d860a57569ef41cc5ef5f363f5a9c4 Mon Sep 17 00:00:00 2001
From: Andrey A Popov <apopov@vt.edu>
Date: Tue, 10 May 2022 15:55:12 -0400
Subject: [PATCH 03/32] added Z0 and fixed jacobia

---
 src/+otp/+constrainedpendulum/ConstrainedPendulumProblem.m | 1 +
 src/+otp/+constrainedpendulum/constraintsjacobian.m        | 4 ++--
 2 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/+otp/+constrainedpendulum/ConstrainedPendulumProblem.m b/src/+otp/+constrainedpendulum/ConstrainedPendulumProblem.m
index c1b11f22..bef541c1 100644
--- a/src/+otp/+constrainedpendulum/ConstrainedPendulumProblem.m
+++ b/src/+otp/+constrainedpendulum/ConstrainedPendulumProblem.m
@@ -5,6 +5,7 @@
     properties
         Constraints
         ConstraintsJacobian
+        Z0 = [0; 0; 0];
     end
 
     methods
diff --git a/src/+otp/+constrainedpendulum/constraintsjacobian.m b/src/+otp/+constrainedpendulum/constraintsjacobian.m
index 75f526f8..2e7961d2 100644
--- a/src/+otp/+constrainedpendulum/constraintsjacobian.m
+++ b/src/+otp/+constrainedpendulum/constraintsjacobian.m
@@ -17,8 +17,8 @@
 
 dc3dx = 0;
 dc3dy = m*g;
-dc3du = m*g*u;
-dc3dv = m*g*v;
+dc3du = m*u;
+dc3dv = m*v;
 
 dc = [dc1dx, dc1dy, dc1du, dc1dv; ...
    dc2dx, dc2dy, dc2du, dc2dv; ...

From afdfa7e57acdf41bc60b55629f7f08c2d1d621cb Mon Sep 17 00:00:00 2001
From: Andrey A Popov <apopov@vt.edu>
Date: Wed, 11 May 2022 09:45:25 -0400
Subject: [PATCH 04/32] file renaming

---
 .../ConstrainedPendulumParameters.m                   | 11 -----------
 .../+presets/Canonical.m                              |  0
 src/+otp/+pendulumdae/PendulumDAEParameters.m         | 11 +++++++++++
 .../PendulumDAEProblem.m}                             |  4 ++--
 .../constraints.m                                     |  0
 .../constraintsjacobian.m                             |  0
 src/+otp/{+constrainedpendulum => +pendulumdae}/f.m   |  0
 src/+otp/+pendulumdae/mass.m                          |  0
 8 files changed, 13 insertions(+), 13 deletions(-)
 delete mode 100644 src/+otp/+constrainedpendulum/ConstrainedPendulumParameters.m
 rename src/+otp/{+constrainedpendulum => +pendulumdae}/+presets/Canonical.m (100%)
 create mode 100644 src/+otp/+pendulumdae/PendulumDAEParameters.m
 rename src/+otp/{+constrainedpendulum/ConstrainedPendulumProblem.m => +pendulumdae/PendulumDAEProblem.m} (89%)
 rename src/+otp/{+constrainedpendulum => +pendulumdae}/constraints.m (100%)
 rename src/+otp/{+constrainedpendulum => +pendulumdae}/constraintsjacobian.m (100%)
 rename src/+otp/{+constrainedpendulum => +pendulumdae}/f.m (100%)
 create mode 100644 src/+otp/+pendulumdae/mass.m

diff --git a/src/+otp/+constrainedpendulum/ConstrainedPendulumParameters.m b/src/+otp/+constrainedpendulum/ConstrainedPendulumParameters.m
deleted file mode 100644
index dd7817c2..00000000
--- a/src/+otp/+constrainedpendulum/ConstrainedPendulumParameters.m
+++ /dev/null
@@ -1,11 +0,0 @@
-classdef ConstrainedPendulumParameters
-    %CONSTRAINEDPENDULUMPARAMETERS
-    properties
-        %Gravity defines the magnitude of acceleration towards the ground
-        Gravity %MATLAB ONLY: (1,1) {mustBeNonnegative}
-        %Mass defines the mass of the pendulum
-        Mass %MATLAB ONLY: (1,1) {mustBeNonnegative}
-        %Length defines the length of the pendulum
-        Length %MATLAB ONLY: (1,1) {mustBeNonnegative}
-    end
-end
diff --git a/src/+otp/+constrainedpendulum/+presets/Canonical.m b/src/+otp/+pendulumdae/+presets/Canonical.m
similarity index 100%
rename from src/+otp/+constrainedpendulum/+presets/Canonical.m
rename to src/+otp/+pendulumdae/+presets/Canonical.m
diff --git a/src/+otp/+pendulumdae/PendulumDAEParameters.m b/src/+otp/+pendulumdae/PendulumDAEParameters.m
new file mode 100644
index 00000000..2465a1c6
--- /dev/null
+++ b/src/+otp/+pendulumdae/PendulumDAEParameters.m
@@ -0,0 +1,11 @@
+classdef PendulumDAEParameters
+    %CONSTRAINEDPENDULUMPARAMETERS
+    properties
+        %GRAVITY is acceleration due to gravity
+        Gravity %MATLAB ONLY: (1,1) {mustBeReal, mustBeFinite, mustBePositive} = 9.8
+        %MASSE of the node of the pendulum
+        Mass %MATLAB ONLY: (:,1) {mustBeReal, mustBeFinite, mustBePositive} = 1
+        %LENGTH to the node of the pendulum
+        Length %MATLAB ONLY: (:,1) {mustBeReal, mustBeFinite, mustBePositive} = 1
+    end
+end
diff --git a/src/+otp/+constrainedpendulum/ConstrainedPendulumProblem.m b/src/+otp/+pendulumdae/PendulumDAEProblem.m
similarity index 89%
rename from src/+otp/+constrainedpendulum/ConstrainedPendulumProblem.m
rename to src/+otp/+pendulumdae/PendulumDAEProblem.m
index bef541c1..e5574010 100644
--- a/src/+otp/+constrainedpendulum/ConstrainedPendulumProblem.m
+++ b/src/+otp/+pendulumdae/PendulumDAEProblem.m
@@ -1,4 +1,4 @@
-classdef ConstrainedPendulumProblem < otp.Problem
+classdef PendulumDAEProblem < otp.Problem
     %CONSTRAINEDPENDULUM PROBLEM This is a Hessenberg Index-2 DAE problem posed in terms of three constraints
     %
     
@@ -9,7 +9,7 @@
     end
 
     methods
-        function obj = ConstrainedPendulumProblem(timeSpan, y0, parameters)
+        function obj = PendulumDAEProblem(timeSpan, y0, parameters)
             obj@otp.Problem('Constrained Pendulum', 4, timeSpan, y0, parameters);
         end
     end
diff --git a/src/+otp/+constrainedpendulum/constraints.m b/src/+otp/+pendulumdae/constraints.m
similarity index 100%
rename from src/+otp/+constrainedpendulum/constraints.m
rename to src/+otp/+pendulumdae/constraints.m
diff --git a/src/+otp/+constrainedpendulum/constraintsjacobian.m b/src/+otp/+pendulumdae/constraintsjacobian.m
similarity index 100%
rename from src/+otp/+constrainedpendulum/constraintsjacobian.m
rename to src/+otp/+pendulumdae/constraintsjacobian.m
diff --git a/src/+otp/+constrainedpendulum/f.m b/src/+otp/+pendulumdae/f.m
similarity index 100%
rename from src/+otp/+constrainedpendulum/f.m
rename to src/+otp/+pendulumdae/f.m
diff --git a/src/+otp/+pendulumdae/mass.m b/src/+otp/+pendulumdae/mass.m
new file mode 100644
index 00000000..e69de29b

From cd25494a0d679f9f6576de8878bb910f0b29429c Mon Sep 17 00:00:00 2001
From: Andrey A Popov <apopov@vt.edu>
Date: Wed, 11 May 2022 12:51:29 -0400
Subject: [PATCH 05/32] Fixes to some of steven's complains

---
 src/+otp/+pendulumdae/+presets/Canonical.m | 14 ++++----
 src/+otp/+pendulumdae/PendulumDAEProblem.m | 37 ++++++++++++++++------
 src/+otp/+pendulumdae/f.m                  | 20 ++++--------
 src/+otp/+pendulumdae/fdifferential.m      | 19 +++++++++++
 src/+otp/+pendulumdae/mass.m               |  5 +++
 5 files changed, 67 insertions(+), 28 deletions(-)
 create mode 100644 src/+otp/+pendulumdae/fdifferential.m

diff --git a/src/+otp/+pendulumdae/+presets/Canonical.m b/src/+otp/+pendulumdae/+presets/Canonical.m
index c4d70fc5..f5e88b93 100644
--- a/src/+otp/+pendulumdae/+presets/Canonical.m
+++ b/src/+otp/+pendulumdae/+presets/Canonical.m
@@ -1,22 +1,24 @@
-classdef Canonical < otp.constrainedpendulum.ConstrainedPendulumProblem
+classdef Canonical < otp.pendulumdae.PendulumDAEProblem
     %CANONICAL The constrained pendulum problem
     %
     % See
-    %    Ascher, Uri. "On symmetric schemes and differential-algebraic equations."
-    %    SIAM journal on scientific and statistical computing 10.5 (1989): 937-949.
+    %    Hairer, E., Roche, M., Lubich, C. (1989). Description of differential-algebraic problems. 
+    %    In: The Numerical Solution of Differential-Algebraic Systems by Runge-Kutta Methods. 
+    %    Lecture Notes in Mathematics, vol 1409. Springer, Berlin, Heidelberg. 
+    %    https://doi.org/10.1007/BFb0093948
     
     methods
         function obj = Canonical
             tspan = [0; 10];
             
-            params = otp.constrainedpendulum.ConstrainedPendulumParameters;
+            params = otp.pendulumdae.PendulumDAEParameters;
             params.Mass = 1;
             params.Length = 1;
             params.Gravity = otp.utils.PhysicalConstants.EarthGravity;
             
-            y0 = [sqrt(2)/2; sqrt(2)/2; 0; 0];
+            y0 = [sqrt(2)/2; sqrt(2)/2; 0; 0; 0; 0; 0];
             
-            obj = obj@otp.constrainedpendulum.ConstrainedPendulumProblem(tspan, y0, params);
+            obj = obj@otp.pendulumdae.PendulumDAEProblem(tspan, y0, params);
         end
     end
 end
diff --git a/src/+otp/+pendulumdae/PendulumDAEProblem.m b/src/+otp/+pendulumdae/PendulumDAEProblem.m
index e5574010..d04bfe3d 100644
--- a/src/+otp/+pendulumdae/PendulumDAEProblem.m
+++ b/src/+otp/+pendulumdae/PendulumDAEProblem.m
@@ -2,15 +2,19 @@
     %CONSTRAINEDPENDULUM PROBLEM This is a Hessenberg Index-2 DAE problem posed in terms of three constraints
     %
     
-    properties
-        Constraints
-        ConstraintsJacobian
-        Z0 = [0; 0; 0];
+    properties (Access=protected)
+        RHSDifferential
+        RHSAlgebraic
+    end
+
+    properties (Dependent)
+      Y0Differential
+      Z0Algebraic
     end
 
     methods
         function obj = PendulumDAEProblem(timeSpan, y0, parameters)
-            obj@otp.Problem('Constrained Pendulum', 4, timeSpan, y0, parameters);
+            obj@otp.Problem('Constrained Pendulum', 7, timeSpan, y0, parameters);
         end
     end
     
@@ -21,15 +25,30 @@ function onSettingsChanged(obj)
             g = obj.Parameters.Gravity;
 
             % get initial energy
-            initialconstraints = otp.constrainedpendulum.constraints([], obj.Y0, g, m, l, 0);
+            initialconstraints = otp.pendulumdae.constraints([], obj.Y0Differential, g, m, l, 0);
             E0 = initialconstraints(3);
             
-            obj.RHS = otp.RHS(@(t, y) otp.constrainedpendulum.f(t, y, g, m, l, E0));
 
-            obj.Constraints = @(t, y) otp.constrainedpendulum.constraints(t, y, g, m, l, E0);
-            obj.ConstraintsJacobian = @(t, y) otp.constrainedpendulum.constraintsjacobian(t, y, g, m, l, E0);
+            % The right hand size in terms of x, y, x', y', and three control parameters z
+            obj.RHS = otp.RHS(@(t, y) otp.pendulumdae.f(t, y, g, m, l, E0), ...
+                'Mass', @(t, y) otp.pendulumdae.mass(t, y, g, m, l, E0));
+
+            obj.RHSDifferential = otp.RHS(@(t, y) otp.pendulumdae.fdifferential(t, y, g, m, l, E0));
+
+            obj.RHSAlgebraic = otp.RHS(@(t, y) otp.pendulumdae.constraints(t, y, g, m, l, E0), ...
+                'Jacobian', @(t, y) otp.pendulumdae.constraintsjacobian(t, y, g, m, l, E0));
+        end
+    end
+
+    methods
+        function y0differential = get.Y0Differential(obj)
+            y0differential = obj.Y0(1:4);
+        end
 
+        function z0algebraic = get.Z0Algebraic(obj)
+            z0algebraic = obj.Y0(5:end);
         end
     end
+
 end
 
diff --git a/src/+otp/+pendulumdae/f.m b/src/+otp/+pendulumdae/f.m
index fb065841..17b88fb8 100644
--- a/src/+otp/+pendulumdae/f.m
+++ b/src/+otp/+pendulumdae/f.m
@@ -1,19 +1,13 @@
-function dstate = f(~, state, g, m, l, ~)
+function dfull = f(t, statepluscontrol, g, m, l, E0)
 
-x = state(1, :);
-y = state(2, :);
-u = state(3, :);
-v = state(4, :);
+state   = statepluscontrol(1:4, :);
+control = statepluscontrol(5:end, :);
 
-lxy2 = x.^2 + y.^2;
+dstate = otp.pendulumdae.fdifferential(t, state, g, m, l, E0) ...
+    - otp.pendulumdae.constraintsjacobian(t, state, g, m, l, E0).'*control;
 
-lambda =  (m/lxy2)*(u.^2 + v.^2) - (g/lxy2)*y;
+c = otp.pendulumdae.constraints(t, state, g, m, l, E0);
 
-dx = u;
-dy = v;
-du = -(lambda/m).*x;
-dv = -(lambda/m)*y - g/m;
-
-dstate = [dx; dy; du; dv];
+dfull = [dstate; c];
 
 end
diff --git a/src/+otp/+pendulumdae/fdifferential.m b/src/+otp/+pendulumdae/fdifferential.m
new file mode 100644
index 00000000..8d49a29a
--- /dev/null
+++ b/src/+otp/+pendulumdae/fdifferential.m
@@ -0,0 +1,19 @@
+function dstate = fdifferential(~, state, g, m, ~, ~)
+
+x = state(1, :);
+y = state(2, :);
+u = state(3, :);
+v = state(4, :);
+
+lxy2 = x.^2 + y.^2;
+
+lambda =  (m/lxy2)*(u.^2 + v.^2) - (g/lxy2)*y;
+
+dx = u;
+dy = v;
+du = -(lambda/m).*x;
+dv = -(lambda/m)*y - g/m;
+
+dstate = [dx; dy; du; dv];
+
+end
diff --git a/src/+otp/+pendulumdae/mass.m b/src/+otp/+pendulumdae/mass.m
index e69de29b..b2f9cf25 100644
--- a/src/+otp/+pendulumdae/mass.m
+++ b/src/+otp/+pendulumdae/mass.m
@@ -0,0 +1,5 @@
+function M = mass(~, ~, ~, ~, ~, ~)
+
+M = [eye(4), zeros(4, 3); zeros(3, 7)];
+
+end

From f072d668fa69b600b3bafd54c5caccd1592f3e92 Mon Sep 17 00:00:00 2001
From: Andrey A Popov <apopov@vt.edu>
Date: Wed, 11 May 2022 13:28:53 -0400
Subject: [PATCH 06/32] access fix

---
 src/+otp/+pendulumdae/PendulumDAEProblem.m | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/+otp/+pendulumdae/PendulumDAEProblem.m b/src/+otp/+pendulumdae/PendulumDAEProblem.m
index d04bfe3d..dac05d2f 100644
--- a/src/+otp/+pendulumdae/PendulumDAEProblem.m
+++ b/src/+otp/+pendulumdae/PendulumDAEProblem.m
@@ -2,7 +2,7 @@
     %CONSTRAINEDPENDULUM PROBLEM This is a Hessenberg Index-2 DAE problem posed in terms of three constraints
     %
     
-    properties (Access=protected)
+    properties (SetAccess=protected)
         RHSDifferential
         RHSAlgebraic
     end

From dfee9887988ae682305a13ddbf04488bde291278 Mon Sep 17 00:00:00 2001
From: Andrey A Popov <apopov@vt.edu>
Date: Thu, 12 May 2022 08:05:41 -0400
Subject: [PATCH 07/32] final fixes

---
 src/+otp/+pendulumdae/PendulumDAEProblem.m    | 32 +++++++++++++++----
 src/+otp/+pendulumdae/constraints.m           | 14 --------
 src/+otp/+pendulumdae/f.m                     |  6 ++--
 .../{fdifferential.m => fDifferential.m}      |  2 +-
 src/+otp/+pendulumdae/invariants.m            | 19 +++++++++++
 ...traintsjacobian.m => invariantsJacobian.m} |  2 +-
 6 files changed, 50 insertions(+), 25 deletions(-)
 delete mode 100644 src/+otp/+pendulumdae/constraints.m
 rename src/+otp/+pendulumdae/{fdifferential.m => fDifferential.m} (81%)
 create mode 100644 src/+otp/+pendulumdae/invariants.m
 rename src/+otp/+pendulumdae/{constraintsjacobian.m => invariantsJacobian.m} (85%)

diff --git a/src/+otp/+pendulumdae/PendulumDAEProblem.m b/src/+otp/+pendulumdae/PendulumDAEProblem.m
index dac05d2f..d3497fd8 100644
--- a/src/+otp/+pendulumdae/PendulumDAEProblem.m
+++ b/src/+otp/+pendulumdae/PendulumDAEProblem.m
@@ -25,18 +25,38 @@ function onSettingsChanged(obj)
             g = obj.Parameters.Gravity;
 
             % get initial energy
-            initialconstraints = otp.pendulumdae.constraints([], obj.Y0Differential, g, m, l, 0);
+            initialconstraints = otp.pendulumdae.invariants([], obj.Y0Differential, g, m, l, 0);
             E0 = initialconstraints(3);
-            
 
             % The right hand size in terms of x, y, x', y', and three control parameters z
             obj.RHS = otp.RHS(@(t, y) otp.pendulumdae.f(t, y, g, m, l, E0), ...
-                'Mass', @(t, y) otp.pendulumdae.mass(t, y, g, m, l, E0));
+                'Mass', otp.pendulumdae.mass([], [], g, m, l, E0));
 
-            obj.RHSDifferential = otp.RHS(@(t, y) otp.pendulumdae.fdifferential(t, y, g, m, l, E0));
+            % Generate the constituent RHS for the differential part
+            obj.RHSDifferential = otp.RHS(@(t, y) otp.pendulumdae.fDifferential(t, y, g, m, l, E0));
 
-            obj.RHSAlgebraic = otp.RHS(@(t, y) otp.pendulumdae.constraints(t, y, g, m, l, E0), ...
-                'Jacobian', @(t, y) otp.pendulumdae.constraintsjacobian(t, y, g, m, l, E0));
+            % Generate the constituent RHS for the algebraic part
+            obj.RHSAlgebraic = otp.RHS(@(t, y) otp.pendulumdae.invariants(t, y, g, m, l, E0), ...
+                'Jacobian', @(t, y) otp.pendulumdae.invariantsJacobian(t, y, g, m, l, E0));
+        end
+
+        function label = internalIndex2label(~, index)
+            switch index
+                case 1
+                    label = 'x position';
+                case 2
+                    label = 'y position';
+                case 3
+                    label = 'x velocity';
+                case 4
+                    label = 'y velocity';
+                case 5
+                    label = 'Control 1';
+                case 6
+                    label = 'Control 2';
+                case 7
+                    label = 'Control 3';
+            end
         end
     end
 
diff --git a/src/+otp/+pendulumdae/constraints.m b/src/+otp/+pendulumdae/constraints.m
deleted file mode 100644
index 26a7b808..00000000
--- a/src/+otp/+pendulumdae/constraints.m
+++ /dev/null
@@ -1,14 +0,0 @@
-function c = constraints(~, state, g, m, l, E0)
-
-x = state(1, :);
-y = state(2, :);
-u = state(3, :);
-v = state(4, :);
-
-c1 = x.^2 + y.^2 - l^2;
-c2 = 2*(x.*u + y.*v);
-c3 = m*(g*(y + l) + 0.5*(u.^2 + v.^2)) - E0;
-
-c = [c1; c2; c3];
-
-end
\ No newline at end of file
diff --git a/src/+otp/+pendulumdae/f.m b/src/+otp/+pendulumdae/f.m
index 17b88fb8..f42d81b9 100644
--- a/src/+otp/+pendulumdae/f.m
+++ b/src/+otp/+pendulumdae/f.m
@@ -3,10 +3,10 @@
 state   = statepluscontrol(1:4, :);
 control = statepluscontrol(5:end, :);
 
-dstate = otp.pendulumdae.fdifferential(t, state, g, m, l, E0) ...
-    - otp.pendulumdae.constraintsjacobian(t, state, g, m, l, E0).'*control;
+dstate = otp.pendulumdae.fDifferential(t, state, g, m, l, E0) ...
+    - otp.pendulumdae.invariantsJacobian(t, state, g, m, l, E0).'*control;
 
-c = otp.pendulumdae.constraints(t, state, g, m, l, E0);
+c = otp.pendulumdae.invariants(t, state, g, m, l, E0);
 
 dfull = [dstate; c];
 
diff --git a/src/+otp/+pendulumdae/fdifferential.m b/src/+otp/+pendulumdae/fDifferential.m
similarity index 81%
rename from src/+otp/+pendulumdae/fdifferential.m
rename to src/+otp/+pendulumdae/fDifferential.m
index 8d49a29a..66be78cd 100644
--- a/src/+otp/+pendulumdae/fdifferential.m
+++ b/src/+otp/+pendulumdae/fDifferential.m
@@ -1,4 +1,4 @@
-function dstate = fdifferential(~, state, g, m, ~, ~)
+function dstate = fDifferential(~, state, g, m, ~, ~)
 
 x = state(1, :);
 y = state(2, :);
diff --git a/src/+otp/+pendulumdae/invariants.m b/src/+otp/+pendulumdae/invariants.m
new file mode 100644
index 00000000..afe0dcd2
--- /dev/null
+++ b/src/+otp/+pendulumdae/invariants.m
@@ -0,0 +1,19 @@
+function c = invariants(~, state, g, m, l, E0)
+
+x = state(1, :);
+y = state(2, :);
+u = state(3, :);
+v = state(4, :);
+
+% the fisrt invariant is length preservation
+c1 = x.^2 + y.^2 - l^2;
+
+% the second invariant is the derivative of the above, to convert this into an index-2 DAE
+c2 = 2*(x.*u + y.*v);
+
+% the third invariant is energy preservation, for numerical stability
+c3 = m*(g*(y + l) + 0.5*(u.^2 + v.^2)) - E0;
+
+c = [c1; c2; c3];
+
+end
\ No newline at end of file
diff --git a/src/+otp/+pendulumdae/constraintsjacobian.m b/src/+otp/+pendulumdae/invariantsJacobian.m
similarity index 85%
rename from src/+otp/+pendulumdae/constraintsjacobian.m
rename to src/+otp/+pendulumdae/invariantsJacobian.m
index 2e7961d2..57cc3f71 100644
--- a/src/+otp/+pendulumdae/constraintsjacobian.m
+++ b/src/+otp/+pendulumdae/invariantsJacobian.m
@@ -1,4 +1,4 @@
-function dc = constraintsjacobian(~, state, g, m, ~, ~)
+function dc = invariantsJacobian(~, state, g, m, ~, ~)
 
 x = state(1);
 y = state(2);

From 13cf20d17b999b3f45784d6ec5a988bb6317b73f Mon Sep 17 00:00:00 2001
From: Andrey A Popov <apopov@vt.edu>
Date: Thu, 12 May 2022 13:55:41 -0400
Subject: [PATCH 08/32] Added a bunch of derivatives

---
 src/+otp/+pendulumdae/PendulumDAEProblem.m    | 16 ++++++++--
 src/+otp/+pendulumdae/f.m                     |  2 +-
 src/+otp/+pendulumdae/fDifferential.m         |  4 +--
 .../invariantsHessianAdjointVectorProduct.m   | 19 ++++++++++++
 .../invariantsJacobianAdjointVectorProduct.m  | 19 ++++++++++++
 .../invariantsJacobianVectorProduct.m         | 20 ++++++++++++
 .../jacobianDifferentialVectorProduct.m       | 31 +++++++++++++++++++
 src/+otp/+pendulumdae/jacobianVectorProduct.m | 17 ++++++++++
 8 files changed, 122 insertions(+), 6 deletions(-)
 create mode 100644 src/+otp/+pendulumdae/invariantsHessianAdjointVectorProduct.m
 create mode 100644 src/+otp/+pendulumdae/invariantsJacobianAdjointVectorProduct.m
 create mode 100644 src/+otp/+pendulumdae/invariantsJacobianVectorProduct.m
 create mode 100644 src/+otp/+pendulumdae/jacobianDifferentialVectorProduct.m
 create mode 100644 src/+otp/+pendulumdae/jacobianVectorProduct.m

diff --git a/src/+otp/+pendulumdae/PendulumDAEProblem.m b/src/+otp/+pendulumdae/PendulumDAEProblem.m
index d3497fd8..3d6fe813 100644
--- a/src/+otp/+pendulumdae/PendulumDAEProblem.m
+++ b/src/+otp/+pendulumdae/PendulumDAEProblem.m
@@ -30,14 +30,24 @@ function onSettingsChanged(obj)
 
             % The right hand size in terms of x, y, x', y', and three control parameters z
             obj.RHS = otp.RHS(@(t, y) otp.pendulumdae.f(t, y, g, m, l, E0), ...
-                'Mass', otp.pendulumdae.mass([], [], g, m, l, E0));
+                'Mass', otp.pendulumdae.mass([], [], g, m, l, E0), ...
+                'JacobianVectorProduct', ...
+                @(t, y, v) otp.pendulumdae.jacobianVectorProduct(t, y, g, m, l, E0, v));
 
             % Generate the constituent RHS for the differential part
-            obj.RHSDifferential = otp.RHS(@(t, y) otp.pendulumdae.fDifferential(t, y, g, m, l, E0));
+            obj.RHSDifferential = otp.RHS(@(t, y) otp.pendulumdae.fDifferential(t, y, g, m, l, E0), ...
+                'JacobianVectorProduct', ...
+                @(t, y, v) otp.pendulumdae.jacobianDifferentialVectorProduct(t, y, g, m, l, E0, v));
 
             % Generate the constituent RHS for the algebraic part
             obj.RHSAlgebraic = otp.RHS(@(t, y) otp.pendulumdae.invariants(t, y, g, m, l, E0), ...
-                'Jacobian', @(t, y) otp.pendulumdae.invariantsJacobian(t, y, g, m, l, E0));
+                'Jacobian', @(t, y) otp.pendulumdae.invariantsJacobian(t, y, g, m, l, E0), ...
+                'JacobianVectorProduct', ...
+                @(t, y, v) otp.pendulumdae.invariantsJacobianVectorProduct(t, y, g, m, l, E0, v), ...
+                'JacobianAdjointVectorProduct', ...
+                @(t, y, v) otp.pendulumdae.invariantsJacobianAdjointVectorProduct(t, y, g, m, l, E0, v), ...
+                'HessianAdjointVectorProduct', ...
+                @(t, y, v, u) otp.pendulumdae.invariantsHessianAdjointVectorProduct(t, y, g, m, l, E0, v, u));
         end
 
         function label = internalIndex2label(~, index)
diff --git a/src/+otp/+pendulumdae/f.m b/src/+otp/+pendulumdae/f.m
index f42d81b9..a970332d 100644
--- a/src/+otp/+pendulumdae/f.m
+++ b/src/+otp/+pendulumdae/f.m
@@ -4,7 +4,7 @@
 control = statepluscontrol(5:end, :);
 
 dstate = otp.pendulumdae.fDifferential(t, state, g, m, l, E0) ...
-    - otp.pendulumdae.invariantsJacobian(t, state, g, m, l, E0).'*control;
+    - otp.pendulumdae.invariantsJacobianAdjointVectorProduct(t, state, g, m, l, E0, control);
 
 c = otp.pendulumdae.invariants(t, state, g, m, l, E0);
 
diff --git a/src/+otp/+pendulumdae/fDifferential.m b/src/+otp/+pendulumdae/fDifferential.m
index 66be78cd..a99ef132 100644
--- a/src/+otp/+pendulumdae/fDifferential.m
+++ b/src/+otp/+pendulumdae/fDifferential.m
@@ -7,12 +7,12 @@
 
 lxy2 = x.^2 + y.^2;
 
-lambda =  (m/lxy2)*(u.^2 + v.^2) - (g/lxy2)*y;
+lambda =  (m./lxy2).*(u.^2 + v.^2) - (g./lxy2).*y;
 
 dx = u;
 dy = v;
 du = -(lambda/m).*x;
-dv = -(lambda/m)*y - g/m;
+dv = -(lambda/m).*y - g/m;
 
 dstate = [dx; dy; du; dv];
 
diff --git a/src/+otp/+pendulumdae/invariantsHessianAdjointVectorProduct.m b/src/+otp/+pendulumdae/invariantsHessianAdjointVectorProduct.m
new file mode 100644
index 00000000..1c6af7f1
--- /dev/null
+++ b/src/+otp/+pendulumdae/invariantsHessianAdjointVectorProduct.m
@@ -0,0 +1,19 @@
+function dvp = invariantsHessianAdjointVectorProduct(~, ~, g, m, ~, ~, control, vec)
+
+z1 = control(1, :);
+z2 = control(2, :);
+z3 = control(3, :);
+
+vec1 = vec(1, :);
+vec2 = vec(2, :);
+vec3 = vec(3, :);
+vec4 = vec(4, :);
+
+dvp1 = 2*(vec1.*z1 + vec3.*z2);
+dvp2 = 2*(vec2.*z1 + vec4.*z2);
+dvp3 = 2*vec1.*z2 + m*vec3.*z3;
+dvp4 = 2*vec2.*z2 + m*vec4.*z3;
+
+dvp = [dvp1; dvp2; dvp3; dvp4];
+
+end
diff --git a/src/+otp/+pendulumdae/invariantsJacobianAdjointVectorProduct.m b/src/+otp/+pendulumdae/invariantsJacobianAdjointVectorProduct.m
new file mode 100644
index 00000000..80ed8e17
--- /dev/null
+++ b/src/+otp/+pendulumdae/invariantsJacobianAdjointVectorProduct.m
@@ -0,0 +1,19 @@
+function dvp = invariantsJacobianAdjointVectorProduct(~, state, g, m, ~, ~, control)
+
+x = state(1, :);
+y = state(2, :);
+u = state(3, :);
+v = state(4, :);
+
+z1 = control(1, :);
+z2 = control(2, :);
+z3 = control(3, :);
+
+dvp1 = 2*(x.*z1 + u.*z2);
+dvp2 = 2*(y.*z1 + v.*z2) + g*m*z3;
+dvp3 = 2*x.*z2 + m*u.*z3;
+dvp4 = 2*y.*z2 + m*v.*z3;
+
+dvp = [dvp1; dvp2; dvp3; dvp4];
+
+end
diff --git a/src/+otp/+pendulumdae/invariantsJacobianVectorProduct.m b/src/+otp/+pendulumdae/invariantsJacobianVectorProduct.m
new file mode 100644
index 00000000..8e20369f
--- /dev/null
+++ b/src/+otp/+pendulumdae/invariantsJacobianVectorProduct.m
@@ -0,0 +1,20 @@
+function dvp = invariantsJacobianVectorProduct(~, state, g, m, ~, ~, vec)
+
+x = state(1, :);
+y = state(2, :);
+u = state(3, :);
+v = state(4, :);
+
+vec1 = vec(1, :);
+vec2 = vec(2, :);
+vec3 = vec(3, :);
+vec4 = vec(4, :);
+
+
+dvp1 = 2*(x.*vec1 + y.*vec2);
+dvp2 = 2*(u.*vec1 + v.*vec2 + x.*vec3 + y.*vec4);
+dvp3 = m*(g*vec2 + u.*vec3 + v.*vec4);
+
+dvp = [dvp1; dvp2; dvp3];
+
+end
diff --git a/src/+otp/+pendulumdae/jacobianDifferentialVectorProduct.m b/src/+otp/+pendulumdae/jacobianDifferentialVectorProduct.m
new file mode 100644
index 00000000..82758956
--- /dev/null
+++ b/src/+otp/+pendulumdae/jacobianDifferentialVectorProduct.m
@@ -0,0 +1,31 @@
+function dstatevp = jacobianDifferentialVectorProduct(~, state, g, m, ~, ~, vec)
+
+x = state(1, :);
+y = state(2, :);
+u = state(3, :);
+v = state(4, :);
+
+vec1 = vec(1, :);
+vec2 = vec(2, :);
+vec3 = vec(3, :);
+vec4 = vec(4, :);
+
+lxy2 = x.^2 + y.^2;
+
+lambda = (m./lxy2).*(u.^2 + v.^2) - (g./lxy2).*y;
+
+dlambdadx = (-2*m*(u.^2 + v.^2).*x + 2*g*x.*y)./(lxy2.^2);
+dlambdady = (-2*m*(u.^2 + v.^2).*y + g*(y.^2 - x.^2))./(lxy2.^2);
+dlambdadu = (2*m./lxy2).*u;
+dlambdadv = (2*m./lxy2).*v;
+
+inner = (vec1.*dlambdadx + vec2.*dlambdady + vec3.*dlambdadu + vec4.*dlambdadv);
+
+dxvp = vec3;
+dyvp = vec4;
+duvp = -(1/m)*(vec1.*lambda + x.*inner);
+dvvp = -(1/m)*(vec2.*lambda + y.*inner);
+
+dstatevp = [dxvp; dyvp; duvp; dvvp];
+
+end
diff --git a/src/+otp/+pendulumdae/jacobianVectorProduct.m b/src/+otp/+pendulumdae/jacobianVectorProduct.m
new file mode 100644
index 00000000..c9479c73
--- /dev/null
+++ b/src/+otp/+pendulumdae/jacobianVectorProduct.m
@@ -0,0 +1,17 @@
+function dfull = jacobianVectorProduct(t, statepluscontrol, g, m, l, E0, v)
+
+state   = statepluscontrol(1:4, :);
+control = statepluscontrol(5:end, :);
+
+vstate = v(1:4, :);
+vcontrol = v(5:end, :);
+
+dstate = otp.pendulumdae.jacobianDifferentialVectorProduct(t, state, g, m, l, E0, vstate) ...
+    - otp.pendulumdae.invariantsHessianAdjointVectorProduct(t, state, g, m, l, E0, control, vstate) ...
+    - otp.pendulumdae.invariantsJacobianAdjointVectorProduct(t, state, g, m, l, E0, vcontrol);
+
+c = otp.pendulumdae.invariantsJacobianVectorProduct(t, state, g, m, l, E0, vstate);
+
+dfull = [dstate; c];
+
+end

From 0bc36d2dd2d6d2b5c72a25409aad04753b3821af Mon Sep 17 00:00:00 2001
From: Andrey A Popov <apopov@vt.edu>
Date: Thu, 12 May 2022 13:56:47 -0400
Subject: [PATCH 09/32] added vectorization

---
 src/+otp/+pendulumdae/PendulumDAEProblem.m | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/src/+otp/+pendulumdae/PendulumDAEProblem.m b/src/+otp/+pendulumdae/PendulumDAEProblem.m
index 3d6fe813..256bd6e9 100644
--- a/src/+otp/+pendulumdae/PendulumDAEProblem.m
+++ b/src/+otp/+pendulumdae/PendulumDAEProblem.m
@@ -32,12 +32,14 @@ function onSettingsChanged(obj)
             obj.RHS = otp.RHS(@(t, y) otp.pendulumdae.f(t, y, g, m, l, E0), ...
                 'Mass', otp.pendulumdae.mass([], [], g, m, l, E0), ...
                 'JacobianVectorProduct', ...
-                @(t, y, v) otp.pendulumdae.jacobianVectorProduct(t, y, g, m, l, E0, v));
+                @(t, y, v) otp.pendulumdae.jacobianVectorProduct(t, y, g, m, l, E0, v), ...
+                'Vectorized', 'on');
 
             % Generate the constituent RHS for the differential part
             obj.RHSDifferential = otp.RHS(@(t, y) otp.pendulumdae.fDifferential(t, y, g, m, l, E0), ...
                 'JacobianVectorProduct', ...
-                @(t, y, v) otp.pendulumdae.jacobianDifferentialVectorProduct(t, y, g, m, l, E0, v));
+                @(t, y, v) otp.pendulumdae.jacobianDifferentialVectorProduct(t, y, g, m, l, E0, v), ...
+                'Vectorized', 'on');
 
             % Generate the constituent RHS for the algebraic part
             obj.RHSAlgebraic = otp.RHS(@(t, y) otp.pendulumdae.invariants(t, y, g, m, l, E0), ...
@@ -47,7 +49,8 @@ function onSettingsChanged(obj)
                 'JacobianAdjointVectorProduct', ...
                 @(t, y, v) otp.pendulumdae.invariantsJacobianAdjointVectorProduct(t, y, g, m, l, E0, v), ...
                 'HessianAdjointVectorProduct', ...
-                @(t, y, v, u) otp.pendulumdae.invariantsHessianAdjointVectorProduct(t, y, g, m, l, E0, v, u));
+                @(t, y, v, u) otp.pendulumdae.invariantsHessianAdjointVectorProduct(t, y, g, m, l, E0, v, u), ...
+                'Vectorized', 'on');
         end
 
         function label = internalIndex2label(~, index)

From 42d822fab65fbf2ed747444540b3ac2f0b962194 Mon Sep 17 00:00:00 2001
From: Andrey A Popov <apopov@vt.edu>
Date: Thu, 12 May 2022 17:42:03 -0400
Subject: [PATCH 10/32] name changes to make everything consistent

---
 src/+otp/+pendulumdae/PendulumDAEProblem.m       | 16 ++++++++--------
 src/+otp/+pendulumdae/f.m                        |  4 ++--
 .../+pendulumdae/{invariants.m => fAlgebraic.m}  |  2 +-
 ....m => hessianAdjointVectorProductAlgebraic.m} |  2 +-
 ...m => jacobianAdjointVectorProductAlgebraic.m} |  2 +-
 ...{invariantsJacobian.m => jacobianAlgebraic.m} |  2 +-
 src/+otp/+pendulumdae/jacobianVectorProduct.m    |  8 ++++----
 ...roduct.m => jacobianVectorProductAlgebraic.m} |  2 +-
 ...uct.m => jacobianVectorProductDifferential.m} |  2 +-
 9 files changed, 20 insertions(+), 20 deletions(-)
 rename src/+otp/+pendulumdae/{invariants.m => fAlgebraic.m} (89%)
 rename src/+otp/+pendulumdae/{invariantsHessianAdjointVectorProduct.m => hessianAdjointVectorProductAlgebraic.m} (77%)
 rename src/+otp/+pendulumdae/{invariantsJacobianAdjointVectorProduct.m => jacobianAdjointVectorProductAlgebraic.m} (76%)
 rename src/+otp/+pendulumdae/{invariantsJacobian.m => jacobianAlgebraic.m} (85%)
 rename src/+otp/+pendulumdae/{invariantsJacobianVectorProduct.m => jacobianVectorProductAlgebraic.m} (79%)
 rename src/+otp/+pendulumdae/{jacobianDifferentialVectorProduct.m => jacobianVectorProductDifferential.m} (90%)

diff --git a/src/+otp/+pendulumdae/PendulumDAEProblem.m b/src/+otp/+pendulumdae/PendulumDAEProblem.m
index 256bd6e9..d9cb18bd 100644
--- a/src/+otp/+pendulumdae/PendulumDAEProblem.m
+++ b/src/+otp/+pendulumdae/PendulumDAEProblem.m
@@ -9,7 +9,7 @@
 
     properties (Dependent)
       Y0Differential
-      Z0Algebraic
+      Y0Algebraic
     end
 
     methods
@@ -38,18 +38,18 @@ function onSettingsChanged(obj)
             % Generate the constituent RHS for the differential part
             obj.RHSDifferential = otp.RHS(@(t, y) otp.pendulumdae.fDifferential(t, y, g, m, l, E0), ...
                 'JacobianVectorProduct', ...
-                @(t, y, v) otp.pendulumdae.jacobianDifferentialVectorProduct(t, y, g, m, l, E0, v), ...
+                @(t, y, v) otp.pendulumdae.jacobianVectorProductDifferential(t, y, g, m, l, E0, v), ...
                 'Vectorized', 'on');
 
             % Generate the constituent RHS for the algebraic part
             obj.RHSAlgebraic = otp.RHS(@(t, y) otp.pendulumdae.invariants(t, y, g, m, l, E0), ...
-                'Jacobian', @(t, y) otp.pendulumdae.invariantsJacobian(t, y, g, m, l, E0), ...
+                'Jacobian', @(t, y) otp.pendulumdae.jacobianAlgebraic(t, y, g, m, l, E0), ...
                 'JacobianVectorProduct', ...
-                @(t, y, v) otp.pendulumdae.invariantsJacobianVectorProduct(t, y, g, m, l, E0, v), ...
+                @(t, y, v) otp.pendulumdae.jacobianVectorProductAlgebraic(t, y, g, m, l, E0, v), ...
                 'JacobianAdjointVectorProduct', ...
-                @(t, y, v) otp.pendulumdae.invariantsJacobianAdjointVectorProduct(t, y, g, m, l, E0, v), ...
+                @(t, y, v) otp.pendulumdae.jacobianAdjointVectorProductAlgebraic(t, y, g, m, l, E0, v), ...
                 'HessianAdjointVectorProduct', ...
-                @(t, y, v, u) otp.pendulumdae.invariantsHessianAdjointVectorProduct(t, y, g, m, l, E0, v, u), ...
+                @(t, y, v, u) otp.pendulumdae.hessianAdjointVectorProductAlgebraic(t, y, g, m, l, E0, v, u), ...
                 'Vectorized', 'on');
         end
 
@@ -78,8 +78,8 @@ function onSettingsChanged(obj)
             y0differential = obj.Y0(1:4);
         end
 
-        function z0algebraic = get.Z0Algebraic(obj)
-            z0algebraic = obj.Y0(5:end);
+        function y0algebraic = get.Y0Algebraic(obj)
+            y0algebraic = obj.Y0(5:end);
         end
     end
 
diff --git a/src/+otp/+pendulumdae/f.m b/src/+otp/+pendulumdae/f.m
index a970332d..92f452f3 100644
--- a/src/+otp/+pendulumdae/f.m
+++ b/src/+otp/+pendulumdae/f.m
@@ -4,9 +4,9 @@
 control = statepluscontrol(5:end, :);
 
 dstate = otp.pendulumdae.fDifferential(t, state, g, m, l, E0) ...
-    - otp.pendulumdae.invariantsJacobianAdjointVectorProduct(t, state, g, m, l, E0, control);
+    - otp.pendulumdae.jacobianAdjointVectorProductAlgebraic(t, state, g, m, l, E0, control);
 
-c = otp.pendulumdae.invariants(t, state, g, m, l, E0);
+c = otp.pendulumdae.fAlgebraic(t, state, g, m, l, E0);
 
 dfull = [dstate; c];
 
diff --git a/src/+otp/+pendulumdae/invariants.m b/src/+otp/+pendulumdae/fAlgebraic.m
similarity index 89%
rename from src/+otp/+pendulumdae/invariants.m
rename to src/+otp/+pendulumdae/fAlgebraic.m
index afe0dcd2..b306ad87 100644
--- a/src/+otp/+pendulumdae/invariants.m
+++ b/src/+otp/+pendulumdae/fAlgebraic.m
@@ -1,4 +1,4 @@
-function c = invariants(~, state, g, m, l, E0)
+function c = fAlgebraic(~, state, g, m, l, E0)
 
 x = state(1, :);
 y = state(2, :);
diff --git a/src/+otp/+pendulumdae/invariantsHessianAdjointVectorProduct.m b/src/+otp/+pendulumdae/hessianAdjointVectorProductAlgebraic.m
similarity index 77%
rename from src/+otp/+pendulumdae/invariantsHessianAdjointVectorProduct.m
rename to src/+otp/+pendulumdae/hessianAdjointVectorProductAlgebraic.m
index 1c6af7f1..8d9de4d7 100644
--- a/src/+otp/+pendulumdae/invariantsHessianAdjointVectorProduct.m
+++ b/src/+otp/+pendulumdae/hessianAdjointVectorProductAlgebraic.m
@@ -1,4 +1,4 @@
-function dvp = invariantsHessianAdjointVectorProduct(~, ~, g, m, ~, ~, control, vec)
+function dvp = hessianAdjointVectorProductAlgebraic(~, ~, g, m, ~, ~, control, vec)
 
 z1 = control(1, :);
 z2 = control(2, :);
diff --git a/src/+otp/+pendulumdae/invariantsJacobianAdjointVectorProduct.m b/src/+otp/+pendulumdae/jacobianAdjointVectorProductAlgebraic.m
similarity index 76%
rename from src/+otp/+pendulumdae/invariantsJacobianAdjointVectorProduct.m
rename to src/+otp/+pendulumdae/jacobianAdjointVectorProductAlgebraic.m
index 80ed8e17..4b7c7e3b 100644
--- a/src/+otp/+pendulumdae/invariantsJacobianAdjointVectorProduct.m
+++ b/src/+otp/+pendulumdae/jacobianAdjointVectorProductAlgebraic.m
@@ -1,4 +1,4 @@
-function dvp = invariantsJacobianAdjointVectorProduct(~, state, g, m, ~, ~, control)
+function dvp = jacobianAdjointVectorProductAlgebraic(~, state, g, m, ~, ~, control)
 
 x = state(1, :);
 y = state(2, :);
diff --git a/src/+otp/+pendulumdae/invariantsJacobian.m b/src/+otp/+pendulumdae/jacobianAlgebraic.m
similarity index 85%
rename from src/+otp/+pendulumdae/invariantsJacobian.m
rename to src/+otp/+pendulumdae/jacobianAlgebraic.m
index 57cc3f71..bc9b49b2 100644
--- a/src/+otp/+pendulumdae/invariantsJacobian.m
+++ b/src/+otp/+pendulumdae/jacobianAlgebraic.m
@@ -1,4 +1,4 @@
-function dc = invariantsJacobian(~, state, g, m, ~, ~)
+function dc = jacobianAlgebraic(~, state, g, m, ~, ~)
 
 x = state(1);
 y = state(2);
diff --git a/src/+otp/+pendulumdae/jacobianVectorProduct.m b/src/+otp/+pendulumdae/jacobianVectorProduct.m
index c9479c73..3bfcba40 100644
--- a/src/+otp/+pendulumdae/jacobianVectorProduct.m
+++ b/src/+otp/+pendulumdae/jacobianVectorProduct.m
@@ -6,11 +6,11 @@
 vstate = v(1:4, :);
 vcontrol = v(5:end, :);
 
-dstate = otp.pendulumdae.jacobianDifferentialVectorProduct(t, state, g, m, l, E0, vstate) ...
-    - otp.pendulumdae.invariantsHessianAdjointVectorProduct(t, state, g, m, l, E0, control, vstate) ...
-    - otp.pendulumdae.invariantsJacobianAdjointVectorProduct(t, state, g, m, l, E0, vcontrol);
+dstate = otp.pendulumdae.jacobianVectorProductDifferential(t, state, g, m, l, E0, vstate) ...
+    - otp.pendulumdae.hessianAdjointVectorProductAlgebraic(t, state, g, m, l, E0, control, vstate) ...
+    - otp.pendulumdae.jacobianAdjointVectorProductAlgebraic(t, state, g, m, l, E0, vcontrol);
 
-c = otp.pendulumdae.invariantsJacobianVectorProduct(t, state, g, m, l, E0, vstate);
+c = otp.pendulumdae.jacobianVectorProductAlgebraic(t, state, g, m, l, E0, vstate);
 
 dfull = [dstate; c];
 
diff --git a/src/+otp/+pendulumdae/invariantsJacobianVectorProduct.m b/src/+otp/+pendulumdae/jacobianVectorProductAlgebraic.m
similarity index 79%
rename from src/+otp/+pendulumdae/invariantsJacobianVectorProduct.m
rename to src/+otp/+pendulumdae/jacobianVectorProductAlgebraic.m
index 8e20369f..bc622072 100644
--- a/src/+otp/+pendulumdae/invariantsJacobianVectorProduct.m
+++ b/src/+otp/+pendulumdae/jacobianVectorProductAlgebraic.m
@@ -1,4 +1,4 @@
-function dvp = invariantsJacobianVectorProduct(~, state, g, m, ~, ~, vec)
+function dvp = jacobianVectorProductAlgebraic(~, state, g, m, ~, ~, vec)
 
 x = state(1, :);
 y = state(2, :);
diff --git a/src/+otp/+pendulumdae/jacobianDifferentialVectorProduct.m b/src/+otp/+pendulumdae/jacobianVectorProductDifferential.m
similarity index 90%
rename from src/+otp/+pendulumdae/jacobianDifferentialVectorProduct.m
rename to src/+otp/+pendulumdae/jacobianVectorProductDifferential.m
index 82758956..db55c02e 100644
--- a/src/+otp/+pendulumdae/jacobianDifferentialVectorProduct.m
+++ b/src/+otp/+pendulumdae/jacobianVectorProductDifferential.m
@@ -1,4 +1,4 @@
-function dstatevp = jacobianDifferentialVectorProduct(~, state, g, m, ~, ~, vec)
+function dstatevp = jacobianVectorProductDifferential(~, state, g, m, ~, ~, vec)
 
 x = state(1, :);
 y = state(2, :);

From 9b2ee981b474c0d3f9140f4edc487c8985305f0a Mon Sep 17 00:00:00 2001
From: Andrey A Popov <apopov@vt.edu>
Date: Thu, 12 May 2022 17:43:55 -0400
Subject: [PATCH 11/32] one more name fix

---
 src/+otp/+pendulumdae/PendulumDAEProblem.m | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/+otp/+pendulumdae/PendulumDAEProblem.m b/src/+otp/+pendulumdae/PendulumDAEProblem.m
index d9cb18bd..4b060a17 100644
--- a/src/+otp/+pendulumdae/PendulumDAEProblem.m
+++ b/src/+otp/+pendulumdae/PendulumDAEProblem.m
@@ -42,7 +42,7 @@ function onSettingsChanged(obj)
                 'Vectorized', 'on');
 
             % Generate the constituent RHS for the algebraic part
-            obj.RHSAlgebraic = otp.RHS(@(t, y) otp.pendulumdae.invariants(t, y, g, m, l, E0), ...
+            obj.RHSAlgebraic = otp.RHS(@(t, y) otp.pendulumdae.fAlgebraic(t, y, g, m, l, E0), ...
                 'Jacobian', @(t, y) otp.pendulumdae.jacobianAlgebraic(t, y, g, m, l, E0), ...
                 'JacobianVectorProduct', ...
                 @(t, y, v) otp.pendulumdae.jacobianVectorProductAlgebraic(t, y, g, m, l, E0, v), ...

From a114be868b751feb17fe62bfb9d05c689ffa142c Mon Sep 17 00:00:00 2001
From: Andrey A Popov <apopov@vt.edu>
Date: Thu, 12 May 2022 18:51:17 -0400
Subject: [PATCH 12/32] fixes to names and argument order

---
 src/+otp/+pendulumdae/PendulumDAEProblem.m           | 12 ++++++------
 src/+otp/+pendulumdae/f.m                            |  2 +-
 .../hessianAdjointVectorProductAlgebraic.m           |  2 +-
 .../jacobianAdjointVectorProductAlgebraic.m          |  2 +-
 src/+otp/+pendulumdae/jacobianVectorProduct.m        | 10 +++++-----
 .../+pendulumdae/jacobianVectorProductAlgebraic.m    |  2 +-
 .../+pendulumdae/jacobianVectorProductDifferential.m |  2 +-
 7 files changed, 16 insertions(+), 16 deletions(-)

diff --git a/src/+otp/+pendulumdae/PendulumDAEProblem.m b/src/+otp/+pendulumdae/PendulumDAEProblem.m
index 4b060a17..08636416 100644
--- a/src/+otp/+pendulumdae/PendulumDAEProblem.m
+++ b/src/+otp/+pendulumdae/PendulumDAEProblem.m
@@ -25,31 +25,31 @@ function onSettingsChanged(obj)
             g = obj.Parameters.Gravity;
 
             % get initial energy
-            initialconstraints = otp.pendulumdae.invariants([], obj.Y0Differential, g, m, l, 0);
+            initialconstraints = otp.pendulumdae.fAlgebraic([], obj.Y0Differential, g, m, l, 0);
             E0 = initialconstraints(3);
 
             % The right hand size in terms of x, y, x', y', and three control parameters z
             obj.RHS = otp.RHS(@(t, y) otp.pendulumdae.f(t, y, g, m, l, E0), ...
                 'Mass', otp.pendulumdae.mass([], [], g, m, l, E0), ...
                 'JacobianVectorProduct', ...
-                @(t, y, v) otp.pendulumdae.jacobianVectorProduct(t, y, g, m, l, E0, v), ...
+                @(t, y, v) otp.pendulumdae.jacobianVectorProduct(t, y, v, g, m, l, E0), ...
                 'Vectorized', 'on');
 
             % Generate the constituent RHS for the differential part
             obj.RHSDifferential = otp.RHS(@(t, y) otp.pendulumdae.fDifferential(t, y, g, m, l, E0), ...
                 'JacobianVectorProduct', ...
-                @(t, y, v) otp.pendulumdae.jacobianVectorProductDifferential(t, y, g, m, l, E0, v), ...
+                @(t, y, v) otp.pendulumdae.jacobianVectorProductDifferential(t, y, v, g, m, l, E0), ...
                 'Vectorized', 'on');
 
             % Generate the constituent RHS for the algebraic part
             obj.RHSAlgebraic = otp.RHS(@(t, y) otp.pendulumdae.fAlgebraic(t, y, g, m, l, E0), ...
                 'Jacobian', @(t, y) otp.pendulumdae.jacobianAlgebraic(t, y, g, m, l, E0), ...
                 'JacobianVectorProduct', ...
-                @(t, y, v) otp.pendulumdae.jacobianVectorProductAlgebraic(t, y, g, m, l, E0, v), ...
+                @(t, y, v) otp.pendulumdae.jacobianVectorProductAlgebraic(t, y, v, g, m, l, E0), ...
                 'JacobianAdjointVectorProduct', ...
-                @(t, y, v) otp.pendulumdae.jacobianAdjointVectorProductAlgebraic(t, y, g, m, l, E0, v), ...
+                @(t, y, v) otp.pendulumdae.jacobianAdjointVectorProductAlgebraic(t, y, v, g, m, l, E0, v), ...
                 'HessianAdjointVectorProduct', ...
-                @(t, y, v, u) otp.pendulumdae.hessianAdjointVectorProductAlgebraic(t, y, g, m, l, E0, v, u), ...
+                @(t, y, v, u) otp.pendulumdae.hessianAdjointVectorProductAlgebraic(t, y, v, u, g, m, l, E0), ...
                 'Vectorized', 'on');
         end
 
diff --git a/src/+otp/+pendulumdae/f.m b/src/+otp/+pendulumdae/f.m
index 92f452f3..1a85d022 100644
--- a/src/+otp/+pendulumdae/f.m
+++ b/src/+otp/+pendulumdae/f.m
@@ -4,7 +4,7 @@
 control = statepluscontrol(5:end, :);
 
 dstate = otp.pendulumdae.fDifferential(t, state, g, m, l, E0) ...
-    - otp.pendulumdae.jacobianAdjointVectorProductAlgebraic(t, state, g, m, l, E0, control);
+    - otp.pendulumdae.jacobianAdjointVectorProductAlgebraic(t, state, control, g, m, l, E0);
 
 c = otp.pendulumdae.fAlgebraic(t, state, g, m, l, E0);
 
diff --git a/src/+otp/+pendulumdae/hessianAdjointVectorProductAlgebraic.m b/src/+otp/+pendulumdae/hessianAdjointVectorProductAlgebraic.m
index 8d9de4d7..1b5b70bb 100644
--- a/src/+otp/+pendulumdae/hessianAdjointVectorProductAlgebraic.m
+++ b/src/+otp/+pendulumdae/hessianAdjointVectorProductAlgebraic.m
@@ -1,4 +1,4 @@
-function dvp = hessianAdjointVectorProductAlgebraic(~, ~, g, m, ~, ~, control, vec)
+function dvp = hessianAdjointVectorProductAlgebraic(~, ~, control, vec, ~, m, ~, ~)
 
 z1 = control(1, :);
 z2 = control(2, :);
diff --git a/src/+otp/+pendulumdae/jacobianAdjointVectorProductAlgebraic.m b/src/+otp/+pendulumdae/jacobianAdjointVectorProductAlgebraic.m
index 4b7c7e3b..086de231 100644
--- a/src/+otp/+pendulumdae/jacobianAdjointVectorProductAlgebraic.m
+++ b/src/+otp/+pendulumdae/jacobianAdjointVectorProductAlgebraic.m
@@ -1,4 +1,4 @@
-function dvp = jacobianAdjointVectorProductAlgebraic(~, state, g, m, ~, ~, control)
+function dvp = jacobianAdjointVectorProductAlgebraic(~, state, control, g, m, ~, ~)
 
 x = state(1, :);
 y = state(2, :);
diff --git a/src/+otp/+pendulumdae/jacobianVectorProduct.m b/src/+otp/+pendulumdae/jacobianVectorProduct.m
index 3bfcba40..fe141ac5 100644
--- a/src/+otp/+pendulumdae/jacobianVectorProduct.m
+++ b/src/+otp/+pendulumdae/jacobianVectorProduct.m
@@ -1,4 +1,4 @@
-function dfull = jacobianVectorProduct(t, statepluscontrol, g, m, l, E0, v)
+function dfull = jacobianVectorProduct(t, statepluscontrol, v, g, m, l, E0)
 
 state   = statepluscontrol(1:4, :);
 control = statepluscontrol(5:end, :);
@@ -6,11 +6,11 @@
 vstate = v(1:4, :);
 vcontrol = v(5:end, :);
 
-dstate = otp.pendulumdae.jacobianVectorProductDifferential(t, state, g, m, l, E0, vstate) ...
-    - otp.pendulumdae.hessianAdjointVectorProductAlgebraic(t, state, g, m, l, E0, control, vstate) ...
-    - otp.pendulumdae.jacobianAdjointVectorProductAlgebraic(t, state, g, m, l, E0, vcontrol);
+dstate = otp.pendulumdae.jacobianVectorProductDifferential(t, state, vstate, g, m, l, E0) ...
+    - otp.pendulumdae.hessianAdjointVectorProductAlgebraic(t, state, control, vstate, g, m, l, E0) ...
+    - otp.pendulumdae.jacobianAdjointVectorProductAlgebraic(t, state, vcontrol, g, m, l, E0);
 
-c = otp.pendulumdae.jacobianVectorProductAlgebraic(t, state, g, m, l, E0, vstate);
+c = otp.pendulumdae.jacobianVectorProductAlgebraic(t, state, vstate, g, m, l, E0);
 
 dfull = [dstate; c];
 
diff --git a/src/+otp/+pendulumdae/jacobianVectorProductAlgebraic.m b/src/+otp/+pendulumdae/jacobianVectorProductAlgebraic.m
index bc622072..2c303dbe 100644
--- a/src/+otp/+pendulumdae/jacobianVectorProductAlgebraic.m
+++ b/src/+otp/+pendulumdae/jacobianVectorProductAlgebraic.m
@@ -1,4 +1,4 @@
-function dvp = jacobianVectorProductAlgebraic(~, state, g, m, ~, ~, vec)
+function dvp = jacobianVectorProductAlgebraic(~, state, vec, g, m, ~, ~)
 
 x = state(1, :);
 y = state(2, :);
diff --git a/src/+otp/+pendulumdae/jacobianVectorProductDifferential.m b/src/+otp/+pendulumdae/jacobianVectorProductDifferential.m
index db55c02e..9e60d72b 100644
--- a/src/+otp/+pendulumdae/jacobianVectorProductDifferential.m
+++ b/src/+otp/+pendulumdae/jacobianVectorProductDifferential.m
@@ -1,4 +1,4 @@
-function dstatevp = jacobianVectorProductDifferential(~, state, g, m, ~, ~, vec)
+function dstatevp = jacobianVectorProductDifferential(~, state, vec, g, m, ~, ~)
 
 x = state(1, :);
 y = state(2, :);

From c3463f39ec5f32de341f0705e7a8d628bde96d88 Mon Sep 17 00:00:00 2001
From: Andrey A Popov <apopov@vt.edu>
Date: Fri, 13 May 2022 15:34:55 -0400
Subject: [PATCH 13/32] Added Half-Explicit Jacobian and vector products

---
 src/+otp/+pendulumdae/PendulumDAEProblem.m    |  9 +++++++++
 src/+otp/+pendulumdae/jacobianHalfExplicit.m  | 19 +++++++++++++++++++
 .../jacobianVectorProductHalfExplicit.m       | 19 +++++++++++++++++++
 3 files changed, 47 insertions(+)
 create mode 100644 src/+otp/+pendulumdae/jacobianHalfExplicit.m
 create mode 100644 src/+otp/+pendulumdae/jacobianVectorProductHalfExplicit.m

diff --git a/src/+otp/+pendulumdae/PendulumDAEProblem.m b/src/+otp/+pendulumdae/PendulumDAEProblem.m
index 08636416..f3eea454 100644
--- a/src/+otp/+pendulumdae/PendulumDAEProblem.m
+++ b/src/+otp/+pendulumdae/PendulumDAEProblem.m
@@ -5,6 +5,7 @@
     properties (SetAccess=protected)
         RHSDifferential
         RHSAlgebraic
+        RHSHalfExplicit
     end
 
     properties (Dependent)
@@ -51,6 +52,14 @@ function onSettingsChanged(obj)
                 'HessianAdjointVectorProduct', ...
                 @(t, y, v, u) otp.pendulumdae.hessianAdjointVectorProductAlgebraic(t, y, v, u, g, m, l, E0), ...
                 'Vectorized', 'on');
+
+            % Generate an Auxillary RHS for the half-explicit Jacobian
+            obj.RHSHalfExplicit = otp.RHS([], ...
+                'Jacobian', @(t, y) otp.pendulumdae.jacobianHalfExplicit(t, y, g, m, l, E0), ...
+                'JacobianVectorProduct', ...
+                @(t, y, v) otp.pendulumdae.jacobianVectorProductHalfExplicit(t, y, v, g, m, l, E0), ...
+                'Vectorized', 'on');
+
         end
 
         function label = internalIndex2label(~, index)
diff --git a/src/+otp/+pendulumdae/jacobianHalfExplicit.m b/src/+otp/+pendulumdae/jacobianHalfExplicit.m
new file mode 100644
index 00000000..ab9b6980
--- /dev/null
+++ b/src/+otp/+pendulumdae/jacobianHalfExplicit.m
@@ -0,0 +1,19 @@
+function gyfz = jacobianHalfExplicit(~, state, g, m, ~, ~)
+
+x = state(1);
+y = state(2);
+u = state(3);
+v = state(4);
+
+gyfz11 = -4*(x^2 + y^2);
+gyfz12 = -4*(u*x + v*y);
+gyfz13 = -2*g*m*y;
+gyfz22 = -4*(x^2 + y^2 + u^2 + v^2);
+gyfz23 = -2*m*(u*x + v*(g + y));
+gyfz33 = -(m^2)*(g^2 + u^2 + v^2);
+
+gyfz = [gyfz11, gyfz12, gyfz13; ...
+    gyfz12, gyfz22, gyfz23; ...
+    gyfz13, gyfz23, gyfz33];
+
+end
diff --git a/src/+otp/+pendulumdae/jacobianVectorProductHalfExplicit.m b/src/+otp/+pendulumdae/jacobianVectorProductHalfExplicit.m
new file mode 100644
index 00000000..4115a5fa
--- /dev/null
+++ b/src/+otp/+pendulumdae/jacobianVectorProductHalfExplicit.m
@@ -0,0 +1,19 @@
+function gyfzvp = jacobianVectorProductHalfExplicit(~, state, vec, g, m, ~, ~)
+
+x = state(1, :);
+y = state(2, :);
+u = state(3, :);
+v = state(4, :);
+
+vec1 = vec(1, :);
+vec2 = vec(2, :);
+vec3 = vec(3, :);
+
+gyfz1 = -2*(2*u.*vec2.*x + (2*v.*vec2 + g*m*vec3).*y + 2*vec1.*(x.^2 + y.^2));
+gyfz2 = -2*(2*(u.^2).*vec2 + 2.*(v.^2).*vec2 + u.*(2*vec1 + m*vec3).*x + 2.*v.*vec1.*y ...
+    + m.*v.*vec3.*(g + y) + 2*vec2.*(x.^2 + y.^2));
+gyfz3 = -m*((g^2)*m*vec3 + m*(u.^2 + v.^2).*vec3 + 2*vec2.*(u.*x + v.*y) + 2*g*(v.*vec2 + vec1.*y));
+
+gyfzvp = [gyfz1; gyfz2; gyfz3];
+
+end

From 1b997dd5c6825f3fb249afd1aa72b7a4a68360a3 Mon Sep 17 00:00:00 2001
From: Andrey A Popov <apopov@vt.edu>
Date: Sat, 14 May 2022 11:22:13 -0400
Subject: [PATCH 14/32] Added all jacobians

---
 src/+otp/+pendulumdae/PendulumDAEProblem.m   |  2 ++
 src/+otp/+pendulumdae/jacobian.m             | 23 +++++++++++++++++++
 src/+otp/+pendulumdae/jacobianDifferential.m | 24 ++++++++++++++++++++
 3 files changed, 49 insertions(+)
 create mode 100644 src/+otp/+pendulumdae/jacobian.m
 create mode 100644 src/+otp/+pendulumdae/jacobianDifferential.m

diff --git a/src/+otp/+pendulumdae/PendulumDAEProblem.m b/src/+otp/+pendulumdae/PendulumDAEProblem.m
index f3eea454..25a5cf38 100644
--- a/src/+otp/+pendulumdae/PendulumDAEProblem.m
+++ b/src/+otp/+pendulumdae/PendulumDAEProblem.m
@@ -32,12 +32,14 @@ function onSettingsChanged(obj)
             % The right hand size in terms of x, y, x', y', and three control parameters z
             obj.RHS = otp.RHS(@(t, y) otp.pendulumdae.f(t, y, g, m, l, E0), ...
                 'Mass', otp.pendulumdae.mass([], [], g, m, l, E0), ...
+                'Jacobian', @(t, y) otp.pendulumdae.jacobian(t, y, g, m, l, E0), ...
                 'JacobianVectorProduct', ...
                 @(t, y, v) otp.pendulumdae.jacobianVectorProduct(t, y, v, g, m, l, E0), ...
                 'Vectorized', 'on');
 
             % Generate the constituent RHS for the differential part
             obj.RHSDifferential = otp.RHS(@(t, y) otp.pendulumdae.fDifferential(t, y, g, m, l, E0), ...
+                'Jacobian', @(t, y) otp.pendulumdae.jacobianDifferential(t, y, g, m, l, E0), ...
                 'JacobianVectorProduct', ...
                 @(t, y, v) otp.pendulumdae.jacobianVectorProductDifferential(t, y, v, g, m, l, E0), ...
                 'Vectorized', 'on');
diff --git a/src/+otp/+pendulumdae/jacobian.m b/src/+otp/+pendulumdae/jacobian.m
new file mode 100644
index 00000000..b5044ad4
--- /dev/null
+++ b/src/+otp/+pendulumdae/jacobian.m
@@ -0,0 +1,23 @@
+function j = jacobian(t, statepluscontrol, g, m, l, E0)
+
+state   = statepluscontrol(1:4);
+control = statepluscontrol(5:end);
+
+
+dstate = otp.pendulumdae.fDifferential(t, state, g, m, l, E0) ...
+    - otp.pendulumdae.jacobianAdjointVectorProductAlgebraic(t, state, control, g, m, l, E0);
+
+c = otp.pendulumdae.fAlgebraic(t, state, g, m, l, E0);
+
+
+jaccontrol = otp.pendulumdae.jacobianAlgebraic(t, state, g, m, l, E0);
+
+jacstate = otp.pendulumdae.jacobianDifferential(t, state, g, m, l, E0) ...
+    - otp.pendulumdae.hessianAdjointVectorProductAlgebraic(t, state, control, eye(4), g, m, l, E0);
+
+jacstatecontrol = -jaccontrol.';
+
+
+j = [jacstate, jacstatecontrol; jaccontrol, zeros(3, 3)];
+
+end
diff --git a/src/+otp/+pendulumdae/jacobianDifferential.m b/src/+otp/+pendulumdae/jacobianDifferential.m
new file mode 100644
index 00000000..265985f7
--- /dev/null
+++ b/src/+otp/+pendulumdae/jacobianDifferential.m
@@ -0,0 +1,24 @@
+function j = jacobianDifferential(~, state, g, m, ~, ~)
+
+x = state(1, :);
+y = state(2, :);
+u = state(3, :);
+v = state(4, :);
+
+lxy2 = x.^2 + y.^2;
+
+lambda = (m./lxy2).*(u.^2 + v.^2) - (g./lxy2).*y;
+
+dlambdadx = (-2*m*(u.^2 + v.^2).*x + 2*g*x.*y)./(lxy2.^2);
+dlambdady = (-2*m*(u.^2 + v.^2).*y + g*(y.^2 - x.^2))./(lxy2.^2);
+dlambdadu = (2*m./lxy2).*u;
+dlambdadv = (2*m./lxy2).*v;
+
+dx = [0, 0, 1, 0];
+dy = [0, 0, 0, 1];
+du = -(1/m)*[lambda + x.*dlambdadx, x.*dlambdady, x.*dlambdadu, x.*dlambdadv];
+dv = -(1/m)*[y.*dlambdadx, lambda + y.*dlambdady, y.*dlambdadu, y.*dlambdadv];
+
+j = [dx; dy; du; dv];
+
+end

From 3bd98e85b0325ce11490980f2d77faa091cb3775 Mon Sep 17 00:00:00 2001
From: Andrey A Popov <apopov@vt.edu>
Date: Sat, 14 May 2022 12:05:56 -0400
Subject: [PATCH 15/32] fixes to jacobians

---
 src/+otp/+pendulumdae/jacobian.m             | 8 --------
 src/+otp/+pendulumdae/jacobianDifferential.m | 8 ++++----
 2 files changed, 4 insertions(+), 12 deletions(-)

diff --git a/src/+otp/+pendulumdae/jacobian.m b/src/+otp/+pendulumdae/jacobian.m
index b5044ad4..1a42d41f 100644
--- a/src/+otp/+pendulumdae/jacobian.m
+++ b/src/+otp/+pendulumdae/jacobian.m
@@ -3,13 +3,6 @@
 state   = statepluscontrol(1:4);
 control = statepluscontrol(5:end);
 
-
-dstate = otp.pendulumdae.fDifferential(t, state, g, m, l, E0) ...
-    - otp.pendulumdae.jacobianAdjointVectorProductAlgebraic(t, state, control, g, m, l, E0);
-
-c = otp.pendulumdae.fAlgebraic(t, state, g, m, l, E0);
-
-
 jaccontrol = otp.pendulumdae.jacobianAlgebraic(t, state, g, m, l, E0);
 
 jacstate = otp.pendulumdae.jacobianDifferential(t, state, g, m, l, E0) ...
@@ -17,7 +10,6 @@
 
 jacstatecontrol = -jaccontrol.';
 
-
 j = [jacstate, jacstatecontrol; jaccontrol, zeros(3, 3)];
 
 end
diff --git a/src/+otp/+pendulumdae/jacobianDifferential.m b/src/+otp/+pendulumdae/jacobianDifferential.m
index 265985f7..8c99abda 100644
--- a/src/+otp/+pendulumdae/jacobianDifferential.m
+++ b/src/+otp/+pendulumdae/jacobianDifferential.m
@@ -1,9 +1,9 @@
 function j = jacobianDifferential(~, state, g, m, ~, ~)
 
-x = state(1, :);
-y = state(2, :);
-u = state(3, :);
-v = state(4, :);
+x = state(1);
+y = state(2);
+u = state(3);
+v = state(4);
 
 lxy2 = x.^2 + y.^2;
 

From 5fa4f927d4aee36ea22a7f6261cf0ad5b0f2fe00 Mon Sep 17 00:00:00 2001
From: Andrey A Popov <apopov@vt.edu>
Date: Sun, 15 May 2022 07:15:17 -0400
Subject: [PATCH 16/32] Added an initial implementation of a DAE solver

---
 src/+otp/+pendulumdae/PendulumDAEProblem.m |   4 +
 src/+otp/+utils/+solvers/dae34.m           | 153 +++++++++++++++++++++
 2 files changed, 157 insertions(+)
 create mode 100644 src/+otp/+utils/+solvers/dae34.m

diff --git a/src/+otp/+pendulumdae/PendulumDAEProblem.m b/src/+otp/+pendulumdae/PendulumDAEProblem.m
index 25a5cf38..709098ed 100644
--- a/src/+otp/+pendulumdae/PendulumDAEProblem.m
+++ b/src/+otp/+pendulumdae/PendulumDAEProblem.m
@@ -82,6 +82,10 @@ function onSettingsChanged(obj)
                     label = 'Control 3';
             end
         end
+
+        function sol = internalSolve(obj, varargin)
+            sol = internalSolve@otp.Problem(obj, 'Solver', @otp.utils.solvers.dae34, varargin{:});
+        end
     end
 
     methods
diff --git a/src/+otp/+utils/+solvers/dae34.m b/src/+otp/+utils/+solvers/dae34.m
new file mode 100644
index 00000000..0c4572b8
--- /dev/null
+++ b/src/+otp/+utils/+solvers/dae34.m
@@ -0,0 +1,153 @@
+function [sol, y] = dae34(f, tspan, y0, options)
+% See
+%    Cameron, F., Palmroth, M., & Piché, R. (2002). 
+%    Quasi stage order conditions for SDIRK methods. 
+%    Applied numerical mathematics, 42(1-3), 61-75.
+
+
+J = options.Jacobian;
+M = options.Mass;
+abstol = options.AbsTol;
+reltol = options.RelTol;
+
+if isempty(abstol)
+    abstol = 1e-6;
+end
+
+if isempty(reltol)
+    reltol = 1e-3;
+end
+
+if isempty(J)
+    error('OTP:DAEJacobianRequired', 'The default DAE solver requires a Jacobian to be provided.')
+end
+
+if isempty(M)
+    M = speye(numel(y0));
+end
+
+if ~isa(M, 'function_handle')
+    M = @(t, y) M;
+end
+
+gamma = 1/4;
+
+A = zeros(4,4);
+A(2,1) = 1/7;
+A(3,1) = 61/144;
+A(3,2) = -49/144;
+A(4,1) = 0;
+A(4,2) = 0;
+A(4,3) = 3/4;
+
+b = zeros(1, 4);
+b(3) = 3/4;
+b(4) = 1/4;
+
+bhat = zeros(1, 4);
+bhat(1) = -61/600;
+bhat(2) = 49/600;
+bhat(3) = 79/100;
+bhat(4) = 23/100;
+
+orderE = 3;
+
+C = sum(A, 2) + gamma;
+
+t = tspan(1);
+y = y0.';
+
+yc = y0;
+tc = tspan(1);
+h = 1e-3;
+
+stagenum = size(A, 1);
+
+tend = tspan(end);
+
+step = 1;
+
+while tc < tend
+
+    if tc + h > tend
+        h = tend - tc;
+    end
+
+    stages = zeros(numel(yc), stagenum);
+
+    gh = gamma*h;
+
+    for stage = 1:stagenum
+
+        staget = tc + C(stage)*h;
+
+        stagedy = 0;
+        for i = 1:(stage - 1)
+            stagedy = stagedy + A(stagenum, i)*h*stages(:, i);
+        end
+
+        newtonk0 = zeros(size(yc));
+
+        np = inf;
+
+        ntol = 1e-6;
+        nmaxits = 5;
+        its = 0;
+        while norm(np) > ntol || its < nmaxits
+            Mc = M(staget, yc + stagedy + gh*newtonk0);
+            g = Mc*newtonk0 - f(staget, yc + stagedy + gh*newtonk0);
+            H = Mc - gh*J(staget, yc + stagedy + gh*newtonk0);
+            [np, ~] = gmres(H, g);
+
+            newtonk0 = newtonk0 - np;
+            its = its + 1;
+        end
+
+        stages(:, stage) = newtonk0;
+
+    end
+
+    yhat = yc;
+    for bi = 1:numel(b)
+        yc = yc + h*b(bi)*stages(:, bi);
+        yhat = yhat + h*bhat(bi)*stages(:, bi);
+    end
+
+    sc = abstol + max(abs(yc), abs(yhat))*reltol;
+
+    err = rms((M(tc + h, yc)*(yc-yhat))./sc);
+
+    fac = 0.38^(1/(orderE + 1));
+
+    facmin = 0.1;
+    if err > 1 || isnan(err)
+        % Reject timestep
+        facmax = 1;
+    else
+        % Accept time step
+        tc = tc + h;
+
+        if tc + h > tend
+            h = tend - tc;
+        end
+
+        t(step + 1) = tc;
+        y(step + 1, :) = yc.';
+
+        step = step + 1;
+
+        facmax = 2.5;
+    end
+
+    % adjust step-size
+    h = h*min(facmax, max(facmin, fac*(1/err)^1/(orderE + 1)));
+
+end
+
+if nargout < 2
+    sol = struct('x', t, 'y', y.');
+else
+    sol = t;
+end
+
+end

From 90bc7afd4b47bce2e74c03971bf17018215b6037 Mon Sep 17 00:00:00 2001
From: Andrey A Popov <apopov@vt.edu>
Date: Sun, 15 May 2022 08:32:55 -0400
Subject: [PATCH 17/32] fixes to DAE method

---
 src/+otp/+utils/+solvers/dae34.m | 63 +++++++++++++++++---------------
 1 file changed, 33 insertions(+), 30 deletions(-)

diff --git a/src/+otp/+utils/+solvers/dae34.m b/src/+otp/+utils/+solvers/dae34.m
index 0c4572b8..c7566b96 100644
--- a/src/+otp/+utils/+solvers/dae34.m
+++ b/src/+otp/+utils/+solvers/dae34.m
@@ -4,27 +4,14 @@
 %    Quasi stage order conditions for SDIRK methods. 
 %    Applied numerical mathematics, 42(1-3), 61-75.
 
+f1 = f(tspan(1), y0);
+hdefault = norm(y0)/norm(f1)*0.01;
 
-J = options.Jacobian;
-M = options.Mass;
-abstol = options.AbsTol;
-reltol = options.RelTol;
-
-if isempty(abstol)
-    abstol = 1e-6;
-end
-
-if isempty(reltol)
-    reltol = 1e-3;
-end
-
-if isempty(J)
-    error('OTP:DAEJacobianRequired', 'The default DAE solver requires a Jacobian to be provided.')
-end
-
-if isempty(M)
-    M = speye(numel(y0));
-end
+h = odeget(options, 'InitialStep', hdefault);
+reltol = odeget(options, 'RelTol', 1e-3);
+abstol = odeget(options, 'AbsTol', 1e-6);
+J = odeget(options, 'Jacobian', @(t, y) jacapprox(f, t, y));
+M = odeget(options, 'Mass', speye(numel(y0)));
 
 if ~isa(M, 'function_handle')
     M = @(t, y) M;
@@ -59,7 +46,6 @@
 
 yc = y0;
 tc = tspan(1);
-h = 1e-3;
 
 stagenum = size(A, 1);
 
@@ -81,9 +67,10 @@
 
         staget = tc + C(stage)*h;
 
-        stagedy = 0;
-        for i = 1:(stage - 1)
-            stagedy = stagedy + A(stagenum, i)*h*stages(:, i);
+        if stage > 1
+            stagedy = h*(stages(:, 1:(stage - 1))*A(stage, 1:(stage - 1)).');
+        else
+            stagedy = 0;
         end
 
         newtonk0 = zeros(size(yc));
@@ -107,15 +94,14 @@
 
     end
 
-    yhat = yc;
-    for bi = 1:numel(b)
-        yc = yc + h*b(bi)*stages(:, bi);
-        yhat = yhat + h*bhat(bi)*stages(:, bi);
-    end
+    yhat = yc + h*stages*bhat.';
+    yc = yc + h*stages*b.';
 
     sc = abstol + max(abs(yc), abs(yhat))*reltol;
 
-    err = rms((M(tc + h, yc)*(yc-yhat))./sc);
+    Mc = M(tc + h, yc);
+
+    err = rms((Mc*(yc-yhat))./sc);
 
     fac = 0.38^(1/(orderE + 1));
 
@@ -151,3 +137,20 @@
 end
 
 end
+
+function J = jacapprox(f, t, y)
+
+n = numel(y);
+
+J = zeros(n, n);
+
+h = sqrt(1e-6);
+
+f0 = f(t, y);
+
+for i = 1:n
+    e = zeros(n, 1); e(i) = 1;
+    J(:, i) = (f(t, y + h*e) - f0)/h;
+end
+
+end

From c51c4782d6da32e442d9ccaa7e6ced04eb75ede1 Mon Sep 17 00:00:00 2001
From: Andrey A Popov <apopov@vt.edu>
Date: Sun, 15 May 2022 20:03:01 -0400
Subject: [PATCH 18/32] Some tests.

---
 src/+otp/+pendulumdae/PendulumDAEProblem.m    |   2 +-
 src/+otp/+utils/+solvers/{dae34.m => dae24.m} |  23 ++-
 src/+otp/+utils/+solvers/dae43.m              | 156 ++++++++++++++++++
 3 files changed, 168 insertions(+), 13 deletions(-)
 rename src/+otp/+utils/+solvers/{dae34.m => dae24.m} (87%)
 create mode 100644 src/+otp/+utils/+solvers/dae43.m

diff --git a/src/+otp/+pendulumdae/PendulumDAEProblem.m b/src/+otp/+pendulumdae/PendulumDAEProblem.m
index 709098ed..f4e35382 100644
--- a/src/+otp/+pendulumdae/PendulumDAEProblem.m
+++ b/src/+otp/+pendulumdae/PendulumDAEProblem.m
@@ -84,7 +84,7 @@ function onSettingsChanged(obj)
         end
 
         function sol = internalSolve(obj, varargin)
-            sol = internalSolve@otp.Problem(obj, 'Solver', @otp.utils.solvers.dae34, varargin{:});
+            sol = internalSolve@otp.Problem(obj, 'Solver', @otp.utils.solvers.dae43, varargin{:});
         end
     end
 
diff --git a/src/+otp/+utils/+solvers/dae34.m b/src/+otp/+utils/+solvers/dae24.m
similarity index 87%
rename from src/+otp/+utils/+solvers/dae34.m
rename to src/+otp/+utils/+solvers/dae24.m
index c7566b96..bf8fab9f 100644
--- a/src/+otp/+utils/+solvers/dae34.m
+++ b/src/+otp/+utils/+solvers/dae24.m
@@ -1,4 +1,4 @@
-function [sol, y] = dae34(f, tspan, y0, options)
+function [sol, y] = dae24(f, tspan, y0, options)
 % See
 %    Cameron, F., Palmroth, M., & Piché, R. (2002). 
 %    Quasi stage order conditions for SDIRK methods. 
@@ -37,7 +37,7 @@
 bhat(3) = 79/100;
 bhat(4) = 23/100;
 
-orderE = 3;
+orderE = 1;
 
 C = sum(A, 2) + gamma;
 
@@ -78,9 +78,9 @@
         np = inf;
 
         ntol = 1e-6;
-        nmaxits = 5;
+        nmaxits = 10;
         its = 0;
-        while norm(np) > ntol || its < nmaxits
+        while norm(np) > ntol && its < nmaxits
             Mc = M(staget, yc + stagedy + gh*newtonk0);
             g = Mc*newtonk0 - f(staget, yc + stagedy + gh*newtonk0);
             H = Mc - gh*J(staget, yc + stagedy + gh*newtonk0);
@@ -95,13 +95,14 @@
     end
 
     yhat = yc + h*stages*bhat.';
-    yc = yc + h*stages*b.';
+    ycnew = yc + h*stages*b.';
 
-    sc = abstol + max(abs(yc), abs(yhat))*reltol;
+    sc = abstol + max(abs(ycnew), abs(yhat))*reltol;
 
-    Mc = M(tc + h, yc);
+    Mc = M(tc + h, ycnew);
 
-    err = rms((Mc*(yc-yhat))./sc);
+    err = rms((Mc*(ycnew-yhat))./sc);
+    %err = rms(((ycnew-yhat))./sc);
 
     fac = 0.38^(1/(orderE + 1));
 
@@ -112,14 +113,12 @@
     else
         % Accept time step
         tc = tc + h;
-
-        if tc + h > tend
-            h = tend - tc;
-        end
+        yc = ycnew;
 
         t(step + 1) = tc;
         y(step + 1, :) = yc.';
 
+
         step = step + 1;
 
         facmax = 2.5;
diff --git a/src/+otp/+utils/+solvers/dae43.m b/src/+otp/+utils/+solvers/dae43.m
new file mode 100644
index 00000000..d2fc1a2d
--- /dev/null
+++ b/src/+otp/+utils/+solvers/dae43.m
@@ -0,0 +1,156 @@
+function [sol, y] = dae43(f, tspan, y0, options)
+
+f1 = f(tspan(1), y0);
+hdefault = norm(y0)/norm(f1)*0.01;
+
+n = numel(y0);
+
+h = odeget(options, 'InitialStep', hdefault);
+reltol = odeget(options, 'RelTol', 1e-3);
+abstol = odeget(options, 'AbsTol', 1e-6);
+J = odeget(options, 'Jacobian', @(t, y) jacapprox(f, t, y));
+M = odeget(options, 'Mass', speye(numel(y0)));
+MStateDependence = odeget(options, 'MStateDependence', 'none');
+
+% We won't support state-dependent Mass, simple as that
+if strcmp(MStateDependence, 'strong')
+    error('OTP:MassStateDependent', 'State dependent mass is not supported.')
+end
+
+if ~isa(M, 'function_handle')
+    M = @(t) M;
+else
+    if nargin(M) > 1
+        M = @(t) M(t, y0);
+    end
+end
+
+% Lobatto IIIC
+A = [1/6, -1/3, 1/6; 1/6, 5/12, -1/12; 1/6, 2/3, 1/6];
+b = [1/6, 2/3, 1/6];
+c = [0, 1/2, 1];
+bhat = [-1/2, 2, -1/2];
+
+orderE = 3;
+
+t = tspan(1);
+y = y0.';
+
+yc = y0;
+tc = tspan(1);
+
+stagenum = size(A, 1);
+tend = tspan(end);
+
+step = 1;
+
+Mc = M(tspan(1));
+Mfull = zeros(n*stagenum, n*stagenum, 'like', Mc);
+gfull = zeros(n*stagenum, 1);
+Jfull = zeros(n*stagenum, n*stagenum, 'like', Mc);
+newtonk = zeros(n*stagenum, 1);
+
+while tc < tend
+
+    if tc + h > tend
+        h = tend - tc;
+    end
+
+    % build M
+    for stage = 1:stagenum
+        %Mfull(((stage - 1)*n + 1):(stage*n), :) = kron(ones(1, stagenum), M(tc + h*c(stage)));
+        si = ((stage - 1)*n + 1):(stage*n);
+        Mfull(si, si) = M(tc + h*c(stage));
+    end
+
+    np = inf;
+
+    ntol = 1e-10;
+    nmaxits = 100;
+    its = 0;
+
+    newtonk = 0*newtonk;
+
+    while norm(np) > ntol && its < nmaxits
+        % build g and Jacobians
+        for stage = 1:stagenum
+            si = ((stage - 1)*n + 1):(stage*n);
+            staget = tc + c(stage)*h;
+            ycs = yc + h*reshape(newtonk, n, [])*(A(stage, :).');
+       
+            Mc = Mfull(si, si);
+            gfull(si) =  Mc*newtonk(si) - f(staget, ycs);
+            Jc = J(staget, ycs);
+
+            Jfull(si, :) = kron(A(stage, :), Jc);
+        end
+
+        H = Mfull - h*Jfull;
+
+        np = pinv(H)*gfull;
+
+        newtonk = newtonk - np;
+        its = its + 1;
+    end
+
+    yhat = yc + h*reshape(newtonk, n, [])*bhat.';
+    ycnew = yc + h*reshape(newtonk, n, [])*b.';
+
+    sc = abstol + max(abs(ycnew), abs(yhat))*reltol;
+
+    Mc = M(tc + h);
+
+    err = rms((Mc*(ycnew-yhat))./sc);
+
+    %err = rms(((ycnew-yhat))./sc);
+
+    fac = 0.38^(1/(orderE + 1));
+
+    facmin = 0.1;
+    if err > 1 || isnan(err)
+        % Reject timestep
+        facmax = 1;
+    else
+        % Accept time step
+        tc = tc + h;
+
+        yc = ycnew;
+
+        t(step + 1) = tc;
+        y(step + 1, :) = yc.';
+
+
+        step = step + 1;
+
+        facmax = 4;
+    end
+
+    % adjust step-size
+    h = h*min(facmax, max(facmin, fac*(1/err)^1/(orderE + 1)));
+
+end
+
+if nargout < 2
+    sol = struct('x', t, 'y', y.');
+else
+    sol = t;
+end
+
+end
+
+function J = jacapprox(f, t, y)
+
+n = numel(y);
+
+J = zeros(n, n);
+
+h = sqrt(1e-6);
+
+f0 = f(t, y);
+
+for i = 1:n
+    e = zeros(n, 1); e(i) = 1;
+    J(:, i) = (f(t, y + h*e) - f0)/h;
+end
+
+end

From 46a4436a2f1c48470dd884bd3284b90fabc0912f Mon Sep 17 00:00:00 2001
From: Andrey A Popov <apopov@vt.edu>
Date: Sun, 15 May 2022 21:27:38 -0400
Subject: [PATCH 19/32] moved to symmlq

---
 src/+otp/+utils/+solvers/dae24.m | 2 +-
 src/+otp/+utils/+solvers/dae43.m | 6 ++----
 2 files changed, 3 insertions(+), 5 deletions(-)

diff --git a/src/+otp/+utils/+solvers/dae24.m b/src/+otp/+utils/+solvers/dae24.m
index bf8fab9f..e70b2366 100644
--- a/src/+otp/+utils/+solvers/dae24.m
+++ b/src/+otp/+utils/+solvers/dae24.m
@@ -84,7 +84,7 @@
             Mc = M(staget, yc + stagedy + gh*newtonk0);
             g = Mc*newtonk0 - f(staget, yc + stagedy + gh*newtonk0);
             H = Mc - gh*J(staget, yc + stagedy + gh*newtonk0);
-            [np, ~] = gmres(H, g);
+            [np, ~] = symmlq(H, g);
 
             newtonk0 = newtonk0 - np;
             its = its + 1;
diff --git a/src/+otp/+utils/+solvers/dae43.m b/src/+otp/+utils/+solvers/dae43.m
index d2fc1a2d..8ff0d5d8 100644
--- a/src/+otp/+utils/+solvers/dae43.m
+++ b/src/+otp/+utils/+solvers/dae43.m
@@ -87,7 +87,7 @@
 
         H = Mfull - h*Jfull;
 
-        np = pinv(H)*gfull;
+        [np, ~] = symmlq(H, gfull);
 
         newtonk = newtonk - np;
         its = its + 1;
@@ -102,9 +102,7 @@
 
     err = rms((Mc*(ycnew-yhat))./sc);
 
-    %err = rms(((ycnew-yhat))./sc);
-
-    fac = 0.38^(1/(orderE + 1));
+    fac = 0.9;
 
     facmin = 0.1;
     if err > 1 || isnan(err)

From d23d13db3218315fc1dad18c6187ed29363778ff Mon Sep 17 00:00:00 2001
From: Andrey A Popov <apopov@vt.edu>
Date: Mon, 16 May 2022 08:49:50 -0400
Subject: [PATCH 20/32] Moved to LSQR

---
 src/+otp/+pendulumdae/PendulumDAEProblem.m | 2 +-
 src/+otp/+utils/+solvers/dae24.m           | 2 +-
 src/+otp/+utils/+solvers/dae43.m           | 2 +-
 3 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/+otp/+pendulumdae/PendulumDAEProblem.m b/src/+otp/+pendulumdae/PendulumDAEProblem.m
index f4e35382..05bda461 100644
--- a/src/+otp/+pendulumdae/PendulumDAEProblem.m
+++ b/src/+otp/+pendulumdae/PendulumDAEProblem.m
@@ -84,7 +84,7 @@ function onSettingsChanged(obj)
         end
 
         function sol = internalSolve(obj, varargin)
-            sol = internalSolve@otp.Problem(obj, 'Solver', @otp.utils.solvers.dae43, varargin{:});
+            sol = internalSolve@otp.Problem(obj, 'Solver', @otp.utils.solvers.dae24, varargin{:});
         end
     end
 
diff --git a/src/+otp/+utils/+solvers/dae24.m b/src/+otp/+utils/+solvers/dae24.m
index e70b2366..eb39bd75 100644
--- a/src/+otp/+utils/+solvers/dae24.m
+++ b/src/+otp/+utils/+solvers/dae24.m
@@ -84,7 +84,7 @@
             Mc = M(staget, yc + stagedy + gh*newtonk0);
             g = Mc*newtonk0 - f(staget, yc + stagedy + gh*newtonk0);
             H = Mc - gh*J(staget, yc + stagedy + gh*newtonk0);
-            [np, ~] = symmlq(H, g);
+            [np, ~] = lsqr(H, g, [], size(H, 1));
 
             newtonk0 = newtonk0 - np;
             its = its + 1;
diff --git a/src/+otp/+utils/+solvers/dae43.m b/src/+otp/+utils/+solvers/dae43.m
index 8ff0d5d8..35873fd8 100644
--- a/src/+otp/+utils/+solvers/dae43.m
+++ b/src/+otp/+utils/+solvers/dae43.m
@@ -87,7 +87,7 @@
 
         H = Mfull - h*Jfull;
 
-        [np, ~] = symmlq(H, gfull);
+        [np, ~] = lsqr(H, gfull, [], size(H, 1));
 
         newtonk = newtonk - np;
         its = its + 1;

From 8453493a6d71651d76b680d957222c8dd229b49d Mon Sep 17 00:00:00 2001
From: Andrey A Popov <apopov@vt.edu>
Date: Mon, 16 May 2022 11:00:52 -0400
Subject: [PATCH 21/32] added a bunch of stuff to the SDIRK method

---
 src/+otp/+utils/+solvers/dae24.m | 141 ++++++++++++++++++++++++-------
 src/+otp/+utils/+solvers/dae43.m |  29 +++++--
 2 files changed, 132 insertions(+), 38 deletions(-)

diff --git a/src/+otp/+utils/+solvers/dae24.m b/src/+otp/+utils/+solvers/dae24.m
index eb39bd75..05ecf789 100644
--- a/src/+otp/+utils/+solvers/dae24.m
+++ b/src/+otp/+utils/+solvers/dae24.m
@@ -4,18 +4,6 @@
 %    Quasi stage order conditions for SDIRK methods. 
 %    Applied numerical mathematics, 42(1-3), 61-75.
 
-f1 = f(tspan(1), y0);
-hdefault = norm(y0)/norm(f1)*0.01;
-
-h = odeget(options, 'InitialStep', hdefault);
-reltol = odeget(options, 'RelTol', 1e-3);
-abstol = odeget(options, 'AbsTol', 1e-6);
-J = odeget(options, 'Jacobian', @(t, y) jacapprox(f, t, y));
-M = odeget(options, 'Mass', speye(numel(y0)));
-
-if ~isa(M, 'function_handle')
-    M = @(t, y) M;
-end
 
 gamma = 1/4;
 
@@ -37,9 +25,48 @@
 bhat(3) = 79/100;
 bhat(4) = 23/100;
 
-orderE = 1;
+c = sum(A, 2) + gamma;
+
+orderM = 3;
+orderE = 2;
 
-C = sum(A, 2) + gamma;
+%% Get all relevant options out 
+h = odeget(options, 'InitialStep', []);
+reltol = odeget(options, 'RelTol', 1e-3);
+abstol = odeget(options, 'AbsTol', 1e-6);
+J = odeget(options, 'Jacobian', @(t, y) jacapprox(f, t, y));
+M = odeget(options, 'Mass', speye(numel(y0)));
+MStateDependence = odeget(options, 'MStateDependence', 'none');
+
+% We won't support state-dependent Mass, simple as that
+if strcmp(MStateDependence, 'strong')
+    error('OTP:MassStateDependent', 'State dependent mass is not supported.')
+end
+
+if ~isa(M, 'function_handle')
+    M = @(t) M;
+else
+    if nargin(M) > 1
+        M = @(t) M(t, y0);
+    end
+end
+
+if isempty(h)
+    sc = abstol + reltol*abs(y0);
+    f0 = f(tspan(1), y0);
+    d0 = mean((y0./sc).^2);
+    d1 = mean((f0./sc).^2);
+    h0 = (d0/d1)*0.01;
+
+    y1 = y0 + h0*f0;
+    f1 = f(tspan(1) + h0, y1);
+
+    d2 = mean(((f1 - f0)./sc).^2)/h0;
+
+    h1 = (0.01/max(d1, d2))^(1/orderM);
+
+    h = min(100*h0, h1);
+end
 
 t = tspan(1);
 y = y0.';
@@ -48,13 +75,15 @@
 tc = tspan(1);
 
 stagenum = size(A, 1);
-
 tend = tspan(end);
 
-step = 1;
+newtonk0 = zeros(size(yc));
 
+step = 1;
 while tc < tend
 
+    bnewtonreject = false;
+
     if tc + h > tend
         h = tend - tc;
     end
@@ -65,7 +94,7 @@
 
     for stage = 1:stagenum
 
-        staget = tc + C(stage)*h;
+        staget = tc + c(stage)*h;
 
         if stage > 1
             stagedy = h*(stages(:, 1:(stage - 1))*A(stage, 1:(stage - 1)).');
@@ -73,36 +102,84 @@
             stagedy = 0;
         end
 
-        newtonk0 = zeros(size(yc));
-
-        np = inf;
+        newtonk0 = 0*newtonk0;
 
-        ntol = 1e-6;
-        nmaxits = 10;
+        ntol = min(abstol, reltol);
+        nmaxits = 1e2;
+        alpha = 1;
         its = 0;
-        while norm(np) > ntol && its < nmaxits
-            Mc = M(staget, yc + stagedy + gh*newtonk0);
-            g = Mc*newtonk0 - f(staget, yc + stagedy + gh*newtonk0);
-            H = Mc - gh*J(staget, yc + stagedy + gh*newtonk0);
-            [np, ~] = lsqr(H, g, [], size(H, 1));
 
-            newtonk0 = newtonk0 - np;
+        etak = inf;
+        nnp = inf;
+
+        kappa = 1e-1;
+
+        ng = inf;
+
+
+        Jc = J(staget, yc + stagedy + gh*newtonk0);
+        while kappa*(etak*nnp) >= ntol && its < nmaxits
+            Mc = M(staget);
+            ycs = yc + stagedy + gh*newtonk0;
+            g = Mc*newtonk0 - f(staget, ycs);
+            H = Mc - gh*Jc;
+            [npnew, ~] = qmr(H, g, [], size(H, 1));
+
+            newtonknew = newtonk0 - alpha*npnew;
+
+            sc = abstol + reltol*abs(ycs);
+
+
+            if its > 2
+                ngnew = sqrt(mean((g./sc).^2));
+                % recompute the jacobian
+                if ngnew > 1.25*ng
+                    Jc = J(staget, ycs);
+                end
+                ng = ngnew;
+
+                nnpnew = sqrt(mean((npnew./sc).^2));
+               
+                thetak = nnpnew/nnp;
+
+                if thetak > 1.25 || isnan(thetak)
+                    bnewtonreject = true;
+                    break;
+                end
+
+                etak = thetak/(1 - thetak);
+            end
+
+            newtonk0 = newtonknew;
+
+            np = npnew;
+            nnp = sqrt(mean((np./sc).^2));
+
             its = its + 1;
         end
 
+        if bnewtonreject
+            break;
+        end
+
         stages(:, stage) = newtonk0;
 
     end
 
+    if bnewtonreject
+        h = h/2;
+        continue;
+    end
+
     yhat = yc + h*stages*bhat.';
     ycnew = yc + h*stages*b.';
 
-    sc = abstol + max(abs(ycnew), abs(yhat))*reltol;
+    sc = abstol + max(abs(ycnew), abs(yc))*reltol;
 
-    Mc = M(tc + h, ycnew);
+    Mc = M(tc + h);
 
-    err = rms((Mc*(ycnew-yhat))./sc);
-    %err = rms(((ycnew-yhat))./sc);
+    err = sqrt(mean(((Mc*(ycnew - yhat))./sc).^2));
+    %err = rms(((ycnew - yhat))./sc);
 
     fac = 0.38^(1/(orderE + 1));
 
diff --git a/src/+otp/+utils/+solvers/dae43.m b/src/+otp/+utils/+solvers/dae43.m
index 35873fd8..5401905d 100644
--- a/src/+otp/+utils/+solvers/dae43.m
+++ b/src/+otp/+utils/+solvers/dae43.m
@@ -1,11 +1,8 @@
 function [sol, y] = dae43(f, tspan, y0, options)
 
-f1 = f(tspan(1), y0);
-hdefault = norm(y0)/norm(f1)*0.01;
-
 n = numel(y0);
 
-h = odeget(options, 'InitialStep', hdefault);
+h = odeget(options, 'InitialStep', []);
 reltol = odeget(options, 'RelTol', 1e-3);
 abstol = odeget(options, 'AbsTol', 1e-6);
 J = odeget(options, 'Jacobian', @(t, y) jacapprox(f, t, y));
@@ -25,6 +22,26 @@
     end
 end
 
+
+% Compute 
+if isempty(h)
+    sc = abstol + reltol*abs(y0);
+    f0 = f(tspan(1), y0);
+    d0 = mean((y0./sc).^2);
+    d1 = mean((f0./sc).^2);
+    h0 = (d0/d1)*0.01;
+
+    y1 = y0 + h0*f0;
+    f1 = f(tspan(1) + h0, y1);
+
+    d2 = mean(((f1 - f0)./sc).^2)/h0;
+
+    h1 = (0.01/max(d1, d2))^(1/orderM);
+
+    h = min(100*h0, h1);
+end
+
+
 % Lobatto IIIC
 A = [1/6, -1/3, 1/6; 1/6, 5/12, -1/12; 1/6, 2/3, 1/6];
 b = [1/6, 2/3, 1/6];
@@ -96,11 +113,11 @@
     yhat = yc + h*reshape(newtonk, n, [])*bhat.';
     ycnew = yc + h*reshape(newtonk, n, [])*b.';
 
-    sc = abstol + max(abs(ycnew), abs(yhat))*reltol;
+    sc = abstol + max(abs(ycnew), abs(yc))*reltol;
 
     Mc = M(tc + h);
 
-    err = rms((Mc*(ycnew-yhat))./sc);
+    err = rms((Mc*(ycnew - yhat))./sc);
 
     fac = 0.9;
 

From 7ec54c6a7f66a6181abfcce524b4f486bf977d5b Mon Sep 17 00:00:00 2001
From: Andrey A Popov <apopov@vt.edu>
Date: Mon, 16 May 2022 13:07:40 -0400
Subject: [PATCH 22/32] Fixed most issues with dae34, and renamed it

---
 src/+otp/+pendulumdae/PendulumDAEProblem.m    |  2 +-
 src/+otp/+utils/+solvers/{dae24.m => dae34.m} | 12 ++++--------
 2 files changed, 5 insertions(+), 9 deletions(-)
 rename src/+otp/+utils/+solvers/{dae24.m => dae34.m} (96%)

diff --git a/src/+otp/+pendulumdae/PendulumDAEProblem.m b/src/+otp/+pendulumdae/PendulumDAEProblem.m
index 05bda461..709098ed 100644
--- a/src/+otp/+pendulumdae/PendulumDAEProblem.m
+++ b/src/+otp/+pendulumdae/PendulumDAEProblem.m
@@ -84,7 +84,7 @@ function onSettingsChanged(obj)
         end
 
         function sol = internalSolve(obj, varargin)
-            sol = internalSolve@otp.Problem(obj, 'Solver', @otp.utils.solvers.dae24, varargin{:});
+            sol = internalSolve@otp.Problem(obj, 'Solver', @otp.utils.solvers.dae34, varargin{:});
         end
     end
 
diff --git a/src/+otp/+utils/+solvers/dae24.m b/src/+otp/+utils/+solvers/dae34.m
similarity index 96%
rename from src/+otp/+utils/+solvers/dae24.m
rename to src/+otp/+utils/+solvers/dae34.m
index 05ecf789..89642dd6 100644
--- a/src/+otp/+utils/+solvers/dae24.m
+++ b/src/+otp/+utils/+solvers/dae34.m
@@ -1,4 +1,4 @@
-function [sol, y] = dae24(f, tspan, y0, options)
+function [sol, y] = dae34(f, tspan, y0, options)
 % See
 %    Cameron, F., Palmroth, M., & Piché, R. (2002). 
 %    Quasi stage order conditions for SDIRK methods. 
@@ -28,7 +28,7 @@
 c = sum(A, 2) + gamma;
 
 orderM = 3;
-orderE = 2;
+orderE = 4;
 
 %% Get all relevant options out 
 h = odeget(options, 'InitialStep', []);
@@ -108,15 +108,11 @@
         nmaxits = 1e2;
         alpha = 1;
         its = 0;
-
         etak = inf;
         nnp = inf;
-
         kappa = 1e-1;
-
         ng = inf;
 
-
         Jc = J(staget, yc + stagedy + gh*newtonk0);
         while kappa*(etak*nnp) >= ntol && its < nmaxits
             Mc = M(staget);
@@ -178,8 +174,8 @@
 
     Mc = M(tc + h);
 
-    err = sqrt(mean(((Mc*(ycnew - yhat))./sc).^2));
-    %err = rms(((ycnew - yhat))./sc);
+    errdifferential = sqrt(mean(((Mc*(ycnew - yhat))./sc).^2));
+    err = errdifferential;
 
     fac = 0.38^(1/(orderE + 1));
 

From c5cce23c9e776813f4472923bfa20683c92b2d5d Mon Sep 17 00:00:00 2001
From: Andrey A Popov <apopov@vt.edu>
Date: Tue, 17 May 2022 13:25:38 -0400
Subject: [PATCH 23/32] First commit of ESDIRK method.

---
 src/+otp/+utils/+solvers/dae46.m | 247 +++++++++++++++++++++++++++++++
 1 file changed, 247 insertions(+)
 create mode 100644 src/+otp/+utils/+solvers/dae46.m

diff --git a/src/+otp/+utils/+solvers/dae46.m b/src/+otp/+utils/+solvers/dae46.m
new file mode 100644
index 00000000..5bc7aa00
--- /dev/null
+++ b/src/+otp/+utils/+solvers/dae46.m
@@ -0,0 +1,247 @@
+function [sol, y] = dae46(f, tspan, y0, options)
+% See
+%    Cameron, F., Palmroth, M., & Piché, R. (2002). 
+%    Quasi stage order conditions for SDIRK methods. 
+%    Applied numerical mathematics, 42(1-3), 61-75.
+
+
+gamma = 1/4;
+
+A = zeros(6,6);
+A(2,1) = 1/4;
+A(3,1) = -1/36;
+A(3,2) = -1/18;
+A(4,1) = -21283/32000;
+A(4,2) = -5143/64000;
+A(4,3) = 90909/64000;
+A(5,1) = 46010759/749250000;
+A(5,2) = -737693/40500000;
+A(5,3) = 10931269/45500000;
+A(5,4) = -1140071/34090875;
+A(6,1) = 89/444;
+A(6,2) = 89/804756;
+A(6,3) = -27/364;
+A(6,4) = -20000/171717;
+A(6,5) = 843750/1140071;
+
+%b = A(6, :);
+
+bhat5 = 1/6;
+bhat6 = 0;
+
+bhat1 = 163/333 + (204072709/112387500)* bhat5 - (725/111)*bhat6;
+bhat2 = 17/18 + (5929277/6075000)*bhat5 - (20/3)*bhat6;
+bhat3 = -103/182 - (204072709/61425000)*bhat5 + (1074/91)*bhat6; 
+bhat4 = 4000/30303 - (48017108/102272625)*bhat5 + (4000/10101)*bhat6;
+
+bhat = [bhat1, bhat2, bhat3, bhat4, bhat5, bhat6];
+
+c = sum(A, 2) + gamma;
+
+orderM = 4;
+orderE = 3;
+
+%% Get all relevant options out 
+h = odeget(options, 'InitialStep', []);
+reltol = odeget(options, 'RelTol', 1e-3);
+abstol = odeget(options, 'AbsTol', 1e-6);
+J = odeget(options, 'Jacobian', @(t, y) jacapprox(f, t, y));
+M = odeget(options, 'Mass', speye(numel(y0)));
+MStateDependence = odeget(options, 'MStateDependence', 'none');
+laststage = odeget(options, 'InitialSlope', []);
+
+% We won't support state-dependent Mass, simple as that
+if strcmp(MStateDependence, 'strong')
+    error('OTP:MassStateDependent', 'State dependent mass is not supported.')
+end
+
+if ~isa(M, 'function_handle')
+    M = @(t) M;
+else
+    if nargin(M) > 1
+        M = @(t) M(t, y0);
+    end
+end
+
+if isempty(h)
+    sc = abstol + reltol*abs(y0);
+    f0 = f(tspan(1), y0);
+    d0 = mean((y0./sc).^2);
+    d1 = mean((f0./sc).^2);
+    h0 = (d0/d1)*0.01;
+
+    y1 = y0 + h0*f0;
+    f1 = f(tspan(1) + h0, y1);
+
+    d2 = mean(((f1 - f0)./sc).^2)/h0;
+
+    h1 = (0.01/max(d1, d2))^(1/orderM);
+
+    h = min(100*h0, h1);
+end
+
+t = tspan(1);
+y = y0.';
+
+yc = y0;
+tc = tspan(1);
+
+stagenum = size(A, 1);
+tend = tspan(end);
+
+newtonk0 = zeros(size(yc));
+
+stages = zeros(numel(yc), stagenum);
+
+step = 1;
+while tc < tend
+
+    bnewtonreject = false;
+
+    if tc + h > tend
+        h = tend - tc;
+    end
+
+    gh = gamma*h;
+
+    if step == 1 && isempty(laststage)
+        [laststage, ~] = qmr(M(tc), f(tc, yc));
+    end
+
+    stages(:, 1) = laststage;
+
+    for stage = 2:stagenum
+
+        staget = tc + c(stage)*h;
+
+        stagedy = h*(stages(:, 1:(stage - 1))*A(stage, 1:(stage - 1)).');
+
+        newtonk0 = 0*newtonk0;
+
+        ntol = min(abstol, reltol);
+        nmaxits = 1e2;
+        alpha = 1;
+        its = 0;
+        etak = inf;
+        nnp = inf;
+        kappa = 1e-1;
+        ng = inf;
+
+        Jc = J(staget, yc + stagedy + gh*newtonk0);
+        while kappa*(etak*nnp) >= ntol && its < nmaxits
+            Mc = M(staget);
+            ycs = yc + stagedy + gh*newtonk0;
+            g = Mc*newtonk0 - f(staget, ycs);
+            H = Mc - gh*Jc;
+            [npnew, ~] = qmr(H, g, [], size(H, 1));
+
+            newtonknew = newtonk0 - alpha*npnew;
+
+            sc = abstol + reltol*abs(ycs);
+
+
+            if its > 3
+                ngnew = sqrt(mean((g./sc).^2));
+                % recompute the jacobian
+                if ngnew > 1.25*ng
+                    Jc = J(staget, ycs);
+                end
+                ng = ngnew;
+
+                nnpnew = sqrt(mean((npnew./sc).^2));
+               
+                thetak = nnpnew/nnp;
+
+                if thetak > 10 || isnan(thetak)
+                    bnewtonreject = true;
+                    break;
+                end
+
+                etak = thetak/(1 - thetak);
+            end
+
+            newtonk0 = newtonknew;
+
+            np = npnew;
+            nnp = sqrt(mean((np./sc).^2));
+
+            its = its + 1;
+        end
+
+        if bnewtonreject
+            break;
+        end
+
+        stages(:, stage) = newtonk0;
+
+    end
+
+    if bnewtonreject
+       h = h/2;
+       continue;
+    end
+
+    yhat = yc + h*stages*bhat.';
+    %ycnew = yc + h*stages*b.';
+
+    ycnew = ycs;
+
+    sc = abstol + max(abs(ycnew), abs(yc))*reltol;
+
+    Mc = M(tc + h);
+
+    errdifferential = sqrt(mean(((Mc*(ycnew - yhat))./sc).^2));
+    err = errdifferential;
+
+    fac = 0.38^(1/(orderE + 1));
+
+    facmin = 0.1;
+    if err > 1 || isnan(err)
+        % Reject timestep
+        facmax = 1;
+    else
+        % Accept time step
+        tc = tc + h;
+        yc = ycnew;
+
+        t(step + 1) = tc;
+        y(step + 1, :) = yc.';
+
+
+        step = step + 1;
+
+        facmax = 2.5;
+
+        laststage = stages(:, end);
+
+    end
+
+    % adjust step-size
+    h = h*min(facmax, max(facmin, fac*(1/err)^1/(orderE + 1)));
+
+end
+
+if nargout < 2
+    sol = struct('x', t, 'y', y.');
+else
+    sol = t;
+end
+
+end
+
+function J = jacapprox(f, t, y)
+
+n = numel(y);
+
+J = zeros(n, n);
+
+h = sqrt(1e-6);
+
+f0 = f(t, y);
+
+for i = 1:n
+    e = zeros(n, 1); e(i) = 1;
+    J(:, i) = (f(t, y + h*e) - f0)/h;
+end
+
+end

From ff96669c9d089d964531fd5b0482a1d306a0451b Mon Sep 17 00:00:00 2001
From: Andrey A Popov <apopov@vt.edu>
Date: Tue, 17 May 2022 13:27:22 -0400
Subject: [PATCH 24/32] replace every solver with bicg

---
 src/+otp/+utils/+solvers/dae46.m | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/+otp/+utils/+solvers/dae46.m b/src/+otp/+utils/+solvers/dae46.m
index 5bc7aa00..63dc3f1b 100644
--- a/src/+otp/+utils/+solvers/dae46.m
+++ b/src/+otp/+utils/+solvers/dae46.m
@@ -105,7 +105,7 @@
     gh = gamma*h;
 
     if step == 1 && isempty(laststage)
-        [laststage, ~] = qmr(M(tc), f(tc, yc));
+        [laststage, ~] = bicg(M(tc), f(tc, yc));
     end
 
     stages(:, 1) = laststage;
@@ -133,7 +133,7 @@
             ycs = yc + stagedy + gh*newtonk0;
             g = Mc*newtonk0 - f(staget, ycs);
             H = Mc - gh*Jc;
-            [npnew, ~] = qmr(H, g, [], size(H, 1));
+            [npnew, ~] = bicg(H, g, [], size(H, 1));
 
             newtonknew = newtonk0 - alpha*npnew;
 

From fc51b18ce1daf28b964f51d8fb05c1e364dcee86 Mon Sep 17 00:00:00 2001
From: Andrey A Popov <apopov@vt.edu>
Date: Tue, 17 May 2022 16:17:39 -0400
Subject: [PATCH 25/32] optimize embedded coefficients

---
 src/+otp/+utils/+solvers/dae46.m | 7 ++-----
 1 file changed, 2 insertions(+), 5 deletions(-)

diff --git a/src/+otp/+utils/+solvers/dae46.m b/src/+otp/+utils/+solvers/dae46.m
index 63dc3f1b..48c55078 100644
--- a/src/+otp/+utils/+solvers/dae46.m
+++ b/src/+otp/+utils/+solvers/dae46.m
@@ -24,11 +24,8 @@
 A(6,4) = -20000/171717;
 A(6,5) = 843750/1140071;
 
-%b = A(6, :);
-
-bhat5 = 1/6;
-bhat6 = 0;
-
+bhat5 = 1/4;
+bhat6 = 1/4;
 bhat1 = 163/333 + (204072709/112387500)* bhat5 - (725/111)*bhat6;
 bhat2 = 17/18 + (5929277/6075000)*bhat5 - (20/3)*bhat6;
 bhat3 = -103/182 - (204072709/61425000)*bhat5 + (1074/91)*bhat6; 

From 3c0116499e46a9a1a59bf61b376ed2608c185cfc Mon Sep 17 00:00:00 2001
From: Andrey A Popov <apopov@vt.edu>
Date: Thu, 19 May 2022 09:11:51 -0400
Subject: [PATCH 26/32] Working codes

---
 src/+otp/+utils/+solvers/dae34.m |  8 +++---
 src/+otp/+utils/+solvers/dae43.m | 48 ++++++++++++++------------------
 src/+otp/+utils/+solvers/dae46.m | 32 ++++++++++-----------
 3 files changed, 40 insertions(+), 48 deletions(-)

diff --git a/src/+otp/+utils/+solvers/dae34.m b/src/+otp/+utils/+solvers/dae34.m
index 89642dd6..4ab90542 100644
--- a/src/+otp/+utils/+solvers/dae34.m
+++ b/src/+otp/+utils/+solvers/dae34.m
@@ -54,14 +54,14 @@
 if isempty(h)
     sc = abstol + reltol*abs(y0);
     f0 = f(tspan(1), y0);
-    d0 = mean((y0./sc).^2);
-    d1 = mean((f0./sc).^2);
+    d0 = sqrt(mean((y0./sc).^2));
+    d1 = sqrt(mean((f0./sc).^2));
     h0 = (d0/d1)*0.01;
 
     y1 = y0 + h0*f0;
     f1 = f(tspan(1) + h0, y1);
 
-    d2 = mean(((f1 - f0)./sc).^2)/h0;
+    d2 = sqrt(mean(((f1 - f0)./sc).^2))/h0;
 
     h1 = (0.01/max(d1, d2))^(1/orderM);
 
@@ -119,7 +119,7 @@
             ycs = yc + stagedy + gh*newtonk0;
             g = Mc*newtonk0 - f(staget, ycs);
             H = Mc - gh*Jc;
-            [npnew, ~] = qmr(H, g, [], size(H, 1));
+            npnew = H\g;
 
             newtonknew = newtonk0 - alpha*npnew;
 
diff --git a/src/+otp/+utils/+solvers/dae43.m b/src/+otp/+utils/+solvers/dae43.m
index 5401905d..099bce41 100644
--- a/src/+otp/+utils/+solvers/dae43.m
+++ b/src/+otp/+utils/+solvers/dae43.m
@@ -11,45 +11,40 @@
 
 % We won't support state-dependent Mass, simple as that
 if strcmp(MStateDependence, 'strong')
-    error('OTP:MassStateDependent', 'State dependent mass is not supported.')
+    error('OTP:MassStateDependent', 'Strong state dependent mass is not supported.')
 end
 
 if ~isa(M, 'function_handle')
-    M = @(t) M;
-else
-    if nargin(M) > 1
-        M = @(t) M(t, y0);
-    end
+    M = @(t, y) M;
 end
 
+% Lobatto IIIC
+A = [1/6, -1/3, 1/6; 1/6, 5/12, -1/12; 1/6, 2/3, 1/6];
+b = [1/6, 2/3, 1/6];
+c = [0, 1/2, 1];
+bhat = [-1/2, 2, -1/2];
+
+orderM = 4;
+orderE = 3;
 
-% Compute 
+% Compute initial step size
 if isempty(h)
     sc = abstol + reltol*abs(y0);
     f0 = f(tspan(1), y0);
-    d0 = mean((y0./sc).^2);
-    d1 = mean((f0./sc).^2);
+    d0 = sqrt(mean((y0./sc).^2));
+    d1 = sqrt(mean((f0./sc).^2));
     h0 = (d0/d1)*0.01;
 
     y1 = y0 + h0*f0;
     f1 = f(tspan(1) + h0, y1);
 
-    d2 = mean(((f1 - f0)./sc).^2)/h0;
+    d2 = sqrt(mean(((f1 - f0)./sc).^2))/h0;
 
     h1 = (0.01/max(d1, d2))^(1/orderM);
 
     h = min(100*h0, h1);
 end
 
-
-% Lobatto IIIC
-A = [1/6, -1/3, 1/6; 1/6, 5/12, -1/12; 1/6, 2/3, 1/6];
-b = [1/6, 2/3, 1/6];
-c = [0, 1/2, 1];
-bhat = [-1/2, 2, -1/2];
-
-orderE = 3;
-
 t = tspan(1);
 y = y0.';
 
@@ -61,7 +56,7 @@
 
 step = 1;
 
-Mc = M(tspan(1));
+Mc = M(tspan(1), y0);
 Mfull = zeros(n*stagenum, n*stagenum, 'like', Mc);
 gfull = zeros(n*stagenum, 1);
 Jfull = zeros(n*stagenum, n*stagenum, 'like', Mc);
@@ -75,15 +70,14 @@
 
     % build M
     for stage = 1:stagenum
-        %Mfull(((stage - 1)*n + 1):(stage*n), :) = kron(ones(1, stagenum), M(tc + h*c(stage)));
         si = ((stage - 1)*n + 1):(stage*n);
-        Mfull(si, si) = M(tc + h*c(stage));
+        Mfull(si, si) = M(tc + h*c(stage), yc);
     end
 
     np = inf;
 
-    ntol = 1e-10;
-    nmaxits = 100;
+    ntol = min(abstol, reltol);
+    nmaxits = 1e2;
     its = 0;
 
     newtonk = 0*newtonk;
@@ -104,7 +98,7 @@
 
         H = Mfull - h*Jfull;
 
-        [np, ~] = lsqr(H, gfull, [], size(H, 1));
+        np = H\gfull;
 
         newtonk = newtonk - np;
         its = its + 1;
@@ -115,9 +109,9 @@
 
     sc = abstol + max(abs(ycnew), abs(yc))*reltol;
 
-    Mc = M(tc + h);
+    Mc = M(tc + h , ycnew);
 
-    err = rms((Mc*(ycnew - yhat))./sc);
+    err = sqrt(mean(((Mc*(ycnew - yhat))./sc).^2));
 
     fac = 0.9;
 
diff --git a/src/+otp/+utils/+solvers/dae46.m b/src/+otp/+utils/+solvers/dae46.m
index 48c55078..a036f8cd 100644
--- a/src/+otp/+utils/+solvers/dae46.m
+++ b/src/+otp/+utils/+solvers/dae46.m
@@ -24,6 +24,9 @@
 A(6,4) = -20000/171717;
 A(6,5) = 843750/1140071;
 
+b = A(6, :);
+b(6) = gamma;
+
 bhat5 = 1/4;
 bhat6 = 1/4;
 bhat1 = 163/333 + (204072709/112387500)* bhat5 - (725/111)*bhat6;
@@ -49,28 +52,24 @@
 
 % We won't support state-dependent Mass, simple as that
 if strcmp(MStateDependence, 'strong')
-    error('OTP:MassStateDependent', 'State dependent mass is not supported.')
+    error('OTP:MassStateDependent', 'Strong state dependent mass is not supported.')
 end
 
 if ~isa(M, 'function_handle')
-    M = @(t) M;
-else
-    if nargin(M) > 1
-        M = @(t) M(t, y0);
-    end
+    M = @(t, y) M;
 end
 
 if isempty(h)
     sc = abstol + reltol*abs(y0);
     f0 = f(tspan(1), y0);
-    d0 = mean((y0./sc).^2);
-    d1 = mean((f0./sc).^2);
+    d0 = sqrt(mean((y0./sc).^2));
+    d1 = sqrt(mean((f0./sc).^2));
     h0 = (d0/d1)*0.01;
 
     y1 = y0 + h0*f0;
     f1 = f(tspan(1) + h0, y1);
 
-    d2 = mean(((f1 - f0)./sc).^2)/h0;
+    d2 = sqrt(mean(((f1 - f0)./sc).^2))/h0;
 
     h1 = (0.01/max(d1, d2))^(1/orderM);
 
@@ -102,7 +101,7 @@
     gh = gamma*h;
 
     if step == 1 && isempty(laststage)
-        [laststage, ~] = bicg(M(tc), f(tc, yc));
+        [laststage, ~] = bicg(M(tc, yc), f(tc, yc));
     end
 
     stages(:, 1) = laststage;
@@ -126,11 +125,12 @@
 
         Jc = J(staget, yc + stagedy + gh*newtonk0);
         while kappa*(etak*nnp) >= ntol && its < nmaxits
-            Mc = M(staget);
             ycs = yc + stagedy + gh*newtonk0;
+            Mc = M(staget, ycs);
             g = Mc*newtonk0 - f(staget, ycs);
             H = Mc - gh*Jc;
-            [npnew, ~] = bicg(H, g, [], size(H, 1));
+            %[npnew, ~] = bicg(H, g, [], size(H, 1));
+            npnew = H\g;
 
             newtonknew = newtonk0 - alpha*npnew;
 
@@ -149,7 +149,7 @@
                
                 thetak = nnpnew/nnp;
 
-                if thetak > 10 || isnan(thetak)
+                if thetak > 1.25 || isnan(thetak)
                     bnewtonreject = true;
                     break;
                 end
@@ -179,13 +179,11 @@
     end
 
     yhat = yc + h*stages*bhat.';
-    %ycnew = yc + h*stages*b.';
-
-    ycnew = ycs;
+    ycnew = yc + h*stages*b.';
 
     sc = abstol + max(abs(ycnew), abs(yc))*reltol;
 
-    Mc = M(tc + h);
+    Mc = M(tc + h, ycnew);
 
     errdifferential = sqrt(mean(((Mc*(ycnew - yhat))./sc).^2));
     err = errdifferential;

From e04aa6c4b7094ec047ea9b0b46da02f32c954ae6 Mon Sep 17 00:00:00 2001
From: Andrey A Popov <apopov@vt.edu>
Date: Thu, 19 May 2022 09:26:01 -0400
Subject: [PATCH 27/32] Newton's method control for Lobatto

---
 src/+otp/+utils/+solvers/dae43.m | 38 ++++++++++++++++++++++++++++----
 1 file changed, 34 insertions(+), 4 deletions(-)

diff --git a/src/+otp/+utils/+solvers/dae43.m b/src/+otp/+utils/+solvers/dae43.m
index 099bce41..595e8847 100644
--- a/src/+otp/+utils/+solvers/dae43.m
+++ b/src/+otp/+utils/+solvers/dae43.m
@@ -79,16 +79,24 @@
     ntol = min(abstol, reltol);
     nmaxits = 1e2;
     its = 0;
+    etak = inf;
+    nnp = inf;
+    kappa = 1e-1;
 
     newtonk = 0*newtonk;
+    sc = 0*newtonk;
 
-    while norm(np) > ntol && its < nmaxits
+    bnewtonreject = false;
+
+    while kappa*(etak*nnp) >= ntol && its < nmaxits
         % build g and Jacobians
         for stage = 1:stagenum
             si = ((stage - 1)*n + 1):(stage*n);
             staget = tc + c(stage)*h;
             ycs = yc + h*reshape(newtonk, n, [])*(A(stage, :).');
-       
+
+            sc(si) = abstol + reltol*abs(ycs);
+
             Mc = Mfull(si, si);
             gfull(si) =  Mc*newtonk(si) - f(staget, ycs);
             Jc = J(staget, ycs);
@@ -98,12 +106,34 @@
 
         H = Mfull - h*Jfull;
 
-        np = H\gfull;
+        npnew = H\gfull;
+
+        if its > 2
+            nnpnew = sqrt(mean((npnew./sc).^2));
+
+            thetak = nnpnew/nnp;
+
+            if thetak > 1.25 || isnan(thetak)
+                bnewtonreject = true;
+                break;
+            end
+
+            etak = thetak/(1 - thetak);
+        end
+
+        newtonk = newtonk - npnew;
+
+        np = npnew;
+        nnp = sqrt(mean((np./sc).^2));
 
-        newtonk = newtonk - np;
         its = its + 1;
     end
 
+    if bnewtonreject
+       h = h/2;
+       continue;
+    end
+
     yhat = yc + h*reshape(newtonk, n, [])*bhat.';
     ycnew = yc + h*reshape(newtonk, n, [])*b.';
 

From 5a2cef029c405b1530ec2ed30a937da82f2e6b8a Mon Sep 17 00:00:00 2001
From: Andrey A Popov <apopov@vt.edu>
Date: Thu, 19 May 2022 14:19:57 -0400
Subject: [PATCH 28/32] Increase the number of iterations

---
 src/+otp/+utils/+solvers/dae43.m | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/+otp/+utils/+solvers/dae43.m b/src/+otp/+utils/+solvers/dae43.m
index 595e8847..1cb631e0 100644
--- a/src/+otp/+utils/+solvers/dae43.m
+++ b/src/+otp/+utils/+solvers/dae43.m
@@ -77,7 +77,7 @@
     np = inf;
 
     ntol = min(abstol, reltol);
-    nmaxits = 1e2;
+    nmaxits = 1e3;
     its = 0;
     etak = inf;
     nnp = inf;

From 73a155a67ed534b39016bdf5ce8bc8eb808a1b75 Mon Sep 17 00:00:00 2001
From: Andrey A Popov <apopov@vt.edu>
Date: Thu, 19 May 2022 16:49:40 -0400
Subject: [PATCH 29/32] slight changes to dae43

---
 src/+otp/+utils/+solvers/dae43.m | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/+otp/+utils/+solvers/dae43.m b/src/+otp/+utils/+solvers/dae43.m
index 1cb631e0..da0d5eae 100644
--- a/src/+otp/+utils/+solvers/dae43.m
+++ b/src/+otp/+utils/+solvers/dae43.m
@@ -108,12 +108,12 @@
 
         npnew = H\gfull;
 
-        if its > 2
+        if its > 1
             nnpnew = sqrt(mean((npnew./sc).^2));
 
             thetak = nnpnew/nnp;
 
-            if thetak > 1.25 || isnan(thetak)
+            if thetak > 1.25 || isnan(thetak) || isinf(thetak)
                 bnewtonreject = true;
                 break;
             end

From 026c815d7f293af3ee515df34dbb2c28daa2e5eb Mon Sep 17 00:00:00 2001
From: Andrey A Popov <apopov@vt.edu>
Date: Fri, 20 May 2022 09:24:50 -0400
Subject: [PATCH 30/32] Added Jacobian storage and recomputation criteria.
 Added a starting algorithm.

The starting algorithm for FIRK can actually just use the previous stages. This speeds up computation and just works. For SDIRK this does not work, just for some Lobatto methods
---
 src/+otp/+utils/+solvers/dae43.m | 45 +++++++++++++++++++++++++-------
 1 file changed, 35 insertions(+), 10 deletions(-)

diff --git a/src/+otp/+utils/+solvers/dae43.m b/src/+otp/+utils/+solvers/dae43.m
index da0d5eae..0d8f8beb 100644
--- a/src/+otp/+utils/+solvers/dae43.m
+++ b/src/+otp/+utils/+solvers/dae43.m
@@ -61,6 +61,9 @@
 gfull = zeros(n*stagenum, 1);
 Jfull = zeros(n*stagenum, n*stagenum, 'like', Mc);
 newtonk = zeros(n*stagenum, 1);
+sc = zeros(n*stagenum, 1);
+
+brejectstage = false;
 
 while tc < tend
 
@@ -74,8 +77,6 @@
         Mfull(si, si) = M(tc + h*c(stage), yc);
     end
 
-    np = inf;
-
     ntol = min(abstol, reltol);
     nmaxits = 1e3;
     its = 0;
@@ -83,25 +84,29 @@
     nnp = inf;
     kappa = 1e-1;
 
-    newtonk = 0*newtonk;
-    sc = 0*newtonk;
-
     bnewtonreject = false;
 
+    % Here we never reset the newton stages, as the previous stages are good starting points 
+    % This has empirically reduced the time it takes to compute everything.
+
+    % Compute the Jacobian once per step
+    for stage = 1:stagenum
+        si = ((stage - 1)*n + 1):(stage*n);
+        staget = tc + c(stage)*h;
+        ycs = yc + h*reshape(newtonk, n, [])*(A(stage, :).');
+        Jc = J(staget, ycs);
+        Jfull(si, :) = kron(A(stage, :), Jc);
+    end
+
     while kappa*(etak*nnp) >= ntol && its < nmaxits
         % build g and Jacobians
         for stage = 1:stagenum
             si = ((stage - 1)*n + 1):(stage*n);
             staget = tc + c(stage)*h;
             ycs = yc + h*reshape(newtonk, n, [])*(A(stage, :).');
-
             sc(si) = abstol + reltol*abs(ycs);
-
             Mc = Mfull(si, si);
             gfull(si) =  Mc*newtonk(si) - f(staget, ycs);
-            Jc = J(staget, ycs);
-
-            Jfull(si, :) = kron(A(stage, :), Jc);
         end
 
         H = Mfull - h*Jfull;
@@ -114,10 +119,30 @@
             thetak = nnpnew/nnp;
 
             if thetak > 1.25 || isnan(thetak) || isinf(thetak)
+                
+                % If theta is large, we recompute the Jacobian and try again
+                if ~brejectstage
+                    brejectstage = true;
+
+
+                    for stage = 1:stagenum
+                        si = ((stage - 1)*n + 1):(stage*n);
+                        staget = tc + c(stage)*h;
+                        ycs = yc + h*reshape(newtonk, n, [])*(A(stage, :).');
+                        Jc = J(staget, ycs);
+                        Jfull(si, :) = kron(A(stage, :), Jc);
+                    end
+                    continue;
+                end
+
+                % If theta is still large after recomputing the Jacobian, we reject the step and decrease the stepsize
+
                 bnewtonreject = true;
                 break;
             end
 
+            brejectstage = false;
+
             etak = thetak/(1 - thetak);
         end
 

From 0e458490783377c098711b05282043683d543063 Mon Sep 17 00:00:00 2001
From: Andrey A Popov <apopov@vt.edu>
Date: Fri, 20 May 2022 12:40:42 -0400
Subject: [PATCH 31/32] Added sparse support and support for Jacobian
 decompositions

---
 src/+otp/+utils/+solvers/dae43.m | 39 ++++++++++++++++++++++++++++----
 1 file changed, 34 insertions(+), 5 deletions(-)

diff --git a/src/+otp/+utils/+solvers/dae43.m b/src/+otp/+utils/+solvers/dae43.m
index 0d8f8beb..f4c29f22 100644
--- a/src/+otp/+utils/+solvers/dae43.m
+++ b/src/+otp/+utils/+solvers/dae43.m
@@ -5,7 +5,7 @@
 h = odeget(options, 'InitialStep', []);
 reltol = odeget(options, 'RelTol', 1e-3);
 abstol = odeget(options, 'AbsTol', 1e-6);
-J = odeget(options, 'Jacobian', @(t, y) jacapprox(f, t, y));
+J = odeget(options, 'Jacobian', []);
 M = odeget(options, 'Mass', speye(numel(y0)));
 MStateDependence = odeget(options, 'MStateDependence', 'none');
 
@@ -14,7 +14,20 @@
     error('OTP:MassStateDependent', 'Strong state dependent mass is not supported.')
 end
 
-if ~isa(M, 'function_handle')
+if isempty(J)
+    J = @(t, y) jacapprox(f, t, y);
+    usesparse = false;
+else
+    usesparse = issparse(J(tspan(1), y0));
+end
+
+if isempty(M)
+    if usesparse
+        M = @(t, y) speye(numel(y0));
+    else
+        M = @(t, y) eye(numel(y0));
+    end
+elseif ~isa(M, 'function_handle')
     M = @(t, y) M;
 end
 
@@ -98,6 +111,13 @@
         Jfull(si, :) = kron(A(stage, :), Jc);
     end
 
+    H = Mfull - h*Jfull;
+    if usesparse
+        [L, U, P, Q, D] = lu(H);
+    else
+        [L, U] = lu(H);
+    end
+
     while kappa*(etak*nnp) >= ntol && its < nmaxits
         % build g and Jacobians
         for stage = 1:stagenum
@@ -109,9 +129,11 @@
             gfull(si) =  Mc*newtonk(si) - f(staget, ycs);
         end
 
-        H = Mfull - h*Jfull;
-
-        npnew = H\gfull;
+        if usesparse
+            npnew = Q*(U\(L\(P*(D\gfull))));
+        else
+            npnew = U\(L\gfull);
+        end
 
         if its > 1
             nnpnew = sqrt(mean((npnew./sc).^2));
@@ -132,6 +154,13 @@
                         Jc = J(staget, ycs);
                         Jfull(si, :) = kron(A(stage, :), Jc);
                     end
+                    H = Mfull - h*Jfull;
+                    if usesparse
+                        [L, U, P, Q, D] = lu(H);
+                    else
+                        [L, U] = lu(H);
+                    end
+
                     continue;
                 end
 

From 2d282cceb1a525e0919c0f67a0ca420e1a9efd52 Mon Sep 17 00:00:00 2001
From: Andrey A Popov <apopov@vt.edu>
Date: Sun, 22 May 2022 21:41:58 -0400
Subject: [PATCH 32/32] Made all changes that steven requested

---
 src/+otp/+utils/+solvers/dae43.m        | 42 ++++++++++---------------
 src/+otp/+utils/jacobianApproximation.m | 17 ++++++++++
 2 files changed, 33 insertions(+), 26 deletions(-)
 create mode 100644 src/+otp/+utils/jacobianApproximation.m

diff --git a/src/+otp/+utils/+solvers/dae43.m b/src/+otp/+utils/+solvers/dae43.m
index f4c29f22..8fd8624a 100644
--- a/src/+otp/+utils/+solvers/dae43.m
+++ b/src/+otp/+utils/+solvers/dae43.m
@@ -6,7 +6,7 @@
 reltol = odeget(options, 'RelTol', 1e-3);
 abstol = odeget(options, 'AbsTol', 1e-6);
 J = odeget(options, 'Jacobian', []);
-M = odeget(options, 'Mass', speye(numel(y0)));
+M = odeget(options, 'Mass', []);
 MStateDependence = odeget(options, 'MStateDependence', 'none');
 
 % We won't support state-dependent Mass, simple as that
@@ -15,7 +15,7 @@
 end
 
 if isempty(J)
-    J = @(t, y) jacapprox(f, t, y);
+    J = @(t, y) otp.utils.jacobianApproximation(f, t, y);
     usesparse = false;
 else
     usesparse = issparse(J(tspan(1), y0));
@@ -29,6 +29,8 @@
     end
 elseif ~isa(M, 'function_handle')
     M = @(t, y) M;
+elseif isa(M, 'function_handle') && nargin(M) == 1
+    M = @(t, y) M(t);
 end
 
 % Lobatto IIIC
@@ -42,7 +44,7 @@
 
 % Compute initial step size
 if isempty(h)
-    sc = abstol + reltol*abs(y0);
+    sc = abstol + reltol.*abs(y0);
     f0 = f(tspan(1), y0);
     d0 = sqrt(mean((y0./sc).^2));
     d1 = sqrt(mean((f0./sc).^2));
@@ -69,10 +71,15 @@
 
 step = 1;
 
-Mc = M(tspan(1), y0);
-Mfull = zeros(n*stagenum, n*stagenum, 'like', Mc);
+if usesparse
+    Mfull = sparse(n*stagenum, n*stagenum);
+    Jfull = sparse(n*stagenum, n*stagenum);
+else
+    Mfull = zeros(n*stagenum, n*stagenum);
+    Jfull = zeros(n*stagenum, n*stagenum);
+end
+
 gfull = zeros(n*stagenum, 1);
-Jfull = zeros(n*stagenum, n*stagenum, 'like', Mc);
 newtonk = zeros(n*stagenum, 1);
 sc = zeros(n*stagenum, 1);
 
@@ -90,7 +97,7 @@
         Mfull(si, si) = M(tc + h*c(stage), yc);
     end
 
-    ntol = min(abstol, reltol);
+    ntol = min(abstol, min(reltol));
     nmaxits = 1e3;
     its = 0;
     etak = inf;
@@ -124,7 +131,7 @@
             si = ((stage - 1)*n + 1):(stage*n);
             staget = tc + c(stage)*h;
             ycs = yc + h*reshape(newtonk, n, [])*(A(stage, :).');
-            sc(si) = abstol + reltol*abs(ycs);
+            sc(si) = abstol + reltol.*abs(ycs);
             Mc = Mfull(si, si);
             gfull(si) =  Mc*newtonk(si) - f(staget, ycs);
         end
@@ -191,7 +198,7 @@
     yhat = yc + h*reshape(newtonk, n, [])*bhat.';
     ycnew = yc + h*reshape(newtonk, n, [])*b.';
 
-    sc = abstol + max(abs(ycnew), abs(yc))*reltol;
+    sc = abstol + max(abs(ycnew), abs(yc)).*reltol;
 
     Mc = M(tc + h , ycnew);
 
@@ -230,20 +237,3 @@
 end
 
 end
-
-function J = jacapprox(f, t, y)
-
-n = numel(y);
-
-J = zeros(n, n);
-
-h = sqrt(1e-6);
-
-f0 = f(t, y);
-
-for i = 1:n
-    e = zeros(n, 1); e(i) = 1;
-    J(:, i) = (f(t, y + h*e) - f0)/h;
-end
-
-end
diff --git a/src/+otp/+utils/jacobianApproximation.m b/src/+otp/+utils/jacobianApproximation.m
new file mode 100644
index 00000000..e743bf18
--- /dev/null
+++ b/src/+otp/+utils/jacobianApproximation.m
@@ -0,0 +1,17 @@
+function J = jacobianApproximation(f, t, y)
+% TODO: make this more robust
+
+n = numel(y);
+
+J = zeros(n, n);
+
+h = sqrt(1e-6);
+
+f0 = f(t, y);
+
+for i = 1:n
+    e = zeros(n, 1); e(i) = 1;
+    J(:, i) = (f(t, y + h*e) - f0)/h;
+end
+
+end