classdef ShFunc_StressNorm3 < ShFunWithElasticPdes

    properties (Access = private)
        adjointProb
        fAdjoint
        stressNorm
        stressNormSquare
        stressNorml2LpNorm
        isElementToOptimize
        integralSigmaP

        ngaus
        nelem
        sigma
        sigmaA
        sigma2
        sigmaP

        dsigHPdsigH2v
        PpdSigHP_DsigH2M
    end

    methods (Access = public)

        function obj = ShFunc_StressNorm3(cParams)
            cParams.filterParams.quadratureOrder = 'LINEAR';
            obj.init(cParams);
            fileName = cParams.femSettings.fileName;
            obj.createEquilibriumProblem(fileName);
            obj.createAdjointProblem(fileName);
            obj.createOrientationUpdater();
            obj.createElementsToOptimize();
            obj.updateHomogenizedMaterialProperties();
            obj.nelem = obj.physicalProblem.mesh.nelem;
            obj.ngaus = obj.physicalProblem.element.quadrature.ngaus;
        end

        function f = getPdesVariablesToPrint(obj)
            f{1} = obj.getPdeVariableToPrint(obj.physicalProblem);
            f{2} = obj.getPdeVariableToPrint(obj.adjointProb);
        end

       function fP = addPrintableVariables(obj)
            fP{1}.value = obj.getPdeVariableToPrint(obj.physicalProblem);
            fP{2}.value = obj.getPdeVariableToPrint(obj.adjointProb);
            fP{3}.value = sqrt(obj.sigma2');
            fP{4}.value = obj.designVariable.alpha;
            fP{5}.value = abs(obj.designVariable.alpha);
            fP = obj.addHomogVariables(fP);
       end

        function fP = createPrintVariables(obj)
            types = {'Elasticity','Elasticity','ScalarGauss'...
                        'VectorGauss','VectorGauss'};
            names = {'Primal','Adjoint','StressNormGauss',...
                        'AlphaGauss','AlphaAbsGauss'};
            fP = obj.obtainPrintVariables(types,names);
            fP = obj.addHomogPrintVariablesNames(fP);
        end

        function v = getVariablesToPlot(obj)
            v{1} = max(sqrt(obj.sigma2));
            sNorm2 = obj.computeClosedSigmaProduct(obj.sigma,obj.sigma);
            v{2} = max(sqrt(sNorm2));
        end

        function t = getTitlesToPlot(obj)
            t{1} = 'Max amplified stress';
            t{2} = 'Max stress';
        end

        function setPnorm(obj,pNorm)
            obj.target_parameters.stressNormExponent = pNorm;
        end

    end

    methods (Access = protected)

        function updateHomogenizedMaterialProperties(obj)
            obj.filterDesignVariable();
            obj.homogenizedVariablesComputer.computeCtensor(obj.regDesignVariable);
            pNorm = obj.obtainPnorm();
            obj.homogenizedVariablesComputer.computePtensor(obj.regDesignVariable,pNorm);
        end

        function computeFunctionValue(obj)
            p  = obj.obtainPnorm();
            Pp = obj.homogenizedVariablesComputer.Pp;
            obj.sigma  = obj.computeSigmaTensor();
            obj.sigmaA = obj.amplifySigma(Pp,obj.sigma);
            obj.sigma2 = obj.computeClosedSigmaProduct(obj.sigmaA,obj.sigmaA);
            obj.sigmaP = obj.computeSigmaP(obj.sigma2);
            obj.integralSigmaP = obj.integrateSigmaP();
            obj.value = obj.integralSigmaP.^(1/p);
        end

        function solveState(obj)
            obj.physicalProblem.setC(obj.homogenizedVariablesComputer.C);
            obj.physicalProblem.computeVariables();
        end

        function computeGradientValue(obj)
            dJdx = obj.computeDJdx();
            dHdx = obj.computeDHdx();
            obj.gradient = dJdx + dHdx;
        end

        function solveAdjoint(obj)
            obj.computeFadjoint();
            obj.adjointProb.setC(obj.homogenizedVariablesComputer.C);
            obj.adjointProb.computeVariablesWithBodyForces(obj.fAdjoint);
        end

    end

    methods (Access = private)

        function g = initG(obj)
            g = zeros(obj.nelem,obj.ngaus,obj.nVariables);
        end

        function euR = rotateStrain(obj,eu)
            rot = obj.homogenizedVariablesComputer.rotator;
            euR  = rot.rotateStrain(eu);
        end

        function sR = rotateStress(obj,s)
            rot = obj.homogenizedVariablesComputer.rotator;
            sR  = rot.rotateStress(s);
        end

        function ds2ds = computedSigma2dSigma(obj,ds)
            Pp    = obj.homogenizedVariablesComputer.Pp;
            s     = obj.sigma;
            sA    = obj.amplifySigma(Pp,s);
            dsA   = obj.amplifySigma(Pp,ds);
            sXds  = obj.computeClosedSigmaProduct(sA,dsA);
            dsXs  = obj.computeClosedSigmaProduct(dsA,sA);
            ds2ds = sXds + dsXs;
        end

        function ds2dP = computedSigma2dP(obj,dPp)
            s     = obj.sigma;
            Pp    = obj.homogenizedVariablesComputer.Pp;
            sA    = obj.amplifySigma(Pp,s);
            dsA   = obj.amplifySigma(dPp,s);
            sXds  = obj.computeClosedSigmaProduct(sA,dsA);
            dsXs  = obj.computeClosedSigmaProduct(dsA,sA);
            ds2dP = sXds + dsXs;
        end

        function s = computeSigmaTensor(obj)
            eu = obj.physicalProblem.variables.strain;
            C  = obj.homogenizedVariablesComputer.Cref;
            s  =  obj.computeSigma(C,eu);
        end

        function sA = amplifySigma(obj,Pp,s)
            sA = obj.computeSigma(Pp,s);
        end

        function computeSigma2(obj)
            s = obj.sigmaA;
            obj.sigma2  = obj.computeClosedSigmaProduct(s,s);
        end

        function sNorm = computeClosedSigmaProduct(obj,sa,sb)
            nStre = size(sa,2);
            factor = [1 1 2];
            sNorm = zeros(obj.ngaus,obj.nelem);
            for iStre = 1:nStre
                sAi = obj.squeezeParticular(sa(:,iStre,:),2);
                sBi = obj.squeezeParticular(sb(:,iStre,:),2);
                sNorm = sNorm + factor(iStre)*(sAi.*sBi);
            end
        end

        function intSigmaP = integrateSigmaP(obj)
            dvolum = obj.physicalProblem.geometry.dvolu';
            int    = obj.sigmaP.*dvolum;
            intSigmaP = int(:,obj.isElementToOptimize);
            intSigmaP = sum(intSigmaP(:));
        end

        function sP = computeSigmaP(obj,s2)
            p  = obj.obtainPnorm;
            sP = s2.^(p/2);
        end

        function computeFadjoint(obj)
            dSigmaPdu    = obj.computedSigmaPdu();
            dJdu         = obj.computedJdSigmaP(dSigmaPdu);
            obj.fAdjoint = obj.assambleVector(-dJdu);
        end

        function dJdx = computeDJdx(obj)
            dSigmaPdx = obj.computedSigmaPdx();
            dJdx      = obj.computedJdSigmaP(dSigmaPdx);
            dJdx(~obj.isElementToOptimize,:) = 0;
        end

        function s = computeSigma(obj,C,eu)
            %eu = obj.rotateStrain(eu);
            s  = obj.computeStress(C,eu);
            %s = obj.rotateStress(s);
        end

        function dSigmaPdx = computedSigmaPdx(obj)
            eu = obj.physicalProblem.variables.strain;
            dC = obj.homogenizedVariablesComputer.dCref;
            dP = obj.homogenizedVariablesComputer.dPp;
            nX = size(dC,3);
            dSigmaPdx = zeros(nX,obj.ngaus,obj.nelem);
            for ix = 1:nX
                dCdx       = squeeze(dC(:,:,ix,:));
                dPdx       = squeeze(dP(:,:,ix,:));
                dSigmadx   = obj.computeSigma(dCdx,eu);
                dSigma2dxA = obj.computedSigma2dSigma(dSigmadx);
                dSigma2dxB = obj.computedSigma2dP(dPdx);
                dSigma2dx  = dSigma2dxA + dSigma2dxB;
                dSigmaPdx(ix,:,:) = obj.computedSigmaPdSigma2(dSigma2dx);
            end
            dSigmaPdx = permute(dSigmaPdx,[3 2 1]);
        end

        function dSigmaPdu = computedSigmaPdu(obj)
            C    = obj.homogenizedVariablesComputer.Cref;
            dEps = obj.computedEps();
            nV   = size(dEps,1);
            dSigmaPdu = zeros(nV,obj.ngaus,obj.nelem);
            for iv = 1:nV
                dEpsDu    = obj.squeezeParticular(dEps(iv,:,:,:),1);
                dSigmadu  = obj.computeSigma(C,dEpsDu);
                dSigma2du = obj.computedSigma2dSigma(dSigmadu);
                dSigmaPdu(iv,:,:) = obj.computedSigmaPdSigma2(dSigma2du);
            end
        end

        function dSigmaPdx = computedSigmaPdSigma2(obj,dSigma2dx)
            p = obj.obtainPnorm();
            dSigmaPdSigma2 = p/2*(obj.sigma2).^(p/2-1);
            dSigmaPdx = dSigmaPdSigma2.*dSigma2dx;
        end

        function s = computeStress(obj,C,e)
            s = zeros(size(e));
            for igaus = 1:obj.ngaus
                eG(1,:,:) = squeeze(e(igaus,:,:));
                sGt = bsxfun(@times,C,eG);
                s(igaus,:,:) = obj.squeezeParticular(sum(sGt,2),2);
            end
        end

        function g = computeDHdx(obj)
            eu = obj.physicalProblem.variables.strain;
            %eu = obj.rotateStrain(eu);
            dC = obj.homogenizedVariablesComputer.dCref;
            ep = obj.adjointProb.variables.strain;
            g = obj.initG();
            for ivar = 1:obj.nVariables
                dCiv = squeeze(dC(:,:,ivar,:));
                ds = obj.computeSigma(dCiv,ep);
                g(:,:,ivar) = obj.squeezeParticular(sum(eu.*ds,2),2)';
            end
        end

        function p = obtainPnorm(obj)
            p = obj.target_parameters.stressNormExponent;
        end

        function createAdjointProblem(obj,fileName)
            obj.adjointProb = FEM.create(fileName);
        end

        function dEps = computedEps(obj)
            dvolum = obj.physicalProblem.geometry.dvolu';
            nstre = obj.physicalProblem.element.getNstre();
            nnode = obj.physicalProblem.element.nnode;
            nunkn = obj.physicalProblem.element.dof.nunkn;
            nv    = nnode*nunkn;
            dEps = zeros(nv,obj.ngaus,nstre,obj.nelem);
            for igaus = 1:obj.ngaus
               B  = obj.physicalProblem.element.computeB(igaus);
               Bm = permute(B,[2 1 3]);
               dvG(1,1,:) = squeeze(dvolum(igaus,:));
               dvGm = repmat(dvG,nv,nstre,1);
               dEps(:,igaus,:,:) = Bm.*dvGm;
            end
        end

        function fAdjoint = assambleVector(obj,dsigmaPdu)
            eforce = dsigmaPdu;
            eforce(:,:,~obj.isElementToOptimize) = 0;
            Fvol = obj.physicalProblem.element.AssembleVector({eforce});
            fAdjoint = Fvol;
        end

        function dJdx = computedJdSigmaP(obj,dSigmaPdx)
            p = obj.obtainPnorm();
            intSigmaP = obj.integralSigmaP;
            dJ_dSigmaP = 1/p*intSigmaP^(1/p-1);
            dJdx = dJ_dSigmaP*dSigmaPdx;
        end

        function createElementsToOptimize(obj)
             phy = obj.getPhysicalProblems();
             m = phy{1}.mesh;
             xG = transpose(m.computeBaricenter);
             x = xG(:,1);
             y = xG(:,2);
%             isDirichletPartX = x > -1e-12 & x < 0.05;
%             isDirichletPartY = y > 0.20 & y < 0.80;
%             isDirichletPart = isDirichletPartX & isDirichletPartY;
%             isNeumannPartX = x > (1-0.05) & x < (1+1e-12);
%             isNeumannPartY = y > 0.40 & y < 0.60;
%             isNeumannPart = isNeumannPartX & isNeumannPartY;
%             isForOptimizing = ~isDirichletPart & ~isNeumannPart;
%             obj.isElementToOptimize = isForOptimizing;

%             s.connec = m.connec(isForOptimizing,:);
%             s.coord  = m.coord;
%             m2 = Mesh(s);
%             m2.plot();
           obj.isElementToOptimize = true(size(x));
        end

    end

    methods (Access = private, Static)

        function squeezed = squeezeParticular(t,dim)
            z = size(t);
            index = setdiff(1:length(z),dim);
            squeezed = reshape(t,[z(index) z(dim)]);
        end

    end

end


function lf=openmeeg_helper(pos,vol,splitdim)
%
% Splits the calculation of leadfields in smaller chunks
% pos: positions of dipoles
% vol: volume conductor
% splitdim: splitting factor

lf   = [];
ndip = size(pos,1);
numsol   = fix(ndip./splitdim);
nleft    = rem(ndip,splitdim);
lf = zeros(size(vol.mat,1),ndip*3);
if isfield(vol,'mat')
    for i = 1:splitdim
      dsm = openmeeg_dsm(pos((numsol*(i - 1) + 1):numsol*i,:),vol);
      tmp = vol.mat*dsm;
      lf(:,(numsol*(i - 1)*3 + 1):numsol*i*3) = tmp;
    end
    dsm = openmeeg_dsm(pos(ndip-nleft+1:ndip,:),vol);
    tmp = vol.mat*dsm;
    lf(:,(ndip-nleft)*3+1:ndip*3) = tmp;
else
  error('No system matrix is present, BEM head model not calculated yet')
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   PARAMETERS Returns a data structure containing the parameters of the
%   IRB_4600.
%
%   Author: Arturo Gil. Universidad Miguel Hern�ndez de Elche.
%   email: arturo.gil@umh.es date:   09/01/2012
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE 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.
%
% ARTE 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 ARTE.  If not, see <http://www.gnu.org/licenses/>.
function robot = parameters()

robot.name= 'abb_IRB_4600';

%Path where everything is stored for this robot
robot.path = 'robots/abb/IRB_4600';

robot.DH.theta= '[q(1) q(2)-pi/2 q(3) q(4) q(5) q(6)+pi]';
robot.DH.d='[0.495 0 0 0.960 0 0.135]';
robot.DH.a='[0.175 0.9 0.175 0 0 0]';
robot.DH.alpha= '[-pi/2 0 -pi/2 pi/2 -pi/2 0]';
robot.J=[];

robot.inversekinematic_fn = 'inversekinematic_IRB_4600(robot, T)';

%number of degrees of freedom
robot.DOF = 6;

%rotational: 0, translational: 1
robot.kind=['R' 'R' 'R' 'R' 'R' 'R'];

%minimum and maximum rotation angle in rad
robot.maxangle =[deg2rad(-180) deg2rad(180); %Axis 1, minimum, maximum
                deg2rad(-90) deg2rad(150); %Axis 2, minimum, maximum
                deg2rad(-180) deg2rad(75); %Axis 3
                deg2rad(-400) deg2rad(400); %Axis 4: Unlimited (400� default)
                deg2rad(-125) deg2rad(120); %Axis 5
                deg2rad(-400) deg2rad(400)]; %Axis 6: Unlimited (800� default)

%maximum absolute speed of each joint rad/s or m/s
robot.velmax = [deg2rad(200); %Axis 1, rad/s
                deg2rad(200); %Axis 2, rad/s
                deg2rad(260); %Axis 3, rad/s
                deg2rad(360); %Axis 4, rad/s
                deg2rad(360); %Axis 5, rad/s
                deg2rad(450)];%Axis 6, rad/s

robot.accelmax=robot.velmax/0.1; % 0.1 is here an acceleration time

            % end effectors maximum velocity
robot.linear_velmax = 2.5; %m/s

%base reference system
robot.T0 = eye(4);

%INITIALIZATION OF VARIABLES REQUIRED FOR THE SIMULATION
%position, velocity and acceleration
robot=init_sim_variables(robot);

% GRAPHICS
robot.graphical.has_graphics=1;
robot.graphical.color = [255 20 40]./255;
%for transparency
robot.graphical.draw_transparent=0;
%draw DH systems
robot.graphical.draw_axes=1;
%DH system length and Font size, standard is 1/10. Select 2/20, 3/30 for
%bigger robots
robot.graphical.axes_scale=1;
%adjust for a default view of the robot
robot.axis=[-0.75 0.75 -0.75 0.75 0 1.2];
%read graphics files
robot = read_graphics(robot);

%DYNAMICS
robot.has_dynamics=0;


% MatrixUser, a multi-dimensional matrix analysis software package
% https://sourceforge.net/projects/matrixuser/
%
% The MatrixUser is a matrix analysis software package developed under Matlab
% Graphical User Interface Developing Environment (GUIDE). It features
% functions that are designed and optimized for working with multi-dimensional
% matrix under Matlab. These functions typically includes functions for
% multi-dimensional matrix display, matrix (image stack) analysis and matrix
% processing.
%
% Author:
%   Fang Liu <leoliuf@gmail.com>
%   University of Wisconsin-Madison
%   Aug-30-2014

function MU_funcImTool(Temp,Event,handles)
handles = guidata(handles.MU_matrix_display);
try
    BMatrix=handles.BMatrix;
    imtool(BMatrix);
    colormap(handles.V.Color_map);
    set(gca,'Clim',[handles.V.C_lower handles.V.C_upper]);
catch me
    error_msg{1,1}='ERROR!!! imtool is not working in this Matlab version.';
    error_msg{2,1}=me.message;
    errordlg(error_msg);
end

end


function c=ref_dgt2(f,g1,g2,a1,a2,M1,M2);
%REF_DGT2  Reference DGT2
%
%  Compute a DGT2 using a DGT along each dimension.

L1=size(f,1);
L2=size(f,2);

N1=L1/a1;
N2=L2/a2;

c=dgt(f,g1,a1,M1);

c=reshape(c,M1*N1,L2);

c=c.';

c=dgt(c,g2,a2,M2);

c=reshape(c,M2*N2,M1*N1);

c=c.';

c=reshape(c,M1,N1,M2,N2);


function [imc] = undistort_image(im, KK, kc, nx, ny, KK_new)

fc = [ KK(1, 1); KK(2, 2) ];
alpha_c = 0;
cc = [ KK(1,3); KK(2, 3) ];

dist_amount = 1; %(1+kc(1)*r2_extreme + kc(2)*r2_extreme^2);
fc_new = dist_amount * fc;

if ~exist('KK_new','var')
    KK_new = [fc_new(1) alpha_c*fc_new(1) cc(1);0 fc_new(2) cc(2) ; 0 0 1];
end

I = im;

if (size(I,1)>ny)|(size(I,2)>nx),
    I = I(1:ny,1:nx);
end;

%% SHOW THE ORIGINAL IMAGE:
%{
if size(I,3) == 3
    figure(2);
    image(uint8(I));
    title('Original image (with distortion) - Stored in array I')
    axis equal;
    axis tight;
    drawnow;
else
    figure(2);
    image(I);
    colormap(gray(256));
    axis equal;
    axis tight;
    title('Original image (with distortion) - Stored in array I');
    drawnow;
end
%}

%% UNDISTORT THE IMAGE:

fprintf(1,'Computing the undistorted image...')

if size(I,3) == 3
    [Ipart_1] = rect(I(:,:,1),eye(3),fc,cc,kc,alpha_c,KK_new);
    [Ipart_2] = rect(I(:,:,2),eye(3),fc,cc,kc,alpha_c,KK_new);
    [Ipart_3] = rect(I(:,:,3),eye(3),fc,cc,kc,alpha_c,KK_new);

    I2 = ones(ny, nx,3);
    I2(:,:,1) = Ipart_1;
    I2(:,:,2) = Ipart_2;
    I2(:,:,3) = Ipart_3;

    fprintf(1,'done\n')

    %{
    figure(3);
    image(uint8(I2));
    axis equal;
    axis tight;
    %}
else
    [I2] = rect(I,eye(3),fc,cc,kc,alpha_c,KK_new);

    fprintf(1,'done\n');

    %{
    figure(3);
    image(I2);
    colormap(gray(256));
    axis equal;
    axis tight;
    %}
end;

%title('Undistorted image - Stored in array I2')
%drawnow;

imc = I2;

%


function [Psi2, P] = linard2VardistPsi2Compute(linard2kern, vardist, Z)

% LINARD2VARDISTPSI2COMPUTE description.

% VARGPLVM

%Psi1 = linard2KernCompute(linard2kern, vardist.means, Z);
%sqrtAS = sparse(diag(linard2kern.inputScales.*sqrt(sum(vardist.covars,1))));
%Zsc = Z*sqrtAS;
%P = Psi1'*Psi1;
%Psi2 = P + Zsc*Zsc';

ZA = Z*sparse(diag(linard2kern.inputScales));

P = vardist.means'*vardist.means + diag(sum(vardist.covars,1));

Psi2 = ZA*P*ZA';


function [ hBlobs_valid, hBlobs_centroid ] = regionShapeValidation( hBlobs, H, W, min_size, aspectratio, arearatio, iscuboid )
%REGIONSHAPEVALIDATION Summary of this function goes here
%   check whether to find a cuboid or rectangle fitting based on
%   segmentation shape
numSeg = length(hBlobs);
invalidMask = false(H,W);
if iscuboid
    invalidMask(:,1) = true;   invalidMask(:,W) = true;
    invalidMask(1,:) = true;   invalidMask(H,:) = true;
    invalidMask(round(H/2), 1) = true;
    invalidMask(round(H/2), round(W/4)) = true;
    invalidMask(round(H/2), round(W/2)) = true;
    invalidMask(round(H/2), round(3*W/4)) = true;
    invalidMask(round(H/2), round(W)) = true;
    invalidMask = bwdist(invalidMask)<10;
end
SE = strel('disk', 5, 4);
SS = strel('disk', 2, 4);

hBlobs_valid = false(numSeg, 1);
hBlobs_centroid = zeros(numSeg,2);

parfor bid = 1:length(hBlobs)
%     fprintf('%d/%d\n', bid, length(hBlobs));

    if hBlobs{bid}.size<min_size
        continue;
    end
    seg = false(H,W);
    rect = hBlobs{bid}.rect;
    seg(rect(1):rect(3),rect(2):rect(4)) = hBlobs{bid}.mask;

    binaryMap = imdilate( seg, SE);
    binaryMap = imerode( binaryMap, SE);
    binaryMap = imerode( binaryMap, SS);
    binaryMap = imdilate( binaryMap, SS);
    binaryMap = imfill(binaryMap, 'hole');

    CC = bwconncomp(binaryMap);
    valid = true(CC.NumObjects,1);
    for j = 1:CC.NumObjects
        if length(CC.PixelIdxList{j})<min_size
            valid(j) = false;
        end
        if any(invalidMask(CC.PixelIdxList{j}))
            valid(j) = false;
        end
    end
    CC.NumObjects = sum(valid);
    CC.PixelIdxList = CC.PixelIdxList(valid);

    if CC.NumObjects ~= 1
        continue;
    end

    R = regionprops( CC, 'BoundingBox', 'Area', 'ConvexArea', 'Centroid');
    boundingbox = R(1).BoundingBox;
    asrt = boundingbox(4)/boundingbox(3);
    if asrt>aspectratio || asrt<1/aspectratio
        continue;
    end
    if  R(1).Area/(R(1).ConvexArea+0.0001)<arearatio
        continue;
    end
    hBlobs_valid(bid) = true;
    hBlobs_centroid(bid,:) = R(1).Centroid;
end

end


% DEMBRENDANVARGPLVM3 Run variational GPLVM on Brendan face data.

% VARGPLVM

% Fix seeds
randn('seed', 1e6);
rand('seed', 1e6);

dataSetName = 'brendan';
experimentNo = 3;
printDiagram = 1;

% load data
[Y, lbls] = lvmLoadData(dataSetName);

% training and test sets
Ntr = 1000;
perm = randperm(size(Y,1));
Ytr = Y(perm(1:Ntr),:);      %lblsTr = lbls(perm(1:Ntr),:);
YtsOriginal = Y(perm(Ntr+1:end),:);  %lblsTs = lbls(perm(Ntr+1:end),:);

% Set up model
options = vargplvmOptions('dtcvar');
options.kern = 'rbfardjit'; %{'rbfard2', 'white'};
options.numActive = 50;
options.initSNR = 100;
%options.scale2var1 = 1; % scale data to have variance 1

options.optimiser = 'scg2';
latentDim = 25;

model = vargplvmCreate(latentDim, size(Ytr, 2), Ytr, options);
model = vargplvmParamInit(model, model.m, model.X, options);
model.vardist.covars = 0.5*ones(size(model.vardist.covars)) + 0.001*randn(size(model.vardist.covars));

iters = 1200;
display = 1;

fprintf('# Initialising the variational distribution for 800 iterations...\n')
model.initVardist = 1; model.learnSigmaf = 0; model.learnBeta = 0;
model = vargplvmOptimise(model, display, 800);
modelInit = model;

fprintf(['# Optimising the model for ' num2str(iters) ' iterations...\n'])
model.initVardist = 0; model.learnSigmaf = 1; model.learnBeta = 1;
model = vargplvmOptimise(model, display, iters);

%% Visualise the results real-time
% lvmVisualise(model, lbls, 'imageVisualise', 'imageModify', [20 28], 1, 0);

%% PREDICTIONS (reconstruction of test outputs)
testIters = 200;

% 50% missing outputs from the each test point
numIndPresent = round(0.5*model.d);
indexP = [];
Init = [];
Varmu = zeros(size(Yts));
Varsigma = [];
mini = zeros(1, size(Yts,1));
Yts = YtsOriginal;

fprintf(['\n\n# Test phase: each point optimised for ' num2str(testIters) ' iterations...\n'])

% patrial reconstruction of test points
for i=1:size(Yts,1)
    fprintf(['\n\n# Test point # ' num2str(i) '...\n\n'])
    %
    % randomly choose which outputs are present
    permi = randperm(model.d);
    indexPresent =  permi(1:numIndPresent);
    indexP(i,:) = indexPresent;
    indexMissing = setdiff(1:model.d, indexPresent); %
    Yts(i,indexMissing) = NaN; %
    % initialize the latent point using the nearest neighbour
    % from he training data
    dst = dist2(Yts(i,indexPresent), Ytr(:,indexPresent));
    [mind, mini(i)] = min(dst);

    % create the variational distribtion for the test latent point
    vardistx = vardistCreate(model.vardist.means(mini(i),:), model.q, 'gaussian');
    vardistx.covars = 0.2*ones(size(vardistx.covars));

    % optimize mean and vars of the latent point
    model.vardistx = vardistx;
%   [x, varx] = vargplvmOptimisePoint(model, vardistx, Yts(i, indexPresent), indexPresent, display, testIters); %old
    [x, varx] = vargplvmOptimisePoint(model, vardistx, Yts(i, :), display, testIters); %

    % reconstruct the missing outputs
    [mu, sigma] = vargplvmPosteriorMeanVar(model, x, varx);
    Varmu(i,:) = mu;
    Varsigma(i,:) = sigma;
    %
end

capName = dataSetName;
capName(1) = upper(capName(1));
modelType = model.type;
modelType(1) = upper(modelType(1));
save(['dem' capName modelType num2str(experimentNo) '.mat'], 'model', 'perm', 'indexP', 'Varmu', 'Varsigma');

%%
close all;
showReconstruction = 1:5:size(Yts,1);
for i = showReconstruction
    subplot(1,4,1);
    imagesc(reshape(Yts(i,:), 20,28)'); colormap('gray'); title('Given')
    subplot(1,4,2);
    imagesc(reshape(YtsOriginal(i,:), 20,28)');  colormap('gray'); title('Original')
    subplot(1,4,3);
    imagesc(reshape(Varmu(i,:), 20, 28)');  colormap('gray');  title('Predicted')
    subplot(1,4,4);
    imagesc(reshape(model.y(mini(i),:), 20, 28)');  colormap('gray');  title('NN')
    fprintf(1, '# Press any key to continue...')
    pause
    fprintf(1, '\n')
end


function getVoxelHull(obj, voxelSize, varargin)
% GETVOXELHULL Compute the voxel hull of a point cloud.
% ------------------------------------------------------------------------------
% DESCRIPTION/NOTES
% * The voxel hull is a low resolution representation of the volume occupied by
%   a point cloud. For the computation of the voxel hull the object space is
%   subdivided into a voxel structure. The voxel hull of the point cloud
%   consists of all voxels which contain at least one point of the point cloud.
%
% * Only active points are considered for the computation of the voxel hull.
%
% * The voxel hull can be used to select points, which are overlapping with
%   another point cloud. An example for this task is given in the documentation
%   of the 'select' method. For this call 'help pointCloud.select' and see the
%   examples for the 'InVoxelHull' selection strategy.
% ------------------------------------------------------------------------------
% INPUT
% 1 [voxelSize]
%   Voxel size (equal to edge length) of a single voxel.
% ------------------------------------------------------------------------------
% OUTPUT
% 1 [obj]
%   The voxel hull is attached to the following properties of the ouput object:
%   * obj.voxelHull          = n-by-3 matrix containing the x, y, z coordinates
%                              of the centers of the voxels
%   * obj.voxelHullVoxelSize = edge length of voxels (given by parameter
%                              voxelSize)
% ------------------------------------------------------------------------------
% EXAMPLES
% 1 Import a point cloud and compute its voxel hull.
%   pc = pointCloud('Lion.xyz');
%   pc.getVoxelHull(5); % voxel size is 5mm
%   % The voxel hull can now be found in pc.voxelHull and pc.voxelHullVoxelSize
% ------------------------------------------------------------------------------
% philipp.glira@gmail.com
% ------------------------------------------------------------------------------

% Input parsing ----------------------------------------------------------------

p = inputParser;
p.addRequired('voxelSize', @(x) isscalar(x) && x>0);
% Undocumented
p.addParameter('Centroids', false, @islogical);
p.parse(voxelSize, varargin{:});
p = p.Results;
% Clear required inputs to avoid confusion
clear voxelSize

% Start ------------------------------------------------------------------------

procHierarchy = {'POINTCLOUD' 'GETVOXELHULL'};
msg('S', procHierarchy);
msg('I', procHierarchy, sprintf('Point cloud label = ''%s''', obj.label));

% Compute voxel hull -----------------------------------------------------------

% Lower left point of activated points
lim.min = min(obj.X(obj.act,:), [], 1);
% Round origin (voxel hulls have coincident voxel centers if mod(100, p.voxelSize) == 0)
lim.min = (floor(lim.min/100))*100;

% Indices of voxel cells in x, y and z direction (indices start with 0!)
idxXYZ = [floor( (obj.X(obj.act,1) - lim.min(1)) / p.voxelSize ) ...
          floor( (obj.X(obj.act,2) - lim.min(2)) / p.voxelSize ) ...
          floor( (obj.X(obj.act,3) - lim.min(3)) / p.voxelSize )];

% Remove multiple points to get unique voxels
[voxel, ~, ic] = unique(idxXYZ, 'rows'); % ic for centroids

% Transformation of indices to coordinate system and save to object
obj.voxelHull = [lim.min(1) + p.voxelSize/2 + voxel(:,1) * p.voxelSize ...
                 lim.min(2) + p.voxelSize/2 + voxel(:,2) * p.voxelSize ...
                 lim.min(3) + p.voxelSize/2 + voxel(:,3) * p.voxelSize];

% Round voxel hull (because e.g. -100+0.025+1982*0.05 ~= -0.875)
if p.voxelSize < 1
    noDigits = abs(floor(log10(p.voxelSize)-1)) + 2; % e.g. for 0.05 -> noDigits = 3+2 = 5 (+2 is arbitrary choice to be on the safe side)
    obj.voxelHull = round(obj.voxelHull, noDigits);
end

% Save voxel size to object
obj.voxelHullVoxelSize = p.voxelSize;

% Calculate centroids within each voxel ----------------------------------------

if p.Centroids

    % Add centroid matrix
    obj.voxelHull = [obj.voxelHull zeros(size(voxel,1),3)];

    % Centroid for each voxel
    % Var1
    % tic;
    % for i = 1:size(voxel,1), obj.voxelHull(i,4:6) = mean(obj.X(ic == i,:),1); end
    % toc;
    % c1 = obj.voxelHull(:,4:6);

    % Var2
    % tic;
    % cellX = cell(size(voxel,1),1);
    % for i = 1:size(voxel,1), cellX{i,1} = obj.X(ic == i,:); end
    % cellMean = cellfun(@(x) mean(x,1), cellX, 'UniformOutput', false);
    % obj.voxelHull(:,4:6) = vertcat(cellMean{:});
    % toc;
    % c2 = obj.voxelHull(:,4:6);

    % Var3
    % tic;
    % cellMean = arrayfun(@(x) mean(obj.X(ic == x,:),1), [1:size(voxel,1)]', 'UniformOutput', false);
    % obj.voxelHull(:,4:6) = vertcat(cellMean{:});
    % toc;
    % c3 = obj.voxelHull(:,4:6);

    % Var4
    % tic;
    [~, idx] = sort(ic);
    Xs = obj.X(idx,:);
    rowDist = diff(find([1; diff(ic(idx)); 1]));
    cellX = mat2cell(Xs, rowDist);
    cellMean = cellfun(@(x) mean(x,1), cellX, 'UniformOutput', false);
    obj.voxelHull(:,4:6) = vertcat(cellMean{:});
    % toc;
    % c4 = obj.voxelHull(:,4:6);

end

% End --------------------------------------------------------------------------

msg('E', {'POINTCLOUD' 'GETVOXELHULL'});

end


% en_softth - soft threshold function for the Elastic-net regularization
%
% Copyright(c) 2009 Ryota Tomioka
% This software is distributed under the MIT license. See license.txt

function [vv,ss]=en_softth(vv,lambda,info)

n = size(vv,1);
theta  = info.theta;

if theta<1
  I=find(abs(vv)>lambda*theta);
  vv=sparse(I,1,(abs(vv(I))-lambda*theta).*sign(vv(I))/(1+lambda*(1-theta)),n,1);
else
  vv=l1_softth(vv,lambda,info);
end

ss=en_spec(vv,theta);


% Runs a step of non-local Bayes
%
% USAGE: [deno, aggw] = nlbayes_step(nisy, bsic, sigma, prms)
%
%  -> nisy    : noisy image
%  -> bsic    : basic estimate (can be empty for the first step)
%  -> sigma   : noise std. dev.
%  -> prms    : struc with prms (wx, px, np, r)
%
%  <- deno    : denoised image
%  <- aggw    : aggregation weights
function [deno, aggw] = nlbayes_step(nisy, bsic, sigma, prms)

% image size and channels
w = size(nisy,2);
h = size(nisy,1);
chnls = size(nisy,3);

% in the absence of the basic estimate, define basic as the noisy image
if isempty(bsic),
	bsic = nisy;
	step2 = false;
else
	step2 = true
end

% aggregation
aggw = zeros(size(nisy)); % aggregation weights
aggp = zeros(size(nisy)); % mask of already processed patches (for speed-up trick)
aggu = zeros(size(nisy)); % aggregated image
deno = zeros(size(nisy)); % denoised image (deno = aggu ./ aggw);

% step sizes
stepx = floor(prms.px/2);
stepy = floor(prms.px/2);
ii = 0; % iteration counter

% patch dimensionality
pdim = prms.px * prms.px * chnls;

% use dct basis
% U = kron(eye(chnls), dct_basis(prms.px, prms.px));
% use pca basis
U = [];

% use an aggregation window
if prms.pw,
	wx = chebwin(prms.px);
else
	wx = ones(prms.px,1);
end
wwx = repmat(reshape(wx*wx',[prms.px*prms.px 1]),[chnls prms.np]);

% main loop
for pay = [1:stepy:h - prms.px+1,h - prms.px+1],
for pax = [1:stepx:w - prms.px+1,w - prms.px+1], ii = ii + 1;

	% acceleration: skip iteration if patch has been been already denoised
	if aggp(pay,pax,1),
		continue;
	end

	% -------------------------------------------------- compute patch group

	% patches in search region
	srch_region = bsic(max(1,pay - prms.wx):min(h,pay + prms.wx + prms.px - 1),...
	                   max(1,pax - prms.wx):min(w,pax + prms.wx + prms.px - 1),:);

	srch_patches = im2col_ch(srch_region, [prms.px prms.px]);
	refe_patch = bsic(pay:pay+prms.px-1,pax:pax+prms.px-1,:);

	[distances, idx] = sort(L2_distance(refe_patch(:), srch_patches));

	% coordinates of the np nearest neighbors to the ref patch
	idx = idx(1:prms.np)';

	srch_h = size(srch_region,1) - prms.px + 1;
	patches.coords = [max(1,pax - prms.wx) + floor((idx-1)/srch_h),...
	                  max(1,pay - prms.wx) +   mod( idx-1 ,srch_h)];

	% ---------------------------------------------- extract similar patches
	if step2,
		patches.bsic = srch_patches(:,idx);

		% extract noisy patches
		srch_region = nisy(max(1,pay - prms.wx):min(h,pay + prms.wx + prms.px - 1),...
		                   max(1,pax - prms.wx):min(w,pax + prms.wx + prms.px - 1),:);
		patches.nisy = im2col_ch(srch_region, [prms.px prms.px]);
		patches.nisy = patches.nisy(:,idx);
	else
		patches.nisy = srch_patches(:,idx);
	end

	% ----------------------------------------------- compute bayes estimate
	if step2,
		aa = reshape(patches.bsic,[pdim prms.np]);
		[dd,gg] = compute_bayes_estimate(patches.nisy,aa,sigma,prms.r,'pos',U);
	else
		[dd,gg] = compute_bayes_estimate(patches.nisy,[],sigma,prms.r,'pos',U);
	end

	% ------------------------------------------- aggregate patches on image
	gg = ones(size(dd,1),1)*gg;
	aggu = aggregate_patches(aggu, wwx.*gg.*dd, [prms.px prms.px], patches.coords);
	aggw = aggregate_patches(aggw, wwx.*gg    , [prms.px prms.px], patches.coords);

	aggp(patches.coords(:,2) + (patches.coords(:,1)-1)*h) = 1+...
	           aggp(patches.coords(:,2) + (patches.coords(:,1)-1)*h);

	% draw
	if (mod(ii,50) == 1)
		nonzero = find(aggw ~= 0);
		deno(nonzero) = min(255, max(0, aggu(nonzero) ./ aggw(nonzero)));

		% draw a red box indicating limits of search region
		deno(max(1,pay-prms.wx):min(h,pay+prms.wx),max(1,pax-prms.wx),:) = 0;
		deno(max(1,pay-prms.wx):min(h,pay+prms.wx),min(w,pax+prms.wx),:) = 0;
		deno(max(1,pay-prms.wx),max(1,pax-prms.wx):min(w,pax+prms.wx),:) = 0;
		deno(min(h,pay+prms.wx),max(1,pax-prms.wx):min(w,pax+prms.wx),:) = 0;
		deno(max(1,pay-prms.wx):min(h,pay+prms.wx),max(1,pax-prms.wx),1) = 255;
		deno(max(1,pay-prms.wx):min(h,pay+prms.wx),min(w,pax+prms.wx),1) = 255;
		deno(max(1,pay-prms.wx),max(1,pax-prms.wx):min(w,pax+prms.wx),1) = 255;
		deno(min(h,pay+prms.wx),max(1,pax-prms.wx):min(w,pax+prms.wx),1) = 255;

		imagesc(max(min([deno;255-aggw],255),0)/255,[0 1]);
		axis equal, axis off,
		colormap gray
		drawnow
		pause(.01)

		% reset red pixels to 0
		deno(max(1,pay-prms.wx):min(h,pay+prms.wx),max(1,pax-prms.wx),1) = 0;
		deno(max(1,pay-prms.wx):min(h,pay+prms.wx),min(w,pax+prms.wx),1) = 0;
		deno(max(1,pay-prms.wx),max(1,pax-prms.wx):min(w,pax+prms.wx),1) = 0;
		deno(min(h,pay+prms.wx),max(1,pax-prms.wx):min(w,pax+prms.wx),1) = 0;
	end

end
end

% compute denoised image
nonzero = find(aggw ~= 0);
deno(nonzero) = min(255, max(0, aggu(nonzero) ./ aggw(nonzero)));

end


function [nxx,nyy] =  zoom_size(nx,ny,factor)
nxx = nx*factor + 0.5;
nyy = ny*factor +0.5;
end


% vectdata() - vector data interpolation with optional moving
%              average.
%
% Usage:
%   >> [interparray timesout] = vectdata( array, timesin, 'key', 'val', ... );
%
% Inputs:
%   array      - 1-D or 2-D float array. If 2-D, the second dimension
%                only is interpolated.
%   timesin    - [float vector] time point indices. Same dimension as
%                the interpolated dimension in array.
%
% Optional inputs
%   'timesout' - [float vector] time point indices for interpolating
%                data.
%   'method'   - method for interpolation
%        'linear'    -> Triangle-based linear interpolation (default).
%        'cubic'     -> Triangle-based cubic interpolation.
%        'nearest'   -> Nearest neighbor interpolation.
%        'v4'        -> MATLAB 4 griddata method.
%   'average'  - [real] moving average in the dimension of timesin
%                note that extreme values might be inacurate (see 'borders').
%                Default none or [].
%   'avgtype'  - ['const'|'gauss'] use a const value when averaging (array of
%                ones) or a gaussian window. Default is 'const'.
%   'border'   - ['on'|'off'] correct border effect when smoothing.
%                default is 'off'.
%
% Outputs:
%   interparray - interpolated array
%   timesout    - output time points
%
% Author: Arnaud Delorme, CNL / Salk Institute, 20 Oct 2002
%
% See also: griddata()

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

function [interparray, timesout] = vectdata( array, timevect, varargin );

if nargin < 3
    help vectdata;
    return;
end;

g = finputcheck( varargin, { 'timesout'   'real'  []           [];
                             'average'    'real'  []           [];
                             'gauss'      'real'  []           [];
                             'border'     'string' { 'on' 'off' } 'off';
                             'avgtype'    'string' { 'const' 'gauss' } 'const';
                             'method'     'string' { 'linear' 'cubic' 'nearest' 'v4' } 'linear'});
if isstr(g), error(g); end;

if size(array,2) == 1
    array = transpose(array);
end;

if ~isempty(g.average)
    timediff = timevect(2:end) -timevect(1:end-1);
    if any( (timediff - mean(timediff)) > 1e-8 ) % not uniform values
        fprintf('Data has to be interpolated uniformly for moving average\n');
        minspace = mean(timediff);
        newtimevect = linspace(timevect(1), timevect(end), ceil((timevect(end)-timevect(1))/minspace));
        array = interpolate( array, timevect, newtimevect, g.method);
        timevect = newtimevect;
    end;
    oldavg = g.average;
    g.average = round(g.average/(timevect(2)-timevect(1)));
    if oldavg ~= g.average
        fprintf('Moving average updated from %3.2f to %3.2f (=%d points)\n', ...
                oldavg, g.average*(timevect(2)-timevect(1)), g.average);
    end;
    if strcmpi(g.border, 'on')
        if strcmpi(g.avgtype, 'const')
            array = convolve(array, ones(1, g.average));
        else
            convolution = gauss2d(1,g.average,1,round(0.15*g.average));
            array = convolve(array, convolution);
        end;
    else
        if strcmpi(g.avgtype, 'const')
            array = conv2(array, ones(1, g.average)/g.average, 'same');
        else
            convolution = gauss2d(1,g.average,1,round(0.15*g.average));
            array = conv2(array, convolution/sum(convolution), 'same');
        end;
    end;
end;

interparray = interpolate( array, timevect, g.timesout, g.method);
timesout = g.timesout;

% interpolation function
% ----------------------
function [interparray] = interpolate( array, timesin, timesout, method);
    interparray = zeros(size(array,1), length(timesout));
    for index = 1:size(array,1)
        tmpa = [array(index,:) ; array(index,:)];

        [Xi,Yi,Zi] = griddata(timesin, [1 2]', tmpa, timesout, [1 2]', method); % Interpolate data
        interparray(index,:) = Zi(1,:);
    end;


clc
clear all
easy = 1; %% easy=1 用于测试：EN, SF,SD,PSNR,MSE, MI, VIF, AG, CC, SCD, Qabf等指标； easy=0 用于测试：Nabf, SSIM, MS_SSIM, FMI_pixel, FMI_dct, FMI_w等指标
dataset = 'TNO'
row_name1 = 'row1';
row_data1 = 'row2';
Method_name = 'SeAFusion';
row = 'A';
row_name = strrep(row_name1, 'row', row);
row_data = strrep(row_data1, 'row', row);
fileFolder=fullfile('../Image/Source-Image/TNO/ir'); % 源图像A所在文件夹 此处是'Evaluation\Image\Source-Image\TNO\ir'
dirOutput=dir(fullfile(fileFolder,'*.*'));
fileNames = {dirOutput.name};
[m, num] = size(fileNames);
ir_dir = fullfile('../Image/Source-Image/TNO/ir'); % 源图像A所在文件夹 此处是'Evaluation\Image\Source-Image\TNO\ir'
vi_dir = fullfile('../Image/Source-Image/TNO/vi'); % 源图像B所在文件夹 此处是'Evaluation\Image\Source-Image\TNO\vi'
Fused_dir = fullfile('../Image/Algorithm/SeAFusion_TNO'); % 融合结果所在文件夹 此处是 'Evaluation\Image\Algorithm\SeAFusion_TNO'
EN_set = [];    SF_set = [];SD_set = [];PSNR_set = [];
MSE_set = [];MI_set = [];VIF_set = []; AG_set = [];
CC_set = [];SCD_set = []; Qabf_set = [];
SSIM_set = []; MS_SSIM_set = [];
Nabf_set = [];FMI_pixel_set = [];
FMI_dct_set = []; FMI_w_set = [];
 for j = 1:num
    if (isequal(fileNames{j}, '.') || isequal(fileNames{j}, '..'))
        continue;
    else
        fileName_source_ir = fullfile(ir_dir, fileNames{j});
        fileName_source_vi = fullfile(vi_dir, fileNames{j});
        fileName_Fusion = fullfile(Fused_dir, fileNames{j});
        ir_image = imread(fileName_source_ir);
        vi_image = imread(fileName_source_vi);
        fused_image   = imread(fileName_Fusion);
        if size(ir_image, 3)>2
            ir_image = rgb2gray(ir_image);
        end

        if size(vi_image, 3)>2
            vi_image = rgb2gray(vi_image);
        end

        if size(fused_image, 3)>2
            fused_image = rgb2gray(fused_image);
        end

        [m, n] = size(fused_image);
    %     fused_image = fused_image(7:m-6, 7:n-6);
        ir_size = size(ir_image);
        vi_size = size(vi_image);
        fusion_size = size(fused_image);
        if length(ir_size) < 3 && length(vi_size) < 3
            [EN, SF,SD,PSNR,MSE, MI, VIF, AG, CC, SCD, Qabf, Nabf, SSIM, MS_SSIM, FMI_pixel, FMI_dct, FMI_w] = analysis_Reference(fused_image,ir_image,vi_image, easy);
            EN_set = [EN_set, EN];SF_set = [SF_set,SF];SD_set = [SD_set, SD];PSNR_set = [PSNR_set, PSNR];
            MSE_set = [MSE_set, MSE];MI_set = [MI_set, MI]; VIF_set = [VIF_set, VIF];
            AG_set = [AG_set, AG]; CC_set = [CC_set, CC];SCD_set = [SCD_set, SCD];
            Qabf_set = [Qabf_set, Qabf]; Nabf_set = [Nabf_set, Nabf];
            SSIM_set = [SSIM_set, SSIM]; MS_SSIM_set = [MS_SSIM_set, MS_SSIM];
            FMI_pixel_set = [FMI_pixel_set, FMI_pixel]; FMI_dct_set = [FMI_dct_set,FMI_dct];
            FMI_w_set = [FMI_w_set, FMI_w];
        else
            disp('unsucessful!')
            disp( fileName_Fusion)
        end

        fprintf('Fusion Method:%s, Image Name: %s\n', Method_name, fileNames{j})
    end
end
save_dir = '../Metric'; %存放Excel结果的文件夹
if exist(save_dir,'dir')==0
    mkdir(save_dir);
end
file_name = fullfile(save_dir, strcat('Metric_', Method_name, '.xlsx')); %存放Excel文件的文件名
%% 将测试结果写入 Excel， 此处采用writetable， 第一行可能会有问题，算法名在第二行，评估结果从第三行开始
if easy ==1
    SD_table = table(SD_set');
    PSNR_table = table(PSNR_set');
    MSE_table = table(MSE_set');
    MI_table = table(MI_set');
    VIF_table = table(VIF_set');
    AG_table = table(AG_set');
    CC_table = table(CC_set');
    SCD_table = table(SCD_set');
    EN_table = table(EN_set');
    Qabf_table = table(Qabf_set');
    SF_table = table(SF_set');
    method_name = cellstr(Method_name);
    method_table = table(method_name);

    writetable(SD_table,file_name,'Sheet','SD','Range',row_data);
    writetable(PSNR_table,file_name,'Sheet','PSNR','Range',row_data);
    writetable(MSE_table,file_name,'Sheet','MSE','Range',row_data);
    writetable(MI_table,file_name,'Sheet','MI','Range',row_data);
    writetable(VIF_table,file_name,'Sheet','VIF','Range',row_data);
    writetable(AG_table,file_name,'Sheet','AG','Range',row_data);
    writetable(CC_table,file_name,'Sheet','CC','Range',row_data);
    writetable(SCD_table,file_name,'Sheet','SCD','Range',row_data);
    writetable(EN_table,file_name,'Sheet','EN','Range',row_data);
    writetable(Qabf_table,file_name,'Sheet','Qabf','Range',row_data);
    writetable(SF_table,file_name,'Sheet','SF','Range',row_data);

    writetable(method_table,file_name,'Sheet','SD','Range',row_name);
    writetable(method_table,file_name,'Sheet','PSNR','Range',row_name);
    writetable(method_table,file_name,'Sheet','MSE','Range',row_name);
    writetable(method_table,file_name,'Sheet','MI','Range',row_name);
    writetable(method_table,file_name,'Sheet','VIF','Range',row_name);
    writetable(method_table,file_name,'Sheet','AG','Range',row_name);
    writetable(method_table,file_name,'Sheet','CC','Range',row_name);
    writetable(method_table,file_name,'Sheet','SCD','Range',row_name);
    writetable(method_table,file_name,'Sheet','EN','Range',row_name);
    writetable(method_table,file_name,'Sheet','Qabf','Range',row_name);
    writetable(method_table,file_name,'Sheet','SF','Range',row_name);
else
    Nabf_table = table(Nabf_set');
    SSIM_table = table(SSIM_set');
    MS_SSIM_table = table(MS_SSIM_set');
    FMI_pixel_table = table(FMI_pixel_set');
    FMI_dct_table = table(FMI_dct_set');
    FMI_w_table = table(FMI_w_set');
    method_name = cellstr(Method_name);
    method_table = table(method_name);

    writetable(Nabf_table,file_name,'Sheet','Nabf','Range',row_data);
    writetable(SSIM_table,file_name,'Sheet','SSIM','Range',row_data);
    writetable(MS_SSIM_table,file_name,'Sheet','MS_SSIM','Range',row_data);
    writetable(FMI_pixel_table,file_name,'Sheet','FMI_pixel','Range',row_data);
    writetable(FMI_dct_table,file_name,'Sheet','FMI_dct','Range',row_data);
    writetable(FMI_w_table,file_name,'Sheet','FMI_w','Range',row_data);

    writetable(method_table,file_name,'Sheet','Nabf','Range',row_name);
    writetable(method_table,file_name,'Sheet','SSIM','Range',row_name);
    writetable(method_table,file_name,'Sheet','MS_SSIM','Range',row_name);
    writetable(method_table,file_name,'Sheet','FMI_pixel','Range',row_name);
    writetable(method_table,file_name,'Sheet','FMI_dct','Range',row_name);
    writetable(method_table,file_name,'Sheet','FMI_w','Range',row_name);

end


		% Determinarea domeniului de definitie al functiei
t=-10:10;
		% Apelarea functiei MATLAB def_funct
f=def_funct(t,0,3,5,5,1)


function i4vec_index_insert_test ( )

%*****************************************************************************80
%
%% I4VEC_INDEX_INSERT_TEST tests I4VEC_INDEX_INSERT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 April 2009
%
%  Author:
%
%    John Burkardt
%
  n_max = 25;
  n = 0;
  x = [];
  indx = [];

  fprintf ( 1, '\n' );
  fprintf ( 1, 'I4VEC_INDEX_INSERT_TEST\n' );
  fprintf ( 1, '  I4VEC_INDEX_INSERT inserts values into an\n' );
  fprintf ( 1, '  index sorted array of integers.\n' );

  xval = 8;
  [ n, x, indx ] = i4vec_index_insert ( n, x, indx, xval );

  xval = 7;
  [ n, x, indx ] = i4vec_index_insert ( n, x, indx, xval );

  b = 0;
  c = 20;
  seed = 123456789;

  for i = 1 : 20
    [ xval, seed ] = i4_uniform_ab ( b, c, seed );
    fprintf ( 1, '  %6d\n', xval );
    [ n, x, indx ] = i4vec_index_insert ( n, x, indx, xval );
  end

  xval = 7;
  [ n, x, indx ] = i4vec_index_insert ( n, x, indx, xval );

  xval = 8;
  [ n, x, indx ] = i4vec_index_insert ( n, x, indx, xval );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Indexed list of entries:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  I  INDX(I)  X(I)  X(INDX(I))\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %6d  %6d  %6d  %6d\n', i, indx(i), x(i), x(indx(i)) );
  end

  return
end


function computeAssociations_Clinical_Old(pathUnivariate,cohorts,clinical,outcomes,timeToEvent)

startpath = pwd;
cd(pathUnivariate)

nameOutcomes = fieldnames(outcomes.(cohorts{1})); nOutcomes = numel(nameOutcomes); nCohort = numel(cohorts);
clinicVar = fieldnames(clinical.(cohorts{1})); nClinic = numel(clinicVar);
rsMat_Binary = zeros(nClinic,nOutcomes); pMat_Binary = zeros(nClinic,nOutcomes); stringCell_Binary = cell(nClinic,nOutcomes);
rsMat_Time = zeros(nClinic,nOutcomes); pMat_Time = zeros(nClinic,nOutcomes); stringCell_Time = cell(nClinic,nOutcomes);
for i = 1:nClinic
    for j = 1:nOutcomes
        var = []; outcome = []; time = [];
        for c = 1:nCohort
            cohort = cohorts{c};
            var = [var;clinical.(cohort).(clinicVar{i})];
            outcome = [outcome;outcomes.(cohort).(nameOutcomes{j})];
            time = [time;timeToEvent.(cohort).(nameOutcomes{j})];
        end
        [rsMat_Binary(i,j),pMat_Binary(i,j)] = corr(var,outcome,'type','Spearman','rows','pairwise');
        [rsMat_Time(i,j),pMat_Time(i,j)] = corr(var,time,'type','Spearman','rows','pairwise');
        if pMat_Binary(i,j) < 0.01
            stringCell_Binary{i,j} = ['rs = ',num2str(rsMat_Binary(i,j),'%.2f'),', p = ',num2str(pMat_Binary(i,j),'%.2i')];
        else
            stringCell_Binary{i,j} = ['rs = ',num2str(rsMat_Binary(i,j),'%.2f'),', p = ',num2str(pMat_Binary(i,j),'%.2f')];
        end
        if pMat_Time(i,j) < 0.01
            stringCell_Time{i,j} = ['rs = ',num2str(rsMat_Time(i,j),'%.2f'),', p = ',num2str(pMat_Time(i,j),'%.2i')];
        else
            stringCell_Time{i,j} = ['rs = ',num2str(rsMat_Time(i,j),'%.2f'),', p = ',num2str(pMat_Time(i,j),'%.2f')];
        end
    end
end
for j = 1:nOutcomes
    pVal = pMat_Binary(:,j);
    [significance] = benjamini_hochberg(pVal,0.10);
    for i = 1:nClinic
        if significance(i)
            stringCell_Binary{i,j} = ['*',stringCell_Binary{i,j},'*']; % Showing significance;
        end
    end
    pVal = pMat_Time(:,j);
    [significance] = benjamini_hochberg(pVal,0.10);
    for i = 1:nClinic
        if significance(i)
            stringCell_Time{i,j} = ['*',stringCell_Time{i,j},'*']; % Showing significance;
        end
    end
end
Locoregional = stringCell_Binary(:,1); Distant = stringCell_Binary(:,2); Death = stringCell_Binary(:,3);
results.tableCorr = table(Locoregional,Distant,Death,'RowNames',clinicVar);
results.rsMat = rsMat_Binary; results.pMat = pMat_Binary;
save('clinical_UniV_Binary','results'), clear results
Locoregional = stringCell_Time(:,1); Distant = stringCell_Time(:,2); Death = stringCell_Time(:,3);
results.tableCorr = table(Locoregional,Distant,Death,'RowNames',clinicVar);
results.rsMat = rsMat_Time; results.pMat = pMat_Time;
save('clinical_UniV_Time','results'), clear results

cd(startpath)
end


function [p,f] = spm_powell(p,xi,tolsc,func,varargin)
% Powell optimisation method
% FORMAT [p,f] = spm_powell(p,xi,tolsc,func,varargin)
%   p        - Starting parameter values
%   xi       - columns containing directions in which to begin searching
%   tolsc    - stopping criteria, optimisation stops when
%                sqrt(sum(((p-p_prev)./tolsc).^2))<1
%   func     - name of evaluated function
%   varargin - remaining arguments to func (after p)
%
%   p        - final parameter estimates
%   f        - function value at minimum
%__________________________________________________________________________
%
% Method is based on Powell's optimisation method described in
% Numerical Recipes (Press, Flannery, Teukolsky & Vetterling).
%__________________________________________________________________________
% Copyright (C) 2001-2011 Wellcome Trust Centre for Neuroimaging

% John Ashburner
% $Id: spm_powell.m 4423 2011-08-04 16:28:51Z guillaume $

p = p(:);
f = feval(func,p,varargin{:});
for iter=1:512
    %if numel(p)>1, fprintf('iteration %d...\n', iter); end;            %-#
    ibig = numel(p);
    pp   = p;
    fp   = f;
    del  = 0;
    for i=1:length(p)
        ft = f;
        [p,junk,f] = min1d(p,xi(:,i),func,f,tolsc,varargin{:});
        if abs(ft-f) > del,
            del  = abs(ft-f);
            ibig = i;
        end
    end
    if numel(p)==1 || sqrt(sum(((p(:)-pp(:))./tolsc(:)).^2))<1, return; end
    ft = feval(func,2.0*p-pp,varargin{:});
    if ft < f
        [p,xi(:,ibig),f] = min1d(p,p-pp,func,f,tolsc,varargin{:});
    end
end
warning('Too many optimisation iterations');

%==========================================================================
% function [p,pi,f] = min1d(p,pi,func,f,tolsc,varargin)
%==========================================================================
function [p,pi,f] = min1d(p,pi,func,f,tolsc,varargin)
% Line search for minimum.

global lnm % used in funeval
lnm      = struct('p',p,'pi',pi,'func',func,'args',[]);
lnm.args = varargin;

min1d_plot('Init', 'Line Minimisation','Function','Parameter Value');
min1d_plot('Set', 0, f);

tol      = 1/sqrt(sum((pi(:)./tolsc(:)).^2));
t        = bracket(f);
[f,pmin] = search(t,tol);
pi       = pi*pmin;
p        = p + pi;

%if length(p)<12,
%    for i=1:length(p), fprintf('%-8.4g ', p(i)); end;                  %-#
%    fprintf('| %.5g\n', f);                                            %-#
%else
%    fprintf('%.5g\n', f);                                              %-#
%end
min1d_plot('Clear');

%==========================================================================
% function f = funeval(p)
%==========================================================================
function f = funeval(p)
% Reconstruct parameters and evaluate.

global lnm % defined in min1d
pt = lnm.p+p.*lnm.pi;
f  = feval(lnm.func,pt,lnm.args{:});
min1d_plot('Set',p,f);

%==========================================================================
% function t = bracket(f)
%==========================================================================
function t = bracket(f)
% Bracket the minimum (t(2)) between t(1) and t(3)

gold   = (1+sqrt(5))/2; % Golden ratio

t(1)   = struct('p',0,'f',f);
t(2).p = 1;
t(2).f = funeval(t(2).p);

% if t(2) not better than t(1) then swap
if t(2).f > t(1).f
    t(3) = t(1);
    t(1) = t(2);
    t(2) = t(3);
end

t(3).p = t(2).p + gold*(t(2).p-t(1).p);
t(3).f = funeval(t(3).p);

while t(2).f > t(3).f

    % fit a polynomial to t
    tmp = cat(1,t.p)-t(2).p;
    pol = pinv([ones(3,1) tmp tmp.^2])*cat(1,t.f);

    % minimum is when gradient of polynomial is zero
    % sign of pol(3) (the 2nd deriv) should be +ve
    if pol(3)>0
        % minimum is when gradient of polynomial is zero
        d    = -pol(2)/(2*pol(3)+eps);

        % A very conservative constraint on the displacement
        if d > (1+gold)*(t(3).p-t(2).p),
            d = (1+gold)*(t(3).p-t(2).p);
        end
        u.p  = t(2).p+d;
    else
        % sign of pol(3) (the 2nd deriv) is not +ve
        % so extend out by golden ratio instead
        u.p  = t(3).p+gold*(t(3).p-t(2).p);
    end

    % FUNCTION EVALUATION
    u.f  = funeval(u.p);

    if (t(2).p < u.p) == (u.p < t(3).p)

        % u is between t(2) and t(3)
        if u.f < t(3).f
            % minimum between t(2) and t(3) - done
            t(1) = t(2);
            t(2) = u;
            return
        elseif u.f > t(2).f
            % minimum between t(1) and u - done
            t(3) = u;
            return;
        end
    end

    % Move all 3 points along
    t(1) = t(2);
    t(2) = t(3);
    t(3) = u;
end

%==========================================================================
% function [f,p] = search(t, tol)
%==========================================================================
function [f,p] = search(t, tol)
% Brent's method for line searching - given that minimum is bracketed

gold1 = 1-(sqrt(5)-1)/2;

% Current and previous displacements
d     = Inf;
pd    = Inf;

% sort t into best first order
[junk,ind] = sort(cat(1,t.f));
t   = t(ind);
brk = [min(cat(1,t.p)) max(cat(1,t.p))];

for iter=1:128
    % check stopping criterion
    if abs(t(1).p - 0.5*(brk(1)+brk(2)))+0.5*(brk(2)-brk(1)) <= 2*tol
        p = t(1).p;
        f = t(1).f;
        return;
    end

    % keep last two displacents
    ppd = pd;
    pd  = d;

    % fit a polynomial to t
    tmp = cat(1,t.p)-t(1).p;
    pol = pinv([ones(3,1) tmp tmp.^2])*cat(1,t.f);

    % minimum is when gradient of polynomial is zero
    d   = -pol(2)/(2*pol(3)+eps);
    u.p = t(1).p+d;

    % check so that displacement is less than the last but two,
    % that the displaced point is between the brackets
    % and that the solution is a minimum rather than a maximum
    eps2 = 2*eps*abs(t(1).p)+eps;
    if abs(d) > abs(ppd)/2 || u.p < brk(1)+eps2 || u.p > brk(2)-eps2 || pol(3)<=0
        % if criteria are not met, then golden search into the larger part
        if t(1).p >= 0.5*(brk(1)+brk(2)),
            d = gold1*(brk(1)-t(1).p);
        else
            d = gold1*(brk(2)-t(1).p);
        end
        u.p = t(1).p+d;
    end

    % FUNCTION EVALUATION
    u.f = funeval(u.p);

    % Insert the new point into the appropriate position and update
    % the brackets if necessary
    if u.f <= t(1).f
        if u.p >= t(1).p, brk(1)=t(1).p; else brk(2)=t(1).p; end
        t(3) = t(2);
        t(2) = t(1);
        t(1) = u;
    else
        if u.p < t(1).p, brk(1)=u.p; else brk(2)=u.p; end
        if u.f <= t(2).f
            t(3) = t(2);
            t(2) = u;
        elseif u.f <= t(3).f
            t(3) = u;
        end
    end
end

%==========================================================================
% function min1d_plot(action,arg1,arg2,arg3)
%==========================================================================
function min1d_plot(action,arg1,arg2,arg3)
% Visual output for line minimisation
persistent min1dplot

if ~nargin, action = 'Init'; end

% Find the Interactive window and exit if not
%--------------------------------------------------------------------------
fg = spm_figure('FindWin','Interactive');
if isempty(fg), return; end

%-Initialize
%--------------------------------------------------------------------------
if strcmpi(action,'init')
    if nargin<4, arg3 = 'Function';          end
    if nargin<3, arg2 = 'Value';             end
    if nargin<2, arg1 = 'Line minimisation'; end

    min1dplot = struct('pointer',get(fg,'Pointer'),...
                       'name',   get(fg,'Name'),...
                       'ax',     [],...
                       'buffer', get(fg,'DoubleBuffer'));
    min1d_plot('Clear');
    set(fg,'Pointer','Watch');
    set(fg,'DoubleBuffer','on');
    min1dplot.ax = axes('Position', [0.15 0.1 0.8 0.75],...
                        'Box',      'on',...
                        'Parent',   fg);
    lab = get(min1dplot.ax,'Xlabel');
    set(lab,'string',arg3,'FontSize',10);
    lab = get(min1dplot.ax,'Ylabel');
    set(lab,'string',arg2,'FontSize',10);
    lab = get(min1dplot.ax,'Title');
    set(lab,'string',arg1);
    line('Xdata',[], 'Ydata',[],...
        'LineWidth',2,'Tag','LinMinPlot',...
        'LineStyle','-','Marker','o',...
        'Parent',min1dplot.ax);
    drawnow;

%-Reset
%--------------------------------------------------------------------------
elseif strcmpi(action,'set')
    br = findobj(fg,'Tag','LinMinPlot');
    if ~isempty(br)
        [xd,indx] = sort([get(br,'Xdata') arg1]);
        yd = [get(br,'Ydata') arg2];
        yd = yd(indx);
        set(br,'Ydata',yd,'Xdata',xd);
        drawnow;
    end

%-Clear
%--------------------------------------------------------------------------
elseif strcmpi(action,'clear')
    fg = spm_figure('FindWin','Interactive');
    if isstruct(min1dplot)
        if ishandle(min1dplot.ax), delete(min1dplot.ax); end
        set(fg,'Pointer',min1dplot.pointer);
        set(fg,'Name',min1dplot.name);
        set(fg,'DoubleBuffer',min1dplot.buffer);
    end
    spm_figure('Clear',fg);
    drawnow;
end


function [models] = featureSelectionTime_HN(X,Y,maxOrder,nBoot,Info,censoring,seed)
% -------------------------------------------------------------------------
% function [models] = featureSelection(X,Y,maxOrder,nBoot,Info,imbalance,batchNum)
% -------------------------------------------------------------------------
% DESCRIPTION:
% This function computes feature set selection according to the 0.632+
% bootstrap methodology for an input matrix of features and and input
% outcome vector, and for multiple model orders as defined by the user.
% See ref. [1] for more details. This function uses logistic regression
% utilities from DREES <http://www.cerr.info/drees>, and a fast
% implementation of AUC calculation by Enric Junqué de Fortuny that is
% available at: <http://www.mathworks.com/matlabcentral/fileexchange/41258-faster-roc-auc>
%
% NOTE: This function now maximizes 0.5*AUC + 0.5*(1-abs(SENSITIVITY-SPECIFICITY))
% -------------------------------------------------------------------------
% REFERENCE:
% [1] Vallieres, M. et al. (2015). A radiomics model from joint FDG-PET and
%     MRI texture features for the prediction of lung metastases in soft-tissue
%     sarcomas of the extremities. Physics in Medicine and Biology, 60(14),
%     5471-5496. doi:10.1088/0031-9155/60/14/5471
% -------------------------------------------------------------------------
% INPUTS:
% - X: Matrix of size [nInst X nFeat], specifying the numerical data of the
%      features of the input features, where 'nInst' refers to the number
%      of instances in X, and 'nFeat' to the number of features in X.
%      Each column is a different feature.
% - Y: Column vector of size [nInst X 1] specifying the outcome status
%      (1 or 0) for all instances.
% - maxOrder: Integer specifying the maximal model order to construct.
% - nBoot: Number of bootstrap samples to use.
% - Info: Cell of size [nFeat X 1] of strings specifying the name of each
%         feature in 'X'.
% - imbalance: String specifying the type of imbalance-adjustement strategy
%              employed. Either 'IABR' for imbalance-adjusted bootstrap
%              resampling (see ref.[1]), or 'IALR' for imbalance-adjusted
%              logistic regression.
% - batchNum: (optional input). If present, integer that specifies the
%             batch number for parallelization purposes.
% -------------------------------------------------------------------------
% OUTPUTS:
% - models: Structure specifying the resulting multivariable models for the
%           input feature set in 'data', for each model order. Example for
%           order 4:
%        --> models.Order4.Data: Matrix of size [nInst X order], specifying
%                                the selected features, in order of
%                                selection.
%        --> models.Order4.Name: Cell specifying the names of the selected
%                                features, in order of selection.
% -------------------------------------------------------------------------
% AUTHOR(S):
% - Martin Vallieres <mart.vallieres@gmail.com>
% - DREES development team <http://www.cerr.info/drees> (logistic regression)
% - Enric Junqué de Fortuny (fastAUC.cpp)
% -------------------------------------------------------------------------
% HISTORY:
% - Creation - May 2015
% - Revision I - July 2015: including imbalance-adjusted logistic regression
% - Revision II - July 2015: maximizing 0.5*AUC + 0.5*(1-abs(SENSITIVITY-SPECIFICITY))
%--------------------------------------------------------------------------
% STATEMENT:
% This file is part of <https://github.com/mvallieres/radiomics/>,
% a package providing MATLAB programming tools for radiomics analysis.
% --> Copyright (C) 2015  Martin Vallieres
% --> Copyright 2010, Joseph O. Deasy, on behalf of the DREES development team.
%
%    This package 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 package 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 package.  If not, see <http://www.gnu.org/licenses/>.
%
%    _______________________________________________________________
%
% --> Copyright (c) 2013, Enric Junqué de Fortuny
%     All rights reserved.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are
% met:
%
%     * Redistributions of source code must retain the above copyright
%       notice, this list of conditions and the following disclaimer.
%     * Redistributions in binary form must reproduce the above copyright
%       notice, this list of conditions and the following disclaimer in
%       the documentation and/or other materials provided with the distribution
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
% POSSIBILITY OF SUCH DAMAGE.
% -------------------------------------------------------------------------

% RANDOM NUMBER GENERATOR SEED
rng(seed);

% INITIALIZATION
top = 1-1/exp(1);
low = 1/exp(1);
nFeat = size(X,2);
models = struct; % Final model structure

% GETTING BOOTSTRAP SAMPLES FOR ALL EXPERIMENTS
nInst = numel(Y);
trainSets = ceil(nInst .* rand(nInst,nBoot));
testSets = findBootTestSet(trainSets);

% FORWARD FEATURE SELECTION (for all different starters)
modelMat = zeros(nFeat,maxOrder);
metricMat = zeros(nFeat,maxOrder);
for i = 1:nFeat
     indLeft = 1:nFeat;
     % Order 1
     indLeft(i) = [];
     modelMat(i,1) = i;
     Xtrain = X(:,i);
     [Xtrain] = normalizeZeroOne(Xtrain);

     % Cox regression code
     CIdata = applyCoxfit(Xtrain,Y,Xtrain,Y,censoring,censoring);

     CItemp = 0;
     for n = 1:nBoot
         Xtrain = X(trainSets(:,n),i);Xtest = X(testSets{n},i); Ytrain = Y(trainSets(:,n),1); Ytest = Y(testSets{n},1);
         [Xtrain,Xtest] = normalizeZeroOne(Xtrain,Xtest);

         % Cox regression code
         CIboot = applyCoxfit(Xtrain,Ytrain,Xtest,Ytest,censoring(trainSets(:,n)),censoring(testSets{n}));

         % FOR CI (concordance index)
         alpha = top/(1-low*(CIdata-CIboot)/(CIdata-0.5+eps));
         if alpha > 1
             alpha = 1;
         elseif alpha < top
             alpha = top;
         end
         if CIboot < 0.5
             CIboot = 0.5;
         end
         CItemp = CItemp + (1-alpha)*CIdata + alpha*CIboot;

     end
     CItemp = CItemp/nBoot;
     metricMat(i,1) = CItemp;

     % Going for orders 2 to maxOrder
     for j = 2:maxOrder
         maxMetric = 0;
         for k = 1:(nFeat-j+1)
             indexModel = [modelMat(i,1:(j-1)),indLeft(k)];
             Xtrain = X(:,indexModel);
             [Xtrain] = normalizeZeroOne(Xtrain);

             % Cox regression code
             CIdata = applyCoxfit(Xtrain,Y,Xtrain,Y,censoring,censoring);

             CItemp = 0;
             for n = 1:nBoot
                 Xtrain = X(trainSets(:,n),indexModel); Xtest = X(testSets{n},indexModel); Ytrain = Y(trainSets(:,n),1); Ytest = Y(testSets{n},1);
                 [Xtrain,Xtest] = normalizeZeroOne(Xtrain,Xtest);

                 % Cox regression code
                 CIboot = applyCoxfit(Xtrain,Ytrain,Xtest,Ytest,censoring(trainSets(:,n)),censoring(testSets{n}));

                 % FOR CI (concordance index)
                 alpha = top/(1-low*(CIdata-CIboot)/(CIdata-0.5+eps));
                 if alpha > 1
                     alpha = 1;
                 elseif alpha < top
                     alpha = top;
                 end
                 if CIboot < 0.5
                     CIboot = 0.5;
                 end
                 CItemp = CItemp + (1-alpha)*CIdata + alpha*CIboot;

             end
             CItemp = CItemp/nBoot;
             metricTemp = CItemp;
             if metricTemp >= maxMetric
                 maxMetric = metricTemp;
                 index = indLeft(k);
             end
         end
         modelMat(i,j) = index;
         metricMat(i,j) = maxMetric;
         indLeft(find(indLeft==index)) = [];
     end
end

% OBTAINING MAXIMUM RESULTS FOR EVERY MODEL ORDER (maximum from all different starters)
[~,indMax] = max(metricMat);
for i = 1:maxOrder
    nameOrder = ['Order',num2str(i)];
    models.(nameOrder).Data = X(:,modelMat(indMax(i),1:i));
    models.(nameOrder).Name = cell(i,1);
    for j = 1:i
        models.(nameOrder).Name{j} = Info{modelMat(indMax(i),j)};
    end
end

end


% version from 102901
function [w,h]=nmf(v,r,verbose,winit,hinit,w_fixvec,h_fixvec)
%
% Jean-Philippe Brunet
% Cancer Genomics
% The Broad Institute
% brunet@broad.mit.edu
%
% This software and its documentation are copyright 2004 by the
% Broad Institute/Massachusetts Institute of Technology. All rights are reserved.
% This software is supplied without any warranty or guaranteed support whatsoever.
% Neither the Broad Institute nor MIT can not be responsible for its use, misuse,
% or functionality.
%
% NMF divergence update equations :
% Lee, D..D., and Seung, H.S., (2001), 'Algorithms for Non-negative Matrix
% Factorization', Adv. Neural Info. Proc. Syst. 13, 556-562.
%
% v (n,m) : N (genes) x M (samples) original matrix
%           Numerical data only.
%           Must be non negative.
%           Not all entries in a row can be 0. If so, add a small constant to the
%           matrix, eg.v+0.01*min(min(v)),and restart.
%
% r       : number of desired factors (rank of the factorization)
%
% verbose : prints iteration count and changes in connectivity matrix elements
%           unless verbose is 0
%
% Note : NMF iterations stop when connectivity matrix has not changed
%        for 10*stopconv interations. This is experimental and can be
%        adjusted.
%
% w    : N x r NMF factor
% h    : r x M NMF factor
%
% winit - initial value for w
% hinit - initial value for h
% h_fixvec & w_fixvec:
% fixvec - binary vector which component should be fixed: eg [2 3] will
% fix second and third component while varying the first...

if ~exist ('w_fixvec', 'var')
    w_fixvec = [];
end

if ~exist ('h_fixvec', 'var')
    h_fixvec = [];
end

% test for negative values in v
if min(min(v)) < 0
    error('matrix entries can not be negative');
    return
end
if min(sum(v,2)) == 0
    error('not all entries in a row can be zero');
    return
end

[n,m]=size(v);
stopconv=40;      % stopping criterion (can be adjusted)
niter = 10000;     % maximum number of iterations (can be adjusted)

cons=zeros(m,m);
consold=cons;
inc=0;
j=0;

%
% initialize random w and h
%
w=rand(n,r);
if exist('winit', 'var')
    if ~isempty(winit)
        w = winit;
        fprintf ('initial values of ''w'' used...\n');
    end
else
    winit = zeros(n,r);
end

h=rand(r,m);
if exist('hinit', 'var')
    if ~isempty(hinit)
        h = hinit;
        fprintf ('initial values of ''h'' used...\n');
    end
else
    hinit = zeros(r,m);
end

if ~isempty(w_fixvec)
    fprintf('Fixing [ ');
    fprintf('%g ', w_fixvec);
    fprintf('] component of ''w''\n');
end

if ~isempty(h_fixvec)
    fprintf('Fixing [ ');
    fprintf('%g ', h_fixvec);
    fprintf('] component of ''h''\n');
end

% oldpar = load('/afs/inf.ed.ac.uk/user/s08/s0880377/project/MATLAB/qdots/h5.mat');
% w = oldpar.w;
% h=oldpar.h;

for i=1:niter

    % divergence-reducing NMF iterations

    x1=repmat(sum(w,1)',1,m);
    h=h.*(w'*(v./(w*h)))./x1;
    h(h_fixvec,:) = hinit(h_fixvec,:);

    x2=repmat(sum(h,2)',n,1);
    w=w.*((v./(w*h))*h')./x2;
    w(:,w_fixvec) = winit(:,w_fixvec);

    % test convergence every 10 iterations

    if(mod(i,10)==0)
        j=j+1;

        % adjust small values to avoid undeflow
        h=max(h,eps);w=max(w,eps);

        % construct connectivity matrix
        [y,index]=max(h,[],1);   %find largest factor
        mat1=repmat(index,m,1);  % spread index down
        mat2=repmat(index',1,m); % spread index right
        cons=mat1==mat2;

        if(sum(sum(cons~=consold))==0) % connectivity matrix has not changed
            inc=inc+1;                     %accumulate count
        else
            inc=0;                         % else restart count
        end
        if verbose                     % prints number of changing elements
            fprintf('\t%d\t%d\t%d\n',i,inc,sum(sum(cons~=consold))),
        end

        if(inc>stopconv)
            break,                % assume convergence is connectivity stops changing
        end

        consold=cons;

    end
end

if i == niter
    fprintf('\nMAximum number of iteration (%g) reached! \n', niter)
end


function info = muteinf(A, Y, method)
	n = size(A,1);
	Z = [A Y];
	if(n/10 > 20)
		nbins = 20;
	else
		nbins = max(floor(n/10),10);
	end;
	pA = hist(A, nbins);
	pA = pA ./ n;

	i = find(pA == 0); % that's a hack!
	pA(i) = 0.00001;
	if(strcmp(method,'regression'))
		pY = hist(Y, nbins);
		pY = pY ./ n;
		j = find(pY == 0);
		pY(j) = 0.00001;

		rx = abs(max(A) - min(A)) / nbins;
		ry = abs(max(Y) - min(Y)) / nbins;
		yl = min(Y);
		p = zeros(nbins, nbins);
		for i = 1:nbins
			xl = min(A);
			for j = 1:nbins
				%disp(['intervals [' num2str(xl) ',' num2str(xl+rx) '|' num2str(yl) ',' num2str(yl+ry) ']'])
				interval = (xl <= Z(:,1)) & (yl <= Z(:,2));
				if(j < nbins)
					interval = interval & (Z(:,1) < xl + rx);
				end;
				if(i < nbins)
					interval = interval & (Z(:,2) < yl + ry);
				end;
				%find(interval)
				p(i,j) = length(find(interval));

				if p(i,j) == 0 % hack!
					p(i,j) = 0.00001;
				end

				xl = xl + rx;
			end;
			yl = yl + ry;
		end;
		HA = -sum(pA .* log(pA));
		HY = -sum(pY .* log(pY));
		pA = repmat(pA,nbins,1);
		pY = repmat(pY',1,nbins);
	else
		od = size(Y,2);
		cl = od;
		if(od == 1)
			pY = [length(find(Y==+1)) length(find(Y==-1))] / n;
			cl = 2;
		else
			pY = zeros(1,od);
			for i=1:od
				pY(i) = length(find(Y==+1));
			end;
			pY = pY / n;
		end;
		p = zeros(cl,nbins);
		rx = abs(max(A) - min(A)) / nbins;
		for i = 1:cl
			xl = min(A);
			for j = 1:nbins
				if(i == 2) & (od == 1)
					interval = (xl <= Z(:,1)) & (Z(:,2) == -1);
				else
					interval = (xl <= Z(:,1)) & (Z(:,i+1) == +1);
				end;
				if(j < nbins)
					interval = interval & (Z(:,1) < xl + rx);
				end;
				%find(interval)
				p(i,j) = length(find(interval));

				if p(i,j) == 0 % hack!
					p(i,j) = 0.00001;
				end

				xl = xl + rx;
			end;
		end;
		HA = -sum(pA .* log(pA));
		HY = -sum(pY .* log(pY));
		pA = repmat(pA,cl,1);
		pY = repmat(pY',1,nbins);
	end;
	p = p ./ n;

	info = sum(sum(p .* log(p ./ (pA .* pY))));
	info = 2 * info ./ (HA + HY);


function [shape] = normalize_first_shape( Data, options )

shape = Data.shape;

%% calculating bounding box %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[cropmin,cropmax,offset,minshape,marginW,marginH] = bounding_box ( shape );

%% calculate scale factor %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
W_H = cropmax - cropmin;
wh1 = W_H(1);
wh2 = W_H(2);

CanvasSize = options.canvasSize;

scf = CanvasSize(1)/wh1;
if(scf*wh2 > CanvasSize(2))
    scf = CanvasSize(2)/wh2;
end

%% croping image (for debug only) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
debug = 0;

if debug
    img = imread(Data.img);
    cropImage   = img(cropmin(2):cropmax(2), cropmin(1):cropmax(1));
    scaleImage  = imresize(cropImage, scf);
end

%% scale shape and image %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
shape = shape - repmat((minshape - [marginW marginH] + offset) ...
    , size(shape, 1), 1);
shape = shape*scf;

if debug
    % Displaying image and feature points.
    figure(1);
    imshow(scaleImage);
    hold on;
    plot(shape(:, 1), shape(:, 2), 'g*');
    pause;
end

end


function SO3VF = rotate(SO3VF,rot,varargin)
% rotate a SO3 vector field by a rotation
%
% Syntax
%   SO3VF = rotate(SO3VF,rot)
%
% Input
%  SO3VF - @SO3VectorField
%  rot  - @rotation
%
% Output
%  SO3VF - @SO3VectorFieldHandle
%

SO3VF = SO3VectorFieldHandle(@(q) SO3VF.eval(inv(rot)*q),SO3VF.CS,SO3VF.SS);


clear all
close all
clc
load('\dades.mat')
k=4;

%%%%%%%%%%%%%%%%%%%%%
%  Funcion k-means  %
%%%%%%%%%%%%%%%%%%%%%
[centro, Xcluster, Ycluster, clustering]=kmedia(X, Y, k);

% Presentacion de resultados por pantalla.

% Creamos los colores.
colr=zeros(k,3);
for i=1:k
    colr(i,:)=rand(1,3);
end;

% Representamosla informacion.
figure;
hold on;
for i=1:k,
    n=find(clustering==i);
    p=plot(X(n), Y(n),'.');set(p,'Color',colr(i,:));title(k)
end;

clc;
fprintf('Numero de agrupaciones: %d',A);
% Borramos variables temporales.
clc;clear n;clear i;clear p;clear colr;


function test94()

% Test for fixedrankNewGHquotientfactory geometry (low rank completion)
%
% Test CG scheme

% This file is part of Manopt: www.manopt.org.
% Original author: Bamdev Mishra, Dec. 30, 2012.
% Contributors:
% Change log:

clc; close all;
%     reset(RandStream.getDefaultStream);
%     randnfoo = randn(1, 1); %#ok<NASGU>

m = 500;
n = 500;
r = 5;
A = randn(m, r);
B = randn(n, r);
C = A*B';

problem.M = fixedrankNewGHquotientfactory(m, n, r);

df = problem.M.dim();
p = 4*df/(m*n);
mask = rand(m, n) <= p;

symm = @(M) .5*(M+M');

problem.cost = @cost;
    function f = cost(X)
        f = 0.5*(norm(mask.*(X.G*X.H' - C), 'fro')^2);
    end

problem.grad = @grad;
    function g = grad(X)
        G = X.G;
        H = X.H;
        GtG = X.G'*X.G;
        HtH = X.H'*X.H;
        invGtG = eye(size(GtG)) / GtG;
        invHtH = eye(size(HtH)) / HtH;

        R = mask.^2 .* (G*H' - C);
        g = struct('G', (R*H)*invHtH, ...
            'H', (R'*G)*invGtG );
    end

problem.hess = @hess;
    function Hess = hess(X, eta)

        S = mask.*( X.G*X.H' - C);
        S_star  = mask.*(eta.G*X.H' + X.G*eta.H');

        GtG = X.G'*X.G;
        HtH = X.H'*X.H;
        invGtG = eye(size(GtG)) / GtG;
        invHtH = eye(size(HtH)) / HtH;

        Hess.G = S_star*X.H*invHtH;
        Hess.H = S_star'*X.G*invGtG;

        ShH = S*eta.H;
        ShG = S'*eta.G;
        SH = S*X.H;
        SG = S'*X.G;
        gradG = SH*invHtH;
        gradH = SG*invGtG;

        Hess.G = Hess.G + ShH*invHtH - 2*SH*(invHtH * symm(eta.H'*X.H) * invHtH);
        Hess.H = Hess.H + ShG*invGtG - 2*SG*(invGtG * symm(eta.G'*X.G) * invGtG);

        % I still need a correction factor for the non-constant metric
        Hess.G = Hess.G + gradG*symm(eta.H'*X.H)*invHtH + eta.G*symm(gradH'*X.H)*invHtH - X.G*symm(eta.H'*gradH)*invHtH;
        Hess.H = Hess.H + gradH*symm(eta.G'*X.G)*invGtG + eta.H*symm(gradG'*X.G)*invGtG - X.H*symm(eta.G'*gradG)*invGtG;

        Hess = problem.M.proj(X, Hess);

    end

%
%     checkgradient(problem);
%     drawnow;
%     pause;
%     checkhessian(problem);
%     drawnow;
%     pause;
%
%     problem = rmfield(problem, 'hess');

% [U S V] = svds(mask.*C, r);
% G0 = U*(S.^0.5);
% H0 = V*(S.^0.5);
G0 = randn(m, r);
H0 = randn(n, r);

X0 = struct('G', G0, 'H', H0 );

options.statsfun = @statsfun;

    function stats = statsfun(problem, x, stats)
        stats.RMSE = nan;%norm(C - x.M*x.N', 'fro')/sqrt(m*n);
    end

% options.linesearch = @linesearch_adaptive;

options.maxiter = inf;
options.maxinner = 30;
options.maxtime = 120;
%     options.mininner = problem.M.dim();
options.tolgradnorm = 1e-2;
options.Delta_bar = min(m, n) * r;
options.Delta0 = options.Delta_bar / 8;

%     options.useRand = true;
% fprintf('----------- TR -----------\n');
% [Xopt costopt info] = trustregions(problem, X0, options);

fprintf('----------- Steepest descent with NW -----------\n');
options.linesearch = @linesearch;
[Xopt costopt infos_SD] = steepestdescent(problem, X0, options);

fprintf('----------- Steepest descent with adaptive -----------\n');
options.linesearch = @linesearch_adaptive;
[Xopt costopt infos_SD_adaptive] = steepestdescent(problem, X0, options);

options.beta_type = 'P-R'; % Other options are 'steep' and 'FR'
options.orth_value = 0.1; Inf;

fprintf('----------- CG algorithm with default -----------\n');
options.linesearch = @linesearch;
[Xopt costopt infos_CG] = conjugategradient(problem, X0, options);

fprintf('----------- CG algorithm with adaptive -----------\n');
options.linesearch = @linesearch_adaptive;
[Xopt costopt infos_CG_adaptive] = conjugategradient(problem, X0, options);

%     keyboard;

% subplot(3, 1, 1);
% semilogy([info.iter], [info.cost], '.-');
% subplot(3, 1, 2);
% semilogy([info.iter], [info.gradnorm], '.-');
% subplot(3, 1, 3);
% semilogy([info.iter], [info.RMSE], '.-');

% Cost versus iterations
fs = 20;
figure;
semilogy(0:length([infos_SD.cost])-1,[infos_SD.cost],'Color','b','LineWidth',2);
hold on;
semilogy(0:length([infos_SD_adaptive.cost])-1,[infos_SD_adaptive.cost],'Color','r','LineWidth',2);
semilogy(0:length([infos_CG.cost])-1,[infos_CG.cost],'--','Color','b','LineWidth',2);
semilogy(0:length([infos_CG_adaptive.cost])-1,[infos_CG_adaptive.cost],'--','Color','r','LineWidth',2);

hold off;
ax1 = gca;
set(ax1,'FontSize',fs);
xlabel(ax1,'Number of iterations','FontSize',fs);
ylabel(ax1,'Cost ','FontSize',fs);
mincost = min([infos_SD.cost]);
maxcost = max([infos_SD.cost]);
axis([get(gca,'XLim') mincost maxcost])
legend('SD NW','SD adaptive', 'CG NW','CG adaptive');
legend 'boxoff';
box off;

end


function [chnsReg,chnsSim] = edgesChns( I, opts )
% Compute features for structured edge detection.
%
% For an introductory tutorial please see edgesDemo.m.
%
% USAGE
%  [chnsReg,chnsSim] = edgesChns( I, opts )
%
% INPUTS
%  I          - [h x w x 3] color input image
%  opts       - structured edge model options
%
% OUTPUTS
%  chnsReg    - [h x w x nChannel] regular output channels
%  chnsSim    - [h x w x nChannel] self-similarity output channels
%
% EXAMPLE
%
% See also edgesDemo, edgesTrain, edgesDetect, gradientMag
%
% Structured Edge Detection Toolbox      Version 3.0
% Copyright 2014 Piotr Dollar.  [pdollar-at-microsoft.com]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the MSR-LA Full Rights License [see license.txt]

shrink=opts.shrink; nTypes=1; chns=cell(1,opts.nChns); k=0;
if(size(I,3)>3), nTypes=2; Is={I(:,:,1:3),I(:,:,4:end)}; end
for t=1:nTypes
  if(nTypes>1), I=Is{t}; end
  if(size(I,3)==1), cs='gray'; else cs='luv'; end; I=rgbConvert(I,cs);
  Ishrink=imResample(I,1/shrink); k=k+1; chns{k}=Ishrink;
  for i = 1:2, s=2^(i-1);
    if(s==shrink), I1=Ishrink; else I1=imResample(I,1/s); end
    I1 = convTri( I1, opts.grdSmooth );
    [M,O] = gradientMag( I1, 0, opts.normRad, .01 );
    H = gradientHist( M, O, max(1,shrink/s), opts.nOrients, 0 );
    k=k+1; chns{k}=imResample(M,s/shrink);
    k=k+1; chns{k}=imResample(H,max(1,s/shrink));
  end
end
chns=cat(3,chns{1:k}); assert(size(chns,3)==opts.nChns);
chnSm=opts.chnSmooth/shrink; if(chnSm>1), chnSm=round(chnSm); end
simSm=opts.simSmooth/shrink; if(simSm>1), simSm=round(simSm); end
chnsReg=convTri(chns,chnSm); chnsSim=convTri(chns,simSm);

end


% xct_bh_example.m
%
% illustrate X-ray CT iterative reconstruction of an object
% consisting of water and bones, for a polyenergetic spectrum.
%
% Copyright 2008-12-05, Jeff Fessler, University of Michigan

warn 'work in progress'

if ~isvar('ssino'), printm 'ssino'
	ig = image_geom('nx', 128, 'ny', 120, 'dx', 0.4);
	ig.mask = ig.circ > 0;
	ellw = [0 0 15 10 0 1;
		-6 0 2 2 0 -1;
		6 -0 2 2 0 -1];
	wtrue = ellipse_im(ig, ellw, 'oversample', 3);

	ellb = [-6 0 2 2 0 1.9;
		6 -0 2 2 0 1.9];
	btrue = ellipse_im(ig, ellb, 'oversample', 3);

	dens_true = wtrue + btrue;
	im plc 2 3, clim = [0 1.2];
	im(1, dens_true, clim, 'dens true'), cbar

	sg = sino_geom('par', 'nb', 140, 'na', 100, 'dr', ig.dx);
	wsino = ellipse_sino(sg, ellw, 'oversample', 2);
	bsino = ellipse_sino(sg, ellb, 'oversample', 2);

	ssino = cat(3, wsino, bsino);
	im(4, sg.s, sg.ad, ssino, 's sino'), cbar
prompt
end

if ~isvar('ftab2'), printm 'ftab2'
	xrs = xray_read_spectra('poly1,140', ...
		'filters', {{'aluminum', 0.25, 'copper', 0.05}});

	sls1 = de_ftab_sls('max', 50, 'n', 101);
	mas1 = xray_read_mac('water');
	ftab1 = de_ftab(xrs, mas1, 'sls', sls1, 'ctype', 'newt', ...
		'ftype','exp', 'fit_args', {'kev', 10:5:160, 'mac', []});

	sls2 = de_ftab_sls('max', [50 40], 'n', [101 11]);
	mas2 = xray_read_mac({'water', 'bone'});
	ftab2 = de_ftab(xrs, mas2, 'sls', sls2, 'ctype', 'newt', ...
		'ftype','exp', 'fit_args', {'kev', 10:5:160, 'mac', []});

	ftab2.plot_fm
%	ftab1.inv1.plot % todo: fails - fix!
	ftab1.inv1.plot(ftab1.fit);
%	im subplot 2
prompt
end

if 0
	clf
	tmp = ftab1.inv1.fun(ftab2.fm);
	plot(sls1.sl{1}, tmp)
	xlabel 's1', ylabel ''
return
end

if ~isvar('fsino'), printm 'fsino'
	fsino = ftab2.fit.fmfun(ssino);
	im(5, sg.s, sg.ad, fsino, 'f sino'), cbar
prompt
end

if ~isvar('yi'), printm 'yi'
%	yi = fsino; % no noise
%	wi = 1; % unweighted

	f.I0 = 1e6; % high snr
	yi = poisson(f.I0 * exp(-fsino), 7) / f.I0;
	wi = yi;
	yi = -log(max(yi, 1/f.I0));
	im(6, sg.s, sg.ad, yi, 'yi sino'), cbar
	im(3, sg.s, sg.ad, wi, 'wi sino'), cbar
prompt
end

if ~isvar('fbpu'), printm 'fbpu'
	f.fbp = fbp2(sg, ig);
	tmp = yi / ftab2.mac.bar(1); % scale correct for effective water mac
	fbpu = fbp2(tmp, f.fbp, 'window', 'hanning,0.8');
	im(2, fbpu, 'fbp uncorrected', clim), cbar
prompt
end

if ~isvar('fbpc'), printm 'fbpc'
	tmp = ftab1.inv1.fun(yi); % water BH correction
	fbpc = fbp2(tmp, f.fbp, 'window', 'hanning,0.8');
	im(3, fbpc, 'fbp corrected', clim), cbar

	prompt
	if 1
		clf
		iy = ig.ny/2+1; ix = 1:ig.nx;
		pro = @(x) x(ix,iy);
		% pseudo-density
		xtrue = wtrue + btrue * ftab2.mac.bar(2) / ftab2.mac.bar(1);
		plot([pro(xtrue) pro(dens_true) pro(fbpc) pro(fbpu)])
		legend('xtrue', 'dens true', 'FBP corrected', 'FBP uncorrected')
	prompt
	end
end

if ~isvar('G'), printm 'G'
	if has_mex_jf
		f.tab_type = {'square/strip', 'chat', 0, 'Ltab', ...
			1000, 'strip_width', ig.dx};
		G = Gtomo2_table(sg, ig, f.tab_type, 'nthread', 1);
	else
		G = Gtomo2_strip(sg, ig);
	end

	if 0
		tmp = G * xtrue;
		im(tmp), cbar
		max_percent_diff(tmp, ssino)
	end
end

if ~isvar('fbpb'), printm 'fbpb' % "bone only recon"
	tmp = fbpc .* (fbpc > 1.3); % bone only image
	im(tmp)

	tmp = G * tmp;
	tmp = tmp.^2;
	fbpb = fbp2(tmp, f.fbp, 'window', 'hanning,0.8');
	im(fbpb)
return
end

if 1 % test it - it kind of works!
	scale =  0.005; % empirical value
	fbp1 = fbpc + scale * fbpb;

	if 1
		clf
		iy = ig.ny/2+1; ix = 1:ig.nx;
		pro = @(x) x(ix,iy);
		% pseudo-density
		xtrue = wtrue + btrue * ftab2.mac.bar(2) / ftab2.mac.bar(1);
		plot([pro(xtrue) pro(dens_true) pro(fbpc) pro(fbp1)])
		legend('xtrue', 'dens true', 'FBP corrected', 'FBP1 corrected')
	end

	clim = 1 + [-1 1] * 0.10;
	im(stackup(xtrue, fbpu, fbpc, fbp1), clim), cbar
return
end

if ~isvar('R'), printm 'R'
	% Fessler 1996 spatial resolution properties...
	kappa = sqrt(div0(G' * wi, G' * sg.ones));
	R = Reg1(kappa, 'type_denom', 'matlab', 'beta', 2^3, ...
		'pot_arg', {'hyper3', 0.1});
	if 1
		qpwls_psf(G, R, 1, ig.mask, Gdiag(wi));
	end
prompt
end

%
% corrected iterative recon
%

if ~isvar('xc'), printm 'xc'
	f.niter = 400;
	xinit = fbpu;
%	xinit = fbpc > 0.9;
	data = {yi(:), wi(:), ftab, 1};
	dercurv = @wls_water_dercurv;
	tmp = pl_pcg_qs_ls(ig.maskit(xinit), G, ...
		data, dercurv, R, ...
		'niter', f.niter, 'isave', 'all');
	xc = ig.embed(tmp);

	clf, im(xc, clim)
	prompt
prompt
end

	if im
		clf, im pl 2 3
		im(1, xtrue, clim, 'true')
		im(2, fbpc, clim, 'fbp corrected')
		im(3, xc(:,:,end), clim, 'iterative')

		subplot(212)
		plot([pro(xtrue) pro(fbpc) pro(xc(:,:,end))])
		legend('true', 'FBP corrected', 'iterative')
	return
	end

%	movie2(xc, 'file', '/y/fessler/tmp3.avi')


% eeg_point2lat() - convert latency in data points to latency in ms relative
%                   to the time locking. Used in eeglab().
% Usage:
%       >> [newlat ] = eeg_point2lat( lat_array, [], srate);
%       >> [newlat ] = eeg_point2lat( lat_array, epoch_array,...
%                                 srate, timelimits, timeunit);
% Inputs:
%   lat_array   - latency array in data points assuming concatenated
%                 data epochs (see eeglab() event structure)
%   epoch_array - epoch number corresponding to each latency value
%   srate       - data sampling rate in Hz
%   timelimits  - [min max] timelimits in 'timeunit' units (see below)
%   timeunit    - time unit in second. Default is 1 = seconds.
%
% Outputs:
%   newlat      - converted latency values (in 'timeunit' units) for each epoch
%
% Example:
%   tmpevent = EEG.event;
%   eeg_point2lat( [ tmpevent.latency ], [], EEG.srate, [EEG.xmin EEG.xmax]);
%   % returns the latency of all events in second for a continuous
%   % dataset EEG
%
%   eeg_point2lat( [ tmpevent.latency ], [ tmpevent.epoch ],
%                 EEG.srate, [EEG.xmin EEG.xmax]*1000, 1E-3);
%   % returns the latency of all events in millisecond for a dataset
%   % containing data epochs.
%
%
% Author: Arnaud Delorme, CNL / Salk Institute, 2 Mai 2002
%
% See also: eeg_lat2point(), eeglab(), pop_editieventvals(), pop_loaddat()

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

function newlat = eeg_point2lat( lat_array, epoch_array, srate, timewin, timeunit);

if nargin <3
    help eeg_point2lat;
    return;
end;
if isempty( epoch_array )
    epoch_array = ones( size(lat_array) );
end;
if nargin <4
    timewin = 0;
end;
if nargin <5
	timeunit = 1;
end;

if length(lat_array) ~= length(epoch_array)
	if length(epoch_array)~= 1
		disp('eeg_point2lat: latency and epoch arrays must have the same length'); return;
	else
		epoch_array = ones(1,length(lat_array))*epoch_array;
	end;
end;
if length(timewin) ~= 2
    disp('eeg_point2lat: timelimits array must have length 2'); return;
end;
if iscell(epoch_array)
	epoch_array = [ epoch_array{:} ];
end;
if iscell(lat_array)
	lat_array = [ lat_array{:} ];
end

timewin = timewin*timeunit;

if length(timewin) == 2
    pnts = (timewin(2)-timewin(1))*srate+1;
else
    pnts = 0;
end;
newlat  = ((lat_array - (epoch_array-1)*pnts-1)/srate+timewin(1))/timeunit;
newlat = round(newlat*1E9)*1E-9;


function xf = cfft2(x)

% calculate output size
in_sz = size(x);

% if both dimensions are odd
if all(mod(in_sz(1:2), 2) == 1)
    xf = fftshift(fftshift(fft2(x), 1), 2);
else
    out_sz = in_sz;
    out_sz(1:2) = out_sz(1:2) + mod(out_sz(1:2)+1,2);

    % allocate
    xf = complex(zeros(out_sz, 'single'));

    xf(1:in_sz(1),1:in_sz(2),:,:) = fftshift(fftshift(fft2(x), 1), 2);

    if out_sz(1) ~= in_sz(1)
        xf(end,:,:,:) = conj(fliplr(xf(1,:,:,:)));
    end
    if out_sz(2) ~= in_sz(2)
        xf(:,end,:,:) = conj(flipud(xf(:,1,:,:)));
    end
end


%%%  AttitudeControl_Sim
clear
path('./icon/',path);
Init;

%Constant value
RAD2DEG = 57.2957795;
DEG2RAD = 0.0174533;
%throttle when UAV is hovering
THR_HOVER = 0.609;

%% Initial condition
ModelInit_PosE = [0, 0, 0];
ModelInit_VelB = [0, 0, 0];
ModelInit_AngEuler = [0, 0, 0];
ModelInit_RateB = [0, 0, 0];
ModelInit_Rads = 0;
%% control parameter
%Attitude PID parameters
Kp_PITCH_ANGLE = 6.5;
Kp_PITCH_AngleRate = 0.1;
Ki_PITCH_AngleRate = 0.02;
Kd_PITCH_AngleRate = 0.001;
Kp_ROLL_ANGLE = 6.5;
Kp_ROLL_AngleRate = 0.1;
Ki_ROLL_AngleRate = 0.02;
Kd_ROLL_AngleRate = 0.001;

Kp_YAW_AngleRate = 0.5;
Ki_YAW_AngleRate = 0.01;
Kd_YAW_AngleRate = 0.00;
%integral saturation
Saturation_I_RP_Max = 0.3;
Saturation_I_RP_Min = -0.3;
Saturation_I_Y_Max = 0.2;
Saturation_I_Y_Min = -0.2;
%max control angle,default 35deg
MAX_CONTROL_ANGLE_ROLL = 35;
MAX_CONTROL_ANGLE_PITCH  = 35;
%max control angle rate,rad/s
MAX_CONTROL_ANGLE_RATE_PITCH = 220;
MAX_CONTROL_ANGLE_RATE_ROLL = 220;
MAX_CONTROL_ANGLE_RATE_Y = 200;
%% run simulink model
AttitudeControl_Sim