
%%%%%%%%%%%%%%%%%%%%%%%% Solve the first equation %%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


d_pile = reshape(d(1:Mm), m, M);

B_1_diag = sum(d_pile);
B_1_diag_cell = mat2cell(B_1_diag, 1, m_vec);
cc= sum(d(n_col-m+1-N-1:n_col-N-1));
y = d(n_col-m+2-N-1: n_col-N-1)/cc;

pile_cell = mat2cell(d_pile, m, m_vec);
D_pile = reshape(D', m, M);
D_pile_cell = mat2cell(D_pile, m, m_vec);
DE1 = cellfun(@(x,y) sum(x.*y,1), D_pile_cell, pile_cell, 'UniformOutput',0);

T2 = cellfun(@(x,y) x./y, DE1, B_1_diag_cell, 'UniformOutput',0);

DD = cellfun(@(x,y) sum(sum(x.*x.*y)), D_pile_cell, mat2cell(d_pile, m, m_vec) ,'UniformOutput',0);

II = diag(d(Mm+m+1:Mm+m+N));
JJ = d(end) * ones(N);

d_pile(1,:) = [];
d_pile_cell = mat2cell(d_pile, m-1, m_vec);
T = cellfun(@(x,y) x./y, d_pile_cell, B_1_diag_cell, 'UniformOutput',0);

B3_diag_cell= cellfun(@(x) sum(x,2), d_pile_cell,  'UniformOutput',0);

B2_B3inv_cell = cellfun(@(x,y) x'./y' , d_pile_cell,B3_diag_cell, 'UniformOutput',false );

center_inv_cell = cellfun(@(x,y,z) diag(x')-y*z, B_1_diag_cell, B2_B3inv_cell, d_pile_cell,  'UniformOutput',0  );
center_inv_cell = cellfun(@inv, center_inv_cell , 'UniformOutput',false );

B3_inv_diag_cell = cellfun(@(x) 1./x, B3_diag_cell,  'UniformOutput',0);

A_1_inv_cell = cellfun(@(x,y,z) diag(x)+y'*z*y, B3_inv_diag_cell, B2_B3inv_cell, center_inv_cell, 'UniformOutput',0 );
inv_sum = sum( cat(3,A_1_inv_cell{:} ),3 );

BB = diag( d(n_col-m+2-N-1: n_col-N-1) ) - d(n_col-m+2-N-1: n_col-N-1)*d(n_col-m+2-N-1: n_col-N-1)'/cc;
trN = inv_sum + inv(BB);  
trN_inv = inv(trN);

D_pile(1,:) = [];
D_pile_cell = mat2cell(D_pile, m-1, m_vec);
DE2 = cellfun(@(x,y) sum(x.*y,2)', D_pile_cell, d_pile_cell, 'UniformOutput',0);

tmp_cell = cellfun(@(x,y,z) x-y*z', DE2 ,T2 ,d_pile_cell, 'UniformOutput', 0);  %D()E2'
tmp1_cell = cellfun(@(x,y) y*x*y', A_1_inv_cell, tmp_cell, 'UniformOutput', false);
tmp1 = cat(1,tmp1_cell{:});
tmp2_cell = cellfun(@(x,y) y*x, A_1_inv_cell, tmp_cell, 'UniformOutput', false);
tmp3_cell = cellfun(@(x) trN_inv*x', tmp2_cell, 'UniformOutput', false);
T4_1 = zeros(N);
for i=1:N
    for j=1:N
        ti = cell2mat(tmp2_cell(i));
        tj = cell2mat(tmp3_cell(j));
        T4_1(i,j) = ti * tj;
    end
end
T4_1 = diag(tmp1) - T4_1;
T4_2_cell = cellfun(@(x,y,z) z-x./y*x', DE1, B_1_diag_cell, DD, 'UniformOutput',0);
T4_2 = diag(cat(1,T4_2_cell{:}));
T4 = T4_2 - T4_1 + II + JJ;

ys = kron( ones(N,1), y ); % to be used

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Start solving
xx=t2;

tmp = xx(1 : M);
tmp_cell = mat2cell(tmp, m_vec, 1)';
tmp_cell = cellfun(@mtimes, T, tmp_cell, 'UniformOutput', false);
xx(M+1 : n_row-1-N) = xx(M+1 : n_row-1-N)  - cat(1,tmp_cell{:} );
tmp2_cell = cellfun(@mtimes, T2, mat2cell(tmp, m_vec, 1)', 'UniformOutput', false);
xx(n_row-N+1 : n_row) = xx(n_row-N+1 : n_row) - cat(1,tmp2_cell{:} );

xx(M+1: n_row-1-N) = xx(M+1: n_row-1-N) + kron( ones(N,1),y )*xx(n_row-N);

tmp3 = xx(M+1 : M+(m-1)*N);
tmp3_cell = mat2cell(tmp3, (m-1)*ones(N,1), 1)';
tmp3_cell = cellfun(@mtimes, A_1_inv_cell, tmp3_cell, 'UniformOutput', false);
tmp3 = cat(1,tmp3_cell{:});
tmp3_1 = tmp3;
tmp3_2 = zeros( N*(m-1),1 );
tmp3_2( 1:(N-1)*(m-1) )=  tmp3( 1:(N-1)*(m-1) );
tmp3_2( (N-1)*(m-1)+1: N*(m-1) ) = sum(reshape(tmp3, m-1, N), 2);
tmp3_2 = [zeros((N-1)*(m-1) ,1 ); trN_inv*tmp3_2((N-1)*(m-1)+1: N*(m-1))];
tmp3_2( 1: (N-1)*(m-1) ) = kron( ones(N-1,1), tmp3_2( (N-1)*(m-1)+1: N*(m-1) ) );
tmp3_2_cell = mat2cell(tmp3_2, (m-1)*ones(N,1),1 )';
tmp3_2_cell = cellfun(@mtimes, A_1_inv_cell, tmp3_2_cell, 'UniformOutput',false );
tmp3_2 = cat(1,tmp3_2_cell{:});
tmp3 = tmp3_1 - tmp3_2;
tmp3_3_cell = mat2cell(tmp3, (m-1)*ones(N,1), 1)';
tmp3_4_cell = cellfun(@(x,y,z,w) x*y-z*w'*y, DE2 ,tmp3_3_cell ,T2 ,d_pile_cell, 'UniformOutput',false );
xx(n_row-N+1 : n_row) = xx(n_row-N+1 : n_row) - cat(1,tmp3_4_cell{:});

xx(1:M) =  xx(1:M)./B_1_diag';

xx(n_row-N) = xx(n_row-N)/cc;

xx(n_row-N+1:n_row) = T4\xx(n_row-N+1 : n_row);

tilde_x = xx(M+1: n_row-1-N);
tilde_x_cell = mat2cell(tilde_x, (m-1)*ones(N,1), 1)';
tilde_x_cell = cellfun(@mtimes, A_1_inv_cell, tilde_x_cell, 'UniformOutput', 0);
tilde_x = cat(1,tilde_x_cell{:});

tilde_x_1 = tilde_x;
tilde_x_2 = zeros( N*(m-1),1 );
tilde_x_2( 1:(N-1)*(m-1) )=  tilde_x( 1:(N-1)*(m-1) );
tilde_x_2( (N-1)*(m-1)+1: N*(m-1) ) = sum(reshape(tilde_x, m-1, N), 2);

tilde_x_2 = [zeros((N-1)*(m-1) ,1 ); trN_inv*tilde_x_2((N-1)*(m-1)+1: N*(m-1))];

tilde_x_2( 1: (N-1)*(m-1) ) = kron( ones(N-1,1), tilde_x_2( (N-1)*(m-1)+1: N*(m-1) ) );

tilde_x_2_cell = mat2cell(tilde_x_2, (m-1)*ones(N,1),1 )';
tilde_x_2_cell = cellfun(@mtimes, A_1_inv_cell, tilde_x_2_cell, 'UniformOutput',false );
tilde_x_2 = cat(1,tilde_x_2_cell{:});

tilde_x = tilde_x_1 - tilde_x_2;

xx(M+1: n_row-1-N) = tilde_x;

tmp_cell = mat2cell(xx(n_row-N+1:n_row), ones(N,1), 1)';
tmp_cell = cellfun(@(x,y,z,w) x'.*y-w*z'.*y, DE2 ,tmp_cell ,T2 ,d_pile_cell, 'UniformOutput',false );
tmp2_cell = cellfun(@mtimes, A_1_inv_cell, tmp_cell, 'UniformOutput', false);
tmp2 = cat(1,tmp2_cell{:});
tmp2_1 = tmp2;
tmp2_2 = zeros(N*(m-1),1);
tmp2_2(1 : (N-1)*(m-1)) =  tmp2(1 : (N-1)*(m-1));
tmp2_2((N-1)*(m-1)+1 : N*(m-1)) = sum(reshape(tmp2, m-1, N), 2);
tmp2_2 = [zeros((N-1)*(m-1) ,1); trN_inv*tmp2_2((N-1)*(m-1)+1 : N*(m-1))];
tmp2_2(1: (N-1)*(m-1)) = kron(ones(N-1,1), tmp2_2((N-1)*(m-1)+1 : N*(m-1)));
tmp2_2_cell = mat2cell(tmp2_2, (m-1)*ones(N,1), 1)';
tmp2_2_cell = cellfun(@mtimes, A_1_inv_cell, tmp2_2_cell, 'UniformOutput', 0);
tmp2_2 = cat(1,tmp2_2_cell{:});
tmp2 = tmp2_1 - tmp2_2;
xx(M+1:n_row-1-N) = xx(M+1:n_row-1-N) - tmp2;

xx(n_row-N) = xx(n_row-N) + dot(ys, xx(M+1 : n_row-1-N));

tmp_cell = mat2cell(xx(M+1 : n_row-1-N), (m-1)*ones(N,1),1 )';
tmp_cell = cellfun(@(x,y) x'*y, T, tmp_cell, 'UniformOutput', 0);
tmp = cat(1,tmp_cell{:});
xx(1:M) = xx(1:M) - tmp;
tmp2_cell = mat2cell(xx(n_row-N+1:n_row), ones(N,1), 1)';
tmp2_cell = cellfun(@(x,y) x' .* y, T2, tmp2_cell, 'UniformOutput', false);
tmp2 = cat(1, tmp2_cell{:});
xx(1:M) = xx(1:M) - tmp2;

dp = xx;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%% End solving %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
