%% STUDY_04_wrist_scaffold_multiple_fiber_evaluation
% Below is a demonstration for:
%
% * Finite element analysis of the performance of additively manufactured scaffolds for scapholunate ligament reconstruction
%___________________________________________________________________________________________________________________________
%  This study presents a patient-specific computational biomechanical evaluation of the effect of scaffold length,
%  and positioning of the bone attachment sites. Through segmentation and image processing of medical image data
%  for natural wrist motion, detailed 3D geometries as well as patient-specific physiological wrist motion could
%  be derived. This data formed the input for detailed finite element analysis, enabling computational of scaffold
%  stress and strain distributions, which are key predictors of scaffold structural integrity.
%___________________________________________________________________________________________________________________________
% * febio_spec version 3.0
% * febio, FEBio
% * hexahedral elements, hex8
% * static, solid
% * hyperelastic, Ogden
% * displacement logfile
% * stress logfile

clear; close all; clc;

% Plot settings
fontSize=40;
numSmoothStepsMain=1;
cParSmoothMain.n=numSmoothStepsMain;
cParSmoothMain.Method='HC';

%%
% Path names
defaultFolder = fileparts(fileparts(mfilename('fullpath')));
savePath=fullfile(defaultFolder,'2021_Nataliya_Perevoshchikova_wrist_project','data','temp');
loadPathSurface=fullfile(defaultFolder,'2021_Nataliya_Perevoshchikova_wrist_project','data','STL');
loadPathMotion=fullfile(defaultFolder,'2021_Nataliya_Perevoshchikova_wrist_project','data','motion');

%% Building a quadrilateral circular mesh
d=0.35;
h=7;
xSpacing=0.93;
ySpacing=0.525;
nCopies_x=4;
nCopies_y=7;
r=d/2;
ne=2; %Elements in radius
f=0.6; %Fraction (with respect to outer radius) where central square appears

pointSpacingHeight=0.1;
numStepsSweep=ceil(h/pointSpacingHeight);

xc=zeros(numStepsSweep,1);
yc=zeros(numStepsSweep,1);
zc=linspace(-h/2,h/2,numStepsSweep)';
Vc=[xc yc zc];

%SweepLoft
%Create the mesh
[Fq,Vq]=discQuadMesh(ne,r,f);

Vq(:,3)=0;
Vq1=Vq;
Vq1(:,3)=Vq1(:,3)-h/2;
Vq2=Vq;
Vq2(:,3)=Vq2(:,3)+h/2;

% Visualizing mesh
cFigure; hold on;
title('Single fiber','FontSize',fontSize);
plotV(Vq1,'k.','lineWidth',5,'MarkerSize',25);
plotV(Vq2,'r.','lineWidth',5,'MarkerSize',25);
plotV(Vc,'g.-','lineWidth',5,'MarkerSize',25);

axisGeom;
camlight headlight;
drawnow;

[~,~,~,S]=sweepLoft(Vq1,Vq2,[0 0 1],[0 0 1],Vc,numStepsSweep,0,0);

X=S.X'; Y=S.Y'; Z=S.Z'; %Coordinate matrices
V=[X(:) Y(:) Z(:)]; %Create node list

I=size(Vq,1)*((1:1:numStepsSweep)-1);
I=I(ones(size(Fq,1),1),:);
I=I(:);

FQ=repmat(Fq,numStepsSweep,1)+I(:,ones(size(Fq,2),1));
Efib=[FQ(1:end-size(Fq,1),:) FQ(size(Fq,1)+1:end,:)]; %The hexahedral elements

[Efib,V,ind1,ind2]=mergeVertices(Efib,V); %Merge nodes (start and end are not shared yet)
%%
nCopies=nCopies_x*nCopies_y;
VC=repmat({V},nCopies,1);
EC=repmat({Efib},nCopies,1);
numNodesBar=size(V,1);
[Efib,V]=joinElementSets(EC,VC);

c=1;

for qx=1:1:nCopies_x
    for qy=1:1:nCopies_y
        if c>1
            ind1=((c-1)*numNodesBar)+1;
            ind2=(c*numNodesBar);
            V(ind1:ind2,1)=V(ind1:ind2,1)+(qx-1)*xSpacing;
            V(ind1:ind2,2)=V(ind1:ind2,2)+(qy-1)*ySpacing;
        end
        c=c+1;
    end
end

[F]=element2patch(Efib);

%% Get boundary conditions faces
%Get boundary faces
ind=tesBoundary(F,V);
Fb=F(ind,:);
%Get tops/bottoms
Nb=patchNormal(Fb,V);

zVec=[0 0 1];
d=dot(Nb,zVec(ones(size(Nb,1),1),:),2);
Z=V(:,3);
ZF=mean(Z(Fb),2);
logicTop_Fb=(d>0.9) & ZF>=(max(V(:,3))-eps(1));
logicBottom_Fb=(d<-0.9) & ZF<=(min(V(:,3))+eps(1));
F_start=Fb(logicTop_Fb,:);
F_end=Fb(logicBottom_Fb,:);

logic_Lig=(ZF>-1.5) & ZF<=1.5;
F_Lig=Fb(logic_Lig,:);

