%Collect measurements and its index to be processed for SE
%INPUT:  meas_bus   meas_line flag_error line nol
%OUTPUT: m_Pinj m_Qinj m_Pflow m_Qflow m_VM m_VA m_IM m_IA m_Ireal m_Iimag
%        slack_ang
%        each variable m_* has three col: col1: meas_index col2:meas_value 
%        and col3 as variance

m_Pinj=[];m_Qinj=[];
m_VM=[];m_VA=[];

for i=1:length(meas_bus(:,1))
    if meas_bus(i,2)==1 %Pinj
        if flag_error==2 %Uniform PDF
            temp=(meas_bus(i,1)*err_spec(1)/100)^2/3;
        else             %Gaussian PDF
            temp=err_spec(1)^2;
        end
        [m_Pinj]=[m_Pinj;meas_bus(i,3) meas_bus(i,1) temp];
    elseif meas_bus(i,2)==2 %Qinj
        if flag_error==2 %Uniform PDF
            temp=(meas_bus(i,1)*err_spec(1)/100)^2/3;
        else             %Gaussian PDF
            temp=err_spec(1)^2;
        end
        [m_Qinj]=[m_Qinj;meas_bus(i,3) meas_bus(i,1) temp];
    elseif meas_bus(i,2)==3  %VM
        if flag_error==2 %Uniform PDF
            temp=(meas_bus(i,1)*err_spec(3)/100)^2/3;
        else             %Gaussian PDF
            temp=err_spec(3)^2;
        end
        [m_VM]=[m_VM;meas_bus(i,3) meas_bus(i,1) temp];
    elseif meas_bus(i,2)==4  %VA
        if flag_error==2 %Uniform PDF
            temp=(pi*err_spec(5)/180)^2/3;
        else             %Gaussian PDF
            temp=err_spec(4)^2;
        end
        [m_VA]=[m_VA;meas_bus(i,3) meas_bus(i,1) temp];
        if meas_bus(i,3)==slack
            slack_ang=[meas_bus(i,1) temp slack];
        end
    end
end
if isempty(m_VA)==0
    m_VA(:,2)=m_VA(:,2)-slack_ang(1);%Angle value update
    m_VA(:,3)=m_VA(:,3)+slack_ang(2);%Angle variance update
end

%Combine Voltage abs and angle together col1:index col2:V_abs col3:V_ang
% col4:var(V_abs) col5:variance(V_angle)
m_VMA=[];
if isempty(m_VM)==0  
    for i=1:length(m_VM(:,1))
        for j=1:length(m_VA(:,1))
            if m_VA(j,1)==m_VM(i,1)
                [m_VMA]=[m_VMA;m_VM(i,1:2) m_VA(j,2) m_VM(i,3) m_VA(j,3)];
            end
        end
    end
end

m_Pflow=[];m_Qflow=[];
m_IM=[];m_IA=[];m_Ireal=[];m_Iimag=[];

for i=1:length(meas_line(:,1))    
    if meas_line(i,4)==line(meas_line(i,3),1) && meas_line(i,5)==line(meas_line(i,3),2)
        temp_idx=meas_line(i,3);
    elseif meas_line(i,4)==line(meas_line(i,3),2) && meas_line(i,5)==line(meas_line(i,3),1)
        temp_idx=meas_line(i,3)+nol;
    else        
        temp_idx=0;
    end
    
    if temp_idx~=0
        if meas_line(i,2)==1 %Pflow
            if flag_error==2 %Uniform PDF
                temp=(meas_line(i,1)*err_spec(2)/100)^2/3;
            else           %Gaussian PDF
                temp=err_spec(2)^2;
            end
            [m_Pflow]=[m_Pflow;temp_idx meas_line(i,1) temp];
        elseif meas_line(i,2)==2 %Qflow
            if flag_error==2 %Uniform PDF
                temp=(meas_line(i,1)*err_spec(2)/100)^2/3;
            else           %Gaussian PDF
                temp=err_spec(2)^2;
            end
            [m_Qflow]=[m_Qflow;temp_idx meas_line(i,1) temp];
        elseif meas_line(i,2)==3  %IM
            if flag_error==2 %Uniform PDF
                temp=(meas_line(i,1)*err_spec(4)/100)^2/3;
            else           %Gaussian PDF
                temp=err_spec(3)^2;
            end
            [m_IM]=[m_IM;temp_idx meas_line(i,1) temp];
        elseif meas_line(i,2)==4  %IA
            if flag_error==2 %Uniform PDF
                temp=(pi*err_spec(5)/180)^2/3;
            else           %Gaussian PDF
                temp=err_spec(4)^2;
            end
            [m_IA]=[m_IA;temp_idx meas_line(i,1) temp];
        elseif meas_line(i,2)==5  %Ireal
            if flag_error==2 %Uniform PDF
                temp=(meas_line(i,1)*err_spec(4)/100)^2/3;
            else           %Gaussian PDF
                temp=err_spec(3)^2;
            end
            [m_Ireal]=[m_Ireal;temp_idx meas_line(i,1) temp];
        elseif meas_line(i,2)==6  %Iimag
            if flag_error==2 %Uniform PDF
                temp=(meas_line(i,1)*err_spec(4)/100)^2/3;
            else           %Gaussian PDF
                temp=err_spec(3)^2;
            end
            [m_Iimag]=[m_Iimag;temp_idx meas_line(i,1) temp];
        end
    end
