ticALL = tic;



% ======  Inputs ==========================================================
%
%  Reference Inputs (All GEOTIFF)
%    refDSM
%    refDTM
%    RefMask
%
%  Test Inputs (LAS FILE, GEOTIFF Building Mask)
%    TestLAS
%    TestMask
%
% =========================================================================


PROC_FULL_MODEL = false;

GRID_METHOD = 1;  % Max
% GRID_METHOD = [];  % Average

DO_PLOTS = 1;
SAVE_PLOTS = 0;
SAVE_FIGS = 0;

BUILDING_VALUE = 240;



if isunix
    dirRootAosShare      = '/mnt/aos/AOSShare/';
    dirRootU             = '/mnt/u/';
else
    dirRootAosShare      = 'A:/AOSShare/';
    dirRootU             = 'U:';
end

saveDir = [];

USE_SHR3D_DSM = false;
if USE_SHR3D_DSM
    %     refInfo = geotiffinfo(refReferenceFootprintFilename);
    FILL_VALUE = -10000;
else
    %     refInfo = refDSMInfo;
    FILL_VALUE = -9999;
end

MASK_TRUE = 6;
DTM_FILL_VALUE = FILL_VALUE;
DSM_FILL_VALUE = FILL_VALUE;

if DO_PLOTS
    caxDTM = [15 40];
    caxObj = [10 30] - 10;
end

% Test LAS Point Cloud Model
performerCloudDir      = fullfile(dirRootAosShare, 'IARPA-CORE3D/SiteDataPackage/SanFernando/OpenSourceMVS/PCs_registered');

% Test GEOTIFF Building Footprint Mask
performerFootprintDir =  fullfile(dirRootAosShare, 'IARPA-CORE3D/SiteDataPackage/SanFernando/OpenSourceMVS/PCs_registered/shr3d');

refDir                = fullfile(dirRootAosShare, 'SOCOM-3D-Data-Engineering/2016/160921-TEXPERT-Analysis/Data for Tests/SanFernando/50cm');
% refDir                = fullfile(dirRootU, 'AOS/CORE3D/Data/ARG_Master2/GroundTruth');


truthFootprintDir      = fullfile(dirRootAosShare, 'IARPA-CORE3D/Workspace/Results/Ground Truth/San Fernando/');
orthoImageDir          = fullfile(dirRootAosShare, 'IARPA-CORE3D/Workspace/Results/Ground Truth/San Fernando/Input');

% ------


if USE_SHR3D_DSM
    
    refDSMFilename       = fullfile( fullfile(dirRootU, 'AOS/CORE3D/Data/ARG_Master2/GroundTruth'),     'MasterProvisional2_dem.tif');
    
    refDTMFilename       = fullfile(refDir,    'Master2.las_DTM.tif');
    refReferenceFootprintFilename = fullfile(dirRootU, 'AOS/CORE3D/Data/ARG_Master2/GroundTruth', 'MasterProvisional2_dem.tif');
else
    refDTMFilename       = fullfile(refDir,     'MasterProvisional2_bare_dem.tif');
    refDSMFilename       = fullfile(refDir,     'MasterProvisional2_dem.tif');
end



refMaskFilename = fullfile(truthFootprintDir, 'SanFernando_AOI2_GT_refined.tif');


orthoImageFilename     = fullfile(orthoImageDir, 'SanFernando_AOI2_Vricon_Master2_adj_rgb.tif');

performerCloudNames = { 'S2P'
    'JHU/APL'
    'ASP-SDRDIS'
    'RSP'
    };



performerCloudFileList = {
    'S2P.las_adj.las'
    'JHUAPL.las_adj.las'
    'ASPSDRDIS.las_adj.las'
    'RSP.las_adj.las'
    };



performerFootprintFileList = {
    'S2P.las_adj.las_buildings.tif'
    'JHUAPL.las_adj.las_buildings.tif'
    'ASPSDRDIS.las_adj.las_buildings.tif'
    'RSP.las_adj.las_buildings.tif'
    };



loopPerformer = 1;


performerCloudFilenameList         = cellfun(@(x) fullfile(performerCloudDir,x) , performerCloudFileList, 'UniformOutput', 0);
performaerFootprintFilenameList    = cellfun(@(x) fullfile(performerFootprintDir,x) , performerFootprintFileList, 'UniformOutput', 0);


fprintf('\nReference DSM GEOTIFF file:\n')
fprintf('\t%s\n', refDSMFilename)

