function [Xar, A1, A2, xkn, Xkn, xkn1] = LSAR_QRSmodified(X,R,Fs,p,indmin)

% Least square Auto-regressive interpolation algorithm
% X = original signal
% R = heart beat time instants
% Fs = smpling frequency
% p = short term AR filter order

if nargin < 3 | isempty(Fs)
    Fs = 1000;
end

if nargin < 4 | isempty(p)
    p = 10;
end

if nargin < 5 | isempty(indmin)
    indmin = 0.04;
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if indmin == 0.04
%         Interval = 3*Fs;
Interval = round(0.1*Fs);
else Interval = round((0.1+(indmin-0.04))*Fs);
end

% for i = 1:length(R)-1
%     RR(i) = R(i+1)-R(i);
% end

% Interval = floor(min(RR))-floor(indmin*Fs)-1;

for j = 1:length(R)
    q(j) = R(j)-round(indmin*Fs);
end


for i = 2:length(R)-2
    
    % AR model coefficient vector estimate
    clear a A1 A2 b
    clear Xkn xkn xkn1 xUk
%     xkn = X([q(i-1)+Interval+p+1:q(i)-1,q(i)+Interval+p+1:q(i+1)-1])';
    xkn = X([q(i-1)+Interval+p-1:q(i)-1,q(i)+Interval+p-1:q(i+1)-1])';
    for k1 = 1:length(q(i-1)+Interval+p-1:q(i)-1) %k1 = 1:length(q(i-1)+Interval+p+1:q(i)-1)
        
        Xkn(k1,:) = X(q(i-1)+Interval+p+k1-3:-1:q(i-1)+Interval+k1-2);
    end
    for k2 = 1:length(q(i)+Interval+p-1:q(i+1)-1) %k2 = 1:length(q(i)+Interval+p+1:q(i+1)-1)
        
        Xkn(k2+length(q(i-1)+Interval+p-1:q(i)-1),:) = X(q(i)+Interval+p+k2-3:-1:q(i)+Interval+k2-2);
    end

    a = inv(Xkn'*Xkn)*(Xkn'*xkn);
    
    % estimation of the unknown samples
    %A1
    b = [];
    for k3 = 1:Interval+p
        %         k3
        if p < Interval
            if k3 <= p+1
                for k4 = 1:k3-1
                    b(k4) = a(k3-k4);
                end
                A1(k3,:) = [-b 1 zeros(1,Interval-length(b)-1)];
                %             size(A1)
            elseif k3 > p+1 & length(zeros(1,length(p+1:k3-1)))+length(a) < Interval
                for k4 = 1:length(a)
                    b(k4) = a(length(a)-k4+1);
                end
                A1(k3,:) = [zeros(1,length(p+1:k3-1)) -b 1 zeros(1,Interval-length(zeros(1,length(p+1:k3-1)))-length(b)-1)];
                %             size(A1)
            elseif k3 > p+1 & length(zeros(1,length(p+1:k3-1)))+length(a) >= Interval
                clear b
                for k4 = 1:Interval-length(zeros(1,length(p+1:k3-1)))
                    b(k4) = a(length(a)-k4+1);
                end
                A1(k3,:) = [zeros(1,length(p+1:k3-1)) -b];
                %             size(A1)

            end
        elseif p >= Interval
            if k3 <= Interval
                for k4 = 1:k3-1
                    b(k4) = a(k3-k4);
                end
                A1(k3,:) = [-b 1 zeros(1,Interval-length(b)-1)];
            elseif k3 > Interval & k3-1 <= p
                c2 = 1;
                for k4 = k3-1:-1:k3-Interval
                    b(c2) = a(k4);
                    c2 = c2+1;
                end
                A1(k3,:) = [-b];
            elseif k3 > Interval & k3-1 > p
%                 k3
                clear b
                c3 = 1;
                for k4 = p:-1:p-(Interval-(k3-p))
                    b(c3) = a(k4);
                    c3 = c3+1;
                end
%                 size(b)
%                 size(zeros(1,k3-1-p))
                A1(k3,:) = [zeros(1,k3-1-p), -b];
            end
        end

end
    
    %A2
%     clear b
%     b = [];
    clear c1 k3
    c1 = 1;
    for k3 = 1:Interval+p
%         k3
        if k3 <= p
            clear b
            for k4 = 1:length(a)-k3+1
                b(k4) = a(length(a)-k4+1);
            end
            A2(k3,:) = [zeros(1,k3-1) -b zeros(1,Interval+p)];
%             size(A1)
        elseif k3 >p & k3 <= Interval
            A2(k3,:) = [zeros(1,Interval+2*p)];
        elseif k3 > Interval
            clear b
            for k4 = 1:c1-1
                b(k4) = a(c1-k4);
            end
            if c1 == 1
                A2(k3,:) = [zeros(1,Interval+p) 1 zeros(1,p-1)];
            else A2(k3,:) = [zeros(1,Interval+p) -b 1 zeros(1,p-length(b)-1)];
            end
            c1 = c1+1;
        end
    end
    
    xkn1 = [X(q(i)-p:q(i)-1),zeros(1,Interval),X(q(i)+Interval:q(i)+Interval+p-1)]';
    xUk = -inv(A1'*A1)*(A1'*A2)*xkn1;
    
    X1(:,i) = xUk;
    
end


Xar = X;
for i = 2:length(R)-2
    Xar(q(i):q(i)+Interval-1) = X1(:,i);
end

% [Paa,Faa,M,IN,SC1,SC2,SC3,Fprinc] = SC_QRSresidual(Xar,1000);

% plot([X;Xar]')