end

if isempty(m_IA)==0
    m_IA(:,2)=m_IA(:,2)-slack_ang(1);
    m_IA(:,3)=m_IA(:,3)+slack_ang(2);
end
%Combine PMU current abs and angle together col1:index col2:I_abs 
% col3:I_ang col4:var(I_abs) col5:variance(I_angle)
m_IMA=[];
if isempty(m_IM)==0  
    for i=1:length(m_IM(:,1))
        for j=1:length(m_IA(:,1))
            if m_IA(j,1)==m_IM(i,1)
                [m_IMA]=[m_IMA;m_IM(i,1:2) m_IA(j,2) m_IM(i,3) m_IA(j,3)];
            end
        end
    end
end

%Combine PMU current real and imag together col1:index col2:I_real 
% col3:I_imag col4:var(I_real) col5:variance(I_imag)
m_IRI=[];
if isempty(m_Ireal)==0  
    for i=1:length(m_Ireal(:,1))
        for j=1:length(m_Iimag(:,1))
            if m_Iimag(j,1)==m_Ireal(i,1)
                %Effect of slack bus angle on meas and their variance
                t_Ir=m_Ireal(i,2);t_Ii=m_Iimag(j,2);
                t_Rr=m_Ireal(i,3);t_Ri=m_Iimag(j,3);
                
                %Modified real and imaginary current meas
                t_Ir_m=t_Ir*cos(slack_ang(1))+t_Ii*sin(slack_ang(1));
                t_Ii_m=t_Ii*cos(slack_ang(1))-t_Ir*sin(slack_ang(1));
                
                %Modified variance
                temp1=cos(slack_ang(1));temp2=sin(slack_ang(1));
                temp3=-t_Ir*temp2+t_Ii*temp1;temp4=t_Ii*temp2+t_Ir*temp1;
                t_Rr_m=t_Rr*(temp1^2)+t_Ri*(temp2^2)+slack_ang(2)*(temp3^2);
                t_Ri_m=t_Ri*(temp1^2)+t_Rr*(temp2^2)+slack_ang(2)*(temp4^2);
                
                [m_IRI]=[m_IRI;m_Ireal(i,1) t_Ir_m t_Ii_m t_Rr_m t_Ri_m];
            end
        end
    end
end

if flag_eq==0
    if ~isempty(z_Pinj)
        temp=[z_Pinj zeros(length(z_Pinj),1) ones(length(z_Pinj),1)*1e-8 ];
        [m_Pinj]=[m_Pinj;temp];
    end
    if ~isempty(z_Qinj)
        temp=[z_Qinj zeros(length(z_Qinj),1) ones(length(z_Qinj),1)*1e-8 ];
        [m_Qinj]=[m_Qinj;temp];
    end
    if ~isempty(z_Pflow)
        temp=[z_Pflow zeros(length(z_Pflow),1) ones(length(z_Pflow),1)*1e-8 ];
        [m_Pflow]=[m_Pflow;temp];
    end
    if ~isempty(z_Qflow)
        temp=[z_Qflow zeros(length(z_Qflow),1) ones(length(z_Qflow),1)*1e-8];
        [m_Qflow]=[m_Qflow;temp];
    end
end

clear i j temp temp_idx t_Ir t_Ii t_Rr t_Ri t_Ir_m t_Ii_m t_Rr_m t_Ri_m;
clear m_IM m_IA m_Ireal m_Iimag m_VM m_VA temp1 temp2 temp3 temp4;