%% DEMO_aorta_build_passive_01
% Below is a demonstration for:
%
% * Building geometry for a subject-specific aorta
% * Creating the FEA mesh
% * Coding the abaqus structure

%% Keywords
%
% * ABAQUS
% * aorta
% * hexahedral elements, hex8

%%

clear; close all; clc;

%% Directory

saveOn=0;

%Define working directory
defaultFolder = fileparts(fileparts(mfilename('fullpath')));
savePath=fullfile(defaultFolder,'data','MAT');
fileName='Concannon_aorta_segmentation.mat';

%Define save names
abaqusInpFileNamePart='tempModel';
abaqusInpFileName=fullfile(savePath,[abaqusInpFileNamePart,'.inp']); %INP file name
matfileSaveName=fullfile(savePath,[abaqusInpFileNamePart,'.mat']); %INP file name

%Load data as structure
dataStruct=load(fullfile(savePath,fileName));

%% Control parameters
%Define smoothing Parameters
pointSpacing=1.7; %
numThickenSteps=2;
smoothFactorCentreLine=0.01; %Cubic smooth spline parameter [0-1] use empty to turn off
smoothFactorSegments=0.01; %Cubic smooth spline parameter [0-1], 0=straight line, 1=cubic
trunkSegmentReduceFactor=1; %

numSmoothTrunk=100; %
numSmoothPants_LAP=100; %Number of Laplacian smoothing iterations for Iliacs
numSmoothPants_HC=100; %Number of HC smoothing iterations for Iliacs
numSmoothBranchAttachments=100; %Number of smoothing iterations for branch origins (50)
numSmoothBranches=100; %Number of smoothing iterations for rest of branches
numOffsetSteps=1; %Number of steps taken to offset mesh by wall thickness
numSmoothOffset=2; %
methodSmoothOffset='LAP'; %Smoothing method Laplacian=agressive
qualityMetricSmoothOffset=120; %
distSmoothGrowth=10; %
numHexSplit=1; %Split elements by factor
t=1; %Time value corresponding to phase in cardiac cycle
%Compressibility factor (Nolan, McGarry)
kfactor=8.56;
%Define thickness information
dataStruct.WallThickness=[1.425	0.9	1	1.025	0.833333333	0.891666667	0.95	0.975	0.9	0.825];
thicknessIndexTransitionRegion=10; %Index of thickness at Iliac bifurcation
thicknessIndexBifurcation=10; %Index of thickness distal to Iliac bifurcation
thicknessIndicesBranches=[8 8 7 7 7 7 7]; %R_Renal_Ori,L_Renal_Ori,SMA_Ori,COE_Ori,LSA_Ori,LCCA_Ori,V_BCA_Ori
wallThickness=dataStruct.WallThickness; %Raw data for wall thickness as a function of location
numThicknessLevels=numel(wallThickness);
%Define material parameter information (from segment fitting [1-10])
numMaterials=100;
materialIndicesSelect=linspace(1,10,10); %Plane indices to interpolate between
dataStruct.mu=0.02; %Constant
dataStruct.k1=0.0001; %Constant
dataStruct.k2=1; %Constant
dataStruct.kappa=0; %Constant
dataStruct.theta1=90; %Circumferential
dataStruct.theta2=-90; %Circumferential
dataStruct.sigact=0; %Constant
dataStruct.ke=0.1; %Constant
dataStruct.Ee=[0.82 0.74 0.63 0.5 0.45 0.4 0.35 0.28 0.27 0.27];
dataStruct.thetaE1=90; %Circumferential
dataStruct.thetaE2=-90; %Circumferential
dataStruct.iswitch=1; %[1=local; 2=global] %Constant
dataStruct.xeps=[0.34 0.31 0.28 0.28 0.25 0.20 0.18 0.20 0.20 0.20];
dataStruct.Vcol=[0.20 0.31 0.28 0.30 0.44 0.44 0.44 0.44 0.45 0.52];
dataStruct.Vela=[0.30 0.28 0.25 0.22 0.22 0.22 0.24	0.20 0.18 0.16];
dataStruct.Vsmc=[0.20 0.20 0.20	0.23 0.24 0.25 0.26	0.26 0.27 0.27];
%Acces data
mu_data=dataStruct.mu;
k1_data=dataStruct.k1;
k2_data=dataStruct.k2;
kappa_data=dataStruct.kappa;
theta1_data=dataStruct.theta1;
theta2_data=dataStruct.theta2;
sigact_data=dataStruct.sigact;
ke_data=dataStruct.ke;
Ee_data=dataStruct.Ee;
thetaE1_data=dataStruct.thetaE1;
thetaE2_data=dataStruct.thetaE2;
xeps_data=dataStruct.xeps;
Vcol_data=dataStruct.Vcol;
Vela_data=dataStruct.Vela;
Vsmc_data=dataStruct.Vsmc;
iswitch=dataStruct.iswitch;
%Define plot color (black or white background)
colorMode=1;
switch colorMode
    case 1
        figStruct.ColorDef='white';
        figStruct.Color='w';
    case 2
        figStruct.ColorDef='black';
        figStruct.Color='k';
end

%% Access data structure components
V_cent=dataStruct.Cent; %Centroid list
segmentCell=dataStruct.Points; %Lumen boundary coordinates

%% Resampling aorta section contours
%Resample boundary points so each plane has same number of points for lofting
%Find number of points to use based on biggest circumference
d=zeros(size(segmentCell,2),1);
for indNow=1:1:size(segmentCell,2)
    Vs_1=segmentCell{t,indNow}';
    d(indNow)=max(pathLength(Vs_1));
end
nSegment=round(max(d)/pointSpacing);
%Resample
segmentCellSmooth=segmentCell;
segmentCellMean=segmentCell;
w=ones(size(V_cent,1),1); %Cubic smoothing spline weights
indexPlanePoints_V_cent=zeros(1,size(segmentCell,2)); %Indices of centre line points at sections
for indNow=1:1:size(segmentCell,2)
    %Resample section contour
    Vs_1=segmentCell{t,indNow}'; %Current contour
    Vs_1_smooth=evenlySampleCurve(Vs_1,nSegment,smoothFactorSegments,1); %Resample evenly
    Vs_1_mean=mean(Vs_1_smooth,1);
    segmentCellSmooth{t,indNow}=Vs_1_smooth';
    segmentCellMean{t,indNow}=Vs_1_mean;
    %Prepare for center line smoothing by setting weight vector
    [~,indVertex_1]=min(sqrt(sum((V_cent-Vs_1_mean(ones(size(V_cent,1),1),:)).^2,2))); %Index closest to section
    w(indVertex_1)=1e9; %Heigh weight at contour sections
    indexPlanePoints_V_cent(indNow)=indVertex_1; %Store index of closets
end

