%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SVD Orbit Correction                                     %
% Similar middle layer commands: setorbit, setorbitdefault %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

num_bpmy = length(getam('BPMy'));	% get the number of vert. BPMs
num_vcm = length(getam('VCM'));	% get the number of vert. correctors

% Put a random orbit error on the horizontal plane
setsp('VCM', 0);          % Start from zero corrector strength

y00 = getam('BPMy');

vcm = .1 * randn(num_vcm,1);  % num_vcm vertical correctors at the ALS
stepsp('VCM', vcm);

Ry = getrespmat('BPMy',getlist('BPMy'),'VCM',getlist('VCM'));         % Get the vertical BPM response matrix

[U,S,V] = svd(Ry, 0);

y0 = getam('BPMy');
vcm0 = getsp('VCM');

figure(1); clf
for n = 1:size(S,2)
    Minv = V(:,1:n) * diag(1./diag(S(1:n,1:n))) * U(:,1:n)';
    
    y = getam('BPMy');
    vcm = Minv * (-1*(y-y00));
    setsp('VCM', vcm0+vcm);     % Set the horizontal correctors

    y1 = getam('BPMy');
    
    plot(1:num_bpmy,y0-y00,1:num_bpmy,y1-y00); 
    axis([1 num_bpmy min(y0-y00) max(y0-y00)]); 
    title(sprintf('Orbit Error for %d Singular Values',n));
    legend('no correction',sprintf('%d singular values',n))
    drawnow;
    
    ystd(n) = std(y1-y00);
    ymax(n) = max(abs(y1-y00));
    vcmsum(n) = sum(abs(vcm));
    vcmtot(n) = sum(abs(vcm+vcm0));
    
    setsp('VCM', vcm0);
end
setsp('VCM', 0);  % Reset the corrector

figure(1);
subplot(3,1,1);
semilogy(diag(S), '.-'); axis tight;
ylabel('Singular Values');

subplot(3,1,2);
plot(1:length(ystd),ystd, '.-',1:length(ymax),ymax, 'x-'); axis tight;
ylabel('Error');
legend('RMS','MAX');

subplot(3,1,3);
plot(1:length(vcmsum),vcmsum, '.-',1:length(vcmtot),vcmtot, 'x-'); axis tight;
ylabel('Total Amps');
xlabel('Number of Singular Values');
legend('correction','correction+initial error');
orient tall


% now with only 1 corrector error
setsp('VCM', 0);          % Start from zero corrector strength

y00 = getam('BPMy');

stepsp('VCM', 1, [7 1]);

Ry = getrespmat('BPMy',getlist('BPMy'),'VCM',getlist('VCM'));         % Get the vertical BPM response matrix

[U,S,V] = svd(Ry, 0);

y0 = getam('BPMy');
vcm0 = getsp('VCM');

figure(2); clf
for n = 1:size(S,2)
    Minv = V(:,1:n) * diag(1./diag(S(1:n,1:n))) * U(:,1:n)';
    
    y = getam('BPMy');
    vcm = Minv * (-1*(y-y00));
    setsp('VCM', vcm0+vcm);     % Set the horizontal correctors

    y1 = getam('BPMy');
    
    plot(1:num_bpmy,y0-y00,1:num_bpmy,y1-y00); 
    axis([1 num_bpmy min(y0-y00) max(y0-y00)]); 
    title(sprintf('Orbit Error for %d Singular Values',n));
    legend('no correction',sprintf('%d singular values',n))
    drawnow;
    
    ystd(n) = std(y1-y00);
    ymax(n) = max(abs(y1-y00));
    vcmsum(n) = sum(abs(vcm));
    vcmtot(n) = sum(abs(vcm+vcm0));
    
    setsp('VCM', vcm0);
end
setsp('VCM', 0);  % Reset the corrector

figure(2);
subplot(3,1,1);
semilogy(diag(S), '.-'); axis tight;
ylabel('Singular Values');

subplot(3,1,2);
plot(1:length(ystd),ystd, '.-',1:length(ymax),ymax, 'x-'); axis tight;
ylabel('Error');
legend('RMS','MAX');

subplot(3,1,3);
plot(1:length(vcmsum),vcmsum, '.-',1:length(vcmtot),vcmtot, 'x-'); axis tight;
ylabel('Total Amps');
xlabel('Number of Singular Values');
legend('correction','correction+initial error');
orient tall