function focalStacking()
  path = 'dataset-watch\';
  sharpnessPath = strcat(path, 'sharpness\');
  outputPath = strcat(path, '\output\');
  step = 1;   
  imageFiles = dir(strcat(path, 'frame*.jpg'));       
  T = imread(strcat(path, imageFiles(1).name));
  height = size(T, 1);
  width = size(T, 2);
  sharpnessFiles = dir(strcat(sharpnessPath, '*.jpg'));  
  CreateSharpnessFiles(step, imageFiles, path, sharpnessPath);
  MaskImage = CreateMaskFile(height, width, sharpnessFiles, sharpnessPath, outputPath);
  FusionImage = CreateFinalImage(height, width, step, MaskImage, imageFiles, sharpnessFiles, path, sharpnessPath);
  FilterFinalImage(FusionImage, outputPath);
end

function FilterFinalImage(FusionImage, outputPath)
  height = size(FusionImage, 1);
  width = size(FusionImage, 2);
  h = zeros(height, width);
  for i = 1 : height
    for j = 1 : width
      ii = i - height / 2;
      jj = j - width / 2;
      r = sqrt(ii * ii + jj * jj);
      h(i, j) = 1 / (sqrt(2 * pi)) * erfc(r / sqrt(2) * 0.01);
    end
  end      
  h = h / max(max(h));  
  h = real(fftshift(ifft2(ifftshift(h))));    
  FusionImage(:, :, 1) = deconvwnr(FusionImage(:, :, 1), h, 0.05);
  FusionImage(:, :, 2) = deconvwnr(FusionImage(:, :, 2), h, 0.05);
  FusionImage(:, :, 3) = deconvwnr(FusionImage(:, :, 3), h, 0.05);
  FusionImage(:, :, 1) = uint8(FusionImage(:, :, 1));
  FusionImage(:, :, 1) = max(min(FusionImage(:, :, 1), 255), 0);
  FusionImage(:, :, 2) = uint8(FusionImage(:, :, 2));
  FusionImage(:, :, 2) = max(min(FusionImage(:, :, 2), 255), 0);
  FusionImage(:, :, 3) = uint8(FusionImage(:, :, 3));
  FusionImage(:, :, 3) = max(min(FusionImage(:, :, 3), 255), 0);
 imwrite(uint8(FusionImage), strcat(outputPath, 'composite.bmp'));
end


function FusionImage = CreateFinalImage(height, width, step, MaskImage, imageFiles, sharpnessFiles, path, sharpnessPath)
  FusionImage = zeros(height, width, 3);  
  WeightImage = zeros(height, width);
  index = 0;
  for k = 1 : step : length(imageFiles)
    index = index + 1;
    I = double(imread(strcat(path, imageFiles(k).name)));
    J = double(imread(strcat(sharpnessPath, sharpnessFiles(index).name)));
    J = J + 0.001;
    FusionImage(:, :, 1) = FusionImage(:, :, 1) + I(:, :, 1) .* J;
    FusionImage(:, :, 2) = FusionImage(:, :, 2) + I(:, :, 2) .* J;
    FusionImage(:, :, 3) = FusionImage(:, :, 3) + I(:, :, 3) .* J;    
    WeightImage = WeightImage + J;
  end
  FusionImage(:, :, 1) = FusionImage(:, :, 1) ./ WeightImage;
  FusionImage(:, :, 2) = FusionImage(:, :, 2) ./ WeightImage;
  FusionImage(:, :, 3) = FusionImage(:, :, 3) ./ WeightImage;      
end


% function FusionImage = CreateFinalImage(height, width, step, MaskImage, imageFiles, sharpnessFiles, path, sharpnessPath)
%   FusionImage = zeros(height, width, 3);  
%   index = 0;
%   for k = 1 : step : length(imageFiles)    
%     index = index + 1;
%     I = double(imread(strcat(path, imageFiles(k).name)));    
%     for i = 1 : height
%       for j = 1 : width
%         if (index == floor(MaskImage(i, j))) || (index == ceil(MaskImage(i, j)))
%           FusionImage(i, j, 1) = FusionImage(i, j, 1) + I(i, j, 1) * abs(MaskImage(i, j) - index);
%           FusionImage(i, j, 2) = FusionImage(i, j, 2) + I(i, j, 2) * abs(MaskImage(i, j) - index);
%           FusionImage(i, j, 3) = FusionImage(i, j, 3) + I(i, j, 3) * abs(MaskImage(i, j) - index);
%         end
%       end
%     end    
%   end
% end

function MaskImage = CreateMaskFile(height, width, sharpnessFiles, sharpnessPath, outputPath)
  MaskImage = zeros(height, width);
  WeightImage = zeros(height, width);
  ValueImage = zeros(height, width);  
  for k = 1 : length(sharpnessFiles)
    J = double(imread(strcat(sharpnessPath, sharpnessFiles(k).name)));    
    for i = 1 : height * width
      if J(i) >= ValueImage(i)
        ValueImage(i) = J(i);
        MaskImage(i) = MaskImage(i) + J(i) * k;        
      end
    end
    WeightImage = WeightImage + J;
  end
  MaskImage = MaskImage ./ (WeightImage + 0.0001);
  MaskImage = MaskImage / max(max(MaskImage));
  imwrite(MaskImage, strcat(outputPath, 'mask.bmp'));
end

function CreateSharpnessFiles(step, imageFiles, path, sharpnessPath)  
  for i = 1 : step : length(imageFiles)
    I = imread(strcat(path, imageFiles(i).name));
    I = rgb2gray(I);
    I = double(I);
    J = computeSharpness(I);
    %J = imfilter(J, fspecial('gaussian', 5, 2));
    J = J / max(max(J));
    imwrite(255.0 * J, strcat(sharpnessPath, strrep(imageFiles(i).name, '.jpg', '_s.jpg')));
  end
end

function Ixy = computeSharpness(I)
  h = fspecial('sobel');
  Ix = imfilter(I, h);
  Iy = imfilter(I, h');
  Ixy = Ix .^ 2 + Iy .^ 2;
end