% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

%%begin

% DO VARIOUS ROBOTS: 2 LINK, HYPER2D, HYPER3D

% Inverse kinematics is the problem of finding the robot joint coordinates,
% given a homogeneous transform representing the last link of the manipulator.
% It is very useful when the path is planned in Cartesian space, for instance
% a straight line path as shown in the trajectory demonstration.
%
% First generate the transform corresponding to a particular joint coordinate,

mdl_puma560
q = [0 -pi/4 -pi/4 0 pi/8 0]
T = p560.fkine(q)

% Now the inverse kinematic procedure for any specific robot can be derived
% symbolically and in general an efficient closed-form solution can be
% obtained.  However we are given only a generalized description of the
% manipulator in terms of kinematic parameters so an iterative solution will
% be used. The procedure is slow, and the choice of starting value affects
% search time and the solution found, since in general a manipulator may
% have several poses which result in the same transform for the last
% link. The starting point for the first point may be specified, or else it
% defaults to zero (which is not a particularly good choice in this case)

qi = p560.ikine(T);

% and in fact it does not converge

qi

% We can help the solution along by using the 'pinv' option

qi = p560.ikine(T, 'pinv');
% and the result
qi
% is the same as the original set of joint angles
q

% However in general this will not be the case, there are multiple
% solutions, and the solution that is found depends on the initial
% choice of angles.
%
% A more efficient approach is to use an analytic solution and the toolbox
% supports the common case of a 6-axis robot arm with a spherical wrist

qi = p560.ikine6s(T)
% which is different to the original joint angles, but as expected

p560.fkine(qi)

% it does give the same end-effector pose.

% The analytic solution allows the specific solution to be specified
% using a character string and to get the same set of joint angles

p560.ikine6s(T, 'rdf')
% where we have specified that the robot is in a right-handed configuration
% (r), with its elbow down (d), and the wrist flipped (f).

% A solution is not always possible, for instance if the specified
% transform describes a point out of reach of the manipulator.  As
% mentioned above the solutions are not necessarily unique, and there
% are singularities at which the manipulator loses degrees of freedom
% and joint coordinates become linearly dependent.

% Inverse kinematics may also be computed for a trajectory.
% If we take a Cartesian straight line path between two poses in 50 steps

T1 = transl(0.6, -0.5, 0.0) % define the start point
T2 = transl(0.4, 0.5, 0.2)	% and destination
T = ctraj(T1, T2, 50); 	% compute a Cartesian path

% now solve the inverse kinematics

q = p560.ikine6s(T);
about q
% which has one row per time step and one column per joint angle

% Let's examine the joint space trajectory that results in straightline
% Cartesian motion

subplot(3,1,1); plot(q(:,1)); xlabel('Time (s)'); ylabel('Joint 1 (rad)');
subplot(3,1,2); plot(q(:,2)); xlabel('Time (s)'); ylabel('Joint 2 (rad)');
subplot(3,1,3); plot(q(:,3)); xlabel('Time (s)'); ylabel('Joint 3 (rad)');

% This joint space trajectory can now be animated
clf
p560.plot(q)


%% DEMO_febio_00014_cube_varying_material
% Below is a demonstration for:
%
% * Building geometry for a cube with hexahedral elements
% * Defining the boundary conditions
% * Coding the febio structure
% * Running the model
% * Importing and visualizing the displacement and stress results

%% Keywords
%
% * febio_spec version 3.0
% * febio, FEBio
% * compression, tension, compressive, tensile
% * displacement control, displacement boundary condition
% * hexahedral elements, hex8
% * cube, box, rectangular
% * static, solid
% * spatially varying material properties
% * hyperelastic, Ogden
% * displacement logfile
% * stress logfile

%%

clear; close all; clc;

%% Plot settings
fontSize=15;
faceAlpha1=0.8;
markerSize=40;
lineWidth=3;
cMap=spectral(250);

%% Control parameters

% Path names
defaultFolder = fileparts(fileparts(mfilename('fullpath')));
savePath=fullfile(defaultFolder,'data','temp');

% 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 force
febioLogFileName_sed=[febioFebFileNamePart,'_sed_out.txt']; %Log file name for exporting strain energy density

%Specifying dimensions and number of elements
pointSpacings=1*ones(1,3); %Desired point spacing between nodes
cubeSize=10;
sampleWidth=cubeSize; %Width
sampleThickness=cubeSize; %Thickness
sampleHeight=cubeSize; %Height
numElementsWidth=round(sampleWidth/pointSpacings(1)); %Number of elemens in dir 1
numElementsThickness=round(sampleThickness/pointSpacings(2)); %Number of elemens in dir 2
numElementsHeight=round(sampleHeight/pointSpacings(3)); %Number of elemens in dir 3

%Define applied displacement
appliedStrain=0.3; %Linear strain (Only used to compute applied stretch)
loadingOption='tension'; % or 'compression'
switch loadingOption
    case 'compression'
        stretchLoad=1-appliedStrain; %The applied stretch for uniaxial loading
    case 'tension'
        stretchLoad=1+appliedStrain; %The applied stretch for uniaxial loading
end
displacementMagnitude=(stretchLoad*sampleHeight)-sampleHeight; %The displacement magnitude

%Material parameter sets
testOpt=2; %1=Linear gradient of material, or 2=gyroid based material distribution
E_youngs_min=1e-3; %Lowest Youngs modulus
E_youngs_max=1; %Highest Youngs modulus
nu_min=0.3; %Lowest Poissons ratio
nu_max=0.45; %Lowest Poissons ratio

% FEA control settings
numTimeSteps=10; %Number of time steps desired
max_refs=25; %Max reforms
max_ups=0; %Set to zero to use full-Newton iterations
opt_iter=6; %Optimum number of iterations
max_retries=5; %Maximum number of retires
dtmin=(1/numTimeSteps)/100; %Minimum time step size
dtmax=1/numTimeSteps; %Maximum time step size
runMode='external';

%% Creating model geometry and mesh
% A box is created with tri-linear hexahedral (hex8) elements using the
% |hexMeshBox| function. The function offers the boundary faces with
% seperate labels for the top, bottom, left, right, front, and back sides.
% As such these can be used to define boundary conditions on the exterior.

% Create a box with hexahedral elements
cubeDimensions=[sampleWidth sampleThickness sampleHeight]; %Dimensions
cubeElementNumbers=[numElementsWidth numElementsThickness numElementsHeight]; %Number of elements
outputStructType=2; %A structure compatible with mesh view
[meshStruct]=hexMeshBox(cubeDimensions,cubeElementNumbers,outputStructType);

%Access elements, nodes, and faces from the structure
E=meshStruct.elements; %The elements
V=meshStruct.nodes; %The nodes (vertices)
Fb=meshStruct.facesBoundary; %The boundary faces
Cb=meshStruct.boundaryMarker; %The "colors" or labels for the boundary faces

%% Define spatially varying material distribution data
% Here the element centre coordinates are used assign the material
% stiffness based on a particular function on these coordinates.

VE=patchCentre(E,V); %Element centres
switch testOpt
    case 1 %linear gradient in X direction
        S=VE(:,1);
    case 2 %gyroid
        %Scale coordinates for gyroid
        VE=VE-min(VE(:));
        VE=VE./max(VE(:));
        VE=(VE.*2*pi)-pi;

        %Evaluate gyroid
        S=triplyPeriodicMinimal(VE(:,1),VE(:,2),VE(:,3),'g');
end

%Normalize data
S=S-min(S(:)); %Subtract minimum -> range [0-...]
S=S./max(S(:)); %Devide by max -> range [0-1]

%Use scaling data S to generate element Youngs moduli
E_youngs_elem=S.*(E_youngs_max-E_youngs_min)+E_youngs_min;
nu_elem=S.*(nu_max-nu_min)+nu_min;

%Fix mesh struct for plotting
meshStruct.elements=E;
meshStruct.elementData=E_youngs_elem;

%%
% Plotting model boundary surfaces and a cut view

hFig=cFigure;

subplot(1,2,1); hold on;
title('Model boundary surfaces and labels','FontSize',fontSize);
gpatch(Fb,V,Cb,'k',faceAlpha1);
colormap(gca,gjet(6)); icolorbar;
axisGeom(gca,fontSize);

hs=subplot(1,2,2); hold on;
title('Cut view of solid mesh and materials','FontSize',fontSize);
optionStruct.hFig=[hFig hs];
meshView(meshStruct,optionStruct);
colormap(gca,cMap); colorbar; caxis([E_youngs_min E_youngs_max]);
axisGeom(gca,fontSize);

drawnow;

%% Defining the boundary conditions
% The visualization of the model boundary shows colors for each side of the
% cube. These labels can be used to define boundary conditions.

%Define supported node sets
bcSupportList=unique(Fb(Cb==5,:)); %Node set part of selected face

%Prescribed displacement nodes
bcPrescribeList=unique(Fb(Cb==6,:)); %Node set part of selected face

%%
% Visualizing boundary conditions. Markers plotted on the semi-transparent
% model denote the nodes in the various boundary condition lists.

hf=cFigure;
title('Boundary conditions','FontSize',fontSize);
xlabel('X','FontSize',fontSize); ylabel('Y','FontSize',fontSize); zlabel('Z','FontSize',fontSize);
hold on;

gpatch(Fb,V,'kw','k',0.5);

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

legend(hl,{'BC full support','BC z prescribe'});

axisGeom(gca,fontSize);
camlight headlight;
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';

%Control section
febio_spec.Control.analysis='STATIC';
febio_spec.Control.time_steps=numTimeSteps;
febio_spec.Control.step_size=1/numTimeSteps;
febio_spec.Control.solver.max_refs=max_refs;
febio_spec.Control.solver.max_ups=max_ups;
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;

%Material section
materialName1='Material1';
dataMapName1='MaterialParameterMap1';
dataMapName2='MaterialParameterMap2';
febio_spec.Material.material{1}.ATTR.name=materialName1;
febio_spec.Material.material{1}.ATTR.type='neo-Hookean';
febio_spec.Material.material{1}.ATTR.id=1;
febio_spec.Material.material{1}.E.ATTR.type='map'; %Calls for mapping of parameter
febio_spec.Material.material{1}.E.VAL=dataMapName1; %Calls for mapping of parameter
febio_spec.Material.material{1}.v.ATTR.type='map'; %Calls for mapping of parameter
febio_spec.Material.material{1}.v.VAL=dataMapName2; %Calls for mapping of parameter

% Mesh 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 this part
febio_spec.Mesh.Elements{1}.ATTR.type='hex8'; %Element type
febio_spec.Mesh.Elements{1}.elem.ATTR.id=(1:1:size(E,1))'; %Element id's
febio_spec.Mesh.Elements{1}.elem.VAL=E; %The element matrix

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

febio_spec.Mesh.NodeSet{1}.ATTR.name=nodeSetName1;
febio_spec.Mesh.NodeSet{1}.node.ATTR.id=bcSupportList(:);

febio_spec.Mesh.NodeSet{2}.ATTR.name=nodeSetName2;
febio_spec.Mesh.NodeSet{2}.node.ATTR.id=bcPrescribeList(:);

%MeshData secion
%-> Element data
febio_spec.MeshData.ElementData{1}.ATTR.name=dataMapName1;
febio_spec.MeshData.ElementData{1}.ATTR.elem_set=partName1;
febio_spec.MeshData.ElementData{1}.elem.ATTR.lid=(1:1:size(E,1))';
febio_spec.MeshData.ElementData{1}.elem.VAL=E_youngs_elem;

febio_spec.MeshData.ElementData{2}.ATTR.name=dataMapName2;
febio_spec.MeshData.ElementData{2}.ATTR.elem_set=partName1;
febio_spec.MeshData.ElementData{2}.elem.ATTR.lid=(1:1:size(E,1))';
febio_spec.MeshData.ElementData{2}.elem.VAL=nu_elem;

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

%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,y,z';

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

febio_spec.Boundary.bc{3}.ATTR.type='prescribe';
febio_spec.Boundary.bc{3}.ATTR.node_set=nodeSetName2;
febio_spec.Boundary.bc{3}.dof='z';
febio_spec.Boundary.bc{3}.scale.ATTR.lc=1;
febio_spec.Boundary.bc{3}.scale.VAL=displacementMagnitude;
febio_spec.Boundary.bc{3}.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; 1 1];

%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.element_data{1}.ATTR.file=febioLogFileName_sed;
febio_spec.Output.logfile.element_data{1}.ATTR.data='sed';
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
% febView(febioFebFileName);

%% 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.runMode=runMode;

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

%% Import FEBio results

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

     %%
    % Importing nodal displacements from a log file
    dataStruct=importFEBio_logfile(fullfile(savePath,febioLogFileName_disp),1,1);

    %Access data
    N_disp_mat=dataStruct.data; %Displacement
    timeVec=dataStruct.time; %Time

    %Create deformed coordinate set
    V_DEF=N_disp_mat+repmat(V,[1 1 size(N_disp_mat,3)]);

    %%
    % Importing element stress from a log file
    dataStruct=importFEBio_logfile(fullfile(savePath,febioLogFileName_sed),1,1);

    %Access data
    E_sed_mat=dataStruct.data;

    %%
    % Plotting the simulated results using |anim8| to visualize and animate
    % deformations

    [CV]=faceToVertexMeasure(E,V,E_sed_mat(:,:,end));

    % Create basic view and store graphics handle to initiate animation
    hf=cFigure; %Open figure
    gtitle([febioFebFileNamePart,': Press play to animate']);
    title('$\Psi$ $[J/m^3]$','Interpreter','Latex')

    hp=gpatch(Fb,V_DEF(:,:,end),CV,'k',1); %Add graphics object to animate
    hp.FaceColor='interp';

    axisGeom(gca,fontSize);
    colormap(cMap); colorbar;
    caxis([min(E_sed_mat(:)) max(E_sed_mat(:))]/3);
    axis(axisLim(V_DEF)); %Set axis limits statically
    camlight headlight;

    % Set up animation features
    animStruct.Time=timeVec; %The time vector
    for qt=1:1:size(N_disp_mat,3) %Loop over time increments

        [CV]=faceToVertexMeasure(E,V,E_sed_mat(:,:,qt));

        %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(:,:,qt),CV}; %Property values for to set in order to animate
    end
    anim8(hf,animStruct); %Initiate animation feature
    drawnow;

