
%% Alvarez Lens Correction C-implementaion
%
% Solves for the correct (un-distorted/original) image which 
% gave rise to the input distorted coordinates. The input image coordinates
% are assumed to originate from imaged straight lines using a camera
% following a radial lens distortion model (see plumb_line.h)
%
% An algebraic method is applied using a radial distortion model based on [1]. 
%
% Correction accepts two different input modes: 
% (1) image sequence specified - which initiates a program to manually
% select line segments
% (2) line points (pts, lineSizeMat) are specified leading to correction of
% a 'dummy' input image, and generation of lens distortion coefficients for
% computation of the plumb-line cost
%
%
%References:
%1)Alvarez, Luis, Luis Gomez, and J. Rafael Sendra. "Algebraic lens distortion
%  model estimation." Image Processing On Line 1 (2010)


%% Input Arguments

% seqName - Optional image sequence name. (default: 'calibration_pattern')
%           String.

% seqOutName - Optional output image sequence name. (default: 'output')
%              String.

% dirPath - Optional parent directory path to image sequence.
%           String (default: '.')

% dirOutPath - Optional parent directory path for output image sequence.
%              String (default: '.');

% pts - Optional Normalized coordinate matrix containing ordered list of all line points
%       grouped hiearchically by line and then group (image)
%       Nx2 matrix.

% lineSizeMat - Optional Size of sets matrix. Columns correspond to group (image) index,
%               and rows correspond to line index. To deduce number of points 
%               within line segment with group and line index i and j, 
%               respectively then N = setSize(i,j)
%               GxL matrix. 

% optimizeCenter - Optional flag indicating whether distortion center is
%                  assumed to coincide with image center (0) or whether it is
%                  optimized (1). (default: 0 - no optimization)
%                  Boolean

% outDir - Optional output directory for save-files
%          String

% outName - Optional output file name for save-files
%           String


%% Output Arguments
 
% coefs - Lens distortion model parameters for each corrected image (Q
%         images)
%         Qx1 cell. 

% error - Error (residual l2-norm) of final lens distortion correction for
%         each corrected image.
%         Qx1 vector


%% Blind Lens Correction C-implementation

function [coefs, error] = alvarez_image_lens_correction(varargin)

%add lsd tools
matlabDir = userpath;
addpath(strcat(matlabDir(1:end-1),'/MASC/Camera Calibration Tools/Nonmetric Calibration/Calibration Harp/'));
addpath(strcat(matlabDir(1:end-1),'/MASC/Camera Calibration Tools/Nonmetric Calibration/Calibration Harp/standard plumb-line method/plumb_line_cost_function'));
addpath(strcat(matlabDir(1:end-1),'/MASC/External Tools/Alvarez/alglens/'));

% Parse input parameters
[disPts, lineSizeMat, isGivenLines, fileInNames, fullDirPath, fileOutNames, nImgs, nRows, nCols, optimizeCenter, saveFile] = input_parser(varargin{:});

tic;
% Correct each image using Alvarez et al. C implementation, and compute
%error metric
coefs = cell(nImgs, 1);
error = nan(nImgs, 1);

for i = 1:nImgs,

    % User-selected line segments if not provided
    if ~isGivenLines %DEBUG ONLY "|| true" (delete this!!)
        
        [disPts, lineSizeMat] = user_line_selection(fileInNames{i}, 'dirPath', fullDirPath, 'nRepeat', 3);        
        
    end
    
    % Check image files are bitmap-format, otherwise convert image
    [~, imgInName, ext] = fileparts(fileInNames{i});
    [imgOutPath, imgOutName, ~] = fileparts(fileOutNames{i});
    if ~strcmp(ext, '.bmp')
        img = imread(fileInNames{i});
        fileInNames{i} = strcat(imgInName,'.bmp');
        fileOutNames{i}= strcat(imgOutName,'.bmp');
        imwrite(img, fileInNames{i});        
    end
    
    %write_alvarez_line_datafile(disPts, lineSizeMat, 'fileName', 'alvarez_lines');

    %DEBUG ONLY- copyfile (delete this!!)
    %copyfile('alvarez_lines.dat','./archive/alvarez_lines.dat');
    
    cmd = sprintf('./lens_distortion_estimation "%s" "%s" alvarez_lines.dat output_results.dat %d', fileInNames{i}, fileOutNames{i}, optimizeCenter);
    system(cmd);

    coefs{i} = read_alvarez_coef_datafile('tmp');
    
    % Convert output image to desired format
    if ~strcmp(ext, '.bmp')
        img = imread(fileOutNames{i});
        delete(fileOutNames{i});
        delete(fileInNames{i}); %local copy
        fileOutNames{i} = fullfile(imgOutPath, strcat(imgOutName, ext));
        imwrite(img, fileOutNames{i}); 
    end
    
    % Normalize line size points prior to error-computation
    nPts  = size(disPts,1);
    ctrPt = [nCols; nRows]/2;    
    disPts(:,1) = ( disPts(:,1) - ctrPt(1)*ones(nPts,1) )/nCols;
    disPts(:,2) = ( disPts(:,2) - ctrPt(2)*ones(nPts,1) )/nRows;
    
    [error(i), ~] = plumb_line_cost_function(disPts, lineSizeMat, coefs{i}, 'methodType', 'std', 'modelType', 'rad');
        
