%% STUDY_02_wrist_scaffold_single_fiber_iFEA_viscoelastic
% Below is a demonstration for:
%
% * Inverse FEA based material parameter optimisation
% * 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;
markerSize=25;
lineWidth=3;

%% Control parameters
% Path names
defaultFolder = fileparts(fileparts(mfilename('fullpath')));
savePath=fullfile(defaultFolder,'2021_Nataliya_Perevoshchikova_wrist_project','data','temp');
loadPathMatProp=fullfile(defaultFolder,'2021_Nataliya_Perevoshchikova_wrist_project','data','mat_prop','PCL');

%%
% Building a quadrilateral circular mesh
pointSpacingHeight=0.1;
d=0.35;
h=10;
xSpacing=0.5;
ySpacing=0.4;
nCopies_x=8;
nCopies_y=8;

r=d/2;
ne=2; %Elements in radius
f=0.6; %Fraction (with respect to outer radius) where central square appears
NumStrands=4*7;
%Create the mesh
[Fq,Vq]=discQuadMesh(ne,r,f);

%%
% Lofting to a fiber
Vq(:,3)=0;
Vq1=Vq;
Vq1(:,3)=Vq1(:,3)-h/2;
Vq2=Vq;
Vq2(:,3)=Vq2(:,3)+h/2;

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

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

%Compose hexahedral elements
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));
E=[FQ(1:end-size(Fq,1),:) FQ(size(Fq,1)+1:end,:)]; %The hexahedral elements
F_start=FQ(1:size(Fq,1),:);
F_end=FQ(end-size(Fq,1)+1:end,:);

[E,V,ind1,ind2]=mergeVertices(E,V); %Merge nodes (start and end are not shared yet)

[F]=element2patch(E); %Element faces
F_start=ind2(F_start); %Start faces for BC's
F_end=ind2(F_end); %End faces for BC's
ind=tesBoundary(F,V); %Indices of boundary faces
Fb=F(ind,:); %Boundary faces (for plotting)

%%
% Visualizing mesh

cFigure; hold on;
title('Hexahedral mesh one fiber','fontSize',fontSize);
gpatch(Fb,V,'gw','k',0.5);
gpatch(F_start,V,'rw','k',1);
gpatch(F_end,V,'bw','k',1);
axisGeom;
camlight headlight;
set(gca,'FontSize',fontSize);
drawnow;

%%
% 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

C1=57.21894783089972;
C2=57.23071414361289;
K=11444.966197451262;
M1=2.00012552045284;
M2=-1.9997730268725347;
%Initial Viscoelastic parameters
g1_ini=1.25;%Viscoelastic QLV proportional coefficient
t1_ini=25;%Viscoelastic QLV time coefficient
P=[t1_ini g1_ini C1 M1 C2 M2];
displacementMagnitude=0.6030;

% Experimental viscoelastic properties
matname = fullfile(loadPathMatProp,'Viscoelastic_properties.mat');
load(matname);

cFigure;
title('Viscoelastic region','fontSize',fontSize);
hold on;
xlabel('Time, s','FontSize',fontSize);
ylabel('Load rate','FontSize',fontSize);
plot(V_time,lc,'k.-','LineWidth',3);
set(gca,'FontSize',fontSize);
camlight headlight;

[M,I]=max(lc);
time_load=V_time(1:I);
time_unload=V_time(I+1:end);
lc_load=lc(1:I);
lc_unload=lc(I+1:end);

% FEA control settings
max_refs=50; %Max reforms
max_ups=0; %Set to zero to use full-Newton iterations
opt_iter=6; %Optimum number of iterations
max_retries=10; %Maximum number of retires
k_factor=100;
NumStrands=4*7;

t_load=max(time_load); %Time from start to max load
t_step_ini1=t_load/50; %Initial desired step size
numTimeSteps1=round(t_load/t_step_ini1); %Number of time steps desired
t_step1=t_load/numTimeSteps1; %Step size
dtmin1=t_step1/100; %Smallest allowed step size
dtmax1=t_step1; %Largest allowed step size

t_unload=V_time(end)-max(time_load);
t_step_ini2=t_step_ini1; %Initial desired step size
numTimeSteps2=round(t_unload/t_step_ini2); %Number of time steps desired
t_step2=t_unload/numTimeSteps2; %Step size
dtmin2=t_step2/100; %Smallest allowed step size
dtmax2=t_step2; %Largest allowed step size

cFigure;
title('Viscoelastic region','fontSize',fontSize);
hold on;
xlabel('Time, s','FontSize',fontSize);
ylabel('Load rate','FontSize',fontSize);
plot(time_load,lc_load,'r.-','LineWidth',3);
plot(time_unload,lc_unload,'k.-','LineWidth',3);
set(gca,'FontSize',fontSize);
camlight headlight;

%% DEFINE BC's
%Supported nodes
bcRigidList=unique(F_start(:));

%Prescribed force nodes
bcPrescribeList=unique(F_end(:));
bcPrescribeMagnitudes=displacementMagnitude(ones(1,numel(bcPrescribeList)),:);

%Visualize BC's
cFigure; hold on;
title('Boundary conditions','FontSize',fontSize);
gpatch(F,V,'y','k',1);

hl(1)=plotV(V(bcRigidList,:),'k.','MarkerSize',markerSize);
hl(2)=plotV(V(bcPrescribeList,:),'r.','MarkerSize',markerSize);

legend(hl,{'BC full support','BC prescribed Z displacement'})
axisGeom;
camlight headlight;
set(gca,'FontSize',fontSize);
drawnow;

%% 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';

analysisType='STATIC';
%Control sections for each step
febio_spec.Step.step{1}.Control=febio_spec.Control; %Copy from template
febio_spec.Step.step{1}.ATTR.id=1;
febio_spec.Step.step{1}.Control.analysis=analysisType;
febio_spec.Step.step{1}.Control.time_steps=numTimeSteps1;
febio_spec.Step.step{1}.Control.step_size=t_step1;
febio_spec.Step.step{1}.Control.solver.max_refs=max_refs;
febio_spec.Step.step{1}.Control.solver.max_ups=max_ups;
febio_spec.Step.step{1}.Control.time_stepper.dtmin=dtmin1;
febio_spec.Step.step{1}.Control.time_stepper.dtmax=dtmax1;
febio_spec.Step.step{1}.Control.time_stepper.max_retries=max_retries;
febio_spec.Step.step{1}.Control.time_stepper.opt_iter=opt_iter;

febio_spec.Step.step{2}.Control=febio_spec.Control; %Copy from template
febio_spec.Step.step{2}.ATTR.id=2;
febio_spec.Step.step{2}.Control.analysis=analysisType;
febio_spec.Step.step{2}.Control.time_steps=numTimeSteps2;
febio_spec.Step.step{2}.Control.step_size=t_step2;
febio_spec.Step.step{2}.Control.solver.max_refs=max_refs;
febio_spec.Step.step{2}.Control.solver.max_ups=max_ups;
febio_spec.Step.step{2}.Control.time_stepper.dtmin=dtmin2;
febio_spec.Step.step{2}.Control.time_stepper.dtmax=dtmax2;
febio_spec.Step.step{2}.Control.time_stepper.max_retries=max_retries;
febio_spec.Step.step{2}.Control.time_stepper.opt_iter=opt_iter;

%Remove control field (part of template) since step specific control sections are used
febio_spec=rmfield(febio_spec,'Control');

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

%Viscoelastic part
febio_spec.Material.material{1}.ATTR.type='uncoupled viscoelastic';
febio_spec.Material.material{1}.ATTR.id=1;
febio_spec.Material.material{1}.g1=g1_ini;
febio_spec.Material.material{1}.t1=t1_ini;

%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;

%Geometry section
% -> Nodes
febio_spec.Mesh.Nodes{1}.ATTR.name='Object1'; %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='Part1';
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;
%

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

% % -> NodeSets
% -> NodeSets
nodeSetName1='bcSupportList';
nodeSetName2='bcPrescribeList';

febio_spec.Mesh.NodeSet{1}.ATTR.name=nodeSetName1;
febio_spec.Mesh.NodeSet{1}.node.ATTR.id=bcRigidList(:);
%
febio_spec.Mesh.NodeSet{2}.ATTR.name=nodeSetName2;
febio_spec.Mesh.NodeSet{2}.node.ATTR.id=bcPrescribeList(:);
%
% %Boundary condition section
% % -> Fix boundary conditions
febio_spec.Boundary.bc{1}.ATTR.type='fix';
febio_spec.Boundary.bc{1}.ATTR.node_set=nodeSetName1;
febio_spec.Boundary.bc{1}.dofs='x';

febio_spec.Boundary.bc{2}.ATTR.type='fix';
febio_spec.Boundary.bc{2}.ATTR.node_set=nodeSetName1;
febio_spec.Boundary.bc{2}.dofs='y';

febio_spec.Boundary.bc{3}.ATTR.type='fix';
febio_spec.Boundary.bc{3}.ATTR.node_set=nodeSetName1;
febio_spec.Boundary.bc{3}.dofs='z';

febio_spec.Boundary.bc{4}.ATTR.type='fix';
febio_spec.Boundary.bc{4}.ATTR.node_set=nodeSetName2;
febio_spec.Boundary.bc{4}.dofs='x';

febio_spec.Boundary.bc{5}.ATTR.type='fix';
febio_spec.Boundary.bc{5}.ATTR.node_set=nodeSetName2;
febio_spec.Boundary.bc{5}.dofs='y';

febio_spec.Boundary.bc{6}.ATTR.type='prescribe';
febio_spec.Boundary.bc{6}.ATTR.node_set=nodeSetName2;
febio_spec.Boundary.bc{6}.dof='z';
febio_spec.Boundary.bc{6}.scale.ATTR.lc=1;
febio_spec.Boundary.bc{6}.scale.VAL=displacementMagnitude;
febio_spec.Boundary.bc{6}.relative=0;

%LoadData section
% -> load_controller
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=[0 0;time_load lc_load;time_unload lc_unload];

%Output section
% -> log file
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';
febio_spec.Output.logfile.element_data{1}.ATTR.delim=',';
febio_spec.Output.logfile.element_data{1}.VAL=1:size(E,1);

%% Quick viewing of the FEBio input file structure
% The |febView| function can be used to view the xml structure in a MATLAB
% figure window.

%%
% 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';%'external';

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

%% Import FEBio results

