function [Rfil Zfil] = CalculatePlasmaPosition_CF1f_SD(magneticData, bobines)

%% Calculate plasma current
%Ip = 2.0 * pi * bobines.rb / 12 * sum(magneticData, 2);
Ip = CalculatePlasmaCurrent(magneticData, bobines);

%% Plasma position - simple algorithm
[R Z] = CalculatePlasmaPosition_SA(magneticData, bobines);

%% Current filaments method
h_R = 0.0001;
h_Z = 0.0001;
lambda = 0.00003;
Rfil = zeros(size(R, 1), 1);
Zfil = zeros(size(Z, 1), 1);
for i = 1:size(magneticData, 1)

    if abs(Ip(i)) < 500
        if mod(i, 1000) == 0
            disp(['i = ' num2str(i) ', count = 0 -->  (Ip = ' num2str(Ip(i)) ' A)']);
        end
        rChi = R(i);
        zChi = Z(i);
        continue;
    end

%    rChi = R(i);
%    zChi = Z(i);
%    if i == 1
%        rChi = R(i);
%        zChi = Z(i);
        %rChi = -0.01;
        %zChi = 0.00;

%         Chi2_matrix = zeros(171, 171);
%         for m = 1:171
%             for n = 1:171
%                 Hp = hpol_espira(bobines, bobines.RM - 0.085 + (m-1)*0.001, -0.085 + (n-1)*0.001);
%                 Chi2_dif = magneticData(i,:)/Ip(i) - Hp;
%                 Chi2_matrix(n,m) = dot(Chi2_dif, Chi2_dif);
%             end
%         end
%         disp(Chi2_matrix(85:90,85:90));
%         figure;
%         imagesc(Chi2_matrix);

%    end

%    disp(['i = ' num2str(i)]);


    normGradChi2 = +inf;
    count = 0;
    while (normGradChi2 > 0.5) && (count < 1000)
        count = count + 1;
        %disp(['i=' num2str(i) ' -->  (' num2str(Ip(i)) ')  (' num2str(rChi-h_R) ',' num2str(zChi) ')']);
        %disp(['i = ' num2str(i) ' ; normGradChi2 = ' num2str(normGradChi2) '      (' num2str(rChi) ',' num2str(zChi) ')']);
        Hp_R1 = hpol_espira(bobines, bobines.RM + rChi - h_R, zChi);
        Hp_R2 = hpol_espira(bobines, bobines.RM + rChi + h_R, zChi);
        Hp_Z1 = hpol_espira(bobines, bobines.RM + rChi, zChi - h_Z);
        Hp_Z2 = hpol_espira(bobines, bobines.RM + rChi, zChi + h_Z);
        Chi2_R1_dif = magneticData(i,:)/abs(Ip(i)) - Hp_R1;
        Chi2_R2_dif = magneticData(i,:)/abs(Ip(i)) - Hp_R2;
        Chi2_Z1_dif = magneticData(i,:)/abs(Ip(i)) - Hp_Z1;
        Chi2_Z2_dif = magneticData(i,:)/abs(Ip(i)) - Hp_Z2;
        Chi2_R1 = dot(Chi2_R1_dif, Chi2_R1_dif);
        Chi2_R2 = dot(Chi2_R2_dif, Chi2_R2_dif);
        Chi2_Z1 = dot(Chi2_Z1_dif, Chi2_Z1_dif);
        Chi2_Z2 = dot(Chi2_Z2_dif, Chi2_Z2_dif);
        dChi2_R = (Chi2_R2 - Chi2_R1) / (2 * h_R);
        dChi2_Z = (Chi2_Z2 - Chi2_Z1) / (2 * h_Z);
        gradChi2 = [dChi2_R dChi2_Z];
        normGradChi2 = norm(gradChi2, 2);
        %disp(['gradChi2 = ' num2str(gradChi2) '  (norm = ' num2str(normGradChi2) ')' ]);
%         if norm(gradChi2, 2) < 0.01
%             break;
%         end
        g = gradChi2 / normGradChi2;
        %disp(['g = ' num2str(g) '  (norm = ' num2str(norm(g,2)) ')' '      (' num2str(rChi) ',' num2str(zChi) ')' ]);
        rChi = rChi - lambda * g(1);
        zChi = zChi - lambda * g(2);
    end

    Rfil(i) = rChi;
    Zfil(i) = zChi;

    if mod(i, 1000) == 0
        disp(['i = ' num2str(i) ', count = ' num2str(count) ' -->  (Ip = ' num2str(Ip(i)) ' A)  (' num2str(rChi) ',' num2str(zChi) ')']);
    end

end

% figure;
% plot(Ip);

% figure;
% plot(R, 'r');
% hold on;
% plot(Z, 'b');
% 
% figure;
% plot(Rfil, 'r');
% hold on;
% plot(Zfil, 'b');