% maps: the 3D coverage map
% cameras: list of cameras, in the form [ cam1_x, cam1_y, cam1_height ; ... ]

function [sc, covg] = em_deployment(maps, cameras)
    global xd yd zd x y w mux muy vr axisvec stimes;
    
    % default renderer set to OpenGL
    set(0,'DefaultFigureRenderer','opengl')

    %costants definition
    stimes = 2.5;  % circle radius is stimes*stddev

    % init some other useful global var
    close all;
    size_x = size(maps, 2);
    size_y = size(maps, 1);
    size_z = size(maps, 3);
    axisvec = [ 0 size_x, 0, size_y, 0, max(size_z, max(cameras(:,3))) ];
    ndata = size_x * size_y * size_z;
    ncam = size(cameras,1);
    
    % project input data into each camera's spherical system
    xd = []; yd=[]; zd=[];
    tmpxd = ones(size_y,1) * (1:size_x);
    tmpxd = reshape(tmpxd, size_x * size_y, 1);
    tmpyd = (ones(size_x,1) * (1:size_y))';
    tmpyd = reshape(tmpyd, size_x * size_y, 1);
    for z=1:size_z
        xd = [xd; tmpxd];
        yd = [yd; tmpyd];
        zd = [zd; ones(size_x*size_y,1)*z];
    end
    x = []; y = []; z = [];
    for i=1:ncam
        [tmpx, tmpy ] = convert_to_camera_sphere(xd, yd, zd, cameras(i, 1), cameras(i, 2), cameras(i, 3));
        x = [ x; tmpx' ];
        y = [ y; tmpy' ];
    end
    clear tmpxd tmpyd tmpzd tmpx tmpy;

    % reshape weights
    w = reshape( maps, 1, size_y * size_x * size_z);
    
	% remove points with w=0 in EACH camera since they're useless. 
	% end-1 since the last map is the one without occlusions, not associated to a specific camera
    xd = xd(w > 1e-7);
    yd = yd(w > 1e-7);
    zd = zd(w > 1e-7);
    x = x(:, w > 1e-7);
    y = y(:, w > 1e-7);
    w = w(w > 1e-7);
    ndata = length(xd);
    
    %% NORMALIZE W so that it summs up to 1
    w = w ./ (sum(sum(sum(w))));

    % data initialization for EM
    mux = zeros(ncam,1);  
    muy = zeros(ncam,1);
    vr = ones(ncam,1) * 0.1^2;
    c = ones(ncam,1) / ncam;
    p = zeros(ncam, ndata);
    oldmux = zeros(ncam,1);
    oldmuy = zeros(ncam,1); 
    iter = 0;
    sc = []; covg = [];
    
    %%%%%%% EM ITERATION %%%%%%%%%%
   
    f1=figure; hold; daspect('manual'); axis(axisvec); 
    while 1
        iter = iter + 1;

        % plot the current state 
        plot_partial_solution(maps, cameras, f1, iter, [-163.9224  444.1570  217.3033]);
        %savefig(['main-',num2str(iter)], 'png', '-r80');
                
        % E STEP
        tmp = zeros(1,ndata);
        for k = 1:ncam
            tmp = tmp + c(k) * w .* gauss( mux(k), muy(k), vr(k), x(k,:), y(k,:) );
        end
        for j=1:ncam
            p(j,:) = c(j) * w .* gauss( mux(j), muy(j), vr(j), x(j,:), y(j,:));
            p(j,:) = p(j,:) ./ tmp;
        end
         
        % M STEP
        for j=1:ncam
            c(j) =  sum( w .* p(j,:) ) / sum( w );
            mux(j) = sum( w .* x(j,:) .* p(j,:) ) / sum( w .* p(j,:) );
            muy(j) = sum( w .* y(j,:) .* p(j,:) ) / sum( w .* p(j,:) );
            % isotropic variance: see tomasi's tutorial on EM + gaussian mixtures
            vr(j) = sum( w .* ((x(j,:) - mux(j)).^2 + (y(j,:) - muy(j)).^2) .* p(j,:) ) ./ (2 * sum( w .* p(j,:)));
        end

        %%% simulate sensor failure
        %if(iter == 50) vr(1) = 0; end
        
        % camera removal for degenerate cases
        delflag = vr < 0.0001;
        c(delflag) = []; mux(delflag) = []; muy(delflag) = []; vr(delflag) = [];
        oldmux(delflag) = []; oldmuy(delflag) = [];
        p(delflag,:) = []; w(delflag,:) = [];
        x(delflag,:) = []; y(delflag,:) = [];
        cameras(delflag,:) = [];
        ncam = ncam - length(find(delflag));
        
        % calc score
        sc = [ sc; score(x, y, w, mux, muy, vr, c, ncam) ];
        covg = [covg; coverage(x, y, w, mux, muy, vr, ncam, stimes) ];
        
        % exit test
        if( (max(abs(mux-oldmux)) < 1e-4 && max(abs(muy-oldmuy)) < 1e-4) || iter > 200 )
            break
        end
        oldmux = mux; oldmuy = muy;
    end
    
    %%%%%%% END EM ITERATION %%%%%%%%%%%%%%

    plot_optimal_solution(maps, cameras);
    %plot_optimal_singlecamera(maps, cameras);
    plot_camera_spaces(cameras);
    %plot_slice(maps,cameras,16);
    %plot_slice(maps,cameras,30);
    
    % print pan/tilt values
    for i=1:ncam
        print_ptz(mux, muy, vr, stimes, i);
    end

    %plot score & coverage
    figure; plot(sc); title('Score');
    figure; plot(covg); title('Coverage');
    covg(end)
end

% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % aux functions
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% log-likelihood, eq (12) in icdsc11
function sc = score(x, y, w, mux, muy, vr, c, ncam)
    s = zeros(1, length(x(1,:)));
     for j=1:ncam
         s = s + c(j) * gauss(mux(j), muy(j), vr(j), x(j,:), y(j,:));
     end
     sc = sum( w .* log( s )  );  
 end

% coverage, as defined in the ICDSC-10 paper
function cov = coverage(x, y, w, mux, muy, vr, ncam, stimes)
     mask = zeros(1, size(x,2));
     for j=1:ncam
         mask = mask | (( (x(j,:)-mux(j)).^2 + (y(j,:)-muy(j)).^2 ) < stimes^2 * vr(j));
     end
     cov = sum( w(mask) ) / sum( w );    
end

function [u,v] = convert_to_camera_sphere(xd, yd, zd, xc, yc, hc)   
    xd = xd - xc;    % centered on the camera
    yd = yd - yc;
    zd = zd - hc;
    pan = atan2( yd, xd );                           % -pi:pi
    tilt = atan2( sqrt(xd.^2 + yd.^2), -zd);         % 0:pi/2
    u = 2*tan(tilt/2) .* cos(pan);
    v = 2*tan(tilt/2) .* sin(pan);
end

function [x,y,z] = convert_from_camera_sphere( u, v, xc, yc, hc, z)
    phi = atan2(v,u);
    theta = 2*atan2(sqrt(u.^2+v.^2),2);
    x = (hc-z) .* cos(phi) .* tan(theta);
    y = (hc-z) .* sin(phi) .* tan(theta);
    x = x + xc;
    y = y + yc;
    z = z * ones(size(x));
end

function [phi,theta,zoom]=calc_ptz(mux, muy, vr, stimes, i)
    n = sqrt(mux(i)*mux(i) + muy(i)*muy(i));
    tmp = [mux(i), muy(i)];
    tmp1 = tmp * (n+sqrt(vr(i))) / n;
    tmp2 = tmp * (n-sqrt(vr(i))) / n;
    theta1 = 2*atan2(norm(tmp1),2);
    theta2 = 2*atan2(norm(tmp2),2);
    phi = atan2(muy(i),mux(i));
    theta = (theta1+theta2)/2;
    zoom = stimes * (theta1-theta2)/2;
end

function print_ptz(mux, muy, vr, stimes, i)
     [p,t,z]=calc_ptz(mux, muy, vr, stimes, i);
     disp(['Camera ', num2str(i), ' Pan: ', num2str(p*360/(2*pi)), ', Tilt: ', num2str(t*360/(2*pi)), ' half FoV: ', num2str( stimes*z*360/(2*pi) )]);
end

function p = gauss(mux, muy, vr, x, y)
    if length(mux) == 1 && length(x) ~= 1
        mux = mux * ones(1,length(x));
        muy = muy * ones(1,length(x));
        vr  = vr  * ones(1,length(x));
    end

    p = (1 ./ (2*pi*vr)) .* exp(- ((x-mux).^2 + (y-muy).^2)./(2*vr));
end

function plotellipse(mux, muy, vr, xc, yc, hc, stimes)
    u = stimes * sqrt(vr) * cos(0:0.05:2*pi) + mux;
    v = stimes * sqrt(vr) * sin(0:0.05:2*pi) + muy;
    [x,y,z] = convert_from_camera_sphere(u, v, xc, yc, hc, 1);    
    %plot(x,y,'Color',color,'LineWidth', 2);
    plot(x,y,'Color',[1 0 0],'LineWidth', 2);

    % plot the camera to circle center line  ---- ATTENZIONE questo non e' il centro dell'asse ottico
    [x,y,z] = convert_from_camera_sphere(mux, muy, xc, yc, hc, 1);   
    %line([xc, x], [yc, y], 'Color', color, 'LineWidth', 2);
    line([xc, x], [yc, y], 'Color', [1 0 0], 'LineWidth', 2);
end

function plotcone(mux, muy, vr, xc, yc, hc, stimes)
    u = stimes * sqrt(vr) * cos(0:0.05:2.01*pi) + mux;
    v = stimes * sqrt(vr) * sin(0:0.05:2.01*pi) + muy;
    x=[]; y=[]; z=[];
    for h=0:hc-1
        [tx,ty,tz] = convert_from_camera_sphere(u, v, xc, yc, hc, h);
        x = [x; tx];
        y = [y; ty];
        z = [z; tz];
    end
    hold on;
    h=surf(x,y,z,'FaceColor','blue','EdgeColor','none','FaceLighting','phong');
    alpha(h,0.4);
end

function plot_optimal_solution(maps, cameras)
    global axisvec mux muy vr xd yd zd w stimes
    ncam = size(cameras,1);

%     % plot the optimal deployment
%     figure; hold; daspect('manual'); axis(axisvec);
%     scatter3(xd, yd, zd, 20, w, 's', 'filled');
%     for j=1:ncam
%         plotcone(mux(j), muy(j), vr(j), cameras(j, 1), cameras(j, 2), cameras(j, 3), stimes );
%     end
%     title('Optimal deployment');
%     %savefig('optimal', 'png');

    % alternative plot for optimal deployment    
    figure; hold on; daspect('manual'); axis(axisvec);
    contour(sum(maps,3));
    hpatch=patch(isosurface(maps,1));
    isonormals(maps,hpatch);
    set(hpatch,'FaceColor','red','Edgecolor','none');
    camlight left
    lighting phong
    xlabel('x'); ylabel('y'); zlabel('z');
    for j=1:ncam
        plotcone(mux(j), muy(j), vr(j), cameras(j, 1), cameras(j, 2), cameras(j, 3), stimes);
    end
    grid on;
    title('Optimal configuration');
end

function plot_optimal_singlecamera(maps, cameras) 
    global axisvec mux muy vr xd yd zd w stimes
    ncam = size(cameras,1);

    % plot for each individual camera
    for i=1:ncam
        figure; hold on; daspect('manual'); axis(axisvec);
        contour(sum(maps,3));
        hpatch=patch(isosurface(maps,1));
        isonormals(maps,hpatch);
        set(hpatch,'FaceColor','red','Edgecolor','none');
        camlight left
        lighting phong
        xlabel('x'); ylabel('y'); zlabel('z');
        plotcone(mux(i), muy(i), vr(i), cameras(i, 1), cameras(i, 2), cameras(i, 3), stimes );
        title(['As seen from camera ', num2str(i)]);
    end
end

function plot_partial_solution(maps, cameras, fig, iter, cpos)
    global axisvec mux muy vr xd yd zd w stimes
    ncam = size(cameras,1);

    % alternative plot for optimal deployment    
    figure(fig); cla; hold on; daspect('manual'); axis(axisvec);
    contour(sum(maps,3));
    hpatch=patch(isosurface(maps,1));
    isonormals(maps,hpatch);
    set(hpatch,'FaceColor','red','Edgecolor','none');
    for j=1:ncam
        plotcone(mux(j), muy(j), vr(j), cameras(j, 1), cameras(j, 2), cameras(j, 3), stimes);
    end
    camlight left
    lighting phong
    campos(cpos);
    xlabel('x'); ylabel('y'); zlabel('z');
    grid on;
    title(['Iter ', num2str(iter)]);
end

function plot_camera_spaces(cameras)
    global axisvec mux muy vr xd yd zd x y w stimes
    ncam = size(cameras,1);

    % plot also each camera space
    for k=1:ncam
        figure; hold; daspect('manual'); 
        axis([-pi/2 pi/2 -pi/2 pi/2]);
        %scatter(x(k,:), y(k,:), 10, w, 's', 'filled');
        
        [tx,ty]=meshgrid([-pi/2:0.01:pi/2]);
        tsx = size(tx,1);
        tt = zeros(tsx,tsx);
        for t1=1:tsx
            for t2=1:tsx
                tbdy = -pi/2 + (pi / tsx) * t1;
                tbdx = -pi/2 + (pi / tsx) * t2;
                tt(t1,t2) = sum ( w ( x(k,:) >= tbdx & x(k,:) < tbdx + (pi/tsx) & y(k,:) >= tbdy & y(k,:) < tbdy + (pi/tsx) ) );
            end
        end
        surf(tx,ty,tt);

        title(['camera space ', num2str(k)]);
        tmpu = stimes * sqrt(vr(k)) * cos(0:0.05:2*pi);
        tmpv = stimes * sqrt(vr(k)) * sin(0:0.05:2*pi);
        plot(tmpu+mux(k), tmpv+muy(k), 'Color', [1 0 0], 'LineWidth', 2);
        %savefig(['cam',num2str(k),'-',num2str(iter)], 'png', '-r80');
    end
end

function plot_slice(maps, cameras, z)
    global stimes vr mux muy axisvec;
    ncam = size(cameras,1);

    figure; hold; daspect('manual'); axis(axisvec);
    hpatch=patch(isocaps(maps(:,:,1:z),1));
    set(hpatch,'FaceColor','interp','Edgecolor','none');

    for i = 1:ncam
        u = stimes * sqrt(vr(i)) * cos(0:0.05:2*pi) + mux(i);
        v = stimes * sqrt(vr(i)) * sin(0:0.05:2*pi) + muy(i);
        [xx,yy,zz] = convert_from_camera_sphere(u, v, cameras(i,1), cameras(i,2), cameras(i,3), z);    
        plot3(xx,yy,zz,'Color',[1 0 0],'LineWidth', 2);
        xlabel('x'); ylabel('y');
        title(['Optimal configuration at level z=', num2str(z)]);
    end
end