if runFlag==1 %i.e. a succesful run

    % Importing nodal displacements from a log file
    [~, N_disp_mat,~]=importFEBio_logfile(fullfile(savePath,febioLogFileName_disp));

    N_disp_mat=N_disp_mat(:,2:end,:);
    sizImport=size(N_disp_mat);
    sizImport(3)=sizImport(3)+1;
    N_disp_mat_n=zeros(sizImport);
    N_disp_mat_n(:,:,2:end)=N_disp_mat;
    N_disp_mat=N_disp_mat_n;
    DN=N_disp_mat(:,:,end);
    DN_magnitude=sqrt(sum(DN(:,1).^2,2)); %X
    V_def=V+DN;
    [CF]=vertexToFaceMeasure(F,DN_magnitude);

    %Importaing force from log file
    [time_mat, Force_mat,~]=importFEBio_logfile(fullfile(savePath,febioLogFileName_force));
    time_mat=[0; time_mat(:)]; %Time
    Force_mat=Force_mat(:,2:end,:);
    sizImport=size(Force_mat);
    sizImport(3)=sizImport(3)+1;
    Force_mat_n=zeros(sizImport);
    Force_mat_n(:,:,2:end)=Force_mat;
    Force_mat=Force_mat_n;
    time_sim=time_mat;
    %%
    % Plotting the simulated results using |anim8| to visualize and animate
    % deformations

    % Create basic view and store graphics handle to initiate animation
    hf=cFigure; %Open figure
    gtitle([febioFebFileNamePart,': Press play to animate']);
    hp=gpatch(F,V_def,CF,'k',1); %Add graphics object to animate
    gpatch(F,V,0.5*ones(1,3),'none',0.25); %A static graphics object

    axisGeom(gca,fontSize);
    colormap(gjet(250)); colorbar;
    caxis([0 max(DN_magnitude)]);
    axis([min(V_def(:,1)) max(V_def(:,1)) min(V_def(:,2)) max(V_def(:,2)) min(V_def(:,3)) max(V_def(:,3))]); %Set axis limits statically
    view(130,25); %Set view direction
    camlight headlight;

    % Set up animation features
    animStruct.Time=time_mat; %The time vector
    for qt=1:1:size(N_disp_mat,3) %Loop over time increments
        DN=N_disp_mat(:,:,qt); %Current displacement
        DN_magnitude=sqrt(sum(DN.^2,2)); %Current displacement magnitude
        V_def=V+DN; %Current nodal coordinates
        [CF]=vertexToFaceMeasure(F,DN_magnitude); %Current color data to use

        %Set entries in animation structure
        animStruct.Handles{qt}=[hp hp]; %Handles of objects to animate
        animStruct.Props{qt}={'Vertices','CData'}; %Properties of objects to animate
        animStruct.Set{qt}={V_def,CF}; %Property values for to set in order to animate
    end
    anim8(hf,animStruct); %Initiate animation feature
    drawnow;

    %%
    % Calculate the simulated applied uniaxial stretch
    DZ_set=N_disp_mat(bcPrescribeList,3,:); %Z displacements of the prescribed set
    DZ_set=mean(DZ_set,1); %Calculate mean Z displacements across nodes
    DZ_set=squeeze(DZ_set);

    %Calculated the simulated force
    FZ_set=Force_mat(bcPrescribeList,3,:);
    FZ_set=sum(FZ_set,1); %Calculate mean X force across nodes
    FZ_set=squeeze(FZ_set);

    disp_sim=DZ_set;
    load_sim=FZ_set*NumStrands;

        %%
    % Visualize curves

    hf=cFigure;
    hold on;
    title('Cyclic load-displacement curve','FontSize',fontSize);
    xlabel('Time','FontSize',fontSize); ylabel('Load','FontSize',fontSize);

    hl(1)=plot(time_mat,load_sim,'r.-','lineWidth',lineWidth*1.1,'markerSize',markerSize*1.3);
    hl(2)=plot(V_time,V_load,'k-','lineWidth',lineWidth);

    legend(hl,{'Simulation','Experiment'},'Location','northwest');

    view(2); axis tight;  grid on; axis square; box on;
    set(gca,'FontSize',fontSize);
    drawnow;

end

%% Create structures for optimization
% Material structure
mat_struct.id=1; %Material id

mat_struct.par_names={'t1','g1','c1','m1','c2','m2','k'}; %Parameter names
mat_struct.par_values={t1_ini g1_ini C1 M1 C2 M2 K}; %Parameter values

objectiveStruct.parNormFactors=abs(P); %This will normalize the parameters to ones(size(P))
objectiveStruct.Pb_struct.xx_c=P; %Parameter constraining centre
objectiveStruct.Pb_struct.xxlim=[[P(1)/100 0.01 P(3)/100 -30 P(5)/100 -30]' [P(1)*1e3 50 P(3)*100 30 P(5)*100 30]']; %Parameter bounds

febioAnalysis.time_on=0;
febioAnalysis.time_log_on=0;

%What should be known to the objective function:
objectiveStruct.h=hl(1);
objectiveStruct.bcPrescribeList=bcPrescribeList;
objectiveStruct.time_exp=V_time;
objectiveStruct.load_exp=V_load;
objectiveStruct.febioAnalysis=febioAnalysis;
objectiveStruct.febio_spec=febio_spec;
objectiveStruct.febioFebFileName=febioFebFileName;
objectiveStruct.mat_struct=mat_struct;
objectiveStruct.k_factor=k_factor;
objectiveStruct.NumStrands=NumStrands;

%Optimisation settings
maxNumberIterations=100; %Maximum number of optimization iterations
maxNumberFunctionEvaluations=maxNumberIterations*10; %Maximum number of function evaluations, N.B. multiple evaluations are used per iteration
functionTolerance=1e-25; %Tolerance on objective function value
parameterTolerance=0.001; %Tolerance on parameter variation
displayTypeIterations='iter';

objectiveStruct.method=2;

%File names of output files
output_names.load=fullfile(savePath,febioLogFileName_force);

objectiveStruct.run_output_names=output_names;

%% start optimization

Pn=P./objectiveStruct.parNormFactors;

switch objectiveStruct.method
    case 1 %fminsearch and Nelder-Mead
        OPT_options=optimset('fminsearch'); % 'Nelder-Mead simplex direct search'
        OPT_options = optimset(OPT_options,'MaxFunEvals',maxNumberFunctionEvaluations,...
            'MaxIter',maxNumberIterations,...
            'TolFun',functionTolerance,...
            'TolX',parameterTolerance,...
            'Display',displayTypeIterations,...
            'FinDiffRelStep',1e-2,...
            'DiffMaxChange',0.5);
        [Pn_opt,OPT_out.fval,OPT_out.exitflag,OPT_out.output]= fminsearch(@(Pn) objectiveFunctionIFEA(Pn,objectiveStruct,savePath),Pn,OPT_options);
    case 2 %lsqnonlin and Levenberg-Marquardt
        OPT_options = optimoptions(@lsqnonlin,'Algorithm','levenberg-marquardt');
        OPT_options = optimoptions(OPT_options,'MaxFunEvals',maxNumberFunctionEvaluations,...
            'MaxIter',maxNumberIterations,...
            'TolFun',functionTolerance,...
            'TolX',parameterTolerance,...
            'Display',displayTypeIterations,...
            'FinDiffRelStep',1e-2,...
            'DiffMaxChange',0.5);
        [Pn_opt,OPT_out.resnorm,OPT_out.residual]= lsqnonlin(@(Pn) objectiveFunctionIFEA(Pn,objectiveStruct,savePath),Pn,[],[],OPT_options);
end

%%
[Fopt,OPT_stats_out]=objectiveFunctionIFEA(Pn_opt,objectiveStruct,savePath);

%% Unnormalize and constrain parameters

P_opt=Pn_opt.*objectiveStruct.parNormFactors; %Scale back, undo normalization

%Constraining parameters
for q=1:1:numel(P)
    [P(q)]=boxconstrain(P(q),objectiveStruct.Pb_struct.xxlim(q,1),objectiveStruct.Pb_struct.xxlim(q,2),objectiveStruct.Pb_struct.xx_c(q));
end

disp_text=sprintf('%6.16e,',P_opt); disp_text=disp_text(1:end-1);
disp(['P_opt=',disp_text]);

%%
hf1=cFigure;
%title('displacement load curves optimised','FontSize',fontSize);
xlabel('Time (s)','FontSize',fontSize); ylabel('Force (N)','FontSize',fontSize); zlabel('Z','FontSize',fontSize); hold on;

Hn(1)=plot(OPT_stats_out.time_sim,OPT_stats_out.load_sim,'r.-','lineWidth',lineWidth*1.1,'markerSize',markerSize*1.3);
%Hn(1)=plot(OPT_stats_out.time_sim,OPT_stats_out.load_sim,'r-','lineWidth',lineWidth,'markerSize',markerSize);
%Hn(2)=plot(V_time,V_load,'g-','lineWidth',lineWidth);
Hn(2)=plot(V_time,V_load,'k-','lineWidth',lineWidth);

legend(Hn,{'Simulation','Experiment'},'Location','northwest');
view(2); axis tight;  grid on;
set(gca,'FontSize',fontSize);
drawnow;

function [Fopt,OPT_stats_out]=objectiveFunctionIFEA(Pn,objectiveStruct,savePath)

%%

febioFebFileName=objectiveStruct.febioFebFileName;
febio_spec=objectiveStruct.febio_spec;

%% Unnormalize and constrain parameters

P=Pn.*objectiveStruct.parNormFactors; %Scale back, undo normalization
P_in=P; %Proposed P

%Constraining parameters
for q=1:1:numel(P)
    [P(q)]=boxconstrain(P(q),objectiveStruct.Pb_struct.xxlim(q,1),objectiveStruct.Pb_struct.xxlim(q,2),objectiveStruct.Pb_struct.xx_c(q));
end

%% Setting material parameters

%Acces material parameters

mat_struct=objectiveStruct.mat_struct;
mat_struct.par_values={P(1) P(2) P(3) P(4) P(5) P(6) (P(3)+P(5))*objectiveStruct.k_factor};

disp('SETTING MATERIAL PARAMETERS...');
disp(['Proposed (norm.): ',sprintf(repmat('%6.6e ',[1,numel(Pn)]),Pn)]);
disp(['Proposed        : ',sprintf(repmat('%6.6e ',[1,numel(P_in)]),P_in)]);
disp(['Set (constr.)   : ',sprintf(repmat('%6.6e ',[1,numel(P)]),P)]);

%Assign material parameters
matId=mat_struct.id;
for q=1:1:2
    parNameNow=mat_struct.par_names{q};
    parValuesNow=mat_struct.par_values{q};
    febio_spec.Material.material{matId}.(parNameNow)=mat2strIntDouble(parValuesNow);
end

for q=3:1:numel(mat_struct.par_names)
    parNameNow=mat_struct.par_names{q};
    parValuesNow=mat_struct.par_values{q};
    febio_spec.Material.material{matId}.elastic{1}.(parNameNow)=mat2strIntDouble(parValuesNow);
end

%febView(febio_spec); %Viewing the febio file|