end

runTime = toc; %#ok<NASGU>

save(saveFile, 'disPts', 'lineSizeMat', 'coefs', 'error', 'runTime');

end% end of blind lens correction


%% Input Parser
function [pts, lineSizeMat, isGivenLines, fileInNames, fullDirPath, fileOutNames, nImgs, nRows, nCols, optimizeCenter, saveFile] = input_parser(varargin)

%add library for 'natural' sorting of strings with numeric components
matlabDir = userpath;
matlabDir = matlabDir(1:end-1);
addpath(strcat(matlabDir,'/MASC/External Tools/SortNat'));

% parse input arguments
p = inputParser;
p.addParameter('seqName', 'calibration_pattern', @(x) ischar(x));
p.addParameter('dirPath', '', @(x) ischar(x));
p.addParameter('seqOutName', 'output', @(x) ischar(x));
p.addParameter('dirOutPath', '', @(x) ischar(x));
p.addParameter('pts', [], @(x) ...
              validateattributes(x,{'numeric'},{'real','nonnan','2d','finite'}));
p.addParameter('lineSizeMat', [], @(x) ...
              validateattributes(x,{'numeric'},{'real','nonnan','2d','finite'}));  
p.addParameter('optimizeCenter', 0, @(x) ...          
               validateattributes(x,{'numeric'},{'real', 'integer', 'nonnan', 'nonnegative', '<=', 1, 'finite','scalar'})); 
p.addParameter('outDir', '.', @(x) ischar(x));	
p.addParameter('outName', 'alvarez_lens_calibration_c.mat', @(x) ischar(x));
p.addParameter('type', '', @(x) ...
                ischar(validatestring(x, {'jpg','ppm','bmp'})));

p.parse(varargin{:});
inputs         = p.Results;
seqName        = inputs.seqName;
dirPath        = inputs.dirPath;
dirOutPath     = inputs.dirOutPath;
seqOutName     = inputs.seqOutName;
pts            = inputs.pts;
lineSizeMat    = inputs.lineSizeMat;
optimizeCenter = inputs.optimizeCenter;
outDir         = inputs.outDir;
outName        = inputs.outName;
type           = inputs.type;


if isempty(dirPath)
    fullDirPath = '.';
else
    if regexp(dirPath, matlabDir)
        fullDirPath = dirPath;
    elseif strncmpi(dirPath, '.', 1)
        fullDirPath = dirPath;
    else
        fullDirPath = fullfile(matlabDir, dirPath);
    end
end

if isempty(dirOutPath)
    fullDirOutPath = '.';
else
    if regexp(dirOutPath, matlabDir)
        fullDirOutPath = dirOutPath;
    elseif strncmpi(dirOutPath, '.', 1)
        fullDirOutPath = dirOutPath;
    else
        fullDirOutPath = fullfile(matlabDir, dirOutPath);
    end
end

%check input image sequence name exists
seqPath         = fullfile(fullDirPath, strcat(seqName,'*'));
dirStruct       = dir(seqPath);
imgSeqFileNames = {dirStruct.name};

if isempty(imgSeqFileNames)
    error('Image sequence cannot be found at %s', fullDirPath)
end

%create array of absolute path names for all files
nImgs     = size(imgSeqFileNames, 2);
fileInNames = cell(nImgs,1);
for i = 1:nImgs,
    fileInNames{i} = fullfile(fullDirPath, imgSeqFileNames{i});
end

%natural-order sorting of file names 
fileInNames = sort_nat(fileInNames, 'ascend');

[~, ~, ext] = fileparts(fileInNames{1});

if ~isempty(type)   
   ext = strcat('.',type); 
end

%create array of absolute path names for all output files
fileOutNames = cell(nImgs, 1);
if nImgs == 1
    [~,~,outExt] = fileparts(seqOutName);
    if isempty(outExt)
        fileOutNames{1} = fullfile(fullDirOutPath, strcat(seqOutName, ext));
    else
        fileOutNames{1} = fullfile(fullDirOutPath, seqOutName);
    end
else
    for i = 1:nImgs,
        fileOutNames{i} = fullfile(fullDirOutPath, strcat(seqOutName, num2str(i-1), ext));
    end
end

firstImg = imread(fileInNames{1});
[nRows, nCols, ~] = size(firstImg);

isGivenLines = ~isempty(pts) && ~isempty(lineSizeMat); 

saveFile = fullfile(outDir, outName);

end %end of input parser