%% Smooth center line
%Fit smoothing spline through centreline points for loft
if ~isempty(smoothFactorCentreLine)
    V_cent_original=V_cent;
    d=pathLength(V_cent);
    V_cent = csaps(d,V_cent_original',smoothFactorCentreLine,d,w)'; %Smoothed
    cFigure(figStruct); hold on;
    h1=plotV(V_cent_original,'g.-','LineWidth',3,'MarkerSize',35);
    h2=plotV(V_cent,'r-','LineWidth',2,'MarkerSize',15);
    h3=plotV(V_cent(w==max(w),:),'b.','MarkerSize',40);
    legend([h1 h2 h3],{'Original','New','At contours'})
    axisGeom;
    drawnow;
end

%% Offsetting section curves outward if thickening is inward
segmentCellSmooth_pre=segmentCellSmooth;

for q=1:size(segmentCellSmooth,2)
    Vc=segmentCellSmooth{t,q}'; %Current curve vertices
    segmentCellSmooth{t,q}=curveOffset(Vc,wallThickness(q))';
end

%% Visualize offset curves
cFigure(figStruct); hold on;
plotV(V_cent,'b-','LineWidth',2,'MarkerSize',15);
for q=1:size(segmentCellSmooth,2)
    plotV(segmentCellSmooth_pre{t,q}','r.-','LineWidth',3,'MarkerSize',15);
    plotV(segmentCellSmooth{t,q}','g.-','LineWidth',3,'MarkerSize',15);
end
axisGeom;
drawnow;

%% Perform main trunk loft
%

% Initialize figure with center line
hf1=cFigure(figStruct); hold on;
plotV(V_cent,'b.-','LineWidth',3,'markerSize',25);
axisGeom;
camlight headlight;
drawnow;
hp=gobjects(11,1);
F_main_cell=cell(1,size(segmentCell,2)-1); %Faces cell
V_main_cell=cell(1,size(segmentCell,2)-1); %Vertex cell
C_main_gradient_cell=cell(1,size(segmentCell,2)-1); %Color/label dataa
segmentCurve_cell=cell(1,size(segmentCell,2)); %
% Eb_main_cell=cell(1,size(segmentCell,2)-1);
logicBoundary=false(0,0);
maxC=0;
indAdd=0;
%Loft from one plane to next in loop along aorta
for indNow=1:1:(size(segmentCell,2)-1)
    Vs_1=segmentCell{t,indNow}';
    Vs_1=Vs_1(1:end-1,:);
    Vs_2=segmentCell{t,indNow+1}';
    Vs_2(1:end-1,:);
    Vs_1_smooth=segmentCellSmooth{t,indNow}';
    Vs_2_smooth=segmentCellSmooth{t,indNow+1}';
    Vs_1_mean=segmentCellMean{t,indNow};
    Vs_2_mean=segmentCellMean{t,indNow+1};
    d=pathLength(V_cent);
    %Get curve part for lofting
    [~,indVertex_1]=min(sqrt(sum((V_cent-Vs_1_mean(ones(size(V_cent,1),1),:)).^2,2))); %Start
    [~,indVertex_2]=min(sqrt(sum((V_cent-Vs_2_mean(ones(size(V_cent,1),1),:)).^2,2))); %End
    V_cent_part=V_cent(indVertex_1:indVertex_2,:);
    nPart=round(max(pathLength(V_cent_part))/pointSpacing);
    [V_cent_part_smooth] = evenlySampleCurve(V_cent_part,nPart,'spline',0);
    Vs_1_smooth=Vs_1_smooth-Vs_1_mean(ones(size(Vs_1_smooth,1),1),:);
    Vs_1_smooth=Vs_1_smooth+V_cent_part_smooth(ones(size(Vs_1_smooth,1),1),:);
    Vs_2_smooth=Vs_2_smooth-Vs_2_mean(ones(size(Vs_2_smooth,1),1),:);
    Vs_2_smooth=Vs_2_smooth+V_cent_part_smooth(size(V_cent_part_smooth,1)*ones(size(Vs_2_smooth,1),1),:);
    v1=vecnormalize(V_cent_part_smooth(2,:)-V_cent_part_smooth(1,:));
    [Q]=pointSetPrincipalDir(Vs_1_smooth-Vs_1_mean(ones(size(Vs_1_smooth,1),1),:));
    n1=Q(:,3)';
    if dot(v1,n1)<0
        n1=-n1;
    end
    v2=vecnormalize(V_cent_part_smooth(end,:)-V_cent_part_smooth(end-1,:));
    [Q]=pointSetPrincipalDir(Vs_2_smooth-Vs_2_mean(ones(size(Vs_2_smooth,1),1),:));
    n2=Q(:,3)';
    if dot(v2,n2)<0
        n2=-n2;
    end
    plotOn=0;
    nLoft=round(nPart/trunkSegmentReduceFactor);
    [Fs,Vs,Cs]=sweepLoft(Vs_1_smooth,Vs_2_smooth,n1,n2,V_cent_part_smooth,nLoft,0,plotOn);
    indLoftBottom=1:nLoft:size(Vs,1);
    indLoftTop=(nLoft:nLoft:size(Vs,1));
    segmentCurve_cell{indNow}=indLoftBottom+indAdd;
    if indNow==(size(segmentCell,2)-1)
        segmentCurve_cell{indNow+1}=indLoftTop+indAdd;
    end
    indAdd=indAdd+size(Vs,1);
    Eb=patchBoundary(Fs,Vs);
    logicBoundaryNow=false(size(Vs,1),1);
    logicBoundaryNow(unique(Eb(:)))=1;
    F_main_cell{indNow}=Fs;
    V_main_cell{indNow}=Vs;
    %Store color gradient information
    C_main_gradient_cell{indNow}=Cs+maxC;
    maxC=maxC+max(Cs);
    logicBoundary=[logicBoundary; logicBoundaryNow];

    %%
    % Plot Loft of main trunk
    figure(hf1);
    gpatch(Fs,Vs,'kw','kw',0.85);
    plotV(V_cent_part_smooth,'m.-','LineWidth',2,'markerSize',15);
    plotV(Vs_1_smooth,'r-','LineWidth',2);
    plotV(V_cent(indVertex_1,:),'r+','markerSize',25);
    plotV(Vs_2_smooth,'r-','LineWidth',2);
    axis off;
    xlim([140 200]); ylim([120 200]); zlim([20 370]);
    drawnow;
end

%% Material model
%linearly interpolate parameters between planes
% Get center line distance metric for each plane
V_cent_crop=V_cent(indexPlanePoints_V_cent(1):indexPlanePoints_V_cent(end),:);
indexPlanePoints_V_cent_crop=(indexPlanePoints_V_cent-indexPlanePoints_V_cent(1))+1;
d=pathLength(V_cent_crop);
d=d./max(d(:)); %Normalised curve length
curveLengthPlanePoints=d(indexPlanePoints_V_cent_crop);

%mu_data=interp1(curveLengthPlanePoints(materialIndicesSelect),mu_data(materialIndicesSelect),curveLengthPlanePoints,'linear');
Ee_data=interp1(curveLengthPlanePoints(materialIndicesSelect),Ee_data(materialIndicesSelect),curveLengthPlanePoints,'linear');
xeps_data=interp1(curveLengthPlanePoints(materialIndicesSelect),xeps_data(materialIndicesSelect),curveLengthPlanePoints,'linear');
Vcol_data=interp1(curveLengthPlanePoints(materialIndicesSelect),Vcol_data(materialIndicesSelect),curveLengthPlanePoints,'linear');
Vela_data=interp1(curveLengthPlanePoints(materialIndicesSelect),Vela_data(materialIndicesSelect),curveLengthPlanePoints,'linear');

%% Main trunk
% Join element sets to form main trunk and form colour information
C_main=[];
for q=1:1:numel(F_main_cell)
    C_main=[C_main; q*ones(size(F_main_cell{q},1),1)];
end
[F_main,V_main,C_path]=joinElementSets(F_main_cell,V_main_cell,C_main_gradient_cell);
%Scale color gradient for wall thickness interpolation
C_path = rescale(C_path,1,numThicknessLevels);
F_main=fliplr(F_main); %Invert orientation
[F_main,V_main,ind1,indFix]=mergeVertices(F_main,V_main);
for q=1:1:numel(segmentCurve_cell)
    segmentCurve_cell{q}=indFix(segmentCurve_cell{q});
end
logicBoundary=logicBoundary(ind1);

% Perform Smoothing on main trunk
controlParameter.n=numSmoothTrunk;
controlParameter.Method='HC';
controlParameter.RigidConstraints=find(logicBoundary); %Ensure smoothing cannot change coordinates of planes of interest
[V_main]=patchSmooth(F_main,V_main,[],controlParameter);
% Plot
cFigure(figStruct);
subplot(1,2,1); hold on;
title('Segments')
plotV(V_cent,'k.-','LineWidth',3,'markerSize',25);
gpatch(F_main,V_main,C_main,'k');
patchNormPlot(F_main,V_main,1,'v'); %Check normals of elements all facing outward
for q=1:1:numel(segmentCurve_cell)
    plotV(V_main(segmentCurve_cell{q},:),'b.-','LineWidth',5,'markerSize',35);
end
plotV(V_main(logicBoundary,:),'r.','markerSize',25);
axisGeom;
colormap(gca,gjet(250)); icolorbar;
camlight headlight;
lighting gouraud;
subplot(1,2,2); hold on;
title('Continuous')
plotV(V_cent,'k.-','LineWidth',3,'markerSize',25);
gpatch(F_main,V_main,C_path,'k');
plotV(V_main(logicBoundary,:),'r.','markerSize',25);
% patchNormPlot(F_main,V_main,1,'v');
axisGeom;
colormap(gca,gjet(250)); colorbar;
camlight headlight;
lighting gouraud;
drawnow;

%%

E_rings=[F_main(:,[1 2]); fliplr(F_main(:,[3 4]))];
E_rings=uniqueIntegerRow(E_rings);

%% Load Iliac Branch Data
%Right Origin
V_R_ori=dataStruct.R_Ori(1:end-1,:);
V_R_ori=resampleCurve(V_R_ori,pointSpacing,1);
V_R_ori(:,1)=V_R_ori(:,1);
V_R_ori=curveOffset(V_R_ori,wallThickness(end));

%Left Origin
V_L_ori=dataStruct.L_Ori(1:end-1,:);
V_L_ori=resampleCurve(V_L_ori,pointSpacing,1);
V_L_ori(:,1)=V_L_ori(:,1)-1;
V_L_ori=curveOffset(V_L_ori,wallThickness(end));

%Left End
V_L_Iliac=dataStruct.L_Iliac(1:end-1,:);
V_L_Iliac=resampleCurve(V_L_Iliac,pointSpacing,1);
V_L_Iliac=curveOffset(V_L_Iliac,wallThickness(end));

%Right End
V_R_Iliac=dataStruct.R_Iliac(1:end-1,:);
V_R_Iliac=resampleCurve(V_R_Iliac,pointSpacing,1);
V_R_Iliac=curveOffset(V_R_Iliac,wallThickness(end));

%Bifurcation Data
V_bifurc=dataStruct.bifurc;
V_bifurc=curveOffset(V_bifurc,wallThickness(end));

%% Resample curve to have same number of points as main trunk for loft
Eb_all=patchBoundary(F_main,V_main);
Eb_lowerSegment=patchBoundary(F_main(C_main==max(C_main),:),V_main);
Eb_lower=Eb_lowerSegment(all(ismember(Eb_lowerSegment,Eb_all),2),:);
indLowerCurve=edgeListToCurve(Eb_lower);
indLowerCurve=indLowerCurve(1:end-1);
indLowerCurve=indLowerCurve(:);
V_main_lowerCurve=V_main(indLowerCurve,:);
if isPolyClockwise(V_bifurc)~=isPolyClockwise(V_main_lowerCurve)
    V_bifurc=flipud(V_bifurc);
end
V_bifurc=evenlySampleCurve(V_bifurc,size(V_main_lowerCurve,1),'spline',1); %resample
[~,indMin]=minDist(V_main_lowerCurve(1,:),V_bifurc);
if indMin>1
    V_bifurc=[V_bifurc(indMin:end,:); V_bifurc(1:indMin-1,:)];
end
% Mesh transition region
cPar.closeLoopOpt=1;
cPar.patchType='quad';
[F_add,V_add,indStart,indEndBifurc]=polyLoftLinear(V_main_lowerCurve,V_bifurc,cPar);
F_add=fliplr(F_add);
C_add=ones(size(F_add,1),1);
F_main=[F_main; F_add+size(V_main,1)];
indEndBifurc=indEndBifurc+size(V_main,1);
V_main=[V_main; V_add];
C_main=[C_main; C_add+max(C_main)];
C_path=[C_path; thicknessIndexTransitionRegion.*ones(size(C_add))];
[F_main,V_main,~,indFix]=mergeVertices(F_main,V_main);
indLowerCurve=indFix(indLowerCurve);
indEndBifurc=indFix(indEndBifurc);
E_rings=indFix(E_rings);
for q=1:1:numel(segmentCurve_cell)
    segmentCurve_cell{q}=indFix(segmentCurve_cell{q});
end

%% Perform Smoothing on Transition Region
indTouch=indLowerCurve(:);
numSmoothGrowSteps=3;
for q=1:1:numSmoothGrowSteps
    logicFacesTouch=any(ismember(F_main,indTouch),2);
    indTouch=F_main(logicFacesTouch,:);
end
smoothControlParameters.n=numSmoothPants_HC;
smoothControlParameters.Method='HC';
smoothControlParameters.RigidConstraints=[unique(F_main(~logicFacesTouch,:)); indLowerCurve];
[V_main]=patchSmooth(F_main,V_main,[],smoothControlParameters);

%%
% Plot Main and Transition Region
cFigure(figStruct); hold on;
gpatch(F_main,V_main,logicFacesTouch,'none');
patchNormPlot(F_main,V_main);
plotV(V_main(indLowerCurve,:),'r.-','markerSize',25);
plotV(V_main(indEndBifurc,:),'g.-','markerSize',25);
for q=1:1:numel(segmentCurve_cell)
    plotV(V_main(segmentCurve_cell{q},:),'b.-','LineWidth',5,'markerSize',35);
end
axisGeom;
colormap gjet;
camlight headlight;
lighting flat;
drawnow;

%% Pinched ellipse to two circle split
ns=ceil(abs(mean(V_main(indEndBifurc,3))-mean(V_R_ori(:,3)))/pointSpacing)+1;
V_cell={V_main(indEndBifurc,:),V_R_ori,V_L_ori};
patchType='quad';
splitMethod='nearMid';
controlParSmooth.Method='LAP';
controlParSmooth.n=numSmoothPants_LAP;
[F_split,V_split,curveIndices,C_split]=splitCurveSetMesh(V_cell,ns,patchType,controlParSmooth,splitMethod,1);

%%
% Plot
cFigure(figStruct); hold on;
% gpatch(F_main,V_main,'kw','k');
gpatch(F_split,V_split,C_split,'k');
patchNormPlot(F_split,V_split);
plotV(V_main(indEndBifurc,:),'r.-','markerSize',25);
% plotV(V_split(curveIndices{1},:),'g.-','markerSize',25);
axisGeom;
colormap gjet;
camlight headlight;
drawnow;

%% Ensure all points for curves are clockwise for loft
indEndBifurc_split=curveIndices{1}; indEndBifurc_split=indEndBifurc_split(:);
indBranch11=curveIndices{2}; indBranch11=indBranch11(:);
indBranch21=curveIndices{3}; indBranch21=indBranch21(:);
V_branch12=evenlySampleCurve(V_L_Iliac,numel(indBranch11),'spline',1);
V_branch22=evenlySampleCurve(V_R_Iliac,numel(indBranch21),'spline',1);
V_branch11=V_split(indBranch11,:);
if ~isPolyClockwise(V_branch11)
    indBranch11=flipud(indBranch11);
    V_branch11=V_split(indBranch11,:);
end
V_branch21=V_split(indBranch21,:);
if ~isPolyClockwise(V_branch21)
    indBranch21=flipud(indBranch21);
    V_branch21=V_split(indBranch21,:);
end
if ~isPolyClockwise(V_branch22)
    V_branch22=flipud(V_branch22);
end
if ~isPolyClockwise(V_branch12)
    V_branch12=flipud(V_branch12);
end
[~,indMin]=min(V_branch12(:,1));
if indMin>1
    V_branch12=[V_branch12(indMin:end,:); V_branch12(1:indMin-1,:)];
end
[~,indMin]=min(V_branch22(:,1));
if indMin>1
    V_branch22=[V_branch22(indMin:end,:); V_branch22(1:indMin-1,:)];
end

%% Define points for iliac extensions and Loft
V_loft_cell1{1}=V_branch11;
V_loft_cell2{1}=V_branch12;
V_loft_cell1{2}=V_branch21;
V_loft_cell2{2}=V_branch22;
V_loft_cent_cell{1}=dataStruct.Cent_I_R;
V_loft_cent_cell{2}=dataStruct.Cent_I_L;
F_iliac_cell=cell(1,2);
V_iliac_cell=cell(1,2);
for q=1:1:2
    V1=V_loft_cell1{q};
    V2=V_loft_cell2{q};
    Vc=V_loft_cent_cell{q};
    [~,indMin]=minDist(mean(V1,1),Vc);
    if indMin>1
        Vc=Vc(indMin:end,:);
    end
    Vc(1,:)=mean(V1,1); %Overide first point with mean of segment
    np=ceil(max(pathLength(Vc))/pointSpacing)+1;
    Vc = evenlySampleCurve(Vc,np,'spline',0);

    %%
    % plot loft paths and profile
    cFigure(figStruct); hold on;
    plotV(V1,'b.-','markerSize',25);
    plotV(V2,'r.-','markerSize',25);
    plotV(Vc,'g.-','markerSize',25);
    axisGeom;
    colormap gjet;
    camlight headlight;
    lighting gouraud;
    drawnow;

    %%
    V2 = evenlySampleCurve(V2,size(V1,1),'spline',1);
    v1=vecnormalize(Vc(2,:)-Vc(1,:));
    [Q]=pointSetPrincipalDir(V1-Vc(ones(size(V1,1),1),:));
    n1=Q(:,3)';
    if dot(v1,n1)<0
        n1=-n1;
    end
    v2=vecnormalize(Vc(end,:)-Vc(end-1,:));
    [Q]=pointSetPrincipalDir(V2-Vc(size(Vc,1)*ones(size(V2,1),1),:));
    n2=Q(:,3)';
    if dot(v2,n2)<0
        n2=-n2;
    end
    pointSpacingNow=mean(diff(pathLength(V1)));
    np=ceil(max(pathLength(Vc))/pointSpacingNow);
    Vc = evenlySampleCurve(Vc,np,'spline',0);
    [F_loft,V_loft,C_loft]=sweepLoft(V1,V2,n1,n2,Vc,size(Vc,1),0,0);
    F_iliac_cell{q}=F_loft;
    V_iliac_cell{q}=V_loft;
end
F_branch1=F_iliac_cell{1};
V_branch1=V_iliac_cell{1};
F_branch2=F_iliac_cell{2};
V_branch2=V_iliac_cell{2};

%%
% Plot Iliac extension Loft
cFigure(figStruct); hold on;
% gpatch(F_main,V_main,C_main,'k');
% gpatch(F_main(C_main==max(C_main),:),V_main,'kw','none');
gpatch(F_split,V_split,C_split,'k');
gpatch(F_branch1,V_branch1,'gw');
patchNormPlot(F_branch1,V_branch1);
gpatch(F_branch2,V_branch2,'bw');
patchNormPlot(F_branch2,V_branch2);
plotV(V_main(indEndBifurc,:),'r.-','markerSize',25);
plotV(V_branch11,'g.-','markerSize',25);
plotV(V_branch12,'g.-','markerSize',25);
plotV(V_branch21,'b.-','markerSize',25);
plotV(V_branch22,'b.-','markerSize',25);
axisGeom;
colormap gjet;
camlight headlight;
lighting gouraud;
drawnow;

%% Join Iliac extensions to Bifurcation
[Fp,Vp,Cp]=joinElementSets({F_split,F_branch1,F_branch2},{V_split,V_branch1,V_branch2});
[Fp,Vp,~,indFix]=mergeVertices(Fp,Vp);
Eb_p=patchBoundary(Fp,Vp);
indEndBifurc_split=indFix(indEndBifurc_split);
indBranch11=indFix(indBranch11);
indBranch21=indFix(indBranch21);
%%Smooth Iliacs
indTouch=[indBranch11(:); indBranch21(:)];
numSmoothGrowSteps=3;
for q=1:1:numSmoothGrowSteps
    logicFacesTouch=any(ismember(Fp,indTouch),2);
    indTouch=Fp(logicFacesTouch,:);
end
smoothControlParameters.n=numSmoothPants_HC;
smoothControlParameters.Method='HC';
smoothControlParameters.RigidConstraints=[unique(Fp(~logicFacesTouch,:)); indEndBifurc_split];
[Vp]=patchSmooth(Fp,Vp,[],smoothControlParameters);

%%
% Plot Iliacs
cFigure(figStruct); hold on;
gpatch(Fp,Vp,logicFacesTouch,'k');
patchNormPlot(Fp,Vp);
plotV(Vp(indBranch11,:),'r-','LineWidth',3)
plotV(Vp(indBranch21,:),'r-','LineWidth',3)
axisGeom;
colormap gjet;
camlight headlight;
lighting gouraud;
drawnow;

%%
% Add to main trunk
F_main=[F_main; Fp+size(V_main,1)];
indBranch11=indBranch11+size(V_main,1);
indBranch21=indBranch21+size(V_main,1);
V_main=[V_main; Vp];
C_main=[C_main; Cp+max(C_main)];
C_path=[C_path; thicknessIndexBifurcation.*ones(size(Cp))];
C_path_mat=C_path; %Copy over path color data for material assignments
[F_main,V_main,~,indFix]=mergeVertices(F_main,V_main);
indLowerCurve=indFix(indLowerCurve);
indBranch11=indFix(indBranch11);
indBranch21=indFix(indBranch21);
E_rings=indFix(E_rings);

for q=1:1:numel(segmentCurve_cell)
    segmentCurve_cell{q}=indFix(segmentCurve_cell{q});
end
%
indTouch=[indLowerCurve(:); indBranch11(:); indBranch21(:)];
nGrowthSteps=round(distSmoothGrowth/pointSpacing);
for q=1:1:numSmoothGrowSteps
    logicFacesTouch=any(ismember(F_main,indTouch),2);
    indTouch=F_main(logicFacesTouch,:);
end
ind1=F_main(~logicFacesTouch,:);
indRigid=unique([ind1(:); indBranch11(:); indBranch21(:)]);
smoothControlParameters.Tolerance=0.01;
smoothControlParameters.n=numSmoothPants_HC;
smoothControlParameters.Method='HC';
smoothControlParameters.RigidConstraints=indRigid;
[V_main]=patchSmooth(F_main,V_main,[],smoothControlParameters);
%
cFigure(figStruct); hold on;
gpatch(F_main,V_main,logicFacesTouch,'k');
% patchNormPlot(F_main,V_main);
plotV(V_main(indLowerCurve,:),'r.-','markerSize',25);
plotV(V_main(indBranch11,:),'r.-','markerSize',25);
plotV(V_main(indBranch21,:),'r.-','markerSize',25);
for q=1:1:numel(segmentCurve_cell)
    plotV(V_main(segmentCurve_cell{q},:),'b.-','LineWidth',5,'markerSize',35);
end
axisGeom;
colormap gjet;
camlight headlight;
lighting gouraud;
drawnow;

%% Load Branch data

%Other branches
V_L_Renal_Ori=dataStruct.L_Renal_Ori(1:end-1,:);
V_L_Renal_Ori=resampleCurve(V_L_Renal_Ori,pointSpacing,1);
V_L_Renal_Ori=curveOffset(V_L_Renal_Ori,wallThickness(7));
V_L_Renal=dataStruct.L_Renal(1:end-1,:);
V_L_Renal=resampleCurve(V_L_Renal,pointSpacing,1);
V_L_Renal=curveOffset(V_L_Renal,wallThickness(7));
V_Cent_L_Renal=dataStruct.Cent_Renal_L;
V_Cent_L_Renal=resampleCurve(V_Cent_L_Renal,pointSpacing,0);

V_R_Renal_Ori=dataStruct.R_Renal_Ori(1:end-1,:);
V_R_Renal_Ori=resampleCurve(V_R_Renal_Ori,pointSpacing,1);
V_R_Renal_Ori=curveOffset(V_R_Renal_Ori,wallThickness(7));

V_R_Renal=dataStruct.R_Renal(1:end-1,:);
V_R_Renal=resampleCurve(V_R_Renal,pointSpacing,1);
V_R_Renal=curveOffset(V_R_Renal,wallThickness(7));
V_Cent_R_Renal=dataStruct.Cent_Renal_R;
V_Cent_R_Renal=resampleCurve(V_Cent_R_Renal,pointSpacing,0);

V_SMA_Ori=dataStruct.SMA_O(1:end-1,:);
V_SMA_Ori=resampleCurve(V_SMA_Ori,pointSpacing,1);
V_SMA_Ori=curveOffset(V_SMA_Ori,wallThickness(7));

V_SMA=dataStruct.SMA(1:end-1,:);
V_SMA=resampleCurve(V_SMA,pointSpacing,1);
V_SMA=curveOffset(V_SMA,wallThickness(7));
V_Cent_SMA=dataStruct.Cent_SMA;
V_Cent_SMA=resampleCurve(V_Cent_SMA,pointSpacing,0);
V_COE_Ori=dataStruct.COE_O(1:end-1,:);
V_COE_Ori=resampleCurve(V_COE_Ori,pointSpacing,1);
V_COE_Ori=curveOffset(V_COE_Ori,wallThickness(7));
V_COE=dataStruct.COE(1:end-1,:);
V_COE=resampleCurve(V_COE,pointSpacing,1);
V_COE=curveOffset(V_COE,wallThickness(7));
V_Cent_COE=dataStruct.Cent_Coeliac;
V_Cent_COE=resampleCurve(V_Cent_COE,pointSpacing,0);
V_BCA_Ori=dataStruct.BCA_O(1:end-1,:);
V_BCA_Ori=resampleCurve(V_BCA_Ori,pointSpacing,1);
V_BCA_Ori=curveOffset(V_BCA_Ori,wallThickness(2));
V_BCA=dataStruct.BCA(1:end-1,:);
V_BCA=resampleCurve(V_BCA,pointSpacing,1);
V_BCA=curveOffset(V_BCA,wallThickness(2));
V_Cent_BCA=dataStruct.Cent_BCA;
V_Cent_BCA=resampleCurve(V_Cent_BCA,pointSpacing,0);
V_LCCA_Ori=dataStruct.LCCA_O(1:end-1,:);
V_LCCA_Ori=resampleCurve(V_LCCA_Ori,pointSpacing,1);
V_LCCA_Ori=curveOffset(V_LCCA_Ori,wallThickness(2));
V_LCCA=dataStruct.LCCA(1:end-1,:);
V_LCCA=resampleCurve(V_LCCA,pointSpacing,1);
V_LCCA=curveOffset(V_LCCA,wallThickness(2));
V_Cent_LCCA=dataStruct.Cent_LCCA;
V_Cent_LCCA=resampleCurve(V_Cent_LCCA,pointSpacing,0);
V_LSA_Ori=dataStruct.LSA_O(1:end-1,:);
V_LSA_Ori=resampleCurve(V_LSA_Ori,pointSpacing,1);
V_LSA_Ori=curveOffset(V_LSA_Ori,wallThickness(2));
V_LSA=dataStruct.LSA(1:end-1,:);
V_LSA=resampleCurve(V_LSA,pointSpacing,1);
V_LSA=curveOffset(V_LSA,wallThickness(2));
V_Cent_LSA=dataStruct.Cent_LSA;
V_Cent_LSA=resampleCurve(V_Cent_LSA,pointSpacing,0);

% Plot
cFigure(figStruct); hold on;
gpatch(F_main,V_main,C_main,0.65);
plotV(V_R_Renal_Ori,'r.-','markerSize',5);
plotV(V_R_Renal,'w.-','markerSize',5);
plotV(V_L_Renal_Ori,'g.-','markerSize',5);
plotV(V_L_Renal,'w.-','markerSize',5);
plotV(V_COE_Ori,'w.-','markerSize',5);
plotV(V_COE,'w.-','markerSize',5);
plotV(V_SMA_Ori,'w.-','markerSize',5);
plotV(V_SMA,'w.-','markerSize',5);
plotV(V_BCA_Ori,'w.-','markerSize',5);
plotV(V_BCA,'w.-','markerSize',5);
plotV(V_LCCA_Ori,'w.-','markerSize',5);
plotV(V_LCCA,'w.-','markerSize',5);
plotV(V_LSA_Ori,'w.-','markerSize',5);
plotV(V_LSA,'w.-','markerSize',5);
xlim([140 200]); ylim([120 200]); zlim([20 370]);
axisGeom;
colormap jet;
camlight headlight;
lighting gouraud;
drawnow;

%% Perform Extrude cut
% Find Centroid of Ori and shoot vector in direction of nearest Centreline
% point. Apply same vector to each point on Ori and every element on the
% main trunk wall it touches is deleted. Loft then from deleted elements of
% main to each branch ori
V_cut=V_R_Renal_Ori;
V_cut_cell={V_R_Renal_Ori,V_L_Renal_Ori,V_SMA_Ori,V_COE_Ori,...
    V_LSA_Ori,V_LCCA_Ori,V_BCA_Ori};
%Loop over connections
smoothControlParameters.n=numSmoothBranchAttachments;
smoothControlParameters.Method='HC';
hw = waitbar(0,'Please wait...');
numSteps=numel(V_cut_cell);
V_endCurve_cell=cell(1,numSteps);
C_main_max=max(C_main)+1;
for q=1:1:numSteps
    waitbar(q/numSteps,hw,['Processing case ',num2str(q),' of ',num2str(numSteps)]);
    VF=patchCentre(F_main,V_main); %Face centre coordinates
    [~,indMin]=minDist(mean(V_cut_cell{q},1),VF);
    c=C_path(indMin);
    [F_main,V_main,C_main,indEnd,logicRemoveFaces,segmentCurve_cell,E_rings]=circleCutExtrude(F_main,V_main,C_main,V_cent,V_cut_cell{q},pointSpacing,0,smoothControlParameters,segmentCurve_cell,E_rings);
    numFacesNewFeature=nnz(C_main==max(C_main));
    C_path=[C_path(~logicRemoveFaces); thicknessIndicesBranches(q)*ones(numFacesNewFeature,1)];
    C_path_mat=[C_path_mat(~logicRemoveFaces); c*ones(numFacesNewFeature,1)];
    V_endCurve_cell{q}=V_main(indEnd,:);
end
close(hw);
% plot
cFigure(figStruct); hold on;
gpatch(F_main,V_main,C_path,'k',1);
for q=1:1:numel(V_endCurve_cell)
    plotV(V_endCurve_cell{q},'m.-','markerSize',25);
end
axisGeom;
colormap gjet; colorbar;
camlight headlight;
drawnow;
% plot
cFigure(figStruct); hold on;
gpatch(F_main,V_main,C_path_mat,'k',1);
axisGeom;
colormap gjet; colorbar;
camlight headlight;
drawnow;

% Resample branch ends and sweep
V_loft_cell1=V_endCurve_cell;
V_loft_cell2={V_R_Renal,V_L_Renal,V_SMA,V_COE,...
    V_LSA,V_LCCA,V_BCA};
V_loft_cent_cell={V_Cent_R_Renal,V_Cent_L_Renal,V_Cent_SMA,V_Cent_COE,V_Cent_LSA,V_Cent_LCCA,V_Cent_BCA};
F_branch_cell=cell(1,numel(V_endCurve_cell));
V_branch_cell=cell(1,numel(V_endCurve_cell));
indBranchTop_cell=cell(1,numel(V_endCurve_cell));
indBranchBottom_cell=cell(1,numel(V_endCurve_cell));
for q=1:1:numel(V_endCurve_cell)
    V1=V_loft_cell1{q};
    pointSpacingNow=mean(diff(pathLength(V1)));
    V2=V_loft_cell2{q};
    Vc=V_loft_cent_cell{q};
    np=ceil(max(pathLength(Vc))/pointSpacingNow);
    Vc = evenlySampleCurve(Vc,np,'spline',0);
    V2 = evenlySampleCurve(V2,size(V1,1),'spline',1);
    v1=vecnormalize(Vc(2,:)-Vc(1,:));
    [Q]=pointSetPrincipalDir(V1-Vc(ones(size(V1,1),1),:));
    n1=Q(:,3)';
    if dot(v1,n1)<0
        n1=-n1;
    end
    v2=vecnormalize(Vc(end,:)-Vc(end-1,:));
    [Q]=pointSetPrincipalDir(V2-Vc(size(Vc,1)*ones(size(V2,1),1),:));
    n2=Q(:,3)';
    if dot(v2,n2)<0
        n2=-n2;
    end
    b2=vecnormalize(V2(2,:)-V2(1,:));
    a2=vecnormalize(V2(1,:)-mean(V2,1));
    c2=cross(b2,a2);
    if dot(n2,c2)<0
        V2=flipud(V2);
    end
    [F_loft,V_loft,C_loft]=sweepLoft(V1,V2,n1,n2,Vc,size(Vc,1),0,0);
    F_loft=fliplr(F_loft);
    indLoftBottom=1:size(Vc,1):size(V_loft,1);
    indLoftTop=(size(Vc,1):size(Vc,1):size(V_loft,1));
    F_branch_cell{q}=F_loft;
    V_branch_cell{q}=V_loft;
    if q>1
        sizAll=cellfun(@(x) size(x,1),V_branch_cell);
        indBranchTop_cell{q}=indLoftTop+sum(sizAll(1:q-1));
        indBranchBottom_cell{q}=indLoftBottom+sum(sizAll(1:q-1));
    else
        indBranchTop_cell{q}=indLoftTop;
        indBranchBottom_cell{q}=indLoftBottom;
    end
    % The below plot highlights each loft path in a loop
    %     cFigure(figStruct); hold on;
    %     title(num2str(q))
    %     gpatch(F_main,V_main,'kw','none',0.5);
    %     gpatch(F_loft,V_loft,'rw','r',1);
    %     plotV(V_loft(indLoftBottom,:),'b.-','LineWidth',3);
    %     plotV(V_loft(indLoftTop,:),'g.-','LineWidth',3);
    % %     plotV(V1,'r.-','LineWidth',3);
    % %     plotV(V2,'b.-','LineWidth',3);
    % %     plotV(Vc,'k.-','LineWidth',3);
    % %     quiverVec(Vc(1,:),n1,10,'k');
    % %     quiverVec(Vc(end,:),n2,10,'k');
    %     axisGeom;
    %     camlight headlight;
    %     drawnow;
end
%Join branches together
VF=patchCentre(F_main,V_main);
%Joining branches
[F_branch,V_branch,C_branch]=joinElementSets(F_branch_cell,V_branch_cell);
%Adding branches to main thing
numVerticesInitial=size(V_main,1);
F_main=[F_main; F_branch+numVerticesInitial];
V_main=[V_main; V_branch];
C_main=[C_main; C_branch+max(C_main)];
C_path_branch=thicknessIndicesBranches(C_branch);
C_path=[C_path; C_path_branch(:)];

%%
% plot
cFigure(figStruct); hold on;
gpatch(F_main,V_main,'kw','none',0.5);
axisGeom;
colormap gjet; colorbar;
camlight headlight;
drawnow;
for q=1:1:numel(V_cut_cell)
    V_mean_now=mean(V_cut_cell{q},1);
    plotV(V_mean_now,'r.','markerSize',25);
    [~,indMin]=minDist(V_mean_now,VF);
    plotV(VF(indMin,:),'b.','markerSize',25);
    c=C_path_mat(indMin);
    C_path_mat=[C_path_mat; c*ones(size(F_branch_cell{q},1),1)];
end

%%

%Fix curve indices for joining sets
for q=1:1:numel(indBranchBottom_cell)
    indBranchTop_cell{q}=indBranchTop_cell{q}+numVerticesInitial;
    indBranchBottom_cell{q}=indBranchBottom_cell{q}+numVerticesInitial;
end
%merging nodes together
[F_main,V_main,~,indFix]=mergeVertices(F_main,V_main);
%Fix curve indices for merging
for q=1:1:numel(indBranchBottom_cell)
    indBranchTop_cell{q}=indFix(indBranchTop_cell{q})';
    indBranchBottom_cell{q}=indFix(indBranchBottom_cell{q})';
end
for q=1:1:numel(segmentCurve_cell)
    segmentCurve_cell{q}=indFix(segmentCurve_cell{q});
end

E_rings=indFix(E_rings);

% Snapping material color data to number of materials
C_path_index=C_path_mat;
C_path_mat=round(rescale(C_path_mat,1,numMaterials));

%%
% plot
cFigure(figStruct); hold on;
gpatch(F_main,V_main,C_path,'k',1);
axisGeom;
colormap gjet; colorbar;
camlight headlight;
drawnow;

%%
%Smoothing of Branches
indTouch=[indBranchBottom_cell{:}];
numSmoothGrowSteps=3;
for q=1:1:numSmoothGrowSteps
    logicFacesTouch=any(ismember(F_main,indTouch),2);
    indTouch=F_main(logicFacesTouch,:);
end

smoothControlParameters.n=numSmoothBranches;
smoothControlParameters.Method='HC';
smoothControlParameters.RigidConstraints=unique(F_main(~logicFacesTouch,:));
[V_main]=patchSmooth(F_main,V_main,[],smoothControlParameters);

% Interpolating thicknesses
thicknessData=dataStruct.WallThickness; %Thickness data
indexData=1:1:numel(thicknessData); %Index data for x-axis for interpolation
C_thickness=interp1(indexData,thicknessData,C_path,'spline');

%%
% plot
hf=cFigure(figStruct); hold on;
gtitle('Wall Thickness')
gpatch(F_main,V_main,C_thickness,'k',1);
axisGeom;
colormap(gjet(250)); colorbar;
camlight headlight;
drawnow;

%% Inverting offset direction
%Converting face thickness data to vertex data
nodalThickness=faceToVertexMeasure(F_main,V_main,C_thickness);
[~,~,N]=patchNormal(F_main,V_main);

%%
% plot
cFigure(figStruct); hold on;
gpatch(F_main,V_main,'kw','kw',0.5);
title('Mesh Offset')
quiverVec(V_main,-N.*nodalThickness,[],nodalThickness);
axisGeom;
colormap(gjet(250)); colorbar;
camlight headlight;
drawnow;

%% Thicken to create hexahedral elements
[ET,VT,Fp1,Fp2]=patchThick(F_main,V_main,-1,nodalThickness,numThickenSteps);
CT=repmat((1:1:size(F_main,1))',numThickenSteps,1);

ET=ET(:,[5:8 1:4]);
FT_inner=Fp2;
FT_outer=Fp1;

[~,logicPositive]=hexVol(ET,VT);
% logicPositive
if any(logicPositive==0)
    error('Negative hex volume found');
end

C_ET_path_mat_index=C_path_mat;
C_ET_path_index=C_path_index;
indicesNodesInner=unique(FT_inner(:));
% Find elements touching the branch ends
indBranchTopAll=[indBranchTop_cell{:}]; %+size(V_main,1)*numThickenSteps;
logicBranchEndElement=any(ismember(ET,indBranchTopAll),2);

%% Retrieve segment curve indices
segmentCurve_cell_outer=segmentCurve_cell;
for q=1:1:numel(segmentCurve_cell)
    segmentCurve_cell{q}=segmentCurve_cell{q}+size(V_main,1)*numThickenSteps;
end

[FT,CFT]=element2patch(ET,CT);

%%
% plot
cFigure(figStruct); hold on;
gpatch(FT,VT,CFT,'k',0.5);
gpatch(FT_inner,VT,'g','k',1);

for q=1:1:numel(segmentCurve_cell)
    plotV(VT(segmentCurve_cell{q},:),'g.-','LineWidth',5,'markerSize',15);
end

for q=1:1:numel(segmentCurve_cell)
    plotV(VT(segmentCurve_cell_outer{q},:),'r.-','LineWidth',5,'markerSize',15);
end

axisGeom;
colormap(gjet(250)); colorbar;
camlight headlight;
drawnow;

%% Grouping ring edges and offset so they are on the inside

%Offset indices inward
E_rings=E_rings+size(V_main,1)*numThickenSteps;

%Group indices to form rings
optionStruct.outputType='label';
G_rings=tesgroup(E_rings,optionStruct);

%Compose ring cell
ringCurve_cell=cell(1,max(G_rings));
for q=1:1:max(G_rings)
    indList=edgeListToCurve(E_rings(G_rings==q,:)); %1=Ring number
    indList=indList(1:end-1);
    ringCurve_cell{q}=indList;
end

%%
% plot
cFigure(figStruct); hold on;
gpatch(FT,VT,'kw','none',0.5);

plotColors=gjet(max(G_rings));
for q=1:1:max(G_rings)
    hp=plotV(VT(ringCurve_cell{q},:),'b.-','LineWidth',5,'markerSize',15);
%     hp.Color=plotColors(q,:);
end

axisGeom;
colormap(gjet(250)); colorbar;
camlight headlight;
drawnow;

%% Create color data for hex elements

C_ET_path_mat_index=C_ET_path_mat_index(CT);
C_ET_path_index=C_ET_path_index(CT);
logicBranchEndElement=logicBranchEndElement(CT); %Colors for original element indices (and sweeping steps)
%%Define Inner Surface Set
logicElementsInner=any(ismember(ET,indicesNodesInner),2);
indicesElementsInner=find(logicElementsInner);

%%
% plot
cFigure(figStruct); hold on;
gpatch(FT_inner,VT,'gw','none',0.5);
gpatch(FT_outer,VT,'rw','none',0.5);
patchNormPlot(FT_inner,VT)
axisGeom;
colormap(gjet(250)); colorbar;
camlight headlight;
drawnow;

%% Material Properties
% Assign element material parameters
% Derive interpolatable parameters [Vcol,Vela,Ee,xeps,mu]
indexData=1:1:numel(xeps_data);
indexDataInterp=linspace(1,numel(xeps_data),numMaterials);
indexData_ET=1:1:numMaterials;

xeps_vector=interp1(indexData,xeps_data,indexDataInterp,'spline');
xeps_ET=interp1(indexData_ET,xeps_vector,C_ET_path_mat_index,'spline');
Ee_vector=interp1(indexData,Ee_data,indexDataInterp,'spline');
Ee_ET=interp1(indexData_ET,Ee_vector,C_ET_path_mat_index,'spline');
Vela_vector=interp1(indexData,Vela_data,indexDataInterp,'spline');
Vela_ET=interp1(indexData_ET,Vela_vector,C_ET_path_mat_index,'spline');
Vcol_vector=interp1(indexData,Vcol_data,indexDataInterp,'spline');
Vcol_ET=interp1(indexData_ET,Vcol_vector,C_ET_path_mat_index,'spline');
Vsmc_vector=interp1(indexData,Vsmc_data,indexDataInterp,'spline');
Vsmc_ET=interp1(indexData_ET,Vsmc_vector,C_ET_path_mat_index,'spline');

%% Define Branch Ends Set
[FT,CFT]=element2patch(ET,logicElementsInner);
[~,CFT_logicBranchEndElement]=element2patch(ET,logicBranchEndElement);
[~,CFT_path_mat]=element2patch(ET,C_ET_path_mat_index);
[~,xeps_FT]=element2patch(ET,xeps_ET);
[~,Ee_FT]=element2patch(ET,Ee_ET);
[~,Vela_FT]=element2patch(ET,Vela_ET);
[~,Vcol_FT]=element2patch(ET,Vcol_ET);
[~,Vsmc_FT]=element2patch(ET,Vsmc_ET);
indBoundary=tesBoundary(FT,VT);
Fb=FT(indBoundary,:);
F1=sort(ET(:,[1 2 3 4]),2);
F2=sort(ET(:,[5 6 7 8]),2);
FT_boundary=FT(indBoundary,:);
sizVirt=size(VT,1)*ones(1,size(FT,2));
indVirt_F1=sub2indn(sizVirt,F1);
indVirt_F2=sub2indn(sizVirt,F2);
indVirt_FT_boundary=sub2indn(sizVirt,sort(FT_boundary,2));
indVirt_FT=sub2indn(sizVirt,sort(FT,2));
CFT_logicBranchEndElement=CFT_logicBranchEndElement & ismember(indVirt_FT,indVirt_FT_boundary); %Only keep boundary members
CFT_logicBranchEndElement=CFT_logicBranchEndElement & ~ismember(indVirt_FT,indVirt_F1); %Cant be member of top
CFT_logicBranchEndElement=CFT_logicBranchEndElement & ~ismember(indVirt_FT,indVirt_F2); %Cant be member of bottom

%Nodes for boundary conditions
indNodesFix=FT(CFT_logicBranchEndElement,:);
indNodesFix=unique(indNodesFix(:));

%%
% plot full mesh
cFigure(figStruct); hold on;
gpatch(FT,VT,CFT_logicBranchEndElement,'r',1);
xlim([140 200]); ylim([120 200]); zlim([20 370]);
axisGeom;
colormap(gjet(250)); colorbar;
camlight headlight;
drawnow;

%%
% plot interpolated parameters

cFigure(figStruct);
subplot(2,3,1);hold on;
title('Indexing color');
gpatch(FT(indBoundary,:),VT,CFT_path_mat(indBoundary,:),'none',1);
axisGeom;
colormap(gjet(250)); colorbar;
camlight headlight;

subplot(2,3,2);hold on;
title('xeps');
gpatch(FT(indBoundary,:),VT,xeps_FT(indBoundary,:),'none',1);
axisGeom;
colormap(gjet(250)); colorbar;
camlight headlight;

subplot(2,3,3);hold on;
title('Ee');
gpatch(FT(indBoundary,:),VT,Ee_FT(indBoundary,:),'none',1);
axisGeom;
colormap(gjet(250)); colorbar;
caxis([min(Ee_data) max(Ee_data)]);
camlight headlight;

subplot(2,3,4);hold on;
title('Vcol');
gpatch(FT(indBoundary,:),VT,Vcol_FT(indBoundary,:),'none',1);
axisGeom;
colormap(gjet(250)); colorbar;
camlight headlight;
%axis off

subplot(2,3,5);hold on;
title('Vela');
gpatch(FT(indBoundary,:),VT,Vela_FT(indBoundary,:),'none',1);
axisGeom;
colormap(gjet(250)); colorbar;
camlight headlight;
%axis off
drawnow;

%End of Model Generation steps

%% Create ABAQUS structure
% Setup structure to define an Abaqus inp file

%%--> Heading
abaqus_spec.Heading.COMMENT{1}='Job name: AORTA';

%%--> Preprint
abaqus_spec.Preprint.ATTR.echo='NO';
abaqus_spec.Preprint.ATTR.model='NO';
abaqus_spec.Preprint.ATTR.history='NO';
abaqus_spec.Preprint.ATTR.contact='NO';

%--> Part

% Node
nodeIds=(1:1:size(VT,1))';
abaqus_spec.Part.COMMENT='This section defines the part geometry in terms of nodes and elements';
abaqus_spec.Part.ATTR.name='Aorta';
abaqus_spec.Part.Node={nodeIds,VT};

% Element
elementIds=(1:1:size(ET,1))';
abaqus_spec.Part.Element{1}.ATTR.type='C3D8';
abaqus_spec.Part.Element{1}.VAL={elementIds,ET};

% Element sets
for q=1:1:numMaterials
    elementIdsSetNow=find(C_ET_path_mat_index==q);
    abaqus_spec.Part.Elset{q}.ATTR.elset=['MatSet-',num2str(q)];
    abaqus_spec.Part.Elset{q}.VAL=elementIdsSetNow';
end

surfaceElementSetName='elementSetInnerSurface';
abaqus_spec.Part.Elset{numMaterials+1}.ATTR.elset=surfaceElementSetName;
abaqus_spec.Part.Elset{numMaterials+1}.ATTR.internal=''; %Remains hidden uppon import
abaqus_spec.Part.Elset{numMaterials+1}.VAL=indicesElementsInner(:)';

% Surfaces
sidePick=1;
abaqus_spec.Part.Surface{1}.ATTR.type='ELEMENT';
abaqus_spec.Part.Surface{1}.ATTR.name=[surfaceElementSetName,'_side',num2str(sidePick)];
abaqus_spec.Part.Surface{1}.VAL={surfaceElementSetName,['S',num2str(sidePick)]};

% Sections
for q=1:1:numMaterials
    elementIdsSetNow=find(C_ET_path_mat_index==q);
    abaqus_spec.Part.Solid_section{q}.ATTR.elset=['MatSet-',num2str(q)];
    abaqus_spec.Part.Solid_section{q}.ATTR.material=['Mat_',num2str(q)];
end

%--> Assembly
abaqus_spec.Assembly.ATTR.name='Assembly-1';
abaqus_spec.Assembly.Instance.ATTR.name='Aorta-assembly';
abaqus_spec.Assembly.Instance.ATTR.part='Aorta';

abaqus_spec.Assembly.Nset{1}.ATTR.nset='NSet_Inner';
abaqus_spec.Assembly.Nset{1}.ATTR.instance=abaqus_spec.Assembly.Instance.ATTR.name;
abaqus_spec.Assembly.Nset{1}.VAL=indicesNodesInner;

%Add segment curve node sets
for q=1:1:numel(segmentCurve_cell)
    indNow=numel(abaqus_spec.Assembly.Nset)+1;
    abaqus_spec.Assembly.Nset{indNow}.ATTR.nset=['Segment',num2str(q)];
    abaqus_spec.Assembly.Nset{indNow}.ATTR.instance=abaqus_spec.Assembly.Instance.ATTR.name;
    abaqus_spec.Assembly.Nset{indNow}.VAL=segmentCurve_cell{q};
end

%Add ring curve node sets
for q=1:1:numel(ringCurve_cell)
    indNow=numel(abaqus_spec.Assembly.Nset)+1;
    abaqus_spec.Assembly.Nset{indNow}.ATTR.nset=['Ring',num2str(q)];
    abaqus_spec.Assembly.Nset{indNow}.ATTR.instance=abaqus_spec.Assembly.Instance.ATTR.name;
    abaqus_spec.Assembly.Nset{indNow}.VAL=ringCurve_cell{q};
end

%Add fix node set
% indNow=numel(abaqus_spec.Assembly.Nset)+1;
% setNameFix=['Set-',num2str(indNow)];
% abaqus_spec.Assembly.Nset{indNow}.ATTR.nset=setNameFix;
% abaqus_spec.Assembly.Nset{indNow}.ATTR.instance=abaqus_spec.Assembly.Instance.ATTR.name;
% abaqus_spec.Assembly.Nset{indNow}.VAL=indNodesFix';

%%--> Material
for q=1:1:numMaterials
    abaqus_spec.Material{q}.ATTR.name=['mat_',num2str(q)];
    abaqus_spec.Material{q}.Depvar.VAL=13;
    abaqus_spec.Material{q}.User_Material.ATTR.constants=17;
    %Define material parameters
    mu_now=mu_data;
    k_now=mu_now*kfactor;
    D_now=2/k_now;
    k1_now=k1_data;
    k2_now=k2_data;
    kappa_now=kappa_data;
    theta1_now=theta1_data;
    theta2_now=theta2_data;
    sigact_now=sigact_data;
    ke_now=ke_data;
    Ee_now=Ee_vector(q);
    thetaE1_now=thetaE1_data;
    thetaE2_now=thetaE2_data;
    iswitch_now=iswitch;
    xeps_now=xeps_vector(q);
    Vcol_now=Vcol_vector(q);
    Vela_now=Vela_vector(q);
    Vsmc_now=Vsmc_vector(q);
    t=vec2strIntDouble([mu_now/2 D_now k1_now k2_now kappa_now theta1_now theta2_now sigact_now ke_now Ee_now thetaE1_now thetaE2_now iswitch_now xeps_now Vcol_now Vela_now Vsmc_now],'%6.7e');
    t=strwrap(t,8,', '); %Wrap to max width of 8 entries
    %abaqus_spec.User_Material{q}.VAL=t;
    abaqus_spec.Material{q}.User_Material.VAL=t;
end
%%--> Step
abaqus_spec.Step.ATTR.name='Step-1';
abaqus_spec.Step.ATTR.nlgeom='YES';
abaqus_spec.Step.Static=[0.01 1 1e-6 0.01];

% Boundary
% setNameFix=abaqus_spec.Assembly.Nset{indNow}.ATTR.nset;
% abaqus_spec.Step.Boundary{1}.VAL={setNameFix,[1,1]};
% abaqus_spec.Step.Boundary{2}.VAL={setNameFix,[2,2]};
% abaqus_spec.Step.Boundary{3}.VAL={setNameFix,[3,3]};

%Output
% abaqus_spec.Step.Restart.ATTR.write='';
% abaqus_spec.Step.Restart.ATTR.frequency=0;
%
% abaqus_spec.Step.Output{1}.ATTR.field='';
% abaqus_spec.Step.Output{1}.ATTR.variable='PRESELECT';
% abaqus_spec.Step.Output{2}.ATTR.history='';
% abaqus_spec.Step.Output{2}.ATTR.variable='PRESELECT';
% abaqus_spec.Step.Node_print.ATTR.nset='all';
% abaqus_spec.Step.Node_print.ATTR.frequency = 1;
% abaqus_spec.Step.Node_print.VAL='COORD';
% abaqus_spec.Step.El_print{1}.VAL='S';
% abaqus_spec.Step.El_print{2}.VAL='E';

% Creating the INP file
% You can use |abaqusStruct2inp| to write the structure data to a file.

%%
if saveOn==1
    % Export INP file
    abaqusStruct2inp(abaqus_spec,abaqusInpFileName);

    saveStruct.ET=ET;
    saveStruct.FT=FT;
    saveStruct.VT=VT;
    saveStruct.Fb=Fb;
    saveStruct.segmentCurve_cell=segmentCurve_cell;
    saveStruct.abaqus_spec=abaqus_spec;
    save(matfileSaveName,'-struct','saveStruct');

    disp('inp file write complete')
end

%% FUNCTIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [Fs,Vs,Cs,indEnd,logicRemoveFaces,segmentCurve_cell,E_rings]=circleCutExtrude(F,V,C,V_cent,V_cut,pointSpacing,plotOn,smoothControlParameters,segmentCurve_cell,E_rings)

%% Conver to triangles
Ft=[F(:,[1 2 3]);F(:,[3 4 1])];

%% Resample center line
np=round(max(pathLength(V_cent))/pointSpacing);
V_cent = evenlySampleCurve(V_cent,np,'spline',0);

%% Get mean of cut curve, project to surface

V_cut_mean=mean(V_cut,1);

rMax=max(sqrt(sum((V_cut-V_cut_mean(ones(size(V_cut,1),1),:)).^2,2)));

[~,indMin]=minDist(V_cut_mean,V_cent);

v=V_cent(indMin,:)-V_cut_mean;

optStruct.eps      = 1e-6;
optStruct.triangle = 'one sided';
optStruct.ray      = 'ray';
optStruct.border   = 'normal';

[V_intersect,L_intersect,~] = triangleRayIntersection(V_cut_mean(ones(size(Ft,1),1),:),v(ones(size(Ft,1),1),:),V,Ft,optStruct);

V_intersect=V_intersect(L_intersect,:);

D=minDist(V,V_intersect);

logicRemoveVertices=D<=rMax;

logicRemoveFaces=any(logicRemoveVertices(F),2);

Eb_cut=patchBoundary(F(logicRemoveFaces,:),V);
indCurveCut=edgeListToCurve(Eb_cut);
indCurveCut=indCurveCut(1:end-1);
indCurveCut=indCurveCut(:);

logicFlip=~any((Eb_cut(:,1)==indCurveCut(1))&(Eb_cut(:,2)==indCurveCut(2)));
if logicFlip
    indCurveCut=flipud(indCurveCut);
end

V_curve_cut=V(indCurveCut,:);

%Resample so they have the same amount of points
np=size(V_curve_cut,1);
V_cut = evenlySampleCurve(V_cut,np,'spline',1);

%Get appropriate start point
[~,indMin]=minDist(V_cut(1,:),V_curve_cut);
if indMin>1
    V_curve_cut=[V_curve_cut(indMin:end,:); V_curve_cut(1:indMin-1,:)];
end

V_curve_cut_mean=mean(V_curve_cut,1);
V_cut_mean=mean(V_cut,1);

% Check order again
nq=round(0.25*np);
a1=dot(V_cut(nq,:)-V_cut_mean,V_curve_cut(nq,:)-V_curve_cut_mean);

V_cut_test=flipud(V_cut);
a2=dot(V_cut_test(nq,:)-V_cut_mean,V_curve_cut(nq,:)-V_curve_cut_mean);

if a2>a1
    V_cut=V_cut_test;
end

%Get appropriate start point
[~,indMin]=minDist(V_cut(1,:),V_curve_cut);
if indMin>1
    V_curve_cut=[V_curve_cut(indMin:end,:); V_curve_cut(1:indMin-1,:)];
end

%Minimize twist
SSQD=zeros(np,1);
for q=1:1:np
    if q>1
        indSort=[q:np 1:q-1];
    else
        indSort=1:np;
    end
    V_curve_cut_test=V_curve_cut(indSort,:);
    SSQD(q)=sum(sqrt(sum((V_curve_cut_test-V_cut).^2,2)).^2);
end

%Get sort order
[~,q]=min(SSQD);
if q>1
    indSort=[q:np 1:q-1];
    V_curve_cut=V_curve_cut(indSort,:);
end

%%

F=F(~logicRemoveFaces,:);
C=C(~logicRemoveFaces);

%% Loft

cPar.closeLoopOpt=1;
cPar.patchType='quad';

[F_merge,V_merge,~,indEnd]=polyLoftLinear(V_curve_cut,V_cut,cPar);
C_merge=max(C(:))+ones(size(F_merge,1),1);

%%

% if plotOn==1
%     cFigure(figStruct); hold on;
%     gpatch(F,V,C,'k',0.5);
%     gpatch(F_merge,V_merge,'g','k',1);
%     plotV(V_cent,'g.-','LineWidth',3,'markerSize',25);
%     plotV(V_cut,'r.-','LineWidth',3,'markerSize',25);
%     plotV(V_cut_mean,'r.','markerSize',50);
%     plotV(V_curve_cut,'b.-','LineWidth',3);
%     plotV(V_intersect,'b.','markerSize',50);
%     quiverVec(V_cut_mean,v,[],'r');
%     axisGeom;
%     colormap gjet;
%     camlight headlight;
%     drawnow;
% end

%%

[Fs,Vs,Cs]=joinElementSets({F,F_merge},{V,V_merge},{C,C_merge});
indEnd=indEnd+size(V,1);

%% Constrained smoothing

nGrowthSteps=2;
logicSmooth=any(ismember(Fs,indCurveCut),2);
for q=1:1:nGrowthSteps-1
    indTouch=unique(Fs(logicSmooth,:));
    logicSmooth=any(ismember(Fs,indTouch),2);
end

logicSmooth=logicSmooth | Cs==max(Cs(:));

[Fs,Vs,~,indFix]=mergeVertices(Fs,Vs); %Merging points
indEnd=indFix(indEnd);

for q=1:1:numel(segmentCurve_cell)
    indSegment=segmentCurve_cell{q};
    indSegment=indSegment(indSegment>0);
    segmentCurve_cell{q}=indFix(indSegment);
end
E_rings=indFix(E_rings);

[Fs,Vs,indFix]=patchCleanUnused(Fs,Vs); %removing unused at hole
indEnd=indFix(indEnd);
E_rings=indFix(E_rings);
E_rings=E_rings(all(E_rings>0,2),:);

for q=1:1:numel(segmentCurve_cell)
    indSegment=segmentCurve_cell{q};
    indSegment=indSegment(indSegment>0);
    segmentCurve_cell{q}=indFix(indSegment);
end

indRigid=unique(Fs(~logicSmooth,:));
indRigid=unique([indRigid(:);indEnd(:)]);

Eb=patchBoundary(Fs,Vs);
smoothControlParameters.RigidConstraints=unique([indRigid(:);Eb(:)]);
[Vs]=patchSmooth(Fs,Vs,[],smoothControlParameters);

%%

if plotOn==1
    cFigure(figStruct); hold on;
    gpatch(Fs,Vs,logicSmooth,'k',1);
    patchNormPlot(Fs,Vs);
    plotV(Vs(indRigid,:),'b.','markerSize',25);
    axisGeom;
    colormap gjet;
    camlight headlight;
    drawnow;
end

end

%%
function V=resampleCurve(V,pointSpacing,closeLoopOpt)

np=ceil(max(pathLength(V))/pointSpacing);
V = evenlySampleCurve(V,np,'spline',closeLoopOpt);

end

%%

function [Vn]=curveOffset(V,wallThickness)
p1=mean(V,1); %Curve center

vf=-vecnormalize(V-[V(2:end,:);V(1,:)]); %Allong curve path vectors
vb=vecnormalize(V-[V(end,:);V(1:end-1,:)]); %Allong curve path vectors
v=(vf+vb)/2;

r=vecnormalize(V-p1); %Position vector wrt mean
v1=vecnormalize(cross(v,r)); %perimeter quasi Z-vectors
n=vecnormalize(cross(v1(ones(size(v,1),1),:),v)); %Outward normal vectors
Vn=(V+n*wallThickness); %Offset to create new curve

% cFigure;
% plotV(V,'k.-');
% quiverVec(V,vf,3,'r');
% quiverVec(V,vb,3,'b');
% quiverVec(V,v,3,'g');
% axisGeom
% drawnow;
end
%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function [P]   = spm_dcm_fit(P,use_parfor)
% Bayesian inversion of DCMs using Variational Laplace
% FORMAT [DCM] = spm_dcm_fit(P)
%
% P          - {N x M} DCM structure array (or filenames) from N subjects
% use_parfor - if true, will attempt to run in parallel (default: false)
%              NB: all DCMs are loaded into memory
%
% DCM  - Inverted (1st level) DCM structures with posterior densities
%__________________________________________________________________________
%
% This routine is just a wrapper that calls the appropriate dcm inversion
% routine for a set a pre-specifed DCMs.
%
% If called with a cell array, each column is assumed to contain 1st level
% DCMs inverted under the same model. Each row contains a different data
% set (or subject).
%__________________________________________________________________________
% Copyright (C) 2015 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_dcm_fit.m 7755 2019-12-16 13:19:28Z spm $

if nargin < 2, use_parfor = false; end

% get filenames and set up
%--------------------------------------------------------------------------
if ~nargin
    [P, sts] = spm_select([1 Inf],'^DCM.*\.mat$','Select DCM*.mat files');
    if ~sts, return; end
end
if ischar(P),   P = cellstr(P);  end
if isstruct(P), P = {P};         end

% Number of subjects (data) and models (of those data)
%--------------------------------------------------------------------------
[Ns,Nm] = size(P);

% Find model class and modality
%--------------------------------------------------------------------------
try, load(P{1}); catch, DCM = P{1}; end

model = spm_dcm_identify(DCM);

if isempty(model)
    warning('unknown inversion scheme');
    return
end

% Get data structure for each subject (column)
%--------------------------------------------------------------------------
for i = 1:Ns
    for j = 2:Nm
        switch model
            case{'DEM'}
                P{i, j}.xY = P{i, 1}.Y;
            otherwise
                P{i, j}.xY = P{i, 1}.xY;
        end
    end
end

% Estimate
%--------------------------------------------------------------------------
if use_parfor
    % Estimate DCMs in parallel using parfor
    P = spm_dcm_load(P);

    parfor i = 1:numel(P)
        P{i} = fit_dcm(P{i}, model);
    end
else
    % Estimate DCMs without parfor
    for i = 1:numel(P)
        try
            DCM = load(P{i});
            DCM = DCM.DCM;
        catch
            DCM = P{i};
        end
        P{i} = fit_dcm(DCM, model);
    end
end

% -------------------------------------------------------------------------
function DCM = fit_dcm(DCM, model)
% Inverts a DCM.
% DCM   - the DCM structure
% model - a string identifying the model type (see spm_dcm_identify)

switch model

    % fMRI model
    %----------------------------------------------------------------------
    case{'fMRI'}
        DCM = spm_dcm_estimate(DCM);

    % conventional neural-mass and mean-field models
    %----------------------------------------------------------------------
    case{'fMRI_CSD'}
        DCM = spm_dcm_fmri_csd(DCM);

    % conventional neural-mass and mean-field models
    %----------------------------------------------------------------------
    case{'ERP'}
        DCM = spm_dcm_erp(DCM);

    % cross-spectral density model (complex)
    %----------------------------------------------------------------------
    case{'CSD'}
        DCM = spm_dcm_csd(DCM);

    % cross-spectral density model (steady-state responses)
    %----------------------------------------------------------------------
    case{'TFM'}
        DCM = spm_dcm_tfm(DCM);

    % induced responses
    %----------------------------------------------------------------------
    case{'IND'}
        DCM = spm_dcm_ind(DCM);

    % phase coupling
    %----------------------------------------------------------------------
    case{'PHA'}
        DCM = spm_dcm_phase(DCM);

    % cross-spectral density model (steady-state responses)
    %----------------------------------------------------------------------
    case{'NFM'}
        DCM = spm_dcm_nfm(DCM);

    % behavioural Markov decision process model
    %----------------------------------------------------------------------
    case{'MDP'}
        DCM = spm_dcm_mdp(DCM);

    % generic nonlinear system identification
    %----------------------------------------------------------------------
    case{'NLSI'}
        [Ep,Cp,Eh,F] = spm_nlsi_GN(DCM.M,DCM.xU,DCM.xY);
        DCM.Ep       = Ep;
        DCM.Eh       = Eh;
        DCM.Cp       = Cp;
        DCM.F        = F;

    % hierarchical dynamic mmodel
    %----------------------------------------------------------------------
    case{'DEM'}
        DCM = spm_DEM(DCM);

    % default
    %----------------------------------------------------------------------
    otherwise
        try
            DCM = feval(model, DCM);
        catch
            error('unknown DCM');
        end
end


function [D,L] = spm_opm_create(S)
% Read or simulate magnetometer data and optionally set up forward model
% FORMAT D = spm_opm_create(S)
%   S               - input structure
% Optional fields of S:
% SENSOR LEVEL INFO
%   S.data          - filepath to .bin file        - Default: Simulates data
%   S.channels      - channels.tsv file            - Default: REQUIRED
%   S.fs            - Sampling frequency (Hz)      - Default: REQUIRED if S.meg is empty
%   S.meg           - meg.json file                - Default: REQUIRED if S.fs is empty
%   S.precision     - 'single' or 'double'         - Default: 'single'
% SIMULATION
%   S.wholehead     - whole head coverage flag     - Deafult: 0
%   S.space         - space between sensors(mm)    - Default: 25
%   S.offset        - scalp to sensor distance(mm) - Default: 6.5
%   S.nSamples      - number of samples            - Default: 1000
%   S.Dens          - number of density checks     - Default: 40

% SOURCE LEVEL INFO
%   S.coordsystem   - coordsystem.json file        - Default:
%   S.positions     - positions.tsv file           - Default:
%   S.sMRI          - Filepath to  MRI file        - Default: uses template
%   S.cortex        - Custom cortical mesh         - Default: Use inverse normalised cortical mesh
%   S.scalp         - Custom scalp mesh            - Default: Use inverse normalised scalp mesh
%   S.oskull        - Custom outer skull mesh      - Default: Use inverse normalised outer skull mesh
%   S.iskull        - Custom inner skull mesh      - Default: Use inverse normalised inner skull mesh
%   S.voltype       - Volume conducter Model type  - Default: 'Single Shell'
%   S.meshres       - mesh resolution(1,2,3)       - Default: 1
%   S.lead          - flag to compute lead field   - Default: 0
% Output:
%  D           - MEEG object (also written to disk)
%  L           - Lead field (also written on disk)
%__________________________________________________________________________
% Copyright (C) 2018 Wellcome Trust Centre for Neuroimaging

% Tim Tierney
% $Id: spm_opm_create.m 7645 2019-07-25 13:58:25Z tim $
spm('FnBanner', mfilename);

%-Set default values
%--------------------------------------------------------------------------
if ~isfield(S, 'voltype'),     S.voltype = 'Single Shell'; end
if ~isfield(S, 'meshres'),     S.meshres = 1; end
if ~isfield(S, 'scalp'),       S.scalp = []; end
if ~isfield(S, 'cortex'),      S.cortex = []; end
if ~isfield(S, 'iskull'),      S.iskull = []; end
if ~isfield(S, 'oskull'),      S.oskull = []; end
if ~isfield(S, 'lead'),        S.lead = 0; end
if ~isfield(S, 'fs'),          S.fs   = 1000; end
if ~isfield(S, 'nSamples'),    S.nSamples   = 1000; end
if ~isfield(S, 'nDens'),       S.nDens   = 40; end
if ~isfield(S, 'space'),       S.space  = 30; end
if ~isfield(S, 'offset'),      S.offset  = 6.5; end
if ~isfield(S, 'data'),        S.data = zeros(1,S.nSamples); end
if ~isfield(S, 'wholehead'),   S.wholehead = 1; end
if ~isfield(S, 'fname'),       S.fname = 'sim_opm'; end
if ~isfield(S, 'precision'),   S.precision = 'single'; end

%- Read Binary File
%----------------------------------------------------------------------
try % to read data
    [direc, dataFile] = fileparts(S.data);
    dat = fopen(S.data);
    S.data = fread(dat,Inf,S.precision,0,'b');
    fclose(dat);
    binData=1;
catch % if not readable check if it is numeric
    if ~isa(S.data,'numeric') % if not numeric throw error
        error('A valid dataest or file was not supplied')
    end
    binData=0;
    direc = pwd();
    dataFile=S.fname;
end
%- identify potential BIDS Files
%----------------------------------------------------------------------
base = strsplit(dataFile,'meg');
chanFile= fullfile(direc,[base{1},'channels.tsv']);
megFile= fullfile(direc,[base{1},'meg.json']);
posFile= spm_select('FPList',direc,[base{1},'positions.tsv']);
coordFile= fullfile(direc,[base{1},'coordsystem.json']);

%- Check for channel Info
%--------------------------------------------------------------------------
try % to load a channels file
    channels = spm_load(S.channels);
catch
    try % to load a BIDS channel file
       channels = spm_load(chanFile);
    catch
        try  % use channel struct if supplied
           channels = S.channels;
        catch % create channel struct
            args=[];
            args.base='Chan';
            args.n= size(S.data,1);
            labs= spm_create_labels(args);
            channels = [];
            channels.name=labs;
            channels.type=repmat({'MEG'},size(S.data,1),1);
            channels.units= repmat({'fT'},size(S.data,1),1);
        end
    end
end

%- reformat data according to channel info
%--------------------------------------------------------------------------
nc = size(channels.name,1);

if binData
    S.data = reshape(S.data,nc,numel(S.data)/nc);
elseif nc~=size(S.data,1)
    error('numer of channels in S.data different to S.channels')
else
    S.data =S.data;
end

%- Check for MEG Info
%--------------------------------------------------------------------------
try % to load a meg file
    meg = spm_load(S.meg);
catch
    try % to load a BIDS meg file
       meg = spm_load(megFile);
    catch
        try % to use meg struct
           meg = S.meg;
        catch
            try % to use S.fs argument to get sampling frequency
                meg =[];
                meg.SamplingFrequency=S.fs;
            catch
                ('A valid meg.json file is required if S.fs is empty');
            end
        end
    end
end

%- Position File check
%----------------------------------------------------------------------
try % to load a channels file
    posOri = spm_load(S.positions);
    positions =1;
catch
    try % to load a BIDS channel file
       posOri = spm_load(posFile);
       positions =1;
    catch
       positions =0;
    end
end

%- Forward model Check
%----------------------------------------------------------------------
subjectSource  = (positions|isfield(S,'space')) & isfield(S,'sMRI');
subjectSensor = ~subjectSource;

if subjectSource
    forward =1;
    template =0;
elseif subjectSensor
    forward =0;
    template =0;
else
    forward =1;
    template =1;
end

%- Create SPM object of simulated or real data
%--------------------------------------------------------------------------
Dtemp = meeg(size(S.data,1),size(S.data,2),size(S.data,3));
Dtemp = fsample(Dtemp,meg.SamplingFrequency);
Dtemp = fname(Dtemp,[dataFile,'.mat']);
Dtemp = path(Dtemp,direc);
Dtemp = chanlabels(Dtemp,1:size(Dtemp,1),channels.name);
Dtemp = units(Dtemp,1:size(Dtemp,1),channels.units);
Dtemp = chantype(Dtemp,1:size(Dtemp,1),channels.type);

%- Overwrite and Save
%--------------------------------------------------------------------------
ma = fullfile(direc,[dataFile,'.mat']);
da = fullfile(direc,[dataFile,'.dat']);

ae = exist(fname(Dtemp),'file')==2;
if(ae)
    delete(ma);
    delete(da);
end
Dtemp.save();
% create data file and insert data
D= blank(Dtemp,[dataFile,'.dat']);
dim=size(D);
D(1:dim(1),1:dim(2),1:dim(3)) = S.data;
D.save();

%- Create Meshes
%--------------------------------------------------------------------------
if forward
    %initially used inverse normalised meshes
    D = spm_eeg_inv_mesh_ui(D,1,S.sMRI,S.meshres);
    save(D);
    % then fill in custom meshes(if they exist)
    args = [];
    args.D = D;
    args.scalp = S.scalp;
    args.cortex = S.cortex;
    args.iskull = S.iskull;
    args.oskull = S.oskull;
    args.template = template;
    D = opm_customMeshes(args);
    save(D);
end

%- Create the Sensor Array
%--------------------------------------------------------------------------
if forward
    try % create positions and orientations
        pos = [posOri.Px,posOri.Py,posOri.Pz];
        ori = [posOri.Ox,posOri.Oy,posOri.Oz];
        cl = posOri.name;
    catch % if no postions and orientations provided then create them
        args = [];
        args.D =D;
        args.offset = S.offset;
        args.space = S.space;
        args.wholehead = S.wholehead;
        args.nDens = S.nDens;
        [pos,ori] = opm_createSensorArray(args);
    end

    nSensors=size(pos,1);
    if nSensors>size(S.data,1) %
        args=[];
        args.base='Chan';
        args.n= nSensors;
        labs= spm_create_labels(args);
        channels = [];
        channels.name=labs;
        channels.type=repmat({'MEG'},nSensors,1);
        channels.units= repmat({'fT'},nSensors,1);
        D = clone(D,fnamedat(D),[nSensors,S.nSamples,1],1);
        D = chanlabels(D,1:size(D,1),channels.name);
        D = units(D,1:size(D,1),channels.units);
        D = chantype(D,1:size(D,1),channels.type);
        cl=chanlabels(D)';
    end
end

%-Place Sensors  in object
%--------------------------------------------------------------------------
if forward
    grad= [];
    grad.label = cl;
    grad.coilpos = pos;
    grad.coilori = ori;
    grad.tra = eye(numel(grad.label));
    grad.chanunit = repmat({'T'}, numel(grad.label), 1);
    grad.chantype= 'MEG';
    grad = ft_datatype_sens(grad, 'amplitude', 'T', 'distance', 'mm');
    D = sensors(D, 'MEG', grad);
    save(D);
end

%- fiducial settings
%--------------------------------------------------------------------------
if subjectSource
    miMat = zeros(3,3);
    fiMat = zeros(3,3);
    fid=[];
    try % to read the coordsystem.json
        coord = spm_load(S.coordsystem);
        fiMat(1,:) = coord.HeadCoilCoordinates.coil1;
        fiMat(2,:) = coord.HeadCoilCoordinates.coil2;
        fiMat(3,:) = coord.HeadCoilCoordinates.coil3;
        miMat(1,:) = coord.AnatomicalLandmarkCoordinates.coil1;
        miMat(2,:) = coord.AnatomicalLandmarkCoordinates.coil2;
        miMat(3,:) = coord.AnatomicalLandmarkCoordinates.coil3;
        fid.fid.label = fieldnames(coord.HeadCoilCoordinates);
        fid.fid.pnt =fiMat;
        fid.pos= []; % headshape field that is left blank (GRB)
        M = fid;
        M.fid.pnt=miMat;
        M.pnt = D.inv{1}.mesh.fid.pnt;
    catch
        try % to find the BIDS coordsystem.json
            coord = spm_load(coordFile);
            fiMat(1,:) = coord.HeadCoilCoordinates.coil1;
            fiMat(2,:) = coord.HeadCoilCoordinates.coil2;
            fiMat(3,:) = coord.HeadCoilCoordinates.coil3;
            miMat(1,:) = coord.AnatomicalLandmarkCoordinates.coil1;
            miMat(2,:) = coord.AnatomicalLandmarkCoordinates.coil2;
            miMat(3,:) = coord.AnatomicalLandmarkCoordinates.coil3;
            fid.fid.label = fieldnames(coord.HeadCoilCoordinates);
            fid.fid.pnt =fiMat;
            fid.pos= []; % headshape field that is left blank (GRB)
            M = fid;
            M.fid.pnt=miMat;
            M.pnt = D.inv{1}.mesh.fid.pnt;
        catch % DEFAULT: transform between fiducials and anatomy is identity
            fid.fid.label = {'nas', 'lpa', 'rpa'}';
            fid.fid.pnt = [0 0 0; -1 0 0; 1 0 0];
            fid.pos= [];
            M = fid;
            M.pnt = D.inv{1}.mesh.fid.pnt;
        end
    end
end

if(template) %make
    fid.fid.label = {'nas', 'lpa', 'rpa'}';
    fid.fid.pnt = D.inv{1}.mesh.fid.fid.pnt(1:3,:);
    fid.pos= []; % headshape field that is left blank (GRB)
    M = fid;
    M.pnt = D.inv{1}.mesh.fid.pnt;
end

%- Coregistration
%--------------------------------------------------------------------------
if(subjectSource)
    D = fiducials(D, fid);
    save(D);
    f=fiducials(D);
    f.pnt =zeros(0,3);
    D = spm_eeg_inv_datareg_ui(D,1,f,M,0);
end
%- 2D view based on mean orientation of sensors
%--------------------------------------------------------------------------
if(forward)
    n1=mean(grad.coilori); n1= n1./sqrt(dot(n1,n1));
    t1=cross(n1,[0 0 1]);
    t2=cross(t1,n1);
    pos2d =zeros(size(grad.coilpos,1),2);
    for i=1:size(grad.coilpos,1)
        pos2d(i,1)=dot(grad.coilpos(i,:),t1);
        pos2d(i,2)=dot(grad.coilpos(i,:),t2);
    end

 nMEG = length(indchantype(D,'MEG'));
    if nMEG~=size(pos2d,1)
        m1 = '2D positions could not be set as there are ';
        m2 =num2str(nMEG);
        m3 = ' channels but only ';
        m4 = num2str(size(pos2d,1));
        m5 =  ' channels with position information.';
        message = [m1,m2,m3,m4,m5];
        warning(message);
    else
        args=[];
        args.D=D;
        args.xy= pos2d';
        args.label=grad.label;
        args.task='setcoor2d';
        D=spm_eeg_prep(args);
        D.save;
    end
end
%- Foward  model specification
%--------------------------------------------------------------------------
if forward
    D.inv{1}.forward.voltype = S.voltype;
    D = spm_eeg_inv_forward(D);
    nverts = length(D.inv{1}.forward.mesh.vert);
    if(S.lead)
    [L,D] = spm_eeg_lgainmat(D,1:nverts);
    end
    spm_eeg_inv_checkforward(D,1,1);
end
save(D);
fprintf('%-40s: %30s\n','Completed',spm('time'));

end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                     Create Sensor Array                                 %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [pos,ori] = opm_createSensorArray(S)
% Given a scalp surface even samples the surface with sensors
%
% FORMAT [pos,ori] = spm_opm_createSensorArray(S)
%   S               - input structure
% Fields of S:
%   S.D            - SPM M/EEG object with surface meshes
%   S.offset       - distance to place sensors(mm) from scalp surface
%   S.space        - distance between sensors(mm)
%   S.wholehead    - boolean: Should whole scalp surface should be covered?
%   S.nDens        - number of density optimisations
% _________________________________________________________________________

% Args
%--------------------------------------------------------------------------
D = S.D;
wholehead = S.wholehead;

% Meshes
%--------------------------------------------------------------------------
scalp = gifti(D.inv{1}.mesh.tess_scalp);
cortex = gifti(D.inv{1}.mesh.tess_ctx);
lp = min(cortex.vertices(:,3));

% create convex hull of mesh
%--------------------------------------------------------------------------
[~,V1] = convhull(double(scalp.vertices));

% outward facing vertx normals of convex hull
%--------------------------------------------------------------------------
[Nv,~] = spm_mesh_normals(scalp,true);
ns = Nv;
vs = scalp.vertices;
cog1=mean(vs);

for i = 1:length(ns)
    ad = vs(i,:) + 5*ns(i,:);
    subtrac = vs(i,:) - 5*ns(i,:);

    d1 = sum((cog1 - ad).^2);
    d2 = sum((cog1 - subtrac).^2);

    if(d2>d1)
        ns(i,:) = -ns(i,:);
    else
        ns(i,:) =ns(i,:);
    end
end

% add an offset to the convex hull
%--------------------------------------------------------------------------
offVertices=vs+ns*S.offset;
[~,V2] = convhull(double(offVertices));

% Expand solution space by ratio of Volumes
%--------------------------------------------------------------------------
T= eye(4)*(V2/V1)^(1/3);
T(4,4)=1;
scalp = spm_mesh_transform(scalp,T);

% Translate solution space so centre of gravities overlap
%--------------------------------------------------------------------------

cog2 = mean(scalp.vertices);
T= eye(4);
T(1:3,4)=cog1-cog2+0;
scalp = spm_mesh_transform(scalp,T);

% Create the sensor array
%--------------------------------------------------------------------------
args= [];
args.space=S.space;
args.g=scalp;
args.nDens=S.nDens;
[pos, ~, ~] = spm_mesh_pack_points(args);

% get orientation of scalp
%--------------------------------------------------------------------------
[~,Nf] = spm_mesh_normals(scalp,true);
cog=mean(scalp.vertices);

v=scalp.vertices;
f=scalp.faces;
faceP=zeros(size(f,1),3);
for i =1:length(faceP)
    whichVerts= f(i,:);
    verts= v(whichVerts,:);
    faceP(i,:)=mean(verts);
end

% make all normals point outwards
%--------------------------------------------------------------------------
for i = 1:length(Nf)
    ad = faceP(i,:) + 5*Nf(i,:);
    subtrac = faceP(i,:) - 5*Nf(i,:);

    d1 = sum((cog - ad).^2);
    d2 = sum((cog - subtrac).^2);

    if(d2>d1)
        Nf(i,:) = -Nf(i,:);
    else
        Nf(i,:) =Nf(i,:);
    end
end

% assign orientatation to sensors of closest face normal
%--------------------------------------------------------------------------
  ori=zeros(size(pos));
for i =1:length(ori)
    tmp=pos(i,:);
    di=sqrt(sum(bsxfun(@minus,faceP,tmp).^2,2));
    [~,indmin]=min(di);
    ori(i,:)=-Nf(indmin,:);
end

% Check if wholehead is requested
%--------------------------------------------------------------------------
if(~wholehead)
    C= pos(:,3)>(lp);
    pos= pos(C,:);
    ori = ori(C,:);
end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                           Custom Meshes                                 %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 function D = opm_customMeshes(S)
% wrapper for adding custom meshes to MEG object
% FORMAT D = spm_opm_customMeshes(S)
%   S               - input structure
% Fields of S:
%   S.D             - Valid MEG object         - Default:
%   S.cortex        - Cortical mesh file       - Default: Use inverse normalised cortical mesh
%   S.scalp         - Scalp mesh file          - Default: Use inverse normalised scalp mesh
%   S.oskull        - Outer skull mesh file    - Default: Use inverse normalised outer skull mesh
%   S.iskull        - Inner skull mesh file    - Default: Use inverse normalised inner skull mesh
%   S.template      - is mesh in MNI space?    - Default: 0
% Output:
%  D           - MEEG object
%--------------------------------------------------------------------------

%- Default values & argument check
%--------------------------------------------------------------------------
if ~isfield(S, 'D'),           error('MEG object needs to be supplied'); end
if ~isfield(S, 'cortex'),      S.cortex = []; end
if ~isfield(S, 'scalp'),       S.scalp = []; end
if ~isfield(S, 'oskull'),      S.oskull = []; end
if ~isfield(S, 'iskull'),      S.iskull = []; end
if ~isfield(S, 'template'),    S.template = 0; end

D = S.D;
if ~isfield(D.inv{1}.mesh,'sMRI')
  error('MEG object needs to be contain inverse normalised meshes already')
end

%- add custom scalp and skull meshes if supplied
%--------------------------------------------------------------------------
if ~isempty(S.scalp)
    D.inv{1}.mesh.tess_scalp = S.scalp;
end

if ~isempty(S.oskull)
    D.inv{1}.mesh.tess_oskull = S.oskull;
end

if ~isempty(S.iskull)
    D.inv{1}.mesh.tess_iskull = S.iskull;
end

%- add custom cortex and replace MNI cortex with warped cortex
%--------------------------------------------------------------------------
if ~isempty(S.cortex)
    D.inv{1}.mesh.tess_ctx = S.cortex;
    if(S.template)
        D.inv{1}.mesh.tess_mni = S.cortex;
    else
        defs.comp{1}.inv.comp{1}.def = {D.inv{1}.mesh.def};
        defs.comp{1}.inv.space = {D.inv{1}.mesh.sMRI};
        defs.out{1}.surf.surface = {D.inv{1}.mesh.tess_ctx};
        defs.out{1}.surf.savedir.savesrc = 1;
        out = spm_deformations(defs);
        D.inv{1}.mesh.tess_mni  = export(gifti(out.surf{1}), 'spm');
    end
end
save(D);

 end


function [success,msg,fps] = ...
  ReadFPS_Ctrax(varargin)

success = false;
msg = ''; %#ok<NASGU>

[fps,intrxfile,annfile] = myparse(varargin,...
  'fps',1,'intrxfile','','annfile','');

% try to read from the trx file first
if isempty(intrxfile),
  msg = 'Input trxfile not yet set';
  return;
end

try
  [trx,~,success1,timestamps] = load_tracks(intrxfile,'',...
    'dosave',false,'annname',annfile,'verbose',false);
  if ~success1,
    msg = sprintf('Could not load tracks from trxfile %s',intrxfile);
    return;
  end
  readfps = false;
  if isfield(trx,'fps'),
    fps = [trx.fps];
    if nnz(~isnan(fps)) > 0,
      fps = nanmedian(fps);
      readfps = true;
    end
  end
  if ~readfps,
    if nnz(~isnan(timestamps)) == 0,
      msg = sprintf('No timestamps read in from trxfile %s',intrxfile);
      return;
    end
    fps = 1/nanmedian(diff(timestamps));
  end
catch ME,
  msg = sprintf('Could not load from trxfile %s: %s',intrxfile,getReport(ME));
  return;
end

success = true;
msg = 'Read fps from trxfile';


function Process_ConvertOptitrack2Pos(fbasename,varargin)

% USAGE
%
%   Process_ConvertOptitrack2Pos(fbasename,varargin)
%
%
%
%    fbasename   basename of the video file (should be filebasenmae.avi)
%    <options>      optional list of property-value pairs (see table below)
%
%    =========================================================================
%     Properties    Values
%    -------------------------------------------------------------------------
%     'syncDatFile' name of binary file where sync signal is stored
%                   (default = filebasename_digitalin.dat)
%     'syncSampFq'  sampling freqeuncy of the sync signal (default= 20kHz)
%     'syncChan'    sync channel to read (default = 1)
%     'syncNbCh'    number of channels in the sync file (default = 1)
%     'posSampFq'   sampling frequency of the final pos file (after
%                   interpolation)
%    =========================================================================
%
%
% This function assumes that a Motive (Optitrack) behavioral session
% has been exported to a .CSV file with default settings.
% (http://wiki.optitrack.com/index.php?title=Data_Export:_CSV)
%
%
%

if strcmp(fbasename,'')
    d = dir('*sessionInfo.mat');
    load(d.name);
    fbasename = sessionInfo.FileName;
end

warning('this functions is now deprecated and has been replaced by processConvertOptitrack2Behav.m')

syncDatFile = ['digitalin.dat']; % defaults if args aren't given... assuming a single digitalin channel on Intan
syncSampFq  = 20000;
syncChan    = 1;
syncNbCh    = 1;
posSampFq   = 120; %in Hz

% Parse options
for i = 1:2:length(varargin)
  if ~isa(varargin{i},'char')
    error(['Parameter ' num2str(i+3) ' is not a property .']);
  end
  switch(lower(varargin{i}))
    case 'syncdatfile'
      syncDatFile = varargin{i+1};
      if ~isa(duration,'char')
        error('Incorrect value for property ''syncDatFile'' .');
      end
    case 'syncsampfq'
      syncSampFq = varargin{i+1};
      if ~isa(frequency,'numeric')
        error('Incorrect value for property ''syncsampfq'' .');
      end
    case 'syncchan'
      syncChan = varargin{i+1};
      if ~isa(start,'numeric')pw
        error('Incorrect value for property ''syncChan'' .');
      end
        if start < 0, start = 0; end
    case 'syncnbch'
      syncNbCh = varargin{i+1};
      if ~isa(syncNbCh,'numeric')
        error('Incorrect value for property ''syncNbCh'' .');
      end
    case 'possampfq'
      posSampFq = varargin{i+1};
      if ~isa(posSampFq,'numeric')
        error('Incorrect value for property ''posSampFq'' .');
      end
    otherwise
      error(['Unknown property ''' num2str(varargin{i}) ''' .']);
  end
end

if exist([fbasename '.csv'],'file') % assumes csv naming matches basename
    dat = importdata([fbasename '.csv']);
    dat = scrubTracking(dat);
elseif ~isempty(dir('*.csv')) % looks for any csv file in the current recording folder
    csv = dir('*.csv');
    if length(csv) > 1
        dat=[];
       for i=1:length(csv)
           dat = [dat; importdata(csv(i).name)];
       end
    else
        dat = importdata(csv.name);
    end

    if isstruct(dat)
        d=[];
        for i=1:length(csv)
             d = [d;dat(i).data];
        end
        clear dat; dat.data = d;
    end
    dat = scrubTracking(dat);
elseif ~isempty(dir('Session*')) % Looks for a /Session*/ folder that Motive/Optitrack has created
    d = dir('Session*');
    cd(d.name)
    csv = dir('*.csv');
    if length(csv) > 1
        dat=[];
       for i=1:length(csv)
           dat = [dat; importdata(csv(i).name)];
       end
    else
        dat = importdata(csv.name);
    end
    if isstruct(dat)
        d=[];
        for i=1:length(csv)
             d = [d;dat(i).data];
        end
        clear dat; dat.data = d;
    else
        d.data = dat;
        dat = d;
    end
    dat = scrubTracking(dat);
    cd ..
end

pos = dat.data;

fid = fopen(syncDatFile);
dig = fread(fid,[syncNbCh inf],'int16=>int16');  % default type for Intan digitalin
dig = dig(syncChan,:);

t = (0:length(dig)-1)'/syncSampFq;

dPos = find(diff(dig)==1);
dNeg = find(diff(dig)==-1);

if length(dPos) == length(dNeg)+1
    dPos = dPos(1:end-1);
elseif length(dPos) > length(dNeg)+1 || length(dPos) < length(dNeg)
     keyboard
end

% Frame timing is the middle of shuter opening
frameT = (t(dPos)+t(dNeg))/2;

% The system sometimes (rarely) keeps on recording a few frames after software stopped
% recording. So we skip the last frames of the TTL

if length(frameT)<size(pos,1)
    warning('Too many video frames!')
    keyboard
elseif length(frameT)>size(pos,1)
    frameT = frameT(1:size(pos,1));
end

% We now interpolate the data at 120 Hz (or sampling fqcy specified in
% arguments)
% recDuration = length(dig)/syncSampFq;
%
% timestamps = (0:1/posSampFq:recDuration-1/posSampFq)';
% pos(pos==-1) = NaN;
% newPos = interp1(frameT,pos,timestamps,'linear');

% timestamps(isnan(newPos(:,2))) = [];
% newPos(isnan(newPos(:,2)),:)=[];

dlmwrite([fbasename '.pos'],[frameT pos],'delimiter','\t', 'precision', 32);

end


% This script train an LSTM or DNN based clean speech log spectrogram
% predictor, using simulated data of Reverb challenge.
%
% Xiong Xiao, Nanyang Technological University, Singapore
% Last modified: 08 Feb 2016.
%
%clear
function TrainEnhanceNet_Regression(modelType, hiddenLayerSize, hiddenLayerSizeFF, DeltaGenerationType, learning_rate, useGPU)
nUtt4Iteration = 10000;
nSequencePerMinibatch = 40;
useFileName = 1;

addpath('local', '../beamforming/lib', '../dereverb/local');     % remember to run addMyPath.m in SignalGraph root directory first.
% define SignalGraph settings. Type ParseOptions2() in command line to see the options.
para.IO.nStream = 2;        % two data streams, one is input signal, one is clean target log spectrogram
para.IO.mode = 'dynamicDistortion';     % distorted speech is generated dynamically during training. So each iteration uses different data.
para.NET.sequential = 1;    % do not randomize at frame level, randomize at sentence level
para.NET.variableLengthMinibatch = 1;   % a minibatch may contain multiple sentences of different lengths
para.NET.nSequencePerMinibatch = nSequencePerMinibatch;    % number of sentences per minibatch
para.NET.nSequencePerMinibatchCV = nSequencePerMinibatch; % number of sentences per minibatch for CV
para.NET.maxNumSentInBlock = 100;       % maximum number of sentences in a block
para.NET.L2weight = 3e-4;
para.NET.learning_rate = learning_rate;
para.NET.momentum = [0.9];
para.NET.learning_rate_decay_rate = 0.999;
para.NET.reduceLearnRateSpeed = 0.9;
para.NET.gradientClipThreshold = 1;
para.NET.weight_clip = 3;
para.useGPU     = useGPU;                  % don't use GPU for single sentence minibatch for LSTM. It's even slower than CPU.
para.minItr     = 100;
para.displayGPUstatus = 1;
para.displayInterval = ceil(para.NET.maxNumSentInBlock / para.NET.nSequencePerMinibatch / 10);
para.skipInitialEval = 1;

para.local.clean_wav_root = ChoosePath4OS({'E:\Data\LibriSpeech\train-clean-100', '/media/xiaoxiong/OS/data1/G/Libri/LibriSpeech/train-clean-100'});
para.local.clean_wav_ext = 'flac';
para.local.rir_wav_root = ChoosePath4OS({'E:\Data\ReverbEstimation\RIR_T60_1ch', '/media/xiaoxiong/DATA1/data1/ReverbEstimation/RIR_T60_1ch'});
para.local.rir_wav_ext = 'wav';
para.local.noise_wav_root = ChoosePath4OS({'E:\Data\musan\noise', '/media/xiaoxiong/DATA1/data1/musan/noise'});
para.local.noise_wav_ext = 'wav';
para.local.useFileName = useFileName;      % if set to 0, load all training data to memory. otherwise, only load file names.
para.local.seglen = 100;
para.local.segshift = 100;

para.topology.useChannel = 1;
para.topology.useWav = 1;
para.topology.RegressionNetType = modelType;
para.topology.useCMN = 0;
para.topology.DeltaGenerationType = DeltaGenerationType;   % Choose 'DeltaByEqn' or 'DeltaByAffine'
para.topology.MSECostWeightSDA = [1 4.5 10];
para.topology.hiddenLayerSize = hiddenLayerSize;
para.topology.hiddenLayerSizeFF = hiddenLayerSizeFF;
para = ConfigDereverbNet_Regression(para);

para.IO.DynamicDistortion.fs = para.topology.fs;
para.IO.DynamicDistortion.nUtt4Iteration = nUtt4Iteration;   % dynamically generate 10000 distorted sentences for each training iteration
para.IO.DynamicDistortion.nHours4Iteration = 10;    % dynamically generate 10 hours of distorted speech for each training iteration
para.IO.DynamicDistortion.SNR_PDF = 'uniform';      % distribution of SNR, choose [uniform|normal]
para.IO.DynamicDistortion.SNR_para = [-20 30];      % parameters of SNR PDF. If use uniform, it is the lowest and highest SNR allowed.
                                                    % if use normal, it is the mean and variance of SNR.
para.IO.DynamicDistortion.seglen = 100;             % cut distorted sentences into equal length segments.
para.IO.DynamicDistortion.segshift = 100;
para.IO.DynamicDistortion.frame_len = para.topology.frame_len;
para.IO.DynamicDistortion.frame_shift = para.topology.frame_shift;

[Data_tr, para] = LoadWavRIRNoise_Libri(para, 1);

[layer, para] = Build_EnhanceNet_Regression(Data_tr, para);

% load the cv data
para.local.cv_wav_root = ChoosePath4OS({'D:\Data\NoiseData\Libri\LibriSpeech\dev-parallel', '/media/xiaoxiong/DATA1/data1/Libri/LibriSpeech/dev-parallel'});
para.local.useFileName = 0;
[Data_cv, para] = LoadParallelWav_Libri(para, 2);

% generate directory and file names to store the networks.
if para.topology.useCMN; para.output = 'nnet/EnhanceRegression.CMN';
else;     para.output = 'nnet/EnhanceRegression.noCMN';    end
para.output = sprintf('%s.%s.MbSize%d.U%d.%d-%s', para.output, para.topology.DeltaGenerationType, ...
    para.NET.nSequencePerMinibatch, length(Data_tr(1).data),  3*(para.topology.fft_len/2+1), para.topology.RegressionNetType);
for i=1:length(para.topology.hiddenLayerSize)
    para.output = sprintf('%s-%d', para.output, para.topology.hiddenLayerSize(i));
end
if ~isempty(para.topology.hiddenLayerSizeFF)
    para.output = [para.output '-DNN'];
    for i=1:length(para.topology.hiddenLayerSizeFF)
        para.output = sprintf('%s-%d', para.output, para.topology.hiddenLayerSizeFF(i));
    end
end
para.output = sprintf('%s-%d.L2_%s.LR_%s/nnet', para.output, 3*(para.topology.fft_len/2+1), FormatFloat4Name(para.NET.L2weight),FormatFloat4Name(para.NET.learning_rate));
LOG = [];
para.displayTag = para.output(1:min(90, length(para.output)));

% show the configurations
para
para.NET
para.IO
para.topology

dnn = load('nnet/EnhanceRegression.noCMN.DeltaByEqn.MbSize20.U28539.771-LSTM-2048-771.L2_3E-4.LR_1E-4/nnet.itr62.LR4.97E-8.CV2439.245.mat');
layer = dnn.layer;

% train the network using SGD
trainGraph_SGD(layer, Data_tr, Data_cv, para, LOG);


% dipfit_gridsearch() - do initial batch-like dipole scan and fit to all
%                       data components and return a dipole model with a
%                       single dipole for each component.
%
% Usage:
%  >> EEGOUT = dipfit_gridsearch( EEGIN, varargin)
%
% Inputs:
%    ...
%
% Optional inputs:
%   'component' - vector with integers, ICA components to scan
%   'xgrid'     - vector with floats, grid positions along x-axis
%   'ygrid'     - vector with floats, grid positions along y-axis
%   'zgrid'     - vector with floats, grid positions along z-axis
%
% Output:
%    ...
%
% Author: Robert Oostenveld, SMI/FCDC, Nijmegen 2003, load/save by
%         Arnaud Delorme
%         Thanks to Nicolas Robitaille for his help on the CTF MEG
%         implementation

% SMI, University Aalborg, Denmark http://www.smi.auc.dk/
% FC Donders Centre, University Nijmegen, the Netherlands http://www.fcdonders.kun.nl

% Copyright (C) 2003 Robert Oostenveld, SMI/FCDC roberto@smi.auc.dk
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [EEGOUT] = dipfit_gridsearch(EEG, varargin)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% convert the optional arguments into a configuration structure that can be
% understood by FIELDTRIPs dipolefitting function
if nargin>2
  cfg = struct(varargin{:});
else
  help dipfit_gridsearch
  return
end

% specify the FieldTrip DIPOLEFITTING configuration
cfg.model      = 'moving';
cfg.gridsearch = 'yes';
cfg.nonlinear  = 'no';
% add some additional settings from EEGLAB to the configuration
tmpchanlocs    = EEG.chanlocs;
cfg.channel    = { tmpchanlocs(EEG.dipfit.chansel).labels };
if isfield(EEG.dipfit, 'vol')
    cfg.vol        = EEG.dipfit.vol;
elseif isfield(EEG.dipfit, 'hdmfile')
    cfg.hdmfile    = EEG.dipfit.hdmfile;
else
    error('no head model in EEG.dipfit')
end
if isfield(EEG.dipfit, 'elecfile') & ~isempty(EEG.dipfit.elecfile)
    cfg.elecfile = EEG.dipfit.elecfile;
end
if isfield(EEG.dipfit, 'gradfile') & ~isempty(EEG.dipfit.gradfile)
    cfg.gradfile = EEG.dipfit.gradfile;
end

% convert the EEGLAB data structure into a structure that looks as if it
% was computed using FIELDTRIPs componentanalysis function
comp = eeglab2fieldtrip(EEG, 'componentanalysis', 'dipfit');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Added code to handle CTF data with multipleSphere head model           %
%  This code is copy-pasted in dipfit_gridSearch, dipfit_nonlinear        %
%  The flag .isMultiSphere is used by dipplot                             %
%  Nicolas Robitaille, January 2007.                                      %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%Do some trick to force fieldtrip to use the multiple sphere model
if strcmpi(EEG.dipfit.coordformat, 'CTF')
   cfg = rmfield(cfg, 'channel');
   comp = rmfield(comp, 'elec');
   cfg.gradfile = EEG.dipfit.chanfile;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% END                                                                     %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if ~isfield(cfg, 'component')
  % default is to scan all components
  cfg.component = 1:size(comp.topo,2);
end

% for each component scan the whole brain with dipoles using FIELDTRIPs
% dipolefitting function
source = ft_dipolefitting(cfg, comp);

% reformat the output dipole sources into EEGLABs data structure
for i=1:length(cfg.component)
  EEG.dipfit.model(cfg.component(i)).posxyz = source.dip(i).pos;
  EEG.dipfit.model(cfg.component(i)).momxyz = reshape(source.dip(i).mom, 3, length(source.dip(i).mom)/3)';
  EEG.dipfit.model(cfg.component(i)).rv     = source.dip(i).rv;
end

EEGOUT = EEG;


function [rxnList, rxnFormulaList] = findRxnsFromMets(model, metList, varargin)
% Returns a list of reactions in which at least one
% metabolite listed in metList participates.
%
% USAGE:
%
%    [rxnList, rxnFormulaList] = findRxnsFromMets(model, metList, printFlag)
%
% INPUTS:
%    model:             COBRA model structure
%    metList:           Metabolite list
%
% OPTIONAL INPUTS:
%    printFlag:          Print reaction formulas to screen (Default = false)
%    Property/Value:    Allowed Properties are:
%                        * `containsAll` - If true only reactions containing all metabolites in metList are returned (Default: false)
%                        * `printFlag` - as above, will overwrite a `printFlag` set individually (Default: false)
%                        * `producersOnly` - Only return reactions which produce any of the given metabolites. (Default: false)
%                        * `consumersOnly` - Only return reactions which consume any of the given metabolites. (Default: false)
%                        * `exclusive` - Only return those reactions, which do not contain any metabolites but those given (Default: false)
%
% OUTPUTS:
%    rxnList:           List of reactions
%    rxnFormulaList:    Reaction formulas coresponding to `rxnList`
%
% .. Authors:
%       - Richard Que (08/12/2010)
%       - Almut Heinken (09/25/2015)- made change so formulas are not printed if reaction list is empty.
%       - Thomas Pfau (21/1/2016) - Additional Options, and minimal speedup of the indexing, also updated behaviour of verbFlag to accurately reflect the description.

verbFlag = false;
containsAll = false;
if isa(metList,'char')
    metList = {metList};
end

if mod(numel(varargin), 2) == 1 % the first argument has to be verbFlag, the remaining are property/value pairs
    verbFlag = varargin{1};
    if numel(varargin) > 1
        varargin = varargin(2:end);
    end
end

parser = inputParser();
parser.addParameter('containsAll',false,@(x) (isnumeric(x) && (x==1 || x ==0)) || islogical(x));
parser.addParameter('producersOnly',false,@(x) (isnumeric(x) && (x==1 || x ==0)) || islogical(x));
parser.addParameter('consumersOnly',false,@(x) (isnumeric(x) && (x==1 || x ==0)) || islogical(x));
parser.addParameter('printFlag',verbFlag,@(x) isnumeric(x) );
parser.addParameter('verbFlag',verbFlag,@(x) isnumeric || islogical(x) ); % backward compatability
parser.addParameter('exclusive',false,@(x)  (isnumeric(x) && (x==1 || x ==0)) || islogical(x));

parser.parse(varargin{:});
%Verbosity flag backward compatability
if ismember('verbFlag',parser.UsingDefaults)
    verbFlag = parser.Results.printFlag;
elseif ismember('printFlag',parser.UsingDefaults)
    verbFlag = parser.Results.verbFag;
else
    verbFlag = max([parser.Results.printFlag,parser.Results.verbFlag]);
end

containsAll = parser.Results.containsAll;
producersOnly = parser.Results.producersOnly;
consumersOnly = parser.Results.consumersOnly;
exclusive = parser.Results.exclusive;

%Find met indicies
index = ismember(model.mets,metList);
if producersOnly && consumersOnly
    producers = sum(model.S(index,:) > 0,1)';
    consumers = sum(model.S(index,:) < 0,1)';
    rels = producers > 0 & model.ub > 0 | consumers > 0 & model.lb < 0 | producers > 0 & model.lb < 0 | consumers > 0 & model.ub > 0;
    totals = producers + consumers;
elseif producersOnly
    producers = sum(model.S(index,:) > 0,1)';
    consumers = sum(model.S(index,:) < 0,1)';
    rels = producers > 0 & model.ub > 0 | consumers > 0 & model.lb < 0;
    totals = producers + consumers;
elseif consumersOnly
    producers = sum(model.S(index,:) > 0,1)';
    consumers = sum(model.S(index,:) < 0,1)';
    rels = producers > 0 & model.lb < 0 | consumers > 0 & model.ub > 0;
    totals = producers + consumers;
else
    totals = sum(model.S(index,:) ~= 0,1);
    rels = totals > 0;
end

if exclusive
    others = sum(model.S(~index,:) ~= 0);
    %exclude anything that has other reactants.
    rels = rels & others == 0;
end

if containsAll
   rxnList = model.rxns(totals == numel(metList) & rels);
else
    %rxns = repmat(model.rxns,1,length(index));
    %find reactions i.e. all columns with at least one non zero value
    rxnList = model.rxns(totals > 0 & rels);
end

if (nargout > 1) || verbFlag
    if ~isempty(rxnList)
        rxnFormulaList = printRxnFormula(model,rxnList,verbFlag);
    else
        rxnFormulaList={};
    end
end


function [balanced_ds,idxs,classes]=cosmo_balance_dataset(ds,varargin)
% sub-sample a dataset to have an equal number of samples for each target
%
% [balanced_ds,idxs,classes]=cosmo_balance_dataset(ds)
%
% Inputs:
%   ds                      dataset struct with fields .samples,
%                           .sa.targets and .sa.chunks. All values in
%                           .sa.chunks must be different from each other.
%   'sample_balancer', f    (optional)
%                           function handle with signature
%                               [idxs,classes]=f(targets,seed)
%                           where idxs is a SxC vector with indices for C
%                           classes and S targets per class. If omitted a
%                           builtin function is used.
%   'seed', s               (optional, default=1)
%                           Seed to use for pseudo-random number generation
%
% Output:
%   balanced_ds             dataset with a subset of the samples from ds
%                           so that each target occurs equally often.
%                           Selection is (by default) done in a
%                           pseudo-determistic manner.
%   idxs                    SxC vector indicating which
%   classes                 Cx1 vector containing unique class labels
%
% Notes:
%   - this function is to be used with MEEG datasets. it is not intended
%     for fMRI data.
%
% #   For CoSMoMVPA's copyright information and license terms,   #
% #   see the COPYING file distributed with CoSMoMVPA.           #

    defaults=struct();
    defaults.seed=1;
    defaults.sample_balancer=[];

    opt=cosmo_structjoin(defaults,varargin{:});

    check_inputs(ds,opt)

    [idxs,classes]=select_subset(ds.sa.targets,opt);
    balanced_ds=cosmo_slice(ds,idxs(:),1);

function [idxs,classes]=select_subset(targets,opt)
    sample_balancer=opt.sample_balancer;
    if isempty(sample_balancer)
        sample_balancer=@default_sample_balancer;
    end

    [idxs,classes]=sample_balancer(targets,opt.seed);

function [idxs,classes]=default_sample_balancer(targets,seed)
    [all_idxs,classes]=cosmo_index_unique(targets);
    class_counts=cellfun(@numel,all_idxs);

    min_count=min(class_counts);
    max_count=max(class_counts);

    nclasses=numel(class_counts);
    % invoke PRNG only once
    rand_vals=cosmo_rand(max_count,nclasses,'seed',seed);

    idxs=zeros(min_count,nclasses);
    for k=1:nclasses
        % set class_idxs to have values in the range 1:class_counts in
        % random order
        [unused,class_idxs]=sort(rand_vals(1:class_counts(k),k));

        % select min_count values from the indices in class_idxs
        idxs(:,k)=all_idxs{k}(class_idxs(1:min_count));
    end

function check_inputs(ds,opt)
    cosmo_check_dataset(ds);

    % chunks and targets must be present
    raise_exception=true;
    cosmo_isfield(ds,{'sa.targets','sa.chunks'},raise_exception);

    chunks=ds.sa.chunks;
    if numel(sort(chunks)) ~= numel(unique(chunks))
        error(['All values in .sa.chunks must be unique. If '...
                '*and only if* all '...
                'observations in .samples can be assumed to be '...
                'independent, for a dataset ds you can set '...
                '  ds.sa.chunks(:)=1:numel(ds.sa.chunks,1)'...
                'to indicate independence. This assumption typically '...
                'only applies to M/EEG datasets; this function should '...
                'not be used for typical fMRI datasets']);
    end