%--------------------------
 % @Author: Jingqiao Hu
 % @Date: 2021-12-27 18:08:19
 % @LastEditTime: 2022-01-05 13:47:09
%--------------------------
function [E, dE] = align_energy(conn_list, seeds, stress, pmax, globalx, globaly)
    
    s = reshape(stress', 2, globaly, globalx);
    p = reshape(pmax, globaly, globalx);

    pnts = round(seeds);
    pnts(:,1) = pnts(:,1) + globalx / 2;
    pnts(:,2) = pnts(:,2) + globaly / 2;

    % the boundary
    pnts(pnts < 1) = 1;
    pnts(pnts(:,1)>globalx, 1) = globalx;
    pnts(pnts(:,2)>globaly-1, 2) = globaly-1;

    npnts = size(seeds, 1);
    Es = zeros(npnts, 1);
    dEs = zeros(npnts, 2);

%     figure; 
    for i = 1 : npnts        
        % 1. di, dj
        neighbor = conn_list{i};
        pj = pnts(neighbor, :); % [j,2] 
        nj = size(pj,1);

        dj = zeros(2, nj); % [2, j];
        wj = zeros(nj, 1); % [j,1];
        for j = 1 : nj
            dj(:,j) = s(:, globaly-pj(j,2), pj(j,1)); 
            wj(j) = p(globaly-pj(j,2), pj(j,1)); 
        end

        pi = pnts(i, :);
        di = s(:, globaly-pi(2), pi(1)); % [2,1]
%         quiver(seeds(i,1), seeds(i,2), di(1), di(2), 5, 'LineWidth',1); axis equal; hold on;
        di = repmat(di, 1, nj);  % [2,j]               

        % 2. eij, eji
        xj = seeds(neighbor,:);
        xi = repmat(seeds(i,:), nj, 1);

        eij = xj - xi; % [j,2]
        eji = xi - xj;

        % 3. Dij, Dji
        veij = eij ./ vecnorm(eij, 2, 2);
        Dij = sum(di' .* veij, 2); % [j,1]
        
        veji = eji ./ vecnorm(eji, 2, 2);
        Dji = sum(dj' .* veji, 2); % [j,1]

        % 4. energy
        wi = p(globaly-pi(2), pi(1));
        Es(i) = sum(Dij .^ 2) * wi;

        % 5. sensitivity
        dDij = - align_sensitivity(Dij, eij, di); % [j,2]
        dDji = align_sensitivity(Dji, eij, dj);
        dDe  = wi * dDij + dDji .* wj;

        dEs(i,:) = sum(dDe);
    end
    dE = reshape(dEs', [], 1);
    E = sum(Es);
end

function dD = align_sensitivity(Dij, eij, di)

    % expand to [3,n]
    e = eij';
    e(3, :) = 0;
    d = di;
    d(3, :) = 0;

    ed = cross(e, d);

    f1 = cross(e, ed);
    f1(3,:) = []; % [2,n]
    f2 = vecnorm(eij, 2, 2).^2 .* vecnorm(ed', 2, 2); % [n,1]

    dphi_ij = f1' ./ f2; % [n,2]

    sin_ij = sqrt(1 - Dij.^2); % [n,1]

    dD = -2 * Dij .* sin_ij .* dphi_ij; % [n,2]
end