febioStruct2xml(febio_spec,febioFebFileName); %Exporting to file and domNode
disp('Done')

%% START FEBio

[runFlag]=runMonitorFEBio(objectiveStruct.febioAnalysis);

pause(0.1);

bcPrescribeList=objectiveStruct.bcPrescribeList;
time_exp=objectiveStruct.time_exp;
load_exp=objectiveStruct.load_exp;

if runFlag==1

    %Importing force from log file
    [time_mat, Force_mat,~]=importFEBio_logfile(objectiveStruct.run_output_names.load);
    time_mat=[0; time_mat(:)]; %Time
    Force_mat=Force_mat(:,2:end,:); %Remove index
    sizImport=size(Force_mat);
    sizImport(3)=sizImport(3)+1;
    Force_mat_n=zeros(sizImport);
    Force_mat_n(:,:,2:end)=Force_mat;
    Force_mat=Force_mat_n; %With added zeros for t=0

    %Calculated the simulated force
    FZ_set=Force_mat(bcPrescribeList,3,:);
    FZ_set=sum(FZ_set,1); %Calculate mean X force across nodes
    FZ_set=squeeze(FZ_set);

    time_sim=time_mat;
    load_sim=FZ_set*objectiveStruct.NumStrands; %Multiply force by number of strands

    if ~isempty(objectiveStruct.h)
        objectiveStruct.h.XData=time_sim;
        objectiveStruct.h.YData=load_sim;
        drawnow;
    end

    load_sim_exp = interp1(time_sim,load_sim,time_exp,'pchip');

    %Derive Fopt
    loadDev=load_exp-load_sim_exp;

    switch objectiveStruct.method
        case 1
            Fopt=sum((loadDev).^2)/length(loadDev); %Sum of squared differences
        case 2
            Fopt=sum((loadDev).^2)/length(loadDev);%loadDev(:); %Squared differences
    end

    OPT_stats_out.load_sim=load_sim;
    OPT_stats_out.time_sim=time_sim;
    OPT_stats_out.loadDev=loadDev;
    OPT_stats_out.Fopt=Fopt;

else %Output NaN
    switch objectiveStruct.method
        case 1
            Fopt=NaN;
        case 2
            Fopt=NaN(size(load_exp)); %Squared differences
    end
    OPT_stats_out=[];
end

end


% This file is part of the following project:
% J. Zico Kolter. Tommi Jaakkola.
% Approximate Inference in Additive Factorial HMMs with Application to Energy Disaggregation.
% In: International Conference on Artificial Intelligence and Statistics (AISTATS). 2012.
% Code provided as supplementary material
% Copyright: J. Zico Kolter, MIT CSAIL, 2014.

% Modified by Romano Cicchetti, ETH Zurich, in the context of the NILM-Eval project

