Index refactor (#26)

* remove ind_total

* WIP: cleaning up index handling via function

* WIP fixed some syntax issues and duplicate X_ki

* fixing prints

* fixing more typos

* optional index parameter

* more typos, and remaining problem variables

* quieter prints and remove virtual forces indexes

* even quieter prints

* remove index from _end

* Added constraint adding util, rename var

* using add_constraint

* fixing issues

* fix typo, add dims check

* fixing more typos

* dims test

* comments, todos

* more todos

src/matlab/add_constraint.m

@@ -0,0 +1,15 @@
+function [problem] = add_constraint(problem, g, lbg, ubg, idx)
+    %ADD_CONSTRAINT Adds a general constraint with upper and lower bounds to the problem.
+    dims = [length(g), length(lbg), length(ubg)];
+    if(~all(dims == dims(1)))
+        error("dimension mismatch, with dims: g: %d, lbg: %d, ubg: %d", dims(1), dims(2), dims(3));
+    end
+    problem.g = {problem.g{:}, g};
+    problem.lbg = [problem.lbg; lbg];
+    problem.ubg = [problem.ubg; ubg];
+    % This is a bit of a hack, to avoid a bunch of if statements (maybe, currently would only be one)
+    if exist('idx','var')
+        ind_set = problem.(strcat('ind_', idx));
+        problem.(strcat('ind_', idx)) = [ind_set, length(problem.lbg)];
+    end
+end

src/matlab/add_variable.m

@@ -0,0 +1,18 @@
+function [problem] = add_variable(problem, w, w0, lbw, ubw, idx)
+    %ADD_VARIABLE Adds a primal variable with upper and lower bounds and initialization to the problem.
+    dims = [length(w), length(w0), length(lbw), length(ubw)];
+    if(~all(dims == dims(1)))
+        error("dimension mismatch, with dims: w: %d, w0: %d, lbw: %d, ubw: %d", dims(1), dims(2), dims(3), dims(4));
+    end
+    problem.w = {problem.w{:}, w};
+    % This is a bit of a hack, to avoid a bunch of if statements.
+    % TODO Maybe separate these out as a "per variable" cell array like done in python. 
+    if exist('idx','var')
+        ind_set = problem.(strcat('ind_', idx));
+        problem.(strcat('ind_', idx)) = [ind_set, length(problem.w0)+1:length(problem.w0)+length(w)];
+    end
+    problem.w0 = [problem.w0; w0];
+    problem.lbw = [problem.lbw; lbw];
+    problem.ubw = [problem.ubw; ubw];
+end
+