end

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>

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


function res = all(a,dim)
%ANY          Like Matlab function "all" for Taylor
%
%Call
%
%   L = all(A)
%   L = all(A,dim)
%
%Same functionality as Matlab/all for intval quantity A
%

% written  05/21/09     S.M. Rump
%

  if nargin==1
    res = any(reshape(all(a.t),a.size));
  else
    res = any(reshape(all(a.t),a.size),dim);
  end


function [numOfEx,vecDim,outDim,numOfCls] = get_dim(dat)

%   [numEx,vDim,oDim,numCls] = get_dim(data)
%
%   Returns (ind given order) the number of examples, the vectors
%   dimension, the number of output dimensions and the number
%   of classes.
%
%   Note:
%   Sometimes the number of classes differs from the output dimension
%   (e.g. in binary pattern recognition often oDim equals 1 and numCls
%   equals 2).

  global X;
  global Y;

  numOfEx=length(dat.index);
  vecDim=length(dat.findex);
  if isempty(dat.myY)
    outp=Y(dat.index,:);
    outDim=size(outp,2);
    numOfCls=outDim;
    if numOfCls==1
        fin1=find(outp==1);
        fin2=find(outp==-1);
    end;
  else
    outp=dat.myY;
    outDim=size(outp,2);
    numOfCls=outDim;
    if numOfCls==1
        fin1=find(outp==1);
        fin2=find(outp==-1);
    end;
  end

  if numOfCls==1
    if length(fin1)>0 & length(fin2)>0 & (length(fin1)+length(fin2))==numOfEx
      numOfCls=2;  %% -1s and +1s => classes
    end
  end


% This code is the implementation of "Multi-scale Guided Image and Video Fusion: A Fast and Efficient Approach"
% Cite this article as:
% Bavirisetti, D.P., Xiao, G., Zhao, J. et al. Circuits Syst Signal Process (2019).
%https://doi.org/10.1007/s00034-019-01131-z
%
% The interface is created by the authors of VIFB.

function img = run_MGFF(imgVI, imgIR, visualization)

    % Guided image filter parameters
    r=9;eps=10^3;

    I1 = double(imread(imgIR.img));
    if size(I1,3)==1
        I1 = repmat(I1,[1,1,3]);
    end

    I2 = double(imread(imgVI.img));

    %% apply multi-scale guided image fusion on source images
    tic
    F = fuse_MGF_RGB(I1, I2, r, eps);
    toc

    fuseimage = im2uint8(F);
    img = fuseimage;

    %% display source images and the fused image
    if visualization == 1
        figure, imshow(uint8(I1), []);
        figure, imshow(uint8(I2),[]);
        figure, imshow((F),[]);
    end
end


function [c,b,c1,g,sn,sp] = constrained_foopsi(y,b,c1,g,sn,options)
% spike inference using a constrained deconvolution approach:
%      min      sum(sp)
%    c,sp,b,c1
%      subject to: sp >= 0
%                   b >= 0
%                  G*c = sp
%                   c1 >= 0
%           ||y-b-c - c_in|| <= sn*sqrt(T)

%   Variables:
%   y:      raw fluorescence data (vector of length(T))
%   c:      denoised calcium concentration (Tx1 vector)
%   b:      baseline concentration (scalar)
%  c1:      initial concentration (scalar)
%   g:      discrete time constant(s) (scalar or 2x1 vector)
%  sn:      noise standard deviation (scalar)
%  sp:      spike vector (Tx1 vector)

%   USAGE:
%   [c,b,c1,g,sn,sp] = constrained_foopsi(y,b,c1,g,sn,OPTIONS)
%   The parameters b,cin,g,sn can be given or else are estimated from the data

%   OPTIONS: (stuct for specifying options)
%         p: order for AR model, used when g is not given (default 2)
%    method: methods for performing spike inference
%   available methods: 'dual' uses dual ascent
%                       'cvx' uses the cvx package available from cvxr.com (default)
%                      'lars' uses the least regression algorithm
%                     'spgl1' uses the spgl1 package available from
%                     math.ucdavis.edu/~mpf/spgl1/  (usually fastest)
%   bas_nonneg:   flag for setting the baseline lower bound. if 1, then b >= 0 else b >= min(y)
%   noise_range:  frequency range over which the noise power is estimated. Default [Fs/4,Fs/2]
%   noise_method: method to average the PSD in order to obtain a robust noise level estimate
%   lags:         number of extra autocovariance lags to be considered when estimating the time constants
%   resparse:     number of times that the solution is resparsened (default 0). Currently available only with methods 'cvx', 'spgl'
%   fudge_factor: scaling constant to reduce bias in the time constant estimation (default 1 - no scaling)

% Written by:
% Eftychios A. Pnevmatikakis, Simons Foundation, 2015

defoptions.p = 2;
defoptions.method = 'cvx';
defoptions.bas_nonneg = 1;              % nonnegativity option for baseline estimation
defoptions.noise_range = [0.25,0.5];    % frequency range over which to estimate the noise
defoptions.noise_method = 'logmexp';    % method for which to estimate the noise level
defoptions.lags = 5;                    % number of extra lags when computing the AR coefficients
defoptions.resparse = 0;                % number of times to re-sparse solution
defoptions.fudge_factor = 1;            % fudge factor for time constants

if nargin < 6
    options = defoptions;
    if nargin < 5
        sn = [];
        if nargin < 4
            g = [];
            if nargin < 3
                c1 = [];
                if nargin < 2
                    b = [];
                end
            end
        end
    end
end

if ~isfield(options,'p');  options.p = defoptions.p;  end
if ~isfield(options,'method'); options.method = defoptions.method; end
if ~isfield(options,'bas_nonneg'); options.bas_nonneg = defoptions.bas_nonneg; end
if ~isfield(options,'noise_range'); options.noise_range = defoptions.noise_range; end
if ~isfield(options,'noise_method'); options.noise_method = defoptions.noise_method; end
if ~isfield(options,'lags'); options.lags = defoptions.lags; end
if ~isfield(options,'resparse'); options.resparse = defoptions.resparse; end
if ~isfield(options,'fudge_factor'); options.fudge_factor = defoptions.fudge_factor; end

method = options.method;
if isempty(b);
    bas_est = 1;
else
    bas_est = 0;
end
if isempty(c1)
    c1_est = 1;
else
    c1_est = 0;
end

y = y(:);
T = length(y);
y_full = y;
mis_data = isnan(y);
E = speye(T);
E(mis_data,:) = [];

if any(mis_data)
    y_full(mis_data) = interp1(find(~mis_data),y(~mis_data),find(mis_data));
end

if isempty(sn)
    sn = GetSn(y_full,options.noise_range,options.noise_method);
