% Copyright 2017 The Johns Hopkins University Applied Physics Laboratory.
% Licensed under the MIT License. See LICENSE.txt in the project root for full license information.

% Extracts statistics from positional, curvature and surface elements of the input solution
% it compares them with Lidar (ground truth) statistics to report error
% Input:meshreference ~ data structure containing the ground truth mesh
% Input:meshinput1 ~ data structure containing the input mesh
% Input:pointspacing ~ GSD of ground truth data 
% Input:visualflag ~ flag indicating whether to plot figures
% Input:Surfacemetrics ~ flag indicating whether surface metrics are
% computed (Geom.Lapl. and Dihedral angle)
% Output:meshrefout ~ ground truth data structure that includes ground truth statistics extracted to compute metrics
% Output:meshinout1 ~ inpu data structure that includes ground truth statistics extracted to compute metrics
% Version History
%   2017/03/28  Marc Bosch       Initial version

function [meshrefout,meshinout1]=error_curvature(meshreference,meshinput1, ...
    pointspacing,visualflag,Surfacemetrics)
fprintf('Evaluation Started...\n');

%configuration
Allpoints=0;% if set to 1, use all points to calculate errors, not just valid points
medianon=0;% if set to 1, report median value instead of RMS

Classbuilding=6;% class label for buildings in AOI

 
dihed=1;%to compute dihedral angle metric
GLflag=1;%to compute geometric laplacian
lambda=2; %error>mean+lambda*std 
Thcomp = 1; %Th for completeness (meters)
ThsCUMUL = [0.1, 0.5, 1, 2, 5, 25];%in meters
ThsZPERC = [0.1, 0.25, 0.5, 0.75, 0.9, 0.99];% percentage of valid points
ThsmeshCUMUL = [0.01, 0.1, 0.25, 0.5, 1, 2, 5];% curvature thresholds


if Surfacemetrics~=1
    GLflag=0;
    dihed=0;
end


meshrefout = meshreference;
meshinout1 = meshinput1;
%assign Lidar vertex coordinates to separate buffers
referencepc.x=meshreference.node_xyz(1,:)';
referencepc.y=meshreference.node_xyz(2,:)';
referencepc.z=meshreference.node_xyz(3,:)';
%assign input data vertex coordinates to separate buffers
inputpc1.x=meshinput1.node_xyz(1,:)';
inputpc1.y=meshinput1.node_xyz(2,:)';
inputpc1.z=meshinput1.node_xyz(3,:)';



%%%Sort data by x axis to make point search faster downstream, face list is
%%%updated accordingly to the data re-arrangement
[inputpc1.x,posorted]=sort(inputpc1.x); 
inputpc1.y=inputpc1.y(posorted);
inputpc1.z=inputpc1.z(posorted);
[~,inverseposorted]=sort(posorted);
meshinout1.triangle_node(1,:)=inverseposorted(meshinout1.triangle_node(1,:));
meshinout1.triangle_node(2,:)=inverseposorted(meshinout1.triangle_node(2,:));
meshinout1.triangle_node(3,:)=inverseposorted(meshinout1.triangle_node(3,:));


%reference curvature statistics
%create 2D grid
minX=min(referencepc.x(:));
maxX=max(referencepc.x(:));
minY=min(referencepc.y(:));
maxY=max(referencepc.y(:));
dx=minX:pointspacing:maxX;
dy=minY:pointspacing:maxY;
minXref=minX;maxXref=maxX;
minYref=minY;maxYref=maxY;
[X,Y]=meshgrid(dx,dy);
[M,N]=size(X);
if abs(dx(end)-maxX)<pointspacing && abs(dx(end)-maxX)>pointspacing*0.1
    N=N+1;    
end
if abs(dy(end)-maxY)<pointspacing && abs(dy(end)-maxY)>pointspacing*0.1
    M=M+1;    
end
fprintf('Lidar mesh nodes projection into 2D-grid...\n');
tic;
X=imresize(X,[M N],'nearest');
Y=imresize(Y,[M N],'nearest');
%assign mesh nodes to the X-Y grid
searchoffset=2;
if floor(pointspacing/meshreference.estimated_pointspacing)>3 & ...
        floor(pointspacing/meshreference.estimated_pointspacing)<=5
    searchoffset=5;
elseif floor(pointspacing/meshreference.estimated_pointspacing)>5
    searchoffset=10;
end
[sz,validsgtgrid,idcorrespgt]=mexbuffer2gridC(X',Y',referencepc.x,referencepc.y,referencepc.z,searchoffset,pointspacing);
Z=reshape(sz,[N M])';
if visualflag
    figure,imagesc(Z),title(' Lidar Z field projected into Grid')
