diff --git a/include/utils/params.hh b/include/utils/params.hh
index 214c8b6..fe9d19f 100644
--- a/include/utils/params.hh
+++ b/include/utils/params.hh
@@ -28,7 +28,6 @@ struct Params{
         double rk4_t;           // runge kutta integration time [s]
         int Nthreads;           // number of threads
         int nPlanning;          // number of points we want from the planner
-        bool TroProfile;        // set to true to follow TRO velocity profile 
         struct Topics{
             string commands;            // Car Commands topic
             string state;               // Car State topic
diff --git a/params/dyn_autocross.yaml b/params/dyn_autocross.yaml
index bddf0f4..7922919 100644
--- a/params/dyn_autocross.yaml
+++ b/params/dyn_autocross.yaml
@@ -8,10 +8,10 @@ dictitems:
   Df: 2785.4
   Dr: 3152.3
   Vy_max: 5.0
-  YawRate_max: 50.0
+  YawRate_max: 80.0
   dMtv: 1.0
   dRd: 0.2
-  diff_delta: 100.0
+  diff_delta: 1000.0
   groups: !!python/object/new:dynamic_reconfigure.encoding.Config
     dictitems:
       Bf: 10.8529
@@ -22,36 +22,36 @@ dictitems:
       Df: 2785.4
       Dr: 3152.3
       Vy_max: 5.0
-      YawRate_max: 50.0
+      YawRate_max: 80.0
       dMtv: 1.0
       dRd: 0.2
-      diff_delta: 100.0
+      diff_delta: 1000.0
       groups: !!python/object/new:dynamic_reconfigure.encoding.Config
         state: []
       id: 0
-      latency: 5
+      latency: 10
       name: Default
       parameters: !!python/object/new:dynamic_reconfigure.encoding.Config
         state: []
       parent: 0
-      q_mu: 0.1
+      q_mu: 0.0
       q_n: 15.0
       q_nN: 15.0
-      q_s: 30.0
-      q_sN: 30.0
+      q_s: 50.0
+      q_sN: 50.0
       q_slack_track: 100.0
       q_slip: 2.0
       state: true
       type: ''
       u_r: 0.45
     state: []
-  latency: 5
-  q_mu: 0.1
+  latency: 10
+  q_mu: 0.0
   q_n: 15.0
   q_nN: 15.0
-  q_s: 30.0
-  q_sN: 30.0
+  q_s: 50.0
+  q_sN: 50.0
   q_slack_track: 100.0
   q_slip: 2.0
   u_r: 0.45
-state: []
+state: []
\ No newline at end of file
diff --git a/params/dyn_trackdrive.yaml b/params/dyn_trackdrive.yaml
index 9d61bc5..0b5fa9f 100644
--- a/params/dyn_trackdrive.yaml
+++ b/params/dyn_trackdrive.yaml
@@ -8,9 +8,9 @@ dictitems:
   Df: 2785.4
   Dr: 3152.3
   Vy_max: 5.0
-  YawRate_max: 40.0
+  YawRate_max: 80.0
   dMtv: 1.0
-  dRd: 0.1
+  dRd: 0.2
   diff_delta: 1000.0
   groups: !!python/object/new:dynamic_reconfigure.encoding.Config
     dictitems:
@@ -22,35 +22,35 @@ dictitems:
       Df: 2785.4
       Dr: 3152.3
       Vy_max: 5.0
-      YawRate_max: 40.0
+      YawRate_max: 80.0
       dMtv: 1.0
-      dRd: 0.1
+      dRd: 0.2
       diff_delta: 1000.0
       groups: !!python/object/new:dynamic_reconfigure.encoding.Config
         state: []
       id: 0
-      latency: 15
+      latency: 10
       name: Default
       parameters: !!python/object/new:dynamic_reconfigure.encoding.Config
         state: []
       parent: 0
-      q_mu: 0.1
-      q_n: 10.0
+      q_mu: 0.0
+      q_n: 15.0
       q_nN: 15.0
-      q_s: 70.0
-      q_sN: 70.0
+      q_s: 50.0
+      q_sN: 50.0
       q_slack_track: 100.0
       q_slip: 2.0
       state: true
       type: ''
       u_r: 0.45
     state: []
-  latency: 15
-  q_mu: 0.1
-  q_n: 10.0
+  latency: 10
+  q_mu: 0.0
+  q_n: 15.0
   q_nN: 15.0
-  q_s: 70.0
-  q_sN: 70.0
+  q_s: 50.0
+  q_sN: 50.0
   q_slack_track: 100.0
   q_slip: 2.0
   u_r: 0.45
diff --git a/params/tailored_mpc.yaml b/params/tailored_mpc.yaml
index f3c29bd..c181074 100644
--- a/params/tailored_mpc.yaml
+++ b/params/tailored_mpc.yaml
@@ -32,13 +32,12 @@ Topics:
   # TO DO: fill with other topics
 
 NLOP:
-  N: 20                                               # Prediction horizon
+  N: 40                                               # Prediction horizon
   Nslacks: 1                                          # Number of slacks variables            
 
 MPC:
   Hz: 20                                                #[s] MPC frequency
   rk4_t: 0.020                                          #[s] Runge Kutta integration time
   Nthreads: 10                                          # Number of threads
-  TroProfile: false                                     # Set to true to follow TRO velocity profile 
 
   nPlanning: 1900                                       # n points saved from the planner
diff --git a/solver/FORCES_problem.m b/solver/FORCES_problem.m
index 811c9dc..6d2be69 100644
--- a/solver/FORCES_problem.m
+++ b/solver/FORCES_problem.m
@@ -16,7 +16,7 @@
 addpath(solverDir);
 
 %% Problem info
-N = 20; % Horizon Length
+N = 40; % Horizon Length
 n_states = 5;
 n_controls = 3;
 
diff --git a/solver/codeGen/TailoredSolver.m b/solver/codeGen/TailoredSolver.m
index 4e751e1..008c6cf 100644
--- a/solver/codeGen/TailoredSolver.m
+++ b/solver/codeGen/TailoredSolver.m
@@ -2,17 +2,17 @@
 %
 %   OUTPUT = TailoredSolver(PARAMS) solves a multistage problem
 %   subject to the parameters supplied in the following struct:
-%       PARAMS.lb - column vector of length 160
-%       PARAMS.ub - column vector of length 140
-%       PARAMS.hu - column vector of length 40
+%       PARAMS.lb - column vector of length 320
+%       PARAMS.ub - column vector of length 280
+%       PARAMS.hu - column vector of length 80
 %       PARAMS.xinit - column vector of length 7
-%       PARAMS.x0 - column vector of length 160
-%       PARAMS.all_parameters - column vector of length 500
+%       PARAMS.x0 - column vector of length 320
+%       PARAMS.all_parameters - column vector of length 1000
 %       PARAMS.num_of_threads - scalar
 %
 %   OUTPUT returns the values of the last iteration of the solver where
-%       OUTPUT.U - column vector of size 80
-%       OUTPUT.X - column vector of size 80
+%       OUTPUT.U - column vector of size 160
+%       OUTPUT.X - column vector of size 160
 %
 %   [OUTPUT, EXITFLAG] = TailoredSolver(PARAMS) returns additionally
 %   the integer EXITFLAG indicating the state of the solution with 
diff --git a/solver/codeGen/TailoredSolver.mexa64 b/solver/codeGen/TailoredSolver.mexa64
index e68b5d0..c1a6d00 100755
Binary files a/solver/codeGen/TailoredSolver.mexa64 and b/solver/codeGen/TailoredSolver.mexa64 differ
diff --git a/solver/codeGen/TailoredSolver/TailoredSolver.m b/solver/codeGen/TailoredSolver/TailoredSolver.m
index 4e751e1..008c6cf 100644
--- a/solver/codeGen/TailoredSolver/TailoredSolver.m
+++ b/solver/codeGen/TailoredSolver/TailoredSolver.m
@@ -2,17 +2,17 @@
 %
 %   OUTPUT = TailoredSolver(PARAMS) solves a multistage problem
 %   subject to the parameters supplied in the following struct:
-%       PARAMS.lb - column vector of length 160
-%       PARAMS.ub - column vector of length 140
-%       PARAMS.hu - column vector of length 40
+%       PARAMS.lb - column vector of length 320
+%       PARAMS.ub - column vector of length 280
+%       PARAMS.hu - column vector of length 80
 %       PARAMS.xinit - column vector of length 7
-%       PARAMS.x0 - column vector of length 160
-%       PARAMS.all_parameters - column vector of length 500
+%       PARAMS.x0 - column vector of length 320
+%       PARAMS.all_parameters - column vector of length 1000
 %       PARAMS.num_of_threads - scalar
 %
 %   OUTPUT returns the values of the last iteration of the solver where
-%       OUTPUT.U - column vector of size 80
-%       OUTPUT.X - column vector of size 80
+%       OUTPUT.U - column vector of size 160
+%       OUTPUT.X - column vector of size 160
 %
 %   [OUTPUT, EXITFLAG] = TailoredSolver(PARAMS) returns additionally
 %   the integer EXITFLAG indicating the state of the solution with 
diff --git a/solver/codeGen/TailoredSolver/include/TailoredSolver.h b/solver/codeGen/TailoredSolver/include/TailoredSolver.h
index d1d74fe..57ebe42 100644
--- a/solver/codeGen/TailoredSolver/include/TailoredSolver.h
+++ b/solver/codeGen/TailoredSolver/include/TailoredSolver.h
@@ -19,7 +19,7 @@ jurisdiction in case of any dispute.
 
 */
 
-/* Generated by FORCESPRO v6.0.0 on Sunday, November 27, 2022 at 12:38:01 PM */
+/* Generated by FORCESPRO v6.0.0 on Monday, December 5, 2022 at 8:27:11 PM */
 #ifndef TailoredSolver_H
 #define TailoredSolver_H
 
@@ -184,23 +184,23 @@ extern size_t TailoredSolver_get_const_size( void );
 /* fill this with data before calling the solver! */
 typedef struct
 {
-    /* vector of size 160 */
-    TailoredSolver_float lb[160];
+    /* vector of size 320 */
+    TailoredSolver_float lb[320];
 
-    /* vector of size 140 */
-    TailoredSolver_float ub[140];
+    /* vector of size 280 */
+    TailoredSolver_float ub[280];
 
-    /* vector of size 40 */
-    TailoredSolver_float hu[40];
+    /* vector of size 80 */
+    TailoredSolver_float hu[80];
 
     /* vector of size 7 */
     TailoredSolver_float xinit[7];
 
-    /* vector of size 160 */
-    TailoredSolver_float x0[160];
+    /* vector of size 320 */
+    TailoredSolver_float x0[320];
 
-    /* vector of size 500 */
-    TailoredSolver_float all_parameters[500];
+    /* vector of size 1000 */
+    TailoredSolver_float all_parameters[1000];
 
     /* scalar */
     solver_int32_unsigned num_of_threads;
@@ -213,11 +213,11 @@ typedef struct
 /* the desired variables are put here by the solver */
 typedef struct
 {
-    /* vector of size 80 */
-    TailoredSolver_float U[80];
+    /* vector of size 160 */
+    TailoredSolver_float U[160];
 
-    /* vector of size 80 */
-    TailoredSolver_float X[80];
+    /* vector of size 160 */
+    TailoredSolver_float X[160];
 
 
 } TailoredSolver_output;
@@ -299,10 +299,10 @@ typedef struct
 
 
 /* SOLVER FUNCTION DEFINITION -------------------------------------------*/
-/* Time of Solver Generation: (UTC) Sunday, November 27, 2022 12:38:04 PM */
+/* Time of Solver Generation: (UTC) Monday, December 5, 2022 8:27:16 PM */
 /* User License expires on: (UTC) Monday, September 25, 2023 10:00:00 PM (approx.) (at the time of code generation) */
 /* Solver Static License expires on: (UTC) Monday, September 25, 2023 10:00:00 PM (approx.) */
-/* Solver Id: 5a4d13a6-bec1-4a6a-b016-8b174252534a */
+/* Solver Id: 484892cb-eeff-4361-94ed-f59372485c0b */
 /* examine exitflag before using the result! */
 #ifdef __cplusplus
 extern "C" {
diff --git a/solver/codeGen/TailoredSolver/include/TailoredSolver_memory.h b/solver/codeGen/TailoredSolver/include/TailoredSolver_memory.h
index 008d397..461bdac 100644
--- a/solver/codeGen/TailoredSolver/include/TailoredSolver_memory.h
+++ b/solver/codeGen/TailoredSolver/include/TailoredSolver_memory.h
@@ -19,7 +19,7 @@ jurisdiction in case of any dispute.
 
 */
 
-/* Generated by FORCESPRO v6.0.0 on Sunday, November 27, 2022 at 12:38:01 PM */
+/* Generated by FORCESPRO v6.0.0 on Monday, December 5, 2022 at 8:27:11 PM */
 #ifndef TailoredSolver_MEMORY_H
 #define TailoredSolver_MEMORY_H
 
diff --git a/solver/codeGen/TailoredSolver/interface/TailoredSolver.m b/solver/codeGen/TailoredSolver/interface/TailoredSolver.m
index 4e751e1..008c6cf 100644
--- a/solver/codeGen/TailoredSolver/interface/TailoredSolver.m
+++ b/solver/codeGen/TailoredSolver/interface/TailoredSolver.m
@@ -2,17 +2,17 @@
 %
 %   OUTPUT = TailoredSolver(PARAMS) solves a multistage problem
 %   subject to the parameters supplied in the following struct:
-%       PARAMS.lb - column vector of length 160
-%       PARAMS.ub - column vector of length 140
-%       PARAMS.hu - column vector of length 40
+%       PARAMS.lb - column vector of length 320
+%       PARAMS.ub - column vector of length 280
+%       PARAMS.hu - column vector of length 80
 %       PARAMS.xinit - column vector of length 7
-%       PARAMS.x0 - column vector of length 160
-%       PARAMS.all_parameters - column vector of length 500
+%       PARAMS.x0 - column vector of length 320
+%       PARAMS.all_parameters - column vector of length 1000
 %       PARAMS.num_of_threads - scalar
 %
 %   OUTPUT returns the values of the last iteration of the solver where
-%       OUTPUT.U - column vector of size 80
-%       OUTPUT.X - column vector of size 80
+%       OUTPUT.U - column vector of size 160
+%       OUTPUT.X - column vector of size 160
 %
 %   [OUTPUT, EXITFLAG] = TailoredSolver(PARAMS) returns additionally
 %   the integer EXITFLAG indicating the state of the solution with 
diff --git a/solver/codeGen/TailoredSolver/interface/TailoredSolver_dynamics_mex.c b/solver/codeGen/TailoredSolver/interface/TailoredSolver_dynamics_mex.c
index f17354d..7b645df 100644
--- a/solver/codeGen/TailoredSolver/interface/TailoredSolver_dynamics_mex.c
+++ b/solver/codeGen/TailoredSolver/interface/TailoredSolver_dynamics_mex.c
@@ -30,7 +30,7 @@ jurisdiction in case of any dispute.
 
 typedef TailoredSolver_float solver_float;
 typedef solver_int32_default solver_int;
-#define NSTAGES ( 20 )
+#define NSTAGES ( 40 )
 #define MAX(X, Y)  ((X) < (Y) ? (Y) : (X))
 
 /* For compatibility with Microsoft Visual Studio 2015 */
@@ -84,14 +84,14 @@ TailoredSolver_extfunc pt2function_TailoredSolver = &TailoredSolver_adtool2force
 static void getDims(const solver_int stage, solver_int* nvar, solver_int* neq, solver_int* dimh, 
              solver_int* dimp, solver_int* diml, solver_int* dimu, solver_int* dimhl, solver_int* dimhu)
 {
-    const solver_int nvarArr[NSTAGES] = {8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8}; 
-    const solver_int neqArr[NSTAGES] = {7, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5};
-    const solver_int dimhArr[NSTAGES] = {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2};
-    const solver_int dimpArr[NSTAGES] = {25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25};
-    const solver_int dimlArr[NSTAGES] = {8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8};
-    const solver_int dimuArr[NSTAGES] = {7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7};
-    const solver_int dimhlArr[NSTAGES] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
-    const solver_int dimhuArr[NSTAGES] = {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2};
+    const solver_int nvarArr[NSTAGES] = {8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8}; 
+    const solver_int neqArr[NSTAGES] = {7, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5};
+    const solver_int dimhArr[NSTAGES] = {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2};
+    const solver_int dimpArr[NSTAGES] = {25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25};
+    const solver_int dimlArr[NSTAGES] = {8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8};
+    const solver_int dimuArr[NSTAGES] = {7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7};
+    const solver_int dimhlArr[NSTAGES] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+    const solver_int dimhuArr[NSTAGES] = {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2};
 
     *nvar = nvarArr[stage];
     *neq = neqArr[stage];
diff --git a/solver/codeGen/TailoredSolver/interface/TailoredSolver_inequalities_mex.c b/solver/codeGen/TailoredSolver/interface/TailoredSolver_inequalities_mex.c
index 9ee99ff..9d2f59f 100644
--- a/solver/codeGen/TailoredSolver/interface/TailoredSolver_inequalities_mex.c
+++ b/solver/codeGen/TailoredSolver/interface/TailoredSolver_inequalities_mex.c
@@ -30,7 +30,7 @@ jurisdiction in case of any dispute.
 
 typedef TailoredSolver_float solver_float;
 typedef solver_int32_default solver_int;
-#define NSTAGES ( 20 )
+#define NSTAGES ( 40 )
 #define MAX(X, Y)  ((X) < (Y) ? (Y) : (X))
 
 /* For compatibility with Microsoft Visual Studio 2015 */
@@ -84,14 +84,14 @@ TailoredSolver_extfunc pt2function_TailoredSolver = &TailoredSolver_adtool2force
 static void getDims(const solver_int stage, solver_int* nvar, solver_int* neq, solver_int* dimh, 
              solver_int* dimp, solver_int* diml, solver_int* dimu, solver_int* dimhl, solver_int* dimhu)
 {
-    const solver_int nvarArr[NSTAGES] = {8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8}; 
-    const solver_int neqArr[NSTAGES] = {7, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5};
-    const solver_int dimhArr[NSTAGES] = {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2};
-    const solver_int dimpArr[NSTAGES] = {25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25};
-    const solver_int dimlArr[NSTAGES] = {8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8};
-    const solver_int dimuArr[NSTAGES] = {7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7};
-    const solver_int dimhlArr[NSTAGES] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
-    const solver_int dimhuArr[NSTAGES] = {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2};
+    const solver_int nvarArr[NSTAGES] = {8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8}; 
+    const solver_int neqArr[NSTAGES] = {7, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5};
+    const solver_int dimhArr[NSTAGES] = {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2};
+    const solver_int dimpArr[NSTAGES] = {25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25};
+    const solver_int dimlArr[NSTAGES] = {8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8};
+    const solver_int dimuArr[NSTAGES] = {7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7};
+    const solver_int dimhlArr[NSTAGES] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+    const solver_int dimhuArr[NSTAGES] = {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2};
 
     *nvar = nvarArr[stage];
     *neq = neqArr[stage];
diff --git a/solver/codeGen/TailoredSolver/interface/TailoredSolver_lib.mdl b/solver/codeGen/TailoredSolver/interface/TailoredSolver_lib.mdl
index 8a82604..16155f8 100644
--- a/solver/codeGen/TailoredSolver/interface/TailoredSolver_lib.mdl
+++ b/solver/codeGen/TailoredSolver/interface/TailoredSolver_lib.mdl
@@ -60,13 +60,13 @@ Model {
       }
     }
   }
-  Created		  "11/27/2022 12:38:04 PM"
+  Created		  "12/5/2022 8:27:17 PM"
   Creator		  "embotech AG"
   UpdateHistory		  "UpdateHistoryNever"
   ModifiedByFormat	  "%<Auto>"
   LastModifiedBy	  "embotech AG"
   ModifiedDateFormat	  "%<Auto>"
-  LastModifiedDate	  "11/27/2022 12:38:04 PM"
+  LastModifiedDate	  "12/5/2022 8:27:17 PM"
   RTWModifiedTimeStamp	  315310195
   ModelVersionFormat	  "1.%<AutoIncrement:2>"
   ConfigurationManager	  "None"
@@ -801,17 +801,17 @@ Model {
 	Help			"TailoredSolver_simulinkBlock provides an easy Simulink interface for simulating your customized solver.  \n"
 	"\nOUTPUTS = TailoredSolver(INPUTS) solves an optimization problem where:\n\n"
 	"INPUTS:\n"
-	" - lb - column vector of length 160\n "
-" - ub - column vector of length 140\n "
-" - hu - column vector of length 40\n "
+	" - lb - column vector of length 320\n "
+" - ub - column vector of length 280\n "
+" - hu - column vector of length 80\n "
 " - xinit - column vector of length 7\n "
-" - x0 - column vector of length 160\n "
-" - all_parameters - column vector of length 500\n "
+" - x0 - column vector of length 320\n "
+" - all_parameters - column vector of length 1000\n "
 " - num_of_threads - column vector of length 1\n "
 
 	"\nOUTPUTS:\n"
-	" - U - column vector of length 80\n "
-" - X - column vector of length 80\n "
+	" - U - column vector of length 160\n "
+" - X - column vector of length 160\n "
 
 	"\n For more information, see https://forces.embotech.com/Documentation/graphical_interface/index.html \n "
 
diff --git a/solver/codeGen/TailoredSolver/interface/TailoredSolver_mex.c b/solver/codeGen/TailoredSolver/interface/TailoredSolver_mex.c
index 29fbe7a..820a270 100644
--- a/solver/codeGen/TailoredSolver/interface/TailoredSolver_mex.c
+++ b/solver/codeGen/TailoredSolver/interface/TailoredSolver_mex.c
@@ -168,14 +168,14 @@ void mexFunction( solver_int32_default nlhs, mxArray *plhs[], solver_int32_defau
     {
     mexErrMsgTxt("PARAMS.lb must be a double.");
     }
-    if( mxGetM(par) != 160 || mxGetN(par) != 1 ) 
+    if( mxGetM(par) != 320 || mxGetN(par) != 1 ) 
 	{
-    mexErrMsgTxt("PARAMS.lb must be of size [160 x 1]");
+    mexErrMsgTxt("PARAMS.lb must be of size [320 x 1]");
     }
 #endif	 
 	if ( (mxGetN(par) != 0) && (mxGetM(par) != 0) )
 	{
-		copyMArrayToC_double(mxGetPr(par), params.lb,160);
+		copyMArrayToC_double(mxGetPr(par), params.lb,320);
 
 	}
 	par = mxGetField(PARAMS, 0, "ub");
@@ -188,14 +188,14 @@ void mexFunction( solver_int32_default nlhs, mxArray *plhs[], solver_int32_defau
     {
     mexErrMsgTxt("PARAMS.ub must be a double.");
     }
-    if( mxGetM(par) != 140 || mxGetN(par) != 1 ) 
+    if( mxGetM(par) != 280 || mxGetN(par) != 1 ) 
 	{
-    mexErrMsgTxt("PARAMS.ub must be of size [140 x 1]");
+    mexErrMsgTxt("PARAMS.ub must be of size [280 x 1]");
     }
 #endif	 
 	if ( (mxGetN(par) != 0) && (mxGetM(par) != 0) )
 	{
-		copyMArrayToC_double(mxGetPr(par), params.ub,140);
+		copyMArrayToC_double(mxGetPr(par), params.ub,280);
 
 	}
 	par = mxGetField(PARAMS, 0, "hu");
@@ -208,14 +208,14 @@ void mexFunction( solver_int32_default nlhs, mxArray *plhs[], solver_int32_defau
     {
     mexErrMsgTxt("PARAMS.hu must be a double.");
     }
-    if( mxGetM(par) != 40 || mxGetN(par) != 1 ) 
+    if( mxGetM(par) != 80 || mxGetN(par) != 1 ) 
 	{
-    mexErrMsgTxt("PARAMS.hu must be of size [40 x 1]");
+    mexErrMsgTxt("PARAMS.hu must be of size [80 x 1]");
     }
 #endif	 
 	if ( (mxGetN(par) != 0) && (mxGetM(par) != 0) )
 	{
-		copyMArrayToC_double(mxGetPr(par), params.hu,40);
+		copyMArrayToC_double(mxGetPr(par), params.hu,80);
 
 	}
 	par = mxGetField(PARAMS, 0, "xinit");
@@ -248,14 +248,14 @@ void mexFunction( solver_int32_default nlhs, mxArray *plhs[], solver_int32_defau
     {
     mexErrMsgTxt("PARAMS.x0 must be a double.");
     }
-    if( mxGetM(par) != 160 || mxGetN(par) != 1 ) 
+    if( mxGetM(par) != 320 || mxGetN(par) != 1 ) 
 	{
-    mexErrMsgTxt("PARAMS.x0 must be of size [160 x 1]");
+    mexErrMsgTxt("PARAMS.x0 must be of size [320 x 1]");
     }
 #endif	 
 	if ( (mxGetN(par) != 0) && (mxGetM(par) != 0) )
 	{
-		copyMArrayToC_double(mxGetPr(par), params.x0,160);
+		copyMArrayToC_double(mxGetPr(par), params.x0,320);
 
 	}
 	par = mxGetField(PARAMS, 0, "all_parameters");
@@ -268,14 +268,14 @@ void mexFunction( solver_int32_default nlhs, mxArray *plhs[], solver_int32_defau
     {
     mexErrMsgTxt("PARAMS.all_parameters must be a double.");
     }
-    if( mxGetM(par) != 500 || mxGetN(par) != 1 ) 
+    if( mxGetM(par) != 1000 || mxGetN(par) != 1 ) 
 	{
-    mexErrMsgTxt("PARAMS.all_parameters must be of size [500 x 1]");
+    mexErrMsgTxt("PARAMS.all_parameters must be of size [1000 x 1]");
     }
 #endif	 
 	if ( (mxGetN(par) != 0) && (mxGetM(par) != 0) )
 	{
-		copyMArrayToC_double(mxGetPr(par), params.all_parameters,500);
+		copyMArrayToC_double(mxGetPr(par), params.all_parameters,1000);
 
 	}
 	par = mxGetField(PARAMS, 0, "num_of_threads");
@@ -314,12 +314,12 @@ void mexFunction( solver_int32_default nlhs, mxArray *plhs[], solver_int32_defau
 
 	/* copy output to matlab arrays */
 	plhs[0] = mxCreateStructMatrix(1, 1, 2, outputnames);
-		outvar = mxCreateDoubleMatrix(80, 1, mxREAL);
-	copyCArrayToM_double( output.U, mxGetPr(outvar), 80);
+		outvar = mxCreateDoubleMatrix(160, 1, mxREAL);
+	copyCArrayToM_double( output.U, mxGetPr(outvar), 160);
 	mxSetField(plhs[0], 0, "U", outvar);
 
-	outvar = mxCreateDoubleMatrix(80, 1, mxREAL);
-	copyCArrayToM_double( output.X, mxGetPr(outvar), 80);
+	outvar = mxCreateDoubleMatrix(160, 1, mxREAL);
+	copyCArrayToM_double( output.X, mxGetPr(outvar), 160);
 	mxSetField(plhs[0], 0, "X", outvar);
 
 
diff --git a/solver/codeGen/TailoredSolver/interface/TailoredSolver_objective_mex.c b/solver/codeGen/TailoredSolver/interface/TailoredSolver_objective_mex.c
index 06aea3f..7f303ab 100644
--- a/solver/codeGen/TailoredSolver/interface/TailoredSolver_objective_mex.c
+++ b/solver/codeGen/TailoredSolver/interface/TailoredSolver_objective_mex.c
@@ -30,7 +30,7 @@ jurisdiction in case of any dispute.
 
 typedef TailoredSolver_float solver_float;
 typedef solver_int32_default solver_int;
-#define NSTAGES ( 20 )
+#define NSTAGES ( 40 )
 #define MAX(X, Y)  ((X) < (Y) ? (Y) : (X))
 
 /* For compatibility with Microsoft Visual Studio 2015 */
@@ -84,14 +84,14 @@ TailoredSolver_extfunc pt2function_TailoredSolver = &TailoredSolver_adtool2force
 static void getDims(const solver_int stage, solver_int* nvar, solver_int* neq, solver_int* dimh, 
              solver_int* dimp, solver_int* diml, solver_int* dimu, solver_int* dimhl, solver_int* dimhu)
 {
-    const solver_int nvarArr[NSTAGES] = {8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8}; 
-    const solver_int neqArr[NSTAGES] = {7, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5};
-    const solver_int dimhArr[NSTAGES] = {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2};
-    const solver_int dimpArr[NSTAGES] = {25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25};
-    const solver_int dimlArr[NSTAGES] = {8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8};
-    const solver_int dimuArr[NSTAGES] = {7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7};
-    const solver_int dimhlArr[NSTAGES] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
-    const solver_int dimhuArr[NSTAGES] = {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2};
+    const solver_int nvarArr[NSTAGES] = {8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8}; 
+    const solver_int neqArr[NSTAGES] = {7, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5};
+    const solver_int dimhArr[NSTAGES] = {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2};
+    const solver_int dimpArr[NSTAGES] = {25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25};
+    const solver_int dimlArr[NSTAGES] = {8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8};
+    const solver_int dimuArr[NSTAGES] = {7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7};
+    const solver_int dimhlArr[NSTAGES] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+    const solver_int dimhuArr[NSTAGES] = {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2};
 
     *nvar = nvarArr[stage];
     *neq = neqArr[stage];
diff --git a/solver/codeGen/TailoredSolver/interface/TailoredSolver_py.py b/solver/codeGen/TailoredSolver/interface/TailoredSolver_py.py
index 0b657ec..d0ada42 100644
--- a/solver/codeGen/TailoredSolver/interface/TailoredSolver_py.py
+++ b/solver/codeGen/TailoredSolver/interface/TailoredSolver_py.py
@@ -22,17 +22,17 @@
 
    OUTPUT = TailoredSolver_py.TailoredSolver_solve(PARAMS) solves a multistage problem
    subject to the parameters supplied in the following dictionary:
-       PARAMS['lb'] - column vector of length 160
-       PARAMS['ub'] - column vector of length 140
-       PARAMS['hu'] - column vector of length 40
+       PARAMS['lb'] - column vector of length 320
+       PARAMS['ub'] - column vector of length 280
+       PARAMS['hu'] - column vector of length 80
        PARAMS['xinit'] - column vector of length 7
-       PARAMS['x0'] - column vector of length 160
-       PARAMS['all_parameters'] - column vector of length 500
+       PARAMS['x0'] - column vector of length 320
+       PARAMS['all_parameters'] - column vector of length 1000
        PARAMS['num_of_threads'] - scalar
 
    OUTPUT returns the values of the last iteration of the solver where
-       OUTPUT['U'] - column vector of size 80
-       OUTPUT['X'] - column vector of size 80
+       OUTPUT['U'] - column vector of size 160
+       OUTPUT['X'] - column vector of size 160
 
    [OUTPUT, EXITFLAG] = TailoredSolver_py.TailoredSolver_solve(PARAMS) returns additionally
    the integer EXITFLAG indicating the state of the solution with 
@@ -88,12 +88,12 @@ class TailoredSolver_params_ctypes(ctypes.Structure):
 #    @classmethod
 #    def from_param(self):
 #        return self
-    _fields_ = [('lb', ctypes.c_double * 160),
-('ub', ctypes.c_double * 140),
-('hu', ctypes.c_double * 40),
+    _fields_ = [('lb', ctypes.c_double * 320),
+('ub', ctypes.c_double * 280),
+('hu', ctypes.c_double * 80),
 ('xinit', ctypes.c_double * 7),
-('x0', ctypes.c_double * 160),
-('all_parameters', ctypes.c_double * 500),
+('x0', ctypes.c_double * 320),
+('all_parameters', ctypes.c_double * 1000),
 ('num_of_threads', ctypes.c_uint),
 ]
 
@@ -126,8 +126,8 @@ class TailoredSolver_outputs_ctypes(ctypes.Structure):
 #    @classmethod
 #    def from_param(self):
 #        return self
-    _fields_ = [('U', ctypes.c_double * 80),
-('X', ctypes.c_double * 80),
+    _fields_ = [('U', ctypes.c_double * 160),
+('X', ctypes.c_double * 160),
 ]
 
 TailoredSolver_outputs = {'U' : np.array([]),
@@ -185,17 +185,17 @@ def TailoredSolver_solve(params_arg):
 
    OUTPUT = TailoredSolver_py.TailoredSolver_solve(PARAMS) solves a multistage problem
    subject to the parameters supplied in the following dictionary:
-       PARAMS['lb'] - column vector of length 160
-       PARAMS['ub'] - column vector of length 140
-       PARAMS['hu'] - column vector of length 40
+       PARAMS['lb'] - column vector of length 320
+       PARAMS['ub'] - column vector of length 280
+       PARAMS['hu'] - column vector of length 80
        PARAMS['xinit'] - column vector of length 7
-       PARAMS['x0'] - column vector of length 160
-       PARAMS['all_parameters'] - column vector of length 500
+       PARAMS['x0'] - column vector of length 320
+       PARAMS['all_parameters'] - column vector of length 1000
        PARAMS['num_of_threads'] - scalar
 
    OUTPUT returns the values of the last iteration of the solver where
-       OUTPUT['U'] - column vector of size 80
-       OUTPUT['X'] - column vector of size 80
+       OUTPUT['U'] - column vector of size 160
+       OUTPUT['X'] - column vector of size 160
 
    [OUTPUT, EXITFLAG] = TailoredSolver_py.TailoredSolver_solve(PARAMS) returns additionally
    the integer EXITFLAG indicating the state of the solution with 
diff --git a/solver/codeGen/TailoredSolver/interface/TailoredSolver_simulinkBlock.c b/solver/codeGen/TailoredSolver/interface/TailoredSolver_simulinkBlock.c
index 47fdddd..f2ad26c 100644
--- a/solver/codeGen/TailoredSolver/interface/TailoredSolver_simulinkBlock.c
+++ b/solver/codeGen/TailoredSolver/interface/TailoredSolver_simulinkBlock.c
@@ -75,21 +75,21 @@ static void mdlInitializeSizes(SimStruct *S)
     if (!ssSetNumInputPorts(S, 7)) return;
     	
 	/* Input Port 0 */
-    ssSetInputPortMatrixDimensions(S,  0, 160, 1);
+    ssSetInputPortMatrixDimensions(S,  0, 320, 1);
     ssSetInputPortDataType(S, 0, SS_DOUBLE);
     ssSetInputPortComplexSignal(S, 0, COMPLEX_NO); /* no complex signals suppported */
     ssSetInputPortDirectFeedThrough(S, 0, 1); /* Feedthrough enabled */
     ssSetInputPortRequiredContiguous(S, 0, 1); /*direct input signal access*/
 	
 	/* Input Port 1 */
-    ssSetInputPortMatrixDimensions(S,  1, 140, 1);
+    ssSetInputPortMatrixDimensions(S,  1, 280, 1);
     ssSetInputPortDataType(S, 1, SS_DOUBLE);
     ssSetInputPortComplexSignal(S, 1, COMPLEX_NO); /* no complex signals suppported */
     ssSetInputPortDirectFeedThrough(S, 1, 1); /* Feedthrough enabled */
     ssSetInputPortRequiredContiguous(S, 1, 1); /*direct input signal access*/
 	
 	/* Input Port 2 */
-    ssSetInputPortMatrixDimensions(S,  2, 40, 1);
+    ssSetInputPortMatrixDimensions(S,  2, 80, 1);
     ssSetInputPortDataType(S, 2, SS_DOUBLE);
     ssSetInputPortComplexSignal(S, 2, COMPLEX_NO); /* no complex signals suppported */
     ssSetInputPortDirectFeedThrough(S, 2, 1); /* Feedthrough enabled */
@@ -103,14 +103,14 @@ static void mdlInitializeSizes(SimStruct *S)
     ssSetInputPortRequiredContiguous(S, 3, 1); /*direct input signal access*/
 	
 	/* Input Port 4 */
-    ssSetInputPortMatrixDimensions(S,  4, 160, 1);
+    ssSetInputPortMatrixDimensions(S,  4, 320, 1);
     ssSetInputPortDataType(S, 4, SS_DOUBLE);
     ssSetInputPortComplexSignal(S, 4, COMPLEX_NO); /* no complex signals suppported */
     ssSetInputPortDirectFeedThrough(S, 4, 1); /* Feedthrough enabled */
     ssSetInputPortRequiredContiguous(S, 4, 1); /*direct input signal access*/
 	
 	/* Input Port 5 */
-    ssSetInputPortMatrixDimensions(S,  5, 500, 1);
+    ssSetInputPortMatrixDimensions(S,  5, 1000, 1);
     ssSetInputPortDataType(S, 5, SS_DOUBLE);
     ssSetInputPortComplexSignal(S, 5, COMPLEX_NO); /* no complex signals suppported */
     ssSetInputPortDirectFeedThrough(S, 5, 1); /* Feedthrough enabled */
@@ -129,12 +129,12 @@ static void mdlInitializeSizes(SimStruct *S)
     if (!ssSetNumOutputPorts(S, 2)) return;    
 		
 	/* Output Port 0 */
-    ssSetOutputPortMatrixDimensions(S,  0, 80, 1);
+    ssSetOutputPortMatrixDimensions(S,  0, 160, 1);
     ssSetOutputPortDataType(S, 0, SS_DOUBLE);
     ssSetOutputPortComplexSignal(S, 0, COMPLEX_NO); /* no complex signals suppported */
 	
 	/* Output Port 1 */
-    ssSetOutputPortMatrixDimensions(S,  1, 80, 1);
+    ssSetOutputPortMatrixDimensions(S,  1, 160, 1);
     ssSetOutputPortDataType(S, 1, SS_DOUBLE);
     ssSetOutputPortComplexSignal(S, 1, COMPLEX_NO); /* no complex signals suppported */
 
@@ -252,17 +252,17 @@ static void mdlOutputs(SimStruct *S, int_T tid)
 	
 
 	/* Copy inputs */
-	for( i=0; i<160; i++)
+	for( i=0; i<320; i++)
 	{ 
 		params.lb[i] = (double) lb[i]; 
 	}
 
-	for( i=0; i<140; i++)
+	for( i=0; i<280; i++)
 	{ 
 		params.ub[i] = (double) ub[i]; 
 	}
 
-	for( i=0; i<40; i++)
+	for( i=0; i<80; i++)
 	{ 
 		params.hu[i] = (double) hu[i]; 
 	}
@@ -272,12 +272,12 @@ static void mdlOutputs(SimStruct *S, int_T tid)
 		params.xinit[i] = (double) xinit[i]; 
 	}
 
-	for( i=0; i<160; i++)
+	for( i=0; i<320; i++)
 	{ 
 		params.x0[i] = (double) x0[i]; 
 	}
 
-	for( i=0; i<500; i++)
+	for( i=0; i<1000; i++)
 	{ 
 		params.all_parameters[i] = (double) all_parameters[i]; 
 	}
@@ -319,12 +319,12 @@ static void mdlOutputs(SimStruct *S, int_T tid)
 	
 
 	/* Copy outputs */
-	for( i=0; i<80; i++)
+	for( i=0; i<160; i++)
 	{ 
 		U[i] = (real_T) output.U[i]; 
 	}
 
-	for( i=0; i<80; i++)
+	for( i=0; i<160; i++)
 	{ 
 		X[i] = (real_T) output.X[i]; 
 	}
diff --git a/solver/codeGen/TailoredSolver/interface/TailoredSolver_simulinkBlockcompact.c b/solver/codeGen/TailoredSolver/interface/TailoredSolver_simulinkBlockcompact.c
index 509e560..6b3a19a 100644
--- a/solver/codeGen/TailoredSolver/interface/TailoredSolver_simulinkBlockcompact.c
+++ b/solver/codeGen/TailoredSolver/interface/TailoredSolver_simulinkBlockcompact.c
@@ -75,21 +75,21 @@ static void mdlInitializeSizes(SimStruct *S)
     if (!ssSetNumInputPorts(S, 7)) return;
     	
 	/* Input Port 0 */
-    ssSetInputPortMatrixDimensions(S,  0, 160, 1);
+    ssSetInputPortMatrixDimensions(S,  0, 320, 1);
     ssSetInputPortDataType(S, 0, SS_DOUBLE);
     ssSetInputPortComplexSignal(S, 0, COMPLEX_NO); /* no complex signals suppported */
     ssSetInputPortDirectFeedThrough(S, 0, 1); /* Feedthrough enabled */
     ssSetInputPortRequiredContiguous(S, 0, 1); /*direct input signal access*/
 	
 	/* Input Port 1 */
-    ssSetInputPortMatrixDimensions(S,  1, 140, 1);
+    ssSetInputPortMatrixDimensions(S,  1, 280, 1);
     ssSetInputPortDataType(S, 1, SS_DOUBLE);
     ssSetInputPortComplexSignal(S, 1, COMPLEX_NO); /* no complex signals suppported */
     ssSetInputPortDirectFeedThrough(S, 1, 1); /* Feedthrough enabled */
     ssSetInputPortRequiredContiguous(S, 1, 1); /*direct input signal access*/
 	
 	/* Input Port 2 */
-    ssSetInputPortMatrixDimensions(S,  2, 40, 1);
+    ssSetInputPortMatrixDimensions(S,  2, 80, 1);
     ssSetInputPortDataType(S, 2, SS_DOUBLE);
     ssSetInputPortComplexSignal(S, 2, COMPLEX_NO); /* no complex signals suppported */
     ssSetInputPortDirectFeedThrough(S, 2, 1); /* Feedthrough enabled */
@@ -103,14 +103,14 @@ static void mdlInitializeSizes(SimStruct *S)
     ssSetInputPortRequiredContiguous(S, 3, 1); /*direct input signal access*/
 	
 	/* Input Port 4 */
-    ssSetInputPortMatrixDimensions(S,  4, 160, 1);
+    ssSetInputPortMatrixDimensions(S,  4, 320, 1);
     ssSetInputPortDataType(S, 4, SS_DOUBLE);
     ssSetInputPortComplexSignal(S, 4, COMPLEX_NO); /* no complex signals suppported */
     ssSetInputPortDirectFeedThrough(S, 4, 1); /* Feedthrough enabled */
     ssSetInputPortRequiredContiguous(S, 4, 1); /*direct input signal access*/
 	
 	/* Input Port 5 */
-    ssSetInputPortMatrixDimensions(S,  5, 500, 1);
+    ssSetInputPortMatrixDimensions(S,  5, 1000, 1);
     ssSetInputPortDataType(S, 5, SS_DOUBLE);
     ssSetInputPortComplexSignal(S, 5, COMPLEX_NO); /* no complex signals suppported */
     ssSetInputPortDirectFeedThrough(S, 5, 1); /* Feedthrough enabled */
@@ -129,7 +129,7 @@ static void mdlInitializeSizes(SimStruct *S)
     if (!ssSetNumOutputPorts(S, 1)) return;    
 		
 	/* Output Port 0 */
-    ssSetOutputPortMatrixDimensions(S,  0, 160, 1);
+    ssSetOutputPortMatrixDimensions(S,  0, 320, 1);
     ssSetOutputPortDataType(S, 0, SS_DOUBLE);
     ssSetOutputPortComplexSignal(S, 0, COMPLEX_NO); /* no complex signals suppported */
 
@@ -246,17 +246,17 @@ static void mdlOutputs(SimStruct *S, int_T tid)
 	
 
 	/* Copy inputs */
-	for( i=0; i<160; i++)
+	for( i=0; i<320; i++)
 	{ 
 		params.lb[i] = (double) lb[i]; 
 	}
 
-	for( i=0; i<140; i++)
+	for( i=0; i<280; i++)
 	{ 
 		params.ub[i] = (double) ub[i]; 
 	}
 
-	for( i=0; i<40; i++)
+	for( i=0; i<80; i++)
 	{ 
 		params.hu[i] = (double) hu[i]; 
 	}
@@ -266,12 +266,12 @@ static void mdlOutputs(SimStruct *S, int_T tid)
 		params.xinit[i] = (double) xinit[i]; 
 	}
 
-	for( i=0; i<160; i++)
+	for( i=0; i<320; i++)
 	{ 
 		params.x0[i] = (double) x0[i]; 
 	}
 
-	for( i=0; i<500; i++)
+	for( i=0; i<1000; i++)
 	{ 
 		params.all_parameters[i] = (double) all_parameters[i]; 
 	}
@@ -313,13 +313,13 @@ static void mdlOutputs(SimStruct *S, int_T tid)
 	
 
 	/* Copy outputs */
-	for( i=0; i<80; i++)
+	for( i=0; i<160; i++)
 	{ 
 		outputs[i] = (real_T) output.U[i]; 
 	}
 
-	k=80; 
-	for( i=0; i<80; i++)
+	k=160; 
+	for( i=0; i<160; i++)
 	{ 
 		outputs[k++] = (real_T) output.X[i]; 
 	}
diff --git a/solver/codeGen/TailoredSolver/interface/TailoredSolvercompact_lib.mdl b/solver/codeGen/TailoredSolver/interface/TailoredSolvercompact_lib.mdl
index 4a54132..9f7a00c 100644
--- a/solver/codeGen/TailoredSolver/interface/TailoredSolvercompact_lib.mdl
+++ b/solver/codeGen/TailoredSolver/interface/TailoredSolvercompact_lib.mdl
@@ -60,13 +60,13 @@ Model {
       }
     }
   }
-  Created		  "11/27/2022 12:38:04 PM"
+  Created		  "12/5/2022 8:27:17 PM"
   Creator		  "embotech AG"
   UpdateHistory		  "UpdateHistoryNever"
   ModifiedByFormat	  "%<Auto>"
   LastModifiedBy	  "embotech AG"
   ModifiedDateFormat	  "%<Auto>"
-  LastModifiedDate	  "11/27/2022 12:38:04 PM"
+  LastModifiedDate	  "12/5/2022 8:27:17 PM"
   RTWModifiedTimeStamp	  315310195
   ModelVersionFormat	  "1.%<AutoIncrement:2>"
   ConfigurationManager	  "None"
@@ -801,16 +801,16 @@ Model {
 	Help			"TailoredSolver_simulinkBlockcompact provides an easy Simulink interface for simulating your customized solver.  \n"
 	"\nOUTPUTS = TailoredSolver(INPUTS) solves an optimization problem where:\n\n"
 	"INPUTS:\n"
-	" - lb - column vector of length 160\n "
-" - ub - column vector of length 140\n "
-" - hu - column vector of length 40\n "
+	" - lb - column vector of length 320\n "
+" - ub - column vector of length 280\n "
+" - hu - column vector of length 80\n "
 " - xinit - column vector of length 7\n "
-" - x0 - column vector of length 160\n "
-" - all_parameters - column vector of length 500\n "
+" - x0 - column vector of length 320\n "
+" - all_parameters - column vector of length 1000\n "
 " - num_of_threads - column vector of length 1\n "
 
 	"\nOUTPUTS:\n"
-	" - outputs - column vector of length 160\n "
+	" - outputs - column vector of length 320\n "
 
 	"\n For more information, see https://forces.embotech.com/Documentation/graphical_interface/index.html \n "
 
diff --git a/solver/codeGen/TailoredSolver/interface/definitions.py b/solver/codeGen/TailoredSolver/interface/definitions.py
index 24fca8e..d8aa95d 100644
--- a/solver/codeGen/TailoredSolver/interface/definitions.py
+++ b/solver/codeGen/TailoredSolver/interface/definitions.py
@@ -6,22 +6,22 @@
 lib = "lib/libTailoredSolver.so"
 lib_static = "lib/libTailoredSolver.a"
 c_header = "include/TailoredSolver.h"
-nstages = 20
+nstages = 40
 
 # Parameter             | Type    | Scalar type      | Ctypes type    | Numpy type   | Shape     | Len
 params = \
-[("lb"                  , "dense" , ""               , ctypes.c_double, numpy.float64, (160,   1),  160),
- ("ub"                  , "dense" , ""               , ctypes.c_double, numpy.float64, (140,   1),  140),
- ("hu"                  , "dense" , ""               , ctypes.c_double, numpy.float64, ( 40,   1),   40),
+[("lb"                  , "dense" , ""               , ctypes.c_double, numpy.float64, (320,   1),  320),
+ ("ub"                  , "dense" , ""               , ctypes.c_double, numpy.float64, (280,   1),  280),
+ ("hu"                  , "dense" , ""               , ctypes.c_double, numpy.float64, ( 80,   1),   80),
  ("xinit"               , "dense" , ""               , ctypes.c_double, numpy.float64, (  7,   1),    7),
- ("x0"                  , "dense" , ""               , ctypes.c_double, numpy.float64, (160,   1),  160),
- ("all_parameters"      , "dense" , ""               , ctypes.c_double, numpy.float64, (500,   1),  500),
+ ("x0"                  , "dense" , ""               , ctypes.c_double, numpy.float64, (320,   1),  320),
+ ("all_parameters"      , "dense" , ""               , ctypes.c_double, numpy.float64, (1000,   1), 1000),
  ("num_of_threads"      , ""      , "solver_int32_unsigned", ctypes.c_uint  , numpy.uint32 , (  0,   1),    1)]
 
 # Output                | Type    | Scalar type      | Ctypes type    | Numpy type   | Shape     | Len
 outputs = \
-[("U"                   , ""      , ""               , ctypes.c_double, numpy.float64,     (  4,),   80),
- ("X"                   , ""      , ""               , ctypes.c_double, numpy.float64,     (  4,),   80)]
+[("U"                   , ""      , ""               , ctypes.c_double, numpy.float64,     (  4,),  160),
+ ("X"                   , ""      , ""               , ctypes.c_double, numpy.float64,     (  4,),  160)]
 
 # Info Struct Fields
 info = \
@@ -69,5 +69,25 @@
 	(8, 5, 2, 25, 8, 7, 0, 2), 
 	(8, 5, 2, 25, 8, 7, 0, 2), 
 	(8, 5, 2, 25, 8, 7, 0, 2), 
+	(8, 5, 2, 25, 8, 7, 0, 2), 
+	(8, 5, 2, 25, 8, 7, 0, 2), 
+	(8, 5, 2, 25, 8, 7, 0, 2), 
+	(8, 5, 2, 25, 8, 7, 0, 2), 
+	(8, 5, 2, 25, 8, 7, 0, 2), 
+	(8, 5, 2, 25, 8, 7, 0, 2), 
+	(8, 5, 2, 25, 8, 7, 0, 2), 
+	(8, 5, 2, 25, 8, 7, 0, 2), 
+	(8, 5, 2, 25, 8, 7, 0, 2), 
+	(8, 5, 2, 25, 8, 7, 0, 2), 
+	(8, 5, 2, 25, 8, 7, 0, 2), 
+	(8, 5, 2, 25, 8, 7, 0, 2), 
+	(8, 5, 2, 25, 8, 7, 0, 2), 
+	(8, 5, 2, 25, 8, 7, 0, 2), 
+	(8, 5, 2, 25, 8, 7, 0, 2), 
+	(8, 5, 2, 25, 8, 7, 0, 2), 
+	(8, 5, 2, 25, 8, 7, 0, 2), 
+	(8, 5, 2, 25, 8, 7, 0, 2), 
+	(8, 5, 2, 25, 8, 7, 0, 2), 
+	(8, 5, 2, 25, 8, 7, 0, 2), 
 	(8, 5, 2, 25, 8, 7, 0, 2)
 ]
\ No newline at end of file
diff --git a/solver/codeGen/TailoredSolver/lib/libTailoredSolver.a b/solver/codeGen/TailoredSolver/lib/libTailoredSolver.a
index 8b74f9e..9e5fb40 100644
Binary files a/solver/codeGen/TailoredSolver/lib/libTailoredSolver.a and b/solver/codeGen/TailoredSolver/lib/libTailoredSolver.a differ
diff --git a/solver/codeGen/TailoredSolver/lib/libTailoredSolver.so b/solver/codeGen/TailoredSolver/lib/libTailoredSolver.so
index a30881f..7de8eaa 100644
Binary files a/solver/codeGen/TailoredSolver/lib/libTailoredSolver.so and b/solver/codeGen/TailoredSolver/lib/libTailoredSolver.so differ
diff --git a/solver/codeGen/TailoredSolver/lib/libTailoredSolver_withModel.so b/solver/codeGen/TailoredSolver/lib/libTailoredSolver_withModel.so
index 4d29615..d4f5c2b 100755
Binary files a/solver/codeGen/TailoredSolver/lib/libTailoredSolver_withModel.so and b/solver/codeGen/TailoredSolver/lib/libTailoredSolver_withModel.so differ
diff --git a/solver/codeGen/TailoredSolver/lib_target/libTailoredSolver.a b/solver/codeGen/TailoredSolver/lib_target/libTailoredSolver.a
index 271d4ed..4a944fe 100644
Binary files a/solver/codeGen/TailoredSolver/lib_target/libTailoredSolver.a and b/solver/codeGen/TailoredSolver/lib_target/libTailoredSolver.a differ
diff --git a/solver/codeGen/TailoredSolver/lib_target/libTailoredSolver.so b/solver/codeGen/TailoredSolver/lib_target/libTailoredSolver.so
index 7c0cf98..bdc3e62 100755
Binary files a/solver/codeGen/TailoredSolver/lib_target/libTailoredSolver.so and b/solver/codeGen/TailoredSolver/lib_target/libTailoredSolver.so differ
diff --git a/solver/codeGen/TailoredSolver_adtool2forces.c b/solver/codeGen/TailoredSolver_adtool2forces.c
index a2037cb..b7b9d56 100644
--- a/solver/codeGen/TailoredSolver_adtool2forces.c
+++ b/solver/codeGen/TailoredSolver_adtool2forces.c
@@ -82,7 +82,7 @@ extern solver_int32_default TailoredSolver_adtool2forces(TailoredSolver_float *x
 	in[2] = l;
 	in[3] = y;
 
-	if ((stage >= 0) && (stage < 19))
+	if ((stage >= 0) && (stage < 39))
 	{
 		out[0] = &this_f;
 		out[1] = nabla_f_sparse;
@@ -140,38 +140,38 @@ extern solver_int32_default TailoredSolver_adtool2forces(TailoredSolver_float *x
 
 	}
 
-	if ((stage >= 19) && (stage < 20))
+	if ((stage >= 39) && (stage < 40))
 	{
 		out[0] = &this_f;
 		out[1] = nabla_f_sparse;
-		TailoredSolver_objective_20(in, out, NULL, w, 0);
+		TailoredSolver_objective_40(in, out, NULL, w, 0);
 		if (nabla_f != NULL)
 		{
-			nrow = TailoredSolver_objective_20_sparsity_out(1)[0];
-			ncol = TailoredSolver_objective_20_sparsity_out(1)[1];
-			colind = TailoredSolver_objective_20_sparsity_out(1) + 2;
-			row = TailoredSolver_objective_20_sparsity_out(1) + 2 + (ncol + 1);
+			nrow = TailoredSolver_objective_40_sparsity_out(1)[0];
+			ncol = TailoredSolver_objective_40_sparsity_out(1)[1];
+			colind = TailoredSolver_objective_40_sparsity_out(1) + 2;
+			row = TailoredSolver_objective_40_sparsity_out(1) + 2 + (ncol + 1);
 			TailoredSolver_sparse2fullcopy(nrow, ncol, colind, row, nabla_f_sparse, nabla_f);
 		}
 
 		out[0] = h_sparse;
 		out[1] = nabla_h_sparse;
-		TailoredSolver_inequalities_20(in, out, NULL, w, 0);
+		TailoredSolver_inequalities_40(in, out, NULL, w, 0);
 		if (h != NULL)
 		{
-			nrow = TailoredSolver_inequalities_20_sparsity_out(0)[0];
-			ncol = TailoredSolver_inequalities_20_sparsity_out(0)[1];
-			colind = TailoredSolver_inequalities_20_sparsity_out(0) + 2;
-			row = TailoredSolver_inequalities_20_sparsity_out(0) + 2 + (ncol + 1);
+			nrow = TailoredSolver_inequalities_40_sparsity_out(0)[0];
+			ncol = TailoredSolver_inequalities_40_sparsity_out(0)[1];
+			colind = TailoredSolver_inequalities_40_sparsity_out(0) + 2;
+			row = TailoredSolver_inequalities_40_sparsity_out(0) + 2 + (ncol + 1);
 			TailoredSolver_sparse2fullcopy(nrow, ncol, colind, row, h_sparse, h);
 		}
 
 		if (nabla_h != NULL)
 		{
-			nrow = TailoredSolver_inequalities_20_sparsity_out(1)[0];
-			ncol = TailoredSolver_inequalities_20_sparsity_out(1)[1];
-			colind = TailoredSolver_inequalities_20_sparsity_out(1) + 2;
-			row = TailoredSolver_inequalities_20_sparsity_out(1) + 2 + (ncol + 1);
+			nrow = TailoredSolver_inequalities_40_sparsity_out(1)[0];
+			ncol = TailoredSolver_inequalities_40_sparsity_out(1)[1];
+			colind = TailoredSolver_inequalities_40_sparsity_out(1) + 2;
+			row = TailoredSolver_inequalities_40_sparsity_out(1) + 2 + (ncol + 1);
 			TailoredSolver_sparse2fullcopy(nrow, ncol, colind, row, nabla_h_sparse, nabla_h);
 		}
 
diff --git a/solver/codeGen/TailoredSolver_adtool2forces.o b/solver/codeGen/TailoredSolver_adtool2forces.o
index 4ab8b46..25eff92 100644
Binary files a/solver/codeGen/TailoredSolver_adtool2forces.o and b/solver/codeGen/TailoredSolver_adtool2forces.o differ
diff --git a/solver/codeGen/TailoredSolver_casadi.c b/solver/codeGen/TailoredSolver_casadi.c
index f16708d..daaf3e6 100644
--- a/solver/codeGen/TailoredSolver_casadi.c
+++ b/solver/codeGen/TailoredSolver_casadi.c
@@ -1674,7 +1674,7 @@ const casadi_int* TailoredSolver_inequalities_1_sparsity_out(casadi_int i) {
     }
 }
 
-/* TailoredSolver_objective_20:(i0[8],i1[25],i2[],i3[])->(o0,o1[1x8,7nz]) */
+/* TailoredSolver_objective_40:(i0[8],i1[25],i2[],i3[])->(o0,o1[1x8,7nz]) */
 static int casadi_f3(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
   casadi_real a0, a1, a10, a11, a12, a13, a14, a15, a16, a17, a18, a19, a2, a20, a21, a22, a23, a24, a25, a3, a4, a5, a6, a7, a8, a9;
     a0=arg[1]? arg[1][0] : 0;
@@ -1778,11 +1778,11 @@ static int casadi_f3(const casadi_real** arg, casadi_real** res, casadi_int* iw,
     return 0;
 }
 
-int TailoredSolver_objective_20(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+int TailoredSolver_objective_40(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
     return casadi_f3(arg, res, iw, w, mem);
 }
 
-const casadi_int* TailoredSolver_objective_20_sparsity_out(casadi_int i) {
+const casadi_int* TailoredSolver_objective_40_sparsity_out(casadi_int i) {
     switch (i) {
         case 0: return casadi_s3;
         case 1: return casadi_s4;
@@ -1790,7 +1790,7 @@ const casadi_int* TailoredSolver_objective_20_sparsity_out(casadi_int i) {
     }
 }
 
-/* TailoredSolver_inequalities_20:(i0[8],i1[25],i2[],i3[])->(o0[2],o1[2x8,6nz]) */
+/* TailoredSolver_inequalities_40:(i0[8],i1[25],i2[],i3[])->(o0[2],o1[2x8,6nz]) */
 static int casadi_f4(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
   casadi_real a0, a1, a2, a3, a4, a5, a6, a7;
     a0=arg[0]? arg[0][4] : 0;
@@ -1842,11 +1842,11 @@ static int casadi_f4(const casadi_real** arg, casadi_real** res, casadi_int* iw,
     return 0;
 }
 
-int TailoredSolver_inequalities_20(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+int TailoredSolver_inequalities_40(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
     return casadi_f4(arg, res, iw, w, mem);
 }
 
-const casadi_int* TailoredSolver_inequalities_20_sparsity_out(casadi_int i) {
+const casadi_int* TailoredSolver_inequalities_40_sparsity_out(casadi_int i) {
     switch (i) {
         case 0: return casadi_s7;
         case 1: return casadi_s8;
diff --git a/solver/codeGen/TailoredSolver_casadi.h b/solver/codeGen/TailoredSolver_casadi.h
index 525773a..a3cd582 100644
--- a/solver/codeGen/TailoredSolver_casadi.h
+++ b/solver/codeGen/TailoredSolver_casadi.h
@@ -34,17 +34,17 @@ void TailoredSolver_inequalities_1_incref(void);
 
 const casadi_int* TailoredSolver_inequalities_1_sparsity_out(casadi_int i);
 
-int TailoredSolver_objective_20(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem);
+int TailoredSolver_objective_40(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem);
 
-void TailoredSolver_objective_20_incref(void);
+void TailoredSolver_objective_40_incref(void);
 
-const casadi_int* TailoredSolver_objective_20_sparsity_out(casadi_int i);
+const casadi_int* TailoredSolver_objective_40_sparsity_out(casadi_int i);
 
-int TailoredSolver_inequalities_20(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem);
+int TailoredSolver_inequalities_40(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem);
 
-void TailoredSolver_inequalities_20_incref(void);
+void TailoredSolver_inequalities_40_incref(void);
 
-const casadi_int* TailoredSolver_inequalities_20_sparsity_out(casadi_int i);
+const casadi_int* TailoredSolver_inequalities_40_sparsity_out(casadi_int i);
 
 #ifdef __cplusplus
 } /* extern "C" */
diff --git a/solver/codeGen/TailoredSolver_casadi.o b/solver/codeGen/TailoredSolver_casadi.o
index bf30052..ff445d7 100644
Binary files a/solver/codeGen/TailoredSolver_casadi.o and b/solver/codeGen/TailoredSolver_casadi.o differ
diff --git a/solver/codeGen/TailoredSolver_py.py b/solver/codeGen/TailoredSolver_py.py
index 0b657ec..d0ada42 100644
--- a/solver/codeGen/TailoredSolver_py.py
+++ b/solver/codeGen/TailoredSolver_py.py
@@ -22,17 +22,17 @@
 
    OUTPUT = TailoredSolver_py.TailoredSolver_solve(PARAMS) solves a multistage problem
    subject to the parameters supplied in the following dictionary:
-       PARAMS['lb'] - column vector of length 160
-       PARAMS['ub'] - column vector of length 140
-       PARAMS['hu'] - column vector of length 40
+       PARAMS['lb'] - column vector of length 320
+       PARAMS['ub'] - column vector of length 280
+       PARAMS['hu'] - column vector of length 80
        PARAMS['xinit'] - column vector of length 7
-       PARAMS['x0'] - column vector of length 160
-       PARAMS['all_parameters'] - column vector of length 500
+       PARAMS['x0'] - column vector of length 320
+       PARAMS['all_parameters'] - column vector of length 1000
        PARAMS['num_of_threads'] - scalar
 
    OUTPUT returns the values of the last iteration of the solver where
-       OUTPUT['U'] - column vector of size 80
-       OUTPUT['X'] - column vector of size 80
+       OUTPUT['U'] - column vector of size 160
+       OUTPUT['X'] - column vector of size 160
 
    [OUTPUT, EXITFLAG] = TailoredSolver_py.TailoredSolver_solve(PARAMS) returns additionally
    the integer EXITFLAG indicating the state of the solution with 
@@ -88,12 +88,12 @@ class TailoredSolver_params_ctypes(ctypes.Structure):
 #    @classmethod
 #    def from_param(self):
 #        return self
-    _fields_ = [('lb', ctypes.c_double * 160),
-('ub', ctypes.c_double * 140),
-('hu', ctypes.c_double * 40),
+    _fields_ = [('lb', ctypes.c_double * 320),
+('ub', ctypes.c_double * 280),
+('hu', ctypes.c_double * 80),
 ('xinit', ctypes.c_double * 7),
-('x0', ctypes.c_double * 160),
-('all_parameters', ctypes.c_double * 500),
+('x0', ctypes.c_double * 320),
+('all_parameters', ctypes.c_double * 1000),
 ('num_of_threads', ctypes.c_uint),
 ]
 
@@ -126,8 +126,8 @@ class TailoredSolver_outputs_ctypes(ctypes.Structure):
 #    @classmethod
 #    def from_param(self):
 #        return self
-    _fields_ = [('U', ctypes.c_double * 80),
-('X', ctypes.c_double * 80),
+    _fields_ = [('U', ctypes.c_double * 160),
+('X', ctypes.c_double * 160),
 ]
 
 TailoredSolver_outputs = {'U' : np.array([]),
@@ -185,17 +185,17 @@ def TailoredSolver_solve(params_arg):
 
    OUTPUT = TailoredSolver_py.TailoredSolver_solve(PARAMS) solves a multistage problem
    subject to the parameters supplied in the following dictionary:
-       PARAMS['lb'] - column vector of length 160
-       PARAMS['ub'] - column vector of length 140
-       PARAMS['hu'] - column vector of length 40
+       PARAMS['lb'] - column vector of length 320
+       PARAMS['ub'] - column vector of length 280
+       PARAMS['hu'] - column vector of length 80
        PARAMS['xinit'] - column vector of length 7
-       PARAMS['x0'] - column vector of length 160
-       PARAMS['all_parameters'] - column vector of length 500
+       PARAMS['x0'] - column vector of length 320
+       PARAMS['all_parameters'] - column vector of length 1000
        PARAMS['num_of_threads'] - scalar
 
    OUTPUT returns the values of the last iteration of the solver where
-       OUTPUT['U'] - column vector of size 80
-       OUTPUT['X'] - column vector of size 80
+       OUTPUT['U'] - column vector of size 160
+       OUTPUT['X'] - column vector of size 160
 
    [OUTPUT, EXITFLAG] = TailoredSolver_py.TailoredSolver_solve(PARAMS) returns additionally
    the integer EXITFLAG indicating the state of the solution with 
diff --git a/src/mpc.cpp b/src/mpc.cpp
index a2fd3cd..1683068 100644
--- a/src/mpc.cpp
+++ b/src/mpc.cpp
@@ -14,11 +14,9 @@ MPC::MPC(const Params* params){
     this->rk4_t         = params->mpc.rk4_t;
     this->nPlanning     = params->mpc.nPlanning;
     this->Nthreads      = params->mpc.Nthreads;
-    this->troProfile    = params->mpc.TroProfile;
 
     cout << "Hz: " << Hz << endl;
     cout << "rk4_t: " << rk4_t << endl;
-    cout << "troProfile: " << troProfile << endl;
 
     // Vehicle params
     this->m         = params->vehicle.m;
@@ -340,7 +338,7 @@ void MPC::set_params_bounds(){
         this->forces.params.all_parameters[23 + k*this->Npar] = pred_velocities(plannerIdx);
         this->forces.params.all_parameters[24 + k*this->Npar] = planner(plannerIdx, 3); // curvature 
         // cout << "Curvature: " << forces.params.all_parameters[24+k*Npar] << endl;
-        cout << "pred velocity: " << pred_velocities(plannerIdx) << endl;
+        // cout << "pred velocity: " << pred_velocities(plannerIdx) << endl;
 
         // Inequality constraints bounds:
         this->forces.params.hu[k*nh]     = fabs(planner(plannerIdx, 5)); // L(s) ortogonal left dist from the path to the track limits
diff --git a/src/utils/params.cpp b/src/utils/params.cpp
index b166b8a..15ff6d3 100644
--- a/src/utils/params.cpp
+++ b/src/utils/params.cpp
@@ -36,6 +36,5 @@ Params::Params(const ros::NodeHandle* nh) {
     nh->param<int>(("MPC/nPlanning"), mpc.nPlanning, 1900);
     nh->param<int>(("MPC/Hz"), mpc.Hz, 20);
     nh->param<int>(("MPC/Nthreads"), mpc.Nthreads, 2);
-    nh->param<bool>(("MPC/TroProfile"), mpc.TroProfile, false);
 
 }
\ No newline at end of file