function [data] = kolter(days, setup, fid)

    % load parameters
    dataset = setup.dataset;
    interval = setup.granularity;
    household = setup.household;
    lratio = setup.filtLRatio;

    % set variables
    edgeThreshold = 5;
    plevelMinLength = 5;

    % get total power consumption
    evaluation_days = days{1};
    true_power = read_smartmeter_data(dataset, num2str(household, '%02d'), evaluation_days, interval, 'powerallphases');

    % total variation denoising
    lmax = tvdiplmax(true_power);
    [data, ~, ~] = tvdip(true_power, lmax*lratio, 1, 1e-3, 100);
    data = data';

    % generate power levels
    plevel = generatePlevels(edgeThreshold, plevelMinLength, data, true_power);

    % generate snippets from power levels
    snippets = generateSnippetsFromPlevels(plevel);

    % generate HMMs from snippets
    HMMs = generateHMMsFromSnippets(snippets);

    % calculate probability of one snippet generating another
    numOfSnippets = length(snippets.mean);
    probsBetweenAllSnippets = zeros(numOfSnippets, numOfSnippets);
    for i = 1:numOfSnippets
        log_trans = log(cell2mat(HMMs.transition(i)));
        mu = cell2mat(HMMs.mean(i));
        sigma = cell2mat(HMMs.std(i));
        numOfStates = length(cell2mat(snippets.mean(i))) + 1;
        for j = 1:numOfSnippets
            if numOfStates ~= length(cell2mat(snippets.mean(j))) + 1
                continue;
            end
            firstPlevelOfSnippet = snippets.start(j);
            lastPlevelOfSnippet = snippets.end(j);
            observation = data(plevel.startidx(firstPlevelOfSnippet):plevel.endidx(lastPlevelOfSnippet)) - ...
                plevel.mean(firstPlevelOfSnippet - 1);
            emit = zeros(numOfStates, length(observation));
            for state = 1:numOfStates
                emit(state, :) = normpdf(observation, mu(state), sigma(state));
            end
            log_emit = log(emit);

            maxProb = zeros(numOfStates, length(observation));
            maxProb(:,1) = log(0);
            maxProb(1,1) = log(1);
            for t = 2:length(observation)
                for k = 1:numOfStates
                    sum = maxProb(:,t-1) + log_trans(:,k) + log_emit(k,t);
                    maxProb(k,t) = max(sum);
                end
                if all(isinf(maxProb(:,t)))
                    break;
                end
            end
            probsBetweenAllSnippets(i,j) = max(maxProb(:,length(observation)));
        end
    end

    % build adjacency Matrix from k-nearest neighbor graph
    numOfStatesOfSnippets = cellfun(@length, HMMs.mean);
    mu = {};
    P = {};
    for numOfStates = 2:5
        idxOfHMMs = find(numOfStatesOfSnippets == numOfStates);
        meansOfHMMs = HMMs.mean(idxOfHMMs);
        transitionOfHMMs = HMMs.transition(idxOfHMMs);
        probsBetweenSelectedSnippets = probsBetweenAllSnippets(idxOfHMMs, idxOfHMMs);
        adjacencyMatrix = zeros(size(probsBetweenSelectedSnippets));
        for i = 1:length(idxOfHMMs)
            idx_nonzero = find(probsBetweenSelectedSnippets(:,i) ~= 0);
            numSnippets = length(idx_nonzero);
            if numSnippets == 0
                continue;
            end
             [~,sortedIdx] = sort(probsBetweenSelectedSnippets(idx_nonzero,i), 'descend');
            sortedIdx = sortedIdx(1:1+floor(numSnippets/4));
            adjacencyMatrix(idx_nonzero(sortedIdx),i) = 1;
        end

        numOfClusters = floor(sqrt(length(idxOfHMMs)/2));
        if numOfClusters == 0
            continue;
        end
        clusters = SpectralClustering(adjacencyMatrix, numOfClusters, 1);

        for cl = 1:numOfClusters
            idx = clusters(:,cl) == 1;
            selectedMeans = cell2mat(meansOfHMMs(idx)');
            mu{end+1} = mean(selectedMeans,2)';
            selectedTrans = transitionOfHMMs(idx);
            Trans = cat(3,selectedTrans{:});
            P{end+1} = mean(Trans,3);
        end
    end

    % write cluster centorids (HMMs) to text file
    fprintf(fid,'%20s\n', 'MEANS:');
    for i = 1:length(mu)
        fprintf(fid, '%f\t', cell2mat(mu(i)));
        fprintf(fid, '\n');
    end

    % run AFAMAP algorithm
    n = 1;
    paramsAfamap = struct;
    paramsAfamap.max_iter = 1;
    paramsAfamap.lambda = Inf;
    paramsAfamap.dlambda = Inf;
    paramsAfamap.dSig = var(diff(data));
    paramsAfamap.Sig = var(data);
    [X0,Z,G] = myAfamap(plevel.mean', mu, plevel.duration', P, paramsAfamap);

end


%+========================================================================+
%|                                                                        |
%|            This script uses the GYPSILAB toolbox for Matlab            |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal, Marc Bakry (c) 2017-2019.                 |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%|             marc.bakry@polytechnique.edu                               |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : nrtHmxVibroSlab.m                             |
%|    #    |   VERSION    : 0.55                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal & Marc Bakry                  |
%|  ( # )  |   CREATION   : 14.03.2019                                    |
%|  / 0 \  |   LAST MODIF :                                               |
%| ( === ) |   SYNOPSIS   :                                               |
%|  `---'  |                                                              |
%+========================================================================+

% Cleaning
clear all
close all
clc

% Gypsilab path
run('../../addpathGypsilab.m')

% Parameters
tol = 1e-3
X0  = [0 0 1]
e   = 0.5;
f   = 900;

% Exterior domain (water)
rho0 = 1000;         % density      (kg.m3)
c0   = 1500;         % celerity     (m.s-1)
k0   = 2*pi/c0*f;    % wave-number  (m-1)
lam0 = c0/f;         % wave-length  (m)

% Interior domain (different from water)
rhoS = 2*rho0;                                 % density                             (kg.m3)
cL   = 2*c0;                                   % celerity of longitudinal waves      (m.s-1)
kL   = 2*pi*f/cL;                              % wave-number of longitudinal waves   (m-1)
lamL = real(cL)/f;                             % wavelength of longitudinal waves    (m)
cT   = 0;                                      % celerity of transverse waves        (m.s-1)
kT   = 2*pi.*f/cT;                             % wave-number of transverse waves     (m-1)
lamT = real(cT)/f;                             % wavelength of transverse waves

% Minimum wavelength
tmp  = [lam0,lamL,lamT];
lmin = min(tmp(tmp>0));

% Slab mesh
L    = 20 * lmin;            % 20 wavelength to simulate infinite slab
nx   = ceil(L/lmin * 6)+1;   % 6 node per wavelength for L
ny   = ceil(L/lmin * 6)+1;   % 6 node per wavelength for L
nz   = ceil(e/lmin * 12)+1;  % 12 node per wavelength for e
N    = nx * ny * nz;         % Total number of nodes
mesh = mshCube(N,[L L e])

% Boundary
bound = mesh.bnd

% Admissible frequency
stp  = mesh.stp;
kmax = 1/stp(2);
if (k0 > kmax) || (kL > kmax/2) || ((kT>kmax/2) && ~isinf(kT))
%     error('frequency is too high for mesh discretization')
end

% Radiative mesh (fixed number of nodes)
radiatx     = mshSquare(1e3,[L L]);
radiaty     = radiatx;
radiatx.vtx = radiatx.vtx(:,[1 3 2]);
radiaty.vtx = radiaty.vtx(:,[3 1 2]);
radiat      = union(radiatx,radiaty);

% Measurement points for trans and refl coeff (1 wavelenth from bound)
Xmes = [0 0 -e/2-lmin ; 0 0 e/2+lmin];

% Cut-off function (50% full, 10% decrease)
cutoffx = vibsCutoff(1,L/5,L/10);
cutoffy = vibsCutoff(2,L/5,L/10);

% Green kernel function
Gxy         = @(X,Y) femGreenKernel(X,Y,'[exp(ikr)/r]',k0);
gradyGxy{1} = @(X,Y) femGreenKernel(X,Y,'grady[exp(ikr)/r]1',k0);
gradyGxy{2} = @(X,Y) femGreenKernel(X,Y,'grady[exp(ikr)/r]2',k0);
gradyGxy{3} = @(X,Y) femGreenKernel(X,Y,'grady[exp(ikr)/r]3',k0);
G0          = '[1/r]';
gradyG0     = 'grady[1/r]';
cteGxy      =  1/(4*pi);
cteG0       =  1/(4*pi);

% Plane wave function
PW         = @(X) exp(1i*k0*X*X0') .* cutoffx(X) .* cutoffy(X);
gradxPW{1} = @(X) 1i*k0*X0(1) .* PW(X);
gradxPW{2} = @(X) 1i*k0*X0(2) .* PW(X);
gradxPW{3} = @(X) 1i*k0*X0(3) .* PW(X);

% Coupling coeff for Brackage-Werner simulation
beta = 1i*k0;

% Graphical representation
figure
plot(bound,'r')
hold on
plot(radiat,log(abs(PW(radiat.vtx))))
% plotNrm(bound,'w')
plot(msh(Xmes),'k')
axis equal
colorbar
view(45,10)

% Quadrature and finite elements (volumn)
omega = dom(mesh,4);
U     = fem(mesh,'P1');

% Quadrature and finite elements (boundary)
sigma = dom(bound,3);
u     = fem(bound,'P1');

% Left-hand side
tic
[A,B,C,D] = vibsHmxBlockOperator(omega,U,sigma,u,cL,cT,rhoS,c0,rho0,f,tol);
toc

% Add dirichlet condition to x and y unknows (penalization)
A(sub2ind(size(A),1:2*length(U),1:2*length(U))) = 1e15;

% Right-hand side
V    = cell(4,1);
V{1} = - integral(sigma,ntimes(U,1),PW);
V{2} = - integral(sigma,ntimes(U,2),PW);
V{3} = - integral(sigma,ntimes(U,3),PW);
V{4} = integral(sigma,ntimes(u),gradxPW);

% Resolution with Schur complement
Fa     = decomposition(A);
LHS    = @(V) D*V - C*(Fa \ (B.Ml*(B.Mr*V)) );
RHS    = V{end} - C*(Fa \ cell2mat(V(1:end-1)) );
mu     = mgcr(LHS,RHS,[],tol,100);
lambda = beta*mu;

% Measure of refexive and transmitted coeff
tic
Pmes = cteGxy .* integral(Xmes,sigma,Gxy,u)*lambda - ...
    cteGxy .* integral(Xmes,sigma,gradyGxy,ntimes(u))*mu;
Pmes(2) = Pmes(2) + PW(Xmes(2,:));
toc

% Finite element radiative operator --> \int_Sy G(x,y) psi(y) dy
tic
Srad = cteGxy .* integral(radiat.vtx,sigma,Gxy,u,tol);
Srad = Srad + cteG0 .* regularize(radiat.vtx,sigma,G0,u);

% Finite element radiative operator --> \int_Sy dnyG(x,y) psi(y) dy
Drad = cteGxy .* integral(radiat.vtx,sigma,gradyGxy,ntimes(u),tol);
Drad = Drad + cteG0 .* regularize(radiat.vtx,sigma,gradyG0,ntimes(u));
toc

% Domain solution
Psca = Srad*lambda - Drad*mu;
Pinc = PW(radiat.vtx);
Ptot = Psca + Pinc;

% Graphical repesentation
figure
subplot(1,2,1)
plot(bound)
hold on
plot(radiat,abs(Psca))
plot(msh(Xmes),abs(Pmes))
title('Scattered')
axis equal
colorbar
view(45,10)

subplot(1,2,2)
plot(bound)
hold on
plot(radiat,abs(Ptot))
plot(msh(Xmes),abs(Pmes))
title('Total')
axis equal
colorbar
view(45,10)

% Analytical solution
tic
c     = ones(length(f),1) * [c0 cL c0];
rho   = [rho0 rhoS rho0];
[R,T] = slabVibro(f,rho,c,e);
toc

% Comparison (db)
ref = 20*log10(abs([R ; T]));
sol = 20*log10(abs(Pmes));

norm(ref-sol)/norm(ref)

disp('~~> Michto gypsilab !')


% J. C. Spall, August 1999
% Written in support of text, Introduction to Stochastic Search and Optimization, 2003
% This program runs a GA with the tournament parent selection scheme
% used.  Elitism is included.  Parent selection includes the elitist
% elements.  The standard binary bit
% form is used here.  As usual, code works in terms of fitness
% values (higher better); results, however, are reported for the loss
% values of actual interest.  This code does not work with constraints
% on theta values other than those directly associated with thetamin,
% thetamax.
%
% This code calls the functions 'bit2num','num2bit',and'fitpop'.
%
% The user is expected to set a variable 'expect_fn' representing the
% expected number of function evaluations allowed.  The main "while"
% loop in the code tests for when the expected number of function
% evaluations exceeds 'expect_fn' and terminates the iteration on the
% first generation that would require a no. of function evaluations
% exceeding this value.  The test in the "while" loop uses a formula
% in ISSO, Chap. 10; the "identical" variable in the formula is replaced
% here by an actual running calculation of the identical total.  Note
% that the code actually makes more function evaluations than the
% expected total due to the desire to avoid excessive bookkeeping and
% slowing of the code.  However, the IDEA of terminating when the
% expecting running total exceeds a threshold is consistent with the
% idea of placing a premium on possibly expensive function evaluations.
%
% This code handles noisy loss functions via the fact that the variables
% 'loss' and 'lossfinal' can call different functions.  The global
% variable 'sigma' is used to generate the scaling for noise in the
% loss measurements.
%
clear all
global p N thetamin thetamax
cases=15;         %no. of Monte Carlo cases
N = 40;           % population size
p=10;             % dimension of theta
P_c =.6;          % crossover rate
P_m =0.0001;      % mutation rate
elite=2;                % no. of population elements for "elitism"
                        % (should pick s.t. N-elite = even no.)
expect_fn=4000;         % expected no. of function evaluations
M=4*ones(p,1);          % no. of places to right of decimal
thetamin=-1.6383*ones(p,1);  %lower bounds on theta elements
thetamax=1.6384*ones(p,1);   %upper bounds: ditto
thetamin_0=-1.6383*ones(p,1);   %lower bounds on theta for initialization
thetamax_0=1.6384*ones(p,1);    %upper bounds: ditto
lossfinaleval ='loss4thorder';    % choice of loss function for final perf.
                              % evaluation
loss='loss4thnoise';
lossfinalsq=0;          %variable for cum.(over 'cases')squared loss values
lossfinal=0;            %variable for cum. loss values
rand('seed',31415927);
randn('seed',3111113)
global z sigma;       %declaration of random var. used for normal noise
                      %generation in loss fn. calls given seed above;
                      %also sigma in noise (noise is dependent on theta)
sigma=0;            %multiplier in loss noise (lower bd. to s.d.)
fitness=zeros(N,1);                %dim.-setting statement
theta=zeros(p,1);                  %dim.-setting statement
% User warning if invalid N-elite value
if (round((N-elite)/2)~=(N-elite)/2)
   disp('WARNING: N-elite should be even number');
else
end
%
% Determination of no. bits/theta element
bits=zeros(p,1);
for i=1:p
   b=1;
   while 10^M(i)*(thetamax(i)-thetamin(i))>2^b-1
      b=b+1;
   end
   bits(i)=b;
end
length=ones(1,p)*bits;              %no. of bits/population element
thetabit=zeros(N,length);  %dummy statement setting dimensions of matrix
                           %containing bit values of all chromosomes
thetabit_noelite=zeros(N-elite,length);%dummy statement setting
                                       %dimension of matrix used to temp.
                                       %store offspring
parent1=zeros(1,length); %dummy dimension statement
parent2=zeros(1,length); %ditto
% Begin outer loop of 'cases' Monte Carlo runs
%
navg=0;
for c=1:cases
   c
   %
   % Initial Random Population
   %
   % Statements below allow intialization of the search using a random
   % placement in natural theta space.  Points are placed uniformly dist.
   % on the hypercube defined by thetamin_0, thetamax_0.  The points are
   % converted to a bit representation for use by the GA.
   %
   for i=1:N
      theta=(thetamax_0-thetamin_0).*rand(p,1)+thetamin_0;
      pointer=1;
      for j=1:p
         thetabit(i,pointer:pointer+bits(j)-1)=num2bit(theta(j),bits(j),...
            thetamin(j),thetamax(j));
         pointer=pointer+bits(j);
      end
   end

%thetabit = rand(N,length) > 0.5;  % sets array of 0/1 for
                                  % all N chromosomes
%loop below is included for testing purposes and artifical insertion of
%known solution (to ensure that alg. properly handles this)
%for i=1:bits(1):p*bits(1)
%   thetabit(N,i:i+bits(1)-1)=[0,1,1,1,1,1,1,1,1,1,1,1,1,1,1];
%end
%zer=bit2num(thetabit(N,1:1+bits(1)-1),bits(1),M(1),thetamin(1),thetamax(1))
%upper = zeros(n, 1);              % fills vector for reporting loss
%average = zeros(n, 1);            % ditto
%lower = zeros(n, 1);              % ditto
   fitness=fitpop(thetabit,bits,loss);%N by 1 vector of fitness values
   [bestfitness,bestindex]=max(fitness);  bestindex
   % Loop below produces the theta associated with the best fitness value
   pointer=1;
   for j=1:p
      theta(j)=bit2num(thetabit(bestindex,pointer:pointer+bits(j)-1),...
            bits(j),thetamin(j),thetamax(j));
      pointer=pointer+bits(j);
   end
   %
   % Main loop of GA
   %
   index=zeros(elite,1);
   %cumfit=zeros(N-elite,1);
   identical=0;   %initialization of variable that keeps track of no.
                  %of identical parents in selection pairs (due to
                  %resampling)
   n=1;
   while n< 1+(expect_fn-N+identical*(1-(1-P_m)^length))/(P_c*(N-elite)...
         +(1-P_c)*(N-elite)*(1-(1-P_m)^length))
      % Evaluate obj. function for each individual (an N-dim.vector)
      %fitness = fitpop(thetabit,bits,loss); %higher better
      % Invoke elitism for 'elite' best chromosomes
      temp_fit=fitness;
      for j=1:elite
         [junk,index(j)]=max(temp_fit);
         temp_fit(index(j))=min(temp_fit);
      end
      for j=1:2:N-elite-1
         % Tournament selection below.  Uses tournament size = 2.
         for k=1:2
            cand=ceil(N*rand(2,1));
            if fitness(cand(1))>fitness(cand(2))
               parent1_idx=cand(1);
            else
               parent1_idx=cand(2);
            end
            cand=ceil(N*rand(2,1));
            if fitness(cand(1))>fitness(cand(2))
               parent2_idx=cand(1);
            else
               parent2_idx=cand(2);
            end
         end
      % Crossover with above two parents
         if rand < P_c
            cross_point=ceil(rand*(length-1)); %an integer 1,...,length-1
            parent1=thetabit(parent1_idx,:);
            %above picks off the chrom. for parent1_idx
            parent2=thetabit(parent2_idx,:);
            identical=identical+2*xor(1,norm(parent1-parent2));
            %above picks off the chrom. for parent2_idx
            child1=[parent1(1:cross_point),...
               parent2(cross_point+1:length)];   %merges parents1&2
            child2=[parent2(1:cross_point),...
               parent1(cross_point+1:length)];   %merges parents1&2
            thetabit_noelite(j,:)=child1;%assigns child1 to new chrom. matrix
            thetabit_noelite(j+1,:)=child2;%ditto for child2
         else
            parent1=thetabit(parent1_idx,:);
               %above picks off the chrom. for parent1_idx
            parent2=thetabit(parent2_idx,:);
               %above picks off the chrom. for parent2_idx
            thetabit_noelite(j,:)=parent1;%assigns parent1 to chrom. matrix
            thetabit_noelite(j+1,:)=parent2;%ditto for parent2
         end
      end
      %
      % Mutation operator applied to thetabit_noelite vector (elite
      % chromosomes not subject to mutation)
      %
      mutate=rand(N-elite,length)<P_m;
      thetabit_noelite=xor(thetabit_noelite,mutate); %exclusive-or operator
                                               %invokes mutation
      % Put elite chromosomes back in population
      %
      for j=1:elite
        thetabit(j,:)=thetabit(index(j),:);
      end
      thetabit(elite+1:N,:)=thetabit_noelite;
      fitness=fitpop(thetabit,bits,loss);
      n=n+1;
   end
      [bestfitness,bestindex]=max(fitness);
   pointer=1;
   for j=1:p
      theta(j)=bit2num(thetabit(bestindex,pointer:pointer+bits(j)-1),...
      bits(j),thetamin(j),thetamax(j));
      pointer=pointer+bits(j);
   end
   lossvalue=feval(lossfinaleval,theta);lossvalue
   lossfinalsq=lossfinalsq+lossvalue^2;
   lossfinal=lossfinal+lossvalue;
   navg=navg+n-1;   	%counter for # of iters. taken
end
navg/cases
% standard dev. of loss values
if cases ~= 1
	disp('standard deviation of mean loss value')
   sd=((cases/(cases-1))^.5)*(lossfinalsq/cases-(lossfinal/cases)^2)^.5;
   sd=sd/(cases^.5)
else
end
% normalized loss values
disp('mean loss value over "cases" runs')
mean=lossfinal/cases


function [cp, vs, i1, iN] = params_legacy_cost(om, name, idx)
%PARAMS_LEGACY_COST  Returns cost parameters for legacy user-defined costs.
%   CP = OM.PARAMS_LEGACY_COST()
%   CP = OM.PARAMS_LEGACY_COST(NAME)
%   CP = OM.PARAMS_LEGACY_COST(NAME, IDX_LIST)
%   [CP, VS] = OM.PARAMS_LEGACY_COST(...)
%   [CP, VS, I1, IN] = OM.PARAMS_LEGACY_COST(...)
%
%   With no input parameters, it assembles and returns the parameters
%   for the aggregate legacy user-defined cost from all legacy cost sets
%   added using ADD_LEGACY_COST. The values of these parameters are cached
%   for subsequent calls. The parameters are contained in the struct CP,
%   described below.
%
%   If a NAME is provided then it simply returns parameter struct CP for the
%   corresponding named set. Likewise for indexed named sets specified
%   by NAME and IDX_LIST.
%
%   An optional 2nd output argument VS indicates the variable sets used by
%   this cost set. The size of CP.N will be consistent with VS.
%
%   If NAME is provided, optional 3rd and 4th output arguments I1 and IN
%   indicate the starting and ending row indices of the corresponding
%   cost set in the full aggregate cost matrix CP.N.
%
%   Let X refer to the vector formed by combining the corresponding varsets VS,
%   and F_U(X, CP) be the cost at X corresponding to the cost parameters
%   contained in CP, where CP is a struct with the following fields:
%       N      - nw x nx sparse matrix (optional, identity matrix by default)
%       Cw     - nw x 1 vector
%       H      - nw x nw sparse matrix (optional, all zeros by default)
%       dd, mm - nw x 1 vectors (optional, all ones by default)
%       rh, kk - nw x 1 vectors (optional, all zeros by default)
%
%   These parameters are used as follows to compute F_U(X, CP)
%
%       R  = N*X - rh
%
%               /  kk(i),  R(i) < -kk(i)
%       K(i) = <   0,     -kk(i) <= R(i) <= kk(i)
%               \ -kk(i),  R(i) > kk(i)
%
%       RR = R + K
%
%       U(i) =  /  0, -kk(i) <= R(i) <= kk(i)
%               \  1, otherwise
%
%       DDL(i) = /  1, dd(i) = 1
%                \  0, otherwise
%
%       DDQ(i) = /  1, dd(i) = 2
%                \  0, otherwise
%
%       Dl = diag(mm) * diag(U) * diag(DDL)
%       Dq = diag(mm) * diag(U) * diag(DDQ)
%
%       w = (Dl + Dq * diag(RR)) * RR
%
%       F_U(X, CP) = 1/2 * w'*H*w + Cw'*w
%
%   See also OPT_MODEL, ADD_LEGACY_COST, EVAL_LEGACY_COST.

%   MATPOWER
%   Copyright (c) 2017, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

if nargin > 1       %% individual set
    if nargin < 3
        idx = {};
    end
    if isempty(idx)                 %% name, no index provided
        if numel(om.cost.idx.i1.(name)) == 1    %% simple named set
            N  = om.cost.data.N.( name);
            Cw = om.cost.data.Cw.(name);
            [nw, nx] = size(N);
            e1 = ones(nw, 1);
            e0 = zeros(nw, 1);
            cp = struct(...
                'N',    N, ...
                'Cw',   Cw, ...
                'H',    sparse(nw, nw), ... %% default => no quadratic term
                'dd',   e1, ...             %% default => linear
                'rh',   e0, ...             %% default => no shift
                'kk',   e0, ...             %% default => no dead zone
                'mm',   e1 );               %% default => no scaling
            if isfield(om.cost.data.H, name) && ~isempty(om.cost.data.H.(name))
                cp.H = om.cost.data.H.(name);
            end
            if isfield(om.cost.data.dd, name) && ~isempty(om.cost.data.dd.(name))
                cp.dd = om.cost.data.dd.(name);
            end
            if isfield(om.cost.data.rh, name) && ~isempty(om.cost.data.rh.(name))
                cp.rh = om.cost.data.rh.(name);
            end
            if isfield(om.cost.data.kk, name) && ~isempty(om.cost.data.kk.(name))
                cp.kk = om.cost.data.kk.(name);
            end
            if isfield(om.cost.data.mm, name) && ~isempty(om.cost.data.mm.(name))
                cp.mm = om.cost.data.mm.(name);
            end
            if nargout > 1
                vs = om.cost.data.vs.(name);
                if nargout > 3
                    i1 = om.cost.idx.i1.(name);     %% starting row index
                    iN = om.cost.idx.iN.(name);     %% ending row index
                end
            end
        else                                    %% indexing required
            error('@opt_model/params_legacy_cost: legacy cost set ''%s'' requires an IDX_LIST arg', name);
        end
    else                            %% indexed named set
        % (calls to substruct() are relatively expensive ...
        % s = substruct('.', name, '{}', idx);
        % ... so replace it with these more efficient lines)
        sc = struct('type', {'.', '{}'}, 'subs', {name, idx});
        N  = subsref(om.cost.data.N,  sc);
        Cw = subsref(om.cost.data.Cw, sc);
        [nw, nx] = size(N);
        e1 = ones(nw, 1);
        e0 = zeros(nw, 1);
        cp = struct(...
            'N',    N, ...
            'Cw',   Cw, ...
            'H',    sparse(nw, nw), ... %% default => no quadratic term
            'dd',   e1, ...             %% default => linear
            'rh',   e0, ...             %% default => no shift
            'kk',   e0, ...             %% default => no dead zone
            'mm',   e1 );               %% default => no scaling
        if isfield(om.cost.data.H, name) && ~isempty(subsref(om.cost.data.H, sc))
            cp.H = subsref(om.cost.data.H,  sc);
        end
        if isfield(om.cost.data.dd, name) && ~isempty(subsref(om.cost.data.dd, sc))
            cp.dd = subsref(om.cost.data.dd, sc);
        end
        if isfield(om.cost.data.rh, name) && ~isempty(subsref(om.cost.data.rh, sc))
            cp.rh = subsref(om.cost.data.rh, sc);
        end
        if isfield(om.cost.data.kk, name) && ~isempty(subsref(om.cost.data.kk, sc))
            cp.kk = subsref(om.cost.data.kk, sc);
        end
        if isfield(om.cost.data.mm, name) && ~isempty(subsref(om.cost.data.mm, sc))
            cp.mm = subsref(om.cost.data.mm, sc);
        end
        if nargout > 1
            vs = subsref(om.cost.data.vs, sc);
            if nargout > 3
                sn = sc; sn(2).type = '()';         %% num array field
                i1 = subsref(om.cost.idx.i1, sn);   %% starting row index
                iN = subsref(om.cost.idx.iN, sn);   %% ending row index
            end
        end
    end
else                %% aggregate
    cache = om.cost.params;
    if isempty(cache)       %% build the aggregate
        nx = om.var.N;          %% number of variables
        nw = om.cost.N;         %% number of cost rows
        Nt = sparse(nx, nw);    %% use N transpose for speed
        Cw = zeros(nw, 1);
        H = sparse(nw, nw);     %% default => no quadratic term
        dd = ones(nw, 1);       %% default => linear
        rh = Cw;                %% default => no shift
        kk = Cw;                %% default => no dead zone
        mm = dd;                %% default => no scaling

        %% fill in each piece
        for k = 1:om.cost.NS
            name = om.cost.order(k).name;
            idx  = om.cost.order(k).idx;
            [cp, vs, i1, iN] = om.params_legacy_cost(name, idx);
            [mk, nk] = size(cp.N);      %% size of Nk
            if mk
                Nkt_full = sparse(nx, nw);
                if isempty(vs)
                    if nk == nx     %% full size
                        Nkt_full(:, i1:iN) = cp.N';
                    else            %% vars added since adding this cost set
                        Nk_all_cols = sparse(mk, nx);
                        Nk_all_cols(:, 1:nk) = cp.N;
                        Nkt_full(:, i1:iN) = Nk_all_cols';
                    end
                else
                    jj = om.varsets_idx(vs);    %% indices for var set
                    Nk_all_cols = sparse(mk, nx);
                    Nk_all_cols(:, jj) = cp.N;
                    Nkt_full(:, i1:iN) = Nk_all_cols';
                end
                Nt = Nt + Nkt_full;
                Cw(i1:iN) = cp.Cw;
                H_all_cols = sparse(mk, nw);
                H_all_cols(:, i1:iN) = cp.H';
                H(:, i1:iN) = H_all_cols';
                dd(i1:iN) = cp.dd;
                rh(i1:iN) = cp.rh;
                kk(i1:iN) = cp.kk;
                mm(i1:iN) = cp.mm;
            end
        end

        %% cache aggregated parameters
        om.cost.params = struct( ...
            'N', Nt', 'Cw', Cw, 'H', H, 'dd', dd, 'rh', rh, 'kk', kk, 'mm', mm );
        cp = om.cost.params;
    else                    %% return cached values
        cp = cache;
    end
    if nargout > 1
        vs = {};
    end
end


function [C,phi,S12,S1,S2,f,zerosp,confC,phistd,Cerr]=coherencysegcpb(data1,data2,win,params,segave,fscorr)
% Multi-taper coherency,cross-spectrum and individual spectra with segmenting
%   computed by segmenting two univariate time series into chunks - continuous and binned point process
%
% Usage:
% [C,phi,S12,S1,S2,f,zerosp,confC,phistd,Cerr]=coherencysegcpb(data1,data2,win,params,segave,fscorr)
% Input:
% Note units have to be consistent. See chronux.m for more information.
%       data1 (column vector, continuous data) -- required
%       data2 (column vector, binned point process data) -- required
%       win   (length of segments) - required
%       params: structure with fields tapers, pad, Fs, fpass, err
%       - optional
%           tapers : precalculated tapers from dpss or in the one of the following
%                    forms:
%                   (1) A numeric vector [TW K] where TW is the
%                       time-bandwidth product and K is the number of
%                       tapers to be used (less than or equal to
%                       2TW-1).
%                   (2) A numeric vector [W T p] where W is the
%                       bandwidth, T is the duration of the data and p
%                       is an integer such that 2TW-p tapers are used. In
%                       this form there is no default i.e. to specify
%                       the bandwidth, you have to specify T and p as
%                       well. Note that the units of W and T have to be
%                       consistent: if W is in Hz, T must be in seconds
%                       and vice versa. Note that these units must also
%                       be consistent with the units of params.Fs: W can
%                       be in Hz if and only if params.Fs is in Hz.
%                       The default is to use form 1 with TW=3 and K=5
%
%	        pad		    (padding factor for the FFT) - optional (can take values -1,0,1,2...).
%                    -1 corresponds to no padding, 0 corresponds to padding
%                    to the next highest power of 2 etc.
%			      	 e.g. For N = 500, if PAD = -1, we do not pad; if PAD = 0, we pad the FFT
%			      	 to 512 points, if pad=1, we pad to 1024 points etc.
%			      	 Defaults to 0.
%           Fs   (sampling frequency) - optional. Default 1.
%           fpass    (frequency band to be used in the calculation in the form
%                                   [fmin fmax])- optional.
%                                   Default all frequencies between 0 and Fs/2
%           err  (error calculation [1 p] - Theoretical error bars; [2 p] - Jackknife error bars
%                                   [0 p] or 0 - no error bars) - optional. Default 0.
%       segave (average over segments for 1, don't average for 0)
%       fscorr   (finite size corrections, 0 (don't use finite size corrections) or
%                1 (use finite size corrections) - optional
%                (available only for spikes). Defaults 0.
% Output:
%       C (magnitude of coherency - frequencies x segments if segave=0; dimension frequencies if segave=1)
%       phi (phase of coherency - frequencies x segments if segave=0; dimension frequencies if segave=1)
%       S12 (cross spectrum -  frequencies x segments if segave=0; dimension frequencies if segave=1)
%       S1 (spectrum 1 - frequencies x segments if segave=0; dimension frequencies if segave=1)
%       S2 (spectrum 2 - frequencies x segments if segave=0; dimension frequencies if segave=1)
%       f (frequencies)
%       zerosp (1 for trials where no spikes were found, 0 otherwise)
%       confC (confidence level for C at 1-p %) - only for err(1)>=1
%       phistd - theoretical/jackknife (depending on err(1)=1/err(1)=2) standard deviation for phi
%                Note that phi + 2 phistd and phi - 2 phistd will give 95% confidence
%                bands for phi - only for err(1)>=1
%       Cerr  (Jackknife error bars for C - use only for Jackknife - err(1)=2)

if nargin < 3; error('Need data1 and data2 and size of segment'); end;
if nargin < 4; params=[]; end;
[tapers,pad,Fs,fpass,err,trialave,params]=getparams(params);
clear tapers pad fpass trialave
if nargin < 5 || isempty(segave); segave=1;end;
if nargin < 6 || isempty(fscorr); fscorr=0; end;
if nargout > 7 && err(1)==0;
%   Errors computed only if err(1) is non-zero. Need to change params and run again.
    error('When errors are desired, err(1) has to be non-zero.');
end;
if nargout > 9 && err(1)~=2;
    error('Cerr computed only for Jackknife. Correct inputs and run again');
end;

N=check_consistency(data1,data2);
dt=1/Fs; % sampling interval
T=N*dt; % length of data in seconds
E=0:win:T-win; % fictitious event triggers
win=[0 win]; % use window length to define left and right limits of windows around triggers
data1=createdatamatc(data1,E,Fs,win); % segmented data 1
data2=createdatamatpb(data2,E,Fs,win); % segmented data 2
params.trialave=segave;
if nargout==7;
  [C,phi,S12,S1,S2,f,zerosp]=coherencycpb(data1,data2,params,fscorr); % compute coherency for segmented data
elseif nargout==9;
  [C,phi,S12,S1,S2,f,zerosp,confC,phistd]=coherencycpb(data1,data2,params,fscorr); % compute coherency for segmented data
elseif nargout==10;
  [C,phi,S12,S1,S2,f,zerosp,confC,phistd,Cerr]=coherencycpb(data1,data2,params,fscorr); % compute coherency for segmented data
end;


classdef form_acp < mp.form_ac
%MP.FORM_ACP  MATPOWER Formulation class for AC polar voltage formulations
%   Each concrete Network Model Element class must inherit, at least
%   indirectly, from both MP.NM_ELEMENT and MP.FORM.
%
%   Subclass of MP.FORM_AC.
%   MP.FORM provides properties and methods related to the specific
%   formulation (e.g. DC version, AC polar power version, etc.)
%
%   Properties
%       (model parameters inherited from MP.FORM_AC)
%
%   Methods
%       form_name() - returns string w/name of formulation ('AC-polar formulation')
%       form_tag() - returns string w/short label for formulation ('acp')

%   MATPOWER
%   Copyright (c) 2019-2020, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

%     properties
%     end

    methods
        function name = form_name(obj)
            name = 'AC-polar';
        end

        function tag = form_tag(obj)
            tag = 'acp';
        end

        function vtypes = model_vvars(obj)
            vtypes = {'va', 'vm'};
        end

        function [Iva, Ivm] = port_inj_current_jac(obj, ...
                n, v_, Y, M, invdiagvic, diagSlincJ)
            % [Iva, Ivm] = obj.port_inj_current_jac(...)

            %% intermediate terms
            diagv = sparse(1:n, 1:n, v_, n, n);
            C = invdiagvic * (diagSlincJ - conj(M * diagv));
            D = sparse(1:n, 1:n, 1 ./ abs(v_), n, n);

%             %% linear current term
%             Ivm = Y * diagv;
%             Iva = 1j * Ivm;
%             Ivm = Ivm * D;
%
%             %% + current from linear power term
%             Iva = Iva + 1j * C;
%             Ivm = Ivm - C * D;

            A = Y * diagv;
            Iva = 1j * (A + C);
            Ivm = (A - C) * D;
        end

        function [Ivava, Ivavm, Ivmvm] = port_inj_current_hess_v(obj, x_, lam, v_, z_, diaginvic, Y, M, diagSlincJ, dlamJ)
            % [Ivava, Ivavm, Ivmvm] = obj.port_inj_current_hess_v(x_, lam)
            % [Ivava, Ivavm, Ivmvm] = obj.port_inj_current_hess_v(x_, lam, sysx)
            % [Ivava, Ivavm, Ivmvm] = obj.port_inj_current_hess_v(x_, lam, sysx, idx)
            % [...] = obj.port_inj_current_hess_vz(x_, lam, v_, z_, diaginvic, Y, M, diagSlincJ, dlamJ)

            if nargin < 10
                sysx = v_;
                if nargin < 5
                    idx = []
                else
                    idx = z_;
                end
                [Y, M, N, s] = obj.get_params(idx, {'Y', 'M', 'N', 's'});
                [v_, z_, vi_] = obj.x2vz(x_, sysx, idx);

                %% compute linear power injections
                if isempty(z_)
                    Slin = M*v_ + s;
                else
                    Slin = M*v_ + N*z_ + s;
                end

                n  = length(v_);    %% number of all port voltages
                ni = length(vi_);   %% number of selected port voltages
                diaginvic = sparse(1:ni, 1:ni, 1 ./ conj(vi_), ni, ni);
                if isempty(idx)     %% all ports
                    diagSlincJ = sparse(1:n, 1:n, conj(Slin), n, n);
                    dlamJ      = sparse(1:n, 1:n, lam, n, n);
                else                %% selected ports
                    diagSlincJ = sparse(1:ni, idx, conj(Slin), ni, n);
                    dlamJ      = sparse(1:ni, idx, lam, ni, n);
                end
            else
                n  = length(v_);    %% number of all port voltages
                ni = length(lam);
            end

            %% intermediate terms
            diagv  = sparse(1:n, 1:n, v_, n, n);
            A = diaginvic * diagSlincJ;
            B = diaginvic * conj(M);
            % B2 = B * conj(diagv);
            % C = A - B2;
            D = sparse(1:n, 1:n, 1 ./ abs(v_), n, n);
            % E = diaginvic * conj(N);
            F = dlamJ.';
            G = F * B * conj(diagv);    %% F * B2
            dBtlam = sparse(1:n, 1:n, B.' * lam, n, n);
            H = dBtlam * conj(diagv);
            K = (F * A).';
            GG = G + G.';
            LL = GG - H - K;
            MM = D * (2*K - GG) * D;

            %% linear current term
            dYtlam = sparse(1:n, 1:n, Y.' * lam, n, n);
            Ivava = -dYtlam * diagv;
            Ivavm = -j * Ivava * D;

            %% + current from linear power term
            Ivava = Ivava + LL;
            Ivavm = Ivavm + 1j * LL.' * D;
            Ivmvm = MM;
        end

        function [Ivazr, Ivazi, Ivmzr, Ivmzi] = port_inj_current_hess_vz(obj, x_, lam, v_, z_, diaginvic, N, dlamJ)
            % [Ivazr, Ivazi, Ivmzr, Ivmzi] = obj.port_inj_current_hess_vz(x_, lam)
            % [...] = obj.port_inj_current_hess_vz(x_, lam, sysx)
            % [...] = obj.port_inj_current_hess_vz(x_, lam, sysx, idx)
            % [...] = obj.port_inj_current_hess_vz(x_, lam, v_, z_, diaginvic, N, dlamJ)

            if nargin < 8
                sysx = v_;
                if nargin < 5
                    idx = []
                else
                    idx = z_;
                end
                N = obj.get_params(idx, 'N');
                [v_, z_, vi_] = obj.x2vz(x_, sysx, idx);

                n  = length(v_);    %% number of all port voltages
                ni = length(vi_);   %% number of selected port voltages
                diaginvic = sparse(1:ni, 1:ni, 1 ./ conj(vi_), ni, ni);
                if isempty(idx)     %% all ports
                    dlamJ = sparse(1:n, 1:n, lam, n, n);
                else                %% selected ports
                    dlamJ = sparse(1:ni, idx, lam, ni, n);
                end
            else
                n  = length(v_);    %% number of all port voltages
                ni = length(lam);
            end

            %% intermediate terms
            D = sparse(1:n, 1:n, 1 ./ abs(v_), n, n);
            E = diaginvic * conj(N);
            NN = dlamJ.' * E;

            %% current from linear power term
            Ivazr = 1j * NN;
            Ivazi = NN;
            Ivmzr = -D * NN;
            Ivmzi = -1j * Ivmzr;
        end

        function [Sva, Svm] = port_inj_power_jac(obj, ...
                n, v_, Y, M, diagv, diagvi, diagIlincJ)
            % [Sva, Svm] = obj.port_inj_power_jac(...)

            %% intermediate terms
            A = diagvi * diagIlincJ;
%             B = diagvi * conj(Y);
%             C = B * conj(diagv);
            C = diagvi * conj(Y * diagv);
            D = sparse(1:n, 1:n, 1 ./ abs(v_), n, n);

%             %% linear power term
%             Svm = M * diagv;
%             Sva = 1j * Svm;
%             Svm = Svm * D;
%
%             %% + power from linear current term
%             Sva = Sva + 1j * (A - C);
%             Svm = Svm + (A + C) * D;

            Svm = M * diagv + A;
            Sva = 1j * (Svm - C);
            Svm = (Svm + C) * D;
        end

        function [Svava, Svavm, Svmvm] = port_inj_power_hess_v(obj, x_, lam, v_, z_, diagvi, Y, M, diagIlincJ, dlamJ)
            % [Svava, Svavm, Svmvm] = obj.port_inj_power_hess_v(x_, lam)
            % [Svava, Svavm, Svmvm] = obj.port_inj_power_hess_v(x_, lam, sysx)
            % [Svava, Svavm, Svmvm] = obj.port_inj_power_hess_v(x_, lam, sysx, idx)
            % [...] = obj.port_inj_power_hess_v(x_, lam, v_, z_, diagvi, Y, M, diagIlincJ, dlamJ)

            if nargin < 10
                sysx = v_;
                if nargin < 5
                    idx = []
                else
                    idx = z_;
                end
                [Y, L, M, i] = obj.get_params(idx, {'Y', 'L', 'M', 'i'});
                [v_, z_, vi_] = obj.x2vz(x_, sysx, idx);

                %% compute linear current injections
                if isempty(z_)
                    Ilin = Y*v_ + i;
                else
                    Ilin = Y*v_ + L*z_ + i;
                end

                n  = length(v_);    %% number of all port voltages
                ni = length(vi_);   %% number of selected port voltages
                diagvi = sparse(1:ni, 1:ni, vi_, ni, ni);
                if isempty(idx)     %% all ports
                    diagIlincJ = sparse(1:n, 1:n, conj(Ilin), n, n);
                    dlamJ      = sparse(1:n, 1:n, lam, n, n);
                else                %% selected ports
                    diagIlincJ = sparse(1:ni, idx, conj(Ilin), ni, n);
                    dlamJ      = sparse(1:ni, idx, lam, ni, n);
                end
            else
                n  = length(v_);    %% number of all port voltages
                ni = length(lam);
            end

            %% intermediate terms
            diagv  = sparse(1:n, 1:n, v_, n, n);

            A = diagvi * diagIlincJ;
            B = diagvi * conj(Y);
            C = B * conj(diagv);
            D = sparse(1:n, 1:n, 1 ./ abs(v_), n, n);
            % E = diagvi * conj(L);
            F = dlamJ.';
            G = F * C;
            dBtlam = sparse(1:n, 1:n, B.' * lam, n, n);
            H = conj(diagv) * ((F*B).' - dBtlam);
            K = F * (C - A);

            %% linear power term
            dMtlam = sparse(1:n, 1:n, M.' * lam, n, n);
            Svava = -dMtlam * diagv;
            Svavm = -j * Svava * D;

            %% + power from linear current term
            Svava = Svava + H + K;
            Svavm = Svavm + 1j * (H - K).' * D;
            Svmvm = D * (G + G.') * D;
        end

        function [Svazr, Svazi, Svmzr, Svmzi] = port_inj_power_hess_vz(obj, x_, lam, v_, z_, diagvi, L, dlamJ)
            % [Svazr, Svazi, Svmzr, Svmzi] = obj.port_inj_power_hess_vz(x_, lam)
            % [...] = obj.port_inj_power_hess_vz(x_, lam, sysx)
            % [...] = obj.port_inj_power_hess_vz(x_, lam, sysx, idx)
            % [...] = obj.port_inj_power_hess_vz(x_, lam, v_, z_, diagvi, L, dlamJ)

            if nargin < 8
                sysx = v_;
                if nargin < 5
                    idx = []
                else
                    idx = z_;
                end
                L = obj.get_params(idx, 'L');
                [v_, z_, vi_] = obj.x2vz(x_, sysx, idx);

                n  = length(v_);    %% number of all port voltages
                ni = length(vi_);   %% number of selected port voltages
                diagvi = sparse(1:ni, 1:ni, vi_, ni, ni);
                if isempty(idx)     %% all ports
                    dlamJ = sparse(1:n, 1:n, lam, n, n);
                else                %% selected ports
                    dlamJ = sparse(1:ni, idx, lam, ni, n);
                end
            else
                n  = length(v_);    %% number of all port voltages
                ni = length(lam);
            end

            %% intermediate terms
            D = sparse(1:n, 1:n, 1 ./ abs(v_), n, n);
            E = diagvi * conj(L);
            LL = dlamJ.' * E;
            M = D * LL;

            %% power from linear current term
            Svazr = 1j * LL;
            Svazi = LL;
            Svmzr = M;
            Svmzi = -1j * M;
        end

        function [va, vm] = aux_data_va_vm(obj, ad)
            va = ad.va;
            vm = ad.vm;
        end
    end     %% methods
end         %% classdef


% INTLAB  INTerval LABoratory.
% Version 7.1   June-24-2013   966 files, 849 m-files, 32,346 lines of Matlab code w/o comments
%                 (54,324 with comments),
%             supported by a test-case library with more than 60,000 lines of Matlab code w/o comments
%
%===> For license information and terms of use, see README.TXT.
%
%
%Directories and toolboxes
%  intval       - Interval package
%  gradient     - Automatic differentiation package
%  hessian      - Automatic Hessian package
%  taylor       - Automatic Taylor package
%  slope        - Automatic slope package
%  polynom      - Polynomial package (univariate and multivariate)
%  long         - Rudimentary long package
%  utility      - Some useful functions
%  AccSum       - Reference implementations for sum/dot routines
%  demos        - several demo routines
%
%Auxiliary files
%  intlablogo   - INTLAB logo display
%  startintlab  - Initialization of INTLAB
%  startup      - Calls startintlab: adapt to your local installation
%  intlabsetting   - Current setting of INTLAB control variables
%  INTLAB_Version_2     - Additions and changes in version 2
%  INTLAB_Version_3     - Additions and changes in version 3
%  INTLAB_Version_3.1   - Additions and changes in version 3.1
%  INTLAB_Version_4     - Additions and changes in version 4
%  INTLAB_Version_4.1   - Additions and changes in version 4.1
%  INTLAB_Version_4.1.1 - Additions and changes in version 4.1.1
%  INTLAB_Version_4.1.2 - Additions and changes in version 4.1.2
%  INTLAB_Version_5     - Additions and changes in version 5
%  INTLAB_Version_5.1   - Additions and changes in version 5.1
%  INTLAB_Version_5.2   - Additions and changes in version 5.2
%  INTLAB_Version_5.3   - Additions and changes in version 5.3
%  INTLAB_Version_5.4   - Additions and changes in version 5.4
%  INTLAB_Version_5.5   - Additions and changes in version 5.5
%  INTLAB_Version_6     - Additions and changes in version 6
%  INTLAB_Version_7     - Additions and changes in version 7
%  INTLAB_Version_7.1   - Additions and changes in version 7.1
%  FAQ          - Frequently asked questions
%  Readme       - Installation, a little tutorial and miscellaneous
%
%
%INTLAB based on
%  S.M. Rump: INTLAB - INTerval LABoratory, in "Developments in
%    Reliable Computing", ed. T. Csendes, Kluwer Academic Publishers,
%    77-104, 1999.
%
%INTLAB implementation of interval arithmetic is based on
%  S.M. Rump: Fast and Parallel Interval Arithmetic,
%    BIT 39(3), 539-560, 1999.
%and
%  S. Oishi, S.M. Rump: Fast verification of solutions of matrix equations,
%    Numerische Mathametik 90, 755-773, 2002.
%
%Real interval standard functions based on
%  S.M. Rump: Rigorous and portable standard functions,
%    BIT 41(3), 540-56, 2001.
%
%Complex interval standard functions based on
%  N.C. Boersken: Komplexe Kreis-Standardfunktionen, Freiburger
%    Intervallberichte 78/2.
%
%Accurate summation and dot product with specified _precision_
%  T. Ogita, S.M. Rump, and S. Oishi. Accurate Sum and Dot Product,
%     SIAM Journal on Scientific Computing (SISC), 26(6):1955-1988, 2005.
%  S.M. Rump, T. Ogita, and S. Oishi. Fast High Precision Summation.
%     Nonlinear Theory and Its Applications (NOLTA), IEICE, 1(1), 2010.
%  S.M. Rump: Ultimately Fast Accurate Summation, SIAM Journal on
%     Scientific Computing (SISC), 31(5):3466-3502, 2009.
%
%Accurate summation and dot product with specified _accuracy_
%  S.M. Rump, T. Ogita, and S. Oishi: Accurate Floating-point Summation I:
%     Faithful Rounding. SIAM Journal on Scientific Computing (SISC),
%     31(1): 189-224, 2008.
%  S.M. Rump, T. Ogita, and S. Oishi: Accurate Floating-point Summation II:
%     Sign, K-fold Faithful and Rounding to Nearest. SIAM Journal on Scientific
%     Computing (SISC), 31(2):1269-1302, 2008.
%  S.M. Rump: Ultimately Fast Accurate Summation, SIAM Journal on
%     Scientific Computing (SISC), 31(5):3466-3502, 2009
%
%The linear system solver including inner inclusions is completely redesigned and
%now based on
%  S.M. Rump. Accurate solution of dense linear systems, Part II: Algorithms using
%    directed rounding. Journal of Computational and Applied Mathematics (JCAM),
%    242:185–212, 2013.
%
%Least squares problems and underdetermined linear systems are based on
%  S.M. Rump. Verified Bounds for Least Squares Problems and Underdetermined Linear Systems.
%    SIAM J. Matrix Anal. Appl. (SIMAX), 33(1):130–148, 2012.
%and new componentwise error estimates on
%  S.M. Rump: Improved componentwise verified error bounds for least squares problems
%    and underdetermined linear systems, to appear.
%
%For references to verification algorithms cf. the corresponding routines. Many
%of them are discussed in
%  S.M. Rump: Verification methods: Rigorous results using floating-point arithmetic.
%    Acta Numerica, 19:287-449, 2010.
%
%Slopes based on
%  R. Krawzcyk, A. Neumaier: Interval slopes for rational functions
%    and associated centered forms, SIAM J. Numer. Anal. 22, 604-616
%    (1985)
%with improvements based on
%  S.M. Rump: Expansion and Estimation of the Range of Nonlinear Functions,
%    Math. Comp. 65(216), pp. 1503-1512, 1996.
%
%Gradients, Hessians and Taylor expansion based on standard forward mode, cf. for example
%  L.B. Rall: Automatic Differentiation: Techniques and Applications,
%    Lecture Notes in Computer Science 120, Springer, 1981.
%
%Long floating point and interval arithmetic based on standard techniques
%  with adaptation and speed up for interpretative systems.
%

% written  12/30/98     S.M. Rump
% modified 03/06/99     S.M. Rump  Version 2
% modified 11/16/99     S.M. Rump  Version 3
% modified 03/07/02     S.M. Rump  Version 3.1
% modified 12/08/02     S.M. Rump  Version 4
% modified 12/27/02     S.M. Rump  Version 4.1
% modified 01/22/03     S.M. Rump  Version 4.1.1
% modified 11/18/03     S.M. Rump  Version 4.1.2
% modified 04/04/04     S.M. Rump  Version 5
% modified 06/04/05     S.M. Rump  Version 5.1
% modified 12/20/05     S.M. Rump  Version 5.2
% modified 05/26/06     S.M. Rump  Version 5.3
% modified 05/31/07     S.M. Rump  Version 5.4
% modified 11/05/08     S.M. Rump  Version 5.5
% modified 05/08/09     S.M. Rump  Version 6
% modified 12/12/12     S.M. Rump  Version 7
% modified 06/24/13     S.M. Rump  Version 7.1
%

% Copyright (c) Siegfried M. Rump, head of the Institute for Reliable Computing,
%               Hamburg University of Technology
%
% For license information and terms of use, see README.TXT.
%
% :::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
% ::  Some references to papers using INTLAB are collected on the
% ::    INTLAB homepage   http://www.ti3.tuhh.de/
% ::  If you have additional references to add, please send me
% ::  a mail ( rump [at] tuhh.de )
%


function CPD = gaussian_CPD(bnet, self, varargin)
% GAUSSIAN_CPD Make a conditional linear Gaussian distrib.
%
% CPD = gaussian_CPD(bnet, node, ...) will create a CPD with random parameters,
% where node is the number of a node in this equivalence class.

% To define this CPD precisely, call the continuous (cts) parents (if any) X,
% the discrete parents (if any) Q, and this node Y. Then the distribution on Y is:
% - no parents: Y ~ N(mu, Sigma)
% - cts parents : Y|X=x ~ N(mu + W x, Sigma)
% - discrete parents: Y|Q=i ~ N(mu(i), Sigma(i))
% - cts and discrete parents: Y|X=x,Q=i ~ N(mu(i) + W(i) x, Sigma(i))
%
% The list below gives optional arguments [default value in brackets].
% (Let ns(i) be the size of node i, X = ns(X), Y = ns(Y) and Q = prod(ns(Q)).)
% Parameters will be reshaped to the right size if necessary.
%
% mean       - mu(:,i) is the mean given Q=i [ randn(Y,Q) ]
% cov        - Sigma(:,:,i) is the covariance given Q=i [ repmat(100*eye(Y,Y), [1 1 Q]) ]
% weights    - W(:,:,i) is the regression matrix given Q=i [ randn(Y,X,Q) ]
% cov_type   - if 'diag', Sigma(:,:,i) is diagonal [ 'full' ]
% tied_cov   - if 1, we constrain Sigma(:,:,i) to be the same for all i [0]
% clamp_mean - if 1, we do not adjust mu(:,i) during learning [0]
% clamp_cov  - if 1, we do not adjust Sigma(:,:,i) during learning [0]
% clamp_weights - if 1, we do not adjust W(:,:,i) during learning [0]
% cov_prior_weight - weight given to I prior for estimating Sigma [0.01]
% cov_prior_entropic - if 1, we also use an entropic prior for Sigma [0]
%
% e.g., CPD = gaussian_CPD(bnet, i, 'mean', [0; 0], 'clamp_mean', 1)

if nargin==0
  % This occurs if we are trying to load an object from a file.
  CPD = init_fields;
  clamp = 0;
  CPD = class(CPD, 'gaussian_CPD', generic_CPD(clamp));
  return;
elseif isa(bnet, 'gaussian_CPD')
  % This might occur if we are copying an object.
  CPD = bnet;
  return;
end
CPD = init_fields;

CPD = class(CPD, 'gaussian_CPD', generic_CPD(0));

args = varargin;
ns = bnet.node_sizes;
ps = parents(bnet.dag, self);
dps = myintersect(ps, bnet.dnodes);
cps = myintersect(ps, bnet.cnodes);
fam_sz = ns([ps self]);

CPD.self = self;
CPD.sizes = fam_sz;

% Figure out which (if any) of the parents are discrete, and which cts, and how big they are
% dps = discrete parents, cps = cts parents
CPD.cps = find_equiv_posns(cps, ps); % cts parent index
CPD.dps = find_equiv_posns(dps, ps);
ss = fam_sz(end);
psz = fam_sz(1:end-1);
dpsz = prod(psz(CPD.dps));
cpsz = sum(psz(CPD.cps));

% set default params
CPD.mean = randn(ss, dpsz);
CPD.cov = 100*repmat(eye(ss), [1 1 dpsz]);
CPD.weights = randn(ss, cpsz, dpsz);
CPD.cov_type = 'full';
CPD.tied_cov = 0;
CPD.clamped_mean = 0;
CPD.clamped_cov = 0;
CPD.clamped_weights = 0;
CPD.cov_prior_weight = 0.01;
CPD.cov_prior_entropic = 0;
nargs = length(args);
if nargs > 0
  CPD = set_fields(CPD, args{:});
end

% Make sure the matrices have 1 dimension per discrete parent.
% Bug fix due to Xuejing Sun 3/6/01
CPD.mean = myreshape(CPD.mean, [ss ns(dps)]);
CPD.cov = myreshape(CPD.cov, [ss ss ns(dps)]);
CPD.weights = myreshape(CPD.weights, [ss cpsz ns(dps)]);

% Precompute indices into block structured  matrices
% to speed up CPD_to_lambda_msg and CPD_to_pi
cpsizes = CPD.sizes(CPD.cps);
CPD.cps_block_ndx = cell(1, length(cps));
for i=1:length(cps)
  CPD.cps_block_ndx{i} = block(i, cpsizes);
end

%%%%%%%%%%%
% Learning stuff

% expected sufficient statistics
CPD.Wsum = zeros(dpsz,1);
CPD.WYsum = zeros(ss, dpsz);
CPD.WXsum = zeros(cpsz, dpsz);
CPD.WYYsum = zeros(ss, ss, dpsz);
CPD.WXXsum = zeros(cpsz, cpsz, dpsz);
CPD.WXYsum = zeros(cpsz, ss, dpsz);

% For BIC
CPD.nsamples = 0;
switch CPD.cov_type
 case 'full',
  % since symmetric
    %ncov_params = ss*(ss-1)/2;
    ncov_params = ss*(ss+1)/2;
  case 'diag',
    ncov_params = ss;
  otherwise
    error(['unrecognized cov_type ' cov_type]);
end
% params = weights + mean + cov
if CPD.tied_cov
  CPD.nparams = ss*cpsz*dpsz + ss*dpsz + ncov_params;
else
  CPD.nparams = ss*cpsz*dpsz + ss*dpsz + dpsz*ncov_params;
end

% for speeding up maximize_params
CPD.useC = exist('rep_mult');

clamped = CPD.clamped_mean & CPD.clamped_cov & CPD.clamped_weights;
CPD = set_clamped(CPD, clamped);

%%%%%%%%%%%

function CPD = init_fields()
% This ensures we define the fields in the same order
% no matter whether we load an object from a file,
% or create it from scratch. (Matlab requires this.)

CPD.self = [];
CPD.sizes = [];
CPD.cps = [];
CPD.dps = [];
CPD.mean = [];
CPD.cov = [];
CPD.weights = [];
CPD.clamped_mean = [];
CPD.clamped_cov = [];
CPD.clamped_weights = [];
CPD.cov_type = [];
CPD.tied_cov = [];
CPD.Wsum = [];
CPD.WYsum = [];
CPD.WXsum = [];
CPD.WYYsum = [];
CPD.WXXsum = [];
CPD.WXYsum = [];
CPD.nsamples = [];
CPD.nparams = [];
CPD.cov_prior_weight = [];
CPD.cov_prior_entropic = [];
CPD.useC = [];
CPD.cps_block_ndx = [];