end
corres=reshape(idcorrespgt,[N M])';
%assign AOI mask to the X-Y grid

[AOImaskvec,~,~]=mexbuffer2gridC(X',Y',referencepc.x,referencepc.y,meshrefout.AOImask(1,:)',searchoffset,pointspacing);
AOImask=reshape(AOImaskvec,[N M])';


fprintf('Lidar fundamental moments extraction...\n');
[K1,H1,Cmax1,Cmin1,normsurf] = fundmoments(X,Y,Z,lambda,M,N);
%copy results into Lidar data structure
meshrefout.stats.K=K1;
meshrefout.stats.H=H1;
meshrefout.stats.Cmax=Cmax1;
meshrefout.stats.Cmin=Cmin1;
shapeindex1=(2/3.1416)*atan((meshrefout.stats.Cmax+meshrefout.stats.Cmin)./ ...
    (meshrefout.stats.Cmin-meshrefout.stats.Cmax));
meshrefout.stats.shapeindex=shapeindex1;
meshrefout.stats.normsurf=normsurf;
meshrefout.stats.X=X;
meshrefout.stats.Y=Y;
meshrefout.stats.Z=Z;
[M,N]=size(X);
toc;

%input solution curvature statistics
%create 2D grid
fprintf('Input mesh nodes projection into 2D-grid...\n');
fprintf('Input mesh fundamental moments extraction...\n');
tic;
minX=min(inputpc1.x(:));
maxX=max(inputpc1.x(:));
minY=min(inputpc1.y(:));
maxY=max(inputpc1.y(:));
[X2,Y2]=meshgrid(minX:pointspacing:maxX,minY:pointspacing:maxY);
[M2,N2]=size(X2);

%pc input curvature statistics
searchoffset=3;
a=inputpc1.y(1:end);
b=[inputpc1.y(2:end);min(inputpc1.y(1:end))];
estimated_pointspacing = median(abs(a-b));
if floor(pointspacing/estimated_pointspacing)>3 & ...
        floor(pointspacing/estimated_pointspacing)<=5
    searchoffset=20;
elseif floor(pointspacing/estimated_pointspacing)>5
    searchoffset=50;
end
[Z2grid,validsgrid,idcorresp] = mexbuffer2gridC(X2,Y2,inputpc1.x,inputpc1.y,inputpc1.z, ...
    searchoffset,pointspacing);
Z2=reshape(Z2grid,[M2 N2]);

if visualflag
    figure,imagesc(Z2),title('Input solution Z field projected into Grid')
end
%fprintf('Input mesh fundamental moments extraction...\n');
[K2,H2,Cmax2,Cmin2,normsurf] = fundmoments(X2,Y2,Z2,lambda,M2,N2);
%copy results into input data structure
meshinout1.stats.K=K2;
meshinout1.stats.H=H2;
meshinout1.stats.Cmax=Cmax2;
meshinout1.stats.Cmin=Cmin2;
shapeindex2=(2/3.1416)*atan((meshinout1.stats.Cmax+meshinout1.stats.Cmin)./ ...
    (meshinout1.stats.Cmin-meshinout1.stats.Cmax));
meshinout1.stats.shapeindex=shapeindex2;
meshinout1.stats.normsurf=normsurf;
meshinout1.stats.X=X2;
meshinout1.stats.Y=Y2;
meshinout1.stats.Z=Z2;
toc;


%find point pair correspondence in the ground truth
%indexpoint contains gridded point correspondence
fprintf('Assign closest ground truth node to input mesh nodes...\n');
tic;
[indexpoint,valids] = mexfindpointreferenceC(X2(:)',Y2(:)',Z2(:)',X',Y',Z', ...
    25,N2,M2,pointspacing);
toc;
%convert to row,col index
[cc,rr]=ind2sub([size(X,2);size(X,1)],indexpoint);
%convert to index sorted by column
indexpointcolmajor=sub2ind(size(X),rr,cc);
%project the Z-Lidar grid into input data grid given the point
%correspondences
kk=Z(indexpointcolmajor);
compensatedgt=reshape(kk,[M2 N2]);


%SURFACE SPECIFIC METRICS
fprintf('Computing Surface Metrics...\n');
if GLflag==1
    tic;
    
    %to be within integer precision and avoid buffer overflown:
    MaxFaces=min(floor(2^30/max(meshinout1.triangle_node(:))),floor(2^30/max(meshrefout.triangle_node(:))));    
    Nnodes=length(inputpc1.x);
    Nnodesref=length(referencepc.x);
    fprintf('Computing Geometric Laplacian for Input and Ground truth...\n');
    [glx,gly,glz,glxgt,glygt,glzgt]= mexGeomLapErrorC( ...
        meshinout1.triangle_node,meshrefout.triangle_node,...
        max(meshinout1.triangle_node(:)),max(meshrefout.triangle_node(:)),Nnodes,Nnodesref,MaxFaces, ...
        inputpc1.x,inputpc1.y,inputpc1.z,referencepc.x,referencepc.y,referencepc.z);
    
    indexpointGL=indexpointcolmajor;
    %project the input gl values into the 2D grid (visualization purposes)
    corres=reshape(idcorresp,[M2 N2]);
    meshinout1.stats.GL(:,1)=glx(corres(:)+1);
    meshinout1.stats.GL(:,2)=gly(corres(:)+1);
    meshinout1.stats.GL(:,3)=glz(corres(:)+1);
    %project the lidar gl values into the 2D grid (visualization purposes)
    corres=reshape(idcorrespgt,[N M])';
    meshrefout.stats.GL(:,1)=glxgt(corres(:)+1);
    meshrefout.stats.GL(:,2)=glygt(corres(:)+1);
    meshrefout.stats.GL(:,3)=glzgt(corres(:)+1);

    toc;

   
    

else
    indexpointGL=indexpointcolmajor;
    meshrefout.stats.GL(1:length(X(:)),1:3)=0;
    meshinout1.stats.GL(1:length(X2(:)),1:3)=0;
    errorGL=NaN;
    validGL=NaN;
end
    
if dihed==1
    fprintf('Computing Dihedral Angle for Input and Ground truth...\n');
    Nnodes=length(inputpc1.x);
    Nnodesref=length(referencepc.x);
    MaxFaces=150;
    %to be within integer precision and avoid buffer overflown:
    MaxFaces=min(floor(2^30/max(meshinout1.triangle_node(:))),floor(2^30/max(meshrefout.triangle_node(:))));  
    [DiAn]=mexDihAngC(meshinout1.triangle_node, max(meshinout1.triangle_node(:)), ...
        Nnodes,MaxFaces, inputpc1.x,inputpc1.y,inputpc1.z,100);
    [DiAnref]=mexDihAngC(meshrefout.triangle_node, max(meshrefout.triangle_node(:)), ...
        Nnodesref,MaxFaces, referencepc.x,referencepc.y,referencepc.z,100);
   
     indexpointDiAn=indexpointcolmajor;
     %project the input gl values into the 2D grid (visualization purposes)
     corres=reshape(idcorresp,[M2 N2]);
     meshinout1.stats.DiAn=DiAn(corres(:)+1);
     %project the lidar gl values into the 2D grid (visualization purposes)
     corres=reshape(idcorrespgt,[N M])';
     meshrefout.stats.DiAn=DiAnref(corres(:)+1);
     

    
else
    meshrefout.stats.DiAn(1:length(X(:)),1:3)=0;
    meshinout1.stats.DiAn(1:length(X2(:)),1:3)=0;
    errorDiAn=NaN;
    validDiAn=NaN;
end

toc;
tic;
%%%%ERRORS
 fprintf('Computing errors...\n');
 %curvature and surface
[errorK,errorH,errorCmax,errorCmin,errorSI,errorGL,errorDiAn ...
    validK,validH,validCmax,validCmin,validSI,validGL,validDiAn] = mexErrorCurvatureMetricsC(indexpoint, ...
    K1',H1',Cmax1',Cmin1',shapeindex1', meshrefout.stats.GL',meshrefout.stats.DiAn', ...
    K2(:)',H2(:)',Cmax2(:)',Cmin2(:)',shapeindex2(:)', meshinout1.stats.GL',...
     meshinout1.stats.DiAn',lambda,indexpointGL);
%contains the buffer with the valid (within pointspacing distance) input values of the 2D grid (X-Y plane)
validpc=reshape(validsgrid,[M2 N2]);


if Allpoints==1
    validsgrid=ones(size(validsgrid,1),size(validsgrid,2));
    validpc=ones(size(validpc,1),size(validpc,2));
%     valids=ones(size(valids,1),size(valids,2));
end


%convert to input grid correspondences


%contains the buffer with the valid (within pointspacing distance) lidar values of the 2D grid (X-Y plane)
validsgtgridsz=reshape(validsgtgrid,[N M])';
%project into the 2D grid of the input data
kk=validsgtgridsz(indexpointcolmajor);
%convert from 1D to 2D array
validsgtgrid2=reshape(kk,[M2 N2]);
%update the valid pairs of input-lidar correspondences given mexfindpointreferenceC
valids(validsgtgrid2==0)=0;
%copy of error buffer into input data structure
meshinout1.errorKpp=errorK;
meshinout1.errorHpp=errorH;
meshinout1.errorCmaxpp=errorCmax;
meshinout1.errorCminpp=errorCmin;
meshinout1.errorSIpp=errorSI;
meshinout1.errorGLpp=errorGL;
meshinout1.errorDiAnpp=errorDiAn;
meshinout1.M=M2;
meshinout1.N=N2;

% figure,imagesc(reshape(meshinout1.errorGL,[M2 N2]))

% calculate the error value given the validity map. Average and error
% distributions at different thresholds are reported
% Gaussian curvature error
validK(valids==0)=0;%error larger than mean+lambda*std
validK(validpc==0)=0;%point not available in input mesh
%errorK=sum(errorK(validK==1))/sum(validK);
if medianon==1
    errorK=median(errorK(validK==1));
else
    errorK=sum(errorK(validK==1))/sum(validK);
end
data=meshinout1.errorKpp(validK==1);
meshinout1.errorKprofile = errorprofiling(data,ThsmeshCUMUL,ThsZPERC);

% Mean curvature error
validH(valids==0)=0;%error larger than mean+lambda*std
validH(validpc==0)=0;%point not available in input mesh
%errorH=sum(errorH(validH==1))/sum(validH);
if medianon==1
    errorH=median(errorH(validH==1));
else
    errorH=sum(errorH(validH==1))/sum(validH);
end
data=meshinout1.errorHpp(validH==1);
meshinout1.errorHprofile = errorprofiling(data,ThsmeshCUMUL,ThsZPERC);

% Max curvature error
validCmax(valids==0)=0;%error larger than mean+lambda*std
validCmax(validpc==0)=0;%point not available in input mesh
%errorCmax=sum(errorCmax(validCmax==1))/sum(validCmax);
if medianon==1
    errorCmax=median(errorCmax(validCmax==1));
else
    errorCmax=sum(errorCmax(validCmax==1))/sum(validCmax);
end
data=meshinout1.errorCmaxpp(validCmax==1);
meshinout1.errorCmaxprofile = errorprofiling(data,ThsmeshCUMUL,ThsZPERC);

% Min curvature error
validCmin(valids==0)=0;%error larger than mean+lambda*std
validCmin(validpc==0)=0;%point not available in input mesh
%errorCmin=sum(errorCmin(validCmin==1))/sum(validCmin);
if medianon==1
    errorCmin=median(errorCmin(validCmin==1));
else
    errorCmin=sum(errorCmin(validCmin==1))/sum(validCmin);
end
data=meshinout1.errorCminpp(validCmin==1);
meshinout1.errorCminprofile = errorprofiling(data,ThsmeshCUMUL,ThsZPERC);

validSI(isnan(errorSI))=0;
validSI(valids==0)=0;%error larger than mean+lambda*std
validSI(validpc==0)=0;%point not available in input mesh
%errorK=sum(errorK(validK==1))/sum(validK);
if medianon==1
    errorSI=median(errorSI(validSI==1));
else
    errorSI=sum(errorSI(validSI==1))/sum(validSI);
end

% Geometric laplacian error
validGL(valids==0)=0;%error larger than mean+lambda*std
validGL(validpc==0)=0;%point not available in input mesh
if medianon==1
    errorGL=median(errorGL(validGL==1));
else
    errorGL=sum(errorGL(validGL==1))/sum(validGL);
end
data=meshinout1.errorGLpp(validGL==1);
meshinout1.errorGLprofile = errorprofiling(data,ThsmeshCUMUL,ThsZPERC);

% Dihedral Angle error
validDiAn(valids==0)=0;%error larger than mean+lambda*std
validDiAn(validpc==0)=0;%point not available in input mesh
%errorDiAn=sum(errorDiAn(validDiAn==1))/sum(validDiAn);
if medianon==1
    errorDiAn=median(errorDiAn(validDiAn==1));
else
    errorDiAn=sum(errorDiAn(validDiAn==1))/sum(validDiAn);
end
data=meshinout1.errorDiAnpp(validDiAn==1);
meshinout1.errorDiAnprofile = errorprofiling(data,ThsmeshCUMUL,ThsZPERC);

%Positional Accuracy error
[errorGriddedRMSpow2,validGriddedRMS] = mexErrorGridded3DAccuracyMetricsC(indexpointcolmajor,X(:),Y(:),Z(:), ...
    X2(:),Y2(:),Z2(:),lambda);
errorGriddedRMSperpoint=sqrt(errorGriddedRMSpow2);
meshinout1.errorGriddedRMSpp=errorGriddedRMSperpoint;

meshinout1.errorRMSpp=errorGriddedRMSperpoint;
validGriddedRMS(valids==0)=0;%error larger than mean+lambda*std
validGriddedRMS(validpc==0)=0;%point not available in input mesh
errorGriddedRMS=sqrt(sum(errorGriddedRMSpow2(validGriddedRMS==1))/sum(validGriddedRMS));
errorRMS=errorGriddedRMS;

sorted_errorRMS=sort(errorGriddedRMSperpoint(validGriddedRMS==1),'ascend');
for ij=1:length(ThsZPERC)
    pos = find(sorted_errorRMS<=ThsZPERC(ij));
    if isempty(pos)==1
      errorZPERC(ij)=NaN;
    else
        errorZPERC(ij) = sorted_errorRMS(round(ThsZPERC(ij)*length(sorted_errorRMS)));
    end
end

%edges for walls and vertical structures
validGriddedRMSEdges=validGriddedRMS;
%calculate edges and count them by half
[gx,gy] = derivative5(compensatedgt,'x','y');
gradmag=gx.^2+gy.^2;
validGriddedRMSEdges(gradmag>mean(gradmag(:))+0.05*std(gradmag(:)))=0.5;
validGriddedRMSEdges=reshape(validGriddedRMS,[M2 N2]).*reshape(validGriddedRMSEdges,[M2 N2]);
maskedges=zeros(M2,N2);
maskedges(validpc==0)=-1;
maskedges(valids==0)=-1;
maskedges(validGriddedRMS==0)=-1;
maskedges(validGriddedRMSEdges==0.5)=1;
maskedges=imdilate(maskedges,ones(3,3));
errorRMSwoEdges=sqrt(sum(errorGriddedRMSpow2(maskedges==0))/length(find(maskedges==0)));
errorRMSonEdges=sqrt(sum(errorGriddedRMSpow2(maskedges==1))/length(find(maskedges==1)));


%curvature/surface metrics without edges
%gaussian
mm=(reshape(validK,[M2 N2]));
med=zeros(M2,N2);
med(maskedges==0)=1;
dataerror=reshape(meshinout1.errorKpp,[M2 N2]);
data = dataerror(mm.*med==1);
meshinout1.errorKprofilewoedges = errorprofiling(data,ThsmeshCUMUL,ThsZPERC);
meshinout1.errorKprofilewoedgesData = data;
%mean
mm=(reshape(validH,[M2 N2]));
med=zeros(M2,N2);
med(maskedges==0)=1;
dataerror=reshape(meshinout1.errorHpp,[M2 N2]);
data = dataerror(mm.*med==1);
meshinout1.errorHprofilewoedges = errorprofiling(data,ThsmeshCUMUL,ThsZPERC);
meshinout1.errorHprofilewoedgesData = data;
%max
mm=(reshape(validCmax,[M2 N2]));
med=zeros(M2,N2);
med(maskedges==0)=1;
dataerror=reshape(meshinout1.errorCmaxpp,[M2 N2]);
data = dataerror(mm.*med==1);
meshinout1.errorCmaxprofilewoedges = errorprofiling(data,ThsmeshCUMUL,ThsZPERC);
meshinout1.errorCmaxprofilewoedgesData = data;
%min
mm=(reshape(validCmin,[M2 N2]));
med=zeros(M2,N2);
med(maskedges==0)=1;
dataerror=reshape(meshinout1.errorCminpp,[M2 N2]);
data = dataerror(mm.*med==1);
meshinout1.errorCminprofilewoedges = errorprofiling(data,ThsmeshCUMUL,ThsZPERC);
meshinout1.errorCminprofilewoedgesData = data;
%dihedral angle
mm=(reshape(validDiAn,[M2 N2]));
med=zeros(M2,N2);
med(maskedges==0)=1;
dataerror=reshape(meshinout1.errorDiAnpp,[M2 N2]);
data = dataerror(mm.*med==1);
meshinout1.errorDiAnprofilewoedges = errorprofiling(data,ThsmeshCUMUL,ThsZPERC);
meshinout1.errorDiAnprofilewoedgesData = data;
%geometric laplacian
mm=(reshape(validGL,[M2 N2]));
med=zeros(M2,N2);
med(maskedges==0)=1;
dataerror=reshape(meshinout1.errorGLpp,[M2 N2]);
data = dataerror(mm.*med==1);
meshinout1.errorGLprofilewoedges = errorprofiling(data,ThsCUMUL,ThsZPERC);
meshinout1.errorGLprofilewoedgesData = data;


%completeness error
[indexpointin2ref,validsin2ref] = mexfindpointreferenceC(X(:)',Y(:)',Z(:)',X2',Y2',Z2', ...
    25,N,M,pointspacing);
[cc,rr]=ind2sub([size(X2,2);size(X2,1)],indexpointin2ref);
indexpointcolmajorin2ref=sub2ind(size(X2),rr,cc);
kk=Z2(indexpointcolmajorin2ref);
compensatedgt=reshape(kk,[M N]);
Zerrcomplete=abs(Z-compensatedgt);
validsgridin2ref=validsgrid(indexpointcolmajorin2ref);
validsgridin2ref=reshape(validsgridin2ref,[M N]).*reshape(validsin2ref,[M N]).*validsgtgridsz;


%debugging plots
if visualflag
    figure,subplot(3,2,1),imagesc(Z2),title('Input Z footprint')
    subplot(3,2,2),imagesc(compensatedgt),title('Input Z projected to gt grid')
    subplot(3,2,3),imagesc(reshape(valids,[M2 N2])),title('mask with valid points')
    subplot(3,2,4),imagesc(Z2.*reshape(validsgrid,[M2 N2])),title('Input Z with valid points')
    subplot(3,2,5),imagesc(Z),title('Ground truth Z footprint in gt grid')
    subplot(3,2,6),imagesc(compensatedgt.*validsgridin2ref),title('Input Z for completeness')
end
errorCOMP = length(find(Zerrcomplete(validsgridin2ref==1)<Thcomp))/sum(validsgtgrid(:));

for ij=1:length(ThsCUMUL)
    errorCUMUL(ij) = length(find(Zerrcomplete(validsgridin2ref==1)<ThsCUMUL(ij)))/sum(validsgtgrid(:));
end

meshinout1.validpc=validpc;
meshinout1.valids=valids;





fprintf('AOI metric computations...\n');
AOImask=validsgtgridsz.*AOImask;%account for the points without grid correspondence
%compute AOI mask validity on the input grid
kk=AOImask(indexpointcolmajor);
AOImask2temp=reshape(kk,[M2 N2]);
validAOI=(AOImask2temp.*reshape(valids,[M2 N2]));
%compute AOI projection on the Lidar grid
corres=reshape(idcorresp,[M2 N2]);
AOImask2input=zeros(length(inputpc1.x),1);
for ij=1:M2
    for ji=1:N2
        AOImask2input(corres(ij,ji)+1,1) = validAOI(ij,ji);
    end
end

% Gaussian curvature error
validK(validAOI==0)=0;
errorKAOI=sum(meshinout1.errorKpp(validK==1))/sum(validK);
if medianon==1
    errorKAOI=median(meshinout1.errorKpp(validK==1));
end
data=meshinout1.errorKpp(validK==1);
meshinout1.errorKAOIprofile = errorprofiling(data,ThsmeshCUMUL,ThsZPERC);
% Mean curvature error
validH(validAOI==0)=0;
errorHAOI=sum(meshinout1.errorHpp(validH==1))/sum(validH);
if medianon==1
    errorHAOI=median(meshinout1.errorHpp(validH==1));
end
data=meshinout1.errorHpp(validH==1);
meshinout1.errorHAOIprofile = errorprofiling(data,ThsmeshCUMUL,ThsZPERC);
% max curvature error
validCmax(validAOI==0)=0;
errorCmaxAOI=sum(meshinout1.errorCmaxpp(validCmax==1))/sum(validCmax);
if medianon==1
    errorCmaxAOI=median(meshinout1.errorCmaxpp(validCmax==1));
end
data=meshinout1.errorCmaxpp(validCmax==1);
meshinout1.errorCmaxAOIprofile = errorprofiling(data,ThsmeshCUMUL,ThsZPERC);
% min curvature error
validCmin(validAOI==0)=0;
errorCminAOI=sum(meshinout1.errorCminpp(validCmin==1))/sum(validCmin);
if medianon==1
    errorCminAOI=median(meshinout1.errorCminpp(validCmin==1));
end
data=meshinout1.errorCminpp(validCmin==1);
meshinout1.errorCminAOIprofile = errorprofiling(data,ThsmeshCUMUL,ThsZPERC);

validSImin(validAOI==0)=0;
errorSIAOI=sum(meshinout1.errorSIpp(validSI==1))/sum(validSI);
if medianon==1
    errorSIAOI=median(meshinout1.errorSIpp(validSI==1));
end
% geometric laplacian curvature error
validGL(validAOI==0)=0;
errorGLAOI=sum(meshinout1.errorGLpp(validGL==1))/sum(validGL);
if medianon==1
    errorGLAOI=median(meshinout1.errorGLpp(validGL==1));
end
data=meshinout1.errorGLpp(validGL==1);
meshinout1.errorGLAOIprofile = errorprofiling(data,ThsmeshCUMUL,ThsZPERC);
% dihedral angle curvature error
validDiAn(validAOI==0)=0;
errorDiAnAOI=sum(meshinout1.errorDiAnpp(validDiAn==1))/sum(validDiAn);
if medianon==1
    errorDiAnAOI=median(meshinout1.errorDiAnpp(validDiAn==1));
end
data=meshinout1.errorDiAnpp(validDiAn==1);
meshinout1.errorDiAnAOIprofile = errorprofiling(data,ThsCUMUL,ThsZPERC);

% Z rms error
validGriddedRMS(validAOI==0)=0;
errorGriddedRMSAOI=sqrt(sum(errorGriddedRMSpow2(validGriddedRMS==1))/sum(validGriddedRMS));
errorRMSAOI=errorGriddedRMSAOI;

% Z completeness in AOI
errorCOMPAOI = length(find(Zerrcomplete(AOImask==Classbuilding)<Thcomp))/length(find(AOImask==Classbuilding));
%error at different accuracy levels
for ij=1:length(ThsCUMUL)
    errorCUMULAOI(ij) = length(find(Zerrcomplete(AOImask==Classbuilding)<ThsCUMUL(ij)))/ ...
        length(find(AOImask==Classbuilding));
end

% Z completeness at different thresholds in AOI
sorted_errorRMS=sort(errorGriddedRMSperpoint(validGriddedRMS==1),'ascend');
for ij=1:length(ThsZPERC)
    pos = find(sorted_errorRMS<=ThsZPERC(ij));
    if isempty(pos)==1
      errorZPERCAOI(ij)=NaN;
    else
        errorZPERCAOI(ij) = sorted_errorRMS(round(ThsZPERC(ij)*length(sorted_errorRMS)));
    end
end


%individual AOIs building average error
if visualflag
    CC=bwconncomp(AOImask,8);
    Nsgms=1;
    validAOIindv=validAOI;
    error_map=zeros(M,N);
    error_map_mean=zeros(M,N);
    error_map_GL=zeros(M,N);
    error_map_SI=zeros(M,N);
    error_map_RMS=zeros(M,N);
    error_map_COMP=zeros(M,N);
    for jji=1:CC.NumObjects
        mask_aoi_indv=zeros(M,N);
        pixpos=CC.PixelIdxList{jji};
        mask_aoi_indv(pixpos)=1;
        kk=mask_aoi_indv(indexpointcolmajor);
        AOImask2temp=reshape(kk,[M2 N2]);
        validAOIindv=(AOImask2temp.*reshape(valids,[M2 N2]));
       % switch metric_string
       %     case 'Mean'
                validindv=validH;
                validindv(validAOIindv==0)=0;
                errorHAOIindv=sum(meshinout1.errorHpp(validindv==1))/sum(validindv);
                error_map_mean(pixpos)=errorHAOIindv;
        %    case 'GL'
                validindv=validGL;
                validindv(validAOIindv==0)=0;
                errorGLAOIindv=sum(meshinout1.errorGLpp(validindv==1))/sum(validindv);
                error_map_GL(pixpos)=errorGLAOIindv;
        %    case 'RMS'
                validindv=validGriddedRMS;
                validindv(validAOIindv==0)=0;
                errorRMSAOIindv=sqrt(sum(errorGriddedRMSpow2(validindv==1))/sum(validindv));
                error_map_RMS(pixpos)=errorRMSAOIindv;
        %    case 'COMP'
                errorCOMPAOIindv = length(find(Zerrcomplete(mask_aoi_indv==1)<Thcomp))/length(find(mask_aoi_indv==1));
                error_map_COMP(pixpos) = errorCOMPAOIindv;
        %end

    end
    figure,imagesc(error_map_RMS),caxis([0 mean(error_map_RMS(:))+3*std(error_map_RMS(:))]),colorbar,colormap(jet),title('Average Z-RMS per AOI')
    figure,imagesc(error_map_mean),caxis([0 mean(error_map_mean(:))+3*std(error_map_mean(:))]),colorbar,colormap(jet),title('Average Mean Curvature per AOI')
%     figure,imagesc(error_map_SI),caxis([0 mean(error_map_SI(:))+3*std(error_map_SI(:))]),colorbar,colormap(jet),title('Shape index per AOI')
    if GLflag==1
        figure,imagesc(error_map_GL),caxis([0 mean(error_map_GL(:))+3*std(error_map_GL(:))]);colorbar,colormap(jet),title('Average Geom Lapl per AOI')
    end
    figure,imagesc(error_map_COMP),caxis([0 1]),colorbar,colormap(jet),title('Average Completeness (1m) per AOI')
end

fprintf('vertex count within the AOI...\n');
%vertex count within the boundaries
meshinout1.Nvertex = length(find(inputpc1.x>=minXref & inputpc1.x<=maxXref & ...
    inputpc1.y>=minYref & inputpc1.y<=maxYref));
meshinout1.Nvertexratio = meshinout1.Nvertex/length(referencepc.x);
fprintf('Semantic vertex count within the AOI...\n');
meshinout1.NSemanticvertex = length(find(inputpc1.x>=minXref & inputpc1.x<=maxXref & ...
    inputpc1.y>=minYref & inputpc1.y<=maxYref & AOImask2input==Classbuilding));
meshinout1.NSemanticvertexratio = meshinout1.NSemanticvertex/length(find(AOImask==Classbuilding));


% fprintf('geon count within the AOI...\n');
% [gt,truthRef]=geotiffread(gtAOIpath);
% CC = bwconncomp(gt>0,4);
% L = labelmatrix(CC);
% NgeonsLidar=length(unique(L));
% [AOIinputmask]=imread(meshinput1.shr3dAOIpath);
% AOIinputmask
% CC = bwconncomp(AOIinputmask>0,4);
% L = labelmatrix(CC);
% Ngeons=length(unique(L));
% meshinout1.Ngeons = Ngeons;
% meshinout1.Ngeonsratio = meshinout1.Ngeons/NgeonsLidar;

% imshu8(maskedges)

%write-out rest of outputs

meshinout1.errorgaussiancurvature=errorK;
meshinout1.errormeancurvature=errorH;
meshinout1.errormaxcurvature=errorCmax;
meshinout1.errormincurvature=errorCmin;
meshinout1.errorSI=errorSI;
meshinout1.errorGL=errorGL;
meshinout1.errorDA=errorDiAn;
meshinout1.errorRMS=errorRMS;
meshinout1.errorRMSGridded=errorGriddedRMS;
meshinout1.errorCUMUL=errorCUMUL;
meshinout1.errorZPERC=errorZPERC;
meshinout1.errorCOMP=errorCOMP;
meshinout1.errorRMSwoEdges=errorRMSwoEdges;
meshinout1.errorRMSonEdges=errorRMSonEdges;
meshinout1.errormeancurvatureAOI=errorKAOI;
meshinout1.errorgaussiancurvatureAOI=errorHAOI;
meshinout1.errormaxcurvatureAOI=errorCmaxAOI;
meshinout1.errormincurvatureAOI=errorCminAOI;
meshinout1.errorSIAOI=errorSIAOI;
meshinout1.errorGLAOI=errorGLAOI;
meshinout1.errorDAAOI=errorDiAnAOI;
meshinout1.errorRMSAOI=errorRMSAOI;
meshinout1.errorCUMULAOI=errorCUMULAOI;
meshinout1.errorZPERCAOI=errorZPERCAOI;
meshinout1.errorCOMPAOI=errorCOMPAOI;


%VISUAL ERRORS:
%figure,imagesc(reshape(H2,[M2 N2])),caxis([0 5])
% figure,imagesc(reshape((H2),[M2 N2]).*validpc.*reshape(valids,[M2 N2])),caxis([0 3])
% figure,imagesc(reshape((H1),[M N])),caxis([0 3])
% figure,imagesc(reshape(meshinout1.errorH,[M2 N2])),caxis([0 5])

if visualflag
    %these are just example visualizations... similar can be done for other
    %metrics
    figure,histogram(meshinout1.errorHpp),title('Histogram of Mean curvature error (full site)')
    if dihed==1
        figure,imagesc(reshape(meshinout1.stats.DiAn,[M2 N2])),caxis([0 90]),title('Dihedral angle of solution')
    end
    figure,imagesc(reshape(meshrefout.stats.H,[M N])),caxis([0 1]),title('Mean curvature of Lidar data')
    if GLflag==1
        figure,imagesc(reshape(validGL,[M2 N2]).*reshape(meshinout1.errorGLpp,[M2 N2])),caxis([0 1]), ...
            title('Geom. Laplacian error in AOI')
    end
end  
toc;
fprintf('Evaluation Finished...\n');