diff --git a/.vscode/launch.json b/.vscode/launch.json
new file mode 100644
index 0000000..b8dfc3d
--- /dev/null
+++ b/.vscode/launch.json
@@ -0,0 +1,28 @@
+{
+    // Use IntelliSense to learn about possible attributes.
+    // Hover to view descriptions of existing attributes.
+    // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
+    "version": "0.2.0",
+    "configurations": [
+        {
+            "name": "(GDb) Launch",
+            "type": "cppdbg",
+            "request": "launch",
+            "program": "${workspaceFolder}/SM-MaponiA3",
+            "args": [],
+            "stopAtEntry": false,
+            "cwd": "${workspaceFolder}",
+            "environment": [],
+            "externalConsole": false,
+            "MIMode": "gdb",
+            "miDebuggerPath": "/opt/intel/compilers_and_libraries/linux/bin/intel64/gdb-ia"
+            "setupCommands": [
+                {
+                    "description": "Enable pretty-printing for gdb",
+                    "text": "-enable-pretty-printing",
+                    "ignoreFailures": true
+                }
+            ]
+        }
+    ]
+}
\ No newline at end of file
diff --git a/.vscode/tasks.json b/.vscode/tasks.json
new file mode 100644
index 0000000..018f1c8
--- /dev/null
+++ b/.vscode/tasks.json
@@ -0,0 +1,27 @@
+{
+    "tasks": [
+        {
+            "type": "cppbuild",
+            "label": "C/C++: clang++ build active file",
+            "command": "/opt/intel/compilers_and_libraries_2020.4.304/linux/bin/clang++",
+            "args": [
+                "-g",
+                "${file}",
+                "-o",
+                "${fileDirname}/${fileBasenameNoExtension}"
+            ],
+            "options": {
+                "cwd": "${workspaceFolder}"
+            },
+            "problemMatcher": [
+                "$gcc"
+            ],
+            "group": {
+                "kind": "build",
+                "isDefault": true
+            },
+            "detail": "Task generated by Debugger."
+        }
+    ],
+    "version": "2.0.0"
+}
\ No newline at end of file
diff --git a/Makefile b/Makefile
index 38bce4c..afaa43f 100644
--- a/Makefile
+++ b/Makefile
@@ -1,6 +1,7 @@
 CC=icc
 CXX=icpc
-CFLAGS=
+CFLAGS=-xCORE-AVX2 -O0 -debug full
+CXXFLAGS=-xCORE-AVX2 -O0 -debug full
 
 DEPS = SM-MaponiA3.cpp
 OBJ = SM-MaponiA3.o 
@@ -10,3 +11,6 @@ OBJ = SM-MaponiA3.o
 
 SM-MaponiA3: $(OBJ)
 	$(CXX) -o $@ $^ $(CFLAGS)
+
+clean:
+	@rm -vf *.o
diff --git a/SMMaponiA4.m b/SMMaponiA4.m
index 59205ed..ee535a5 100644
--- a/SMMaponiA4.m
+++ b/SMMaponiA4.m
@@ -8,15 +8,15 @@ clc ## Clear the screen
 %    1,1,0; ...
 %   -1,0,-1];
 
-## The modified example that gives all singular updates at some point
-A=[1,1,1; ...
-   1,1,0; ...
-  -1,0,-1];
+%## The modified example that gives all singular updates at some point
+%A=[1,1,1; ...
+%   1,1,0; ...
+%  -1,0,-1];
 
-%## A square uniform distributed random integer matrix with entries in [-1,1]  
-%do
-%  A=randi([-5,5],4,4);
-%until (det(A)!=0) ## We neec matrix non-singular
+## A square uniform distributed random integer matrix with entries in [-1,1]  
+do
+  A=randi([-5,5],5,5);
+until (det(A)!=0) ## We need matrix non-singular
 
 % ## A square uniform distributed random float matrix with entries in (0,1)  
 % do