end
if isempty(g)
    g = estimate_time_constants(y_full,options.p,sn,options.lags);
    while max(abs(roots([1,-g(:)']))>1) && options.p < 5
        warning('No stable AR(%i) model found. Checking for AR(%i) model \n',options.p,options.p+1);
        options.p = options.p + 1;
        g = estimate_time_constants(y,options.p,sn,options.lags);
    end
    if options.p == 5
        g = 0;
    end
    %fprintf('Stable AR(%i) model found \n',options.p);
    % re-adjust time constant values
    rg = roots([1;-g(:)]);
    if ~isreal(rg); rg = real(rg) + .001*randn(size(rg)); end
    rg(rg>1) = 0.95 + 0.001*randn(size(rg(rg>1)));
    rg(rg<0) = 0.15 + 0.001*randn(size(rg(rg<0)));
    pg = poly(options.fudge_factor*rg);
    g = -pg(2:end);
end
if options.bas_nonneg  % lower bound for baseline
    b_lb = 0;
else
    b_lb = min(y);
end

if strcmpi(method,'dual'); method = 'dual';
elseif strcmpi(method,'cvx'); method = 'cvx';
elseif strcmpi(method,'lars'); method = 'lars';
elseif strcmpi(method,'spgl1'); method = 'spgl1';
else fprintf('Invalid choice of method. Using CVX \n'); method = 'cvx';
end

if strcmpi(method,'dual') && any(mis_data)
    warning('Dual method does not support missing data. Switching to CVX');
    method = 'cvx';
end

if options.resparse > 0 && (strcmpi(method,'dual') || strcmpi(method,'lars'))
    warning('Resparsening is not supported with chosen method. Switching to CVX');
    method = 'cvx';
end

pathCell = regexp(path, pathsep, 'split');
g = g(:);
G = spdiags(ones(T,1)*[-g(end:-1:1)',1],-length(g):0,T,T);
gd = max(roots([1,-g']));  % decay time constant for initial concentration
gd_vec = gd.^((0:T-1)');

switch method
    case 'dual'
         v = G'*ones(T,1);
        thr = sn*sqrt(T-sum(mis_data));
        if bas_est; b = 0; end
        if c1_est; c1 = 0; end
        myfun = @(Ald) lagrangian_temporal_gradient(Ald,thr^2,y(~mis_data)-b-c1*gd_vec(~mis_data),bas_est,c1_est);
        c = [G\max(G*y,0);zeros(bas_est);zeros(c1_est)];
        options_dual = optimset('GradObj','On','Display','Off','Algorithm','interior-point','TolX',1e-8);
        ld_in = 10;
        [ld,~,flag] = fmincon(myfun,ld_in,[],[],[],[],0,[],[],options_dual);
        if (flag == -2) || (flag == -3)
            warning('Problem seems unbounded or infeasible. Try a different method.');
        end
        if bas_est; b = c(T+bas_est); end
        if c1_est; c1 = c(end); end
        c = c(1:T);
        sp = G*c;
    case 'cvx'
        onPath = ~isempty(which('cvx_begin'));
        if onPath
            c = zeros(T,1+options.resparse);
            sp = zeros(T,1+options.resparse);
            bas = zeros(1+options.resparse,1);
            cin = zeros(1+options.resparse,1);
            w_ = ones(T,1);
            for rep = 1:options.resparse+1
                [c(:,rep),bas(rep),cin(rep)] = cvx_foopsi(y,b,c1,sn,b_lb,g,w_,~mis_data);
                sp(:,rep) = G*c(:,rep);
                w_ = 1./(max(sp(:,rep),0) + 1e-8);
            end
            sp(sp<1e-6) = 0;
            c = G\sp;
            b = bas;
            c1 = cin;
        else
            error('CVX does not appear to be on the MATLAB path. It can be downloaded from cvxr.com \n');
        end
    case 'lars'
         Ginv = E*[full(G\speye(T)),ones(T,bas_est),gd_vec*ones(1,c1_est)];
         if bas_est; b = 0; end
         if c1_est; c1 = 0; end
         [~, ~, spikes, ~, ~] = lars_regression_noise(y(~mis_data)-b_lb*bas_est - b - c1*gd_vec(~mis_data), Ginv, 1, sn^2*(T-sum(mis_data)));
         sp = spikes(1:T);
         b = (spikes(T+bas_est)+b_lb)*bas_est + b*(1-bas_est);
         c1 = spikes(end)*c1_est + c1*(1-c1_est);
         c = G\sp;
    case 'spgl1'
        onPath = ~isempty(which('spgl1'));
        if onPath
            Gx = @(x,mode) G_inv_mat(x,mode,T,g,gd_vec,bas_est,c1_est,E);
            c = zeros(T,1+options.resparse);
            sp = zeros(T,1+options.resparse);
            bas = zeros(1+options.resparse,1);
            cin = zeros(1+options.resparse,1);
            w_ = ones(T,1);
            for rep = 1:options.resparse+1
                if bas_est; b = 0; w_ = [w_;1e-10]; end
                if c1_est; c1 = 0; w_ = [w_;1e-10]; end
                options_spgl = spgSetParms('project',@NormL1NN_project ,'primal_norm', @NormL1NN_primal,'dual_norm',@NormL1NN_dual,'verbosity',0,'weights',w_);
                [spikes,r,~,~] = spg_bpdn( Gx, y(~mis_data)-b_lb*bas_est - (1-bas_est)*b-(1-c1_est)*c1*gd_vec(~mis_data), sn*sqrt(T-sum(mis_data)),options_spgl);
                c(:,rep) = G\spikes(1:T); %Gx([spikes(1:T);0],1);                                  %% calcium signal
                bas(rep) = b*(1-bas_est) + bas_est*spikes(T+bas_est)+b_lb*bas_est;       %% baseline
                cin(rep) = c1*(1-c1_est) + c1_est*spikes(end);
                sp(:,rep) = spikes(1:T);                                           %% spiking signal
                w_ = 1./(spikes(1:T)+1e-8);
            end
            b = bas;
            c1 = cin;
            %sn = norm(r)/sqrt(T);
        else
            error('SPGL1 does not appear to be on the MATLAB path. It can be downloaded from math.ucdavis.edu/~mpf/spgl1 \n');
        end
end

    function sn = GetSn(Y,range_ff,method)
        % estimate noise level with a power spectral density method
        L=length(Y);
%         if ~isempty(which('pmtm'))
%             [psd_Y,ff] = pmtm(Y,5/2,1000,1);
%         end
        if ~isempty(which('pwelch'));
            [psd_Y,ff]=pwelch(Y,round(L/8),[],1000,1);
        else
            xdft = fft(Y);
            xdft = xdft(:,1:round(L/2)+1);
            psd_Y = (1/L) * abs(xdft).^2;
            ff = 0:1/L:1/2;
            psd_Y(2:end-1) = 2*psd_Y(2:end-1);
        end
        ind=ff>range_ff(1);
        ind(ff>range_ff(2))=0;
        switch method
            case 'mean'
                sn=sqrt(mean(psd_Y(ind)/2));
            case 'median'
                sn=sqrt(median(psd_Y(ind)/2));
            case 'logmexp'
                sn = sqrt(exp(mean(log(psd_Y(ind)/2))));
        end
    end

    function g = estimate_time_constants(y,p,sn,lags)
        % estimate time constants from the sample autocovariance function

        lags = lags + p;
        if ~isempty(which('xcov')) %signal processing toolbox
            xc = xcov(y,lags,'biased');
        else
            ynormed = (y - mean(y));
            xc = nan(lags + 1, 1);
            for k = 0:lags
                xc(k + 1) = ynormed(1 + k:end)' * ynormed(1:end - k);
            end
            xc = [flipud(xc(2:end)); xc] / numel(y);
        end
        xc = xc(:);
        A = toeplitz(xc(lags+(1:lags)),xc(lags+(1:p))) - sn^2*eye(lags,p);
        g = pinv(A)*xc(lags+2:end);
    end

    function [f,grad] = lagrangian_temporal_gradient(Al,thr,y_raw,bas_flag,c1_flag)
        options_qp = optimset('Display','Off','Algorithm','interior-point-convex');
        H = [speye(T),ones(T,bas_flag),gd_vec*ones(1,c1_flag);...
            ones(bas_flag,T),T*ones(bas_flag),(1-gd^T)/(1-gd)*ones(c1_flag,bas_flag);...
            (gd_vec*ones(1,c1_flag))',(1-gd^T)/(1-gd)*ones(c1_flag,bas_flag),(1-gd^(2*T))/(1-gd^2)*ones(c1_flag,c1_flag)];
        Ay = [y_raw;sum(y_raw)*ones(bas_flag);gd_vec'*y_raw*ones(c1_flag)];
        c = quadprog(2*Al(1)*H,[v;zeros(bas_flag+c1_flag,1)]-2*Al(1)*Ay,[-G,sparse(T,bas_flag+c1_flag);sparse(bas_flag+c1_flag,T),-speye(bas_flag+c1_flag)]...
            ,[sparse(T,1);-b_lb*ones(bas_flag);zeros(c1_flag)],[],[],[],[],c,options_qp);
        f = v'*c(1:T);
        grad = [sum((c(1:T)-y_raw + c(T+bas_flag)*bas_flag + c(end)*gd_vec*c1_flag).^2)-thr];
        f = f + Al(:)'*grad;
    end

    function b = G_inv_mat(x,mode,NT,gs,gd_vec,bas_flag,c1_flag,Emat)
        if mode == 1
            b = filter(1,[1;-gs(:)],x(1:NT)) + bas_flag*x(NT+bas_flag) + c1_flag*gd_vec*x(end);
            b = Emat*b;
           %b = G\x(1:NT) + x(NT+bas_flag)*bas_flag + x(end)*c1_flag;
        elseif mode == 2
            x = Emat'*x;
            b = [flipud(filter(1,[1;-gs(:)],flipud(x)));ones(bas_flag,1)*sum(x);ones(c1_flag,1)*(gd_vec'*x)];
           %b = [G'\x;ones(bas_flag,1)*sum(x);ones(c1_flag,1)*(gd_vec'*x)] ;
        end
    end

end


%% Low Level Parent Phase Reconstruction
%
%%
% In this section we discuss parent grain reconstruction at the example of
% a titanium alloy. Lets start by importing a sample data set

mtexdata alphaBetaTitanium

% the phase names for the alpha and beta phases
alphaName = 'Ti (alpha)';
betaName = 'Ti (Beta)';

% and plot the alpha phase as an inverse pole figure map
plot(ebsd(alphaName),ebsd(alphaName).orientations,'figSize','large')

%%
% The data set contains 99.8 percent alpha titanium and 0.2 percent beta
% titanium. Our goal is to reconstuct the original beta phase. The
% original grain structure appears almost visible for human eyes.
% Our computations will be based on the Burgers orientation relationship

beta2alpha = orientation.Burgers(ebsd(betaName).CS,ebsd(alphaName).CS)

%%
% that alligns (110) plane of the beta phase with the (0001) plane of the
% alpha phase and the [1-11] direction of the beta phase with the [2110]
% direction of the alpha phase.
%
% Note that all MTEX functions for parent grain reconstruction expect the
% orientation relationship as parent to child and not as child to parent.
%
%% Detecting triple points that belong to the same parent orientation
%
% In a first step we want to identify triple junctions that have
% misorientations that are compatible with a common parent orientations. To
% this end we first compute alpha grains using the option
% <QuadruplePoints.html |removeQuadruplePoints|> which turn all quadruple
% junctions into 2 triple junctions. Furthermore, we choose a very small
% threshold of 1.5 degree for the identification of grain boundaries to
% avoid alpha orientations that belong to different beta grains get merged
% into the same alpha grain.

% reconstruct grains
[grains,ebsd.grainId] = calcGrains(ebsd('indexed'),'threshold',1.5*degree,...
  'removeQuadruplePoints');
grains = smooth(grains,1,'moveTriplePoints');

% plot all alpha pixels
region = [299 401 -500 -440];
plot(ebsd(alphaName),ebsd(alphaName).orientations,...
  'region',region,'micronbar','off','figSize','large');

% and on top the grain boundaries
hold on
plot(grains.boundary,'linewidth',2 ,'region',region);
hold off

%%
% Above we have plotted only a very small subregion of the original data
% set to make the seperation of the qudruple junctions better visible.
%
% Next we extract all alpha - alpha - alpha triple junctions and use the
% command <calcParent.html |calcParent|> to find for each of these triple
% junctions the best fitting parent orientations.

% extract all alpha - alpha - alpha triple points
tP = grains.triplePoints(alphaName,alphaName,alphaName)

% compute for each triple point the best fitting parentId and how well the fit is
tPori = grains(tP.grainId).meanOrientation;
[parentId, fit] = calcParent(tPori,beta2alpha,'numFit',2,'id','threshold',5*degree);

%%
% The command |calcParent| returns for each child orientation a |parentId|
% which allows us later to compute the parent orientation from the child
% orientation. Furthermore, the command return for each triple junction the
% misfit between the adjecent parent orientations in radiant. Finally, the
% option |'numFit',2| causes |calcParent| to return not only the best fit
% but also the second best fit. This will be used later. First we simple
% colorize the triple junctions according to the best fit.

hold on
plot(tP,fit(:,1) ./ degree,'MarkerEdgecolor','k','MarkerSize',10,'region',region)
setColorRange([0,5])
mtexColorMap LaboTeX
mtexColorbar
hold off

%%
% Next we select those triple junctions as reliable that have a fit less
% than 2.5 degree and second best fit that is larger than  2.5 degree

consistenTP = fit(:,1) < 2.5*degree & fit(:,2) > 2.5*degree;

% marke these triple points by a red cicle
hold on
plot(tP(consistenTP),'MarkerEdgecolor','r','MarkerSize',10,...
  'MarkerFaceColor','none','linewidth',2,'region',region)
hold off

%% Recover beta grains from consistent triple junctions
%
% We observe that despite the quite sharp threshold we have many consistent
% triple points. In the next step we check wether all consistent triple
% junctions of a grain vote for the same parent orientation. Such a check
% for consistent votes can be computed by the command <majorityVote.html
% |majorityVote|> using the option |strict|.

% get a unique parentId vote for each grain
[parentId, numVotes] = majorityVote( tP(consistenTP).grainId, ...
  parentId(consistenTP,:,1), max(grains.id),'strict');

%%
% The command |majorityVote| returns for each grain with consistent
% parentId votes this unique parentId and for all other grains |NaN|.
% The second  output argument gives the number of these votes
%
% For all grains with at least 3 unique vote we now use the command
% <variants.html |variants|> to compute the parent orientation
% corresponding to the |parentId|. This parent orientations we assign as
% new |meanOrientation| to our grains.

% lets store the parent grains into a new variable
parentGrains = grains;

% change orientations of consistent grains from child to parent
parentGrains(numVotes>2).meanOrientation = ...
  variants(beta2alpha,grains(numVotes>2).meanOrientation,parentId(numVotes>2));

% update all grain properties that are related to the mean orientation
parentGrains = parentGrains.update;

%%
% Lets plot map of these reconstructed beta grains

% define a color key
ipfKey = ipfColorKey(ebsd(betaName));
ipfKey.inversePoleFigureDirection = vector3d.Y;

% and plot
plot(parentGrains(betaName), ...
  ipfKey.orientation2color(parentGrains(betaName).meanOrientation),'figSize','large')

%%
% We observe that this first step already results in very many Beta grains.
% However, the grain boundaries are still the boundaries of the original
% alpha grains. To overcome this, we merge all Beta grains that have a
% misorientation angle smaller then 2.5 degree.
%
% As an additional consistency check we verify that each parent
% grain has been reconstructed from at least 2 child grains. To this end we
% first make a testrun the merge operation and then revert all parent
% grains that that have less then two childs. This step may not nessesary
% in many case.

% test run of the merge operation
[~,parentId] = merge(parentGrains,'threshold',2.5*degree,'testRun');

% count the number of neighbouring child that would get merged with each child
counts = accumarray(parentId,1);

% revert all beta grains back to alpha grains if they would get merged with
% less then 1 other child grains
setBack = counts(parentId) < 2 & grains.phaseId == grains.name2id(alphaName);
parentGrains(setBack).meanOrientation = grains(setBack).meanOrientation;
parentGrains = parentGrains.update;

%%
% Now we perform the actual merge and the reconstruction of the parent
% grain boundaries.

% merge beta grains
[parentGrains,parentId] = merge(parentGrains,'threshold',2.5*degree);

% set up a EBSD map for the parent phase
parentEBSD = ebsd;

% and store there the grainIds of the parent grains
parentEBSD('indexed').grainId = parentId(ebsd('indexed').grainId);

plot(parentGrains(betaName), ...
  ipfKey.orientation2color(parentGrains(betaName).meanOrientation),'figSize','large')

%% Merge alpha grains to beta grains
%
% After the first two steps we have quite some alpha grains have not yet
% transformed into beta grains. In order to merge those left over alpha
% grains we check whether their misorientation with one of the neighbouring
% beta grains coincides with the parent to grain orientation relationship
% and if yes merge them evantually with the already reconstructed beta
% grains.
%
% First extract a list of all neighbouring alpha - beta grains

% all neighbouring alpha - beta grains
grainPairs = neighbors(parentGrains(alphaName), parentGrains(betaName));

%%
% and check how well they fit to a common parent orientation

% extract the corresponding meanorientations
oriAlpha = parentGrains( grainPairs(:,1) ).meanOrientation;
oriBeta = parentGrains( grainPairs(:,2) ).meanOrientation;

% compute for each alpha / beta pair of grains the best fitting parentId
[parentId, fit] = calcParent(oriAlpha,oriBeta,beta2alpha,'numFit',2,'id');

%%
% Similarly, as in the first step the command <calcParent.html
% |calcParent|> returns a list of |parentId| that allows the convert the
% child orientations into parent orientations using the command
% <variants.html |variants|> and the fitting to the given parent
% orientation. Similarly, as for the triple point we select only those
% alpha beta pairs such that the fit is below the threshold of 2.5 degree
% and at the same time the second best fit is above 2.5 degree.

% consistent pairs are those with a very small misfit
consistenPairs = fit(:,1) < 5*degree & fit(:,2) > 5*degree;

%%
% Next we compute for all alpha grains the majority vote of the surounding
% beta grains and change their orientation from alpha to beta

parentId = majorityVote( grainPairs(consistenPairs,1), ...
  parentId(consistenPairs,1), max(parentGrains.id));

% change grains from child to parent
hasVote = ~isnan(parentId);
parentGrains(hasVote).meanOrientation = ...
  variants(beta2alpha, parentGrains(hasVote).meanOrientation, parentId(hasVote));

% update grain boundaries
parentGrains = parentGrains.update;

% merge new beta grains into the old beta grains
[parentGrains,parentId] = merge(parentGrains,'threshold',5*degree);

% update grainId in the ebsd map
parentEBSD('indexed').grainId = parentId(parentEBSD('indexed').grainId);

% plot the result
color = ipfKey.orientation2color(parentGrains(betaName).meanOrientation);
plot(parentGrains(betaName),color,'linewidth',2)

%%
% The above step has merged

sum(hasVote)

%%
% alpha grains into the already reconstructed beta grain. This reduces the
% amount of grains not yet reconstructed to

sum(parentGrains('Ti (alpha').grainSize) ./ sum(parentGrains.grainSize)*100

%%
% percent. One way to proceed would be to repeat the steps of this section
% multiple time, maybe with increasing threshold, until the percentage of
% reconstructed beta grains is sufficiently high. Another approach in to
% consider the left over alpha grains as noise and use denoising techniques
% to replace them with beta orientations. This will be done in the last
% section.

%% Reconstruct beta orientations in EBSD map
%
% Until now we have only recovered the beta orientations as the mean
% orientations of the beta grains. In this section we want to set up the
% EBSD variable |parentEBSD| to contain for each pixel a reconstruction of
% the parent phase orientation.
%
% Therefore, we first identify all pixels that previously have been alpha
% titanium but now belong to a beta grain.

% consider only original alpha pixels that now belong to beta grains

isNowBeta = parentGrains.phaseId(max(1,parentEBSD.grainId)) == ebsd.name2id(betaName) &...
  parentEBSD.phaseId == ebsd.name2id(alphaName);

%%
% Next we can use once again the function <calcParent.html |calcParent|> to
% recover the original beta orientation from the measured alpha orientation
% giving the mean beta orientation of the grain.

% update beta orientation
[parentEBSD(isNowBeta).orientations, fit] = calcParent(parentEBSD(isNowBeta).orientations,...
  parentGrains(parentEBSD(isNowBeta).grainId).meanOrientation,beta2alpha);

%%
% We obtain even a measure |fit| for the corespondence between the beta
% orientation reconstructed for a single pixel and the beta orientation of
% the grain. Lets visualize this measure of fit

% the beta phase
plot(parentEBSD(isNowBeta),fit ./ degree,'figSize','large')
mtexColorbar
setColorRange([0,5])
mtexColorMap('LaboTeX')

hold on
plot(parentGrains.boundary,'lineWidth',2)
hold off

%%
% Lets finaly plot the reconstructed beta phase

plot(parentEBSD(betaName),ipfKey.orientation2color(parentEBSD(betaName).orientations),'figSize','large')

%% Denoising of the reconstructed beta phase
% As promised we end our discussion by applying denoising techniques to
% fill the remaining holes of alpha grains. To this end we first
% reconstruct grains from the parent orientations and throw away all small
% grains

[parentGrains,parentEBSD.grainId] = calcGrains(parentEBSD('indexed'),'angle',5*degree);

% remove all the small grains
parentEBSD = parentEBSD(parentGrains(parentGrains.grainSize > 15));

% redo grain reconstruction
[parentGrains,parentEBSD.grainId] = calcGrains(parentEBSD('indexed'),'angle',5*degree);

% smooth the grains a bit
parentGrains = smooth(parentGrains,5);

%%
% Finally, we denoise the remaining beta orientations and at the same time
% fill the empty holes. We choose a very small smoothing parameter |alpha|
% to keep as many details as possible.

F= halfQuadraticFilter;
F.alpha = 0.1;
parentEBSD = smooth(parentEBSD,F,'fill',parentGrains);

% plot the resulting beta phase
plot(parentEBSD(betaName),ipfKey.orientation2color(parentEBSD(betaName).orientations),'figSize','large')

hold on
plot(parentGrains.boundary,'lineWidth',3)
hold off

%%
% For comparison the map with original alpha phase and on top the recovered
% beta grain boundaries

plot(ebsd(alphaName),ebsd(alphaName).orientations,'figSize','large')

hold on
plot(parentGrains.boundary,'lineWidth',3)
hold off

%% Summary of relevant thresholds
%
% In parent grain reconstruction several parameters are involve are
% decicive for the success of the reconstruction
%
% * threshold for initial grain segmentation (1.5*degree)
% * maximum misfit at triple junctions (2.5 degree)
% * minimal misfit of the second best solution at triple junctions (2.5 degree)
% * minimum number of consistent votes (2)
% * threshold for merging beta grains (can be skipped)
% * threshold for merging alpha and beta grains (2.5 degree)
%%
%
% Visualize the misorientation to the mean reveals quite some fine
% structure in the reconstructed parent orientations.

cKey = axisAngleColorKey;
color = cKey.orientation2color(parentEBSD(betaName).orientations, parentGrains(parentEBSD(betaName).grainId).meanOrientation);
plot(parentEBSD(betaName),color)

hold on
plot(parentGrains.boundary,'lineWidth',3)
hold off


function res = spm_eeg_specest_hilbert(S, data, time)
% Plugin for spm_eeg_tf implementing spectral estimation using Hilbert transform
% FORMAT res = spm_eeg_specest_hilbert(S, data, time)
%
% S                     - input structure
% fields of S:
%    S.subsample   - factor by which to subsample the time axis (default - 1)
%    S.freqres     - frequency resolutions (plus-minus for each frequency, can
%                    be a vector with a value per frequency)
%    S.frequencies - vector of frequencies
%    S.order       - butterworth filter order (can be a vector with a value
%                    per frequency)
%
% Output:
%  res -
%   If no input is provided the plugin returns a cfg branch for itself
%
%   If input is provided:
%      res.fourier - the complex output of wavelet transform
%      res.time    - time axis
%      res.freq    - frequency axis
%______________________________________________________________________________________
% Copyright (C) 2010 Wellcome Trust Centre for Neuroimaging

% Vladimir Litvak based on the code contributed by Krish Singh
% $Id: spm_eeg_specest_hilbert.m 4463 2011-09-06 10:53:01Z vladimir $

%-This part if for creating a config branch that plugs into spm_cfg_eeg_tf
% Any parameters can be specified and they are then passed to the plugin
% when it's called.
%--------------------------------------------------------------------------
if nargin == 0
    freqres = cfg_entry;
    freqres.tag = 'freqres';
    freqres.name = 'Frequency resolution';
    freqres.strtype = 'r';
    freqres.num = [1 Inf];
    freqres.val = {0.5};
    freqres.help = {'Frequency resolution.',...
        'Note: 1 Hz resolution means plus-minus 1 Hz, i.e. 2 Hz badwidth',...
        'Either a single value or a vector of the same length as frequencies can be input'};

    typ = cfg_menu;
    typ.tag = 'type';
    typ.name = 'Filter type';
    typ.labels = {'Butterworth', 'FIR'};
    typ.values = {'but', 'fir'};
    typ.val = {'but'};
    typ.help = {'Select the filter type.'};

    dir = cfg_menu;
    dir.tag = 'dir';
    dir.name = 'Filter direction';
    dir.labels = {'Zero phase', 'Forward', 'Backward'};
    dir.values = {'twopass', 'onepass', 'onepass-reverse'};
    dir.val = {'twopass'};
    dir.help = {'Select the filter direction.'};

    order = cfg_entry;
    order.tag = 'order';
    order.name = 'Filter order';
    order.strtype = 'n';
    order.num = [1 Inf];
    order.val = {3};
    order.help = {'Butterworth filter order',...
        'Either a single value or a vector of the same length as frequencies can be input'};

    flt = cfg_branch;
    flt.tag = 'filter';
    flt.name = 'Filter';
    flt.val = {typ dir order};

    polyorder = cfg_menu;
    polyorder.tag = 'polyorder';
    polyorder.name = 'Polynomial trend removal order';
    polyorder.labels = {'0', '1', '2', '3'};
    polyorder.values = {0, 1, 2, 3};
    polyorder.val = {1};
    polyorder.help = {'Order of polynome for trend removal (0 for no detrending)'};

    subsample = cfg_entry;
    subsample.tag = 'subsample';
    subsample.name = 'Subsample';
    subsample.strtype = 'n';
    subsample.num = [1 1];
    subsample.val = {1};
    subsample.help = {'Set to N to subsample the time axis to every Nth sample (to reduce the dataset size).'};

    hilbert = cfg_branch;
    hilbert.tag = 'hilbert';
    hilbert.name = 'Hilbert transform';
    hilbert.val = {freqres, flt, polyorder, subsample};

    res = hilbert;

    return
elseif nargin < 3
    error('Three input arguments are required');
end

%-Defaults
%--------------------------------------------------------------------------
if ~isfield(S, 'subsample')
    S.subsample = 1;
end

dt = time(end) - time(1);

if ~isfield(S, 'frequencies') || isempty(S.frequencies)
    S.frequencies = (1/dt):max(1/dt, floor(dt)/dt):48;
end

if ~isfield(S, 'freqres')
    S.freqres = max(1/dt, floor(dt)/dt);
end

%-Data dimensions
%--------------------------------------------------------------------------
[res.fourier, res.freq, res.time] = ft_specest_hilbert(data, time, 'freqoi', S.frequencies, 'timeoi', time(1:S.subsample:end), 'width', S.freqres,...
    'filttype', S.filter.type, 'filtorder', S.filter.order, 'filtdir', S.filter.dir, 'polyremoval', S.polyorder, 'verbose', 0);


function varargout = grErode(varargin)
%GRERODE Morphological erosion on graph.
%
%   LBL2 = grErode(EDGES, LBL1)
%   Each label of the graph is assigned the smallest label of its
%   neighbours, or it keeps the same label this one is smaller.
%
%   Example
%   grErode
%
%   See also
%   grDilate, grOpen, grClose

% ------
% Author: David Legland
% E-mail: david.legland@grignon.inra.fr
% Created: 2006-01-20
% Copyright 2006-2022 INRA - CEPIA Nantes - MIAJ (Jouy-en-Josas)

if length(varargin) == 2
    edges   = varargin{1};
    lbl     = varargin{2};
elseif length(varargin) == 3
    edges   = varargin{2};
    lbl     = varargin{3};
else
    error('Wrong number of arguments in "grErode"');
end

lbl2 = zeros(size(lbl));

uni = unique(edges(:));
for n = 1:length(uni)
    neigh = grAdjacentNodes(edges, uni(n));
    lbl2(uni(n)) = min(lbl([uni(n); neigh]));
end

varargout{1} = lbl2;


function F = structBlkDiag(S,field)
% F = structBlkDiag(S,field)
%
% This function creates a block diagional matrix from a field of a struct
% array
%

n = length(S);
cmd = ['F' ' = blkdiag('];
for i=1:n
   cmd = [cmd 'S(' num2str(i) ').' field]; %#ok<AGROW>
   if i < n
      cmd = [cmd ', ']; %#ok<AGROW>
   else
       cmd = [cmd ');'];%#ok<AGROW>
   end
end

eval(cmd);

end


function hb_io_test08 ( )

%*****************************************************************************80
%
%% HB_IO_TEST08 tests HB_RHS_WRITE, HB_GUESS_WRITE, HB_EXACT_WRITE;
%
  neltvl = 0;
  nnzero = 126;
  nrhs = 1;
  nrhsix = 0;
  nrow = 32;

  exact = [ ...
    1.0,   2.0,   3.0,   4.0,   5.0,   6.0,   7.0,   8.0,   9.0,  10.0, ...
   11.0,  12.0,  13.0,  14.0,  15.0,  16.0,  17.0,  18.0,  19.0,  20.0, ...
   21.0,  22.0,  23.0,  24.0,  25.0,  26.0,  27.0,  28.0,  29.0,  30.0, ...
   31.0,  32.0 ]';
  guess = [ ...
    1.0,   1.0,   1.0,   1.0,   1.0,   1.0,   1.0,   1.0,   1.0,   1.0, ...
    1.0,   1.0,   1.0,   1.0,   1.0,   1.0,   1.0,   1.0,   1.0,   1.0, ...
    1.0,   1.0,   1.0,   1.0,   1.0,   1.0,   1.0,   1.0,   1.0,   1.0, ...
    1.0,   1.0 ]';
  indfmt = '(16I5)';
  mxtype = 'RUA';
  output_file = 'rua_32_rhs.txt';
  ptrfmt = '(16I5)';
  rhscrd = 12;
  rhsfmt = '(10F7.1)';
  rhsind = 0;
  rhsptr = 0;
  rhsval = [ ...
    101.0, 102.0, 103.0, 104.0, 107.0, 126.0, 201.0, 202.0, 209.0, 221.0, ...
    228.0, 302.0, 303.0, 306.0, 308.0, 309.0, 329.0, 403.0, 404.0, 405.0, ...
    412.0, 503.0, 505.0, 523.0, 527.0, 601.0, 606.0, 616.0, 703.0, 707.0, ...
    714.0, 721.0 ]';
  rhsvec = 0;
  rhstyp = 'FGX';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'HB_IO_TEST08\n' );
  fprintf ( 1, '  HB_RHS_WRITE writes right hand sides to an HB file.\n' );
  fprintf ( 1, '  HB_GUESS_WRITE writes starting guesses to an HB file.\n' );
  fprintf ( 1, '  HB_EXACT_WRITE writes exact solutions to an HB file.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Writing the file "%s".\n', output_file );

  output_unit = fopen (output_file, 'wt' );

  if ( output_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'HB_IO_TEST08 - Fatal error!\n' );
    fprintf ( 1, '  Error opening the file.\n' );
    return;
  end
%
%  Write the right hand sides.
%
  hb_rhs_write ( output_unit, nrow, nnzero, nrhs, nrhsix, ...
    rhscrd, ptrfmt, indfmt, rhsfmt, mxtype, rhstyp, rhsval, ...
    rhsind, rhsptr, rhsvec );
%
%  Write the right hand sides.
%
  hb_guess_write ( output_unit, nrow, nrhs, rhscrd, rhsfmt, rhstyp, ...
    guess );
%
%  Write the right hand sides.
%
  hb_exact_write ( output_unit, nrow, nrhs, rhscrd, rhsfmt, rhstyp, ...
    exact );

  fclose ( output_unit );

  return
end


function Gamma = initGamma_random(T,K,D,Pstructure,Pistructure,episodic,priorOFFvsON)
% Return a Gamma timecourse corresponding to random transitions with a given
% dirichletdiag (i.e. the transition probability prior)
% For ehmm , D must be a 2x2 matrix of prior transitions - or left
%   empty
%   first state is ON, second is OFF  (always start in OFF)
%
% Author: Romesh Abeysuriya, University of Oxford (2017)
%         Diego Vidaurre, Aarhus University (2020)

if nargin < 4, Pstructure = []; end
if nargin < 5, Pistructure = []; end
if nargin < 6, episodic = false; end
if nargin < 7, priorOFFvsON = 3; end

rng('default')
rng('shuffle') % make this "truly" random

% Form transition probability matrix
if episodic
    P = (D-1)*eye(2)+ones(2);
    P(:,2) = P(:,2) * priorOFFvsON;
    P = bsxfun(@rdivide,P,sum(P,2));
    Pi = [0 1];
    if ~isempty(Pstructure) || ~isempty(Pistructure)
       warning('initGamma_random: Pstructure and Postructure will be ignored')
    end
else
    P = (D-1)*eye(K)+ones(K);
    if nargin>=4, P(~Pstructure) = 0; end
    if nargin<5, Pistructure = true(1,K); end
    P = bsxfun(@rdivide,P,sum(P,2));
    Pi = zeros(1,K);
    Pi(Pistructure) = 1;
    Pi = Pi / sum(Pi);
end

% Preallocate
Gamma = zeros(sum(T),K);

if episodic
    for n = 1:length(T)
        for k = 1:K
            while true
                gamma = zeros(T(n),2);
                gamma(1,2) = 1;
                for t = 2:T(n)
                    gamma(t,:) = mnrnd(1,P(gamma(t-1,:)==1,:));
                end
                if all(sum(gamma)>0), break; end
            end
            gamma = gamma + 0.0001 * rand(T(n),2);
            gamma = gamma ./ repmat(sum(gamma,2),1,2);
            t0 = sum(T(1:n-1)); t1 = sum(T(1:n));
            Gamma(t0+1:t1,k) = gamma(:,1);
        end
    end
elseif isChain(P) % If it is a chain, it P must be ordered
    for n = 1:length(T)
        ch = cumsum(rand(1,K));
        ch = ch / ch(end);
        ch = round(T(n) * ch);
        ch = [0 ch(1:end-1)];
        gamma = zeros(T(n),K);
        for k = 1:K-1, gamma(ch(k)+1:ch(k+1),k) = 1; end
        gamma(ch(k+1)+1:end,K) = 1;
        t0 = sum(T(1:n-1)); t1 = sum(T(1:n));
        Gamma(t0+1:t1,:) = gamma;
    end
else
    for n = 1:length(T)
        gamma = zeros(T(n),K);
        if sum(Pi>0)==1, gamma(1,Pi>0) = 1;
        else, gamma(1,:) = mnrnd(1,Pi);
        end
        for t = 2:T(n)
            gamma(t,:) = mnrnd(1,P(gamma(t-1,:)==1,:));
        end
        gamma = gamma + 0.0001 * rand(T(n),K);
        t0 = sum(T(1:n-1)); t1 = sum(T(1:n));
        Gamma(t0+1:t1,:) = gamma ./ repmat(sum(gamma,2),1,K);
    end
end

end

function bool = isChain(P)
%bool = any(sum(P,2)==1);
P = P~=0;
bool = all(sum(P(1:end-1,:),2)==2) && sum(P(end,:))==1;
end


% LCM - Variational Bayesian inference for latent component models.

% Likelihood: p(Y|A,S,C) = N(Y|A*S,C)
%
% Priors p(A) and p(S) can be arbitrary, because the user must specify
% the posterior estimation function.
%
% Covariance could be a Kronecker product of two arbitrary (or diagonal?)
% covariance matrices C_a and C_s? Missing values cause a nasty problem
% to Kronecker-covariance.. :(
%
% What if also an arbitrary diagonal covariance matrix C_d? The full
% joint covariance matrix would be:
%
% C = sqrt(C_d) * kron(C_a,C_s) * sqrt(C_d)
%
% Then missing values could be considered by using large (infinite)
% values in C_d..?

% Last modified 2010-10-12
% Copyright (c) Jaakko Luttinen (jaakko.luttinen@tkk.fi)

function Q = lcm(Y, D, varargin)

[M,N] = size(Y);

options = struct( ...
    'init',         [],               ...
    'prior',        [],               ...
    'common_tau',   true,             ...
    'common_nu',    false,            ...
    'rotate',       1,                ...
    'update_alpha', 1,               ...
    'update_beta',  1,               ...
    'update_nu',    1,               ...
    'autosavetime', nan,              ...
    'autosavefile', 'vbrfa_autosave', ...
    'maxiter',      100,              ...
    'robustness',   'independent-t', ... % none / multivariate-t / independent-t
    'user_data',    []);

% Parse arguments
[options, errmsg] = argparse( options, varargin{:} );
error(errmsg);

% Missing values
Obs = ~isnan(Y);
[ObsM, ObsN] = find(Obs);

% Helpful variables
Id = speye(D);
log2pi = log(2*pi);

logP = -inf;
loglikelihood = nan*zeros(options.maxiter,1);
start_time = cputime;
time = nan*zeros(options.maxiter,1);

lastsave = now;

for iter=1:options.maxiter

  t = cputime;

  %%%%%%%%%%%%%%%%%%%%%%
  %% Update parameters

  %%%%%%%%%%%%%%%%%
  %
  % X BLOCK
  %
  %%%%%%%%%%%%%%%%%

  %% UPDATE X

  % q(X)
  [X, XX] =
  for n=1:N
    Mmv = Obs(:,n);
% $$$     UTau = U(Mmv,n).*tau(Mmv);
% $$$     UWWTau = sum(bsxfun(@times, ...
% $$$                         reshape(U(Mmv,n),[1,1,sum(Mmv)]), ...
% $$$                         WtauW(:,:,Mmv)), ...
% $$$                  3);

    % Distribution
    UWtauW = wsum(WtauW(:,:,Mmv),U(Mmv,n),3);
    CovX(:,:,n) = inv( speye(D) + UWtauW );
    X(:,n) = CovX(:,:,n) * ...
             ( tauW(Mmv,:)'*spdiag(Y(Mmv,n)) - WtauMu(Mmv,:)' ) * U(Mmv,n);

    % Expectations
    XX(:,:,n) = CovX(:,:,n) + X(:,n)*X(:,n)';
  end

  % KL-term: <log q(X)> - <log p(X)>
  KL_X = 0;
  for n=1:N
    KL_X = KL_X - gaussian_entropy(logdet_cov(CovX(:,:,n)), D);
    KL_X = KL_X - gaussian_logpdf(trace(XX(:,:,n)), ...
                                  0, ...
                                  0, ...
                                  0, ...
                                  D);
  end

  %%%%%%%%%%%%%%%%%
  %
  % W BLOCK
  %
  %%%%%%%%%%%%%%%%%

  %
  % Update distributions and evaluate statistics
  %

  % Update q(W,Mu|tau)
  for m=1:M
    Nmv = Obs(m,:);

    UXX = wsum(XX(:,:,Nmv),U(m,Nmv),3);
    UX = X(:,Nmv)*U(m,Nmv)';
    UY = U(m,Nmv).*Y(m,Nmv);
    UYX = [UY*X(:,Nmv)', sum(UY)];

    % Distribution
    CovWMu(:,:,m) = inv(spdiag([alpha; beta]) + [UXX, UX; UX', sum(U(m,Nmv))]);
    W(m,:) = UYX*CovWMu(:,1:D,m);
    Mu(m) = UYX*CovWMu(:,end,m);
  end

  % Update q(tau)
  for m=1:M
    Nmv = Obs(m,:);
    a_tau(m) = p.a_tau + 0.5 * sum(Nmv);
    b_tau(m) = p.b_tau ...
        + 0.5 * (U(m,Nmv).*Y(m,Nmv))*Y(m,Nmv)' ...
        - 0.5 * [W(m,:),Mu(m)] * inv(CovWMu(:,:,m)) * [W(m,:),Mu(m)]';
  end
  if options.common_tau
    % Common tau
    a_tau(:) = p.a_tau + sum(a_tau-p.a_tau);
    b_tau(:) = p.b_tau + sum(b_tau-p.b_tau);
  end
  tau = a_tau./b_tau;
  logtau = psi(a_tau) - log(b_tau);
  for m=1:M
    % Expectations
    tauW(m,:) = tau(m) * W(m,:);
    tauMu(m) = tau(m) * Mu(m);
    WtauW(:,:,m) = tau(m)*W(m,:)'*W(m,:) + CovWMu(1:D,1:D,m);
    MutauMu(m) = tau(m)*Mu(m)^2 + CovWMu(end,end,m);
    WtauMu(m,:) = tau(m)*Mu(m)*W(m,:) + CovWMu(end,1:D,m);
  end

  % Update q(alpha)
  if (numel(options.update_alpha)==1 && iter >= options.update_alpha) || ...
        any(iter==options.update_alpha)
    a_alpha(:) = p.a_alpha + 0.5*M;
    b_alpha(:) = p.b_alpha + 0.5*diag(sum(WtauW, 3));
  end
  alpha = a_alpha ./ b_alpha;
  logalpha = psi(a_alpha) - log(b_alpha);

  % Update q(beta)
  if (numel(options.update_beta)==1 && iter >= options.update_beta) || ...
        any(iter==options.update_beta)
    a_beta = p.a_beta + 0.5*M;
    b_beta = p.b_beta + 0.5*sum(MutauMu);
  end
  beta = a_beta ./ b_beta;
  logbeta = psi(a_beta) - log(b_beta);

  %
  % Compute KL terms
  %

  % KL-term: <log q(W,Mu|tau)> - <log p(W|tau,alpha)> - <log p(Mu|tau,beta)>
  KL_WMu = 0;
  for m=1:M
    KL_WMu = KL_WMu - gaussian_entropy(-(D+1)*logtau(m) ...
                                       + logdet_cov(CovWMu(:,:,m)), ...
                                       D);
    KL_WMu = KL_WMu - gaussian_logpdf(diag(WtauW(:,:,m))'*alpha, ...
                                      0, ...
                                      0, ...
                                      -D*logtau(m) - sum(logalpha), ...
                                      D);
    KL_WMu = KL_WMu - gaussian_logpdf(diag(MutauMu(m))'*beta, ...
                                      0, ...
                                      0, ...
                                      -logtau(m) - sum(logbeta), ...
                                      1);
  end
% $$$   KL_W = 0;
% $$$   for m=1:M
% $$$     KL_W = KL_W - gaussian_entropy(-D*logtau(m) + logdet_cov(CovW(:,:,m)), D);
% $$$     KL_W = KL_W - gaussian_logpdf(diag(WtauW(:,:,m))'*w, ...
% $$$                                   0, ...
% $$$                                   0, ...
% $$$                                   -D*logtau(m) - sum(logw), ...
% $$$                                   D);
% $$$   end

  % KL-term: <log q(tau)> - <log p(tau)>
  KL_tau = 0;
  if options.common_tau
    KL_tau = KL_tau - gamma_entropy(a_tau(1), b_tau(1));
    KL_tau = KL_tau - gamma_logpdf(tau(1),logtau(1),p.a_tau,p.b_tau);
  else
    KL_tau = KL_tau - sum(gamma_entropy(a_tau, b_tau));
    KL_tau = KL_tau - sum(gamma_logpdf(tau,logtau,p.a_tau,p.b_tau));
  end

  % KL-term: <log q(alpha)> - <log p(alpha)>
  KL_alpha = 0;
  KL_alpha = KL_alpha - sum(gamma_entropy(a_alpha, b_alpha));
  KL_alpha = KL_alpha - sum(gamma_logpdf(alpha, logalpha, p.a_alpha, p.b_alpha));

  % KL-term: <log q(beta)> - <log p(beta)>
  KL_beta = 0;
  KL_beta = KL_beta - sum(gamma_entropy(a_beta, b_beta));
  KL_beta = KL_beta - sum(gamma_logpdf(beta, logbeta, p.a_beta, p.b_beta));

  %%%%%%%%%%%%%%%%%
  %
  % NOISE BLOCK
  %
  %%%%%%%%%%%%%%%%%

  %% UPDATE U AND NU
  switch options.robustness

   case 'none'
    %% Normal PCA / FA
    U(Obs) = 1;
    logU(Obs) = 0;
    nu(:) = inf;
    KL_U = 0;

   case 'multivariate-t'
    %% Joint Student-t for the dimensions (Archambaeu et al), except X
    %% independent of U, i.e., p(X) is Gaussian.

    % Evaluate squared errors
    for m=1:M
      Nmv = Obs(m,:);

      E2(m,Nmv) = tau(m)*Y(m,Nmv).^2 ...
          + mtraceprod(WtauW(:,:,m), XX(:,:,Nmv)) ...
          + MutauMu(m) ...
          - 2*Y(m,Nmv).*(tauW(m,:)*X(:,Nmv)) ...
          - 2*Y(m,Nmv).*tauMu(m) ...
          + 2*WtauMu(m,:)*X(:,Nmv);
    end

    % Update nu
    if (numel(options.update_nu)==1 && iter >= options.update_nu) ...
          || any(iter==options.update_nu)

      % Common nu
      nu(:) = t_ml(sum(E2,1), nu(1), sum(Obs,1));
 %     disp('Update nu');
    end

    % Update U
    a_U(1,:) = (nu(1) + sum(Obs,1)) / 2;
    b_U(1,:) = (nu(1) + sum(E2,1)) / 2;
    a_U(:,:) = repmat(a_U(1,:), [M,1]);
    b_U(:,:) = repmat(b_U(1,:), [M,1]);
    U(Obs) = a_U(Obs)./b_U(Obs);
    logU(Obs) = psi(a_U(Obs)) - log(b_U(Obs));

    % KL-term: <log q(U)> - <log p(U)>
    KL_U = 0;
    Nmv = sum(Obs,1) > 0;
    KL_U = KL_U - sum(gamma_entropy(a_U(1,Nmv),b_U(1,Nmv)));
    KL_U = KL_U - sum(gamma_logpdf(U(1,Nmv),logU(1,Nmv),nu(1)/2,nu(1)/2));

   case 'independent-t'
    %% Independent Student-t for each dimension (Luttinen et al)

    % Evaluate squared errors (this costs A LOT!)
    for m=1:M
      Nmv = Obs(m,:);

      E2(m,Nmv) = tau(m)*Y(m,Nmv).^2 ...
          + mtraceprod(WtauW(:,:,m), XX(:,:,Nmv)) ...
          + MutauMu(m) ...
          - 2*Y(m,Nmv).*(tauW(m,:)*X(:,Nmv)) ...
          - 2*Y(m,Nmv).*tauMu(m) ...
          + 2*WtauMu(m,:)*X(:,Nmv);
    end

    % Update nu
    if (numel(options.update_nu)==1 && iter >= options.update_nu) ...
          || any(iter==options.update_nu)
      if options.common_nu
        % Common nu
        nu(:) = t_ml(E2(Obs), nu(1));
      else
        for m=1:M
          % Separate nu
          if true
            % Type II ML
            nu(m) = t_ml(E2(m,Obs(m,:)), nu(m));
          else
            % EM ML
            nv = Obs(m,:);
            func = @(lognu) (-1-log(exp(lognu)/2)+psi(exp(lognu)/2)- ...
                             mean(logU(m,nv)-U(m,nv)));
            lognu = fzero(func, log(nu(m)));
            nu(m) = exp(lognu);
          end
        end
      end
%      disp('Update nu');
    end

    % Update U
    for m=1:M
      Nmv = Obs(m,:);
      a_U(m,Nmv) = (nu(m)+1) / 2;
      b_U(m,Nmv) = (nu(m) + E2(m,Nmv)) / 2;
% $$$     b_U(m,Nmv) = (nu(m) + E2(m,Nmv)*tau(m)) / 2;
    end
    U(Obs) = a_U(Obs)./b_U(Obs);
    logU(Obs) = psi(a_U(Obs)) - log(b_U(Obs));

    % KL-term: <log q(U)> - <log p(U)>
    KL_U = 0;
    KL_U = KL_U - sum(gamma_entropy(a_U(Obs),b_U(Obs)));
    for m=1:M
      Nmv = Obs(m,:);
      KL_U = KL_U - sum(gamma_logpdf(U(m,Nmv),logU(m,Nmv),nu(m)/2,nu(m)/2));
    end

  end

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  %% Calculate lower bound of the log-likelihood

  monitor_loglikelihood = true;

  if monitor_loglikelihood

    % Cost from Y

    % We can take advantage of the linearity of gaussian_logpdf by summing
    % before the function call:
    tauYh = bsxfun(@plus, tauW*X, tauMu);
    y_invCov_y = (tau(ObsM).*U(Obs))'*(Y(Obs).^2);
    y_invCov_mu = U(Obs)'*(Y(Obs).*tauYh(Obs));
    mu_invCov_mu = 0;
    for m=1:M
      Nmv = Obs(m,:);
      mu_invCov_mu = mu_invCov_mu ...
          + traceprod(WtauW(:,:,m),wsum(XX(:,:,Nmv),U(m,Nmv),3)) ...
          + sum(U(m,Nmv)) * MutauMu(m) ...
          + 2*WtauMu(m,:)*(X(:,Nmv)*U(m,Nmv)');
    end
    logdet_Cov = - sum(logtau(ObsM)) - sum(logU(Obs));
    dim_y = sum(Obs(:));

    logpdf_Y = gaussian_logpdf(y_invCov_y, ...
                               y_invCov_mu, ...
                               mu_invCov_mu, ...
                               logdet_Cov, ...
                               dim_y);

    old_logP = logP;

    %    logpdf_Y

% $$$     KL_U
% $$$     KL_X
% $$$     KL_tau
% $$$     KL_W
% $$$     KL_w
    logP = logpdf_Y - KL_U - KL_X - KL_tau - KL_WMu - KL_alpha - KL_beta;

% $$$     % DEBUG STUFF
% $$$     kl_Y(iter) = logpdf_Y;
% $$$     kl_U(iter) = KL_U;
% $$$     kl_X(iter) = KL_X;
% $$$     kl_W(iter) = KL_W;
% $$$     kl_tau(iter) = KL_tau;
% $$$     kl_w(iter) = KL_w;

    loglikelihood(iter) = logP;
  else
    loglikelihood(iter) = nan;
  end

  time(iter) = cputime - start_time;

  % Debugging: Check that the bound really improves
  if iter > 1 && loglikelihood(iter) < loglikelihood(iter-1)
    logP_diff = (loglikelihood(iter-1)-loglikelihood(iter))/ ...
        abs(loglikelihood(iter-1));
    warmsg = sprintf(['Lower bound decreased %.2e percents. Bug or numerical ' ...
                      'inaccuracy?'], logP_diff);
    warning(warmsg);
  end

  %%%%%%%%%%%%%%%%%%%%%%%%%%%
  %% Monitoring stuff

  % Show progress
  fprintf('Step %d: loglikelihood=%e (%.3f seconds)\n', ...
          iter, loglikelihood(iter), cputime-t);

  % Check whether to save the results
  if (now-lastsave)*3600*24 >= options.autosavetime
    fprintf('Saving to %s...', options.autosavefile);
    save(options.autosavefile, 'W','CovW','w','a_w','b_w','X','CovX', ...
         'tau', 'a_tau','b_tau','U','a_U','b_U','nu', 'loglikelihood', ...
         'time', 'iter', 'options');
    lastsave = now;
    fprintf(' done.\n');
  end

  % Transformations for speeding up
  if (numel(options.rotate)==1 && iter>=options.rotate && options.rotate>=1) ...
        || any(iter==options.rotate)
    %orthogonalize();
    rotation2();
  end

end

% $$$ figure
% $$$ kl = [kl_Y(:), kl_U(:), kl_X(:), kl_W(:), kl_tau(:), kl_w(:)];
% $$$ kl = kl(6:end,:);
% $$$ kl = eminus(kl,mean(kl,1));
% $$$ plot(kl)

% Results as a struct
Q.W = W;
Q.CovWMu = CovWMu;
Q.alpha = alpha;
Q.a_alpha = a_alpha;
Q.b_alpha = b_alpha;
Q.Mu = Mu;
Q.beta = beta;
Q.a_beta = beta;
Q.b_beta = b_beta;
Q.X = X;
Q.CovX = CovX;
Q.tau = tau;
Q.a_tau = a_tau;
Q.b_tau = b_tau;
Q.U = U;
Q.a_U = a_U;
Q.b_U = b_U;
Q.nu = nu;
Q.loglikelihood = loglikelihood(1:iter);
Q.time = time(1:iter);
Q.iter = iter;
Q.options = options;

%%%%%%%%%%%%%%%%%%%%%%
%% NESTED FUNCTIONS

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function rotation2()

%alpha_before = alpha

% Rotates W and X, and moves bias from X to Mu.

%Y0 = bsxfun(@plus, W*X, Mu);

sumX = sum(X,2);
sumXX = [sum(XX,3), sumX; sumX', N]; % extended model: [X;1]*[X;1]'
sumWtauW = sum(WtauW,3);
sumMutauMu = sum(MutauMu);
sumWtauMu = sum(WtauMu,1);
sumWMutauWMu = [sumWtauW, sumWtauMu'; sumWtauMu, sumMutauMu];

%diag_wtauw = diag(sum(WtauW,3))

%
% Optimize the rotation
%

R0 = eye([D,D+1]);
%mycheckgrad(@cost, R0(:), 1e-6);
r = minimize(R0(:), @cost, 50);
%r(:) = randn(size(r)); % DEBUG !!!
R = [reshape(r,[D,D+1]); zeros(1,D), 1];
%R
%R = eye(D+1);% DEBUG!!!
invR = inv(R);

%
% Apply the rotation
%

% W and Mu
tauWMu = [tauW,tauMu]*R;
tauW = tauWMu(:,1:D);
tauMu = tauWMu(:,end);
for m=1:M
  Z = [WtauW(:,:,m), WtauMu(m,:)'; WtauMu(m,:), MutauMu(m)];
  % Expectations
  WtauW(:,:,m) = R(:,1:D)'*Z*R(:,1:D);
  MutauMu(m) = R(:,end)'*Z*R(:,end);
  WtauMu(m,:) = R(:,end)'*Z*R(:,1:D);
end

% X
X = invR(1:D,:)*[X;ones(1,N)];
for n=1:N
  XX(:,:,n) = invR(1:D,:)*[XX(:,:,n),X(:,n);X(:,n)',1]*invR(1:D,:)';
end

% alpha and beta
%diag_wtauw = diag(sum(WtauW,3))
b_alpha = p.b_alpha + 0.5*diag(sum(WtauW,3));
alpha = a_alpha ./ b_alpha;
b_beta = p.b_beta + 0.5*sum(MutauMu);
beta = a_beta ./ b_beta;

%alpha_after = alpha

%meanX = mean(X,2)
%sumXX = sum(XX,3) / N
%sumWW = sum(WtauW,3) / M

%Y1 = bsxfun(@plus, W*X, Mu);

%difference = sqrt(mean((Y1(:)-Y0(:)).^2))

%error('jou')

  function [f,df] = cost(r)

  R = [reshape(r,[D,D+1]); zeros(1,D), 1];
  R(end) = 1;

  invR = inv(R);
  invRR = inv(R*R');
  logdetR = logdet(R);

  % Transform W and Mu
  WW_R = sumWMutauWMu*R;
  R_WW_R = R'*WW_R;

  % Transform q(alpha) and q(beta)
  % (concatenate alpha and beta to one vector for simplicity)
  z = diag(R_WW_R);
  r_b_alpha = [repmat(p.b_alpha,D,1);p.b_beta] + 0.5*z;
  r_alpha = [a_alpha;a_beta] ./ r_b_alpha;
  r_logalpha = - log(r_b_alpha); % up to a constant
  grad_b = WW_R;
  grad_logb = grad_b * diag(1./r_b_alpha);
  grad_logalpha = -grad_logb;
  grad_alpha = grad_logalpha * diag(r_alpha);

  % Transform X

  % Cost from W and Mu
  %
  % TODO: There is something wrong with this!!
  logdet_Cov = 2*M*logdetR;
  x_invCov0_x = traceprod(spdiag(r_alpha),R_WW_R);
  x_invCov0_mu = 0;
  mu_invCov0_mu = 0;
  logdet_Cov0 = -M * sum(r_logalpha);
  grad_logdet_Cov = 2*M*invR';
  grad_x_invCov0_x = 2*WW_R*spdiag(r_alpha) ...
      + grad_alpha * diag(diag(R_WW_R));
  grad_x_invCov0_mu = 0;
  grad_mu_invCov0_mu = 0;
  grad_logdet_Cov0 = -M * grad_logalpha;
  [KL_W,dKL_W] = vb_rotationcost_gaussian(logdet_Cov, ...
                                          x_invCov0_x, ...
                                          x_invCov0_mu, ...
                                          mu_invCov0_mu, ...
                                          logdet_Cov0, ...
                                          grad_logdet_Cov, ...
                                          grad_x_invCov0_x, ...
                                          grad_x_invCov0_mu, ...
                                          grad_mu_invCov0_mu, ...
                                          grad_logdet_Cov0);
  %KL_W = 0;
  %dKL_W = 0;

  % Cost from alpha and beta
  prior_a = [repmat(p.a_alpha,D,1);p.a_beta];
  prior_b = [repmat(p.b_alpha,D,1);p.b_beta];
  post_a = [a_alpha;a_beta];
  A0_logB = prior_a'*log(r_b_alpha);
  A_B0_invB = prior_b'*r_alpha;
  grad_A0_logB = grad_logb * diag(prior_a);
  grad_A_B0_invB = grad_logalpha * diag(prior_b);
  [KL_alpha,dKL_alpha] = vb_rotationcost_gamma(A0_logB,      ...
                                               A_B0_invB,    ...
                                               grad_A0_logB, ...
                                               grad_A_B0_invB);
  %KL_alpha = 0;
  %dKL_alpha = 0;

  % Cost from X
  logdet_Cov = -2*N*logdetR;
  x_invCov0_x = traceprod(invR(1:D,:)'*invR(1:D,:),sumXX);
  x_invCov0_mu = 0;
  mu_invCov0_mu = 0;
  logdet_Cov0 = 0;
  grad_logdet_Cov = -2*N*invR';
  grad_x_invCov0_x = -2*invRR*sumXX*invR';
  grad_x_invCov0_mu = 0;
  grad_mu_invCov0_mu = 0;
  grad_logdet_Cov0 = 0;
  [KL_X,dKL_X] = vb_rotationcost_gaussian(logdet_Cov, ...
                                          x_invCov0_x, ...
                                          x_invCov0_mu, ...
                                          mu_invCov0_mu, ...
                                          logdet_Cov0, ...
                                          grad_logdet_Cov, ...
                                          grad_x_invCov0_x, ...
                                          grad_x_invCov0_mu, ...
                                          grad_mu_invCov0_mu, ...
                                          grad_logdet_Cov0);
  %KL_X = 0;
  %dKL_X = 0;

  % Vectorize results
  f = KL_W + KL_alpha + KL_X;
  df = dKL_W + dKL_alpha + dKL_X;
  df = df(1:D,:);
  df = df(:);

  end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function orthogonalize

  %% Move bias (approximate: mean of X to zero)
% $$$   dmu = mean(X,2);
% $$$   X = X - repmat(dmu,1,N);
% $$$   tauMu = tauMu + tauW*dmu;
% $$$   WtauMu = WtauMu ???
% $$$   MutauMu = ???

  %% Rotate W and X

  Yh0 = tauW*X;

  % Whitening of X
  [Vx,D2x] = svd(sum(XX,3)/N);
  Dx = spdiag(sqrt(diag(D2x))); % convert to sparse diagonal form
  Qx = Dx \ Vx';
  Qw = full(Vx * Dx); % funny: could be sparse if size(Dx)=[1 1] ??
  tauW = tauW * Qw;
  WtauMu = WtauMu * Qw;
  for m=1:M
%    CovW(:,:,m) = Qw' * CovW(:,:,m) * Qw;
    WtauW(:,:,m) = Qw' * WtauW(:,:,m) * Qw;
% $$$     WW(:,:,m) = W(m,:)'*W(m,:) + CovW(:,:,m);
  end

  % Orthogonalization of W
  [Vw,Dw] = svd(sum(WtauW,3)/M);
% $$$   [Vw,Dw] = svd(sum(WW,3)/M);
  Qx = Vw' * Qx;
  Qw = Vw;
%  W = W * Qw;
  tauW = tauW * Qw;
  WtauMu = WtauMu * Qw;
  for m=1:M
%    CovW(:,:,m) = Qw' * CovW(:,:,m) * Qw;
    WtauW(:,:,m) = Qw' * WtauW(:,:,m) * Qw; %W(m,:)'*W(m,:) + CovW(:,:,m);
% $$$     WW(:,:,m) = W(m,:)'*W(m,:) + CovW(:,:,m);
  end

  % Apply rotations to X
  X = Qx * X;
  for n = 1:N
    CovX(:,:,n) = Qx * CovX(:,:,n) * Qx';
    XX(:,:,n) = X(:,n)*X(:,n)' + CovX(:,:,n);
  end

  Yh1 = tauW*X;

% $$$   diff_in_orth = norm(Yh1-Yh0)
% $$$
% $$$   sumWtauW = sum(WtauW,3)
% $$$   sumXX = sum(XX,3) / N

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%

function initialize()

  disp('Initialize variables..')

  % Initialize X
  X = nan * zeros(D,N);
  CovX = nan*zeros([D,D,N]); % repmat(eye(D),[1,1,N]);

  % Initialize q(alpha)
  a_alpha = nan * ones(D,1);
  b_alpha = nan * ones(D,1);
  alpha = 1e-3 * ones(D,1);
  logalpha = nan * ones(D,1);

  % Initialize q(beta)
  a_beta = nan;
  b_beta = nan;
  beta = 1e-3;
  logbeta = nan;

  % Initialize W and Mu
  W = randn(M,D);
  Mu = zeros(M,1);
  CovWMu = repmat(eye(D+1),[1,1,M]);

  % Initialize tau
  a_tau = nan * ones(M,1);
  b_tau = nan * ones(M,1);
  tau = 1e1 * ones(M,1);
  logtau = nan * ones(M,1);

  % Initialize nu and q(U)
  nu = 1 * ones(M,1);
  a_U = nan * ones(M,N);
  b_U = nan * ones(M,N);
  U = ones(M,N);
  logU = nan * zeros(M,N);

  % Initialize log likelihood
  loglikelihood = -Inf;

  % Initialize mean squared errors < (y-wx-m)^2 >
  E2 = zeros(size(Y));

  % Use given initialization (size matching is checked by using ':' )
  if isstruct(options.init)
    % X
    if isfield(options.init, 'X')
      disp('Custom initialization for X.');
      warning(['Currently, X is updated first, so given initialization has ' ...
               'no effect.']);
      X(:,:) = options.init.X;
    end
    if isfield(options.init, 'CovX')
      disp('Custom initialization for CovX.');
      warning(['Currently, X is updated first, so given initialization has ' ...
               'no effect.']);
      CovX(:,:,:) = options.init.CovX;
    end
    % W and Mu
    if isfield(options.init, 'W')
      disp('Custom initialization for W.');
      W(:,:) = options.init.W;
    end
    if isfield(options.init, 'Mu')
      disp('Custom initialization for Mu.');
      Mu(:,:) = options.init.Mu;
    end
    if isfield(options.init, 'CovWMu')
      disp('Custom initialization for CovWMu.');
      CovWMu(:,:,:) = options.init.CovWMu;
    end
    % alpha
    if isfield(options.init, 'alpha')
      disp('Custom initialization for alpha.');
      alpha(:) = options.init.alpha;
    end
% $$$     if isfield(options.init, 'a_alpha')
% $$$       disp('Custom initialization for a_alpha.');
% $$$       a_alpha(:) = options.init.a_alpha;
% $$$     end
% $$$     if isfield(options.init, 'b_alpha')
% $$$       disp('Custom initialization for b_alpha.');
% $$$       b_alpha(:) = options.init.b_alpha;
% $$$     end
    % beta
    if isfield(options.init, 'beta')
      disp('Custom initialization for beta.');
      beta(:) = options.init.beta;
    end
% $$$     if isfield(options.init, 'a_beta')
% $$$       disp('Custom initialization for a_beta.');
% $$$       a_beta(:) = options.init.a_beta;
% $$$     end
% $$$     if isfield(options.init, 'b_beta')
% $$$       disp('Custom initialization for b_beta.');
% $$$       b_beta(:) = options.init.b_beta;
% $$$     end
    % tau
    if isfield(options.init, 'tau')
      disp('Custom initialization for tau.');
      tau(:) = options.init.tau;
    end
% $$$     if isfield(options.init, 'a_tau')
% $$$       disp('Custom initialization for a_tau.');
% $$$       a_tau(:) = options.init.a_tau;
% $$$     end
% $$$     if isfield(options.init, 'b_tau')
% $$$       disp('Custom initialization for b_tau.');
% $$$       b_tau(:) = options.init.b_tau;
% $$$     end
    % nu
    if isfield(options.init, 'nu')
      disp('Custom initialization for nu.');
      nu(:) = options.init.nu;
    end
    % U
    if isfield(options.init, 'U')
      disp('Custom initialization for U.');
      U(:,:) = options.init.U;
    end
  end

  % Initialize second moments for X
  XX = zeros(D,D,N);
  for n=1:N
    XX(:,:,n) = X(:,n)*X(:,n)' + CovX(:,:,n);
  end

  % Initialize expectations for W and Mu
  tauW = zeros(M,D);
  tauMu = zeros(M,1);
  WtauW = zeros(D,D,M);
  MutauMu = zeros(M,1);
  WtauMu = zeros(M,D);
  for m=1:M
    tauW(m,:) = tau(m) * W(m,:);
    tauMu(m) = tau(m) * Mu(m);
    WtauW(:,:,m) = tau(m)*W(m,:)'*W(m,:) + CovWMu(1:D,1:D,m);
    MutauMu(m) = tau(m)*Mu(m)^2 + CovWMu(end,end,m);
    WtauMu(m,:) = tau(m)*Mu(m)*W(m,:) + CovWMu(end,1:D,m);
  end

end

%%%%%%%%%%%%%%%%%%%%%%%%%

function set_priors()

  % Default prior
  p.a_alpha = 1e-5;
  p.b_alpha = 1e-5;
  p.a_beta = 1e-5;
  p.b_beta = 1e-5;
  p.a_tau = 1e-5;
  p.b_tau = 1e-5;

  % Custom prior
  if isstruct(options.prior)

    % Prior for alpha
    if isfield(options.prior, 'a_alpha')
      p.a_alpha(1) = options.prior.a_alpha;
      disp('Custom prior parameter a_alpha');
    end
    if isfield(options.prior, 'b_alpha')
      p.b_alpha(1) = options.prior.b_alpha;
      disp('Custom prior parameter b_alpha');
    end

    % Prior for beta
    if isfield(options.prior, 'a_beta')
      p.a_beta(1) = options.prior.a_beta;
      disp('Custom prior parameter a_beta');
    end
    if isfield(options.prior, 'b_beta')
      p.b_beta(1) = options.prior.b_beta;
      disp('Custom prior parameter b_beta');
    end

    % Prior for tau
    if isfield(options.prior, 'a_tau')
      p.a_tau(1) = options.prior.a_tau
      disp('Custom prior parameter a_tau');
    end
    if isfield(options.prior, 'b_tau')
      p.b_tau(1) = options.prior.b_tau
      disp('Custom prior parameter b_tau');
    end

  end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function rotate

% I am not absolutely sure that this is bug free

% $$$ D = size(XX,1);
% $$$ N = size(XX,3);
% $$$ M = size(CC,3);

% R should be non-singular because we need to take log(abs(det(R)))

% X_X_ = sum(XX(:,:,1:(N-1)),3) + x0x0;
% CovA = 1/D * (sum(AA,3) - A'*A);
% $$$ Psi = sum(XX,3) - 2*A*sum(X_X,3) + A*X_X_*A' + traceprod(CovA,X_X_)*eye(D);

% Start with identity matrix which does not make the results worse
R = eye(D);

r = minimize(R(:),@cost,100);
R = reshape(r,[D,D]);

% Compute new statistics
for n=1:N
  XX(:,:,n) = R \ XX(:,:,n) / R';
  X_X(:,:,n) = R \ X_X(:,:,n) / R';
end
x0x0 = R \ x0x0 / R';
A = R \ A * R;
CovA = 1/D * trace(inv(R*R')) * R'*CovA*R;
for d=1:D
  AA(:,:,d) = A(d,:)'*A(d,:) + CovA;
end
C = C*R;
for m=1:M
  CC(:,:,m) = R'*CC(:,:,m)*R;
end

I = eye(D);
Rtmp = randn(D);

% Compute new hyperparameters here or after this function!

  function [f,df] = cost(x)

    R = reshape(x,[D,D]);
    [f,df] = bound(R);
    f = -f;
    df = -df(:);

  end

  function [l,dl] = bound(R)

  % Use QR to evaluate these more efficiently?
  RR = R'*R;
  invR = inv(R); % inv??
  invRR = inv(R*R'); % inv??
  logdetR = logdet(R);
  trinvRR = trace(invRR);

  Psi = sum(XX,3)*invR'/p.Cov_X - 2*inv(p.Cov_X)*sum(p.mu_X'*X,2);

  % A_invRR_A = A'*invRR*A + trinvRR*CovA;
  % diag_RA_invRR_AR = diag(R'*A_invRR_A*R);
  diagRCCR = diag(R'*sum(WtauW,3)*R); % not efficient

  % Bound terms
  lpX = -0.5 * traceprod(invRR,Psi);
  lqX = N * logdetR;
  lpA = 0; %-0.5 * D * sum(log(diag_RA_invRR_AR));
  lqA = 0; %-0.5*D*logabsdet(trinvRR*RR);
  lpC = -0.5 * M * sum(log(diagRCCR));
  lqC = -M * logdetR;

  % Bound
  l = lpX + lpA + lpC - lqX - lqA - lqC;

  % Derivative terms
  dlpX = invR * Psi * invRR;
  dlqX = N * invR;
  dlpA = 0; %-D*(diag(1./diag_RA_invRR_AR)*R'*A_invRR_A) ...
         %+ D*invR*A*R*diag(1./diag_RA_invRR_AR)*R'*A'*invRR ...
         %+ D*traceprod(diag(1./diag_RA_invRR_AR),R'*CovA*R)*invR*invRR;
  dlqA = 0; %-D * invR + D^2 * invR*invRR / trinvRR;
  dlpC = -M * (diag(1./diagRCCR)*R'*sum(CC,3));
  dlqC = -M * invR;

  % Derivative
  dl = (dlpX + dlpA + dlpC - dlqX - dlqA - dlqC)';
  end

end

end


function [CTTraceS, RTOGPBVectorsM, RTOGPBVectorsM_MC, PBMaskM, rowPBV, colPBV, xPBPosV, yPBPosV, beamlet_delta_x, beamlet_delta_y] = ...
   getPBRayData(edgeS, sourceS, numSamplePts, xySampleRate,planC, xPosV, yPosV, beamlet_delta_x, beamlet_delta_y, gA);
%JOD
% global planC
%
% indexS=planC{end};

water = 1000; %Assumes water equals 1000.

%-----------Get CT scan---------------------%

[CTUniform3D, CTUniformInfoS] = getUniformizedCTScan(1,planC);

xOffset = CTUniformInfoS.xOffset;
yOffset = CTUniformInfoS.yOffset;

%-----------Fix source characteristics---------------------%

orgV   = [sourceS.isocenter.x, sourceS.isocenter.y, sourceS.isocenter.z];

%-----------Get ray parameters---------------------%

% [RTOGPBVectorsM, RTOGPBVectorsM_MC, PBMaskM, rowPBV, colPBV, xPBPosV, yPBPosV] = ...
%       getPBRays(edgeS, sourceS, xySampleRate);
[RTOGPBVectorsM, RTOGPBVectorsM_MC, PBMaskM, rowPBV, colPBV, xPBPosV, yPBPosV, beamlet_delta_x, beamlet_delta_y] = ...
      getPBRays(xPosV, yPosV, beamlet_delta_x, beamlet_delta_y, gA);

rayLength = 500;  %in cm.  This is the length of the rays which are passed
                  %through the CT matrix to determine cumulative CT densities.

numSlices = size(CTUniform3D,3);

zFirst = CTUniformInfoS.firstZValue;

sliceThickness = CTUniformInfoS.sliceThickness;

%minBox an maxBox containt coordinates of the corners of the CT box as used
%by the ray intersection routine.

maxBoxS.z = (numSlices - 1) * sliceThickness + zFirst + sliceThickness/2;

minBoxS.z = zFirst - sliceThickness/2;

delta_xy = CTUniformInfoS.grid1Units;

imageWidth = CTUniformInfoS.size(1);

minBoxS.x = - imageWidth/2 * delta_xy + xOffset;
maxBoxS.x =   imageWidth/2 * delta_xy + xOffset;

minBoxS.y = - imageWidth/2 * delta_xy + yOffset;
maxBoxS.y =   imageWidth/2 * delta_xy + yOffset;

imageWidth = CTUniformInfoS.sizeOfDimension1;

CTTraceS = struct('CTNumsRay',[],'CTCumNumsRay',[],'distSamplePts',[]);

for i = 1 : size(RTOGPBVectorsM,1)

  rayDeltaS.x = RTOGPBVectorsM(i,1) * rayLength;
  rayDeltaS.y = RTOGPBVectorsM(i,2) * rayLength;
  rayDeltaS.z = RTOGPBVectorsM(i,3) * rayLength;

  %are the components of the ray's direction and maximum length.

  deltaV = [rayDeltaS.x, rayDeltaS.y, rayDeltaS.z];

  t_entrance = rayBoxIntersection(sourceS,rayDeltaS,minBoxS,maxBoxS);

  %The entrance point is therefore
  entranceV = orgV + t_entrance * deltaV;

  %Reflect to find exit point (assume length of ray is long enough that ray does exit):

  if t_entrance ~= -1

    %find exit point
    %get end of ray
    rayOrgS2.xRel = sourceS.xRel + rayDeltaS.x;  %reflected source positions
    rayOrgS2.yRel = sourceS.yRel + rayDeltaS.y;
    rayOrgS2.zRel = sourceS.zRel + rayDeltaS.z;

    rayDeltaS2.x = - rayDeltaS.x;
    rayDeltaS2.y = - rayDeltaS.y;
    rayDeltaS2.z = - rayDeltaS.z;

    rayOrgS2.isocenter = sourceS.isocenter;

    t = rayBoxIntersection(rayOrgS2,rayDeltaS2,minBoxS,maxBoxS);

    t_exit = 1 - t;

    exitV = orgV + t_exit * deltaV;

else
    error('PB Ray does not intersect CT scan.');
end

  %Now produce a set of sampling points between the entrance and the exit

  nV = 1 : numSamplePts;

  delta_t  = (t_exit - t_entrance)/(numSamplePts - 1);

  tV = t_entrance + (nV - 1) * delta_t;

  CTTraceS(i).distSamplePts = tV * sum(deltaV.^2).^0.5;

  sampleV.x =  sourceS.xRel + tV * deltaV(1);

  sampleV.y =  sourceS.yRel + tV * deltaV(2);

  sampleV.z =  sourceS.zRel + tV * deltaV(3);

  sampleRTOGV.x = sampleV.x + sourceS.isocenter.x;
  sampleRTOGV.y = sampleV.y + sourceS.isocenter.y;
  sampleRTOGV.z = sampleV.z + sourceS.isocenter.z;

  %---------Sample CT densities----------%

  %To go from sample points in RTOG system to CT densities, we convert as follows:

  %What is the slice number?
  sliceV = 1 + (sampleRTOGV.z - zFirst)/sliceThickness;

  %Now do 3-D interpolation:
  zFieldV = [minBoxS.z + 0.5 * sliceThickness:sliceThickness:maxBoxS.z - 0.5 * sliceThickness];
  xFieldV = [minBoxS.x + 0.5 * delta_xy, delta_xy, maxBoxS.x - 0.5 * delta_xy];
  yFieldV = [minBoxS.y + 0.5 * delta_xy, delta_xy, maxBoxS.y - 0.5 * delta_xy];

  [CTNumsV] = finterp3(sampleRTOGV.x, sampleRTOGV.y, sampleRTOGV.z, CTUniform3D, xFieldV, yFieldV, zFieldV, 0);

%   [CTNumsV] = finterp3(sampleRTOGV.x, sampleRTOGV.y, sampleRTOGV.z, CTUniform3D, xFieldV, yFieldV, zFieldV, 0);

  CTTraceS(i).densityRay = delta_t * norm(deltaV) * CTNumsV/water;
  CTTraceS(i).cumDensityRay = cumsum(CTTraceS(i).densityRay);  %Account for sampling rate to convert to g/cm^2.

  if any(CTTraceS(i).cumDensityRay > 50)
      warning('Cumulative density ray appears to exceed maximum length.');
  end

end

%-----------fini---------------------%


function test_bug3124

% WALLTIME 00:30:00
% MEM 3gb
% DEPENDENCY ft_sourceanalysis

%%

load(dccnpath('/home/common/matlab/fieldtrip/data/ftp/tutorial/beamformer/headmodel.mat'))
load(dccnpath('/home/common/matlab/fieldtrip/data/ftp/tutorial/beamformer/data_all.mat'))

%%

cfg = [];
cfg.method = 'mtmfft';
cfg.taper = 'hanning';
cfg.output = 'fourier';
freq = ft_freqanalysis(cfg, data_all);

%%
% with 1 cm grid -> 3000 grid points, 1484 inside
% with 2 cm grid -> 186 grid points

cfg = [];
cfg.headmodel = ft_convert_units(headmodel, 'cm');
cfg.resolution = 2;
cfg.channel = 'MEG';

sourcemodel = ft_prepare_leadfield(cfg, freq);

%%
% make a manual selection of a single frequency

fcfg = [];
fcfg.frequency = 10;
freq1 = ft_selectdata(fcfg, freq);

fcfg = [];
fcfg.foilim = 10;
freq2 = ft_selectdata(fcfg, freq);

%%
% the result should be at 10 Hz in all cases

cfg = [];
cfg.headmodel = headmodel;
cfg.sourcemodel = sourcemodel;
cfg.frequency = 10;
source0 = ft_sourceanalysis(cfg, freq);

cfg = [];
cfg.headmodel = headmodel;
cfg.sourcemodel = sourcemodel;
source1 = ft_sourceanalysis(cfg, freq1);

cfg = [];
cfg.headmodel = headmodel;
cfg.sourcemodel = sourcemodel;
source2 = ft_sourceanalysis(cfg, freq2);

assert(isequal(source0.freq, [10]));
assert(isequal(source1.freq, [10]));
assert(isequal(source2.freq, [10]));

assert(isequaln(source0.avg.pow, source1.avg.pow));
assert(isequaln(source0.avg.pow, source2.avg.pow));

%%
% make a manual selection of a range

fcfg = [];
fcfg.frequency = [9 11];
freq1 = ft_selectdata(fcfg, freq);

fcfg = [];
fcfg.foilim = [9 11];
freq2 = ft_selectdata(fcfg, freq);

%%
% the result should be at 10 Hz in all cases, i.e. averaged from 9-11

cfg = [];
cfg.headmodel = headmodel;
cfg.sourcemodel = sourcemodel;
cfg.frequency = [9 11];
source0 = ft_sourceanalysis(cfg, freq);

cfg = [];
cfg.headmodel = headmodel;
cfg.sourcemodel = sourcemodel;
source1 = ft_sourceanalysis(cfg, freq1);

cfg = [];
cfg.headmodel = headmodel;
cfg.sourcemodel = sourcemodel;
source2 = ft_sourceanalysis(cfg, freq2);

assert(isequal(source0.freq, [10]));
assert(isequal(source1.freq, [10]));
assert(isequal(source2.freq, [10]));

assert(isequaln(source0.avg.pow, source1.avg.pow));
assert(isequaln(source0.avg.pow, source2.avg.pow));


function test_bug27

% MEM 8gb
% WALLTIME 00:10:00
% DEPENDENCY ft_preproc_lowpassfilter ft_resampledata ft_resampledata ft_datatype_raw fixtimeaxes

% this script tests for bug 27 and for the solution
% the 'bug' is that if ft_resampledata is called on data with different time axes
% after resampling the time axes of the trials may be shifted with respect to one
% another with a fractional amount (governed by the input fsample)

%% generate some data
% shifted by one sample
data1          = [];
data1.trial{1} = randn(2,1000);
data1.trial{2} = randn(2,1000);
data1.time{1}  = [0:0.001:0.999] - 0.5;
data1.time{2}  = [0.001:0.001:1] - 0.5;
data1.label    = {'chan1';'chan2'};
data1.fsample  = 1000;
data1.cfg.trl  = zeros(2,3);

for k = 1:2
  data1.trial{k} = ft_preproc_lowpassfilter(data1.trial{k}, 1000, 40);
end

%% resample with a factor 1x
cfg            = [];
cfg.resamplefs = 1000;
cfg.detrend    = 'no';
data2          = ft_resampledata(cfg, data1);

[ok, msg] = isalmostequal(data1.time, data2.time, 'abstol', 1e-10);
assert(ok, msg);

%% resample
cfg            = [];
cfg.resamplefs = 256;
cfg.detrend    = 'no';
data2          = ft_resampledata(cfg, data1);

figure
subplot(2,2,1), plot(data1.time{1}, data1.trial{1}, 'b'); xlim([data1.time{1}(1) data1.time{1}(end)])
subplot(2,2,2), plot(data1.time{2}, data1.trial{2}, 'b'); xlim([data1.time{2}(1) data1.time{2}(end)])
subplot(2,2,3), plot(data2.time{1}, data2.trial{1}, 'r'); xlim([data1.time{1}(1) data1.time{1}(end)])
subplot(2,2,4), plot(data2.time{2}, data2.trial{2}, 'r'); xlim([data1.time{2}(1) data1.time{2}(end)])

%% make it a bit more extreme
data1.time{2} = data1.time{2} + 3.1152;
data2 = ft_resampledata(cfg, data1);

figure
subplot(2,2,1), plot(data1.time{1}, data1.trial{1}, 'b'); xlim([data1.time{1}(1) data1.time{1}(end)])
subplot(2,2,2), plot(data1.time{2}, data1.trial{2}, 'b'); xlim([data1.time{2}(1) data1.time{2}(end)])
subplot(2,2,3), plot(data2.time{1}, data2.trial{1}, 'r'); xlim([data1.time{1}(1) data1.time{1}(end)])
subplot(2,2,4), plot(data2.time{2}, data2.trial{2}, 'r'); xlim([data1.time{2}(1) data1.time{2}(end)])

%% make it a bit more extreme
cfg.resamplefs = 17;
data2 = ft_resampledata(cfg, data1);

figure
subplot(2,2,1), plot(data1.time{1}, data1.trial{1}, 'b'); xlim([data1.time{1}(1) data1.time{1}(end)])
subplot(2,2,2), plot(data1.time{2}, data1.trial{2}, 'b'); xlim([data1.time{2}(1) data1.time{2}(end)])
subplot(2,2,3), plot(data2.time{1}, data2.trial{1}, 'r'); xlim([data1.time{1}(1) data1.time{1}(end)])
subplot(2,2,4), plot(data2.time{2}, data2.trial{2}, 'r'); xlim([data1.time{2}(1) data1.time{2}(end)])

%%

% this contains a problematic data structure from Mats
load(dccnpath('/home/common/matlab/fieldtrip/data/test/bug27.mat'), 'data');

clear sample0 time0
for i=1:numel(data.time)
  % find the sample corresponding with time zero
  sample0(i)  = nearest(data.time{i}, 0);
  time0(i)    = data.time{i}(sample0(i));
end
figure
plot(time0, '.-'); % this looks ok, no jitter
assert(std(time0)<1e-6, 'there is too much jitter in time0');

cfg = [];
cfg.resamplefs = 600;
data_resampled = ft_resampledata(cfg, data);

clear sample0 time0
for i=1:numel(data.time)
  % find the sample corresponding with time zero
  sample0(i)  = nearest(data_resampled.time{i}, 0);
  time0(i)    = data_resampled.time{i}(sample0(i));
end
figure
plot(time0, '.-'); % this showed quite some jitter over trials
assert(std(time0)<1e-6, 'there is too much jitter in time0');


% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com
theta = linspace(-pi, pi, 75);
[Q2,Q3] = meshgrid(theta, theta);
for i=1:numcols(Q2),
	for j=1:numcols(Q3);
		g = p560.gravload([0 Q2(i,j) Q3(i,j) 0 0 0]);
		g2(i,j) = g(2);
		g3(i,j) = g(3);
	end
end
surfl(Q2, Q3, g2); surfl(Q2, Q3, g3);


function icol = r8col_find ( m, n, a, x )

%*****************************************************************************80
%
%% R8COL_FIND seeks a column value in an R8COL.
%
%  Example:
%
%    Input:
%
%      M = 3,
%      N = 4,
%
%      A = (
%        1.  2.  3.  4.
%        5.  6.  7.  8.
%        9. 10. 11. 12. )
%
%      x = ( 3.,
%            7.,
%           11. )
%
%    Output:
%
%      ICOL = 3
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 October 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, real A(M,N), a table of numbers, regarded as
%    N columns of vectors of length M.
%
%    Input, real X(M), a vector to be matched with a column of A.
%
%    Output, integer ICOL, the index of the first column of A
%    which exactly matches every entry of X, or -1 if no match
%    could be found.
%
  icol = -1;

  for j = 1 : n

    if ( x(1:m) == a(1:m,j)' )
      icol = j;
      return
    end

  end

  return
end


function [v] = tapas_mh_mc3_arc(ollh, olpp, nllh, nlpp, ...
    ratio, T)
%% Acceptance rejection criterion for metropolis hastings in the context of
% population mcmc.
%
% aponteeduardo@gmail.com
% copyright (C) 2016
%

v = bsxfun(@times, nllh, T) + nlpp - (bsxfun(@times, ollh, T) + olpp) ...
    + ratio;

nansv = isnan(v);
v(nansv) = -inf;

v = rand(size(v)) < exp(v);

assert(all(-inf < nllh(v) + nlpp(v)), 'tapas:mh', ...
    '-inf value in the new samples');

end


%2D level 0
%tic
load elements.mat
fprintf('2D triangulation:  %d elements, ',size(elements,1));
fprintf('%d nodes ',max(max(elements)));
[element2edges, edge2nodes]=getEdges(elements);
edge2elements=entryInWhichRows(element2edges);
fprintf('--> %d edges numbered. \n',size(edge2nodes,1));
%toc

%2D level 1
%tic
load elements_1.mat
fprintf('2D triangulation:  %d elements, ',size(elements,1));
fprintf('%d nodes ',max(max(elements)));
[element2edges, edge2nodes]=getEdges(elements);
edge2elements=entryInWhichRows(element2edges);
fprintf('--> %d edges numbered. \n',size(edge2nodes,1));
%toc

%2D level 9
tic
load elements_9.mat
fprintf('2D triangulation:  %d elements, ',size(elements,1));
fprintf('%d nodes ',max(max(elements)));
tic
[element2edges, edge2nodes]=getEdges(elements);
edge2elements=entryInWhichRows(element2edges);
fprintf('--> %d edges numbered. \n',size(edge2nodes,1));
toc

%3D
%tic
elements=[1 2 3 4; 1 2 3 5];
fprintf('3D triangulation:  %d elements, ',size(elements,1));
fprintf('%d nodes ',max(max(elements)));
[element2edges, edge2nodes]=getEdges(elements);
edge2elements=entryInWhichRows(element2edges);
fprintf('--> %d edges numbered. \n',size(edge2nodes,1));
%toc

%3D
tic
load elements3D
fprintf('3D triangulation:  %d elements, ',size(elements,1));
fprintf('%d nodes ',max(max(elements)));
[element2edges, edge2nodes]=getEdges(elements);
edge2elements=entryInWhichRows(element2edges);
fprintf('--> %d edges numbered. \n',size(edge2nodes,1));
toc