fprintf('\nReference DTM GEOTIFF file:\n')
fprintf('\t%s\n', refDTMFilename)

fprintf('\nReference footprint GEOTIFF file:\n')
fprintf('\t%s\n', refMaskFilename)

fprintf('\nTest LAS Point Cloud Models:\n')
fprintf('\t%s\n', performerCloudFilenameList{:})

fprintf('\nTest GEOTIFF Building Footprint Mask:\n')
fprintf('\t%s\n', performaerFootprintFilenameList{:})


%%
if DO_PLOTS > 0
    DO_PLOTS = 200;
end

parms = [];
params.DSM_FILL_VALUE  = -9999;  % TODO
params.DTM_FILL_VALUE  = -9999;  % TODO
params.MASK_TRUE       = 6;      % TODO
params.DO_PLOTS        = DO_PLOTS;
params.SAVE_PLOTS      = SAVE_PLOTS;
params.SAVE_FIGS       = SAVE_FIGS;
params.SAVE_DIR        = saveDir;

[refObj, refMask, refDTM, refInfo] = loadReferenceData(refDSMFilename, refDTMFilename, refMaskFilename, params);

 
%%
% TODO - apply registration offsets


metrics2D = [];
metrics3D = [];
metrics3Dsoft = [];
%%
for loopPerformer = 1%:numel(performerCloudFileList)
    
    model_prefix = strrep(performerCloudNames{loopPerformer}, '/', '');
    model_prefix_str = model_prefix;  % Used in plot titles
    
    testModelFilename      = performerCloudFilenameList{loopPerformer};
    testFootprintFilename  = performaerFootprintFilenameList{loopPerformer};
    
    %%
    
    
    % ----- Load Performer Data -------------------------------------------
    fprintf('START Prepping Test Data...'); testtic = tic;
    
    
    % ** IMPORTANT VARIABLES **
    %
    % testMask    mask of test object footprints
    % testObj     (test raster - reference DTM) with non-object pixels set to NaN
    
    
    % ---------------------------------------------------------------------
    fprintf('\tLoading test data...');    btic = tic;
    
    % Load LAS File
    testLAS = lasdata(testModelFilename);
    
    % Load mask
    testMaskImg  = geotiffread(testFootprintFilename);
    testMaskInfo = geotiffinfo(testFootprintFilename);
    
    fprintf('     (%0.3f)\n', toc(btic));
    
    % ---------------------------------------------------------------------
    fprintf('\tProcessing test data... ');    btic = tic;
    
    % Align objective mask and point cloud with reference image space
    testMask     = imwarpGeoTiff(double(testMaskImg == BUILDING_VALUE), testMaskInfo, refInfo, double(false));
    [row, col]   = map2pix(refInfo.RefMatrix, testLAS.x, testLAS.y);
    
    % Bin the test cloud
    outSize = [refInfo.Height refInfo.Width];
    [testRaster, ~, N] = rasterData([col row], testLAS.z, outSize,  ~isnan(testMask), GRID_METHOD);
    
    % Processing
    testMask = ~isnan(testRaster) & (testMask==1);
    testObj = testRaster - refDTM;
    testObj(testMask == 0) = nan;
    
    fprintf('%0.3f\n', toc(btic));
    
    
    
    
    % ---------- Make some plots ----------------------------------------------
    if DO_PLOTS
        fprintf('\tMaking plots of test data...'); btic = tic;
        
        figure(300)
        imagesc(testRaster); axis image ij; colorbar; caxis(caxDTM);
        title(sprintf('%s - Mask (testRaster)', model_prefix_str));
        
        if SAVE_PLOTS
            str = sprintf('%s_mask', model_prefix);
            saveas(gcf, fullfile(saveDir, sprintf('%s.%s', str, 'png')));
            if SAVE_FIGS
                saveas(gcf, fullfile(saveDir, sprintf('%s.%s', str, 'fig')));
            end
        end
        
        figure(301)
        imagesc(N); axis image ij;  colorbar;
        title(sprintf('%s - Points Per Pixel (N)', model_prefix_str));
        
        if SAVE_PLOTS
            str = sprintf('%s_counts', model_prefix);
            saveas(gcf, fullfile(saveDir, sprintf('%s.%s', str, 'png')));
            if SAVE_FIGS
                saveas(gcf, fullfile(saveDir, sprintf('%s.%s', str, 'fig')));
            end
        end
        
        figure(302)
        imagesc(testMask); axis image ij;  colorbar
        title(sprintf('%s - Mask (testMask)', model_prefix_str));
        
        if SAVE_PLOTS
            str = sprintf('%s_mask', model_prefix);
            saveas(gcf, fullfile(saveDir, sprintf('%s.%s', str, 'png')));
            if SAVE_FIGS
                saveas(gcf, fullfile(saveDir, sprintf('%s.%s', str, 'fig')));
            end
        end
        
        figure(303)
        imagesc(testObj); axis image ij; colorbar; caxis(caxObj);
        title(sprintf('%s - Objects (testObj)', model_prefix_str));
        
        if SAVE_PLOTS
            str = sprintf('%s_mask', model_prefix);
            saveas(gcf, fullfile(saveDir, sprintf('%s.%s', str, 'png')));
            if SAVE_FIGS
                saveas(gcf, fullfile(saveDir, sprintf('%s.%s', str, 'fig')));
            end
        end
        
        fprintf('%0.3f\n', toc(btic));
    end
    
    
    
    tTestPrepTime = toc(testtic);
    
    fprintf('END Prepping Test Data...     (%0.3f)\n', tTestPrepTime);
    
    %% Compute Metrics
    
    % ----- Load Performer Data -------------------------------------------
    fprintf('START Metric Computation...'); metrictic = tic;
    
    
    % Inputs: refMask, refObj, testMask, testObj
    
    
    refOnlyMask  = double(refMask  & ~testMask);
    testOnlyMask = double(testMask & ~refMask);
    overlapMask  = double(refMask  &  testMask);
    
    
    unitArea = abs(refInfo.RefMatrix(1,2)*refInfo.RefMatrix(2,1));
    
    
    % --- Hard Error ------------------------------------------------------
    % -------- False Positive ---------------------------------------------
    unitCountFP = sum(sum(testOnlyMask,1),2);
    oobFP = nansum(nansum(testOnlyMask.*testObj,1),2).*unitArea;
    
    % -------- False Negative ---------------------------------------------
    unitCountFN = sum(sum(refOnlyMask,1),2);
    oobFN = nansum(nansum(refOnlyMask.*refObj,1),2).*unitArea;
    
    
    % --- Soft Error ------------------------------------------------------
    overlap = overlapMask .* (testObj - refObj);
    unitCountTP = sum(sum(~isnan(overlap),1),2);
    
    % -------- False Positive -------------------------------------------------
    FP =  nansum(nansum( (overlap > 0) .* overlap,1),2).*unitArea;
    
    % -------- False Negative -------------------------------------------------
    FN = -nansum(nansum( (overlap < 0) .* overlap,1),2).*unitArea;
    
    % -------- True Positve ---------------------------------------------------
    TP =  nansum(nansum( refObj .* overlapMask,1),2).*unitArea - FN;
    
    tolFP = FP + oobFP;
    tolFN = FN + oobFN;
    tolTP = TP;
    
    mop2D     = calcMops(unitCountTP, unitCountFN, unitCountFP);
    mop3D     = calcMops(tolTP, tolFN, tolFP);
    mop3Dsoft = calcMops(TP, FN, FP);
    
    
    metrics2D{loopPerformer} = mop2D;
    metrics3D{loopPerformer} = mop3D;
    metrics3Ds{loopPerformer} = mop3Dsoft;
    
    typeMap = zeros(size(refObj));
    typeMap(refOnlyMask == 1)  = 1;   % False Negetive
    typeMap(testOnlyMask == 1) = 2;   % False Positive
    typeMap(overlapMask == 1)  = 3;   % True Positive
    
    
    fprintf('\n====================================\n');
    fprintf('2D Metrics\n');
    
    fprintf('\tFalse Positive =\t% 9.1f (m^2)\n', unitCountFP.*unitArea);
    fprintf('\tFalse Negetive =\t% 9.1f (m^2)\n', unitCountFN.*unitArea);
    fprintf('\tTrue Positive  =\t% 9.1f (m^2)\n', unitCountTP.*unitArea);
    fprintf('\tCompleteness =\t%0.5f\n', mop2D.completeness);
    fprintf('\tCorrectness =\t%0.5f\n', mop2D.correctness);
    
    fprintf('\n3D Metrics\n');
    fprintf('\tFalse Positive =\t% 9.1f (m^3)\n', tolFP);
    fprintf('\tFalse Negetive =\t% 9.1f (m^3)\n', tolFN);
    fprintf('\tTrue Positive  =\t% 9.1f (m^3)\n', tolTP);
    
    fprintf('\tCompleteness =\t%0.5f\n', mop3D.completeness);
    fprintf('\tCorrectness =\t%0.5f\n',  mop3D.correctness);
    
    
    fprintf('\n====================================\n');
    
    
    % ---------- Make some plots ----------------------------------------------
    if DO_PLOTS
        
        fprintf('\tMaking plots of test data...'); btic = tic;
        
        figure(400); clf; cla; ax = [];
        ax(3) = subplot(1,3,1); imagesc(refOnlyMask); axis image ij; title('False Negative'); axis off
        ax(4) = subplot(1,3,2); imagesc(testOnlyMask); axis image ij; title('False Positive'); axis off
        ax(5) = subplot(1,3,3); imagesc(overlapMask); axis image ij; title('True Positive'); axis off
        linkaxes(ax);
        colormap summer
        if SAVE_PLOTS
            str = sprintf('%s_result', model_prefix);
            saveas(gcf, fullfile(saveDir, sprintf('%s.%s', str, 'png')))
            if SAVE_FIGS
                saveas(gcf, fullfile(saveDir, sprintf('%s.%s', str, 'fig')))
            end
        end
        
        
        
        figure(401)
        imagesc(testOnlyMask); axis image ij;
        title(sprintf('%s - Mask - tmpReg', model_prefix_str));
        
        if SAVE_PLOTS
            str = sprintf('%s_mask', model_prefix);
            saveas(gcf, fullfile(saveDir, sprintf('%s.%s', str, 'png')));
            if SAVE_FIGS
                saveas(gcf, fullfile(saveDir, sprintf('%s.%s', str, 'fig')));
            end
        end
        
        figure(402); clf; cla
        imagesc(typeMap); caxis([0 3]); colorbar; colormap([ [0.1 0.3 0.4].*1.2; 1 0.3 0.1; 1 1 0.3; 0.2 0.6 0.3])
        colorbar('Ticks',linspace(0.4,2.6,4),...
            'TickLabels',{'TN','FN','FP','TP'})
        title('Error - Categories')
        axis image ij off
        ax207 = gca;
        
        
        if SAVE_PLOTS
            str = sprintf('%s_result_label', model_prefix);
            saveas(gcf, fullfile(saveDir, sprintf('%s.%s', str, 'png')))
            if SAVE_FIGS
                saveas(gcf, fullfile(saveDir, sprintf('%s.%s', str, 'fig')))
            end
        end
        
        errorMap = nan(size(refObj));
        errorMap(testOnlyMask == 1) = testObj(testOnlyMask == 1);  % False Positive
        errorMap(refOnlyMask == 1)  = -refObj(refOnlyMask == 1);   % False Negetive
        errorMap(overlapMask == 1)  = overlap(overlapMask == 1);
        
        figure(403); cla
        imagesc(errorMap); colorbar
        cm = colormap;
        axis image ij
        title('Error - Height (meters)')
        ax208 = gca;
        
        
        if SAVE_PLOTS
            str = sprintf('%s_result_error', model_prefix);
            saveas(gcf, fullfile(saveDir, sprintf('%s.%s', str, 'png')))
            if SAVE_FIGS
                saveas(gcf, fullfile(saveDir, sprintf('%s.%s', str, 'fig')))
            end
        end
        
        
        
        upperLim =  5;
        lowerLim = -5;
        errorMap(errorMap > upperLim) = upperLim;
        errorMap(errorMap < lowerLim) = lowerLim;
        
        cm = jet(128);
        errorMap( isnan(errorMap) ) = lowerLim - (upperLim - lowerLim) / (size(colormap,1) - 1 );
        
        figure(404)
        imagesc(errorMap); colormap(cm); colorbar
        
        axis image ij
        title('Error - Height (meters)')
        ax208 = gca;
        
        cm(1,:) = [1 1 1]*0.2;
        colormap(cm);
        
        if SAVE_PLOTS
            str = sprintf('%s_result_error_clipped', model_prefix);
            saveas(gcf, fullfile(saveDir, sprintf('%s.%s', str, 'png')))
            if SAVE_FIGS
                saveas(gcf, fullfile(saveDir, sprintf('%s.%s', str, 'fig')))
            end
        end
        
    end
    
    fprintf('Done (%0.3f)\n', toc(btic));
    
    tMetricTime = toc(metrictic);
    fprintf('END Metric Computation...     (%0.3f)\n', tTestPrepTime);
    
end


toc(ticALL)