@@ -26,6 +26,8 @@ A=[1,1,1; ...
 
 nCols=columns(A); ## The number of coluns of A (M in accompanying PDF)
 Id=eye(nCols);
+% d=norm(A);
+% d=max(eig(A));
 d=0.1
 A0=d*eye(nCols);
 Ar=A-A0;           ## The remainder of A
@@ -34,6 +36,8 @@ Ainv=zeros(nCols,nCols);
 ylk=zeros(nCols,nCols,nCols);
 breakdown=zeros(nCols,1);
 cutOff=1e-10;
+p=zeros(nCols,1);
+P=zeros(nCols);
 
 ## Calculate the inverse of A0 and populate p-vector
 for i=1:nCols
@@ -59,24 +63,29 @@ endfor
 
 
 ## Calculate all the ylk from the y0k calculated previously
-for l=2:3#nCols
+for l=2:nCols
   el=Id(:,l-1);
   ## Calculate break-down conditions and put in a vector
   for j=l-1:nCols
-    #breakdown(j) = abs( el(j) + ylk(j,l-1,l-1) )
+    %breakdown(j) = abs( el(j) + ylk(j,l-1,l-1) ) ## Condition from Maponi. Probably not correct!
+    l
+    ylk(j,j,l-1);
     breakdown(j) = abs( el(j) + ylk(j,j,l-1) )
     % printf("|el(%d) + ylk(%d,%d,%d)|\n", j, j, l-1, l-1);
   endfor
-  [val, s] = max(breakdown); ## Find the index of the max value
-  printf("l = %d\ns = %d\n",l-1,s);
+  [val, s] = max(breakdown) ## Find the index of the max value
+  % printf("l = %d\ns = %d\n",l-1,s);
   breakdown=zeros(nCols,1); ## Reset the entries to zero for next l-round
   if (s!=l-1) ## Apply partial pivoting
   ## Swap yl-1,k(r) and yl-1,k(s) for all k=l,l+1,...,M
-    r=l-1
+    r=l-1;
+    tmp=p(r);
+    p(r)=p(s);
+    p(s)=tmp;
     for k=l-1:nCols
-      tmp=ylk(r,k,l-1)
+      tmp=ylk(r,k,l-1);
       ylk(r,k,l-1)=ylk(s,k,l-1);
-      printf("ylk(%d,%d,%d)=ylk(%d,%d,%d)\n",r,k,l-1,s,k,l-1);
+      % printf("ylk(%d,%d,%d)=ylk(%d,%d,%d)\n",r,k,l-1,s,k,l-1);
       ylk(s,k,l-1)=tmp;
     endfor
     ## Modify yl-1,r and yl-1,s
@@ -89,7 +98,7 @@ for l=2:3#nCols
   for k=l:nCols
     for i=1:nCols
       ylk(i,k,l) = ylk(i,k,l-1) - ylk(l-1,k,l-1) / (1 + ylk(l-1,l-1,l-1)) * ylk(i,l-1,l-1);
-      printf("ylk(%d,%d,%d) = ylk(%d,%d,%d) - (ylk(%d,%d,%d) / (1+ylk(%d,%d,%d) * (ylk(%d,%d,%d)\n",i,k,l,i,k,l-1,l-1,k,l-1,l-1,l-1,l-1,i,l-1,l-1);
+      % printf("ylk(%d,%d,%d) = ylk(%d,%d,%d) - (ylk(%d,%d,%d) / (1+ylk(%d,%d,%d) * (ylk(%d,%d,%d)\n",i,k,l,i,k,l-1,l-1,k,l-1,l-1,l-1,l-1,i,l-1,l-1);
     endfor
   endfor
 endfor
@@ -100,13 +109,15 @@ endfor
 Ainv=A0inv;
 for l=1:nCols
   Ainv=(Id - ylk(:,l,l) * transpose(Id(:,l)) / (1 + ylk(l,l,l))) * Ainv;
+  P(:,l)=Id(:,p(l)); ## Construct permutation matrix to swap columns of Ainv
   % printf("Ainv=(Id - ylk(:,%d,%d) * transpose(Id(:,%d)) / (1 + ylk(%d,%d,%d))) * Ainv\n",l,l,l,l,l,l);
 endfor
+Ainv=transpose(P*transpose(Ainv)); ## Swap the 
 
 
 
 ## Test if the inverse found is really an inverse (does not work if values are floats)
-IdTest=A*Ainv
+IdTest=A*Ainv;
 if (IdTest==eye(nCols))
    printf("\n");
    printf("Inverse of A^{-1} FOUND!\n");