%%
% Visualizing mesh
cFigure; hold on;
title('Multiple fibers','FontSize',fontSize);
patch('faces',Fb,'vertices',V,'FaceColor','g','FaceAlpha',0.3,'EdgeColor','None');
plotV(V(F_end',:),'k.','lineWidth',5,'MarkerSize',20);
plotV(V(F_start',:),'r.','lineWidth',5,'MarkerSize',20);

camlight headlight;
axisGeom(gca,fontSize);
drawnow;

F_L=F_end;
F_S=F_start;

%% Scaphoid and lunate at first frame from TriPlane, Brown University
loadName=fullfile(loadPathSurface,['modelGeometry.mat']);
dataStruct=load(loadName);

FS_triP=dataStruct.FS;
FL_triP=dataStruct.FL;
FC_triP=dataStruct.FC;
FR_triP=dataStruct.FR;

VL_triP=dataStruct.VL;
VS_triP=dataStruct.VS;
VC_triP=dataStruct.VC;
VR_triP=dataStruct.VR;

%%%
hFig=cFigure;
hold on;
title('Radiography','FontSize',fontSize);

gpatch(FS_triP,VS_triP,'w','none',0.5);
gpatch(FL_triP,VL_triP,'w','none',0.5);
gpatch(FC_triP,VC_triP,'w','none',0.5);
gpatch(FR_triP,VR_triP,'w','none',0.5);

grid on; axis equal;
axisGeom(gca,fontSize);
camlight headlight;
drawnow;

%% Cutting Radius
pointSpacingR=mean(patchEdgeLengths(FR_triP,VR_triP));
%% Cut Capitate bone
% The capitate is cut in the x direction.

%Create a logic for cutting away faces
max_X1=max(VR_triP(:,1))-70*pointSpacingR; %Max x-level used for cutting
logicVertices=VR_triP(:,1)<max_X1; %Logic for the points below this level
logicFaces=all(logicVertices(FR_triP),2); %Logic for the faces
logicFaces=triSurfLogicSharpFix(FR_triP,logicFaces,3); %Altered logic so it is smoother

%%
% Visualize
cFigure; hold on;
gpatch(FS_triP,VS_triP,'w','none',0.5);
gpatch(FL_triP,VL_triP,'w','none',0.5);
gpatch(FR_triP,VR_triP,logicFaces,'none',0.5);
gpatch(FC_triP,VC_triP,'w','none',0.5);

camlight('headlight');
axisGeom(gca,fontSize);
drawnow;

%%
% Cut away faces using logic
FR_triP=FR_triP(logicFaces,:); %The faces to keep
[FR_triP,VR_triP]=patchCleanUnused(FR_triP,VR_triP); %Remove unused points

%Attempt to self triangulate potentially jagged edge
Eb=patchBoundary(FR_triP,VR_triP); %Get boundary edges
indBoundary=edgeListToCurve(Eb); %Convert boundary edges to a curve list
indBoundary=indBoundary(1:end-1); %Trim off last point since it is equal to first on a closed loop
angleThreshold=pi*(120/180); %threshold for self triangulation
[FR_triP,VR_triP,indBoundaryTop]=triSurfSelfTriangulateBoundary(FR_triP,VR_triP,indBoundary,angleThreshold,1);

%Force boundary to have the max X level chosen
VR_triP(indBoundaryTop,1)=max_X1;

%%
% Visualize
cFigure; hold on;
gpatch(FS_triP,VS_triP,'w','none',0.5);
gpatch(FL_triP,VL_triP,'w','none',0.5);
gpatch(FC_triP,VC_triP,'w','none',0.5);
gpatch(FR_triP,VR_triP,'w','k',0.5);
plotV(VR_triP(indBoundaryTop,:),'r-','LineWidth',3);

camlight('headlight');
axisGeom(gca,fontSize);
drawnow;

%% Close over top of radius
% The top boundary curve of the cut surface is filled with triangles. This
% is a 2D method. The x-coordinate is added after.
[F2t,V2t]=regionTriMesh2D({VR_triP(indBoundaryTop,[2 3])},pointSpacingR,0,0);
V2t(:,3)=V2t(:,2);
V2t(:,2)=V2t(:,1);
V2t(:,1)=max_X1; %Add/set X-level

%%
% Visualize
cFigure; hold on;
gpatch(FS_triP,VS_triP,'w','none',0.5);
gpatch(FL_triP,VL_triP,'w','none',0.5);
gpatch(FC_triP,VC_triP,'w','none',0.5);
gpatch(FR_triP,VR_triP,'rw','k',0.5);
gpatch(F2t,V2t,'gw','k',1);
plotV(VR_triP(indBoundaryTop,:),'r-','LineWidth',3);
camlight('headlight');
axisGeom(gca,fontSize);
drawnow;

%% Joining surface features
% Add all surface sets together in joint list of faces, vertices
[FR_triP,VR_triP]=joinElementSets({FR_triP,F2t},{VR_triP,V2t});
%% Merge shared nodes
% The join operation only adds the sets together. Nodes with the same
% coordinates are not seen as the same yet and need to be merged.
[FR_triP,VR_triP]=mergeVertices(FR_triP,VR_triP); %Merge nodes

%Remove tri connected
[FS,VS]=triSurfRemoveThreeConnect(FS_triP,VS_triP);
[FL,VL]=triSurfRemoveThreeConnect(FL_triP,VL_triP);
[FC,VC]=triSurfRemoveThreeConnect(FC_triP,VC_triP);
[FR,VR]=triSurfRemoveThreeConnect(FR_triP,VR_triP);
%Smoothen
[VL]=patchSmooth(FL,VL,[],cParSmoothMain);
[VS]=patchSmooth(FS,VS,[],cParSmoothMain);
[VR]=patchSmooth(FR,VR,[],cParSmoothMain);
[VC]=patchSmooth(FC,VC,[],cParSmoothMain);
%Refine
[FS,VS]=subtri(FS,VS,1);
[FL,VL]=subtri(FL,VL,1);
[FR,VR]=subtri(FR,VR,1);
[FC,VC]=subtri(FC,VC,1);
%Smoothen
[VL]=patchSmooth(FL,VL,[],cParSmoothMain);
[VS]=patchSmooth(FS,VS,[],cParSmoothMain);
[VR]=patchSmooth(FR,VR,[],cParSmoothMain);
[VC]=patchSmooth(FC,VC,[],cParSmoothMain);
%%%%

%Construct placement according to natural SLIL positioning
alphaRot=pi/2;
R=euler2DCM([alphaRot 0 0]); %The rotation tensor for each step
V=V*R; %Rotated 1 part for visualization of stepwise amount

alphaRot=pi/2;
R=euler2DCM([0 alphaRot 0]); %The rotation tensor for each step
V=V*R; %Rotated 1 part for visualization of stepwise amount

alphaRot=-pi/15;
R=euler2DCM([0 0 alphaRot]); %The rotation tensor for each step
V=V*R; %Rotated 1 part for visualization of stepwise amount

V(:,3)=V(:,3)-1.5;
V(:,1)=V(:,1)-15;
V(:,2)=V(:,2)-3.2;

hFig=cFigure;
hold on;

gpatch(FS,VS,'w','none',0.7);
gpatch(FL,VL,'w','none',0.7);

gpatch(FC,VC,'w','none',0.5);
gpatch(FR,VR,'w','none',0.5);

gpatch(Fb,V,'gw','k',1);

grid on; axis equal;
axisGeom(gca,fontSize);
camlight headlight;
drawnow;

%% Joining node sets
[E_F,V,C]=joinElementSets({Efib,FS,FL,FC,FR},{V,VS,VL,VC,VR});
E=E_F{1};
FS1=E_F{2};
FL1=E_F{3};
FC1=E_F{4};
FR1=E_F{5};

% hFig=cFigure;
% hold on;
%
% gpatch(FS1,V,'w','none',0.7);
% gpatch(FL1,V,'w','none',0.7);
%
% gpatch(FC1,V,'w','none',0.5);
% gpatch(FR1,V,'w','none',0.5);
%
% gpatch(E,V,'gw','k',1);
%
% grid on; axis equal;
% axisGeom(gca,fontSize);
% camlight headlight;
% drawnow;

%% FEA control settings
Step=5;
numTimeSteps=round((1989-1000)/Step);%Number of time steps desired
max_refs=300; %Max reforms
max_ups=0; %Set to zero to use full-Newton iterations
opt_iter=150; %Optimum number of iterations
max_retries=50; %Maximum number of retires
dtmin=(1/numTimeSteps)/10; %Minimum time step size
dtmax=1/numTimeSteps; %Maximum time step size
tStep=0.005*Step; %Duration of step
min_residual=1e-20;
Mag_val=1;

%% Control parameters
% Defining file names
febioFebFileNamePart='tempModel';
febioFebFileName=fullfile(savePath,[febioFebFileNamePart,'.feb']); %FEB file name
febioLogFileName=[febioFebFileNamePart,'.txt']; %FEBio log file name
febioLogFileName_disp=[febioFebFileNamePart,'_disp_out.txt']; %Log file name for exporting displacement
febioLogFileName_force=[febioFebFileNamePart,'_force_out.txt']; %Log file name for exporting force
febioLogFileName_stress=[febioFebFileNamePart,'_stress_out.txt']; %Log file name for exporting stress

%% Defining the FEBio input structure
% See also |febioStructTemplate| and |febioStruct2xml| and the FEBio user
% manual.

%Get a template with default settings
[febio_spec]=febioStructTemplate;

%febio_spec version
febio_spec.ATTR.version='3.0';

%Module section
febio_spec.Module.ATTR.type='solid';

%Control section
febio_spec.Control.analysis='STATIC';
febio_spec.Control.time_steps=numTimeSteps;
febio_spec.Control.step_size=tStep;
febio_spec.Control.time_stepper.dtmin=dtmin;
febio_spec.Control.time_stepper.dtmax=dtmax;
febio_spec.Control.time_stepper.max_retries=max_retries;
febio_spec.Control.time_stepper.opt_iter=opt_iter;
febio_spec.Control.solver.max_refs=max_refs;
febio_spec.Control.solver.max_ups=max_ups;
febio_spec.Control.solver.min_residual=min_residual;

%Material section
materialName1='Material1';
febio_spec.Material.material{1}.ATTR.id=1;
febio_spec.Material.material{1}.ATTR.name=materialName1;

%Material section
C1=57.21894783089972;
C2=57.23071414361289;
K=11444.966197451262;
M1=2.00012552045284;
M2=-1.9997730268725347;
t1=25.365;
g1=1.23339;

materialOption='elastic';%'elastic_viscoelastic'
switch materialOption
    case 'elastic'
        febio_spec.Material.material{1}.ATTR.type='Ogden';
        febio_spec.Material.material{1}.c1=C1;
        febio_spec.Material.material{1}.m1=M1;
        febio_spec.Material.material{1}.c2=C2;
        febio_spec.Material.material{1}.m2=M2;
        febio_spec.Material.material{1}.k=K;
    case 'elastic_viscoelastic'
        %Viscoelastic part
        febio_spec.Material.material{1}.ATTR.type='uncoupled viscoelastic';
        febio_spec.Material.material{1}.g1=g1;
        febio_spec.Material.material{1}.t1=t1;

        %Elastic part
        febio_spec.Material.material{1}.elastic{1}.ATTR.type='Ogden';
        febio_spec.Material.material{1}.elastic{1}.c1=C1;
        febio_spec.Material.material{1}.elastic{1}.m1=M1;
        febio_spec.Material.material{1}.elastic{1}.c2=C2;
        febio_spec.Material.material{1}.elastic{1}.m2=M2;
        febio_spec.Material.material{1}.elastic{1}.k=K;

end

materialName2='Lunate_side';
febio_spec.Material.material{2}.ATTR.name=materialName2;
febio_spec.Material.material{2}.ATTR.type='rigid body';
febio_spec.Material.material{2}.ATTR.id=2;
febio_spec.Material.material{2}.density=1;
febio_spec.Material.material{2}.center_of_mass=[0 0 0];

materialName3='Scaphoid_side';
febio_spec.Material.material{3}.ATTR.name=materialName3;
febio_spec.Material.material{3}.ATTR.type='rigid body';
febio_spec.Material.material{3}.ATTR.id=3;
febio_spec.Material.material{3}.density=1;
febio_spec.Material.material{3}.center_of_mass=[0 0 0];

materialName4='Lunate';
febio_spec.Material.material{4}.ATTR.name=materialName4;
febio_spec.Material.material{4}.ATTR.type='rigid body';
febio_spec.Material.material{4}.ATTR.id=4;
febio_spec.Material.material{4}.density=1;
febio_spec.Material.material{4}.center_of_mass=[0 0 0];

materialName5='Scaphoid';
febio_spec.Material.material{5}.ATTR.name=materialName5;
febio_spec.Material.material{5}.ATTR.type='rigid body';
febio_spec.Material.material{5}.ATTR.id=5;
febio_spec.Material.material{5}.density=1;
febio_spec.Material.material{5}.center_of_mass=[0 0 0];

materialName6='Capitate';
febio_spec.Material.material{6}.ATTR.name=materialName6;
febio_spec.Material.material{6}.ATTR.type='rigid body';
febio_spec.Material.material{6}.ATTR.id=6;
febio_spec.Material.material{6}.density=1;
febio_spec.Material.material{6}.center_of_mass=[0 0 0];

materialName7='Radius';
febio_spec.Material.material{7}.ATTR.name=materialName7;
febio_spec.Material.material{7}.ATTR.type='rigid body';
febio_spec.Material.material{7}.ATTR.id=7;
febio_spec.Material.material{7}.density=1;
febio_spec.Material.material{7}.center_of_mass=[0 0 0];

%Geometry section
% -> Nodes
febio_spec.Mesh.Nodes{1}.ATTR.name='nodeSet_all'; %The node set name
febio_spec.Mesh.Nodes{1}.node.ATTR.id=(1:size(V,1))'; %The node id's
febio_spec.Mesh.Nodes{1}.node.VAL=V; %The nodel coordinates

% % -> Elements
partName1='Scaffold';
febio_spec.Mesh.Elements{1}.ATTR.name=partName1; %Name of the element set
febio_spec.Mesh.Elements{1}.ATTR.mat=1; %material index for this set
febio_spec.Mesh.Elements{1}.ATTR.type='hex8'; %Element type of this set
febio_spec.Mesh.Elements{1}.elem.ATTR.id=(1:1:size(E,1))'; %Element id's
febio_spec.Mesh.Elements{1}.elem.VAL=E;

partName2='Lunate side';
febio_spec.Mesh.Elements{2}.ATTR.name=partName2; %Name of the element set
febio_spec.Mesh.Elements{2}.ATTR.mat=2; %material index for this set
febio_spec.Mesh.Elements{2}.ATTR.type='quad4'; %Element type of this set
febio_spec.Mesh.Elements{2}.elem.ATTR.id=size(E,1)+(1:1:size(F_L,1))'; %Element id's
febio_spec.Mesh.Elements{2}.elem.VAL=F_L;

partName3='Scaphoid side';
febio_spec.Mesh.Elements{3}.ATTR.name=partName3; %Name of the element set
febio_spec.Mesh.Elements{3}.ATTR.mat=3; %material index for this set
febio_spec.Mesh.Elements{3}.ATTR.type='quad4'; %Element type of this set
febio_spec.Mesh.Elements{3}.elem.ATTR.id=size(E,1)+(1:1:size(F_S,1))'; %Element id's
febio_spec.Mesh.Elements{3}.elem.VAL=F_S;

partName4='Lunate';
febio_spec.Mesh.Elements{4}.ATTR.name=partName4; %Name of the element set
febio_spec.Mesh.Elements{4}.ATTR.mat=4; %material index for this set
febio_spec.Mesh.Elements{4}.ATTR.type='tri3'; %Element type of this set
febio_spec.Mesh.Elements{4}.elem.ATTR.id=size(E,1)+(1:1:size(FL1,1))'; %Element id's
febio_spec.Mesh.Elements{4}.elem.VAL=FL1;

partName5='Scaphoid';
febio_spec.Mesh.Elements{5}.ATTR.name=partName5; %Name of the element set
febio_spec.Mesh.Elements{5}.ATTR.mat=5; %material index for this set
febio_spec.Mesh.Elements{5}.ATTR.type='tri3'; %Element type of this set
febio_spec.Mesh.Elements{5}.elem.ATTR.id=size(E,1)+(1:1:size(FS1,1))'; %Element id's
febio_spec.Mesh.Elements{5}.elem.VAL=FS1;

%Capitate
partName6='Capitate';
febio_spec.Mesh.Elements{6}.ATTR.name=partName6; %Name of the element set
febio_spec.Mesh.Elements{6}.ATTR.mat=6; %material index for this set
febio_spec.Mesh.Elements{6}.ATTR.type='tri3'; %Element type of this set
febio_spec.Mesh.Elements{6}.elem.ATTR.id=size(E,1)+(1:1:size(FC1,1))'; %Element id's
febio_spec.Mesh.Elements{6}.elem.VAL=FC1;

%Radius
partName7='Radius';
febio_spec.Mesh.Elements{7}.ATTR.name=partName7; %Name of the element set
febio_spec.Mesh.Elements{7}.ATTR.mat=7; %material index for this set
febio_spec.Mesh.Elements{7}.ATTR.type='tri3'; %Element type of this set
febio_spec.Mesh.Elements{7}.elem.ATTR.id=size(E,1)+(1:1:size(FR1,1))'; %Element id's
febio_spec.Mesh.Elements{7}.elem.VAL=FR1;

%MeshDomains section
febio_spec.MeshDomains.SolidDomain.ATTR.name=partName1;
febio_spec.MeshDomains.SolidDomain.ATTR.mat=materialName1;

febio_spec.MeshDomains.ShellDomain{1}.ATTR.name=partName2;
febio_spec.MeshDomains.ShellDomain{1}.ATTR.mat=materialName2;

febio_spec.MeshDomains.ShellDomain{2}.ATTR.name=partName3;
febio_spec.MeshDomains.ShellDomain{2}.ATTR.mat=materialName3;

febio_spec.MeshDomains.ShellDomain{3}.ATTR.name=partName4;
febio_spec.MeshDomains.ShellDomain{3}.ATTR.mat=materialName4;

febio_spec.MeshDomains.ShellDomain{4}.ATTR.name=partName5;
febio_spec.MeshDomains.ShellDomain{4}.ATTR.mat=materialName5;

febio_spec.MeshDomains.ShellDomain{5}.ATTR.name=partName6;
febio_spec.MeshDomains.ShellDomain{5}.ATTR.mat=materialName6;

febio_spec.MeshDomains.ShellDomain{6}.ATTR.name=partName7;
febio_spec.MeshDomains.ShellDomain{6}.ATTR.mat=materialName7;

% Boundary conditions
i=1;
for count=1:numTimeSteps

    FileData = load(fullfile(loadPathMotion, sprintf('Flex_Ext_%d_TriPlane_Lunate.mat',count)));
    MatT_Lunate = FileData.Trans_L;

    FileData = load(fullfile(loadPathMotion, sprintf('Flex_Ext_%d_TriPlane_Scaphoid.mat',count)));
    MatT_Scaphoid = FileData.Trans_S;

    FileData = load(fullfile(loadPathMotion, sprintf('Flex_Ext_%d_TriPlane_Capitate.mat',count)));
    MatT_Capitate = FileData.Trans_C;

    RLunate                       = MatT_Lunate(1:3,1:3);%radians
    RScaphoid                     = MatT_Scaphoid(1:3,1:3); %radians
    RCapitate                     = MatT_Capitate(1:3,1:3); %radians

    Prescribe_Lunate{i}       = MatT_Lunate(1:3,end);
    Prescribe_Scaphoid{i}     = MatT_Scaphoid(1:3,end);
    Prescribe_Capitate{i}     = MatT_Capitate(1:3,end);

    axangL                    = rotm2axang(RLunate);%Axis-Angle {[x, y, z], angle (radians)}
    AngleLunate{i}            = axangL(1:3)*axangL(4);%angle *rot_axis; Axis with angle magnitude (radians) [x, y, z]
    axangS                    = rotm2axang(RScaphoid);
    AngleScaphoid{i}          = axangS(1:3)*axangS(4);%angle*rot_axis;
    axangC                    = rotm2axang(RCapitate);
    AngleCapitate{i}          = axangC(1:3)*axangC(4);%angle*rot_axis;
    tStartNow(i)=count*tStep;
    i=i+1;
end

%Boundary conditions
%Rigid section
% %Start and End
febio_spec.Rigid.rigid_constraint{1}.ATTR.name='Rigid_Lunate_X';
febio_spec.Rigid.rigid_constraint{1}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{1}.rb=2;
febio_spec.Rigid.rigid_constraint{1}.dof='Rx';
febio_spec.Rigid.rigid_constraint{1}.value.ATTR.lc=1;
febio_spec.Rigid.rigid_constraint{1}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{1}.relative=0;

febio_spec.Rigid.rigid_constraint{2}.ATTR.name='Rigid_Lunate_Y';
febio_spec.Rigid.rigid_constraint{2}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{2}.rb=2;
febio_spec.Rigid.rigid_constraint{2}.dof='Ry';
febio_spec.Rigid.rigid_constraint{2}.value.ATTR.lc=2;
febio_spec.Rigid.rigid_constraint{2}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{2}.relative=0;

febio_spec.Rigid.rigid_constraint{3}.ATTR.name='Rigid_Lunate_Z';
febio_spec.Rigid.rigid_constraint{3}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{3}.rb=2;
febio_spec.Rigid.rigid_constraint{3}.dof='Rz';
febio_spec.Rigid.rigid_constraint{3}.value.ATTR.lc=3;
febio_spec.Rigid.rigid_constraint{3}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{3}.relative=0;

febio_spec.Rigid.rigid_constraint{4}.ATTR.name='Rigid_Lunate_Ru';
febio_spec.Rigid.rigid_constraint{4}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{4}.rb=2;
febio_spec.Rigid.rigid_constraint{4}.dof='Ru';
febio_spec.Rigid.rigid_constraint{4}.value.ATTR.lc=4;
febio_spec.Rigid.rigid_constraint{4}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{4}.relative=0;

febio_spec.Rigid.rigid_constraint{5}.ATTR.name='Rigid_Lunate_Rv';
febio_spec.Rigid.rigid_constraint{5}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{5}.rb=2;
febio_spec.Rigid.rigid_constraint{5}.dof='Rv';
febio_spec.Rigid.rigid_constraint{5}.value.ATTR.lc=5;
febio_spec.Rigid.rigid_constraint{5}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{5}.relative=0;

febio_spec.Rigid.rigid_constraint{6}.ATTR.name='Rigid_Lunate_Rw';
febio_spec.Rigid.rigid_constraint{6}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{6}.rb=2;
febio_spec.Rigid.rigid_constraint{6}.dof='Rw';
febio_spec.Rigid.rigid_constraint{6}.value.ATTR.lc=6;
febio_spec.Rigid.rigid_constraint{6}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{6}.relative=0;

febio_spec.Rigid.rigid_constraint{7}.ATTR.name='Rigid_Scaphoid_X';
febio_spec.Rigid.rigid_constraint{7}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{7}.rb=3;
febio_spec.Rigid.rigid_constraint{7}.dof='Rx';
febio_spec.Rigid.rigid_constraint{7}.value.ATTR.lc=7;
febio_spec.Rigid.rigid_constraint{7}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{7}.relative=0;

febio_spec.Rigid.rigid_constraint{8}.ATTR.name='Rigid_Scaphoid_Y';
febio_spec.Rigid.rigid_constraint{8}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{8}.rb=3;
febio_spec.Rigid.rigid_constraint{8}.dof='Ry';
febio_spec.Rigid.rigid_constraint{8}.value.ATTR.lc=8;
febio_spec.Rigid.rigid_constraint{8}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{8}.relative=0;

febio_spec.Rigid.rigid_constraint{9}.ATTR.name='Rigid_Scaphoid_Z';
febio_spec.Rigid.rigid_constraint{9}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{9}.rb=3;
febio_spec.Rigid.rigid_constraint{9}.dof='Rz';
febio_spec.Rigid.rigid_constraint{9}.value.ATTR.lc=9;
febio_spec.Rigid.rigid_constraint{9}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{9}.relative=0;

febio_spec.Rigid.rigid_constraint{10}.ATTR.name='Rigid_Scaphoid_RX';
febio_spec.Rigid.rigid_constraint{10}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{10}.rb=3;
febio_spec.Rigid.rigid_constraint{10}.dof='Ru';
febio_spec.Rigid.rigid_constraint{10}.value.ATTR.lc=10;
febio_spec.Rigid.rigid_constraint{10}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{10}.relative=0;

febio_spec.Rigid.rigid_constraint{11}.ATTR.name='Rigid_Scaphoid_RY';
febio_spec.Rigid.rigid_constraint{11}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{11}.rb=3;
febio_spec.Rigid.rigid_constraint{11}.dof='Rv';
febio_spec.Rigid.rigid_constraint{11}.value.ATTR.lc=11;
febio_spec.Rigid.rigid_constraint{11}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{11}.relative=0;

febio_spec.Rigid.rigid_constraint{12}.ATTR.name='Rigid_Scaphoid_RZ';
febio_spec.Rigid.rigid_constraint{12}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{12}.rb=3;
febio_spec.Rigid.rigid_constraint{12}.dof='Rw';
febio_spec.Rigid.rigid_constraint{12}.value.ATTR.lc=12;
febio_spec.Rigid.rigid_constraint{12}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{12}.relative=0;

% % -> Prescribed boundary conditions on the rigid body bones
febio_spec.Rigid.rigid_constraint{13}.ATTR.name='Rigid_Lunate_side_X';
febio_spec.Rigid.rigid_constraint{13}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{13}.rb=4;
febio_spec.Rigid.rigid_constraint{13}.dof='Rx';
febio_spec.Rigid.rigid_constraint{13}.value.ATTR.lc=1;
febio_spec.Rigid.rigid_constraint{13}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{13}.relative=0;

febio_spec.Rigid.rigid_constraint{14}.ATTR.name='Rigid_Lunate_side_Y';
febio_spec.Rigid.rigid_constraint{14}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{14}.rb=4;
febio_spec.Rigid.rigid_constraint{14}.dof='Ry';
febio_spec.Rigid.rigid_constraint{14}.value.ATTR.lc=2;
febio_spec.Rigid.rigid_constraint{14}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{14}.relative=0;

febio_spec.Rigid.rigid_constraint{15}.ATTR.name='Rigid_Lunate_side_Z';
febio_spec.Rigid.rigid_constraint{15}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{15}.rb=4;
febio_spec.Rigid.rigid_constraint{15}.dof='Rz';
febio_spec.Rigid.rigid_constraint{15}.value.ATTR.lc=3;
febio_spec.Rigid.rigid_constraint{15}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{15}.relative=0;

febio_spec.Rigid.rigid_constraint{16}.ATTR.name='Rigid_Lunate_side_RX';
febio_spec.Rigid.rigid_constraint{16}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{16}.rb=4;
febio_spec.Rigid.rigid_constraint{16}.dof='Ru';
febio_spec.Rigid.rigid_constraint{16}.value.ATTR.lc=4;
febio_spec.Rigid.rigid_constraint{16}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{16}.relative=0;

febio_spec.Rigid.rigid_constraint{17}.ATTR.name='Rigid_Lunate_side_RY';
febio_spec.Rigid.rigid_constraint{17}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{17}.rb=4;
febio_spec.Rigid.rigid_constraint{17}.dof='Rv';
febio_spec.Rigid.rigid_constraint{17}.value.ATTR.lc=5;
febio_spec.Rigid.rigid_constraint{17}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{17}.relative=0;

febio_spec.Rigid.rigid_constraint{18}.ATTR.name='Rigid_Lunate_side_RZ';
febio_spec.Rigid.rigid_constraint{18}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{18}.rb=4;
febio_spec.Rigid.rigid_constraint{18}.dof='Rw';
febio_spec.Rigid.rigid_constraint{18}.value.ATTR.lc=6;
febio_spec.Rigid.rigid_constraint{18}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{18}.relative=0;

% %Scaphoid
febio_spec.Rigid.rigid_constraint{19}.ATTR.name='Rigid_Scaphoid_side_X';
febio_spec.Rigid.rigid_constraint{19}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{19}.rb=5;
febio_spec.Rigid.rigid_constraint{19}.dof='Rx';
febio_spec.Rigid.rigid_constraint{19}.value.ATTR.lc=7;
febio_spec.Rigid.rigid_constraint{19}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{19}.relative=0;

febio_spec.Rigid.rigid_constraint{20}.ATTR.name='Rigid_Scaphoid_side_Y';
febio_spec.Rigid.rigid_constraint{20}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{20}.rb=5;
febio_spec.Rigid.rigid_constraint{20}.dof='Ry';
febio_spec.Rigid.rigid_constraint{20}.value.ATTR.lc=8;
febio_spec.Rigid.rigid_constraint{20}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{20}.relative=0;

febio_spec.Rigid.rigid_constraint{21}.ATTR.name='Rigid_Scaphoid_side_Z';
febio_spec.Rigid.rigid_constraint{21}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{21}.rb=5;
febio_spec.Rigid.rigid_constraint{21}.dof='Rz';
febio_spec.Rigid.rigid_constraint{21}.value.ATTR.lc=9;
febio_spec.Rigid.rigid_constraint{21}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{21}.relative=0;

febio_spec.Rigid.rigid_constraint{22}.ATTR.name='Rigid_Scaphoid_side_RX';
febio_spec.Rigid.rigid_constraint{22}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{22}.rb=5;
febio_spec.Rigid.rigid_constraint{22}.dof='Ru';
febio_spec.Rigid.rigid_constraint{22}.value.ATTR.lc=10;
febio_spec.Rigid.rigid_constraint{22}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{22}.relative=0;

febio_spec.Rigid.rigid_constraint{23}.ATTR.name='Rigid_Scaphoid_side_RY';
febio_spec.Rigid.rigid_constraint{23}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{23}.rb=5;
febio_spec.Rigid.rigid_constraint{23}.dof='Rv';
febio_spec.Rigid.rigid_constraint{23}.value.ATTR.lc=11;
febio_spec.Rigid.rigid_constraint{23}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{23}.relative=0;

febio_spec.Rigid.rigid_constraint{24}.ATTR.name='Rigid_Scaphoid_side_RZ';
febio_spec.Rigid.rigid_constraint{24}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{24}.rb=5;
febio_spec.Rigid.rigid_constraint{24}.dof='Rw';
febio_spec.Rigid.rigid_constraint{24}.value.ATTR.lc=12;
febio_spec.Rigid.rigid_constraint{24}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{24}.relative=0;

%%Capitate
febio_spec.Rigid.rigid_constraint{25}.ATTR.name='Rigid_Capitate_X';
febio_spec.Rigid.rigid_constraint{25}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{25}.rb=6;
febio_spec.Rigid.rigid_constraint{25}.dof='Rx';
febio_spec.Rigid.rigid_constraint{25}.value.ATTR.lc=13;
febio_spec.Rigid.rigid_constraint{25}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{25}.relative=0;

febio_spec.Rigid.rigid_constraint{26}.ATTR.name='Rigid_Capitate_Y';
febio_spec.Rigid.rigid_constraint{26}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{26}.rb=6;
febio_spec.Rigid.rigid_constraint{26}.dof='Ry';
febio_spec.Rigid.rigid_constraint{26}.value.ATTR.lc=14;
febio_spec.Rigid.rigid_constraint{26}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{26}.relative=0;

febio_spec.Rigid.rigid_constraint{27}.ATTR.name='Rigid_Capitate_Z';
febio_spec.Rigid.rigid_constraint{27}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{27}.rb=6;
febio_spec.Rigid.rigid_constraint{27}.dof='Rz';
febio_spec.Rigid.rigid_constraint{27}.value.ATTR.lc=15;
febio_spec.Rigid.rigid_constraint{27}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{27}.relative=0;

febio_spec.Rigid.rigid_constraint{28}.ATTR.name='Rigid_Capitate_RX';
febio_spec.Rigid.rigid_constraint{28}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{28}.rb=6;
febio_spec.Rigid.rigid_constraint{28}.dof='Ru';
febio_spec.Rigid.rigid_constraint{28}.value.ATTR.lc=16;
febio_spec.Rigid.rigid_constraint{28}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{28}.relative=0;

febio_spec.Rigid.rigid_constraint{29}.ATTR.name='Rigid_Capitate_RY';
febio_spec.Rigid.rigid_constraint{29}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{29}.rb=6;
febio_spec.Rigid.rigid_constraint{29}.dof='Rv';
febio_spec.Rigid.rigid_constraint{29}.value.ATTR.lc=17;
febio_spec.Rigid.rigid_constraint{29}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{29}.relative=0;

febio_spec.Rigid.rigid_constraint{30}.ATTR.name='Rigid_Capitate_RZ';
febio_spec.Rigid.rigid_constraint{30}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{30}.rb=6;
febio_spec.Rigid.rigid_constraint{30}.dof='Rw';
febio_spec.Rigid.rigid_constraint{30}.value.ATTR.lc=18;
febio_spec.Rigid.rigid_constraint{30}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{30}.relative=0;

% %Radius
febio_spec.Rigid.rigid_constraint{31}.ATTR.name='Rigid_Radiuas';
febio_spec.Rigid.rigid_constraint{31}.ATTR.type='fix';
febio_spec.Rigid.rigid_constraint{31}.rb=7;
febio_spec.Rigid.rigid_constraint{31}.dofs='Rx,Ry,Rz,Ru,Rv,Rw';

k=1;
for i=1:numTimeSteps+1

    if k==1
        Mat_X_Lunate(k,1)=0;
        Mat_X_Lunate(k,2)=0;

        Mat_Y_Lunate(k,1)=0;
        Mat_Y_Lunate(k,2)=0;

        Mat_Z_Lunate(k,1)=0;
        Mat_Z_Lunate(k,2)=0;

        Mat_RX_Lunate(k,1)=0;
        Mat_RX_Lunate(k,2)=0;

        Mat_RY_Lunate(k,1)=0;
        Mat_RY_Lunate(k,2)=0;

        Mat_RZ_Lunate(k,1)=0;
        Mat_RZ_Lunate(k,2)=0;

        Mat_X_Scaphoid(k,1)=0;
        Mat_X_Scaphoid(k,2)=0;

        Mat_Y_Scaphoid(k,1)=0;
        Mat_Y_Scaphoid(k,2)=0;

        Mat_Z_Scaphoid(k,1)=0;
        Mat_Z_Scaphoid(k,2)=0;

        Mat_RX_Scaphoid(k,1)=0;
        Mat_RX_Scaphoid(k,2)=0;

        Mat_RY_Scaphoid(k,1)=0;
        Mat_RY_Scaphoid(k,2)=0;

        Mat_RZ_Scaphoid(k,1)=0;
        Mat_RZ_Scaphoid(k,2)=0;

        Mat_X_Capitate(k,1)=0;
        Mat_X_Capitate(k,2)=0;

        Mat_Y_Capitate(k,1)=0;
        Mat_Y_Capitate(k,2)=0;

        Mat_Z_Capitate(k,1)=0;
        Mat_Z_Capitate(k,2)=0;

        Mat_RX_Capitate(k,1)=0;
        Mat_RX_Capitate(k,2)=0;

        Mat_RY_Capitate(k,1)=0;
        Mat_RY_Capitate(k,2)=0;

        Mat_RZ_Capitate(k,1)=0;
        Mat_RZ_Capitate(k,2)=0;
    else
        Mat_X_Lunate(k,1)=tStartNow(k-1);
        Mat_X_Lunate(k,2)=Prescribe_Lunate{k-1}(1);

        Mat_Y_Lunate(k,1)=tStartNow(k-1);
        Mat_Y_Lunate(k,2)=Prescribe_Lunate{k-1}(2);

        Mat_Z_Lunate(k,1)=tStartNow(k-1);
        Mat_Z_Lunate(k,2)=Prescribe_Lunate{k-1}(3);

        Mat_RX_Lunate(k,1)=tStartNow(k-1);
        Mat_RX_Lunate(k,2)=AngleLunate{k-1}(1);

        Mat_RY_Lunate(k,1)=tStartNow(k-1);
        Mat_RY_Lunate(k,2)=AngleLunate{k-1}(2);

        Mat_RZ_Lunate(k,1)=tStartNow(k-1);
        Mat_RZ_Lunate(k,2)=AngleLunate{k-1}(3);

        Mat_X_Scaphoid(k,1)=tStartNow(k-1);
        Mat_X_Scaphoid(k,2)=Prescribe_Scaphoid{k-1}(1);

        Mat_Y_Scaphoid(k,1)=tStartNow(k-1);
        Mat_Y_Scaphoid(k,2)=Prescribe_Scaphoid{k-1}(2);

        Mat_Z_Scaphoid(k,1)=tStartNow(k-1);
        Mat_Z_Scaphoid(k,2)=Prescribe_Scaphoid{k-1}(3);

        Mat_RX_Scaphoid(k,1)=tStartNow(k-1);
        Mat_RX_Scaphoid(k,2)=AngleScaphoid{k-1}(1);

        Mat_RY_Scaphoid(k,1)=tStartNow(k-1);
        Mat_RY_Scaphoid(k,2)=AngleScaphoid{k-1}(2);

        Mat_RZ_Scaphoid(k,1)=tStartNow(k-1);
        Mat_RZ_Scaphoid(k,2)=AngleScaphoid{k-1}(3);

        Mat_X_Capitate(k,1)=tStartNow(k-1);
        Mat_X_Capitate(k,2)=Prescribe_Capitate{k-1}(1);

        Mat_Y_Capitate(k,1)=tStartNow(k-1);
        Mat_Y_Capitate(k,2)=Prescribe_Capitate{k-1}(2);

        Mat_Z_Capitate(k,1)=tStartNow(k-1);
        Mat_Z_Capitate(k,2)=Prescribe_Capitate{k-1}(3);

        Mat_RX_Capitate(k,1)=tStartNow(k-1);
        Mat_RX_Capitate(k,2)=AngleCapitate{k-1}(1);

        Mat_RY_Capitate(k,1)=tStartNow(k-1);
        Mat_RY_Capitate(k,2)=AngleCapitate{k-1}(2);

        Mat_RZ_Capitate(k,1)=tStartNow(k-1);
        Mat_RZ_Capitate(k,2)=AngleCapitate{k-1}(3);

    end
   k=k+1;
end

%Lunate
febio_spec.LoadData.load_controller{1}.ATTR.id=1;
febio_spec.LoadData.load_controller{1}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{1}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{1}.points.point.VAL=Mat_X_Lunate;

febio_spec.LoadData.load_controller{2}.ATTR.id=2;
febio_spec.LoadData.load_controller{2}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{2}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{2}.points.point.VAL=Mat_Y_Lunate;

febio_spec.LoadData.load_controller{3}.ATTR.id=3;
febio_spec.LoadData.load_controller{3}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{3}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{3}.points.point.VAL=Mat_Z_Lunate;

febio_spec.LoadData.load_controller{4}.ATTR.id=4;
febio_spec.LoadData.load_controller{4}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{4}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{4}.points.point.VAL=Mat_RX_Lunate;

febio_spec.LoadData.load_controller{5}.ATTR.id=5;
febio_spec.LoadData.load_controller{5}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{5}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{5}.points.point.VAL=Mat_RY_Lunate;

febio_spec.LoadData.load_controller{6}.ATTR.id=6;
febio_spec.LoadData.load_controller{6}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{6}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{6}.points.point.VAL=Mat_RZ_Lunate;

% %Scaphoid
febio_spec.LoadData.load_controller{7}.ATTR.id=7;
febio_spec.LoadData.load_controller{7}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{7}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{7}.points.point.VAL=Mat_X_Scaphoid;

febio_spec.LoadData.load_controller{8}.ATTR.id=8;
febio_spec.LoadData.load_controller{8}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{8}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{8}.points.point.VAL=Mat_Y_Scaphoid;

febio_spec.LoadData.load_controller{9}.ATTR.id=9;
febio_spec.LoadData.load_controller{9}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{9}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{9}.points.point.VAL=Mat_Z_Scaphoid;

febio_spec.LoadData.load_controller{10}.ATTR.id=10;
febio_spec.LoadData.load_controller{10}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{10}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{10}.points.point.VAL=Mat_RX_Scaphoid;

febio_spec.LoadData.load_controller{11}.ATTR.id=11;
febio_spec.LoadData.load_controller{11}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{11}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{11}.points.point.VAL=Mat_RY_Scaphoid;

febio_spec.LoadData.load_controller{12}.ATTR.id=12;
febio_spec.LoadData.load_controller{12}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{12}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{12}.points.point.VAL=Mat_RZ_Scaphoid;

% %Capitate
febio_spec.LoadData.load_controller{13}.ATTR.id=13;
febio_spec.LoadData.load_controller{13}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{13}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{13}.points.point.VAL=Mat_X_Capitate;

febio_spec.LoadData.load_controller{14}.ATTR.id=14;
febio_spec.LoadData.load_controller{14}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{14}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{14}.points.point.VAL=Mat_Y_Capitate;

febio_spec.LoadData.load_controller{15}.ATTR.id=15;
febio_spec.LoadData.load_controller{15}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{15}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{15}.points.point.VAL=Mat_Z_Capitate;

febio_spec.LoadData.load_controller{16}.ATTR.id=16;
febio_spec.LoadData.load_controller{16}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{16}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{16}.points.point.VAL=Mat_RX_Capitate;

febio_spec.LoadData.load_controller{17}.ATTR.id=17;
febio_spec.LoadData.load_controller{17}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{17}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{17}.points.point.VAL=Mat_RY_Capitate;

febio_spec.LoadData.load_controller{18}.ATTR.id=18;
febio_spec.LoadData.load_controller{18}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{18}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{18}.points.point.VAL=Mat_RZ_Capitate;

febio_spec.Output.logfile.ATTR.file=febioLogFileName;
febio_spec.Output.logfile.node_data{1}.ATTR.file=febioLogFileName_disp;
febio_spec.Output.logfile.node_data{1}.ATTR.data='ux;uy;uz';
febio_spec.Output.logfile.node_data{1}.ATTR.delim=',';
febio_spec.Output.logfile.node_data{1}.VAL=1:size(V,1);

febio_spec.Output.logfile.node_data{2}.ATTR.file=febioLogFileName_force;
febio_spec.Output.logfile.node_data{2}.ATTR.data='Rx;Ry;Rz';
febio_spec.Output.logfile.node_data{2}.ATTR.delim=',';
febio_spec.Output.logfile.node_data{2}.VAL=1:size(V,1);

febio_spec.Output.logfile.element_data{1}.ATTR.file=febioLogFileName_stress;
febio_spec.Output.logfile.element_data{1}.ATTR.data='sx;sy;sz';
febio_spec.Output.logfile.element_data{1}.ATTR.delim=',';
%% Quick viewing of the FEBio input file structure
%The febView function can be used to view the xml structure in a MATLAB figure window.
%
%disp('Viewing the febio file');
%febView(febio_spec); %Viewing the febio file

%% Exporting the FEBio input file
% Exporting the febio_spec structure to an FEBio input file is done using
% the |febioStruct2xml| function.
febioStruct2xml(febio_spec,febioFebFileName); %Exporting to file and domNode

% %
% %
% % %% Running the FEBio analysis
% % % To run the analysis defined by the created FEBio input file the
% % % |runMonitorFEBio| function is used. The input for this function is a
% % % structure defining job settings e.g. the FEBio input file name. The
% % % optional output runFlag informs the user if the analysis was run
% % % succesfully.

febioAnalysis.run_filename=febioFebFileName; %The input file name
febioAnalysis.run_logname=febioLogFileName; %The name for the log file
febioAnalysis.disp_on=1; %Display information on the command window
febioAnalysis.disp_log_on=1; %Display convergence information in the command window
febioAnalysis.runMode='internal';%'internal';
febioAnalysis.t_check=0.25; %Time for checking log file (dont set too small)
febioAnalysis.maxtpi=1e99; %Max analysis time
febioAnalysis.maxLogCheckTime=3; %Max log file checking time

[runFlag]=runMonitorFEBio(febioAnalysis);%START FEBio NOW!!!!!!!!


function [p, v, q] = ch_nav_equ_local_tan(p, v, q ,acc, gyr, dt, gN)
%  惯导解算更新，当地直角坐标系，不考虑地球自转
% p          位置 XYZ 单位 m
% v          速度 XYZ 单位 m/s
% q         Qb2n姿态,四元数表示
% acc      比力， 加速度计测量值 单位  (m/s^2),
% gyr      角速度 (rad/s)]
% dt        dt (s) 积分间隔如 0.01s
% gn       当地重力向量

old_v = v;

sf = acc;

%  姿态结算
q = ch_att_upt(q, gyr, dt);

% 速度解算
sf = ch_qmulv(q, sf);
sf = sf + gN;
v = old_v + dt *sf;

% 位置解算
p = p + (old_v + v) *dt/2;

end

%
%
% function x = ch_nav_equ_local_tan(x ,u, dt, gN)
%
% persistent a_old;
% if isempty(a_old)
%    a_old= u(1:3);
% end
%
% old_v = x(4:6);
%
% a_new =u(1:3);
% %sf = sf + 0.5*cross(u(4:6)*dt, sf);
%
% %  姿态结算
% gyr = u(4:6);
% q_old = x(7:10);
% x(7:10) = ch_att_upt(x(7:10), gyr, dt);
% q_new = x(7:10);
%
% % 速度解算
%
% x(4:6) = old_v + ((ch_qmulv(q_new, a_new) + ch_qmulv(q_old, a_old) )/2 + gN) *dt;
%
% % 位置解算
% x(1:3) = x(1:3) + (old_v + x(4:6)) *dt/2;
% a_old = a_new;
% end
%
%


function value = month_length_islamic ( y, m )

%*****************************************************************************80
%
%% MONTH_LENGTH_ISLAMIC returns the number of days in an Islamic month.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 April 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, the year in which the month occurred.
%
%    Input, integer M, the number of the month.
%
%    Output, integer MONTH_LENGTH_ISLAMIC, the number of days
%    in the month.
%
  mdays = [ 30, 29, 30, 29, 30, 29, 30, 29, 30, 29, 30, 29 ];
%
%  Copy the input.
%
  m2 = m;
  y2 = y;
%
%  Check the input.
%
  [ y2, m2, ierror ] = ym_check_islamic ( y2, m2 );

  if ( ierror ~= 0 )
    month_length_islamic = 0;
    return
  end
%
%  Get the number of days in the month.
%
  value = mdays(m2);
%
%  If necessary, add 1 day for a leap year.
%
  if ( m2 == 12 && year_is_leap_islamic ( y2 ) )
    value = value + 1;
  end

  return
end


function [varargout] = xpower(x,varargin)
% [z80_ind,z80_grp,OUT] = xpower(x,[c],[true],[N],[gsd],[V],[S],[trueX])
% Calculates individual and group power as a function of design matrix,
% true effect magnitudes, and contrasts
% using central limit theorem approximations/theory for estimates
%
% tor wager, 2 / 22 / 04
%
% Defaults
% gsd = .28;          % group standard deviation, relative to true effect;
%                       default from VNL experiment (could be overestimate, bec. btwn + within
%                       not separated)
% N = 10;             % number of subjects, for group stats
% true                true effects; 1 is an increase of 1 std. dev. of scanner noise per event
%                     default is .47 SNR everywhere the first contrast has positive
%                     weights
%                     VNL estimates of signal / noise ratio for visual impulse response, 16 Hz
%                     flashing checkerboard for .5 s
%                     0.83% signal change response, 1.67% signal change error std. dev., .47 SNR
% c = [1 1 -1 -1];    % contrast matrix; one row per contrast, one column
% per condition; default is one contrast for each effect
%
% V                   is noise autocorrelation matrix,
%                       symmetric and ones on the diagonal, n x n (n is number of samples)
%                       default is white noise; see also
%                       canonical_autocorrelation
%
% S                   is filtering (high/low pass) matrix; see getSmoothing
%
%
% see power_map1.m for examples and scripts
% see xzpower for a simulation-based approach
%
% outputs:
% z80_ind and z80_grp are the power estimates for individual and group
% designs.  These correspond to the z-value that is expected to be reached
% in 80% of the realizations of the design.

% Another way of creating the matrices:
%E = inline('inv(x'' * x) * x'' * Vi','x','Vi'); % E is error cov matrix
% matrix of orthogonal projection onto model space * Vi
% test: demo to verify that E is constant for constant SNR, regardless of
% sigma and t (as long as their ratio is equivalent)
% sigma = 1; t = .5; e = E(S*X,S* sigma * Vi);ee = e * e', t ./ trace(ee).^.5

% -----------------------------------------------------------------
% Set up input arguments and defaults
% -----------------------------------------------------------------

% notes: 2/7/07:
% keep true signal |h| = 1, or input as entered
% wsd and gsd are error st. deviations, so SNR = true / wsd (individual
% level) and true / ...

wsd = .47;          % within-subjects d, signal / noise == wsd / 1
gsd = .28;            % group standard deviation, relative to true effect; default from VNL
N = 10;             % number of subjects, for group stats

for i = 1:length(varargin)
    switch i
        case 1
            if isempty(varargin{i})
                c = [eye(size(x,2)-1) zeros(size(x,2)-1,1)];
                % contrast matrix; one row per contrast, one column per condition
            else
                c = varargin{i};
            end
        case 2
            if isempty(varargin{i})
                true = c(1,:) > 0;   % true effects; 1 is an increase of magnitude 1; SNR depends on wsd and gsd
                % % % true = true .* .47;  % estimated SNR from visual experiment (VNL), n = 11
            else
                true = varargin{i};
            end
        case 3
            N = varargin{i};
        case 4
            gsd = varargin{i};
        case 5
            V = varargin{i};
        case 6
            S = varargin{i};
        case 7
            trueX = varargin{i};
    end
end

% defaults, if not entered

if ~(exist('c') == 1) || isempty(c), c = [eye(size(x,2)-1) zeros(size(x,2)-1,1)];   end
if ~(exist('true') == 1) || isempty(true),true = 1 .* (c(1,:) > 0); end

if ~(exist('V') == 1) || isempty(V), V = eye(size(x,1)); xc = [1 0 0];
    %[xc,V] = canonical_autocorrelation
end
if ~(exist('S') == 1), S = []; end

if ~(exist('trueX') == 1) || isempty(trueX), trueX = x; end

% defaults, if empty
%if isempty(true),true = 0.47 .* (c(1,:) > 0);,end
if isempty(N),N = 10; end
if isempty(gsd),gsd = 0.28; end
%if isempty(V), V = eye(size(x,1)); ,xc = [1 0 0];,end

% -----------------------------------------------------------------
% Do the power calculation - start with noncentrality parameters
% -----------------------------------------------------------------

if ~isempty(S),
    if size(S,1) ~= size(S,2), error('S is not square.'), end
    if size(S,1) ~= size(x,1), error('S and X dims do not match.'), end
    if size(S,1) ~= size(V,1), error('S and Vi dims do not match.'), end

    %xo = x;     % save original x, for true signal estimate
    x = S * x; V = S * V * S';  %V = S * V;
end

if size(c,2) < size(x,2), c(:,end+1) = 0;, end
if size(true,2) < size(x,2), true(:,end+1) = 0;, end

c = c';

if ~isempty(S)
    %y = S * x * true'; % S * xorig * true and x * true are equivalent; no need for xo
    % tor modified may 06
    y = S * trueX * true';      % which = S * (xorig*true), filtered orig. data
else
    y = trueX * true';
end

xtxi = inv(x' * x);
px = xtxi * x'; %pinv(x);

%b = c' * xtxi * x' * y; % REDUCES to input vector true, regardless of filtering; b = true';
b = c' * px * y;

% i think this one below has V as the sqrt of the V in Friston
%sx = sqrt(diag(c' * xtxi * x' * V' * V * x * xtxi * c))   % design part -- std of x, sqrt(var(x))
sx = sqrt(diag(c' * px * V * px' * c));

% %n = b ./ sx;    % noncentrality
% mod 2/07
n = b ./ (wsd .* sx);

% GETTING THE NONCENTRALITY PARAMETER n
% noncentrality parameter n = snr * b/se
% snr = |h| / sigma, where |h| = scaling of impulse response to neural
% event.  see Zarahn 2001, "Reference Effect for Power Analysis"
% We can simplify the computation by always assuming |h| = 1 and varying
% sigma, in the simplest case setting snr = 1.
%
% t-values are directly related to snr and n.
% t = (|h| * n) / sigma
% in the simplest case, where snr = 1, t = n.
%
% n is contrast weight invariant.  contrasts can be normalized by sum of
% squared contrast weights.
%
% FROM n to POWER
% By the central limit theorem, n = t if snr = 1, and n is normally
% distributed with std 1.  Then we can integrate the normal distribution.
%
% but since the noncentrality param is essentially a t-value, as is the 20%
% level for the 80% power value, we have to do a small additional
% adjustment: convert t to Z scores based on the df.

% Degrees of freedom: Worsley and Friston (1995) way (effective df),
% df reduction due to autocorrelation.  This will be different if
% a prewhitening strategy is used.

R = eye(size(x,1)) - x * xtxi * x';
U = R * V * V';
df = (trace(U) .^ 2) ./ (trace(U * U));

%df = size(x,1) - size(x,2);  % standard way, but not correct with
%autocorrelated errors

%t80_ind = norminv(.2,n,1);
% mod 2/07
t80_ind = nctinv(.2,df,n);  % 20th% of noncentral t distribution
ztmp = spm_t2z(t80_ind,df);
ztmp = min(ztmp,t80_ind);    % use min, as conversion fails for high-power

% % tmp = norminv(tcdf(t80_ind,df));
% % tmp = min(tmp,t80_ind);             % use min, as conversion fails for high-power
% %
varargout{1} = ztmp;

% GROUP POWER
% group power is calculated by considering the contribution of the
% individual se to the group se, and assuming a fixed intersubject variance (set nominally to 1).  with a
% with group rrue intersubject standard deviation (gsd) of 1, then
% gse = (gsd + E(indiv_se)) ./ sqrt(2N)
% by the central limit theorem.
% we use sqrt(2N) because std dev of n rnd variables = (s1 + s2 ... sn) / sqrt(n)
% individual se (ise) is the standard deviation of the sampling distribution
% gse is the standard deviation
%
% take as ise the coefficient of variation (cv), sx / b, which is contrast
% scale independent.

if nargout > 1

    %gse = (gsd + (sx ./ b)) ./ sqrt(2 * N);

    %ng = 1 ./ gse;  % normalized b / gse.  this is contrast weight-scaling independent

    % group standard error.  sqrt of sum of within + between, over
    % sqrt(N-1) to get standard error from standard deviation
    gse = sqrt( (wsd.*sx).^2 + gsd.^2 ) ./ sqrt(N - 1);

    ng = true(1:end-1)' ./ gse;  %  b / gse.  this is contrast weight-scaling independent

    dfg = N - 1;
    t80_grp = nctinv(.2, dfg, ng);  % 20th% of noncentral t distribution
    ztmp = spm_t2z(t80_grp,dfg);
    ztmp = min(ztmp,t80_grp);    % use min, as conversion fails for high-power

% %     t80_grp = norminv(.2,ng,1);
% %
% %     tmp = norminv(tcdf(t80_grp,dfg));   % adjust to Z-score based on df
% %     tmp = min(tmp,t80_grp);             % use min, as conversion fails for high-power
    varargout{2} = ztmp;

    if nargout > 2

        OUT.x = x;
        OUT.gsd = gsd;
        OUT.N = N;
        OUT.true = true;
        OUT.c = c';
        OUT.y = y;
        OUT.b = b;
        OUT.sx = sx;
        OUT.n = n;
        OUT.df = df;
        OUT.ng = ng;

        OUT.t80_ind = t80_ind;
        OUT.t80_grp = t80_grp;
        OUT.expected_t = b ./ (wsd .* sx);

        varargout{3} = OUT;

    end

end

return


function [ var_node, var_num ] = ns_t6_var_count ( element_num, ...
  element_node, node_num )

%*****************************************************************************80
%
%% NS_T6_VAR_COUNT counts the Navier Stokes variables on a T6 grid.
%
%  Discussion:
%
%    We are given a mesh of T6 elements, and asked to count, in advance,
%    the number of Navier-Stokes variables associated with the grid.
%    In particular, every node has two velocity variables associated with
%    it, but only a node that is a vertex of the element will also have
%    an associated pressure variable.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer ELEMENT_NUM, the number of elements.
%
%    Input, integer ELEMENT_NODE(ELEMENT_ORDER,ELEMENT_NUM);
%    ELEMENT_NODE(I,J) is the global index of local node I in element J.
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Output, integer VAR_NODE(NODE_NUM+1), used to find the variables
%    associated with a given node, which are in VAR in locations
%    VAR_NODE(NODE) to VAR_NODE(NODE+1)-1.  Note that the last entry of
%    this array points to the location just after the last location in VAR.
%
%    Output, integer VAR_NUM, the number of variables.
%
  element_order = 6;
%
%  Our job is easy once we determine which nodes are vertices.
%  So to begin with, let VAR_NODE count the number of variables
%  associated with each node.
%
  var_node(1:node_num) = 2;

  for element = 1 : element_num
    for order = 1 : 3
      node = element_node(order,element);
      var_node(node) = 3;
    end
  end
%
%  Count them.
%
  var_num = sum ( var_node(1:node_num) );
%
%  Make pointers.
%
  total = 1;

  for node = 1 : node_num
    num = var_node(node);
    var_node(node) = total;
    total = total + num;
  end
  var_node(node_num+1) = total;

  return
end


function result = normalizekm(km)
% Copyright 2012 Nino Shervashidze, Karsten Borgwardt
% normalizes kernelmatrix km
% such that diag(result) = 1, i.e. K(x,y) / sqrt(K(x,x) * K(y,y))
% @author Karsten Borgwardt
% @date June 3rd 2005
% all rights reserved

nv = sqrt(diag(km));
nm =  nv * nv';
knm = nm .^ -1;
for i = 1:size(knm,1)
for j = 1:size(knm,2)
if (knm(i,j) == Inf)
  knm(i,j) = 0;
end
end
end
result = km .* knm;


function [H, Z, S, psi] = sfactorization_wilson3x3(S,freq,Niterations,tol,cmbindx,fb,init,checkflag,stabilityfix)

% SFACTORIZATION_WILSON3X3 performs triplet-wise non-parametric spectral factorization on
% cross-spectra, based on Wilson's algorithm.
%
% Usage  : [H, Z, psi] = sfactorization_wilson(S,freq);
%
% Inputs : S (1-sided, 3D-spectral matrix in the form of Channel x Channel x frequency)
%        : freq (a vector of frequencies) at which S is given.
%
% Outputs: H (transfer function)
%        : Z (noise covariance)
%        : S (cross-spectral density 1-sided)
%        : psi (left spectral factor)
%
% This function is an implemention of Wilson's algorithm (Eq. 3.1)
% for spectral matrix factorization.
%
% Ref: G.T. Wilson,"The Factorization of Matricial Spectral Densities,"
% SIAM J. Appl. Math.23,420-426(1972).
% Written by M. Dhamala & G. Rangarajan, UF, Aug 3-4, 2006.
% Email addresses: mdhamala@bme.ufl.edu, rangaraj@math.iisc.ernet.in

% Copyright (C) 2009-2017, Jan-Mathijs Schoffelen
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

if nargin<9, stabilityfix = false; end
if nargin<8, checkflag = true;   end
if nargin<7, init      = 'chol'; end
if nargin<6, fb        = 'none'; end
if nargin<5
  ft_error('FieldTrip:connectivity:sfactorization_wilson3x3', 'when requesting multiple triplet-wise spectral decomposition, ''cmbindx'' needs to be specified');
end
if nargin<4, tol        = 1e-8;   end
if nargin<3, Niterations = 1000;  end

dfreq = round(diff(freq)*1e5)./1e5; % allow for some numeric issues
if ~all(dfreq==dfreq(1))
  ft_error('FieldTrip:connectivity:sfactorization_wilson3x3', 'frequency axis is not evenly spaced');
end

if freq(1)~=0
  ft_warning('FieldTrip:connectivity:sfactorization_wilson3x3', 'when performing non-parametric spectral factorization, the frequency axis should ideally start at 0, zero padding the spectral density');
  dfreq = mean(dfreq);
  npad  = freq(1)./dfreq;

  % update the freq axis and keep track of the frequency bins that are
  % expected in the output
  selfreq  = (1:numel(freq)) + npad;
  freq     = [(0:(npad-1))./dfreq freq];
  S        = cat(3, zeros(size(S,1), size(S,1), npad), S);
else
  selfreq  = 1:numel(freq);
end

% ensure input S is double (mex-files don't work with single)
S = double(S);

% check whether the last frequency bin is strictly real-valued.
% if that's the case, then it is assumed to be the Nyquist frequency
% and the two-sided spectral density will have an even number of
% frequency bins. if not, in order to preserve hermitian symmetry,
% the number of frequency bins needs to be odd.
Send = S(:,:,end);
N    = numel(freq);
m    = size(cmbindx,1);
if all(imag(Send(:))<abs(trace(Send)./size(Send,1)*1e-9))
  hasnyq = true;
  N2     = 2*(N-1);
else
  hasnyq = false;
  N2     = 2*(N-1)+1;
end

% preallocate memory for the identity matrix
I      = repmat(eye(3),[1 1 m N2]); % Defining 3 x 3 identity matrix

% %Step 1: Forming 2-sided spectral densities for ifft routine in matlab
% Sarr   = zeros(3,3,m,N2) + 1i.*zeros(3,3,m,N2);
% for c = 1:m
%   Stmp  = S(cmbindx(c,:),cmbindx(c,:),:);
%
%   % the input cross-spectral density is assumed to be weighted with a
%   % factor of 2 in all non-DC and Nyquist bins, therefore weight the
%   % DC-bin with a factor of 2 to get a correct two-sided representation
%   Sarr(:,:,c,1) = Stmp(:,:,1).*2;
%
%   for f_ind = 2:N
%     Sarr(:,:,c,       f_ind) = Stmp(:,:,f_ind);
%     Sarr(:,:,c,(N2+2)-f_ind) = Stmp(:,:,f_ind).';
%   end
% end
% Sarr2 = Sarr;

% preallocate memory for the 2-sided spectral density
Sarr = zeros(3,3,N2,m) + 1i.*zeros(3,3,N2,m);
for c = 1:m
  Sarr(:,:,1:N,c) = S(cmbindx(c,:),cmbindx(c,:),:);
end
if hasnyq
  N1 = N;
else
  N1 = N + 1; % the highest frequency needs to be represented twice, for symmetry purposes
end
Sarr(:,:,N1:N2,:) = flip(Sarr(:,:,2:N,:),3);
Sarr(2,1,N1:N2,:) = conj(Sarr(2,1,N1:N2,:));
Sarr(1,2,N1:N2,:) = conj(Sarr(1,2,N1:N2,:));
Sarr(3,1,N1:N2,:) = conj(Sarr(3,1,N1:N2,:));
Sarr(1,3,N1:N2,:) = conj(Sarr(1,3,N1:N2,:));
Sarr(3,2,N1:N2,:) = conj(Sarr(3,2,N1:N2,:));
Sarr(2,3,N1:N2,:) = conj(Sarr(2,3,N1:N2,:));
Sarr              = permute(Sarr, [1 2 4 3]);
Sarr(:,:,:,1)     = Sarr(:,:,:,1).*2; % weight the DC-bin

% the input cross-spectral density is assumed to be weighted with a
% factor of 2 in all non-DC and Nyquist bins, therefore weight the
% Nyquist bin with a factor of 2 to get a correct two-sided representation
if hasnyq
  Sarr(:,:,:,N) = Sarr(:,:,:,N).*2;
end

%Step 2: Computing covariance matrices
gam = real(reshape(ifft(reshape(Sarr, [9*m N2]), [], 2),[3 3 m N2]));

%Step 3: Initializing for iterations
gam0 = gam(:,:,:,1);

h    = complex(zeros(size(gam0)));
for k = 1:m
  switch init
    case 'chol'
      [tmp, dum] = chol(gam0(:,:,k));
      if dum
        ft_warning('initialization with ''chol'' for iterations did not work well, using arbitrary starting condition');
        tmp = rand(3,3); %arbitrary initial condition
        tmp = triu(tmp);
      end
    case 'rand'
      tmp = rand(3,3); %arbitrary initial condition
      tmp = triu(tmp);
    otherwise
      ft_error('initialization method should be eithe ''chol'' or ''rand''');
  end
  h(:,:,k) = tmp;

  %h(:,:,k) = chol(gam0(:,:,k));
end
psi  = repmat(h, [1 1 1 N2]);

%Step 4: Iterating to get spectral factors
ft_progress('init', fb, 'computing spectral factorization');
for iter = 1:Niterations
  ft_progress(iter./Niterations, 'computing iteration %d/%d\n', iter, Niterations);
  invpsi = inv3x3(psi);
  g      = sandwich3x3(invpsi, Sarr) + I;
  gp     = PlusOperator3x3(g,m,N,stabilityfix); %gp constitutes positive and half of zero lags

  psi_old = psi;
  psi     = mtimes3x3(psi, gp);

  if checkflag
    psierr  = abs(psi-psi_old)./abs(psi);
    psierrf = mean(psierr(:));
    if(psierrf<tol)
      fprintf('reaching convergence at iteration %d\n',iter);
      break;
    end % checking convergence
  end
end
ft_progress('close');

%Step 5: Getting covariance matrix from spectral factors
gamtmp = reshape(real(ifft(transpose(reshape(psi, [9*m N2]))))', [3 3 m N2]);

%Step 6: Getting noise covariance & transfer function (see Example pp. 424)
%
A0    = gamtmp(:,:,:,1);
A0inv = inv3x3(A0);

Z = zeros(3,3,m);
for k = 1:m
  %Z     = A0*A0.'*fs; %Noise covariance matrix
  Z(:,:,k) = A0(:,:,k)*A0(:,:,k).'; %Noise covariance matrix not multiplied by sampling frequency
  %FIXME check this; at least not multiplying it removes the need to correct later on
  %this also makes it more equivalent to the noisecov estimated by biosig's mvar-function
end

% H = complex(zeros(3,3,m,N));
% S = complex(zeros(3,3,m,N));
% for k = 1:N
%   for kk = 1:m
%     H(:,:,kk,k) = psi(:,:,kk,k)*A0inv(:,:,kk);  % Transfer function
%     S(:,:,kk,k) = psi(:,:,kk,k)*psi(:,:,kk,k)'; % Cross-spectral density
%   end
% end
H = mtimes3x3(psi,A0inv(:,:,:,ones(1,size(psi,4))));
S = mtimes3x3(psi,ctranspose3x3(psi));

siz = [size(H) 1 1];
H   = reshape(H, [9*siz(3) siz(4:end)]);
siz = [size(S) 1 1];
S   = reshape(S, [9*siz(3) siz(4:end)]);
siz = [size(Z) 1 1];
Z   = reshape(Z, [9*siz(3) siz(4:end)]);
siz = [size(psi) 1 1];
psi = reshape(psi, [9*siz(3) siz(4:end)]);

%if numel(selfreq)~=numel(freq)
  % return only the frequency bins that were in the input
  H   =   H(:,selfreq,:,:);
  S   =   S(:,selfreq,:,:);
  psi = psi(:,selfreq,:,:);
%end

%---------------------------------------------------------------------
function gp = PlusOperator3x3(g,ncmb,nfreq, stabilityfix)

% This function is for [ ]+operation:
% to take the positive lags & half of the zero lag and reconstitute
% M. Dhamala, UF, August 2006

g   = transpose(reshape(g, 9*ncmb, []));
gam = ifft(g);

% taking only the positive lags and half of the zero lag
gamp  = gam;
beta0 = 0.5*gam(1,:);

%for k = 1:ncmb
%  gamp(1,(k-1)*4+1:k*4) = reshape(triu(reshape(beta0(1,(k-1)*4+1:k*4),[2 2])),[1 4]);
%end
beta0(2:9:9*ncmb)   = 0;
beta0(3:9:9*ncmb)   = 0;
beta0(6:9:9*ncmb)   = 0;
gamp(1,:)           = beta0;
gamp(nfreq+1:end,:) = 0;

% smooth with a window, only for the long latency boundary: this is a
% stabilityfix proposed by Martin Vinck
if stabilityfix
  w = tukeywin(nfreq*2, 0.5);
  gamp(1:nfreq,:) = gamp(1:nfreq,:).*repmat(w(nfreq+1:end),[1 nchan^2]);
else
  % nothing to be done here
end

% reconstituting
gp = fft(gamp);
gp = reshape(transpose(gp), [3 3 ncmb numel(gp)/(9*ncmb)]);


% MANIPULATOR TRAJECTORY GENERATION
% Generates Joint space trajectories by performing inverse kinematics on
% each waypoint and interpolating between the joint angles.
%
% Copyright 2019 The MathWorks, Inc.

%% Setup
clear, clc, close all

% Define waypoint information
createWaypointData;

% Define IK
ik = inverseKinematics('RigidBodyTree',gen3);
ikWeights = [1 1 1 1 1 1];
ikInitGuess = jointAnglesHome';
ikInitGuess(ikInitGuess > pi) = ikInitGuess(ikInitGuess > pi) - 2*pi;
ikInitGuess(ikInitGuess < -pi) = ikInitGuess(ikInitGuess < -pi) + 2*pi;

% Set up plot
plotMode = 1; % 0 = None, 1 = Trajectory, 2 = Coordinate Frames
show(gen3,gen3.homeConfiguration,'Frames','off','PreservePlot',false);
xlim([-1 1]), ylim([-1 1]), zlim([0 1.2])
hold on
if plotMode == 1
    hTraj = plot3(waypoints(1,1),waypoints(2,1),waypoints(3,1),'b.-');
end
plot3(waypoints(1,:),waypoints(2,:),waypoints(3,:),'ro','LineWidth',2);

%% Solve IK for all waypoints
includeOrientation = false; % Set this to use zero vs. nonzero orientations

numWaypoints = size(waypoints,2);
numJoints = numel(gen3.homeConfiguration);
jointWaypoints = zeros(numJoints,numWaypoints);

for idx = 1:numWaypoints
    if includeOrientation
        tgtPose = trvec2tform(waypoints(:,idx)') * eul2tform(orientations(:,idx)');
    else
        tgtPose =  trvec2tform(waypoints(:,idx)');
    end
    [config,info] = ik(eeName,tgtPose,ikWeights,ikInitGuess);
    jointWaypoints(:,idx) = config';
end

%% Generate trajectory on joint space
trajType = 'trap';
switch trajType
    case 'trap'
        [q,qd,qdd] = trapveltraj(jointWaypoints,numel(trajTimes), ...
            'AccelTime',repmat(waypointAccelTimes,[numJoints 1]), ...
            'EndTime',repmat(diff(waypointTimes),[numJoints 1]));

    case 'cubic'
        [q,qd,qdd] = cubicpolytraj(jointWaypoints,waypointTimes,trajTimes, ...
            'VelocityBoundaryCondition',zeros(numJoints,numWaypoints));

    case 'quintic'
        [q,qd,qdd] = quinticpolytraj(jointWaypoints,waypointTimes,trajTimes, ...
            'VelocityBoundaryCondition',zeros(numJoints,numWaypoints), ...
            'AccelerationBoundaryCondition',zeros(numJoints,numWaypoints));

    case 'bspline'
        ctrlpoints = jointWaypoints; % Can adapt this as needed
        [q,qd,qdd] = bsplinepolytraj(ctrlpoints,waypointTimes([1 end]),trajTimes);

    otherwise
        error('Invalid trajectory type! Use ''trap'', ''cubic'', ''quintic'', or ''bspline''');
end

% To visualize the trajectory, run the following line
% plotTrajectory(trajTimes,q,qd,qdd,'Names',"Joint " + string(1:numJoints),'WaypointTimes',waypointTimes)

%% Trajectory following loop
for idx = 1:numel(trajTimes)

    config = q(:,idx)';

    % Find Cartesian points for visualization
    eeTform = getTransform(gen3,config,eeName);
    if plotMode == 1
        eePos = tform2trvec(eeTform);
        set(hTraj,'xdata',[hTraj.XData eePos(1)], ...
                  'ydata',[hTraj.YData eePos(2)], ...
                  'zdata',[hTraj.ZData eePos(3)]);
    elseif plotMode == 2
        plotTransforms(tform2trvec(eeTform),tform2quat(eeTform),'FrameSize',0.05);
    end

    % Show the robot
    show(gen3,config,'Frames','off','PreservePlot',false);
    title(['Trajectory at t = ' num2str(trajTimes(idx))])
    drawnow

end


function lik = lik_gaussian(varargin)
%LIK_GAUSSIAN  Create a Gaussian likelihood structure
%
%  Description
%    LIK = LIK_GAUSSIAN('PARAM1',VALUE1,'PARAM2,VALUE2,...)
%    creates a Gaussian likelihood structure in which the named
%    parameters have the specified values. Any unspecified
%    parameters are set to default values.
%
%    LIK = LIK_GAUSSIAN(LIK,'PARAM1',VALUE1,'PARAM2,VALUE2,...)
%    modify a likelihood function structure with the named
%    parameters altered with the specified values.
%
%    Parameters for Gaussian likelihood function [default]
%      sigma2       - variance [0.1]
%      sigma2_prior - prior for sigma2 [prior_logunif]
%      n            - number of observations per input (See using average
%                     observations below)
%
%    Note! If the prior is 'prior_fixed' then the parameter in
%    question is considered fixed and it is not handled in
%    optimization, grid integration, MCMC etc.
%
%    Using average observations
%    The lik_gaussian can be used to model data where each input vector is
%    attached to an average of varying number of observations. That is, we
%    have input vectors x_i, average observations y_i and sample sizes n_i.
%    Each observation is distributed
%
%        y_i ~ N(f(x_i), sigma2/n_i)
%
%    The model is constructed as lik_gaussian('n', n), where n is the same
%    length as y and collects the sample sizes.
%
%  See also
%    GP_SET, PRIOR_*, LIK_*

% Internal note: Because Gaussian noise can be combined
% analytically to the covariance matrix, lik_gaussian is internally
% little between lik_* and gpcf_* functions.
%
% Copyright (c) 2007-2017 Jarno Vanhatalo
% Copyright (c) 2010 Aki Vehtari

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

  ip=inputParser;
  ip.FunctionName = 'LIK_GAUSSIAN';
  ip.addOptional('lik', [], @(x) isstruct(x) || isempty(x));
  ip.addParamValue('sigma2',0.1, @(x) isscalar(x) && x>=0);
  ip.addParamValue('sigma2_prior',prior_logunif(), @(x) isstruct(x) || isempty(x));
  ip.addParamValue('n',[], @(x) isreal(x) && all(x>0));
  ip.parse(varargin{:});
  lik=ip.Results.lik;

  if isempty(lik)
    init=true;
    lik.type = 'Gaussian';
  else
    if ~isfield(lik,'type') || ~isequal(lik.type,'Gaussian')
      error('First argument does not seem to be a valid likelihood function structure')
    end
    init=false;
  end

  % Initialize parameters
  if init || ~ismember('sigma2',ip.UsingDefaults)
    lik.sigma2 = ip.Results.sigma2;
  end
  if init || ~ismember('n',ip.UsingDefaults)
    lik.n = ip.Results.n;
  end
  % Initialize prior structure
  if init
    lik.p=[];
  end
  if init || ~ismember('sigma2_prior',ip.UsingDefaults)
    lik.p.sigma2=ip.Results.sigma2_prior;
  end
  if init
    % Set the function handles to the subfunctions
    lik.fh.pak = @lik_gaussian_pak;
    lik.fh.unpak = @lik_gaussian_unpak;
    lik.fh.ll = @lik_gaussian_ll;
    lik.fh.llg = @lik_gaussian_llg;
    lik.fh.llg2 = @lik_gaussian_llg2;
    lik.fh.llg3 = @lik_gaussian_llg3;
    lik.fh.lp = @lik_gaussian_lp;
    lik.fh.lpg = @lik_gaussian_lpg;
    lik.fh.cfg = @lik_gaussian_cfg;
    lik.fh.tiltedMoments = @lik_gaussian_tiltedMoments;
    lik.fh.trcov  = @lik_gaussian_trcov;
    lik.fh.trvar  = @lik_gaussian_trvar;
    lik.fh.predy = @lik_gaussian_predy;
    lik.fh.siteDeriv = @lik_gaussian_siteDeriv;
    lik.fh.recappend = @lik_gaussian_recappend;
  end

end

function [w, s, h] = lik_gaussian_pak(lik)
%LIK_GAUSSIAN_PAK  Combine likelihood parameters into one vector.
%
%  Description
%    W = LIK_GAUSSIAN_PAK(LIK) takes a likelihood structure LIK
%    and combines the parameters into a single row vector W.
%    This is a mandatory subfunction used for example in energy
%    and gradient computations.
%
%       w = [ log(lik.sigma2)
%             (hyperparameters of lik.magnSigma2)]'
%
%  See also
%    LIK_GAUSSIAN_UNPAK

  w = []; s = {}; h=[];
  if ~isempty(lik.p.sigma2)
    w = [w log(lik.sigma2)];
    s = [s; 'log(gaussian.sigma2)'];
    h = [h 0];
    % Hyperparameters of sigma2
    [wh, sh, hh] = lik.p.sigma2.fh.pak(lik.p.sigma2);
    w = [w wh];
    s = [s; sh];
    h = [h hh];
  end

end

function [lik, w] = lik_gaussian_unpak(lik, w)
%LIK_GAUSSIAN_UNPAK  Extract likelihood parameters from the vector.
%
%  Description
%    W = LIK_GAUSSIAN_UNPAK(W, LIK) takes a likelihood structure
%    LIK and extracts the parameters from the vector W to the LIK
%    structure. This is a mandatory subfunction used for example
%    in energy and gradient computations.
%
%    Assignment is inverse of
%       w = [ log(lik.sigma2)
%             (hyperparameters of lik.magnSigma2)]'
%
%  See also
%    LIK_GAUSSIAN_PAK

  if ~isempty(lik.p.sigma2)
    lik.sigma2 = exp(w(1));
    w = w(2:end);

    % Hyperparameters of sigma2
    [p, w] = lik.p.sigma2.fh.unpak(lik.p.sigma2, w);
    lik.p.sigma2 = p;
  end
end

function logLik = lik_gaussian_ll(lik, y, f, ~)
%LIK_GAUSSIAN_LL    Log likelihood
%
%  Description
%    E = LIK_GAUSSIAN_LL(LIK, Y, F, Z) takes a likelihood data
%    structure LIK, incedence counts Y, expected counts Z, and
%    latent values F. Returns the log likelihood, log p(y|f,z).
%    This subfunction is needed when using Laplace approximation
%    or MCMC for inference with non-Gaussian likelihoods. This
%    subfunction is also used in information criteria (DIC, WAIC)
%    computations.
%
%  See also
%    LIK_GAUSSIAN_LLG, LIK_GAUSSIAN_LLG3, LIK_GAUSSIAN_LLG2, GPLA_E

  s2 = lik.sigma2;
  r2 = (f-y).^2;
  logLik =  sum(-0.5 * r2./s2 - 0.5*log(s2) - 0.5*log(2*pi));

end

function llg = lik_gaussian_llg(lik, y, f, param, ~)
%LIK_GAUSSIAN_LLG    Gradient of the log likelihood
%
%  Description
%    G = LIK_GAUSSIAN_LLG(LIK, Y, F, PARAM) takes a likelihood
%    structure LIK, incedence counts Y, expected counts Z
%    and latent values F. Returns the gradient of the log
%    likelihood with respect to PARAM. At the moment PARAM can be
%    'param' or 'latent'. This subfunction is needed when using
%    Laplace approximation or MCMC for inference with non-Gaussian
%    likelihoods.
%
%  See also
%    LIK_GAUSSIAN_LL, LIK_GAUSSIAN_LLG2, LIK_GAUSSIAN_LLG3, GPLA_E

switch param
    case 'param'
        % there is also correction due to the log transformation
        s2 = lik.sigma2;
        r2 = (f-y).^2;
        llg =  0.5.* sum( r2./s2 -1 );

    case 'latent'
        s2 = lik.sigma2;
        r = f-y;
        llg =  - r./s2 ;
end
end

function llg2 = lik_gaussian_llg2(lik, y, f, param, ~)
%LIK_GAUSSIAN_LLG2  Second gradients of the log likelihood
%
%  Description
%    G2 = LIK_GAUSSIAN_LLG2(LIK, Y, F, PARAM) takes a likelihood
%    structure LIK, incedence counts Y, expected counts Z,
%    and latent values F. Returns the Hessian of the log
%    likelihood with respect to PARAM. At the moment PARAM can be
%    only 'latent'. G2 is a vector with diagonal elements of the
%    Hessian matrix (off diagonals are zero). This subfunction
%    is needed when using Laplace approximation or EP for inference
%    with non-Gaussian likelihoods.

%
%  See also
%    LIK_GAUSSIAN_LL, LIK_GAUSSIAN_LLG, LIK_GAUSSIAN_LLG3, GPLA_E

switch param
    case 'latent'
        s2 = lik.sigma2;
        llg2 =  - ones(size(y))./s2 ;
    case 'latent+param'
        % there is also correction due to the log transformation
        s2 = lik.sigma2;
        r = f-y;
        llg2 =  r./s2 ;

end
end

function llg3 = lik_gaussian_llg3(lik, y, f, param, z)
%LIK_GAUSSIAN_LLG3  Third gradients of the log likelihood
%
%  Description
%    G3 = LIK_GAUSSIAN_LLG3(LIK, Y, F, PARAM) takes a likelihood
%    structure LIK, incedence counts Y, expected counts Z
%    and latent values F and returns the third gradients of the
%    log likelihood with respect to PARAM. At the moment PARAM
%    can be only 'latent'. G3 is a vector with third gradients.
%    This subfunction is needed when using Laplace approximation
%    for inference with non-Gaussian likelihoods.
%
%  See also
%    LIK_GAUSSIAN_LL, LIK_GAUSSIAN_LLG, LIK_GAUSSIAN_LLG2, GPLA_E, GPLA_G

switch param
    case 'latent'
        llg3 = zeros(size(y));
    case 'latent2+param'
        % there is also correction due to the log transformation
        s2 = lik.sigma2;
        llg3 = ones(size(y))./s2 ;
end
end

function lp = lik_gaussian_lp(lik)
%LIK_GAUSSIAN_LP  Evaluate the log prior of likelihood parameters
%
%  Description
%    LP = LIK_T_LP(LIK) takes a likelihood structure LIK and
%    returns log(p(th)), where th collects the parameters.
%    This subfunctions is needed when there are likelihood
%    parameters.
%
%  See also
%    LIK_GAUSSIAN_PAK, LIK_GAUSSIAN_UNPAK, LIK_GAUSSIAN_G, GP_E

  lp = 0;

  if ~isempty(lik.p.sigma2)
    likp=lik.p;
    lp = likp.sigma2.fh.lp(lik.sigma2, likp.sigma2) + log(lik.sigma2);
  end
end

function lpg = lik_gaussian_lpg(lik)
%LIK_GAUSSIAN_LPG  Evaluate gradient of the log prior with respect
%                  to the parameters.
%
%  Description
%    LPG = LIK_GAUSSIAN_LPG(LIK) takes a Gaussian likelihood
%    function structure LIK and returns LPG = d log (p(th))/dth,
%    where th is the vector of parameters. This subfunction is
%    needed when there are likelihood parameters.
%
%  See also
%    LIK_GAUSSIAN_PAK, LIK_GAUSSIAN_UNPAK, LIK_GAUSSIAN_E, GP_G

  lpg = [];

  if ~isempty(lik.p.sigma2)
    likp=lik.p;

    lpgs = likp.sigma2.fh.lpg(lik.sigma2, likp.sigma2);
    lpg = lpgs(1).*lik.sigma2 + 1;
    if length(lpgs) > 1
      lpg = [lpg lpgs(2:end)];
    end
  end
end

function DKff = lik_gaussian_cfg(lik, x, x2)
%LIK_GAUSSIAN_CFG  Evaluate gradient of covariance with respect to
%                 Gaussian noise
%
%  Description
%    Gaussian likelihood is a special case since it can be
%    analytically combined with covariance functions and thus we
%    compute gradient of covariance instead of gradient of likelihood.
%
%    DKff = LIK_GAUSSIAN_CFG(LIK, X) takes a Gaussian likelihood
%    function structure LIK, a matrix X of input vectors and
%    returns DKff, the gradients of Gaussian noise covariance
%    matrix Kff = k(X,X) with respect to th (cell array with
%    matrix elements). This subfunction is needed only in Gaussian
%    likelihood.
%
%    DKff = LIK_GAUSSIAN_CFG(LIK, X, X2) takes a Gaussian
%    likelihood function structure LIK, a matrix X of input
%    vectors and returns DKff, the gradients of Gaussian noise
%    covariance matrix Kff = k(X,X) with respect to th (cell
%    array with matrix elements). This subfunction is needed
%    only in Gaussian likelihood.
%
%  See also
%    LIK_GAUSSIAN_PAK, LIK_GAUSSIAN_UNPAK, LIK_GAUSSIAN_E, GP_G

  DKff = {};
  if ~isempty(lik.p.sigma2)
      if isempty(lik.n)
          DKff{1}=lik.sigma2;
      else
          n=size(x,1);
          DKff{1} = sparse(1:n, 1:n, lik.sigma2./lik.n, n, n);
      end
  end
end

function DKff  = lik_gaussian_ginput(lik, x, t, g_ind, gdata_ind, gprior_ind, varargin)
%LIK_GAUSSIAN_GINPUT  Evaluate gradient of likelihood function with
%                     respect to x.
%
%  Description
%    DKff = LIK_GAUSSIAN_GINPUT(LIK, X) takes a likelihood
%    function structure LIK, a matrix X of input vectors and
%    returns DKff, the gradients of likelihood matrix Kff =
%    k(X,X) with respect to X (cell array with matrix elements).
%    This subfunction is needed only in Gaussian likelihood.
%
%    DKff = LIK_GAUSSIAN_GINPUT(LIK, X, X2) takes a likelihood
%    function structure LIK, a matrix X of input vectors and
%    returns DKff, the gradients of likelihood matrix Kff =
%    k(X,X2) with respect to X (cell array with matrix elements).
%    This subfunction is needed only in Gaussian likelihood.
%
%  See also
%    LIK_GAUSSIAN_PAK, LIK_GAUSSIAN_UNPAK, LIK_GAUSSIAN_E, GP_G

end

function C = lik_gaussian_trcov(lik, x)
%LIK_GAUSSIAN_TRCOV  Evaluate training covariance matrix
%                    corresponding to Gaussian noise
%
%  Description
%    C = LIK_GAUSSIAN_TRCOV(GP, TX) takes in covariance function
%    of a Gaussian process GP and matrix TX that contains
%    training input vectors. Returns covariance matrix C. Every
%    element ij of C contains covariance between inputs i and j
%    in TX. This subfunction is needed only in Gaussian likelihood.
%
%  See also
%    LIK_GAUSSIAN_COV, LIK_GAUSSIAN_TRVAR, GP_COV, GP_TRCOV

  [n, m] =size(x);
  n1=n+1;

  if isempty(lik.n)
      C = sparse(1:n,1:n,ones(n,1).*lik.sigma2,n,n);
  else
      C = sparse(1:n, 1:n, lik.sigma2./lik.n, n, n);
  end

end

function C = lik_gaussian_trvar(lik, x)
%LIK_GAUSSIAN_TRVAR  Evaluate training variance vector
%                    corresponding to Gaussian noise
%
%  Description
%    C = LIK_GAUSSIAN_TRVAR(LIK, TX) takes in covariance function
%    of a Gaussian process LIK and matrix TX that contains
%    training inputs. Returns variance vector C. Every element i
%    of C contains variance of input i in TX. This subfunction is
%    needed only in Gaussian likelihood.
%
%
%  See also
%    LIK_GAUSSIAN_COV, GP_COV, GP_TRCOV

  [n, m] =size(x);
  if isempty(lik.n)
      C=repmat(lik.sigma2,n,1);
  else
      C=lik.sigma2./lik.n(:);
  end

end

function [lpy, Ey, Vary] = lik_gaussian_predy(lik, Ef, Varf, yt, zt)
%LIK_Gaussian_PREDY    Returns the predictive mean, variance and density of y
%
%  Description
%    LPY = LIK_POISSON_PREDY(LIK, EF, VARF YT, ZT)
%    Returns also the predictive density of YT, that is
%        p(yt | y,zt) = \int p(yt | f, zt) p(f|y) df.
%    This requires also the incedence counts YT, expected counts ZT.
%    This subfunction is needed when computing posterior predictive
%    distributions for future observations.
%
%    [LPY, EY, VARY] = LIK_POISSON_PREDY(LIK, EF, VARF, YT, ZT)
%    takes a likelihood structure LIK, posterior mean EF and
%    posterior variance VARF of the latent variable and returns the
%    posterior predictive mean EY and variance VARY of the
%    observations related to the latent variables. This subfunction
%    is needed when computing posterior predictive distributions for
%    future observations.
%
%
%  See also
%    GPLA_PRED, GPEP_PRED, GPMC_PRED

  lpy = [];
  Ey = Ef;
  EVary = lik.sigma2;
  VarEy = Varf;
  Vary = EVary + VarEy;
  if numel(yt) ~= 0
      lpy = norm_lpdf(yt,Ey,sqrt(Vary));
  end
end

function [logM_0, m_1, sigm2hati1] = lik_gaussian_tiltedMoments(lik, y, i1, sigm2_i, myy_i, z)
%LIK_PROBIT_TILTEDMOMENTS  Returns the marginal moments for EP algorithm
%
%  Description
%    [M_0, M_1, M2] = LIK_PROBIT_TILTEDMOMENTS(LIK, Y, I, S2,
%    MYY) takes a likelihood structure LIK, class labels Y, index
%    I and cavity variance S2 and mean MYY. Returns the zeroth
%    moment M_0, mean M_1 and variance M_2 of the posterior
%    marginal (see Rasmussen and Williams (2006): Gaussian
%    processes for Machine Learning, page 55). This subfunction
%    is needed when using EP for inference with non-Gaussian
%    likelihoods.
%
%  See also
%    GPEP_E

%   m_1=myy_i;
%   sigm2hati1=sigm2_i;
%   logM_0=zeros(size(y));

s2 = lik.sigma2;
tau = 1./s2 + 1./sigm2_i;
w = (y(i1)./s2 + myy_i./sigm2_i)./tau;
%Zi = 1 ./( sqrt( 2.*pi.*(s2+sigm2_i) ) ) .* exp( -0.5*(y(i1)-myy_i).^2./(s2+sigm2_i) );

m_1 = w;
sigm2hati1 = 1./tau;
%logM_0 = log(Zi)
logM_0 = -0.5*log( 2.*pi) - 0.5*log( (s2+sigm2_i) )  + ( -0.5*(y(i1)-myy_i).^2./(s2+sigm2_i) );

end

function [g_i] = lik_gaussian_siteDeriv(lik, y, i1, sigm2_i, myy_i, z)
%LIK_NEGBIN_SITEDERIV  Evaluate the expectation of the gradient
%                      of the log likelihood term with respect
%                      to the likelihood parameters for EP
%
%  Description [M_0, M_1, M2] =
%    LIK_NEGBIN_SITEDERIV(LIK, Y, I, S2, MYY, Z) takes a
%    likelihood structure LIK, incedence counts Y, expected
%    counts Z, index I and cavity variance S2 and mean MYY.
%    Returns E_f [d log p(y_i|f_i) /d a], where a is the
%    likelihood parameter and the expectation is over the
%    marginal posterior. This term is needed when evaluating the
%    gradients of the marginal likelihood estimate Z_EP with
%    respect to the likelihood parameters (see Seeger (2008):
%    Expectation propagation for exponential families). This
%    subfunction is needed when using EP for inference with
%    non-Gaussian likelihoods and there are likelihood parameters.
%
%  See also
%    GPEP_G

s2 = lik.sigma2;
tau = 1/s2 + 1/sigm2_i;
w = (y(i1)/s2 + myy_i/sigm2_i)/tau;
%Zi = 1/( sqrt(2*pi*(s2+sigm2_i)) )*exp( -0.5*(y(i1)-myy_i)^2/(s2+sigm2_i) );

%g_i = 0.5*( (1/tau + w.^2 -2*w*y(i1) + y(i1).^2 ) /s2 - 1)/s2 * s2;
g_i = 0.5*( (1/tau + w.^2 -2*w*y(i1) + y(i1).^2 ) /s2 - 1);

end

function reclik = lik_gaussian_recappend(reclik, ri, lik)
%RECAPPEND  Record append
%
%  Description
%    RECLIK = LIK_GAUSSIAN_RECAPPEND(RECLIK, RI, LIK) takes a
%    likelihood function record structure RECLIK, record index RI
%    and likelihood function structure LIK with the current MCMC
%    samples of the parameters. Returns RECLIK which contains all
%    the old samples and the current samples from LIK. This
%    subfunction is needed when using MCMC sampling (gp_mc).
%
%  See also
%    GP_MC and GP_MC -> RECAPPEND

  if nargin == 2
    % Initialize the record
    reclik.type = 'Gaussian';

    % Initialize the parameters
    reclik.sigma2 = [];
    reclik.n = [];

    % Set the function handles
    reclik.fh.pak = @lik_gaussian_pak;
    reclik.fh.unpak = @lik_gaussian_unpak;
    reclik.fh.lp = @lik_gaussian_lp;
    reclik.fh.lpg = @lik_gaussian_lpg;
    reclik.fh.ll= @lik_gaussian_ll;
    reclik.fh.llg = @lik_gaussian_llg;
    reclik.fh.llg2 = @lik_gaussian_llg2;
    reclik.fh.llg3 = @lik_gaussian_llg3;
    reclik.fh.tiltedMoments = @lik_gaussian_tiltedMoments;
    reclik.fh.siteDeriv = @lik_gaussian_siteDeriv;
    reclik.fh.cfg = @lik_gaussian_cfg;
    reclik.fh.trcov  = @lik_gaussian_trcov;
    reclik.fh.trvar  = @lik_gaussian_trvar;
    reclik.fh.predy = @lik_gaussian_predy;
    reclik.fh.recappend = @lik_gaussian_recappend;

    reclik.p=[];
    reclik.p.sigma2=[];
    if ~isempty(ri.p.sigma2)
      reclik.p.sigma2 = ri.p.sigma2;
    end
  else
    % Append to the record
    likp = lik.p;

    % record sigma2
    reclik.sigma2(ri,:)=lik.sigma2;
    if isfield(likp,'sigma2') && ~isempty(likp.sigma2)
      reclik.p.sigma2 = likp.sigma2.fh.recappend(reclik.p.sigma2, ri, likp.sigma2);
    end
    % record n if given
    if isfield(lik,'n') && ~isempty(lik.n)
      reclik.n(ri,:)=lik.n(:)';
    end
  end
end


%--- help for statespace/forecast ---
%
% FORECAST Forecast states and observations of state-space models
%
%  Syntax:
%
%    [Y,YMSE,X,XMSE] = forecast(Mdl,numPeriods,Y0)
%    [Y,YMSE,X,XMSE] = forecast(Mdl,numPeriods,name,value,...)
%
%  Description:
%
%    Generate multiple-period forecasts for observation vector y(t) and state
%    vector x(t) for a general state-space model (SSM) of the form:
%
%    State equation:       x(t) = A(t) * x(t-1) + B(t) * u(t)
%    Observation equation: y(t) = C(t) * x(t)   + D(t) * e(t)
%
%    where u(t) and e(t) are uncorrelated, unit-variance white noise vector
%    processes. The length of x(t), y(t), u(t), and e(t) is m, n, k, and h,
%    respectively.
%
%    Additionally, forecast uncertainties are also generated.
%
%  Input Arguments:
%
%    Mdl - A state-space model, as created by the SSM constructor or
%      SSM/ESTIMATE method.
%
%    numPeriods - Positive, scalar, integer specifying the forecast horizon.
%
%    Y0 - Observed response data to be forecasted. For time-invariant models
%      in which the length of each observation vector (n) is the same, Y0 is
%      a T-by-n matrix. For time-varying models in which the length of the
%      observation vector changes, Y0 is a T-by-1 cell array in which each
%      element contains a time-varying n-element vector of observations, y(t),
%      associated with the corresponding period. The last observation is the
%      most recent.
%
%  Optional Input Name/Value Pairs:
%
%    'A' Cell vector of forecasted state transition matrices in which each
%        element is a matrix corresponding to a period in the forecast horizon
%        at time t = 1,2,...,numPeriods. If the length of the state vector
%        x(t) is constant, then each element is a square m-by-m matrix;
%        however, if the length of x(t) changes, then some elements are
%        non-square matrices. The length of the cell array must be at least
%        numPeriods, and any elements beyond the forecast horizon are ignored.
%        By default, the last coefficient of the input SSM model Mdl is used
%        for all future periods.
%
%    'B' Cell vector of forecasted state disturbance loading matrices in which
%        each element is a matrix corresponding to a period in the forecast
%        horizon at time t = 1,2,...,numPeriods. If the lengths of the state
%        vector x(t) and disturbance vector u(t) are constant, then each
%        element is an m-by-k matrix; however, if the length of x(t) or u(t)
%        changes, then the elements are matrices of various sizes. The length
%        of the cell array must be at least numPeriods, and any elements
%        beyond the forecast horizon are ignored. By default, the last
%        coefficient of the input SSM model Mdl is used for all future periods.
%
%    'C' Cell vector of forecasted measurement sensitivity matrices in which
%        each element is a matrix corresponding to a period in the forecast
%        horizon at time t = 1,2,...,numPeriods. If the lengths of the
%        observation vector y(t) and state vector x(t) are constant, then each
%        element is an n-by-m matrix; however, if the length of y(t) or x(t)
%        changes, then the elements are matrices of various sizes. The length
%        of the cell array must be at least numPeriods, and any elements
%        beyond the forecast horizon are ignored. By default, the last
%        coefficient of the input SSM model Mdl is used for all future periods.
%
%    'D' Cell vector of forecasted observation innovation matrices in which
%        each element is a matrix corresponding to a period in the forecast
%        horizon at time t = 1,2,...,numPeriods. If the lengths of the
%        observation vector y(t) and innovation vector e(t) are constant, then
%        each element is an n-by-h matrix; however, if the length of y(t) or
%        e(t) changes, then the elements are matrices of various sizes. The
%        length of the cell array must be at least numPeriods, and any
%        elements beyond the forecast horizon are ignored. By default, the
%        last coefficient of the input SSM model Mdl is used for all future
%        periods.
%
%    'Predictors0' T-by-d matrix of common predictor variables used to
%                  include a regression component in the observation equation.
%                  Observations at time t are deflated such that
%
%                  [y(t) - z(t)*b] = C * x(t) + D * e(t)
%
%                  where z(t) is a vector of predictor variables and b is
%                  the regression coefficient vector (see below). The default
%                  is an empty matrix (no regression component)
%
%    'PredictorsF' numPeriods-by-d matrix of common predictor variables used to
%                  include a regression component in the observation equation.
%
%    'Beta'        d-by-n matrix of regression coefficients associated with
%                  predictors (see above).
%
%  Output Arguments:
%
%    Y - Point forecasts of observations, E[y(t)|y(t-1),...,y(1)], for
%      t = 1,2,...,numPeriods. For time-invariant models in which the length
%      of each observation vector (n) is the same, this is a numPeriods-by-n
%      matrix. For time-varying models in which the length of the observation
%      vector changes, this is a numPeriods-by-1 cell array in which each
%      element contains a time-varying n-element vector of forecasts associated
%      with the corresponding period.
%
%    YMSE - Forecast error variances of future observations,
%      Cov[y(t)|y(t-1),...,y(1)], for t = 1,2,...,numPeriods. For
%      time-invariant models in which the length of each observation vector
%      (n) is the same, this is a numPeriods-by-n matrix. For time-varying
%      models in which the length of the observation vector changes, this is
%      a numPeriods-by-1 cell array in which each element contains a
%      time-varying n-element vector of forecast error variances associated
%      with the corresponding period.
%
%    X - Point forecasts of states, E[x(t)|y(t-1),...,y(1)], for t = 1,2,...,
%      numPeriods. For time-invariant models in which the length of each state
%      vector (m) is the same, this is a numPeriods-by-m matrix. For
%      time-varying models in which the length of the state vector changes,
%      this is a numPeriods-by-1 cell array in which each element contains a
%      time-varying m-element vector of forecasts associated with the
%      corresponding period.
%
%    XMSE - Forecast error variances of future states,
%      Cov[x(t)|y(t-1),...,y(1)], for t = 1,2,...,numPeriods. For
%      time-invariant models in which the length of each state vector (m) is
%      the same, this is a numPeriods-by-m matrix. For time-varying models
%      in which the length of the state vector changes, this is a
%      numPeriods-by-1 cell array in which each element contains a time-varying
%      m-element vector of forecast error variances associated with the
%      corresponding period.
%
%  See also SSM, FILTER, SMOOTH, SIMULATE, ESTIMATE.
%
%    Other functions named forecast
%
%       abstvar/forecast         garch/forecast
%       arima/forecast           generic/forecast
%       conjugateblm/forecast    gjr/forecast
%       customblm/forecast       regARIMA/forecast
%       diffuseblm/forecast      semiconjugateblm/forecast
%       egarch/forecast          varm/forecast
%       empiricalblm/forecast    vecm/forecast
%


% Copyright 2013 Oliver Johnson, Srikanth Patala
%
% This file is part of MisorientationMaps.
%
%     MisorientationMaps 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.
%
%     MisorientationMaps 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 MisorientationMaps.  If not, see <http://www.gnu.org/licenses/>.

function rgb = colormap432(v)

% this is to adjust to the "correct" fundamental sector
sR = sphericalRegion([vector3d(0,1,-1),vector3d(1,-1,0),vector3d.Z]);
cs = crystalSymmetry('m-3m');
v = project2FundamentalRegion(v,cs,sR.center);

pts = reshape(double(v),[],3);

kmax = sqrt(2)-1;
theta = atan2(pts(:,3),pts(:,2));
t2ind2 = find(pts(:,1) > 1/3 & theta > atan((1-2*pts(:,1))./(pts(:,1))));

tempvar2 = pts(t2ind2,2).*(1-pts(t2ind2,1));
tempvar3 = pts(t2ind2,1).*(pts(t2ind2,2)+pts(t2ind2,3))./(tempvar2 + 1*(tempvar2 == 0));
pts(t2ind2,:) = [pts(t2ind2,1) tempvar3.*pts(t2ind2,2) tempvar3.*pts(t2ind2,3)];
g1 = rotvec2mat([1 0 0 -3*pi/8]);
pts = pts*g1;
pts = [pts(:,1) - kmax pts(:,2) pts(:,3)];

tempvar = (1 + pts(:,2).*tan(pi/8)./(pts(:,3) + 1*(pts(:,3)==0)));
pts = [pts(:,1) pts(:,2).*tempvar pts(:,3).*tempvar];

%%%%% Def Step 3 %%%%%
pts = [pts(:,1) pts(:,2)*cos(pi/8)/tan(pi/8) (pts(:,3) - pts(:,1)./cos(pi/8))];

%%%%% Def Step 4 %%%%%%
theta = atan2(-pts(:,1),pts(:,2));
pts = [pts(:,1).*(sin(theta) + abs(cos(theta))) ...
    pts(:,2).*(sin(theta) + abs(cos(theta))) pts(:,3)];

%%%% Def Step 5 %%%%%
theta = atan2(-pts(:,1),pts(:,2));
rad = hypot(pts(:,2),pts(:,1));
pts = [-rad.*sin(2*theta) rad.*cos(2*theta) pts(:,3)];
kmax = (sqrt(2)-1);tempk = cos(pi/8)/tan(pi/8);
pts(:,1) = pts(:,1)/kmax;pts(:,2) = pts(:,2)/kmax;pts(:,3) = pts(:,3)*tempk;
g1 = rotvec2mat([0 0 1 -pi/6]);
pts = pts*g1;

% S= hsvconereg(pts);
initS = hsvconereg(pts);
g1 = rotvec2mat([0 1 0 pi]);
initS = initS*g1;
initS = [initS(:,1) + 1 initS(:,2) initS(:,3)+1];
rgb = [initS(:,2) initS(:,3) initS(:,1)];
rgb(rgb > 1 & rgb - 1 <= eps) = 1;
rgb(rgb < 0 & rgb >= -10*eps) = 0;

end

% testing code
%v = 0.2*equispacedS2Grid('points',1000);

%rgb = colormap432(v);
%plot(v,rgb)