# R = response matrix
# b = bpm readings
# l = number of desired correctors
# returns : P (selected) correctors 
#           X correction

function [P,X] = MICADO(R,b,l)
  l = min(l,columns(R));
  P = 1:columns(R);
  X = [];
  for j=1:l
    r_norm2_min_inv = 0.0;
    for corrector_index=j:columns(R)
      R_j = R(:, [1:(j-1), corrector_index ]);
      x_j = - R_j \ b;
      r = b - R_j * x_j;
      r_norm2 = sum(r.*r);
      if r_norm2 * r_norm2_min_inv < 1.0
	r_norm2_min_inv = 1.0 / r_norm2;
	best_corrector_index = corrector_index;
        X = x_j;
      end
    end
    if best_corrector_index != j
      dummy = R(:,best_corrector_index);
      R(:,best_corrector_index) = R(:,j);
      R(:,j) = dummy;
      dummy = P(best_corrector_index);
      P(best_corrector_index) = P(j);
      P(j) = dummy;
    end
  end
  P = P(1:l);
endfunction
